dynamic-pinocchio.h 9.14 KB
Newer Older
Thomas Moulard's avatar
Thomas Moulard committed
1
2
3
4
/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
Francois Bleibel's avatar
Francois Bleibel committed
5
 *
Thomas Moulard's avatar
Thomas Moulard committed
6
 * CNRS/AIST
Francois Bleibel's avatar
Francois Bleibel committed
7
 *
Thomas Moulard's avatar
Thomas Moulard committed
8
 */
Francois Bleibel's avatar
Francois Bleibel committed
9

10
11
#ifndef __SOT_DYNAMIC_PINOCCHIO_H__
#define __SOT_DYNAMIC_PINOCCHIO_H__
Francois Bleibel's avatar
Francois Bleibel committed
12
13
14
15
16

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

florent's avatar
florent committed
17
18
19
20
/* STD */
#include <string>
#include <map>

Francois Bleibel's avatar
Francois Bleibel committed
21
/* SOT */
22
#include <pinocchio/fwd.hpp>
23
#include <sot/core/flags.hh>
Francois Bleibel's avatar
Francois Bleibel committed
24
25
26
27
#include <dynamic-graph/entity.h>
#include <dynamic-graph/pool.h>
#include <dynamic-graph/signal-ptr.h>
#include <dynamic-graph/signal-time-dependent.h>
28
#include <sot/core/exception-dynamic.hh>
29
#include <sot/core/matrix-geometry.hh>
30
31
/* Matrix */
#include <dynamic-graph/linear-algebra.h>
Francois Bleibel's avatar
Francois Bleibel committed
32

33
/* PINOCCHIO */
34
#include <pinocchio/macros.hpp>
35
36
37
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
38
#include <pinocchio/algorithm/frames.hpp>
39

Francois Bleibel's avatar
Francois Bleibel committed
40
41
42
43
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
44
45
46
#if defined(WIN32)
#if defined(dynamic_EXPORTS)
#define SOTDYNAMIC_EXPORT __declspec(dllexport)
Francois Bleibel's avatar
Francois Bleibel committed
47
#else
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
48
49
50
51
#define SOTDYNAMIC_EXPORT __declspec(dllimport)
#endif
#else
#define SOTDYNAMIC_EXPORT
Francois Bleibel's avatar
Francois Bleibel committed
52
53
#endif

Guilhem Saurel's avatar
Guilhem Saurel committed
54
namespace dynamicgraph {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
namespace sot {
namespace dg = dynamicgraph;

namespace command {
class SetFile;
class CreateOpPoint;
}  // namespace command
   /* --------------------------------------------------------------------- */
   /* --- CLASS ----------------------------------------------------------- */
   /* --------------------------------------------------------------------- */

/*! @ingroup signals
  \brief This class provides an inverse dynamic model of the robot.
  More precisely it wraps the newton euler algorithm implemented
  by the dynamicsJRLJapan library to make it accessible in the stack of tasks.
  The robot is described by a VRML file.
*/
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;
Guilhem Saurel's avatar
Guilhem Saurel committed
76

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
77
 public:
78
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
79
  DYNAMIC_GRAPH_ENTITY_DECL();
Francois Bleibel's avatar
Francois Bleibel committed
80

81
  /*  --- MODEL ATRIBUTES --- */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
82
83
  pinocchio::Model* m_model;
  pinocchio::Data* m_data;
florent's avatar
florent committed
84

85
  /*  --- MODEL ATRIBUTES --- */
Francois Bleibel's avatar
Francois Bleibel committed
86

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
87
 public:
88
  /* --- SIGNAL ACTIVATION --- */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
89
90
91
92
  dg::SignalTimeDependent<dg::Matrix, int>& createEndeffJacobianSignal(const std::string& signame, const std::string&,
                                                                       const bool isLocal = true);
  dg::SignalTimeDependent<dg::Matrix, int>& createJacobianSignal(const std::string& signame, const std::string&);
  void destroyJacobianSignal(const std::string& signame);
Guilhem Saurel's avatar
Guilhem Saurel committed
93

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
94
95
  dg::SignalTimeDependent<MatrixHomogeneous, int>& createPositionSignal(const std::string&, const std::string&);
  void destroyPositionSignal(const std::string& signame);
Guilhem Saurel's avatar
Guilhem Saurel committed
96

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
97
98
  dg::SignalTimeDependent<dg::Vector, int>& createVelocitySignal(const std::string&, const std::string&);
  void destroyVelocitySignal(const std::string& signame);
Guilhem Saurel's avatar
Guilhem Saurel committed
99

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
100
101
  dg::SignalTimeDependent<dg::Vector, int>& createAccelerationSignal(const std::string&, const std::string&);
  void destroyAccelerationSignal(const std::string& signame);
Francois Bleibel's avatar
Francois Bleibel committed
102

103
  /*! @} */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
104
  std::list<dg::SignalBase<int>*> genericSignalRefs;
Francois Bleibel's avatar
Francois Bleibel committed
105

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
106
 public:
107
108
  /* --- SIGNAL --- */
  typedef int Dummy;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
  dg::SignalPtr<dg::Vector, int> jointPositionSIN;
  dg::SignalPtr<dg::Vector, int> freeFlyerPositionSIN;
  dg::SignalPtr<dg::Vector, int> jointVelocitySIN;
  dg::SignalPtr<dg::Vector, int> freeFlyerVelocitySIN;
  dg::SignalPtr<dg::Vector, int> jointAccelerationSIN;
  dg::SignalPtr<dg::Vector, int> freeFlyerAccelerationSIN;

  dg::SignalTimeDependent<dg::Vector, int> pinocchioPosSINTERN;
  dg::SignalTimeDependent<dg::Vector, int> pinocchioVelSINTERN;
  dg::SignalTimeDependent<dg::Vector, int> pinocchioAccSINTERN;

  dg::SignalTimeDependent<Dummy, int> newtonEulerSINTERN;
  dg::SignalTimeDependent<Dummy, int> jacobiansSINTERN;
  dg::SignalTimeDependent<Dummy, int> forwardKinematicsSINTERN;
  dg::SignalTimeDependent<Dummy, int> ccrbaSINTERN;

  int& computeNewtonEuler(int& dummy, const int& time);
  int& computeForwardKinematics(int& dummy, const int& time);
  int& computeCcrba(int& dummy, const int& time);
  int& computeJacobians(int& dummy, const int& time);

  dg::SignalTimeDependent<dg::Vector, int> zmpSOUT;
  dg::SignalTimeDependent<dg::Matrix, int> JcomSOUT;
  dg::SignalTimeDependent<dg::Vector, int> comSOUT;
  dg::SignalTimeDependent<dg::Matrix, int> inertiaSOUT;

  dg::SignalTimeDependent<dg::Matrix, int>& jacobiansSOUT(const std::string& name);
  dg::SignalTimeDependent<MatrixHomogeneous, int>& positionsSOUT(const std::string& name);
  dg::SignalTimeDependent<dg::Vector, int>& velocitiesSOUT(const std::string& name);
  dg::SignalTimeDependent<dg::Vector, int>& accelerationsSOUT(const std::string& name);

  dg::SignalTimeDependent<double, int> footHeightSOUT;
  dg::SignalTimeDependent<dg::Vector, int> upperJlSOUT;
  dg::SignalTimeDependent<dg::Vector, int> lowerJlSOUT;
  dg::SignalTimeDependent<dg::Vector, int> upperVlSOUT;
  dg::SignalTimeDependent<dg::Vector, int> upperTlSOUT;

  dg::Signal<dg::Vector, int> inertiaRotorSOUT;
  dg::Signal<dg::Vector, int> gearRatioSOUT;
  dg::SignalTimeDependent<dg::Matrix, int> inertiaRealSOUT;
  dg::SignalTimeDependent<dg::Vector, int> MomentaSOUT;
  dg::SignalTimeDependent<dg::Vector, int> AngularMomentumSOUT;
  dg::SignalTimeDependent<dg::Vector, int> dynamicDriftSOUT;

 public:
154
  /* --- CONSTRUCTOR --- */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
155
156
  DynamicPinocchio(const std::string& name);
  virtual ~DynamicPinocchio(void);
Francois Bleibel's avatar
Francois Bleibel committed
157

158
  /* --- MODEL CREATION --- */
Francois Bleibel's avatar
Francois Bleibel committed
159

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
160
161
162
163
  void displayModel() const {
    assert(m_model);
    std::cout << (*m_model) << std::endl;
  };
164

165
  void setModel(pinocchio::Model*);
166

167
  void setData(pinocchio::Data*);
Guilhem Saurel's avatar
Guilhem Saurel committed
168

169
  /* --- GETTERS --- */
170

171
  /// \brief Get joint position lower limits
172
  ///
173
174
  /// \param[out] result vector
  /// \return result vector
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
175
  dg::Vector& getLowerPositionLimits(dg::Vector& res, const int&) const;
176

177
  /// \brief Get joint position upper limits
178
  ///
179
180
  /// \param[out] result vector
  /// \return result vector
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
181
  dg::Vector& getUpperPositionLimits(dg::Vector& res, const int&) const;
182

183
  /// \brief Get joint velocity upper limits
184
  ///
185
186
  /// \param[out] result vector
  /// \return result vector
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
187
  dg::Vector& getUpperVelocityLimits(dg::Vector& res, const int&) const;
188

189
  /// \brief Get joint effort upper limits
florent's avatar
florent committed
190
  ///
191
192
  /// \param[out] result vector
  /// \return result vector
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
193
  dg::Vector& getMaxEffortLimits(dg::Vector& res, const int&) const;
194
195
196

  //  dg::Vector& getAnklePositionInFootFrame() const;

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
 protected:
  dg::Matrix& computeGenericJacobian(const bool isFrame, const int jointId, dg::Matrix& res, const int& time);
  dg::Matrix& computeGenericEndeffJacobian(const bool isFrame, const bool isLocal, const int jointId, dg::Matrix& res,
                                           const int& time);
  MatrixHomogeneous& computeGenericPosition(const bool isFrame, const int jointId, MatrixHomogeneous& res,
                                            const int& time);
  dg::Vector& computeGenericVelocity(const int jointId, dg::Vector& res, const int& time);
  dg::Vector& computeGenericAcceleration(const int jointId, dg::Vector& res, const int& time);

  dg::Vector& computeZmp(dg::Vector& res, const int& time);
  dg::Vector& computeMomenta(dg::Vector& res, const int& time);
  dg::Vector& computeAngularMomentum(dg::Vector& res, const int& time);
  dg::Matrix& computeJcom(dg::Matrix& res, const int& time);
  dg::Vector& computeCom(dg::Vector& res, const int& time);
  dg::Matrix& computeInertia(dg::Matrix& res, const int& time);
  dg::Matrix& computeInertiaReal(dg::Matrix& res, const int& time);
  double& computeFootHeight(double& res, const int& time);

  dg::Vector& computeTorqueDrift(dg::Vector& res, const int& time);
216
217

 public: /* --- PARAMS --- */
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
218
219
220
221
222
223
224
  void cmd_createOpPointSignals(const std::string& sig, const std::string& j);
  void cmd_createJacobianWorldSignal(const std::string& sig, const std::string& j);
  void cmd_createJacobianEndEffectorSignal(const std::string& sig, const std::string& j);
  void cmd_createJacobianEndEffectorWorldSignal(const std::string& sig, const std::string& j);
  void cmd_createPositionSignal(const std::string& sig, const std::string& j);
  void cmd_createVelocitySignal(const std::string& sig, const std::string& j);
  void cmd_createAccelerationSignal(const std::string& sig, const std::string& j);
225

florent's avatar
florent committed
226
227
 private:
  /// \brief map of joints in construction.
Guilhem Saurel's avatar
Guilhem Saurel committed
228
  /// map: jointName -> (jointType,jointPosition (in parent frame), function_ptr to pinocchio Joint)
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
229
  dg::Vector& getPinocchioPos(dg::Vector& q, const int& time);
230
  dg::Vector& getPinocchioVel(dg::Vector& v, const int& time);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
231
  dg::Vector& getPinocchioAcc(dg::Vector& a, const int& time);
Guilhem Saurel's avatar
Guilhem Saurel committed
232

233
  //\brief Index list for the first dof of (spherical joints)/ (spherical part of free-flyer joint).
234
  std::vector<int> sphericalJoints;
Francois Bleibel's avatar
Francois Bleibel committed
235
236
};

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
237
238
239
// std::ostream& operator<<(std::ostream& os, const CjrlJoint& r);
} /* namespace sot */
} /* namespace dynamicgraph */
Francois Bleibel's avatar
Francois Bleibel committed
240

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
241
#endif  // #ifndef __SOT_DYNAMIC_PINOCCHIO_H__