Add IkFast parameter accessors to IkFast class (#1396)

CHANGELOG.md

@@ -2,6 +2,10 @@
 
 ### [DART 6.10.0 (20XX-XX-XX)](https://github.com/dartsim/dart/milestone/58?closed=1)
 
+* Kinematics
+
+  * Added IkFast parameter accessors to IkFast class: [#1396](https://github.com/dartsim/dart/pull/1396)
+
 * Dynamics
 
   * Fixed friction and restitution of individual shapes in a body: [#1369](https://github.com/dartsim/dart/pull/1369)

dart/common/SharedLibrary.hpp

@@ -60,6 +60,26 @@ using hInstance = HINSTANCE__*;
 
 #endif
 
+#if DART_OS_LINUX
+static constexpr const char* DART_SHARED_LIB_EXTENSION = "so";
+#elif DART_OS_MACOS
+static constexpr const char* DART_SHARED_LIB_EXTENSION = "dylib";
+#elif DART_OS_WINDOWS
+static constexpr const char* DART_SHARED_LIB_EXTENSION = "dll";
+#else
+#  error Unhandled platform
+#endif
+
+#if DART_OS_LINUX
+static constexpr const char* DART_SHARED_LIB_PREFIX = "lib";
+#elif DART_OS_MACOS
+static constexpr const char* DART_SHARED_LIB_PREFIX = "lib";
+#elif DART_OS_WINDOWS
+static constexpr const char* DART_SHARED_LIB_PREFIX = "";
+#else
+#  error Unhandled platform
+#endif
+
 namespace dart {
 namespace common {
 

dart/common/detail/SharedLibraryManager.cpp

@@ -47,8 +47,8 @@ std::shared_ptr<SharedLibrary> SharedLibraryManager::load(
   const bool exists = boost::filesystem::exists(path);
   if (!exists)
   {
-    dtwarn << "[SharedLibraryManager::load] The given path doesn't exist. "
-           << "Returning nullptr.\n";
+    dtwarn << "[SharedLibraryManager::load] Failed to load the shared library '"
+           << path.c_str() << "'. The file doesn't exist. Returning nullptr.\n";
     return nullptr;
   }
 

dart/dynamics/IkFast.cpp

@@ -62,6 +62,29 @@ void convertTransform(
   eetrans[2] = tf.translation()[2];
 }
 
+//==============================================================================
+void convertTransform(
+    const std::array<IkReal, 3>& eetrans,
+    const std::array<IkReal, 9>& eerot,
+    Eigen::Isometry3d& tf)
+{
+  tf.setIdentity();
+
+  tf(0, 0) = eerot[0 * 3 + 0];
+  tf(0, 1) = eerot[0 * 3 + 1];
+  tf(0, 2) = eerot[0 * 3 + 2];
+  tf(1, 0) = eerot[1 * 3 + 0];
+  tf(1, 1) = eerot[1 * 3 + 1];
+  tf(1, 2) = eerot[1 * 3 + 2];
+  tf(2, 0) = eerot[2 * 3 + 0];
+  tf(2, 1) = eerot[2 * 3 + 1];
+  tf(2, 2) = eerot[2 * 3 + 2];
+
+  tf(3, 0) = eetrans[0];
+  tf(3, 1) = eetrans[1];
+  tf(3, 2) = eetrans[2];
+}
+
 //==============================================================================
 std::vector<bool> getFreeJointFlags(
     int numParams, int numFreeParams, const int* freeParams)
@@ -232,12 +255,88 @@ auto IkFast::getDofs() const -> const std::vector<std::size_t>&
   return mDofs;
 }
 
+//==============================================================================
+const std::vector<std::size_t>& IkFast::getFreeDofs() const
+{
+  return mFreeDofs;
+}
+
 //==============================================================================
 bool IkFast::isConfigured() const
 {
   return mConfigured;
 }
 
+//==============================================================================
+std::size_t IkFast::getNumFreeParameters2() const
+{
+  return static_cast<std::size_t>(getNumFreeParameters());
+}
+
+//==============================================================================
+std::size_t IkFast::getNumJoints2() const
+{
+  return static_cast<std::size_t>(getNumJoints());
+}
+
+//==============================================================================
+IkFast::IkType IkFast::getIkType2() const
+{
+  // Following conversion is referred from:
+  // https://github.com/rdiankov/openrave/blob/b1ebe135b4217823ebdf56d9af5fe89b29723603/include/openrave/openrave.h#L575-L623
+
+  const int type = getIkType();
+
+  if (type == 0)
+    return IkType::UNKNOWN;
+  else if (type == 0x67000001)
+    return IkType::TRANSFORM_6D;
+  else if (type == 0x34000002)
+    return IkType::ROTATION_3D;
+  else if (type == 0x34000003)
+    return IkType::TRANSLATION_3D;
+  else if (type == 0x34000004)
+    return IkType::DIRECTION_3D;
+  else if (type == 0x34000005)
+    return IkType::RAY_4D;
+  else if (type == 0x34000006)
+    return IkType::LOOKAT_3D;
+  else if (type == 0x34000007)
+    return IkType::TRANSLATION_DIRECTION_5D;
+  else if (type == 0x34000008)
+    return IkType::TRANSLATION_XY_2D;
+  else if (type == 0x34000009)
+    return IkType::TRANSLATION_XY_ORIENTATION_3D;
+  else if (type == 0x3400000a)
+    return IkType::TRANSLATION_LOCAL_GLOBAL_6D;
+  else if (type == 0x3400000b)
+    return IkType::TRANSLATION_X_AXIS_ANGLE_4D;
+  else if (type == 0x3400000c)
+    return IkType::TRANSLATION_Y_AXIS_ANGLE_4D;
+  else if (type == 0x3400000d)
+    return IkType::TRANSLATION_Z_AXIS_ANGLE_4D;
+  else if (type == 0x3400000e)
+    return IkType::TRANSLATION_X_AXIS_ANGLE_Z_NORM_4D;
+  else if (type == 0x3400000f)
+    return IkType::TRANSLATION_Y_AXIS_ANGLE_X_NORM_4D;
+  else if (type == 0x34000010)
+    return IkType::TRANSLATION_Z_AXIS_ANGLE_Y_NORM_4D;
+
+  return IkType::UNKNOWN;
+}
+
+//==============================================================================
+const std::string IkFast::getKinematicsHash2() const
+{
+  return const_cast<IkFast*>(this)->getKinematicsHash();
+}
+
+//==============================================================================
+std::string IkFast::getIkFastVersion2() const
+{
+  return const_cast<IkFast*>(this)->getIkFastVersion();
+}
+
 //==============================================================================
 void IkFast::configure() const
 {
@@ -268,7 +367,7 @@ void IkFast::configure() const
   if (!checkDofMapValidity(mIK.get(), mFreeDofs, "free dof map"))
     return;
 
-  mFreeParams.resize(ikFastNumFreeJoints);
+  mFreeParams.resize(static_cast<std::size_t>(ikFastNumFreeJoints));
 
   mConfigured = true;
 }
@@ -321,5 +420,27 @@ auto IkFast::computeSolutions(const Eigen::Isometry3d& desiredBodyTf)
   return mSolutions;
 }
 
+//==============================================================================
+Eigen::Isometry3d IkFast::computeFk(const Eigen::VectorXd& parameters)
+{
+  const std::size_t ikFastNumNonFreeJoints
+      = getNumJoints2() - getNumFreeParameters2();
+  if (static_cast<std::size_t>(parameters.size()) != ikFastNumNonFreeJoints)
+  {
+    dtwarn << "[IkFast::computeFk] The dimension of given joint positions "
+           << "doesn't agree with the number of joints of this IkFast solver. "
+           << "Returning identity.\n";
+    return Eigen::Isometry3d::Identity();
+  }
+
+  std::array<IkReal, 3> eetrans;
+  std::array<IkReal, 9> eerot;
+  computeFk(parameters.data(), eetrans.data(), eerot.data());
+
+  Eigen::Isometry3d tf;
+  convertTransform(eetrans, eerot, tf);
+  return tf;
+}
+
 } // namespace dynamics
 } // namespace dart

dart/dynamics/IkFast.hpp

@@ -50,6 +50,85 @@ namespace dynamics {
 class IkFast : public InverseKinematics::Analytical
 {
 public:
+  /// Inverse kinematics types supported by IkFast
+  // Following conversion is referred from:
+  // https://github.com/rdiankov/openrave/blob/b1ebe135b4217823ebdf56d9af5fe89b29723603/include/openrave/openrave.h#L575-L623
+  enum IkType
+  {
+    /// End effector reaches desired 6D transformation
+    TRANSFORM_6D,
+
+    /// End effector reaches desired 3D rotation
+    ROTATION_3D,
+
+    /// End effector origin reaches desired 3D translation
+    TRANSLATION_3D,
+
+    /// Direction on end effector coordinate system reaches desired direction
+    DIRECTION_3D,
+
+    /// Ray on end effector coordinate system reaches desired global ray
+    RAY_4D,
+
+    /// Direction on end effector coordinate system points to desired 3D
+    /// position
+    LOOKAT_3D,
+
+    /// End effector origin and direction reaches desired 3D translation and
+    /// direction. Can be thought of as Ray IK where the origin of the ray must
+    /// coincide.
+    TRANSLATION_DIRECTION_5D,
+
+    /// End effector origin reaches desired XY translation position, Z is
+    /// ignored. The coordinate system with relative to the base link.
+    TRANSLATION_XY_2D,
+
+    /// 2D translation along XY plane and 1D rotation around Z axis. The offset
+    /// of the rotation is measured starting at +X, so at +X is it 0, at +Y it
+    /// is pi/2.
+    TRANSLATION_XY_ORIENTATION_3D,
+
+    /// Local point on end effector origin reaches desired 3D global point.
+    /// Because both local point and global point can be specified, there are 6
+    /// values.
+    TRANSLATION_LOCAL_GLOBAL_6D,
+
+    /// End effector origin reaches desired 3D translation, manipulator
+    /// direction makes a specific angle with x-axis (defined in the
+    /// manipulator base link’s coordinate system)
+    TRANSLATION_X_AXIS_ANGLE_4D,
+
+    /// End effector origin reaches desired 3D translation, manipulator
+    /// direction makes a specific angle with y-axis (defined in the
+    /// manipulator base link’s coordinate system)
+    TRANSLATION_Y_AXIS_ANGLE_4D,
+
+    /// End effector origin reaches desired 3D translation, manipulator
+    /// direction makes a specific angle with z-axis (defined in the
+    /// manipulator base link’s coordinate system)
+    TRANSLATION_Z_AXIS_ANGLE_4D,
+
+    /// End effector origin reaches desired 3D translation, manipulator
+    /// direction needs to be orthogonal to z-axis and be rotated at a certain
+    /// angle starting from the x-axis (defined in the manipulator base link's
+    /// coordinate system)
+    TRANSLATION_X_AXIS_ANGLE_Z_NORM_4D,
+
+    /// End effector origin reaches desired 3D translation, manipulator
+    /// direction needs to be orthogonal to z-axis and be rotated at a certain
+    /// angle starting from the y-axis (defined in the manipulator base link's
+    /// coordinate system)
+    TRANSLATION_Y_AXIS_ANGLE_X_NORM_4D,
+
+    /// End effector origin reaches desired 3D translation, manipulator
+    /// direction needs to be orthogonal to z-axis and be rotated at a certain
+    /// angle starting from the z-axis (defined in the manipulator base link's
+    /// coordinate system)
+    TRANSLATION_Z_AXIS_ANGLE_Y_NORM_4D,
+
+    UNKNOWN,
+  };
+
   /// Constructor
   ///
   /// \param[in] ik The parent InverseKinematics solver that is associated with
@@ -70,39 +149,90 @@ class IkFast : public InverseKinematics::Analytical
       const Analytical::Properties& properties = Analytical::Properties());
 
   // Documentation inherited.
-  auto computeSolutions(const Eigen::Isometry3d& desiredBodyTf)
-      -> const std::vector<InverseKinematics::Analytical::Solution>& override;
+  const std::vector<InverseKinematics::Analytical::Solution>& computeSolutions(
+      const Eigen::Isometry3d& desiredBodyTf) override;
 
-  // Documentation inherited.
-  auto getDofs() const -> const std::vector<std::size_t>& override;
+  /// Computes forward kinematics given joint positions where the dimension is
+  /// the same as getNumJoints2().
+  Eigen::Isometry3d computeFk(const Eigen::VectorXd& parameters);
+
+  /// Returns the indices of the DegreeOfFreedoms that are part of the joints
+  /// that IkFast solves for.
+  const std::vector<std::size_t>& getDofs() const override;
+
+  /// Returns the indices of the DegreeOfFreedoms that are part of the joints
+  /// that IkFast solves but the values should be set by the user in prior.
+  const std::vector<std::size_t>& getFreeDofs() const;
 
   /// Returns true if this IkFast is ready to solve.
   virtual bool isConfigured() const;
 
+  /// Returns the number of free parameters users has to set apriori.
+  std::size_t getNumFreeParameters2() const;
+  // TODO(JS): Rename to getNumFreeParameters() in DART 7
+
+  /// Returns the total number of indices of the chane.
+  std::size_t getNumJoints2() const;
+  // TODO(JS): Rename to getNumJoints in DART 7
+
+  /// Returns the IK type.
+  IkType getIkType2() const;
+  // TODO(JS): Rename to getIkType() in DART 7
+
+  /// Returns a hash of all the chain values used for double checking that the
+  /// correct IK is used.
+  const std::string getKinematicsHash2() const;
+  // TODO(JS): Rename to getKinematicsHash() in DART 7
+
+  /// Returns the IkFast version used to generate this file
+  std::string getIkFastVersion2() const;
+  // TODO(JS): Rename to getIkFastVersion() in DART 7
+
 protected:
+  /// Returns the number of free parameters users has to set apriori.
   virtual int getNumFreeParameters() const = 0;
+  // TODO(JS): Remove in DART 7
+
+  /// Returns the indicies of the free parameters indexed by the chain joints.
   virtual int* getFreeParameters() const = 0;
+  // TODO(JS): Remove in DART 7
+
+  /// Returns the total number of indices of the chane.
   virtual int getNumJoints() const = 0;
+  // TODO(JS): Remove in DART 7
+
+  /// Returns the size in bytes of the configured number type.
   virtual int getIkRealSize() const = 0;
+
+  /// Returns the IK type.
   virtual int getIkType() const = 0;
 
   /// Computes the inverse kinematics solutions using the generated IKFast code.
   virtual bool computeIk(
-      const IkReal* mTargetTranspose,
-      const IkReal* mTargetRotation,
+      const IkReal* targetTranspose,
+      const IkReal* targetRotation,
       const IkReal* pfree,
       ikfast::IkSolutionListBase<IkReal>& solutions)
       = 0;
 
+  /// Computes the forward kinematics solutions using the generated IKFast code.
+  virtual void computeFk(
+      const IkReal* parameters, IkReal* targetTranspose, IkReal* targetRotation)
+      = 0;
+
+  /// Returns a hash of all the chain values used for double checking that the
+  /// correct IK is used.
   virtual const char* getKinematicsHash() = 0;
+  // TODO(JS): Rename in DART 7
 
+  /// Returns the IkFast version used to generate this file
   virtual const char* getIkFastVersion() = 0;
+  // TODO(JS): Rename in DART 7
 
   /// Configure IkFast. If it's successfully configured, isConfigured() returns
   /// true.
   virtual void configure() const;
 
-protected:
   mutable std::vector<double> mFreeParams;
 
   /// True if this IkFast is ready to solve.