#include <RobotModelPinocchio.hpp>

Inheritance diagram for wbc::RobotModelPinocchio:

Public Member Functions
	RobotModelPinocchio ()

	~RobotModelPinocchio ()

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)

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 configured 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 configured 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::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 &	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::MatrixXd &	jointSpaceInertiaMatrix ()
	Compute and return the joint space mass-inertia matrix, which is nj x nj, where nj is the number of joints of the system.

virtual const base::VectorXd &	biasForces ()
	Compute and return the bias force vector, which is nj x 1, where nj is the number of joints of the system.

virtual const base::samples::RigidBodyStateSE3 &	centerOfMass ()
	Compute and return center of mass expressed in base frame.

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

const base::samples::Joints &	jointState (const std::vector< std::string > &joint_names)

const std::vector< std::string > &	jointNames ()
	Return all joint names.

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 index of joint name.

const std::string &	baseFrame ()
	Get the base frame of the robot.

const std::string &	worldFrame ()
	Get the world frame id.

const base::JointLimits &	jointLimits ()
	Return current joint limits.

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.

const base::MatrixXd &	selectionMatrix ()
	Return current selection matrix S that maps complete joint vector to actuated joint vector: q_a = S * q. The matrix consists of only zeros and ones. Size is na x nq, where na is the number of actuated joints and nq the total number of joints.

void	setActiveContacts (const ActiveContacts &contacts)
	Provide information about which link is currently in contact with the environment.

const ActiveContacts &	getActiveContacts ()
	Provide links names that are possibly in contact with the environment (typically the end effector links)

uint	noOfJoints ()
	Return number of joints.

uint	noOfActuatedJoints ()
	Return number of actuated/active joints.

void	setGravityVector (const base::Vector3d &g)
	Set the current gravity vector.

const base::samples::RigidBodyStateSE3 &	floatingBaseState ()
	Get current status of floating base.

void	setContactWrenches (const base::samples::Wrenches &wrenches)
	Set contact wrenches, names have to consistent with the configured contact points.

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

bool	hasFloatingBase ()
	Is floating base robot?

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

Protected Types
typedef std::shared_ptr< pinocchio::Data >	DataPtr

Protected Types inherited from wbc::RobotModel
typedef std::map< std::string, base::MatrixXd >	JacobianMap

Protected Member Functions
void	clear ()

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

const std::string	chainID (const std::string &root, const std::string &tip)

Protected Attributes
Eigen::VectorXd	q

Eigen::VectorXd	qd

Eigen::VectorXd	qdd

pinocchio::Model	model

DataPtr	data

Protected Attributes inherited from wbc::RobotModel
ActiveContacts	active_contacts

base::Vector3d	gravity

base::samples::RigidBodyStateSE3	floating_base_state

base::samples::Wrenches	contact_wrenches

RobotModelConfig	robot_model_config

std::string	world_frame

std::string	base_frame

base::JointLimits	joint_limits

std::vector< std::string >	actuated_joint_names

std::vector< std::string >	independent_joint_names

std::vector< std::string >	joint_names

std::vector< std::string >	joint_names_floating_base

urdf::ModelInterfaceSharedPtr	robot_urdf

bool	has_floating_base

base::samples::Joints	joint_state

base::MatrixXd	joint_space_inertia_mat

base::MatrixXd	com_jac

base::VectorXd	bias_forces

base::Acceleration	spatial_acc_bias

base::MatrixXd	selection_matrix

base::samples::RigidBodyStateSE3	com_rbs

base::samples::RigidBodyStateSE3	rbs

JacobianMap	space_jac_map

JacobianMap	body_jac_map

JacobianMap	jac_dot_map

base::samples::Joints	joint_state_out

Static Protected Attributes
static RobotModelRegistry< RobotModelPinocchio >	reg

Member Typedef Documentation

◆ DataPtr

std::shared_ptr<pinocchio::Data> wbc::RobotModelPinocchio::DataPtr

protected

Constructor & Destructor Documentation

◆ RobotModelPinocchio()

wbc::RobotModelPinocchio::RobotModelPinocchio ( )

◆ ~RobotModelPinocchio()

wbc::RobotModelPinocchio::~RobotModelPinocchio ( )

Member Function Documentation

◆ biasForces()

const base::VectorXd & wbc::RobotModelPinocchio::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::RobotModelPinocchio::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 configured 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 matrix, where N is the number of robot joints

Implements wbc::RobotModel.

◆ centerOfMass()

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

virtual

Compute and return center of mass expressed in base frame.

Implements wbc::RobotModel.

◆ clear()

void wbc::RobotModelPinocchio::clear ( )

protected

Free all data

◆ comJacobian()

const base::MatrixXd & wbc::RobotModelPinocchio::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::RobotModelPinocchio::computeInverseDynamics ( base::commands::Joints & solver_output )

virtual

Compute and return the inverse dynamics solution.

Implements wbc::RobotModel.

◆ configure()

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

◆ jacobianDot()

const base::MatrixXd & wbc::RobotModelPinocchio::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 matrix, where N is the number of robot joints

Implements wbc::RobotModel.

◆ jointSpaceInertiaMatrix()

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

virtual

Returns the pose, twist and spatial acceleration between the two given frames. All quantities are defined in root_frame coordinates

Implements wbc::RobotModel.

◆ spaceJacobian()

const base::MatrixXd & wbc::RobotModelPinocchio::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 configured 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 matrix, where N is the number of robot joints

Implements wbc::RobotModel.

◆ spatialAccelerationBias()

const base::Acceleration & wbc::RobotModelPinocchio::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

Implements wbc::RobotModel.

◆ systemState()

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

virtual

Return entire system state

Implements wbc::RobotModel.

◆ update()

void wbc::RobotModelPinocchio::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.

Implements wbc::RobotModel.

Member Data Documentation

◆ data

DataPtr wbc::RobotModelPinocchio::data

protected

◆ model

pinocchio::Model wbc::RobotModelPinocchio::model

protected

◆ q

Eigen::VectorXd wbc::RobotModelPinocchio::q

protected

◆ qd

Eigen::VectorXd wbc::RobotModelPinocchio::qd

protected

◆ qdd

Eigen::VectorXd wbc::RobotModelPinocchio::qdd

protected

◆ reg

RobotModelRegistry< RobotModelPinocchio > wbc::RobotModelPinocchio::reg

staticprotected

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

src/robot_models/pinocchio/RobotModelPinocchio.hpp
src/robot_models/pinocchio/RobotModelPinocchio.cpp

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Static Protected Attributes

Member Typedef Documentation

◆ DataPtr

Constructor & Destructor Documentation

◆ RobotModelPinocchio()

◆ ~RobotModelPinocchio()

Member Function Documentation

◆ biasForces()

◆ bodyJacobian()

◆ centerOfMass()

◆ clear()

◆ comJacobian()

◆ computeInverseDynamics()

◆ configure()

◆ jacobianDot()

◆ jointSpaceInertiaMatrix()

◆ rigidBodyState()

◆ spaceJacobian()

◆ spatialAccelerationBias()

◆ systemState()

◆ update()

Member Data Documentation

◆ data

◆ model

◆ q

◆ qd

◆ qdd

◆ reg