10 #ifndef __SOT_DYNAMIC_PINOCCHIO_H__
11 #define __SOT_DYNAMIC_PINOCCHIO_H__
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>
31 #include <dynamic-graph/linear-algebra.h>
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>
45 #if defined(dynamic_EXPORTS)
46 #define SOTDYNAMIC_EXPORT __declspec(dllexport)
48 #define SOTDYNAMIC_EXPORT __declspec(dllimport)
51 #define SOTDYNAMIC_EXPORT
73 friend class sot::command::SetFile;
74 friend class sot::command::CreateOpPoint;
78 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
79 DYNAMIC_GRAPH_ENTITY_DECL();
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);
94 dg::SignalTimeDependent<MatrixHomogeneous, int>& createPositionSignal(
const std::string&,
const std::string&);
95 void destroyPositionSignal(
const std::string& signame);
97 dg::SignalTimeDependent<dg::Vector, int>& createVelocitySignal(
const std::string&,
const std::string&);
98 void destroyVelocitySignal(
const std::string& signame);
100 dg::SignalTimeDependent<dg::Vector, int>& createAccelerationSignal(
const std::string&,
const std::string&);
101 void destroyAccelerationSignal(
const std::string& signame);
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);
130 dg::SignalTimeDependent<dg::Vector, int>
zmpSOUT;
132 dg::SignalTimeDependent<dg::Vector, int>
comSOUT;
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);
162 std::cout << (*m_model) << std::endl;
165 void setModel(pinocchio::Model*);
167 void setData(pinocchio::Data*);
175 dg::Vector& getLowerPositionLimits(dg::Vector& res,
const int&)
const;
181 dg::Vector& getUpperPositionLimits(dg::Vector& res,
const int&)
const;
187 dg::Vector& getUpperVelocityLimits(dg::Vector& res,
const int&)
const;
193 dg::Vector& getMaxEffortLimits(dg::Vector& res,
const int&)
const;
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,
201 MatrixHomogeneous& computeGenericPosition(
const bool isFrame,
const int jointId, MatrixHomogeneous& res,
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);
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);
215 dg::Vector& computeTorqueDrift(dg::Vector& res,
const int& time);
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);
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);
234 std::vector<int> sphericalJoints;
241 #endif // #ifndef __SOT_DYNAMIC_PINOCCHIO_H__