DART 6.8.5
Loading...
Searching...
No Matches
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
50
51namespace dart {
52namespace dynamics {
53
54const double DefaultIKTolerance = 1e-6;
55const double DefaultIKErrorClamp = 1.0;
58const double DefaultIKDLSCoefficient = 0.05;
59const double DefaultIKAngularWeight = 0.4;
60const double DefaultIKLinearWeight = 1.0;
61
76{
77public:
78
81
84
87
89 virtual ~InverseKinematics();
90
135 DART_DEPRECATED(6.8)
136 bool solve(bool applySolution = true);
137
143 DART_DEPRECATED(6.8)
144 bool solve(Eigen::VectorXd& positions, bool applySolution = true);
145
191 bool findSolution(Eigen::VectorXd& positions);
192
203 bool solveAndApply(bool allowIncompleteResult = true);
204
218 bool solveAndApply(
219 Eigen::VectorXd& positions, bool allowIncompleteResult = true);
220
225 InverseKinematicsPtr clone(JacobianNode* _newNode) const;
226
227 // For the definitions of these classes, see the bottom of this header
228 class Function;
229
230 // Methods for computing IK error
231 class ErrorMethod;
232 class TaskSpaceRegion;
233
234 // Methods for computing IK gradient
235 class GradientMethod;
236 class JacobianDLS;
237 class JacobianTranspose;
238 class Analytical;
239
244 void setActive(bool _active=true);
245
247 void setInactive();
248
250 bool isActive() const;
251
256 void setHierarchyLevel(std::size_t _level);
257
259 std::size_t getHierarchyLevel() const;
260
265 void useChain();
266
268 void useWholeBody();
269
272 template <class DegreeOfFreedomT>
273 void setDofs(const std::vector<DegreeOfFreedomT*>& _dofs);
274
278 void setDofs(const std::vector<std::size_t>& _dofs);
279
281 const std::vector<std::size_t>& getDofs() const;
282
289 const std::vector<int>& getDofMap() const;
290
294 void setObjective(const std::shared_ptr<optimizer::Function>& _objective);
295
297 const std::shared_ptr<optimizer::Function>& getObjective();
298
300 std::shared_ptr<const optimizer::Function> getObjective() const;
301
311 const std::shared_ptr<optimizer::Function>& _nsObjective);
312
314 const std::shared_ptr<optimizer::Function>& getNullSpaceObjective();
315
317 std::shared_ptr<const optimizer::Function> getNullSpaceObjective() const;
318
320 bool hasNullSpaceObjective() const;
321
326 template <class IKErrorMethod, typename... Args>
327 IKErrorMethod& setErrorMethod(Args&&... args);
328
332
335 const ErrorMethod& getErrorMethod() const;
336
341 template <class IKGradientMethod, typename... Args>
342 IKGradientMethod& setGradientMethod(Args&&... args);
343
347
350 const GradientMethod& getGradientMethod() const;
351
356
360 const Analytical* getAnalytical() const;
361
363 const std::shared_ptr<optimizer::Problem>& getProblem();
364
366 std::shared_ptr<const optimizer::Problem> getProblem() const;
367
374 void resetProblem(bool _clearSeeds=false);
375
378 void setSolver(const std::shared_ptr<optimizer::Solver>& _newSolver);
379
381 const std::shared_ptr<optimizer::Solver>& getSolver();
382
384 std::shared_ptr<const optimizer::Solver> getSolver() const;
385
391 void setOffset(const Eigen::Vector3d& _offset = Eigen::Vector3d::Zero());
392
396 const Eigen::Vector3d& getOffset() const;
397
401 bool hasOffset() const;
402
409 void setTarget(std::shared_ptr<SimpleFrame> _newTarget);
410
413 std::shared_ptr<SimpleFrame> getTarget();
414
417 std::shared_ptr<const SimpleFrame> getTarget() const;
418
421
423 const JacobianNode* getNode() const;
424
428
431 const JacobianNode* getAffiliation() const;
432
436 const math::Jacobian& computeJacobian() const;
437
441 Eigen::VectorXd getPositions() const;
442
446 void setPositions(const Eigen::VectorXd& _q);
447
453 void clearCaches();
454
455protected:
456
457 // For the definitions of these functions, see the bottom of this header
458 class Objective;
459 friend class Objective;
460 class Constraint;
461 friend class Constraint;
462
465
467 void initialize();
468
471
473 void resetNodeConnection();
474
476 common::Connection mTargetConnection;
477
479 common::Connection mNodeConnection;
480
483
486
489 std::vector<std::size_t> mDofs;
490
492 std::vector<int> mDofMap;
493
495 std::shared_ptr<optimizer::Function> mObjective;
496
498 std::shared_ptr<optimizer::Function> mNullSpaceObjective;
499
502
505
512
514 std::shared_ptr<optimizer::Problem> mProblem;
515
517 std::shared_ptr<optimizer::Solver> mSolver;
518
520 Eigen::Vector3d mOffset;
521
524
526 std::shared_ptr<SimpleFrame> mTarget;
527
530
532 mutable math::Jacobian mJacobian;
533};
534
536
537//==============================================================================
546{
547public:
548
551
553 virtual ~Function() = default;
554};
555
556//==============================================================================
560{
561public:
562
563 typedef std::pair<Eigen::Vector6d, Eigen::Vector6d> Bounds;
564
568 {
570 Properties(const Bounds& _bounds =
571 Bounds(Eigen::Vector6d::Constant(-DefaultIKTolerance),
572 Eigen::Vector6d::Constant( DefaultIKTolerance)),
573
574 double _errorClamp = DefaultIKErrorClamp,
575
576 const Eigen::Vector6d& _errorWeights = Eigen::compose(
577 Eigen::Vector3d::Constant(DefaultIKAngularWeight),
578 Eigen::Vector3d::Constant(DefaultIKLinearWeight)));
579
582 std::pair<Eigen::Vector6d, Eigen::Vector6d> mBounds;
583
588
595
596 // To get byte-aligned Eigen vectors
597 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
598 };
599
602 const std::string& _methodName,
603 const Properties& _properties = Properties());
604
606 virtual ~ErrorMethod() = default;
607
609 virtual std::unique_ptr<ErrorMethod> clone(
610 InverseKinematics* _newIK) const = 0;
611
622
627 virtual Eigen::Isometry3d computeDesiredTransform(
628 const Eigen::Isometry3d& _currentTf, const Eigen::Vector6d& _error) = 0;
629
631 const Eigen::Vector6d& evalError(const Eigen::VectorXd& _q);
632
634 const std::string& getMethodName() const;
635
637 void setBounds(
638 const Eigen::Vector6d& _lower =
639 Eigen::Vector6d::Constant(-DefaultIKTolerance),
640 const Eigen::Vector6d& _upper =
641 Eigen::Vector6d::Constant( DefaultIKTolerance));
642
644 void setBounds(const std::pair<Eigen::Vector6d, Eigen::Vector6d>& _bounds);
645
647 const std::pair<Eigen::Vector6d, Eigen::Vector6d>& getBounds() const;
648
650 void setAngularBounds(
651 const Eigen::Vector3d& _lower =
652 Eigen::Vector3d::Constant(-DefaultIKTolerance),
653 const Eigen::Vector3d& _upper =
654 Eigen::Vector3d::Constant( DefaultIKTolerance));
655
657 void setAngularBounds(
658 const std::pair<Eigen::Vector3d, Eigen::Vector3d>& _bounds);
659
661 std::pair<Eigen::Vector3d, Eigen::Vector3d> getAngularBounds() const;
662
664 void setLinearBounds(
665 const Eigen::Vector3d& _lower =
666 Eigen::Vector3d::Constant(-DefaultIKTolerance),
667 const Eigen::Vector3d& _upper =
668 Eigen::Vector3d::Constant( DefaultIKTolerance));
669
671 void setLinearBounds(
672 const std::pair<Eigen::Vector3d, Eigen::Vector3d>& _bounds);
673
675 std::pair<Eigen::Vector3d, Eigen::Vector3d> getLinearBounds() const;
676
679 void setErrorLengthClamp(double _clampSize = DefaultIKErrorClamp);
680
683 double getErrorLengthClamp() const;
684
687 void setErrorWeights(const Eigen::Vector6d& _weights);
688
691 const Eigen::Vector6d& getErrorWeights() const;
692
695 void setAngularErrorWeights(
696 const Eigen::Vector3d& _weights =
697 Eigen::Vector3d::Constant(DefaultIKAngularWeight));
698
701 Eigen::Vector3d getAngularErrorWeights() const;
702
705 void setLinearErrorWeights(
706 const Eigen::Vector3d& _weights =
707 Eigen::Vector3d::Constant(DefaultIKLinearWeight));
708
711 Eigen::Vector3d getLinearErrorWeights() const;
712
714 Properties getErrorMethodProperties() const;
715
718 void clearCache();
719
720protected:
721
724
726 std::string mMethodName;
727
729 Eigen::VectorXd mLastPositions;
730
733
736
737public:
738 // To get byte-aligned Eigen vectors
739 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
740
741};
742
743//==============================================================================
747{
748public:
749
751 {
760
762 UniqueProperties(bool computeErrorFromCenter = true);
763 };
764
766 {
769 const ErrorMethod::Properties& errorProperties =
771 const UniqueProperties& taskSpaceProperties = UniqueProperties());
772 };
773
776 const Properties& _properties = Properties());
777
779 virtual ~TaskSpaceRegion() = default;
780
781 // Documentation inherited
782 std::unique_ptr<ErrorMethod> clone(InverseKinematics* _newIK) const override;
783
784 // Documentation inherited
785 Eigen::Isometry3d computeDesiredTransform(
786 const Eigen::Isometry3d& _currentTf,
787 const Eigen::Vector6d& _error) override;
788
789 // Documentation inherited
790 Eigen::Vector6d computeError() override;
791
794 void setComputeFromCenter(bool computeFromCenter);
795
798 bool isComputingFromCenter() const;
799
801 Properties getTaskSpaceRegionProperties() const;
802
803protected:
804
807
808};
809
810//==============================================================================
814{
815public:
816
818 {
821
823 Eigen::VectorXd mComponentWeights;
824
826 Properties(double clamp = DefaultIKGradientComponentClamp,
827 const Eigen::VectorXd& weights = Eigen::VectorXd());
828 };
829
832 const std::string& _methodName,
833 const Properties& _properties);
834
836 virtual ~GradientMethod() = default;
837
839 virtual std::unique_ptr<GradientMethod> clone(
840 InverseKinematics* _newIK) const = 0;
841
857 virtual void computeGradient(const Eigen::Vector6d& _error,
858 Eigen::VectorXd& _grad) = 0;
859
862 void evalGradient(const Eigen::VectorXd& _q,
863 Eigen::Map<Eigen::VectorXd> _grad);
864
866 const std::string& getMethodName() const;
867
869 void clampGradient(Eigen::VectorXd& _grad) const;
870
873 void setComponentWiseClamp(double _clamp = DefaultIKGradientComponentClamp);
874
876 double getComponentWiseClamp() const;
877
880 void applyWeights(Eigen::VectorXd& _grad) const;
881
888 void setComponentWeights(const Eigen::VectorXd& _weights);
889
891 const Eigen::VectorXd& getComponentWeights() const;
892
902 void convertJacobianMethodOutputToGradient(
903 Eigen::VectorXd& grad, const std::vector<std::size_t>& dofs);
904
906 Properties getGradientMethodProperties() const;
907
910 void clearCache();
911
913 InverseKinematics* getIK();
914
916 const InverseKinematics* getIK() const;
917
918protected:
919
922
924 std::string mMethodName;
925
927 Eigen::VectorXd mLastPositions;
928
930 Eigen::VectorXd mLastGradient;
931
934
935private:
936
939 Eigen::VectorXd mInitialPositionsCache;
940};
941
942//==============================================================================
952{
953public:
954
956 {
958 double mDamping;
959
961 UniqueProperties(double damping = DefaultIKDLSCoefficient);
962 };
963
965 {
968 const GradientMethod::Properties& gradientProperties =
970 const UniqueProperties& dlsProperties = UniqueProperties());
971 };
972
974 explicit JacobianDLS(InverseKinematics* _ik,
975 const Properties& properties = Properties());
976
978 virtual ~JacobianDLS() = default;
979
980 // Documentation inherited
981 std::unique_ptr<GradientMethod> clone(
982 InverseKinematics* _newIK) const override;
983
984 // Documentation inherited
985 void computeGradient(const Eigen::Vector6d& _error,
986 Eigen::VectorXd& _grad) override;
987
991 void setDampingCoefficient(double _damping = DefaultIKDLSCoefficient);
992
994 double getDampingCoefficient() const;
995
997 Properties getJacobianDLSProperties() const;
998
999protected:
1000
1003
1004};
1005
1006//==============================================================================
1013{
1014public:
1015
1018 const Properties& properties = Properties());
1019
1021 virtual ~JacobianTranspose() = default;
1022
1023 // Documentation inherited
1024 std::unique_ptr<GradientMethod> clone(
1025 InverseKinematics* _newIK) const override;
1026
1027 // Documentation inherited
1028 void computeGradient(const Eigen::Vector6d& _error,
1029 Eigen::VectorXd& _grad) override;
1030};
1031
1032//==============================================================================
1047{
1048public:
1049
1053 {
1054 VALID = 0,
1055 OUT_OF_REACH = 1 << 0,
1056 LIMIT_VIOLATED = 1 << 1
1058 // TODO(JS): Change to enum class?
1059
1079 {
1080 UNUSED = 0,
1083 PRE_AND_POST_ANALYTICAL
1085 // TODO(JS): Change to enum class?
1086
1088 {
1090 Solution(const Eigen::VectorXd& _config = Eigen::VectorXd(),
1091 int _validity = VALID);
1092
1094 Eigen::VectorXd mConfig;
1095
1099 };
1100
1101 // std::function template for comparing the quality of configurations
1102 typedef std::function<bool(
1103 const Eigen::VectorXd& _better,
1104 const Eigen::VectorXd& _worse,
1106
1108 {
1111
1114
1117
1119 UniqueProperties(ExtraDofUtilization extraDofUtilization = UNUSED,
1120 double extraErrorLengthClamp = DefaultIKErrorClamp);
1121
1123 UniqueProperties(ExtraDofUtilization extraDofUtilization,
1124 double extraErrorLengthClamp,
1125 QualityComparison qualityComparator);
1126
1128 void resetQualityComparisonFunction();
1129 };
1130
1132 {
1133 // Default constructor
1134 Properties(
1135 const GradientMethod::Properties& gradientProperties =
1137 const UniqueProperties& analyticalProperties = UniqueProperties());
1138
1139 // Construct Properties by specifying the UniqueProperties. The
1140 // GradientMethod::Properties components will be set to defaults.
1141 Properties(const UniqueProperties& analyticalProperties);
1142 };
1143
1145 Analytical(InverseKinematics* _ik, const std::string& _methodName,
1146 const Properties& _properties);
1147
1149 virtual ~Analytical() = default;
1150
1155 const std::vector<Solution>& getSolutions();
1156
1161 const std::vector<Solution>& getSolutions(
1162 const Eigen::Isometry3d& _desiredTf);
1163
1166 void computeGradient(const Eigen::Vector6d& _error,
1167 Eigen::VectorXd& _grad) override;
1168
1178 virtual const std::vector<Solution>& computeSolutions(
1179 const Eigen::Isometry3d& _desiredBodyTf) = 0;
1180
1186 virtual const std::vector<std::size_t>& getDofs() const = 0;
1187
1190 void setPositions(const Eigen::VectorXd& _config);
1191
1194 Eigen::VectorXd getPositions() const;
1195
1197 void setExtraDofUtilization(ExtraDofUtilization _utilization);
1198
1200 ExtraDofUtilization getExtraDofUtilization() const;
1201
1203 void setExtraErrorLengthClamp(double _clamp);
1204
1207 double getExtraErrorLengthClamp() const;
1208
1223 void setQualityComparisonFunction(const QualityComparison& _func);
1224
1226 void resetQualityComparisonFunction();
1227
1229 Properties getAnalyticalProperties() const;
1230
1237 void constructDofMap();
1238
1239protected:
1240
1252 virtual void addExtraDofGradient(
1253 Eigen::VectorXd& grad,
1254 const Eigen::Vector6d& error,
1255 ExtraDofUtilization utilization);
1256
1262 void checkSolutionJointLimits();
1263
1265 std::vector<Solution> mSolutions;
1266
1269
1270private:
1271
1274 std::vector<int> mDofMap;
1275
1279 std::vector<std::size_t> mExtraDofs;
1280
1285 std::vector<Solution> mValidSolutionsCache;
1286
1288 std::vector<Solution> mOutOfReachCache;
1289
1291 std::vector<Solution> mLimitViolationCache;
1292
1294 Eigen::VectorXd mConfigCache;
1295
1299 Eigen::VectorXd mRestoreConfigCache;
1300
1302 Eigen::VectorXd mExtraDofGradCache;
1303};
1304
1305//==============================================================================
1312 public Function, public optimizer::Function
1313{
1314public:
1315
1317
1318
1320
1322 virtual ~Objective() = default;
1323
1324 // Documentation inherited
1325 optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override;
1326
1327 // Documentation inherited
1328 double eval(const Eigen::VectorXd& _x) override;
1329
1330 // Documentation inherited
1331 void evalGradient(const Eigen::VectorXd& _x,
1332 Eigen::Map<Eigen::VectorXd> _grad) override;
1333
1334protected:
1335
1338
1340 Eigen::VectorXd mGradCache;
1341
1343 Eigen::JacobiSVD<math::Jacobian> mSVDCache;
1344 // TODO(JS): Need to define aligned operator new for this?
1345
1347 Eigen::MatrixXd mNullSpaceCache;
1348
1349public:
1350 // To get byte-aligned Eigen vectors
1351 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
1352};
1353
1354//==============================================================================
1362 public Function, public optimizer::Function
1363{
1364public:
1365
1368
1370 virtual ~Constraint() = default;
1371
1372 // Documentation inherited
1373 optimizer::FunctionPtr clone(InverseKinematics* _newIK) const override;
1374
1375 // Documentation inherited
1376 double eval(const Eigen::VectorXd& _x) override;
1377
1378 // Documentation inherited
1379 void evalGradient(const Eigen::VectorXd& _x,
1380 Eigen::Map<Eigen::VectorXd> _grad) override;
1381
1382protected:
1383
1386};
1387
1388} // namespace dynamics
1389} // namespace dart
1390
1392
1393#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:148
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:1047
Eigen::VectorXd mRestoreConfigCache
A cache for storing the current configuration so that it can be restored later.
Definition InverseKinematics.hpp:1299
std::vector< Solution > mLimitViolationCache
A cache for solutions that violate joint limits.
Definition InverseKinematics.hpp:1291
Eigen::VectorXd mExtraDofGradCache
A cache for storing the gradient for the extra DOFs.
Definition InverseKinematics.hpp:1302
UniqueProperties mAnalyticalP
Properties for this Analytical IK solver.
Definition InverseKinematics.hpp:1268
std::vector< Solution > mValidSolutionsCache
A cache for the valid solutions.
Definition InverseKinematics.hpp:1285
Validity_t
Bitwise enumerations that are used to describe some properties of each solution produced by the analy...
Definition InverseKinematics.hpp:1053
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:1079
@ POST_ANALYTICAL
Definition InverseKinematics.hpp:1082
@ PRE_ANALYTICAL
Definition InverseKinematics.hpp:1081
Eigen::VectorXd mConfigCache
A cache for storing the current configuration.
Definition InverseKinematics.hpp:1294
virtual const std::vector< Solution > & computeSolutions(const Eigen::Isometry3d &_desiredBodyTf)=0
Use this function to fill the entries of the mSolutions variable.
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::vector< Solution > mSolutions
Vector of solutions.
Definition InverseKinematics.hpp:1265
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:1279
std::vector< Solution > mOutOfReachCache
A cache for the out of reach solutions.
Definition InverseKinematics.hpp:1288
std::function< bool(const Eigen::VectorXd &_better, const Eigen::VectorXd &_worse, const InverseKinematics *_ik)> QualityComparison
Definition InverseKinematics.hpp:1105
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:1274
The InverseKinematics::Constraint Function is simply meant to be used to merge the ErrorMethod and Gr...
Definition InverseKinematics.hpp:1363
virtual ~Constraint()=default
Virtual constructor.
sub_ptr< InverseKinematics > mIK
Pointer to this Constraint's IK module.
Definition InverseKinematics.hpp:1385
ErrorMethod is a base class for different ways of computing the error of an InverseKinematics module.
Definition InverseKinematics.hpp:560
common::sub_ptr< InverseKinematics > mIK
Pointer to the IK module of this ErrorMethod.
Definition InverseKinematics.hpp:723
virtual ~ErrorMethod()=default
Virtual destructor.
Properties mErrorP
The properties of this ErrorMethod.
Definition InverseKinematics.hpp:735
Eigen::Vector6d mLastError
The last error vector computed by this ErrorMethod.
Definition InverseKinematics.hpp:732
std::pair< Eigen::Vector6d, Eigen::Vector6d > Bounds
Definition InverseKinematics.hpp:563
Eigen::VectorXd mLastPositions
The last joint positions passed into this ErrorMethod.
Definition InverseKinematics.hpp:729
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:726
virtual std::unique_ptr< ErrorMethod > clone(InverseKinematics *_newIK) const =0
Enable this ErrorMethod to be cloned to a new IK module.
This class should be inherited by optimizer::Function classes that have a dependency on the InverseKi...
Definition InverseKinematics.hpp:546
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:814
Eigen::VectorXd mInitialPositionsCache
Cache used by convertJacobianMethodOutputToGradient to avoid reallocating this vector on each iterati...
Definition InverseKinematics.hpp:939
std::string mMethodName
The name of this method.
Definition InverseKinematics.hpp:924
virtual ~GradientMethod()=default
Virtual destructor.
Eigen::VectorXd mLastPositions
The last positions that was passed to this GradientMethod.
Definition InverseKinematics.hpp:927
common::sub_ptr< InverseKinematics > mIK
The IK module that this GradientMethod belongs to.
Definition InverseKinematics.hpp:921
virtual std::unique_ptr< GradientMethod > clone(InverseKinematics *_newIK) const =0
Enable this GradientMethod to be cloned to a new IK module.
Eigen::VectorXd mLastGradient
The last gradient that was computed by this GradientMethod.
Definition InverseKinematics.hpp:930
Properties mGradientP
Properties for this GradientMethod.
Definition InverseKinematics.hpp:933
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:952
UniqueProperties mDLSProperties
Properties of this Damped Least Squares method.
Definition InverseKinematics.hpp:1002
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:1013
virtual ~JacobianTranspose()=default
Virtual destructor.
The InverseKinematics::Objective Function is simply used to merge the objective and null space object...
Definition InverseKinematics.hpp:1313
Eigen::MatrixXd mNullSpaceCache
Cache for the null space.
Definition InverseKinematics.hpp:1347
virtual ~Objective()=default
Virtual destructor.
Eigen::VectorXd mGradCache
Cache for the gradient of the Objective.
Definition InverseKinematics.hpp:1340
sub_ptr< InverseKinematics > mIK
Pointer to this Objective's IK module.
Definition InverseKinematics.hpp:1337
Eigen::JacobiSVD< math::Jacobian > mSVDCache
Cache for the null space SVD.
Definition InverseKinematics.hpp:1343
The TaskSpaceRegion is a nicely generalized method for computing the error of an IK Problem.
Definition InverseKinematics.hpp:747
virtual ~TaskSpaceRegion()=default
Virtual destructor.
UniqueProperties mTaskSpaceP
Properties of this TaskSpaceRegion.
Definition InverseKinematics.hpp:806
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:1323
std::shared_ptr< SimpleFrame > getTarget()
Get the target that is being used by this IK module.
Definition InverseKinematics.cpp:1583
void clearCaches()
Clear the caches of this IK module.
Definition InverseKinematics.cpp:1660
const std::shared_ptr< optimizer::Solver > & getSolver()
Get the Solver that is being used by this IK module.
Definition InverseKinematics.cpp:1533
const std::shared_ptr< optimizer::Function > & getNullSpaceObjective()
Get the null space objective for this IK module.
Definition InverseKinematics.cpp:1439
void resetNodeConnection()
Reset the signal connection for this IK module's Node.
Definition InverseKinematics.cpp:1835
math::Jacobian mJacobian
Jacobian cache for the IK module.
Definition InverseKinematics.hpp:532
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:1367
void resetProblem(bool _clearSeeds=false)
Reset the Problem that is being maintained by this IK module.
Definition InverseKinematics.cpp:1507
void setPositions(const Eigen::VectorXd &_q)
Set the current joint positions of the Skeleton.
Definition InverseKinematics.cpp:1645
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:1399
void useChain()
When solving the IK for this module's Node, use the longest available dynamics::Chain that goes from ...
Definition InverseKinematics.cpp:1353
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:1522
void initialize()
Gets called during construction.
Definition InverseKinematics.cpp:1789
bool mHasOffset
True if the offset is non-zero.
Definition InverseKinematics.hpp:523
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:529
const std::shared_ptr< optimizer::Function > & getObjective()
Get the objective function for this IK module.
Definition InverseKinematics.cpp:1418
InverseKinematics(const InverseKinematics &)=delete
Copying is not allowed.
void resetTargetConnection()
Reset the signal connection for this IK module's target.
Definition InverseKinematics.cpp:1825
JacobianNode * getAffiliation()
This is the same as getNode().
Definition InverseKinematics.cpp:1607
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:1557
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:1595
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:1563
Analytical * getAnalytical()
Get the Analytical IK method for this module, if one is available.
Definition InverseKinematics.cpp:1483
bool mActive
True if this IK module should be active in its IK hierarcy.
Definition InverseKinematics.hpp:482
void setTarget(std::shared_ptr< SimpleFrame > _newTarget)
Set the target for this IK module.
Definition InverseKinematics.cpp:1569
std::vector< int > mDofMap
Map for the DOFs that are to be used by this IK module.
Definition InverseKinematics.hpp:492
common::Connection mTargetConnection
Connection to the target update.
Definition InverseKinematics.hpp:476
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:1431
std::vector< std::size_t > mDofs
A list of the DegreeOfFreedom Skeleton indices that will be used by this IK module.
Definition InverseKinematics.hpp:489
std::shared_ptr< optimizer::Function > mNullSpaceObjective
Null space objective for the IK module.
Definition InverseKinematics.hpp:498
std::size_t mHierarchyLevel
Hierarchy level for this IK module.
Definition InverseKinematics.hpp:485
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:495
bool isActive() const
Returns true if this IK module is allowed to be active in a HierarchicalIK.
Definition InverseKinematics.cpp:1335
common::Connection mNodeConnection
Connection to the node update.
Definition InverseKinematics.hpp:479
bool hasNullSpaceObjective() const
Returns false if the null space objective does nothing.
Definition InverseKinematics.cpp:1452
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:520
void setOffset(const Eigen::Vector3d &_offset=Eigen::Vector3d::Zero())
Inverse kinematics can be performed on any point within the body frame.
Definition InverseKinematics.cpp:1545
const std::shared_ptr< optimizer::Problem > & getProblem()
Get the Problem that is being maintained by this IK module.
Definition InverseKinematics.cpp:1495
Analytical * mAnalytical
If mGradientMethod is an Analytical extension, then this will have the same value is mGradientMethod.
Definition InverseKinematics.hpp:511
Eigen::VectorXd getPositions() const
Get the current joint positions of the Skeleton.
Definition InverseKinematics.cpp:1639
std::shared_ptr< optimizer::Solver > mSolver
The solver that this IK module will use for iterative methods.
Definition InverseKinematics.hpp:517
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:1470
void setInactive()
Equivalent to setActive(false)
Definition InverseKinematics.cpp:1329
std::shared_ptr< optimizer::Problem > mProblem
The Problem that will be maintained by this IK module.
Definition InverseKinematics.hpp:514
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:1619
void setHierarchyLevel(std::size_t _level)
Set the hierarchy level of this module.
Definition InverseKinematics.cpp:1341
ErrorMethod & getErrorMethod()
Get the ErrorMethod for this IK module.
Definition InverseKinematics.cpp:1458
std::unique_ptr< ErrorMethod > mErrorMethod
The method that this IK module will use to compute errors.
Definition InverseKinematics.hpp:501
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:1411
InverseKinematics & operator=(const InverseKinematics &)=delete
Assignment is not allowed.
std::unique_ptr< GradientMethod > mGradientMethod
The method that this IK module will use to compute gradients.
Definition InverseKinematics.hpp:504
std::size_t getHierarchyLevel() const
Get the hierarchy level of this modle.
Definition InverseKinematics.cpp:1347
std::shared_ptr< SimpleFrame > mTarget
Target that this IK module should use.
Definition InverseKinematics.hpp:526
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:1405
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:54
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
const double DefaultIKGradientComponentWeight
Definition InverseKinematics.hpp:57
const double DefaultIKErrorClamp
Definition InverseKinematics.hpp:55
std::shared_ptr< Function > FunctionPtr
Definition Function.hpp:84
Definition BulletCollisionDetector.cpp:63
Definition SharedLibraryManager.hpp:43
Definition InverseKinematics.hpp:1088
int mValidity
Bitmap for whether this configuration is valid.
Definition InverseKinematics.hpp:1098
Eigen::VectorXd mConfig
Configuration computed by the Analytical solver.
Definition InverseKinematics.hpp:1094
QualityComparison mQualityComparator
Function for comparing the qualities of solutions.
Definition InverseKinematics.hpp:1116
ExtraDofUtilization mExtraDofUtilization
Flag for how to use the extra DOFs in the IK module.
Definition InverseKinematics.hpp:1110
double mExtraErrorLengthClamp
How much to clamp the extra error that gets applied to DOFs.
Definition InverseKinematics.hpp:1113
The Properties struct contains settings that are commonly used by methods that compute error for inve...
Definition InverseKinematics.hpp:568
Eigen::Vector6d mErrorWeights
These weights will be applied to the error vector component-wise.
Definition InverseKinematics.hpp:594
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:582
double mErrorLengthClamp
The error vector will be clamped to this length with each iteration.
Definition InverseKinematics.hpp:587
double mComponentWiseClamp
The component-wise clamp for this GradientMethod.
Definition InverseKinematics.hpp:820
Eigen::VectorXd mComponentWeights
The weights for this GradientMethod.
Definition InverseKinematics.hpp:823
double mDamping
Damping coefficient.
Definition InverseKinematics.hpp:958
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:759