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 <string>
19 #include <map>
20 #include <memory>
21 
22 /* SOT */
23 #include <pinocchio/fwd.hpp>
24 #include <sot/core/flags.hh>
25 #include <dynamic-graph/entity.h>
26 #include <dynamic-graph/pool.h>
27 #include <dynamic-graph/signal-ptr.h>
28 #include <dynamic-graph/signal-time-dependent.h>
29 #include <sot/core/exception-dynamic.hh>
30 #include <sot/core/matrix-geometry.hh>
31 /* Matrix */
32 #include <dynamic-graph/linear-algebra.h>
33 #include <sot/dynamic-pinocchio/deprecated.hh>
34 
35 /* PINOCCHIO */
36 #include <pinocchio/macros.hpp>
37 #include <pinocchio/multibody/model.hpp>
38 #include <pinocchio/algorithm/rnea.hpp>
39 #include <pinocchio/algorithm/jacobian.hpp>
40 #include <pinocchio/algorithm/frames.hpp>
41 
42 /* --------------------------------------------------------------------- */
43 /* --- API ------------------------------------------------------------- */
44 /* --------------------------------------------------------------------- */
45 
46 #if defined(WIN32)
47 #if defined(dynamic_EXPORTS)
48 #define SOTDYNAMIC_EXPORT __declspec(dllexport)
49 #else
50 #define SOTDYNAMIC_EXPORT __declspec(dllimport)
51 #endif
52 #else
53 #define SOTDYNAMIC_EXPORT
54 #endif
55 
56 namespace dynamicgraph {
57 namespace sot {
58 namespace dg = dynamicgraph;
59 
60 namespace command {
61 class SetFile;
62 class CreateOpPoint;
63 } // namespace command
64  /* --------------------------------------------------------------------- */
65  /* --- CLASS ----------------------------------------------------------- */
66  /* --------------------------------------------------------------------- */
67 
74 class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {
75  friend class sot::command::SetFile;
76  friend class sot::command::CreateOpPoint;
77  // friend class sot::command::InitializeRobot;
78 
79  public:
80  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
81  DYNAMIC_GRAPH_ENTITY_DECL();
82 
83  /* --- MODEL ATRIBUTES --- */
84  pinocchio::Model* m_model;
85  std::unique_ptr<pinocchio::Data> m_data;
86 
87  /* --- MODEL ATRIBUTES --- */
88 
89  public:
90  /* --- SIGNAL ACTIVATION --- */
91  dg::SignalTimeDependent<dg::Matrix, int>& createEndeffJacobianSignal(const std::string& signame, const std::string&,
92  const bool isLocal = true);
93  dg::SignalTimeDependent<dg::Matrix, int>& createJacobianSignal(const std::string& signame, const std::string&);
94  void destroyJacobianSignal(const std::string& signame);
95 
96  dg::SignalTimeDependent<MatrixHomogeneous, int>& createPositionSignal(const std::string&, const std::string&);
97  void destroyPositionSignal(const std::string& signame);
98 
99  dg::SignalTimeDependent<dg::Vector, int>& createVelocitySignal(const std::string&, const std::string&);
100  void destroyVelocitySignal(const std::string& signame);
101 
102  dg::SignalTimeDependent<dg::Vector, int>& createAccelerationSignal(const std::string&, const std::string&);
103  void destroyAccelerationSignal(const std::string& signame);
104 
106  std::list<dg::SignalBase<int>*> genericSignalRefs;
107 
108  public:
109  /* --- SIGNAL --- */
110  typedef int Dummy;
111  dg::SignalPtr<dg::Vector, int> jointPositionSIN;
112  dg::SignalPtr<dg::Vector, int> freeFlyerPositionSIN;
113  dg::SignalPtr<dg::Vector, int> jointVelocitySIN;
114  dg::SignalPtr<dg::Vector, int> freeFlyerVelocitySIN;
115  dg::SignalPtr<dg::Vector, int> jointAccelerationSIN;
116  dg::SignalPtr<dg::Vector, int> freeFlyerAccelerationSIN;
117 
118  dg::SignalTimeDependent<dg::Vector, int> pinocchioPosSINTERN;
119  dg::SignalTimeDependent<dg::Vector, int> pinocchioVelSINTERN;
120  dg::SignalTimeDependent<dg::Vector, int> pinocchioAccSINTERN;
121 
122  dg::SignalTimeDependent<Dummy, int> newtonEulerSINTERN;
123  dg::SignalTimeDependent<Dummy, int> jacobiansSINTERN;
124  dg::SignalTimeDependent<Dummy, int> forwardKinematicsSINTERN;
125  dg::SignalTimeDependent<Dummy, int> ccrbaSINTERN;
126 
127  int& computeNewtonEuler(int& dummy, const int& time);
128  int& computeForwardKinematics(int& dummy, const int& time);
129  int& computeCcrba(int& dummy, const int& time);
130  int& computeJacobians(int& dummy, const int& time);
131 
132  dg::SignalTimeDependent<dg::Vector, int> zmpSOUT;
133  dg::SignalTimeDependent<dg::Matrix, int> JcomSOUT;
134  dg::SignalTimeDependent<dg::Vector, int> comSOUT;
135  dg::SignalTimeDependent<dg::Matrix, int> inertiaSOUT;
136 
137  dg::SignalTimeDependent<dg::Matrix, int>& jacobiansSOUT(const std::string& name);
138  dg::SignalTimeDependent<MatrixHomogeneous, int>& positionsSOUT(const std::string& name);
139  dg::SignalTimeDependent<dg::Vector, int>& velocitiesSOUT(const std::string& name);
140  dg::SignalTimeDependent<dg::Vector, int>& accelerationsSOUT(const std::string& name);
141 
142  dg::SignalTimeDependent<double, int> footHeightSOUT;
143  dg::SignalTimeDependent<dg::Vector, int> upperJlSOUT;
144  dg::SignalTimeDependent<dg::Vector, int> lowerJlSOUT;
145  dg::SignalTimeDependent<dg::Vector, int> upperVlSOUT;
146  dg::SignalTimeDependent<dg::Vector, int> upperTlSOUT;
147 
148  dg::Signal<dg::Vector, int> inertiaRotorSOUT;
149  dg::Signal<dg::Vector, int> gearRatioSOUT;
150  dg::SignalTimeDependent<dg::Matrix, int> inertiaRealSOUT;
151  dg::SignalTimeDependent<dg::Vector, int> MomentaSOUT;
152  dg::SignalTimeDependent<dg::Vector, int> AngularMomentumSOUT;
153  dg::SignalTimeDependent<dg::Vector, int> dynamicDriftSOUT;
154 
155  public:
156  /* --- CONSTRUCTOR --- */
157  DynamicPinocchio(const std::string& name);
158  virtual ~DynamicPinocchio(void);
159 
160  /* --- MODEL CREATION --- */
161 
162  void displayModel() const {
163  assert(m_model);
164  std::cout << (*m_model) << std::endl;
165  };
166 
167  void setModel(pinocchio::Model*);
168 
169  void createData();
170 
173  void setData(pinocchio::Data*) SOT_DYNAMIC_PINOCCHIO_DEPRECATED;
174 
175  pinocchio::Model* getModel() { return m_model; };
176 
177  pinocchio::Data* getData() { return m_data.get(); };
178 
179  /* --- GETTERS --- */
180 
185  dg::Vector& getLowerPositionLimits(dg::Vector& res, const int&) const;
186 
191  dg::Vector& getUpperPositionLimits(dg::Vector& res, const int&) const;
192 
197  dg::Vector& getUpperVelocityLimits(dg::Vector& res, const int&) const;
198 
203  dg::Vector& getMaxEffortLimits(dg::Vector& res, const int&) const;
204 
205  // dg::Vector& getAnklePositionInFootFrame() const;
206 
207  protected:
208  dg::Matrix& computeGenericJacobian(const bool isFrame, const int jointId, dg::Matrix& res, const int& time);
209  dg::Matrix& computeGenericEndeffJacobian(const bool isFrame, const bool isLocal, const int jointId, dg::Matrix& res,
210  const int& time);
211  MatrixHomogeneous& computeGenericPosition(const bool isFrame, const int jointId, MatrixHomogeneous& res,
212  const int& time);
213  dg::Vector& computeGenericVelocity(const int jointId, dg::Vector& res, const int& time);
214  dg::Vector& computeGenericAcceleration(const int jointId, dg::Vector& res, const int& time);
215 
216  dg::Vector& computeZmp(dg::Vector& res, const int& time);
217  dg::Vector& computeMomenta(dg::Vector& res, const int& time);
218  dg::Vector& computeAngularMomentum(dg::Vector& res, const int& time);
219  dg::Matrix& computeJcom(dg::Matrix& res, const int& time);
220  dg::Vector& computeCom(dg::Vector& res, const int& time);
221  dg::Matrix& computeInertia(dg::Matrix& res, const int& time);
222  dg::Matrix& computeInertiaReal(dg::Matrix& res, const int& time);
223  double& computeFootHeight(double& res, const int& time);
224 
225  dg::Vector& computeTorqueDrift(dg::Vector& res, const int& time);
226 
227  public: /* --- PARAMS --- */
228  void cmd_createOpPointSignals(const std::string& sig, const std::string& j);
229  void cmd_createJacobianWorldSignal(const std::string& sig, const std::string& j);
230  void cmd_createJacobianEndEffectorSignal(const std::string& sig, const std::string& j);
231  void cmd_createJacobianEndEffectorWorldSignal(const std::string& sig, const std::string& j);
232  void cmd_createPositionSignal(const std::string& sig, const std::string& j);
233  void cmd_createVelocitySignal(const std::string& sig, const std::string& j);
234  void cmd_createAccelerationSignal(const std::string& sig, const std::string& j);
235 
236  private:
239  dg::Vector& getPinocchioPos(dg::Vector& q, const int& time);
240  dg::Vector& getPinocchioVel(dg::Vector& v, const int& time);
241  dg::Vector& getPinocchioAcc(dg::Vector& a, const int& time);
242 
243  //\brief Index list for the first dof of (spherical joints)/ (spherical part of free-flyer joint).
244  std::vector<int> sphericalJoints;
245 };
246 
247 // std::ostream& operator<<(std::ostream& os, const CjrlJoint& r);
248 } /* namespace sot */
249 } /* namespace dynamicgraph */
250 
251 #endif // #ifndef __SOT_DYNAMIC_PINOCCHIO_H__
dg::SignalPtr< dg::Vector, int > freeFlyerVelocitySIN
Definition: dynamic-pinocchio.h:114
dg::SignalTimeDependent< Dummy, int > jacobiansSINTERN
Definition: dynamic-pinocchio.h:123
dg::SignalTimeDependent< Dummy, int > newtonEulerSINTERN
Definition: dynamic-pinocchio.h:122
dg::SignalTimeDependent< dg::Matrix, int > JcomSOUT
Definition: dynamic-pinocchio.h:133
pinocchio::Model * m_model
Definition: dynamic-pinocchio.h:84
dg::Signal< dg::Vector, int > inertiaRotorSOUT
Definition: dynamic-pinocchio.h:148
dg::SignalTimeDependent< dg::Vector, int > comSOUT
Definition: dynamic-pinocchio.h:134
dg::SignalPtr< dg::Vector, int > freeFlyerPositionSIN
Definition: dynamic-pinocchio.h:112
std::unique_ptr< pinocchio::Data > m_data
Definition: dynamic-pinocchio.h:85
dg::SignalPtr< dg::Vector, int > jointVelocitySIN
Definition: dynamic-pinocchio.h:113
dg::SignalTimeDependent< dg::Matrix, int > inertiaSOUT
Definition: dynamic-pinocchio.h:135
dg::SignalTimeDependent< dg::Vector, int > MomentaSOUT
Definition: dynamic-pinocchio.h:151
This class provides an inverse dynamic model of the robot. More precisely it wraps the newton euler a...
Definition: dynamic-pinocchio.h:74
dg::SignalTimeDependent< dg::Vector, int > pinocchioAccSINTERN
Definition: dynamic-pinocchio.h:120
pinocchio::Data * getData()
Definition: dynamic-pinocchio.h:177
dg::SignalTimeDependent< dg::Vector, int > dynamicDriftSOUT
Definition: dynamic-pinocchio.h:153
dg::SignalTimeDependent< Dummy, int > ccrbaSINTERN
Definition: dynamic-pinocchio.h:125
dg::SignalPtr< dg::Vector, int > jointAccelerationSIN
Definition: dynamic-pinocchio.h:115
dg::SignalTimeDependent< dg::Vector, int > lowerJlSOUT
Definition: dynamic-pinocchio.h:144
dg::Signal< dg::Vector, int > gearRatioSOUT
Definition: dynamic-pinocchio.h:149
#define SOTDYNAMIC_EXPORT
Definition: dynamic-pinocchio.h:53
dg::SignalTimeDependent< double, int > footHeightSOUT
Definition: dynamic-pinocchio.h:142
dg::SignalTimeDependent< dg::Vector, int > pinocchioVelSINTERN
Definition: dynamic-pinocchio.h:119
int Dummy
Definition: dynamic-pinocchio.h:110
pinocchio::Model * getModel()
Definition: dynamic-pinocchio.h:175
dg::SignalTimeDependent< dg::Vector, int > AngularMomentumSOUT
Definition: dynamic-pinocchio.h:152
std::list< dg::SignalBase< int > * > genericSignalRefs
Definition: dynamic-pinocchio.h:106
dg::SignalTimeDependent< dg::Vector, int > upperJlSOUT
Definition: dynamic-pinocchio.h:143
dg::SignalTimeDependent< dg::Vector, int > upperTlSOUT
Definition: dynamic-pinocchio.h:146
dg::SignalPtr< dg::Vector, int > freeFlyerAccelerationSIN
Definition: dynamic-pinocchio.h:116
dg::SignalTimeDependent< dg::Vector, int > upperVlSOUT
Definition: dynamic-pinocchio.h:145
dg::SignalTimeDependent< dg::Vector, int > zmpSOUT
Definition: dynamic-pinocchio.h:132
dg::SignalTimeDependent< Dummy, int > forwardKinematicsSINTERN
Definition: dynamic-pinocchio.h:124
void displayModel() const
Definition: dynamic-pinocchio.h:162
dg::SignalTimeDependent< dg::Matrix, int > inertiaRealSOUT
Definition: dynamic-pinocchio.h:150
dg::SignalTimeDependent< dg::Vector, int > pinocchioPosSINTERN
Definition: dynamic-pinocchio.h:118
Definition: angle-estimator.h:42
dg::SignalPtr< dg::Vector, int > jointPositionSIN
Definition: dynamic-pinocchio.h:111