10 #ifndef __SOT_DYNAMIC_PINOCCHIO_H__ 11 #define __SOT_DYNAMIC_PINOCCHIO_H__ 24 #include <pinocchio/fwd.hpp> 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> 32 #include <sot/core/exception-dynamic.hh> 33 #include <sot/core/flags.hh> 34 #include <sot/core/matrix-geometry.hh> 36 #include <dynamic-graph/linear-algebra.h> 38 #include <sot/dynamic-pinocchio/deprecated.hh> 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> 52 #if defined(dynamic_EXPORTS) 53 #define SOTDYNAMIC_EXPORT __declspec(dllexport) 55 #define SOTDYNAMIC_EXPORT __declspec(dllimport) 58 #define SOTDYNAMIC_EXPORT 80 friend class sot::command::SetFile;
81 friend class sot::command::CreateOpPoint;
85 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
86 DYNAMIC_GRAPH_ENTITY_DECL();
90 std::unique_ptr<pinocchio::Data>
m_data;
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);
103 dg::SignalTimeDependent<MatrixHomogeneous, int>& createPositionSignal(
104 const std::string&,
const std::string&);
105 void destroyPositionSignal(
const std::string& signame);
107 dg::SignalTimeDependent<dg::Vector, int>& createVelocitySignal(
108 const std::string&,
const std::string&);
109 void destroyVelocitySignal(
const std::string& signame);
111 dg::SignalTimeDependent<dg::Vector, int>& createAccelerationSignal(
112 const std::string&,
const std::string&);
113 void destroyAccelerationSignal(
const std::string& signame);
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);
142 dg::SignalTimeDependent<dg::Vector, int>
zmpSOUT;
144 dg::SignalTimeDependent<dg::Vector, int>
comSOUT;
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);
178 std::cout << (*m_model) << std::endl;
181 void setModel(pinocchio::Model*);
187 void setData(pinocchio::Data*) SOT_DYNAMIC_PINOCCHIO_DEPRECATED;
191 pinocchio::Data*
getData() {
return m_data.get(); };
199 dg::Vector& getLowerPositionLimits(dg::Vector& res,
const int&)
const;
205 dg::Vector& getUpperPositionLimits(dg::Vector& res,
const int&)
const;
211 dg::Vector& getUpperVelocityLimits(dg::Vector& res,
const int&)
const;
217 dg::Vector& getMaxEffortLimits(dg::Vector& res,
const int&)
const;
222 dg::Matrix& computeGenericJacobian(
const bool isFrame,
const int jointId,
223 dg::Matrix& res,
const int& time);
224 dg::Matrix& computeGenericEndeffJacobian(
const bool isFrame,
226 const int jointId, dg::Matrix& res,
228 MatrixHomogeneous& computeGenericPosition(
const bool isFrame,
230 MatrixHomogeneous& res,
232 dg::Vector& computeGenericVelocity(
const int jointId, dg::Vector& res,
234 dg::Vector& computeGenericAcceleration(
const int jointId, dg::Vector& res,
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);
246 dg::Vector& computeTorqueDrift(dg::Vector& res,
const int& time);
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);
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);
271 std::vector<int> sphericalJoints;
278 #endif // #ifndef __SOT_DYNAMIC_PINOCCHIO_H__ dg::SignalPtr< dg::Vector, int > freeFlyerVelocitySIN
Definition: dynamic-pinocchio.h:124
dg::SignalTimeDependent< Dummy, int > jacobiansSINTERN
Definition: dynamic-pinocchio.h:133
dg::SignalTimeDependent< Dummy, int > newtonEulerSINTERN
Definition: dynamic-pinocchio.h:132
dg::SignalTimeDependent< dg::Matrix, int > JcomSOUT
Definition: dynamic-pinocchio.h:143
pinocchio::Model * m_model
Definition: dynamic-pinocchio.h:89
dg::Signal< dg::Vector, int > inertiaRotorSOUT
Definition: dynamic-pinocchio.h:162
dg::SignalTimeDependent< dg::Vector, int > comSOUT
Definition: dynamic-pinocchio.h:144
dg::SignalPtr< dg::Vector, int > freeFlyerPositionSIN
Definition: dynamic-pinocchio.h:122
std::unique_ptr< pinocchio::Data > m_data
Definition: dynamic-pinocchio.h:90
dg::SignalPtr< dg::Vector, int > jointVelocitySIN
Definition: dynamic-pinocchio.h:123
dg::SignalTimeDependent< dg::Matrix, int > inertiaSOUT
Definition: dynamic-pinocchio.h:145
dg::SignalTimeDependent< dg::Vector, int > MomentaSOUT
Definition: dynamic-pinocchio.h:165
This class provides an inverse dynamic model of the robot. More precisely it wraps the newton euler a...
Definition: dynamic-pinocchio.h:79
dg::SignalTimeDependent< dg::Vector, int > pinocchioAccSINTERN
Definition: dynamic-pinocchio.h:130
pinocchio::Data * getData()
Definition: dynamic-pinocchio.h:191
dg::SignalTimeDependent< dg::Vector, int > dynamicDriftSOUT
Definition: dynamic-pinocchio.h:167
dg::SignalTimeDependent< Dummy, int > ccrbaSINTERN
Definition: dynamic-pinocchio.h:135
dg::SignalPtr< dg::Vector, int > jointAccelerationSIN
Definition: dynamic-pinocchio.h:125
dg::SignalTimeDependent< dg::Vector, int > lowerJlSOUT
Definition: dynamic-pinocchio.h:158
dg::Signal< dg::Vector, int > gearRatioSOUT
Definition: dynamic-pinocchio.h:163
#define SOTDYNAMIC_EXPORT
Definition: dynamic-pinocchio.h:58
dg::SignalTimeDependent< double, int > footHeightSOUT
Definition: dynamic-pinocchio.h:156
dg::SignalTimeDependent< dg::Vector, int > pinocchioVelSINTERN
Definition: dynamic-pinocchio.h:129
int Dummy
Definition: dynamic-pinocchio.h:120
pinocchio::Model * getModel()
Definition: dynamic-pinocchio.h:189
dg::SignalTimeDependent< dg::Vector, int > AngularMomentumSOUT
Definition: dynamic-pinocchio.h:166
std::list< dg::SignalBase< int > * > genericSignalRefs
Definition: dynamic-pinocchio.h:116
dg::SignalTimeDependent< dg::Vector, int > upperJlSOUT
Definition: dynamic-pinocchio.h:157
dg::SignalTimeDependent< dg::Vector, int > upperTlSOUT
Definition: dynamic-pinocchio.h:160
dg::SignalPtr< dg::Vector, int > freeFlyerAccelerationSIN
Definition: dynamic-pinocchio.h:126
dg::SignalTimeDependent< dg::Vector, int > upperVlSOUT
Definition: dynamic-pinocchio.h:159
dg::SignalTimeDependent< dg::Vector, int > zmpSOUT
Definition: dynamic-pinocchio.h:142
dg::SignalTimeDependent< Dummy, int > forwardKinematicsSINTERN
Definition: dynamic-pinocchio.h:134
void displayModel() const
Definition: dynamic-pinocchio.h:176
dg::SignalTimeDependent< dg::Matrix, int > inertiaRealSOUT
Definition: dynamic-pinocchio.h:164
dg::SignalTimeDependent< dg::Vector, int > pinocchioPosSINTERN
Definition: dynamic-pinocchio.h:128
Definition: angle-estimator.h:43
dg::SignalPtr< dg::Vector, int > jointPositionSIN
Definition: dynamic-pinocchio.h:121