#include <articulated_body.h>

Public Member Functions
	ArticulatedBody ()

	ArticulatedBody (const std::string &name)

	ArticulatedBody (const ArticulatedBody &ab)

	ArticulatedBody (ArticulatedBody &&ab)

virtual	~ArticulatedBody ()

ArticulatedBody &	operator= (const ArticulatedBody &ab)

size_t	dof () const

int	AddRigidBody (const RigidBody &rb, int id_parent=-1)

const std::vector< RigidBody > &	rigid_bodies () const

const RigidBody &	rigid_bodies (int i) const

const Eigen::VectorXd &	q () const

double	q (int i) const

virtual void	set_q (Eigen::Ref< const Eigen::VectorXd > q)

const Eigen::VectorXd &	dq () const

double	dq (int i) const

virtual void	set_dq (Eigen::Ref< const Eigen::VectorXd > dq)

const SpatialMotiond &	g () const

void	set_g (const Eigen::Vector3d &g)

virtual void	AddLoad (const SpatialInertiad &inertia, int idx_link=-1)

virtual void	ReplaceLoad (const SpatialInertiad &inertia, int idx_link=-1)

virtual void	ClearLoad (int idx_link=-1)

const std::map< size_t, SpatialInertiad > &	inertia_load () const

const Eigen::Isometry3d &	T_base_to_world () const

template<typename Derived >
void	set_T_base_to_world (const Eigen::RotationBase< Derived, 3 > &ori_in_world, const Eigen::Vector3d &pos_in_world)

void	set_T_base_to_world (const Eigen::Isometry3d &T_to_world)

const SpatialInertiad &	inertia_base () const

void	set_inertia_base (double mass, const Eigen::Vector3d &com, const Eigen::Vector6d &I_com_flat)

void	set_inertia_base (const SpatialInertiad &inertia)

const Eigen::Isometry3d &	T_to_parent (int i) const

Eigen::Isometry3d	T_to_parent (int i, double q) const

Eigen::Isometry3d	T_from_parent (int i) const

Eigen::Isometry3d	T_from_parent (int i, double q) const

const Eigen::Isometry3d &	T_to_world (int i) const

Eigen::Isometry3d	T_to_world (int i, Eigen::Ref< const Eigen::VectorXd > q) const

Eigen::Isometry3d	T_from_world (int i) const

Eigen::Isometry3d	T_from_world (int i, Eigen::Ref< const Eigen::VectorXd > q) const

const std::vector< int > &	ancestors (int i) const

const std::vector< int > &	subtree (int i) const

Eigen::VectorXd	Map (const std::function< double(const RigidBody &rb)> &rb_function) const

Public Attributes
std::string	name

std::vector< Graphics >	graphics

Detailed Description

Main articulated body struct for spatial_dyn.

See also: Python: spatialdyn.ArticulatedBody

Constructor & Destructor Documentation

◆ ArticulatedBody() [1/4]

spatial_dyn::ArticulatedBody::ArticulatedBody ( )

Default constructor.

◆ ArticulatedBody() [2/4]

spatial_dyn::ArticulatedBody::ArticulatedBody ( const std::string & name )

Constructor that sets the name of the articulated body.

Parameters

name	Name of the articulated body.

See also: Python: spatialdyn.ArticulatedBody.__init__()

◆ ArticulatedBody() [3/4]

spatial_dyn::ArticulatedBody::ArticulatedBody ( const ArticulatedBody & ab )

Copy constructor.

◆ ArticulatedBody() [4/4]

spatial_dyn::ArticulatedBody::ArticulatedBody ( ArticulatedBody && ab )

Move constructor.

◆ ~ArticulatedBody()

spatial_dyn::ArticulatedBody::~ArticulatedBody ( )

virtual

Destructor.

Member Function Documentation

◆ AddLoad()

void spatial_dyn::ArticulatedBody::AddLoad	(	const SpatialInertiad &	inertia,
		int	idx_link = `-1`
	)

virtual

Add a spatial inertia to the existing load on the specified link.

If a load doesn't exist on the link, a new load will be created.

Parameters

inertia	Spatial inertia of load to add represented in the link's frame.
idx_link	Index of the desired link.

See also: Python: spatialdyn.ArticulatedBody.add_load

◆ AddRigidBody()

int spatial_dyn::ArticulatedBody::AddRigidBody	(	const RigidBody &	rb,
		int	id_parent = `-1`
	)

Add a rigid body to the articulated body and return its assigned ID.

IDs will be assigned to rigid bodies starting from 0 and incrementing each time AddRigidBody() is called. These IDs can be used to index into the corresponding rigid_bodies() vector or joint variables such as q().

Parameters

rb	Rigid body to add.
id_parent	ID of the parent rigid body. If `parent_id` is `-1`, the rigid body will be added to the base.

Returns: ID assigned to the new rigid body.

See also: Python: spatialdyn.ArticulatedBody.add_rigid_body()

◆ ancestors()

const std::vector< int > & spatial_dyn::ArticulatedBody::ancestors ( int i ) const

Get the ancestors of rigid body i.

Parameters

i	Index of the desired frame. Uses Pythonic indexing (if `i < 0`, count from the back).

Returns: Vector of ancestor IDs of rigid body i, from the base to the rigid body (inclusive).

See also: Python: spatialdyn.ArticulatedBody.ancestors()

◆ ClearLoad()

void spatial_dyn::ArticulatedBody::ClearLoad ( int idx_link = -1 )

virtual

Clear the existing inertial load on the specified link.

Parameters

idx_link Index of the desired link.

See also: Python: spatialdyn.ArticulatedBody.clear_load

◆ dof()

size_t spatial_dyn::ArticulatedBody::dof ( ) const

inline

Returns: Degrees of freedom of the articulated body.

See also: Python: spatialdyn.ArticulatedBody.dof

◆ dq() [1/2]

const Eigen::VectorXd& spatial_dyn::ArticulatedBody::dq ( ) const

inline

Returns: Joint velocities. Uninitialized values will be 0 by default.

See also: Python: spatialdyn.ArticulatedBody.dq

◆ dq() [2/2]

double spatial_dyn::ArticulatedBody::dq ( int i ) const

Returns: ith joint velocity with Pythonic indexing (if i < 0, count from the back).

◆ g()

const SpatialMotiond& spatial_dyn::ArticulatedBody::g ( ) const

inline

Returns: 6d spatial gravity vector acting on the articulated body. Defaults to -9.81 in the linear z direction.

See also: Python: spatialdyn.ArticulatedBody.g

◆ inertia_base()

const SpatialInertiad& spatial_dyn::ArticulatedBody::inertia_base ( ) const

inline

Returns: Spatial inertia of the base link. Defaults to a 1kg point mass at the origin of the rigid body's frame.

See also: Python: spatialdyn.ArticulatedBody.inertia_base

◆ inertia_load()

const std::map<size_t, SpatialInertiad>& spatial_dyn::ArticulatedBody::inertia_load ( ) const

inline

Returns: Reference to Map of (index, inertia) pairs where the inertia is the inertia of a load attached to the associated rigid body index.

See also: Python: spatialdyn.ArticulatedBody.inertia_load

◆ Map()

Eigen::VectorXd spatial_dyn::ArticulatedBody::Map ( const std::function< double(const RigidBody &rb)> & rb_function ) const

Map the given function across the articulated body structure.

Parameters

rb_function Function to apply to each rigid body.

Returns: Vector of doubles containing the mapped function results.

See also: Python: spatialdyn.ArticulatedBody.map()

◆ operator=()

ArticulatedBody & spatial_dyn::ArticulatedBody::operator= ( const ArticulatedBody & ab )

Copy assignment operator.

◆ q() [1/2]

const Eigen::VectorXd& spatial_dyn::ArticulatedBody::q ( ) const

inline

Returns: Joint positions. Uninitialized values will be 0 by default.

See also: Python: spatialdyn.ArticulatedBody.q

◆ q() [2/2]

double spatial_dyn::ArticulatedBody::q ( int i ) const

Returns: ith joint position with Pythonic indexing (if i < 0, count from the back).

◆ ReplaceLoad()

void spatial_dyn::ArticulatedBody::ReplaceLoad	(	const SpatialInertiad &	inertia,
		int	idx_link = `-1`
	)

virtual

Replace the existing inertial load on the specified link.

If a load doesn't exist on the link, a new load will be created.

Parameters

inertia	Spatial inertia of load to add represented in the link's frame.
idx_link	Index of the desired link.

See also: Python: spatialdyn.ArticulatedBody.replace_load

◆ rigid_bodies() [1/2]

const std::vector<RigidBody>& spatial_dyn::ArticulatedBody::rigid_bodies ( ) const

inline

Returns: Vector of rigid bodies, indexed by IDs assigned in AddRigidBody().

See also: Python: spatialdyn.ArticulatedBody.rigid_bodies

◆ rigid_bodies() [2/2]

const RigidBody & spatial_dyn::ArticulatedBody::rigid_bodies ( int i ) const

Returns: ith rigid body with Pythonic indexing (if i < 0, count from the back).

◆ set_dq()

void spatial_dyn::ArticulatedBody::set_dq ( Eigen::Ref< const Eigen::VectorXd > dq )

virtual

Set the joint velocities.

◆ set_g()

void spatial_dyn::ArticulatedBody::set_g ( const Eigen::Vector3d & g )

Set the gravity vector acting on the articulated body.

◆ set_inertia_base() [1/2]

void spatial_dyn::ArticulatedBody::set_inertia_base ( const SpatialInertiad & inertia )

Set the spatial inertia of the base link.

Parameters

inertia Spatial inertia of the rigid body.

◆ set_inertia_base() [2/2]

void spatial_dyn::ArticulatedBody::set_inertia_base	(	double	mass,
		const Eigen::Vector3d &	com,
		const Eigen::Vector6d &	I_com_flat
	)

Set the spatial inertia of the base link.

Parameters

mass	Mass of the rigid body.
com	Center of mass of the rigid body.
I_com_flat	Inertia of the rigid body as a flat vector `{I_xx, I_xy, I_xz, I_yy, I_yz, I_zz}``.

◆ set_q()

void spatial_dyn::ArticulatedBody::set_q ( Eigen::Ref< const Eigen::VectorXd > q )

virtual

Set the joint positions and internally precompute the frames of the articulated body.

◆ set_T_base_to_world() [1/2]

void spatial_dyn::ArticulatedBody::set_T_base_to_world ( const Eigen::Isometry3d & T_to_world )

Set the transform from the articulated body's base frame to the world frame.

Parameters

T_to_world Transform from the base frame to the world frame.

◆ set_T_base_to_world() [2/2]

template<typename Derived >

void spatial_dyn::ArticulatedBody::set_T_base_to_world	(	const Eigen::RotationBase< Derived, 3 > &	ori_in_world,
		const Eigen::Vector3d &	pos_in_world
	)

inline

Set the transform from the articulated body's base frame to the world frame.

Parameters

ori_in_world	Orientation of the articulated body in the world frame.
pos_in_world	Position of the articulated body in the world frame.

◆ subtree()

const std::vector< int > & spatial_dyn::ArticulatedBody::subtree ( int i ) const

Get the subtree of rigid body i.

Parameters

i	Index of the desired frame. Uses Pythonic indexing (if `i < 0`, count from the back).

Returns: Vector of IDs in the subtree of rigid body i, going from the rigid body (inclusive) to the leaves.

See also: Python: spatialdyn.ArticulatedBody.subtree()

◆ T_base_to_world()

const Eigen::Isometry3d& spatial_dyn::ArticulatedBody::T_base_to_world ( ) const

inline

Get the transform from the articulated body's base frame to the world frame.

This transformation matrix will transform a vector represented in the articulated body's base frame to one represented in the world frame. Set to identity by default.

Example

// Arbitrary position vector represented in the articulated body's base frame
Eigen::Vector3d pos_in_ab(0.1, 0.1, 0.1);
 
// Same position vector now represented in the world frame
Eigen::Vector3d pos_in_world = ab.T_base_to_world() * pos_in_ab;

Returns: Transform from the base frame to the world frame.

See also: Python: spatialdyn.ArticulatedBody.T_base_to_world

◆ T_from_parent() [1/2]

Eigen::Isometry3d spatial_dyn::ArticulatedBody::T_from_parent ( int i ) const

inline

Get the transform from rigid body i's parent frame to its own frame.

Parameters

i	Index of the desired frame. Uses Pythonic indexing (if `i < 0`, count from the back).

Returns: Inverse transform of T_to_parent(int).

See also: Python: spatialdyn.ArticulatedBody.T_from_parent()

◆ T_from_parent() [2/2]

Eigen::Isometry3d spatial_dyn::ArticulatedBody::T_from_parent	(	int	i,
		double	q
	)		const

inline

Get the transform from rigid body i's parent frame to its own frame given joint position q.

Parameters

i	Index of the desired frame. Uses Pythonic indexing (if `i < 0`, count from the back).
q	Position of joint `i`.

Returns: Inverse transform of T_to_parent(int, double).

See also: Python: spatialdyn.ArticulatedBody.T_from_parent()

◆ T_from_world() [1/2]

Eigen::Isometry3d spatial_dyn::ArticulatedBody::T_from_world ( int i ) const

inline

Get the transform from the world frame to rigid body i's frame.

Parameters

i	Index of the desired frame. Uses Pythonic indexing (if `i < 0`, count from the back).

Returns: Inverse transform of T_to_world(int).

See also: Python: spatialdyn.ArticulatedBody.T_from_world()

◆ T_from_world() [2/2]

Eigen::Isometry3d spatial_dyn::ArticulatedBody::T_from_world	(	int	i,
		Eigen::Ref< const Eigen::VectorXd >	q
	)		const

inline

Get the transform from the world frame to rigid body i's frame given joint position q.

Parameters

i	Index of the desired frame. Uses Pythonic indexing (if `i < 0`, count from the back).
q	Joint configuration of articulated body.

Returns: Inverse transform of T_to_world(int, double).

See also: Python: spatialdyn.ArticulatedBody.T_from_world()

◆ T_to_parent() [1/2]

const Eigen::Isometry3d & spatial_dyn::ArticulatedBody::T_to_parent ( int i ) const

Get the transform from rigid body i's frame to its parent's frame.

The computed transform is cached to avoid recomputation in subsequent calls to T_to_parent() for the same joint configuration.

This transformation matrix will transform a vector represented in frame i to one represented in frame i-1 according to the ith value of the articulated body's current joint positions q(). Uses Pythonic indexing (if i < 0, count from the back).

Example

// Arbitrary position vector represented in the end-effector's frame
Eigen::Vector3d pos_in_ee(0.1, 0.1, 0.1);
 
// Same positiion vector represented in the frame of the end-effector's parent
Eigen::Vector3d pos_in_ee_parent = ab.T_to_parent(-1);

Parameters

i	Index of the desired frame. Uses Pythonic indexing (if `i < 0`, count from the back).

Returns: Reference to the ached transform from the ith rigid body's frame to its parent's frame.

See also: Python: spatialdyn.ArticulatedBody.T_to_parent()

◆ T_to_parent() [2/2]

Eigen::Isometry3d spatial_dyn::ArticulatedBody::T_to_parent	(	int	i,
		double	q
	)		const

Get the transform from rigid body i's frame to its parent's frame given joint position q.

Parameters

i	Index of the desired frame. Uses Pythonic indexing (if `i < 0`, count from the back).
q	Position of joint `i`.

Returns: Transform from the ith rigid body's frame to its parent's frame.

See also: Python: spatialdyn.ArticulatedBody.T_to_parent()

◆ T_to_world() [1/2]

const Eigen::Isometry3d & spatial_dyn::ArticulatedBody::T_to_world ( int i ) const

Get the transform from rigid body i's frame to the world frame.

The computed transform is cached to avoid recomputation in subsequent calls to T_to_world() for the same joint configuration.

This transformation matrix will transform a vector represented in frame i to one represented in the world frame according to the articulated body's current joint positions q(). Uses Pythonic indexing (if i < 0, count from the back).

Example

// Arbitrary position vector represented in the end-effector's frame
Eigen::Vector3d pos_in_ee(0.1, 0.1, 0.1);
 
// Same position vector represented in the world frame
Eigen::Vector3d pos_in_ee_parent = ab.T_to_parent(-1);

Parameters

i	Index of the desired frame. Uses Pythonic indexing (if `i < 0`, count from the back).

Returns: Reference to the cached transform from the ith rigid body's frame to the world.

See also: Python: spatialdyn.ArticulatedBody.T_to_world()

◆ T_to_world() [2/2]

Eigen::Isometry3d spatial_dyn::ArticulatedBody::T_to_world	(	int	i,
		Eigen::Ref< const Eigen::VectorXd >	q
	)		const

Get the transform from rigid body i's frame to the world frame given joint position q.

Parameters

i	Index of the desired frame. Uses Pythonic indexing (if `i < 0`, count from the back).
q	Joint configuration of articulated body.

Returns: Transform from the ith rigid body's frame to the world.

See also: Python: spatialdyn.ArticulatedBody.T_to_world()

Member Data Documentation

◆ graphics

std::vector<Graphics> spatial_dyn::ArticulatedBody::graphics

mutable

Graphics for the base.

See also: Python: spatialdyn.ArticulatedBody.graphics

◆ name

std::string spatial_dyn::ArticulatedBody::name

mutable

Move assignment operator. Name of the articulated body for debugging purposes.

See also: Python: spatialdyn.ArticulatedBody.name

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

/home/runner/work/spatial-dyn/spatial-dyn/include/spatial_dyn/structs/articulated_body.h
/home/runner/work/spatial-dyn/spatial-dyn/src/structs/articulated_body.cc

Public Member Functions

Public Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ArticulatedBody() [1/4]

◆ ArticulatedBody() [2/4]

◆ ArticulatedBody() [3/4]

◆ ArticulatedBody() [4/4]

◆ ~ArticulatedBody()

Member Function Documentation

◆ AddLoad()

◆ AddRigidBody()

◆ ancestors()

◆ ClearLoad()

◆ dof()

◆ dq() [1/2]

◆ dq() [2/2]

◆ g()

◆ inertia_base()

◆ inertia_load()

◆ Map()

◆ operator=()

◆ q() [1/2]

◆ q() [2/2]

◆ ReplaceLoad()

◆ rigid_bodies() [1/2]

◆ rigid_bodies() [2/2]

◆ set_dq()

◆ set_g()

◆ set_inertia_base() [1/2]

◆ set_inertia_base() [2/2]

◆ set_q()

◆ set_T_base_to_world() [1/2]

◆ set_T_base_to_world() [2/2]

◆ subtree()

◆ T_base_to_world()

◆ T_from_parent() [1/2]

◆ T_from_parent() [2/2]

◆ T_from_world() [1/2]

◆ T_from_world() [2/2]

◆ T_to_parent() [1/2]

◆ T_to_parent() [2/2]

◆ T_to_world() [1/2]

◆ T_to_world() [2/2]

Member Data Documentation

◆ graphics

◆ name