GCC Code Coverage Report

Directory: ./
File: include/sot/dynamic-pinocchio/dynamic-pinocchio.h
Date: 2024-09-28 11:08:19
Exec Total Coverage
Lines: 0 7 0.0%
Branches: 0 2 0.0%
Line Branch Exec Source
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
38 #include <sot/dynamic-pinocchio/deprecated.hh>
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
61 namespace dynamicgraph {
62 namespace sot {
63 namespace dg = dynamicgraph;
64
65 namespace command {
66 class SetFile;
67 class CreateOpPoint;
68 } // namespace command
69 /* --------------------------------------------------------------------- */
70 /* --- CLASS ----------------------------------------------------------- */
71 /* --------------------------------------------------------------------- */
72
73 /*! @ingroup signals
74 \brief This class provides an inverse dynamic model of the robot.
75 More precisely it wraps the newton euler algorithm implemented
76 by the dynamicsJRLJapan library to make it accessible in the stack of tasks.
77 The robot is described by a VRML file.
78 */
79 class 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
86 ✗ DYNAMIC_GRAPH_ENTITY_DECL();
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
115 /*! @} */
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
183 void createData();
184
185 /// \deprecated this function does nothing. This class has its own
186 /// pinocchio::Data object, which can be access with \ref getData.
187 void setData(pinocchio::Data*) SOT_DYNAMIC_PINOCCHIO_DEPRECATED;
188
189 ✗ pinocchio::Model* getModel() { return m_model; };
190
191 ✗ pinocchio::Data* getData() { return m_data.get(); };
192
193 /* --- GETTERS --- */
194
195 /// \brief Get joint position lower limits
196 ///
197 /// \param[out] result vector
198 /// \return result vector
199 dg::Vector& getLowerPositionLimits(dg::Vector& res, const int&) const;
200
201 /// \brief Get joint position upper limits
202 ///
203 /// \param[out] result vector
204 /// \return result vector
205 dg::Vector& getUpperPositionLimits(dg::Vector& res, const int&) const;
206
207 /// \brief Get joint velocity upper limits
208 ///
209 /// \param[out] result vector
210 /// \return result vector
211 dg::Vector& getUpperVelocityLimits(dg::Vector& res, const int&) const;
212
213 /// \brief Get joint effort upper limits
214 ///
215 /// \param[out] result vector
216 /// \return result vector
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:
262 /// \brief map of joints in construction.
263 /// map: jointName -> (jointType,jointPosition (in parent frame), function_ptr
264 /// to pinocchio Joint)
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__
279