Code cleanup and Docker fixes (#57)

Author: sea-bass

Diff for: .docker/Dockerfile

@@ -19,7 +19,9 @@ RUN apt-get update && \
         ca-certificates \
         gnupg \
         lsb-release \
+        python3-pip \
         wget -y && \
+    pip3 install --break-system-packages pre-commit && \
     wget -qO- https://drake-apt.csail.mit.edu/drake.asc | gpg --dearmor - \
         | sudo tee /etc/apt/trusted.gpg.d/drake.gpg >/dev/null && \
     echo "deb [arch=amd64] https://drake-apt.csail.mit.edu/$(lsb_release -cs) $(lsb_release -cs) main" \

Diff for: CMakeLists.txt

@@ -26,7 +26,7 @@ find_package(warehouse_ros REQUIRED)
 find_package(shape_msgs REQUIRED)
 
 generate_parameter_library(ktopt_moveit_parameters
-                           res/ktopt_moveit_parameters.yaml)
+                           parameters/ktopt_moveit_parameters.yaml)
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
     ament_cmake
@@ -44,7 +44,7 @@ include_directories(include)
 # ktopt planning plugin
 add_library(
   moveit_drake SHARED
-  # KTOPT
+  # Kinematic Trajectory Optimization (KTOpt)
   src/ktopt_planner_manager.cpp
   src/ktopt_planning_context.cpp
   # TOPPRA

Diff for: README.md

@@ -1,40 +1,39 @@
 # Experimental MoveIt 2 - Drake Integration
 
-NOTE: Experimental and will continue to have breaking changes until first
-release.
+> [!NOTE]
+> Experimental and will continue to have breaking changes until first release.
+
+`moveit_drake` brings together the vertical ROS integration of the [MoveIt 2](https://moveit.ai/) motion planning framework, with the Mathematical Programming interface within [Drake](https://drake.mit.edu/).
+This allows the user to setup motion planning as an optimization problem within ROS, with the rich specification of constraints and costs provided by `drake`.
 
-`moveit_drake` brings together the vertical ROS integration of the
-[MoveIt 2](https://moveit.ai/) motion planning framework, with the Mathematical
-Programming interface within [Drake](https://drake.mit.edu/). This allows the
-user to setup motion planning as an optimization problem within ROS, with the
-rich specification of constraints and costs provided by `drake`.
 
 ## Features
 
-- Exposes
-  [`KinematicTrajectoryOptimization`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1planning_1_1trajectory__optimization_1_1_kinematic_trajectory_optimization.html)
-  implementation in `drake`.
-- Exposes [`TOPPRA`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_toppra.html) implementation in `drake`.
+- Exposes [`KinematicTrajectoryOptimization`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1planning_1_1trajectory__optimization_1_1_kinematic_trajectory_optimization.html) implementation in `drake` as a motion planner.
+- Exposes [`TOPPRA`](https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_toppra.html) implementation in `drake` as a trajectory post-processing adapter.
 
 ## Docker Workflow (Preferred and tested)
 
 ### Requirements
-`docker` and `docker-compose` - Follow instructions
-[here](https://docs.docker.com/engine/install/ubuntu/).
+Docker and Docker Compose - Follow the instructions [here](https://docs.docker.com/engine/install/ubuntu/).
+Also, run the [Linux post-installation steps](https://docs.docker.com/engine/install/linux-postinstall/).
 
 ### Steps
-The following steps clone and build the base image that you will require to
-test/build/run/develop with the repo
+The following steps clone and build the base image that you will require to test/build/run/develop with the repo.
 
-    git clone https://github.com/moveit/moveit_drake.git
-    cd moveit_drake
-    docker compose build
+```bash
+git clone https://github.com/moveit/moveit_drake.git
+cd moveit_drake
+docker compose build
+```
 
 This should give you an image with `drake` and `moveit2`.
 Next, create a container with the following and create shell access.
 
-    docker compose up
-    docker compose exec -it moveit_drake bash
+```bash
+docker compose up
+docker compose exec -it moveit_drake bash
+```
 
 Follow [instructions](#build-moveit_drake) below to build `moveit_drake`
 
@@ -47,45 +46,62 @@ 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 source.
+Follow the [MoveIt Source Build](https://moveit.ros.org/install-moveit2/source/) instructions to set up a ROS 2 workspace with MoveIt from source.
 
-Open a command line to your colcon workspace:
+Open a command line and navigate to your workspace:
 
-    cd ${WORKSPACE}/src
+```bash
+cd ${WORKSPACE}/src
+```
 
-Download the MoveIt Tutorials source code:
+Clone this repo to your workspace, including upstream dependencies:
 
-    git clone https://github.com/moveit/moveit_drake.git
-    vcs import < moveit_drake/moveit_drake.repos
-    rosdep install -r --from-paths . --ignore-src --rosdistro ${ROS_DISTRO} -y
+```bash
+git clone https://github.com/moveit/moveit_drake.git
+vcs import < moveit_drake/moveit_drake.repos
+rosdep install -r --from-paths . --ignore-src --rosdistro ${ROS_DISTRO} -y
+```
 
-Configure and build the workspace (this will take some time, as it builds
-moveit):
+Configure and build the workspace (this will take some time, as it builds MoveIt):
 
-    cd ${WORKSPACE}
-    colcon build --event-handlers desktop_notification- status- --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers 1
+```bash
+cd ${WORKSPACE}
+colcon build --event-handlers desktop_notification- status- --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers 1
+```
 
-### Run the demo
 
-```
+## Examples
+
+The planning pipeline testbench compares `moveit_drake` planners with existing MoveIt planners such as OMPL and Pilz.
+
+```bash
 ros2 launch moveit_drake pipeline_testbench.launch.py
 ```
 
-### Development
+This interactive example shows constrained planning using the Drake KTOpt planner.
 
-- Use [pre-commit to format your
-  code](https://moveit.ros.org/documentation/contributing/code/#pre-commit-formatting-checks)
+```bash
+ros2 launch moveit_drake constrained_planning_demo.launch.py
+```
 
-Within the container you can run the following command to format the code
 
-    # inside the moveit_drake package
-    pre-commit run -a
+## Development
 
-### Some helper commands
-To just rebuild `moveit_drake`
+### Formatting
+
+We use [pre-commit](https://moveit.ros.org/documentation/contributing/code/#pre-commit-formatting-checks) to format the code in this repo.
+
+Within the container, you can run the following command to format the code:
+
+```bash
+cd src/moveit_drake
+pre-commit run -a
 ```
+
+### Some helper commands
+To rebuild only the `moveit_drake` package:
+
+```bash
 rm -rf build/moveit_drake install/moveit_drake
 colcon build --packages-select moveit_drake
 ```

Diff for: docker-compose.yaml

@@ -16,7 +16,7 @@ services:
     volumes:
       - /tmp/.X11-unix:/tml/.X11-unix:rw
       - ${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority
-      - ./:/root/workspace/src/moveit_drake/
+      - ./:/root/ws_moveit/src/moveit_drake/
     tty: true
     stdin_open: true
     network_mode: "host"

Diff for: include/ktopt_interface/ktopt_planning_context.hpp

@@ -1,103 +1,119 @@
 #pragma once
 
+#include <ktopt_moveit_parameters.hpp>
 #include <moveit/planning_interface/planning_interface.h>
 #include <shape_msgs/msg/solid_primitive.h>
-#include <ktopt_moveit_parameters.hpp>
 
 // relevant drake includes
-#include "drake/common/eigen_types.h"
-#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"
-#include "drake/geometry/meshcat.h"
-#include "drake/geometry/meshcat_visualizer.h"
-#include "drake/geometry/drake_visualizer.h"
-#include "drake/geometry/meshcat_params.h"
-#include "drake/geometry/geometry_frame.h"
-#include "drake/geometry/geometry_instance.h"
-#include "drake/geometry/geometry_roles.h"
-#include "drake/geometry/proximity_properties.h"
-#include "drake/visualization/visualization_config.h"
-#include "drake/visualization/visualization_config_functions.h"
+#include <drake/geometry/meshcat.h>
+#include <drake/geometry/meshcat_visualizer.h>
+#include <drake/geometry/scene_graph.h>
+#include <drake/planning/trajectory_optimization/kinematic_trajectory_optimization.h>
+#include <drake/systems/framework/diagram.h>
+#include <drake/systems/framework/diagram_builder.h>
 #include <drake/multibody/inverse_kinematics/minimum_distance_lower_bound_constraint.h>
 #include <drake/multibody/inverse_kinematics/position_constraint.h>
+#include <drake/multibody/parsing/parser.h>
+#include <drake/multibody/plant/multibody_plant.h>
 
 namespace ktopt_interface
 {
 // declare all namespaces to be used
-using drake::geometry::AddRigidHydroelasticProperties;
-using drake::geometry::Box;
-using drake::geometry::Cylinder;
-using drake::geometry::FrameId;
-using drake::geometry::GeometryFrame;
-using drake::geometry::GeometryId;
-using drake::geometry::GeometryInstance;
-using drake::geometry::IllustrationProperties;
-using drake::geometry::Meshcat;
-using drake::geometry::MeshcatParams;
-using drake::geometry::MeshcatVisualizer;
-using drake::geometry::MeshcatVisualizerParams;
-using drake::geometry::PerceptionProperties;
-using drake::geometry::ProximityProperties;
-using drake::geometry::Role;
-using drake::geometry::SceneGraph;
-using drake::geometry::Shape;
-using drake::geometry::SourceId;
-using drake::geometry::Sphere;
-using drake::math::RigidTransformd;
-using drake::multibody::AddMultibodyPlantSceneGraph;
-using drake::multibody::Frame;
 using drake::multibody::MinimumDistanceLowerBoundConstraint;
 using drake::multibody::MultibodyPlant;
-using drake::multibody::PackageMap;
-using drake::multibody::Parser;
 using drake::multibody::PositionConstraint;
 using drake::planning::trajectory_optimization::KinematicTrajectoryOptimization;
-using drake::solvers::Solve;
 using drake::systems::Context;
 using drake::systems::Diagram;
 using drake::systems::DiagramBuilder;
-using drake::visualization::ApplyVisualizationConfig;
-using drake::visualization::VisualizationConfig;
-using Eigen::Matrix3d;
-using Eigen::MatrixXd;
-using Eigen::Vector3d;
-using Eigen::VectorXd;
 using Joints = std::vector<const moveit::core::JointModel*>;
 
+/**
+ * @brief Helper class that defines a planning context for Drake Kinematic Trajectory Optimization (KTOpt).
+ * @details For more information, refer to the Drake documentation:
+ * https://drake.mit.edu/doxygen_cxx/classdrake_1_1planning_1_1trajectory__optimization_1_1_kinematic_trajectory_optimization.html
+ */
 class KTOptPlanningContext : public planning_interface::PlanningContext
 {
 public:
+  /**
+   * @brief Constructs an instance of a KTOpt plannign context.
+   * @param name The name of the planning context.
+   * @param group_name The name of the joint group used for motion planning.
+   * @param params The ROS parameters for this planner.
+   */
   KTOptPlanningContext(const std::string& name, const std::string& group_name, const ktopt_interface::Params& params);
 
+  /**
+   * @brief Calculates a trajectory for the current request of this context.
+   * @param res The result containing the respective trajectory, or error code on failure.
+   */
   void solve(planning_interface::MotionPlanResponse& res) override;
+
+  /**
+   * @brief Calculates a trajectory for the current request of this context.
+   * @details This function just delegates to the common response.
+   * However, here the same trajectory is stored with the descriptions "plan", "simplify", or "interpolate".
+   * @param res The detailed result containing the respective trajectory, or error code on failure.
+   */
   void solve(planning_interface::MotionPlanDetailedResponse& res) override;
 
+  /**
+   * @brief Terminates any running solutions.
+   * @return True if successful, otherwise false.
+   */
   bool terminate() override;
+
+  /// @brief Clear the data structures used by the planner.
   void clear() override;
 
-  void setRobotDescription(std::string robot_description);
+  /**
+   * @brief Sets the current robot description for planning.
+   * @param robot_description The URDF string containing the robot description.
+   */
+  void setRobotDescription(const std::string& robot_description);
+
+  /**
+   * @brief Transcribes a MoveIt planning scene to the Drake multibody plant used by this planner.
+   * @param planning_scene The MoveIt planning scene to transcribe.
+   */
   void transcribePlanningScene(const planning_scene::PlanningScene& planning_scene);
+
+  /**
+   * @brief Adds path position constraints, if any, to the planning problem.
+   * @param trajopt The Drake object containing the trajectory optimization problem.
+   * @param plant The Drake multibody plant to use for planning.
+   * @param plant_context The context associated with the multibody plant.
+   * @param padding Additional position padding on the MoveIt constraint, in meters.
+   * This ensures that constraints are more likely to hold for the entire trajectory, since the
+   * Drake mathematical program only optimizes constraints at discrete points along the path.
+   */
   void addPathPositionConstraints(KinematicTrajectoryOptimization& trajopt, const MultibodyPlant<double>& plant,
                                   Context<double>& plant_context, const double padding);
 
 private:
+  /// @brief The ROS parameters associated with this motion planner.
   const ktopt_interface::Params params_;
+
+  /// @brief The URDF robot description.
   std::string robot_description_;
 
-  // drake related variables
+  /// @brief The Drake diagram describing the entire system.
   std::unique_ptr<Diagram<double>> diagram_;
+
+  /// @brief The builder for the Drake system diagram.
   std::unique_ptr<DiagramBuilder<double>> builder;
+
+  /// @brief The context that contains all the data necessary to perform computations on the diagram.
   std::unique_ptr<Context<double>> diagram_context_;
-  VectorXd nominal_q_;
-  const std::string OCTOMAP_NS = "<octomap>";
 
-  // visualization
-  std::shared_ptr<Meshcat> meshcat_;
-  MeshcatVisualizer<double>* visualizer_;
+  /// @brief The nominal joint configuration of the robot, used for joint centering objectives.
+  Eigen::VectorXd nominal_q_;
+
+  /// @brief Pointer to the Meshcat instance associated with this planner.
+  std::shared_ptr<drake::geometry::Meshcat> meshcat_;
+
+  /// @brief The Drake MeshCat visualizer associated with this planner.
+  drake::geometry::MeshcatVisualizer<double>* visualizer_;
 };
 }  // namespace ktopt_interface

Diff for: res/ktopt_moveit_parameters.yaml renamed to parameters/ktopt_moveit_parameters.yaml

@@ -83,7 +83,7 @@ rclcpp::Logger getLogger()
 }
 }  // namespace
 /**
- * @brief Post-processing adapter that timeparametermizes a trajectory based on reachability analysis. For details see
+ * @brief Post-processing adapter that time-parameterizes a trajectory based on reachability analysis. For details see
  * https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_toppra.html
  *
  */
@@ -152,7 +152,7 @@ class AddToppraTimeParameterization : public planning_interface::PlanningRespons
       return;
     }
 
-    // Update plan from planning scene
+    // Update Drake plant from MoveIt planning scene
     auto& plant = diagram_->GetDowncastSubsystemByName<MultibodyPlant<double>>("plant");
     auto& plant_context = diagram_->GetMutableSubsystemContext(plant, diagram_context_.get());
     Eigen::VectorXd q = Eigen::VectorXd::Zero(plant.num_positions() + plant.num_velocities());

Diff for: src/add_toppra_time_parameterization.cpp

@@ -16,6 +16,10 @@ rclcpp::Logger getLogger()
 {
   return moveit::getLogger("moveit.planners.ktopt.planner_manager");
 }
+
+/**
+ * @brief Implementation for the Drake Kinematic Trajectory Optimization (KTOpt) motion planner in MoveIt.
+ */
 class KTOptPlannerManager : public planning_interface::PlannerManager
 {
 public: