WIP: Create diff ik plugin

CMakeLists.txt

@@ -1,16 +1,25 @@
-cmake_minimum_required(VERSION 3.10.2)
-project(moveit_drake)
+cmake_minimum_required(VERSION 3.22)
+project(moveit_drake LANGUAGES CXX)
+
+list(APPEND CMAKE_PREFIX_PATH /opt/drake/lib)
+find_package(drake REQUIRED)
+
 find_package(ament_cmake REQUIRED)
 
 # Common cmake code applied to all moveit packages
 find_package(moveit_common REQUIRED)
 moveit_package()
 
 find_package(ament_cmake REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(class_loader REQUIRED)
+find_package(generate_parameter_library REQUIRED)
 find_package(moveit_core REQUIRED)
+find_package(moveit_msgs REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(moveit_visual_tools REQUIRED)
+find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rviz_visual_tools REQUIRED)
 find_package(warehouse_ros REQUIRED)
@@ -24,6 +33,16 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
     rclcpp
     rviz_visual_tools
     warehouse_ros)
+
+include_directories(include)
+
+add_library(moveit_diff_ik_plugin SHARED src/diff_ik_plugin.cpp)
+
+ament_target_dependencies(moveit_diff_ik_plugin rclcpp pluginlib moveit_core
+                          moveit_msgs EIGEN3)
+
+target_link_libraries(moveit_diff_ik_plugin drake::drake)
+
 add_subdirectory(demo)
 
 ament_package()

README.md

@@ -35,4 +35,4 @@ ros2 launch moveit_drake pipeline_testbench.launch.py
 
 ### Development
 
-- Use [pre-commit to format your code](https://moveit.ros.org/documentation/contributing/code/#pre-commit-formatting-checks)
+- Use [pre-commit to format your code](https://moveit.ros.org/documentation/contributing/code/#pre-commit-formatting-checks)

include/moveit/drake/diff_ik_plugin.hpp

@@ -0,0 +1,119 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024, PickNik Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Sebastian Jahr
+   Desc: TODO */
+
+#pragma once
+
+// ROS2
+#include <rclcpp/rclcpp.hpp>
+
+// System
+#include <memory>
+
+// ROS msgs
+#include <geometry_msgs/msg/pose.hpp>
+#include <moveit_msgs/msg/kinematic_solver_info.hpp>
+#include <moveit_msgs/msg/move_it_error_codes.hpp>
+
+// MoveIt
+#include <moveit/kinematics_base/kinematics_base.h>
+#include <moveit/robot_state/robot_state.h>
+
+namespace diff_ik_plugin
+{
+/**
+ * @brief TODO
+ */
+class DiffIKPlugin : public kinematics::KinematicsBase
+{
+public:
+  DiffIKPlugin();
+
+  bool
+  getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state,
+                std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
+                const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override;
+
+  bool searchPositionIK(
+      const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
+      std::vector<double>& solution, moveit_msgs::msg::MoveItErrorCodes& error_code,
+      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override;
+
+  bool searchPositionIK(
+      const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
+      const std::vector<double>& consistency_limits, std::vector<double>& solution,
+      moveit_msgs::msg::MoveItErrorCodes& error_code,
+      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override;
+
+  bool searchPositionIK(
+      const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
+      std::vector<double>& solution, const IKCallbackFn& solution_callback,
+      moveit_msgs::msg::MoveItErrorCodes& error_code,
+      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override;
+
+  bool searchPositionIK(
+      const geometry_msgs::msg::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
+      const std::vector<double>& consistency_limits, std::vector<double>& solution,
+      const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code,
+      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override;
+
+  bool searchPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
+                        double timeout, const std::vector<double>& consistency_limits, std::vector<double>& solution,
+                        const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code,
+                        const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
+                        const moveit::core::RobotState* context_state = nullptr) const override;
+
+  bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
+                     std::vector<geometry_msgs::msg::Pose>& poses) const override;
+
+  bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model,
+                  const std::string& group_name, const std::string& base_name,
+                  const std::vector<std::string>& tip_frames, double search_discretization) override;
+
+  /**
+   * @brief  Return all the joint names in the order they are used internally
+   */
+  const std::vector<std::string>& getJointNames() const override;
+
+  /**
+   * @brief  Return all the link names in the order they are represented internally
+   */
+  const std::vector<std::string>& getLinkNames() const override;
+
+protected:
+  bool setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices) override;
+};
+}  // namespace diff_ik_plugin

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

@@ -9,21 +9,23 @@
   <license>BSD</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>moveit_common</depend>
 
+  <depend>class_loader</depend>
+  <depend>eigen</depend>
+  <depend>generate_parameter_library</depend>
   <depend>moveit_core</depend>
+  <depend>moveit_msgs</depend>
+  <depend>moveit_ros_planning</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>