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