Adds orientation constraints (#55)

* tweaks constrained planning tut to have only box and orientation constraints

* [FT] adds orientation constraints

* Add docstring based on latest code cleanup

* Fix build errors

* Working inequality and equality constraints position and orientation

---------

Co-authored-by: Sebastian Castro <[email protected]>
Co-authored-by: Sebastian Castro <[email protected]>

Author: kamiradi

Diff for: include/ktopt_interface/ktopt_planning_context.hpp

@@ -11,17 +11,13 @@
 #include <drake/planning/trajectory_optimization/kinematic_trajectory_optimization.h>
 #include <drake/systems/framework/diagram.h>
 #include <drake/systems/framework/diagram_builder.h>
-#include <drake/multibody/inverse_kinematics/minimum_distance_lower_bound_constraint.h>
-#include <drake/multibody/inverse_kinematics/position_constraint.h>
 #include <drake/multibody/parsing/parser.h>
 #include <drake/multibody/plant/multibody_plant.h>
 
 namespace ktopt_interface
 {
 // declare all namespaces to be used
-using drake::multibody::MinimumDistanceLowerBoundConstraint;
 using drake::multibody::MultibodyPlant;
-using drake::multibody::PositionConstraint;
 using drake::planning::trajectory_optimization::KinematicTrajectoryOptimization;
 using drake::systems::Context;
 using drake::systems::Diagram;
@@ -91,6 +87,18 @@ class KTOptPlanningContext : public planning_interface::PlanningContext
   void addPathPositionConstraints(KinematicTrajectoryOptimization& trajopt, const MultibodyPlant<double>& plant,
                                   Context<double>& plant_context, const double padding);
 
+  /**
+   * @brief Adds path orientation constraints, if any, to the planning problem.
+   * @param trajopt The Drake object 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 orientation padding on the MoveIt constraint, in radians.
+   * 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 addPathOrientationConstraints(KinematicTrajectoryOptimization& trajopt, const MultibodyPlant<double>& plant,
+                                     Context<double>& plant_context, const double padding);
+
 private:
   /// @brief The ROS parameters associated with this motion planner.
   const ktopt_interface::Params params_;

Diff for: parameters/ktopt_moveit_parameters.yaml

@@ -76,14 +76,22 @@ ktopt_interface:
       gt_eq<>: [0.0]
     }
   }
-  num_position_constraint_points: {
+  num_position_inequality_points: {
     type: int,
     description: "Number of points on the path where MoveIt's bounding box constraint needs to be imposed.",
     default_value: 10,
     validation: {
       gt_eq<>: [2]
     }
   }
+  num_position_equality_points: {
+    type: int,
+    description: "Number of points on the path where MoveIt's equality position constraint needs to be imposed.",
+    default_value: 25,
+    validation: {
+      gt_eq<>: [2]
+    }
+  }
   position_constraint_padding: {
     type: double,
     description: "Padding distance for position constraints, in meters, to make sure optimizing at sampled points still globally meets constraints.",

Diff for: src/ktopt_planning_context.cpp

@@ -5,6 +5,11 @@
 #include <drake/geometry/geometry_instance.h>
 #include <drake/geometry/geometry_roles.h>
 #include <drake/geometry/proximity_properties.h>
+#include <drake/multibody/inverse_kinematics/minimum_distance_lower_bound_constraint.h>
+#include <drake/multibody/inverse_kinematics/orientation_constraint.h>
+#include <drake/multibody/inverse_kinematics/position_constraint.h>
+#include <drake/math/rigid_transform.h>
+#include <drake/math/rotation_matrix.h>
 #include <drake/solvers/solve.h>
 #include <drake/visualization/visualization_config.h>
 #include <drake/visualization/visualization_config_functions.h>
@@ -78,7 +83,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)
-  auto q = Eigen::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);
 
@@ -137,6 +142,7 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
 
   // process path_constraints
   addPathPositionConstraints(trajopt, plant, plant_context, params_.position_constraint_padding);
+  addPathOrientationConstraints(trajopt, plant, plant_context, params_.orientation_constraint_padding);
 
   // solve the program
   auto result = drake::solvers::Solve(prog);
@@ -155,7 +161,7 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
   // add collision constraints
   for (int s = 0; s < params_.num_collision_check_points; ++s)
   {
-    trajopt.AddPathPositionConstraint(std::make_shared<MinimumDistanceLowerBoundConstraint>(
+    trajopt.AddPathPositionConstraint(std::make_shared<drake::multibody::MinimumDistanceLowerBoundConstraint>(
                                           &plant, params_.collision_check_lower_distance_bound, &plant_context),
                                       static_cast<double>(s) / (params_.num_collision_check_points - 1));
   }
@@ -204,66 +210,145 @@ void KTOptPlanningContext::addPathPositionConstraints(KinematicTrajectoryOptimiz
   const auto& req = getMotionPlanRequest();
 
   // check for path position constraints
-  if (!req.path_constraints.position_constraints.empty())
+  for (const auto& position_constraint : req.path_constraints.position_constraints)
   {
-    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);
+    Eigen::Quaterniond box_orientation(primitive_pose.orientation.w, primitive_pose.orientation.x,
+                                       primitive_pose.orientation.y, primitive_pose.orientation.z);
+    drake::math::RigidTransformd X_AbarA(box_orientation, box_center);
+
+    // 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 link_ee_name = position_constraint.link_name;
+    if (!plant.HasBodyNamed(link_ee_name))
     {
-      // 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 link_ee_name = position_constraint.link_name;
-      if (!plant.HasBodyNamed(link_ee_name))
-      {
-        RCLCPP_ERROR(getLogger(),
-                     "The link specified in the PositionConstraint message, %s, does not exist in the Drake "
-                     "plant.",
-                     link_ee_name.c_str());
-        continue;
-      }
-      const auto& link_ee_frame = plant.GetFrameByName(link_ee_name);
+      RCLCPP_ERROR(getLogger(),
+                   "The link specified in the PositionConstraint message, %s, does not exist in the Drake "
+                   "plant.",
+                   link_ee_name.c_str());
+      continue;
+    }
+    // frameB
+    const auto& link_ee_frame = plant.GetFrameByName(link_ee_name);
 
-      const auto base_frame_name = position_constraint.header.frame_id;
-      if (!plant.HasBodyNamed(base_frame_name))
-      {
-        RCLCPP_ERROR(getLogger(),
-                     "The link specified in the PositionConstraint message, %s, does not exist in the Drake "
-                     "plant.",
-                     base_frame_name.c_str());
-        continue;
-      }
-      const auto& base_frame = plant.GetFrameByName(base_frame_name);
+    const auto base_frame_name = position_constraint.header.frame_id;
+    if (!plant.HasBodyNamed(base_frame_name))
+    {
+      RCLCPP_ERROR(getLogger(),
+                   "The link specified in the PositionConstraint message, %s, does not exist in the Drake "
+                   "plant.",
+                   base_frame_name.c_str());
+      continue;
+    }
+    // frameA
+    const auto& base_frame = plant.GetFrameByName(base_frame_name);
 
-      const auto& primitive = position_constraint.constraint_region.primitives[0];
-      if (primitive.type != shape_msgs::msg::SolidPrimitive::BOX)
-      {
-        RCLCPP_WARN(getLogger(), "Expects a bounding box constraint as a SolidPrimitive::BOX");
-        continue;
-      }
+    const auto& primitive = position_constraint.constraint_region.primitives[0];
+    if (primitive.type != shape_msgs::msg::SolidPrimitive::BOX)
+    {
+      RCLCPP_WARN(getLogger(), "Expects a bounding box constraint as a SolidPrimitive::BOX");
+      continue;
+    }
 
-      // Calculate the lower and upper bounds based on the box dimensions
-      // around the center
-      const auto offset_vec = Eigen::Vector3d(std::max(0.0, primitive.dimensions[0] / 2.0 - padding),
-                                              std::max(0.0, primitive.dimensions[1] / 2.0 - padding),
-                                              std::max(0.0, primitive.dimensions[2] / 2.0 - padding));
-      const auto lower_bound = box_center - offset_vec;
-      const auto upper_bound = box_center + offset_vec;
+    // Calculate the lower and upper bounds based on the box dimensions
+    // around the center
+    const auto offset_vec = Eigen::Vector3d(std::max(padding, primitive.dimensions[0] / 2.0 - padding),
+                                            std::max(padding, primitive.dimensions[1] / 2.0 - padding),
+                                            std::max(padding, primitive.dimensions[2] / 2.0 - padding));
 
+    // Check if equality constraint
+    if (req.path_constraints.name == "use_equality_constraints")
+    {
+      // Add position constraint to each knot point of the trajectory
+      for (int i = 0; i < params_.num_position_equality_points; ++i)
+      {
+        trajopt.AddPathPositionConstraint(std::make_shared<drake::multibody::PositionConstraint>(
+                                              &plant, base_frame, X_AbarA, -offset_vec, offset_vec, link_ee_frame,
+                                              Eigen::Vector3d(0.0, 0.0, 0.0), &plant_context),
+                                          static_cast<double>(i) / (params_.num_position_equality_points - 1));
+      }
+    }
+    else
+    {
       // Add position constraint to each knot point of the trajectory
-      for (int i = 0; i < params_.num_position_constraint_points; ++i)
+      for (int i = 0; i < params_.num_position_inequality_points; ++i)
       {
-        trajopt.AddPathPositionConstraint(
-            std::make_shared<PositionConstraint>(&plant, base_frame, lower_bound, upper_bound, link_ee_frame,
-                                                 Eigen::Vector3d(0.0, 0.0, 0.0), &plant_context),
-            static_cast<double>(i) / (params_.num_position_constraint_points - 1));
+        trajopt.AddPathPositionConstraint(std::make_shared<drake::multibody::PositionConstraint>(
+                                              &plant, base_frame, X_AbarA, -offset_vec, offset_vec, link_ee_frame,
+                                              Eigen::Vector3d(0.0, 0.0, 0.0), &plant_context),
+                                          static_cast<double>(i) / (params_.num_position_inequality_points - 1));
       }
     }
   }
 }
 
+void KTOptPlanningContext::addPathOrientationConstraints(KinematicTrajectoryOptimization& trajopt,
+                                                         const MultibodyPlant<double>& plant,
+                                                         Context<double>& plant_context, const double padding)
+{
+  // retrieve the motion planning request
+  const auto& req = getMotionPlanRequest();
+
+  // check for path position constraints
+  for (const auto& orientation_constraint : req.path_constraints.orientation_constraints)
+  {
+    // NOTE: Current thesis, when you apply a Drake Orientation Constraint it
+    // expects the following
+    // Frame A: The frame in which the orientation constraint is defined
+    // Frame B: The frame to which the orientation constraint is enforced
+    // theta_bound: The angle difference between frame A's orientation and
+    //              frame B's orientation
+
+    // Extract FrameA and FrameB for orientation constraint
+    // frame B
+    const auto link_ee_name = orientation_constraint.link_name;
+    if (!plant.HasBodyNamed(link_ee_name))
+    {
+      RCLCPP_ERROR(getLogger(),
+                   "The link specified in the Orientation Constraint message, %s, does not exist in the Drake "
+                   "plant.",
+                   link_ee_name.c_str());
+      continue;
+    }
+    const auto& link_ee_frame = plant.GetFrameByName(link_ee_name);
+
+    // frame A
+    const auto base_frame_name = orientation_constraint.header.frame_id;
+    if (!plant.HasBodyNamed(base_frame_name))
+    {
+      RCLCPP_ERROR(getLogger(),
+                   "The link specified in the Orientation Constraint message, %s, does not exist in the Drake "
+                   "plant.",
+                   base_frame_name.c_str());
+      continue;
+    }
+    const auto& base_frame = plant.GetFrameByName(base_frame_name);
+
+    // Extract the orientation of that Frame B needs to be constrained to w.r.t Frame A
+    const auto& constrained_orientation = orientation_constraint.orientation;
+    // convert to drake's RotationMatrix
+    const auto R_BbarB = drake::math::RotationMatrixd(Eigen::Quaterniond(
+        constrained_orientation.w, constrained_orientation.x, constrained_orientation.y, constrained_orientation.z));
+
+    // NOTE: There are 3 tolerances given for the orientation constraint in moveit
+    // message, Drake only takes in a single theta bound. For now, we take any
+    // of the tolerances as the theta bound
+    const double theta_bound = orientation_constraint.absolute_x_axis_tolerance - padding;
+
+    // Add orientation constraint to each knot point of the trajectory
+    for (int i = 0; i < params_.num_orientation_constraint_points; ++i)
+    {
+      trajopt.AddPathPositionConstraint(std::make_shared<drake::multibody::OrientationConstraint>(
+                                            &plant, link_ee_frame, drake::math::RotationMatrixd::Identity(), base_frame,
+                                            R_BbarB, theta_bound, &plant_context),
+                                        static_cast<double>(i) / (params_.num_orientation_constraint_points - 1));
+    }
+  }
+}
+
 bool KTOptPlanningContext::terminate()
 {
   RCLCPP_ERROR(getLogger(), "KTOptPlanningContext::terminate() is not implemented!");