DART  6.10.1
dart::dynamics::WholeBodyIK Class Reference

The WholeBodyIK class provides an interface for simultaneously solving all the IK constraints of all BodyNodes and EndEffectors belonging to a single Skeleton. More...

#include <HierarchicalIK.hpp>

Inheritance diagram for dart::dynamics::WholeBodyIK:
dart::dynamics::HierarchicalIK dart::common::Subject

Public Member Functions

std::shared_ptr< HierarchicalIKclone (const SkeletonPtr &_newSkel) const override
 Clone this HierarchicalIK module. More...
 
virtual std::shared_ptr< WholeBodyIKcloneWholeBodyIK (const SkeletonPtr &_newSkel) const
 Same as clone(), but produces a more complete type. More...
 
void refreshIKHierarchy () override
 Refresh the IK hierarchy of this IK module. More...
 
bool solve (bool applySolution=true)
 Solve the IK Problem. More...
 
bool solve (Eigen::VectorXd &positions, bool applySolution=true)
 Same as solve(bool), but the positions vector will be filled with the solved positions. More...
 
bool findSolution (Eigen::VectorXd &positions)
 Finds a solution of the IK problem without applying the solution. More...
 
bool solveAndApply (bool allowIncompleteResult=true)
 Identical to findSolution(), but this function applies the solution when the solver successfully found a solution or allowIncompleteResult is set to true. More...
 
bool solveAndApply (Eigen::VectorXd &positions, bool allowIncompleteResult=true)
 Identical to solveAndApply(bool), but position will be filled with the solved positions. More...
 
void setObjective (const std::shared_ptr< optimizer::Function > &_objective)
 Set the objective function for this HierarchicalIK. More...
 
const std::shared_ptr< optimizer::Function > & getObjective ()
 Get the objective function for this HierarchicalIK. More...
 
std::shared_ptr< const optimizer::FunctiongetObjective () const
 Get the objective function for this HierarchicalIK. More...
 
void setNullSpaceObjective (const std::shared_ptr< optimizer::Function > &_nsObjective)
 Set the null space objective for this HierarchicalIK. More...
 
const std::shared_ptr< optimizer::Function > & getNullSpaceObjective ()
 Get the null space objective for this HierarchicalIK. More...
 
std::shared_ptr< const optimizer::FunctiongetNullSpaceObjective () const
 Get the null space objective for this HierarchicalIK. More...
 
bool hasNullSpaceObjective () const
 Returns true if this HierarchicalIK has a null space objective. More...
 
const std::shared_ptr< optimizer::Problem > & getProblem ()
 Get the Problem that is being maintained by this HierarchicalIK module. More...
 
std::shared_ptr< const optimizer::ProblemgetProblem () const
 Get the Problem that is being maintained by this HierarchicalIK module. More...
 
void resetProblem (bool _clearSeeds=false)
 Reset the Problem that is being maintained by this HierarchicalIK module. More...
 
void setSolver (const std::shared_ptr< optimizer::Solver > &_newSolver)
 Set the Solver that should be used by this IK module, and set it up with the Problem that is configured by this IK module. More...
 
const std::shared_ptr< optimizer::Solver > & getSolver ()
 Get the Solver that is being used by this IK module. More...
 
std::shared_ptr< const optimizer::SolvergetSolver () const
 Get the Solver that is being used by this IK module. More...
 
const IKHierarchygetIKHierarchy () const
 Get the IK hierarchy of this IK module. More...
 
const std::vector< Eigen::MatrixXd > & computeNullSpaces () const
 Compute the null spaces of each level of the hierarchy. More...
 
Eigen::VectorXd getPositions () const
 Get the current joint positions of the Skeleton associated with this IK module. More...
 
void setPositions (const Eigen::VectorXd &_q)
 Set the current joint positions of the Skeleton associated with this IK module. More...
 
SkeletonPtr getSkeleton ()
 Get the Skeleton that this IK module is associated with. More...
 
ConstSkeletonPtr getSkeleton () const
 Get the Skeleton that this IK module is associated with. More...
 
SkeletonPtr getAffiliation ()
 This is the same as getSkeleton(). More...
 
ConstSkeletonPtr getAffiliation () const
 This is the same as getSkeleton(). More...
 
void clearCaches ()
 Clear the caches of this IK module. More...
 

Static Public Member Functions

static std::shared_ptr< WholeBodyIKcreate (const SkeletonPtr &_skel)
 Create a WholeBodyIK. More...
 

Protected Member Functions

 WholeBodyIK (const SkeletonPtr &_skel)
 Constructor. More...
 
void initialize (const std::shared_ptr< HierarchicalIK > &my_ptr)
 Setup the module. More...
 
void copyOverSetup (const std::shared_ptr< HierarchicalIK > &_otherIK) const
 Copy the setup of this HierarchicalIK module into another HierarchicalIK module. 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

WeakSkeletonPtr mSkeleton
 Pointer to the Skeleton that this IK is tied to. More...
 
IKHierarchy mHierarchy
 Cache for the IK hierarcy. More...
 
std::shared_ptr< optimizer::ProblemmProblem
 The Problem that this IK module is maintaining. More...
 
std::shared_ptr< optimizer::SolvermSolver
 The Solver that this IK module will use. More...
 
optimizer::FunctionPtr mObjective
 The Objective of this IK module. More...
 
optimizer::FunctionPtr mNullSpaceObjective
 The null space Objective of this IK module. More...
 
std::weak_ptr< HierarchicalIKmPtr
 Weak pointer to self. More...
 
Eigen::VectorXd mLastPositions
 Cache for the last positions. More...
 
std::vector< Eigen::MatrixXd > mNullSpaceCache
 Cache for null space computations. More...
 
Eigen::MatrixXd mPartialNullspaceCache
 Cache for a partial null space computation. More...
 
Eigen::JacobiSVD< math::JacobianmSVDCache
 Cache for the null space SVD. More...
 
math::Jacobian mJacCache
 Cache for Jacobians. More...
 
std::set< Observer * > mObservers
 List of current Observers. More...
 

Detailed Description

The WholeBodyIK class provides an interface for simultaneously solving all the IK constraints of all BodyNodes and EndEffectors belonging to a single Skeleton.

Constructor & Destructor Documentation

◆ WholeBodyIK()

dart::dynamics::WholeBodyIK::WholeBodyIK ( const SkeletonPtr _skel)
protected

Constructor.

Member Function Documentation

◆ addObserver()

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

Add an Observer to the list of Observers.

◆ clearCaches()

void dart::dynamics::HierarchicalIK::clearCaches ( )
inherited

Clear the caches of this IK module.

It should generally not be necessary to call this function.

◆ clone()

std::shared_ptr< HierarchicalIK > dart::dynamics::WholeBodyIK::clone ( const SkeletonPtr _newSkel) const
overridevirtual

Clone this HierarchicalIK module.

Implements dart::dynamics::HierarchicalIK.

◆ cloneWholeBodyIK()

std::shared_ptr< WholeBodyIK > dart::dynamics::WholeBodyIK::cloneWholeBodyIK ( const SkeletonPtr _newSkel) const
virtual

Same as clone(), but produces a more complete type.

◆ computeNullSpaces()

const std::vector< Eigen::MatrixXd > & dart::dynamics::HierarchicalIK::computeNullSpaces ( ) const
inherited

Compute the null spaces of each level of the hierarchy.

◆ copyOverSetup()

void dart::dynamics::HierarchicalIK::copyOverSetup ( const std::shared_ptr< HierarchicalIK > &  _otherIK) const
protectedinherited

Copy the setup of this HierarchicalIK module into another HierarchicalIK module.

◆ create()

std::shared_ptr< WholeBodyIK > dart::dynamics::WholeBodyIK::create ( const SkeletonPtr _skel)
static

Create a WholeBodyIK.

◆ findSolution()

bool dart::dynamics::HierarchicalIK::findSolution ( Eigen::VectorXd &  positions)
inherited

Finds a solution of the IK problem without applying the solution.

Parameters
[out]positionsThe solution of the IK problem. If the solver failed to find a solution then it will still set the position with the best guess. For example, iterative solvers will fill position with the last result of the iterations.
Returns
True if a solution is successfully found.
See also
solveAndApply()

◆ getAffiliation() [1/2]

SkeletonPtr dart::dynamics::HierarchicalIK::getAffiliation ( )
inherited

This is the same as getSkeleton().

It is used by the HierarchicalIKPtr to provide a common interface for the various IK smart pointer types.

◆ getAffiliation() [2/2]

ConstSkeletonPtr dart::dynamics::HierarchicalIK::getAffiliation ( ) const
inherited

This is the same as getSkeleton().

It is used by the HierarchicalIKPtr to provide a common interface for the various IK smart pointer types.

◆ getIKHierarchy()

const IKHierarchy & dart::dynamics::HierarchicalIK::getIKHierarchy ( ) const
inherited

Get the IK hierarchy of this IK module.

◆ getNullSpaceObjective() [1/2]

const std::shared_ptr< optimizer::Function > & dart::dynamics::HierarchicalIK::getNullSpaceObjective ( )
inherited

Get the null space objective for this HierarchicalIK.

◆ getNullSpaceObjective() [2/2]

std::shared_ptr< const optimizer::Function > dart::dynamics::HierarchicalIK::getNullSpaceObjective ( ) const
inherited

Get the null space objective for this HierarchicalIK.

◆ getObjective() [1/2]

const std::shared_ptr< optimizer::Function > & dart::dynamics::HierarchicalIK::getObjective ( )
inherited

Get the objective function for this HierarchicalIK.

◆ getObjective() [2/2]

std::shared_ptr< const optimizer::Function > dart::dynamics::HierarchicalIK::getObjective ( ) const
inherited

Get the objective function for this HierarchicalIK.

◆ getPositions()

Eigen::VectorXd dart::dynamics::HierarchicalIK::getPositions ( ) const
inherited

Get the current joint positions of the Skeleton associated with this IK module.

◆ getProblem() [1/2]

const std::shared_ptr< optimizer::Problem > & dart::dynamics::HierarchicalIK::getProblem ( )
inherited

Get the Problem that is being maintained by this HierarchicalIK module.

◆ getProblem() [2/2]

std::shared_ptr< const optimizer::Problem > dart::dynamics::HierarchicalIK::getProblem ( ) const
inherited

Get the Problem that is being maintained by this HierarchicalIK module.

◆ getSkeleton() [1/2]

SkeletonPtr dart::dynamics::HierarchicalIK::getSkeleton ( )
inherited

Get the Skeleton that this IK module is associated with.

◆ getSkeleton() [2/2]

ConstSkeletonPtr dart::dynamics::HierarchicalIK::getSkeleton ( ) const
inherited

Get the Skeleton that this IK module is associated with.

◆ getSolver() [1/2]

const std::shared_ptr< optimizer::Solver > & dart::dynamics::HierarchicalIK::getSolver ( )
inherited

Get the Solver that is being used by this IK module.

◆ getSolver() [2/2]

std::shared_ptr< const optimizer::Solver > dart::dynamics::HierarchicalIK::getSolver ( ) const
inherited

Get the Solver that is being used by this IK module.

◆ hasNullSpaceObjective()

bool dart::dynamics::HierarchicalIK::hasNullSpaceObjective ( ) const
inherited

Returns true if this HierarchicalIK has a null space objective.

◆ initialize()

void dart::dynamics::HierarchicalIK::initialize ( const std::shared_ptr< HierarchicalIK > &  my_ptr)
protectedinherited

Setup the module.

◆ refreshIKHierarchy()

void dart::dynamics::WholeBodyIK::refreshIKHierarchy ( )
overridevirtual

Refresh the IK hierarchy of this IK module.

Implements dart::dynamics::HierarchicalIK.

◆ removeObserver()

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

Remove an Observer from the list of Observers.

◆ resetProblem()

void dart::dynamics::HierarchicalIK::resetProblem ( bool  _clearSeeds = false)
inherited

Reset the Problem that is being maintained by this HierarchicalIK module.

This will clear out all Functions from the Problem and then configure the Problem to use this IK module's Objective and Constraint functions.

Setting _clearSeeds to true will clear out any seeds that have been loaded into the Problem.

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

◆ setNullSpaceObjective()

void dart::dynamics::HierarchicalIK::setNullSpaceObjective ( const std::shared_ptr< optimizer::Function > &  _nsObjective)
inherited

Set the null space objective for this HierarchicalIK.

◆ setObjective()

void dart::dynamics::HierarchicalIK::setObjective ( const std::shared_ptr< optimizer::Function > &  _objective)
inherited

Set the objective function for this HierarchicalIK.

◆ setPositions()

void dart::dynamics::HierarchicalIK::setPositions ( const Eigen::VectorXd &  _q)
inherited

Set the current joint positions of the Skeleton associated with this IK module.

The vector must include all DOFs in the Skeleton.

◆ setSolver()

void dart::dynamics::HierarchicalIK::setSolver ( const std::shared_ptr< optimizer::Solver > &  _newSolver)
inherited

Set the Solver that should be used by this IK module, and set it up with the Problem that is configured by this IK module.

◆ solve() [1/2]

bool dart::dynamics::HierarchicalIK::solve ( bool  applySolution = true)
inherited

Solve the IK Problem.

By default, the Skeleton itself will retain the solved joint positions. If you pass in false for applySolution, then the joint positions will be return to their original positions after the problem is solved.

Deprecated:
Deprecated in DART 6.8. Please use solveAndApply() instead.

◆ solve() [2/2]

bool dart::dynamics::HierarchicalIK::solve ( Eigen::VectorXd &  positions,
bool  applySolution = true 
)
inherited

Same as solve(bool), but the positions vector will be filled with the solved positions.

Deprecated:
Deprecated in DART 6.8. Please use solveAndApply() or findSolution() instead.

◆ solveAndApply() [1/2]

bool dart::dynamics::HierarchicalIK::solveAndApply ( bool  allowIncompleteResult = true)
inherited

Identical to findSolution(), but this function applies the solution when the solver successfully found a solution or allowIncompleteResult is set to true.

Parameters
[in]allowIncompleteResultAllow to apply the solution even when the solver failed to find solution. This option would be useful when an iterative solver is used because they will often do a decent job of getting a result close to a solution even if it failed to find the solution.
Returns
True if a solution is successfully found

◆ solveAndApply() [2/2]

bool dart::dynamics::HierarchicalIK::solveAndApply ( Eigen::VectorXd &  positions,
bool  allowIncompleteResult = true 
)
inherited

Identical to solveAndApply(bool), but position will be filled with the solved positions.

Parameters
[out]positionsThe solution of the IK problem. If the solver failed to find a solution then it will still set the position with the best guess. For example, iterative solvers will fill positions with the last result of the iterations.
[in]allowIncompleteResultAllow to apply the solution even when the solver failed to find solution. This option would be useful when an iterative solver is used because they will often do a decent job of getting a result close to a solution even if it failed to find the solution.
Returns
True if a solution is successfully found

Member Data Documentation

◆ mHierarchy

IKHierarchy dart::dynamics::HierarchicalIK::mHierarchy
protectedinherited

Cache for the IK hierarcy.

◆ mJacCache

math::Jacobian dart::dynamics::HierarchicalIK::mJacCache
mutableprotectedinherited

Cache for Jacobians.

◆ mLastPositions

Eigen::VectorXd dart::dynamics::HierarchicalIK::mLastPositions
mutableprotectedinherited

Cache for the last positions.

◆ mNullSpaceCache

std::vector<Eigen::MatrixXd> dart::dynamics::HierarchicalIK::mNullSpaceCache
mutableprotectedinherited

Cache for null space computations.

◆ mNullSpaceObjective

optimizer::FunctionPtr dart::dynamics::HierarchicalIK::mNullSpaceObjective
protectedinherited

The null space Objective of this IK module.

◆ mObjective

optimizer::FunctionPtr dart::dynamics::HierarchicalIK::mObjective
protectedinherited

The Objective of this IK module.

◆ mObservers

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

List of current Observers.

◆ mPartialNullspaceCache

Eigen::MatrixXd dart::dynamics::HierarchicalIK::mPartialNullspaceCache
mutableprotectedinherited

Cache for a partial null space computation.

◆ mProblem

std::shared_ptr<optimizer::Problem> dart::dynamics::HierarchicalIK::mProblem
protectedinherited

The Problem that this IK module is maintaining.

◆ mPtr

std::weak_ptr<HierarchicalIK> dart::dynamics::HierarchicalIK::mPtr
protectedinherited

Weak pointer to self.

◆ mSkeleton

WeakSkeletonPtr dart::dynamics::HierarchicalIK::mSkeleton
protectedinherited

Pointer to the Skeleton that this IK is tied to.

◆ mSolver

std::shared_ptr<optimizer::Solver> dart::dynamics::HierarchicalIK::mSolver
protectedinherited

The Solver that this IK module will use.

◆ mSVDCache

Eigen::JacobiSVD<math::Jacobian> dart::dynamics::HierarchicalIK::mSVDCache
mutableprotectedinherited

Cache for the null space SVD.