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 
127  Data m_data;
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;
139  Matrix m_Jc;
140  std::shared_ptr<math::ConstraintEquality> m_baseDynamics;
141 
143  Vector m_dv;
144  Vector m_f;
145  Vector m_tau;
146 
147  std::vector<std::shared_ptr<ContactTransitionInfo> > m_contactTransitions;
148  };
149 
150 }
151 
152 #endif // ifndef __invdyn_inverse_dynamics_formulation_acc_force_hpp__
Definition: solver-HQP-output.hpp:32
Base template of a Task. Each class is defined according to a constant model of a robot...
Definition: task-base.hpp:36
HQPData m_hqpData
Definition: inverse-dynamics-formulation-acc-force.hpp:128
Definition: inverse-dynamics-formulation-acc-force.hpp:42
Data m_data
Definition: inverse-dynamics-formulation-acc-force.hpp:127
Definition: task-contact-force.hpp:30
tasks::TaskBase TaskBase
Definition: inverse-dynamics-formulation-acc-force.hpp:52
tasks::TaskMotion TaskMotion
Definition: inverse-dynamics-formulation-acc-force.hpp:53
tasks::TaskContactForce TaskContactForce
Definition: inverse-dynamics-formulation-acc-force.hpp:54
double time_end
Definition: inverse-dynamics-formulation-acc-force.hpp:36
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef pinocchio::Data Data
Definition: inverse-dynamics-formulation-acc-force.hpp:48
math::RefVector RefVector
Definition: inverse-dynamics-formulation-base.hpp:69
std::vector< std::shared_ptr< ContactLevel > > m_contacts
Definition: inverse-dynamics-formulation-acc-force.hpp:132
unsigned int m_eq
number of unactuated DoFs
Definition: inverse-dynamics-formulation-acc-force.hpp:137
bool m_solutionDecoded
Definition: inverse-dynamics-formulation-acc-force.hpp:142
Base template of a Contact.
Definition: contact-base.hpp:34
Definition: task-motion.hpp:28
Vector m_f
Definition: inverse-dynamics-formulation-acc-force.hpp:144
std::shared_ptr< math::ConstraintEquality > m_baseDynamics
contact force Jacobian
Definition: inverse-dynamics-formulation-acc-force.hpp:140
Definition: task-actuation.hpp:27
Matrix m_Jc
number of inequality constraints
Definition: inverse-dynamics-formulation-acc-force.hpp:139
unsigned int m_u
number of acceleration variables
Definition: inverse-dynamics-formulation-acc-force.hpp:136
solvers::HQPOutput HQPOutput
Definition: inverse-dynamics-formulation-acc-force.hpp:56
unsigned int m_in
number of equality constraints
Definition: inverse-dynamics-formulation-acc-force.hpp:138
unsigned int m_v
number of contact-force variables
Definition: inverse-dynamics-formulation-acc-force.hpp:135
solvers::HQPData HQPData
Definition: inverse-dynamics-formulation-base.hpp:76
double m_t
Definition: inverse-dynamics-formulation-acc-force.hpp:133
const Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
EIGEN_MAKE_ALIGNED_OPERATOR_NEW double time_start
Definition: inverse-dynamics-formulation-acc-force.hpp:35
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:40
Vector m_dv
Definition: inverse-dynamics-formulation-acc-force.hpp:143
double fMax_start
Definition: inverse-dynamics-formulation-acc-force.hpp:37
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > Matrix
Definition: fwd.hpp:38
std::shared_ptr< ContactLevel > contactLevel
max normal force at time time_end
Definition: inverse-dynamics-formulation-acc-force.hpp:39
double fMax_end
max normal force at time time_start
Definition: inverse-dynamics-formulation-acc-force.hpp:38
std::vector< std::shared_ptr< TaskLevelForce > > m_taskContactForces
Definition: inverse-dynamics-formulation-acc-force.hpp:130
std::vector< std::shared_ptr< TaskLevel > > m_taskActuations
Definition: inverse-dynamics-formulation-acc-force.hpp:131
Vector m_tau
Definition: inverse-dynamics-formulation-acc-force.hpp:145
std::vector< std::shared_ptr< TaskLevel > > m_taskMotions
Definition: inverse-dynamics-formulation-acc-force.hpp:129
std::vector< std::shared_ptr< ContactTransitionInfo > > m_contactTransitions
Definition: inverse-dynamics-formulation-acc-force.hpp:147
Definition: constraint-bound.hpp:26
math::ConstRefVector ConstRefVector
Definition: inverse-dynamics-formulation-acc-force.hpp:51
Definition: inverse-dynamics-formulation-acc-force.hpp:30
tasks::TaskActuation TaskActuation
Definition: inverse-dynamics-formulation-acc-force.hpp:55
math::Vector Vector
Definition: inverse-dynamics-formulation-acc-force.hpp:49
Wrapper for a robot based on pinocchio.
Definition: inverse-dynamics-formulation-base.hpp:62
unsigned int m_k
time
Definition: inverse-dynamics-formulation-acc-force.hpp:134
pinocchio::Data Data
Definition: inverse-dynamics-formulation-acc-force.cpp:29
math::Matrix Matrix
Definition: inverse-dynamics-formulation-acc-force.hpp:50