DART  6.10.1
IkFast.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_IKFAST_HPP_
34 #define DART_DYNAMICS_IKFAST_HPP_
35 
36 #include <array>
37 
38 #define IKFAST_HAS_LIBRARY
39 #include "dart/external/ikfast/ikfast.h"
40 
42 
43 namespace dart {
44 namespace dynamics {
45 
51 {
52 public:
54  // Following conversion is referred from:
55  // https://github.com/rdiankov/openrave/blob/b1ebe135b4217823ebdf56d9af5fe89b29723603/include/openrave/openrave.h#L575-L623
56  enum IkType
57  {
60 
63 
66 
69 
72 
76 
81 
85 
90 
95 
100 
105 
110 
116 
122 
128 
130  };
131 
144  IkFast(
145  InverseKinematics* ik,
146  const std::vector<std::size_t>& dofMap,
147  const std::vector<std::size_t>& freeDofMap,
148  const std::string& methodName = "IKFast",
149  const Analytical::Properties& properties = Analytical::Properties());
150 
151  // Documentation inherited.
152  const std::vector<InverseKinematics::Analytical::Solution>& computeSolutions(
153  const Eigen::Isometry3d& desiredBodyTf) override;
154 
157  Eigen::Isometry3d computeFk(const Eigen::VectorXd& parameters);
158 
161  const std::vector<std::size_t>& getDofs() const override;
162 
165  const std::vector<std::size_t>& getFreeDofs() const;
166 
168  virtual bool isConfigured() const;
169 
171  std::size_t getNumFreeParameters2() const;
172  // TODO(JS): Rename to getNumFreeParameters() in DART 7
173 
175  std::size_t getNumJoints2() const;
176  // TODO(JS): Rename to getNumJoints in DART 7
177 
179  IkType getIkType2() const;
180  // TODO(JS): Rename to getIkType() in DART 7
181 
184  const std::string getKinematicsHash2() const;
185  // TODO(JS): Rename to getKinematicsHash() in DART 7
186 
188  std::string getIkFastVersion2() const;
189  // TODO(JS): Rename to getIkFastVersion() in DART 7
190 
191 protected:
193  virtual int getNumFreeParameters() const = 0;
194  // TODO(JS): Remove in DART 7
195 
197  virtual int* getFreeParameters() const = 0;
198  // TODO(JS): Remove in DART 7
199 
201  virtual int getNumJoints() const = 0;
202  // TODO(JS): Remove in DART 7
203 
205  virtual int getIkRealSize() const = 0;
206 
208  virtual int getIkType() const = 0;
209 
211  virtual bool computeIk(
212  const IkReal* targetTranspose,
213  const IkReal* targetRotation,
214  const IkReal* pfree,
215  ikfast::IkSolutionListBase<IkReal>& solutions)
216  = 0;
217 
219  virtual void computeFk(
220  const IkReal* parameters, IkReal* targetTranspose, IkReal* targetRotation)
221  = 0;
222 
225  virtual const char* getKinematicsHash() = 0;
226  // TODO(JS): Rename in DART 7
227 
229  virtual const char* getIkFastVersion() = 0;
230  // TODO(JS): Rename in DART 7
231 
234  virtual void configure() const;
235 
236  mutable std::vector<double> mFreeParams;
237 
239  mutable bool mConfigured;
240 
243  mutable std::vector<std::size_t> mDofs;
244 
247  mutable std::vector<std::size_t> mFreeDofs;
248 
249 private:
251  std::array<IkReal, 9> mTargetRotation;
252 
254  std::array<IkReal, 3> mTargetTranspose;
255 };
256 
272 bool wrapCyclicSolution(double curr, double lb, double ub, double& sol);
273 
274 } // namespace dynamics
275 } // namespace dart
276 
277 #endif // DART_DYNAMICS_IKFAST_HPP_
BodyPropPtr properties
Definition: SdfParser.cpp:80
A base class for IkFast-based analytical inverse kinematics classes.
Definition: IkFast.hpp:51
const std::vector< InverseKinematics::Analytical::Solution > & computeSolutions(const Eigen::Isometry3d &desiredBodyTf) override
Use this function to fill the entries of the mSolutions variable.
Definition: IkFast.cpp:399
virtual bool isConfigured() const
Returns true if this IkFast is ready to solve.
Definition: IkFast.cpp:288
const std::vector< std::size_t > & getFreeDofs() const
Returns the indices of the DegreeOfFreedoms that are part of the joints that IkFast solves but the va...
Definition: IkFast.cpp:282
std::array< IkReal, 9 > mTargetRotation
Cache data for the target rotation used by IKFast.
Definition: IkFast.hpp:251
virtual bool computeIk(const IkReal *targetTranspose, const IkReal *targetRotation, const IkReal *pfree, ikfast::IkSolutionListBase< IkReal > &solutions)=0
Computes the inverse kinematics solutions using the generated IKFast code.
virtual int getIkType() const =0
Returns the IK type.
virtual int getIkRealSize() const =0
Returns the size in bytes of the configured number type.
bool mConfigured
True if this IkFast is ready to solve.
Definition: IkFast.hpp:239
IkFast(InverseKinematics *ik, const std::vector< std::size_t > &dofMap, const std::vector< std::size_t > &freeDofMap, const std::string &methodName="IKFast", const Analytical::Properties &properties=Analytical::Properties())
Constructor.
Definition: IkFast.cpp:248
std::string getIkFastVersion2() const
Returns the IkFast version used to generate this file.
Definition: IkFast.cpp:358
std::size_t getNumFreeParameters2() const
Returns the number of free parameters users has to set apriori.
Definition: IkFast.cpp:294
virtual const char * getIkFastVersion()=0
Returns the IkFast version used to generate this file.
std::size_t getNumJoints2() const
Returns the total number of indices of the chane.
Definition: IkFast.cpp:300
std::vector< double > mFreeParams
Definition: IkFast.hpp:236
const std::string getKinematicsHash2() const
Returns a hash of all the chain values used for double checking that the correct IK is used.
Definition: IkFast.cpp:352
std::vector< std::size_t > mDofs
Indices of the DegreeOfFreedoms associated to the variable parameters of this IkFast.
Definition: IkFast.hpp:243
std::vector< std::size_t > mFreeDofs
Indices of the DegreeOfFreedoms associated to the free parameters of this IkFast.
Definition: IkFast.hpp:247
virtual int getNumFreeParameters() const =0
Returns the number of free parameters users has to set apriori.
virtual void configure() const
Configure IkFast.
Definition: IkFast.cpp:364
IkType
Inverse kinematics types supported by IkFast.
Definition: IkFast.hpp:57
@ TRANSLATION_3D
End effector origin reaches desired 3D translation.
Definition: IkFast.hpp:65
@ DIRECTION_3D
Direction on end effector coordinate system reaches desired direction.
Definition: IkFast.hpp:68
@ TRANSLATION_Y_AXIS_ANGLE_X_NORM_4D
End effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to z...
Definition: IkFast.hpp:121
@ UNKNOWN
Definition: IkFast.hpp:129
@ TRANSLATION_Y_AXIS_ANGLE_4D
End effector origin reaches desired 3D translation, manipulator direction makes a specific angle with...
Definition: IkFast.hpp:104
@ LOOKAT_3D
Direction on end effector coordinate system points to desired 3D position.
Definition: IkFast.hpp:75
@ ROTATION_3D
End effector reaches desired 3D rotation.
Definition: IkFast.hpp:62
@ TRANSLATION_Z_AXIS_ANGLE_4D
End effector origin reaches desired 3D translation, manipulator direction makes a specific angle with...
Definition: IkFast.hpp:109
@ TRANSLATION_Z_AXIS_ANGLE_Y_NORM_4D
End effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to z...
Definition: IkFast.hpp:127
@ TRANSLATION_DIRECTION_5D
End effector origin and direction reaches desired 3D translation and direction.
Definition: IkFast.hpp:80
@ TRANSLATION_XY_ORIENTATION_3D
2D translation along XY plane and 1D rotation around Z axis.
Definition: IkFast.hpp:89
@ TRANSFORM_6D
End effector reaches desired 6D transformation.
Definition: IkFast.hpp:59
@ RAY_4D
Ray on end effector coordinate system reaches desired global ray.
Definition: IkFast.hpp:71
@ TRANSLATION_XY_2D
End effector origin reaches desired XY translation position, Z is ignored.
Definition: IkFast.hpp:84
@ TRANSLATION_LOCAL_GLOBAL_6D
Local point on end effector origin reaches desired 3D global point.
Definition: IkFast.hpp:94
@ TRANSLATION_X_AXIS_ANGLE_4D
End effector origin reaches desired 3D translation, manipulator direction makes a specific angle with...
Definition: IkFast.hpp:99
@ TRANSLATION_X_AXIS_ANGLE_Z_NORM_4D
End effector origin reaches desired 3D translation, manipulator direction needs to be orthogonal to z...
Definition: IkFast.hpp:115
virtual int getNumJoints() const =0
Returns the total number of indices of the chane.
virtual int * getFreeParameters() const =0
Returns the indicies of the free parameters indexed by the chain joints.
IkType getIkType2() const
Returns the IK type.
Definition: IkFast.cpp:306
virtual const char * getKinematicsHash()=0
Returns a hash of all the chain values used for double checking that the correct IK is used.
virtual void computeFk(const IkReal *parameters, IkReal *targetTranspose, IkReal *targetRotation)=0
Computes the forward kinematics solutions using the generated IKFast code.
std::array< IkReal, 3 > mTargetTranspose
Cache data for the target translation used by IKFast.
Definition: IkFast.hpp:254
const std::vector< std::size_t > & getDofs() const override
Returns the indices of the DegreeOfFreedoms that are part of the joints that IkFast solves for.
Definition: IkFast.cpp:263
Eigen::Isometry3d computeFk(const Eigen::VectorXd &parameters)
Computes forward kinematics given joint positions where the dimension is the same as getNumJoints2().
Definition: IkFast.cpp:447
Analytical is a base class that should be inherited by methods that are made to solve the IK analytic...
Definition: InverseKinematics.hpp:1051
The InverseKinematics class provides a convenient way of setting up an IK optimization problem.
Definition: InverseKinematics.hpp:76
bool wrapCyclicSolution(double currentValue, double lb, double ub, double &solutionValue)
Tries to wrap a single dof IK solution, from IKFast, in the range of joint limits.
Definition: IkFast.cpp:469
Definition: BulletCollisionDetector.cpp:65