diff --git a/include/ktopt_interface/ktopt_planning_context.hpp b/include/ktopt_interface/ktopt_planning_context.hpp index f73f09f..ded3269 100644 --- a/include/ktopt_interface/ktopt_planning_context.hpp +++ b/include/ktopt_interface/ktopt_planning_context.hpp @@ -11,17 +11,13 @@ #include #include #include -#include -#include #include #include 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& plant, Context& 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& plant, + Context& plant_context, const double padding); + private: /// @brief The ROS parameters associated with this motion planner. const ktopt_interface::Params params_; diff --git a/parameters/ktopt_moveit_parameters.yaml b/parameters/ktopt_moveit_parameters.yaml index 0aca384..82de886 100644 --- a/parameters/ktopt_moveit_parameters.yaml +++ b/parameters/ktopt_moveit_parameters.yaml @@ -76,7 +76,7 @@ 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, @@ -84,6 +84,14 @@ ktopt_interface: 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 --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index 7fbc302..65afb68 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -5,6 +5,11 @@ #include #include #include +#include +#include +#include +#include +#include #include #include #include @@ -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( + trajopt.AddPathPositionConstraint(std::make_shared( &plant, params_.collision_check_lower_distance_bound, &plant_context), static_cast(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( + &plant, base_frame, X_AbarA, -offset_vec, offset_vec, link_ee_frame, + Eigen::Vector3d(0.0, 0.0, 0.0), &plant_context), + static_cast(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(&plant, base_frame, lower_bound, upper_bound, link_ee_frame, - Eigen::Vector3d(0.0, 0.0, 0.0), &plant_context), - static_cast(i) / (params_.num_position_constraint_points - 1)); + trajopt.AddPathPositionConstraint(std::make_shared( + &plant, base_frame, X_AbarA, -offset_vec, offset_vec, link_ee_frame, + Eigen::Vector3d(0.0, 0.0, 0.0), &plant_context), + static_cast(i) / (params_.num_position_inequality_points - 1)); } } } } +void KTOptPlanningContext::addPathOrientationConstraints(KinematicTrajectoryOptimization& trajopt, + const MultibodyPlant& plant, + Context& 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( + &plant, link_ee_frame, drake::math::RotationMatrixd::Identity(), base_frame, + R_BbarB, theta_bound, &plant_context), + static_cast(i) / (params_.num_orientation_constraint_points - 1)); + } + } +} + bool KTOptPlanningContext::terminate() { RCLCPP_ERROR(getLogger(), "KTOptPlanningContext::terminate() is not implemented!");