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_INVERSEKINEMATICS_HPP_ 34 #define DART_DYNAMICS_INVERSEKINEMATICS_HPP_ 35 36 #include <functional> 37 #include <memory> 38 39 #include <Eigen/SVD> 40 41 #include "dart/common/Signal.hpp" 42 #include "dart/common/Subject.hpp" 43 #include "dart/common/sub_ptr.hpp" 44 #include "dart/dynamics/JacobianNode.hpp" 45 #include "dart/dynamics/SmartPointer.hpp" 46 #include "dart/math/Geometry.hpp" 47 #include "dart/optimizer/Function.hpp" 48 #include "dart/optimizer/Problem.hpp" 49 #include "dart/optimizer/Solver.hpp" 50 51 namespace dart { 52 namespace dynamics { 53 54 const double DefaultIKTolerance = 1e-6; 55 const double DefaultIKErrorClamp = 1.0; 56 const double DefaultIKGradientComponentClamp = 0.2; 57 const double DefaultIKGradientComponentWeight = 1.0; 58 const double DefaultIKDLSCoefficient = 0.05; 59 const double DefaultIKAngularWeight = 0.4; 60 const double DefaultIKLinearWeight = 1.0; 61 62 /// The InverseKinematics class provides a convenient way of setting up an IK 63 /// optimization problem. It generates constraint functions based on the 64 /// specified InverseKinematics::ErrorMethod and 65 /// InverseKinematics::GradientMethod. The optimization problem is then solved 66 /// by any classes derived from optimizer::Solver class. The default solver is 67 /// optimizer::GradientDescentSolver. 68 /// 69 /// It also provides a convenient way of specifying a configuration space 70 /// objective and a null space objective. It is also possible to fully customize 71 /// the optimizer::Problem that the module creates, and the IK module can be 72 /// safely cloned over to another JacobianNode, as long as every 73 /// optimizer::Function that depends on the JacobianNode inherits the 74 /// InverseKinematics::Function class and correctly overloads the clone function 75 class InverseKinematics : public common::Subject 76 { 77 public: 78 /// Create an InverseKinematics module for a specified node 79 static InverseKinematicsPtr create(JacobianNode* _node); 80 81 /// Copying is not allowed 82 InverseKinematics(const InverseKinematics&) = delete; 83 84 /// Assignment is not allowed 85 InverseKinematics& operator=(const InverseKinematics&) = delete; 86 87 /// Virtual destructor 88 virtual ~InverseKinematics(); 89 90 /// Solve the IK Problem. 91 /// 92 /// The initial guess for the IK optimization problem is the current joint 93 /// positions of the target system. If the iterative solver fails to find a 94 /// successive solution, it attempts more to solve the problem with other seed 95 /// configurations or random configurations if enough seed is not provided. 96 /// 97 /// Here is the pseudocode as described above: 98 /// 99 /// \code 100 /// attempts <- 0 101 /// initial_guess <- current_joint_positions 102 /// while attempts <= max_attempts: 103 /// result <- solve(initial_guess) 104 /// if result = success: 105 /// return 106 /// else: 107 /// attempts <- attempts + 1 108 /// if attempts <= num_seed: 109 /// initial_guess <- seed[attempts - 1] 110 /// else: 111 /// initial_guess <- random_configuration // within the bounds 112 /// \endcode 113 /// 114 /// By default, the max_attempts is 1, but this can be changed by calling 115 /// InverseKinematics::getSolver() and casting the SolverPtr to an 116 /// optimizer::GradientDescentSolver (unless you have changed the Solver type) 117 /// and then calling GradientDescentSolver::setMaxAttempts(std::size_t). 118 /// 119 /// By default, the list of seeds is empty, but they can be added by calling 120 /// InverseKinematics::getProblem() and then using 121 /// Problem::addSeed(Eigen::VectorXd). 122 /// 123 /// By default, the Skeleton itself will retain the solved joint positions. 124 /// If you pass in false for _applySolution, then the joint positions will be 125 /// returned to their original positions after the problem is solved. 126 /// 127 /// Calling this function will automatically call Position::setLowerBounds(~) 128 /// and Position::setUpperBounds(~) with the lower/upper position bounds of 129 /// the corresponding Degrees Of Freedom. Problem::setDimension(~) will be 130 /// taken care of automatically, and Problem::setInitialGuess(~) will be 131 /// called with the current positions of the Degrees Of Freedom. 132 /// 133 /// \deprecated Deprecated in DART 6.8. Please use solveAndApply() instead. 134 DART_DEPRECATED(6.8) 135 bool solve(bool applySolution = true); 136 137 /// Same as solve(bool), but the positions vector will be filled with the 138 /// solved positions. 139 /// 140 /// \deprecated Deprecated in DART 6.8. Please use solveAndApply() or 141 /// findSolution() instead. 142 DART_DEPRECATED(6.8) 143 bool solve(Eigen::VectorXd& positions, bool applySolution = true); 144 145 /// Finds a solution of the IK problem without applying the solution. 146 /// 147 /// The initial guess for the IK optimization problem is the current joint 148 /// positions of the target system. If the iterative solver fails to find a 149 /// successive solution, it attempts more to solve the problem with other seed 150 /// configurations or random configurations if enough seed is not provided. 151 /// 152 /// Here is the pseudocode as described above: 153 /// 154 /// \code 155 /// attempts <- 0 156 /// initial_guess <- current_joint_positions 157 /// while attempts <= max_attempts: 158 /// result <- solve(initial_guess) 159 /// if result = success: 160 /// return 161 /// else: 162 /// attempts <- attempts + 1 163 /// if attempts <= num_seed: 164 /// initial_guess <- seed[attempts - 1] 165 /// else: 166 /// initial_guess <- random_configuration // within the bounds 167 /// \endcode 168 /// 169 /// By default, the max_attempts is 1, but this can be changed by calling 170 /// InverseKinematics::getSolver() and casting the SolverPtr to an 171 /// optimizer::GradientDescentSolver (unless you have changed the Solver type) 172 /// and then calling GradientDescentSolver::setMaxAttempts(std::size_t). 173 /// 174 /// By default, the list of seeds is empty, but they can be added by calling 175 /// InverseKinematics::getProblem() and then using 176 /// Problem::addSeed(Eigen::VectorXd). 177 /// 178 /// Calling this function will automatically call Position::setLowerBounds(~) 179 /// and Position::setUpperBounds(~) with the lower/upper position bounds of 180 /// the corresponding Degrees Of Freedom. Problem::setDimension(~) will be 181 /// taken care of automatically, and Problem::setInitialGuess(~) will be 182 /// called with the current positions of the Degrees Of Freedom. 183 /// 184 /// \param[out] positions The solution of the IK problem. If the solver failed 185 /// to find a solution then it will still set the position with the best 186 /// guess. For example, iterative solvers will fill \c positions with the last 187 /// result of the iterations. 188 /// \return True if a solution is successfully found. 189 /// \sa solveAndApply() 190 bool findSolution(Eigen::VectorXd& positions); 191 192 /// Identical to findSolution(), but this function applies the solution when 193 /// the solver successfully found a solution or \c allowIncompleteResult is 194 /// set to true. 195 /// 196 /// \param[in] allowIncompleteResult Allow to apply the solution even when 197 /// the solver failed to find solution. This option would be useful when an 198 /// iterative solver is used because they will often do a decent job of 199 /// getting a result close to a solution even if it failed to find the 200 /// solution. 201 /// \return True if a solution is successfully found 202 bool solveAndApply(bool allowIncompleteResult = true); 203 204 /// Identical to solveAndApply(bool), but \c position will be filled with the 205 /// solved positions. 206 /// 207 /// \param[out] positions The solution of the IK problem. If the solver failed 208 /// to find a solution then it will still set the position with the best 209 /// guess. For example, iterative solvers will fill \c positions with the last 210 /// result of the iterations. 211 /// \param[in] allowIncompleteResult Allow to apply the solution even when 212 /// the solver failed to find solution. This option would be useful when an 213 /// iterative solver is used because they will often do a decent job of 214 /// getting a result close to a solution even if it failed to find the 215 /// solution. 216 /// \return True if a solution is successfully found 217 bool solveAndApply( 218 Eigen::VectorXd& positions, bool allowIncompleteResult = true); 219 220 /// Clone this IK module, but targeted at a new Node. Any Functions in the 221 /// Problem that inherit InverseKinematics::Function will be adapted to the 222 /// new IK module. Any generic optimizer::Function will just be copied over 223 /// by pointer instead of being cloned. 224 InverseKinematicsPtr clone(JacobianNode* _newNode) const; 225 226 // For the definitions of these classes, see the bottom of this header 227 class Function; 228 229 // Methods for computing IK error 230 class ErrorMethod; 231 class TaskSpaceRegion; 232 233 // Methods for computing IK gradient 234 class GradientMethod; 235 class JacobianDLS; 236 class JacobianTranspose; 237 class Analytical; 238 239 /// If this IK module is set to active, then it will be utilized by any 240 /// HierarchicalIK that has it in its list. If it is set to inactive, then it 241 /// will be ignored by any HierarchicalIK holding onto it, but you can still 242 /// use the solve() function with this. 243 void setActive(bool _active = true); 244 245 /// Equivalent to setActive(false) 246 void setInactive(); 247 248 /// Returns true if this IK module is allowed to be active in a HierarchicalIK 249 bool isActive() const; 250 251 /// Set the hierarchy level of this module. Modules with a larger hierarchy 252 /// value will be projected through the null spaces of all modules with a 253 /// smaller hierarchy value. In other words, IK modules with a hierarchy level 254 /// closer to 0 are given higher priority. 255 void setHierarchyLevel(std::size_t _level); 256 257 /// Get the hierarchy level of this modle. 258 std::size_t getHierarchyLevel() const; 259 260 /// When solving the IK for this module's Node, use the longest available 261 /// dynamics::Chain that goes from this module's Node towards the root of the 262 /// Skeleton. Using this will prevent any other branches in the Skeleton from 263 /// being affected by this IK module. 264 void useChain(); 265 266 /// Use all relevant joints on the Skeleton to solve the IK. 267 void useWholeBody(); 268 269 /// Explicitly set which degrees of freedom should be used to solve the IK for 270 /// this module. 271 template <class DegreeOfFreedomT> 272 void setDofs(const std::vector<DegreeOfFreedomT*>& _dofs); 273 274 /// Explicitly set which degrees of freedom should be used to solve the IK for 275 /// this module. The values in the vector should correspond to the Skeleton 276 /// indices of each DOF. 277 void setDofs(const std::vector<std::size_t>& _dofs); 278 279 /// Get the indices of the DOFs that this IK module will use when solving. 280 const std::vector<std::size_t>& getDofs() const; 281 282 /// When a Jacobian is computed for a JacobianNode, it will include a column 283 /// for every DegreeOfFreedom that the node depends on. Given the column index 284 /// of one of those dependent coordinates, this map will return its location 285 /// in the mDofs vector. A value of -1 means that it is not present in the 286 /// mDofs vector and therefore should not be used when performing inverse 287 /// kinematics. 288 const std::vector<int>& getDofMap() const; 289 290 /// Set an objective function that should be minimized while satisfying the 291 /// inverse kinematics constraint. Pass in a nullptr to remove the objective 292 /// and make it a constant-zero function. 293 void setObjective(const std::shared_ptr<optimizer::Function>& _objective); 294 295 /// Get the objective function for this IK module 296 const std::shared_ptr<optimizer::Function>& getObjective(); 297 298 /// Get the objective function for this IK module 299 std::shared_ptr<const optimizer::Function> getObjective() const; 300 301 /// Set an objective function that should be minimized within the null space 302 /// of the inverse kinematics constraint. The gradient of this function will 303 /// always be projected through the null space of this IK module's Jacobian. 304 /// Pass in a nullptr to remove the null space objective. 305 /// 306 /// Note: The objectives given to setObjective() and setNullSpaceObjective() 307 /// will always be superimposed (added together) via the evalObjective() 308 /// function. 309 void setNullSpaceObjective( 310 const std::shared_ptr<optimizer::Function>& _nsObjective); 311 312 /// Get the null space objective for this IK module 313 const std::shared_ptr<optimizer::Function>& getNullSpaceObjective(); 314 315 /// Get the null space objective for this IK module 316 std::shared_ptr<const optimizer::Function> getNullSpaceObjective() const; 317 318 /// Returns false if the null space objective does nothing 319 bool hasNullSpaceObjective() const; 320 321 /// Set the ErrorMethod for this IK module. You can pass in arguments for the 322 /// constructor, but you should ignore the constructor's first argument. The 323 /// first argument of the ErrorMethod's constructor must be a pointer to this 324 /// IK module, which will be automatically passed by this function. 325 template <class IKErrorMethod, typename... Args> 326 IKErrorMethod& setErrorMethod(Args&&... args); 327 328 /// Get the ErrorMethod for this IK module. Every IK module always has an 329 /// ErrorMethod available, so this is passed by reference. 330 ErrorMethod& getErrorMethod(); 331 332 /// Get the ErrorMethod for this IK module. Every IK module always has an 333 /// ErrorMethod available, so this is passed by reference. 334 const ErrorMethod& getErrorMethod() const; 335 336 /// Set the GradientMethod for this IK module. You can pass in arguments for 337 /// the constructor, but you should ignore the constructor's first argument. 338 /// The first argument of the GradientMethod's constructor must be a pointer 339 /// to this IK module, which will be automatically passed by this function. 340 template <class IKGradientMethod, typename... Args> 341 IKGradientMethod& setGradientMethod(Args&&... args); 342 343 /// Get the GradientMethod for this IK module. Every IK module always has a 344 /// GradientMethod available, so this is passed by reference. 345 GradientMethod& getGradientMethod(); 346 347 /// Get the GradientMethod for this IK module. Every IK module always has a 348 /// GradientMethod available, so this is passed by reference. 349 const GradientMethod& getGradientMethod() const; 350 351 /// Get the Analytical IK method for this module, if one is available. 352 /// Analytical methods are not provided by default. If this IK module does not 353 /// have an analytical method, then this will return a nullptr. 354 Analytical* getAnalytical(); 355 356 /// Get the Analytical IK method for this module, if one is available. 357 /// Analytical methods are not provided by default. If this IK module does not 358 /// have an analytical method, then this will return a nullptr. 359 const Analytical* getAnalytical() const; 360 361 /// Get the Problem that is being maintained by this IK module. 362 const std::shared_ptr<optimizer::Problem>& getProblem(); 363 364 /// Get the Problem that is being maintained by this IK module. 365 std::shared_ptr<const optimizer::Problem> getProblem() const; 366 367 /// Reset the Problem that is being maintained by this IK module. This will 368 /// clear out all Functions from the Problem and then configure the Problem to 369 /// use this IK module's Objective and Constraint functions. 370 /// 371 /// Setting _clearSeeds to true will clear out any seeds that have been loaded 372 /// into the Problem. 373 void resetProblem(bool _clearSeeds = false); 374 375 /// Set the Solver that should be used by this IK module, and set it up with 376 /// the Problem that is configured by this IK module 377 void setSolver(const std::shared_ptr<optimizer::Solver>& _newSolver); 378 379 /// Get the Solver that is being used by this IK module. 380 const std::shared_ptr<optimizer::Solver>& getSolver(); 381 382 /// Get the Solver that is being used by this IK module. 383 std::shared_ptr<const optimizer::Solver> getSolver() const; 384 385 /// Inverse kinematics can be performed on any point within the body frame. 386 /// The default point is the origin of the body frame. Use this function to 387 /// change the point that will be used. _offset must represent the offset of 388 /// the desired point from the body origin, expressed in coordinates of the 389 /// body frame. 390 void setOffset(const Eigen::Vector3d& _offset = Eigen::Vector3d::Zero()); 391 392 /// Get the offset from the origin of the body frame that will be used when 393 /// performing inverse kinematics. The offset will be expressed in the 394 /// coordinates of the body frame. 395 const Eigen::Vector3d& getOffset() const; 396 397 /// This returns false if the offset for the inverse kinematics is a zero 398 /// vector. Otherwise, it returns true. Use setOffset() to set the offset and 399 /// getOffset() to get the offset. 400 bool hasOffset() const; 401 402 /// Set the target for this IK module. 403 /// 404 /// Note that a target will automatically be created for the IK module upon 405 /// instantiation, so you typically do not need to use this function. If you 406 /// want to attach the target to an arbitrary (non-SimpleFrame) reference 407 /// frame, you can do getTarget()->setParentFrame(arbitraryFrame) 408 void setTarget(std::shared_ptr<SimpleFrame> _newTarget); 409 410 /// Get the target that is being used by this IK module. You never have to 411 /// check whether this is a nullptr, because it cannot ever be set to nullptr. 412 std::shared_ptr<SimpleFrame> getTarget(); 413 414 /// Get the target that is being used by this IK module. You never have to 415 /// check whether this is a nullptr, because it cannot ever be set to nullptr. 416 std::shared_ptr<const SimpleFrame> getTarget() const; 417 418 /// Get the JacobianNode that this IK module operates on. 419 JacobianNode* getNode(); 420 421 /// Get the JacobianNode that this IK module operates on. 422 const JacobianNode* getNode() const; 423 424 /// This is the same as getNode(). It is used by the InverseKinematicsPtr to 425 /// provide a common interface for the various IK smart pointer types. 426 JacobianNode* getAffiliation(); 427 428 /// This is the same as getNode(). It is used by the InverseKinematicsPtr to 429 /// provide a common interface for the various IK smart pointer types. 430 const JacobianNode* getAffiliation() const; 431 432 /// Compute the Jacobian for this IK module's node, using the Skeleton's 433 /// current joint positions and the DOFs that have been assigned to the 434 /// module. 435 const math::Jacobian& computeJacobian() const; 436 437 /// Get the current joint positions of the Skeleton. This will only include 438 /// the DOFs that have been assigned to this IK module, and the components of 439 /// the vector will correspond to the components of getDofs(). 440 Eigen::VectorXd getPositions() const; 441 442 /// Set the current joint positions of the Skeleton. This must only include 443 /// the DOFs that have been assigned to this IK module, and the components of 444 /// the vector must correspond to the components of getDofs(). 445 void setPositions(const Eigen::VectorXd& _q); 446 447 /// Clear the caches of this IK module. It should generally not be necessary 448 /// to call this function. However, if you have some non-standard external 449 /// dependency for your error and/or gradient method computations, then you 450 /// will need to tie this function to something that tracks changes in that 451 /// dependency. 452 void clearCaches(); 453 454 protected: 455 // For the definitions of these functions, see the bottom of this header 456 class Objective; 457 friend class Objective; 458 class Constraint; 459 friend class Constraint; 460 461 /// Constructor that accepts a JacobianNode 462 InverseKinematics(JacobianNode* _node); 463 464 /// Gets called during construction 465 void initialize(); 466 467 /// Reset the signal connection for this IK module's target 468 void resetTargetConnection(); 469 470 /// Reset the signal connection for this IK module's Node 471 void resetNodeConnection(); 472 473 /// Connection to the target update 474 common::Connection mTargetConnection; 475 476 /// Connection to the node update 477 common::Connection mNodeConnection; 478 479 /// True if this IK module should be active in its IK hierarcy 480 bool mActive; 481 482 /// Hierarchy level for this IK module 483 std::size_t mHierarchyLevel; 484 485 /// A list of the DegreeOfFreedom Skeleton indices that will be used by this 486 /// IK module 487 std::vector<std::size_t> mDofs; 488 489 /// Map for the DOFs that are to be used by this IK module 490 std::vector<int> mDofMap; 491 492 /// Objective for the IK module 493 std::shared_ptr<optimizer::Function> mObjective; 494 495 /// Null space objective for the IK module 496 std::shared_ptr<optimizer::Function> mNullSpaceObjective; 497 498 /// The method that this IK module will use to compute errors 499 std::unique_ptr<ErrorMethod> mErrorMethod; 500 501 /// The method that this IK module will use to compute gradients 502 std::unique_ptr<GradientMethod> mGradientMethod; 503 504 /// If mGradientMethod is an Analytical extension, then this will have the 505 /// same value is mGradientMethod. Otherwise, this will be a nullptr. 506 /// 507 /// Note that this pointer's memory does not need to be managed, because it is 508 /// always either nullptr or a reference to mGradientMethod. 509 Analytical* mAnalytical; 510 511 /// The Problem that will be maintained by this IK module 512 std::shared_ptr<optimizer::Problem> mProblem; 513 514 /// The solver that this IK module will use for iterative methods 515 std::shared_ptr<optimizer::Solver> mSolver; 516 517 /// The offset that this IK module should use when computing IK 518 Eigen::Vector3d mOffset; 519 520 /// True if the offset is non-zero 521 bool mHasOffset; 522 523 /// Target that this IK module should use 524 std::shared_ptr<SimpleFrame> mTarget; 525 526 /// JacobianNode that this IK module is associated with 527 sub_ptr<JacobianNode> mNode; 528 529 /// Jacobian cache for the IK module 530 mutable math::Jacobian mJacobian; 531 }; 532 533 typedef InverseKinematics IK; 534 535 //============================================================================== 536 /// This class should be inherited by optimizer::Function classes that have a 537 /// dependency on the InverseKinematics module that they belong to. If you 538 /// pass an InverseKinematics::Function into the Problem of an 539 /// InverseKinematics module, then it will be properly cloned whenever the 540 /// InverseKinematics module that it belongs to gets cloned. Any Function 541 /// classes in the Problem that do not inherit InverseKinematics::Function 542 /// will just be copied over by reference. 543 class InverseKinematics::Function 544 { 545 public: 546 /// Enable this function to be cloned to a new IK module. 547 virtual optimizer::FunctionPtr clone(InverseKinematics* _newIK) const = 0; 548 549 /// Virtual destructor 550 virtual ~Function() = default; 551 }; 552 553 //============================================================================== 554 /// ErrorMethod is a base class for different ways of computing the error of 555 /// an InverseKinematics module. 556 class InverseKinematics::ErrorMethod : public common::Subject 557 { 558 public: 559 typedef std::pair<Eigen::Vector6d, Eigen::Vector6d> Bounds; 560 561 /// The Properties struct contains settings that are commonly used by 562 /// methods that compute error for inverse kinematics. 563 struct Properties 564 { 565 /// Default constructor 566 Properties( 567 const Bounds& _bounds = Bounds( 568 Eigen::Vector6d::Constant(-DefaultIKTolerance), 569 Eigen::Vector6d::Constant(DefaultIKTolerance)), 570 571 double _errorClamp = DefaultIKErrorClamp, 572 573 const Eigen::Vector6d& _errorWeights = Eigen::compose( 574 Eigen::Vector3d::Constant(DefaultIKAngularWeight), 575 Eigen::Vector3d::Constant(DefaultIKLinearWeight))); 576 577 /// Bounds that define the acceptable range of the Node's transform 578 /// relative to its target frame. 579 std::pair<Eigen::Vector6d, Eigen::Vector6d> mBounds; 580 581 /// The error vector will be clamped to this length with each iteration. 582 /// This is used to enforce sane behavior, even when there are extremely 583 /// large error vectors. 584 double mErrorLengthClamp; 585 586 /// These weights will be applied to the error vector component-wise. This 587 /// allows you to set some components of error as more important than 588 /// others, or to scale their coordinate spaces. For example, you will 589 /// often want the first three components (orientation error) to have 590 /// smaller weights than the last three components (translation error). 591 Eigen::Vector6d mErrorWeights; 592 593 // To get byte-aligned Eigen vectors 594 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 595 }; 596 597 /// Constructor 598 ErrorMethod( 599 InverseKinematics* _ik, 600 const std::string& _methodName, 601 const Properties& _properties = Properties()); 602 603 /// Virtual destructor 604 virtual ~ErrorMethod() = default; 605 606 /// Enable this ErrorMethod to be cloned to a new IK module. 607 virtual std::unique_ptr<ErrorMethod> clone( 608 InverseKinematics* _newIK) const = 0; 609 610 /// Override this function with your implementation of the error vector 611 /// computation. The expectation is that the first three components of the 612 /// vector will correspond to orientation error (in an angle-axis format) 613 /// while the last three components correspond to translational error. 614 /// 615 /// When implementing this function, you should assume that the Skeleton's 616 /// current joint positions corresponds to the positions that you 617 /// must use to compute the error. This function will only get called when 618 /// an update is needed. 619 virtual Eigen::Vector6d computeError() = 0; 620 621 /// Override this function with your implementation of computing the desired 622 /// given the current transform and error vector. If you want the desired 623 /// transform to always be equal to the Target's transform, you can simply 624 /// call ErrorMethod::computeDesiredTransform to implement this function. 625 virtual Eigen::Isometry3d computeDesiredTransform( 626 const Eigen::Isometry3d& _currentTf, const Eigen::Vector6d& _error) 627 = 0; 628 629 /// This function is used to handle caching the error vector. 630 const Eigen::Vector6d& evalError(const Eigen::VectorXd& _q); 631 632 /// Get the name of this ErrorMethod. 633 const std::string& getMethodName() const; 634 635 /// Set all the error bounds. 636 void setBounds( 637 const Eigen::Vector6d& _lower 638 = Eigen::Vector6d::Constant(-DefaultIKTolerance), 639 const Eigen::Vector6d& _upper 640 = Eigen::Vector6d::Constant(DefaultIKTolerance)); 641 642 /// Set all the error bounds. 643 void setBounds(const std::pair<Eigen::Vector6d, Eigen::Vector6d>& _bounds); 644 645 /// Get all the error bounds. 646 const std::pair<Eigen::Vector6d, Eigen::Vector6d>& getBounds() const; 647 648 /// Set the error bounds for orientation. 649 void setAngularBounds( 650 const Eigen::Vector3d& _lower 651 = Eigen::Vector3d::Constant(-DefaultIKTolerance), 652 const Eigen::Vector3d& _upper 653 = Eigen::Vector3d::Constant(DefaultIKTolerance)); 654 655 /// Set the error bounds for orientation. 656 void setAngularBounds( 657 const std::pair<Eigen::Vector3d, Eigen::Vector3d>& _bounds); 658 659 /// Get the error bounds for orientation. 660 std::pair<Eigen::Vector3d, Eigen::Vector3d> getAngularBounds() const; 661 662 /// Set the error bounds for translation. 663 void setLinearBounds( 664 const Eigen::Vector3d& _lower 665 = Eigen::Vector3d::Constant(-DefaultIKTolerance), 666 const Eigen::Vector3d& _upper 667 = Eigen::Vector3d::Constant(DefaultIKTolerance)); 668 669 /// Set the error bounds for translation. 670 void setLinearBounds( 671 const std::pair<Eigen::Vector3d, Eigen::Vector3d>& _bounds); 672 673 /// Get the error bounds for translation. 674 std::pair<Eigen::Vector3d, Eigen::Vector3d> getLinearBounds() const; 675 676 /// Set the clamp that will be applied to the length of the error vector 677 /// each iteration. 678 void setErrorLengthClamp(double _clampSize = DefaultIKErrorClamp); 679 680 /// Set the clamp that will be applied to the length of the error vector 681 /// each iteration. 682 double getErrorLengthClamp() const; 683 684 /// Set the weights that will be applied to each component of the error 685 /// vector. 686 void setErrorWeights(const Eigen::Vector6d& _weights); 687 688 /// Get the weights that will be applied to each component of the error 689 /// vector. 690 const Eigen::Vector6d& getErrorWeights() const; 691 692 /// Set the weights that will be applied to each angular component of the 693 /// error vector. 694 void setAngularErrorWeights( 695 const Eigen::Vector3d& _weights 696 = Eigen::Vector3d::Constant(DefaultIKAngularWeight)); 697 698 /// Get the weights that will be applied to each angular component of the 699 /// error vector. 700 Eigen::Vector3d getAngularErrorWeights() const; 701 702 /// Set the weights that will be applied to each linear component of the 703 /// error vector. 704 void setLinearErrorWeights( 705 const Eigen::Vector3d& _weights 706 = Eigen::Vector3d::Constant(DefaultIKLinearWeight)); 707 708 /// Get the weights that will be applied to each linear component of the 709 /// error vector. 710 Eigen::Vector3d getLinearErrorWeights() const; 711 712 /// Get the Properties of this ErrorMethod 713 Properties getErrorMethodProperties() const; 714 715 /// Clear the cache to force the error to be recomputed. It should generally 716 /// not be necessary to call this function. 717 void clearCache(); 718 719 protected: 720 /// Pointer to the IK module of this ErrorMethod 721 common::sub_ptr<InverseKinematics> mIK; 722 723 /// Name of this error method 724 std::string mMethodName; 725 726 /// The last joint positions passed into this ErrorMethod 727 Eigen::VectorXd mLastPositions; 728 729 /// The last error vector computed by this ErrorMethod 730 Eigen::Vector6d mLastError; 731 732 /// The properties of this ErrorMethod 733 Properties mErrorP; 734 735 public: 736 // To get byte-aligned Eigen vectors 737 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 738 }; 739 740 //============================================================================== 741 /// The TaskSpaceRegion is a nicely generalized method for computing the error 742 /// of an IK Problem. 743 class InverseKinematics::TaskSpaceRegion : public ErrorMethod 744 { 745 public: 746 struct UniqueProperties 747 { 748 /// Setting this to true (which is default) will tell it to compute the 749 /// error based on the center of the Task Space Region instead of the edge 750 /// of the Task Space Region. This often results in faster convergence, as 751 /// the Node will enter the Task Space Region more aggressively. 752 /// 753 /// Once the Node is inside the Task Space Region, the error vector will 754 /// drop to zero, regardless of whether this flag is true or false. 755 bool mComputeErrorFromCenter; 756 757 /// The reference frame that the task space region is expressed. If this 758 /// frame is set to nullptr, which is the default, then the parent frame of 759 /// the target frame is used instead. 760 SimpleFramePtr mReferenceFrame; 761 762 /// Default constructor 763 UniqueProperties( 764 bool computeErrorFromCenter = true, 765 SimpleFramePtr referenceFrame = nullptr); 766 }; 767 768 struct Properties : ErrorMethod::Properties, UniqueProperties 769 { 770 /// Default constructor 771 Properties( 772 const ErrorMethod::Properties& errorProperties 773 = ErrorMethod::Properties(), 774 const UniqueProperties& taskSpaceProperties = UniqueProperties()); 775 }; 776 777 /// Default Constructor 778 explicit TaskSpaceRegion( 779 InverseKinematics* _ik, const Properties& _properties = Properties()); 780 781 /// Virtual destructor 782 virtual ~TaskSpaceRegion() = default; 783 784 // Documentation inherited 785 std::unique_ptr<ErrorMethod> clone(InverseKinematics* _newIK) const override; 786 787 // Documentation inherited 788 Eigen::Isometry3d computeDesiredTransform( 789 const Eigen::Isometry3d& _currentTf, 790 const Eigen::Vector6d& _error) override; 791 792 // Documentation inherited 793 Eigen::Vector6d computeError() override; 794 795 /// Set whether this TaskSpaceRegion should compute its error vector from 796 /// the center of the region. 797 void setComputeFromCenter(bool computeFromCenter); 798 799 /// Get whether this TaskSpaceRegion is set to compute its error vector from 800 /// the center of the region. 801 bool isComputingFromCenter() const; 802 803 /// Set the reference frame that the task space region is expressed. Pass 804 /// nullptr to use the parent frame of the target frame instead. 805 void setReferenceFrame(SimpleFramePtr referenceFrame); 806 807 /// Get the reference frame that the task space region is expressed. 808 ConstSimpleFramePtr getReferenceFrame() const; 809 810 /// Get the Properties of this TaskSpaceRegion 811 Properties getTaskSpaceRegionProperties() const; 812 813 protected: 814 /// Properties of this TaskSpaceRegion 815 UniqueProperties mTaskSpaceP; 816 }; 817 818 //============================================================================== 819 /// GradientMethod is a base class for different ways of computing the 820 /// gradient of an InverseKinematics module. 821 class InverseKinematics::GradientMethod : public common::Subject 822 { 823 public: 824 struct Properties 825 { 826 /// The component-wise clamp for this GradientMethod 827 double mComponentWiseClamp; 828 829 /// The weights for this GradientMethod 830 Eigen::VectorXd mComponentWeights; 831 832 /// Default constructor 833 Properties( 834 double clamp = DefaultIKGradientComponentClamp, 835 const Eigen::VectorXd& weights = Eigen::VectorXd()); 836 }; 837 838 /// Constructor 839 GradientMethod( 840 InverseKinematics* _ik, 841 const std::string& _methodName, 842 const Properties& _properties); 843 844 /// Virtual destructor 845 virtual ~GradientMethod() = default; 846 847 /// Enable this GradientMethod to be cloned to a new IK module 848 virtual std::unique_ptr<GradientMethod> clone( 849 InverseKinematics* _newIK) const = 0; 850 851 /// Override this function with your implementation of the gradient 852 /// computation. The direction that this gradient points in should make the 853 /// error **worse** if applied to the joint positions, because the 854 /// Problem is configured as a gradient **descent** error minimization 855 /// Problem. 856 /// 857 /// The error vector that is passed in will be determined by the IK module's 858 /// ErrorMethod. The expectation is that the first three components of the 859 /// vector correspond to orientation error (in an angle-axis format) while 860 /// the last three components correspond to translational error. 861 /// 862 /// When implementing this function, you should assume that the Skeleton's 863 /// current joint positions corresponds to the positions that you 864 /// must use to compute the error. This function will only get called when 865 /// an update is needed. 866 virtual void computeGradient( 867 const Eigen::Vector6d& _error, Eigen::VectorXd& _grad) 868 = 0; 869 870 /// This function is used to handle caching the gradient vector and 871 /// interfacing with the solver. 872 void evalGradient( 873 const Eigen::VectorXd& _q, Eigen::Map<Eigen::VectorXd> _grad); 874 875 /// Get the name of this GradientMethod. 876 const std::string& getMethodName() const; 877 878 /// Clamp the gradient based on the clamp settings of this GradientMethod. 879 void clampGradient(Eigen::VectorXd& _grad) const; 880 881 /// Set the component-wise clamp for this GradientMethod. Each component 882 /// of the gradient will be individually clamped to this size. 883 void setComponentWiseClamp(double _clamp = DefaultIKGradientComponentClamp); 884 885 /// Get the component-wise clamp for this GradientMethod. 886 double getComponentWiseClamp() const; 887 888 /// Apply weights to the gradient based on the weight settings of this 889 /// GradientMethod. 890 void applyWeights(Eigen::VectorXd& _grad) const; 891 892 /// Set the weights that will be applied to each component of the gradient. 893 /// If the number of components in _weights is smaller than the number of 894 /// components in the gradient, then a weight of 1.0 will be applied to all 895 /// components that are out of the range of _weights. Passing in an empty 896 /// vector for _weights will effectively make all the gradient components 897 /// unweighted. 898 void setComponentWeights(const Eigen::VectorXd& _weights); 899 900 /// Get the weights of this GradientMethod. 901 const Eigen::VectorXd& getComponentWeights() const; 902 903 /// Convert the gradient that gets generated by Jacobian methods into a 904 /// gradient that can be used by a GradientDescentSolver. 905 /// 906 /// Not all Joint types can be integrated using standard addition (e.g. 907 /// FreeJoint and BallJoint), therefore Jacobian-based differential methods 908 /// will tend to generate gradients that cannot be correctly used by a 909 /// simple addition-based GradientDescentSolver. Running your gradient 910 /// through this function before returning it will make the gradient 911 /// suitable for a standard solver. 912 void convertJacobianMethodOutputToGradient( 913 Eigen::VectorXd& grad, const std::vector<std::size_t>& dofs); 914 915 /// Get the Properties of this GradientMethod 916 Properties getGradientMethodProperties() const; 917 918 /// Clear the cache to force the gradient to be recomputed. It should 919 /// generally not be necessary to call this function. 920 void clearCache(); 921 922 /// Returns the IK module that this GradientMethod belongs to. 923 InverseKinematics* getIK(); 924 925 /// Returns the IK module that this GradientMethod belongs to. 926 const InverseKinematics* getIK() const; 927 928 protected: 929 /// The IK module that this GradientMethod belongs to. 930 common::sub_ptr<InverseKinematics> mIK; 931 932 /// The name of this method 933 std::string mMethodName; 934 935 /// The last positions that was passed to this GradientMethod 936 Eigen::VectorXd mLastPositions; 937 938 /// The last gradient that was computed by this GradientMethod 939 Eigen::VectorXd mLastGradient; 940 941 /// Properties for this GradientMethod 942 Properties mGradientP; 943 944 private: 945 /// Cache used by convertJacobianMethodOutputToGradient to avoid 946 /// reallocating this vector on each iteration. 947 Eigen::VectorXd mInitialPositionsCache; 948 }; 949 950 //============================================================================== 951 /// JacobianDLS refers to the Damped Least Squares Jacobian Pseudoinverse 952 /// (specifically, Moore-Penrose Pseudoinverse). This is a very precise method 953 /// for computing the gradient and is especially suitable for performing IK on 954 /// industrial manipulators that need to follow very exact workspace paths. 955 /// However, it is vulnerable to be jittery around singularities (though the 956 /// damping helps with this), and each cycle might take more time to compute 957 /// than the JacobianTranspose method (although the JacobianDLS method will 958 /// usually converge in fewer cycles than JacobianTranspose). 959 class InverseKinematics::JacobianDLS : public GradientMethod 960 { 961 public: 962 struct UniqueProperties 963 { 964 /// Damping coefficient 965 double mDamping; 966 967 /// Default constructor 968 UniqueProperties(double damping = DefaultIKDLSCoefficient); 969 }; 970 971 struct Properties : GradientMethod::Properties, UniqueProperties 972 { 973 /// Default constructor 974 Properties( 975 const GradientMethod::Properties& gradientProperties 976 = GradientMethod::Properties(), 977 const UniqueProperties& dlsProperties = UniqueProperties()); 978 }; 979 980 /// Constructor 981 explicit JacobianDLS( 982 InverseKinematics* _ik, const Properties& properties = Properties()); 983 984 /// Virtual destructor 985 virtual ~JacobianDLS() = default; 986 987 // Documentation inherited 988 std::unique_ptr<GradientMethod> clone( 989 InverseKinematics* _newIK) const override; 990 991 // Documentation inherited 992 void computeGradient( 993 const Eigen::Vector6d& _error, Eigen::VectorXd& _grad) override; 994 995 /// Set the damping coefficient. A higher damping coefficient will smooth 996 /// out behavior around singularities but will also result in less precision 997 /// in general. The default value is appropriate for most use cases. 998 void setDampingCoefficient(double _damping = DefaultIKDLSCoefficient); 999 1000 /// Get the damping coefficient. 1001 double getDampingCoefficient() const; 1002 1003 /// Get the Properties of this JacobianDLS 1004 Properties getJacobianDLSProperties() const; 1005 1006 protected: 1007 /// Properties of this Damped Least Squares method 1008 UniqueProperties mDLSProperties; 1009 }; 1010 1011 //============================================================================== 1012 /// JacobianTranspose will simply apply the transpose of the Jacobian to the 1013 /// error vector in order to compute the gradient. This method tends to be 1014 /// very smooth but imprecise, requiring more iterations before converging 1015 /// and being less precise in general. This method is suitable for animations 1016 /// where smoothness is prefered over precision. 1017 class InverseKinematics::JacobianTranspose : public GradientMethod 1018 { 1019 public: 1020 /// Constructor 1021 explicit JacobianTranspose( 1022 InverseKinematics* _ik, const Properties& properties = Properties()); 1023 1024 /// Virtual destructor 1025 virtual ~JacobianTranspose() = default; 1026 1027 // Documentation inherited 1028 std::unique_ptr<GradientMethod> clone( 1029 InverseKinematics* _newIK) const override; 1030 1031 // Documentation inherited 1032 void computeGradient( 1033 const Eigen::Vector6d& _error, Eigen::VectorXd& _grad) override; 1034 }; 1035 1036 //============================================================================== 1037 /// Analytical is a base class that should be inherited by methods that are 1038 /// made to solve the IK analytically instead of iteratively. This provides an 1039 /// extended API that is relevant to Analytical solvers but not iterative 1040 /// solvers. 1041 /// 1042 /// Creating an Analytical solver will have the side effect of removing the 1043 /// error clamp and error weights from your ErrorMethod. If you still want 1044 /// your error computations to be clamped and weighted, you should set it 1045 /// again after creating the Analytical solver. Clamping and weighting the 1046 /// error vector often helps iterative methods to converge smoothly, but it is 1047 /// counter-productive for analytical methods which do not typically rely on 1048 /// convergence; analytical methods can usually solve the entire error vector 1049 /// directly. 1050 class InverseKinematics::Analytical : public GradientMethod 1051 { 1052 public: 1053 /// Bitwise enumerations that are used to describe some properties of each 1054 /// solution produced by the analytical IK. 1055 enum Validity_t 1056 { 1057 VALID = 0, ///< The solution is completely valid and reaches the target 1058 OUT_OF_REACH = 1 << 0, ///< The solution does not reach the target 1059 LIMIT_VIOLATED = 1 << 1 ///< The solution has one or more joint positions 1060 ///< that violate the joint limits 1061 }; 1062 // TODO(JS): Change to enum class? 1063 1064 /// If there are extra DOFs in the IK module which your Analytical solver 1065 /// implementation does not make use of, those DOFs can be used to 1066 /// supplement the analytical solver using Jacobian transpose iteration. 1067 /// This enumeration is used to indicate whether you want those DOFs to be 1068 /// used before applying the analytical solution, after applying the 1069 /// analytical solution, or not be used at all. 1070 /// 1071 /// Jacobian transpose is used for the extra DOFs because it is inexpensive 1072 /// and robust to degenerate Jacobians which are common in low dimensional 1073 /// joint spaces. The primary advantage of pseudoinverse methods over 1074 /// Jacobian transpose methods is their precision, but analytical methods 1075 /// are even more precise than pseudoinverse methods, so that precision is 1076 /// not needed in this case. 1077 /// 1078 /// If you want the extra DOFs to use a different method than Jacobian 1079 /// transpose, you can create two seperate IK modules (one which is 1080 /// analytical and one with the iterative method of your choice) and combine 1081 /// them in a HierarchicalIK. 1082 enum ExtraDofUtilization 1083 { 1084 UNUSED = 0, 1085 PRE_ANALYTICAL, 1086 POST_ANALYTICAL, 1087 PRE_AND_POST_ANALYTICAL 1088 }; 1089 // TODO(JS): Change to enum class? 1090 1091 struct Solution 1092 { 1093 /// Default constructor 1094 Solution( 1095 const Eigen::VectorXd& _config = Eigen::VectorXd(), 1096 int _validity = VALID); 1097 1098 /// Configuration computed by the Analytical solver 1099 Eigen::VectorXd mConfig; 1100 1101 /// Bitmap for whether this configuration is valid. Bitwise-compare it to 1102 /// the enumerations in Validity_t to whether this configuration is valid. 1103 int mValidity; 1104 }; 1105 1106 // std::function template for comparing the quality of configurations 1107 typedef std::function<bool( 1108 const Eigen::VectorXd& _better, 1109 const Eigen::VectorXd& _worse, 1110 const InverseKinematics* _ik)> 1111 QualityComparison; 1112 1113 struct UniqueProperties 1114 { 1115 /// Flag for how to use the extra DOFs in the IK module. 1116 ExtraDofUtilization mExtraDofUtilization; 1117 1118 /// How much to clamp the extra error that gets applied to DOFs 1119 double mExtraErrorLengthClamp; 1120 1121 /// Function for comparing the qualities of solutions 1122 QualityComparison mQualityComparator; 1123 1124 /// Default constructor. Uses a default quality comparison function. 1125 UniqueProperties( 1126 ExtraDofUtilization extraDofUtilization = UNUSED, 1127 double extraErrorLengthClamp = DefaultIKErrorClamp); 1128 1129 /// Constructor that allows you to set the quality comparison function. 1130 UniqueProperties( 1131 ExtraDofUtilization extraDofUtilization, 1132 double extraErrorLengthClamp, 1133 QualityComparison qualityComparator); 1134 1135 /// Reset the quality comparison function to its default behavior. 1136 void resetQualityComparisonFunction(); 1137 }; 1138 1139 struct Properties : GradientMethod::Properties, UniqueProperties 1140 { 1141 // Default constructor 1142 Properties( 1143 const GradientMethod::Properties& gradientProperties 1144 = GradientMethod::Properties(), 1145 const UniqueProperties& analyticalProperties = UniqueProperties()); 1146 1147 // Construct Properties by specifying the UniqueProperties. The 1148 // GradientMethod::Properties components will be set to defaults. 1149 Properties(const UniqueProperties& analyticalProperties); 1150 }; 1151 1152 /// Constructor 1153 Analytical( 1154 InverseKinematics* _ik, 1155 const std::string& _methodName, 1156 const Properties& _properties); 1157 1158 /// Virtual destructor 1159 virtual ~Analytical() = default; 1160 1161 /// Get the solutions for this IK module, along with a tag indicating 1162 /// whether each solution is valid. This function will assume that you want 1163 /// to use the desired transform given by the IK module's current 1164 /// ErrorMethod. 1165 const std::vector<Solution>& getSolutions(); 1166 1167 /// Get the solutions for this IK module, along with a tag indicating 1168 /// whether each solution is valid. This function will compute the 1169 /// configurations using the given desired transform instead of using the 1170 /// IK module's current ErrorMethod. 1171 const std::vector<Solution>& getSolutions( 1172 const Eigen::Isometry3d& _desiredTf); 1173 1174 /// You should not need to override this function. Instead, you should 1175 /// override computeSolutions. 1176 void computeGradient( 1177 const Eigen::Vector6d& _error, Eigen::VectorXd& _grad) override; 1178 1179 /// Use this function to fill the entries of the mSolutions variable. Be 1180 /// sure to clear the mSolutions vector at the start, and to also return the 1181 /// mSolutions vector at the end. Note that you are not expected to evaluate 1182 /// any of the solutions for their quality. However, you should set the 1183 /// Solution::mValidity flag to OUT_OF_REACH for each solution that does not 1184 /// actually reach the desired transform, and you should call 1185 /// checkSolutionJointLimits() and the end of the function, which will set 1186 /// the LIMIT_VIOLATED flags of any configurations that are outside of the 1187 /// position limits. 1188 virtual const std::vector<Solution>& computeSolutions( 1189 const Eigen::Isometry3d& _desiredBodyTf) 1190 = 0; 1191 1192 /// Get a list of the DOFs that will be included in the entries of the 1193 /// solutions returned by getSolutions(). Ideally, this should match up with 1194 /// the DOFs being used by the InverseKinematics module, but this might not 1195 /// always be possible, so this function ensures that solutions can be 1196 /// interpreted correctly. 1197 virtual const std::vector<std::size_t>& getDofs() const = 0; 1198 1199 /// Set the configuration of the DOFs. The components of _config must 1200 /// correspond to the DOFs provided by getDofs(). 1201 void setPositions(const Eigen::VectorXd& _config); 1202 1203 /// Get the configuration of the DOFs. The components of this vector will 1204 /// correspond to the DOFs provided by getDofs(). 1205 Eigen::VectorXd getPositions() const; 1206 1207 /// Set how you want extra DOFs to be utilized by the IK module 1208 void setExtraDofUtilization(ExtraDofUtilization _utilization); 1209 1210 /// Get how extra DOFs are being utilized by the IK module 1211 ExtraDofUtilization getExtraDofUtilization() const; 1212 1213 /// Set how much to clamp the error vector that gets applied to extra DOFs 1214 void setExtraErrorLengthClamp(double _clamp); 1215 1216 /// Get how much we will clamp the error vector that gets applied to extra 1217 /// DOFs 1218 double getExtraErrorLengthClamp() const; 1219 1220 /// Set the function that will be used to compare the qualities of two 1221 /// solutions. This function should return true if the first argument is a 1222 /// better solution than the second argument. 1223 /// 1224 /// By default, it will prefer the solution which has the smallest size for 1225 /// its largest change in joint angle. In other words, for each 1226 /// configuration that it is given, it will compare the largest change in 1227 /// joint angle for each configuration and pick the one that is smallest. 1228 /// 1229 /// Note that outside of this comparison function, the Solutions will be 1230 /// split between which are valid, which are out-of-reach, and which are 1231 /// in violation of joint limits. Valid solutions will always be ranked 1232 /// above invalid solutions, and joint limit violations will always be 1233 /// ranked last. 1234 void setQualityComparisonFunction(const QualityComparison& _func); 1235 1236 /// Reset the quality comparison function to the default method. 1237 void resetQualityComparisonFunction(); 1238 1239 /// Get the Properties for this Analytical class 1240 Properties getAnalyticalProperties() const; 1241 1242 /// Construct a mapping from the DOFs of getDofs() to their indices within 1243 /// the Node's list of dependent DOFs. This will be called immediately after 1244 /// the Analytical is constructed; this one call is sufficient as long as 1245 /// the DOFs of Analytical::getDofs() is not changed. However, if your 1246 /// Analytical is able to change the DOFs that it operates on, then you will 1247 /// need to call this function each time the DOFs have changed. 1248 void constructDofMap(); 1249 1250 protected: 1251 /// This function will compute a gradient which utilizes the extra DOFs 1252 /// that go unused by the Analytical solution and then it will add the 1253 /// components of that gradient to the output parameter: grad. 1254 /// 1255 /// You can override this function to customize how the extra DOFs are used. 1256 /// The default behavior is to use a simple Jacobian Transpose method. 1257 /// 1258 /// The utilization flag will be PRE_ANALYTICAL if the function is being 1259 /// called before the Analytical solution is computed; it will be 1260 /// POST_ANALYTICAL if the function is being called after the Analytical 1261 /// solution is computed. 1262 virtual void addExtraDofGradient( 1263 Eigen::VectorXd& grad, 1264 const Eigen::Vector6d& error, 1265 ExtraDofUtilization utilization); 1266 1267 /// Go through the mSolutions vector and tag entries with LIMIT_VIOLATED if 1268 /// any components of their configuration are outside of their position 1269 /// limits. This will NOT clear the LIMIT_VIOLATED flag from entries of 1270 /// mSolutions which are already tagged with it, even if they do not violate 1271 /// any limits. 1272 void checkSolutionJointLimits(); 1273 1274 /// Vector of solutions 1275 std::vector<Solution> mSolutions; 1276 1277 /// Properties for this Analytical IK solver 1278 UniqueProperties mAnalyticalP; 1279 1280 private: 1281 /// This maps the DOFs provided by getDofs() to their index in the Node's 1282 /// list of dependent DOFs. This map is constructed by constructDofMap(). 1283 std::vector<int> mDofMap; 1284 1285 /// List of extra DOFs in the module which are not covered by the Analytical 1286 /// IK implementation. The index of each DOF is its dependency index in the 1287 /// JacobianNode (i.e. the column it applies to in the Node's Jacobian). 1288 std::vector<std::size_t> mExtraDofs; 1289 1290 /// A cache for the valid solutions. The valid and invalid solution caches 1291 /// are kept separate so that they can each be sorted by quality 1292 /// individually. Valid solutions will always be at the top of mFinalResults 1293 /// even if their quality is scored below the invalid solutions. 1294 std::vector<Solution> mValidSolutionsCache; 1295 1296 /// A cache for the out of reach solutions. 1297 std::vector<Solution> mOutOfReachCache; 1298 1299 /// A cache for solutions that violate joint limits 1300 std::vector<Solution> mLimitViolationCache; 1301 1302 /// A cache for storing the current configuration 1303 Eigen::VectorXd mConfigCache; 1304 1305 /// A cache for storing the current configuration so that it can be restored 1306 /// later. This is different from mConfigCache which will not be used to 1307 /// restore the configuration. 1308 Eigen::VectorXd mRestoreConfigCache; 1309 1310 /// A cache for storing the gradient for the extra DOFs 1311 Eigen::VectorXd mExtraDofGradCache; 1312 }; 1313 1314 //============================================================================== 1315 /// The InverseKinematics::Objective Function is simply used to merge the 1316 /// objective and null space objective functions that are being held by an 1317 /// InverseKinematics module. This class is not meant to be extended or 1318 /// instantiated by a user. Call InverseKinematics::resetProblem() to set 1319 /// the objective of the module's Problem to an InverseKinematics::Objective. 1320 class InverseKinematics::Objective final : public Function, 1321 public optimizer::Function 1322 { 1323 public: 1324 DART_DEFINE_ALIGNED_SHARED_OBJECT_CREATOR(InverseKinematics::Objective) 1325 1326 /// Constructor 1327 Objective(InverseKinematics* _ik); 1328 1329 /// Virtual destructor 1330 virtual ~Objective() = default; 1331 1332 // Documentation inherited 1333 optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override; 1334 1335 // Documentation inherited 1336 double eval(const Eigen::VectorXd& _x) override; 1337 1338 // Documentation inherited 1339 void evalGradient( 1340 const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) override; 1341 1342 protected: 1343 /// Pointer to this Objective's IK module 1344 sub_ptr<InverseKinematics> mIK; 1345 1346 /// Cache for the gradient of the Objective 1347 Eigen::VectorXd mGradCache; 1348 1349 /// Cache for the null space SVD 1350 Eigen::JacobiSVD<math::Jacobian> mSVDCache; 1351 // TODO(JS): Need to define aligned operator new for this? 1352 1353 /// Cache for the null space 1354 Eigen::MatrixXd mNullSpaceCache; 1355 1356 public: 1357 // To get byte-aligned Eigen vectors 1358 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 1359 }; 1360 1361 //============================================================================== 1362 /// The InverseKinematics::Constraint Function is simply meant to be used to 1363 /// merge the ErrorMethod and GradientMethod that are being held by an 1364 /// InverseKinematics module. This class is not meant to be extended or 1365 /// instantiated by a user. Call InverseKinematics::resetProblem() to set the 1366 /// first equality constraint of the module's Problem to an 1367 /// InverseKinematics::Constraint. 1368 class InverseKinematics::Constraint final : public Function, 1369 public optimizer::Function 1370 { 1371 public: 1372 /// Constructor 1373 Constraint(InverseKinematics* _ik); 1374 1375 /// Virtual constructor 1376 virtual ~Constraint() = default; 1377 1378 // Documentation inherited 1379 optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override; 1380 1381 // Documentation inherited 1382 double eval(const Eigen::VectorXd& _x) override; 1383 1384 // Documentation inherited 1385 void evalGradient( 1386 const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) override; 1387 1388 protected: 1389 /// Pointer to this Constraint's IK module 1390 sub_ptr<InverseKinematics> mIK; 1391 }; 1392 1393 } // namespace dynamics 1394 } // namespace dart 1395 1396 #include "dart/dynamics/detail/InverseKinematics.hpp" 1397 1398 #endif // DART_DYNAMICS_INVERSEKINEMATICS_HPP_ 1399