robowflex::darts::SO3Joint Class Reference

A SO(3) joint modeled with quaternions. More...

#include <joints.h>

Inheritance diagram for robowflex::darts::SO3Joint:

Public Member Functions
	SO3Joint (StateSpace *space, dart::dynamics::Joint *joint)
	Constructor. More...
Eigen::Quaterniond	toQuat (const Eigen::Ref< const Eigen::VectorXd > &a) const
	Get the quaternion corresponding to the joint configuration. More...
void	setQuat (Eigen::Ref< Eigen::VectorXd > a, const Eigen::Quaterniond &q) const
	Set the joint configuration to a quaternion. 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...
void	setJointState (WorldPtr world, const Eigen::Ref< const Eigen::VectorXd > &a) const override
	Set the state of the joint in world. More...
void	getJointState (WorldPtr world, Eigen::Ref< Eigen::VectorXd > a) const override
	Get the state of the joint in world. 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...
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(3) joint modeled with quaternions.

Definition at line 316 of file joints.h.

Constructor & Destructor Documentation

SO3Joint()

SO3Joint::SO3Joint	(	StateSpace *	space,
		dart::dynamics::Joint *	joint
	)

Constructor.

Parameters

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

Definition at line 11 of file so3joint.cpp.

  : Joint(space, joint, 4)
{
    space_->addDimension(-1., 1.);  // q w
    space_->addDimension(-1., 1.);  // q x
    space_->addDimension(-1., 1.);  // q y
    space_->addDimension(-1., 1.);  // q z
}

Member Function Documentation

distance()

double SO3Joint::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 35 of file so3joint.cpp.

{
    return toQuat(a).angularDistance(toQuat(b));
}

enforceBounds()

void SO3Joint::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 55 of file so3joint.cpp.

{
    auto q = toQuat(a).normalized();
    setQuat(a, q);
}

getJointState()

void SO3Joint::getJointState ( WorldPtr  world, Eigen::Ref< Eigen::VectorXd >  a  ) const

overridevirtual

Get the state of the joint in world.

Parameters

[in]	world	World to get joint state from.
[out]	a	Joint configuration to set.

Reimplemented from robowflex::darts::Joint.

Definition at line 109 of file so3joint.cpp.

{
    auto *joint = getJoint(world);
 
    Eigen::Isometry3d tf = joint->getRelativeTransform();
    Eigen::Quaterniond q(tf.linear());
 
    setQuat(a, q);
}

getMaximumExtent()

double SO3Joint::getMaximumExtent ( ) const

overridevirtual

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

Returns

The maximum extent.

Implements robowflex::darts::Joint.

Definition at line 41 of file so3joint.cpp.

{
    return constants::half_pi;
}

interpolate()

void SO3Joint::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 46 of file so3joint.cpp.

{
    auto slerp = toQuat(a).slerp(t, toQuat(b));
    setQuat(c, slerp);
}

sample()

void SO3Joint::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 67 of file so3joint.cpp.

{
    double q[4];
    rng_.quaternion(q);
    a[0] = q[3];
    a[1] = q[0];
    a[2] = q[1];
    a[3] = q[2];
}

sampleNear()

void SO3Joint::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 77 of file so3joint.cpp.

{
    if (r >= constants::quarter_pi)
        sample(a);
 
    else
    {
        double d = rng_.uniform01();
        auto qnear = toQuat(near);
        Eigen::Quaterniond q(                                                                            //
            Eigen::AngleAxisd(2. * std::pow(d, constants::third) * r,                                    //
                              Eigen::Vector3d{rng_.gaussian01(), rng_.gaussian01(), rng_.gaussian01()})  //
        );
        auto qs = qnear * q;
        setQuat(a, qs);
    }
}

satisfiesBounds()

bool SO3Joint::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 61 of file so3joint.cpp.

{
    double n = a.norm();
    return (n - 1.) < constants::eps;
}

setJointState()

void SO3Joint::setJointState ( WorldPtr  world, const Eigen::Ref< const Eigen::VectorXd > &  a  ) const

overridevirtual

Set the state of the joint in world.

Parameters

[out]	world	World to set joint state in.
[in]	a	Joint configuration to set.

Reimplemented from robowflex::darts::Joint.

Definition at line 97 of file so3joint.cpp.

{
    auto *joint = getJoint(world);
    auto *j = static_cast<dart::dynamics::FreeJoint *>(joint);
 
    Eigen::Isometry3d tf;
    tf.translation() = j->getPositions();
    tf.linear() = toQuat(a).toRotationMatrix();
 
    j->setRelativeTransform(tf);
}

setQuat()

void SO3Joint::setQuat	(	Eigen::Ref< Eigen::VectorXd >	a,
		const Eigen::Quaterniond &	q
	)		const

Set the joint configuration to a quaternion.

Parameters

[out]	a	Joint configuration.
[in]	q	Quaternion to set.

Definition at line 26 of file so3joint.cpp.

{
    a[0] = q.w();
    a[1] = q.x();
    a[2] = q.y();
    a[3] = q.z();
}

toQuat()

Eigen::Quaterniond SO3Joint::toQuat

(

const Eigen::Ref< const Eigen::VectorXd > &

a

)

const

Get the quaternion corresponding to the joint configuration.

Parameters

[in]

a

Joint configuration.

Returns

Quaternion of configuration.

Definition at line 21 of file so3joint.cpp.

{
    return Eigen::Quaterniond(a[0], a[1], a[2], a[3]);
}

---

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

• joints.h
• so3joint.cpp