33#ifndef DART_DYNAMICS_HIERARCHICALIK_HPP_
34#define DART_DYNAMICS_HIERARCHICALIK_HPP_
36#include <unordered_set>
48typedef std::vector< std::vector< std::shared_ptr<InverseKinematics> > >
IKHierarchy;
76 bool
solve(
bool applySolution = true);
84 bool solve(
Eigen::VectorXd& positions,
bool applySolution = true);
122 Eigen::VectorXd& positions,
bool allowIncompleteResult = true);
141 const std::shared_ptr<HierarchicalIK>& _newIK)
const = 0;
148 void setObjective(
const std::shared_ptr<optimizer::Function>& _objective);
151 const std::shared_ptr<optimizer::Function>&
getObjective();
154 std::shared_ptr<const optimizer::Function>
getObjective()
const;
158 const std::shared_ptr<optimizer::Function>& _nsObjective);
170 const std::shared_ptr<optimizer::Problem>&
getProblem();
173 std::shared_ptr<const optimizer::Problem>
getProblem()
const;
185 void setSolver(
const std::shared_ptr<optimizer::Solver>& _newSolver);
188 const std::shared_ptr<optimizer::Solver>&
getSolver();
191 std::shared_ptr<const optimizer::Solver>
getSolver()
const;
240 Objective(
const std::shared_ptr<HierarchicalIK>& _ik);
247 const std::shared_ptr<HierarchicalIK>& _newIK)
const override;
250 double eval(
const Eigen::VectorXd &_x)
override;
254 Eigen::Map<Eigen::VectorXd> _grad)
override;
259 std::weak_ptr<HierarchicalIK>
mIK;
275 Constraint(
const std::shared_ptr<HierarchicalIK>& _ik);
282 const std::shared_ptr<HierarchicalIK>& _newIK)
const override;
285 double eval(
const Eigen::VectorXd& _x)
override;
289 Eigen::Map<Eigen::VectorXd> _grad)
override;
294 std::weak_ptr<HierarchicalIK>
mIK;
307 void initialize(
const std::shared_ptr<HierarchicalIK>& my_ptr);
311 void copyOverSetup(
const std::shared_ptr<HierarchicalIK>& _otherIK)
const;
332 std::weak_ptr<HierarchicalIK>
mPtr;
351 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
361 typedef std::unordered_set< std::shared_ptr<InverseKinematics> >
ModuleSet;
362 typedef std::unordered_set< std::shared_ptr<const InverseKinematics> >
ConstModuleSet;
368 std::shared_ptr<HierarchicalIK>
clone(
378 bool addModule(
const std::shared_ptr<InverseKinematics>& _ik);
395 std::unordered_set< std::shared_ptr<InverseKinematics> >
mModuleSet;
409 std::shared_ptr<HierarchicalIK>
clone(
#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