CjrlDynamicRobot Class Referenceabstract

Abstract class that instantiates a robot with dynamic properties. More...

#include <abstract-robot-dynamics/dynamic-robot.hh>

Inheritance diagram for CjrlDynamicRobot:

Public Member Functions

virtual bool getJacobian (const CjrlJoint &inStartJoint, const CjrlJoint &inEndJoint, const vector3d &inFrameLocalPosition, matrixNxP &outjacobian, unsigned int offset=0, bool inIncludeStartFreeFlyer=true)=0
 Compute and get position and orientation jacobian. More...
 
virtual bool getPositionJacobian (const CjrlJoint &inStartJoint, const CjrlJoint &inEndJoint, const vector3d &inFrameLocalPosition, matrixNxP &outjacobian, unsigned int offset=0, bool inIncludeStartFreeFlyer=true)=0
 
virtual bool getOrientationJacobian (const CjrlJoint &inStartJoint, const CjrlJoint &inEndJoint, matrixNxP &outjacobian, unsigned int offset=0, bool inIncludeStartFreeFlyer=true)=0
 
virtual bool getJacobianCenterOfMass (const CjrlJoint &inStartJoint, matrixNxP &outjacobian, unsigned int offset=0, bool inIncludeStartFreeFlyer=true)=0
 
virtual bool getSpecializedInverseKinematics (const CjrlJoint &, const CjrlJoint &, const matrix4d &, const matrix4d &, vectorN &)
 Compute Speciliazed InverseKinematics between two joints. More...
 
Initialization
virtual bool initialize ()=0
 Initialize data-structure necessary to dynamic computations This function should be called after building the tree of joints. More...
 
virtual ~CjrlDynamicRobot ()
 Destructor. More...
 
Kinematic chain
virtual void rootJoint (CjrlJoint &inJoint)=0
 Set the root joint of the robot. More...
 
virtual CjrlJointrootJoint () const =0
 Get the root joint of the robot. More...
 
virtual std::vector< CjrlJoint * > jointVector ()=0
 Get a vector containing all the joints. More...
 
virtual std::vector< CjrlJoint * > jointsBetween (const CjrlJoint &inStartJoint, const CjrlJoint &inEndJoint) const =0
 Get the chain of joints between two joints. More...
 
virtual double upperBoundDof (unsigned int inRankInConfiguration)=0
 Get the upper bound for ith dof. More...
 
virtual double lowerBoundDof (unsigned int inRankInConfiguration)=0
 Get the lower bound for ith dof. More...
 
virtual double upperBoundDof (unsigned int inRankInConfiguration, const vectorN &inConfig)=0
 Compute the upper bound for ith dof using other configuration values if possible. More...
 
virtual double lowerBoundDof (unsigned int inRankInConfiguration, const vectorN &inConfig)=0
 Compute the lower bound for ith dof using other configuration values if possible. More...
 
virtual double upperVelocityBoundDof (unsigned int inRankInConfiguration)=0
 Get the upper velocity bound for ith dof. More...
 
virtual double lowerVelocityBoundDof (unsigned int inRankInConfiguration)=0
 Get the lower velocity bound for ith dof. More...
 
virtual double upperTorqueBoundDof (unsigned int inRankInConfiguration)=0
 Get the upper torque bound for ith dof. More...
 
virtual double lowerTorqueBoundDof (unsigned int inRankInConfiguration)=0
 Get the lower torque bound for ith dof. More...
 
virtual unsigned int numberDof () const =0
 Get the number of degrees of freedom of the robot. More...
 
virtual void setJointOrderInConfig (std::vector< CjrlJoint *> inJointVector)=0
 Set the joint ordering in the configuration vector. More...
 
Configuration, velocity and acceleration
virtual bool currentConfiguration (const vectorN &inConfig)=0
 Set the current configuration of the robot. More...
 
virtual const vectorNcurrentConfiguration () const =0
 Get the current configuration of the robot. More...
 
virtual bool currentVelocity (const vectorN &inVelocity)=0
 Set the current velocity of the robot. More...
 
virtual const vectorNcurrentVelocity () const =0
 Get the current velocity of the robot. More...
 
virtual bool currentAcceleration (const vectorN &inAcceleration)=0
 Set the current acceleration of the robot. More...
 
virtual const vectorNcurrentAcceleration () const =0
 Get the current acceleration of the robot. More...
 
virtual const matrixNxPcurrentForces () const =0
 Get the current forces of the robot. More...
 
virtual const matrixNxPcurrentTorques () const =0
 Get the current torques of the robot. More...
 
virtual const vectorNcurrentJointTorques () const =0
 Get the current joint torques of the robot. More...
 
Forward kinematics and dynamics
virtual bool computeForwardKinematics ()=0
 Compute forward kinematics. More...
 
virtual bool computeCenterOfMassDynamics ()=0
 Compute the dynamics of the center of mass. More...
 
virtual const vector3dpositionCenterOfMass () const =0
 Get the position of the center of mass. More...
 
virtual const vector3dvelocityCenterOfMass ()=0
 Get the velocity of the center of mass. More...
 
virtual const vector3daccelerationCenterOfMass ()=0
 Get the acceleration of the center of mass. More...
 
virtual const vector3dlinearMomentumRobot ()=0
 Get the linear momentum of the robot. More...
 
virtual const vector3dderivativeLinearMomentum ()=0
 Get the time-derivative of the linear momentum. More...
 
virtual const vector3dangularMomentumRobot ()=0
 Get the angular momentum of the robot at the center of mass. More...
 
virtual const vector3dderivativeAngularMomentum ()=0
 Get the time-derivative of the angular momentum at the center of mass. More...
 
virtual double mass () const =0
 Get the total mass of the robot. More...
 
Control of the implementation
virtual bool isSupported (const std::string &)
 Whether the specified property in implemented. More...
 
virtual bool getProperty (const std::string &, std::string &) const
 Get property corresponding to command name. More...
 
virtual bool setProperty (std::string &, const std::string &)
 Set property corresponding to command name. More...
 
Inertia matrix related methods
virtual void computeInertiaMatrix ()=0
 Compute the inertia matrix of the robot according wrt ${\bf q}$. More...
 
virtual const matrixNxPinertiaMatrix () const =0
 Get the inertia matrix of the robot according wrt ${\bf q}$. More...
 
Actuated joints related methods.
virtual const std::vector< CjrlJoint * > & getActuatedJoints () const =0
 Returns the list of actuated joints. More...
 
virtual void setActuatedJoints (std::vector< CjrlJoint *> &lActuatedJoints)=0
 Specifies the list of actuated joints. More...
 

Detailed Description

Abstract class that instantiates a robot with dynamic properties.

Definition
A robot is a kinematic chain of joints. To each joint is attached a body. Each body has a mass and inertia matrix. The kinematic chain is recursively constructed by adding children to each joint. (See CjrlJoint). The configuration of a robot is defined by a vector ${\bf q}$ called the configuration vector and containing the values of each degree of freedom. The dimension of this vector is denoted by $n_{dof}$.

The time derivative ${\bf \dot{q}}$ of the configuration vector is called the velocity vector.

The time derivative ${\bf \ddot{q}}$ of the velocity vector. is called the acceleration vector.

Control of the implementation
In some cases, it is desirable to control the implementation of this class in order to selectively compute only some kinematic and dynamic values. For instance, for speed of computation purposes, one may want to compute only velocities and not accelerations. In order to keep the interface light, a control mechanism based on properties is proposed through the following methods: isSupported(), getProperty(), setProperty(). Each implementation is responsible for its own methods. However, in order to keep some compatibility, some recommended methods are listed in this page.
Actuated Joints.
In order to make a distinction between actuated joints and none actuacted joints, a vector of actuated joints is provided through method: getActuatedJoints().

Constructor & Destructor Documentation

◆ ~CjrlDynamicRobot()

Member Function Documentation

◆ accelerationCenterOfMass()

virtual const vector3d& CjrlDynamicRobot::accelerationCenterOfMass ( )
pure virtual

Get the acceleration of the center of mass.

Referenced by ~CjrlDynamicRobot().

◆ angularMomentumRobot()

virtual const vector3d& CjrlDynamicRobot::angularMomentumRobot ( )
pure virtual

Get the angular momentum of the robot at the center of mass.

Referenced by ~CjrlDynamicRobot().

◆ computeCenterOfMassDynamics()

virtual bool CjrlDynamicRobot::computeCenterOfMassDynamics ( )
pure virtual

Compute the dynamics of the center of mass.

Compute the linear and angular momentum and their time derivatives, at the center of mass.

Referenced by ~CjrlDynamicRobot().

◆ computeForwardKinematics()

virtual bool CjrlDynamicRobot::computeForwardKinematics ( )
pure virtual

Compute forward kinematics.

Update the position, velocity and accelerations of each joint wrt ${\bf {q}}$, ${\bf \dot{q}}$, ${\bf \ddot{q}}$.

Referenced by ~CjrlDynamicRobot().

◆ computeInertiaMatrix()

virtual void CjrlDynamicRobot::computeInertiaMatrix ( )
pure virtual

Compute the inertia matrix of the robot according wrt ${\bf q}$.

Referenced by ~CjrlDynamicRobot().

◆ currentAcceleration() [1/2]

virtual bool CjrlDynamicRobot::currentAcceleration ( const vectorN inAcceleration)
pure virtual

Set the current acceleration of the robot.

Parameters
inAccelerationthe acceleration vector ${\bf \ddot{q}}$.
Returns
true if success, false if failure (the dimension of the input vector does not fit the number of degrees of freedom of the robot).

◆ currentAcceleration() [2/2]

virtual const vectorN& CjrlDynamicRobot::currentAcceleration ( ) const
pure virtual

Get the current acceleration of the robot.

Returns
the acceleration vector ${\bf \ddot{q}}$.

Referenced by ~CjrlDynamicRobot().

◆ currentConfiguration() [1/2]

virtual bool CjrlDynamicRobot::currentConfiguration ( const vectorN inConfig)
pure virtual

Set the current configuration of the robot.

Parameters
inConfigthe configuration vector ${\bf q}$.
Returns
true if success, false if failure (the dimension of the input vector does not fit the number of degrees of freedom of the robot).

◆ currentConfiguration() [2/2]

virtual const vectorN& CjrlDynamicRobot::currentConfiguration ( ) const
pure virtual

Get the current configuration of the robot.

Returns
the configuration vector ${\bf q}$.

Referenced by ~CjrlDynamicRobot().

◆ currentForces()

virtual const matrixNxP& CjrlDynamicRobot::currentForces ( ) const
pure virtual

Get the current forces of the robot.

Returns
the force vector ${\bf f}$.

Referenced by ~CjrlDynamicRobot().

◆ currentJointTorques()

virtual const vectorN& CjrlDynamicRobot::currentJointTorques ( ) const
pure virtual

Get the current joint torques of the robot.

The torques are computed by internal calls to the direct dynamic computations. Dynamics is computed in free-floating mode, supposing no contact with the environments, and knowing given position, velocity and acceleration. This accessor only give a reference on the already-computed values.

Returns
the torque vector ${\bf \tau }$.

Referenced by ~CjrlDynamicRobot().

◆ currentTorques()

virtual const matrixNxP& CjrlDynamicRobot::currentTorques ( ) const
pure virtual

Get the current torques of the robot.

Returns
the torque vector ${\bf \tau }$.

Referenced by ~CjrlDynamicRobot().

◆ currentVelocity() [1/2]

virtual bool CjrlDynamicRobot::currentVelocity ( const vectorN inVelocity)
pure virtual

Set the current velocity of the robot.

Parameters
inVelocitythe velocity vector ${\bf \dot{q}}$.
Returns
true if success, false if failure (the dimension of the input vector does not fit the number of degrees of freedom of the robot).

◆ currentVelocity() [2/2]

virtual const vectorN& CjrlDynamicRobot::currentVelocity ( ) const
pure virtual

Get the current velocity of the robot.

Returns
the velocity vector ${\bf \dot{q}}$.

Referenced by ~CjrlDynamicRobot().

◆ derivativeAngularMomentum()

virtual const vector3d& CjrlDynamicRobot::derivativeAngularMomentum ( )
pure virtual

Get the time-derivative of the angular momentum at the center of mass.

Referenced by ~CjrlDynamicRobot().

◆ derivativeLinearMomentum()

virtual const vector3d& CjrlDynamicRobot::derivativeLinearMomentum ( )
pure virtual

Get the time-derivative of the linear momentum.

Referenced by ~CjrlDynamicRobot().

◆ getActuatedJoints()

virtual const std::vector<CjrlJoint*>& CjrlDynamicRobot::getActuatedJoints ( ) const
pure virtual

Returns the list of actuated joints.

Referenced by ~CjrlDynamicRobot().

◆ getJacobian()

virtual bool CjrlDynamicRobot::getJacobian ( const CjrlJoint inStartJoint,
const CjrlJoint inEndJoint,
const vector3d inFrameLocalPosition,
matrixNxP outjacobian,
unsigned int  offset = 0,
bool  inIncludeStartFreeFlyer = true 
)
pure virtual

Compute and get position and orientation jacobian.

Parameters
inStartJointFirst joint in the chain of joints influencing the jacobian.
inEndJointJoint where the control frame is located.
inFrameLocalPositionPosition of the control frame in inEndJoint local frame.
Return values
outjacobiancomputed jacobian matrix.
Parameters
offsetis the rank of the column from where the jacobian is written.
inIncludeStartFreeFlyerOption to include the contribution of a fictive freeflyer superposed with inStartJoint
Returns
false if matrix has inadequate size. Number of columns in matrix outJacobian must be at least numberDof() if inIncludeStartFreeFlyer = true. It must be at least numberDof()-6 otherwise.

Referenced by ~CjrlDynamicRobot().

◆ getJacobianCenterOfMass()

virtual bool CjrlDynamicRobot::getJacobianCenterOfMass ( const CjrlJoint inStartJoint,
matrixNxP outjacobian,
unsigned int  offset = 0,
bool  inIncludeStartFreeFlyer = true 
)
pure virtual

Referenced by ~CjrlDynamicRobot().

◆ getOrientationJacobian()

virtual bool CjrlDynamicRobot::getOrientationJacobian ( const CjrlJoint inStartJoint,
const CjrlJoint inEndJoint,
matrixNxP outjacobian,
unsigned int  offset = 0,
bool  inIncludeStartFreeFlyer = true 
)
pure virtual

Referenced by ~CjrlDynamicRobot().

◆ getPositionJacobian()

virtual bool CjrlDynamicRobot::getPositionJacobian ( const CjrlJoint inStartJoint,
const CjrlJoint inEndJoint,
const vector3d inFrameLocalPosition,
matrixNxP outjacobian,
unsigned int  offset = 0,
bool  inIncludeStartFreeFlyer = true 
)
pure virtual

Referenced by ~CjrlDynamicRobot().

◆ getProperty()

bool CjrlDynamicRobot::getProperty ( const std::string &  ,
std::string &   
) const
inlinevirtual

Get property corresponding to command name.

Parameters
inPropertyname of the property.
Return values
outValuevalue of the property if implemented.
Note
The returned string needs to be cast into the right type (double, int,...).

Referenced by ~CjrlDynamicRobot().

◆ getSpecializedInverseKinematics()

bool CjrlDynamicRobot::getSpecializedInverseKinematics ( const CjrlJoint ,
const CjrlJoint ,
const matrix4d ,
const matrix4d ,
vectorN  
)
inlinevirtual

Compute Speciliazed InverseKinematics between two joints.

Specialized means that this method can be re implemented to be extremly efficient and used the particularity of your robot. For instance in some case, it is possible to use an exact inverse kinematics to compute a set of articular value.

This method does not intend to replace an architecture computing inverse kinematics through the Jacobian.

jointRootPosition and jointEndPosition have to be expressed in the same frame.

Parameters
[in]jointRootThe root of the joint chain for which the specialized inverse kinematics should be computed.
[in]jointEndThe end of the joint chain for which the specialized inverse kinematics should be computed.
[in]jointRootPositionThe desired position of the root.
[in]jointEndPositionThe end position of the root.
[out]qResult i.e. the articular values.

Referenced by ~CjrlDynamicRobot().

◆ inertiaMatrix()

virtual const matrixNxP& CjrlDynamicRobot::inertiaMatrix ( ) const
pure virtual

Get the inertia matrix of the robot according wrt ${\bf q}$.

Referenced by ~CjrlDynamicRobot().

◆ initialize()

virtual bool CjrlDynamicRobot::initialize ( )
pure virtual

Initialize data-structure necessary to dynamic computations This function should be called after building the tree of joints.

◆ isSupported()

bool CjrlDynamicRobot::isSupported ( const std::string &  )
inlinevirtual

Whether the specified property in implemented.

Referenced by ~CjrlDynamicRobot().

◆ jointsBetween()

virtual std::vector<CjrlJoint*> CjrlDynamicRobot::jointsBetween ( const CjrlJoint inStartJoint,
const CjrlJoint inEndJoint 
) const
pure virtual

Get the chain of joints between two joints.

Parameters
inStartJointFirst joint.
inEndJointSecond joint.

Referenced by ~CjrlDynamicRobot().

◆ jointVector()

virtual std::vector<CjrlJoint*> CjrlDynamicRobot::jointVector ( )
pure virtual

Get a vector containing all the joints.

Referenced by ~CjrlDynamicRobot().

◆ linearMomentumRobot()

virtual const vector3d& CjrlDynamicRobot::linearMomentumRobot ( )
pure virtual

Get the linear momentum of the robot.

Referenced by ~CjrlDynamicRobot().

◆ lowerBoundDof() [1/2]

virtual double CjrlDynamicRobot::lowerBoundDof ( unsigned int  inRankInConfiguration)
pure virtual

Get the lower bound for ith dof.

Referenced by ~CjrlDynamicRobot().

◆ lowerBoundDof() [2/2]

virtual double CjrlDynamicRobot::lowerBoundDof ( unsigned int  inRankInConfiguration,
const vectorN inConfig 
)
pure virtual

Compute the lower bound for ith dof using other configuration values if possible.

◆ lowerTorqueBoundDof()

virtual double CjrlDynamicRobot::lowerTorqueBoundDof ( unsigned int  inRankInConfiguration)
pure virtual

Get the lower torque bound for ith dof.

Referenced by ~CjrlDynamicRobot().

◆ lowerVelocityBoundDof()

virtual double CjrlDynamicRobot::lowerVelocityBoundDof ( unsigned int  inRankInConfiguration)
pure virtual

Get the lower velocity bound for ith dof.

Referenced by ~CjrlDynamicRobot().

◆ mass()

virtual double CjrlDynamicRobot::mass ( ) const
pure virtual

Get the total mass of the robot.

Referenced by ~CjrlDynamicRobot().

◆ numberDof()

virtual unsigned int CjrlDynamicRobot::numberDof ( ) const
pure virtual

Get the number of degrees of freedom of the robot.

Referenced by ~CjrlDynamicRobot().

◆ positionCenterOfMass()

virtual const vector3d& CjrlDynamicRobot::positionCenterOfMass ( ) const
pure virtual

Get the position of the center of mass.

Referenced by ~CjrlDynamicRobot().

◆ rootJoint() [1/2]

virtual void CjrlDynamicRobot::rootJoint ( CjrlJoint inJoint)
pure virtual

Set the root joint of the robot.

◆ rootJoint() [2/2]

virtual CjrlJoint* CjrlDynamicRobot::rootJoint ( ) const
pure virtual

Get the root joint of the robot.

Referenced by ~CjrlDynamicRobot().

◆ setActuatedJoints()

virtual void CjrlDynamicRobot::setActuatedJoints ( std::vector< CjrlJoint *> &  lActuatedJoints)
pure virtual

Specifies the list of actuated joints.

Referenced by ~CjrlDynamicRobot().

◆ setJointOrderInConfig()

virtual void CjrlDynamicRobot::setJointOrderInConfig ( std::vector< CjrlJoint *>  inJointVector)
pure virtual

Set the joint ordering in the configuration vector.

Parameters
inJointVectorVector of the robot joints

Specifies the order of the joints in the configuration vector. The vector should contain all the joints of the current robot.

Referenced by ~CjrlDynamicRobot().

◆ setProperty()

bool CjrlDynamicRobot::setProperty ( std::string &  ,
const std::string &   
)
inlinevirtual

Set property corresponding to command name.

Parameters
inPropertyname of the property.
inValuevalue of the property.
Note
The value string is obtained by writing the corresponding value in a string (operator<<).

Referenced by ~CjrlDynamicRobot().

◆ upperBoundDof() [1/2]

virtual double CjrlDynamicRobot::upperBoundDof ( unsigned int  inRankInConfiguration)
pure virtual

Get the upper bound for ith dof.

Referenced by ~CjrlDynamicRobot().

◆ upperBoundDof() [2/2]

virtual double CjrlDynamicRobot::upperBoundDof ( unsigned int  inRankInConfiguration,
const vectorN inConfig 
)
pure virtual

Compute the upper bound for ith dof using other configuration values if possible.

◆ upperTorqueBoundDof()

virtual double CjrlDynamicRobot::upperTorqueBoundDof ( unsigned int  inRankInConfiguration)
pure virtual

Get the upper torque bound for ith dof.

Referenced by ~CjrlDynamicRobot().

◆ upperVelocityBoundDof()

virtual double CjrlDynamicRobot::upperVelocityBoundDof ( unsigned int  inRankInConfiguration)
pure virtual

Get the upper velocity bound for ith dof.

Referenced by ~CjrlDynamicRobot().

◆ velocityCenterOfMass()

virtual const vector3d& CjrlDynamicRobot::velocityCenterOfMass ( )
pure virtual

Get the velocity of the center of mass.

Referenced by ~CjrlDynamicRobot().