CjrlDynamicRobot Class Reference

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

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

Inheritance diagram for CjrlDynamicRobot:

List of all members.

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

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

virtual CjrlDynamicRobot::~CjrlDynamicRobot ( ) [inline, virtual]

Destructor.


Member Function Documentation

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

Get the acceleration of the center of mass.

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

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

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.

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}}$.

virtual void CjrlDynamicRobot::computeInertiaMatrix ( ) [pure virtual]

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

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).
virtual const vectorN& CjrlDynamicRobot::currentAcceleration ( ) const [pure virtual]

Get the current acceleration of the robot.

Returns:
the acceleration vector ${\bf \ddot{q}}$.
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).
virtual const vectorN& CjrlDynamicRobot::currentConfiguration ( ) const [pure virtual]

Get the current configuration of the robot.

Returns:
the configuration vector ${\bf q}$.
virtual const matrixNxP& CjrlDynamicRobot::currentForces ( ) const [pure virtual]

Get the current forces of the robot.

Returns:
the force vector ${\bf f}$.
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 }$.
virtual const matrixNxP& CjrlDynamicRobot::currentTorques ( ) const [pure virtual]

Get the current torques of the robot.

Returns:
the torque vector ${\bf \tau }$.
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).
virtual const vectorN& CjrlDynamicRobot::currentVelocity ( ) const [pure virtual]

Get the current velocity of the robot.

Returns:
the velocity vector ${\bf \dot{q}}$.
virtual const vector3d& CjrlDynamicRobot::derivativeAngularMomentum ( ) [pure virtual]

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

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

Get the time-derivative of the linear momentum.

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

Returns the list of actuated joints.

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.
virtual bool CjrlDynamicRobot::getJacobianCenterOfMass ( const CjrlJoint inStartJoint,
matrixNxP outjacobian,
unsigned int  offset = 0,
bool  inIncludeStartFreeFlyer = true 
) [pure virtual]
virtual bool CjrlDynamicRobot::getOrientationJacobian ( const CjrlJoint inStartJoint,
const CjrlJoint inEndJoint,
matrixNxP outjacobian,
unsigned int  offset = 0,
bool  inIncludeStartFreeFlyer = true 
) [pure virtual]
virtual bool CjrlDynamicRobot::getPositionJacobian ( const CjrlJoint inStartJoint,
const CjrlJoint inEndJoint,
const vector3d inFrameLocalPosition,
matrixNxP outjacobian,
unsigned int  offset = 0,
bool  inIncludeStartFreeFlyer = true 
) [pure virtual]
bool CjrlDynamicRobot::getProperty ( const std::string &  ,
std::string &   
) const [inline, virtual]

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,...).
bool CjrlDynamicRobot::getSpecializedInverseKinematics ( const CjrlJoint ,
const CjrlJoint ,
const matrix4d ,
const matrix4d ,
vectorN  
) [inline, virtual]

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]jointRoot,:The root of the joint chain for which the specialized inverse kinematics should be computed.
[in]jointEnd,:The end of the joint chain for which the specialized inverse kinematics should be computed.
[in]jointRootPosition,:The desired position of the root.
[in]jointEndPosition,:The end position of the root.
[out]q,:Result i.e. the articular values.
virtual const matrixNxP& CjrlDynamicRobot::inertiaMatrix ( ) const [pure virtual]

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

virtual bool CjrlDynamicRobot::initialize ( ) [pure virtual]

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

bool CjrlDynamicRobot::isSupported ( const std::string &  ) [inline, virtual]

Whether the specified property in implemented.

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.
virtual std::vector<CjrlJoint*> CjrlDynamicRobot::jointVector ( ) [pure virtual]

Get a vector containing all the joints.

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

Get the linear momentum of the robot.

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

Get the lower bound for ith dof.

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.

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

Get the lower torque bound for ith dof.

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

Get the lower velocity bound for ith dof.

virtual double CjrlDynamicRobot::mass ( ) const [pure virtual]

Get the total mass of the robot.

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

Get the number of degrees of freedom of the robot.

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

Get the position of the center of mass.

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

Set the root joint of the robot.

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

Get the root joint of the robot.

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

Specifies the list of actuated joints.

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.

bool CjrlDynamicRobot::setProperty ( std::string &  ,
const std::string &   
) [inline, virtual]

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<<).
virtual double CjrlDynamicRobot::upperBoundDof ( unsigned int  inRankInConfiguration) [pure virtual]

Get the upper bound for ith dof.

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.

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

Get the upper torque bound for ith dof.

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

Get the upper velocity bound for ith dof.

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

Get the velocity of the center of mass.