A SO(2) joint. Bounds are from -pi to pi, and wraps. More...

#include <joints.h>

Inheritance diagram for robowflex::darts::SO2Joint:

Public Member Functions
	SO2Joint (StateSpace space, dart::dynamics::Joint joint, unsigned int index=0)
	Constructor. More...

double	distance (const Eigen::Ref< const Eigen::VectorXd > &a, const Eigen::Ref< const Eigen::VectorXd > &b) const override
	Compute the distance between two joint configurations. More...

double	getMaximumExtent () const override
	Get the maximum extent of this joint (maximum distance). More...

void	interpolate (const Eigen::Ref< const Eigen::VectorXd > &a, const Eigen::Ref< const Eigen::VectorXd > &b, double t, Eigen::Ref< Eigen::VectorXd > c) const override
	Interpolate to a new configuration c which is t from a to b. More...

void	enforceBounds (Eigen::Ref< Eigen::VectorXd > a) const override
	Enforce bounds on a joint configuration. More...

bool	satisfiesBounds (const Eigen::Ref< const Eigen::VectorXd > &a) const override
	Check if a joint configuration satisfies bounds. More...

void	sample (Eigen::Ref< Eigen::VectorXd > a) const override
	Sample a configuration for this joint. More...

void	sampleNear (Eigen::Ref< Eigen::VectorXd > a, const Eigen::Ref< const Eigen::VectorXd > &near, double r) const override
	Sample a configuration for this joint near another configuration near. More...

Public Member Functions inherited from robowflex::darts::Joint
	Joint (StateSpace *space, unsigned int skelIndex, unsigned int jointIndex, unsigned int sizeInSpace, unsigned int startIndex, unsigned int numDof)
	Constructor. More...

	Joint (StateSpace space, dart::dynamics::Joint joint, unsigned int sizeInSpace, unsigned int startIndex=0, unsigned int numDof=0)
	Constructor. More...

Eigen::Ref< Eigen::VectorXd >	getSpaceVars (Eigen::Ref< Eigen::VectorXd > a)
	Gets the joint configuration (subvector of variables for this joint) from a configuration. More...

Eigen::Ref< const Eigen::VectorXd >	getSpaceVarsConst (const Eigen::Ref< const Eigen::VectorXd > &a)
	Gets the subvector of variables for this joint from a configuration. More...

const std::vector< std::size_t > &	getIndices () const
	Get the indices in the skeleton for this joint. More...

const std::vector< std::string > &	getDofs () const
	Get the names of the DoF in the skeleton for this joint. More...

std::size_t	getSkeletonIndex () const
	Get the index of the skeleton for this joint in the world. More...

std::size_t	getJointIndex () const
	Get the index of the joint in the skeleton. More...

std::size_t	getDimension () const
	Get the dimension of this joint. More...

virtual void	setUpperLimits (const Eigen::Ref< const Eigen::VectorXd > &v)
	Set the upper limits of a joint. More...

virtual void	setLowerLimits (const Eigen::Ref< const Eigen::VectorXd > &v)
	Set the lower limits of a joint. More...

virtual Eigen::VectorXd	getUpperLimits () const
	Get the upper limits of a joint. More...

virtual Eigen::VectorXd	getLowerLimits () const
	Get the lower limits of a joint. More...

virtual void	setJointState (WorldPtr world, const Eigen::Ref< const Eigen::VectorXd > &a) const
	Set the state of the joint in world. More...

virtual void	getJointState (WorldPtr world, Eigen::Ref< Eigen::VectorXd > a) const
	Get the state of the joint in world. More...

dart::dynamics::Joint *	getJoint (WorldPtr world) const
	Get the Dart joint from a world. More...

Additional Inherited Members
Protected Attributes inherited from robowflex::darts::Joint
StateSpace *	space_
	State space this joint is for. More...

ompl::RNG &	rng_
	Random number generator. More...

std::vector< std::size_t >	indices_
	Indices this joint corresponds to. More...

std::vector< std::string >	dofs_
	Controlled DoF names. More...

unsigned int	skelIndex_
	Index of skeleton. More...

unsigned int	jointIndex_
	Index of joint. More...

unsigned int	startInSpace_
	Start index in space configuration. More...

unsigned int	sizeInSpace_
	Size of joint in space configuration. More...

unsigned int	startIndex_
	Start index in joint. More...

unsigned int	numDof_
	Number of DoF this joint controls. More...

Detailed Description

A SO(2) joint. Bounds are from -pi to pi, and wraps.

Definition at line 284 of file joints.h.

Constructor & Destructor Documentation

◆ SO2Joint()

SO2Joint::SO2Joint	(	StateSpace *	space,
		dart::dynamics::Joint *	joint,
		unsigned int	index = `0`
	)

Constructor.

Parameters

[in]	space	State space.
[in]	joint	Joint.

SO2Joint

Definition at line 15 of file so2joint.cpp.

   : Joint(space, joint, 1, index, 1)
 {
     space_->addDimension(-constants::pi, constants::pi);
 }

Member Function Documentation

◆ distance()

double SO2Joint::distance	(	const Eigen::Ref< const Eigen::VectorXd > &	a,
		const Eigen::Ref< const Eigen::VectorXd > &	b
	)		const

overridevirtual

Compute the distance between two joint configurations.

Parameters

[in]	a	Joint configuration a.
[in]	b	Joint configuration b.

Returns: Distance between configurations.

Implements robowflex::darts::Joint.

Definition at line 23 of file so2joint.cpp.

 {
     double d = std::fabs(a[0] - b[0]);
     return (d > constants::pi) ? constants::two_pi - d : d;
 }

◆ enforceBounds()

void SO2Joint::enforceBounds ( Eigen::Ref< Eigen::VectorXd > a ) const

overridevirtual

Enforce bounds on a joint configuration.

Parameters

[in,out] a Joint configuration to modify.

Implements robowflex::darts::Joint.

Definition at line 60 of file so2joint.cpp.

 {
     double &v = a[0];
     v = std::fmod(v, constants::two_pi);
     if (v < -constants::pi)
         v += constants::two_pi;
     else if (v >= constants::pi)
         v -= constants::two_pi;
 }

◆ getMaximumExtent()

double SO2Joint::getMaximumExtent ( ) const

overridevirtual

Get the maximum extent of this joint (maximum distance).

Returns: The maximum extent.

Implements robowflex::darts::Joint.

Definition at line 30 of file so2joint.cpp.

 {
     return constants::pi;
 }

◆ interpolate()

void SO2Joint::interpolate	(	const Eigen::Ref< const Eigen::VectorXd > &	a,
		const Eigen::Ref< const Eigen::VectorXd > &	b,
		double	t,
		Eigen::Ref< Eigen::VectorXd >	c
	)		const

overridevirtual

Interpolate to a new configuration c which is t from a to b.

Parameters

[in]	a	Joint configuration a.
[in]	b	Joint configuration b.
[in]	t	Amount to interpolate.
[out]	c	Interpolated configuration.

Implements robowflex::darts::Joint.

Definition at line 35 of file so2joint.cpp.

 {
     double diff = b[0] - a[0];
     if (std::fabs(diff) <= constants::pi)
         c[0] = a[0] + diff * t;
     else
     {
         if (diff > 0.0)
             diff = constants::two_pi - diff;
         else
             diff = -constants::two_pi - diff;
  
         double &v = c[0];
         v = a[0] - diff * t;
  
         if (v > constants::pi)
             v -= constants::two_pi;
         else if (v < -constants::pi)
             v += constants::two_pi;
     }
 }

◆ sample()

void SO2Joint::sample ( Eigen::Ref< Eigen::VectorXd > a ) const

overridevirtual

Sample a configuration for this joint.

Parameters

[out] a Joint configuration to sample.

Implements robowflex::darts::Joint.

Definition at line 76 of file so2joint.cpp.

 {
     a[0] = rng_.uniformReal(-constants::pi, constants::pi);
 }

◆ sampleNear()

void SO2Joint::sampleNear	(	Eigen::Ref< Eigen::VectorXd >	a,
		const Eigen::Ref< const Eigen::VectorXd > &	near,
		double	r
	)		const

overridevirtual

Sample a configuration for this joint near another configuration near.

Parameters

[out]	a	Joint configuration to sample.
[in]	near	Joint configuration to sample near.
[in]	r	Distance from near to sample within.

Implements robowflex::darts::Joint.

Definition at line 81 of file so2joint.cpp.

 {
     a[0] = rng_.uniformReal(near[0] - r, near[0] + r);
     enforceBounds(a);
 }

◆ satisfiesBounds()

bool SO2Joint::satisfiesBounds ( const Eigen::Ref< const Eigen::VectorXd > & a ) const

overridevirtual

Check if a joint configuration satisfies bounds.

Parameters

[in,out] a Joint configuration to check.

Returns: True if configuration satisfies bounds, false otherwise.

Implements robowflex::darts::Joint.

Definition at line 70 of file so2joint.cpp.

 {
     const double &v = a[0];
     return (v >= -constants::pi) and (v <= constants::pi);
 }

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

Public Member Functions

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ SO2Joint()

Member Function Documentation

◆ distance()

◆ enforceBounds()

◆ getMaximumExtent()

◆ interpolate()

◆ sample()

◆ sampleNear()

◆ satisfiesBounds()