Stanford Whole-Body Control Framework: jspace::Model Class Reference

List of all members.


Public Member Functions
	Model ()
	Please use the init() method in order to initialize your jspace::Model.
	~Model ()
int	init (tao_tree_info_s kgm_tree, tao_tree_info_s cc_tree, std::ostream *msg)
	Initialize the model with a TAO tree.
void	update (State const &state)
	Calls setState(), updateKinematics(), and updateDynamics().
void	setState (State const &state)
	Inform the model about the joint state.
State const &	getState () const
	Retrieve the state passed to setState() (or update(), for that matter).
size_t	getNNodes () const
	Compute or retrieve the cached number of nodes in the robot.
size_t	getNJoints () const
	Compute or retrieve the cached number of joints in the robot.
size_t	getNDOF () const
	Compute or retrieve the cached number of degrees of freedom of the robot.
std::string	getNodeName (size_t id) const
	Retrieve the name of a node.
std::string	getJointName (size_t id) const
	Retrieve the name of a joint.
taoDNode *	getNode (size_t id) const
	Retrieve a node by ID.
taoDNode *	getNodeByName (std::string const &name) const
	Retrieve a node by its name.
taoDNode *	getNodeByJointName (std::string const &name) const
	Retrieve a node by joint name.
void	getJointLimits (Vector &joint_limits_lower, Vector &joint_limits_upper) const
	Retrieve joint limit information.
void	updateKinematics ()
	Computes the node origins wrt the global frame.
bool	getGlobalFrame (taoDNode const *node, Transform &global_transform) const
	Retrieve the frame (translation and rotation) of a node origin.
bool	computeGlobalFrame (taoDNode const *node, Transform const &local_transform, Transform &global_transform) const
	Compute the global frame (translation and rotation) corresponding to a local frame expressed wrt the origin of a given node.
bool	computeGlobalFrame (taoDNode const *node, double local_x, double local_y, double local_z, Transform &global_transform) const
	Convenience method in case you are only interested in the translational part and hold the local point in three doubles.
bool	computeGlobalFrame (taoDNode const *node, Vector const &local_translation, Transform &global_transform) const
	Convenience method in case you are only interested in the translational part and hold the local point in a three-dimensional vector.
bool	computeGlobalCOMFrame (taoDNode const *node, Transform &global_com_transform) const
bool	computeJacobian (taoDNode const *node, Matrix &jacobian) const
	Compute the Jacobian (J_v over J_omega) at the origin of a given node.
bool	computeJacobian (taoDNode const *node, double gx, double gy, double gz, Matrix &jacobian) const
	Compute the Jacobian (J_v over J_omega) for a given node, at a point expressed wrt to the global frame.
bool	computeJacobian (taoDNode const *node, Vector const &global_point, Matrix &jacobian) const
	Convenience method in case you are holding the global position in a three-dimensional vector.
void	updateDynamics ()
	Calls computeGravity(), computeCoriolisCentrifugal(), computeMassInertia(), and computeInverseMassInertia().
bool	computeCOM (Vector &com, Matrix *opt_jcom) const
	Computes the location of the center of gravity, and optionally also its Jacobian.
void	computeGravity ()
	Compute the gravity joint-torque vector.
bool	disableGravityCompensation (size_t index, bool disable)
	Disable (or enable) gravity compensation for a given DOF (specified using its index).
bool	getGravity (Vector &gravity) const
	Retrieve the gravity joint-torque vector.
void	computeCoriolisCentrifugal ()
	Compute the Coriolis and contrifugal joint-torque vector.
bool	getCoriolisCentrifugal (Vector &coriolis_centrifugal) const
	Retrieve the Coriolis and contrifugal joint-torque vector.
void	computeMassInertia ()
	Compute the joint-space mass-inertia matrix, a.k.a.
bool	getMassInertia (Matrix &mass_inertia) const
	Retrieve the joint-space mass-inertia matrix, a.k.a.
void	computeInverseMassInertia ()
	Compute the inverse joint-space mass-inertia matrix.
bool	getInverseMassInertia (Matrix &inverse_mass_inertia) const
	Retrieve the inverse joint-space mass-inertia matrix.
tao_tree_info_s *	_getKGMTree ()
	For debugging only, access to the kinematics-gravity-mass-inertia tree.
tao_tree_info_s *	_getCCTree ()
	For debugging only, access to the optional Coriolis-centrifugal tree.
Classes
struct	ancestry_entry_s

Detailed Description

Constructor & Destructor Documentation

jspace::Model::Model ( )

Please use the init() method in order to initialize your jspace::Model.

It does some sanity checking, and error handling from within a constructor is just not so great.

Definition at line 60 of file Model.cpp.

jspace::Model::~Model ( )

Definition at line 141 of file Model.cpp.

Member Function Documentation

int jspace::Model::init	(	tao_tree_info_s *	kgm_tree,
		tao_tree_info_s *	cc_tree,
		std::ostream *	msg
	)

Initialize the model with a TAO tree.

Actually, it needs two copies of the same tree if you want to estimate centrifugal and Coriolis effects, but that is optional.

This method also does some sanity checks and will return a non-zero error code if something is amiss. In order to get human-readable error strings, just pass a non-NULL msg pointer in as well. For instance, &std::cout will do nicely in most cases.

Note:: Transfers ownership of the given TAO trees. They will be deleted when this jspace::Model instance is destructed. Also note that their info vector might get reordered in order to ensure that each node sits at the index that corresponds to its ID.

Returns:: 0 on success.

Parameters:

kgm_tree	TAO tree info used for computing kinematics, the gravity torque vector, the mass-inertia matrix, and its inverse. This tree will be deleted in the jspace::Model destructor.
cc_tree	Optional TAO tree info for computing Coriolis and centrifugal torques. If you set this to NULL, then the Coriolis and centrifugal forces won't be computed. This tree will be deleted in the jspace::Model destructor.
msg	Optional stream that will receive error messages from the consistency checks.

Definition at line 69 of file Model.cpp.

References jspace::tao_tree_info_s::info, jspace::tao_tree_info_s::root, jspace::tao_tree_info_s::sort(), and jspace::tao_consistency_check().

Here is the call graph for this function:

void jspace::Model::update ( State const & state )

Calls setState(), updateKinematics(), and updateDynamics().

After calling the update() method, you can use any of the other methods without worrying whether you have already called the corresponding computeFoo() method.

Note:: The given state has to have the correct dimensions, but this is not checked by the implementation. If the given state has too few dimensions, then some positions and velocities of the model will remain at their old values. If there are too many dimensions, they will be ignored.

Definition at line 149 of file Model.cpp.

References setState(), updateDynamics(), and updateKinematics().

Referenced by get_puma().

Here is the call graph for this function:

void jspace::Model::setState ( State const & state )

Inform the model about the joint state.

We have to separate the state update from the computation of the various quantities in order to efficiently use the TAO tree representation, which forces us to distribute the state over its nodes before computing the model.

Note:: The given state has to have the correct dimensions, but this is not checked by the implementation. If the given state has too few dimensions, then some positions and velocities of the model will remain at their old values. If there are too many dimensions, they will be ignored.

Definition at line 158 of file Model.cpp.

References jspace::tao_tree_info_s::info.

Referenced by update().

State const& jspace::Model::getState ( ) const [inline]

Retrieve the state passed to setState() (or update(), for that matter).

Definition at line 124 of file Model.hpp.

Referenced by opspace::ClassicTaskPostureController::computeCommand(), jspace::JointGoalController::computeCommand(), tut04::JTask::init(), tut03::JTask::init(), tut02::JTask::init(), opspace::JPosTrjTask::init(), opspace::SelectedJointPostureTask::init(), opspace::JPosTask::init(), opspace::DraftPIDTask::init(), jspace::GoalControllerBase::init(), jspace::JointGoalController::latch(), tut04::JTask::update(), tut03::JTask::update(), tut02::JTask::update(), opspace::JointLimitTask::update(), opspace::JPosTrjTask::update(), opspace::CartPosTrjTask::update(), opspace::SelectedJointPostureTask::update(), opspace::JPosTask::update(), opspace::CartPosTask::update(), opspace::DraftPIDTask::update(), opspace::OrientationTask::updateActual(), opspace::JointLimitTask::updateState(), and tut05::PTask::updateStateAndJacobian().

size_t jspace::Model::getNNodes ( ) const

Compute or retrieve the cached number of nodes in the robot.

Note:

the root node is NOT included in this count.
in principle, each node can have any number of joints, and each joint can have any number of degrees of freedom, which is why getNJoints() and getNDOF() might come in handy, too.

Definition at line 181 of file Model.cpp.

size_t jspace::Model::getNJoints ( ) const

Compute or retrieve the cached number of joints in the robot.

Note that each joint can have any number of degrees of freedom, which is why getNDOF() might come in handy, too.

Definition at line 188 of file Model.cpp.

Referenced by jspace::jspace_controller_info_getter_s::getDOFNames(), and jspace::jspace_controller_info_getter_s::getDOFUnits().

size_t jspace::Model::getNDOF ( ) const

Compute or retrieve the cached number of degrees of freedom of the robot.

Note that each joint can have any number of degrees of freedom, which is why this method might return something else than getNJoints().

Definition at line 196 of file Model.cpp.

Referenced by opspace::ClassicTaskPostureController::computeCommand(), jspace::JointGoalController::computeCommand(), get_puma(), tut04::JTask::init(), tut03::JTask::init(), tut02::JTask::init(), opspace::JointLimitTask::init(), opspace::JPosTrjTask::init(), opspace::SelectedJointPostureTask::init(), opspace::JPosTask::init(), opspace::DraftPIDTask::init(), jspace::GoalControllerBase::init(), and opspace::JointLimitTask::updateState().

std::string jspace::Model::getNodeName ( size_t id ) const

Retrieve the name of a node.

Returns an empty string in case the id is invalid. Use getNNodes() to find out how many nodes there are.

Definition at line 204 of file Model.cpp.

References jspace::tao_tree_info_s::info.

std::string jspace::Model::getJointName ( size_t id ) const

Retrieve the name of a joint.

Returns an empty string in case the id is invalid. Use getNJoints() to find out how many joints there are.

Todo:: A joint can have any number of DOF, which means there should be a way to get at them individually, but currently we only support exactly one 1-DOF joints per node.

Definition at line 215 of file Model.cpp.

References jspace::tao_tree_info_s::info.

Referenced by jspace::jspace_controller_info_getter_s::getDOFNames().

taoDNode * jspace::Model::getNode ( size_t id ) const

Retrieve a node by ID.

Definition at line 226 of file Model.cpp.

References jspace::tao_tree_info_s::info.

Referenced by jspace::jspace_controller_info_getter_s::getDOFUnits(), opspace::OrientationTask::updateActual(), opspace::CartPosTrjTask::updateActual(), and tut05::PTask::updateStateAndJacobian().

taoDNode * jspace::Model::getNodeByName ( std::string const & name ) const

Retrieve a node by its name.

Todo:: Add support for registering aliases, such as "end-effector" or "End_Effector", which might correspond to "right-gripper" or so depending on the robot.

Definition at line 236 of file Model.cpp.

References jspace::tao_tree_info_s::info.

Referenced by opspace::CartPosTask::updateActual().

taoDNode * jspace::Model::getNodeByJointName ( std::string const & name ) const

Retrieve a node by joint name.

This will find and retrieve the node to which the joint is attached (see also getNodeByName()), which allows you to retrieve the taoJoint instance itself.

Note:: In principle, a taoDNode can have any number of joints, so you might have to search through them to find the exact one you're looking for. However, all use cases so far seem to be limited to exactly one joint per node (and each node having exactly one joint).

Todo:: Add support for registering aliases, just as for getNodeByName().

Definition at line 248 of file Model.cpp.

References jspace::tao_tree_info_s::info.

void jspace::Model::getJointLimits	(	Vector &	joint_limits_lower,
		Vector &	joint_limits_upper
	)			const

Retrieve joint limit information.

This method fills the provided vectors with the lower and upper joint limits. In case no joint limit information is available, it sets the lower limit to std::numeric_limits<double>::min() and the upper limit to std::numeric_limits<double>::max().

Definition at line 260 of file Model.cpp.

References jspace::tao_tree_info_s::info.

Referenced by jspace::jspace_controller_info_getter_s::getLimits().

void jspace::Model::updateKinematics ( )

Computes the node origins wrt the global frame.

Definition at line 273 of file Model.cpp.

References jspace::tao_tree_info_s::root.

Referenced by update().

bool jspace::Model::getGlobalFrame	(	taoDNode const *	node,
		Transform &	global_transform
	)			const

Retrieve the frame (translation and rotation) of a node origin.

Returns:: True on success. The only possible failure stems from an invalid node, so if you got that using getNode() or one of the related methods you can safely ignore the return value.

Definition at line 285 of file Model.cpp.

Referenced by computeGlobalCOMFrame(), and computeGlobalFrame().

bool jspace::Model::computeGlobalFrame	(	taoDNode const *	node,
		Transform const &	local_transform,
		Transform &	global_transform
	)			const

Compute the global frame (translation and rotation) corresponding to a local frame expressed wrt the origin of a given node.

Returns:: True on success. The only possible failure stems from an invalid node, so if you got that using getNode() or one of the related methods you can safely ignore the return value.

Definition at line 307 of file Model.cpp.

References getGlobalFrame().

Referenced by computeGlobalCOMFrame(), opspace::OrientationTask::updateActual(), opspace::CartPosTrjTask::updateActual(), opspace::CartPosTask::updateActual(), and tut05::PTask::updateStateAndJacobian().

Here is the call graph for this function:

bool jspace::Model::computeGlobalFrame	(	taoDNode const *	node,
		double	local_x,
		double	local_y,
		double	local_z,
		Transform &	global_transform
	)			const

Convenience method in case you are only interested in the translational part and hold the local point in three doubles.

The copmuted global_transform will have the same rotational component as the node's origin.

Returns:: True on success. The only possible failure stems from an invalid node, so if you got that using getNode() or one of the related methods you can safely ignore the return value.

Definition at line 333 of file Model.cpp.

References getGlobalFrame().

Here is the call graph for this function:

bool jspace::Model::computeGlobalFrame	(	taoDNode const *	node,
		Vector const &	local_translation,
		Transform &	global_transform
	)			const

Convenience method in case you are only interested in the translational part and hold the local point in a three-dimensional vector.

The copmuted global_transform will have the same rotational component as the node's origin.

Returns:: True on success. The only possible failure stems from an invalid node, so if you got that using getNode() or one of the related methods you can safely ignore the return value.

Definition at line 320 of file Model.cpp.

References getGlobalFrame().

Here is the call graph for this function:

bool jspace::Model::computeGlobalCOMFrame	(	taoDNode const *	node,
		Transform &	global_com_transform
	)			const

Definition at line 346 of file Model.cpp.

References computeGlobalFrame(), and getGlobalFrame().

Here is the call graph for this function:

bool jspace::Model::computeJacobian	(	taoDNode const *	node,
		Matrix &	jacobian
	)			const

Compute the Jacobian (J_v over J_omega) at the origin of a given node.

Note:: This just ends up calling the other computeJacobian() which takes a global point as argument, passing in the origin of the given node.

Returns:: True on success. There are two possible failures: an invalid node, or an unsupported joint type. If you got the node using getNode() or one of the related methods, then you need to extend this implementation when it returns false.

Definition at line 366 of file Model.cpp.

Referenced by computeCOM(), computeJacobian(), opspace::CartPosTrjTask::update(), opspace::CartPosTask::update(), opspace::OrientationTask::updateActual(), and tut05::PTask::updateStateAndJacobian().

bool jspace::Model::computeJacobian	(	taoDNode const *	node,
		double	gx,
		double	gy,
		double	gz,
		Matrix &	jacobian
	)			const

Compute the Jacobian (J_v over J_omega) for a given node, at a point expressed wrt to the global frame.

Todo:: Implement support for more than one joint per node, and more than one DOF per joint.

Returns:: True on success. There are two possible failures: an invalid node, or an unsupported joint type. If you got the node using getNode() or one of the related methods, then you need to extend this implementation when it returns false.

Definition at line 378 of file Model.cpp.

bool jspace::Model::computeJacobian	(	taoDNode const *	node,
		Vector const &	global_point,
		Matrix &	jacobian
	)			const `[inline]`

Convenience method in case you are holding the global position in a three-dimensional vector.

Definition at line 281 of file Model.hpp.

References computeJacobian().

Here is the call graph for this function:

void jspace::Model::updateDynamics ( )

Calls computeGravity(), computeCoriolisCentrifugal(), computeMassInertia(), and computeInverseMassInertia().

Definition at line 440 of file Model.cpp.

References computeCoriolisCentrifugal(), computeGravity(), computeInverseMassInertia(), and computeMassInertia().

Referenced by update().

Here is the call graph for this function:

bool jspace::Model::computeCOM	(	Vector &	com,
		Matrix *	opt_jcom
	)			const

Computes the location of the center of gravity, and optionally also its Jacobian.

Pass opt_jcom=0 if you are not interested in the Jacobian. Failures can only be due to calls of computeJacobian() that happens for each node's contribution to the Jacobian of the COM.

Definition at line 450 of file Model.cpp.

References computeJacobian(), and jspace::tao_tree_info_s::info.

Here is the call graph for this function:

void jspace::Model::computeGravity ( )

Compute the gravity joint-torque vector.

Definition at line 484 of file Model.cpp.

References earth_gravity(), jspace::tao_tree_info_s::info, and jspace::tao_tree_info_s::root.

Referenced by updateDynamics().

Here is the call graph for this function:

bool jspace::Model::disableGravityCompensation	(	size_t	index,
		bool	disable
	)

Disable (or enable) gravity compensation for a given DOF (specified using its index).

If you set disable to true, then getGravity() will knock out (set to zero) the corresponding entry of the gravity joint-torque vector.

Note:: Invalid indices are silently ignore, and true is returned. Valid indices are 0<=index<getNDOF().

Returns:: The previous value of disable for this joint.

Definition at line 495 of file Model.cpp.

bool jspace::Model::getGravity ( Vector & gravity ) const

Retrieve the gravity joint-torque vector.

Returns:: True on success. The only possibility of receiving false is if you never called updateDynamics(), which gets called by updateDynamics(), which gets called by update().

Definition at line 520 of file Model.cpp.

Referenced by opspace::ClassicTaskPostureController::computeCommand(), jspace::JointGoalController::computeCommand(), jspace::FloatController::computeCommand(), tut04::JTask::update(), and tut03::JTask::update().

void jspace::Model::computeCoriolisCentrifugal ( )

Compute the Coriolis and contrifugal joint-torque vector.

If you set cc_tree=NULL in the constructor, then this is a no-op.

Definition at line 536 of file Model.cpp.

References jspace::tao_tree_info_s::info, jspace::tao_tree_info_s::root, and zero_gravity().

Referenced by updateDynamics().

Here is the call graph for this function:

bool jspace::Model::getCoriolisCentrifugal ( Vector & coriolis_centrifugal ) const

Retrieve the Coriolis and contrifugal joint-torque vector.

Returns:: True on success. There are two possibility of receiving false: (i) you set cc_tree=NULL in the constructor, or (ii) you never called computeCoriolisCentrifugal(), which gets called by updateDynamics(), which gets called by update().

Definition at line 549 of file Model.cpp.

Referenced by jspace::JointGoalController::computeCommand(), and tut04::JTask::update().

void jspace::Model::computeMassInertia ( )

Compute the joint-space mass-inertia matrix, a.k.a.

the kinetic energy matrix.

Definition at line 563 of file Model.cpp.

References jspace::tao_tree_info_s::info, jspace::tao_tree_info_s::root, squareToTriangularIndex(), and zero_gravity().

Referenced by updateDynamics().

Here is the call graph for this function:

bool jspace::Model::getMassInertia ( Matrix & mass_inertia ) const

Retrieve the joint-space mass-inertia matrix, a.k.a.

the kinetic energy matrix.

Returns:: True on success. The only possibility of receiving false is if you never called computeMassInertia(), which gets called by updateDynamics(), which gets called by update().

Definition at line 600 of file Model.cpp.

References squareToTriangularIndex().

Referenced by jspace::JointGoalController::computeCommand(), and tut04::JTask::update().

Here is the call graph for this function:

void jspace::Model::computeInverseMassInertia ( )

Compute the inverse joint-space mass-inertia matrix.

Definition at line 621 of file Model.cpp.

References jspace::tao_tree_info_s::info, jspace::tao_tree_info_s::root, squareToTriangularIndex(), and zero_gravity().

Referenced by updateDynamics().

Here is the call graph for this function:

bool jspace::Model::getInverseMassInertia ( Matrix & inverse_mass_inertia ) const

Retrieve the inverse joint-space mass-inertia matrix.

Returns:: True on success. The only possibility of receiving false is if you never called computeMassInertia(), which gets called by updateDynamics(), which gets called by update().

Definition at line 657 of file Model.cpp.

References squareToTriangularIndex().

Referenced by opspace::ClassicTaskPostureController::computeCommand().

Here is the call graph for this function:

tao_tree_info_s* jspace::Model::_getKGMTree ( ) [inline]

For debugging only, access to the kinematics-gravity-mass-inertia tree.

Definition at line 360 of file Model.hpp.

tao_tree_info_s* jspace::Model::_getCCTree ( ) [inline]

For debugging only, access to the optional Coriolis-centrifugal tree.

Can NULL if the user is not interested in Coriolis-centrifugal effects.

Definition at line 365 of file Model.hpp.

jspace::Model Class Reference

Public Member Functions

Classes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation