Loading...
Searching...
No Matches
dynamic-pinocchio.h
Go to the documentation of this file.
1/*
2 * Copyright 2010,
3 * François Bleibel,
4 * Olivier Stasse,
5 *
6 * CNRS/AIST
7 *
8 */
9
10#ifndef __SOT_DYNAMIC_PINOCCHIO_H__
11#define __SOT_DYNAMIC_PINOCCHIO_H__
12
13/* --------------------------------------------------------------------- */
14/* --- INCLUDE --------------------------------------------------------- */
15/* --------------------------------------------------------------------- */
16
17/* STD */
18#include <map>
19#include <memory>
20#include <string>
21
22/* pinocchio */
23
24#include <pinocchio/fwd.hpp>
25
26/* SOT */
27#include <dynamic-graph/entity.h>
28#include <dynamic-graph/pool.h>
29#include <dynamic-graph/signal-ptr.h>
30#include <dynamic-graph/signal-time-dependent.h>
31
32#include <sot/core/exception-dynamic.hh>
33#include <sot/core/flags.hh>
34#include <sot/core/matrix-geometry.hh>
35/* Matrix */
36#include <dynamic-graph/linear-algebra.h>
37
39
40/* PINOCCHIO */
41#include <pinocchio/algorithm/frames.hpp>
42#include <pinocchio/algorithm/jacobian.hpp>
43#include <pinocchio/algorithm/rnea.hpp>
44#include <pinocchio/macros.hpp>
45#include <pinocchio/multibody/model.hpp>
46
47/* --------------------------------------------------------------------- */
48/* --- API ------------------------------------------------------------- */
49/* --------------------------------------------------------------------- */
50
51#if defined(WIN32)
52#if defined(dynamic_EXPORTS)
53#define SOTDYNAMIC_EXPORT __declspec(dllexport)
54#else
55#define SOTDYNAMIC_EXPORT __declspec(dllimport)
56#endif
57#else
58#define SOTDYNAMIC_EXPORT
59#endif
60
61namespace dynamicgraph {
62namespace sot {
63namespace dg = dynamicgraph;
64
65namespace command {
66class SetFile;
67class CreateOpPoint;
68} // namespace command
69 /* --------------------------------------------------------------------- */
70 /* --- CLASS ----------------------------------------------------------- */
71 /* --------------------------------------------------------------------- */
72
79class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {
80 friend class sot::command::SetFile;
81 friend class sot::command::CreateOpPoint;
82 // friend class sot::command::InitializeRobot;
83
84 public:
85 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
87
88 /* --- MODEL ATRIBUTES --- */
89 pinocchio::Model* m_model;
90 std::unique_ptr<pinocchio::Data> m_data;
91
92 /* --- MODEL ATRIBUTES --- */
93
94 public:
95 /* --- SIGNAL ACTIVATION --- */
96 dg::SignalTimeDependent<dg::Matrix, int>& createEndeffJacobianSignal(
97 const std::string& signame, const std::string&,
98 const bool isLocal = true);
99 dg::SignalTimeDependent<dg::Matrix, int>& createJacobianSignal(
100 const std::string& signame, const std::string&);
101 void destroyJacobianSignal(const std::string& signame);
102
103 dg::SignalTimeDependent<MatrixHomogeneous, int>& createPositionSignal(
104 const std::string&, const std::string&);
105 void destroyPositionSignal(const std::string& signame);
106
107 dg::SignalTimeDependent<dg::Vector, int>& createVelocitySignal(
108 const std::string&, const std::string&);
109 void destroyVelocitySignal(const std::string& signame);
110
111 dg::SignalTimeDependent<dg::Vector, int>& createAccelerationSignal(
112 const std::string&, const std::string&);
113 void destroyAccelerationSignal(const std::string& signame);
114
116 std::list<dg::SignalBase<int>*> genericSignalRefs;
117
118 public:
119 /* --- SIGNAL --- */
120 typedef int Dummy;
121 dg::SignalPtr<dg::Vector, int> jointPositionSIN;
122 dg::SignalPtr<dg::Vector, int> freeFlyerPositionSIN;
123 dg::SignalPtr<dg::Vector, int> jointVelocitySIN;
124 dg::SignalPtr<dg::Vector, int> freeFlyerVelocitySIN;
125 dg::SignalPtr<dg::Vector, int> jointAccelerationSIN;
126 dg::SignalPtr<dg::Vector, int> freeFlyerAccelerationSIN;
127
128 dg::SignalTimeDependent<dg::Vector, int> pinocchioPosSINTERN;
129 dg::SignalTimeDependent<dg::Vector, int> pinocchioVelSINTERN;
130 dg::SignalTimeDependent<dg::Vector, int> pinocchioAccSINTERN;
131
132 dg::SignalTimeDependent<Dummy, int> newtonEulerSINTERN;
133 dg::SignalTimeDependent<Dummy, int> jacobiansSINTERN;
134 dg::SignalTimeDependent<Dummy, int> forwardKinematicsSINTERN;
135 dg::SignalTimeDependent<Dummy, int> ccrbaSINTERN;
136
137 int& computeNewtonEuler(int& dummy, const int& time);
138 int& computeForwardKinematics(int& dummy, const int& time);
139 int& computeCcrba(int& dummy, const int& time);
140 int& computeJacobians(int& dummy, const int& time);
141
142 dg::SignalTimeDependent<dg::Vector, int> zmpSOUT;
143 dg::SignalTimeDependent<dg::Matrix, int> JcomSOUT;
144 dg::SignalTimeDependent<dg::Vector, int> comSOUT;
145 dg::SignalTimeDependent<dg::Matrix, int> inertiaSOUT;
146
147 dg::SignalTimeDependent<dg::Matrix, int>& jacobiansSOUT(
148 const std::string& name);
149 dg::SignalTimeDependent<MatrixHomogeneous, int>& positionsSOUT(
150 const std::string& name);
151 dg::SignalTimeDependent<dg::Vector, int>& velocitiesSOUT(
152 const std::string& name);
153 dg::SignalTimeDependent<dg::Vector, int>& accelerationsSOUT(
154 const std::string& name);
155
156 dg::SignalTimeDependent<double, int> footHeightSOUT;
157 dg::SignalTimeDependent<dg::Vector, int> upperJlSOUT;
158 dg::SignalTimeDependent<dg::Vector, int> lowerJlSOUT;
159 dg::SignalTimeDependent<dg::Vector, int> upperVlSOUT;
160 dg::SignalTimeDependent<dg::Vector, int> upperTlSOUT;
161
162 dg::Signal<dg::Vector, int> inertiaRotorSOUT;
163 dg::Signal<dg::Vector, int> gearRatioSOUT;
164 dg::SignalTimeDependent<dg::Matrix, int> inertiaRealSOUT;
165 dg::SignalTimeDependent<dg::Vector, int> MomentaSOUT;
166 dg::SignalTimeDependent<dg::Vector, int> AngularMomentumSOUT;
167 dg::SignalTimeDependent<dg::Vector, int> dynamicDriftSOUT;
168
169 public:
170 /* --- CONSTRUCTOR --- */
171 DynamicPinocchio(const std::string& name);
172 virtual ~DynamicPinocchio(void);
173
174 /* --- MODEL CREATION --- */
175
176 void displayModel() const {
177 assert(m_model);
178 std::cout << (*m_model) << std::endl;
179 };
180
181 void setModel(pinocchio::Model*);
182
184
188
189 pinocchio::Model* getModel() { return m_model; };
190
191 pinocchio::Data* getData() { return m_data.get(); };
192
193 /* --- GETTERS --- */
194
199 dg::Vector& getLowerPositionLimits(dg::Vector& res, const int&) const;
200
205 dg::Vector& getUpperPositionLimits(dg::Vector& res, const int&) const;
206
211 dg::Vector& getUpperVelocityLimits(dg::Vector& res, const int&) const;
212
217 dg::Vector& getMaxEffortLimits(dg::Vector& res, const int&) const;
218
219 // dg::Vector& getAnklePositionInFootFrame() const;
220
221 protected:
222 dg::Matrix& computeGenericJacobian(const bool isFrame, const int jointId,
223 dg::Matrix& res, const int& time);
224 dg::Matrix& computeGenericEndeffJacobian(const bool isFrame,
225 const bool isLocal,
226 const int jointId, dg::Matrix& res,
227 const int& time);
228 MatrixHomogeneous& computeGenericPosition(const bool isFrame,
229 const int jointId,
230 MatrixHomogeneous& res,
231 const int& time);
232 dg::Vector& computeGenericVelocity(const int jointId, dg::Vector& res,
233 const int& time);
234 dg::Vector& computeGenericAcceleration(const int jointId, dg::Vector& res,
235 const int& time);
236
237 dg::Vector& computeZmp(dg::Vector& res, const int& time);
238 dg::Vector& computeMomenta(dg::Vector& res, const int& time);
239 dg::Vector& computeAngularMomentum(dg::Vector& res, const int& time);
240 dg::Matrix& computeJcom(dg::Matrix& res, const int& time);
241 dg::Vector& computeCom(dg::Vector& res, const int& time);
242 dg::Matrix& computeInertia(dg::Matrix& res, const int& time);
243 dg::Matrix& computeInertiaReal(dg::Matrix& res, const int& time);
244 double& computeFootHeight(double& res, const int& time);
245
246 dg::Vector& computeTorqueDrift(dg::Vector& res, const int& time);
247
248 public: /* --- PARAMS --- */
249 void cmd_createOpPointSignals(const std::string& sig, const std::string& j);
250 void cmd_createJacobianWorldSignal(const std::string& sig,
251 const std::string& j);
252 void cmd_createJacobianEndEffectorSignal(const std::string& sig,
253 const std::string& j);
254 void cmd_createJacobianEndEffectorWorldSignal(const std::string& sig,
255 const std::string& j);
256 void cmd_createPositionSignal(const std::string& sig, const std::string& j);
257 void cmd_createVelocitySignal(const std::string& sig, const std::string& j);
258 void cmd_createAccelerationSignal(const std::string& sig,
259 const std::string& j);
260
261 private:
265 dg::Vector& getPinocchioPos(dg::Vector& q, const int& time);
266 dg::Vector& getPinocchioVel(dg::Vector& v, const int& time);
267 dg::Vector& getPinocchioAcc(dg::Vector& a, const int& time);
268
269 //\brief Index list for the first dof of (spherical joints)/ (spherical part
270 // of free-flyer joint).
271 std::vector<int> sphericalJoints;
272};
273
274// std::ostream& operator<<(std::ostream& os, const CjrlJoint& r);
275} /* namespace sot */
276} /* namespace dynamicgraph */
277
278#endif // #ifndef __SOT_DYNAMIC_PINOCCHIO_H__
This class provides an inverse dynamic model of the robot. More precisely it wraps the newton euler a...
Definition dynamic-pinocchio.h:79
MatrixHomogeneous & computeGenericPosition(const bool isFrame, const int jointId, MatrixHomogeneous &res, const int &time)
pinocchio::Model * getModel()
Definition dynamic-pinocchio.h:189
void displayModel() const
Definition dynamic-pinocchio.h:176
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DYNAMIC_GRAPH_ENTITY_DECL()
double & computeFootHeight(double &res, const int &time)
dg::SignalTimeDependent< dg::Matrix, int > & jacobiansSOUT(const std::string &name)
dg::Matrix & computeGenericJacobian(const bool isFrame, const int jointId, dg::Matrix &res, const int &time)
dg::SignalTimeDependent< double, int > footHeightSOUT
Definition dynamic-pinocchio.h:156
dg::Vector & computeTorqueDrift(dg::Vector &res, const int &time)
dg::SignalPtr< dg::Vector, int > jointPositionSIN
Definition dynamic-pinocchio.h:121
int & computeCcrba(int &dummy, const int &time)
dg::Vector & getUpperPositionLimits(dg::Vector &res, const int &) const
Get joint position upper limits.
std::unique_ptr< pinocchio::Data > m_data
Definition dynamic-pinocchio.h:90
dg::SignalTimeDependent< dg::Vector, int > pinocchioPosSINTERN
Definition dynamic-pinocchio.h:128
pinocchio::Model * m_model
Definition dynamic-pinocchio.h:89
void cmd_createVelocitySignal(const std::string &sig, const std::string &j)
dg::SignalTimeDependent< dg::Vector, int > zmpSOUT
Definition dynamic-pinocchio.h:142
dg::SignalTimeDependent< dg::Vector, int > lowerJlSOUT
Definition dynamic-pinocchio.h:158
void cmd_createOpPointSignals(const std::string &sig, const std::string &j)
int & computeForwardKinematics(int &dummy, const int &time)
dg::SignalTimeDependent< dg::Vector, int > upperJlSOUT
Definition dynamic-pinocchio.h:157
dg::SignalTimeDependent< dg::Vector, int > pinocchioAccSINTERN
Definition dynamic-pinocchio.h:130
dg::Vector & getMaxEffortLimits(dg::Vector &res, const int &) const
Get joint effort upper limits.
dg::SignalTimeDependent< dg::Vector, int > upperTlSOUT
Definition dynamic-pinocchio.h:160
dg::SignalTimeDependent< Dummy, int > jacobiansSINTERN
Definition dynamic-pinocchio.h:133
dg::Vector & computeGenericAcceleration(const int jointId, dg::Vector &res, const int &time)
void cmd_createJacobianEndEffectorSignal(const std::string &sig, const std::string &j)
void cmd_createAccelerationSignal(const std::string &sig, const std::string &j)
dg::SignalTimeDependent< MatrixHomogeneous, int > & createPositionSignal(const std::string &, const std::string &)
void destroyJacobianSignal(const std::string &signame)
dg::SignalTimeDependent< dg::Matrix, int > inertiaRealSOUT
Definition dynamic-pinocchio.h:164
pinocchio::Data * getData()
Definition dynamic-pinocchio.h:191
dg::Matrix & computeInertia(dg::Matrix &res, const int &time)
int & computeNewtonEuler(int &dummy, const int &time)
void setData(pinocchio::Data *) SOT_DYNAMIC_PINOCCHIO_DEPRECATED
dg::Matrix & computeGenericEndeffJacobian(const bool isFrame, const bool isLocal, const int jointId, dg::Matrix &res, const int &time)
dg::Vector & computeMomenta(dg::Vector &res, const int &time)
void destroyAccelerationSignal(const std::string &signame)
dg::SignalTimeDependent< dg::Matrix, int > & createJacobianSignal(const std::string &signame, const std::string &)
void destroyVelocitySignal(const std::string &signame)
dg::Matrix & computeInertiaReal(dg::Matrix &res, const int &time)
dg::SignalPtr< dg::Vector, int > jointVelocitySIN
Definition dynamic-pinocchio.h:123
dg::Vector & computeGenericVelocity(const int jointId, dg::Vector &res, const int &time)
dg::SignalTimeDependent< dg::Vector, int > & accelerationsSOUT(const std::string &name)
dg::SignalTimeDependent< dg::Matrix, int > JcomSOUT
Definition dynamic-pinocchio.h:143
dg::SignalTimeDependent< dg::Matrix, int > inertiaSOUT
Definition dynamic-pinocchio.h:145
void setModel(pinocchio::Model *)
dg::SignalTimeDependent< Dummy, int > ccrbaSINTERN
Definition dynamic-pinocchio.h:135
dg::SignalTimeDependent< dg::Vector, int > pinocchioVelSINTERN
Definition dynamic-pinocchio.h:129
dg::SignalTimeDependent< dg::Matrix, int > & createEndeffJacobianSignal(const std::string &signame, const std::string &, const bool isLocal=true)
dg::Signal< dg::Vector, int > gearRatioSOUT
Definition dynamic-pinocchio.h:163
dg::SignalTimeDependent< dg::Vector, int > & createVelocitySignal(const std::string &, const std::string &)
dg::Vector & computeCom(dg::Vector &res, const int &time)
dg::SignalTimeDependent< Dummy, int > forwardKinematicsSINTERN
Definition dynamic-pinocchio.h:134
dg::Signal< dg::Vector, int > inertiaRotorSOUT
Definition dynamic-pinocchio.h:162
DynamicPinocchio(const std::string &name)
dg::SignalPtr< dg::Vector, int > freeFlyerVelocitySIN
Definition dynamic-pinocchio.h:124
int & computeJacobians(int &dummy, const int &time)
dg::SignalTimeDependent< dg::Vector, int > AngularMomentumSOUT
Definition dynamic-pinocchio.h:166
void destroyPositionSignal(const std::string &signame)
dg::SignalTimeDependent< dg::Vector, int > MomentaSOUT
Definition dynamic-pinocchio.h:165
dg::SignalTimeDependent< dg::Vector, int > & velocitiesSOUT(const std::string &name)
dg::SignalTimeDependent< MatrixHomogeneous, int > & positionsSOUT(const std::string &name)
dg::Vector & computeZmp(dg::Vector &res, const int &time)
dg::Matrix & computeJcom(dg::Matrix &res, const int &time)
dg::SignalTimeDependent< Dummy, int > newtonEulerSINTERN
Definition dynamic-pinocchio.h:132
void cmd_createJacobianEndEffectorWorldSignal(const std::string &sig, const std::string &j)
dg::Vector & computeAngularMomentum(dg::Vector &res, const int &time)
dg::Vector & getUpperVelocityLimits(dg::Vector &res, const int &) const
Get joint velocity upper limits.
std::list< dg::SignalBase< int > * > genericSignalRefs
Definition dynamic-pinocchio.h:116
void cmd_createJacobianWorldSignal(const std::string &sig, const std::string &j)
dg::SignalPtr< dg::Vector, int > jointAccelerationSIN
Definition dynamic-pinocchio.h:125
void cmd_createPositionSignal(const std::string &sig, const std::string &j)
dg::SignalPtr< dg::Vector, int > freeFlyerAccelerationSIN
Definition dynamic-pinocchio.h:126
dg::SignalTimeDependent< dg::Vector, int > dynamicDriftSOUT
Definition dynamic-pinocchio.h:167
dg::SignalPtr< dg::Vector, int > freeFlyerPositionSIN
Definition dynamic-pinocchio.h:122
dg::SignalTimeDependent< dg::Vector, int > comSOUT
Definition dynamic-pinocchio.h:144
dg::SignalTimeDependent< dg::Vector, int > & createAccelerationSignal(const std::string &, const std::string &)
int Dummy
Definition dynamic-pinocchio.h:120
dg::Vector & getLowerPositionLimits(dg::Vector &res, const int &) const
Get joint position lower limits.
dg::SignalTimeDependent< dg::Vector, int > upperVlSOUT
Definition dynamic-pinocchio.h:159
#define SOT_DYNAMIC_PINOCCHIO_DEPRECATED
Definition deprecated.hh:37
#define SOTDYNAMIC_EXPORT
Definition dynamic-pinocchio.h:58
Definition angle-estimator.h:43