#include <RobotModelHyrodyn.hpp>

Inheritance diagram for wbc::RobotModelHyrodyn:

Public Member Functions
	RobotModelHyrodyn ()

virtual	~RobotModelHyrodyn ()

virtual bool	configure (const RobotModelConfig &cfg)
	Load and configure the robot model.

virtual void	update (const base::samples::Joints &joint_state, const base::samples::RigidBodyStateSE3 &floating_base_state=base::samples::RigidBodyStateSE3())
	Update the robot configuration.

virtual void	systemState (base::VectorXd &q, base::VectorXd &qd, base::VectorXd &qdd)

virtual const base::samples::RigidBodyStateSE3 &	rigidBodyState (const std::string &root_frame, const std::string &tip_frame)
	Computes and returns the relative transform between the two given frames. By convention this is the pose of the tip frame in root coordinates. This will create a kinematic chain between root and tip frame, if called for the first time with the given arguments.

virtual const base::MatrixXd &	spaceJacobian (const std::string &root_frame, const std::string &tip_frame)
	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 base::MatrixXd &	bodyJacobian (const std::string &root_frame, const std::string &tip_frame)
	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 base::MatrixXd &	comJacobian ()
	Returns the CoM Jacobian for the entire robot, which maps the robot joint velocities to linear spatial velocities in robot base 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 base::MatrixXd &	jacobianDot (const std::string &root_frame, const std::string &tip_frame)
	Returns the derivative of the Jacobian for the kinematic chain between root and the tip frame as full body Jacobian. By convention reference frame & reference point of the Jacobian will be the root frame (corresponding to the 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 base::Acceleration &	spatialAccelerationBias (const std::string &root_frame, const std::string &tip_frame)
	Returns the spatial acceleration bias, i.e. the term Jdot*qdot.

virtual const base::MatrixXd &	jointSpaceInertiaMatrix ()

virtual const base::VectorXd &	biasForces ()

virtual const base::samples::RigidBodyStateSE3 &	centerOfMass ()
	Return Current center of gravity in expressed base frame.

hyrodyn::RobotModel_HyRoDyn *	hyrodynHandle ()
	Return pointer to the internal hyrodyn model.

virtual void	computeInverseDynamics (base::commands::Joints &solver_output)
	Compute and return the inverse dynamics solution.

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.

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)=0
	Update kinematics/dynamics.

const types::JointState &	jointState ()

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

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

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

virtual const Eigen::MatrixXd &	spaceJacobian (const std::string &frame_id)=0
	Returns the Space Jacobian for the given frame. The order of the Jacobian's columns will be the same as in the model (alphabetic). Note that the linear part of the jacobian will be aligned to world frame (floating base robot) or robot root link (fixed base), and the angular part will be in body coordinates. This is refered to as "hybrid" representation.

virtual const Eigen::MatrixXd &	bodyJacobian (const std::string &frame_id)=0
	Returns the Body Jacobian for the given frame. The order of the Jacobian's columns will be the same as in the model (alphabetic).

virtual const types::SpatialAcceleration &	spatialAccelerationBias (const std::string &frame_id)=0
	Returns the spatial acceleration bias, i.e. the term Jdot*qdot. The linear part of the acceleration will be aligned to world frame (floating base robot) or robot root link (fixed base), and the angular part will be in body coordinates. This is refered to as "hybrid" representation.

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.

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

Protected Member Functions
void	clear ()

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

void	resetData ()

void	updateData ()

Protected Attributes
hyrodyn::RobotModel_HyRoDyn	hyrodyn

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

Constructor & Destructor Documentation

◆ RobotModelHyrodyn()

wbc::RobotModelHyrodyn::RobotModelHyrodyn ( )

◆ ~RobotModelHyrodyn()

wbc::RobotModelHyrodyn::~RobotModelHyrodyn ( )

virtual

Member Function Documentation

◆ biasForces()

const base::VectorXd & wbc::RobotModelHyrodyn::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 base::MatrixXd & wbc::RobotModelHyrodyn::bodyJacobian	(	const std::string &	root_frame,
		const std::string &	tip_frame )

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.
tip_frame	Tip frame of the chain. Has to be a valid link in the robot model.

◆ centerOfMass()

const base::samples::RigidBodyStateSE3 & wbc::RobotModelHyrodyn::centerOfMass ( )

virtual

Return Current center of gravity in expressed base frame.

Implements wbc::RobotModel.

◆ clear()

void wbc::RobotModelHyrodyn::clear ( )

protected

◆ comJacobian()

const base::MatrixXd & wbc::RobotModelHyrodyn::comJacobian ( )

virtual

Returns the CoM Jacobian for the entire robot, which maps the robot joint velocities to linear spatial velocities in robot base 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.

◆ computeInverseDynamics()

void wbc::RobotModelHyrodyn::computeInverseDynamics ( base::commands::Joints & solver_output )

virtual

Compute and return the inverse dynamics solution.

◆ configure()

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

virtual

Load and configure the robot model.

Parameters

cfg	Model configuration. See RobotModelConfig.hpp for details

Returns: True in case of success, else false

Implements wbc::RobotModel.

◆ hyrodynHandle()

hyrodyn::RobotModel_HyRoDyn * wbc::RobotModelHyrodyn::hyrodynHandle ( )

inline

Return pointer to the internal hyrodyn model.

◆ jacobianDot()

const base::MatrixXd & wbc::RobotModelHyrodyn::jacobianDot	(	const std::string &	root_frame,
		const std::string &	tip_frame )

virtual

Returns the derivative of the Jacobian for the kinematic chain between root and the tip frame as full body Jacobian. By convention reference frame & reference point of the Jacobian will be the root frame (corresponding to the 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.
tip_frame	Tip frame of the chain. Has to be a valid link in the robot model.

Returns: A 6xN Jacobian derivative matrix, where N is the number of robot joints

◆ jointSpaceInertiaMatrix()

const base::MatrixXd & wbc::RobotModelHyrodyn::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.

◆ rigidBodyState()

const base::samples::RigidBodyStateSE3 & wbc::RobotModelHyrodyn::rigidBodyState	(	const std::string &	root_frame,
		const std::string &	tip_frame )

virtual

Computes and returns the relative transform between the two given frames. By convention this is the pose of the tip frame in root coordinates. This will create a kinematic chain between root and tip frame, if called for the first time with the given arguments.

Parameters

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

◆ spaceJacobian()

const base::MatrixXd & wbc::RobotModelHyrodyn::spaceJacobian	(	const std::string &	root_frame,
		const std::string &	tip_frame )

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.
tip_frame	Tip frame of the chain. Has to be a valid link in the robot model.

◆ spatialAccelerationBias()

const base::Acceleration & wbc::RobotModelHyrodyn::spatialAccelerationBias	(	const std::string &	root_frame,
		const std::string &	tip_frame )

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.
tip_frame	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

◆ systemState()

void wbc::RobotModelHyrodyn::systemState	(	base::VectorXd &	q,
		base::VectorXd &	qd,
		base::VectorXd &	qdd )

virtual

Return entire system state

◆ update()

void wbc::RobotModelHyrodyn::update	(	const base::samples::Joints &	joint_state,
		const base::samples::RigidBodyStateSE3 &	floating_base_state = base::samples::RigidBodyStateSE3() )

virtual

Update the robot configuration.

Parameters

joint_state	The joint_state vector. Has to contain all robot joints that are configured in the model.
poses	Optional, only for floating base robots: update the floating base state of the robot model.

Member Data Documentation

◆ hyrodyn

hyrodyn::RobotModel_HyRoDyn wbc::RobotModelHyrodyn::hyrodyn

protected

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

src/robot_models/hyrodyn/RobotModelHyrodyn.hpp
src/robot_models/hyrodyn/RobotModelHyrodyn.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Constructor & Destructor Documentation

◆ RobotModelHyrodyn()

◆ ~RobotModelHyrodyn()

Member Function Documentation

◆ biasForces()

◆ bodyJacobian()

◆ centerOfMass()

◆ clear()

◆ comJacobian()

◆ computeInverseDynamics()

◆ configure()

◆ hyrodynHandle()

◆ jacobianDot()

◆ jointSpaceInertiaMatrix()

◆ rigidBodyState()

◆ spaceJacobian()

◆ spatialAccelerationBias()

◆ systemState()

◆ update()

Member Data Documentation

◆ hyrodyn