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 38 #include "dart/dynamics/InverseKinematics.hpp" 39 40 namespace dart { 41 namespace dynamics { 42 43 /// An IKHierarchy is a sorted set of IK modules. The outer vector represents 44 /// the precedence of the IK modules. Modules with the same precedence will have 45 /// their gradients added. Precedence of the modules decreases as the index of 46 /// the outer vector increases. Modules with lower precedence will be projected 47 /// through the null spaces of modules with higher precedence. 48 typedef std::vector<std::vector<std::shared_ptr<InverseKinematics> > > 49 IKHierarchy; 50 51 /// The HierarchicalIK class provides a convenient way of setting up a 52 /// hierarchical inverse kinematics optimization problem which combines several 53 /// InverseKinematics problems into one. InverseKinematics problems with a 54 /// larger hierarchy level will be projected into null spaces of the problems 55 /// that have a smaller hierarchy number. 56 /// 57 /// Note that the HierarchicalIK will only account for the 58 /// InverseKinematics::ErrorMethod and InverseKinematics::GradientMethod that 59 /// the IK modules specify; it will ignore any other constraints or objectives 60 /// put into the IK modules' Problems. Any additional constraints or objectives 61 /// that you want the HierarchicalIK to solve should be put directly into the 62 /// HierarchicalIK's Problem. 63 class HierarchicalIK : public common::Subject 64 { 65 public: 66 /// Virtual destructor 67 virtual ~HierarchicalIK() = default; 68 69 /// Solve the IK Problem. By default, the Skeleton itself will retain the 70 /// solved joint positions. If you pass in false for \c applySolution, then 71 /// the joint positions will be return to their original positions after the 72 /// problem is solved. 73 /// 74 /// \deprecated Deprecated in DART 6.8. Please use solveAndApply() instead. 75 DART_DEPRECATED(6.8) 76 bool solve(bool applySolution = true); 77 78 /// Same as solve(bool), but the positions vector will be filled with the 79 /// solved positions. 80 /// 81 /// \deprecated Deprecated in DART 6.8. Please use solveAndApply() or 82 /// findSolution() instead. 83 DART_DEPRECATED(6.8) 84 bool solve(Eigen::VectorXd& positions, bool applySolution = true); 85 86 /// Finds a solution of the IK problem without applying the solution. 87 /// 88 /// \param[out] positions The solution of the IK problem. If the solver failed 89 /// to find a solution then it will still set the position with the best 90 /// guess. For example, iterative solvers will fill \c position with the last 91 /// result of the iterations. 92 /// \return True if a solution is successfully found. 93 /// \sa solveAndApply() 94 bool findSolution(Eigen::VectorXd& positions); 95 96 /// Identical to findSolution(), but this function applies the solution when 97 /// the solver successfully found a solution or \c allowIncompleteResult is 98 /// set to true. 99 /// 100 /// \param[in] allowIncompleteResult Allow to apply the solution even when 101 /// the solver failed to find solution. This option would be useful when an 102 /// iterative solver is used because they will often do a decent job of 103 /// getting a result close to a solution even if it failed to find the 104 /// solution. 105 /// \return True if a solution is successfully found 106 bool solveAndApply(bool allowIncompleteResult = true); 107 108 /// Identical to solveAndApply(bool), but \c position will be filled with the 109 /// solved positions. 110 /// 111 /// \param[out] positions The solution of the IK problem. If the solver failed 112 /// to find a solution then it will still set the position with the best 113 /// guess. For example, iterative solvers will fill \c positions with the last 114 /// result of the iterations. 115 /// \param[in] allowIncompleteResult Allow to apply the solution even when 116 /// the solver failed to find solution. This option would be useful when an 117 /// iterative solver is used because they will often do a decent job of 118 /// getting a result close to a solution even if it failed to find the 119 /// solution. 120 /// \return True if a solution is successfully found 121 bool solveAndApply( 122 Eigen::VectorXd& positions, bool allowIncompleteResult = true); 123 124 /// Clone this HierarchicalIK module 125 virtual std::shared_ptr<HierarchicalIK> clone( 126 const SkeletonPtr& _newSkel) const = 0; 127 128 /// This class should be inherited by optimizer::Function classes that have a 129 /// dependency on the HierarchicalIK module that they belong to. If you 130 /// pass an HierarchicalIK::Function into the Problem of an 131 /// HierarchicalIK module, then it will be properly cloned whenever the 132 /// HierarchicalIK module that it belongs to gets cloned. Any Function 133 /// classes in the Problem that do not inherit HierarchicalIK::Function 134 /// will just be copied over by reference. 135 class Function 136 { 137 public: 138 /// Enable this function to be cloned to a new IK module. 139 virtual optimizer::FunctionPtr clone( 140 const std::shared_ptr<HierarchicalIK>& _newIK) const = 0; 141 142 /// Virtual destructor 143 virtual ~Function() = default; 144 }; 145 146 /// Set the objective function for this HierarchicalIK. 147 void setObjective(const std::shared_ptr<optimizer::Function>& _objective); 148 149 /// Get the objective function for this HierarchicalIK. 150 const std::shared_ptr<optimizer::Function>& getObjective(); 151 152 /// Get the objective function for this HierarchicalIK. 153 std::shared_ptr<const optimizer::Function> getObjective() const; 154 155 /// Set the null space objective for this HierarchicalIK. 156 void setNullSpaceObjective( 157 const std::shared_ptr<optimizer::Function>& _nsObjective); 158 159 /// Get the null space objective for this HierarchicalIK. 160 const std::shared_ptr<optimizer::Function>& getNullSpaceObjective(); 161 162 /// Get the null space objective for this HierarchicalIK. 163 std::shared_ptr<const optimizer::Function> getNullSpaceObjective() const; 164 165 /// Returns true if this HierarchicalIK has a null space objective. 166 bool hasNullSpaceObjective() const; 167 168 /// Get the Problem that is being maintained by this HierarchicalIK module 169 const std::shared_ptr<optimizer::Problem>& getProblem(); 170 171 /// Get the Problem that is being maintained by this HierarchicalIK module 172 std::shared_ptr<const optimizer::Problem> getProblem() const; 173 174 /// Reset the Problem that is being maintained by this HierarchicalIK module. 175 /// This will clear out all Functions from the Problem and then configure the 176 /// Problem to use this IK module's Objective and Constraint functions. 177 /// 178 /// Setting _clearSeeds to true will clear out any seeds that have been loaded 179 /// into the Problem. 180 void resetProblem(bool _clearSeeds = false); 181 182 /// Set the Solver that should be used by this IK module, and set it up with 183 /// the Problem that is configured by this IK module 184 void setSolver(const std::shared_ptr<optimizer::Solver>& _newSolver); 185 186 /// Get the Solver that is being used by this IK module. 187 const std::shared_ptr<optimizer::Solver>& getSolver(); 188 189 /// Get the Solver that is being used by this IK module. 190 std::shared_ptr<const optimizer::Solver> getSolver() const; 191 192 /// Refresh the IK hierarchy of this IK module 193 virtual void refreshIKHierarchy() = 0; 194 195 /// Get the IK hierarchy of this IK module 196 const IKHierarchy& getIKHierarchy() const; 197 198 /// Compute the null spaces of each level of the hierarchy 199 const std::vector<Eigen::MatrixXd>& computeNullSpaces() const; 200 201 /// Get the current joint positions of the Skeleton associated with this 202 /// IK module. 203 Eigen::VectorXd getPositions() const; 204 205 /// Set the current joint positions of the Skeleton associated with this 206 /// IK module. The vector must include all DOFs in the Skeleton. 207 void setPositions(const Eigen::VectorXd& _q); 208 209 /// Get the Skeleton that this IK module is associated with 210 SkeletonPtr getSkeleton(); 211 212 /// Get the Skeleton that this IK module is associated with 213 ConstSkeletonPtr getSkeleton() const; 214 215 /// This is the same as getSkeleton(). It is used by the HierarchicalIKPtr to 216 /// provide a common interface for the various IK smart pointer types. 217 SkeletonPtr getAffiliation(); 218 219 /// This is the same as getSkeleton(). It is used by the HierarchicalIKPtr to 220 /// provide a common interface for the various IK smart pointer types. 221 ConstSkeletonPtr getAffiliation() const; 222 223 /// Clear the caches of this IK module. It should generally not be necessary 224 /// to call this function. 225 void clearCaches(); 226 227 protected: 228 /// The HierarchicalIK::Objective Function is simply used to merge the 229 /// objective and null space objective functions that are being held by this 230 /// HierarchicalIK module. This class is not meant to be extended or 231 /// instantiated by a user. Call HierarchicalIK::resetProblem() to set 232 /// the objective of the module's Problem to an HierarchicalIK::Objective. 233 class Objective final : public Function, public optimizer::Function 234 { 235 public: 236 /// Constructor 237 Objective(const std::shared_ptr<HierarchicalIK>& _ik); 238 239 /// Virtual destructor 240 virtual ~Objective() = default; 241 242 // Documentation inherited 243 optimizer::FunctionPtr clone( 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: 254 /// Pointer to this Objective's HierarchicalIK module 255 std::weak_ptr<HierarchicalIK> mIK; 256 257 /// Cache for the gradient computation 258 Eigen::VectorXd mGradCache; 259 }; 260 261 /// The HierarchicalIK::Constraint Function is simply used to merge the 262 /// constraints of the InverseKinematics modules that belong to the hierarchy 263 /// of this HierarchicalIK module. This class is not meant to be extended or 264 /// instantiated by a user. Call HierarchicalIK::resetProblem() to set 265 /// the constraint of the module's Problem to an HierarchicalIK::Constraint. 266 class Constraint final : public Function, public optimizer::Function 267 { 268 public: 269 /// Constructor 270 Constraint(const std::shared_ptr<HierarchicalIK>& _ik); 271 272 /// Virtual destructor 273 virtual ~Constraint() = default; 274 275 // Documentation inherited 276 optimizer::FunctionPtr clone( 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: 287 /// Pointer to this Constraint's HierarchicalIK module 288 std::weak_ptr<HierarchicalIK> mIK; 289 290 /// Cache for the gradient of a level 291 Eigen::VectorXd mLevelGradCache; 292 293 /// Cache for temporary gradients 294 Eigen::VectorXd mTempGradCache; 295 }; 296 297 /// Constructor 298 HierarchicalIK(const SkeletonPtr& _skeleton); 299 300 /// Setup the module 301 void initialize(const std::shared_ptr<HierarchicalIK>& my_ptr); 302 303 /// Copy the setup of this HierarchicalIK module into another HierarchicalIK 304 /// module 305 void copyOverSetup(const std::shared_ptr<HierarchicalIK>& _otherIK) const; 306 307 /// Pointer to the Skeleton that this IK is tied to 308 WeakSkeletonPtr mSkeleton; 309 310 /// Cache for the IK hierarcy 311 IKHierarchy mHierarchy; 312 313 /// The Problem that this IK module is maintaining 314 std::shared_ptr<optimizer::Problem> mProblem; 315 316 /// The Solver that this IK module will use 317 std::shared_ptr<optimizer::Solver> mSolver; 318 319 /// The Objective of this IK module 320 optimizer::FunctionPtr mObjective; 321 322 /// The null space Objective of this IK module 323 optimizer::FunctionPtr mNullSpaceObjective; 324 325 /// Weak pointer to self 326 std::weak_ptr<HierarchicalIK> mPtr; 327 328 /// Cache for the last positions 329 mutable Eigen::VectorXd mLastPositions; 330 331 /// Cache for null space computations 332 mutable std::vector<Eigen::MatrixXd> mNullSpaceCache; 333 334 /// Cache for a partial null space computation 335 mutable Eigen::MatrixXd mPartialNullspaceCache; 336 337 /// Cache for the null space SVD 338 mutable Eigen::JacobiSVD<math::Jacobian> mSVDCache; 339 340 /// Cache for Jacobians 341 mutable math::Jacobian mJacCache; 342 343 public: 344 // To get byte-aligned Eigen vectors 345 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 346 }; 347 348 /// The CompositeIK class allows you to specify an arbitrary hierarchy of 349 /// InverseKinematics modules for a single Skeleton. Simply add in each IK 350 /// module that should be used. 351 class CompositeIK : public HierarchicalIK 352 { 353 public: 354 typedef std::unordered_set<std::shared_ptr<InverseKinematics> > ModuleSet; 355 typedef std::unordered_set<std::shared_ptr<const InverseKinematics> > 356 ConstModuleSet; 357 358 /// Create a CompositeIK module 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 365 /// Same as clone(), but passes back a more complete type 366 virtual std::shared_ptr<CompositeIK> cloneCompositeIK( 367 const SkeletonPtr& _newSkel) const; 368 369 /// Add an IK module to this CompositeIK. This function will return true if 370 /// the module belongs to the Skeleton that this CompositeIK is associated 371 /// with, otherwise it will return false. 372 bool addModule(const std::shared_ptr<InverseKinematics>& _ik); 373 374 /// Get the set of modules being used by this CompositeIK 375 const ModuleSet& getModuleSet(); 376 377 /// Get the set of modules being used by this CompositeIK 378 ConstModuleSet getModuleSet() const; 379 380 // Documentation inherited 381 void refreshIKHierarchy() override; 382 383 protected: 384 /// Constructor 385 CompositeIK(const SkeletonPtr& _skel); 386 387 /// The set of modules being used by this CompositeIK 388 std::unordered_set<std::shared_ptr<InverseKinematics> > mModuleSet; 389 }; 390 391 /// The WholeBodyIK class provides an interface for simultaneously solving all 392 /// the IK constraints of all BodyNodes and EndEffectors belonging to a single 393 /// Skeleton. 394 class WholeBodyIK : public HierarchicalIK 395 { 396 public: 397 /// Create a WholeBodyIK 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 404 /// Same as clone(), but produces a more complete type 405 virtual std::shared_ptr<WholeBodyIK> cloneWholeBodyIK( 406 const SkeletonPtr& _newSkel) const; 407 408 // Documentation inherited 409 void refreshIKHierarchy() override; 410 411 protected: 412 /// Constructor 413 WholeBodyIK(const SkeletonPtr& _skel); 414 }; 415 416 } // namespace dynamics 417 } // namespace dart 418 419 #endif // DART_DYNAMICS_HIERARCHICALIK_HPP_ 420