GenInequalityConstraint.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 GenInequalityConstraint : public Constraint<tasks::qp::GenInequality, UpdateT>
{
  using Base = Constraint<tasks::qp::GenInequality, 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 GenInequalityConstraint(unsigned int rIndex) : Base(rIndex)
  {
  }
 
  template<typename U = UpdateT, typename = typename std::enable_if<std::is_same<U, utils::UpdateLambda>::value>::type>
  explicit GenInequalityConstraint(const tasks::qp::ContactId & cid) : Base(cid)
  {
  }
 
  template<typename U = UpdateT, typename = typename std::enable_if<std::is_same<U, utils::UpdateForce>::value>::type>
  GenInequalityConstraint(const mc_solver::QPSolver & solver, const tasks::qp::ContactId & cid) : Base(solver, cid)
  {
  }
 
  const Eigen::MatrixXd & AGenInEq() const override { return this->AFull_; }
 
  std::string descGenInEq(const std::vector<rbd::MultiBody> &, int i) override
  {
    std::stringstream ss;
    ss << "Failure at line " << i << " for inequality " << this->nameGenInEq();
    return ss.str();
  }
};
 
} // namespace utils
 
struct GenInequalityConstraintRobot : public utils::GenInequalityConstraint<utils::UpdateRobot>
{
  explicit GenInequalityConstraintRobot(unsigned int rIndex)
  : utils::GenInequalityConstraint<utils::UpdateRobot>(rIndex)
  {
  }
};
 
struct GenInequalityConstraintLambda : public utils::GenInequalityConstraint<utils::UpdateLambda>
{
  explicit GenInequalityConstraintLambda(const tasks::qp::ContactId & cid)
  : utils::GenInequalityConstraint<utils::UpdateLambda>(cid)
  {
  }
};
 
struct GenInequalityConstraintForce : public utils::GenInequalityConstraint<utils::UpdateForce>
{
  GenInequalityConstraintForce(const mc_solver::QPSolver & solver, const tasks::qp::ContactId & cid)
  : utils::GenInequalityConstraint<utils::UpdateForce>(solver, cid)
  {
  }
};
 
} // namespace mc_solver