10 #ifndef __SOT_DYNAMIC_PINOCCHIO_H__
11 #define __SOT_DYNAMIC_PINOCCHIO_H__
23 #include <pinocchio/fwd.hpp>
24 #include <sot/core/flags.hh>
25 #include <dynamic-graph/entity.h>
26 #include <dynamic-graph/pool.h>
27 #include <dynamic-graph/signal-ptr.h>
28 #include <dynamic-graph/signal-time-dependent.h>
29 #include <sot/core/exception-dynamic.hh>
30 #include <sot/core/matrix-geometry.hh>
32 #include <dynamic-graph/linear-algebra.h>
36 #include <pinocchio/macros.hpp>
37 #include <pinocchio/multibody/model.hpp>
38 #include <pinocchio/algorithm/rnea.hpp>
39 #include <pinocchio/algorithm/jacobian.hpp>
40 #include <pinocchio/algorithm/frames.hpp>
47 #if defined(dynamic_EXPORTS)
48 #define SOTDYNAMIC_EXPORT __declspec(dllexport)
50 #define SOTDYNAMIC_EXPORT __declspec(dllimport)
53 #define SOTDYNAMIC_EXPORT
75 friend class sot::command::SetFile;
76 friend class sot::command::CreateOpPoint;
80 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
85 std::unique_ptr<pinocchio::Data>
m_data;
92 const bool isLocal =
true);
93 dg::SignalTimeDependent<dg::Matrix, int>&
createJacobianSignal(
const std::string& signame,
const std::string&);
96 dg::SignalTimeDependent<MatrixHomogeneous, int>&
createPositionSignal(
const std::string&,
const std::string&);
132 dg::SignalTimeDependent<dg::Vector, int>
zmpSOUT;
134 dg::SignalTimeDependent<dg::Vector, int>
comSOUT;
137 dg::SignalTimeDependent<dg::Matrix, int>&
jacobiansSOUT(
const std::string& name);
138 dg::SignalTimeDependent<MatrixHomogeneous, int>&
positionsSOUT(
const std::string& name);
139 dg::SignalTimeDependent<dg::Vector, int>&
velocitiesSOUT(
const std::string& name);
164 std::cout << (*m_model) << std::endl;
177 pinocchio::Data*
getData() {
return m_data.get(); };
239 dg::Vector& getPinocchioPos(dg::Vector& q,
const int& time);
240 dg::Vector& getPinocchioVel(dg::Vector& v,
const int& time);
241 dg::Vector& getPinocchioAcc(dg::Vector& a,
const int& time);
244 std::vector<int> sphericalJoints;
This class provides an inverse dynamic model of the robot. More precisely it wraps the newton euler a...
Definition: dynamic-pinocchio.h:74
dg::Vector & computeGenericVelocity(const int jointId, dg::Vector &res, const int &time)
void displayModel() const
Definition: dynamic-pinocchio.h:162
EIGEN_MAKE_ALIGNED_OPERATOR_NEW DYNAMIC_GRAPH_ENTITY_DECL()
dg::Vector & getLowerPositionLimits(dg::Vector &res, const int &) const
Get joint position lower limits.
dg::Vector & computeTorqueDrift(dg::Vector &res, const int &time)
MatrixHomogeneous & computeGenericPosition(const bool isFrame, const int jointId, MatrixHomogeneous &res, const int &time)
pinocchio::Data * getData()
Definition: dynamic-pinocchio.h:177
dg::Vector & computeGenericAcceleration(const int jointId, dg::Vector &res, const int &time)
dg::SignalTimeDependent< double, int > footHeightSOUT
Definition: dynamic-pinocchio.h:142
dg::Matrix & computeJcom(dg::Matrix &res, const int &time)
dg::SignalPtr< dg::Vector, int > jointPositionSIN
Definition: dynamic-pinocchio.h:111
dg::Matrix & computeGenericEndeffJacobian(const bool isFrame, const bool isLocal, const int jointId, dg::Matrix &res, const int &time)
dg::Vector & computeZmp(dg::Vector &res, const int &time)
std::unique_ptr< pinocchio::Data > m_data
Definition: dynamic-pinocchio.h:85
dg::SignalTimeDependent< dg::Vector, int > pinocchioPosSINTERN
Definition: dynamic-pinocchio.h:118
int & computeNewtonEuler(int &dummy, const int &time)
pinocchio::Model * m_model
Definition: dynamic-pinocchio.h:84
void cmd_createVelocitySignal(const std::string &sig, const std::string &j)
dg::SignalTimeDependent< dg::Vector, int > zmpSOUT
Definition: dynamic-pinocchio.h:132
dg::SignalTimeDependent< dg::Vector, int > lowerJlSOUT
Definition: dynamic-pinocchio.h:144
void cmd_createOpPointSignals(const std::string &sig, const std::string &j)
dg::SignalTimeDependent< MatrixHomogeneous, int > & positionsSOUT(const std::string &name)
double & computeFootHeight(double &res, const int &time)
dg::SignalTimeDependent< dg::Vector, int > upperJlSOUT
Definition: dynamic-pinocchio.h:143
dg::SignalTimeDependent< dg::Vector, int > pinocchioAccSINTERN
Definition: dynamic-pinocchio.h:120
int & computeJacobians(int &dummy, const int &time)
dg::SignalTimeDependent< dg::Vector, int > upperTlSOUT
Definition: dynamic-pinocchio.h:146
dg::SignalTimeDependent< Dummy, int > jacobiansSINTERN
Definition: dynamic-pinocchio.h:123
dg::SignalTimeDependent< dg::Matrix, int > & jacobiansSOUT(const std::string &name)
dg::SignalTimeDependent< dg::Vector, int > & velocitiesSOUT(const std::string &name)
void cmd_createJacobianEndEffectorSignal(const std::string &sig, const std::string &j)
void cmd_createAccelerationSignal(const std::string &sig, const std::string &j)
dg::SignalTimeDependent< MatrixHomogeneous, int > & createPositionSignal(const std::string &, const std::string &)
void destroyJacobianSignal(const std::string &signame)
dg::SignalTimeDependent< dg::Matrix, int > inertiaRealSOUT
Definition: dynamic-pinocchio.h:150
dg::Vector & computeAngularMomentum(dg::Vector &res, const int &time)
dg::Vector & computeMomenta(dg::Vector &res, const int &time)
void setData(pinocchio::Data *) SOT_DYNAMIC_PINOCCHIO_DEPRECATED
void destroyAccelerationSignal(const std::string &signame)
dg::Matrix & computeInertiaReal(dg::Matrix &res, const int &time)
dg::SignalTimeDependent< dg::Matrix, int > & createJacobianSignal(const std::string &signame, const std::string &)
void destroyVelocitySignal(const std::string &signame)
dg::SignalPtr< dg::Vector, int > jointVelocitySIN
Definition: dynamic-pinocchio.h:113
dg::SignalTimeDependent< dg::Matrix, int > JcomSOUT
Definition: dynamic-pinocchio.h:133
dg::SignalTimeDependent< dg::Matrix, int > inertiaSOUT
Definition: dynamic-pinocchio.h:135
void setModel(pinocchio::Model *)
dg::SignalTimeDependent< Dummy, int > ccrbaSINTERN
Definition: dynamic-pinocchio.h:125
int & computeCcrba(int &dummy, const int &time)
dg::SignalTimeDependent< dg::Vector, int > pinocchioVelSINTERN
Definition: dynamic-pinocchio.h:119
dg::Signal< dg::Vector, int > gearRatioSOUT
Definition: dynamic-pinocchio.h:149
dg::SignalTimeDependent< dg::Vector, int > & createVelocitySignal(const std::string &, const std::string &)
dg::SignalTimeDependent< Dummy, int > forwardKinematicsSINTERN
Definition: dynamic-pinocchio.h:124
dg::Signal< dg::Vector, int > inertiaRotorSOUT
Definition: dynamic-pinocchio.h:148
DynamicPinocchio(const std::string &name)
dg::SignalPtr< dg::Vector, int > freeFlyerVelocitySIN
Definition: dynamic-pinocchio.h:114
dg::SignalTimeDependent< dg::Vector, int > & createAccelerationSignal(const std::string &, const std::string &)
dg::SignalTimeDependent< dg::Matrix, int > & createEndeffJacobianSignal(const std::string &signame, const std::string &, const bool isLocal=true)
dg::SignalTimeDependent< dg::Vector, int > AngularMomentumSOUT
Definition: dynamic-pinocchio.h:152
dg::Vector & getUpperVelocityLimits(dg::Vector &res, const int &) const
Get joint velocity upper limits.
dg::Matrix & computeInertia(dg::Matrix &res, const int &time)
void destroyPositionSignal(const std::string &signame)
dg::SignalTimeDependent< dg::Vector, int > MomentaSOUT
Definition: dynamic-pinocchio.h:151
int & computeForwardKinematics(int &dummy, const int &time)
dg::SignalTimeDependent< Dummy, int > newtonEulerSINTERN
Definition: dynamic-pinocchio.h:122
void cmd_createJacobianEndEffectorWorldSignal(const std::string &sig, const std::string &j)
dg::Vector & getMaxEffortLimits(dg::Vector &res, const int &) const
Get joint effort upper limits.
std::list< dg::SignalBase< int > * > genericSignalRefs
Definition: dynamic-pinocchio.h:106
void cmd_createJacobianWorldSignal(const std::string &sig, const std::string &j)
pinocchio::Model * getModel()
Definition: dynamic-pinocchio.h:175
dg::SignalPtr< dg::Vector, int > jointAccelerationSIN
Definition: dynamic-pinocchio.h:115
void cmd_createPositionSignal(const std::string &sig, const std::string &j)
dg::Vector & computeCom(dg::Vector &res, const int &time)
dg::SignalTimeDependent< dg::Vector, int > & accelerationsSOUT(const std::string &name)
dg::SignalPtr< dg::Vector, int > freeFlyerAccelerationSIN
Definition: dynamic-pinocchio.h:116
virtual ~DynamicPinocchio(void)
dg::SignalTimeDependent< dg::Vector, int > dynamicDriftSOUT
Definition: dynamic-pinocchio.h:153
dg::Matrix & computeGenericJacobian(const bool isFrame, const int jointId, dg::Matrix &res, const int &time)
dg::SignalPtr< dg::Vector, int > freeFlyerPositionSIN
Definition: dynamic-pinocchio.h:112
dg::SignalTimeDependent< dg::Vector, int > comSOUT
Definition: dynamic-pinocchio.h:134
int Dummy
Definition: dynamic-pinocchio.h:110
dg::SignalTimeDependent< dg::Vector, int > upperVlSOUT
Definition: dynamic-pinocchio.h:145
dg::Vector & getUpperPositionLimits(dg::Vector &res, const int &) const
Get joint position upper limits.
#define SOT_DYNAMIC_PINOCCHIO_DEPRECATED
Definition: deprecated.hh:32
#define SOTDYNAMIC_EXPORT
Definition: dynamic-pinocchio.h:53
Definition: angle-estimator.h:42