DynamicsConstraint.h

Go to the documentation of this file.

/*
 * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_solver/KinematicsConstraint.h>
 
#include <mc_tvm/DynamicFunction.h>
 
#include <Tasks/QPMotionConstr.h>
 
namespace mc_solver
{
 
struct MC_SOLVER_DLLAPI DynamicsConstraint : public KinematicsConstraint
{
public:
  DynamicsConstraint(const mc_rbdyn::Robots & robots, unsigned int robotIndex, double timeStep, bool infTorque = false);
 
  DynamicsConstraint(const mc_rbdyn::Robots & robots,
                     unsigned int robotIndex,
                     double timeStep,
                     const std::array<double, 3> & damper,
                     double velocityPercent = 1.0,
                     bool infTorque = false);
 
  inline tasks::qp::MotionConstr & motionConstr() noexcept
  {
    assert(backend_ == QPSolver::Backend::Tasks);
    return *static_cast<tasks::qp::MotionConstr *>(motion_constr_.get());
  }
 
  inline mc_tvm::DynamicFunction & dynamicFunction()
  {
    assert(backend_ == QPSolver::Backend::TVM);
    return *(static_cast<mc_tvm::DynamicFunctionPtr *>(motion_constr_.get())->get());
  }
 
  void addToSolverImpl(QPSolver & solver) override;
 
  void removeFromSolverImpl(QPSolver & solver) override;
 
  inline unsigned int robotIndex() const noexcept { return robotIndex_; }
 
protected:
  mc_rtc::void_ptr motion_constr_;
  unsigned int robotIndex_;
};
 
} // namespace mc_solver