DART  6.10.1
dart::dynamics::InverseKinematics::TaskSpaceRegion Class Reference

The TaskSpaceRegion is a nicely generalized method for computing the error of an IK Problem. More...

#include <InverseKinematics.hpp>

Inheritance diagram for dart::dynamics::InverseKinematics::TaskSpaceRegion:
dart::dynamics::InverseKinematics::ErrorMethod dart::common::Subject

Classes

struct  Properties
 
struct  UniqueProperties
 

Public Types

typedef std::pair< Eigen::Vector6d, Eigen::Vector6dBounds
 

Public Member Functions

 TaskSpaceRegion (InverseKinematics *_ik, const Properties &_properties=Properties())
 Default Constructor. More...
 
virtual ~TaskSpaceRegion ()=default
 Virtual destructor. More...
 
std::unique_ptr< ErrorMethodclone (InverseKinematics *_newIK) const override
 Enable this ErrorMethod to be cloned to a new IK module. More...
 
Eigen::Isometry3d computeDesiredTransform (const Eigen::Isometry3d &_currentTf, const Eigen::Vector6d &_error) override
 Override this function with your implementation of computing the desired given the current transform and error vector. More...
 
Eigen::Vector6d computeError () override
 Override this function with your implementation of the error vector computation. More...
 
void setComputeFromCenter (bool computeFromCenter)
 Set whether this TaskSpaceRegion should compute its error vector from the center of the region. More...
 
bool isComputingFromCenter () const
 Get whether this TaskSpaceRegion is set to compute its error vector from the center of the region. More...
 
void setReferenceFrame (SimpleFramePtr referenceFrame)
 Set the reference frame that the task space region is expressed. More...
 
ConstSimpleFramePtr getReferenceFrame () const
 Get the reference frame that the task space region is expressed. More...
 
Properties getTaskSpaceRegionProperties () const
 Get the Properties of this TaskSpaceRegion. More...
 
const Eigen::Vector6devalError (const Eigen::VectorXd &_q)
 This function is used to handle caching the error vector. More...
 
const std::string & getMethodName () const
 Get the name of this ErrorMethod. More...
 
void setBounds (const Eigen::Vector6d &_lower=Eigen::Vector6d::Constant(-DefaultIKTolerance), const Eigen::Vector6d &_upper=Eigen::Vector6d::Constant(DefaultIKTolerance))
 Set all the error bounds. More...
 
void setBounds (const std::pair< Eigen::Vector6d, Eigen::Vector6d > &_bounds)
 Set all the error bounds. More...
 
const std::pair< Eigen::Vector6d, Eigen::Vector6d > & getBounds () const
 Get all the error bounds. More...
 
void setAngularBounds (const Eigen::Vector3d &_lower=Eigen::Vector3d::Constant(-DefaultIKTolerance), const Eigen::Vector3d &_upper=Eigen::Vector3d::Constant(DefaultIKTolerance))
 Set the error bounds for orientation. More...
 
void setAngularBounds (const std::pair< Eigen::Vector3d, Eigen::Vector3d > &_bounds)
 Set the error bounds for orientation. More...
 
std::pair< Eigen::Vector3d, Eigen::Vector3d > getAngularBounds () const
 Get the error bounds for orientation. More...
 
void setLinearBounds (const Eigen::Vector3d &_lower=Eigen::Vector3d::Constant(-DefaultIKTolerance), const Eigen::Vector3d &_upper=Eigen::Vector3d::Constant(DefaultIKTolerance))
 Set the error bounds for translation. More...
 
void setLinearBounds (const std::pair< Eigen::Vector3d, Eigen::Vector3d > &_bounds)
 Set the error bounds for translation. More...
 
std::pair< Eigen::Vector3d, Eigen::Vector3d > getLinearBounds () const
 Get the error bounds for translation. More...
 
void setErrorLengthClamp (double _clampSize=DefaultIKErrorClamp)
 Set the clamp that will be applied to the length of the error vector each iteration. More...
 
double getErrorLengthClamp () const
 Set the clamp that will be applied to the length of the error vector each iteration. More...
 
void setErrorWeights (const Eigen::Vector6d &_weights)
 Set the weights that will be applied to each component of the error vector. More...
 
const Eigen::Vector6dgetErrorWeights () const
 Get the weights that will be applied to each component of the error vector. More...
 
void setAngularErrorWeights (const Eigen::Vector3d &_weights=Eigen::Vector3d::Constant(DefaultIKAngularWeight))
 Set the weights that will be applied to each angular component of the error vector. More...
 
Eigen::Vector3d getAngularErrorWeights () const
 Get the weights that will be applied to each angular component of the error vector. More...
 
void setLinearErrorWeights (const Eigen::Vector3d &_weights=Eigen::Vector3d::Constant(DefaultIKLinearWeight))
 Set the weights that will be applied to each linear component of the error vector. More...
 
Eigen::Vector3d getLinearErrorWeights () const
 Get the weights that will be applied to each linear component of the error vector. More...
 
Properties getErrorMethodProperties () const
 Get the Properties of this ErrorMethod. More...
 
void clearCache ()
 Clear the cache to force the error to be recomputed. More...
 

Protected Member Functions

void sendDestructionNotification () const
 Send a destruction notification to all Observers. More...
 
void addObserver (Observer *_observer) const
 Add an Observer to the list of Observers. More...
 
void removeObserver (Observer *_observer) const
 Remove an Observer from the list of Observers. More...
 

Protected Attributes

UniqueProperties mTaskSpaceP
 Properties of this TaskSpaceRegion. More...
 
common::sub_ptr< InverseKinematicsmIK
 Pointer to the IK module of this ErrorMethod. More...
 
std::string mMethodName
 Name of this error method. More...
 
Eigen::VectorXd mLastPositions
 The last joint positions passed into this ErrorMethod. More...
 
Eigen::Vector6d mLastError
 The last error vector computed by this ErrorMethod. More...
 
Properties mErrorP
 The properties of this ErrorMethod. More...
 
std::set< Observer * > mObservers
 List of current Observers. More...
 

Detailed Description

The TaskSpaceRegion is a nicely generalized method for computing the error of an IK Problem.

Member Typedef Documentation

◆ Bounds

Constructor & Destructor Documentation

◆ TaskSpaceRegion()

dart::dynamics::InverseKinematics::TaskSpaceRegion::TaskSpaceRegion ( InverseKinematics _ik,
const Properties _properties = Properties() 
)
explicit

Default Constructor.

◆ ~TaskSpaceRegion()

virtual dart::dynamics::InverseKinematics::TaskSpaceRegion::~TaskSpaceRegion ( )
virtualdefault

Virtual destructor.

Member Function Documentation

◆ addObserver()

void dart::common::Subject::addObserver ( Observer _observer) const
protectedinherited

Add an Observer to the list of Observers.

◆ clearCache()

void dart::dynamics::InverseKinematics::ErrorMethod::clearCache ( )
inherited

Clear the cache to force the error to be recomputed.

It should generally not be necessary to call this function.

◆ clone()

std::unique_ptr< InverseKinematics::ErrorMethod > dart::dynamics::InverseKinematics::TaskSpaceRegion::clone ( InverseKinematics _newIK) const
overridevirtual

Enable this ErrorMethod to be cloned to a new IK module.

Implements dart::dynamics::InverseKinematics::ErrorMethod.

◆ computeDesiredTransform()

Eigen::Isometry3d dart::dynamics::InverseKinematics::TaskSpaceRegion::computeDesiredTransform ( const Eigen::Isometry3d &  _currentTf,
const Eigen::Vector6d _error 
)
overridevirtual

Override this function with your implementation of computing the desired given the current transform and error vector.

If you want the desired transform to always be equal to the Target's transform, you can simply call ErrorMethod::computeDesiredTransform to implement this function.

Implements dart::dynamics::InverseKinematics::ErrorMethod.

◆ computeError()

Eigen::Vector6d dart::dynamics::InverseKinematics::TaskSpaceRegion::computeError ( )
overridevirtual

Override this function with your implementation of the error vector computation.

The expectation is that the first three components of the vector will correspond to orientation error (in an angle-axis format) while the last three components correspond to translational error.

When implementing this function, you should assume that the Skeleton's current joint positions corresponds to the positions that you must use to compute the error. This function will only get called when an update is needed.

Implements dart::dynamics::InverseKinematics::ErrorMethod.

◆ evalError()

const Eigen::Vector6d & dart::dynamics::InverseKinematics::ErrorMethod::evalError ( const Eigen::VectorXd &  _q)
inherited

This function is used to handle caching the error vector.

◆ getAngularBounds()

std::pair< Eigen::Vector3d, Eigen::Vector3d > dart::dynamics::InverseKinematics::ErrorMethod::getAngularBounds ( ) const
inherited

Get the error bounds for orientation.

◆ getAngularErrorWeights()

Eigen::Vector3d dart::dynamics::InverseKinematics::ErrorMethod::getAngularErrorWeights ( ) const
inherited

Get the weights that will be applied to each angular component of the error vector.

◆ getBounds()

const std::pair< Eigen::Vector6d, Eigen::Vector6d > & dart::dynamics::InverseKinematics::ErrorMethod::getBounds ( ) const
inherited

Get all the error bounds.

◆ getErrorLengthClamp()

double dart::dynamics::InverseKinematics::ErrorMethod::getErrorLengthClamp ( ) const
inherited

Set the clamp that will be applied to the length of the error vector each iteration.

◆ getErrorMethodProperties()

InverseKinematics::ErrorMethod::Properties dart::dynamics::InverseKinematics::ErrorMethod::getErrorMethodProperties ( ) const
inherited

Get the Properties of this ErrorMethod.

◆ getErrorWeights()

const Eigen::Vector6d & dart::dynamics::InverseKinematics::ErrorMethod::getErrorWeights ( ) const
inherited

Get the weights that will be applied to each component of the error vector.

◆ getLinearBounds()

std::pair< Eigen::Vector3d, Eigen::Vector3d > dart::dynamics::InverseKinematics::ErrorMethod::getLinearBounds ( ) const
inherited

Get the error bounds for translation.

◆ getLinearErrorWeights()

Eigen::Vector3d dart::dynamics::InverseKinematics::ErrorMethod::getLinearErrorWeights ( ) const
inherited

Get the weights that will be applied to each linear component of the error vector.

◆ getMethodName()

const std::string & dart::dynamics::InverseKinematics::ErrorMethod::getMethodName ( ) const
inherited

Get the name of this ErrorMethod.

◆ getReferenceFrame()

ConstSimpleFramePtr dart::dynamics::InverseKinematics::TaskSpaceRegion::getReferenceFrame ( ) const

Get the reference frame that the task space region is expressed.

◆ getTaskSpaceRegionProperties()

InverseKinematics::TaskSpaceRegion::Properties dart::dynamics::InverseKinematics::TaskSpaceRegion::getTaskSpaceRegionProperties ( ) const

Get the Properties of this TaskSpaceRegion.

◆ isComputingFromCenter()

bool dart::dynamics::InverseKinematics::TaskSpaceRegion::isComputingFromCenter ( ) const

Get whether this TaskSpaceRegion is set to compute its error vector from the center of the region.

◆ removeObserver()

void dart::common::Subject::removeObserver ( Observer _observer) const
protectedinherited

Remove an Observer from the list of Observers.

◆ sendDestructionNotification()

void dart::common::Subject::sendDestructionNotification ( ) const
protectedinherited

Send a destruction notification to all Observers.

This will cause all Observers to behave as if this Subject has been permanently deleted, so it should only be called when that behavior is desired.

◆ setAngularBounds() [1/2]

void dart::dynamics::InverseKinematics::ErrorMethod::setAngularBounds ( const Eigen::Vector3d &  _lower = Eigen::Vector3d::Constant(-DefaultIKTolerance),
const Eigen::Vector3d &  _upper = Eigen::Vector3d::Constant(DefaultIKTolerance) 
)
inherited

Set the error bounds for orientation.

◆ setAngularBounds() [2/2]

void dart::dynamics::InverseKinematics::ErrorMethod::setAngularBounds ( const std::pair< Eigen::Vector3d, Eigen::Vector3d > &  _bounds)
inherited

Set the error bounds for orientation.

◆ setAngularErrorWeights()

void dart::dynamics::InverseKinematics::ErrorMethod::setAngularErrorWeights ( const Eigen::Vector3d &  _weights = Eigen::Vector3d::Constant(DefaultIKAngularWeight))
inherited

Set the weights that will be applied to each angular component of the error vector.

◆ setBounds() [1/2]

void dart::dynamics::InverseKinematics::ErrorMethod::setBounds ( const Eigen::Vector6d _lower = Eigen::Vector6d::Constant(-DefaultIKTolerance),
const Eigen::Vector6d _upper = Eigen::Vector6d::Constant(DefaultIKTolerance) 
)
inherited

Set all the error bounds.

◆ setBounds() [2/2]

void dart::dynamics::InverseKinematics::ErrorMethod::setBounds ( const std::pair< Eigen::Vector6d, Eigen::Vector6d > &  _bounds)
inherited

Set all the error bounds.

◆ setComputeFromCenter()

void dart::dynamics::InverseKinematics::TaskSpaceRegion::setComputeFromCenter ( bool  computeFromCenter)

Set whether this TaskSpaceRegion should compute its error vector from the center of the region.

◆ setErrorLengthClamp()

void dart::dynamics::InverseKinematics::ErrorMethod::setErrorLengthClamp ( double  _clampSize = DefaultIKErrorClamp)
inherited

Set the clamp that will be applied to the length of the error vector each iteration.

◆ setErrorWeights()

void dart::dynamics::InverseKinematics::ErrorMethod::setErrorWeights ( const Eigen::Vector6d _weights)
inherited

Set the weights that will be applied to each component of the error vector.

◆ setLinearBounds() [1/2]

void dart::dynamics::InverseKinematics::ErrorMethod::setLinearBounds ( const Eigen::Vector3d &  _lower = Eigen::Vector3d::Constant(-DefaultIKTolerance),
const Eigen::Vector3d &  _upper = Eigen::Vector3d::Constant(DefaultIKTolerance) 
)
inherited

Set the error bounds for translation.

◆ setLinearBounds() [2/2]

void dart::dynamics::InverseKinematics::ErrorMethod::setLinearBounds ( const std::pair< Eigen::Vector3d, Eigen::Vector3d > &  _bounds)
inherited

Set the error bounds for translation.

◆ setLinearErrorWeights()

void dart::dynamics::InverseKinematics::ErrorMethod::setLinearErrorWeights ( const Eigen::Vector3d &  _weights = Eigen::Vector3d::Constant(DefaultIKLinearWeight))
inherited

Set the weights that will be applied to each linear component of the error vector.

◆ setReferenceFrame()

void dart::dynamics::InverseKinematics::TaskSpaceRegion::setReferenceFrame ( SimpleFramePtr  referenceFrame)

Set the reference frame that the task space region is expressed.

Pass nullptr to use the parent frame of the target frame instead.

Member Data Documentation

◆ mErrorP

Properties dart::dynamics::InverseKinematics::ErrorMethod::mErrorP
protectedinherited

The properties of this ErrorMethod.

◆ mIK

common::sub_ptr<InverseKinematics> dart::dynamics::InverseKinematics::ErrorMethod::mIK
protectedinherited

Pointer to the IK module of this ErrorMethod.

◆ mLastError

Eigen::Vector6d dart::dynamics::InverseKinematics::ErrorMethod::mLastError
protectedinherited

The last error vector computed by this ErrorMethod.

◆ mLastPositions

Eigen::VectorXd dart::dynamics::InverseKinematics::ErrorMethod::mLastPositions
protectedinherited

The last joint positions passed into this ErrorMethod.

◆ mMethodName

std::string dart::dynamics::InverseKinematics::ErrorMethod::mMethodName
protectedinherited

Name of this error method.

◆ mObservers

std::set<Observer*> dart::common::Subject::mObservers
mutableprotectedinherited

List of current Observers.

◆ mTaskSpaceP

UniqueProperties dart::dynamics::InverseKinematics::TaskSpaceRegion::mTaskSpaceP
protected