DART 6.8.5
Loading...
Searching...
No Matches
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
40namespace dart {
41namespace dynamics {
42
48typedef std::vector< std::vector< std::shared_ptr<InverseKinematics> > > IKHierarchy;
49
63{
64public:
65
67 virtual ~HierarchicalIK() = default;
68
76 bool solve(bool applySolution = true);
77
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
136 {
137 public:
138
141 const std::shared_ptr<HierarchicalIK>& _newIK) const = 0;
142
144 virtual ~Function() = default;
145 };
146
148 void setObjective(const std::shared_ptr<optimizer::Function>& _objective);
149
151 const std::shared_ptr<optimizer::Function>& getObjective();
152
154 std::shared_ptr<const optimizer::Function> getObjective() const;
155
158 const std::shared_ptr<optimizer::Function>& _nsObjective);
159
161 const std::shared_ptr<optimizer::Function>& getNullSpaceObjective();
162
164 std::shared_ptr<const optimizer::Function> getNullSpaceObjective() const;
165
167 bool hasNullSpaceObjective() const;
168
170 const std::shared_ptr<optimizer::Problem>& getProblem();
171
173 std::shared_ptr<const optimizer::Problem> getProblem() const;
174
181 void resetProblem(bool _clearSeeds=false);
182
185 void setSolver(const std::shared_ptr<optimizer::Solver>& _newSolver);
186
188 const std::shared_ptr<optimizer::Solver>& getSolver();
189
191 std::shared_ptr<const optimizer::Solver> getSolver() const;
192
194 virtual void refreshIKHierarchy() = 0;
195
197 const IKHierarchy& getIKHierarchy() const;
198
200 const std::vector<Eigen::MatrixXd>& computeNullSpaces() const;
201
204 Eigen::VectorXd getPositions() const;
205
208 void setPositions(const Eigen::VectorXd& _q);
209
212
215
219
223
226 void clearCaches();
227
228protected:
229
235 class Objective final : public Function, public optimizer::Function
236 {
237 public:
238
240 Objective(const std::shared_ptr<HierarchicalIK>& _ik);
241
243 virtual ~Objective() = default;
244
245 // Documentation inherited
247 const std::shared_ptr<HierarchicalIK>& _newIK) const override;
248
249 // Documentation inherited
250 double eval(const Eigen::VectorXd &_x) override;
251
252 // Documentation inherited
253 void evalGradient(const Eigen::VectorXd& _x,
254 Eigen::Map<Eigen::VectorXd> _grad) override;
255
256 protected:
257
259 std::weak_ptr<HierarchicalIK> mIK;
260
262 Eigen::VectorXd mGradCache;
263 };
264
270 class Constraint final : public Function, public optimizer::Function
271 {
272 public:
273
275 Constraint(const std::shared_ptr<HierarchicalIK>& _ik);
276
278 virtual ~Constraint() = default;
279
280 // Documentation inherited
282 const std::shared_ptr<HierarchicalIK>& _newIK) const override;
283
284 // Documentation inherited
285 double eval(const Eigen::VectorXd& _x) override;
286
287 // Documentation inherited
288 void evalGradient(const Eigen::VectorXd& _x,
289 Eigen::Map<Eigen::VectorXd> _grad) override;
290
291 protected:
292
294 std::weak_ptr<HierarchicalIK> mIK;
295
297 Eigen::VectorXd mLevelGradCache;
298
300 Eigen::VectorXd mTempGradCache;
301 };
302
304 HierarchicalIK(const SkeletonPtr& _skeleton);
305
307 void initialize(const std::shared_ptr<HierarchicalIK>& my_ptr);
308
311 void copyOverSetup(const std::shared_ptr<HierarchicalIK>& _otherIK) const;
312
315
318
320 std::shared_ptr<optimizer::Problem> mProblem;
321
323 std::shared_ptr<optimizer::Solver> mSolver;
324
327
330
332 std::weak_ptr<HierarchicalIK> mPtr;
333
335 mutable Eigen::VectorXd mLastPositions;
336
338 mutable std::vector<Eigen::MatrixXd> mNullSpaceCache;
339
341 mutable Eigen::MatrixXd mPartialNullspaceCache;
342
344 mutable Eigen::JacobiSVD<math::Jacobian> mSVDCache;
345
348
349public:
350 // To get byte-aligned Eigen vectors
351 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
352};
353
358{
359public:
360
361 typedef std::unordered_set< std::shared_ptr<InverseKinematics> > ModuleSet;
362 typedef std::unordered_set< std::shared_ptr<const InverseKinematics> > ConstModuleSet;
363
365 static std::shared_ptr<CompositeIK> create(const SkeletonPtr& _skel);
366
367 // Documentation inherited
368 std::shared_ptr<HierarchicalIK> clone(
369 const SkeletonPtr &_newSkel) const override;
370
372 virtual std::shared_ptr<CompositeIK> cloneCompositeIK(
373 const SkeletonPtr& _newSkel) const;
374
378 bool addModule(const std::shared_ptr<InverseKinematics>& _ik);
379
381 const ModuleSet& getModuleSet();
382
385
386 // Documentation inherited
387 void refreshIKHierarchy() override;
388
389protected:
390
392 CompositeIK(const SkeletonPtr& _skel);
393
395 std::unordered_set< std::shared_ptr<InverseKinematics> > mModuleSet;
396};
397
402{
403public:
404
406 static std::shared_ptr<WholeBodyIK> create(const SkeletonPtr& _skel);
407
408 // Documentation inherited
409 std::shared_ptr<HierarchicalIK> clone(
410 const SkeletonPtr &_newSkel) const override;
411
413 virtual std::shared_ptr<WholeBodyIK> cloneWholeBodyIK(
414 const SkeletonPtr& _newSkel) const;
415
416 // Documentation inherited
417 void refreshIKHierarchy() override;
418
419protected:
420
422 WholeBodyIK(const SkeletonPtr& _skel);
423};
424
425} // namespace dynamics
426} // namespace dart
427
428#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:358
void refreshIKHierarchy() override
Refresh the IK hierarchy of this IK module.
Definition HierarchicalIK.cpp:719
std::unordered_set< std::shared_ptr< const InverseKinematics > > ConstModuleSet
Definition HierarchicalIK.hpp:362
static std::shared_ptr< CompositeIK > create(const SkeletonPtr &_skel)
Create a CompositeIK module.
Definition HierarchicalIK.cpp:640
std::unordered_set< std::shared_ptr< InverseKinematics > > mModuleSet
The set of modules being used by this CompositeIK.
Definition HierarchicalIK.hpp:395
std::shared_ptr< HierarchicalIK > clone(const SkeletonPtr &_newSkel) const override
Clone this HierarchicalIK module.
Definition HierarchicalIK.cpp:648
bool addModule(const std::shared_ptr< InverseKinematics > &_ik)
Add an IK module to this CompositeIK.
Definition HierarchicalIK.cpp:685
std::unordered_set< std::shared_ptr< InverseKinematics > > ModuleSet
Definition HierarchicalIK.hpp:361
virtual std::shared_ptr< CompositeIK > cloneCompositeIK(const SkeletonPtr &_newSkel) const
Same as clone(), but passes back a more complete type.
Definition HierarchicalIK.cpp:655
const ModuleSet & getModuleSet()
Get the set of modules being used by this CompositeIK.
Definition HierarchicalIK.cpp:703
The HierarchicalIK::Constraint Function is simply used to merge the constraints of the InverseKinemat...
Definition HierarchicalIK.hpp:271
Eigen::VectorXd mTempGradCache
Cache for temporary gradients.
Definition HierarchicalIK.hpp:300
std::weak_ptr< HierarchicalIK > mIK
Pointer to this Constraint's HierarchicalIK module.
Definition HierarchicalIK.hpp:294
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:525
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:477
Eigen::VectorXd mLevelGradCache
Cache for the gradient of a level.
Definition HierarchicalIK.hpp:297
double eval(const Eigen::VectorXd &_x) override
Evaluates and returns the objective function at the point x.
Definition HierarchicalIK.cpp:483
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:236
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:401
double eval(const Eigen::VectorXd &_x) override
Evaluates and returns the objective function at the point x.
Definition HierarchicalIK.cpp:408
virtual ~Objective()=default
Virtual destructor.
std::weak_ptr< HierarchicalIK > mIK
Pointer to this Objective's HierarchicalIK module.
Definition HierarchicalIK.hpp:259
Eigen::VectorXd mGradCache
Cache for the gradient computation.
Definition HierarchicalIK.hpp:262
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:432
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:388
IKHierarchy mHierarchy
Cache for the IK hierarcy.
Definition HierarchicalIK.hpp:317
std::shared_ptr< optimizer::Solver > mSolver
The Solver that this IK module will use.
Definition HierarchicalIK.hpp:323
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:329
WeakSkeletonPtr mSkeleton
Pointer to the Skeleton that this IK is tied to.
Definition HierarchicalIK.hpp:314
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:326
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:356
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:376
void initialize(const std::shared_ptr< HierarchicalIK > &my_ptr)
Setup the module.
Definition HierarchicalIK.cpp:586
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:332
void copyOverSetup(const std::shared_ptr< HierarchicalIK > &_otherIK) const
Copy the setup of this HierarchicalIK module into another HierarchicalIK module.
Definition HierarchicalIK.cpp:617
Eigen::VectorXd mLastPositions
Cache for the last positions.
Definition HierarchicalIK.hpp:335
std::vector< Eigen::MatrixXd > mNullSpaceCache
Cache for null space computations.
Definition HierarchicalIK.hpp:338
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:341
std::shared_ptr< optimizer::Problem > mProblem
The Problem that this IK module is maintaining.
Definition HierarchicalIK.hpp:320
Eigen::JacobiSVD< math::Jacobian > mSVDCache
Cache for the null space SVD.
Definition HierarchicalIK.hpp:344
Eigen::VectorXd getPositions() const
Get the current joint positions of the Skeleton associated with this IK module.
Definition HierarchicalIK.cpp:346
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:364
bool solve(bool applySolution=true)
Solve the IK Problem.
Definition HierarchicalIK.cpp:44
math::Jacobian mJacCache
Cache for Jacobians.
Definition HierarchicalIK.hpp:347
The WholeBodyIK class provides an interface for simultaneously solving all the IK constraints of all ...
Definition HierarchicalIK.hpp:402
virtual std::shared_ptr< WholeBodyIK > cloneWholeBodyIK(const SkeletonPtr &_newSkel) const
Same as clone(), but produces a more complete type.
Definition HierarchicalIK.cpp:767
static std::shared_ptr< WholeBodyIK > create(const SkeletonPtr &_skel)
Create a WholeBodyIK.
Definition HierarchicalIK.cpp:752
void refreshIKHierarchy() override
Refresh the IK hierarchy of this IK module.
Definition HierarchicalIK.cpp:776
std::shared_ptr< HierarchicalIK > clone(const SkeletonPtr &_newSkel) const override
Clone this HierarchicalIK module.
Definition HierarchicalIK.cpp:760
Definition Function.hpp:46
Definition Random-impl.hpp:92
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
Definition SharedLibraryManager.hpp:43