DART  6.6.2
dart::dynamics::InverseKinematics::Analytical Class Referenceabstract

Analytical is a base class that should be inherited by methods that are made to solve the IK analytically instead of iteratively. More...

#include <InverseKinematics.hpp>

Inheritance diagram for dart::dynamics::InverseKinematics::Analytical:
dart::dynamics::InverseKinematics::GradientMethod dart::common::Subject dart::dynamics::IkFast dart::dynamics::SharedLibraryIkFast

Classes

struct  Properties
 
struct  Solution
 
struct  UniqueProperties
 

Public Types

enum  Validity_t { VALID = 0 , OUT_OF_REACH = 1 << 0 , LIMIT_VIOLATED = 1 << 1 }
 Bitwise enumerations that are used to describe some properties of each solution produced by the analytical IK. More...
 
enum  ExtraDofUtilization { UNUSED = 0 , PRE_ANALYTICAL , POST_ANALYTICAL , PRE_AND_POST_ANALYTICAL }
 If there are extra DOFs in the IK module which your Analytical solver implementation does not make use of, those DOFs can be used to supplement the analytical solver using Jacobian transpose iteration. More...
 
typedef std::function< bool(const Eigen::VectorXd &_better, const Eigen::VectorXd &_worse, const InverseKinematics *_ik)> QualityComparison
 

Public Member Functions

 Analytical (InverseKinematics *_ik, const std::string &_methodName, const Properties &_properties)
 Constructor. More...
 
virtual ~Analytical ()=default
 Virtual destructor. More...
 
const std::vector< Solution > & getSolutions ()
 Get the solutions for this IK module, along with a tag indicating whether each solution is valid. More...
 
const std::vector< Solution > & getSolutions (const Eigen::Isometry3d &_desiredTf)
 Get the solutions for this IK module, along with a tag indicating whether each solution is valid. More...
 
void computeGradient (const Eigen::Vector6d &_error, Eigen::VectorXd &_grad) override
 You should not need to override this function. More...
 
virtual const std::vector< Solution > & computeSolutions (const Eigen::Isometry3d &_desiredBodyTf)=0
 Use this function to fill the entries of the mSolutions variable. More...
 
virtual const std::vector< std::size_t > & getDofs () const =0
 Get a list of the DOFs that will be included in the entries of the solutions returned by getSolutions(). More...
 
void setPositions (const Eigen::VectorXd &_config)
 Set the configuration of the DOFs. More...
 
Eigen::VectorXd getPositions () const
 Get the configuration of the DOFs. More...
 
void setExtraDofUtilization (ExtraDofUtilization _utilization)
 Set how you want extra DOFs to be utilized by the IK module. More...
 
ExtraDofUtilization getExtraDofUtilization () const
 Get how extra DOFs are being utilized by the IK module. More...
 
void setExtraErrorLengthClamp (double _clamp)
 Set how much to clamp the error vector that gets applied to extra DOFs. More...
 
double getExtraErrorLengthClamp () const
 Get how much we will clamp the error vector that gets applied to extra DOFs. More...
 
void setQualityComparisonFunction (const QualityComparison &_func)
 Set the function that will be used to compare the qualities of two solutions. More...
 
void resetQualityComparisonFunction ()
 Reset the quality comparison function to the default method. More...
 
Properties getAnalyticalProperties () const
 Get the Properties for this Analytical class. More...
 
void constructDofMap ()
 Construct a mapping from the DOFs of getDofs() to their indices within the Node's list of dependent DOFs. More...
 
virtual std::unique_ptr< GradientMethodclone (InverseKinematics *_newIK) const =0
 Enable this GradientMethod to be cloned to a new IK module. More...
 
void evalGradient (const Eigen::VectorXd &_q, Eigen::Map< Eigen::VectorXd > _grad)
 This function is used to handle caching the gradient vector and interfacing with the solver. More...
 
const std::string & getMethodName () const
 Get the name of this GradientMethod. More...
 
void clampGradient (Eigen::VectorXd &_grad) const
 Clamp the gradient based on the clamp settings of this GradientMethod. More...
 
void setComponentWiseClamp (double _clamp=DefaultIKGradientComponentClamp)
 Set the component-wise clamp for this GradientMethod. More...
 
double getComponentWiseClamp () const
 Get the component-wise clamp for this GradientMethod. More...
 
void applyWeights (Eigen::VectorXd &_grad) const
 Apply weights to the gradient based on the weight settings of this GradientMethod. More...
 
void setComponentWeights (const Eigen::VectorXd &_weights)
 Set the weights that will be applied to each component of the gradient. More...
 
const Eigen::VectorXd & getComponentWeights () const
 Get the weights of this GradientMethod. More...
 
void convertJacobianMethodOutputToGradient (Eigen::VectorXd &grad, const std::vector< std::size_t > &dofs)
 Convert the gradient that gets generated by Jacobian methods into a gradient that can be used by a GradientDescentSolver. More...
 
Properties getGradientMethodProperties () const
 Get the Properties of this GradientMethod. More...
 
void clearCache ()
 Clear the cache to force the gradient to be recomputed. More...
 
InverseKinematicsgetIK ()
 Returns the IK module that this GradientMethod belongs to. More...
 
const InverseKinematicsgetIK () const
 Returns the IK module that this GradientMethod belongs to. More...
 

Protected Member Functions

virtual void addExtraDofGradient (Eigen::VectorXd &grad, const Eigen::Vector6d &error, ExtraDofUtilization utilization)
 This function will compute a gradient which utilizes the extra DOFs that go unused by the Analytical solution and then it will add the components of that gradient to the output parameter: grad. More...
 
void checkSolutionJointLimits ()
 Go through the mSolutions vector and tag entries with LIMIT_VIOLATED if any components of their configuration are outside of their position limits. More...
 
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

std::vector< SolutionmSolutions
 Vector of solutions. More...
 
UniqueProperties mAnalyticalP
 Properties for this Analytical IK solver. More...
 
common::sub_ptr< InverseKinematicsmIK
 The IK module that this GradientMethod belongs to. More...
 
std::string mMethodName
 The name of this method. More...
 
Eigen::VectorXd mLastPositions
 The last positions that was passed to this GradientMethod. More...
 
Eigen::VectorXd mLastGradient
 The last gradient that was computed by this GradientMethod. More...
 
Properties mGradientP
 Properties for this GradientMethod. More...
 
std::set< Observer * > mObservers
 List of current Observers. More...
 

Private Attributes

std::vector< int > mDofMap
 This maps the DOFs provided by getDofs() to their index in the Node's list of dependent DOFs. More...
 
std::vector< std::size_t > mExtraDofs
 List of extra DOFs in the module which are not covered by the Analytical IK implementation. More...
 
std::vector< SolutionmValidSolutionsCache
 A cache for the valid solutions. More...
 
std::vector< SolutionmOutOfReachCache
 A cache for the out of reach solutions. More...
 
std::vector< SolutionmLimitViolationCache
 A cache for solutions that violate joint limits. More...
 
Eigen::VectorXd mConfigCache
 A cache for storing the current configuration. More...
 
Eigen::VectorXd mRestoreConfigCache
 A cache for storing the current configuration so that it can be restored later. More...
 
Eigen::VectorXd mExtraDofGradCache
 A cache for storing the gradient for the extra DOFs. More...
 
Eigen::VectorXd mInitialPositionsCache
 Cache used by convertJacobianMethodOutputToGradient to avoid reallocating this vector on each iteration. More...
 

Detailed Description

Analytical is a base class that should be inherited by methods that are made to solve the IK analytically instead of iteratively.

This provides an extended API that is relevant to Analytical solvers but not iterative solvers.

Creating an Analytical solver will have the side effect of removing the error clamp and error weights from your ErrorMethod. If you still want your error computations to be clamped and weighted, you should set it again after creating the Analytical solver. Clamping and weighting the error vector often helps iterative methods to converge smoothly, but it is counter-productive for analytical methods which do not typically rely on convergence; analytical methods can usually solve the entire error vector directly.

Member Typedef Documentation

◆ QualityComparison

typedef std::function<bool( const Eigen::VectorXd& _better, const Eigen::VectorXd& _worse, const InverseKinematics* _ik)> dart::dynamics::InverseKinematics::Analytical::QualityComparison

Member Enumeration Documentation

◆ ExtraDofUtilization

If there are extra DOFs in the IK module which your Analytical solver implementation does not make use of, those DOFs can be used to supplement the analytical solver using Jacobian transpose iteration.

This enumeration is used to indicate whether you want those DOFs to be used before applying the analytical solution, after applying the analytical solution, or not be used at all.

Jacobian transpose is used for the extra DOFs because it is inexpensive and robust to degenerate Jacobians which are common in low dimensional joint spaces. The primary advantage of pseudoinverse methods over Jacobian transpose methods is their precision, but analytical methods are even more precise than pseudoinverse methods, so that precision is not needed in this case.

If you want the extra DOFs to use a different method than Jacobian transpose, you can create two seperate IK modules (one which is analytical and one with the iterative method of your choice) and combine them in a HierarchicalIK.

Enumerator
UNUSED 
PRE_ANALYTICAL 
POST_ANALYTICAL 
PRE_AND_POST_ANALYTICAL 

◆ Validity_t

Bitwise enumerations that are used to describe some properties of each solution produced by the analytical IK.

Enumerator
VALID 

The solution is completely valid and reaches the target.

OUT_OF_REACH 

The solution does not reach the target.

LIMIT_VIOLATED 

The solution has one or more joint positions that violate the joint limits.

Constructor & Destructor Documentation

◆ Analytical()

dart::dynamics::InverseKinematics::Analytical::Analytical ( InverseKinematics _ik,
const std::string &  _methodName,
const Properties _properties 
)

Constructor.

◆ ~Analytical()

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

Virtual destructor.

Member Function Documentation

◆ addExtraDofGradient()

void dart::dynamics::InverseKinematics::Analytical::addExtraDofGradient ( Eigen::VectorXd &  grad,
const Eigen::Vector6d error,
ExtraDofUtilization  utilization 
)
protectedvirtual

This function will compute a gradient which utilizes the extra DOFs that go unused by the Analytical solution and then it will add the components of that gradient to the output parameter: grad.

You can override this function to customize how the extra DOFs are used. The default behavior is to use a simple Jacobian Transpose method.

The utilization flag will be PRE_ANALYTICAL if the function is being called before the Analytical solution is computed; it will be POST_ANALYTICAL if the function is being called after the Analytical solution is computed.

◆ addObserver()

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

Add an Observer to the list of Observers.

◆ applyWeights()

void dart::dynamics::InverseKinematics::GradientMethod::applyWeights ( Eigen::VectorXd &  _grad) const
inherited

Apply weights to the gradient based on the weight settings of this GradientMethod.

◆ checkSolutionJointLimits()

void dart::dynamics::InverseKinematics::Analytical::checkSolutionJointLimits ( )
protected

Go through the mSolutions vector and tag entries with LIMIT_VIOLATED if any components of their configuration are outside of their position limits.

This will NOT clear the LIMIT_VIOLATED flag from entries of mSolutions which are already tagged with it, even if they do not violate any limits.

◆ clampGradient()

void dart::dynamics::InverseKinematics::GradientMethod::clampGradient ( Eigen::VectorXd &  _grad) const
inherited

Clamp the gradient based on the clamp settings of this GradientMethod.

◆ clearCache()

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

Clear the cache to force the gradient to be recomputed.

It should generally not be necessary to call this function.

◆ clone()

virtual std::unique_ptr<GradientMethod> dart::dynamics::InverseKinematics::GradientMethod::clone ( InverseKinematics _newIK) const
pure virtualinherited

◆ computeGradient()

void dart::dynamics::InverseKinematics::Analytical::computeGradient ( const Eigen::Vector6d _error,
Eigen::VectorXd &  _grad 
)
overridevirtual

You should not need to override this function.

Instead, you should override computeSolutions.

Implements dart::dynamics::InverseKinematics::GradientMethod.

◆ computeSolutions()

virtual const std::vector<Solution>& dart::dynamics::InverseKinematics::Analytical::computeSolutions ( const Eigen::Isometry3d &  _desiredBodyTf)
pure virtual

Use this function to fill the entries of the mSolutions variable.

Be sure to clear the mSolutions vector at the start, and to also return the mSolutions vector at the end. Note that you are not expected to evaluate any of the solutions for their quality. However, you should set the Solution::mValidity flag to OUT_OF_REACH for each solution that does not actually reach the desired transform, and you should call checkSolutionJointLimits() and the end of the function, which will set the LIMIT_VIOLATED flags of any configurations that are outside of the position limits.

Implemented in dart::dynamics::IkFast.

◆ constructDofMap()

void dart::dynamics::InverseKinematics::Analytical::constructDofMap ( )

Construct a mapping from the DOFs of getDofs() to their indices within the Node's list of dependent DOFs.

This will be called immediately after the Analytical is constructed; this one call is sufficient as long as the DOFs of Analytical::getDofs() is not changed. However, if your Analytical is able to change the DOFs that it operates on, then you will need to call this function each time the DOFs have changed.

◆ convertJacobianMethodOutputToGradient()

void dart::dynamics::InverseKinematics::GradientMethod::convertJacobianMethodOutputToGradient ( Eigen::VectorXd &  grad,
const std::vector< std::size_t > &  dofs 
)
inherited

Convert the gradient that gets generated by Jacobian methods into a gradient that can be used by a GradientDescentSolver.

Not all Joint types can be integrated using standard addition (e.g. FreeJoint and BallJoint), therefore Jacobian-based differential methods will tend to generate gradients that cannot be correctly used by a simple addition-based GradientDescentSolver. Running your gradient through this function before returning it will make the gradient suitable for a standard solver.

◆ evalGradient()

void dart::dynamics::InverseKinematics::GradientMethod::evalGradient ( const Eigen::VectorXd &  _q,
Eigen::Map< Eigen::VectorXd >  _grad 
)
inherited

This function is used to handle caching the gradient vector and interfacing with the solver.

◆ getAnalyticalProperties()

InverseKinematics::Analytical::Properties dart::dynamics::InverseKinematics::Analytical::getAnalyticalProperties ( ) const

Get the Properties for this Analytical class.

◆ getComponentWeights()

const Eigen::VectorXd & dart::dynamics::InverseKinematics::GradientMethod::getComponentWeights ( ) const
inherited

Get the weights of this GradientMethod.

◆ getComponentWiseClamp()

double dart::dynamics::InverseKinematics::GradientMethod::getComponentWiseClamp ( ) const
inherited

Get the component-wise clamp for this GradientMethod.

◆ getDofs()

virtual const std::vector<std::size_t>& dart::dynamics::InverseKinematics::Analytical::getDofs ( ) const
pure virtual

Get a list of the DOFs that will be included in the entries of the solutions returned by getSolutions().

Ideally, this should match up with the DOFs being used by the InverseKinematics module, but this might not always be possible, so this function ensures that solutions can be interpreted correctly.

Implemented in dart::dynamics::IkFast.

◆ getExtraDofUtilization()

IK::Analytical::ExtraDofUtilization dart::dynamics::IK::Analytical::getExtraDofUtilization ( ) const

Get how extra DOFs are being utilized by the IK module.

◆ getExtraErrorLengthClamp()

double dart::dynamics::IK::Analytical::getExtraErrorLengthClamp ( ) const

Get how much we will clamp the error vector that gets applied to extra DOFs.

◆ getGradientMethodProperties()

InverseKinematics::GradientMethod::Properties dart::dynamics::InverseKinematics::GradientMethod::getGradientMethodProperties ( ) const
inherited

Get the Properties of this GradientMethod.

◆ getIK() [1/2]

InverseKinematics * dart::dynamics::InverseKinematics::GradientMethod::getIK ( )
inherited

Returns the IK module that this GradientMethod belongs to.

◆ getIK() [2/2]

const InverseKinematics * dart::dynamics::InverseKinematics::GradientMethod::getIK ( ) const
inherited

Returns the IK module that this GradientMethod belongs to.

◆ getMethodName()

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

Get the name of this GradientMethod.

◆ getPositions()

Eigen::VectorXd dart::dynamics::InverseKinematics::Analytical::getPositions ( ) const

Get the configuration of the DOFs.

The components of this vector will correspond to the DOFs provided by getDofs().

◆ getSolutions() [1/2]

const std::vector< IK::Analytical::Solution > & dart::dynamics::IK::Analytical::getSolutions ( )

Get the solutions for this IK module, along with a tag indicating whether each solution is valid.

This function will assume that you want to use the desired transform given by the IK module's current ErrorMethod.

◆ getSolutions() [2/2]

const std::vector< IK::Analytical::Solution > & dart::dynamics::IK::Analytical::getSolutions ( const Eigen::Isometry3d &  _desiredTf)

Get the solutions for this IK module, along with a tag indicating whether each solution is valid.

This function will compute the configurations using the given desired transform instead of using the IK module's current ErrorMethod.

◆ removeObserver()

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

Remove an Observer from the list of Observers.

◆ resetQualityComparisonFunction()

void dart::dynamics::InverseKinematics::Analytical::resetQualityComparisonFunction ( )

Reset the quality comparison function to the default method.

◆ 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.

◆ setComponentWeights()

void dart::dynamics::InverseKinematics::GradientMethod::setComponentWeights ( const Eigen::VectorXd &  _weights)
inherited

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

If the number of components in _weights is smaller than the number of components in the gradient, then a weight of 1.0 will be applied to all components that are out of the range of _weights. Passing in an empty vector for _weights will effectively make all the gradient components unweighted.

◆ setComponentWiseClamp()

void dart::dynamics::InverseKinematics::GradientMethod::setComponentWiseClamp ( double  _clamp = DefaultIKGradientComponentClamp)
inherited

Set the component-wise clamp for this GradientMethod.

Each component of the gradient will be individually clamped to this size.

◆ setExtraDofUtilization()

void dart::dynamics::InverseKinematics::Analytical::setExtraDofUtilization ( ExtraDofUtilization  _utilization)

Set how you want extra DOFs to be utilized by the IK module.

◆ setExtraErrorLengthClamp()

void dart::dynamics::IK::Analytical::setExtraErrorLengthClamp ( double  _clamp)

Set how much to clamp the error vector that gets applied to extra DOFs.

◆ setPositions()

void dart::dynamics::InverseKinematics::Analytical::setPositions ( const Eigen::VectorXd &  _config)

Set the configuration of the DOFs.

The components of _config must correspond to the DOFs provided by getDofs().

◆ setQualityComparisonFunction()

void dart::dynamics::InverseKinematics::Analytical::setQualityComparisonFunction ( const QualityComparison _func)

Set the function that will be used to compare the qualities of two solutions.

This function should return true if the first argument is a better solution than the second argument.

By default, it will prefer the solution which has the smallest size for its largest change in joint angle. In other words, for each configuration that it is given, it will compare the largest change in joint angle for each configuration and pick the one that is smallest.

Note that outside of this comparison function, the Solutions will be split between which are valid, which are out-of-reach, and which are in violation of joint limits. Valid solutions will always be ranked above invalid solutions, and joint limit violations will always be ranked last.

Member Data Documentation

◆ mAnalyticalP

UniqueProperties dart::dynamics::InverseKinematics::Analytical::mAnalyticalP
protected

Properties for this Analytical IK solver.

◆ mConfigCache

Eigen::VectorXd dart::dynamics::InverseKinematics::Analytical::mConfigCache
private

A cache for storing the current configuration.

◆ mDofMap

std::vector<int> dart::dynamics::InverseKinematics::Analytical::mDofMap
private

This maps the DOFs provided by getDofs() to their index in the Node's list of dependent DOFs.

This map is constructed by constructDofMap().

◆ mExtraDofGradCache

Eigen::VectorXd dart::dynamics::InverseKinematics::Analytical::mExtraDofGradCache
private

A cache for storing the gradient for the extra DOFs.

◆ mExtraDofs

std::vector<std::size_t> dart::dynamics::InverseKinematics::Analytical::mExtraDofs
private

List of extra DOFs in the module which are not covered by the Analytical IK implementation.

The index of each DOF is its dependency index in the JacobianNode (i.e. the column it applies to in the Node's Jacobian).

◆ mGradientP

Properties dart::dynamics::InverseKinematics::GradientMethod::mGradientP
protectedinherited

◆ mIK

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

The IK module that this GradientMethod belongs to.

◆ mInitialPositionsCache

Eigen::VectorXd dart::dynamics::InverseKinematics::GradientMethod::mInitialPositionsCache
privateinherited

Cache used by convertJacobianMethodOutputToGradient to avoid reallocating this vector on each iteration.

◆ mLastGradient

Eigen::VectorXd dart::dynamics::InverseKinematics::GradientMethod::mLastGradient
protectedinherited

The last gradient that was computed by this GradientMethod.

◆ mLastPositions

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

The last positions that was passed to this GradientMethod.

◆ mLimitViolationCache

std::vector<Solution> dart::dynamics::InverseKinematics::Analytical::mLimitViolationCache
private

A cache for solutions that violate joint limits.

◆ mMethodName

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

The name of this method.

◆ mObservers

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

List of current Observers.

◆ mOutOfReachCache

std::vector<Solution> dart::dynamics::InverseKinematics::Analytical::mOutOfReachCache
private

A cache for the out of reach solutions.

◆ mRestoreConfigCache

Eigen::VectorXd dart::dynamics::InverseKinematics::Analytical::mRestoreConfigCache
private

A cache for storing the current configuration so that it can be restored later.

This is different from mConfigCache which will not be used to restore the configuration.

◆ mSolutions

std::vector<Solution> dart::dynamics::InverseKinematics::Analytical::mSolutions
protected

Vector of solutions.

◆ mValidSolutionsCache

std::vector<Solution> dart::dynamics::InverseKinematics::Analytical::mValidSolutionsCache
private

A cache for the valid solutions.

The valid and invalid solution caches are kept separate so that they can each be sorted by quality individually. Valid solutions will always be at the top of mFinalResults even if their quality is scored below the invalid solutions.