TSRs describe end-effector constraint sets as subsets of SE(3). More...

#include <aikido/constraint/dart/TSR.hpp>

Inheritance diagram for aikido::constraint::dart::TSR:

Public Member Functions
	TSR (std::unique_ptr< common::RNG > _rng, const Eigen::Isometry3d &_T0_w=Eigen::Isometry3d::Identity(), const Eigen::Matrix< double, 6, 2 > &_Bw=Eigen::Matrix< double, 6, 2 >::Zero(), const Eigen::Isometry3d &_Tw_e=Eigen::Isometry3d::Identity(), double _testableTolerance=1e-6)
	Constructor. More...

	TSR (const Eigen::Isometry3d &_T0_w=Eigen::Isometry3d::Identity(), const Eigen::Matrix< double, 6, 2 > &_Bw=Eigen::Matrix< double, 6, 2 >::Zero(), const Eigen::Isometry3d &_Tw_e=Eigen::Isometry3d::Identity(), double _testableTolerance=1e-6)
	Constructor with default random seed generator. More...

	TSR (const TSR &other)

	TSR (TSR &&other)

TSR &	operator= (const TSR &other)

TSR &	operator= (TSR &&other)

virtual	~TSR ()=default

statespace::ConstStateSpacePtr	getStateSpace () const override
	Gets the StateSpace that this constraint operates on. More...

std::shared_ptr< statespace::SE3 >	getSE3 () const
	Returns the SE3 which this TSR operates in. More...

std::unique_ptr< SampleGenerator >	createSampleGenerator () const override
	Creates a SampleGenerator for sampling from this constraint. More...

bool	isSatisfied (const statespace::StateSpace::State _s, TestableOutcome outcome=nullptr) const override
	Returns true if state satisfies this constraint. More...

std::unique_ptr< TestableOutcome >	createOutcome () const override
	Return an instance of DefaultTestableOutcome, since this class doesn't have a more specialized TestableOutcome derivative assigned to it. More...

void	validate () const
	Throws an invalid_argument exception if this TSR is invalid. More...

void	setRNG (std::unique_ptr< common::RNG > rng)
	Set the random number generator used by SampleGenerators for this TSR. More...

std::size_t	getConstraintDimension () const override
	Size of constraints. More...

void	getValue (const statespace::StateSpace::State *_s, Eigen::VectorXd &_out) const override
	Get the value of constraints at _s. More...

void	getJacobian (const statespace::StateSpace::State *_s, Eigen::MatrixXd &_out) const override
	Jacobian of TSR with respect to the se(3) tangent vector of _s. More...

std::vector< ConstraintType >	getConstraintTypes () const override
	Returns a vector of constraints' types, i-th element correspoinding to the type of i-th constraint. More...

bool	project (const statespace::StateSpace::State _s, statespace::StateSpace::State _out) const override
	Projection _s to _out. More...

double	getTestableTolerance ()
	Get the testable tolerance used in isSatisfiable. More...

void	setTestableTolerance (double _testableTolerance)
	Set the testable tolerance used in isSatisfiable. More...

virtual void	getValueAndJacobian (const statespace::StateSpace::State *_s, Eigen::VectorXd &_val, Eigen::MatrixXd &_jac) const
	Get both Value and Jacobian. More...

Public Member Functions inherited from aikido::constraint::Sampleable
virtual	~Sampleable ()=default

Public Member Functions inherited from aikido::constraint::Differentiable
virtual	~Differentiable ()=default
	Destructor. More...

virtual void	getValueAndJacobian (const statespace::StateSpace::State *_s, Eigen::VectorXd &_val, Eigen::MatrixXd &_jac) const
	Get both Value and Jacobian. More...

Public Member Functions inherited from aikido::constraint::Testable
virtual	~Testable ()=default

Public Member Functions inherited from aikido::constraint::Projectable
virtual	~Projectable ()=default

virtual bool	project (statespace::StateSpace::State *_s) const
	Performs an in-place projection. More...

Public Attributes
Eigen::Isometry3d	mT0_w
	Transformation from origin frame into the TSR frame "w". More...

Eigen::Matrix< double, 6, 2 >	mBw
	Bounds on "wiggling" in `x, y, z, roll, pitch, yaw`. More...

Eigen::Isometry3d	mTw_e
	Transformation from "w" frame into end frame. More...

Private Attributes
double	mTestableTolerance
	Tolerance used in isSatisfied as a testable. More...

std::unique_ptr< common::RNG >	mRng

std::shared_ptr< statespace::SE3 >	mStateSpace

Detailed Description

TSRs describe end-effector constraint sets as subsets of SE(3).

A TSR consists of three parts: T0_w: transform from the origin to the TSR frame w B_w: 6 × 2 matrix of bounds in the coordinates of w. Tw_e: end-effector offset transform in the coordinates of w See: Berenson, Dmitry, Siddhartha S. Srinivasa, and James Kuffner. "Task space regions: A framework for pose-constrained manipulation planning." IJRR 2001: http://repository.cmu.edu/cgi/viewcontent.cgi?article=2024&context=robotics

Constructor & Destructor Documentation

◆ TSR() [1/4]

aikido::constraint::dart::TSR::TSR	(	std::unique_ptr< common::RNG >	_rng,
		const Eigen::Isometry3d &	_T0_w = `Eigen::Isometry3d::Identity()`,
		const Eigen::Matrix< double, 6, 2 > &	_Bw = `Eigen::Matrix< double, 6, 2 >::Zero()`,
		const Eigen::Isometry3d &	_Tw_e = `Eigen::Isometry3d::Identity()`,
		double	_testableTolerance = `1e-6`
	)

Constructor.

Parameters

_rng	Random number generator used by SampleGenerators for this TSR.
_T0_w	transform from the origin to the TSR frame w
_Bw	6 × 2 matrix of bounds in the coordinates of w. Top three rows bound translation, and bottom three rows bound rotation following Roll-Pitch-Yaw convention. _Bw(i, 0) should be less than or equal to _Bw(i, 1).
_Tw_e	end-effector offset transform in the coordinates of w
_testableTolerance	tolerance used in isSatisfiable as testable

◆ TSR() [2/4]

aikido::constraint::dart::TSR::TSR	(	const Eigen::Isometry3d &	_T0_w = `Eigen::Isometry3d::Identity()`,
		const Eigen::Matrix< double, 6, 2 > &	_Bw = `Eigen::Matrix< double, 6, 2 >::Zero()`,
		const Eigen::Isometry3d &	_Tw_e = `Eigen::Isometry3d::Identity()`,
		double	_testableTolerance = `1e-6`
	)

Constructor with default random seed generator.

Parameters

_T0_w	transform from the origin to the TSR frame w
_Bw	6 × 2 matrix of bounds in the coordinates of w. Top three rows bound translation, and bottom three rows bound rotation following Roll-Pitch-Yaw convention. _Bw(i, 0) should be less than or equal to _Bw(i, 1).
_Tw_e	end-effector offset transform in the coordinates of w
_testableTolerance	tolerance used in isSatisfiable as testable

◆ TSR() [3/4]

aikido::constraint::dart::TSR::TSR ( const TSR & other )

◆ TSR() [4/4]

aikido::constraint::dart::TSR::TSR ( TSR && other )

◆ ~TSR()

virtual aikido::constraint::dart::TSR::~TSR ( )

virtualdefault

Member Function Documentation

◆ createOutcome()

std::unique_ptr<TestableOutcome> aikido::constraint::dart::TSR::createOutcome ( ) const

overridevirtual

Return an instance of DefaultTestableOutcome, since this class doesn't have a more specialized TestableOutcome derivative assigned to it.

Implements aikido::constraint::Testable.

◆ createSampleGenerator()

std::unique_ptr<SampleGenerator> aikido::constraint::dart::TSR::createSampleGenerator ( ) const

overridevirtual

Creates a SampleGenerator for sampling from this constraint.

Implements aikido::constraint::Sampleable.

◆ getConstraintDimension()

std::size_t aikido::constraint::dart::TSR::getConstraintDimension ( ) const

overridevirtual

Size of constraints.

Implements aikido::constraint::Differentiable.

◆ getConstraintTypes()

std::vector<ConstraintType> aikido::constraint::dart::TSR::getConstraintTypes ( ) const

overridevirtual

Returns a vector of constraints' types, i-th element correspoinding to the type of i-th constraint.

Implements aikido::constraint::Differentiable.

◆ getJacobian()

void aikido::constraint::dart::TSR::getJacobian	(	const statespace::StateSpace::State *	_s,
		Eigen::MatrixXd &	_out
	)		const

overridevirtual

Jacobian of TSR with respect to the se(3) tangent vector of _s.

The jacobian is w.r.t. the origin frame. se(3) tangent vector follows dart convention: top 3 rows is the angle-axis representation of _s's rotation. bottom 3 rows represent the translation.

Parameters

	_s	State to be evaluated at.
[out]	_out	Jacobian, 6 x 6 matrix.

Implements aikido::constraint::Differentiable.

◆ getSE3()

std::shared_ptr<statespace::SE3> aikido::constraint::dart::TSR::getSE3 ( ) const

Returns the SE3 which this TSR operates in.

◆ getStateSpace()

statespace::ConstStateSpacePtr aikido::constraint::dart::TSR::getStateSpace ( ) const

overridevirtual

Gets the StateSpace that this constraint operates on.

Implements aikido::constraint::Differentiable.

◆ getTestableTolerance()

double aikido::constraint::dart::TSR::getTestableTolerance ( )

Get the testable tolerance used in isSatisfiable.

◆ getValue()

void aikido::constraint::dart::TSR::getValue	(	const statespace::StateSpace::State *	_s,
		Eigen::VectorXd &	_out
	)		const

overridevirtual

Get the value of constraints at _s.

Should be 0 to satisfy equality constraints. Should be <=0 to satisfy inequality constraints.

Parameters

	_s	State to be evaluated at.
[out]	_out	Vector to store the value. Length should match the number of constraints.

Implements aikido::constraint::Differentiable.

◆ getValueAndJacobian()

virtual void aikido::constraint::Differentiable::getValueAndJacobian

Get both Value and Jacobian.

Parameters

	_s	State to be evaluated.
[out]	_val	Value of constraints.
[out]	_jac	Jacoiban of constraints.

◆ isSatisfied()

bool aikido::constraint::dart::TSR::isSatisfied	(	const statespace::StateSpace::State *	_state,
		TestableOutcome *	outcome = `nullptr`
	)		const

overridevirtual

Returns true if state satisfies this constraint.

Parameters

[in]	_state	given state to test.
[in]	outcome	pointer to TestableOutcome derivative instance that method will populate with useful information. Each derivative class of Testable may expect outcome to be a different derivative class of TestableOutcome (this casting and population is done under the hood). If this argument is missing, it is ignored.

Implements aikido::constraint::Testable.

◆ operator=() [1/2]

TSR& aikido::constraint::dart::TSR::operator= ( const TSR & other )

◆ operator=() [2/2]

TSR& aikido::constraint::dart::TSR::operator= ( TSR && other )

◆ project()

bool aikido::constraint::dart::TSR::project	(	const statespace::StateSpace::State *	_s,
		statespace::StateSpace::State *	_out
	)		const

overridevirtual

Projection _s to _out.

Returns false if projection cannot be done.

Parameters

_s	state to be projected.
_out	resulting projection.

Implements aikido::constraint::Projectable.

◆ setRNG()

void aikido::constraint::dart::TSR::setRNG ( std::unique_ptr< common::RNG > rng )

Set the random number generator used by SampleGenerators for this TSR.

◆ setTestableTolerance()

void aikido::constraint::dart::TSR::setTestableTolerance ( double _testableTolerance )

Set the testable tolerance used in isSatisfiable.

Parameters

_testableTolerance Testable tolerance to set.

◆ validate()

void aikido::constraint::dart::TSR::validate ( ) const

Throws an invalid_argument exception if this TSR is invalid.

For a TSR to be valid, mBw(i, 0) <= mBw(i, 1).

Member Data Documentation

◆ mBw

Eigen::Matrix<double, 6, 2> aikido::constraint::dart::TSR::mBw

Bounds on "wiggling" in x, y, z, roll, pitch, yaw.

◆ mRng

std::unique_ptr<common::RNG> aikido::constraint::dart::TSR::mRng

private

◆ mStateSpace

std::shared_ptr<statespace::SE3> aikido::constraint::dart::TSR::mStateSpace

private

◆ mT0_w

Eigen::Isometry3d aikido::constraint::dart::TSR::mT0_w

Transformation from origin frame into the TSR frame "w".

"w" is usually centered at the origin of an object held by the hand or at a location on an object that is useful for grasping.

◆ mTestableTolerance

double aikido::constraint::dart::TSR::mTestableTolerance

private

Tolerance used in isSatisfied as a testable.

◆ mTw_e

Eigen::Isometry3d aikido::constraint::dart::TSR::mTw_e

Transformation from "w" frame into end frame.

This often represent an offset from "w" to the origin of the end-effector.

Public Member Functions

Public Attributes

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ TSR() [1/4]

◆ TSR() [2/4]

◆ TSR() [3/4]

◆ TSR() [4/4]

◆ ~TSR()

Member Function Documentation

◆ createOutcome()

◆ createSampleGenerator()

◆ getConstraintDimension()

◆ getConstraintTypes()

◆ getJacobian()

◆ getSE3()

◆ getStateSpace()

◆ getTestableTolerance()

◆ getValue()

◆ getValueAndJacobian()

◆ isSatisfied()

◆ operator=() [1/2]

◆ operator=() [2/2]

◆ project()

◆ setRNG()

◆ setTestableTolerance()

◆ validate()

Member Data Documentation

◆ mBw

◆ mRng

◆ mStateSpace

◆ mT0_w

◆ mTestableTolerance

◆ mTw_e