README clean up and integration of Kinematic limits from Robot Model

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`

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]
+    }
+  }

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<double> lower_position_bounds;
+  std::vector<double> upper_position_bounds;
+  std::vector<double> lower_velocity_bounds;
+  std::vector<double> upper_velocity_bounds;
+  std::vector<double> lower_acceleration_bounds;
+  std::vector<double> upper_acceleration_bounds;
+  std::vector<double> lower_jerk_bounds;
+  std::vector<double> 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<const Eigen::VectorXd> lower_position_bounds_eigen(lower_position_bounds.data(),
+                                                                lower_position_bounds.size());
+  Eigen::Map<const Eigen::VectorXd> upper_position_bounds_eigen(upper_position_bounds.data(),
+                                                                upper_position_bounds.size());
+  Eigen::Map<const Eigen::VectorXd> lower_velocity_bounds_eigen(lower_velocity_bounds.data(),
+                                                                lower_velocity_bounds.size());
+  Eigen::Map<const Eigen::VectorXd> upper_velocity_bounds_eigen(upper_velocity_bounds.data(),
+                                                                upper_velocity_bounds.size());
+  Eigen::Map<const Eigen::VectorXd> lower_acceleration_bounds_eigen(lower_acceleration_bounds.data(),
+                                                                    lower_acceleration_bounds.size());
+  Eigen::Map<const Eigen::VectorXd> upper_acceleration_bounds_eigen(upper_acceleration_bounds.data(),
+                                                                    upper_acceleration_bounds.size());
+  Eigen::Map<const Eigen::VectorXd> lower_jerk_bounds_eigen(lower_jerk_bounds.data(), lower_jerk_bounds.size());
+  Eigen::Map<const Eigen::VectorXd> 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"));