diff --git a/README.md b/README.md index cfd88e0..0f6574f 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,26 @@ # Experimental MoveIt 2 - Drake Integration -Under construction +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`. + +## 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`. ## 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 instructions +[here](https://docs.docker.com/engine/install/ubuntu/). ### Steps The following steps clone and build the base image that you will require to @@ -33,7 +48,9 @@ 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 +colcon workspace with MoveIt from source. Open a command line to your colcon workspace: @@ -58,28 +75,11 @@ 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 +- Use [pre-commit to format your + code](https://moveit.ros.org/documentation/contributing/code/#pre-commit-formatting-checks) -### Potential issues -- Assumes that planner managers initialize will set robot description before a - call to getPlanningContext. + # inside the moveit_drake package + pre-commit run -a ### Some helper commands To just rebuild `moveit_drake` diff --git a/res/ktopt_moveit_parameters.yaml b/res/ktopt_moveit_parameters.yaml index dce6e71..4a993c0 100644 --- a/res/ktopt_moveit_parameters.yaml +++ b/res/ktopt_moveit_parameters.yaml @@ -1,4 +1,9 @@ ktopt_interface: + drake_robot_description: { + type: string, + description: "Robot description to be loaded by the internal Drake MultibodyPlant", + default_value: "package://drake_models/franka_description/urdf/panda_arm_hand.urdf", + } num_iterations: { type: int, description: "Number of iterations for the Drake mathematical program solver.", @@ -39,3 +44,11 @@ ktopt_interface: gt_eq<>: [0.0] } } + joint_jerk_bound: { + type: double, + description: "Maximum jerk that is allowed for generated trajectory", + default_value: 1.0, + validation: { + gt_eq<>: [0.0] + } + } diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index dc26dee..1a5e4b2 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -16,6 +16,9 @@ rclcpp::Logger getLogger() { return moveit::getLogger("moveit.planners.ktopt_interface.planning_context"); } + +constexpr double kDefaultJointMaxPosition = 1.0e6; + } // namespace KTOptPlanningContext::KTOptPlanningContext(const std::string& name, const std::string& group_name, @@ -47,6 +50,74 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) const moveit::core::RobotState start_state(*getPlanningScene()->getCurrentStateUpdated(req.start_state)); const auto joint_model_group = getPlanningScene()->getRobotModel()->getJointModelGroup(getGroupName()); RCLCPP_INFO_STREAM(getLogger(), "Planning for group: " << getGroupName()); + const int num_positions = plant.num_positions(); + const int num_velocities = plant.num_velocities(); + + // extract position and velocity bounds + std::vector lower_position_bounds; + std::vector upper_position_bounds; + std::vector lower_velocity_bounds; + std::vector upper_velocity_bounds; + std::vector lower_acceleration_bounds; + std::vector upper_acceleration_bounds; + std::vector lower_jerk_bounds; + std::vector upper_jerk_bounds; + lower_position_bounds.reserve(num_positions); + upper_position_bounds.reserve(num_positions); + lower_velocity_bounds.reserve(num_positions); + upper_velocity_bounds.reserve(num_positions); + lower_acceleration_bounds.reserve(num_positions); + upper_acceleration_bounds.reserve(num_positions); + lower_jerk_bounds.reserve(num_positions); + upper_jerk_bounds.reserve(num_positions); + + const auto& all_joint_models = joint_model_group->getJointModels(); + for (const auto& joint_model : all_joint_models) + { + const auto& joint_name = joint_model->getName(); + const bool is_active = joint_model_group->hasJointModel(joint_name); + + if (is_active) + { + // Set bounds for active joints + const auto& bounds = joint_model->getVariableBounds()[0]; + lower_position_bounds.push_back(bounds.min_position_); + upper_position_bounds.push_back(bounds.max_position_); + lower_velocity_bounds.push_back(-bounds.max_velocity_); + upper_velocity_bounds.push_back(bounds.max_velocity_); + lower_acceleration_bounds.push_back(-bounds.max_acceleration_); + upper_acceleration_bounds.push_back(bounds.max_acceleration_); + lower_jerk_bounds.push_back(-params_.joint_jerk_bound); + upper_jerk_bounds.push_back(params_.joint_jerk_bound); + } + else + { + // Set default values for inactive joints + lower_position_bounds.push_back(-kDefaultJointMaxPosition); + upper_position_bounds.push_back(kDefaultJointMaxPosition); + lower_velocity_bounds.push_back(0.0); + upper_velocity_bounds.push_back(0.0); + lower_acceleration_bounds.push_back(0.0); + upper_acceleration_bounds.push_back(0.0); + lower_jerk_bounds.push_back(0.0); + upper_jerk_bounds.push_back(0.0); + } + } + + Eigen::Map lower_position_bounds_eigen(lower_position_bounds.data(), + lower_position_bounds.size()); + Eigen::Map upper_position_bounds_eigen(upper_position_bounds.data(), + upper_position_bounds.size()); + Eigen::Map lower_velocity_bounds_eigen(lower_velocity_bounds.data(), + lower_velocity_bounds.size()); + Eigen::Map upper_velocity_bounds_eigen(upper_velocity_bounds.data(), + upper_velocity_bounds.size()); + Eigen::Map lower_acceleration_bounds_eigen(lower_acceleration_bounds.data(), + lower_acceleration_bounds.size()); + Eigen::Map upper_acceleration_bounds_eigen(upper_acceleration_bounds.data(), + upper_acceleration_bounds.size()); + Eigen::Map lower_jerk_bounds_eigen(lower_jerk_bounds.data(), lower_jerk_bounds.size()); + Eigen::Map upper_jerk_bounds_eigen(upper_jerk_bounds.data(), upper_jerk_bounds.size()); // q represents the complete state (joint positions and velocities) VectorXd q = VectorXd::Zero(plant.num_positions() + plant.num_velocities()); @@ -97,12 +168,10 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) trajopt.AddPathVelocityConstraint(goal_velocity, goal_velocity, 1.0); // TODO: Add constraints on joint position/velocity/acceleration - // trajopt.AddPositionBounds( - // plant_->GetPositionLowerLimits(), - // plant_->GetPositionUpperLimits()); - // trajopt.AddVelocityBounds( - // plant_->GetVelocityLowerLimits(), - // plant_->GetVelocityUpperLimits()); + trajopt.AddPositionBounds(lower_position_bounds_eigen, upper_position_bounds_eigen); + trajopt.AddVelocityBounds(lower_velocity_bounds_eigen, upper_velocity_bounds_eigen); + trajopt.AddAccelerationBounds(lower_acceleration_bounds_eigen, upper_acceleration_bounds_eigen); + trajopt.AddJerkBounds(lower_jerk_bounds_eigen, upper_jerk_bounds_eigen); // Add constraints on duration // TODO: These should be parameters @@ -181,9 +250,7 @@ void KTOptPlanningContext::setRobotDescription(std::string robot_description) // TODO:(kamiradi) Figure out object parsing // auto robot_instance = Parser(plant_, scene_graph_).AddModelsFromString(robot_description_, ".urdf"); - // HACK: For now loading directly from drake's package map - const char* ModelUrl = "package://drake_models/franka_description/" - "urdf/panda_arm_hand.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); plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"));