33 #ifndef DART_DYNAMICS_HIERARCHICALIK_HPP_
34 #define DART_DYNAMICS_HIERARCHICALIK_HPP_
36 #include <unordered_set>
48 typedef std::vector<std::vector<std::shared_ptr<InverseKinematics> > >
76 bool
solve(
bool applySolution = true);
84 bool solve(
Eigen::VectorXd& positions,
bool applySolution = true);
122 Eigen::VectorXd& positions,
bool allowIncompleteResult = true);
140 const std::shared_ptr<HierarchicalIK>& _newIK)
const = 0;
147 void setObjective(
const std::shared_ptr<optimizer::Function>& _objective);
150 const std::shared_ptr<optimizer::Function>&
getObjective();
153 std::shared_ptr<const optimizer::Function>
getObjective()
const;
157 const std::shared_ptr<optimizer::Function>& _nsObjective);
169 const std::shared_ptr<optimizer::Problem>&
getProblem();
172 std::shared_ptr<const optimizer::Problem>
getProblem()
const;
184 void setSolver(
const std::shared_ptr<optimizer::Solver>& _newSolver);
187 const std::shared_ptr<optimizer::Solver>&
getSolver();
190 std::shared_ptr<const optimizer::Solver>
getSolver()
const;
237 Objective(
const std::shared_ptr<HierarchicalIK>& _ik);
244 const std::shared_ptr<HierarchicalIK>& _newIK)
const override;
247 double eval(
const Eigen::VectorXd& _x)
override;
251 const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad)
override;
255 std::weak_ptr<HierarchicalIK>
mIK;
270 Constraint(
const std::shared_ptr<HierarchicalIK>& _ik);
277 const std::shared_ptr<HierarchicalIK>& _newIK)
const override;
280 double eval(
const Eigen::VectorXd& _x)
override;
284 const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad)
override;
288 std::weak_ptr<HierarchicalIK>
mIK;
301 void initialize(
const std::shared_ptr<HierarchicalIK>& my_ptr);
305 void copyOverSetup(
const std::shared_ptr<HierarchicalIK>& _otherIK)
const;
326 std::weak_ptr<HierarchicalIK>
mPtr;
345 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
354 typedef std::unordered_set<std::shared_ptr<InverseKinematics> >
ModuleSet;
355 typedef std::unordered_set<std::shared_ptr<const InverseKinematics> >
362 std::shared_ptr<HierarchicalIK>
clone(
372 bool addModule(
const std::shared_ptr<InverseKinematics>& _ik);
388 std::unordered_set<std::shared_ptr<InverseKinematics> >
mModuleSet;
401 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: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