[FT] Sets up pos/vel bounds in KTOPT

This commit adds upper/lower position/velocity bounds. Reads them from
the robot model VariableBound API and sets them up as Drake
PositionBund and VelocityBound constraints. Adds some additional docs on
readme

README.md

@@ -1,11 +1,19 @@
 # Experimental MoveIt 2 - Drake Integration
 
+NOTE: Experimental and will continue to have breaking changes until first
+release.
+
 `moveit_drake` brings together the vertical ROS integration of the
 [MoveIt2](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` implementation in `drake`.
+- Exposes `TOPPRA` implementation in `drake`.
+
 ## Docker Workflow (Preferred and tested)
 
 ### Requirements

src/ktopt_planning_context.cpp

@@ -48,6 +48,54 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
   const auto joint_model_group = getPlanningScene()->getRobotModel()->getJointModelGroup(getGroupName());
   RCLCPP_INFO_STREAM(getLogger(), "Planning for group: " << getGroupName());
 
+  // 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;
+  for (const auto &joint_model : joint_model_group->getActiveJointModels()) {
+    const std::string &joint_name = joint_model->getName();
+    const moveit::core::VariableBounds &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_);
+
+    // std::cout << "Joint " << joint_name << ": Position ["
+    //           << bounds.min_position_ << ", " << bounds.max_position_
+    //           << "], Velocity [-" << bounds.max_velocity_ << ", "
+    //           << bounds.max_velocity_ << "]" << std::endl;
+  }
+  int num_positions = plant.num_positions();
+  int num_velocities = plant.num_velocities(); 
+
+  // Ensure the bounds have the correct size
+  if (lower_position_bounds.size() != num_positions ||
+      upper_position_bounds.size() != num_positions) {
+    lower_position_bounds.resize(num_positions,
+                                 -1e6);
+    upper_position_bounds.resize(num_positions,
+                                 1e6);
+  }
+  if (lower_velocity_bounds.size() != num_velocities ||
+      upper_velocity_bounds.size() != num_velocities) {
+    // Resize velocity bounds to match number of velocities, if necessary
+    lower_velocity_bounds.resize(num_velocities,
+                                 -1e6);
+    upper_velocity_bounds.resize(num_velocities,
+                                 1e6);
+  }
+  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());
+
   // q represents the complete state (joint positions and velocities)
   VectorXd q = VectorXd::Zero(plant.num_positions() + plant.num_velocities());
   q << moveit::drake::getJointPositionVector(start_state, getGroupName(), plant);
@@ -97,12 +145,12 @@ 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);
 
   // Add constraints on duration
   // TODO: These should be parameters