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. More...
#include <dynamic-pinocchio.h>

Public Types | |
| typedef int | Dummy |
Public Types inherited from dynamicgraph::Entity | |
| typedef std::map< const std::string, command::Command *> | CommandMap_t |
| typedef std::map< std::string, SignalBase< int > *> | SignalMap |
Public Member Functions | |
| dg::SignalTimeDependent< dg::Vector, int > & | accelerationsSOUT (const std::string &name) |
| void | cmd_createAccelerationSignal (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_createJacobianWorldSignal (const std::string &sig, const std::string &j) |
| void | cmd_createOpPointSignals (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) |
| int & | computeCcrba (int &dummy, const int &time) |
| int & | computeForwardKinematics (int &dummy, const int &time) |
| int & | computeJacobians (int &dummy, const int &time) |
| int & | computeNewtonEuler (int &dummy, const int &time) |
| dg::SignalTimeDependent< dg::Vector, int > & | createAccelerationSignal (const std::string &, const std::string &) |
| void | createData () |
| 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 &) |
| dg::SignalTimeDependent< MatrixHomogeneous, int > & | createPositionSignal (const std::string &, const std::string &) |
| dg::SignalTimeDependent< dg::Vector, int > & | createVelocitySignal (const std::string &, const std::string &) |
| void | destroyAccelerationSignal (const std::string &signame) |
| void | destroyJacobianSignal (const std::string &signame) |
| void | destroyPositionSignal (const std::string &signame) |
| void | destroyVelocitySignal (const std::string &signame) |
| void | displayModel () const |
| EIGEN_MAKE_ALIGNED_OPERATOR_NEW | DYNAMIC_GRAPH_ENTITY_DECL () |
| DynamicPinocchio (const std::string &name) | |
| pinocchio::Data * | getData () |
| dg::Vector & | getLowerPositionLimits (dg::Vector &res, const int &) const |
| Get joint position lower limits. More... | |
| dg::Vector & | getMaxEffortLimits (dg::Vector &res, const int &) const |
| Get joint effort upper limits. More... | |
| pinocchio::Model * | getModel () |
| dg::Vector & | getUpperPositionLimits (dg::Vector &res, const int &) const |
| Get joint position upper limits. More... | |
| dg::Vector & | getUpperVelocityLimits (dg::Vector &res, const int &) const |
| Get joint velocity upper limits. More... | |
| dg::SignalTimeDependent< dg::Matrix, int > & | jacobiansSOUT (const std::string &name) |
| dg::SignalTimeDependent< MatrixHomogeneous, int > & | positionsSOUT (const std::string &name) |
| void | setData (pinocchio::Data *) SOT_DYNAMIC_PINOCCHIO_DEPRECATED |
| void | setModel (pinocchio::Model *) |
| dg::SignalTimeDependent< dg::Vector, int > & | velocitiesSOUT (const std::string &name) |
| virtual | ~DynamicPinocchio (void) |
Public Member Functions inherited from dynamicgraph::Entity | |
| virtual void | display (std::ostream &os) const |
| std::ostream & | displaySignalList (std::ostream &os) const |
| Entity (const std::string &name) | |
| virtual const std::string & | getClassName () const |
| const std::string & | getCommandList () const |
| virtual std::string | getDocString () const |
| LoggerVerbosity | getLoggerVerbosityLevel () |
| LoggerVerbosity | getLoggerVerbosityLevel () |
| const std::string & | getName () const |
| command::Command * | getNewStyleCommand (const std::string &cmdName) |
| CommandMap_t | getNewStyleCommandMap () |
| SignalBase< int > & | getSignal (const std::string &signalName) |
| const SignalBase< int > & | getSignal (const std::string &signalName) const |
| SignalMap | getSignalMap () const |
| double | getStreamPrintPeriod () |
| double | getStreamPrintPeriod () |
| double | getTimeSample () |
| double | getTimeSample () |
| bool | hasSignal (const std::string &signame) const |
| Logger & | logger () |
| Logger const & | logger () const |
| Logger & | logger () |
| Logger const & | logger () const |
| void | sendMsg (const std::string &msg, MsgType t=MSG_TYPE_INFO, const std::string &lineId="") |
| void | sendMsg (const std::string &msg, MsgType t=MSG_TYPE_INFO, const std::string &lineId="") |
| void | setLoggerVerbosityLevel (LoggerVerbosity lv) |
| void | setLoggerVerbosityLevel (LoggerVerbosity lv) |
| bool | setStreamPrintPeriod (double t) |
| bool | setStreamPrintPeriod (double t) |
| bool | setTimeSample (double t) |
| bool | setTimeSample (double t) |
| virtual SignalBase< int > * | test () |
| virtual void | test2 (SignalBase< int > *) |
| virtual std::ostream & | writeCompletionList (std::ostream &os) const |
| virtual std::ostream & | writeGraph (std::ostream &os) const |
| virtual | ~Entity () |
Protected Member Functions | |
| dg::Vector & | computeAngularMomentum (dg::Vector &res, const int &time) |
| dg::Vector & | computeCom (dg::Vector &res, const int &time) |
| double & | computeFootHeight (double &res, const int &time) |
| dg::Vector & | computeGenericAcceleration (const int jointId, dg::Vector &res, const int &time) |
| dg::Matrix & | computeGenericEndeffJacobian (const bool isFrame, const bool isLocal, const int jointId, dg::Matrix &res, const int &time) |
| dg::Matrix & | computeGenericJacobian (const bool isFrame, 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::Matrix & | computeInertia (dg::Matrix &res, const int &time) |
| dg::Matrix & | computeInertiaReal (dg::Matrix &res, const int &time) |
| dg::Matrix & | computeJcom (dg::Matrix &res, const int &time) |
| dg::Vector & | computeMomenta (dg::Vector &res, const int &time) |
| dg::Vector & | computeTorqueDrift (dg::Vector &res, const int &time) |
| dg::Vector & | computeZmp (dg::Vector &res, const int &time) |
Protected Member Functions inherited from dynamicgraph::Entity | |
| void | addCommand (const std::string &name, command::Command *command) |
| void | entityDeregistration () |
| void | entityRegistration () |
| void | signalDeregistration (const std::string &name) |
| void | signalRegistration (const SignalArray< int > &signals) |
Private Member Functions | |
| dg::Vector & | getPinocchioAcc (dg::Vector &a, const int &time) |
| dg::Vector & | getPinocchioPos (dg::Vector &q, const int &time) |
| map of joints in construction. map: jointName -> (jointType,jointPosition (in parent frame), function_ptr to pinocchio Joint) More... | |
| dg::Vector & | getPinocchioVel (dg::Vector &v, const int &time) |
Private Attributes | |
| std::vector< int > | sphericalJoints |
Friends | |
| class | sot::command::CreateOpPoint |
| class | sot::command::SetFile |
Additional Inherited Members | |
Protected Attributes inherited from dynamicgraph::Entity | |
| CommandMap_t | commandMap |
| Logger | logger_ |
| std::string | name |
| SignalMap | signalMap |
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.
Definition at line 79 of file dynamic-pinocchio.h.
| typedef int dynamicgraph::sot::DynamicPinocchio::Dummy |
Definition at line 120 of file dynamic-pinocchio.h.
| DynamicPinocchio::DynamicPinocchio | ( | const std::string & | name | ) |
Definition at line 32 of file sot-dynamic-pinocchio.cpp.
|
virtual |
Definition at line 280 of file sot-dynamic-pinocchio.cpp.
| dg::SignalTimeDependent< dg::Vector, int > & DynamicPinocchio::accelerationsSOUT | ( | const std::string & | name | ) |
Definition at line 1062 of file sot-dynamic-pinocchio.cpp.
| void DynamicPinocchio::cmd_createAccelerationSignal | ( | const std::string & | sig, |
| const std::string & | j | ||
| ) |
Definition at line 1111 of file sot-dynamic-pinocchio.cpp.
| void DynamicPinocchio::cmd_createJacobianEndEffectorSignal | ( | const std::string & | sig, |
| const std::string & | j | ||
| ) |
Definition at line 1093 of file sot-dynamic-pinocchio.cpp.
| void DynamicPinocchio::cmd_createJacobianEndEffectorWorldSignal | ( | const std::string & | sig, |
| const std::string & | j | ||
| ) |
Definition at line 1098 of file sot-dynamic-pinocchio.cpp.
| void DynamicPinocchio::cmd_createJacobianWorldSignal | ( | const std::string & | sig, |
| const std::string & | j | ||
| ) |
Definition at line 1089 of file sot-dynamic-pinocchio.cpp.
| void DynamicPinocchio::cmd_createOpPointSignals | ( | const std::string & | sig, |
| const std::string & | j | ||
| ) |
Definition at line 1084 of file sot-dynamic-pinocchio.cpp.
| void DynamicPinocchio::cmd_createPositionSignal | ( | const std::string & | sig, |
| const std::string & | j | ||
| ) |
Definition at line 1103 of file sot-dynamic-pinocchio.cpp.
| void DynamicPinocchio::cmd_createVelocitySignal | ( | const std::string & | sig, |
| const std::string & | j | ||
| ) |
Definition at line 1107 of file sot-dynamic-pinocchio.cpp.
|
protected |
Definition at line 1006 of file sot-dynamic-pinocchio.cpp.
| int & DynamicPinocchio::computeCcrba | ( | int & | dummy, |
| const int & | time | ||
| ) |
Definition at line 783 of file sot-dynamic-pinocchio.cpp.
|
protected |
Definition at line 932 of file sot-dynamic-pinocchio.cpp.
Definition at line 969 of file sot-dynamic-pinocchio.cpp.
| int & DynamicPinocchio::computeForwardKinematics | ( | int & | dummy, |
| const int & | time | ||
| ) |
Definition at line 770 of file sot-dynamic-pinocchio.cpp.
|
protected |
Definition at line 895 of file sot-dynamic-pinocchio.cpp.
|
protected |
Definition at line 814 of file sot-dynamic-pinocchio.cpp.
|
protected |
Definition at line 793 of file sot-dynamic-pinocchio.cpp.
|
protected |
Definition at line 865 of file sot-dynamic-pinocchio.cpp.
|
protected |
Definition at line 883 of file sot-dynamic-pinocchio.cpp.
|
protected |
Definition at line 943 of file sot-dynamic-pinocchio.cpp.
|
protected |
Definition at line 953 of file sot-dynamic-pinocchio.cpp.
| int & DynamicPinocchio::computeJacobians | ( | int & | dummy, |
| const int & | time | ||
| ) |
Definition at line 762 of file sot-dynamic-pinocchio.cpp.
|
protected |
Definition at line 924 of file sot-dynamic-pinocchio.cpp.
|
protected |
Definition at line 994 of file sot-dynamic-pinocchio.cpp.
| int & DynamicPinocchio::computeNewtonEuler | ( | int & | dummy, |
| const int & | time | ||
| ) |
Definition at line 907 of file sot-dynamic-pinocchio.cpp.
|
protected |
Definition at line 985 of file sot-dynamic-pinocchio.cpp.
|
protected |
Definition at line 740 of file sot-dynamic-pinocchio.cpp.
| dg::SignalTimeDependent< dg::Vector, int > & DynamicPinocchio::createAccelerationSignal | ( | const std::string & | signame, |
| const std::string & | jointName | ||
| ) |
Definition at line 620 of file sot-dynamic-pinocchio.cpp.
| void DynamicPinocchio::createData | ( | ) |
Definition at line 313 of file sot-dynamic-pinocchio.cpp.
| dg::SignalTimeDependent< dg::Matrix, int > & DynamicPinocchio::createEndeffJacobianSignal | ( | const std::string & | signame, |
| const std::string & | jointName, | ||
| const bool | isLocal = true |
||
| ) |
Definition at line 538 of file sot-dynamic-pinocchio.cpp.
| dg::SignalTimeDependent< dg::Matrix, int > & DynamicPinocchio::createJacobianSignal | ( | const std::string & | signame, |
| const std::string & | jointName | ||
| ) |
Definition at line 507 of file sot-dynamic-pinocchio.cpp.
| dg::SignalTimeDependent< MatrixHomogeneous, int > & DynamicPinocchio::createPositionSignal | ( | const std::string & | signame, |
| const std::string & | jointName | ||
| ) |
Definition at line 570 of file sot-dynamic-pinocchio.cpp.
| SignalTimeDependent< dg::Vector, int > & DynamicPinocchio::createVelocitySignal | ( | const std::string & | signame, |
| const std::string & | jointName | ||
| ) |
Definition at line 600 of file sot-dynamic-pinocchio.cpp.
| void DynamicPinocchio::destroyAccelerationSignal | ( | const std::string & | signame | ) |
Definition at line 711 of file sot-dynamic-pinocchio.cpp.
| void DynamicPinocchio::destroyJacobianSignal | ( | const std::string & | signame | ) |
Definition at line 639 of file sot-dynamic-pinocchio.cpp.
| void DynamicPinocchio::destroyPositionSignal | ( | const std::string & | signame | ) |
Definition at line 662 of file sot-dynamic-pinocchio.cpp.
| void DynamicPinocchio::destroyVelocitySignal | ( | const std::string & | signame | ) |
Definition at line 687 of file sot-dynamic-pinocchio.cpp.
|
inline |
Definition at line 176 of file dynamic-pinocchio.h.
| EIGEN_MAKE_ALIGNED_OPERATOR_NEW dynamicgraph::sot::DynamicPinocchio::DYNAMIC_GRAPH_ENTITY_DECL | ( | ) |
|
inline |
Definition at line 191 of file dynamic-pinocchio.h.
| dg::Vector & DynamicPinocchio::getLowerPositionLimits | ( | dg::Vector & | res, |
| const int & | |||
| ) | const |
Get joint position lower limits.
| [out] | result | vector |
Definition at line 319 of file sot-dynamic-pinocchio.cpp.
| dg::Vector & DynamicPinocchio::getMaxEffortLimits | ( | dg::Vector & | res, |
| const int & | |||
| ) | const |
Get joint effort upper limits.
| [out] | result | vector |
Definition at line 419 of file sot-dynamic-pinocchio.cpp.
|
inline |
Definition at line 189 of file dynamic-pinocchio.h.
|
private |
Definition at line 491 of file sot-dynamic-pinocchio.cpp.
|
private |
map of joints in construction. map: jointName -> (jointType,jointPosition (in parent frame), function_ptr to pinocchio Joint)
Definition at line 432 of file sot-dynamic-pinocchio.cpp.
|
private |
Definition at line 477 of file sot-dynamic-pinocchio.cpp.
| dg::Vector & DynamicPinocchio::getUpperPositionLimits | ( | dg::Vector & | res, |
| const int & | |||
| ) | const |
Get joint position upper limits.
| [out] | result | vector |
Definition at line 366 of file sot-dynamic-pinocchio.cpp.
| dg::Vector & DynamicPinocchio::getUpperVelocityLimits | ( | dg::Vector & | res, |
| const int & | |||
| ) | const |
Get joint velocity upper limits.
| [out] | result | vector |
Definition at line 406 of file sot-dynamic-pinocchio.cpp.
| dg::SignalTimeDependent< dg::Matrix, int > & DynamicPinocchio::jacobiansSOUT | ( | const std::string & | name | ) |
Definition at line 1021 of file sot-dynamic-pinocchio.cpp.
| dg::SignalTimeDependent< MatrixHomogeneous, int > & DynamicPinocchio::positionsSOUT | ( | const std::string & | name | ) |
Definition at line 1035 of file sot-dynamic-pinocchio.cpp.
| void DynamicPinocchio::setData | ( | pinocchio::Data * | ) |
Definition at line 311 of file sot-dynamic-pinocchio.cpp.
| void DynamicPinocchio::setModel | ( | pinocchio::Model * | modelPtr | ) |
Definition at line 296 of file sot-dynamic-pinocchio.cpp.
| dg::SignalTimeDependent< dg::Vector, int > & DynamicPinocchio::velocitiesSOUT | ( | const std::string & | name | ) |
Definition at line 1048 of file sot-dynamic-pinocchio.cpp.
|
friend |
Definition at line 81 of file dynamic-pinocchio.h.
|
friend |
Definition at line 80 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::AngularMomentumSOUT |
Definition at line 166 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<Dummy, int> dynamicgraph::sot::DynamicPinocchio::ccrbaSINTERN |
Definition at line 135 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::comSOUT |
Definition at line 144 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::dynamicDriftSOUT |
Definition at line 167 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<double, int> dynamicgraph::sot::DynamicPinocchio::footHeightSOUT |
Definition at line 156 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<Dummy, int> dynamicgraph::sot::DynamicPinocchio::forwardKinematicsSINTERN |
Definition at line 134 of file dynamic-pinocchio.h.
| dg::SignalPtr<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::freeFlyerAccelerationSIN |
Definition at line 126 of file dynamic-pinocchio.h.
| dg::SignalPtr<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::freeFlyerPositionSIN |
Definition at line 122 of file dynamic-pinocchio.h.
| dg::SignalPtr<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::freeFlyerVelocitySIN |
Definition at line 124 of file dynamic-pinocchio.h.
| dg::Signal<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::gearRatioSOUT |
Definition at line 163 of file dynamic-pinocchio.h.
| std::list<dg::SignalBase<int>*> dynamicgraph::sot::DynamicPinocchio::genericSignalRefs |
Definition at line 116 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<dg::Matrix, int> dynamicgraph::sot::DynamicPinocchio::inertiaRealSOUT |
Definition at line 164 of file dynamic-pinocchio.h.
| dg::Signal<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::inertiaRotorSOUT |
Definition at line 162 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<dg::Matrix, int> dynamicgraph::sot::DynamicPinocchio::inertiaSOUT |
Definition at line 145 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<Dummy, int> dynamicgraph::sot::DynamicPinocchio::jacobiansSINTERN |
Definition at line 133 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<dg::Matrix, int> dynamicgraph::sot::DynamicPinocchio::JcomSOUT |
Definition at line 143 of file dynamic-pinocchio.h.
| dg::SignalPtr<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::jointAccelerationSIN |
Definition at line 125 of file dynamic-pinocchio.h.
| dg::SignalPtr<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::jointPositionSIN |
Definition at line 121 of file dynamic-pinocchio.h.
| dg::SignalPtr<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::jointVelocitySIN |
Definition at line 123 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::lowerJlSOUT |
Definition at line 158 of file dynamic-pinocchio.h.
| std::unique_ptr<pinocchio::Data> dynamicgraph::sot::DynamicPinocchio::m_data |
Definition at line 90 of file dynamic-pinocchio.h.
| pinocchio::Model* dynamicgraph::sot::DynamicPinocchio::m_model |
Definition at line 89 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::MomentaSOUT |
Definition at line 165 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<Dummy, int> dynamicgraph::sot::DynamicPinocchio::newtonEulerSINTERN |
Definition at line 132 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::pinocchioAccSINTERN |
Definition at line 130 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::pinocchioPosSINTERN |
Definition at line 128 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::pinocchioVelSINTERN |
Definition at line 129 of file dynamic-pinocchio.h.
|
private |
Definition at line 271 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::upperJlSOUT |
Definition at line 157 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::upperTlSOUT |
Definition at line 160 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::upperVlSOUT |
Definition at line 159 of file dynamic-pinocchio.h.
| dg::SignalTimeDependent<dg::Vector, int> dynamicgraph::sot::DynamicPinocchio::zmpSOUT |
Definition at line 142 of file dynamic-pinocchio.h.