#include <RobotModelRBDL.hpp>

Inheritance diagram for wbc::RobotModelRBDL:

Public Member Functions
	RobotModelRBDL ()

virtual	~RobotModelRBDL ()

virtual bool	configure (const RobotModelConfig &cfg)
	This will read the robot model from the given URDF file and initialize all members accordingly.

virtual void	update (const Eigen::VectorXd &joint_positions, const Eigen::VectorXd &joint_velocities, const Eigen::VectorXd &joint_accelerations, const types::Pose &fb_pose, const types::Twist &fb_twist, const types::SpatialAcceleration &fb_acc)
	Update kinematics/dynamics.

virtual const types::Pose &	pose (const std::string &frame_id)

virtual const types::Twist &	twist (const std::string &frame_id)

virtual const types::SpatialAcceleration &	acceleration (const std::string &frame_id)

virtual const Eigen::MatrixXd &	spaceJacobian (const std::string &frame_id)
	Returns the Space Jacobian for the kinematic chain between root and the tip frame as full body Jacobian. Size of the Jacobian will be 6 x nJoints, where nJoints is the number of joints of the whole robot. The order of the columns will be the same as the joint order of the robot. The columns that correspond to joints that are not part of the kinematic chain will have only zeros as entries.

virtual const Eigen::MatrixXd &	bodyJacobian (const std::string &frame_id)
	Returns the Body Jacobian for the kinematic chain between root and the tip frame as full body Jacobian. Size of the Jacobian will be 6 x nJoints, where nJoints is the number of joints of the whole robot. The order of the columns will be the same as the joint order of the robot. The columns that correspond to joints that are not part of the kinematic chain will have only zeros as entries.

virtual const Eigen::MatrixXd &	comJacobian ()
	Returns the CoM Jacobian for the entire robot, which maps the robot joint velocities to linear spatial velocities in robot Eigen coordinates. Size of the Jacobian will be 3 x nJoints, where nJoints is the number of joints of the whole robot. The order of the columns will be the same as the configured joint order of the robot.

virtual const types::SpatialAcceleration &	spatialAccelerationBias (const std::string &frame_id)
	Returns the spatial acceleration bias, i.e. the term Jdot*qdot.

virtual const Eigen::MatrixXd &	jointSpaceInertiaMatrix ()

virtual const Eigen::VectorXd &	biasForces ()

virtual const types::RigidBodyState &	centerOfMass ()
	Return Current center of gravity in expressed Eigen frame.

virtual const Eigen::VectorXd &	inverseDynamics (const Eigen::VectorXd &qdd_ref=Eigen::VectorXd(), const std::vector< types::Wrench > &f_ext=std::vector< types::Wrench >())
	Compute tau from internal state.

Public Member Functions inherited from wbc::RobotModel
	RobotModel ()

virtual	~RobotModel ()

void	update (const Eigen::VectorXd &joint_positions, const Eigen::VectorXd &joint_velocities)
	Update kinematics/dynamics for fixed base robots. Joint acceleration will be assumed zero.

virtual void	update (const Eigen::VectorXd &joint_positions, const Eigen::VectorXd &joint_velocities, const Eigen::VectorXd &joint_accelerations)
	Update kinematics/dynamics for fixed base robots.

void	update (const Eigen::VectorXd &joint_positions, const Eigen::VectorXd &joint_velocities, const types::Pose &fb_pose, const types::Twist &fb_twist)
	Update kinematics/dynamics for floating base robots. Joint and spatial acceleration will be assumed zero.

const types::JointState &	jointState ()

const std::vector< std::string > &	jointNames ()
	Return all joint names excluding the floating base. This will be.

const std::vector< std::string > &	actuatedJointNames ()
	Return only actuated joint names.

const std::vector< std::string > &	independentJointNames ()
	Return only independent joint names.

uint	jointIndex (const std::string &joint_name)
	Get the internal index for a joint name.

const std::string &	baseFrame ()
	Get the base frame of the robot, i.e. the root of the URDF model.

const std::string &	worldFrame ()
	Get the world frame id. Will be "world" in case of floating base robot and equal to base_frame in case of fixed base robot.

const types::JointLimits	jointLimits ()
	Return current joint limits.

const Eigen::MatrixXd &	selectionMatrix ()
	Return current selection matrix, mapping actuated to all dof.

bool	hasLink (const std::string &link_name)
	Return True if given link name is available in robot model, false otherwise.

bool	hasJoint (const std::string &joint_name)
	Return True if given joint name is available in robot model, false otherwise.

bool	hasActuatedJoint (const std::string &joint_name)
	Return True if given joint name is an actuated joint in robot model, false otherwise.

void	setContacts (const std::vector< types::Contact > &contacts)
	Set new contact points.

const std::vector< types::Contact > &	getContacts ()
	get current contact points

uint	nj ()
	Return number of joints.

uint	na ()
	Return number of actuated joints.

uint	nfb ()
	Return dof of the floating base, will be either 0 (floating_base == false) or 6 (floating_base = = false)

uint	nac ()

uint	nc ()
	Return total number of contact points.

const Eigen::VectorXd &	getQ ()
	Return full system position vector, incl. floating base. Convention [floating_base_pos, floating_base_ori, joint_positions]: [fb_x,fb_y,fb_z, fb_qx,fb_qy,fb_qz,fb_qw, q_1 ... q_n)].

const Eigen::VectorXd &	getQd ()
	Return full system velocity vector, incl. floating base. Convention [floating_base_vel, floating_base_rot_vel, joint_velocities]: [fb_vx,fb_vx,fb_vx, fb_wx,fb_wy,fb_wz, qd_1 ... qd_n)].

const Eigen::VectorXd &	getQdd ()
	Return full system acceleration vector, incl. floating base. Convention [floating_base_acc, floating_base_rot_acc, joint_accelerations]: [fb_dvx,fb_dvx,fb_dvx, fb_dwx,fb_dwy,fb_dwz, qdd_1 ... qdd_n)].

void	setGravityVector (const Eigen::Vector3d &g)
	Set the current gravity vector. Default is (0,0,-9.81)

const types::RigidBodyState &	floatingBaseState ()
	Get current status of floating base.

const RobotModelConfig &	getRobotModelConfig ()
	Get current robot model config.

bool	hasFloatingBase ()
	Is is a floating base robot?

urdf::ModelInterfaceSharedPtr	loadRobotURDF (const std::string &file_or_string)
	Load URDF model from either file or string.

urdf::ModelInterfaceSharedPtr	getURDFModel ()
	Return current URDF model.

void	setJointWeights (const Eigen::VectorXd &weights)
	set Joint weights by given name

const Eigen::VectorXd &	getJointWeights () const
	Get Joint weights as Named vector.

Protected Member Functions
std::vector< std::string >	jointNamesInRBDLOrder (const std::string &urdf_file)

void	clear ()

void	updateFK (Eigen::VectorXd &_q, Eigen::VectorXd &_qd, Eigen::VectorXd &_qdd)

void	updateFK (Eigen::VectorXd &_q, Eigen::VectorXd &_qd)

Protected Member Functions inherited from wbc::RobotModel
void	clear ()

void	resetData ()

void	updateData ()

Protected Attributes
std::shared_ptr< RigidBodyDynamics::Model >	rbdl_model

Eigen::VectorXd	tau

RigidBodyDynamics::Math::MatrixNd	J

RigidBodyDynamics::Math::MatrixNd	H_q

Protected Attributes inherited from wbc::RobotModel
std::vector< types::Contact >	contacts

Eigen::Vector3d	gravity

types::RigidBodyState	floating_base_state

RobotModelConfig	robot_model_config

std::string	world_frame

std::string	base_frame

types::JointLimits	joint_limits

std::vector< std::string >	actuated_joint_names

std::vector< std::string >	independent_joint_names

std::vector< std::string >	joint_names

urdf::ModelInterfaceSharedPtr	robot_urdf

bool	has_floating_base

types::JointState	joint_state

Eigen::MatrixXd	selection_matrix

Eigen::VectorXd	q

Eigen::VectorXd	qd

Eigen::VectorXd	qdd

Eigen::VectorXd	tau

Eigen::VectorXd	zero_jnt

std::vector< Matrix >	com_jac

std::vector< Matrix >	joint_space_inertia_mat

std::vector< Vector >	bias_forces

std::vector< RigidBodyState >	com_rbs

std::map< std::string, Matrix >	space_jac_map

std::map< std::string, Matrix >	body_jac_map

std::map< std::string, Pose >	pose_map

std::map< std::string, Twist >	twist_map

std::map< std::string, SpatialAcceleration >	acc_map

std::map< std::string, SpatialAcceleration >	spatial_acc_bias_map

bool	fk_needs_recompute

bool	fk_with_zero_acc_needs_recompute

Eigen::VectorXd	joint_weights

types::SpatialAcceleration	zero_acc

bool	configured

bool	updated

Static Protected Attributes
static RobotModelRegistry< RobotModelRBDL >	reg

Constructor & Destructor Documentation

◆ RobotModelRBDL()

wbc::RobotModelRBDL::RobotModelRBDL ( )

◆ ~RobotModelRBDL()

wbc::RobotModelRBDL::~RobotModelRBDL ( )

virtual

Member Function Documentation

◆ acceleration()

const types::SpatialAcceleration & wbc::RobotModelRBDL::acceleration ( const std::string & frame_id )

virtual

Returns the spatial acceleration of the body defined by frame_id. Twist will be in "local-world-aligned" (hybrid) representation, i.e., with respect to a frame attached to the given body, but aligned to world coordinates (floating base robot) or robot root link (fixed base)

Implements wbc::RobotModel.

◆ biasForces()

const Eigen::VectorXd & wbc::RobotModelRBDL::biasForces ( )

virtual

Compute and return the bias force vector, which is nj x 1, where nj is the number of joints of the system

Implements wbc::RobotModel.

◆ bodyJacobian()

const Eigen::MatrixXd & wbc::RobotModelRBDL::bodyJacobian ( const std::string & frame_id )

virtual

Returns the Body Jacobian for the kinematic chain between root and the tip frame as full body Jacobian. Size of the Jacobian will be 6 x nJoints, where nJoints is the number of joints of the whole robot. The order of the columns will be the same as the joint order of the robot. The columns that correspond to joints that are not part of the kinematic chain will have only zeros as entries.

Parameters

root_frame	Root frame of the chain. Has to be a valid link in the robot model.
frame_id	Tip frame of the chain. Has to be a valid link in the robot model.

Implements wbc::RobotModel.

◆ centerOfMass()

const types::RigidBodyState & wbc::RobotModelRBDL::centerOfMass ( )

virtual

Return Current center of gravity in expressed Eigen frame.

Implements wbc::RobotModel.

◆ clear()

void wbc::RobotModelRBDL::clear ( )

protected

reset model

◆ comJacobian()

const Eigen::MatrixXd & wbc::RobotModelRBDL::comJacobian ( )

virtual

Returns the CoM Jacobian for the entire robot, which maps the robot joint velocities to linear spatial velocities in robot Eigen coordinates. Size of the Jacobian will be 3 x nJoints, where nJoints is the number of joints of the whole robot. The order of the columns will be the same as the configured joint order of the robot.

Returns: A 3xN matrix, where N is the number of robot joints

Implements wbc::RobotModel.

◆ configure()

bool wbc::RobotModelRBDL::configure ( const RobotModelConfig & cfg )

virtual

This will read the robot model from the given URDF file and initialize all members accordingly.

Parameters

cfg	Model configuration. See RobotModelConfig.hpp for details

Returns: True in case of success, else false

Implements wbc::RobotModel.

◆ inverseDynamics()

const Eigen::VectorXd & wbc::RobotModelRBDL::inverseDynamics	(	const Eigen::VectorXd &	qdd_ref = Eigen::VectorXd(),
		const std::vector< types::Wrench > &	f_ext = std::vector<types::Wrench>() )

virtual

Compute tau from internal state.

Parameters

qdd_ref	(Optional) Desired reference joint acceleration (incl. floating base), if empty actual acceleration will be used
f_ext	(Optional) Contact wrenches, if not empty, size has to match the number of contact points. All wrenches will be in local coordinates, but aligned to world

Implements wbc::RobotModel.

◆ jointNamesInRBDLOrder()

std::vector< std::string > wbc::RobotModelRBDL::jointNamesInRBDLOrder ( const std::string & urdf_file )

protected

◆ jointSpaceInertiaMatrix()

const Eigen::MatrixXd & wbc::RobotModelRBDL::jointSpaceInertiaMatrix ( )

virtual

Compute and return the joint space mass-inertia matrix, which is nj x nj, where nj is the number of joints of the system

Implements wbc::RobotModel.

◆ pose()

const types::Pose & wbc::RobotModelRBDL::pose ( const std::string & frame_id )

virtual

Returns the pose of the body defined by frame_id. Pose will be computed with respect to world frame (floating base robot) or robot root link (fixed base)

Implements wbc::RobotModel.

◆ spaceJacobian()

const Eigen::MatrixXd & wbc::RobotModelRBDL::spaceJacobian ( const std::string & frame_id )

virtual

Returns the Space Jacobian for the kinematic chain between root and the tip frame as full body Jacobian. Size of the Jacobian will be 6 x nJoints, where nJoints is the number of joints of the whole robot. The order of the columns will be the same as the joint order of the robot. The columns that correspond to joints that are not part of the kinematic chain will have only zeros as entries.

Parameters

root_frame	Root frame of the chain. Has to be a valid link in the robot model.
frame_id	Tip frame of the chain. Has to be a valid link in the robot model.

Implements wbc::RobotModel.

◆ spatialAccelerationBias()

const types::SpatialAcceleration & wbc::RobotModelRBDL::spatialAccelerationBias ( const std::string & frame_id )

virtual

Returns the spatial acceleration bias, i.e. the term Jdot*qdot.

Parameters

root_frame	Root frame of the chain. Has to be a valid link in the robot model.
frame_id	Tip frame of the chain. Has to be a valid link in the robot model.

Returns: A Nx1 vector, where N is the number of robot joints

Implements wbc::RobotModel.

◆ twist()

const types::Twist & wbc::RobotModelRBDL::twist ( const std::string & frame_id )

virtual

Returns the twist of the body defined by frame_id. Twist will be in "local-world-aligned" (hybrid) representation, i.e., with respect to a frame attached to the given body, but aligned to world coordinates (floating base robot) or robot root link (fixed base)

Implements wbc::RobotModel.

◆ update()

void wbc::RobotModelRBDL::update	(	const Eigen::VectorXd &	joint_positions,
		const Eigen::VectorXd &	joint_velocities,
		const Eigen::VectorXd &	joint_accelerations,
		const types::Pose &	fb_pose,
		const types::Twist &	fb_twist,
		const types::SpatialAcceleration &	fb_acc )

virtual

Update kinematics/dynamics.

Parameters

joint_positions	Positions of all independent joints. These have to be in the same order as used by the model (alphabetic). For getting the joint order, call jointNames().
joint_velocities	Velocities of all independent joints. These have to be in the same order as used by the model (alphabetic). For getting the joint order, call jointNames().
joint_acceleration	Velocities of all independent joints. These have to be in the same order as used by the model (alphabetic). For getting the joint order, call jointNames().
fb_pose	Pose of the floating base in world coordinates
fb_twist	Twist of the floating base in "local-world-aligned" (hybrid) representation, i.e., with respect to a frame attached to the floating base (robot root), but aligned to world coordinates
fb_acc	Spatial accelerationof the floating base in "local-world-aligned" (hybrid) representation, i.e., with respect to a frame attached to the floating base (robot root), but aligned to world coordinates

Implements wbc::RobotModel.

◆ updateFK() [1/2]

void wbc::RobotModelRBDL::updateFK	(	Eigen::VectorXd &	_q,
		Eigen::VectorXd &	_qd )

protected

Update forward kinematics with zero acceleration

◆ updateFK() [2/2]

void wbc::RobotModelRBDL::updateFK	(	Eigen::VectorXd &	_q,
		Eigen::VectorXd &	_qd,
		Eigen::VectorXd &	_qdd )

protected

Update forward kinematics

Member Data Documentation

◆ H_q

RigidBodyDynamics::Math::MatrixNd wbc::RobotModelRBDL::H_q

protected

◆ J

RigidBodyDynamics::Math::MatrixNd wbc::RobotModelRBDL::J

protected

◆ rbdl_model

std::shared_ptr<RigidBodyDynamics::Model> wbc::RobotModelRBDL::rbdl_model

protected

◆ reg

RobotModelRegistry< RobotModelRBDL > wbc::RobotModelRBDL::reg

staticprotected

◆ tau

Eigen::VectorXd wbc::RobotModelRBDL::tau

protected

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

src/robot_models/rbdl/RobotModelRBDL.hpp
src/robot_models/rbdl/RobotModelRBDL.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Static Protected Attributes

Constructor & Destructor Documentation

◆ RobotModelRBDL()

◆ ~RobotModelRBDL()

Member Function Documentation

◆ acceleration()

◆ biasForces()

◆ bodyJacobian()

◆ centerOfMass()

◆ clear()

◆ comJacobian()

◆ configure()

◆ inverseDynamics()

◆ jointNamesInRBDLOrder()

◆ jointSpaceInertiaMatrix()

◆ pose()

◆ spaceJacobian()

◆ spatialAccelerationBias()

◆ twist()

◆ update()

◆ updateFK() [1/2]

◆ updateFK() [2/2]

Member Data Documentation

◆ H_q

◆ J

◆ rbdl_model

◆ reg

◆ tau