BalanceConstraint is a kinematic constraint function designed to be passed into a HierarchicalIK module. More...

#include <BalanceConstraint.hpp>

Inheritance diagram for dart::constraint::BalanceConstraint:

Public Types
enum	ErrorMethod_t { FROM_CENTROID = 0 , FROM_EDGE , OPTIMIZE_BALANCE }
	The ErrorMethod_t determines whether the error should be computed based on the center of mass's distance from the centroid of the support polygon (FROM_CENTROID) or the COM's distance from the edge of the support polygon (FROM_EDGE). More...

enum	BalanceMethod_t { SHIFT_SUPPORT = 0 , SHIFT_COM }
	The BalanceMethod_t determines whether balancing should be achieved by shifting the locations of the supporting EndEffectors or by shifting the center of mass without moving the support locations. More...

Public Member Functions
	BalanceConstraint (const std::shared_ptr< dynamics::HierarchicalIK > &_ik, BalanceMethod_t _balanceMethod=SHIFT_SUPPORT, ErrorMethod_t _errorMethod=FROM_CENTROID)
	Constructor. More...

virtual	~BalanceConstraint ()=default
	Virtual destructor. More...

optimizer::FunctionPtr	clone (const std::shared_ptr< dynamics::HierarchicalIK > &_newIK) const override

double	eval (const Eigen::VectorXd &_x) override
	Evaluates and returns the objective function at the point x. More...

void	evalGradient (const Eigen::VectorXd &_x, Eigen::Map< Eigen::VectorXd > _grad) override
	Evaluates and returns the objective function at the point x. More...

void	setErrorMethod (ErrorMethod_t _method)
	Set the method that this constraint function will use to compute the error. More...

ErrorMethod_t	getErrorMethod () const
	Get the method that this constraint function will use to compute the error. More...

void	setBalanceMethod (BalanceMethod_t _method)
	Set the method that this constraint function will use to achieve balance. More...

BalanceMethod_t	getBalanceMethod () const
	Get the method that this constraint function will use to achieve balance. More...

void	setOptimizationTolerance (double _tol)
	Set the tolerance for how far the center of mass can be from the support polygon centroid before the balance is considered optimized. More...

double	getOptimizationTolerance () const
	Get the tolerance for how far the center of mass can be from the support polygon centroid before the balance is considered optimized. More...

void	setPseudoInverseDamping (double _damping)
	Set the damping factor that will be used when computing the pseudoinverse. More...

double	getPseudoInverseDamping () const
	Get the damping factor that will be used when computing the pseudoinverse. More...

const Eigen::Vector3d &	getLastError () const
	Get the last error vector that was computed by this BalanceConstraint. More...

void	clearCaches ()
	Clear the caches to force the error computation to update. More...

virtual void	setName (const std::string &newName)
	Sets the name of this Function. More...

const std::string &	getName () const
	Returns the name of this Function. More...

void	evalGradient (const Eigen::VectorXd &_x, Eigen::VectorXd &_grad)
	Evaluates and return the objective function at the point x. More...

virtual void	evalHessian (const Eigen::VectorXd &_x, Eigen::Map< Eigen::VectorXd, Eigen::RowMajor > _Hess)
	Evaluates and return the objective function at the point x. More...

virtual optimizer::FunctionPtr	clone (const std::shared_ptr< HierarchicalIK > &_newIK) const =0
	Enable this function to be cloned to a new IK module. More...

Protected Member Functions
void	convertJacobianMethodOutputToGradient (Eigen::Map< Eigen::VectorXd > &grad)
	Convert the gradient that gets generated via Jacobian methods into a gradient that can be used by a GradientDescentSolver. More...

Protected Attributes
std::weak_ptr< dynamics::HierarchicalIK >	mIK
	Pointer to the hierarchical IK that owns this Function. More...

ErrorMethod_t	mErrorMethod
	The method that will be used to compute error. More...

BalanceMethod_t	mBalanceMethod
	The method that will be used for balance. More...

double	mOptimizationTolerance
	The tolerance allowed before optimization is considered successful. More...

double	mDamping
	The damping factor for the pseudoinverse. More...

std::size_t	mClosestEndEffector [2]
	The indices of the supporting end effectors that are closest to the center of mass. More...

Eigen::Vector3d	mLastError
	The error vector points away from the direction that the center of mass should move in order to reduce the balance error. More...

Eigen::Vector3d	mLastCOM
	The last computed location of the center of mass. More...

std::size_t	mLastSupportVersion
	The last version of the support polygon that was computed. More...

math::LinearJacobian	mComJacCache
	Cache for the center of mass Jacobian so that the memory space does not need to be reallocated each loop. More...

math::LinearJacobian	mEEJacCache
	Cache for the end effector Jacobians so the space does not need to be reallocated each loop. More...

Eigen::JacobiSVD< math::LinearJacobian >	mSVDCache
	Cache for the SVD. More...

Eigen::MatrixXd	mNullSpaceCache
	Cache for the full null space. More...

Eigen::MatrixXd	mPartialNullSpaceCache
	Cache for an individual null space. More...

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

std::string	mName
	Name of this function. More...

Detailed Description

BalanceConstraint is a kinematic constraint function designed to be passed into a HierarchicalIK module.

Adding this constraint to the Problem of a HierarchicalIK will allow the IK solver to constrain the Skeleton so that it satisfies a support polygon style balancing constraint.

Member Enumeration Documentation

◆ BalanceMethod_t

enum dart::constraint::BalanceConstraint::BalanceMethod_t

The BalanceMethod_t determines whether balancing should be achieved by shifting the locations of the supporting EndEffectors or by shifting the center of mass without moving the support locations.

Note that if the BalanceMethod_t is SHIFT_SUPPORT, then its behavior will change significantly based on the ErrorMethod_t: if the ErrorMethod_t is FROM_CENTROID or OPTIMIZE BALANCE, then all support points will be shifted towards the center of mass simultaneously. However, if the ErrorMethod_t is FROM_EDGE, then only the EndEffector that is closest to the center of mass will be shifted.

Enumerator
SHIFT_SUPPORT
SHIFT_COM

◆ ErrorMethod_t

enum dart::constraint::BalanceConstraint::ErrorMethod_t

The ErrorMethod_t determines whether the error should be computed based on the center of mass's distance from the centroid of the support polygon (FROM_CENTROID) or the COM's distance from the edge of the support polygon (FROM_EDGE).

Note that once the center of mass is inside the support polygon, the error will immediately drop to zero regardless of which is chosen.

Alternatively, choosing OPTIMIZE_BALANCE will not drop the error to zero while inside the support polygon. Instead, it will try to drive the center of mass to the centroid of the support polygon. The error will only drop to zero once it is within a certain tolerance of the centroid. The tolerance can be set by using setOptimizationTolerance(). This method is ideal for use as an Objective function to improve the quality of a configuration rather than as a constraint function.

Enumerator
FROM_CENTROID
FROM_EDGE
OPTIMIZE_BALANCE

Constructor & Destructor Documentation

◆ BalanceConstraint()

dart::constraint::BalanceConstraint::BalanceConstraint	(	const std::shared_ptr< dynamics::HierarchicalIK > &	_ik,
		BalanceMethod_t	_balanceMethod = `SHIFT_SUPPORT`,
		ErrorMethod_t	_errorMethod = `FROM_CENTROID`
	)

Constructor.

◆ ~BalanceConstraint()

virtual dart::constraint::BalanceConstraint::~BalanceConstraint ( )

virtualdefault

Virtual destructor.

Member Function Documentation

◆ clearCaches()

void dart::constraint::BalanceConstraint::clearCaches ( )

Clear the caches to force the error computation to update.

It should not generally be necessary to call this function.

◆ clone() [1/2]

optimizer::FunctionPtr dart::constraint::BalanceConstraint::clone ( const std::shared_ptr< dynamics::HierarchicalIK > & _newIK ) const

override

◆ clone() [2/2]

virtual optimizer::FunctionPtr dart::dynamics::HierarchicalIK::Function::clone ( const std::shared_ptr< HierarchicalIK > & _newIK ) const

pure virtualinherited

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

Implemented in dart::dynamics::HierarchicalIK::Constraint, and dart::dynamics::HierarchicalIK::Objective.

◆ convertJacobianMethodOutputToGradient()

void dart::constraint::BalanceConstraint::convertJacobianMethodOutputToGradient ( Eigen::Map< Eigen::VectorXd > & grad )

protected

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

◆ eval()

double dart::constraint::BalanceConstraint::eval ( const Eigen::VectorXd & x )

overridevirtual

Evaluates and returns the objective function at the point x.

Implements dart::optimizer::Function.

◆ evalGradient() [1/2]

void dart::constraint::BalanceConstraint::evalGradient	(	const Eigen::VectorXd &	_x,
		Eigen::Map< Eigen::VectorXd >	_grad
	)

overridevirtual

Evaluates and returns the objective function at the point x.

Reimplemented from dart::optimizer::Function.

◆ evalGradient() [2/2]

void dart::optimizer::Function::evalGradient	(	const Eigen::VectorXd &	_x,
		Eigen::VectorXd &	_grad
	)

inherited

Evaluates and return the objective function at the point x.

If you have a raw array that the gradient will be passed in, then use evalGradient(const Eigen::VectorXd&, Eigen::Map<Eigen::VectorXd>) for better performance.

◆ evalHessian()

void dart::optimizer::Function::evalHessian	(	const Eigen::VectorXd &	_x,
		Eigen::Map< Eigen::VectorXd, Eigen::RowMajor >	_Hess
	)

virtualinherited

Evaluates and return the objective function at the point x.

Reimplemented in dart::optimizer::NullFunction, and dart::optimizer::ModularFunction.

◆ getBalanceMethod()

BalanceConstraint::BalanceMethod_t dart::constraint::BalanceConstraint::getBalanceMethod ( ) const

Get the method that this constraint function will use to achieve balance.

◆ getErrorMethod()

BalanceConstraint::ErrorMethod_t dart::constraint::BalanceConstraint::getErrorMethod ( ) const

Get the method that this constraint function will use to compute the error.

◆ getLastError()

const Eigen::Vector3d & dart::constraint::BalanceConstraint::getLastError ( ) const

Get the last error vector that was computed by this BalanceConstraint.

◆ getName()

const std::string & dart::optimizer::Function::getName ( ) const

inherited

Returns the name of this Function.

◆ getOptimizationTolerance()

double dart::constraint::BalanceConstraint::getOptimizationTolerance ( ) const

Get the tolerance for how far the center of mass can be from the support polygon centroid before the balance is considered optimized.

◆ getPseudoInverseDamping()

double dart::constraint::BalanceConstraint::getPseudoInverseDamping ( ) const

Get the damping factor that will be used when computing the pseudoinverse.

◆ setBalanceMethod()

void dart::constraint::BalanceConstraint::setBalanceMethod ( BalanceMethod_t _method )

Set the method that this constraint function will use to achieve balance.

See the BalanceMethod_t docs for more information.

◆ setErrorMethod()

void dart::constraint::BalanceConstraint::setErrorMethod ( ErrorMethod_t _method )

Set the method that this constraint function will use to compute the error.

See the ErrorMethod_t docs for more information.

◆ setName()

void dart::optimizer::Function::setName ( const std::string & newName )

virtualinherited

Sets the name of this Function.

◆ setOptimizationTolerance()

void dart::constraint::BalanceConstraint::setOptimizationTolerance ( double _tol )

Set the tolerance for how far the center of mass can be from the support polygon centroid before the balance is considered optimized.

◆ setPseudoInverseDamping()

void dart::constraint::BalanceConstraint::setPseudoInverseDamping ( double _damping )

Set the damping factor that will be used when computing the pseudoinverse.

Member Data Documentation

◆ mBalanceMethod

BalanceMethod_t dart::constraint::BalanceConstraint::mBalanceMethod

protected

The method that will be used for balance.

◆ mClosestEndEffector

std::size_t dart::constraint::BalanceConstraint::mClosestEndEffector[2]

protected

The indices of the supporting end effectors that are closest to the center of mass.

These are used when using FROM_EDGE

◆ mComJacCache

math::LinearJacobian dart::constraint::BalanceConstraint::mComJacCache

protected

Cache for the center of mass Jacobian so that the memory space does not need to be reallocated each loop.

◆ mDamping

double dart::constraint::BalanceConstraint::mDamping

protected

The damping factor for the pseudoinverse.

◆ mEEJacCache

math::LinearJacobian dart::constraint::BalanceConstraint::mEEJacCache

protected

Cache for the end effector Jacobians so the space does not need to be reallocated each loop.

◆ mErrorMethod

ErrorMethod_t dart::constraint::BalanceConstraint::mErrorMethod

protected

The method that will be used to compute error.

◆ mIK

std::weak_ptr<dynamics::HierarchicalIK> dart::constraint::BalanceConstraint::mIK

protected

Pointer to the hierarchical IK that owns this Function.

Note that this Function does not work correctly without a HierarchicalIK.

◆ mInitialPositionsCache

Eigen::VectorXd dart::constraint::BalanceConstraint::mInitialPositionsCache

protected

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

◆ mLastCOM

Eigen::Vector3d dart::constraint::BalanceConstraint::mLastCOM

protected

The last computed location of the center of mass.

◆ mLastError

Eigen::Vector3d dart::constraint::BalanceConstraint::mLastError

protected

The error vector points away from the direction that the center of mass should move in order to reduce the balance error.

◆ mLastSupportVersion

std::size_t dart::constraint::BalanceConstraint::mLastSupportVersion

protected

The last version of the support polygon that was computed.

◆ mName

std::string dart::optimizer::Function::mName

protectedinherited

Name of this function.

◆ mNullSpaceCache

Eigen::MatrixXd dart::constraint::BalanceConstraint::mNullSpaceCache

protected

Cache for the full null space.

◆ mOptimizationTolerance

double dart::constraint::BalanceConstraint::mOptimizationTolerance

protected

The tolerance allowed before optimization is considered successful.

◆ mPartialNullSpaceCache

Eigen::MatrixXd dart::constraint::BalanceConstraint::mPartialNullSpaceCache

protected

Cache for an individual null space.

◆ mSVDCache

Eigen::JacobiSVD<math::LinearJacobian> dart::constraint::BalanceConstraint::mSVDCache

protected

Cache for the SVD.

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Enumeration Documentation

◆ BalanceMethod_t

◆ ErrorMethod_t

Constructor & Destructor Documentation

◆ BalanceConstraint()

◆ ~BalanceConstraint()

Member Function Documentation

◆ clearCaches()

◆ clone() [1/2]

◆ clone() [2/2]

◆ convertJacobianMethodOutputToGradient()

◆ eval()

◆ evalGradient() [1/2]

◆ evalGradient() [2/2]

◆ evalHessian()

◆ getBalanceMethod()

◆ getErrorMethod()

◆ getLastError()

◆ getName()

◆ getOptimizationTolerance()

◆ getPseudoInverseDamping()

◆ setBalanceMethod()

◆ setErrorMethod()

◆ setName()

◆ setOptimizationTolerance()

◆ setPseudoInverseDamping()

Member Data Documentation

◆ mBalanceMethod

◆ mClosestEndEffector

◆ mComJacCache

◆ mDamping

◆ mEEJacCache

◆ mErrorMethod

◆ mIK

◆ mInitialPositionsCache

◆ mLastCOM

◆ mLastError

◆ mLastSupportVersion

◆ mName

◆ mNullSpaceCache

◆ mOptimizationTolerance

◆ mPartialNullSpaceCache

◆ mSVDCache