From 36d535b61bfe30f427763acdce9edeafed0997f3 Mon Sep 17 00:00:00 2001
From: Kzernobog <adityakamireddypalli@gmail.com>
Date: Sat, 5 Oct 2024 17:40:33 +0100
Subject: [PATCH] [WIP] adds Box Constraints

---
 .../ktopt_planning_context.hpp                |  3 +
 res/ktopt_moveit_parameters.yaml              |  5 ++
 src/ktopt_planning_context.cpp                | 56 +++++++++++++++++++
 3 files changed, 64 insertions(+)

diff --git a/include/ktopt_interface/ktopt_planning_context.hpp b/include/ktopt_interface/ktopt_planning_context.hpp
index eca811f..469f784 100644
--- a/include/ktopt_interface/ktopt_planning_context.hpp
+++ b/include/ktopt_interface/ktopt_planning_context.hpp
@@ -23,6 +23,8 @@
 #include "drake/visualization/visualization_config.h"
 #include "drake/visualization/visualization_config_functions.h"
 #include <drake/multibody/inverse_kinematics/minimum_distance_lower_bound_constraint.h>
+#include <drake/multibody/inverse_kinematics/position_constraint.h>
+
 
 namespace ktopt_interface
 {
@@ -51,6 +53,7 @@ using drake::multibody::AddMultibodyPlantSceneGraph;
 using drake::multibody::MinimumDistanceLowerBoundConstraint;
 using drake::multibody::MultibodyPlant;
 using drake::multibody::PackageMap;
+using drake::multibody::PositionConstraint;
 using drake::multibody::Parser;
 using drake::planning::trajectory_optimization::KinematicTrajectoryOptimization;
 using drake::solvers::Solve;
diff --git a/res/ktopt_moveit_parameters.yaml b/res/ktopt_moveit_parameters.yaml
index 4a993c0..abdd887 100644
--- a/res/ktopt_moveit_parameters.yaml
+++ b/res/ktopt_moveit_parameters.yaml
@@ -4,6 +4,11 @@ ktopt_interface:
     description: "Robot description to be loaded by the internal Drake MultibodyPlant",
     default_value: "package://drake_models/franka_description/urdf/panda_arm_hand.urdf",
   }
+  link_ee: {
+    type: string,
+    description: "Name of the end-effector frame as per urdf/sdf",
+    default_value: "panda_hand"
+  }
   num_iterations: {
     type: int,
     description: "Number of iterations for the Drake mathematical program solver.",
diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp
index 1a5e4b2..de494ba 100644
--- a/src/ktopt_planning_context.cpp
+++ b/src/ktopt_planning_context.cpp
@@ -6,6 +6,7 @@
 #include <moveit/constraint_samplers/constraint_sampler_manager.h>
 #include <moveit/planning_scene/planning_scene.h>
 
+
 #include "ktopt_interface/ktopt_planning_context.hpp"
 
 namespace ktopt_interface
@@ -52,6 +53,9 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
   RCLCPP_INFO_STREAM(getLogger(), "Planning for group: " << getGroupName());
   const int num_positions = plant.num_positions();
   const int num_velocities = plant.num_velocities();
+  const auto link_ee = params_.link_ee;
+  const auto link_ee_frame = plant.GetFrameByName(link_ee);
+
 
   // extract position and velocity bounds
   std::vector<double> lower_position_bounds;
@@ -177,6 +181,58 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
   // TODO: These should be parameters
   trajopt.AddDurationConstraint(0.5, 5);
 
+  // process path_constraints
+  if (!req.path_constraints.position_constraints.empty()) {
+    for (const auto &position_constraint :
+         req.path_constraints.position_constraints) {
+      // Extract the bounding box's center (primitive pose)
+      const auto &primitive_pose =
+          position_constraint.constraint_region.primitive_poses[0];
+      Eigen::Vector3d box_center(primitive_pose.position.x,
+                                 primitive_pose.position.y,
+                                 primitive_pose.position.z);
+
+      // Extract dimensions of the bounding box from
+      // constraint_region.primitives Assuming it is a box
+      // (shape_msgs::SolidPrimitive::BOX) and has dimensions in [x, y, z]
+      const auto &primitive =
+          position_constraint.constraint_region.primitives[0];
+      if (primitive.type == shape_msgs::SolidPrimitive::BOX) {
+        double x_dim =
+            primitive.dimensions[0] / 2.0;
+        double y_dim =
+            primitive.dimensions[1] / 2.0;
+        double z_dim =
+            primitive.dimensions[2] / 2.0;
+
+        // Calculate the lower and upper bounds based on the box dimensions
+        // around the center
+        Eigen::Vector3d lower_bound =
+            box_center - Eigen::Vector3d(x_dim, y_dim, z_dim);
+        Eigen::Vector3d upper_bound =
+            box_center + Eigen::Vector3d(x_dim, y_dim, z_dim);
+
+        // Create PositionConstraint for the path
+        PositionConstraint path_constraint(
+            &plant,
+            plant.world_frame(), // Reference frame is the world frame
+            lower_bound,         // Lower bound on position
+            upper_bound,         // Upper bound on position
+            gripper_frame,       // Frame of the gripper (end-effector)
+            Eigen::Vector3d(
+                0.0, 0.1,
+                0.0), // Reference point in the gripper frame (if any offset)
+            plant_context.get() // Plant context
+        );
+
+        // Add position constraint to each knot point of the trajectory
+        for (int i = 0; i < 10; ++i) {
+          trajopt.AddPathPositionConstraint(path_constraint, static_cast<double>(i) / 10.0);
+        }
+      }
+    }
+  }
+
   // solve the program
   auto result = Solve(prog);