Adds orientation constraints #55

kamiradi · 2024-10-14T08:47:45Z

The orientation constraints are working. The padding was a really good catch @sea-bass. Also, for some reason I don't understand completely, not including orientation constraints also passes MoveIt's constraint validation process. It looks like the B-spline representation handles orientation error pretty well, thought that was interesting.

I have tested this on the constrained demo. Can you test it and we can collect some trajectories and visualise them.

Addresses #44

…traints

sea-bass

LGTM, just a few typos and more of a question.

Also, just remove the extra tutorial code instead of commenting out

sea-bass · 2024-10-14T12:43:07Z

demo/src/constrained_planning_demo.cpp


  // 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);*/


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

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.

sea-bass · 2024-10-14T12:43:23Z

demo/src/constrained_planning_demo.cpp

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


Remove whatever is commented out

src/ktopt_planning_context.cpp

sea-bass · 2024-10-14T12:50:13Z

src/ktopt_planning_context.cpp

+      // of the tolerances as the theta bound
+      const double theta_bound = orientation_constraint.absolute_x_axis_tolerance;
+
+      // Add position constraint to each knot point of the trajectory


I was wondering in the last PR whether we should have used the same number of knot points for all position/orientation constraints. What do you think?

It is a parameter, so the user has the option of adding more/less constraints along the path.

Yes, I meant should there be 2 separate parameters for position/orientation constraints, or just use the same parameter for all constraints?

Ah I meant, right now even though im reading theta bound as the tolerance, I am using the one @sea-bass added as a parameter.

sjahr

Test it and it works for me, we can merge it once the comments are addressed

sjahr · 2024-10-14T13:00:05Z

demo/src/constrained_planning_demo.cpp


  // 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);*/


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

sjahr · 2024-10-14T13:00:26Z

demo/src/constrained_planning_demo.cpp

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


rhaschke

Just a few remarks

rhaschke · 2024-10-15T08:58:59Z

src/ktopt_planning_context.cpp

+      // Frame A: The frame in which the orientation constraint is defined
+      // Frame B: The frame to which the orientation constraint is enforced


In the code below, the roles of A and B are exactly swapped: orientation_constraint.link_name defines the frame which is enforced and orientation_constraint.header.frame_id is the defining frame.

Oooh nice catch. Thanks for that! However, I was adding it correctly into the constraints. I'll update the comments

rhaschke · 2024-10-15T09:04:39Z

src/ktopt_planning_context.cpp

+      // Add position constraint to each knot point of the trajectory
+      for (int i = 0; i < params_.num_orientation_constraint_points; ++i)
+      {
+        trajopt.AddPathPositionConstraint(


PositionConstraint?

Strangely enough, this is correct...
https://stackoverflow.com/q/79041210

I believe the terminology reflects that it is indeed still a constraint on the generalized positions, which happens to be a vector in the configuration space. Therefore, you are adding a constraint on one coordinate (link end-effector) out of all the available coordinates. At least this is my understanding

rhaschke · 2024-10-15T09:04:59Z

src/ktopt_planning_context.cpp

+      // NOTE: There are 3 tolerances given for the orientation constraint in moveit
+      // message, rake 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;


theta_bound is not used!?

Yeah, I think theta_bound and padding need to be combined in some way in the constructor of OrientationConstraint below, whereas only padding is currently used at the moment. That seems incorrect.

Probably just theta_bound - padding, or theta_bound - 2.0 * padding depending on how we want to enforce it? (plus some max to make sure the bound does not go below zero)

Also, since theta_bound uses the whole angle (per the documentation), does it make sense to do something like take the norm of the X/Y/Z angle tolerances?

This is still not a proper calculation, so I think we anyhow have to be "creative" in how we interpret the tolerances in the MoveIt message vs. what translates to here.

It is but only via the orientation padding parameter in the yaml file.

no, the variable theta_bound needs to be used in the actual OrientationConstraint creation.

I agree with #55 (comment) to use the norm or sum of all angle tolerances. MoveIt's orientation constraint is much more flexible than Drake's one, allowing deviations to be specified for all rotation axes separately. It will only translate in a meaningful way if the MoveIt tolerances are all identical.

So for this, a better approach would be to add an additional rotational transformation to R_BbarB, computing R_BbarBx, R_BbarBy and R_BbarBz - add 3 different orientation constraints for each axis with the theta_bounds being the individual tolerances.

This will not work. Drake's orientation constraint already fully constrains all three axes. Adding more orientation constraints doesn't make sense.

sea-bass · 2024-10-15T10:43:09Z

@kamiradi I resolved the conflicts with #57 -- all yours again!

rhaschke · 2024-10-15T11:57:14Z

src/ktopt_planning_context.cpp

+      trajopt.AddPathPositionConstraint(std::make_shared<drake::multibody::OrientationConstraint>(
+                                            &plant, base_frame, drake::math::RotationMatrixd::Identity(), link_ee_frame,
+                                            R_BbarB, padding, &plant_context),


Having read the documentation, I think link_ee_frame and base_frame need to be exchanged:

Suggested change

trajopt.AddPathPositionConstraint(std::make_shared<drake::multibody::OrientationConstraint>(

&plant, base_frame, drake::math::RotationMatrixd::Identity(), link_ee_frame,

R_BbarB, padding, &plant_context),

trajopt.AddPathPositionConstraint(std::make_shared<drake::multibody::OrientationConstraint>(

&plant, link_ee_frame, drake::math::RotationMatrixd::Identity(), base_frame,

R_BbarB, theta_bound, &plant_context),

In MoveIt, the constraint's orientation is an offset given w.r.t. frame_id / base_frame, not w.r.t. link_ee_frame.

Thanks for this @rhaschke. You are absolutely right, I assumed it would be the same as PositionConstraint. Thanks for pointing this out.

rhaschke · 2024-10-15T12:01:57Z

src/ktopt_planning_context.cpp

+      // NOTE: There are 3 tolerances given for the orientation constraint in moveit
+      // message, rake 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;


I agree with #55 (comment) to use the norm or sum of all angle tolerances. MoveIt's orientation constraint is much more flexible than Drake's one, allowing deviations to be specified for all rotation axes separately. It will only translate in a meaningful way if the MoveIt tolerances are all identical.

kamiradi · 2024-10-15T14:45:59Z

Allright, we have the constrained demo working for the box constraint, orientation and the mixed-constraint example.

For the equality constraints, either you have to increase the number of points of constraint application - making it difficult for the optimization to succeed, or have only 10 points and the validator fails the motion plan. I have played around with the ideal number, it returns a plan if number of points are 25 60% of the time. I wouldn't know how to make this more deterministic right now.

Do test and let me know.

sea-bass · 2024-10-15T16:34:00Z

LGTM, go for it!

kamiradi added 2 commits October 14, 2024 09:46

tweaks constrained planning tut to have only box and orientation cons…

6fad1fd

…traints

[FT] adds orientation constraints

5d894c8

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

kamiradi marked this pull request as ready for review October 14, 2024 11:07

sea-bass approved these changes Oct 14, 2024

View reviewed changes

sjahr approved these changes Oct 14, 2024

View reviewed changes

Merge branch 'main' into pr-orientation

ca77262

rhaschke reviewed Oct 15, 2024

View reviewed changes

sea-bass added 2 commits October 15, 2024 06:33

Merge remote-tracking branch 'origin/main' into pr-orientation

7164d03

Add docstring based on latest code cleanup

66f6797

Fix build errors

b4b99a0

rhaschke reviewed Oct 15, 2024

View reviewed changes

Working inequality and equality constraints position and orientation

f04c8db

kamiradi mentioned this pull request Oct 15, 2024

Split Moveit Orientation Constraint into individual axes within drake optimizer #58

Open

kamiradi mentioned this pull request Oct 15, 2024

Enable position and orientation constraint support in KTOpt #44

Closed

kamiradi merged commit 154e32b into moveit:main Oct 15, 2024
2 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Adds orientation constraints #55

Adds orientation constraints #55

kamiradi commented Oct 14, 2024 •

edited

Loading

sea-bass left a comment •

edited

Loading

sea-bass Oct 14, 2024

sjahr Oct 14, 2024

sea-bass Oct 14, 2024

sea-bass Oct 14, 2024

sjahr Oct 14, 2024

sea-bass Oct 14, 2024

kamiradi Oct 14, 2024

sea-bass Oct 14, 2024

kamiradi Oct 14, 2024

sjahr left a comment

sjahr Oct 14, 2024

sjahr Oct 14, 2024

rhaschke left a comment

rhaschke Oct 15, 2024

kamiradi Oct 15, 2024 •

edited

Loading

rhaschke Oct 15, 2024

sea-bass Oct 15, 2024

kamiradi Oct 15, 2024

rhaschke Oct 15, 2024

sea-bass Oct 15, 2024 •

edited

Loading

sea-bass Oct 15, 2024

kamiradi Oct 15, 2024

sea-bass Oct 15, 2024

rhaschke Oct 15, 2024

kamiradi Oct 15, 2024

rhaschke Oct 15, 2024

sea-bass commented Oct 15, 2024

rhaschke Oct 15, 2024

kamiradi Oct 15, 2024

rhaschke Oct 15, 2024

kamiradi commented Oct 15, 2024 •

edited

Loading

sea-bass commented Oct 15, 2024

		// Frame A: The frame in which the orientation constraint is defined
		// Frame B: The frame to which the orientation constraint is enforced

Adds orientation constraints #55

Adds orientation constraints #55

Conversation

kamiradi commented Oct 14, 2024 • edited Loading

sea-bass left a comment • edited Loading

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

sjahr left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rhaschke left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kamiradi Oct 15, 2024 • edited Loading

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

sea-bass Oct 15, 2024 • edited Loading

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

sea-bass commented Oct 15, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kamiradi commented Oct 15, 2024 • edited Loading

sea-bass commented Oct 15, 2024

kamiradi commented Oct 14, 2024 •

edited

Loading

sea-bass left a comment •

edited

Loading

kamiradi Oct 15, 2024 •

edited

Loading

sea-bass Oct 15, 2024 •

edited

Loading

kamiradi commented Oct 15, 2024 •

edited

Loading