33 #ifndef DART_CONSTRAINT_BALANCECONSTRAINT_HPP_
34 #define DART_CONSTRAINT_BALANCECONSTRAINT_HPP_
39 namespace constraint {
88 const std::shared_ptr<dynamics::HierarchicalIK>& _ik,
97 const std::shared_ptr<dynamics::HierarchicalIK>& _newIK)
const override;
100 double eval(
const Eigen::VectorXd& _x)
override;
104 const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad)
override;
149 std::weak_ptr<dynamics::HierarchicalIK>
mIK;
BalanceConstraint is a kinematic constraint function designed to be passed into a HierarchicalIK modu...
Definition: BalanceConstraint.hpp:47
BalanceMethod_t mBalanceMethod
The method that will be used for balance.
Definition: BalanceConstraint.hpp:155
Eigen::Vector3d mLastError
The error vector points away from the direction that the center of mass should move in order to reduc...
Definition: BalanceConstraint.hpp:169
void setOptimizationTolerance(double _tol)
Set the tolerance for how far the center of mass can be from the support polygon centroid before the ...
Definition: BalanceConstraint.cpp:364
virtual ~BalanceConstraint()=default
Virtual destructor.
void evalGradient(const Eigen::VectorXd &_x, Eigen::Map< Eigen::VectorXd > _grad) override
Evaluates and returns the objective function at the point x.
Definition: BalanceConstraint.cpp:187
BalanceMethod_t getBalanceMethod() const
Get the method that this constraint function will use to achieve balance.
Definition: BalanceConstraint.cpp:358
BalanceConstraint(const std::shared_ptr< dynamics::HierarchicalIK > &_ik, BalanceMethod_t _balanceMethod=SHIFT_SUPPORT, ErrorMethod_t _errorMethod=FROM_CENTROID)
Constructor.
Definition: BalanceConstraint.cpp:42
std::size_t mClosestEndEffector[2]
The indices of the supporting end effectors that are closest to the center of mass.
Definition: BalanceConstraint.hpp:165
ErrorMethod_t getErrorMethod() const
Get the method that this constraint function will use to compute the error.
Definition: BalanceConstraint.cpp:342
Eigen::MatrixXd mPartialNullSpaceCache
Cache for an individual null space.
Definition: BalanceConstraint.hpp:192
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 G...
Definition: BalanceConstraint.cpp:409
double mOptimizationTolerance
The tolerance allowed before optimization is considered successful.
Definition: BalanceConstraint.hpp:158
std::weak_ptr< dynamics::HierarchicalIK > mIK
Pointer to the hierarchical IK that owns this Function.
Definition: BalanceConstraint.hpp:149
BalanceMethod_t
The BalanceMethod_t determines whether balancing should be achieved by shifting the locations of the ...
Definition: BalanceConstraint.hpp:81
@ SHIFT_COM
Definition: BalanceConstraint.hpp:83
@ SHIFT_SUPPORT
Definition: BalanceConstraint.hpp:82
Eigen::JacobiSVD< math::LinearJacobian > mSVDCache
Cache for the SVD.
Definition: BalanceConstraint.hpp:186
void setPseudoInverseDamping(double _damping)
Set the damping factor that will be used when computing the pseudoinverse.
Definition: BalanceConstraint.cpp:380
double mDamping
The damping factor for the pseudoinverse.
Definition: BalanceConstraint.hpp:161
ErrorMethod_t
The ErrorMethod_t determines whether the error should be computed based on the center of mass's dista...
Definition: BalanceConstraint.hpp:64
@ OPTIMIZE_BALANCE
Definition: BalanceConstraint.hpp:67
@ FROM_EDGE
Definition: BalanceConstraint.hpp:66
@ FROM_CENTROID
Definition: BalanceConstraint.hpp:65
optimizer::FunctionPtr clone(const std::shared_ptr< dynamics::HierarchicalIK > &_newIK) const override
Definition: BalanceConstraint.cpp:59
double eval(const Eigen::VectorXd &_x) override
Evaluates and returns the objective function at the point x.
Definition: BalanceConstraint.cpp:66
double getOptimizationTolerance() const
Get the tolerance for how far the center of mass can be from the support polygon centroid before the ...
Definition: BalanceConstraint.cpp:374
std::size_t mLastSupportVersion
The last version of the support polygon that was computed.
Definition: BalanceConstraint.hpp:175
void clearCaches()
Clear the caches to force the error computation to update.
Definition: BalanceConstraint.cpp:402
Eigen::Vector3d mLastCOM
The last computed location of the center of mass.
Definition: BalanceConstraint.hpp:172
const Eigen::Vector3d & getLastError() const
Get the last error vector that was computed by this BalanceConstraint.
Definition: BalanceConstraint.cpp:396
double getPseudoInverseDamping() const
Get the damping factor that will be used when computing the pseudoinverse.
Definition: BalanceConstraint.cpp:390
math::LinearJacobian mComJacCache
Cache for the center of mass Jacobian so that the memory space does not need to be reallocated each l...
Definition: BalanceConstraint.hpp:179
Eigen::MatrixXd mNullSpaceCache
Cache for the full null space.
Definition: BalanceConstraint.hpp:189
void setBalanceMethod(BalanceMethod_t _method)
Set the method that this constraint function will use to achieve balance.
Definition: BalanceConstraint.cpp:348
ErrorMethod_t mErrorMethod
The method that will be used to compute error.
Definition: BalanceConstraint.hpp:152
Eigen::VectorXd mInitialPositionsCache
Cache used by convertJacobianMethodOutputToGradient to avoid reallocating this vector on each iterati...
Definition: BalanceConstraint.hpp:196
void setErrorMethod(ErrorMethod_t _method)
Set the method that this constraint function will use to compute the error.
Definition: BalanceConstraint.cpp:332
math::LinearJacobian mEEJacCache
Cache for the end effector Jacobians so the space does not need to be reallocated each loop.
Definition: BalanceConstraint.hpp:183
This class should be inherited by optimizer::Function classes that have a dependency on the Hierarchi...
Definition: HierarchicalIK.hpp:136
Definition: Function.hpp:46
Eigen::Matrix< double, 3, Eigen::Dynamic > LinearJacobian
Definition: MathTypes.hpp:112
std::shared_ptr< Function > FunctionPtr
Definition: Function.hpp:84
Definition: BulletCollisionDetector.cpp:65