Calculates poses of links and dynamic properties of the robot. More...

#include <model_kdl.h>

Inheritance diagram for franka_gazebo::ModelKDL:

Public Member Functions
std::array< double, 42 >	bodyJacobian (franka::Frame frame, const std::array< double, 7 > &q, const std::array< double, 16 > &F_T_EE, const std::array< double, 16 > &EE_T_K) const override
	Gets the 6x7 Jacobian for the given frame, relative to that frame. More...

std::array< double, 7 >	coriolis (const std::array< double, 7 > &q, const std::array< double, 7 > &dq, const std::array< double, 9 > &I_total, double m_total, const std::array< double, 3 > &F_x_Ctotal) const override
	Calculates the Coriolis force vector (state-space equation): $c= C \times dq$ , in . More...

std::array< double, 7 >	gravity (const std::array< double, 7 > &q, double m_total, const std::array< double, 3 > &F_x_Ctotal, const std::array< double, 3 > &gravity_earth) const override
	Calculates the gravity vector. More...

std::array< double, 49 >	mass (const std::array< double, 7 > &q, const std::array< double, 9 > &I_total, double m_total, const std::array< double, 3 > &F_x_Ctotal) const override
	Calculates the 7x7 mass matrix. More...

	ModelKDL (const urdf::Model &model, const std::string &root, const std::string &tip, double singularity_threshold=-1)
	Create a new implementation for the ModelBase with KDL as backend. More...

std::array< double, 16 >	pose (franka::Frame frame, const std::array< double, 7 > &q, const std::array< double, 16 > &F_T_EE, const std::array< double, 16 > &EE_T_K) const override
	Gets the 4x4 pose matrix for the given frame in base frame. More...

std::array< double, 42 >	zeroJacobian (franka::Frame frame, const std::array< double, 7 > &q, const std::array< double, 16 > &F_T_EE, const std::array< double, 16 > &EE_T_K) const override
	Gets the 6x7 Jacobian for the given joint relative to the base frame. More...

Public Member Functions inherited from franka_hw::ModelBase
std::array< double, 42 >	bodyJacobian (franka::Frame frame, const franka::RobotState &robot_state) const

std::array< double, 7 >	coriolis (const franka::RobotState &robot_state) const

std::array< double, 7 >	gravity (const franka::RobotState &robot_state) const

std::array< double, 7 >	gravity (const franka::RobotState &robot_state, const std::array< double, 3 > &gravity_earth) const

std::array< double, 7 >	gravity (const std::array< double, 7 > &q, double m_total, const std::array< double, 3 > &F_x_Ctotal) const

std::array< double, 49 >	mass (const franka::RobotState &robot_state) const

std::array< double, 16 >	pose (franka::Frame frame, const franka::RobotState &robot_state) const

std::array< double, 42 >	zeroJacobian (franka::Frame frame, const franka::RobotState &robot_state) const

virtual	~ModelBase () noexcept=default

Static Public Member Functions
static void	augmentFrame (const std::string &name, const std::array< double, 16 > &transform, KDL::Chain &chain)
	Augment a kinematic chain by adding a virtual frame to it. More...

static void	augmentFrame (const std::string &name, const std::array< double, 3 > &center_of_mass, double mass, const std::array< double, 9 > &inertia, KDL::Chain &chain)
	Augment a kinematic chain by adding a virtual frame with an inertia to it. More...

Private Member Functions
bool	isCloseToSingularity (const KDL::Jacobian &jacobian) const

Static Private Member Functions
static int	segment (franka::Frame frame)

static std::string	strError (const int error)

Private Attributes
KDL::Chain	chain_

double	singularity_threshold_

Detailed Description

Calculates poses of links and dynamic properties of the robot.

This implementation of ModelBase uses KDL as backend for calculating dynamic and kinematic properties of the robot.

Definition at line 21 of file model_kdl.h.

Constructor & Destructor Documentation

◆ ModelKDL()

franka_gazebo::ModelKDL::ModelKDL	(	const urdf::Model &	model,
		const std::string &	root,
		const std::string &	tip,
		double	singularity_threshold = `-1`
	)

Create a new implementation for the ModelBase with KDL as backend.

Parameters

[in]	model	the URDF from which to interprete the kinematic chain
[in]	root	the link name of the root of the chain
[in]	tip	the link name of the tip of the chain
[in]	singularity_threshold	below which lowest singular value of SVD(J x J^T) a warning should be printed. Use -1 to disable.

Exceptions

std::invalid_argument when either root or tip cannot be found in the URDF

Definition at line 67 of file model_kdl.cpp.

Member Function Documentation

◆ augmentFrame() [1/2]

void franka_gazebo::ModelKDL::augmentFrame	(	const std::string &	name,
		const std::array< double, 16 > &	transform,
		KDL::Chain &	chain
	)

static

Augment a kinematic chain by adding a virtual frame to it.

Parameters

[in]	name	the name of the frame. Must be unique in the `chain`
[in]	transform	the homogenous transformation of the frame relative to the tip of `chain` which to add to the chain
[out]	chain	a reference to the kinematic chain which to augment. The segment will be added to this chain, so if you want to keep your original chain, make a copy before

Definition at line 85 of file model_kdl.cpp.

◆ augmentFrame() [2/2]

void franka_gazebo::ModelKDL::augmentFrame	(	const std::string &	name,
		const std::array< double, 3 > &	center_of_mass,
		double	mass,
		const std::array< double, 9 > &	inertia,
		KDL::Chain &	chain
	)

static

Augment a kinematic chain by adding a virtual frame with an inertia to it.

Parameters

[in]	name	the name of the frame. Must be unique in the `chain`
[in]	center_of_mass	the position of the center of mass of the virtual frame which should be added, relative to the tip of `chain`
[in]	mass	the mass in of this virtual frame
[in]	inertia	the 3d inertia tensor (column major) of the new frame, measured at `center_of_mass`.
[out]	chain	a reference to the kinematic chain which to augment. The segment will be added to this chain, so if you want to keep your original chain, make a copy before

Definition at line 95 of file model_kdl.cpp.

◆ bodyJacobian()

std::array< double, 42 > franka_gazebo::ModelKDL::bodyJacobian	(	franka::Frame	frame,
		const std::array< double, 7 > &	q,
		const std::array< double, 16 > &	F_T_EE,
		const std::array< double, 16 > &	EE_T_K
	)		const

overridevirtual

Gets the 6x7 Jacobian for the given frame, relative to that frame.

The Jacobian is represented as a 6x7 matrix in column-major format.

Parameters

[in]	frame	The desired frame.
[in]	q	Joint position.
[in]	F_T_EE	End effector in flange frame.
[in]	EE_T_K	Stiffness frame K in the end effector frame.

Returns: Vectorized 6x7 Jacobian, column-major.

Note: Not yet implemented

Implements franka_hw::ModelBase.

Definition at line 141 of file model_kdl.cpp.

◆ coriolis()

std::array< double, 7 > franka_gazebo::ModelKDL::coriolis	(	const std::array< double, 7 > &	q,
		const std::array< double, 7 > &	dq,
		const std::array< double, 9 > &	I_total,
		double	m_total,
		const std::array< double, 3 > &	F_x_Ctotal
	)		const

overridevirtual

Calculates the Coriolis force vector (state-space equation): $c= C \times dq$ , in $[Nm]$ .

Parameters

[in]	q	Joint position.
[in]	dq	Joint velocity.
[in]	I_total	Inertia of the attached total load including end effector, relative to center of mass, given as vectorized 3x3 column-major matrix. Unit: $[kg \times m^2]$ .
[in]	m_total	Weight of the attached total load including end effector. Unit: .
[in]	F_x_Ctotal	Translation from flange to center of mass of the attached total load. Unit: .

Returns: Coriolis force vector.

Implements franka_hw::ModelBase.

Definition at line 242 of file model_kdl.cpp.

◆ gravity()

std::array< double, 7 > franka_gazebo::ModelKDL::gravity	(	const std::array< double, 7 > &	q,
		double	m_total,
		const std::array< double, 3 > &	F_x_Ctotal,
		const std::array< double, 3 > &	gravity_earth
	)		const

overridevirtual

Calculates the gravity vector.

Unit: $[Nm]$ .

Parameters

[in]	q	Joint position.
[in]	m_total	Weight of the attached total load including end effector. Unit: .
[in]	F_x_Ctotal	Translation from flange to center of mass of the attached total load. Unit: .
[in]	gravity_earth	Earth's gravity vector. Unit: $\frac{m}{s^2}$ .

Returns: Gravity vector.

Implements franka_hw::ModelBase.

Definition at line 268 of file model_kdl.cpp.

◆ isCloseToSingularity()

bool franka_gazebo::ModelKDL::isCloseToSingularity ( const KDL::Jacobian & jacobian ) const

private

Definition at line 35 of file model_kdl.cpp.

◆ mass()

std::array< double, 49 > franka_gazebo::ModelKDL::mass	(	const std::array< double, 7 > &	q,
		const std::array< double, 9 > &	I_total,
		double	m_total,
		const std::array< double, 3 > &	F_x_Ctotal
	)		const

overridevirtual

Calculates the 7x7 mass matrix.

Unit: $[kg \times m^2]$ .

Parameters

[in]	q	Joint position.
[in]	I_total	Inertia of the attached total load including end effector, relative to center of mass, given as vectorized 3x3 column-major matrix. Unit: $[kg \times m^2]$ .
[in]	m_total	Weight of the attached total load including end effector. Unit: .
[in]	F_x_Ctotal	Translation from flange to center of mass of the attached total load. Unit: .

Returns: Vectorized 7x7 mass matrix, column-major.

Implements franka_hw::ModelBase.

Definition at line 217 of file model_kdl.cpp.

◆ pose()

std::array< double, 16 > franka_gazebo::ModelKDL::pose	(	franka::Frame	frame,
		const std::array< double, 7 > &	q,
		const std::array< double, 16 > &	F_T_EE,
		const std::array< double, 16 > &	EE_T_K
	)		const

overridevirtual

Gets the 4x4 pose matrix for the given frame in base frame.

The pose is represented as a 4x4 matrix in column-major format.

Parameters

[in]	frame	The desired frame.
[in]	q	Joint position.
[in]	F_T_EE	End effector in flange frame.
[in]	EE_T_K	Stiffness frame K in the end effector frame.

Returns: Vectorized 4x4 pose matrix, column-major.

Implements franka_hw::ModelBase.

Definition at line 109 of file model_kdl.cpp.

◆ segment()

int franka_gazebo::ModelKDL::segment ( franka::Frame frame )

staticprivate

Definition at line 17 of file model_kdl.cpp.

◆ strError()

std::string franka_gazebo::ModelKDL::strError ( const int error )

staticprivate

Definition at line 47 of file model_kdl.cpp.

◆ zeroJacobian()

std::array< double, 42 > franka_gazebo::ModelKDL::zeroJacobian	(	franka::Frame	frame,
		const std::array< double, 7 > &	q,
		const std::array< double, 16 > &	F_T_EE,
		const std::array< double, 16 > &	EE_T_K
	)		const

overridevirtual

Gets the 6x7 Jacobian for the given joint relative to the base frame.

The Jacobian is represented as a 6x7 matrix in column-major format.

Parameters

[in]	frame	The desired frame.
[in]	q	Joint position.
[in]	F_T_EE	End effector in flange frame.
[in]	EE_T_K	Stiffness frame K in the end effector frame.

Returns: Vectorized 6x7 Jacobian, column-major.

Implements franka_hw::ModelBase.

Definition at line 184 of file model_kdl.cpp.

Member Data Documentation

◆ chain_

KDL::Chain franka_gazebo::ModelKDL::chain_

private

Definition at line 195 of file model_kdl.h.

◆ singularity_threshold_

double franka_gazebo::ModelKDL::singularity_threshold_

private

Definition at line 196 of file model_kdl.h.

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

Public Member Functions

Static Public Member Functions

Private Member Functions

Static Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ModelKDL()

Member Function Documentation

◆ augmentFrame() [1/2]

◆ augmentFrame() [2/2]

◆ bodyJacobian()

◆ coriolis()

◆ gravity()

◆ isCloseToSingularity()

◆ mass()

◆ pose()

◆ segment()

◆ strError()

◆ zeroJacobian()

Member Data Documentation

◆ chain_

◆ singularity_threshold_