A general class for computing Kinematic properties. More...

#include <Kinematics.h>

Public Types
using	Ptr_t = std::shared_ptr< Kinematics< RBD, N_EE > >

using	ROBCOGEN = RBD

using	SCALAR = typename ROBCOGEN::SCALAR

using	HomogeneousTransform = typename ROBCOGEN::HomogeneousTransform

using	HomogeneousTransforms = typename ROBCOGEN::HomogeneousTransforms

using	ForceTransform = typename ROBCOGEN::ForceTransform

using	Jacobian = typename ROBCOGEN::Jacobian

using	Jacobians = typename ROBCOGEN::Jacobians

using	Vector3Tpl = Eigen::Matrix< SCALAR, 3, 1 >

using	Matrix3Tpl = Eigen::Matrix< SCALAR, 3, 3 >

using	Position3Tpl = kindr::Position< SCALAR, 3 >

using	Velocity3Tpl = kindr::Velocity< SCALAR, 3 >

using	QuaterionTpl = kindr::RotationQuaternion< SCALAR >

using	RigidBodyPoseTpl = tpl::RigidBodyPose< SCALAR >

using	EEForce = SpatialForceVector< SCALAR >

using	EEForceLinear = Vector3Tpl

using	JointState_t = JointState< NJOINTS, SCALAR >

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	Kinematics (std::shared_ptr< RBD > rbdContainer=std::shared_ptr< RBD >(new RBD()))

	Kinematics (const Kinematics< RBD, N_EE > &other)

virtual	~Kinematics ()=default

Kinematics< RBD, N_EE > *	clone () const

void	initEndeffectors (std::array< EndEffector< NJOINTS, SCALAR >, NUM_EE > &endeffectors)

EndEffector< NJOINTS, SCALAR > &	getEndEffector (size_t id)
	Get an end-effector. More...

void	setEndEffector (size_t id, const EndEffector< NJOINTS, SCALAR > &ee)
	Set an end-effector. More...

Jacobian	getJacobianBaseEEbyId (size_t eeId, const RBDState< NJOINTS, SCALAR > &rbdState)

FloatingBaseTransforms< RBD > &	floatingBaseTransforms ()

FloatingBaseTransforms< RBD > &	floatingBaseTransforms () const

const HomogeneousTransforms &	transforms () const

HomogeneousTransforms &	transforms ()

const Jacobians &	jacobians () const

Jacobians &	jacobians ()

Velocity3Tpl	getEEVelocityInBase (size_t eeId, const RBDState< NJOINTS, SCALAR > &rbdState)

Velocity3Tpl	getEEVelocityInWorld (size_t eeId, const RBDState< NJOINTS, SCALAR > &rbdState)

Position3Tpl	getEEPositionInBase (size_t eeID, const typename JointState_t::Position &jointPosition)

RigidBodyPoseTpl	getEEPoseInBase (size_t eeID, const typename JointState_t::Position &jointPosition)

Matrix3Tpl	getEERotInBase (size_t eeID, const typename JointState_t::Position &jointPosition)

Position3Tpl	getEEPositionInWorld (size_t eeID, const RigidBodyPoseTpl &basePose, const typename JointState_t::Position &jointPosition)

RigidBodyPoseTpl	getEEPoseInWorld (size_t eeID, const RigidBodyPoseTpl &basePose, const typename JointState_t::Position &jointPosition)
	get the end-effector pose in world coordinates More...

Matrix3Tpl	getEERotInWorld (size_t eeID, const RigidBodyPoseTpl &basePose, const typename JointState_t::Position &jointPosition)
	get the end-effector rotation matrix expressed in world coordinates More...

void	addIKSolver (const std::shared_ptr< InverseKinematicsBase< NJOINTS, SCALAR >> &solver, size_t eeID, size_t solverID=0)

std::shared_ptr< InverseKinematicsBase< NJOINTS, SCALAR > >	getIKSolver (const size_t eeID, const size_t solverID=0) const

EEForce	mapForceFromWorldToLink3d (const Vector3Tpl &W_force, const RigidBodyPoseTpl &basePose, const typename JointState_t::Position &jointPosition, size_t eeId)
	Transforms a force applied at an end-effector and expressed in the world into the link frame the EE is rigidly connected to. More...

EEForce	mapForceFromWorldToLink (const EEForce &W_force, const RigidBodyPoseTpl &basePose, const typename JointState_t::Position &jointPosition, size_t eeId)
	Transforms a force applied at an end-effector and expressed in the world into the link frame the EE is rigidly connected to. More...

EEForce	mapForceFromWorldToLink (const EEForce &W_force, const RigidBodyPoseTpl &basePose, const typename JointState_t::Position &jointPosition, const Position3Tpl &B_x_EE, size_t eeId)
	Transforms a force applied at an end-effector and expressed in the world into the link frame the EE is rigidly connected to. More...

EEForce	mapForceFromEEToLink (const EEForce &EE_force, const RigidBodyPoseTpl &T_B_EE, const typename JointState_t::Position &jointPosition, size_t eeId)
	Transforms a force applied at an end-effector expressed in an arbitrary (end-effector) frame into the link frame the EE is rigidly connected to. More...

RBD &	robcogen ()

Static Public Attributes
static const size_t	NUM_EE = N_EE

static const size_t	NJOINTS = RBD::NJOINTS

static const size_t	NLINKS = RBD::NLINKS

Detailed Description

template<class RBD, size_t N_EE>
class ct::rbd::Kinematics< RBD, N_EE >

A general class for computing Kinematic properties.

This class implements useful Kinematic quantities. It wraps RobCoGen to have access to efficient transforms and jacobians.

Member Typedef Documentation

◆ Ptr_t

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::Ptr_t = std::shared_ptr<Kinematics<RBD, N_EE> >

◆ ROBCOGEN

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::ROBCOGEN = RBD

◆ SCALAR

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::SCALAR = typename ROBCOGEN::SCALAR

◆ HomogeneousTransform

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::HomogeneousTransform = typename ROBCOGEN::HomogeneousTransform

◆ HomogeneousTransforms

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::HomogeneousTransforms = typename ROBCOGEN::HomogeneousTransforms

◆ ForceTransform

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::ForceTransform = typename ROBCOGEN::ForceTransform

◆ Jacobian

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::Jacobian = typename ROBCOGEN::Jacobian

◆ Jacobians

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::Jacobians = typename ROBCOGEN::Jacobians

◆ Vector3Tpl

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::Vector3Tpl = Eigen::Matrix<SCALAR, 3, 1>

◆ Matrix3Tpl

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::Matrix3Tpl = Eigen::Matrix<SCALAR, 3, 3>

◆ Position3Tpl

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::Position3Tpl = kindr::Position<SCALAR, 3>

◆ Velocity3Tpl

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::Velocity3Tpl = kindr::Velocity<SCALAR, 3>

◆ QuaterionTpl

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::QuaterionTpl = kindr::RotationQuaternion<SCALAR>

◆ RigidBodyPoseTpl

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::RigidBodyPoseTpl = tpl::RigidBodyPose<SCALAR>

◆ EEForce

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::EEForce = SpatialForceVector<SCALAR>

◆ EEForceLinear

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::EEForceLinear = Vector3Tpl

◆ JointState_t

template<class RBD, size_t N_EE>

using ct::rbd::Kinematics< RBD, N_EE >::JointState_t = JointState<NJOINTS, SCALAR>

Constructor & Destructor Documentation

◆ Kinematics() [1/2]

template<class RBD, size_t N_EE>

EIGEN_MAKE_ALIGNED_OPERATOR_NEW ct::rbd::Kinematics< RBD, N_EE >::Kinematics ( std::shared_ptr< RBD > rbdContainer = std::shared_ptr<RBD>(new RBD()) )

inline

◆ Kinematics() [2/2]

template<class RBD, size_t N_EE>

ct::rbd::Kinematics< RBD, N_EE >::Kinematics ( const Kinematics< RBD, N_EE > & other )

inline

◆ ~Kinematics()

template<class RBD, size_t N_EE>

virtual ct::rbd::Kinematics< RBD, N_EE >::~Kinematics ( )

virtualdefault

Referenced by ct::rbd::Kinematics< RBD, NEE >::Kinematics().

Member Function Documentation

◆ clone()

template<class RBD, size_t N_EE>

Kinematics<RBD, N_EE>* ct::rbd::Kinematics< RBD, N_EE >::clone ( ) const

inline

◆ initEndeffectors()

template<class RBD, size_t N_EE>

void ct::rbd::Kinematics< RBD, N_EE >::initEndeffectors ( std::array< EndEffector< NJOINTS, SCALAR >, NUM_EE > & endeffectors )

inline

Referenced by ct::rbd::Kinematics< RBD, NEE >::Kinematics().

◆ getEndEffector()

template<class RBD, size_t N_EE>

EndEffector<NJOINTS, SCALAR>& ct::rbd::Kinematics< RBD, N_EE >::getEndEffector ( size_t id )

inline

Get an end-effector.

Parameters

id	end-effector id

Returns

Referenced by ct::rbd::Kinematics< RBD, NEE >::mapForceFromEEToLink(), and ct::rbd::Kinematics< RBD, NEE >::mapForceFromWorldToLink().

◆ setEndEffector()

template<class RBD, size_t N_EE>

void ct::rbd::Kinematics< RBD, N_EE >::setEndEffector	(	size_t	id,
		const EndEffector< NJOINTS, SCALAR > &	ee
	)

inline

Set an end-effector.

Parameters

id	end-effector id
ee	end-effector

◆ getJacobianBaseEEbyId()

template<class RBD, size_t N_EE>

Jacobian ct::rbd::Kinematics< RBD, N_EE >::getJacobianBaseEEbyId	(	size_t	eeId,
		const RBDState< NJOINTS, SCALAR > &	rbdState
	)

inline

◆ floatingBaseTransforms() [1/2]

template<class RBD, size_t N_EE>

FloatingBaseTransforms<RBD>& ct::rbd::Kinematics< RBD, N_EE >::floatingBaseTransforms ( )

inline

◆ floatingBaseTransforms() [2/2]

template<class RBD, size_t N_EE>

FloatingBaseTransforms<RBD>& ct::rbd::Kinematics< RBD, N_EE >::floatingBaseTransforms ( ) const

inline

◆ transforms() [1/2]

template<class RBD, size_t N_EE>

const HomogeneousTransforms& ct::rbd::Kinematics< RBD, N_EE >::transforms ( ) const

inline

◆ transforms() [2/2]

template<class RBD, size_t N_EE>

HomogeneousTransforms& ct::rbd::Kinematics< RBD, N_EE >::transforms ( )

inline

◆ jacobians() [1/2]

template<class RBD, size_t N_EE>

const Jacobians& ct::rbd::Kinematics< RBD, N_EE >::jacobians ( ) const

inline

◆ jacobians() [2/2]

template<class RBD, size_t N_EE>

Jacobians& ct::rbd::Kinematics< RBD, N_EE >::jacobians ( )

inline

◆ getEEVelocityInBase()

template<class RBD, size_t N_EE>

Velocity3Tpl ct::rbd::Kinematics< RBD, N_EE >::getEEVelocityInBase	(	size_t	eeId,
		const RBDState< NJOINTS, SCALAR > &	rbdState
	)

inline

Referenced by ct::rbd::Kinematics< RBD, NEE >::getEEVelocityInWorld().

◆ getEEVelocityInWorld()

template<class RBD, size_t N_EE>

Velocity3Tpl ct::rbd::Kinematics< RBD, N_EE >::getEEVelocityInWorld	(	size_t	eeId,
		const RBDState< NJOINTS, SCALAR > &	rbdState
	)

inline

◆ getEEPositionInBase()

template<class RBD, size_t N_EE>

Position3Tpl ct::rbd::Kinematics< RBD, N_EE >::getEEPositionInBase	(	size_t	eeID,
		const typename JointState_t::Position &	jointPosition
	)

inline

Computes the forward kinematics for the end-effector position and expresses the end-effector position in robot base coordinates.

Parameters

eeID	unique identifier of the end-effector in question
jointPosition	current robot joint positions

Returns

the current end-effector position in base coordinates

 * \xrefitem todo 2.

Referenced by ct::rbd::Kinematics< RBD, NEE >::getEEPositionInWorld(), ct::rbd::Kinematics< RBD, NEE >::getEEVelocityInBase(), ct::rbd::tpl::ConstraintJacobian< ct::rbd::Kinematics< RBD, NEE >, MAX_JAC_SIZE, NJOINTS, Scalar >::getJacobianOrigin(), and ct::rbd::Kinematics< RBD, NEE >::mapForceFromWorldToLink().

◆ getEEPoseInBase()

template<class RBD, size_t N_EE>

RigidBodyPoseTpl ct::rbd::Kinematics< RBD, N_EE >::getEEPoseInBase	(	size_t	eeID,
		const typename JointState_t::Position &	jointPosition
	)

inline

Computes the forward kinematics for the end-effector position and expresses the end-effector pose in robot base coordinates.

Parameters

eeID	unique identifier of the end-effector in question
jointPosition	current robot joint positions

Returns: the current end-effector pose in base coordinates

Referenced by ct::rbd::Kinematics< RBD, NEE >::getEEPoseInWorld().

◆ getEERotInBase()

template<class RBD, size_t N_EE>

Matrix3Tpl ct::rbd::Kinematics< RBD, N_EE >::getEERotInBase	(	size_t	eeID,
		const typename JointState_t::Position &	jointPosition
	)

inline

compute the forward kinematics and return a rotation matrix specifying the ee-rotation w.r.t. the base frame

Referenced by ct::rbd::Kinematics< RBD, NEE >::getEERotInWorld().

◆ getEEPositionInWorld()

template<class RBD, size_t N_EE>

Position3Tpl ct::rbd::Kinematics< RBD, N_EE >::getEEPositionInWorld	(	size_t	eeID,
		const RigidBodyPoseTpl &	basePose,
		const typename JointState_t::Position &	jointPosition
	)

inline

Computes the forward kinematics for the end-effector position and expresses the end-effector position in world coordinates

Parameters

eeID	unique identifier of the end-effector in question
basePose	current robot base pose
jointPosition	current robot joint positions

Returns: the current end-effector position in world coordinates

Todo:: integrate this into getEEPoseInWorld

◆ getEEPoseInWorld()

template<class RBD, size_t N_EE>

RigidBodyPoseTpl ct::rbd::Kinematics< RBD, N_EE >::getEEPoseInWorld	(	size_t	eeID,
		const RigidBodyPoseTpl &	basePose,
		const typename JointState_t::Position &	jointPosition
	)

inline

get the end-effector pose in world coordinates

◆ getEERotInWorld()

template<class RBD, size_t N_EE>

Matrix3Tpl ct::rbd::Kinematics< RBD, N_EE >::getEERotInWorld	(	size_t	eeID,
		const RigidBodyPoseTpl &	basePose,
		const typename JointState_t::Position &	jointPosition
	)

inline

get the end-effector rotation matrix expressed in world coordinates

◆ addIKSolver()

template<class RBD, size_t N_EE>

void ct::rbd::Kinematics< RBD, N_EE >::addIKSolver	(	const std::shared_ptr< InverseKinematicsBase< NJOINTS, SCALAR >> &	solver,
		size_t	eeID,
		size_t	solverID = `0`
	)

inline

◆ getIKSolver()

template<class RBD, size_t N_EE>

std::shared_ptr<InverseKinematicsBase<NJOINTS, SCALAR> > ct::rbd::Kinematics< RBD, N_EE >::getIKSolver	(	const size_t	eeID,
		const size_t	solverID = `0`
	)		const

inline

◆ mapForceFromWorldToLink3d()

template<class RBD, size_t N_EE>

EEForce ct::rbd::Kinematics< RBD, N_EE >::mapForceFromWorldToLink3d	(	const Vector3Tpl &	W_force,
		const RigidBodyPoseTpl &	basePose,
		const typename JointState_t::Position &	jointPosition,
		size_t	eeId
	)

inline

Transforms a force applied at an end-effector and expressed in the world into the link frame the EE is rigidly connected to.

Parameters

W_force	Force expressed in world coordinates
basePose	Pose of the base (in the world)
jointPosition	Joint angles
eeId	ID of the end-effector

Returns

◆ mapForceFromWorldToLink() [1/2]

template<class RBD, size_t N_EE>

EEForce ct::rbd::Kinematics< RBD, N_EE >::mapForceFromWorldToLink	(	const EEForce &	W_force,
		const RigidBodyPoseTpl &	basePose,
		const typename JointState_t::Position &	jointPosition,
		size_t	eeId
	)

inline

Transforms a force applied at an end-effector and expressed in the world into the link frame the EE is rigidly connected to.

Parameters

W_force	Force expressed in world coordinates
basePose	Pose of the base (in the world)
jointPosition	Joint angles
eeId	ID of the end-effector

Returns

Referenced by ct::rbd::Kinematics< RBD, NEE >::mapForceFromWorldToLink(), and ct::rbd::Kinematics< RBD, NEE >::mapForceFromWorldToLink3d().

◆ mapForceFromWorldToLink() [2/2]

template<class RBD, size_t N_EE>

EEForce ct::rbd::Kinematics< RBD, N_EE >::mapForceFromWorldToLink	(	const EEForce &	W_force,
		const RigidBodyPoseTpl &	basePose,
		const typename JointState_t::Position &	jointPosition,
		const Position3Tpl &	B_x_EE,
		size_t	eeId
	)

inline

Transforms a force applied at an end-effector and expressed in the world into the link frame the EE is rigidly connected to.

Parameters

W_force	Force expressed in world coordinates
basePose	Pose of the base (in the world)
jointPosition	Joint angles
B_x_EE	Position of the end effector in the base
eeId	ID of the end-effector

Returns

◆ mapForceFromEEToLink()

template<class RBD, size_t N_EE>

EEForce ct::rbd::Kinematics< RBD, N_EE >::mapForceFromEEToLink	(	const EEForce &	EE_force,
		const RigidBodyPoseTpl &	T_B_EE,
		const typename JointState_t::Position &	jointPosition,
		size_t	eeId
	)

inline

Transforms a force applied at an end-effector expressed in an arbitrary (end-effector) frame into the link frame the EE is rigidly connected to.

This function does not assume any position or orientation of the end-effector. Therefore, the user must specify the orientation of the end-effector in the base. There are two common choices: use zero rotation to assume forces/torques are expressed in the base. Or use the base orientation to assume the end-effector forces/torques are expressed in the world. For this variant, also see mapForceFromWorldToLink().

NOTE: Even if zero or base rotation is assumed, you have to pass the correct position of the end-effector expressed in the base as part of T_B_EE! Do NOT pass the base pose directly here!

Parameters

EE_force	6D torque/force vector expressed in the EE frame
T_B_EE	transform from end-effector to base
jointPosition	joint angles
eeId	ID of the end-effector

Returns

◆ robcogen()

template<class RBD, size_t N_EE>

RBD& ct::rbd::Kinematics< RBD, N_EE >::robcogen ( )

inline

Member Data Documentation

◆ NUM_EE

template<class RBD, size_t N_EE>

const size_t ct::rbd::Kinematics< RBD, N_EE >::NUM_EE = N_EE

static

Referenced by ct::rbd::Kinematics< RBD, NEE >::initEndeffectors().

◆ NJOINTS

template<class RBD, size_t N_EE>

const size_t ct::rbd::Kinematics< RBD, N_EE >::NJOINTS = RBD::NJOINTS

static

◆ NLINKS

template<class RBD, size_t N_EE>

const size_t ct::rbd::Kinematics< RBD, N_EE >::NLINKS = RBD::NLINKS

static

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

/home/gim2rng/ct_devel_ws/src/control-toolbox/ct_rbd/include/ct/rbd/robot/Kinematics.h

Public Types

Public Member Functions

Static Public Attributes

Detailed Description

template<class RBD, size_t N_EE> class ct::rbd::Kinematics< RBD, N_EE >

Member Typedef Documentation

◆ Ptr_t

◆ ROBCOGEN

◆ SCALAR

◆ HomogeneousTransform

◆ HomogeneousTransforms

◆ ForceTransform

◆ Jacobian

◆ Jacobians

◆ Vector3Tpl

◆ Matrix3Tpl

◆ Position3Tpl

◆ Velocity3Tpl

◆ QuaterionTpl

◆ RigidBodyPoseTpl

◆ EEForce

◆ EEForceLinear

◆ JointState_t

Constructor & Destructor Documentation

◆ Kinematics() [1/2]

◆ Kinematics() [2/2]

◆ ~Kinematics()

Member Function Documentation

◆ clone()

◆ initEndeffectors()

◆ getEndEffector()

◆ setEndEffector()

◆ getJacobianBaseEEbyId()

◆ floatingBaseTransforms() [1/2]

◆ floatingBaseTransforms() [2/2]

◆ transforms() [1/2]

◆ transforms() [2/2]

◆ jacobians() [1/2]

◆ jacobians() [2/2]

◆ getEEVelocityInBase()

◆ getEEVelocityInWorld()

◆ getEEPositionInBase()

◆ getEEPoseInBase()

◆ getEERotInBase()

◆ getEEPositionInWorld()

◆ getEEPoseInWorld()

◆ getEERotInWorld()

◆ addIKSolver()

◆ getIKSolver()

◆ mapForceFromWorldToLink3d()

◆ mapForceFromWorldToLink() [1/2]

◆ mapForceFromWorldToLink() [2/2]

◆ mapForceFromEEToLink()

◆ robcogen()

Member Data Documentation

◆ NUM_EE

◆ NJOINTS

◆ NLINKS

template<class RBD, size_t N_EE>
class ct::rbd::Kinematics< RBD, N_EE >