DART  6.7.3
BalanceConstraint.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2019, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_CONSTRAINT_BALANCECONSTRAINT_HPP_
34 #define DART_CONSTRAINT_BALANCECONSTRAINT_HPP_
35 
37 
38 namespace dart {
39 namespace constraint {
40 
47 {
48 public:
49 
65  {
69  };
70 
82  {
84  SHIFT_COM
85  };
86 
88  BalanceConstraint(const std::shared_ptr<dynamics::HierarchicalIK>& _ik,
89  BalanceMethod_t _balanceMethod = SHIFT_SUPPORT,
90  ErrorMethod_t _errorMethod = FROM_CENTROID);
91 
93  virtual ~BalanceConstraint() = default;
94 
95  // Documentation inherited
97  const std::shared_ptr<dynamics::HierarchicalIK>& _newIK) const override;
98 
99  // Documentation inherited
100  double eval(const Eigen::VectorXd& _x) override;
101 
102  // Documentation inherited
103  void evalGradient(const Eigen::VectorXd& _x,
104  Eigen::Map<Eigen::VectorXd> _grad) override;
105 
108  void setErrorMethod(ErrorMethod_t _method);
109 
113 
116  void setBalanceMethod(BalanceMethod_t _method);
117 
120 
123  void setOptimizationTolerance(double _tol);
124 
127  double getOptimizationTolerance() const;
128 
130  void setPseudoInverseDamping(double _damping);
131 
133  double getPseudoInverseDamping() const;
134 
136  const Eigen::Vector3d& getLastError() const;
137 
140  void clearCaches();
141 
142 protected:
143 
147  Eigen::Map<Eigen::VectorXd>& grad);
148 
151  std::weak_ptr<dynamics::HierarchicalIK> mIK;
152 
155 
158 
161 
163  double mDamping;
164 
167  std::size_t mClosestEndEffector[2];
168 
171  Eigen::Vector3d mLastError;
172 
174  Eigen::Vector3d mLastCOM;
175 
177  std::size_t mLastSupportVersion;
178 
182 
186 
188  Eigen::JacobiSVD<math::LinearJacobian> mSVDCache;
189 
191  Eigen::MatrixXd mNullSpaceCache;
192 
194  Eigen::MatrixXd mPartialNullSpaceCache;
195 
198  Eigen::VectorXd mInitialPositionsCache;
199 };
200 
201 } // namespace constraint
202 } // namespace dart
203 
204 #endif // DART_CONSTRAINT_BALANCECONSTRAINT_HPP_
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:157
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:171
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:357
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:180
BalanceMethod_t getBalanceMethod() const
Get the method that this constraint function will use to achieve balance.
Definition: BalanceConstraint.cpp:351
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:167
ErrorMethod_t getErrorMethod() const
Get the method that this constraint function will use to compute the error.
Definition: BalanceConstraint.cpp:335
Eigen::MatrixXd mPartialNullSpaceCache
Cache for an individual null space.
Definition: BalanceConstraint.hpp:194
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:402
double mOptimizationTolerance
The tolerance allowed before optimization is considered successful.
Definition: BalanceConstraint.hpp:160
std::weak_ptr< dynamics::HierarchicalIK > mIK
Pointer to the hierarchical IK that owns this Function.
Definition: BalanceConstraint.hpp:151
BalanceMethod_t
The BalanceMethod_t determines whether balancing should be achieved by shifting the locations of the ...
Definition: BalanceConstraint.hpp:82
@ SHIFT_COM
Definition: BalanceConstraint.hpp:84
@ SHIFT_SUPPORT
Definition: BalanceConstraint.hpp:83
Eigen::JacobiSVD< math::LinearJacobian > mSVDCache
Cache for the SVD.
Definition: BalanceConstraint.hpp:188
void setPseudoInverseDamping(double _damping)
Set the damping factor that will be used when computing the pseudoinverse.
Definition: BalanceConstraint.cpp:373
double mDamping
The damping factor for the pseudoinverse.
Definition: BalanceConstraint.hpp:163
ErrorMethod_t
The ErrorMethod_t determines whether the error should be computed based on the center of mass's dista...
Definition: BalanceConstraint.hpp:65
@ OPTIMIZE_BALANCE
Definition: BalanceConstraint.hpp:68
@ FROM_EDGE
Definition: BalanceConstraint.hpp:67
@ FROM_CENTROID
Definition: BalanceConstraint.hpp:66
optimizer::FunctionPtr clone(const std::shared_ptr< dynamics::HierarchicalIK > &_newIK) const override
Definition: BalanceConstraint.cpp:58
double eval(const Eigen::VectorXd &_x) override
Evaluates and returns the objective function at the point x.
Definition: BalanceConstraint.cpp:65
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:367
std::size_t mLastSupportVersion
The last version of the support polygon that was computed.
Definition: BalanceConstraint.hpp:177
void clearCaches()
Clear the caches to force the error computation to update.
Definition: BalanceConstraint.cpp:395
Eigen::Vector3d mLastCOM
The last computed location of the center of mass.
Definition: BalanceConstraint.hpp:174
const Eigen::Vector3d & getLastError() const
Get the last error vector that was computed by this BalanceConstraint.
Definition: BalanceConstraint.cpp:389
double getPseudoInverseDamping() const
Get the damping factor that will be used when computing the pseudoinverse.
Definition: BalanceConstraint.cpp:383
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:181
Eigen::MatrixXd mNullSpaceCache
Cache for the full null space.
Definition: BalanceConstraint.hpp:191
void setBalanceMethod(BalanceMethod_t _method)
Set the method that this constraint function will use to achieve balance.
Definition: BalanceConstraint.cpp:341
ErrorMethod_t mErrorMethod
The method that will be used to compute error.
Definition: BalanceConstraint.hpp:154
Eigen::VectorXd mInitialPositionsCache
Cache used by convertJacobianMethodOutputToGradient to avoid reallocating this vector on each iterati...
Definition: BalanceConstraint.hpp:198
void setErrorMethod(ErrorMethod_t _method)
Set the method that this constraint function will use to compute the error.
Definition: BalanceConstraint.cpp:325
math::LinearJacobian mEEJacCache
Cache for the end effector Jacobians so the space does not need to be reallocated each loop.
Definition: BalanceConstraint.hpp:185
This class should be inherited by optimizer::Function classes that have a dependency on the Hierarchi...
Definition: HierarchicalIK.hpp:91
Definition: Function.hpp:46
Eigen::Matrix< double, 3, Eigen::Dynamic > LinearJacobian
Definition: MathTypes.hpp:106
std::shared_ptr< Function > FunctionPtr
Definition: Function.hpp:84
Definition: BulletCollisionDetector.cpp:63