DART  6.7.3
dart::constraint::BalanceConstraint Class Referenceabstract

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

#include <BalanceConstraint.hpp>

Inheritance diagram for dart::constraint::BalanceConstraint:
dart::optimizer::Function dart::dynamics::HierarchicalIK::Function

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::HierarchicalIKmIK
 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::LinearJacobianmSVDCache
 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

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

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.