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