A soft contact model that only uses end-effector positions/velocities to compute the contact force. More...

#include <EEContactModel.h>

Public Types
enum	VELOCITY_SMOOTHING { NONE = 0, SIGMOID = 1, TANH = 2, ABS = 3 }
	the type of velcity smoothing More...

typedef Kinematics::SCALAR	SCALAR

typedef iit::rbd::tpl::TraitSelector< SCALAR >::Trait	TRAIT

typedef std::array< bool, NUM_EE >	ActiveMap

typedef Kinematics::EEForceLinear	EEForceLinear

typedef std::array< EEForceLinear, NUM_EE >	EEForcesLinear

typedef Eigen::Matrix< SCALAR, 3, 1 >	Vector3s

typedef kindr::Position< SCALAR, 3 >	Position3S

typedef kindr::Velocity< SCALAR, 3 >	Velocity3S

Public Member Functions
	EEContactModel (const SCALAR &k=SCALAR(5000), const SCALAR &d=SCALAR(500.0), const SCALAR &alpha=SCALAR(100.0), const SCALAR &alpha_n=SCALAR(-1.0), const SCALAR &zOffset=SCALAR(0.0), const VELOCITY_SMOOTHING &smoothing=NONE, const std::shared_ptr< Kinematics > &kinematics=std::shared_ptr< Kinematics >(new Kinematics()))
	Default constructor. More...

	EEContactModel (const EEContactModel &other)
	Clone operator. More...

EEContactModel *	clone () const

void	setActiveEE (const ActiveMap &activeMap)
	Sets which end-effectors can have forces excerted on them. More...

EEForcesLinear	computeContactForces (const RBDState< NJOINTS, SCALAR > &state)
	Computes the contact forces given a state of the robot. Returns forces expressed in the world frame. More...

SCALAR &	alpha ()

SCALAR &	alpha_n ()

SCALAR &	k ()

SCALAR &	d ()

SCALAR &	zOffset ()

VELOCITY_SMOOTHING &	smoothing ()

Static Public Attributes
static const size_t	NUM_EE = Kinematics::NUM_EE

static const size_t	NJOINTS = Kinematics::NJOINTS

Detailed Description

template<class Kinematics>
class ct::rbd::EEContactModel< Kinematics >

A soft contact model that only uses end-effector positions/velocities to compute the contact force.

This contact model computes forces/torques at the endeffectors given the current state of the robot expressed in generalized coordinates

${}_W \lambda = f(q, \dot{q})$

The contact model assumes a plane with fixed orientation located at the origin (0, 0, 0). The contact dynamics are a combination of a spring-damper perpendicular and a damper in parallel to the surface. The force expressed in world coordinates without velocity smoothing or normal force smoothing is defined as

${}_W \lambda = f(q, \dot{q}) = - k ({}_W x_z - z_{offset}) - d {}_W \dot{x}$

where ${}_W x_z$ is the ground penetration with respect to an offset $z_{offset}$ expressed in world coordinates and $\dot{x}$ is the velocity of the endeffector.

In case normal force smoothing is activated, the first term becomes

${}_W \lambda_n = k e^{-\alpha_n {}_W x_z}$

In case velocity smoothing is activated, the second term becomes

${}_W \lambda_t = s({}_W x_z, \dot{x}) ~ d {}_W \dot{x}_{xy}$

where the smoothing coefficient is one of the following

sigmoid
$s({}_W x_z, \dot{x}) = \frac{1}{1 + e^{{}_W x_z \alpha}}$
tanh (same as sigmoid but computed differently)
$s({}_W x_z, \dot{x}) = \frac{1}{2} tanh(-\frac{1}{2} {}_W x_z \alpha) + \frac{1}{2}$
abs
$s({}_W x_z, \dot{x}) = - \frac{1}{2} \frac{{}_W x_z \alpha}{1 + abs(-{}_W x_z \alpha)} + \frac{1}{2}$

Template Parameters

Kinematics the Kinematics implementation of the robot

Member Typedef Documentation

◆ SCALAR

template<class Kinematics>

typedef Kinematics::SCALAR ct::rbd::EEContactModel< Kinematics >::SCALAR

◆ TRAIT

template<class Kinematics>

typedef iit::rbd::tpl::TraitSelector<SCALAR>::Trait ct::rbd::EEContactModel< Kinematics >::TRAIT

◆ ActiveMap

template<class Kinematics>

typedef std::array<bool, NUM_EE> ct::rbd::EEContactModel< Kinematics >::ActiveMap

◆ EEForceLinear

template<class Kinematics>

typedef Kinematics::EEForceLinear ct::rbd::EEContactModel< Kinematics >::EEForceLinear

◆ EEForcesLinear

template<class Kinematics>

typedef std::array<EEForceLinear, NUM_EE> ct::rbd::EEContactModel< Kinematics >::EEForcesLinear

◆ Vector3s

template<class Kinematics>

typedef Eigen::Matrix<SCALAR, 3, 1> ct::rbd::EEContactModel< Kinematics >::Vector3s

◆ Position3S

template<class Kinematics>

typedef kindr::Position<SCALAR, 3> ct::rbd::EEContactModel< Kinematics >::Position3S

◆ Velocity3S

template<class Kinematics>

typedef kindr::Velocity<SCALAR, 3> ct::rbd::EEContactModel< Kinematics >::Velocity3S

Member Enumeration Documentation

◆ VELOCITY_SMOOTHING

template<class Kinematics>

enum ct::rbd::EEContactModel::VELOCITY_SMOOTHING

the type of velcity smoothing

Enumerator
NONE	none
SIGMOID	sigmoid
TANH	tanh
ABS	abs

Constructor & Destructor Documentation

◆ EEContactModel() [1/2]

template<class Kinematics>

ct::rbd::EEContactModel< Kinematics >::EEContactModel	(	const SCALAR &	k = `SCALAR(5000)`,
		const SCALAR &	d = `SCALAR(500.0)`,
		const SCALAR &	alpha = `SCALAR(100.0)`,
		const SCALAR &	alpha_n = `SCALAR(-1.0)`,
		const SCALAR &	zOffset = `SCALAR(0.0)`,
		const VELOCITY_SMOOTHING &	smoothing = `NONE`,
		const std::shared_ptr< Kinematics > &	kinematics = `std::shared_ptr<Kinematics>(new Kinematics())`
	)

inline

Default constructor.

Parameters

k	stiffness of vertical spring
d	damper coefficient
alpha	velocity smoothing coefficient
alpha_n	normal force smoothing coefficient
zOffset	z offset of the plane with respect to (0, 0, 0)
smoothing	the type of velocity smoothing
kinematics	instance of the kinematics (optionally). Should be provided for efficiency when using Auto-Diff.

References i, and ct::rbd::EEContactModel< Kinematics >::NUM_EE.

Referenced by ct::rbd::EEContactModel< Kinematics >::clone().

◆ EEContactModel() [2/2]

template<class Kinematics>

ct::rbd::EEContactModel< Kinematics >::EEContactModel ( const EEContactModel< Kinematics > & other )

inline

Clone operator.

Parameters

other instance to clone

Member Function Documentation

◆ clone()

template<class Kinematics>

EEContactModel* ct::rbd::EEContactModel< Kinematics >::clone ( ) const

inline

References ct::rbd::EEContactModel< Kinematics >::EEContactModel().

◆ setActiveEE()

template<class Kinematics>

void ct::rbd::EEContactModel< Kinematics >::setActiveEE ( const ActiveMap & activeMap )

inline

Sets which end-effectors can have forces excerted on them.

Parameters

activeMap flags of active end-effectors

◆ computeContactForces()

template<class Kinematics>

EEForcesLinear ct::rbd::EEContactModel< Kinematics >::computeContactForces ( const RBDState< NJOINTS, SCALAR > & state )

inline

Computes the contact forces given a state of the robot. Returns forces expressed in the world frame.

Parameters

state The state of the robot

Returns: End-effector forces expressed in the world frame

References ct::rbd::RBDState< NJOINTS, SCALAR >::basePose(), i, ct::rbd::RBDState< NJOINTS, SCALAR >::jointPositions(), and ct::rbd::EEContactModel< Kinematics >::NUM_EE.

Referenced by TEST().

◆ alpha()

template<class Kinematics>

SCALAR& ct::rbd::EEContactModel< Kinematics >::alpha ( )

inline

◆ alpha_n()

template<class Kinematics>

SCALAR& ct::rbd::EEContactModel< Kinematics >::alpha_n ( )

inline

◆ k()

template<class Kinematics>

SCALAR& ct::rbd::EEContactModel< Kinematics >::k ( )

inline

◆ d()

template<class Kinematics>

SCALAR& ct::rbd::EEContactModel< Kinematics >::d ( )

inline

◆ zOffset()

template<class Kinematics>

SCALAR& ct::rbd::EEContactModel< Kinematics >::zOffset ( )

inline

◆ smoothing()

template<class Kinematics>

VELOCITY_SMOOTHING& ct::rbd::EEContactModel< Kinematics >::smoothing ( )

inline

References ct::rbd::EEContactModel< Kinematics >::ABS, eeId, ct::rbd::EEContactModel< Kinematics >::NONE, ct::rbd::EEContactModel< Kinematics >::SIGMOID, and ct::rbd::EEContactModel< Kinematics >::TANH.

Member Data Documentation

◆ NUM_EE

template<class Kinematics>

const size_t ct::rbd::EEContactModel< Kinematics >::NUM_EE = Kinematics::NUM_EE

static

Referenced by ct::rbd::EEContactModel< Kinematics >::computeContactForces(), and ct::rbd::EEContactModel< Kinematics >::EEContactModel().

◆ NJOINTS

template<class Kinematics>

const size_t ct::rbd::EEContactModel< Kinematics >::NJOINTS = Kinematics::NJOINTS

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/physics/EEContactModel.h

Public Types

Public Member Functions

Static Public Attributes

Detailed Description

template<class Kinematics> class ct::rbd::EEContactModel< Kinematics >

Member Typedef Documentation

◆ SCALAR

◆ TRAIT

◆ ActiveMap

◆ EEForceLinear

◆ EEForcesLinear

◆ Vector3s

◆ Position3S

◆ Velocity3S

Member Enumeration Documentation

◆ VELOCITY_SMOOTHING

Constructor & Destructor Documentation

◆ EEContactModel() [1/2]

◆ EEContactModel() [2/2]

Member Function Documentation

◆ clone()

◆ setActiveEE()

◆ computeContactForces()

◆ alpha()

◆ alpha_n()

◆ k()

◆ d()

◆ zOffset()

◆ smoothing()

Member Data Documentation

◆ NUM_EE

◆ NJOINTS

template<class Kinematics>
class ct::rbd::EEContactModel< Kinematics >