DART  6.10.1
HierarchicalIK.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, 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_DYNAMICS_HIERARCHICALIK_HPP_
34 #define DART_DYNAMICS_HIERARCHICALIK_HPP_
35 
36 #include <unordered_set>
37 
39 
40 namespace dart {
41 namespace dynamics {
42 
48 typedef std::vector<std::vector<std::shared_ptr<InverseKinematics> > >
50 
64 {
65 public:
67  virtual ~HierarchicalIK() = default;
68 
75  DART_DEPRECATED(6.8)
76  bool solve(bool applySolution = true);
77 
83  DART_DEPRECATED(6.8)
84  bool solve(Eigen::VectorXd& positions, bool applySolution = true);
85 
94  bool findSolution(Eigen::VectorXd& positions);
95 
106  bool solveAndApply(bool allowIncompleteResult = true);
107 
121  bool solveAndApply(
122  Eigen::VectorXd& positions, bool allowIncompleteResult = true);
123 
125  virtual std::shared_ptr<HierarchicalIK> clone(
126  const SkeletonPtr& _newSkel) const = 0;
127 
135  class Function
136  {
137  public:
140  const std::shared_ptr<HierarchicalIK>& _newIK) const = 0;
141 
143  virtual ~Function() = default;
144  };
145 
147  void setObjective(const std::shared_ptr<optimizer::Function>& _objective);
148 
150  const std::shared_ptr<optimizer::Function>& getObjective();
151 
153  std::shared_ptr<const optimizer::Function> getObjective() const;
154 
157  const std::shared_ptr<optimizer::Function>& _nsObjective);
158 
160  const std::shared_ptr<optimizer::Function>& getNullSpaceObjective();
161 
163  std::shared_ptr<const optimizer::Function> getNullSpaceObjective() const;
164 
166  bool hasNullSpaceObjective() const;
167 
169  const std::shared_ptr<optimizer::Problem>& getProblem();
170 
172  std::shared_ptr<const optimizer::Problem> getProblem() const;
173 
180  void resetProblem(bool _clearSeeds = false);
181 
184  void setSolver(const std::shared_ptr<optimizer::Solver>& _newSolver);
185 
187  const std::shared_ptr<optimizer::Solver>& getSolver();
188 
190  std::shared_ptr<const optimizer::Solver> getSolver() const;
191 
193  virtual void refreshIKHierarchy() = 0;
194 
196  const IKHierarchy& getIKHierarchy() const;
197 
199  const std::vector<Eigen::MatrixXd>& computeNullSpaces() const;
200 
203  Eigen::VectorXd getPositions() const;
204 
207  void setPositions(const Eigen::VectorXd& _q);
208 
211 
214 
218 
222 
225  void clearCaches();
226 
227 protected:
233  class Objective final : public Function, public optimizer::Function
234  {
235  public:
237  Objective(const std::shared_ptr<HierarchicalIK>& _ik);
238 
240  virtual ~Objective() = default;
241 
242  // Documentation inherited
244  const std::shared_ptr<HierarchicalIK>& _newIK) const override;
245 
246  // Documentation inherited
247  double eval(const Eigen::VectorXd& _x) override;
248 
249  // Documentation inherited
250  void evalGradient(
251  const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) override;
252 
253  protected:
255  std::weak_ptr<HierarchicalIK> mIK;
256 
258  Eigen::VectorXd mGradCache;
259  };
260 
266  class Constraint final : public Function, public optimizer::Function
267  {
268  public:
270  Constraint(const std::shared_ptr<HierarchicalIK>& _ik);
271 
273  virtual ~Constraint() = default;
274 
275  // Documentation inherited
277  const std::shared_ptr<HierarchicalIK>& _newIK) const override;
278 
279  // Documentation inherited
280  double eval(const Eigen::VectorXd& _x) override;
281 
282  // Documentation inherited
283  void evalGradient(
284  const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) override;
285 
286  protected:
288  std::weak_ptr<HierarchicalIK> mIK;
289 
291  Eigen::VectorXd mLevelGradCache;
292 
294  Eigen::VectorXd mTempGradCache;
295  };
296 
298  HierarchicalIK(const SkeletonPtr& _skeleton);
299 
301  void initialize(const std::shared_ptr<HierarchicalIK>& my_ptr);
302 
305  void copyOverSetup(const std::shared_ptr<HierarchicalIK>& _otherIK) const;
306 
309 
312 
314  std::shared_ptr<optimizer::Problem> mProblem;
315 
317  std::shared_ptr<optimizer::Solver> mSolver;
318 
321 
324 
326  std::weak_ptr<HierarchicalIK> mPtr;
327 
329  mutable Eigen::VectorXd mLastPositions;
330 
332  mutable std::vector<Eigen::MatrixXd> mNullSpaceCache;
333 
335  mutable Eigen::MatrixXd mPartialNullspaceCache;
336 
338  mutable Eigen::JacobiSVD<math::Jacobian> mSVDCache;
339 
342 
343 public:
344  // To get byte-aligned Eigen vectors
345  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
346 };
347 
352 {
353 public:
354  typedef std::unordered_set<std::shared_ptr<InverseKinematics> > ModuleSet;
355  typedef std::unordered_set<std::shared_ptr<const InverseKinematics> >
357 
359  static std::shared_ptr<CompositeIK> create(const SkeletonPtr& _skel);
360 
361  // Documentation inherited
362  std::shared_ptr<HierarchicalIK> clone(
363  const SkeletonPtr& _newSkel) const override;
364 
366  virtual std::shared_ptr<CompositeIK> cloneCompositeIK(
367  const SkeletonPtr& _newSkel) const;
368 
372  bool addModule(const std::shared_ptr<InverseKinematics>& _ik);
373 
375  const ModuleSet& getModuleSet();
376 
379 
380  // Documentation inherited
381  void refreshIKHierarchy() override;
382 
383 protected:
385  CompositeIK(const SkeletonPtr& _skel);
386 
388  std::unordered_set<std::shared_ptr<InverseKinematics> > mModuleSet;
389 };
390 
395 {
396 public:
398  static std::shared_ptr<WholeBodyIK> create(const SkeletonPtr& _skel);
399 
400  // Documentation inherited
401  std::shared_ptr<HierarchicalIK> clone(
402  const SkeletonPtr& _newSkel) const override;
403 
405  virtual std::shared_ptr<WholeBodyIK> cloneWholeBodyIK(
406  const SkeletonPtr& _newSkel) const;
407 
408  // Documentation inherited
409  void refreshIKHierarchy() override;
410 
411 protected:
413  WholeBodyIK(const SkeletonPtr& _skel);
414 };
415 
416 } // namespace dynamics
417 } // namespace dart
418 
419 #endif // DART_DYNAMICS_HIERARCHICALIK_HPP_
#define DART_DEPRECATED(version)
Definition: Deprecated.hpp:51
The Subject class is a base class for any object that wants to report when it gets destroyed.
Definition: Subject.hpp:58
The CompositeIK class allows you to specify an arbitrary hierarchy of InverseKinematics modules for a...
Definition: HierarchicalIK.hpp:352
std::unordered_set< std::shared_ptr< InverseKinematics > > ModuleSet
Definition: HierarchicalIK.hpp:354
void refreshIKHierarchy() override
Refresh the IK hierarchy of this IK module.
Definition: HierarchicalIK.cpp:722
std::unordered_set< std::shared_ptr< const InverseKinematics > > ConstModuleSet
Definition: HierarchicalIK.hpp:356
static std::shared_ptr< CompositeIK > create(const SkeletonPtr &_skel)
Create a CompositeIK module.
Definition: HierarchicalIK.cpp:643
CompositeIK(const SkeletonPtr &_skel)
Constructor.
Definition: HierarchicalIK.cpp:748
std::shared_ptr< HierarchicalIK > clone(const SkeletonPtr &_newSkel) const override
Clone this HierarchicalIK module.
Definition: HierarchicalIK.cpp:651
bool addModule(const std::shared_ptr< InverseKinematics > &_ik)
Add an IK module to this CompositeIK.
Definition: HierarchicalIK.cpp:688
virtual std::shared_ptr< CompositeIK > cloneCompositeIK(const SkeletonPtr &_newSkel) const
Same as clone(), but passes back a more complete type.
Definition: HierarchicalIK.cpp:658
const ModuleSet & getModuleSet()
Get the set of modules being used by this CompositeIK.
Definition: HierarchicalIK.cpp:706
std::unordered_set< std::shared_ptr< InverseKinematics > > mModuleSet
The set of modules being used by this CompositeIK.
Definition: HierarchicalIK.hpp:388
The HierarchicalIK::Constraint Function is simply used to merge the constraints of the InverseKinemat...
Definition: HierarchicalIK.hpp:267
Eigen::VectorXd mTempGradCache
Cache for temporary gradients.
Definition: HierarchicalIK.hpp:294
std::weak_ptr< HierarchicalIK > mIK
Pointer to this Constraint's HierarchicalIK module.
Definition: HierarchicalIK.hpp:288
virtual ~Constraint()=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: HierarchicalIK.cpp:528
optimizer::FunctionPtr clone(const std::shared_ptr< HierarchicalIK > &_newIK) const override
Enable this function to be cloned to a new IK module.
Definition: HierarchicalIK.cpp:479
Constraint(const std::shared_ptr< HierarchicalIK > &_ik)
Constructor.
Definition: HierarchicalIK.cpp:471
Eigen::VectorXd mLevelGradCache
Cache for the gradient of a level.
Definition: HierarchicalIK.hpp:291
double eval(const Eigen::VectorXd &_x) override
Evaluates and returns the objective function at the point x.
Definition: HierarchicalIK.cpp:486
This class should be inherited by optimizer::Function classes that have a dependency on the Hierarchi...
Definition: HierarchicalIK.hpp:136
virtual optimizer::FunctionPtr clone(const std::shared_ptr< HierarchicalIK > &_newIK) const =0
Enable this function to be cloned to a new IK module.
virtual ~Function()=default
Virtual destructor.
The HierarchicalIK::Objective Function is simply used to merge the objective and null space objective...
Definition: HierarchicalIK.hpp:234
Objective(const std::shared_ptr< HierarchicalIK > &_ik)
Constructor.
Definition: HierarchicalIK.cpp:395
optimizer::FunctionPtr clone(const std::shared_ptr< HierarchicalIK > &_newIK) const override
Enable this function to be cloned to a new IK module.
Definition: HierarchicalIK.cpp:402
double eval(const Eigen::VectorXd &_x) override
Evaluates and returns the objective function at the point x.
Definition: HierarchicalIK.cpp:409
virtual ~Objective()=default
Virtual destructor.
std::weak_ptr< HierarchicalIK > mIK
Pointer to this Objective's HierarchicalIK module.
Definition: HierarchicalIK.hpp:255
Eigen::VectorXd mGradCache
Cache for the gradient computation.
Definition: HierarchicalIK.hpp:258
void evalGradient(const Eigen::VectorXd &_x, Eigen::Map< Eigen::VectorXd > _grad) override
Evaluates and returns the objective function at the point x.
Definition: HierarchicalIK.cpp:433
The HierarchicalIK class provides a convenient way of setting up a hierarchical inverse kinematics op...
Definition: HierarchicalIK.hpp:64
virtual std::shared_ptr< HierarchicalIK > clone(const SkeletonPtr &_newSkel) const =0
Clone this HierarchicalIK module.
void clearCaches()
Clear the caches of this IK module.
Definition: HierarchicalIK.cpp:389
IKHierarchy mHierarchy
Cache for the IK hierarcy.
Definition: HierarchicalIK.hpp:311
std::shared_ptr< optimizer::Solver > mSolver
The Solver that this IK module will use.
Definition: HierarchicalIK.hpp:317
void resetProblem(bool _clearSeeds=false)
Reset the Problem that is being maintained by this HierarchicalIK module.
Definition: HierarchicalIK.cpp:207
optimizer::FunctionPtr mNullSpaceObjective
The null space Objective of this IK module.
Definition: HierarchicalIK.hpp:323
WeakSkeletonPtr mSkeleton
Pointer to the Skeleton that this IK is tied to.
Definition: HierarchicalIK.hpp:308
bool hasNullSpaceObjective() const
Returns true if this HierarchicalIK has a null space objective.
Definition: HierarchicalIK.cpp:189
optimizer::FunctionPtr mObjective
The Objective of this IK module.
Definition: HierarchicalIK.hpp:320
void setNullSpaceObjective(const std::shared_ptr< optimizer::Function > &_nsObjective)
Set the null space objective for this HierarchicalIK.
Definition: HierarchicalIK.cpp:168
const std::shared_ptr< optimizer::Solver > & getSolver()
Get the Solver that is being used by this IK module.
Definition: HierarchicalIK.cpp:233
void setPositions(const Eigen::VectorXd &_q)
Set the current joint positions of the Skeleton associated with this IK module.
Definition: HierarchicalIK.cpp:357
const IKHierarchy & getIKHierarchy() const
Get the IK hierarchy of this IK module.
Definition: HierarchicalIK.cpp:245
void setSolver(const std::shared_ptr< optimizer::Solver > &_newSolver)
Set the Solver that should be used by this IK module, and set it up with the Problem that is configur...
Definition: HierarchicalIK.cpp:222
const std::shared_ptr< optimizer::Function > & getObjective()
Get the objective function for this HierarchicalIK.
Definition: HierarchicalIK.cpp:156
virtual ~HierarchicalIK()=default
Virtual destructor.
virtual void refreshIKHierarchy()=0
Refresh the IK hierarchy of this IK module.
SkeletonPtr getAffiliation()
This is the same as getSkeleton().
Definition: HierarchicalIK.cpp:377
void initialize(const std::shared_ptr< HierarchicalIK > &my_ptr)
Setup the module.
Definition: HierarchicalIK.cpp:589
const std::shared_ptr< optimizer::Problem > & getProblem()
Get the Problem that is being maintained by this HierarchicalIK module.
Definition: HierarchicalIK.cpp:195
bool solveAndApply(bool allowIncompleteResult=true)
Identical to findSolution(), but this function applies the solution when the solver successfully foun...
Definition: HierarchicalIK.cpp:129
std::weak_ptr< HierarchicalIK > mPtr
Weak pointer to self.
Definition: HierarchicalIK.hpp:326
void copyOverSetup(const std::shared_ptr< HierarchicalIK > &_otherIK) const
Copy the setup of this HierarchicalIK module into another HierarchicalIK module.
Definition: HierarchicalIK.cpp:620
Eigen::VectorXd mLastPositions
Cache for the last positions.
Definition: HierarchicalIK.hpp:329
std::vector< Eigen::MatrixXd > mNullSpaceCache
Cache for null space computations.
Definition: HierarchicalIK.hpp:332
const std::shared_ptr< optimizer::Function > & getNullSpaceObjective()
Get the null space objective for this HierarchicalIK.
Definition: HierarchicalIK.cpp:176
const std::vector< Eigen::MatrixXd > & computeNullSpaces() const
Compute the null spaces of each level of the hierarchy.
Definition: HierarchicalIK.cpp:251
void setObjective(const std::shared_ptr< optimizer::Function > &_objective)
Set the objective function for this HierarchicalIK.
Definition: HierarchicalIK.cpp:149
Eigen::MatrixXd mPartialNullspaceCache
Cache for a partial null space computation.
Definition: HierarchicalIK.hpp:335
std::shared_ptr< optimizer::Problem > mProblem
The Problem that this IK module is maintaining.
Definition: HierarchicalIK.hpp:314
Eigen::JacobiSVD< math::Jacobian > mSVDCache
Cache for the null space SVD.
Definition: HierarchicalIK.hpp:338
Eigen::VectorXd getPositions() const
Get the current joint positions of the Skeleton associated with this IK module.
Definition: HierarchicalIK.cpp:347
bool findSolution(Eigen::VectorXd &positions)
Finds a solution of the IK problem without applying the solution.
Definition: HierarchicalIK.cpp:67
SkeletonPtr getSkeleton()
Get the Skeleton that this IK module is associated with.
Definition: HierarchicalIK.cpp:365
HierarchicalIK(const SkeletonPtr &_skeleton)
Constructor.
Definition: HierarchicalIK.cpp:581
bool solve(bool applySolution=true)
Solve the IK Problem.
Definition: HierarchicalIK.cpp:44
math::Jacobian mJacCache
Cache for Jacobians.
Definition: HierarchicalIK.hpp:341
The WholeBodyIK class provides an interface for simultaneously solving all the IK constraints of all ...
Definition: HierarchicalIK.hpp:395
WholeBodyIK(const SkeletonPtr &_skel)
Constructor.
Definition: HierarchicalIK.cpp:842
virtual std::shared_ptr< WholeBodyIK > cloneWholeBodyIK(const SkeletonPtr &_newSkel) const
Same as clone(), but produces a more complete type.
Definition: HierarchicalIK.cpp:769
static std::shared_ptr< WholeBodyIK > create(const SkeletonPtr &_skel)
Create a WholeBodyIK.
Definition: HierarchicalIK.cpp:754
void refreshIKHierarchy() override
Refresh the IK hierarchy of this IK module.
Definition: HierarchicalIK.cpp:778
std::shared_ptr< HierarchicalIK > clone(const SkeletonPtr &_newSkel) const override
Clone this HierarchicalIK module.
Definition: HierarchicalIK.cpp:762
Definition: Function.hpp:46
Definition: Random-impl.hpp:92
std::shared_ptr< const Skeleton > ConstSkeletonPtr
Definition: SmartPointer.hpp:60
std::weak_ptr< Skeleton > WeakSkeletonPtr
Definition: SmartPointer.hpp:60
std::vector< std::vector< std::shared_ptr< InverseKinematics > > > IKHierarchy
An IKHierarchy is a sorted set of IK modules.
Definition: HierarchicalIK.hpp:49
std::shared_ptr< Skeleton > SkeletonPtr
Definition: SmartPointer.hpp:60
Eigen::Matrix< double, 6, Eigen::Dynamic > Jacobian
Definition: MathTypes.hpp:114
std::shared_ptr< Function > FunctionPtr
Definition: Function.hpp:84
Definition: BulletCollisionDetector.cpp:65
Definition: SharedLibraryManager.hpp:46