Loading...
Searching...
No Matches
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
27namespace 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
48 typedef pinocchio::Data Data;
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
75 double weight,
76 unsigned int priorityLevel,
77 double transition_duration=0.0);
78
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
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,
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__
Definition: inverse-dynamics-formulation-acc-force.hpp:31
EIGEN_MAKE_ALIGNED_OPERATOR_NEW double time_start
Definition: inverse-dynamics-formulation-acc-force.hpp:35
double fMax_start
Definition: inverse-dynamics-formulation-acc-force.hpp:37
double fMax_end
max normal force at time time_start
Definition: inverse-dynamics-formulation-acc-force.hpp:38
std::shared_ptr< ContactLevel > contactLevel
max normal force at time time_end
Definition: inverse-dynamics-formulation-acc-force.hpp:39
double time_end
Definition: inverse-dynamics-formulation-acc-force.hpp:36
Definition: inverse-dynamics-formulation-acc-force.hpp:44
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
unsigned int nVar() const
Definition: inverse-dynamics-formulation-acc-force.cpp:56
unsigned int nIn() const
Definition: inverse-dynamics-formulation-acc-force.cpp:66
void addTask(TaskLevelPointer task, double weight, unsigned int priorityLevel)
Definition: inverse-dynamics-formulation-acc-force.cpp:86
HQPData m_hqpData
Definition: inverse-dynamics-formulation-acc-force.hpp:128
bool addActuationTask(TaskActuation &task, double weight, unsigned int priorityLevel, double transition_duration=0.0)
Definition: inverse-dynamics-formulation-acc-force.cpp:159
const HQPData & computeProblemData(double time, ConstRefVector q, ConstRefVector v)
Definition: inverse-dynamics-formulation-acc-force.cpp:289
Data & data()
Definition: inverse-dynamics-formulation-acc-force.cpp:51
unsigned int m_u
number of acceleration variables
Definition: inverse-dynamics-formulation-acc-force.hpp:136
std::vector< std::shared_ptr< TaskLevelForce > > m_taskContactForces
Definition: inverse-dynamics-formulation-acc-force.hpp:130
Vector m_tau
Definition: inverse-dynamics-formulation-acc-force.hpp:145
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef pinocchio::Data Data
Definition: inverse-dynamics-formulation-acc-force.hpp:48
unsigned int m_in
number of equality constraints
Definition: inverse-dynamics-formulation-acc-force.hpp:138
bool updateTaskWeight(const std::string &task_name, double weight)
Definition: inverse-dynamics-formulation-acc-force.cpp:199
tasks::TaskBase TaskBase
Definition: inverse-dynamics-formulation-acc-force.hpp:52
unsigned int m_eq
number of unactuated DoFs
Definition: inverse-dynamics-formulation-acc-force.hpp:137
bool decodeSolution(const HQPOutput &sol)
Definition: inverse-dynamics-formulation-acc-force.cpp:459
void resizeHqpData()
Definition: inverse-dynamics-formulation-acc-force.cpp:72
unsigned int m_k
time
Definition: inverse-dynamics-formulation-acc-force.hpp:134
math::Matrix Matrix
Definition: inverse-dynamics-formulation-acc-force.hpp:50
std::vector< std::shared_ptr< TaskLevel > > m_taskMotions
Definition: inverse-dynamics-formulation-acc-force.hpp:129
tasks::TaskContactForce TaskContactForce
Definition: inverse-dynamics-formulation-acc-force.hpp:54
math::Vector Vector
Definition: inverse-dynamics-formulation-acc-force.hpp:49
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
const Vector & getContactForces(const HQPOutput &sol)
Definition: inverse-dynamics-formulation-acc-force.cpp:487
tasks::TaskMotion TaskMotion
Definition: inverse-dynamics-formulation-acc-force.hpp:53
math::ConstRefVector ConstRefVector
Definition: inverse-dynamics-formulation-acc-force.hpp:51
std::shared_ptr< math::ConstraintEquality > m_baseDynamics
contact force Jacobian
Definition: inverse-dynamics-formulation-acc-force.hpp:140
std::vector< std::shared_ptr< TaskLevel > > m_taskActuations
Definition: inverse-dynamics-formulation-acc-force.hpp:131
Vector m_f
Definition: inverse-dynamics-formulation-acc-force.hpp:144
bool removeTask(const std::string &taskName, double transition_duration=0.0)
Definition: inverse-dynamics-formulation-acc-force.cpp:527
Matrix m_Jc
number of inequality constraints
Definition: inverse-dynamics-formulation-acc-force.hpp:139
bool m_solutionDecoded
Definition: inverse-dynamics-formulation-acc-force.hpp:142
Data m_data
Definition: inverse-dynamics-formulation-acc-force.hpp:127
bool removeFromHqpData(const std::string &name)
Definition: inverse-dynamics-formulation-acc-force.cpp:643
solvers::HQPOutput HQPOutput
Definition: inverse-dynamics-formulation-acc-force.hpp:56
std::vector< std::shared_ptr< ContactLevel > > m_contacts
Definition: inverse-dynamics-formulation-acc-force.hpp:132
Vector m_dv
Definition: inverse-dynamics-formulation-acc-force.hpp:143
unsigned int m_v
number of contact-force variables
Definition: inverse-dynamics-formulation-acc-force.hpp:135
const Vector & getActuatorForces(const HQPOutput &sol)
Definition: inverse-dynamics-formulation-acc-force.cpp:475
unsigned int nEq() const
Definition: inverse-dynamics-formulation-acc-force.cpp:61
bool addForceTask(TaskContactForce &task, double weight, unsigned int priorityLevel, double transition_duration=0.0)
Definition: inverse-dynamics-formulation-acc-force.cpp:137
double m_t
Definition: inverse-dynamics-formulation-acc-force.hpp:133
bool addMotionTask(TaskMotion &task, double weight, unsigned int priorityLevel, double transition_duration=0.0)
Definition: inverse-dynamics-formulation-acc-force.cpp:112
const Vector & getAccelerations(const HQPOutput &sol)
Definition: inverse-dynamics-formulation-acc-force.cpp:481
std::vector< std::shared_ptr< ContactTransitionInfo > > m_contactTransitions
Definition: inverse-dynamics-formulation-acc-force.hpp:147
bool removeRigidContact(const std::string &contactName, double transition_duration=0.0)
Definition: inverse-dynamics-formulation-acc-force.cpp:579
tasks::TaskActuation TaskActuation
Definition: inverse-dynamics-formulation-acc-force.hpp:55
Wrapper for a robot based on pinocchio.
Definition: inverse-dynamics-formulation-base.hpp:63
math::RefVector RefVector
Definition: inverse-dynamics-formulation-base.hpp:69
solvers::HQPData HQPData
Definition: inverse-dynamics-formulation-base.hpp:76
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
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic > Matrix
Definition: fwd.hpp:38
const Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
Definition: constraint-bound.hpp:27