Skip to content

Commit

Permalink
Adds orientation constraints (#55)
Browse files Browse the repository at this point in the history 
* 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]>
  • Loading branch information
@kamiradi @sea-bass
3 people authored Oct 15, 2024
1 parent 4abd38f commit 154e32b
Show file tree
Hide file tree
Showing 3 changed files with 155 additions and 54 deletions.
16 changes: 12 additions & 4 deletions include/ktopt_interface/ktopt_planning_context.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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_;
Expand Down
10 changes: 9 additions & 1 deletion parameters/ktopt_moveit_parameters.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.",
Expand Down
183 changes: 134 additions & 49 deletions src/ktopt_planning_context.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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);
Expand All @@ -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));
}
Expand Down Expand Up @@ -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!");
Expand Down

0 comments on commit 154e32b

Please sign in to comment.