DART  6.7.3
InverseKinematics.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2019, 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 <memory>
37 #include <functional>
38 
39 #include <Eigen/SVD>
40 
41 #include "dart/common/sub_ptr.hpp"
42 #include "dart/common/Signal.hpp"
43 #include "dart/common/Subject.hpp"
44 #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:
78 
81 
84 
87 
89  virtual ~InverseKinematics();
90 
133  bool solve(bool _applySolution = true);
134 
137  bool solve(Eigen::VectorXd& positions, bool _applySolution = true);
138 
143  InverseKinematicsPtr clone(JacobianNode* _newNode) const;
144 
145  // For the definitions of these classes, see the bottom of this header
146  class Function;
147 
148  // Methods for computing IK error
149  class ErrorMethod;
150  class TaskSpaceRegion;
151 
152  // Methods for computing IK gradient
153  class GradientMethod;
154  class JacobianDLS;
155  class JacobianTranspose;
156  class Analytical;
157 
162  void setActive(bool _active=true);
163 
165  void setInactive();
166 
168  bool isActive() const;
169 
174  void setHierarchyLevel(std::size_t _level);
175 
177  std::size_t getHierarchyLevel() const;
178 
183  void useChain();
184 
186  void useWholeBody();
187 
190  template <class DegreeOfFreedomT>
191  void setDofs(const std::vector<DegreeOfFreedomT*>& _dofs);
192 
196  void setDofs(const std::vector<std::size_t>& _dofs);
197 
199  const std::vector<std::size_t>& getDofs() const;
200 
207  const std::vector<int>& getDofMap() const;
208 
212  void setObjective(const std::shared_ptr<optimizer::Function>& _objective);
213 
215  const std::shared_ptr<optimizer::Function>& getObjective();
216 
218  std::shared_ptr<const optimizer::Function> getObjective() const;
219 
229  const std::shared_ptr<optimizer::Function>& _nsObjective);
230 
232  const std::shared_ptr<optimizer::Function>& getNullSpaceObjective();
233 
235  std::shared_ptr<const optimizer::Function> getNullSpaceObjective() const;
236 
238  bool hasNullSpaceObjective() const;
239 
244  template <class IKErrorMethod, typename... Args>
245  IKErrorMethod& setErrorMethod(Args&&... args);
246 
250 
253  const ErrorMethod& getErrorMethod() const;
254 
259  template <class IKGradientMethod, typename... Args>
260  IKGradientMethod& setGradientMethod(Args&&... args);
261 
265 
268  const GradientMethod& getGradientMethod() const;
269 
274 
278  const Analytical* getAnalytical() const;
279 
281  const std::shared_ptr<optimizer::Problem>& getProblem();
282 
284  std::shared_ptr<const optimizer::Problem> getProblem() const;
285 
292  void resetProblem(bool _clearSeeds=false);
293 
296  void setSolver(const std::shared_ptr<optimizer::Solver>& _newSolver);
297 
299  const std::shared_ptr<optimizer::Solver>& getSolver();
300 
302  std::shared_ptr<const optimizer::Solver> getSolver() const;
303 
309  void setOffset(const Eigen::Vector3d& _offset = Eigen::Vector3d::Zero());
310 
314  const Eigen::Vector3d& getOffset() const;
315 
319  bool hasOffset() const;
320 
327  void setTarget(std::shared_ptr<SimpleFrame> _newTarget);
328 
331  std::shared_ptr<SimpleFrame> getTarget();
332 
335  std::shared_ptr<const SimpleFrame> getTarget() const;
336 
339 
341  const JacobianNode* getNode() const;
342 
346 
349  const JacobianNode* getAffiliation() const;
350 
354  const math::Jacobian& computeJacobian() const;
355 
359  Eigen::VectorXd getPositions() const;
360 
364  void setPositions(const Eigen::VectorXd& _q);
365 
371  void clearCaches();
372 
373 protected:
374 
375  // For the definitions of these functions, see the bottom of this header
376  class Objective;
377  friend class Objective;
378  class Constraint;
379  friend class Constraint;
380 
383 
385  void initialize();
386 
388  void resetTargetConnection();
389 
391  void resetNodeConnection();
392 
395 
398 
400  bool mActive;
401 
403  std::size_t mHierarchyLevel;
404 
407  std::vector<std::size_t> mDofs;
408 
410  std::vector<int> mDofMap;
411 
413  std::shared_ptr<optimizer::Function> mObjective;
414 
416  std::shared_ptr<optimizer::Function> mNullSpaceObjective;
417 
419  std::unique_ptr<ErrorMethod> mErrorMethod;
420 
422  std::unique_ptr<GradientMethod> mGradientMethod;
423 
430 
432  std::shared_ptr<optimizer::Problem> mProblem;
433 
435  std::shared_ptr<optimizer::Solver> mSolver;
436 
438  Eigen::Vector3d mOffset;
439 
442 
444  std::shared_ptr<SimpleFrame> mTarget;
445 
448 
451 };
452 
454 
455 //==============================================================================
464 {
465 public:
466 
468  virtual optimizer::FunctionPtr clone(InverseKinematics* _newIK) const = 0;
469 
471  virtual ~Function() = default;
472 };
473 
474 //==============================================================================
478 {
479 public:
480 
481  typedef std::pair<Eigen::Vector6d, Eigen::Vector6d> Bounds;
482 
485  struct Properties
486  {
488  Properties(const Bounds& _bounds =
489  Bounds(Eigen::Vector6d::Constant(-DefaultIKTolerance),
490  Eigen::Vector6d::Constant( DefaultIKTolerance)),
491 
492  double _errorClamp = DefaultIKErrorClamp,
493 
494  const Eigen::Vector6d& _errorWeights = Eigen::compose(
495  Eigen::Vector3d::Constant(DefaultIKAngularWeight),
496  Eigen::Vector3d::Constant(DefaultIKLinearWeight)));
497 
500  std::pair<Eigen::Vector6d, Eigen::Vector6d> mBounds;
501 
506 
513 
514  // To get byte-aligned Eigen vectors
515  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
516  };
517 
520  const std::string& _methodName,
521  const Properties& _properties = Properties());
522 
524  virtual ~ErrorMethod() = default;
525 
527  virtual std::unique_ptr<ErrorMethod> clone(
528  InverseKinematics* _newIK) const = 0;
529 
540 
545  virtual Eigen::Isometry3d computeDesiredTransform(
546  const Eigen::Isometry3d& _currentTf, const Eigen::Vector6d& _error) = 0;
547 
549  const Eigen::Vector6d& evalError(const Eigen::VectorXd& _q);
550 
552  const std::string& getMethodName() const;
553 
555  void setBounds(
556  const Eigen::Vector6d& _lower =
557  Eigen::Vector6d::Constant(-DefaultIKTolerance),
558  const Eigen::Vector6d& _upper =
559  Eigen::Vector6d::Constant( DefaultIKTolerance));
560 
562  void setBounds(const std::pair<Eigen::Vector6d, Eigen::Vector6d>& _bounds);
563 
565  const std::pair<Eigen::Vector6d, Eigen::Vector6d>& getBounds() const;
566 
568  void setAngularBounds(
569  const Eigen::Vector3d& _lower =
570  Eigen::Vector3d::Constant(-DefaultIKTolerance),
571  const Eigen::Vector3d& _upper =
572  Eigen::Vector3d::Constant( DefaultIKTolerance));
573 
575  void setAngularBounds(
576  const std::pair<Eigen::Vector3d, Eigen::Vector3d>& _bounds);
577 
579  std::pair<Eigen::Vector3d, Eigen::Vector3d> getAngularBounds() const;
580 
582  void setLinearBounds(
583  const Eigen::Vector3d& _lower =
584  Eigen::Vector3d::Constant(-DefaultIKTolerance),
585  const Eigen::Vector3d& _upper =
586  Eigen::Vector3d::Constant( DefaultIKTolerance));
587 
589  void setLinearBounds(
590  const std::pair<Eigen::Vector3d, Eigen::Vector3d>& _bounds);
591 
593  std::pair<Eigen::Vector3d, Eigen::Vector3d> getLinearBounds() const;
594 
597  void setErrorLengthClamp(double _clampSize = DefaultIKErrorClamp);
598 
601  double getErrorLengthClamp() const;
602 
605  void setErrorWeights(const Eigen::Vector6d& _weights);
606 
609  const Eigen::Vector6d& getErrorWeights() const;
610 
614  const Eigen::Vector3d& _weights =
615  Eigen::Vector3d::Constant(DefaultIKAngularWeight));
616 
619  Eigen::Vector3d getAngularErrorWeights() const;
620 
624  const Eigen::Vector3d& _weights =
625  Eigen::Vector3d::Constant(DefaultIKLinearWeight));
626 
629  Eigen::Vector3d getLinearErrorWeights() const;
630 
633 
636  void clearCache();
637 
638 protected:
639 
642 
644  std::string mMethodName;
645 
647  Eigen::VectorXd mLastPositions;
648 
651 
654 
655 public:
656  // To get byte-aligned Eigen vectors
657  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
658 
659 };
660 
661 //==============================================================================
665 {
666 public:
667 
669  {
678 
680  UniqueProperties(bool computeErrorFromCenter = true);
681  };
682 
684  {
686  Properties(
687  const ErrorMethod::Properties& errorProperties =
689  const UniqueProperties& taskSpaceProperties = UniqueProperties());
690  };
691 
693  explicit TaskSpaceRegion(InverseKinematics* _ik,
694  const Properties& _properties = Properties());
695 
697  virtual ~TaskSpaceRegion() = default;
698 
699  // Documentation inherited
700  std::unique_ptr<ErrorMethod> clone(InverseKinematics* _newIK) const override;
701 
702  // Documentation inherited
703  Eigen::Isometry3d computeDesiredTransform(
704  const Eigen::Isometry3d& _currentTf,
705  const Eigen::Vector6d& _error) override;
706 
707  // Documentation inherited
708  Eigen::Vector6d computeError() override;
709 
712  void setComputeFromCenter(bool computeFromCenter);
713 
716  bool isComputingFromCenter() const;
717 
720 
721 protected:
722 
725 
726 };
727 
728 //==============================================================================
732 {
733 public:
734 
735  struct Properties
736  {
739 
741  Eigen::VectorXd mComponentWeights;
742 
745  const Eigen::VectorXd& weights = Eigen::VectorXd());
746  };
747 
750  const std::string& _methodName,
751  const Properties& _properties);
752 
754  virtual ~GradientMethod() = default;
755 
757  virtual std::unique_ptr<GradientMethod> clone(
758  InverseKinematics* _newIK) const = 0;
759 
775  virtual void computeGradient(const Eigen::Vector6d& _error,
776  Eigen::VectorXd& _grad) = 0;
777 
780  void evalGradient(const Eigen::VectorXd& _q,
781  Eigen::Map<Eigen::VectorXd> _grad);
782 
784  const std::string& getMethodName() const;
785 
787  void clampGradient(Eigen::VectorXd& _grad) const;
788 
792 
794  double getComponentWiseClamp() const;
795 
798  void applyWeights(Eigen::VectorXd& _grad) const;
799 
806  void setComponentWeights(const Eigen::VectorXd& _weights);
807 
809  const Eigen::VectorXd& getComponentWeights() const;
810 
821  Eigen::VectorXd& grad, const std::vector<std::size_t>& dofs);
822 
825 
828  void clearCache();
829 
832 
834  const InverseKinematics* getIK() const;
835 
836 protected:
837 
840 
842  std::string mMethodName;
843 
845  Eigen::VectorXd mLastPositions;
846 
848  Eigen::VectorXd mLastGradient;
849 
852 
853 private:
854 
857  Eigen::VectorXd mInitialPositionsCache;
858 };
859 
860 //==============================================================================
870 {
871 public:
872 
874  {
876  double mDamping;
877 
879  UniqueProperties(double damping = DefaultIKDLSCoefficient);
880  };
881 
883  {
885  Properties(
886  const GradientMethod::Properties& gradientProperties =
888  const UniqueProperties& dlsProperties = UniqueProperties());
889  };
890 
892  explicit JacobianDLS(InverseKinematics* _ik,
893  const Properties& properties = Properties());
894 
896  virtual ~JacobianDLS() = default;
897 
898  // Documentation inherited
899  std::unique_ptr<GradientMethod> clone(
900  InverseKinematics* _newIK) const override;
901 
902  // Documentation inherited
903  void computeGradient(const Eigen::Vector6d& _error,
904  Eigen::VectorXd& _grad) override;
905 
909  void setDampingCoefficient(double _damping = DefaultIKDLSCoefficient);
910 
912  double getDampingCoefficient() const;
913 
916 
917 protected:
918 
921 
922 };
923 
924 //==============================================================================
931 {
932 public:
933 
935  explicit JacobianTranspose(InverseKinematics* _ik,
936  const Properties& properties = Properties());
937 
939  virtual ~JacobianTranspose() = default;
940 
941  // Documentation inherited
942  std::unique_ptr<GradientMethod> clone(
943  InverseKinematics* _newIK) const override;
944 
945  // Documentation inherited
946  void computeGradient(const Eigen::Vector6d& _error,
947  Eigen::VectorXd& _grad) override;
948 };
949 
950 //==============================================================================
965 {
966 public:
967 
971  {
972  VALID = 0,
973  OUT_OF_REACH = 1 << 0,
974  LIMIT_VIOLATED = 1 << 1
975  };
976  // TODO(JS): Change to enum class?
977 
997  {
998  UNUSED = 0,
1002  };
1003  // TODO(JS): Change to enum class?
1004 
1005  struct Solution
1006  {
1008  Solution(const Eigen::VectorXd& _config = Eigen::VectorXd(),
1009  int _validity = VALID);
1010 
1012  Eigen::VectorXd mConfig;
1013 
1017  };
1018 
1019  // std::function template for comparing the quality of configurations
1020  typedef std::function<bool(
1021  const Eigen::VectorXd& _better,
1022  const Eigen::VectorXd& _worse,
1024 
1026  {
1029 
1032 
1035 
1037  UniqueProperties(ExtraDofUtilization extraDofUtilization = UNUSED,
1038  double extraErrorLengthClamp = DefaultIKErrorClamp);
1039 
1041  UniqueProperties(ExtraDofUtilization extraDofUtilization,
1042  double extraErrorLengthClamp,
1043  QualityComparison qualityComparator);
1044 
1047  };
1048 
1050  {
1051  // Default constructor
1052  Properties(
1053  const GradientMethod::Properties& gradientProperties =
1055  const UniqueProperties& analyticalProperties = UniqueProperties());
1056 
1057  // Construct Properties by specifying the UniqueProperties. The
1058  // GradientMethod::Properties components will be set to defaults.
1059  Properties(const UniqueProperties& analyticalProperties);
1060  };
1061 
1063  Analytical(InverseKinematics* _ik, const std::string& _methodName,
1064  const Properties& _properties);
1065 
1067  virtual ~Analytical() = default;
1068 
1073  const std::vector<Solution>& getSolutions();
1074 
1079  const std::vector<Solution>& getSolutions(
1080  const Eigen::Isometry3d& _desiredTf);
1081 
1084  void computeGradient(const Eigen::Vector6d& _error,
1085  Eigen::VectorXd& _grad) override;
1086 
1096  virtual const std::vector<Solution>& computeSolutions(
1097  const Eigen::Isometry3d& _desiredBodyTf) = 0;
1098 
1104  virtual const std::vector<std::size_t>& getDofs() const = 0;
1105 
1108  void setPositions(const Eigen::VectorXd& _config);
1109 
1112  Eigen::VectorXd getPositions() const;
1113 
1115  void setExtraDofUtilization(ExtraDofUtilization _utilization);
1116 
1119 
1121  void setExtraErrorLengthClamp(double _clamp);
1122 
1125  double getExtraErrorLengthClamp() const;
1126 
1142 
1145 
1148 
1155  void constructDofMap();
1156 
1157 protected:
1158 
1170  virtual void addExtraDofGradient(
1171  Eigen::VectorXd& grad,
1172  const Eigen::Vector6d& error,
1173  ExtraDofUtilization utilization);
1174 
1180  void checkSolutionJointLimits();
1181 
1183  std::vector<Solution> mSolutions;
1184 
1187 
1188 private:
1189 
1192  std::vector<int> mDofMap;
1193 
1197  std::vector<std::size_t> mExtraDofs;
1198 
1203  std::vector<Solution> mValidSolutionsCache;
1204 
1206  std::vector<Solution> mOutOfReachCache;
1207 
1209  std::vector<Solution> mLimitViolationCache;
1210 
1212  Eigen::VectorXd mConfigCache;
1213 
1217  Eigen::VectorXd mRestoreConfigCache;
1218 
1220  Eigen::VectorXd mExtraDofGradCache;
1221 };
1222 
1223 //==============================================================================
1230  public Function, public optimizer::Function
1231 {
1232 public:
1233 
1235 
1236 
1238 
1240  virtual ~Objective() = default;
1241 
1242  // Documentation inherited
1243  optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override;
1244 
1245  // Documentation inherited
1246  double eval(const Eigen::VectorXd& _x) override;
1247 
1248  // Documentation inherited
1249  void evalGradient(const Eigen::VectorXd& _x,
1250  Eigen::Map<Eigen::VectorXd> _grad) override;
1251 
1252 protected:
1253 
1256 
1258  Eigen::VectorXd mGradCache;
1259 
1261  Eigen::JacobiSVD<math::Jacobian> mSVDCache;
1262  // TODO(JS): Need to define aligned operator new for this?
1263 
1265  Eigen::MatrixXd mNullSpaceCache;
1266 
1267 public:
1268  // To get byte-aligned Eigen vectors
1269  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
1270 };
1271 
1272 //==============================================================================
1280  public Function, public optimizer::Function
1281 {
1282 public:
1283 
1286 
1288  virtual ~Constraint() = default;
1289 
1290  // Documentation inherited
1291  optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override;
1292 
1293  // Documentation inherited
1294  double eval(const Eigen::VectorXd& _x) override;
1295 
1296  // Documentation inherited
1297  void evalGradient(const Eigen::VectorXd& _x,
1298  Eigen::Map<Eigen::VectorXd> _grad) override;
1299 
1300 protected:
1301 
1304 };
1305 
1306 } // namespace dynamics
1307 } // namespace dart
1308 
1310 
1311 #endif // DART_DYNAMICS_INVERSEKINEMATICS_HPP_
#define DART_DEFINE_ALIGNED_SHARED_OBJECT_CREATOR(class_name)
Definition: Memory.hpp:148
BodyPropPtr properties
Definition: SdfParser.cpp:80
class Connection
Definition: Signal.hpp:47
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:965
ExtraDofUtilization getExtraDofUtilization() const
Get how extra DOFs are being utilized by the IK module.
Definition: InverseKinematics.cpp:1183
void constructDofMap()
Construct a mapping from the DOFs of getDofs() to their indices within the Node's list of dependent D...
Definition: InverseKinematics.cpp:1221
Eigen::VectorXd mRestoreConfigCache
A cache for storing the current configuration so that it can be restored later.
Definition: InverseKinematics.hpp:1217
std::vector< Solution > mLimitViolationCache
A cache for solutions that violate joint limits.
Definition: InverseKinematics.hpp:1209
Eigen::VectorXd mExtraDofGradCache
A cache for storing the gradient for the extra DOFs.
Definition: InverseKinematics.hpp:1220
UniqueProperties mAnalyticalP
Properties for this Analytical IK solver.
Definition: InverseKinematics.hpp:1186
std::vector< Solution > mValidSolutionsCache
A cache for the valid solutions.
Definition: InverseKinematics.hpp:1203
Validity_t
Bitwise enumerations that are used to describe some properties of each solution produced by the analy...
Definition: InverseKinematics.hpp:971
@ VALID
The solution is completely valid and reaches the target.
Definition: InverseKinematics.hpp:972
@ LIMIT_VIOLATED
The solution has one or more joint positions that violate the joint limits.
Definition: InverseKinematics.hpp:974
@ OUT_OF_REACH
The solution does not reach the target.
Definition: InverseKinematics.hpp:973
void checkSolutionJointLimits()
Go through the mSolutions vector and tag entries with LIMIT_VIOLATED if any components of their confi...
Definition: InverseKinematics.cpp:1272
void setExtraErrorLengthClamp(double _clamp)
Set how much to clamp the error vector that gets applied to extra DOFs.
Definition: InverseKinematics.cpp:1189
Properties getAnalyticalProperties() const
Get the Properties for this Analytical class.
Definition: InverseKinematics.cpp:1215
virtual ~Analytical()=default
Virtual destructor.
const std::vector< Solution > & getSolutions()
Get the solutions for this IK module, along with a tag indicating whether each solution is valid.
Definition: InverseKinematics.cpp:970
ExtraDofUtilization
If there are extra DOFs in the IK module which your Analytical solver implementation does not make us...
Definition: InverseKinematics.hpp:997
@ PRE_AND_POST_ANALYTICAL
Definition: InverseKinematics.hpp:1001
@ UNUSED
Definition: InverseKinematics.hpp:998
@ POST_ANALYTICAL
Definition: InverseKinematics.hpp:1000
@ PRE_ANALYTICAL
Definition: InverseKinematics.hpp:999
Eigen::VectorXd mConfigCache
A cache for storing the current configuration.
Definition: InverseKinematics.hpp:1212
virtual void addExtraDofGradient(Eigen::VectorXd &grad, const Eigen::Vector6d &error, ExtraDofUtilization utilization)
This function will compute a gradient which utilizes the extra DOFs that go unused by the Analytical ...
Definition: InverseKinematics.cpp:1039
void computeGradient(const Eigen::Vector6d &_error, Eigen::VectorXd &_grad) override
You should not need to override this function.
Definition: InverseKinematics.cpp:1083
void setPositions(const Eigen::VectorXd &_config)
Set the configuration of the DOFs.
Definition: InverseKinematics.cpp:1162
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:1183
void setExtraDofUtilization(ExtraDofUtilization _utilization)
Set how you want extra DOFs to be utilized by the IK module.
Definition: InverseKinematics.cpp:1175
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:1197
std::vector< Solution > mOutOfReachCache
A cache for the out of reach solutions.
Definition: InverseKinematics.hpp:1206
Analytical(InverseKinematics *_ik, const std::string &_methodName, const Properties &_properties)
Constructor.
Definition: InverseKinematics.cpp:949
Eigen::VectorXd getPositions() const
Get the configuration of the DOFs.
Definition: InverseKinematics.cpp:1169
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...
double getExtraErrorLengthClamp() const
Get how much we will clamp the error vector that gets applied to extra DOFs.
Definition: InverseKinematics.cpp:1195
void resetQualityComparisonFunction()
Reset the quality comparison function to the default method.
Definition: InverseKinematics.cpp:1208
void setQualityComparisonFunction(const QualityComparison &_func)
Set the function that will be used to compare the qualities of two solutions.
Definition: InverseKinematics.cpp:1201
std::function< bool(const Eigen::VectorXd &_better, const Eigen::VectorXd &_worse, const InverseKinematics *_ik)> QualityComparison
Definition: InverseKinematics.hpp:1023
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:1192
The InverseKinematics::Constraint Function is simply meant to be used to merge the ErrorMethod and Gr...
Definition: InverseKinematics.hpp:1281
optimizer::FunctionPtr clone(InverseKinematics *_newIK) const override
Enable this function to be cloned to a new IK module.
Definition: InverseKinematics.cpp:1713
void evalGradient(const Eigen::VectorXd &_x, Eigen::Map< Eigen::VectorXd > _grad) override
Evaluates and returns the objective function at the point x.
Definition: InverseKinematics.cpp:1734
Constraint(InverseKinematics *_ik)
Constructor.
Definition: InverseKinematics.cpp:1706
virtual ~Constraint()=default
Virtual constructor.
double eval(const Eigen::VectorXd &_x) override
Evaluates and returns the objective function at the point x.
Definition: InverseKinematics.cpp:1720
sub_ptr< InverseKinematics > mIK
Pointer to this Constraint's IK module.
Definition: InverseKinematics.hpp:1303
ErrorMethod is a base class for different ways of computing the error of an InverseKinematics module.
Definition: InverseKinematics.hpp:478
common::sub_ptr< InverseKinematics > mIK
Pointer to the IK module of this ErrorMethod.
Definition: InverseKinematics.hpp:641
virtual ~ErrorMethod()=default
Virtual destructor.
virtual Eigen::Isometry3d computeDesiredTransform(const Eigen::Isometry3d &_currentTf, const Eigen::Vector6d &_error)=0
Override this function with your implementation of computing the desired given the current transform ...
Definition: InverseKinematics.cpp:209
void setLinearBounds(const Eigen::Vector3d &_lower=Eigen::Vector3d::Constant(-DefaultIKTolerance), const Eigen::Vector3d &_upper=Eigen::Vector3d::Constant(DefaultIKTolerance))
Set the error bounds for translation.
Definition: InverseKinematics.cpp:316
Properties mErrorP
The properties of this ErrorMethod.
Definition: InverseKinematics.hpp:653
Eigen::Vector6d mLastError
The last error vector computed by this ErrorMethod.
Definition: InverseKinematics.hpp:650
std::pair< Eigen::Vector6d, Eigen::Vector6d > Bounds
Definition: InverseKinematics.hpp:481
const std::pair< Eigen::Vector6d, Eigen::Vector6d > & getBounds() const
Get all the error bounds.
Definition: InverseKinematics.cpp:285
const std::string & getMethodName() const
Get the name of this ErrorMethod.
Definition: InverseKinematics.cpp:261
std::pair< Eigen::Vector3d, Eigen::Vector3d > getAngularBounds() const
Get the error bounds for orientation.
Definition: InverseKinematics.cpp:308
const Eigen::Vector6d & evalError(const Eigen::VectorXd &_q)
This function is used to handle caching the error vector.
Definition: InverseKinematics.cpp:216
void setErrorWeights(const Eigen::Vector6d &_weights)
Set the weights that will be applied to each component of the error vector.
Definition: InverseKinematics.cpp:354
virtual std::unique_ptr< ErrorMethod > clone(InverseKinematics *_newIK) const =0
Enable this ErrorMethod to be cloned to a new IK module.
void setAngularBounds(const Eigen::Vector3d &_lower=Eigen::Vector3d::Constant(-DefaultIKTolerance), const Eigen::Vector3d &_upper=Eigen::Vector3d::Constant(DefaultIKTolerance))
Set the error bounds for orientation.
Definition: InverseKinematics.cpp:291
void setLinearErrorWeights(const Eigen::Vector3d &_weights=Eigen::Vector3d::Constant(DefaultIKLinearWeight))
Set the weights that will be applied to each linear component of the error vector.
Definition: InverseKinematics.cpp:382
double getErrorLengthClamp() const
Set the clamp that will be applied to the length of the error vector each iteration.
Definition: InverseKinematics.cpp:348
void setAngularErrorWeights(const Eigen::Vector3d &_weights=Eigen::Vector3d::Constant(DefaultIKAngularWeight))
Set the weights that will be applied to each angular component of the error vector.
Definition: InverseKinematics.cpp:368
Eigen::Vector3d getLinearErrorWeights() const
Get the weights that will be applied to each linear component of the error vector.
Definition: InverseKinematics.cpp:390
Eigen::VectorXd mLastPositions
The last joint positions passed into this ErrorMethod.
Definition: InverseKinematics.hpp:647
virtual Eigen::Vector6d computeError()=0
Override this function with your implementation of the error vector computation.
void setBounds(const Eigen::Vector6d &_lower=Eigen::Vector6d::Constant(-DefaultIKTolerance), const Eigen::Vector6d &_upper=Eigen::Vector6d::Constant(DefaultIKTolerance))
Set all the error bounds.
Definition: InverseKinematics.cpp:267
const Eigen::Vector6d & getErrorWeights() const
Get the weights that will be applied to each component of the error vector.
Definition: InverseKinematics.cpp:362
std::string mMethodName
Name of this error method.
Definition: InverseKinematics.hpp:644
std::pair< Eigen::Vector3d, Eigen::Vector3d > getLinearBounds() const
Get the error bounds for translation.
Definition: InverseKinematics.cpp:333
ErrorMethod(InverseKinematics *_ik, const std::string &_methodName, const Properties &_properties=Properties())
Constructor.
Definition: InverseKinematics.cpp:195
Properties getErrorMethodProperties() const
Get the Properties of this ErrorMethod.
Definition: InverseKinematics.cpp:397
void clearCache()
Clear the cache to force the error to be recomputed.
Definition: InverseKinematics.cpp:403
void setErrorLengthClamp(double _clampSize=DefaultIKErrorClamp)
Set the clamp that will be applied to the length of the error vector each iteration.
Definition: InverseKinematics.cpp:341
Eigen::Vector3d getAngularErrorWeights() const
Get the weights that will be applied to each angular component of the error vector.
Definition: InverseKinematics.cpp:376
This class should be inherited by optimizer::Function classes that have a dependency on the InverseKi...
Definition: InverseKinematics.hpp:464
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:732
void clampGradient(Eigen::VectorXd &_grad) const
Clamp the gradient based on the clamp settings of this GradientMethod.
Definition: InverseKinematics.cpp:659
Eigen::VectorXd mInitialPositionsCache
Cache used by convertJacobianMethodOutputToGradient to avoid reallocating this vector on each iterati...
Definition: InverseKinematics.hpp:857
InverseKinematics * getIK()
Returns the IK module that this GradientMethod belongs to.
Definition: InverseKinematics.cpp:748
void setComponentWiseClamp(double _clamp=DefaultIKGradientComponentClamp)
Set the component-wise clamp for this GradientMethod.
Definition: InverseKinematics.cpp:671
std::string mMethodName
The name of this method.
Definition: InverseKinematics.hpp:842
void applyWeights(Eigen::VectorXd &_grad) const
Apply weights to the gradient based on the weight settings of this GradientMethod.
Definition: InverseKinematics.cpp:683
virtual std::unique_ptr< GradientMethod > clone(InverseKinematics *_newIK) const =0
Enable this GradientMethod to be cloned to a new IK module.
const Eigen::VectorXd & getComponentWeights() const
Get the weights of this GradientMethod.
Definition: InverseKinematics.cpp:701
const std::string & getMethodName() const
Get the name of this GradientMethod.
Definition: InverseKinematics.cpp:653
virtual ~GradientMethod()=default
Virtual destructor.
void clearCache()
Clear the cache to force the gradient to be recomputed.
Definition: InverseKinematics.cpp:740
Eigen::VectorXd mLastPositions
The last positions that was passed to this GradientMethod.
Definition: InverseKinematics.hpp:845
void setComponentWeights(const Eigen::VectorXd &_weights)
Set the weights that will be applied to each component of the gradient.
Definition: InverseKinematics.cpp:693
common::sub_ptr< InverseKinematics > mIK
The IK module that this GradientMethod belongs to.
Definition: InverseKinematics.hpp:839
double getComponentWiseClamp() const
Get the component-wise clamp for this GradientMethod.
Definition: InverseKinematics.cpp:677
void convertJacobianMethodOutputToGradient(Eigen::VectorXd &grad, const std::vector< std::size_t > &dofs)
Convert the gradient that gets generated by Jacobian methods into a gradient that can be used by a Gr...
Definition: InverseKinematics.cpp:707
void evalGradient(const Eigen::VectorXd &_q, Eigen::Map< Eigen::VectorXd > _grad)
This function is used to handle caching the gradient vector and interfacing with the solver.
Definition: InverseKinematics.cpp:601
Eigen::VectorXd mLastGradient
The last gradient that was computed by this GradientMethod.
Definition: InverseKinematics.hpp:848
Properties mGradientP
Properties for this GradientMethod.
Definition: InverseKinematics.hpp:851
GradientMethod(InverseKinematics *_ik, const std::string &_methodName, const Properties &_properties)
Constructor.
Definition: InverseKinematics.cpp:591
virtual void computeGradient(const Eigen::Vector6d &_error, Eigen::VectorXd &_grad)=0
Override this function with your implementation of the gradient computation.
Properties getGradientMethodProperties() const
Get the Properties of this GradientMethod.
Definition: InverseKinematics.cpp:734
JacobianDLS refers to the Damped Least Squares Jacobian Pseudoinverse (specifically,...
Definition: InverseKinematics.hpp:870
UniqueProperties mDLSProperties
Properties of this Damped Least Squares method.
Definition: InverseKinematics.hpp:920
std::unique_ptr< GradientMethod > clone(InverseKinematics *_newIK) const override
Enable this GradientMethod to be cloned to a new IK module.
Definition: InverseKinematics.cpp:788
double getDampingCoefficient() const
Get the damping coefficient.
Definition: InverseKinematics.cpp:825
void computeGradient(const Eigen::Vector6d &_error, Eigen::VectorXd &_grad) override
Override this function with your implementation of the gradient computation.
Definition: InverseKinematics.cpp:794
Properties getJacobianDLSProperties() const
Get the Properties of this JacobianDLS.
Definition: InverseKinematics.cpp:832
void setDampingCoefficient(double _damping=DefaultIKDLSCoefficient)
Set the damping coefficient.
Definition: InverseKinematics.cpp:819
virtual ~JacobianDLS()=default
Virtual destructor.
JacobianDLS(InverseKinematics *_ik, const Properties &properties=Properties())
Constructor.
Definition: InverseKinematics.cpp:778
JacobianTranspose will simply apply the transpose of the Jacobian to the error vector in order to com...
Definition: InverseKinematics.hpp:931
JacobianTranspose(InverseKinematics *_ik, const Properties &properties=Properties())
Constructor.
Definition: InverseKinematics.cpp:838
virtual ~JacobianTranspose()=default
Virtual destructor.
void computeGradient(const Eigen::Vector6d &_error, Eigen::VectorXd &_grad) override
Override this function with your implementation of the gradient computation.
Definition: InverseKinematics.cpp:854
std::unique_ptr< GradientMethod > clone(InverseKinematics *_newIK) const override
Enable this GradientMethod to be cloned to a new IK module.
Definition: InverseKinematics.cpp:847
The InverseKinematics::Objective Function is simply used to merge the objective and null space object...
Definition: InverseKinematics.hpp:1231
void evalGradient(const Eigen::VectorXd &_x, Eigen::Map< Eigen::VectorXd > _grad) override
Evaluates and returns the objective function at the point x.
Definition: InverseKinematics.cpp:1674
Eigen::MatrixXd mNullSpaceCache
Cache for the null space.
Definition: InverseKinematics.hpp:1265
virtual ~Objective()=default
Virtual destructor.
double eval(const Eigen::VectorXd &_x) override
Evaluates and returns the objective function at the point x.
Definition: InverseKinematics.cpp:1652
Eigen::VectorXd mGradCache
Cache for the gradient of the Objective.
Definition: InverseKinematics.hpp:1258
sub_ptr< InverseKinematics > mIK
Pointer to this Objective's IK module.
Definition: InverseKinematics.hpp:1255
optimizer::FunctionPtr clone(InverseKinematics *_newIK) const override
Enable this function to be cloned to a new IK module.
Definition: InverseKinematics.cpp:1645
Objective(InverseKinematics *_ik)
Constructor.
Definition: InverseKinematics.cpp:1638
Eigen::JacobiSVD< math::Jacobian > mSVDCache
Cache for the null space SVD.
Definition: InverseKinematics.hpp:1261
The TaskSpaceRegion is a nicely generalized method for computing the error of an IK Problem.
Definition: InverseKinematics.hpp:665
bool isComputingFromCenter() const
Get whether this TaskSpaceRegion is set to compute its error vector from the center of the region.
Definition: InverseKinematics.cpp:569
TaskSpaceRegion(InverseKinematics *_ik, const Properties &_properties=Properties())
Default Constructor.
Definition: InverseKinematics.cpp:429
std::unique_ptr< ErrorMethod > clone(InverseKinematics *_newIK) const override
Enable this ErrorMethod to be cloned to a new IK module.
Definition: InverseKinematics.cpp:439
Properties getTaskSpaceRegionProperties() const
Get the Properties of this TaskSpaceRegion.
Definition: InverseKinematics.cpp:576
Eigen::Vector6d computeError() override
Override this function with your implementation of the error vector computation.
Definition: InverseKinematics.cpp:468
Eigen::Isometry3d computeDesiredTransform(const Eigen::Isometry3d &_currentTf, const Eigen::Vector6d &_error) override
Override this function with your implementation of computing the desired given the current transform ...
Definition: InverseKinematics.cpp:446
virtual ~TaskSpaceRegion()=default
Virtual destructor.
UniqueProperties mTaskSpaceP
Properties of this TaskSpaceRegion.
Definition: InverseKinematics.hpp:724
void setComputeFromCenter(bool computeFromCenter)
Set whether this TaskSpaceRegion should compute its error vector from the center of the region.
Definition: InverseKinematics.cpp:562
The InverseKinematics class provides a convenient way of setting up an IK optimization problem.
Definition: InverseKinematics.hpp:76
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:1294
std::shared_ptr< SimpleFrame > getTarget()
Get the target that is being used by this IK module.
Definition: InverseKinematics.cpp:1554
void clearCaches()
Clear the caches of this IK module.
Definition: InverseKinematics.cpp:1631
const std::shared_ptr< optimizer::Solver > & getSolver()
Get the Solver that is being used by this IK module.
Definition: InverseKinematics.cpp:1504
const std::shared_ptr< optimizer::Function > & getNullSpaceObjective()
Get the null space objective for this IK module.
Definition: InverseKinematics.cpp:1410
void resetNodeConnection()
Reset the signal connection for this IK module's Node.
Definition: InverseKinematics.cpp:1806
math::Jacobian mJacobian
Jacobian cache for the IK module.
Definition: InverseKinematics.hpp:450
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:1338
void resetProblem(bool _clearSeeds=false)
Reset the Problem that is being maintained by this IK module.
Definition: InverseKinematics.cpp:1478
void setPositions(const Eigen::VectorXd &_q)
Set the current joint positions of the Skeleton.
Definition: InverseKinematics.cpp:1616
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:1370
void useChain()
When solving the IK for this module's Node, use the longest available dynamics::Chain that goes from ...
Definition: InverseKinematics.cpp:1324
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:1493
void initialize()
Gets called during construction.
Definition: InverseKinematics.cpp:1760
bool mHasOffset
True if the offset is non-zero.
Definition: InverseKinematics.hpp:441
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:447
const std::shared_ptr< optimizer::Function > & getObjective()
Get the objective function for this IK module.
Definition: InverseKinematics.cpp:1389
InverseKinematics(const InverseKinematics &)=delete
Copying is not allowed.
void resetTargetConnection()
Reset the signal connection for this IK module's target.
Definition: InverseKinematics.cpp:1796
JacobianNode * getAffiliation()
This is the same as getNode().
Definition: InverseKinematics.cpp:1578
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:1528
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:1566
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:1534
Analytical * getAnalytical()
Get the Analytical IK method for this module, if one is available.
Definition: InverseKinematics.cpp:1454
bool mActive
True if this IK module should be active in its IK hierarcy.
Definition: InverseKinematics.hpp:400
void setTarget(std::shared_ptr< SimpleFrame > _newTarget)
Set the target for this IK module.
Definition: InverseKinematics.cpp:1540
std::vector< int > mDofMap
Map for the DOFs that are to be used by this IK module.
Definition: InverseKinematics.hpp:410
common::Connection mTargetConnection
Connection to the target update.
Definition: InverseKinematics.hpp:394
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:1402
std::vector< std::size_t > mDofs
A list of the DegreeOfFreedom Skeleton indices that will be used by this IK module.
Definition: InverseKinematics.hpp:407
std::shared_ptr< optimizer::Function > mNullSpaceObjective
Null space objective for the IK module.
Definition: InverseKinematics.hpp:416
std::size_t mHierarchyLevel
Hierarchy level for this IK module.
Definition: InverseKinematics.hpp:403
std::shared_ptr< optimizer::Function > mObjective
Objective for the IK module.
Definition: InverseKinematics.hpp:413
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:1306
common::Connection mNodeConnection
Connection to the node update.
Definition: InverseKinematics.hpp:397
bool hasNullSpaceObjective() const
Returns false if the null space objective does nothing.
Definition: InverseKinematics.cpp:1423
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:438
void setOffset(const Eigen::Vector3d &_offset=Eigen::Vector3d::Zero())
Inverse kinematics can be performed on any point within the body frame.
Definition: InverseKinematics.cpp:1516
const std::shared_ptr< optimizer::Problem > & getProblem()
Get the Problem that is being maintained by this IK module.
Definition: InverseKinematics.cpp:1466
Analytical * mAnalytical
If mGradientMethod is an Analytical extension, then this will have the same value is mGradientMethod.
Definition: InverseKinematics.hpp:429
Eigen::VectorXd getPositions() const
Get the current joint positions of the Skeleton.
Definition: InverseKinematics.cpp:1610
std::shared_ptr< optimizer::Solver > mSolver
The solver that this IK module will use for iterative methods.
Definition: InverseKinematics.hpp:435
InverseKinematicsPtr clone(JacobianNode *_newNode) const
Clone this IK module, but targeted at a new Node.
Definition: InverseKinematics.cpp:137
GradientMethod & getGradientMethod()
Get the GradientMethod for this IK module.
Definition: InverseKinematics.cpp:1441
void setInactive()
Equivalent to setActive(false)
Definition: InverseKinematics.cpp:1300
std::shared_ptr< optimizer::Problem > mProblem
The Problem that will be maintained by this IK module.
Definition: InverseKinematics.hpp:432
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:1590
void setHierarchyLevel(std::size_t _level)
Set the hierarchy level of this module.
Definition: InverseKinematics.cpp:1312
ErrorMethod & getErrorMethod()
Get the ErrorMethod for this IK module.
Definition: InverseKinematics.cpp:1429
std::unique_ptr< ErrorMethod > mErrorMethod
The method that this IK module will use to compute errors.
Definition: InverseKinematics.hpp:419
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:1382
std::unique_ptr< GradientMethod > mGradientMethod
The method that this IK module will use to compute gradients.
Definition: InverseKinematics.hpp:422
std::size_t getHierarchyLevel() const
Get the hierarchy level of this modle.
Definition: InverseKinematics.cpp:1318
std::shared_ptr< SimpleFrame > mTarget
Target that this IK module should use.
Definition: InverseKinematics.hpp:444
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:1376
bool solve(bool _applySolution=true)
Solve the IK Problem.
Definition: InverseKinematics.cpp:57
The JacobianNode class serves as a common interface for BodyNodes and EndEffectors to both be used as...
Definition: JacobianNode.hpp:54
TemplateInverseKinematicsPtr is a templated class that enables users to create a reference-counting I...
Definition: InverseKinematicsPtr.hpp:49
Definition: Function.hpp:46
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
InverseKinematics IK
Definition: InverseKinematics.hpp:453
const double DefaultIKLinearWeight
Definition: InverseKinematics.hpp:60
const double DefaultIKDLSCoefficient
Definition: InverseKinematics.hpp:58
const double DefaultIKGradientComponentWeight
Definition: InverseKinematics.hpp:57
const double DefaultIKErrorClamp
Definition: InverseKinematics.hpp:55
Eigen::Matrix< double, 6, Eigen::Dynamic > Jacobian
Definition: MathTypes.hpp:108
std::shared_ptr< Function > FunctionPtr
Definition: Function.hpp:84
Definition: BulletCollisionDetector.cpp:63
Definition: InverseKinematics.hpp:1050
Properties(const GradientMethod::Properties &gradientProperties=GradientMethod::Properties(), const UniqueProperties &analyticalProperties=UniqueProperties())
Definition: InverseKinematics.cpp:931
Definition: InverseKinematics.hpp:1006
Solution(const Eigen::VectorXd &_config=Eigen::VectorXd(), int _validity=VALID)
Default constructor.
Definition: InverseKinematics.cpp:867
int mValidity
Bitmap for whether this configuration is valid.
Definition: InverseKinematics.hpp:1016
Eigen::VectorXd mConfig
Configuration computed by the Analytical solver.
Definition: InverseKinematics.hpp:1012
QualityComparison mQualityComparator
Function for comparing the qualities of solutions.
Definition: InverseKinematics.hpp:1034
UniqueProperties(ExtraDofUtilization extraDofUtilization=UNUSED, double extraErrorLengthClamp=DefaultIKErrorClamp)
Default constructor. Uses a default quality comparison function.
Definition: InverseKinematics.cpp:876
void resetQualityComparisonFunction()
Reset the quality comparison function to its default behavior.
Definition: InverseKinematics.cpp:897
ExtraDofUtilization mExtraDofUtilization
Flag for how to use the extra DOFs in the IK module.
Definition: InverseKinematics.hpp:1028
double mExtraErrorLengthClamp
How much to clamp the extra error that gets applied to DOFs.
Definition: InverseKinematics.hpp:1031
The Properties struct contains settings that are commonly used by methods that compute error for inve...
Definition: InverseKinematics.hpp:486
Properties(const Bounds &_bounds=Bounds(Eigen::Vector6d::Constant(-DefaultIKTolerance), Eigen::Vector6d::Constant(DefaultIKTolerance)), double _errorClamp=DefaultIKErrorClamp, const Eigen::Vector6d &_errorWeights=Eigen::compose(Eigen::Vector3d::Constant(DefaultIKAngularWeight), Eigen::Vector3d::Constant(DefaultIKLinearWeight)))
Default constructor.
Definition: InverseKinematics.cpp:183
Eigen::Vector6d mErrorWeights
These weights will be applied to the error vector component-wise.
Definition: InverseKinematics.hpp:512
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:500
double mErrorLengthClamp
The error vector will be clamped to this length with each iteration.
Definition: InverseKinematics.hpp:505
Properties(double clamp=DefaultIKGradientComponentClamp, const Eigen::VectorXd &weights=Eigen::VectorXd())
Default constructor.
Definition: InverseKinematics.cpp:582
double mComponentWiseClamp
The component-wise clamp for this GradientMethod.
Definition: InverseKinematics.hpp:738
Eigen::VectorXd mComponentWeights
The weights for this GradientMethod.
Definition: InverseKinematics.hpp:741
Properties(const GradientMethod::Properties &gradientProperties=GradientMethod::Properties(), const UniqueProperties &dlsProperties=UniqueProperties())
Default constructor.
Definition: InverseKinematics.cpp:768
UniqueProperties(double damping=DefaultIKDLSCoefficient)
Default constructor.
Definition: InverseKinematics.cpp:760
double mDamping
Damping coefficient.
Definition: InverseKinematics.hpp:876
Properties(const ErrorMethod::Properties &errorProperties=ErrorMethod::Properties(), const UniqueProperties &taskSpaceProperties=UniqueProperties())
Default constructor.
Definition: InverseKinematics.cpp:419
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:677
UniqueProperties(bool computeErrorFromCenter=true)
Default constructor.
Definition: InverseKinematics.cpp:411