Add constrained planning tutorial #52

sjahr · 2024-10-10T07:17:16Z

Run with

ros2 launch moveit_drake constrained_planning_demo.launch.py

demo/launch/constrained_planning_demo.launch.py

demo/src/constrained_planning_demo.cpp

sjahr · 2024-10-13T11:00:54Z

@kamiradi Good news: The planner is now loadable 🎉 Bad news: KTOpt fails to plan in both demos --> Maybe the problem is overconstrained? I clean-ed up the position - jerk constraint parsing, feel free to double check but it should work but I fear we introduced and error somewhere along the line that we need to debug

sea-bass · 2024-10-13T11:41:37Z

Does it work if you remove the constraints?

kamiradi · 2024-10-13T11:56:41Z

It is most definitely the box constraints. I tried it with the jerk constraints and the pipeline benchmark was working. I knew this was going to be an issue, bit waited for the constrained planing Tut to work.

…

On Sun, 13 Oct, 2024, 12:41 pm Sebastian Castro, ***@***.***> wrote: Does it work if you remove the constraints? — Reply to this email directly, view it on GitHub <#52 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AFNFZJASJLV7Q35PT4JAEA3Z3JMANAVCNFSM6AAAAABPWDELVGVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDIMBYHE2DINZRGU> . You are receiving this because you were mentioned.Message ID: ***@***.***>

sea-bass · 2024-10-13T12:13:17Z

Sorry, I keep forgetting that my computer can't handle building MoveIt from source unless I add swap/zram and I just crashed mid build, then ran out of time to try again.

Things look largely okay, the one place I would check is whether using plant.world_frame() as the "frame A" vs. something like "panda_link0", which is the base of the planning group? Or maybe those frames are coincident and it's something else.

kamiradi · 2024-10-13T13:15:43Z

Yeah no worries. I'll have a look tomorrow morning

…

On Sun, 13 Oct, 2024, 1:13 pm Sebastian Castro, ***@***.***> wrote: Sorry, I keep forgetting that my computer can't handle building MoveIt from source unless I add swap/zram and I just crashed mid build, then ran out of time to try again. Things look largely okay, the one place I would check is whether using plant.world_frame() as the "frame A" vs. something like "panda_link0", which is the base of the planning group? Or maybe those frames are coincident and it's something else. — Reply to this email directly, view it on GitHub <#52 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AFNFZJEY2V4IVVQH2KOG4JLZ3JPXFAVCNFSM6AAAAABPWDELVGVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDIMBYHE2TKNRQGU> . You are receiving this because you were mentioned.Message ID: ***@***.***>

sea-bass · 2024-10-13T15:10:58Z

I can only get this to work if:

I disable the Meschat visualization (to remove the waiting times)
I remove the position/velocity/acceleration/jerk constraints
I remove the path constraints

So I'm not really sure whether 2. or 3. is the problem here.

kamiradi · 2024-10-13T15:20:41Z

Having a look at this now

…

On Sun, 13 Oct, 2024, 4:11 pm Sebastian Castro, ***@***.***> wrote: I can only get this to work if: 1. I disable the Meschat visualization (to remove the waiting times) 2. I remove the position/velocity/acceleration/jerk constraints 3. I remove the path constraints So I'm not really sure whether 2. or 3. is the problem here. — Reply to this email directly, view it on GitHub <#52 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AFNFZJGFUNANP3OB6DC7CI3Z3KERRAVCNFSM6AAAAABPWDELVGVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDIMBZGAYTKNZRG4> . You are receiving this because you were mentioned.Message ID: ***@***.***>

sea-bass · 2024-10-13T16:51:31Z

OK I found the issue with the joint bounds... using -std::numeric_limits<double>::max() for the unbounded joints (in this case, gripper) could be bad -- we should be using std::numeric_limits<double>::lowest().

Also, the gripper joints limits were actually (un)initialized to garbage values, which caused failures.

Furthermore, using these std::numeric_limits extremum values for the velocity, acceleration, and jerk also caused optimization to fail. I defined constant that set it to a large finite value (I'm using 100) for all unbounded joints, and this makes things work.

So that clears issue 2 above... now for the constraints.

sea-bass · 2024-10-13T17:00:39Z

For issue 3, I was able to address this by padding the constraints to be more conservative at the optimizer level.

After fixing the above problem with joint limits, I realized that the optimization now succeeded with constraints, but failed on the validation step. This makes sense, as Drake only optimizes the constraints at the sampled points we specify... and intermediate trajectory points could (and do) therefore violate constraints.

Adding a padding distance parameter to the generate_parameter_library YAML, I could get things working!

2024-10-13.13-00-09.mp4

I could even thin the boundary further!

2024-10-13.13-01-53.mp4

…onstraints

sea-bass

Approving since it now runs correctly, but would be good if someone else gave my changes a look.

sea-bass · 2024-10-13T17:05:07Z

src/ktopt_planning_context.cpp

-    // TODO: This should not hold up planning time
-    std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(params_.trajectory_time_step * 10000.0)));
-  }
-  visualizer_->StopRecording();
-  visualizer_->PublishRecording();
+  // visualizer_->StartRecording();
+  // const auto num_pts = static_cast<size_t>(std::ceil(traj.end_time() / params_.trajectory_time_step) + 1);
+  // for (unsigned int i = 0; i < num_pts; ++i)
+  // {
+  //   const auto t_scale = static_cast<double>(i) / static_cast<double>(num_pts - 1);
+  //   const auto t = std::min(t_scale, 1.0) * traj.end_time();
+  //   plant.SetPositions(&plant_context, traj.value(t));
+  //   auto& vis_context = visualizer_->GetMyContextFromRoot(*diagram_context_);
+  //   visualizer_->ForcedPublish(vis_context);
+  //   // Without these sleeps, the visualizer won't give you time to load your
+  //   // browser
+  //   // TODO: This should not hold up planning time
+  //   std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(params_.trajectory_time_step * 10000.0)));
+  // }
+  // visualizer_->StopRecording();
+  // visualizer_->PublishRecording();


I commented this out BTW. If you don't, then "planning takes forever" (but eventually returns), although it gives you time to hop over into Meshcat to see the results there.

Maybe the meshcat visualization flag should be a boolean parameter in our YAML file?

sea-bass · 2024-10-13T17:13:28Z

One last comment on this -- when I looked at Meshcat, the gripper fingers definitely fly all over the place (which might be why I had to bound the velocity/accel/jerk to finite values, else the gripper fingers might have flown into collision or sparked another silent failure).

I think what we need to do is take all of the joints that are not actively part of this group, and constrain their positions to exactly the starting positions + velocities and further derivatives to zero. Will let someone else try that out.

kamiradi · 2024-10-13T18:02:22Z

Works for me as well 🥳 . I added meshcat visualise to the parameter yaml as suggested.

I'll track the panda finger as a separate issue and get to it. For now the bounding box constraint works, I'll add the orientation constraints and we can call it a ROSCon.

Add contrained planning tutorial

b8103d8

sjahr requested review from sea-bass and kamiradi October 10, 2024 07:17

sjahr commented Oct 10, 2024

View reviewed changes

demo/launch/constrained_planning_demo.launch.py Show resolved Hide resolved

sjahr commented Oct 10, 2024

View reviewed changes

demo/src/constrained_planning_demo.cpp Outdated Show resolved Hide resolved

sjahr changed the title ~~Add contrained planning tutorial~~ Add constrained planning tutorial Oct 10, 2024

sjahr added 3 commits October 13, 2024 12:02

Make it work

2164320

Update limit compution

f2cb400

Re-enable jerk bounds

a49675d

Remove unused variables

490d098

Updates and small bug fix

63ba880

Fix issues by making kinematics limits finite and adding padding to c…

4043fb1

…onstraints

sea-bass approved these changes Oct 13, 2024

View reviewed changes

sea-bass reviewed Oct 13, 2024

View reviewed changes

kamiradi merged commit 6851f8a into moveit:main Oct 13, 2024
2 checks passed

kamiradi mentioned this pull request Oct 14, 2024

Deal with joints not in the planning group #56

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add constrained planning tutorial #52

Add constrained planning tutorial #52

sjahr commented Oct 10, 2024 •

edited

Loading

sjahr commented Oct 13, 2024

sea-bass commented Oct 13, 2024

kamiradi commented Oct 13, 2024 via email

sea-bass commented Oct 13, 2024

kamiradi commented Oct 13, 2024 via email

sea-bass commented Oct 13, 2024 •

edited

Loading

kamiradi commented Oct 13, 2024 via email

sea-bass commented Oct 13, 2024 •

edited

Loading

sea-bass commented Oct 13, 2024 •

edited

Loading

sea-bass left a comment

sea-bass Oct 13, 2024 •

edited

Loading

sea-bass commented Oct 13, 2024 •

edited

Loading

kamiradi commented Oct 13, 2024

Add constrained planning tutorial #52

Add constrained planning tutorial #52

Conversation

sjahr commented Oct 10, 2024 • edited Loading

sjahr commented Oct 13, 2024

sea-bass commented Oct 13, 2024

kamiradi commented Oct 13, 2024 via email

sea-bass commented Oct 13, 2024

kamiradi commented Oct 13, 2024 via email

sea-bass commented Oct 13, 2024 • edited Loading

kamiradi commented Oct 13, 2024 via email

sea-bass commented Oct 13, 2024 • edited Loading

sea-bass commented Oct 13, 2024 • edited Loading

sea-bass left a comment

Choose a reason for hiding this comment

sea-bass Oct 13, 2024 • edited Loading

Choose a reason for hiding this comment

sea-bass commented Oct 13, 2024 • edited Loading

kamiradi commented Oct 13, 2024

sjahr commented Oct 10, 2024 •

edited

Loading

sea-bass commented Oct 13, 2024 •

edited

Loading

sea-bass commented Oct 13, 2024 •

edited

Loading

sea-bass commented Oct 13, 2024 •

edited

Loading

sea-bass Oct 13, 2024 •

edited

Loading

sea-bass commented Oct 13, 2024 •

edited

Loading