DART  6.10.1
InverseKinematics.hpp
Go to the documentation of this file.
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"
46 #include "dart/math/Geometry.hpp"
50 
51 namespace dart {
52 namespace dynamics {
53 
54 const double DefaultIKTolerance = 1e-6;
55 const double DefaultIKErrorClamp = 1.0;
58 const double DefaultIKDLSCoefficient = 0.05;
59 const double DefaultIKAngularWeight = 0.4;
60 const double DefaultIKLinearWeight = 1.0;
61 
76 {
77 public:
80 
83 
86 
88  virtual ~InverseKinematics();
89 
134  DART_DEPRECATED(6.8)
135  bool solve(bool applySolution = true);
136 
142  DART_DEPRECATED(6.8)
143  bool solve(Eigen::VectorXd& positions, bool applySolution = true);
144 
190  bool findSolution(Eigen::VectorXd& positions);
191 
202  bool solveAndApply(bool allowIncompleteResult = true);
203 
217  bool solveAndApply(
218  Eigen::VectorXd& positions, bool allowIncompleteResult = true);
219 
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 
243  void setActive(bool _active = true);
244 
246  void setInactive();
247 
249  bool isActive() const;
250 
255  void setHierarchyLevel(std::size_t _level);
256 
258  std::size_t getHierarchyLevel() const;
259 
264  void useChain();
265 
267  void useWholeBody();
268 
271  template <class DegreeOfFreedomT>
272  void setDofs(const std::vector<DegreeOfFreedomT*>& _dofs);
273 
277  void setDofs(const std::vector<std::size_t>& _dofs);
278 
280  const std::vector<std::size_t>& getDofs() const;
281 
288  const std::vector<int>& getDofMap() const;
289 
293  void setObjective(const std::shared_ptr<optimizer::Function>& _objective);
294 
296  const std::shared_ptr<optimizer::Function>& getObjective();
297 
299  std::shared_ptr<const optimizer::Function> getObjective() const;
300 
310  const std::shared_ptr<optimizer::Function>& _nsObjective);
311 
313  const std::shared_ptr<optimizer::Function>& getNullSpaceObjective();
314 
316  std::shared_ptr<const optimizer::Function> getNullSpaceObjective() const;
317 
319  bool hasNullSpaceObjective() const;
320 
325  template <class IKErrorMethod, typename... Args>
326  IKErrorMethod& setErrorMethod(Args&&... args);
327 
331 
334  const ErrorMethod& getErrorMethod() const;
335 
340  template <class IKGradientMethod, typename... Args>
341  IKGradientMethod& setGradientMethod(Args&&... args);
342 
346 
349  const GradientMethod& getGradientMethod() const;
350 
355 
359  const Analytical* getAnalytical() const;
360 
362  const std::shared_ptr<optimizer::Problem>& getProblem();
363 
365  std::shared_ptr<const optimizer::Problem> getProblem() const;
366 
373  void resetProblem(bool _clearSeeds = false);
374 
377  void setSolver(const std::shared_ptr<optimizer::Solver>& _newSolver);
378 
380  const std::shared_ptr<optimizer::Solver>& getSolver();
381 
383  std::shared_ptr<const optimizer::Solver> getSolver() const;
384 
390  void setOffset(const Eigen::Vector3d& _offset = Eigen::Vector3d::Zero());
391 
395  const Eigen::Vector3d& getOffset() const;
396 
400  bool hasOffset() const;
401 
408  void setTarget(std::shared_ptr<SimpleFrame> _newTarget);
409 
412  std::shared_ptr<SimpleFrame> getTarget();
413 
416  std::shared_ptr<const SimpleFrame> getTarget() const;
417 
420 
422  const JacobianNode* getNode() const;
423 
427 
430  const JacobianNode* getAffiliation() const;
431 
435  const math::Jacobian& computeJacobian() const;
436 
440  Eigen::VectorXd getPositions() const;
441 
445  void setPositions(const Eigen::VectorXd& _q);
446 
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 
463 
465  void initialize();
466 
468  void resetTargetConnection();
469 
471  void resetNodeConnection();
472 
474  common::Connection mTargetConnection;
475 
477  common::Connection mNodeConnection;
478 
480  bool mActive;
481 
484 
487  std::vector<std::size_t> mDofs;
488 
490  std::vector<int> mDofMap;
491 
493  std::shared_ptr<optimizer::Function> mObjective;
494 
496  std::shared_ptr<optimizer::Function> mNullSpaceObjective;
497 
500 
503 
510 
512  std::shared_ptr<optimizer::Problem> mProblem;
513 
515  std::shared_ptr<optimizer::Solver> mSolver;
516 
518  Eigen::Vector3d mOffset;
519 
522 
524  std::shared_ptr<SimpleFrame> mTarget;
525 
528 
530  mutable math::Jacobian mJacobian;
531 };
532 
534 
535 //==============================================================================
544 {
545 public:
547  virtual optimizer::FunctionPtr clone(InverseKinematics* _newIK) const = 0;
548 
550  virtual ~Function() = default;
551 };
552 
553 //==============================================================================
557 {
558 public:
559  typedef std::pair<Eigen::Vector6d, Eigen::Vector6d> Bounds;
560 
563  struct Properties
564  {
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 
579  std::pair<Eigen::Vector6d, Eigen::Vector6d> mBounds;
580 
585 
592 
593  // To get byte-aligned Eigen vectors
594  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
595  };
596 
598  ErrorMethod(
599  InverseKinematics* _ik,
600  const std::string& _methodName,
601  const Properties& _properties = Properties());
602 
604  virtual ~ErrorMethod() = default;
605 
607  virtual std::unique_ptr<ErrorMethod> clone(
608  InverseKinematics* _newIK) const = 0;
609 
620 
625  virtual Eigen::Isometry3d computeDesiredTransform(
626  const Eigen::Isometry3d& _currentTf, const Eigen::Vector6d& _error)
627  = 0;
628 
630  const Eigen::Vector6d& evalError(const Eigen::VectorXd& _q);
631 
633  const std::string& getMethodName() const;
634 
636  void setBounds(
637  const Eigen::Vector6d& _lower
638  = Eigen::Vector6d::Constant(-DefaultIKTolerance),
639  const Eigen::Vector6d& _upper
640  = Eigen::Vector6d::Constant(DefaultIKTolerance));
641 
643  void setBounds(const std::pair<Eigen::Vector6d, Eigen::Vector6d>& _bounds);
644 
646  const std::pair<Eigen::Vector6d, Eigen::Vector6d>& getBounds() const;
647 
649  void setAngularBounds(
650  const Eigen::Vector3d& _lower
651  = Eigen::Vector3d::Constant(-DefaultIKTolerance),
652  const Eigen::Vector3d& _upper
653  = Eigen::Vector3d::Constant(DefaultIKTolerance));
654 
656  void setAngularBounds(
657  const std::pair<Eigen::Vector3d, Eigen::Vector3d>& _bounds);
658 
660  std::pair<Eigen::Vector3d, Eigen::Vector3d> getAngularBounds() const;
661 
663  void setLinearBounds(
664  const Eigen::Vector3d& _lower
665  = Eigen::Vector3d::Constant(-DefaultIKTolerance),
666  const Eigen::Vector3d& _upper
667  = Eigen::Vector3d::Constant(DefaultIKTolerance));
668 
670  void setLinearBounds(
671  const std::pair<Eigen::Vector3d, Eigen::Vector3d>& _bounds);
672 
674  std::pair<Eigen::Vector3d, Eigen::Vector3d> getLinearBounds() const;
675 
678  void setErrorLengthClamp(double _clampSize = DefaultIKErrorClamp);
679 
682  double getErrorLengthClamp() const;
683 
686  void setErrorWeights(const Eigen::Vector6d& _weights);
687 
690  const Eigen::Vector6d& getErrorWeights() const;
691 
694  void setAngularErrorWeights(
695  const Eigen::Vector3d& _weights
696  = Eigen::Vector3d::Constant(DefaultIKAngularWeight));
697 
700  Eigen::Vector3d getAngularErrorWeights() const;
701 
704  void setLinearErrorWeights(
705  const Eigen::Vector3d& _weights
706  = Eigen::Vector3d::Constant(DefaultIKLinearWeight));
707 
710  Eigen::Vector3d getLinearErrorWeights() const;
711 
713  Properties getErrorMethodProperties() const;
714 
717  void clearCache();
718 
719 protected:
722 
724  std::string mMethodName;
725 
727  Eigen::VectorXd mLastPositions;
728 
731 
734 
735 public:
736  // To get byte-aligned Eigen vectors
737  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
738 };
739 
740 //==============================================================================
744 {
745 public:
747  {
756 
761 
764  bool computeErrorFromCenter = true,
765  SimpleFramePtr referenceFrame = nullptr);
766  };
767 
769  {
771  Properties(
772  const ErrorMethod::Properties& errorProperties
774  const UniqueProperties& taskSpaceProperties = UniqueProperties());
775  };
776 
778  explicit TaskSpaceRegion(
779  InverseKinematics* _ik, const Properties& _properties = Properties());
780 
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 
797  void setComputeFromCenter(bool computeFromCenter);
798 
801  bool isComputingFromCenter() const;
802 
805  void setReferenceFrame(SimpleFramePtr referenceFrame);
806 
808  ConstSimpleFramePtr getReferenceFrame() const;
809 
811  Properties getTaskSpaceRegionProperties() const;
812 
813 protected:
816 };
817 
818 //==============================================================================
822 {
823 public:
824  struct Properties
825  {
828 
830  Eigen::VectorXd mComponentWeights;
831 
833  Properties(
834  double clamp = DefaultIKGradientComponentClamp,
835  const Eigen::VectorXd& weights = Eigen::VectorXd());
836  };
837 
840  InverseKinematics* _ik,
841  const std::string& _methodName,
842  const Properties& _properties);
843 
845  virtual ~GradientMethod() = default;
846 
848  virtual std::unique_ptr<GradientMethod> clone(
849  InverseKinematics* _newIK) const = 0;
850 
866  virtual void computeGradient(
867  const Eigen::Vector6d& _error, Eigen::VectorXd& _grad)
868  = 0;
869 
872  void evalGradient(
873  const Eigen::VectorXd& _q, Eigen::Map<Eigen::VectorXd> _grad);
874 
876  const std::string& getMethodName() const;
877 
879  void clampGradient(Eigen::VectorXd& _grad) const;
880 
883  void setComponentWiseClamp(double _clamp = DefaultIKGradientComponentClamp);
884 
886  double getComponentWiseClamp() const;
887 
890  void applyWeights(Eigen::VectorXd& _grad) const;
891 
898  void setComponentWeights(const Eigen::VectorXd& _weights);
899 
901  const Eigen::VectorXd& getComponentWeights() const;
902 
912  void convertJacobianMethodOutputToGradient(
913  Eigen::VectorXd& grad, const std::vector<std::size_t>& dofs);
914 
916  Properties getGradientMethodProperties() const;
917 
920  void clearCache();
921 
923  InverseKinematics* getIK();
924 
926  const InverseKinematics* getIK() const;
927 
928 protected:
931 
933  std::string mMethodName;
934 
936  Eigen::VectorXd mLastPositions;
937 
939  Eigen::VectorXd mLastGradient;
940 
943 
944 private:
947  Eigen::VectorXd mInitialPositionsCache;
948 };
949 
950 //==============================================================================
960 {
961 public:
963  {
965  double mDamping;
966 
968  UniqueProperties(double damping = DefaultIKDLSCoefficient);
969  };
970 
972  {
974  Properties(
975  const GradientMethod::Properties& gradientProperties
977  const UniqueProperties& dlsProperties = UniqueProperties());
978  };
979 
981  explicit JacobianDLS(
983 
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 
998  void setDampingCoefficient(double _damping = DefaultIKDLSCoefficient);
999 
1001  double getDampingCoefficient() const;
1002 
1004  Properties getJacobianDLSProperties() const;
1005 
1006 protected:
1009 };
1010 
1011 //==============================================================================
1018 {
1019 public:
1021  explicit JacobianTranspose(
1023 
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 //==============================================================================
1051 {
1052 public:
1056  {
1057  VALID = 0,
1058  OUT_OF_REACH = 1 << 0,
1059  LIMIT_VIOLATED = 1 << 1
1061  };
1062  // TODO(JS): Change to enum class?
1063 
1083  {
1084  UNUSED = 0,
1087  PRE_AND_POST_ANALYTICAL
1088  };
1089  // TODO(JS): Change to enum class?
1090 
1091  struct Solution
1092  {
1094  Solution(
1095  const Eigen::VectorXd& _config = Eigen::VectorXd(),
1096  int _validity = VALID);
1097 
1099  Eigen::VectorXd mConfig;
1100 
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)>
1112 
1114  {
1117 
1120 
1123 
1126  ExtraDofUtilization extraDofUtilization = UNUSED,
1127  double extraErrorLengthClamp = DefaultIKErrorClamp);
1128 
1131  ExtraDofUtilization extraDofUtilization,
1132  double extraErrorLengthClamp,
1133  QualityComparison qualityComparator);
1134 
1136  void resetQualityComparisonFunction();
1137  };
1138 
1140  {
1141  // Default constructor
1142  Properties(
1143  const GradientMethod::Properties& gradientProperties
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 
1153  Analytical(
1154  InverseKinematics* _ik,
1155  const std::string& _methodName,
1156  const Properties& _properties);
1157 
1159  virtual ~Analytical() = default;
1160 
1165  const std::vector<Solution>& getSolutions();
1166 
1171  const std::vector<Solution>& getSolutions(
1172  const Eigen::Isometry3d& _desiredTf);
1173 
1176  void computeGradient(
1177  const Eigen::Vector6d& _error, Eigen::VectorXd& _grad) override;
1178 
1188  virtual const std::vector<Solution>& computeSolutions(
1189  const Eigen::Isometry3d& _desiredBodyTf)
1190  = 0;
1191 
1197  virtual const std::vector<std::size_t>& getDofs() const = 0;
1198 
1201  void setPositions(const Eigen::VectorXd& _config);
1202 
1205  Eigen::VectorXd getPositions() const;
1206 
1208  void setExtraDofUtilization(ExtraDofUtilization _utilization);
1209 
1211  ExtraDofUtilization getExtraDofUtilization() const;
1212 
1214  void setExtraErrorLengthClamp(double _clamp);
1215 
1218  double getExtraErrorLengthClamp() const;
1219 
1234  void setQualityComparisonFunction(const QualityComparison& _func);
1235 
1237  void resetQualityComparisonFunction();
1238 
1240  Properties getAnalyticalProperties() const;
1241 
1248  void constructDofMap();
1249 
1250 protected:
1262  virtual void addExtraDofGradient(
1263  Eigen::VectorXd& grad,
1264  const Eigen::Vector6d& error,
1265  ExtraDofUtilization utilization);
1266 
1272  void checkSolutionJointLimits();
1273 
1275  std::vector<Solution> mSolutions;
1276 
1279 
1280 private:
1283  std::vector<int> mDofMap;
1284 
1288  std::vector<std::size_t> mExtraDofs;
1289 
1294  std::vector<Solution> mValidSolutionsCache;
1295 
1297  std::vector<Solution> mOutOfReachCache;
1298 
1300  std::vector<Solution> mLimitViolationCache;
1301 
1303  Eigen::VectorXd mConfigCache;
1304 
1308  Eigen::VectorXd mRestoreConfigCache;
1309 
1311  Eigen::VectorXd mExtraDofGradCache;
1312 };
1313 
1314 //==============================================================================
1321  public optimizer::Function
1322 {
1323 public:
1325 
1326 
1328 
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:
1345 
1347  Eigen::VectorXd mGradCache;
1348 
1350  Eigen::JacobiSVD<math::Jacobian> mSVDCache;
1351  // TODO(JS): Need to define aligned operator new for this?
1352 
1354  Eigen::MatrixXd mNullSpaceCache;
1355 
1356 public:
1357  // To get byte-aligned Eigen vectors
1358  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
1359 };
1360 
1361 //==============================================================================
1369  public optimizer::Function
1370 {
1371 public:
1374 
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:
1391 };
1392 
1393 } // namespace dynamics
1394 } // namespace dart
1395 
1397 
1398 #endif // DART_DYNAMICS_INVERSEKINEMATICS_HPP_
#define DART_DEPRECATED(version)
Definition: Deprecated.hpp:51
#define DART_DEFINE_ALIGNED_SHARED_OBJECT_CREATOR(class_name)
Definition: Memory.hpp:155
BodyPropPtr properties
Definition: SdfParser.cpp:80
The Subject class is a base class for any object that wants to report when it gets destroyed.
Definition: Subject.hpp:58
sub_ptr is a pointer to a Subject.
Definition: sub_ptr.hpp:47
Analytical is a base class that should be inherited by methods that are made to solve the IK analytic...
Definition: InverseKinematics.hpp:1051
Eigen::VectorXd mRestoreConfigCache
A cache for storing the current configuration so that it can be restored later.
Definition: InverseKinematics.hpp:1308
std::vector< Solution > mLimitViolationCache
A cache for solutions that violate joint limits.
Definition: InverseKinematics.hpp:1300
Eigen::VectorXd mExtraDofGradCache
A cache for storing the gradient for the extra DOFs.
Definition: InverseKinematics.hpp:1311
UniqueProperties mAnalyticalP
Properties for this Analytical IK solver.
Definition: InverseKinematics.hpp:1278
std::vector< Solution > mValidSolutionsCache
A cache for the valid solutions.
Definition: InverseKinematics.hpp:1294
Validity_t
Bitwise enumerations that are used to describe some properties of each solution produced by the analy...
Definition: InverseKinematics.hpp:1056
virtual ~Analytical()=default
Virtual destructor.
ExtraDofUtilization
If there are extra DOFs in the IK module which your Analytical solver implementation does not make us...
Definition: InverseKinematics.hpp:1083
@ POST_ANALYTICAL
Definition: InverseKinematics.hpp:1086
@ PRE_ANALYTICAL
Definition: InverseKinematics.hpp:1085
Eigen::VectorXd mConfigCache
A cache for storing the current configuration.
Definition: InverseKinematics.hpp:1303
virtual const std::vector< Solution > & computeSolutions(const Eigen::Isometry3d &_desiredBodyTf)=0
Use this function to fill the entries of the mSolutions variable.
std::vector< Solution > mSolutions
Vector of solutions.
Definition: InverseKinematics.hpp:1275
std::vector< std::size_t > mExtraDofs
List of extra DOFs in the module which are not covered by the Analytical IK implementation.
Definition: InverseKinematics.hpp:1288
std::vector< Solution > mOutOfReachCache
A cache for the out of reach solutions.
Definition: InverseKinematics.hpp:1297
virtual const std::vector< std::size_t > & getDofs() const =0
Get a list of the DOFs that will be included in the entries of the solutions returned by getSolutions...
std::function< bool(const Eigen::VectorXd &_better, const Eigen::VectorXd &_worse, const InverseKinematics *_ik)> QualityComparison
Definition: InverseKinematics.hpp:1111
std::vector< int > mDofMap
This maps the DOFs provided by getDofs() to their index in the Node's list of dependent DOFs.
Definition: InverseKinematics.hpp:1283
The InverseKinematics::Constraint Function is simply meant to be used to merge the ErrorMethod and Gr...
Definition: InverseKinematics.hpp:1370
virtual ~Constraint()=default
Virtual constructor.
sub_ptr< InverseKinematics > mIK
Pointer to this Constraint's IK module.
Definition: InverseKinematics.hpp:1390
ErrorMethod is a base class for different ways of computing the error of an InverseKinematics module.
Definition: InverseKinematics.hpp:557
common::sub_ptr< InverseKinematics > mIK
Pointer to the IK module of this ErrorMethod.
Definition: InverseKinematics.hpp:721
virtual ~ErrorMethod()=default
Virtual destructor.
Properties mErrorP
The properties of this ErrorMethod.
Definition: InverseKinematics.hpp:733
Eigen::Vector6d mLastError
The last error vector computed by this ErrorMethod.
Definition: InverseKinematics.hpp:730
std::pair< Eigen::Vector6d, Eigen::Vector6d > Bounds
Definition: InverseKinematics.hpp:559
virtual std::unique_ptr< ErrorMethod > clone(InverseKinematics *_newIK) const =0
Enable this ErrorMethod to be cloned to a new IK module.
Eigen::VectorXd mLastPositions
The last joint positions passed into this ErrorMethod.
Definition: InverseKinematics.hpp:727
virtual Eigen::Vector6d computeError()=0
Override this function with your implementation of the error vector computation.
std::string mMethodName
Name of this error method.
Definition: InverseKinematics.hpp:724
This class should be inherited by optimizer::Function classes that have a dependency on the InverseKi...
Definition: InverseKinematics.hpp:544
virtual optimizer::FunctionPtr clone(InverseKinematics *_newIK) const =0
Enable this function to be cloned to a new IK module.
virtual ~Function()=default
Virtual destructor.
GradientMethod is a base class for different ways of computing the gradient of an InverseKinematics m...
Definition: InverseKinematics.hpp:822
Eigen::VectorXd mInitialPositionsCache
Cache used by convertJacobianMethodOutputToGradient to avoid reallocating this vector on each iterati...
Definition: InverseKinematics.hpp:947
std::string mMethodName
The name of this method.
Definition: InverseKinematics.hpp:933
virtual std::unique_ptr< GradientMethod > clone(InverseKinematics *_newIK) const =0
Enable this GradientMethod to be cloned to a new IK module.
virtual ~GradientMethod()=default
Virtual destructor.
Eigen::VectorXd mLastPositions
The last positions that was passed to this GradientMethod.
Definition: InverseKinematics.hpp:936
common::sub_ptr< InverseKinematics > mIK
The IK module that this GradientMethod belongs to.
Definition: InverseKinematics.hpp:930
Eigen::VectorXd mLastGradient
The last gradient that was computed by this GradientMethod.
Definition: InverseKinematics.hpp:939
Properties mGradientP
Properties for this GradientMethod.
Definition: InverseKinematics.hpp:942
virtual void computeGradient(const Eigen::Vector6d &_error, Eigen::VectorXd &_grad)=0
Override this function with your implementation of the gradient computation.
JacobianDLS refers to the Damped Least Squares Jacobian Pseudoinverse (specifically,...
Definition: InverseKinematics.hpp:960
UniqueProperties mDLSProperties
Properties of this Damped Least Squares method.
Definition: InverseKinematics.hpp:1008
virtual ~JacobianDLS()=default
Virtual destructor.
JacobianTranspose will simply apply the transpose of the Jacobian to the error vector in order to com...
Definition: InverseKinematics.hpp:1018
virtual ~JacobianTranspose()=default
Virtual destructor.
The InverseKinematics::Objective Function is simply used to merge the objective and null space object...
Definition: InverseKinematics.hpp:1322
Eigen::MatrixXd mNullSpaceCache
Cache for the null space.
Definition: InverseKinematics.hpp:1354
virtual ~Objective()=default
Virtual destructor.
Eigen::VectorXd mGradCache
Cache for the gradient of the Objective.
Definition: InverseKinematics.hpp:1347
sub_ptr< InverseKinematics > mIK
Pointer to this Objective's IK module.
Definition: InverseKinematics.hpp:1344
Eigen::JacobiSVD< math::Jacobian > mSVDCache
Cache for the null space SVD.
Definition: InverseKinematics.hpp:1350
The TaskSpaceRegion is a nicely generalized method for computing the error of an IK Problem.
Definition: InverseKinematics.hpp:744
virtual ~TaskSpaceRegion()=default
Virtual destructor.
UniqueProperties mTaskSpaceP
Properties of this TaskSpaceRegion.
Definition: InverseKinematics.hpp:815
The InverseKinematics class provides a convenient way of setting up an IK optimization problem.
Definition: InverseKinematics.hpp:76
bool findSolution(Eigen::VectorXd &positions)
Finds a solution of the IK problem without applying the solution.
Definition: InverseKinematics.cpp:80
void setActive(bool _active=true)
If this IK module is set to active, then it will be utilized by any HierarchicalIK that has it in its...
Definition: InverseKinematics.cpp:1340
std::shared_ptr< SimpleFrame > getTarget()
Get the target that is being used by this IK module.
Definition: InverseKinematics.cpp:1601
void clearCaches()
Clear the caches of this IK module.
Definition: InverseKinematics.cpp:1679
const std::shared_ptr< optimizer::Solver > & getSolver()
Get the Solver that is being used by this IK module.
Definition: InverseKinematics.cpp:1550
const std::shared_ptr< optimizer::Function > & getNullSpaceObjective()
Get the null space objective for this IK module.
Definition: InverseKinematics.cpp:1456
void resetNodeConnection()
Reset the signal connection for this IK module's Node.
Definition: InverseKinematics.cpp:1851
math::Jacobian mJacobian
Jacobian cache for the IK module.
Definition: InverseKinematics.hpp:530
static InverseKinematicsPtr create(JacobianNode *_node)
Create an InverseKinematics module for a specified node.
Definition: InverseKinematics.cpp:43
void useWholeBody()
Use all relevant joints on the Skeleton to solve the IK.
Definition: InverseKinematics.cpp:1384
void resetProblem(bool _clearSeeds=false)
Reset the Problem that is being maintained by this IK module.
Definition: InverseKinematics.cpp:1524
void setPositions(const Eigen::VectorXd &_q)
Set the current joint positions of the Skeleton.
Definition: InverseKinematics.cpp:1664
const std::vector< std::size_t > & getDofs() const
Get the indices of the DOFs that this IK module will use when solving.
Definition: InverseKinematics.cpp:1416
void useChain()
When solving the IK for this module's Node, use the longest available dynamics::Chain that goes from ...
Definition: InverseKinematics.cpp:1370
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: InverseKinematics.cpp:1539
void initialize()
Gets called during construction.
Definition: InverseKinematics.cpp:1806
bool mHasOffset
True if the offset is non-zero.
Definition: InverseKinematics.hpp:521
IKErrorMethod & setErrorMethod(Args &&... args)
Set the ErrorMethod for this IK module.
Definition: InverseKinematics.hpp:45
sub_ptr< JacobianNode > mNode
JacobianNode that this IK module is associated with.
Definition: InverseKinematics.hpp:527
friend class Objective
Definition: InverseKinematics.hpp:456
const std::shared_ptr< optimizer::Function > & getObjective()
Get the objective function for this IK module.
Definition: InverseKinematics.cpp:1435
InverseKinematics(const InverseKinematics &)=delete
Copying is not allowed.
void resetTargetConnection()
Reset the signal connection for this IK module's target.
Definition: InverseKinematics.cpp:1842
JacobianNode * getAffiliation()
This is the same as getNode().
Definition: InverseKinematics.cpp:1625
const Eigen::Vector3d & getOffset() const
Get the offset from the origin of the body frame that will be used when performing inverse kinematics...
Definition: InverseKinematics.cpp:1574
IKGradientMethod & setGradientMethod(Args &&... args)
Set the GradientMethod for this IK module.
Definition: InverseKinematics.hpp:55
JacobianNode * getNode()
Get the JacobianNode that this IK module operates on.
Definition: InverseKinematics.cpp:1613
virtual ~InverseKinematics()
Virtual destructor.
Definition: InverseKinematics.cpp:50
bool hasOffset() const
This returns false if the offset for the inverse kinematics is a zero vector.
Definition: InverseKinematics.cpp:1580
friend class Constraint
Definition: InverseKinematics.hpp:458
Analytical * getAnalytical()
Get the Analytical IK method for this module, if one is available.
Definition: InverseKinematics.cpp:1500
bool mActive
True if this IK module should be active in its IK hierarcy.
Definition: InverseKinematics.hpp:480
void setTarget(std::shared_ptr< SimpleFrame > _newTarget)
Set the target for this IK module.
Definition: InverseKinematics.cpp:1586
std::vector< int > mDofMap
Map for the DOFs that are to be used by this IK module.
Definition: InverseKinematics.hpp:490
common::Connection mTargetConnection
Connection to the target update.
Definition: InverseKinematics.hpp:474
void setNullSpaceObjective(const std::shared_ptr< optimizer::Function > &_nsObjective)
Set an objective function that should be minimized within the null space of the inverse kinematics co...
Definition: InverseKinematics.cpp:1448
std::vector< std::size_t > mDofs
A list of the DegreeOfFreedom Skeleton indices that will be used by this IK module.
Definition: InverseKinematics.hpp:487
std::shared_ptr< optimizer::Function > mNullSpaceObjective
Null space objective for the IK module.
Definition: InverseKinematics.hpp:496
std::size_t mHierarchyLevel
Hierarchy level for this IK module.
Definition: InverseKinematics.hpp:483
bool solve(bool applySolution=true)
Solve the IK Problem.
Definition: InverseKinematics.cpp:57
std::shared_ptr< optimizer::Function > mObjective
Objective for the IK module.
Definition: InverseKinematics.hpp:493
InverseKinematics & operator=(const InverseKinematics &)=delete
Assignment is not allowed.
bool isActive() const
Returns true if this IK module is allowed to be active in a HierarchicalIK.
Definition: InverseKinematics.cpp:1352
common::Connection mNodeConnection
Connection to the node update.
Definition: InverseKinematics.hpp:477
bool hasNullSpaceObjective() const
Returns false if the null space objective does nothing.
Definition: InverseKinematics.cpp:1469
void setDofs(const std::vector< DegreeOfFreedomT * > &_dofs)
Explicitly set which degrees of freedom should be used to solve the IK for this module.
Definition: InverseKinematics.hpp:70
Eigen::Vector3d mOffset
The offset that this IK module should use when computing IK.
Definition: InverseKinematics.hpp:518
void setOffset(const Eigen::Vector3d &_offset=Eigen::Vector3d::Zero())
Inverse kinematics can be performed on any point within the body frame.
Definition: InverseKinematics.cpp:1562
const std::shared_ptr< optimizer::Problem > & getProblem()
Get the Problem that is being maintained by this IK module.
Definition: InverseKinematics.cpp:1512
Analytical * mAnalytical
If mGradientMethod is an Analytical extension, then this will have the same value is mGradientMethod.
Definition: InverseKinematics.hpp:509
Eigen::VectorXd getPositions() const
Get the current joint positions of the Skeleton.
Definition: InverseKinematics.cpp:1658
std::shared_ptr< optimizer::Solver > mSolver
The solver that this IK module will use for iterative methods.
Definition: InverseKinematics.hpp:515
InverseKinematicsPtr clone(JacobianNode *_newNode) const
Clone this IK module, but targeted at a new Node.
Definition: InverseKinematics.cpp:166
GradientMethod & getGradientMethod()
Get the GradientMethod for this IK module.
Definition: InverseKinematics.cpp:1487
void setInactive()
Equivalent to setActive(false)
Definition: InverseKinematics.cpp:1346
std::shared_ptr< optimizer::Problem > mProblem
The Problem that will be maintained by this IK module.
Definition: InverseKinematics.hpp:512
const math::Jacobian & computeJacobian() const
Compute the Jacobian for this IK module's node, using the Skeleton's current joint positions and the ...
Definition: InverseKinematics.cpp:1637
void setHierarchyLevel(std::size_t _level)
Set the hierarchy level of this module.
Definition: InverseKinematics.cpp:1358
ErrorMethod & getErrorMethod()
Get the ErrorMethod for this IK module.
Definition: InverseKinematics.cpp:1475
std::unique_ptr< ErrorMethod > mErrorMethod
The method that this IK module will use to compute errors.
Definition: InverseKinematics.hpp:499
void setObjective(const std::shared_ptr< optimizer::Function > &_objective)
Set an objective function that should be minimized while satisfying the inverse kinematics constraint...
Definition: InverseKinematics.cpp:1428
std::unique_ptr< GradientMethod > mGradientMethod
The method that this IK module will use to compute gradients.
Definition: InverseKinematics.hpp:502
std::size_t getHierarchyLevel() const
Get the hierarchy level of this modle.
Definition: InverseKinematics.cpp:1364
std::shared_ptr< SimpleFrame > mTarget
Target that this IK module should use.
Definition: InverseKinematics.hpp:524
const std::vector< int > & getDofMap() const
When a Jacobian is computed for a JacobianNode, it will include a column for every DegreeOfFreedom th...
Definition: InverseKinematics.cpp:1422
bool solveAndApply(bool allowIncompleteResult=true)
Identical to findSolution(), but this function applies the solution when the solver successfully foun...
Definition: InverseKinematics.cpp:132
The JacobianNode class serves as a common interface for BodyNodes and EndEffectors to both be used as...
Definition: JacobianNode.hpp:55
The SimpleFrame class offers a user-friendly way of creating arbitrary Frames within the kinematic tr...
Definition: SimpleFrame.hpp:52
TemplateInverseKinematicsPtr is a templated class that enables users to create a reference-counting I...
Definition: InverseKinematicsPtr.hpp:49
Definition: Function.hpp:46
Definition: Random-impl.hpp:92
Matrix< double, 6, 1 > Vector6d
Definition: MathTypes.hpp:49
Vector6d compose(const Eigen::Vector3d &_angular, const Eigen::Vector3d &_linear)
Definition: MathTypes.hpp:52
const double DefaultIKGradientComponentClamp
Definition: InverseKinematics.hpp:56
const double DefaultIKTolerance
Definition: InverseKinematics.hpp:54
const double DefaultIKAngularWeight
Definition: InverseKinematics.hpp:59
const double DefaultIKLinearWeight
Definition: InverseKinematics.hpp:60
const double DefaultIKDLSCoefficient
Definition: InverseKinematics.hpp:58
std::shared_ptr< const SimpleFrame > ConstSimpleFramePtr
Definition: SmartPointer.hpp:53
const double DefaultIKGradientComponentWeight
Definition: InverseKinematics.hpp:57
std::shared_ptr< SimpleFrame > SimpleFramePtr
Definition: SmartPointer.hpp:53
const double DefaultIKErrorClamp
Definition: InverseKinematics.hpp:55
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
Definition: InverseKinematics.hpp:1140
Definition: InverseKinematics.hpp:1092
int mValidity
Bitmap for whether this configuration is valid.
Definition: InverseKinematics.hpp:1103
Eigen::VectorXd mConfig
Configuration computed by the Analytical solver.
Definition: InverseKinematics.hpp:1099
QualityComparison mQualityComparator
Function for comparing the qualities of solutions.
Definition: InverseKinematics.hpp:1122
ExtraDofUtilization mExtraDofUtilization
Flag for how to use the extra DOFs in the IK module.
Definition: InverseKinematics.hpp:1116
double mExtraErrorLengthClamp
How much to clamp the extra error that gets applied to DOFs.
Definition: InverseKinematics.hpp:1119
The Properties struct contains settings that are commonly used by methods that compute error for inve...
Definition: InverseKinematics.hpp:564
Eigen::Vector6d mErrorWeights
These weights will be applied to the error vector component-wise.
Definition: InverseKinematics.hpp:591
std::pair< Eigen::Vector6d, Eigen::Vector6d > mBounds
Bounds that define the acceptable range of the Node's transform relative to its target frame.
Definition: InverseKinematics.hpp:579
double mErrorLengthClamp
The error vector will be clamped to this length with each iteration.
Definition: InverseKinematics.hpp:584
double mComponentWiseClamp
The component-wise clamp for this GradientMethod.
Definition: InverseKinematics.hpp:827
Eigen::VectorXd mComponentWeights
The weights for this GradientMethod.
Definition: InverseKinematics.hpp:830
double mDamping
Damping coefficient.
Definition: InverseKinematics.hpp:965
SimpleFramePtr mReferenceFrame
The reference frame that the task space region is expressed.
Definition: InverseKinematics.hpp:760
bool mComputeErrorFromCenter
Setting this to true (which is default) will tell it to compute the error based on the center of the ...
Definition: InverseKinematics.hpp:755