DART  6.6.2
dart::dynamics::InverseKinematics::JacobianDLS Class Reference

JacobianDLS refers to the Damped Least Squares Jacobian Pseudoinverse (specifically, Moore-Penrose Pseudoinverse). More...

#include <InverseKinematics.hpp>

Inheritance diagram for dart::dynamics::InverseKinematics::JacobianDLS:
dart::dynamics::InverseKinematics::GradientMethod dart::common::Subject

Classes

struct  Properties
 
struct  UniqueProperties
 

Public Member Functions

 JacobianDLS (InverseKinematics *_ik, const Properties &properties=Properties())
 Constructor. More...
 
virtual ~JacobianDLS ()=default
 Virtual destructor. More...
 
std::unique_ptr< GradientMethodclone (InverseKinematics *_newIK) const override
 Enable this GradientMethod to be cloned to a new IK module. More...
 
void computeGradient (const Eigen::Vector6d &_error, Eigen::VectorXd &_grad) override
 Override this function with your implementation of the gradient computation. More...
 
void setDampingCoefficient (double _damping=DefaultIKDLSCoefficient)
 Set the damping coefficient. More...
 
double getDampingCoefficient () const
 Get the damping coefficient. More...
 
Properties getJacobianDLSProperties () const
 Get the Properties of this JacobianDLS. 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

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 mDLSProperties
 Properties of this Damped Least Squares method. 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

Eigen::VectorXd mInitialPositionsCache
 Cache used by convertJacobianMethodOutputToGradient to avoid reallocating this vector on each iteration. More...
 

Detailed Description

JacobianDLS refers to the Damped Least Squares Jacobian Pseudoinverse (specifically, Moore-Penrose Pseudoinverse).

This is a very precise method for computing the gradient and is especially suitable for performing IK on industrial manipulators that need to follow very exact workspace paths. However, it is vulnerable to be jittery around singularities (though the damping helps with this), and each cycle might take more time to compute than the JacobianTranspose method (although the JacobianDLS method will usually converge in fewer cycles than JacobianTranspose).

Constructor & Destructor Documentation

◆ JacobianDLS()

dart::dynamics::InverseKinematics::JacobianDLS::JacobianDLS ( InverseKinematics _ik,
const Properties properties = Properties() 
)
explicit

Constructor.

◆ ~JacobianDLS()

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

Virtual destructor.

Member Function Documentation

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

◆ 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()

std::unique_ptr< InverseKinematics::GradientMethod > dart::dynamics::InverseKinematics::JacobianDLS::clone ( InverseKinematics _newIK) const
overridevirtual

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

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

◆ computeGradient()

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

Override this function with your implementation of the gradient computation.

The direction that this gradient points in should make the error worse if applied to the joint positions, because the Problem is configured as a gradient descent error minimization Problem.

The error vector that is passed in will be determined by the IK module's ErrorMethod. The expectation is that the first three components of the vector 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::GradientMethod.

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

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

◆ getDampingCoefficient()

double dart::dynamics::InverseKinematics::JacobianDLS::getDampingCoefficient ( ) const

Get the damping coefficient.

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

◆ getJacobianDLSProperties()

InverseKinematics::JacobianDLS::Properties dart::dynamics::InverseKinematics::JacobianDLS::getJacobianDLSProperties ( ) const

Get the Properties of this JacobianDLS.

◆ getMethodName()

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

Get the name of this GradientMethod.

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

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

◆ setDampingCoefficient()

void dart::dynamics::InverseKinematics::JacobianDLS::setDampingCoefficient ( double  _damping = DefaultIKDLSCoefficient)

Set the damping coefficient.

A higher damping coefficient will smooth out behavior around singularities but will also result in less precision in general. The default value is appropriate for most use cases.

Member Data Documentation

◆ mDLSProperties

UniqueProperties dart::dynamics::InverseKinematics::JacobianDLS::mDLSProperties
protected

Properties of this Damped Least Squares method.

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

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