The HierarchicalIK class provides a convenient way of setting up a hierarchical inverse kinematics optimization problem which combines several InverseKinematics problems into one. More...

#include <HierarchicalIK.hpp>

Inheritance diagram for dart::dynamics::HierarchicalIK:

Classes
class	Constraint
	The HierarchicalIK::Constraint Function is simply used to merge the constraints of the InverseKinematics modules that belong to the hierarchy of this HierarchicalIK module. More...

class	Function
	This class should be inherited by optimizer::Function classes that have a dependency on the HierarchicalIK module that they belong to. More...

class	Objective
	The HierarchicalIK::Objective Function is simply used to merge the objective and null space objective functions that are being held by this HierarchicalIK module. More...

Public Member Functions
virtual	~HierarchicalIK ()=default
	Virtual destructor. 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...

virtual std::shared_ptr< HierarchicalIK >	clone (const SkeletonPtr &_newSkel) const =0
	Clone this HierarchicalIK module. 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::Function >	getObjective () 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::Function >	getNullSpaceObjective () 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::Problem >	getProblem () 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::Solver >	getSolver () const
	Get the Solver that is being used by this IK module. More...

virtual void	refreshIKHierarchy ()=0
	Refresh the IK hierarchy of this IK module. More...

const IKHierarchy &	getIKHierarchy () 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...

Protected Member Functions
	HierarchicalIK (const SkeletonPtr &_skeleton)
	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::Problem >	mProblem
	The Problem that this IK module is maintaining. More...

std::shared_ptr< optimizer::Solver >	mSolver
	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< HierarchicalIK >	mPtr
	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::Jacobian >	mSVDCache
	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 HierarchicalIK class provides a convenient way of setting up a hierarchical inverse kinematics optimization problem which combines several InverseKinematics problems into one.

InverseKinematics problems with a larger hierarchy level will be projected into null spaces of the problems that have a smaller hierarchy number.

Note that the HierarchicalIK will only account for the InverseKinematics::ErrorMethod and InverseKinematics::GradientMethod that the IK modules specify; it will ignore any other constraints or objectives put into the IK modules' Problems. Any additional constraints or objectives that you want the HierarchicalIK to solve should be put directly into the HierarchicalIK's Problem.

Constructor & Destructor Documentation

◆ ~HierarchicalIK()

virtual dart::dynamics::HierarchicalIK::~HierarchicalIK ( )

virtualdefault

Virtual destructor.

◆ HierarchicalIK()

dart::dynamics::HierarchicalIK::HierarchicalIK ( const SkeletonPtr & _skeleton )

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

Clear the caches of this IK module.

It should generally not be necessary to call this function.

◆ clone()

virtual std::shared_ptr<HierarchicalIK> dart::dynamics::HierarchicalIK::clone ( const SkeletonPtr & _newSkel ) const

pure virtual

Clone this HierarchicalIK module.

Implemented in dart::dynamics::WholeBodyIK, and dart::dynamics::CompositeIK.

◆ computeNullSpaces()

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

Compute the null spaces of each level of the hierarchy.

◆ copyOverSetup()

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

protected

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

◆ findSolution()

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

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

Parameters

[out] positions The 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 ( )

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

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

Get the IK hierarchy of this IK module.

◆ getNullSpaceObjective() [1/2]

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

Get the null space objective for this HierarchicalIK.

◆ getNullSpaceObjective() [2/2]

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

Get the null space objective for this HierarchicalIK.

◆ getObjective() [1/2]

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

Get the objective function for this HierarchicalIK.

◆ getObjective() [2/2]

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

Get the objective function for this HierarchicalIK.

◆ getPositions()

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

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

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

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

◆ getSkeleton() [1/2]

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

Get the Skeleton that this IK module is associated with.

◆ getSkeleton() [2/2]

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

Get the Skeleton that this IK module is associated with.

◆ getSolver() [1/2]

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

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

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

◆ hasNullSpaceObjective()

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

Returns true if this HierarchicalIK has a null space objective.

◆ initialize()

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

protected

Setup the module.

◆ refreshIKHierarchy()

virtual void dart::dynamics::HierarchicalIK::refreshIKHierarchy ( )

pure virtual

Refresh the IK hierarchy of this IK module.

Implemented in dart::dynamics::WholeBodyIK, and dart::dynamics::CompositeIK.

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

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 )

Set the null space objective for this HierarchicalIK.

◆ setObjective()

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

Set the objective function for this HierarchicalIK.

◆ setPositions()

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

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 )

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 )

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`
	)

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 )

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

Parameters

[in] allowIncompleteResult Allow 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`
	)

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

Parameters

[out]	positions	The 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]	allowIncompleteResult	Allow 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

protected

Cache for the IK hierarcy.

◆ mJacCache

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

mutableprotected

Cache for Jacobians.

◆ mLastPositions

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

mutableprotected

Cache for the last positions.

◆ mNullSpaceCache

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

mutableprotected

Cache for null space computations.

◆ mNullSpaceObjective

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

protected

The null space Objective of this IK module.

◆ mObjective

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

protected

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

mutableprotected

Cache for a partial null space computation.

◆ mProblem

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

protected

The Problem that this IK module is maintaining.

◆ mPtr

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

protected

Weak pointer to self.

◆ mSkeleton

WeakSkeletonPtr dart::dynamics::HierarchicalIK::mSkeleton

protected

Pointer to the Skeleton that this IK is tied to.

◆ mSolver

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

protected

The Solver that this IK module will use.

◆ mSVDCache

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

mutableprotected

Cache for the null space SVD.

Classes

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ~HierarchicalIK()

◆ HierarchicalIK()

Member Function Documentation

◆ addObserver()

◆ clearCaches()

◆ clone()

◆ computeNullSpaces()

◆ copyOverSetup()

◆ findSolution()

◆ getAffiliation() [1/2]

◆ getAffiliation() [2/2]

◆ getIKHierarchy()

◆ getNullSpaceObjective() [1/2]

◆ getNullSpaceObjective() [2/2]

◆ getObjective() [1/2]

◆ getObjective() [2/2]

◆ getPositions()

◆ getProblem() [1/2]

◆ getProblem() [2/2]

◆ getSkeleton() [1/2]

◆ getSkeleton() [2/2]

◆ getSolver() [1/2]

◆ getSolver() [2/2]

◆ hasNullSpaceObjective()

◆ initialize()

◆ refreshIKHierarchy()

◆ removeObserver()

◆ resetProblem()

◆ sendDestructionNotification()

◆ setNullSpaceObjective()

◆ setObjective()

◆ setPositions()

◆ setSolver()

◆ solve() [1/2]

◆ solve() [2/2]

◆ solveAndApply() [1/2]

◆ solveAndApply() [2/2]

Member Data Documentation

◆ mHierarchy

◆ mJacCache

◆ mLastPositions

◆ mNullSpaceCache

◆ mNullSpaceObjective

◆ mObjective

◆ mObservers

◆ mPartialNullspaceCache

◆ mProblem

◆ mPtr

◆ mSkeleton

◆ mSolver

◆ mSVDCache