inverse-dynamics-formulation-acc-force.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2017 CNRS, NYU, MPI Tübingen, UNITN
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_acc_force_hpp__
19 #define __invdyn_inverse_dynamics_formulation_acc_force_hpp__
20 
24 
25 #include <vector>
26 
27 namespace tsid
28 {
29 
31  {
32  public:
33  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34 
35  double time_start;
36  double time_end;
37  double fMax_start;
38  double fMax_end;
39  std::shared_ptr<ContactLevel> contactLevel;
40  };
41 
44  {
45  public:
46  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
47 
57 
58 
59  InverseDynamicsFormulationAccForce(const std::string & name,
60  RobotWrapper & robot,
61  bool verbose=false);
62 
63  Data & data() ;
64 
65  unsigned int nVar() const;
66  unsigned int nEq() const;
67  unsigned int nIn() const;
68 
69  bool addMotionTask(TaskMotion & task,
70  double weight,
71  unsigned int priorityLevel,
72  double transition_duration=0.0);
73 
74  bool addForceTask(TaskContactForce & task,
75  double weight,
76  unsigned int priorityLevel,
77  double transition_duration=0.0);
78 
79  bool addActuationTask(TaskActuation & task,
80  double weight,
81  unsigned int priorityLevel,
82  double transition_duration=0.0);
83 
84  bool updateTaskWeight(const std::string & task_name,
85  double weight);
86 
87  bool addRigidContact(ContactBase & contact, double force_regularization_weight,
88  double motion_weight=1.0, unsigned int motion_priority_level=0);
89 
90  TSID_DEPRECATED bool addRigidContact(ContactBase & contact);
91 
92  bool updateRigidContactWeights(const std::string & contact_name,
93  double force_regularization_weight,
94  double motion_weight=-1.0);
95 
96  bool removeTask(const std::string & taskName,
97  double transition_duration=0.0);
98 
99  bool removeRigidContact(const std::string & contactName,
100  double transition_duration=0.0);
101 
102  const HQPData & computeProblemData(double time,
103  ConstRefVector q,
104  ConstRefVector v);
105 
106  const Vector & getActuatorForces(const HQPOutput & sol);
107  const Vector & getAccelerations(const HQPOutput & sol);
108  const Vector & getContactForces(const HQPOutput & sol);
109  Vector getContactForces(const std::string & name, const HQPOutput & sol);
110  bool getContactForces(const std::string & name,
111  const HQPOutput & sol,
112  RefVector f);
113 
114  public:
115 
116  template<class TaskLevelPointer>
117  void addTask(TaskLevelPointer task,
118  double weight,
119  unsigned int priorityLevel);
120 
121  void resizeHqpData();
122 
123  bool removeFromHqpData(const std::string & name);
124 
125  bool decodeSolution(const HQPOutput & sol);
126 
129  std::vector<std::shared_ptr<TaskLevel> > m_taskMotions;
130  std::vector<std::shared_ptr<TaskLevelForce> > m_taskContactForces;
131  std::vector<std::shared_ptr<TaskLevel> > m_taskActuations;
132  std::vector<std::shared_ptr<ContactLevel> > m_contacts;
133  double m_t;
134  unsigned int m_k;
135  unsigned int m_v;
136  unsigned int m_u;
137  unsigned int m_eq;
138  unsigned int m_in;
140  std::shared_ptr<math::ConstraintEquality> m_baseDynamics;
141 
146 
147  std::vector<std::shared_ptr<ContactTransitionInfo> > m_contactTransitions;
148  };
149 
150 }
151 
152 #endif // ifndef __invdyn_inverse_dynamics_formulation_acc_force_hpp__
tsid::InverseDynamicsFormulationAccForce::Vector
math::Vector Vector
Definition: inverse-dynamics-formulation-acc-force.hpp:49
tsid::InverseDynamicsFormulationAccForce::resizeHqpData
void resizeHqpData()
Definition: inverse-dynamics-formulation-acc-force.cpp:72
tsid::InverseDynamicsFormulationAccForce::m_v
unsigned int m_v
number of contact-force variables
Definition: inverse-dynamics-formulation-acc-force.hpp:135
tsid::tasks::TaskBase
Base template of a Task. Each class is defined according to a constant model of a robot.
Definition: task-base.hpp:36
tsid::contacts::ContactBase
Base template of a Contact.
Definition: contact-base.hpp:34
tsid::InverseDynamicsFormulationAccForce::TaskContactForce
tasks::TaskContactForce TaskContactForce
Definition: inverse-dynamics-formulation-acc-force.hpp:54
tsid::InverseDynamicsFormulationAccForce::getActuatorForces
const Vector & getActuatorForces(const HQPOutput &sol)
Definition: inverse-dynamics-formulation-acc-force.cpp:475
Data
pinocchio::Data Data
Definition: inverse-dynamics-formulation-acc-force.cpp:29
tsid::InverseDynamicsFormulationAccForce::m_k
unsigned int m_k
time
Definition: inverse-dynamics-formulation-acc-force.hpp:134
tsid::math::Vector
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
tsid::tasks::TaskContactForce
Definition: task-contact-force.hpp:30
tsid::InverseDynamicsFormulationAccForce::m_contactTransitions
std::vector< std::shared_ptr< ContactTransitionInfo > > m_contactTransitions
Definition: inverse-dynamics-formulation-acc-force.hpp:147
tsid::InverseDynamicsFormulationAccForce::addForceTask
bool addForceTask(TaskContactForce &task, double weight, unsigned int priorityLevel, double transition_duration=0.0)
Definition: inverse-dynamics-formulation-acc-force.cpp:137
tsid::InverseDynamicsFormulationAccForce::updateTaskWeight
bool updateTaskWeight(const std::string &task_name, double weight)
Definition: inverse-dynamics-formulation-acc-force.cpp:199
tsid::InverseDynamicsFormulationAccForce::Matrix
math::Matrix Matrix
Definition: inverse-dynamics-formulation-acc-force.hpp:50
tsid::ContactTransitionInfo::time_end
double time_end
Definition: inverse-dynamics-formulation-acc-force.hpp:36
tsid::InverseDynamicsFormulationAccForce::updateRigidContactWeights
bool updateRigidContactWeights(const std::string &contact_name, double force_regularization_weight, double motion_weight=-1.0)
Update the weights associated to the specified contact.
Definition: inverse-dynamics-formulation-acc-force.cpp:255
constraint-equality.hpp
tsid::InverseDynamicsFormulationAccForce::computeProblemData
const HQPData & computeProblemData(double time, ConstRefVector q, ConstRefVector v)
Definition: inverse-dynamics-formulation-acc-force.cpp:289
tsid::math::Matrix
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > Matrix
Definition: fwd.hpp:38
tsid::InverseDynamicsFormulationAccForce::HQPOutput
solvers::HQPOutput HQPOutput
Definition: inverse-dynamics-formulation-acc-force.hpp:56
tsid::InverseDynamicsFormulationBase
Wrapper for a robot based on pinocchio.
Definition: inverse-dynamics-formulation-base.hpp:62
tsid::InverseDynamicsFormulationAccForce::addTask
void addTask(TaskLevelPointer task, double weight, unsigned int priorityLevel)
Definition: inverse-dynamics-formulation-acc-force.cpp:86
tsid::InverseDynamicsFormulationAccForce::m_data
Data m_data
Definition: inverse-dynamics-formulation-acc-force.hpp:127
tsid::InverseDynamicsFormulationAccForce::TaskMotion
tasks::TaskMotion TaskMotion
Definition: inverse-dynamics-formulation-acc-force.hpp:53
tsid::InverseDynamicsFormulationAccForce::m_dv
Vector m_dv
Definition: inverse-dynamics-formulation-acc-force.hpp:143
tsid::InverseDynamicsFormulationAccForce::m_tau
Vector m_tau
Definition: inverse-dynamics-formulation-acc-force.hpp:145
tsid::InverseDynamicsFormulationAccForce::m_taskContactForces
std::vector< std::shared_ptr< TaskLevelForce > > m_taskContactForces
Definition: inverse-dynamics-formulation-acc-force.hpp:130
tsid::solvers::HQPOutput
Definition: solver-HQP-output.hpp:32
tsid::InverseDynamicsFormulationBase::RefVector
math::RefVector RefVector
Definition: inverse-dynamics-formulation-base.hpp:69
tsid::InverseDynamicsFormulationAccForce::m_in
unsigned int m_in
number of equality constraints
Definition: inverse-dynamics-formulation-acc-force.hpp:138
tsid::InverseDynamicsFormulationAccForce::ConstRefVector
math::ConstRefVector ConstRefVector
Definition: inverse-dynamics-formulation-acc-force.hpp:51
tsid::InverseDynamicsFormulationAccForce::m_solutionDecoded
bool m_solutionDecoded
Definition: inverse-dynamics-formulation-acc-force.hpp:142
tsid::InverseDynamicsFormulationAccForce::addActuationTask
bool addActuationTask(TaskActuation &task, double weight, unsigned int priorityLevel, double transition_duration=0.0)
Definition: inverse-dynamics-formulation-acc-force.cpp:159
tsid::InverseDynamicsFormulationAccForce::m_hqpData
HQPData m_hqpData
Definition: inverse-dynamics-formulation-acc-force.hpp:128
tsid::InverseDynamicsFormulationAccForce::decodeSolution
bool decodeSolution(const HQPOutput &sol)
Definition: inverse-dynamics-formulation-acc-force.cpp:459
tsid::InverseDynamicsFormulationAccForce::m_Jc
Matrix m_Jc
number of inequality constraints
Definition: inverse-dynamics-formulation-acc-force.hpp:139
tsid::InverseDynamicsFormulationAccForce::Data
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef pinocchio::Data Data
Definition: inverse-dynamics-formulation-acc-force.hpp:48
tsid::InverseDynamicsFormulationAccForce::nIn
unsigned int nIn() const
Definition: inverse-dynamics-formulation-acc-force.cpp:66
tsid::InverseDynamicsFormulationAccForce::m_f
Vector m_f
Definition: inverse-dynamics-formulation-acc-force.hpp:144
tsid::ContactTransitionInfo
Definition: inverse-dynamics-formulation-acc-force.hpp:30
tsid::InverseDynamicsFormulationAccForce::m_taskActuations
std::vector< std::shared_ptr< TaskLevel > > m_taskActuations
Definition: inverse-dynamics-formulation-acc-force.hpp:131
tsid::tasks::TaskMotion
Definition: task-motion.hpp:28
tsid::math::ConstRefVector
const typedef Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
tsid::InverseDynamicsFormulationAccForce::nEq
unsigned int nEq() const
Definition: inverse-dynamics-formulation-acc-force.cpp:61
tsid::InverseDynamicsFormulationAccForce::removeFromHqpData
bool removeFromHqpData(const std::string &name)
Definition: inverse-dynamics-formulation-acc-force.cpp:643
tsid::ContactTransitionInfo::fMax_end
double fMax_end
max normal force at time time_start
Definition: inverse-dynamics-formulation-acc-force.hpp:38
tsid::InverseDynamicsFormulationAccForce::TaskBase
tasks::TaskBase TaskBase
Definition: inverse-dynamics-formulation-acc-force.hpp:52
tsid::InverseDynamicsFormulationBase::HQPData
solvers::HQPData HQPData
Definition: inverse-dynamics-formulation-base.hpp:76
inverse-dynamics-formulation-base.hpp
tsid::ContactTransitionInfo::fMax_start
double fMax_start
Definition: inverse-dynamics-formulation-acc-force.hpp:37
tsid::InverseDynamicsFormulationAccForce::getAccelerations
const Vector & getAccelerations(const HQPOutput &sol)
Definition: inverse-dynamics-formulation-acc-force.cpp:481
tsid::InverseDynamicsFormulationAccForce::m_taskMotions
std::vector< std::shared_ptr< TaskLevel > > m_taskMotions
Definition: inverse-dynamics-formulation-acc-force.hpp:129
tsid
Definition: constraint-bound.hpp:26
tsid::InverseDynamicsFormulationAccForce::removeTask
bool removeTask(const std::string &taskName, double transition_duration=0.0)
Definition: inverse-dynamics-formulation-acc-force.cpp:527
tsid::tasks::TaskActuation
Definition: task-actuation.hpp:27
tsid::robots::RobotWrapper
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:40
tsid::InverseDynamicsFormulationAccForce
Definition: inverse-dynamics-formulation-acc-force.hpp:42
tsid::InverseDynamicsFormulationAccForce::addRigidContact
bool addRigidContact(ContactBase &contact, double force_regularization_weight, double motion_weight=1.0, unsigned int motion_priority_level=0)
Add a rigid contact constraint to the model, introducing the associated reaction forces as problem va...
Definition: inverse-dynamics-formulation-acc-force.cpp:219
tsid::InverseDynamicsFormulationAccForce::nVar
unsigned int nVar() const
Definition: inverse-dynamics-formulation-acc-force.cpp:56
tsid::ContactTransitionInfo::time_start
EIGEN_MAKE_ALIGNED_OPERATOR_NEW double time_start
Definition: inverse-dynamics-formulation-acc-force.hpp:35
tsid::InverseDynamicsFormulationAccForce::TaskActuation
tasks::TaskActuation TaskActuation
Definition: inverse-dynamics-formulation-acc-force.hpp:55
tsid::InverseDynamicsFormulationAccForce::removeRigidContact
bool removeRigidContact(const std::string &contactName, double transition_duration=0.0)
Definition: inverse-dynamics-formulation-acc-force.cpp:579
tsid::InverseDynamicsFormulationAccForce::m_u
unsigned int m_u
number of acceleration variables
Definition: inverse-dynamics-formulation-acc-force.hpp:136
tsid::InverseDynamicsFormulationAccForce::addMotionTask
bool addMotionTask(TaskMotion &task, double weight, unsigned int priorityLevel, double transition_duration=0.0)
Definition: inverse-dynamics-formulation-acc-force.cpp:112
tsid::ContactTransitionInfo::contactLevel
std::shared_ptr< ContactLevel > contactLevel
max normal force at time time_end
Definition: inverse-dynamics-formulation-acc-force.hpp:39
contact-level.hpp
tsid::InverseDynamicsFormulationAccForce::InverseDynamicsFormulationAccForce
InverseDynamicsFormulationAccForce(const std::string &name, RobotWrapper &robot, bool verbose=false)
Definition: inverse-dynamics-formulation-acc-force.cpp:31
tsid::InverseDynamicsFormulationAccForce::m_contacts
std::vector< std::shared_ptr< ContactLevel > > m_contacts
Definition: inverse-dynamics-formulation-acc-force.hpp:132
tsid::InverseDynamicsFormulationAccForce::m_t
double m_t
Definition: inverse-dynamics-formulation-acc-force.hpp:133
tsid::InverseDynamicsFormulationAccForce::m_eq
unsigned int m_eq
number of unactuated DoFs
Definition: inverse-dynamics-formulation-acc-force.hpp:137
tsid::InverseDynamicsFormulationAccForce::data
Data & data()
Definition: inverse-dynamics-formulation-acc-force.cpp:51
tsid::InverseDynamicsFormulationAccForce::m_baseDynamics
std::shared_ptr< math::ConstraintEquality > m_baseDynamics
contact force Jacobian
Definition: inverse-dynamics-formulation-acc-force.hpp:140
tsid::InverseDynamicsFormulationAccForce::getContactForces
const Vector & getContactForces(const HQPOutput &sol)
Definition: inverse-dynamics-formulation-acc-force.cpp:487