InequalityConstraint.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_solver/utils/Constraint.h>
 
namespace mc_solver
{
 
namespace utils
{
 
template<typename UpdateT>
struct InequalityConstraint : public Constraint<tasks::qp::Inequality, UpdateT>
{
  using Base = Constraint<tasks::qp::Inequality, UpdateT>;
 
  // XXX The following is a hack to work-around the absence of constructor inheritance in GCC 4.7
  // We enable different constructor based on the UpdateT class
 
  template<typename U = UpdateT, typename = typename std::enable_if<std::is_same<U, utils::UpdateRobot>::value>::type>
  explicit InequalityConstraint(unsigned int rIndex) : Base(rIndex)
  {
  }
 
  template<typename U = UpdateT, typename = typename std::enable_if<std::is_same<U, utils::UpdateLambda>::value>::type>
  explicit InequalityConstraint(const tasks::qp::ContactId & cid) : Base(cid)
  {
  }
 
  template<typename U = UpdateT, typename = typename std::enable_if<std::is_same<U, utils::UpdateForce>::value>::type>
  InequalityConstraint(const mc_solver::QPSolver & solver, const tasks::qp::ContactId & cid) : Base(solver, cid)
  {
  }
 
  const Eigen::MatrixXd & AInEq() const override { return this->AFull_; }
 
  std::string descInEq(const std::vector<rbd::MultiBody> &, int i) override
  {
    std::stringstream ss;
    ss << "Failure at line " << i << " for inequality " << this->nameInEq();
    return ss.str();
  }
};
 
} // namespace utils
 
struct InequalityConstraintRobot : public utils::InequalityConstraint<utils::UpdateRobot>
{
  explicit InequalityConstraintRobot(unsigned int rIndex) : utils::InequalityConstraint<utils::UpdateRobot>(rIndex) {}
};
 
struct InequalityConstraintLambda : public utils::InequalityConstraint<utils::UpdateLambda>
{
  explicit InequalityConstraintLambda(const tasks::qp::ContactId & cid)
  : utils::InequalityConstraint<utils::UpdateLambda>(cid)
  {
  }
};
 
struct InequalityConstraintForce : public utils::InequalityConstraint<utils::UpdateForce>
{
  InequalityConstraintForce(const mc_solver::QPSolver & solver, const tasks::qp::ContactId & cid)
  : utils::InequalityConstraint<utils::UpdateForce>(solver, cid)
  {
  }
};
 
} // namespace mc_solver