#include <CDynamicsTao.hpp>

Inheritance diagram for scl::CDynamicsTao:

Collaboration diagram for scl::CDynamicsTao:

Public Member Functions
virtual bool	computeGCModel (SRobotSensors const arg_sensor_data, SGcModel arg_gc_model)

taoDNode *	getTaoIdForLink (std::string arg_link_name)

virtual sBool	integrate (SRobotIO &arg_inputs, const sFloat arg_time_interval)

sBool	setGeneralizedCoordinates (Eigen::VectorXd &arg_q)

sBool	setGeneralizedVelocities (Eigen::VectorXd &arg_dq)

sBool	setGeneralizedForces (Eigen::VectorXd &arg_fgc)

	CDynamicsTao ()

virtual bool	init (const SRobotParsed &arg_robot_data)

virtual sBool	computeTransformsForAllLinks (sutil::CMappedTree< std::string, SRigidBodyDyn > &arg_tree, const Eigen::VectorXd &arg_q)

virtual sBool	computeTransform (SRigidBodyDyn &arg_link, const Eigen::VectorXd &arg_q)

virtual sBool	computeTransformToAncestor (Eigen::Affine3d &arg_T, SRigidBodyDyn &arg_link, const SRigidBodyDyn *arg_ancestor, const Eigen::VectorXd &arg_q)

virtual sBool	computeJacobianWithTransforms (Eigen::MatrixXd &arg_J, SRigidBodyDyn &arg_link, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local)

virtual sBool	computeJacobian (Eigen::MatrixXd &arg_J, const SRigidBodyDyn &arg_link, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local) const

virtual sBool	computeJacobian (Eigen::MatrixXd &arg_J, const SRigidBodyDyn &arg_link, const SRigidBodyDyn *arg_ancestor, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local)

virtual sBool	computeJacobianTrans (Eigen::MatrixXd &arg_J, const SRigidBodyDyn &arg_link, const SRigidBodyDyn *arg_ancestor, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local)

virtual sBool	computeJacobianRot (Eigen::MatrixXd &arg_J, const SRigidBodyDyn &arg_link, const SRigidBodyDyn *arg_ancestor, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local)

virtual sBool	computeExternalContacts (sutil::CMappedList< std::string, SForce > &arg_contacts)

virtual sFloat	computeEnergyKinetic (sutil::CMappedTree< std::string, SRigidBodyDyn > &arg_tree, const Eigen::VectorXd &arg_q, const Eigen::VectorXd &arg_dq)

virtual sFloat	computeEnergyPotential (sutil::CMappedTree< std::string, SRigidBodyDyn > &arg_tree, const Eigen::VectorXd &arg_q)

virtual sBool	hasBeenInit ()

Protected Attributes
sBool	has_been_init_

const SRobotParsed *	robot_parsed_data_

Private Attributes
std::string	robot_name_

taoNodeRoot *	tao_tree_q_root_

taoNodeRoot *	tao_tree_q_dq_root_

jspace::Model *	model_

size_t	ndof_

jspace::State	state_

Eigen::Vector3d	gravity_

Detailed Description

Computes the dynamics of a robot model using the TAO dynamics engine

Constructor & Destructor Documentation

scl::CDynamicsTao::CDynamicsTao ( )

Default constructor sets the initialization state to false

Member Function Documentation

virtual sFloat scl::CDynamicsBase::computeEnergyKinetic	(	sutil::CMappedTree< std::string, SRigidBodyDyn > &	arg_tree,
		const Eigen::VectorXd &	arg_q,
		const Eigen::VectorXd &	arg_dq
	)

inlinevirtualinherited

Gets the robot's kinetic energy

Parameters

arg_tree	The tree for which the transformation matrices are to be updated
arg_q	The current generalized coordinates.
arg_dq	The current generalized velocities.

Reimplemented in scl::CDynamicsScl, scl_ext::CDynamics3d, and scl_ext::CDynamicsSclSpatial.

virtual sFloat scl::CDynamicsBase::computeEnergyPotential	(	sutil::CMappedTree< std::string, SRigidBodyDyn > &	arg_tree,
		const Eigen::VectorXd &	arg_q
	)

inlinevirtualinherited

Gets the robot's potential energy

Parameters

arg_tree	The tree for which the transformation matrices are to be updated
arg_q	The current generalized coordinates.

Reimplemented in scl::CDynamicsScl, scl_ext::CDynamicsSclSpatial, and scl_ext::CDynamics3d.

virtual sBool scl::CDynamicsBase::computeExternalContacts ( sutil::CMappedList< std::string, SForce > & arg_contacts )

inlinevirtualinherited

Calculates the collision forces for the robot to which this dynamics object is assigned.

Uses std::string based force lookup. The dynamics implementation should support this (maintain a map or something).

NOTE : The dynamics engine may delete forces from the passed mapped list at will. Do not store pointers to them at random places in your code.

As of now, this is optional for dynamics engines

Parameters

arg_contacts This is where the simulator will store the contact forces. It may use the std::string to identify when to remove or re-add forces.

Reimplemented in scl_ext::CDynamics3d.

virtual bool scl::CDynamicsTao::computeGCModel	(	SRobotSensors const *	arg_sensor_data,
		SGcModel *	arg_gc_model
	)

inlinevirtual

BUGGY => DEPRACATED!!!

Implements scl::CDynamicsBase.

virtual sBool scl::CDynamicsBase::computeJacobian	(	Eigen::MatrixXd &	arg_J,
		const SRigidBodyDyn &	arg_link,
		const Eigen::VectorXd &	arg_q,
		const Eigen::Vector3d &	arg_pos_local
	)		const

inlinevirtualinherited

Calculates the Jacobian for the robot to which this dynamics object is assigned. dx_global_origin = Jx . dq The Jacobian is specified by a link and an offset (in task space dimensions)from that link. ** CONST VERSION : NOTE : The regular version should call this. **

Parameters

arg_J	The Jacobain will be saved here.
arg_link	The link at which the Jacobian is to be calculated
arg_q	The current generalized coordinates.
arg_pos_local	The offset from the link's frame (in link coordinates).

Reimplemented in scl::CDynamicsScl.

virtual sBool scl::CDynamicsBase::computeJacobian	(	Eigen::MatrixXd &	arg_J,
		const SRigidBodyDyn &	arg_link,
		const SRigidBodyDyn *	arg_ancestor,
		const Eigen::VectorXd &	arg_q,
		const Eigen::Vector3d &	arg_pos_local
	)

inlinevirtualinherited

Calculates the Jacobian for the robot to which this dynamics object is assigned. dx_ancestor_coords = Jx . dq The Jacobian is specified by a link and an offset (in task space dimensions)from that link.

Parameters

arg_J	The Jacobain will be saved here.
arg_link	The link at which the Jacobian is to be calculated
arg_ancestor	The link up to which the Jacobian is to be calculated Pass NULL to compute the Jacobian up to the global root.
arg_q	The current generalized coordinates.
arg_pos_local	The offset from the link's frame (in link coordinates).

virtual sBool scl::CDynamicsBase::computeJacobianRot	(	Eigen::MatrixXd &	arg_J,
		const SRigidBodyDyn &	arg_link,
		const SRigidBodyDyn *	arg_ancestor,
		const Eigen::VectorXd &	arg_q,
		const Eigen::Vector3d &	arg_pos_local
	)

inlinevirtualinherited

Calculates the Jacobian for the robot to which this dynamics object is assigned. ONLY for rotation. Ie. size(Jx) = (3,ndof) dx_ancestor_coords = Jx . dq

Parameters

arg_J	The Jacobain will be saved here.
arg_link	The link at which the Jacobian is to be calculated
arg_ancestor	The link up to which the Jacobian is to be calculated Pass NULL to compute the Jacobian up to the global root.
arg_q	The current generalized coordinates.
arg_pos_local	The offset from the link's frame (in link coordinates).

virtual sBool scl::CDynamicsBase::computeJacobianTrans	(	Eigen::MatrixXd &	arg_J,
		const SRigidBodyDyn &	arg_link,
		const SRigidBodyDyn *	arg_ancestor,
		const Eigen::VectorXd &	arg_q,
		const Eigen::Vector3d &	arg_pos_local
	)

inlinevirtualinherited

Calculates the Jacobian for the robot to which this dynamics object is assigned. ONLY for translation. Ie. size(Jx) = (3,ndof) dx_ancestor_coords = Jx . dq

Parameters

arg_J	The Jacobain will be saved here.
arg_link	The link at which the Jacobian is to be calculated
arg_ancestor	The link up to which the Jacobian is to be calculated Pass NULL to compute the Jacobian up to the global root.
arg_q	The current generalized coordinates.
arg_pos_local	The offset from the link's frame (in link coordinates).

virtual sBool scl::CDynamicsBase::computeJacobianWithTransforms	(	Eigen::MatrixXd &	arg_J,
		SRigidBodyDyn &	arg_link,
		const Eigen::VectorXd &	arg_q,
		const Eigen::Vector3d &	arg_pos_local
	)

inlinevirtualinherited

Calculates the Jacobian for the robot to which this dynamics object is assigned. dx_global_origin = Jx . dq The Jacobian is specified by a link and an offset (in task space dimensions)from that link.

Parameters

arg_J	The Jacobain will be saved here.
arg_link	The link at which the Jacobian is to be calculated. Transforms are updated in this link and ancestors in the tree.
arg_q	The current generalized coordinates.
arg_pos_local	The offset from the link's frame (in link coordinates).

Reimplemented in scl::CDynamicsScl.

virtual sBool scl::CDynamicsBase::computeTransform	(	SRigidBodyDyn &	arg_link,
		const Eigen::VectorXd &	arg_q
	)

inlinevirtualinherited

Calculates the Transformation Matrix for the robot to which this dynamics object is assigned. x_parent_link_coords = arg_link.T_lnk_ * x_link_coords Note that the matrix is stored in the passed link data struct.

Parameters

arg_link	The link at which the transformation matrix is to be calculated
arg_q	The current generalized coordinates.

Reimplemented in scl::CDynamicsScl.

virtual sBool scl::CDynamicsBase::computeTransformsForAllLinks	(	sutil::CMappedTree< std::string, SRigidBodyDyn > &	arg_tree,
		const Eigen::VectorXd &	arg_q
	)

inlinevirtualinherited

Updates the Transformation Matrices for the robot to which this dynamics object is assigned. x_ancestor_link_coords = arg_link.T_lnk_ * x_link_coords

Parameters

arg_tree	The tree for which the transformation matrices are to be updated
arg_q	The current generalized coordinates.

Reimplemented in scl::CDynamicsScl.

virtual sBool scl::CDynamicsBase::computeTransformToAncestor	(	Eigen::Affine3d &	arg_T,
		SRigidBodyDyn &	arg_link,
		const SRigidBodyDyn *	arg_ancestor,
		const Eigen::VectorXd &	arg_q
	)

inlinevirtualinherited

Calculates the Transformation Matrix for the robot to which this dynamics object is assigned. x_ancestor_link_coords = arg_link.T_lnk_ * x_link_coords

Note that the local transformation matrices are updated in all the link data structs from arg_link to arg_ancestor.

Also note that there are two origins, robot origin, which actually has a node allocated for it. And the global scl origin, for which you should pass in a NULL as ancestor.

Parameters

arg_T	The transformation matrix in which to store the answer
arg_link	The link at which the transformation matrix is to be calculated
arg_ancestor	The link up to which the transformation matrix is to be calculated Pass NULL to compute the transform up to the global root.
arg_q	The current generalized coordinates.

Reimplemented in scl::CDynamicsScl.

taoDNode * scl::CDynamicsTao::getTaoIdForLink ( std::string arg_link_name )

Gives an id for a link name.

Enables using calculateJacobain without:

Storing any dynamic-engine specific objects in the controller.
Using inefficient repeated string based lookup (usually with maps)

virtual sBool scl::CDynamicsBase::hasBeenInit ( )

inlinevirtualinherited

Initialization state

bool scl::CDynamicsTao::init ( const SRobotParsed & arg_robot_data )

virtual

Initialize the CDynamicsTao with the TAO trees it needs in order to update the kinematics and/or dynamics of a robot.

Accesses the database.

Returns: True if everything went according to plan.

Implements scl::CDynamicsBase.

sBool scl::CDynamicsTao::integrate	(	SRobotIO &	arg_inputs,
		const sFloat	arg_time_interval
	)

virtual

Integrates the robot's state. Uses the given applied forces, torques, positions and velocities and its internal dynamic model to compute new positions and velocities.

This version performs the entire set of operations on the SRobotIO data structure.

Reads from, and updates: arg_inputs_.sensors_.q_ arg_inputs_.sensors_.dq_ arg_inputs_.sensors_.ddq_

Reads from: arg_inputs_.actuators_.forces_ arg_inputs_.actuators_.force_gc_commanded_

Parameters

arg_inputs	The existing generalized coordinates, velocities and accelerations + The generalized forces + task (euclidean) forces and the list of contact points and links.
arg_time_interval	The time across which the system should integrate the dynamics.

Tao uses a forward euler integrator and uses this as the dt so set it to a small value.

Reimplemented from scl::CDynamicsBase.

sBool scl::CDynamicsTao::setGeneralizedCoordinates ( Eigen::VectorXd & arg_q )

Sets the generalized coordinates

TODO : This dynamics engine implementation should be stateless. This present system is because the intermediate jspace implementation has an extra data layer. Should be cleaned up in the future.

sBool scl::CDynamicsTao::setGeneralizedForces ( Eigen::VectorXd & arg_fgc )

Sets the external generalized forces

TODO : This dynamics engine implementation should be stateless. This present system is because the intermediate jspace implementation has an extra data layer. Should be cleaned up in the future.

sBool scl::CDynamicsTao::setGeneralizedVelocities ( Eigen::VectorXd & arg_dq )

Sets the generalized velocities

TODO : This dynamics engine implementation should be stateless. This present system is because the intermediate jspace implementation has an extra data layer. Should be cleaned up in the future.

Member Data Documentation

Eigen::Vector3d scl::CDynamicsTao::gravity_

private

The gravity acting on the robot

sBool scl::CDynamicsBase::has_been_init_

protectedinherited

True if the dynamics object has been initialized for a given robot

jspace::Model* scl::CDynamicsTao::model_

private

The joint space model, a helper class that wraps around tao and computes the robot's dynamics.

size_t scl::CDynamicsTao::ndof_

private

The robot's degrees of freedom

std::string scl::CDynamicsTao::robot_name_

private

The robot's name

const SRobotParsed* scl::CDynamicsBase::robot_parsed_data_

protectedinherited

A read-only pointer to access parsed data

jspace::State scl::CDynamicsTao::state_

private

The current state of the robot

taoNodeRoot* scl::CDynamicsTao::tao_tree_q_root_

private

Store pointers to the tao trees

The documentation for this class was generated from the following files:

/home/samir/Code/control/scl.git/src/scl/dynamics/tao/CDynamicsTao.hpp
/home/samir/Code/control/scl.git/src/scl/dynamics/tao/CDynamicsTao.cpp

Public Member Functions

Protected Attributes

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation