From d3946de8bff10c9ba9aed495ef09603dd02d42f5 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Mon, 14 Oct 2024 21:35:49 -0400 Subject: [PATCH] Code cleanup and Docker fixes --- .docker/Dockerfile | 2 + CMakeLists.txt | 4 +- README.md | 35 +++-- docker-compose.yaml | 2 +- .../ktopt_planning_context.hpp | 130 ++++++++++-------- .../ktopt_moveit_parameters.yaml | 0 src/ktopt_planner_manager.cpp | 4 + src/ktopt_planning_context.cpp | 73 ++++++---- 8 files changed, 151 insertions(+), 99 deletions(-) rename {res => parameters}/ktopt_moveit_parameters.yaml (100%) diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 37d4beb..32a8b0c 100644 --- a/.docker/Dockerfile +++ b/.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 --git a/CMakeLists.txt b/CMakeLists.txt index bf754c9..2b76935 100644 --- a/CMakeLists.txt +++ b/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 --git a/README.md b/README.md index 4b6aac2..33c8c59 100644 --- a/README.md +++ b/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 ``` diff --git a/docker-compose.yaml b/docker-compose.yaml index 5baf316..cad7278 100644 --- a/docker-compose.yaml +++ b/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 --git a/include/ktopt_interface/ktopt_planning_context.hpp b/include/ktopt_interface/ktopt_planning_context.hpp index 47abcf4..8207bb8 100644 --- a/include/ktopt_interface/ktopt_planning_context.hpp +++ b/include/ktopt_interface/ktopt_planning_context.hpp @@ -1,103 +1,119 @@ #pragma once +#include #include #include -#include // 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 +#include +#include +#include +#include +#include #include #include +#include +#include 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; +/** + * @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& plant, Context& 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_; + + /// @brief The builder for the Drake system diagram. std::unique_ptr> builder; + + /// @brief The context that contains all the data necessary to perform computations on the diagram. std::unique_ptr> diagram_context_; - VectorXd nominal_q_; - const std::string OCTOMAP_NS = ""; - // visualization - std::shared_ptr meshcat_; - MeshcatVisualizer* 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 meshcat_; + + /// @brief The Drake MeshCat visualizer associated with this planner. + drake::geometry::MeshcatVisualizer* visualizer_; }; } // namespace ktopt_interface diff --git a/res/ktopt_moveit_parameters.yaml b/parameters/ktopt_moveit_parameters.yaml similarity index 100% rename from res/ktopt_moveit_parameters.yaml rename to parameters/ktopt_moveit_parameters.yaml diff --git a/src/ktopt_planner_manager.cpp b/src/ktopt_planner_manager.cpp index 405ba29..855931d 100644 --- a/src/ktopt_planner_manager.cpp +++ b/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: diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index 89d43b1..2beae76 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -1,24 +1,35 @@ #include +#include +#include +#include +#include +#include +#include +#include +#include + #include #include #include #include #include -#include "ktopt_interface/ktopt_planning_context.hpp" +#include namespace ktopt_interface { namespace { +/// @brief Helper function that returns the logger instance associated with this planner. +/// @return The logger instance. rclcpp::Logger getLogger() { return moveit::getLogger("moveit.planners.ktopt_interface.planning_context"); } -constexpr double kDefaultJointMaxPosition = 1.0e6; - +/// @brief The namespace corresponding to the octomap in the planning scene. +constexpr auto kOctomapNamespace = ""; } // namespace KTOptPlanningContext::KTOptPlanningContext(const std::string& name, const std::string& group_name, @@ -67,7 +78,7 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) moveit::drake::getJerkBounds(joint_model_group, plant, lower_jerk_bounds, upper_jerk_bounds); // q represents the complete state (joint positions and velocities) - VectorXd q = VectorXd::Zero(plant.num_positions() + plant.num_velocities()); + Eigen::VectorXd q = Eigen::VectorXd::Zero(plant.num_positions() + plant.num_velocities()); q << moveit::drake::getJointPositionVector(start_state, getGroupName(), plant); q << moveit::drake::getJointVelocityVector(start_state, getGroupName(), plant); @@ -94,7 +105,8 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) trajopt.AddPathLengthCost(params_.path_length_cost_weight); // TODO: Adds quadratic cost // This acts as a secondary cost to push the solution towards joint centers - // prog.AddQuadraticErrorCost(MatrixXd::Identity(plant.num_positions(), plant.num_positions()), nominal_q_, prog.control_points()); + // prog.AddQuadraticErrorCost(Eigen::MatrixXd::Identity(plant.num_positions(), plant.num_positions()), nominal_q_, + // prog.control_points()); // Constraints // Add constraints on start joint configuration and velocity @@ -127,7 +139,7 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) addPathPositionConstraints(trajopt, plant, plant_context, params_.position_constraint_padding); // solve the program - auto result = Solve(prog); + auto result = drake::solvers::Solve(prog); if (!result.is_success()) { @@ -258,7 +270,7 @@ bool KTOptPlanningContext::terminate() return true; } -void KTOptPlanningContext::setRobotDescription(std::string robot_description) +void KTOptPlanningContext::setRobotDescription(const std::string& robot_description) { robot_description_ = robot_description; @@ -266,18 +278,18 @@ void KTOptPlanningContext::setRobotDescription(std::string robot_description) builder = std::make_unique>(); // meshcat experiment - const auto meshcat_params = MeshcatParams(); - meshcat_ = std::make_shared(meshcat_params); + const auto meshcat_params = drake::geometry::MeshcatParams(); + meshcat_ = std::make_shared(meshcat_params); - auto [plant, scene_graph] = AddMultibodyPlantSceneGraph(builder.get(), 0.0); + auto [plant, scene_graph] = drake::multibody::AddMultibodyPlantSceneGraph(builder.get(), 0.0); // TODO:(kamiradi) Figure out object parsing // auto robot_instance = Parser(plant_, // scene_graph_).AddModelsFromString(robot_description_, ".urdf"); const char* ModelUrl = params_.drake_robot_description.c_str(); - const std::string urdf = PackageMap{}.ResolveUrl(ModelUrl); - auto robot_instance = Parser(&plant, &scene_graph).AddModels(urdf); + const std::string urdf = drake::multibody::PackageMap{}.ResolveUrl(ModelUrl); + auto robot_instance = drake::multibody::Parser(&plant, &scene_graph).AddModels(urdf); plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0")); // planning scene transcription @@ -288,12 +300,13 @@ void KTOptPlanningContext::setRobotDescription(std::string robot_description) plant.Finalize(); // Apply MeshCat visualization - VisualizationConfig config; - ApplyVisualizationConfig(config, builder.get(), /*lcm_buses*/ nullptr, &plant, &scene_graph, meshcat_); + drake::visualization::VisualizationConfig config; + drake::visualization::ApplyVisualizationConfig(config, builder.get(), /*lcm_buses*/ nullptr, &plant, &scene_graph, + meshcat_); - MeshcatVisualizerParams meshcat_viz_params; - auto& visualizer = - MeshcatVisualizer::AddToBuilder(builder.get(), scene_graph, meshcat_, std::move(meshcat_viz_params)); + drake::geometry::MeshcatVisualizerParams meshcat_viz_params; + auto& visualizer = drake::geometry::MeshcatVisualizer::AddToBuilder(builder.get(), scene_graph, meshcat_, + std::move(meshcat_viz_params)); visualizer_ = &visualizer; // in the future you can add other LeafSystems here. For now building the @@ -320,7 +333,7 @@ void KTOptPlanningContext::transcribePlanningScene(const planning_scene::Plannin { RCLCPP_ERROR_STREAM(getLogger(), "caught exception ... " << e.what()); } - auto& scene_graph = builder->GetMutableDowncastSubsystemByName>("scene_graph"); + auto& scene_graph = builder->GetMutableDowncastSubsystemByName>("scene_graph"); for (const auto& object : planning_scene.getWorld()->getObjectIds()) { const auto& collision_object = planning_scene.getWorld()->getObject(object); @@ -329,7 +342,7 @@ void KTOptPlanningContext::transcribePlanningScene(const planning_scene::Plannin RCLCPP_INFO(getLogger(), "No collision object"); return; } - if (object == OCTOMAP_NS) + if (object == kOctomapNamespace) { RCLCPP_WARN(getLogger(), "Octomap not supported for now ... "); continue; @@ -341,25 +354,26 @@ void KTOptPlanningContext::transcribePlanningScene(const planning_scene::Plannin const auto& pose = collision_object->pose_; const auto& shape_type = collision_object->shapes_[i]->type; - std::unique_ptr shape_ptr; + std::unique_ptr shape_ptr; switch (shape_type) { case shapes::ShapeType::BOX: { const auto object_ptr = std::dynamic_pointer_cast(shape); - shape_ptr = std::make_unique(object_ptr->size[0], object_ptr->size[1], object_ptr->size[2]); + shape_ptr = + std::make_unique(object_ptr->size[0], object_ptr->size[1], object_ptr->size[2]); break; } case shapes::ShapeType::SPHERE: { const auto object_ptr = std::dynamic_pointer_cast(shape); - shape_ptr = std::make_unique(object_ptr->radius); + shape_ptr = std::make_unique(object_ptr->radius); break; } case shapes::ShapeType::CYLINDER: { const auto object_ptr = std::dynamic_pointer_cast(shape); - shape_ptr = std::make_unique(object_ptr->radius, object_ptr->length); + shape_ptr = std::make_unique(object_ptr->radius, object_ptr->length); break; } default: @@ -375,14 +389,15 @@ void KTOptPlanningContext::transcribePlanningScene(const planning_scene::Plannin } // Register the geometry in the scene graph. - const SourceId source_id = scene_graph.RegisterSource(shape_name); - const GeometryId geom_id = scene_graph.RegisterAnchoredGeometry( - source_id, std::make_unique(RigidTransformd(pose), std::move(shape_ptr), shape_name)); + const auto source_id = scene_graph.RegisterSource(shape_name); + const auto geom_id = scene_graph.RegisterAnchoredGeometry( + source_id, std::make_unique(drake::math::RigidTransformd(pose), + std::move(shape_ptr), shape_name)); // add illustration, proximity, perception properties - scene_graph.AssignRole(source_id, geom_id, IllustrationProperties()); - scene_graph.AssignRole(source_id, geom_id, ProximityProperties()); - scene_graph.AssignRole(source_id, geom_id, PerceptionProperties()); + scene_graph.AssignRole(source_id, geom_id, drake::geometry::IllustrationProperties()); + scene_graph.AssignRole(source_id, geom_id, drake::geometry::ProximityProperties()); + scene_graph.AssignRole(source_id, geom_id, drake::geometry::PerceptionProperties()); // TODO: Create and anchor ground entity }