DART
6.10.1
|
BalanceConstraint is a kinematic constraint function designed to be passed into a HierarchicalIK module. More...
#include <BalanceConstraint.hpp>
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... | |
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.
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 |
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 |
dart::constraint::BalanceConstraint::BalanceConstraint | ( | const std::shared_ptr< dynamics::HierarchicalIK > & | _ik, |
BalanceMethod_t | _balanceMethod = SHIFT_SUPPORT , |
||
ErrorMethod_t | _errorMethod = FROM_CENTROID |
||
) |
Constructor.
|
virtualdefault |
Virtual destructor.
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.
|
override |
|
pure virtualinherited |
Enable this function to be cloned to a new IK module.
Implemented in dart::dynamics::HierarchicalIK::Constraint, and dart::dynamics::HierarchicalIK::Objective.
|
protected |
Convert the gradient that gets generated via Jacobian methods into a gradient that can be used by a GradientDescentSolver.
|
overridevirtual |
Evaluates and returns the objective function at the point x.
Implements dart::optimizer::Function.
|
overridevirtual |
Evaluates and returns the objective function at the point x.
Reimplemented from dart::optimizer::Function.
|
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.
|
virtualinherited |
Evaluates and return the objective function at the point x.
Reimplemented in dart::optimizer::NullFunction, and dart::optimizer::ModularFunction.
BalanceConstraint::BalanceMethod_t dart::constraint::BalanceConstraint::getBalanceMethod | ( | ) | const |
Get the method that this constraint function will use to achieve balance.
BalanceConstraint::ErrorMethod_t dart::constraint::BalanceConstraint::getErrorMethod | ( | ) | const |
Get the method that this constraint function will use to compute the error.
const Eigen::Vector3d & dart::constraint::BalanceConstraint::getLastError | ( | ) | const |
Get the last error vector that was computed by this BalanceConstraint.
|
inherited |
Returns the name of this Function.
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.
double dart::constraint::BalanceConstraint::getPseudoInverseDamping | ( | ) | const |
Get the damping factor that will be used when computing the pseudoinverse.
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.
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.
|
virtualinherited |
Sets the name of this Function.
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.
void dart::constraint::BalanceConstraint::setPseudoInverseDamping | ( | double | _damping | ) |
Set the damping factor that will be used when computing the pseudoinverse.
|
protected |
The method that will be used for balance.
|
protected |
The indices of the supporting end effectors that are closest to the center of mass.
These are used when using FROM_EDGE
|
protected |
Cache for the center of mass Jacobian so that the memory space does not need to be reallocated each loop.
|
protected |
The damping factor for the pseudoinverse.
|
protected |
Cache for the end effector Jacobians so the space does not need to be reallocated each loop.
|
protected |
The method that will be used to compute error.
|
protected |
Pointer to the hierarchical IK that owns this Function.
Note that this Function does not work correctly without a HierarchicalIK.
|
protected |
Cache used by convertJacobianMethodOutputToGradient to avoid reallocating this vector on each iteration.
|
protected |
The last computed location of the center of mass.
|
protected |
The error vector points away from the direction that the center of mass should move in order to reduce the balance error.
|
protected |
The last version of the support polygon that was computed.
|
protectedinherited |
Name of this function.
|
protected |
Cache for the full null space.
|
protected |
The tolerance allowed before optimization is considered successful.
|
protected |
Cache for an individual null space.
|
protected |
Cache for the SVD.