Set position constraints via Moveit's PositionConstraint message #51

kamiradi · 2024-10-05T15:02:17Z

This PR setsup a MoveIt PositionConstraint transcription. It achieves the following

Reads Constraints in the MotionPlanningRequest
retrieves PostionConstraint based box constraint
Transcribes a drake Bounding Box constraint based on dimensions and pose of the box constraint.

…s.msg

sea-bass

Awesome to see! Few things I think should change in this one before we merge it.

As another optional recommendation, especially as we also seek to add orientation constraints, it might be helpful to factor out the addition of constraints to a separate function within this file.

sea-bass · 2024-10-07T11:37:20Z

CMakeLists.txt

@@ -23,6 +23,7 @@ find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rviz_visual_tools REQUIRED)
 find_package(warehouse_ros REQUIRED)
+find_package(shape_msgs REQUIRED)


nit: in this whole file, can you add shape_msgs in alphabetical order?

sea-bass · 2024-10-07T11:37:36Z

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",


Suggested change

description: "Name of the end-effector frame as per urdf/sdf",

description: "Name of the end-effector frame as per urdf/srdf",

sea-bass · 2024-10-07T11:39:04Z

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: {


Why do we need this parameter? The constraint message itself contains a link name field and we should likely use that:

https://github.com/moveit/moveit_msgs/blob/ros2/msg/PositionConstraint.msg#L5-L6

I put this because I did not see a panda_EE frame on the visualised robot_description on RViz. Whereas the Drake urdf has it, and the PositionConstraint requires this. I'll debug this further.

Hmmm, weird. Can you use another frame that does exist in both URDFs?

Or maybe this is sufficient to have us make a local version of the Drake URDF? Worth discussing in 30 min.

Per discussion, let's just use the link_name field in the constraint message and "trust" that the planner has a URDF that knows about this frame (or errors if one is not found).

Latest commit reflects this.

sea-bass · 2024-10-07T11:39:57Z

src/ktopt_planning_context.cpp

+      // 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::msg::SolidPrimitive::BOX)


Would prefer the reverse: If the primitive is not a box, raise a warning and continue the loop; else, carry on.

This, in addition to being more readable, will save you an indent level.

sea-bass · 2024-10-07T11:41:25Z

src/ktopt_planning_context.cpp

+        for (int i = 0; i < 10; ++i)
+        {
+          trajopt.AddPathPositionConstraint(
+              std::make_shared<PositionConstraint>(&plant, plant.world_frame(), lower_bound, upper_bound, link_ee_frame,
+                                                   Eigen::Vector3d(0.0, 0.1, 0.0), &plant_context),
+              static_cast<double>(i) / 10.0);


The 10 here should not be hard-coded; it should be a parameter. See e.g. how the collision constraints are setup with num_collision_check_points:

https://github.com/moveit/moveit_drake/blob/main/res/ktopt_moveit_parameters.yaml#L31

sea-bass · 2024-10-07T11:45:30Z

src/ktopt_planning_context.cpp

+        {
+          trajopt.AddPathPositionConstraint(
+              std::make_shared<PositionConstraint>(&plant, plant.world_frame(), lower_bound, upper_bound, link_ee_frame,
+                                                   Eigen::Vector3d(0.0, 0.1, 0.0), &plant_context),


Where does this Eigen::Vector(0.0, 0.1, 0.0) come from for the p_BQ argument?

Based on this description:

"Constrains the position of a point Q, rigidly attached to a frame B, to be within a bounding box measured and expressed in frame A."

It then seems like we should probably use all zeros for the translation if the point we're constraining is exactly the target frame?

Oops, good catch, I copied the argument list from one of my personal examples. You are right, i'll change this.

sea-bass · 2024-10-08T11:31:58Z

CMakeLists.txt

+  moveit_core
+  moveit_msgs
+  EIGEN3
+  shape_msgs)


Here too re: alphabetical order

sea-bass · 2024-10-08T11:33:20Z

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: {


Hmmm, weird. Can you use another frame that does exist in both URDFs?

Or maybe this is sufficient to have us make a local version of the Drake URDF? Worth discussing in 30 min.

sea-bass · 2024-10-08T11:34:01Z

src/ktopt_planning_context.cpp

+      const auto& primitive = position_constraint.constraint_region.primitives[0];
+      if (primitive.type != shape_msgs::msg::SolidPrimitive::BOX)
+      {
+        continue;


Worth a warning here? E.g.

"Geometric primitive must be a box. Cannot add position constraint."

sea-bass · 2024-10-08T11:34:15Z

src/ktopt_planning_context.cpp

+      Eigen::Vector3d upper_bound = box_center + Eigen::Vector3d(x_dim, y_dim, z_dim);
+
+      // Add position constraint to each knot point of the trajectory
+      for (int i = 0; i < 10; ++i)


Use the parameter here as well

sea-bass · 2024-10-08T12:00:46Z

src/ktopt_planning_context.cpp

+  // auto robot_instance = Parser(plant_,
+  // scene_graph_).AddModelsFromString(robot_description_, ".urdf");


weird line break, is this from an autoformatter?

yup. Ill change it

kamiradi added 4 commits October 5, 2024 14:58

[FIX] Readme correction

b5c6f2f

[WIP] adds Box Constraints

36d535b

[FIX] processes Moveit's PositionConstraint message within Constraint…

1851516

…s.msg

pre-commit

1fef47a

kamiradi marked this pull request as ready for review October 7, 2024 10:50

kamiradi requested review from sea-bass and sjahr and removed request for sea-bass October 7, 2024 10:50

sea-bass requested changes Oct 7, 2024

View reviewed changes

[FIX] addresses review comments

b0514d9

kamiradi requested a review from sea-bass October 8, 2024 11:28

sea-bass reviewed Oct 8, 2024

View reviewed changes

[FIX] Adds warning, corrects order in cmakelists

fb4fcb8

sea-bass reviewed Oct 8, 2024

View reviewed changes

[FIX] removes link_ee

db0fe36

kamiradi requested a review from sea-bass October 8, 2024 13:58

sea-bass approved these changes Oct 8, 2024

View reviewed changes

kamiradi merged commit 11e3e4e into moveit:main Oct 8, 2024
2 checks passed

kamiradi deleted the pr-position_constraint branch October 8, 2024 17:01

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Set position constraints via Moveit's PositionConstraint message #51

Set position constraints via Moveit's PositionConstraint message #51

kamiradi commented Oct 5, 2024 •

edited

Loading

sea-bass left a comment

sea-bass Oct 7, 2024

sea-bass Oct 7, 2024

sea-bass Oct 7, 2024

kamiradi Oct 7, 2024

sea-bass Oct 8, 2024

sea-bass Oct 8, 2024

kamiradi Oct 8, 2024

sea-bass Oct 7, 2024

sea-bass Oct 7, 2024

sea-bass Oct 7, 2024

kamiradi Oct 7, 2024

sea-bass Oct 8, 2024

sea-bass Oct 8, 2024

sea-bass Oct 8, 2024

sea-bass Oct 8, 2024

sea-bass Oct 8, 2024

kamiradi Oct 8, 2024

	description: "Name of the end-effector frame as per urdf/sdf",
	description: "Name of the end-effector frame as per urdf/srdf",

		// auto robot_instance = Parser(plant_,
		// scene_graph_).AddModelsFromString(robot_description_, ".urdf");

Set position constraints via Moveit's PositionConstraint message #51

Set position constraints via Moveit's PositionConstraint message #51

Conversation

kamiradi commented Oct 5, 2024 • edited Loading

sea-bass left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kamiradi commented Oct 5, 2024 •

edited

Loading