Kinematic Trajectory Optimization Skeleton PR

.docker/Dockerfile

@@ -47,29 +47,24 @@ RUN apt update && \
 
 # Setup the workspace
 WORKDIR ${WORKSPACE}
+RUN mkdir -p src
 
-RUN mkdir -p src && \
-    cd src && \
-    git clone https://github.com/moveit/moveit2.git -b main && \
-    for repo in moveit2/moveit2.repos \
-        $(f="moveit2/moveit2_${ROS_DISTRO}.repos"; test -r $f && echo $f); \
-        do vcs import < "$repo"; done && \
-    apt update && \
-    . /opt/ros/${ROS_DISTRO}/setup.sh && \
-    rosdep install -r --from-paths . --ignore-src --rosdistro ${ROS_DISTRO} -y
+# Install dependencies of this package
+# NOTE: Copying only the necessary files to not bust cache as much.
+COPY moveit_drake.repos ${WORKSPACE}/src/moveit_drake/
+WORKDIR ${WORKSPACE}/src/
+RUN vcs import < moveit_drake/moveit_drake.repos
 
-RUN . /opt/ros/${ROS_DISTRO}/setup.sh && \
-    apt remove ros-$ROS_DISTRO-moveit*
+WORKDIR ${WORKSPACE}
+COPY package.xml ${WORKSPACE}/src/moveit_drake/
+RUN apt update && \
+    . /opt/ros/${ROS_DISTRO}/setup.sh && \
+    rosdep install -r --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} -y && \
+    rm -rf src/moveit_drake
 
 RUN . /opt/ros/${ROS_DISTRO}/setup.sh && \
     colcon build --mixin release --parallel-workers 1
 
-# Install any other dependencies of this package
-COPY . ${WORKSPACE}/src/moveit_drake
-RUN apt update && \
-    . /opt/ros/${ROS_DISTRO}/setup.sh && \
-    rosdep install -r --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} -y
-
 # Set up the entrypoint
 COPY .docker/ros_entrypoint.sh /sbin/ros_entrypoint.sh
 RUN echo "source /sbin/ros_entrypoint.sh" >> ~/.bashrc

CMakeLists.txt

@@ -1,20 +1,32 @@
-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 CONFIG REQUIRED PATHS /opt/drake)
+
 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)
 
+generate_parameter_library(ktopt_moveit_parameters
+                           res/ktopt_moveit_parameters.yaml)
+
 set(THIS_PACKAGE_INCLUDE_DEPENDS
     ament_cmake
     moveit_core
@@ -24,6 +36,38 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
     rclcpp
     rviz_visual_tools
     warehouse_ros)
+
+include_directories(include)
+
+# ktopt planning plugin
+add_library(moveit_ktopt_planner_plugin SHARED src/ktopt_planner_manager.cpp
+                                               src/ktopt_planning_context.cpp)
+
+ament_target_dependencies(moveit_ktopt_planner_plugin rclcpp pluginlib
+                          moveit_core moveit_msgs EIGEN3)
+target_link_libraries(moveit_ktopt_planner_plugin drake::drake
+                      ktopt_moveit_parameters)
+
+# Ensure that the plugin finds libdrake.so at runtime
+set_target_properties(
+  moveit_ktopt_planner_plugin PROPERTIES INSTALL_RPATH "/opt/drake/lib"
+                                         BUILD_RPATH "/opt/drake/lib")
+
+pluginlib_export_plugin_description_file(moveit_core
+                                         ktopt_interface_plugin_description.xml)
+
+install(
+  TARGETS moveit_ktopt_planner_plugin ktopt_moveit_parameters
+  EXPORT moveit_ktopt_planner_pluginTargets
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES
+  DESTINATION include/ktopt_interface)
+
+ament_export_targets(moveit_ktopt_planner_pluginTargets HAS_LIBRARY_TARGET)
+ament_export_dependencies(moveit_core drake::drake generate_parameter_library)
+
 add_subdirectory(demo)
 
 ament_package()

README.md

@@ -33,22 +33,22 @@ Follow [instructions](#build-moveit_drake) below to build `moveit_drake`
 
 ### Build `moveit_drake`
 
-Follow the [MoveIt Source Build](https://moveit.ros.org/install-moveit2/source/) instructions to set up a colcon workspace with MoveIt from the source.
+Follow the [MoveIt Source Build](https://moveit.ros.org/install-moveit2/source/) instructions to set up a colcon workspace with MoveIt from source.
 
 Open a command line to your colcon workspace:
 
-    cd YOUR_WORKSPACE/src
+    cd ${WORKSPACE}/src
 
 Download the MoveIt Tutorials source code:
 
     git clone https://github.com/moveit/moveit_drake.git
     vcs import < moveit_drake/moveit_drake.repos
-    rosdep install -r --from-paths . --ignore-src --rosdistro rolling -y
+    rosdep install -r --from-paths . --ignore-src --rosdistro ${ROS_DISTRO} -y
 
 Configure and build the workspace:
 
-    cd $COLCON_WS
-    nice colcon build --event-handlers desktop_notification- status- --cmake-args -DCMAKE_BUILD_TYPE=Release
+    cd ${WORKSPACE}
+    colcon build --event-handlers desktop_notification- status- --cmake-args -DCMAKE_BUILD_TYPE=Release
 
 ### Run the demo
 
@@ -59,3 +59,32 @@ 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)
+
+# Todo section
+
+This section keeps a list of immediate todos, will be deleted before repo release
+
+- [x] Create drake planning pipeline option in `pipeline_testbench.launch.py`
+- [x] Declare to moveit, to use the drake ktopt planning pipeline
+- [ ] Build planner manager and planning context to display info from `moveit`
+  and `drake` instance.
+    - [ ] Generated placeholder classes mimicking `stomp` implementation.
+    - [ ] Display info messages during testbench runtime.
+    - [ ]
+- [ ] read Robot description and display onto drake visualizer
+
+### Doubts
+- [x] stomp_moveit::ParamListener, where is this being declared
+- [ ] Why is the parameter file in the "res" directory
+
+### Potential issues
+- Assumes that planner managers initialize will set robot description before a
+  call to getPlanningContext.
+
+### Some helper commands
+To just rebuild `moveit_drake`
+```
+rm -rf build/moveit_drake
+rm -rf install/moveit_drake
+colcon build --packages-select moveit_drake
+```

demo/config/testbench_moveit_cpp.yaml

@@ -10,7 +10,7 @@ planning_scene_monitor_options:
 
 # Define which planning pipelines are loaded by moveit_cpp. These loaded pipelines will be available for simple and parallel planning
 planning_pipelines:
-  pipeline_names: ["stomp", "ompl", "ompl_stomp"]
+  pipeline_names: ["stomp", "ompl", "drake"]
 
 plan_request_params:
   planning_attempts: 1

demo/launch/pipeline_testbench.launch.py

@@ -98,15 +98,33 @@ def launch_setup(context, *args, **kwargs):
         }
     }
 
-    # MoveItCpp demo executable
+    drake_ktopt_planning_pipeline_config = {
+        "drake": {
+            "planning_plugins": [
+                "ktopt_interface/KTOptPlanner",
+            ],
+            "request_adapters": [
+                "default_planning_request_adapters/ResolveConstraintFrames",
+                "default_planning_request_adapters/ValidateWorkspaceBounds",
+                "default_planning_request_adapters/CheckStartStateBounds",
+                "default_planning_request_adapters/CheckStartStateCollision",
+            ],
+            "response_adapters": [
+                "default_planning_response_adapters/ValidateSolution",
+                "default_planning_response_adapters/DisplayMotionPath",
+            ],
+        }
+    }
+
+    # MoveitCpp demo with drake
     moveit_cpp_node = Node(
         name="pipeline_testbench_example",
         package="moveit_drake",
         executable="pipeline_testbench_example",
         output="screen",
         parameters=[
             moveit_config.to_dict(),
-            ompl_stomp_planning_pipeline_config,
+            drake_ktopt_planning_pipeline_config,
             warehouse_ros_config,
         ],
     )

include/ktopt_interface/ktopt_interface.hpp

@@ -0,0 +1 @@
+#pragma once

include/ktopt_interface/ktopt_planning_context.hpp

@@ -0,0 +1,57 @@
+#pragma once
+
+#include <moveit/planning_interface/planning_interface.h>
+#include <ktopt_moveit_parameters.hpp>
+
+// relevant drake includes
+#include "drake/multibody/parsing/parser.h"
+#include "drake/geometry/scene_graph.h"
+#include "drake/systems/framework/diagram.h"
+#include "drake/systems/framework/diagram_builder.h"
+#include "drake/multibody/plant/multibody_plant.h"
+#include "drake/planning/trajectory_optimization/kinematic_trajectory_optimization.h"
+#include "drake/solvers/solve.h"
+
+namespace ktopt_interface
+{
+// declare all namespaces to be used
+using drake::multibody::MultibodyPlant;
+using drake::multibody::AddMultibodyPlantSceneGraph;
+using drake::geometry::SceneGraph;
+using drake::systems::DiagramBuilder;
+using drake::multibody::Parser;
+using drake::planning::trajectory_optimization::KinematicTrajectoryOptimization;
+using drake::solvers::Solve;
+using drake::systems::Diagram;
+using drake::systems::Context;
+
+class KTOptPlanningContext : public planning_interface::PlanningContext
+{
+public:
+    KTOptPlanningContext(
+        const std::string& name,
+        const std::string& group_name,
+        const ktopt_interface::Params& params);
+
+    void solve(
+        planning_interface::MotionPlanResponse& res) override;
+    void solve(
+        planning_interface::MotionPlanDetailedResponse& res) override;
+
+    bool terminate() override;
+    void clear() override;
+
+    void setRobotDescription(std::string robot_description);
+
+private:
+    const ktopt_interface::Params params_;
+    std::string robot_description_;
+
+    // drake related variables
+    SceneGraph<double>* scene_graph_{};
+    MultibodyPlant<double>* plant_{};
+    std::unique_ptr<Context<double>> diagram_context_;
+
+
+};
+} // namespace ktopt_interface

ktopt_interface_plugin_description.xml

@@ -0,0 +1,9 @@
+<library  path="moveit_ktopt_planner_plugin">
+  <class name="ktopt_interface/KTOptPlanner"
+         type="ktopt_interface::KTOptPlannerManager"
+         base_class_type="planning_interface::PlannerManager">
+    <description>
+      A Drake Kinematic Trajectory Optimization plugin
+    </description>
+  </class>
+</library>

package.xml

@@ -36,5 +36,6 @@
 
   <export>
       <build_type>ament_cmake</build_type>
+      <moveit_core plugin="${prefix}/ktopt_interface_plugin_description.xml"/>
   </export>
 </package>

res/ktopt_moveit_parameters.yaml

@@ -0,0 +1,9 @@
+ktopt_interface:
+  num_iterations: {
+    type: int,
+    description: "dummy",
+    default_value: 1000,
+    validation: {
+      gt_eq<>: [1]
+    }
+  }