#include <RobotModelHyrodyn.hpp>

Inheritance diagram for wbc::RobotModelHyrodyn:

Public Member Functions
	RobotModelHyrodyn ()

virtual	~RobotModelHyrodyn ()

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 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)
	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 Eigen::MatrixXd &	comJacobian ()
	Returns the CoM Jacobian for the robot, which maps the robot joint velocities to linear spatial velocities of the CoM expressed in world frame (floating base robot) or robot root link (fixed base). 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. 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.

virtual const Eigen::MatrixXd &	jointSpaceInertiaMatrix ()
	Returns the joint space mass-inertia matrix, which is nj x nj, where nj is the number of joints + number of floating coordinates, i.e. 6 in case of a floating base robot.

virtual const Eigen::VectorXd &	biasForces ()
	Returns the bias force vector, which is nj x 1, where nj is the number of joints + number of floating coordinates, i.e. 6 in case of a floating base robot.

virtual const types::RigidBodyState &	centerOfMass ()
	Return centers of mass expressed in world frame.

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

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
void	clear ()

void	addFloatingBaseToURDF (urdf::ModelInterfaceSharedPtr &robot_urdf, const std::string &world_frame_id="world")

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

◆ acceleration()

const types::SpatialAcceleration & wbc::RobotModelHyrodyn::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.

◆ addFloatingBaseToURDF()

void wbc::RobotModelHyrodyn::addFloatingBaseToURDF	(	urdf::ModelInterfaceSharedPtr &	robot_urdf,
		const std::string &	world_frame_id = "world" )

protected

◆ biasForces()

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

virtual

Returns the bias force vector, which is nj x 1, where nj is the number of joints + number of floating coordinates, i.e. 6 in case of a floating base robot.

Implements wbc::RobotModel.

◆ bodyJacobian()

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

virtual

Returns the Body Jacobian for the given frame. The order of the Jacobian's columns will be the same as in the model (alphabetic).

Parameters

frame_id Reference frame of the Jacobian. Has to be a valid link in the robot model.

Returns: A 6 x nj matrix, where nj is the number of joints + number of floating coordinates, i.e. 6 in case of a floating base robot.

Implements wbc::RobotModel.

◆ centerOfMass()

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

virtual

Return centers of mass expressed in world frame.

Implements wbc::RobotModel.

◆ clear()

void wbc::RobotModelHyrodyn::clear ( )

protected

◆ comJacobian()

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

virtual

Returns the CoM Jacobian for the robot, which maps the robot joint velocities to linear spatial velocities of the CoM expressed in world frame (floating base robot) or robot root link (fixed base). The order of the columns will be the same as the configured joint order of the robot.

Returns: A 3 x nj matrix, where nj is the number of joints + number of floating coordinates, i.e. 6 in case of a floating base robot.

Implements wbc::RobotModel.

◆ configure()

bool wbc::RobotModelHyrodyn::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.

◆ hyrodynHandle()

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

inline

Return pointer to the internal hyrodyn model.

◆ inverseDynamics()

const Eigen::VectorXd & wbc::RobotModelHyrodyn::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 (including 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 wrt. world

Implements wbc::RobotModel.

◆ jointSpaceInertiaMatrix()

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

virtual

Returns the joint space mass-inertia matrix, which is nj x nj, where nj is the number of joints + number of floating coordinates, i.e. 6 in case of a floating base robot.

Implements wbc::RobotModel.

◆ pose()

const types::Pose & wbc::RobotModelHyrodyn::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::RobotModelHyrodyn::spaceJacobian ( const std::string & frame_id )

virtual

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.

Parameters

frame_id Tip frame of the Jacobian. Has to be a valid link in the robot model.

Returns: A 6 x nj matrix, where nj is the number of joints + number of floating coordinates, i.e. 6 in case of a floating base robot.

Implements wbc::RobotModel.

◆ spatialAccelerationBias()

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

virtual

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.

Parameters

frame_id Reference frame of the spatial acceleration. Has to be a valid link in the robot model.

Returns: A 6 dof spatial acceleration.

Implements wbc::RobotModel.

◆ twist()

const types::Twist & wbc::RobotModelHyrodyn::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::RobotModelHyrodyn::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.

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

◆ acceleration()

◆ addFloatingBaseToURDF()

◆ biasForces()

◆ bodyJacobian()

◆ centerOfMass()

◆ clear()

◆ comJacobian()

◆ configure()

◆ hyrodynHandle()

◆ inverseDynamics()

◆ jointSpaceInertiaMatrix()

◆ pose()

◆ spaceJacobian()

◆ spatialAccelerationBias()

◆ twist()

◆ update()

Member Data Documentation

◆ hyrodyn