Sketch Diff IK algo

moveit_drake.repos

@@ -22,4 +22,4 @@ repositories:
   moveit_benchmark_resources:
     type: git
     url: https://github.com/moveit/moveit_benchmark_resources
-    version: main
+    version: main

package.xml

@@ -11,28 +11,21 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>moveit_common</depend>
 
-  <depend>moveit_core</depend>
   <depend>class_loader</depend>
-  <depend>pluginlib</depend>
-  <depend>generate_parameter_library</depend>
   <depend>eigen</depend>
+  <depend>generate_parameter_library</depend>
+  <depend>moveit_core</depend>
   <depend>moveit_msgs</depend>
   <depend>moveit_ros_planning</depend>
-
-  <depend>moveit_core</depend>
   <depend>moveit_visual_tools</depend>
+  <depend>pluginlib</depend>
   <depend>rviz_visual_tools</depend>
 
-  <build_depend>eigen</build_depend>
-  <build_depend>moveit_common</build_depend>
-  <build_depend>moveit_ros_planning</build_depend>
-
   <exec_depend>controller_manager</exec_depend>
   <exec_depend>gripper_controllers</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>joint_state_publisher</exec_depend>
   <exec_depend>joint_trajectory_controller</exec_depend>
-  <exec_depend>moveit</exec_depend>
   <exec_depend>moveit_resources_panda_moveit_config</exec_depend>
   <exec_depend>moveit_configs_utils</exec_depend>
   <exec_depend>moveit_py</exec_depend>

src/diff_ik_plugin.cpp

@@ -140,11 +140,13 @@ bool DiffIKPlugin::searchPositionIK(const std::vector<geometry_msgs::msg::Pose>&
                                     const IKCallbackFn& solution_callback,
                                     moveit_msgs::msg::MoveItErrorCodes& error_code,
                                     const kinematics::KinematicsQueryOptions& /*options*/,
-                                    const moveit::core::RobotState* /*context_state*/) const
+                                    const moveit::core::RobotState* context_state) const
 {
   // TODO
   using namespace drake::multibody;
 
+  // TODO Use multibody plant?
+
   // TODO: This explicitly assumes that num_positions == num_velocities, which
   // would not be true in the case of e.g. quaternion axes. How does MoveIt
   // handle a reduced-DoF tangent space?
@@ -156,11 +158,6 @@ bool DiffIKPlugin::searchPositionIK(const std::vector<geometry_msgs::msg::Pose>&
   Eigen::VectorXd v_current;
   current_state_->copyJointGroupVelocities(joint_model_group_, v_current);
 
-  // TODO: Unclear the exact specifications of this Jacobian: w.r.t. what
-  // reference frame? Analytical or geometric Jacobian? Seems to work OK but
-  // should be clarified.
-  const Eigen::MatrixXd& J = jacobian;
-
   // TODO: The DoDifferentialInverseKinematics function can handle many
   // addition costs and constraints, e.g.:
   // - joint position limits
@@ -179,25 +176,43 @@ bool DiffIKPlugin::searchPositionIK(const std::vector<geometry_msgs::msg::Pose>&
   params.set_joint_velocity_limits({ -v_max, v_max });
   params.set_joint_acceleration_limits({ -vdot_max, vdot_max });
 
-  const auto result = DoDifferentialInverseKinematics(q_current, v_current, delta_x, J, params);
-  if (result.status == DifferentialInverseKinematicsStatus::kSolutionFound)
-  {
-    delta_theta_ = *result.joint_velocities;
-  }
-  else if (result.status == DifferentialInverseKinematicsStatus::kStuck)
+  double error = std::numeric_limits<double>::infinity();
+  // TODO: Expose as parameter + redefine termination criteria
+  while (error > 0.1)
   {
-    // TODO: In this result joint velocities are still generated, they are
-    // just not guaranteed to follow the Cartesian target velocity. Is it
-    // acceptable to still use them? It may be the only way to get unstuck.
-    RCLCPP_WARN(LOGGER, "Differential inverse kinematics is stuck!");
-    return false;
+    // Error between target pose and FK of current pose
+
+    // Calculate Jacobian
+    // TODO: Unclear the exact specifications of this Jacobian: w.r.t. what
+    // reference frame? Analytical or geometric Jacobian? Seems to work OK but
+    // should be clarified.
+    const Eigen::MatrixXd& jacobian;
+    context_state.getJacobian(joint_model_group_ /* reference position? */);
+
+    // Calculate generalized velocities
+    const auto result = DoDifferentialInverseKinematics(q_current, v_current, error, jacobian, params);
+
+    // Process result
+    switch (result.status)
+    {
+      case DifferentialInverseKinematicsStatus::kSolutionFound:
+        // Update
+        break;
+
+      case DifferentialInverseKinematicsStatus::kNoSolutionFound:
+        RCLCPP_WARN(LOGGER, "Differential inverse kinematics failed to converge to a solution!");
+        return false;
+      case DifferentialInverseKinematicsStatus::kStuck:
+        // TODO: In this result joint velocities are still generated, they are
+        // just not guaranteed to follow the Cartesian target velocity. Is it
+        // acceptable to still use them? It may be the only way to get unstuck.
+        RCLCPP_WARN(LOGGER, "Differential inverse kinematics is stuck, likely due to constraints.!");
+        return false;
+    }
   }
-  else if (result.status == DifferentialInverseKinematicsStatus::kNoSolutionFound)
-  {
-    RCLCPP_WARN(LOGGER, "Differential inverse kinematics failed to converge to a solution!");
-    return false;
-  }
-  RCLCPP_INFO(getLogger(), "IK Solver Succeeded!");
+
+  // Store result in solution
+
   return true;
 }
 
@@ -211,7 +226,7 @@ bool DiffIKPlugin::getPositionFK(const std::vector<std::string>& link_names, con
     return false;
   }
 
-  // TODO
+  // TODO Implement FK
   RCLCPP_ERROR(getLogger(), "Forward kinematics not implemented");
 
   return false;