Code cleanup and Docker fixes

.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" \

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

README.md

@@ -9,6 +9,7 @@ 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
@@ -67,25 +68,39 @@ moveit):
     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
+
+Use [pre-commit to format your code](https://moveit.ros.org/documentation/contributing/code/#pre-commit-formatting-checks)
+
+Within the container, you can run the following command to format the code
+
+```bash
+# inside the moveit_drake package
+pre-commit run -a
 ```
+
+### Some helper commands
+To just rebuild `moveit_drake`.
+
+```bash
 rm -rf build/moveit_drake install/moveit_drake
 colcon build --packages-select moveit_drake
 ```

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"

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 mathematical program 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

src/ktopt_planner_manager.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: