DART  6.7.3
HierarchicalIK.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_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> > > IKHierarchy;
49 
63 {
64 public:
65 
67  virtual ~HierarchicalIK() = default;
68 
73  bool solve(bool _applySolution = true);
74 
77  bool solve(Eigen::VectorXd& positions, bool _applySolution = true);
78 
80  virtual std::shared_ptr<HierarchicalIK> clone(
81  const SkeletonPtr& _newSkel) const = 0;
82 
90  class Function
91  {
92  public:
93 
96  const std::shared_ptr<HierarchicalIK>& _newIK) const = 0;
97 
99  virtual ~Function() = default;
100  };
101 
103  void setObjective(const std::shared_ptr<optimizer::Function>& _objective);
104 
106  const std::shared_ptr<optimizer::Function>& getObjective();
107 
109  std::shared_ptr<const optimizer::Function> getObjective() const;
110 
113  const std::shared_ptr<optimizer::Function>& _nsObjective);
114 
116  const std::shared_ptr<optimizer::Function>& getNullSpaceObjective();
117 
119  std::shared_ptr<const optimizer::Function> getNullSpaceObjective() const;
120 
122  bool hasNullSpaceObjective() const;
123 
125  const std::shared_ptr<optimizer::Problem>& getProblem();
126 
128  std::shared_ptr<const optimizer::Problem> getProblem() const;
129 
136  void resetProblem(bool _clearSeeds=false);
137 
140  void setSolver(const std::shared_ptr<optimizer::Solver>& _newSolver);
141 
143  const std::shared_ptr<optimizer::Solver>& getSolver();
144 
146  std::shared_ptr<const optimizer::Solver> getSolver() const;
147 
149  virtual void refreshIKHierarchy() = 0;
150 
152  const IKHierarchy& getIKHierarchy() const;
153 
155  const std::vector<Eigen::MatrixXd>& computeNullSpaces() const;
156 
159  Eigen::VectorXd getPositions() const;
160 
163  void setPositions(const Eigen::VectorXd& _q);
164 
167 
170 
174 
178 
181  void clearCaches();
182 
183 protected:
184 
190  class Objective final : public Function, public optimizer::Function
191  {
192  public:
193 
195  Objective(const std::shared_ptr<HierarchicalIK>& _ik);
196 
198  virtual ~Objective() = default;
199 
200  // Documentation inherited
202  const std::shared_ptr<HierarchicalIK>& _newIK) const override;
203 
204  // Documentation inherited
205  double eval(const Eigen::VectorXd &_x) override;
206 
207  // Documentation inherited
208  void evalGradient(const Eigen::VectorXd& _x,
209  Eigen::Map<Eigen::VectorXd> _grad) override;
210 
211  protected:
212 
214  std::weak_ptr<HierarchicalIK> mIK;
215 
217  Eigen::VectorXd mGradCache;
218  };
219 
225  class Constraint final : public Function, public optimizer::Function
226  {
227  public:
228 
230  Constraint(const std::shared_ptr<HierarchicalIK>& _ik);
231 
233  virtual ~Constraint() = default;
234 
235  // Documentation inherited
237  const std::shared_ptr<HierarchicalIK>& _newIK) const override;
238 
239  // Documentation inherited
240  double eval(const Eigen::VectorXd& _x) override;
241 
242  // Documentation inherited
243  void evalGradient(const Eigen::VectorXd& _x,
244  Eigen::Map<Eigen::VectorXd> _grad) override;
245 
246  protected:
247 
249  std::weak_ptr<HierarchicalIK> mIK;
250 
252  Eigen::VectorXd mLevelGradCache;
253 
255  Eigen::VectorXd mTempGradCache;
256  };
257 
259  HierarchicalIK(const SkeletonPtr& _skeleton);
260 
262  void initialize(const std::shared_ptr<HierarchicalIK>& my_ptr);
263 
266  void copyOverSetup(const std::shared_ptr<HierarchicalIK>& _otherIK) const;
267 
270 
273 
275  std::shared_ptr<optimizer::Problem> mProblem;
276 
278  std::shared_ptr<optimizer::Solver> mSolver;
279 
282 
285 
287  std::weak_ptr<HierarchicalIK> mPtr;
288 
290  mutable Eigen::VectorXd mLastPositions;
291 
293  mutable std::vector<Eigen::MatrixXd> mNullSpaceCache;
294 
296  mutable Eigen::MatrixXd mPartialNullspaceCache;
297 
299  mutable Eigen::JacobiSVD<math::Jacobian> mSVDCache;
300 
303 
304 public:
305  // To get byte-aligned Eigen vectors
306  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
307 };
308 
313 {
314 public:
315 
316  typedef std::unordered_set< std::shared_ptr<InverseKinematics> > ModuleSet;
317  typedef std::unordered_set< std::shared_ptr<const InverseKinematics> > ConstModuleSet;
318 
320  static std::shared_ptr<CompositeIK> create(const SkeletonPtr& _skel);
321 
322  // Documentation inherited
323  std::shared_ptr<HierarchicalIK> clone(
324  const SkeletonPtr &_newSkel) const override;
325 
327  virtual std::shared_ptr<CompositeIK> cloneCompositeIK(
328  const SkeletonPtr& _newSkel) const;
329 
333  bool addModule(const std::shared_ptr<InverseKinematics>& _ik);
334 
336  const ModuleSet& getModuleSet();
337 
340 
341  // Documentation inherited
342  void refreshIKHierarchy() override;
343 
344 protected:
345 
347  CompositeIK(const SkeletonPtr& _skel);
348 
350  std::unordered_set< std::shared_ptr<InverseKinematics> > mModuleSet;
351 };
352 
357 {
358 public:
359 
361  static std::shared_ptr<WholeBodyIK> create(const SkeletonPtr& _skel);
362 
363  // Documentation inherited
364  std::shared_ptr<HierarchicalIK> clone(
365  const SkeletonPtr &_newSkel) const override;
366 
368  virtual std::shared_ptr<WholeBodyIK> cloneWholeBodyIK(
369  const SkeletonPtr& _newSkel) const;
370 
371  // Documentation inherited
372  void refreshIKHierarchy() override;
373 
374 protected:
375 
377  WholeBodyIK(const SkeletonPtr& _skel);
378 };
379 
380 } // namespace dynamics
381 } // namespace dart
382 
383 #endif // DART_DYNAMICS_HIERARCHICALIK_HPP_
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:313
void refreshIKHierarchy() override
Refresh the IK hierarchy of this IK module.
Definition: HierarchicalIK.cpp:681
std::unordered_set< std::shared_ptr< const InverseKinematics > > ConstModuleSet
Definition: HierarchicalIK.hpp:317
static std::shared_ptr< CompositeIK > create(const SkeletonPtr &_skel)
Create a CompositeIK module.
Definition: HierarchicalIK.cpp:602
CompositeIK(const SkeletonPtr &_skel)
Constructor.
Definition: HierarchicalIK.cpp:707
std::unordered_set< std::shared_ptr< InverseKinematics > > mModuleSet
The set of modules being used by this CompositeIK.
Definition: HierarchicalIK.hpp:350
std::shared_ptr< HierarchicalIK > clone(const SkeletonPtr &_newSkel) const override
Clone this HierarchicalIK module.
Definition: HierarchicalIK.cpp:610
bool addModule(const std::shared_ptr< InverseKinematics > &_ik)
Add an IK module to this CompositeIK.
Definition: HierarchicalIK.cpp:647
std::unordered_set< std::shared_ptr< InverseKinematics > > ModuleSet
Definition: HierarchicalIK.hpp:316
virtual std::shared_ptr< CompositeIK > cloneCompositeIK(const SkeletonPtr &_newSkel) const
Same as clone(), but passes back a more complete type.
Definition: HierarchicalIK.cpp:617
const ModuleSet & getModuleSet()
Get the set of modules being used by this CompositeIK.
Definition: HierarchicalIK.cpp:665
The HierarchicalIK::Constraint Function is simply used to merge the constraints of the InverseKinemat...
Definition: HierarchicalIK.hpp:226
Eigen::VectorXd mTempGradCache
Cache for temporary gradients.
Definition: HierarchicalIK.hpp:255
std::weak_ptr< HierarchicalIK > mIK
Pointer to this Constraint's HierarchicalIK module.
Definition: HierarchicalIK.hpp:249
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:487
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:439
Constraint(const std::shared_ptr< HierarchicalIK > &_ik)
Constructor.
Definition: HierarchicalIK.cpp:432
Eigen::VectorXd mLevelGradCache
Cache for the gradient of a level.
Definition: HierarchicalIK.hpp:252
double eval(const Eigen::VectorXd &_x) override
Evaluates and returns the objective function at the point x.
Definition: HierarchicalIK.cpp:445
This class should be inherited by optimizer::Function classes that have a dependency on the Hierarchi...
Definition: HierarchicalIK.hpp:91
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:191
Objective(const std::shared_ptr< HierarchicalIK > &_ik)
Constructor.
Definition: HierarchicalIK.cpp:356
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:363
double eval(const Eigen::VectorXd &_x) override
Evaluates and returns the objective function at the point x.
Definition: HierarchicalIK.cpp:370
virtual ~Objective()=default
Virtual destructor.
std::weak_ptr< HierarchicalIK > mIK
Pointer to this Objective's HierarchicalIK module.
Definition: HierarchicalIK.hpp:214
Eigen::VectorXd mGradCache
Cache for the gradient computation.
Definition: HierarchicalIK.hpp:217
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:394
The HierarchicalIK class provides a convenient way of setting up a hierarchical inverse kinematics op...
Definition: HierarchicalIK.hpp:63
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:350
IKHierarchy mHierarchy
Cache for the IK hierarcy.
Definition: HierarchicalIK.hpp:272
std::shared_ptr< optimizer::Solver > mSolver
The Solver that this IK module will use.
Definition: HierarchicalIK.hpp:278
void resetProblem(bool _clearSeeds=false)
Reset the Problem that is being maintained by this HierarchicalIK module.
Definition: HierarchicalIK.cpp:169
optimizer::FunctionPtr mNullSpaceObjective
The null space Objective of this IK module.
Definition: HierarchicalIK.hpp:284
WeakSkeletonPtr mSkeleton
Pointer to the Skeleton that this IK is tied to.
Definition: HierarchicalIK.hpp:269
bool hasNullSpaceObjective() const
Returns true if this HierarchicalIK has a null space objective.
Definition: HierarchicalIK.cpp:151
optimizer::FunctionPtr mObjective
The Objective of this IK module.
Definition: HierarchicalIK.hpp:281
void setNullSpaceObjective(const std::shared_ptr< optimizer::Function > &_nsObjective)
Set the null space objective for this HierarchicalIK.
Definition: HierarchicalIK.cpp:130
const std::shared_ptr< optimizer::Solver > & getSolver()
Get the Solver that is being used by this IK module.
Definition: HierarchicalIK.cpp:195
void setPositions(const Eigen::VectorXd &_q)
Set the current joint positions of the Skeleton associated with this IK module.
Definition: HierarchicalIK.cpp:318
const IKHierarchy & getIKHierarchy() const
Get the IK hierarchy of this IK module.
Definition: HierarchicalIK.cpp:207
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:184
const std::shared_ptr< optimizer::Function > & getObjective()
Get the objective function for this HierarchicalIK.
Definition: HierarchicalIK.cpp:118
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:338
void initialize(const std::shared_ptr< HierarchicalIK > &my_ptr)
Setup the module.
Definition: HierarchicalIK.cpp:548
const std::shared_ptr< optimizer::Problem > & getProblem()
Get the Problem that is being maintained by this HierarchicalIK module.
Definition: HierarchicalIK.cpp:157
std::weak_ptr< HierarchicalIK > mPtr
Weak pointer to self.
Definition: HierarchicalIK.hpp:287
void copyOverSetup(const std::shared_ptr< HierarchicalIK > &_otherIK) const
Copy the setup of this HierarchicalIK module into another HierarchicalIK module.
Definition: HierarchicalIK.cpp:579
Eigen::VectorXd mLastPositions
Cache for the last positions.
Definition: HierarchicalIK.hpp:290
std::vector< Eigen::MatrixXd > mNullSpaceCache
Cache for null space computations.
Definition: HierarchicalIK.hpp:293
bool solve(bool _applySolution=true)
Solve the IK Problem.
Definition: HierarchicalIK.cpp:44
const std::shared_ptr< optimizer::Function > & getNullSpaceObjective()
Get the null space objective for this HierarchicalIK.
Definition: HierarchicalIK.cpp:138
const std::vector< Eigen::MatrixXd > & computeNullSpaces() const
Compute the null spaces of each level of the hierarchy.
Definition: HierarchicalIK.cpp:213
void setObjective(const std::shared_ptr< optimizer::Function > &_objective)
Set the objective function for this HierarchicalIK.
Definition: HierarchicalIK.cpp:111
Eigen::MatrixXd mPartialNullspaceCache
Cache for a partial null space computation.
Definition: HierarchicalIK.hpp:296
std::shared_ptr< optimizer::Problem > mProblem
The Problem that this IK module is maintaining.
Definition: HierarchicalIK.hpp:275
Eigen::JacobiSVD< math::Jacobian > mSVDCache
Cache for the null space SVD.
Definition: HierarchicalIK.hpp:299
Eigen::VectorXd getPositions() const
Get the current joint positions of the Skeleton associated with this IK module.
Definition: HierarchicalIK.cpp:308
SkeletonPtr getSkeleton()
Get the Skeleton that this IK module is associated with.
Definition: HierarchicalIK.cpp:326
HierarchicalIK(const SkeletonPtr &_skeleton)
Constructor.
Definition: HierarchicalIK.cpp:540
math::Jacobian mJacCache
Cache for Jacobians.
Definition: HierarchicalIK.hpp:302
The WholeBodyIK class provides an interface for simultaneously solving all the IK constraints of all ...
Definition: HierarchicalIK.hpp:357
WholeBodyIK(const SkeletonPtr &_skel)
Constructor.
Definition: HierarchicalIK.cpp:802
virtual std::shared_ptr< WholeBodyIK > cloneWholeBodyIK(const SkeletonPtr &_newSkel) const
Same as clone(), but produces a more complete type.
Definition: HierarchicalIK.cpp:729
static std::shared_ptr< WholeBodyIK > create(const SkeletonPtr &_skel)
Create a WholeBodyIK.
Definition: HierarchicalIK.cpp:714
void refreshIKHierarchy() override
Refresh the IK hierarchy of this IK module.
Definition: HierarchicalIK.cpp:738
std::shared_ptr< HierarchicalIK > clone(const SkeletonPtr &_newSkel) const override
Clone this HierarchicalIK module.
Definition: HierarchicalIK.cpp:722
Definition: Function.hpp:46
std::shared_ptr< const Skeleton > ConstSkeletonPtr
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:48
std::weak_ptr< Skeleton > WeakSkeletonPtr
Definition: SmartPointer.hpp:60
std::shared_ptr< Skeleton > SkeletonPtr
Definition: SmartPointer.hpp:60
Eigen::Matrix< double, 6, Eigen::Dynamic > Jacobian
Definition: MathTypes.hpp:108
std::shared_ptr< Function > FunctionPtr
Definition: Function.hpp:84
Definition: BulletCollisionDetector.cpp:63