dynamicgraph::sot::Dynamic Class Reference

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 <sot-dynamic/dynamic.h>

Inheritance diagram for dynamicgraph::sot::Dynamic:
Collaboration diagram for dynamicgraph::sot::Dynamic:

List of all members.

Public Types

typedef int Dummy

Public Member Functions

 DYNAMIC_GRAPH_ENTITY_DECL ()
 Dynamic (const std::string &name, bool build=true)
virtual ~Dynamic (void)
virtual void buildModel (void)
void setVrmlDirectory (const std::string &filename)
void setVrmlMainFile (const std::string &filename)
void setXmlSpecificityFile (const std::string &filename)
void setXmlRankFile (const std::string &filename)
void parseConfigFiles (void)
dg::SignalTimeDependent
< ml::Matrix, int > & 
createEndeffJacobianSignal (const std::string &signame, CjrlJoint *inJoint)
dg::SignalTimeDependent
< ml::Matrix, int > & 
createJacobianSignal (const std::string &signame, CjrlJoint *inJoint)
void destroyJacobianSignal (const std::string &signame)
dg::SignalTimeDependent
< MatrixHomogeneous, int > & 
createPositionSignal (const std::string &signame, CjrlJoint *inJoint)
void destroyPositionSignal (const std::string &signame)
dg::SignalTimeDependent
< ml::Vector, int > & 
createVelocitySignal (const std::string &signame, CjrlJoint *inJoint)
void destroyVelocitySignal (const std::string &signame)
dg::SignalTimeDependent
< ml::Vector, int > & 
createAccelerationSignal (const std::string &signame, CjrlJoint *inJoint)
void destroyAccelerationSignal (const std::string &signame)
bool zmpActivation (void)
void zmpActivation (const bool &b)
bool comActivation (void)
void comActivation (const bool &b)
int & computeNewtonEuler (int &dummy, int time)
int & initNewtonEuler (int &dummy, int time)
dg::SignalTimeDependent
< ml::Matrix, int > & 
jacobiansSOUT (const std::string &name)
dg::SignalTimeDependent
< MatrixHomogeneous, int > & 
positionsSOUT (const std::string &name)
dg::SignalTimeDependent
< ml::Vector, int > & 
velocitiesSOUT (const std::string &name)
dg::SignalTimeDependent
< ml::Vector, int > & 
accelerationsSOUT (const std::string &name)
virtual void commandLine (const std::string &cmdLine, std::istringstream &cmdArgs, std::ostream &os)
void cmd_createOpPointSignals (const std::string &sig, const std::string &j)
void cmd_createJacobianWorldSignal (const std::string &sig, const std::string &j)
void cmd_createJacobianEndEffectorSignal (const std::string &sig, const std::string &j)
void cmd_createPositionSignal (const std::string &sig, const std::string &j)
Construction of a robot by commands
void createRobot ()
 Create an empty device.
void createJoint (const std::string &inJointName, const std::string &inJointType, const ml::Matrix &inPosition)
 create a joint
void setRootJoint (const std::string &inJointName)
 Set a joint as root joint of the robot.
void addJoint (const std::string &inParentName, const std::string &inChildName)
 Add a child joint to a joint.
void setDofBounds (const std::string &inJointName, unsigned int inDofId, double inMinValue, double inMaxValue)
 Set bound of degree of freedom.
void setMass (const std::string &inJointName, double inMass)
 Set mass of a body.
void setLocalCenterOfMass (const std::string &inJointName, ml::Vector inCom)
 Set local center of mass of a body.
void setInertiaMatrix (const std::string &inJointName, ml::Matrix inMatrix)
 Set inertia matrix of a body.
void setSpecificJoint (const std::string &inJointName, const std::string &inJointType)
 Set specific joints.
void setHandParameters (bool inRight, const ml::Vector &inCenter, const ml::Vector &inThumbAxis, const ml::Vector &inForefingerAxis, const ml::Vector &inPalmNormal)
 Set hand parameters.
void setFootParameters (bool inRight, const double &inSoleLength, const double &inSoleWidth, const ml::Vector &inAnklePosition)
 Set foot parameters.
void setGazeParameters (const ml::Vector &inGazeOrigin, const ml::Vector &inGazeDirection)
 Set gaze parameters.
double getSoleLength () const
 Get length of left foot sole.
double getSoleWidth () const
 Get width of left foot sole.
ml::Vector getAnklePositionInFootFrame () const
 Get left ankle position in foot frame.

Public Attributes

bool init
std::list< dg::SignalBase< int > * > genericSignalRefs
dg::SignalPtr< ml::Vector, int > jointPositionSIN
dg::SignalPtr< ml::Vector, int > freeFlyerPositionSIN
dg::SignalPtr< ml::Vector, int > jointVelocitySIN
dg::SignalPtr< ml::Vector, int > freeFlyerVelocitySIN
dg::SignalPtr< ml::Vector, int > jointAccelerationSIN
dg::SignalPtr< ml::Vector, int > freeFlyerAccelerationSIN
dg::SignalTimeDependent< Dummy,
int > 
firstSINTERN
dg::SignalTimeDependent< Dummy,
int > 
newtonEulerSINTERN
dg::SignalTimeDependent
< ml::Vector, int > 
zmpSOUT
dg::SignalTimeDependent
< ml::Matrix, int > 
JcomSOUT
dg::SignalTimeDependent
< ml::Vector, int > 
comSOUT
dg::SignalTimeDependent
< ml::Matrix, int > 
inertiaSOUT
dg::SignalTimeDependent
< double, int > 
footHeightSOUT
dg::SignalTimeDependent
< ml::Vector, int > 
upperJlSOUT
dg::SignalTimeDependent
< ml::Vector, int > 
lowerJlSOUT
dg::SignalTimeDependent
< ml::Vector, int > 
upperVlSOUT
dg::SignalTimeDependent
< ml::Vector, int > 
lowerVlSOUT
dg::SignalTimeDependent
< ml::Vector, int > 
upperTlSOUT
dg::SignalTimeDependent
< ml::Vector, int > 
lowerTlSOUT
dg::Signal< ml::Vector, int > inertiaRotorSOUT
dg::Signal< ml::Vector, int > gearRatioSOUT
dg::SignalTimeDependent
< ml::Matrix, int > 
inertiaRealSOUT
dg::SignalTimeDependent
< ml::Vector, int > 
MomentaSOUT
dg::SignalTimeDependent
< ml::Vector, int > 
AngularMomentumSOUT
dg::SignalTimeDependent
< ml::Vector, int > 
dynamicDriftSOUT
Fields to access dynamicsJRLJapan Library
CjrlHumanoidDynamicRobot * m_HDR
 Abstract pointer on the structure. Ultimately we should be able to use only the abstract interface and do not care about the implementation.
int debugInertia
std::string vrmlDirectory
 Fields to access the humanoid model.
std::string vrmlMainFile
 Name of the root's robot model file.
std::string xmlSpecificityFile
 Name of the name specifying which end-effector is the head, the feet and so on...
std::string xmlRankFile
 Name of the name specifying which end-effector is the head, the feet and so on...

Protected Member Functions

ml::VectorcomputeZmp (ml::Vector &res, int time)
ml::VectorcomputeMomenta (ml::Vector &res, int time)
ml::VectorcomputeAngularMomentum (ml::Vector &res, int time)
ml::MatrixcomputeJcom (ml::Matrix &res, int time)
ml::VectorcomputeCom (ml::Vector &res, int time)
ml::MatrixcomputeInertia (ml::Matrix &res, int time)
ml::MatrixcomputeInertiaReal (ml::Matrix &res, int time)
double & computeFootHeight (double &res, int time)
ml::MatrixcomputeGenericJacobian (CjrlJoint *j, ml::Matrix &res, int time)
ml::MatrixcomputeGenericEndeffJacobian (CjrlJoint *j, ml::Matrix &res, int time)
MatrixHomogeneouscomputeGenericPosition (CjrlJoint *j, MatrixHomogeneous &res, int time)
ml::VectorcomputeGenericVelocity (CjrlJoint *j, ml::Vector &res, int time)
ml::VectorcomputeGenericAcceleration (CjrlJoint *j, ml::Vector &res, int time)
ml::VectorgetUpperJointLimits (ml::Vector &res, const int &time)
ml::VectorgetLowerJointLimits (ml::Vector &res, const int &time)
ml::VectorgetUpperVelocityLimits (ml::Vector &res, const int &time)
ml::VectorgetLowerVelocityLimits (ml::Vector &res, const int &time)
ml::VectorgetUpperTorqueLimits (ml::Vector &res, const int &time)
ml::VectorgetLowerTorqueLimits (ml::Vector &res, const int &time)
ml::VectorcomputeTorqueDrift (ml::Vector &res, const int &time)

Friends

class sot::command::SetFiles
class sot::command::Parse
class sot::command::CreateOpPoint
class sot::command::InitializeRobot

Detailed Description

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.


Member Typedef Documentation


Constructor & Destructor Documentation

dynamicgraph::sot::Dynamic::Dynamic ( const std::string &  name,
bool  build = true 
)
virtual dynamicgraph::sot::Dynamic::~Dynamic ( void  ) [virtual]

Member Function Documentation

dg::SignalTimeDependent<ml::Vector,int>& dynamicgraph::sot::Dynamic::accelerationsSOUT ( const std::string &  name)
void dynamicgraph::sot::Dynamic::addJoint ( const std::string &  inParentName,
const std::string &  inChildName 
)

Add a child joint to a joint.

Parameters:
inParentNamename of the joint to which a child is added.
inChildNamename of the child joint added.
virtual void dynamicgraph::sot::Dynamic::buildModel ( void  ) [virtual]
void dynamicgraph::sot::Dynamic::cmd_createJacobianEndEffectorSignal ( const std::string &  sig,
const std::string &  j 
)
void dynamicgraph::sot::Dynamic::cmd_createJacobianWorldSignal ( const std::string &  sig,
const std::string &  j 
)
void dynamicgraph::sot::Dynamic::cmd_createOpPointSignals ( const std::string &  sig,
const std::string &  j 
)
void dynamicgraph::sot::Dynamic::cmd_createPositionSignal ( const std::string &  sig,
const std::string &  j 
)
bool dynamicgraph::sot::Dynamic::comActivation ( void  ) [inline]
void dynamicgraph::sot::Dynamic::comActivation ( const bool &  b) [inline]
virtual void dynamicgraph::sot::Dynamic::commandLine ( const std::string &  cmdLine,
std::istringstream &  cmdArgs,
std::ostream &  os 
) [virtual]

Reimplemented from dynamicgraph::Entity.

ml::Vector& dynamicgraph::sot::Dynamic::computeAngularMomentum ( ml::Vector res,
int  time 
) [protected]
ml::Vector& dynamicgraph::sot::Dynamic::computeCom ( ml::Vector res,
int  time 
) [protected]
double& dynamicgraph::sot::Dynamic::computeFootHeight ( double &  res,
int  time 
) [protected]
ml::Vector& dynamicgraph::sot::Dynamic::computeGenericAcceleration ( CjrlJoint *  j,
ml::Vector res,
int  time 
) [protected]
ml::Matrix& dynamicgraph::sot::Dynamic::computeGenericEndeffJacobian ( CjrlJoint *  j,
ml::Matrix res,
int  time 
) [protected]
ml::Matrix& dynamicgraph::sot::Dynamic::computeGenericJacobian ( CjrlJoint *  j,
ml::Matrix res,
int  time 
) [protected]
MatrixHomogeneous& dynamicgraph::sot::Dynamic::computeGenericPosition ( CjrlJoint *  j,
MatrixHomogeneous res,
int  time 
) [protected]
ml::Vector& dynamicgraph::sot::Dynamic::computeGenericVelocity ( CjrlJoint *  j,
ml::Vector res,
int  time 
) [protected]
ml::Matrix& dynamicgraph::sot::Dynamic::computeInertia ( ml::Matrix res,
int  time 
) [protected]
ml::Matrix& dynamicgraph::sot::Dynamic::computeInertiaReal ( ml::Matrix res,
int  time 
) [protected]
ml::Matrix& dynamicgraph::sot::Dynamic::computeJcom ( ml::Matrix res,
int  time 
) [protected]
ml::Vector& dynamicgraph::sot::Dynamic::computeMomenta ( ml::Vector res,
int  time 
) [protected]
int& dynamicgraph::sot::Dynamic::computeNewtonEuler ( int &  dummy,
int  time 
)
ml::Vector& dynamicgraph::sot::Dynamic::computeTorqueDrift ( ml::Vector res,
const int &  time 
) [protected]
ml::Vector& dynamicgraph::sot::Dynamic::computeZmp ( ml::Vector res,
int  time 
) [protected]
dg::SignalTimeDependent< ml::Vector,int >& dynamicgraph::sot::Dynamic::createAccelerationSignal ( const std::string &  signame,
CjrlJoint *  inJoint 
)
dg::SignalTimeDependent< ml::Matrix,int >& dynamicgraph::sot::Dynamic::createEndeffJacobianSignal ( const std::string &  signame,
CjrlJoint *  inJoint 
)
dg::SignalTimeDependent< ml::Matrix,int >& dynamicgraph::sot::Dynamic::createJacobianSignal ( const std::string &  signame,
CjrlJoint *  inJoint 
)
void dynamicgraph::sot::Dynamic::createJoint ( const std::string &  inJointName,
const std::string &  inJointType,
const ml::Matrix inPosition 
)

create a joint

Parameters:
inJointNamename of the joint,
inJointTypetype of joint in ["freeflyer","rotation","translation","anchor"],
inPositionposition of the joint (4x4 homogeneous matrix).
Note:
joints are stored in a map with names as keys for retrieval by other commands. An empty CjrlBody is also created and attached to the joint.
dg::SignalTimeDependent< MatrixHomogeneous,int >& dynamicgraph::sot::Dynamic::createPositionSignal ( const std::string &  signame,
CjrlJoint *  inJoint 
)
void dynamicgraph::sot::Dynamic::createRobot ( )

Create an empty device.

dg::SignalTimeDependent< ml::Vector,int >& dynamicgraph::sot::Dynamic::createVelocitySignal ( const std::string &  signame,
CjrlJoint *  inJoint 
)
void dynamicgraph::sot::Dynamic::destroyAccelerationSignal ( const std::string &  signame)
void dynamicgraph::sot::Dynamic::destroyJacobianSignal ( const std::string &  signame)
void dynamicgraph::sot::Dynamic::destroyPositionSignal ( const std::string &  signame)
void dynamicgraph::sot::Dynamic::destroyVelocitySignal ( const std::string &  signame)
dynamicgraph::sot::Dynamic::DYNAMIC_GRAPH_ENTITY_DECL ( )
ml::Vector dynamicgraph::sot::Dynamic::getAnklePositionInFootFrame ( ) const

Get left ankle position in foot frame.

The robot is assumed to be symmetric.

ml::Vector& dynamicgraph::sot::Dynamic::getLowerJointLimits ( ml::Vector res,
const int &  time 
) [protected]
ml::Vector& dynamicgraph::sot::Dynamic::getLowerTorqueLimits ( ml::Vector res,
const int &  time 
) [protected]
ml::Vector& dynamicgraph::sot::Dynamic::getLowerVelocityLimits ( ml::Vector res,
const int &  time 
) [protected]
double dynamicgraph::sot::Dynamic::getSoleLength ( ) const

Get length of left foot sole.

The robot is assumed to be symmetric.

double dynamicgraph::sot::Dynamic::getSoleWidth ( ) const

Get width of left foot sole.

The robot is assumed to be symmetric.

ml::Vector& dynamicgraph::sot::Dynamic::getUpperJointLimits ( ml::Vector res,
const int &  time 
) [protected]
ml::Vector& dynamicgraph::sot::Dynamic::getUpperTorqueLimits ( ml::Vector res,
const int &  time 
) [protected]
ml::Vector& dynamicgraph::sot::Dynamic::getUpperVelocityLimits ( ml::Vector res,
const int &  time 
) [protected]
int& dynamicgraph::sot::Dynamic::initNewtonEuler ( int &  dummy,
int  time 
)
dg::SignalTimeDependent<ml::Matrix,int>& dynamicgraph::sot::Dynamic::jacobiansSOUT ( const std::string &  name)
void dynamicgraph::sot::Dynamic::parseConfigFiles ( void  )
dg::SignalTimeDependent<MatrixHomogeneous,int>& dynamicgraph::sot::Dynamic::positionsSOUT ( const std::string &  name)
void dynamicgraph::sot::Dynamic::setDofBounds ( const std::string &  inJointName,
unsigned int  inDofId,
double  inMinValue,
double  inMaxValue 
)

Set bound of degree of freedom.

Parameters:
inJointNamename of the joint,
inDofIdid of the degree of freedom in the joint,
inMinValue,inMaxValuevalues of degree of freedom bounds
void dynamicgraph::sot::Dynamic::setFootParameters ( bool  inRight,
const double &  inSoleLength,
const double &  inSoleWidth,
const ml::Vector inAnklePosition 
)

Set foot parameters.

Parameters:
inRighttrue if right foot, false if left foot,
inSoleLengthsole length,
inSoleWidthsole width,
inAnklePositionankle position in foot local frame,
void dynamicgraph::sot::Dynamic::setGazeParameters ( const ml::Vector inGazeOrigin,
const ml::Vector inGazeDirection 
)

Set gaze parameters.

Parameters:
inGazeOriginorigin of the gaze in gaze joint local frame,
inGazeDirectiondirection of the gase in gaze joint local frame.
void dynamicgraph::sot::Dynamic::setHandParameters ( bool  inRight,
const ml::Vector inCenter,
const ml::Vector inThumbAxis,
const ml::Vector inForefingerAxis,
const ml::Vector inPalmNormal 
)

Set hand parameters.

Parameters:
inRighttrue if right hand, false if left hand,
inCentercenter of the hand in wrist local frame,
inThumbAxisthumb axis in wrist local frame,
inForefingerAxisforefinger axis in wrist local frame,
inPalmNormalAxispalm normal in wrist local frame,
void dynamicgraph::sot::Dynamic::setInertiaMatrix ( const std::string &  inJointName,
ml::Matrix  inMatrix 
)

Set inertia matrix of a body.

Parameters:
inJointNamename of the joint to which the body is attached,
inMatrixinertia matrix.
void dynamicgraph::sot::Dynamic::setLocalCenterOfMass ( const std::string &  inJointName,
ml::Vector  inCom 
)

Set local center of mass of a body.

Parameters:
inJointNamename of the joint to which the body is attached,
inComlocal center of mass.
void dynamicgraph::sot::Dynamic::setMass ( const std::string &  inJointName,
double  inMass 
)

Set mass of a body.

Parameters:
inJointNamename of the joint to which the body is attached,
inMassmass of the body
void dynamicgraph::sot::Dynamic::setRootJoint ( const std::string &  inJointName)

Set a joint as root joint of the robot.

void dynamicgraph::sot::Dynamic::setSpecificJoint ( const std::string &  inJointName,
const std::string &  inJointType 
)

Set specific joints.

Parameters:
inJointNamename of the joint,
inJointTypetype of joint in ['chest','waist','left-wrist','right-wrist','left-ankle','right-ankle','gaze'].
void dynamicgraph::sot::Dynamic::setVrmlDirectory ( const std::string &  filename)
void dynamicgraph::sot::Dynamic::setVrmlMainFile ( const std::string &  filename)
void dynamicgraph::sot::Dynamic::setXmlRankFile ( const std::string &  filename)
void dynamicgraph::sot::Dynamic::setXmlSpecificityFile ( const std::string &  filename)
dg::SignalTimeDependent<ml::Vector,int>& dynamicgraph::sot::Dynamic::velocitiesSOUT ( const std::string &  name)
bool dynamicgraph::sot::Dynamic::zmpActivation ( void  ) [inline]
void dynamicgraph::sot::Dynamic::zmpActivation ( const bool &  b) [inline]

Friends And Related Function Documentation

friend class sot::command::CreateOpPoint [friend]
friend class sot::command::InitializeRobot [friend]
friend class sot::command::Parse [friend]
friend class sot::command::SetFiles [friend]

Member Data Documentation

CjrlHumanoidDynamicRobot* dynamicgraph::sot::Dynamic::m_HDR

Abstract pointer on the structure. Ultimately we should be able to use only the abstract interface and do not care about the implementation.

Fields to access the humanoid model.

Directory where the VRML humanoid model is stored

Name of the root's robot model file.

Name of the name specifying which end-effector is the head, the feet and so on...

Name of the name specifying which end-effector is the head, the feet and so on...