Add RobotState::getJacobianDerivative (moveit#3444)

Co-authored-by: Robert Haschke <[email protected]>

moveit_core/robot_state/CMakeLists.txt

@@ -40,6 +40,11 @@ if(CATKIN_ENABLE_TESTING)
     test/test_cartesian_interpolator.cpp)
   target_link_libraries(test_cartesian_interpolator ${MOVEIT_LIB_NAME} moveit_test_utils)
 
+  add_rostest_gtest(test_jacobian_derivative
+    test/test_jacobian_derivative.test
+    test/test_jacobian_derivative.cpp)
+  target_link_libraries(test_jacobian_derivative ${MOVEIT_LIB_NAME} moveit_test_utils)
+
   catkin_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp)
   target_link_libraries(test_robot_state_complex ${MOVEIT_LIB_NAME} moveit_test_utils)
 

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

@@ -1173,6 +1173,35 @@ class RobotState
     return static_cast<const RobotState*>(this)->getJacobian(group, reference_point_position);
   }
 
+  /** \brief Compute the time derivative of the Jacobian with reference to a particular point on a given link, for a
+   * specified group.
+   *
+   * Based on KDL's ChainJntToJacDotSolver implementation and:
+   * Symbolic differentiation of the velocity mapping for a serial kinematic chain
+   * H. Bruyninckx, J. De Schutter
+   * doi:10.1016/0094-114X(95)00069-B
+   * url : http://www.sciencedirect.com/science/article/pii/0094114X9500069B
+   *
+   * \param group The group to compute the Jacobian time derivative for
+   * \param link The link model to compute the Jacobian time derivative for
+   * \param reference_point_position The reference point position (with respect to the link specified in link)
+   * \param jacobian The resultant jacobian
+   * \param jacobian_derivative The resultant jacobian time derivative
+   * \return True if jacobian time derivative was successfully computed, false otherwise
+   */
+  bool getJacobianDerivative(const JointModelGroup* group, const LinkModel* link,
+                             const Eigen::Vector3d& reference_point_position, Eigen::MatrixXd& jacobian,
+                             Eigen::MatrixXd& jacobian_derivative) const;
+
+  /** \brief Compute the partial derivative of a column of the Jacobian wrt a single joint.
+   * \param jacobian The Jacobian matrix
+   * \param column_index Column of the Jacobian to compute the partial derivative for
+   * \param joint_index Joint index to compute the partial derivative with respect to
+   * \return Partial derivative of a Jacobian column wrt a single joint
+   */
+  static Eigen::Matrix<double, 6, 1> getJacobianColumnPartialDerivative(const Eigen::MatrixXd& jacobian,
+                                                                        int column_index, int joint_index);
+
   /** \brief Given a twist for a particular link (\e tip), compute the corresponding velocity for every variable and
    * store it in \e qdot */
   void computeVariableVelocity(const JointModelGroup* jmg, Eigen::VectorXd& qdot, const Eigen::VectorXd& twist,

moveit_core/robot_state/src/robot_state.cpp

@@ -1413,6 +1413,78 @@ bool RobotState::getJacobian(const JointModelGroup* group, const LinkModel* link
   return true;
 }
 
+bool RobotState::getJacobianDerivative(const JointModelGroup* group, const LinkModel* link,
+                                       const Eigen::Vector3d& reference_point_position, Eigen::MatrixXd& jacobian,
+                                       Eigen::MatrixXd& jacobian_derivative) const
+{
+  const int rows = 6;
+  const int columns = group->getVariableCount();
+  jacobian_derivative.setZero(rows, columns);
+
+  // Calculate the Jacobian with use_quaternion_representation = false
+  if (!getJacobian(group, link, reference_point_position, jacobian, false))
+  {
+    ROS_ERROR_NAMED(LOGNAME, "Jacobian computation failed");
+    return false;
+  }
+
+  auto velocities = getJointVelocities(group->getJointModels()[0]);
+
+  while (link)
+  {
+    const JointModel* pjm = link->getParentJointModel();
+    if (pjm->getVariableCount() > 0)
+    {
+      if (!group->hasJointModel(pjm->getName()))
+      {
+        link = pjm->getParentLinkModel();
+        continue;
+      }
+      unsigned int current_joint_index = group->getVariableGroupIndex(pjm->getName());
+      if (pjm->getType() == moveit::core::JointModel::REVOLUTE || pjm->getType() == moveit::core::JointModel::PRISMATIC)
+      {
+        // iterate over all joints, pd_joint_index - partial derivative joint index
+        for (unsigned int pd_joint_index = 0; pd_joint_index < group->getVariableCount(); pd_joint_index++)
+        {
+          jacobian_derivative.col(current_joint_index) +=
+              getJacobianColumnPartialDerivative(jacobian, current_joint_index, pd_joint_index) *
+              velocities[pd_joint_index];
+        }
+      }
+      else
+        ROS_ERROR_NAMED(LOGNAME, "Unsupported type of joint in Jacobian derivative computation");
+    }
+    if (pjm == group->getJointModels()[0])
+      break;
+    link = pjm->getParentLinkModel();
+  }
+  return true;
+}
+
+Eigen::Matrix<double, 6, 1> RobotState::getJacobianColumnPartialDerivative(const Eigen::MatrixXd& jacobian,
+                                                                           int column_index, int joint_index)
+{
+  // Twist is [v omega]^T
+  const Eigen::Matrix<double, 6, 1>& jac_j = jacobian.col(joint_index);
+  const Eigen::Matrix<double, 6, 1>& jac_i = jacobian.col(column_index);
+
+  Eigen::Matrix<double, 6, 1> t_djdq = Eigen::Matrix<double, 6, 1>::Zero();
+
+  if (joint_index <= column_index)
+  {
+    // ref (20)
+    const Eigen::Vector3d& jac_j_angular = jac_j.segment<3>(3);
+    t_djdq.segment<3>(0) = jac_j_angular.cross(jac_i.segment<3>(0));
+    t_djdq.segment<3>(3) = jac_j_angular.cross(jac_i.segment<3>(3));
+  }
+  else if (joint_index > column_index)
+  {
+    // ref (23)
+    t_djdq.segment<3>(0) = -jac_j.segment<3>(0).cross(jac_i.segment<3>(3));
+  }
+  return t_djdq;
+}
+
 bool RobotState::setFromDiffIK(const JointModelGroup* jmg, const Eigen::VectorXd& twist, const std::string& tip,
                                double dt, const GroupStateValidityCallbackFn& constraint)
 {