inverse-dynamics-formulation-base.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2017 CNRS
3 //
4 // This file is part of tsid
5 // tsid is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 // tsid is distributed in the hope that it will be
10 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 // General Lesser Public License for more details. You should have
13 // received a copy of the GNU Lesser General Public License along with
14 // tsid If not, see
15 // <http://www.gnu.org/licenses/>.
16 //
17 
18 #ifndef __invdyn_inverse_dynamics_formulation_base_hpp__
19 #define __invdyn_inverse_dynamics_formulation_base_hpp__
20 
21 #include "tsid/deprecated.hh"
22 #include "tsid/math/fwd.hpp"
29 
30 #include <string>
31 
32 namespace tsid
33 {
34 
35  struct TaskLevel
36  {
37  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38 
40  std::shared_ptr<math::ConstraintBase> constraint;
41  unsigned int priority;
42 
44  unsigned int priority);
45  };
46 
48  {
49  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
50 
52  std::shared_ptr<math::ConstraintBase> constraint;
53  unsigned int priority;
54 
56  unsigned int priority);
57  };
58 
63  {
64  public:
65  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66 
79 
80 
81  InverseDynamicsFormulationBase(const std::string & name,
82  RobotWrapper & robot,
83  bool verbose=false);
84 
85  virtual Data & data() = 0;
86 
87  virtual unsigned int nVar() const = 0;
88  virtual unsigned int nEq() const = 0;
89  virtual unsigned int nIn() const = 0;
90 
91  virtual bool addMotionTask(TaskMotion & task,
92  double weight,
93  unsigned int priorityLevel,
94  double transition_duration=0.0) = 0;
95 
96  virtual bool addForceTask(TaskContactForce & task,
97  double weight,
98  unsigned int priorityLevel,
99  double transition_duration=0.0) = 0;
100 
101  virtual bool addActuationTask(TaskActuation & task,
102  double weight,
103  unsigned int priorityLevel,
104  double transition_duration=0.0) = 0;
105 
106  virtual bool updateTaskWeight(const std::string & task_name,
107  double weight) = 0;
108 
118  virtual bool addRigidContact(ContactBase & contact, double force_regularization_weight,
119  double motion_weight=1.0, unsigned int motion_priority_level=0) = 0;
120 
121  TSID_DEPRECATED virtual bool addRigidContact(ContactBase & contact);
122 
130  virtual bool updateRigidContactWeights(const std::string & contact_name,
131  double force_regularization_weight,
132  double motion_weight=-1.0) = 0;
133 
134  virtual bool removeTask(const std::string & taskName,
135  double transition_duration=0.0) = 0;
136 
137  virtual bool removeRigidContact(const std::string & contactName,
138  double transition_duration=0.0) = 0;
139 
140  virtual const HQPData & computeProblemData(double time,
141  ConstRefVector q,
142  ConstRefVector v) = 0;
143 
144  virtual const Vector & getActuatorForces(const HQPOutput & sol) = 0;
145  virtual const Vector & getAccelerations(const HQPOutput & sol) = 0;
146  virtual const Vector & getContactForces(const HQPOutput & sol) = 0;
147  virtual bool getContactForces(const std::string & name,
148  const HQPOutput & sol,
149  RefVector f) = 0;
150 
151  protected:
152  std::string m_name;
154  bool m_verbose;
155  };
156 
157 }
158 
159 #endif // ifndef __invdyn_inverse_dynamics_formulation_base_hpp__
Wrapper for a robot based on pinocchio.
Definition: inverse-dynamics-formulation-base.hpp:63
tasks::TaskContactForce TaskContactForce
Definition: inverse-dynamics-formulation-base.hpp:72
InverseDynamicsFormulationBase(const std::string &name, RobotWrapper &robot, bool verbose=false)
Definition: inverse-dynamics-formulation-base.cpp:36
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef pinocchio::Data Data
Definition: inverse-dynamics-formulation-base.hpp:67
virtual const Vector & getActuatorForces(const HQPOutput &sol)=0
math::RefVector RefVector
Definition: inverse-dynamics-formulation-base.hpp:69
bool m_verbose
Definition: inverse-dynamics-formulation-base.hpp:154
tasks::TaskActuation TaskActuation
Definition: inverse-dynamics-formulation-base.hpp:73
virtual bool addMotionTask(TaskMotion &task, double weight, unsigned int priorityLevel, double transition_duration=0.0)=0
virtual bool updateRigidContactWeights(const std::string &contact_name, double force_regularization_weight, double motion_weight=-1.0)=0
Update the weights associated to the specified contact.
solvers::HQPOutput HQPOutput
Definition: inverse-dynamics-formulation-base.hpp:77
solvers::HQPData HQPData
Definition: inverse-dynamics-formulation-base.hpp:76
virtual unsigned int nIn() const =0
RobotWrapper m_robot
Definition: inverse-dynamics-formulation-base.hpp:153
virtual bool removeTask(const std::string &taskName, double transition_duration=0.0)=0
robots::RobotWrapper RobotWrapper
Definition: inverse-dynamics-formulation-base.hpp:78
virtual bool removeRigidContact(const std::string &contactName, double transition_duration=0.0)=0
virtual unsigned int nEq() const =0
tasks::TaskBase TaskBase
Definition: inverse-dynamics-formulation-base.hpp:74
virtual bool getContactForces(const std::string &name, const HQPOutput &sol, RefVector f)=0
virtual const Vector & getAccelerations(const HQPOutput &sol)=0
virtual const Vector & getContactForces(const HQPOutput &sol)=0
virtual const HQPData & computeProblemData(double time, ConstRefVector q, ConstRefVector v)=0
std::string m_name
Definition: inverse-dynamics-formulation-base.hpp:152
tasks::TaskMotion TaskMotion
Definition: inverse-dynamics-formulation-base.hpp:71
math::Vector Vector
Definition: inverse-dynamics-formulation-base.hpp:68
virtual unsigned int nVar() const =0
virtual bool updateTaskWeight(const std::string &task_name, double weight)=0
virtual bool addRigidContact(ContactBase &contact, double force_regularization_weight, double motion_weight=1.0, unsigned int motion_priority_level=0)=0
Add a rigid contact constraint to the model, introducing the associated reaction forces as problem va...
virtual bool addActuationTask(TaskActuation &task, double weight, unsigned int priorityLevel, double transition_duration=0.0)=0
virtual bool addForceTask(TaskContactForce &task, double weight, unsigned int priorityLevel, double transition_duration=0.0)=0
math::ConstRefVector ConstRefVector
Definition: inverse-dynamics-formulation-base.hpp:70
contacts::ContactBase ContactBase
Definition: inverse-dynamics-formulation-base.hpp:75
Base template of a Contact.
Definition: contact-base.hpp:35
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:41
Definition: solver-HQP-output.hpp:33
Definition: task-actuation.hpp:28
Base template of a Task. Each class is defined according to a constant model of a robot.
Definition: task-base.hpp:37
Definition: task-contact-force.hpp:31
Definition: task-motion.hpp:29
#define TSID_DEPRECATED
Definition: deprecated.hh:37
pinocchio::Data Data
Definition: inverse-dynamics-formulation-acc-force.cpp:29
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
const Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
Eigen::Ref< Vector > RefVector
Definition: fwd.hpp:49
pinocchio::container::aligned_vector< ConstraintLevel > HQPData
Definition: fwd.hpp:91
Definition: constraint-bound.hpp:27
Definition: inverse-dynamics-formulation-base.hpp:48
TaskLevelForce(tasks::TaskContactForce &task, unsigned int priority)
Definition: inverse-dynamics-formulation-base.cpp:30
unsigned int priority
Definition: inverse-dynamics-formulation-base.hpp:53
std::shared_ptr< math::ConstraintBase > constraint
Definition: inverse-dynamics-formulation-base.hpp:52
EIGEN_MAKE_ALIGNED_OPERATOR_NEW tasks::TaskContactForce & task
Definition: inverse-dynamics-formulation-base.hpp:51
Definition: inverse-dynamics-formulation-base.hpp:36
unsigned int priority
Definition: inverse-dynamics-formulation-base.hpp:41
EIGEN_MAKE_ALIGNED_OPERATOR_NEW tasks::TaskBase & task
Definition: inverse-dynamics-formulation-base.hpp:39
std::shared_ptr< math::ConstraintBase > constraint
Definition: inverse-dynamics-formulation-base.hpp:40
TaskLevel(tasks::TaskBase &task, unsigned int priority)
Definition: inverse-dynamics-formulation-base.cpp:24