Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adds orientation constraints #55

Merged
merged 7 commits into from
Oct 15, 2024
Merged

Adds orientation constraints #55

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
6fad1fd
tweaks constrained planning tut to have only box and orientation cons…
kamiradi Oct 14, 2024
5d894c8
[FT] adds orientation constraints
kamiradi Oct 14, 2024
ca77262
Merge branch 'main' into pr-orientation
sea-bass Oct 15, 2024
7164d03
Merge remote-tracking branch 'origin/main' into pr-orientation
sea-bass Oct 15, 2024
66f6797
Add docstring based on latest code cleanup
sea-bass Oct 15, 2024
b4b99a0
Fix build errors
sea-bass Oct 15, 2024
f04c8db
Working inequality and equality constraints position and orientation
kamiradi Oct 15, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
234 changes: 117 additions & 117 deletions demo/src/constrained_planning_demo.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,16 +101,16 @@ int main(int argc, char** argv)

// Now wait for the user to press Next before trying the planar constraints.
moveit_visual_tools.prompt(
"Press 'Next' in the RvizVisualToolsGui window to continue to the planar constraint example");
"Press 'Next' in the RvizVisualToolsGui window to continue to the orientation constraint example");

// Clear the path constraints and markers for the next example
reset_demo();
/*reset_demo();*/

// In the second problem we plan with the end-effector constrained to a plane.
// We need to create a pose goal that lies in this plane.
// The plane is tilted by 45 degrees, so moving an equal amount in the y and z direction should be ok.
// Any goal or start state should also satisfy the path constraints.
target_pose = get_relative_pose(0.0, 0.3, -0.3);
/*target_pose = get_relative_pose(0.0, 0.3, -0.3);*/
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just talked to Aditya and I think we should keep and uncomment as these are the move-on-line and move-within-plane demos

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sounds good. I do suspect that the rotated bounding box constraints may not work with the current implementation of position constraints -- there is another overload that accepts a rigid transform from a coordinate frame A to A_bar which enables rotating the reference frame, and we may need to achieve this.

Trying it out will tell.


// We create a plane perpendicular to the y-axis and tilt it by 45 degrees

Expand All @@ -122,88 +122,88 @@ int main(int argc, char** argv)
// (:code:`1e-4` by default). MoveIt will use the stricter tolerance (the box width) to check the constraints, and
// many states will appear invalid. That's where the magic number :code:`0.0005` comes from, it is between
// :code:`0.00001` and :code:`0.001`.
moveit_msgs::msg::PositionConstraint plane_constraint;
plane_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame();
plane_constraint.link_name = move_group_interface.getEndEffectorLink();
shape_msgs::msg::SolidPrimitive plane;
plane.type = shape_msgs::msg::SolidPrimitive::BOX;
plane.dimensions = { 1.0, 0.0005, 1.0 };
plane_constraint.constraint_region.primitives.emplace_back(plane);

geometry_msgs::msg::Pose plane_pose;
plane_pose.position.x = current_pose.pose.position.x;
plane_pose.position.y = current_pose.pose.position.y;
plane_pose.position.z = current_pose.pose.position.z;
plane_pose.orientation.x = sin(M_PI_4 / 2);
plane_pose.orientation.y = 0.0;
plane_pose.orientation.z = 0.0;
plane_pose.orientation.w = cos(M_PI_4 / 2);
plane_constraint.constraint_region.primitive_poses.emplace_back(plane_pose);
plane_constraint.weight = 1.0;

// Visualize the constraint
auto d = sqrt(pow(target_pose.pose.position.y, 2) + pow(target_pose.pose.position.z, 2));

Eigen::Vector3d normal(0, 1, 1);
moveit_visual_tools.publishNormalAndDistancePlane(normal, d, rviz_visual_tools::TRANSLUCENT_DARK);
moveit_visual_tools.trigger();

moveit_msgs::msg::Constraints plane_constraints;
plane_constraints.position_constraints.emplace_back(plane_constraint);
plane_constraints.name = "use_equality_constraints";
move_group_interface.setPathConstraints(plane_constraints);

// And again, configure and solve the planning problem
move_group_interface.setPoseTarget(target_pose);
move_group_interface.setPlanningTime(10.0);

success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Plan with plane constraint %s", success ? "" : "FAILED");

moveit_visual_tools.prompt(
"Press 'next' in the RvizVisualToolsGui window to continue to the linear constraint example");

reset_demo();

// We can also plan along a line. We can use the same pose as last time.
target_pose = get_relative_pose(0.0, 0.3, -0.3);

// Building on the previous constraint, we can make it a line, by also reducing the dimension of the box in the x-direction.
moveit_msgs::msg::PositionConstraint line_constraint;
line_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame();
line_constraint.link_name = move_group_interface.getEndEffectorLink();
shape_msgs::msg::SolidPrimitive line;
line.type = shape_msgs::msg::SolidPrimitive::BOX;
line.dimensions = { 0.0005, 0.0005, 1.0 };
line_constraint.constraint_region.primitives.emplace_back(line);

geometry_msgs::msg::Pose line_pose;
line_pose.position.x = current_pose.pose.position.x;
line_pose.position.y = current_pose.pose.position.y;
line_pose.position.z = current_pose.pose.position.z;
line_pose.orientation.x = sin(M_PI_4 / 2);
line_pose.orientation.y = 0.0;
line_pose.orientation.z = 0.0;
line_pose.orientation.w = cos(M_PI_4 / 2);
line_constraint.constraint_region.primitive_poses.emplace_back(line_pose);
line_constraint.weight = 1.0;

// Visualize the constraint
moveit_visual_tools.publishLine(current_pose.pose.position, target_pose.pose.position,
rviz_visual_tools::TRANSLUCENT_DARK);
moveit_visual_tools.trigger();

moveit_msgs::msg::Constraints line_constraints;
line_constraints.position_constraints.emplace_back(line_constraint);
line_constraints.name = "use_equality_constraints";
move_group_interface.setPathConstraints(line_constraints);
move_group_interface.setPoseTarget(target_pose);

success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Plan with line constraint %s", success ? "" : "FAILED");

moveit_visual_tools.prompt(
"Press 'Next' in the RvizVisualToolsGui window to continue to the orientation constraint example");
/*moveit_msgs::msg::PositionConstraint plane_constraint;*/
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Remove whatever is commented out

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Same here

/*plane_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame();*/
/*plane_constraint.link_name = move_group_interface.getEndEffectorLink();*/
/*shape_msgs::msg::SolidPrimitive plane;*/
/*plane.type = shape_msgs::msg::SolidPrimitive::BOX;*/
/*plane.dimensions = { 1.0, 0.0005, 1.0 };*/
/*plane_constraint.constraint_region.primitives.emplace_back(plane);*/
/**/
/*geometry_msgs::msg::Pose plane_pose;*/
/*plane_pose.position.x = current_pose.pose.position.x;*/
/*plane_pose.position.y = current_pose.pose.position.y;*/
/*plane_pose.position.z = current_pose.pose.position.z;*/
/*plane_pose.orientation.x = sin(M_PI_4 / 2);*/
/*plane_pose.orientation.y = 0.0;*/
/*plane_pose.orientation.z = 0.0;*/
/*plane_pose.orientation.w = cos(M_PI_4 / 2);*/
/*plane_constraint.constraint_region.primitive_poses.emplace_back(plane_pose);*/
/*plane_constraint.weight = 1.0;*/
/**/
/*// Visualize the constraint*/
/*auto d = sqrt(pow(target_pose.pose.position.y, 2) + pow(target_pose.pose.position.z, 2));*/
/**/
/*Eigen::Vector3d normal(0, 1, 1);*/
/*moveit_visual_tools.publishNormalAndDistancePlane(normal, d, rviz_visual_tools::TRANSLUCENT_DARK);*/
/*moveit_visual_tools.trigger();*/
/**/
/*moveit_msgs::msg::Constraints plane_constraints;*/
/*plane_constraints.position_constraints.emplace_back(plane_constraint);*/
/*plane_constraints.name = "use_equality_constraints";*/
/*move_group_interface.setPathConstraints(plane_constraints);*/
/**/
/*// And again, configure and solve the planning problem*/
/*move_group_interface.setPoseTarget(target_pose);*/
/*move_group_interface.setPlanningTime(10.0);*/
/**/
/*success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);*/
/*RCLCPP_INFO(LOGGER, "Plan with plane constraint %s", success ? "" : "FAILED");*/
/**/
/*moveit_visual_tools.prompt(*/
/* "Press 'next' in the RvizVisualToolsGui window to continue to the linear constraint example");*/
/**/
/*reset_demo();*/
/**/
/*// We can also plan along a line. We can use the same pose as last time.*/
/*target_pose = get_relative_pose(0.0, 0.3, -0.3);*/
/**/
/*// Building on the previous constraint, we can make it a line, by also reducing the dimension of the box in the x-direction.*/
/*moveit_msgs::msg::PositionConstraint line_constraint;*/
/*line_constraint.header.frame_id = move_group_interface.getPoseReferenceFrame();*/
/*line_constraint.link_name = move_group_interface.getEndEffectorLink();*/
/*shape_msgs::msg::SolidPrimitive line;*/
/*line.type = shape_msgs::msg::SolidPrimitive::BOX;*/
/*line.dimensions = { 0.0005, 0.0005, 1.0 };*/
/*line_constraint.constraint_region.primitives.emplace_back(line);*/
/**/
/*geometry_msgs::msg::Pose line_pose;*/
/*line_pose.position.x = current_pose.pose.position.x;*/
/*line_pose.position.y = current_pose.pose.position.y;*/
/*line_pose.position.z = current_pose.pose.position.z;*/
/*line_pose.orientation.x = sin(M_PI_4 / 2);*/
/*line_pose.orientation.y = 0.0;*/
/*line_pose.orientation.z = 0.0;*/
/*line_pose.orientation.w = cos(M_PI_4 / 2);*/
/*line_constraint.constraint_region.primitive_poses.emplace_back(line_pose);*/
/*line_constraint.weight = 1.0;*/
/**/
/*// Visualize the constraint*/
/*moveit_visual_tools.publishLine(current_pose.pose.position, target_pose.pose.position,*/
/* rviz_visual_tools::TRANSLUCENT_DARK);*/
/*moveit_visual_tools.trigger();*/
/**/
/*moveit_msgs::msg::Constraints line_constraints;*/
/*line_constraints.position_constraints.emplace_back(line_constraint);*/
/*line_constraints.name = "use_equality_constraints";*/
/*move_group_interface.setPathConstraints(line_constraints);*/
/*move_group_interface.setPoseTarget(target_pose);*/
/**/
/*success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);*/
/*RCLCPP_INFO(LOGGER, "Plan with line constraint %s", success ? "" : "FAILED");*/
/**/
/*moveit_visual_tools.prompt(*/
/* "Press 'Next' in the RvizVisualToolsGui window to continue to the orientation constraint example");*/
reset_demo();

// Finally, we can place constraints on orientation.
Expand All @@ -229,42 +229,42 @@ int main(int argc, char** argv)
success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Plan with orientation constraint %s", success ? "" : "FAILED");

moveit_visual_tools.prompt("Press 'Next' in the RvizVisualToolsGui window to try mixed_constraints");
/*moveit_visual_tools.prompt("Press 'Next' in the RvizVisualToolsGui window to try mixed_constraints");*/
reset_demo();

// Finally, we can place constraints on orientation.
// Use the target pose from the previous example
target_pose = get_relative_pose(-0.6, 0.1, 0);

// Reuse the orientation constraint, and make a new box constraint
box.dimensions = { 1.0, 0.6, 1.0 };
box_constraint.constraint_region.primitives[0] = box;

box_pose.position.x = 0;
box_pose.position.y = -0.1;
box_pose.position.z = current_pose.pose.position.z;
box_constraint.constraint_region.primitive_poses[0] = box_pose;
box_constraint.weight = 1.0;

// Visualize the box constraint
Eigen::Vector3d new_box_point_1(box_pose.position.x - box.dimensions[0] / 2,
box_pose.position.y - box.dimensions[1] / 2,
box_pose.position.z - box.dimensions[2] / 2);
Eigen::Vector3d new_box_point_2(box_pose.position.x + box.dimensions[0] / 2,
box_pose.position.y + box.dimensions[1] / 2,
box_pose.position.z + box.dimensions[2] / 2);
moveit_msgs::msg::Constraints mixed_constraints;
mixed_constraints.position_constraints.emplace_back(box_constraint);
mixed_constraints.orientation_constraints.emplace_back(orientation_constraint);
moveit_visual_tools.publishCuboid(new_box_point_1, new_box_point_2, rviz_visual_tools::TRANSLUCENT_DARK);
moveit_visual_tools.trigger();

move_group_interface.setPathConstraints(mixed_constraints);
move_group_interface.setPoseTarget(target_pose);
move_group_interface.setPlanningTime(20.0);

success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
RCLCPP_INFO(LOGGER, "Plan with mixed constraint %s", success ? "" : "FAILED");
/*target_pose = get_relative_pose(-0.6, 0.1, 0);*/
/**/
/*// Reuse the orientation constraint, and make a new box constraint*/
/*box.dimensions = { 1.0, 0.6, 1.0 };*/
/*box_constraint.constraint_region.primitives[0] = box;*/
/**/
/*box_pose.position.x = 0;*/
/*box_pose.position.y = -0.1;*/
/*box_pose.position.z = current_pose.pose.position.z;*/
/*box_constraint.constraint_region.primitive_poses[0] = box_pose;*/
/*box_constraint.weight = 1.0;*/
/**/
/*// Visualize the box constraint*/
/*Eigen::Vector3d new_box_point_1(box_pose.position.x - box.dimensions[0] / 2,*/
/* box_pose.position.y - box.dimensions[1] / 2,*/
/* box_pose.position.z - box.dimensions[2] / 2);*/
/*Eigen::Vector3d new_box_point_2(box_pose.position.x + box.dimensions[0] / 2,*/
/* box_pose.position.y + box.dimensions[1] / 2,*/
/* box_pose.position.z + box.dimensions[2] / 2);*/
/*moveit_msgs::msg::Constraints mixed_constraints;*/
/*mixed_constraints.position_constraints.emplace_back(box_constraint);*/
/*mixed_constraints.orientation_constraints.emplace_back(orientation_constraint);*/
/*moveit_visual_tools.publishCuboid(new_box_point_1, new_box_point_2, rviz_visual_tools::TRANSLUCENT_DARK);*/
/*moveit_visual_tools.trigger();*/
/**/
/*move_group_interface.setPathConstraints(mixed_constraints);*/
/*move_group_interface.setPoseTarget(target_pose);*/
/*move_group_interface.setPlanningTime(20.0);*/
/**/
/*success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);*/
/*RCLCPP_INFO(LOGGER, "Plan with mixed constraint %s", success ? "" : "FAILED");*/

// Done!
moveit_visual_tools.prompt("Press 'Next' in the RvizVisualToolsGui window to clear the markers");
Expand Down
16 changes: 12 additions & 4 deletions include/ktopt_interface/ktopt_planning_context.hpp
View file
Open in desktop
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
Loading
Loading