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 41 #include "dart/dynamics/InverseKinematics.hpp" 42 43 namespace dart { 44 namespace dynamics { 45 46 /// A base class for IkFast-based analytical inverse kinematics classes. 47 /// 48 /// The detail of IkFast can be found here: 49 /// http://openrave.org/docs/0.8.2/openravepy/ikfast/ 50 class IkFast : public InverseKinematics::Analytical 51 { 52 public: 53 /// Inverse kinematics types supported by IkFast 54 // Following conversion is referred from: 55 // https://github.com/rdiankov/openrave/blob/b1ebe135b4217823ebdf56d9af5fe89b29723603/include/openrave/openrave.h#L575-L623 56 enum IkType 57 { 58 /// End effector reaches desired 6D transformation 59 TRANSFORM_6D, 60 61 /// End effector reaches desired 3D rotation 62 ROTATION_3D, 63 64 /// End effector origin reaches desired 3D translation 65 TRANSLATION_3D, 66 67 /// Direction on end effector coordinate system reaches desired direction 68 DIRECTION_3D, 69 70 /// Ray on end effector coordinate system reaches desired global ray 71 RAY_4D, 72 73 /// Direction on end effector coordinate system points to desired 3D 74 /// position 75 LOOKAT_3D, 76 77 /// End effector origin and direction reaches desired 3D translation and 78 /// direction. Can be thought of as Ray IK where the origin of the ray must 79 /// coincide. 80 TRANSLATION_DIRECTION_5D, 81 82 /// End effector origin reaches desired XY translation position, Z is 83 /// ignored. The coordinate system with relative to the base link. 84 TRANSLATION_XY_2D, 85 86 /// 2D translation along XY plane and 1D rotation around Z axis. The offset 87 /// of the rotation is measured starting at +X, so at +X is it 0, at +Y it 88 /// is pi/2. 89 TRANSLATION_XY_ORIENTATION_3D, 90 91 /// Local point on end effector origin reaches desired 3D global point. 92 /// Because both local point and global point can be specified, there are 6 93 /// values. 94 TRANSLATION_LOCAL_GLOBAL_6D, 95 96 /// End effector origin reaches desired 3D translation, manipulator 97 /// direction makes a specific angle with x-axis (defined in the 98 /// manipulator base link’s coordinate system) 99 TRANSLATION_X_AXIS_ANGLE_4D, 100 101 /// End effector origin reaches desired 3D translation, manipulator 102 /// direction makes a specific angle with y-axis (defined in the 103 /// manipulator base link’s coordinate system) 104 TRANSLATION_Y_AXIS_ANGLE_4D, 105 106 /// End effector origin reaches desired 3D translation, manipulator 107 /// direction makes a specific angle with z-axis (defined in the 108 /// manipulator base link’s coordinate system) 109 TRANSLATION_Z_AXIS_ANGLE_4D, 110 111 /// End effector origin reaches desired 3D translation, manipulator 112 /// direction needs to be orthogonal to z-axis and be rotated at a certain 113 /// angle starting from the x-axis (defined in the manipulator base link's 114 /// coordinate system) 115 TRANSLATION_X_AXIS_ANGLE_Z_NORM_4D, 116 117 /// End effector origin reaches desired 3D translation, manipulator 118 /// direction needs to be orthogonal to z-axis and be rotated at a certain 119 /// angle starting from the y-axis (defined in the manipulator base link's 120 /// coordinate system) 121 TRANSLATION_Y_AXIS_ANGLE_X_NORM_4D, 122 123 /// End effector origin reaches desired 3D translation, manipulator 124 /// direction needs to be orthogonal to z-axis and be rotated at a certain 125 /// angle starting from the z-axis (defined in the manipulator base link's 126 /// coordinate system) 127 TRANSLATION_Z_AXIS_ANGLE_Y_NORM_4D, 128 129 UNKNOWN, 130 }; 131 132 /// Constructor 133 /// 134 /// \param[in] ik The parent InverseKinematics solver that is associated with 135 /// this gradient method. 136 /// \param[in] dofMap The indices to the degrees-of-freedom that will be 137 /// solved by IkFast. The number of DOFs can be varied depending on the IkFast 138 /// solvers. 139 /// \param[in] freeDofMap The indices to the DOFs that are not solved by the 140 /// IkFast solver. The values of these DOFs should be set properly. 141 /// \param[in] methodName The name of this analytical inverse kinematics 142 /// method. 143 /// \param[in] properties Properties of InverseKinematics::Analytical. 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 155 /// Computes forward kinematics given joint positions where the dimension is 156 /// the same as getNumJoints2(). 157 Eigen::Isometry3d computeFk(const Eigen::VectorXd& parameters); 158 159 /// Returns the indices of the DegreeOfFreedoms that are part of the joints 160 /// that IkFast solves for. 161 const std::vector<std::size_t>& getDofs() const override; 162 163 /// Returns the indices of the DegreeOfFreedoms that are part of the joints 164 /// that IkFast solves but the values should be set by the user in prior. 165 const std::vector<std::size_t>& getFreeDofs() const; 166 167 /// Returns true if this IkFast is ready to solve. 168 virtual bool isConfigured() const; 169 170 /// Returns the number of free parameters users has to set apriori. 171 std::size_t getNumFreeParameters2() const; 172 // TODO(JS): Rename to getNumFreeParameters() in DART 7 173 174 /// Returns the total number of indices of the chane. 175 std::size_t getNumJoints2() const; 176 // TODO(JS): Rename to getNumJoints in DART 7 177 178 /// Returns the IK type. 179 IkType getIkType2() const; 180 // TODO(JS): Rename to getIkType() in DART 7 181 182 /// Returns a hash of all the chain values used for double checking that the 183 /// correct IK is used. 184 const std::string getKinematicsHash2() const; 185 // TODO(JS): Rename to getKinematicsHash() in DART 7 186 187 /// Returns the IkFast version used to generate this file 188 std::string getIkFastVersion2() const; 189 // TODO(JS): Rename to getIkFastVersion() in DART 7 190 191 protected: 192 /// Returns the number of free parameters users has to set apriori. 193 virtual int getNumFreeParameters() const = 0; 194 // TODO(JS): Remove in DART 7 195 196 /// Returns the indicies of the free parameters indexed by the chain joints. 197 virtual int* getFreeParameters() const = 0; 198 // TODO(JS): Remove in DART 7 199 200 /// Returns the total number of indices of the chane. 201 virtual int getNumJoints() const = 0; 202 // TODO(JS): Remove in DART 7 203 204 /// Returns the size in bytes of the configured number type. 205 virtual int getIkRealSize() const = 0; 206 207 /// Returns the IK type. 208 virtual int getIkType() const = 0; 209 210 /// Computes the inverse kinematics solutions using the generated IKFast code. 211 virtual bool computeIk( 212 const IkReal* targetTranspose, 213 const IkReal* targetRotation, 214 const IkReal* pfree, 215 ikfast::IkSolutionListBase<IkReal>& solutions) 216 = 0; 217 218 /// Computes the forward kinematics solutions using the generated IKFast code. 219 virtual void computeFk( 220 const IkReal* parameters, IkReal* targetTranspose, IkReal* targetRotation) 221 = 0; 222 223 /// Returns a hash of all the chain values used for double checking that the 224 /// correct IK is used. 225 virtual const char* getKinematicsHash() = 0; 226 // TODO(JS): Rename in DART 7 227 228 /// Returns the IkFast version used to generate this file 229 virtual const char* getIkFastVersion() = 0; 230 // TODO(JS): Rename in DART 7 231 232 /// Configure IkFast. If it's successfully configured, isConfigured() returns 233 /// true. 234 virtual void configure() const; 235 236 mutable std::vector<double> mFreeParams; 237 238 /// True if this IkFast is ready to solve. 239 mutable bool mConfigured; 240 241 /// Indices of the DegreeOfFreedoms associated to the variable parameters of 242 /// this IkFast. 243 mutable std::vector<std::size_t> mDofs; 244 245 /// Indices of the DegreeOfFreedoms associated to the free parameters of this 246 /// IkFast. 247 mutable std::vector<std::size_t> mFreeDofs; 248 249 private: 250 /// Cache data for the target rotation used by IKFast. 251 std::array<IkReal, 9> mTargetRotation; 252 253 /// Cache data for the target translation used by IKFast. 254 std::array<IkReal, 3> mTargetTranspose; 255 }; 256 257 /// Tries to wrap a single dof IK solution, from IKFast, in the range of joint 258 /// limits. 259 /// 260 /// This function shouldn't be used for a non-cyclic DegreeOfFreedom. 261 /// 262 /// If multiple solution are available (when the range is wider than 2*pi), 263 /// returns the closest solution to the current joint position. 264 /// 265 /// \param[in] curr The current joint positions before solving IK. 266 /// \param[in] lb The lower joint position limit. 267 /// \param[in] ub The upper joint position limit. 268 /// \param[in,out] sol The IK solution to be wrapped. No assumption that the 269 /// value initially in the bounds. This value is only updated if an available 270 /// solution is found. 271 /// \return True if a solution is found. False, otherwise. 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_ 278