Planning scene transcription #23

kamiradi · 2024-07-23T13:06:20Z

This PR works on transcribing a Moveit planning scene to Drake's scene graph.

Edit: @sea-bass @sjahr This PR is out of the draft stage. It accomplishes the following

Set's up a dummy box in the planning scene. This is reflected in the motion planning request.
Transcribes a list of collision objects inside the Planning Scene into the drake Scene Graph. For now, I manually insert the box. I plan to exhaustively setup an object transcription in upcoming PR.
Adds a MinimumDistanceLowerBoundConstraint to the optimization problem. This does the necessary collision checking.
Solves for a collision free trajectory, uses that to warm-start a collision-checked trajectory in the optimization.
Visualizes everything on RVIZ.

Open for reviews

demo file had to be changed to utilise the loaded drake pipeline and the KTOpt plugin

For some reaosn demo cmake is not being configured.

Adds an error message if the robot description in the planner manager has not yet been set. Will not proceed to service any motion planning request until robot description has been set.

This commit wants tests if the initialized subscriber's callback is executed to set the robot description.

For now, loading urdf from drake models rather than ros subcription

This remains untested, build complete

This commit tested a complete motion plan request from the planning pipeline benchmark. It visualises resulting plan on RViZ

Cleanup configs

This commit made a few changes based on the comments fomr reviews. Additionally joint velocties are also set in the robot state.

Hacked MeshCat together

kamiradi · 2024-07-29T14:42:55Z

Hi @sea-bass @sjahr, following is an update

I have transcribed a basic planning scene with a box somewhere within the vicinity of the robot, big enough that it matters, small enough that it can find a solution. I have a basic transcription (PlanningScene to SceneGraph) up on the latest commit. sanity check: KTOPT plans and cube is visualised in meshcat.

However, I am unable to test it as it keeps running into a segmentation fault. The current hypothesis is that the planning scene doesn't yet have all the information to iterate through at the time the MutlibodyPlant and SceneGraph are created (setRobotDescription). This is happening when the planner manager is asked for a planning context.

Any suggestions on debugging?

On a broader note, I know @sea-bass is in favor of writing tests for each part of the code. I have postponed this up until I have a basic (box-test) planning scene transcription. If I had to start setting these up, could you also point me towards resources.

sea-bass

See my comment about trying to not rely on raw pointers to see about the segfault.

As for testing references, you could maybe borrow from here?
https://github.com/moveit/moveit2/blob/main/moveit_core/planning_scene/test/test_planning_scene.cpp

sea-bass · 2024-07-29T15:47:45Z

include/ktopt_interface/ktopt_planning_context.hpp

+  SceneGraph<double>* scene_graph_{};
+  MultibodyPlant<double>* plant_{};
+  std::unique_ptr<Diagram<double>> diagram_;
+  std::unique_ptr<Context<double>> diagram_context_;
+  Context<double>* plant_context_{};
+  Context<double>* visualizer_context_{};


my first suspicion as to why you're getting segfaults is that storing these raw pointers as member variables is actually very memory unsafe and likely something went out of scope.

I think you should actually try removing scene_graph_, plant_, plant_context_, and visualizer_context_ from the member variables -- we recently ran into this at work.

The correct usage is to leverage the diagram_ smart pointer you have to do extractions like these in the function scopes that require them:

const auto& plant = dynamic_cast<const drake::multibody::MultibodyPlant<double>&>(diagram_->GetSubsystemByName("plant")); auto& plant_context = diagram_->GetMutableSubsystemContext(plant, diagram_context_.get());

(and there's probably a similar usage for retrieving the scene graph)

I know it seems like a lot, but I think this is how Drake is intended to be used based on our tireless comb through the source code in lieu of there being very little examples to borrow from.

Thanks for this, I did see them use this in their examples initially, but I went for whatever was compiling. I'll refactor accordingly and test.

sea-bass

I mostly just found cleanup stuff -- this is looking great!

Question: Why doesn't the gripper show up in Meshcat? Does that come from a separate URDF?

demo/src/pipeline_testbench_main.cpp

include/ktopt_interface/ktopt_planning_context.hpp

src/ktopt_planning_context.cpp

sjahr

Still need to test it but here are some small clean-up comments you should address before we merge it 😃

include/ktopt_interface/ktopt_planning_context.hpp

src/ktopt_planning_context.cpp

sjahr · 2024-08-01T14:26:17Z

Oh an it looks like you need to rebase on main

scastro-bdai · 2024-08-01T14:40:22Z

Oh an it looks like you need to rebase on main

Actually, I meant to bring this up.

@kamiradi I think you might be making things difficult for yourself by continuing to use the same dev branch to put up all your PRs. If you simply make feature branches off the moveit_drake main repo each time, I think you won't need to do this on every PR unless someone else is introducing conflicts.

EDIT: Another day, another using the wrong account 😅

kamiradi · 2024-08-02T07:48:46Z

Yeah, I was about to ask general best practices on git and open source, because I may not be using the tool like I am supposed to. I'll bring it up in our next meeting.

sjahr

Just run pre-commit and we can go ahead and merge this!

kamiradi · 2024-08-03T10:30:08Z

@sea-bass @sjahr. Any idea how I can run the pre-commit script inside the docker container. When I run it inside the container, since it is a submodule it raises a "not a git repository error". I tried running it locally on my machine, but that means I run it with clang-format-12 (I believe 14 is required), and does not autoformat the code. I don't have the option of upgrading it as it may lead to other dependencies being upgraded which may break other projects.

Any workarounds?

sea-bass · 2024-08-03T11:14:31Z

@sea-bass @sjahr. Any idea how I can run the pre-commit script inside the docker container. When I run it inside the container, since it is a submodule it raises a "not a git repository error". I tried running it locally on my machine, but that means I run it with clang-format-12 (I believe 14 is required), and does not autoformat the code. I don't have the option of upgrading it as it may lead to other dependencies being upgraded which may break other projects.

Any workarounds?

Is the .git folder, or the pre-commit-config.yaml file ignored in the dockerignore file and not mounted into the container? Or otherwise just not mounted as a volume? That would be a thing to check right away.

Else, I can look into this. In the meantime, just change the clang-format version in your precommit file!

sea-bass · 2024-08-03T11:27:28Z

Seems like it is... dumb question, are you running this from the workspace root, or from the src/moveit_drake folder? (Should be the latter)

kamiradi · 2024-08-03T11:59:30Z

I think I know what is happening, I am currently working with moveit_drake as a gitsubmodule as part of another moveit_drake_contrib package I created at the start of the project. Which is why it does not have a .git folder but a placeholder file within docker. I'll reclone the repository and it should work within docker.

kamiradi · 2024-08-03T12:36:20Z

Nope that wasnt the issue, I still get "not a git directory" from within docker

sea-bass · 2024-08-03T16:51:32Z

Nope that wasnt the issue, I still get "not a git directory" from within docker

I just tried it out by building the regular Docker container. I had to do the following:

docker compose run base moveit

Then, in the container:

# Can be added to the Dockerfile
pip3 install pre-commit
sudo apt install clang-format-14

# Can be added to the entrypoint
git config --global --add safe.directory src/moveit_drake/ 

cd src/moveit_drake
pre-commit run -a

kamiradi and others added 22 commits July 11, 2024 10:03

local changes stash apply

649910f

[WIP] Demo attempts the drake pipeline

071032e

demo file had to be changed to utilise the loaded drake pipeline and the KTOpt plugin

[BUG] cmake configure error in demo/CMakeLists.txt

ffa7384

For some reaosn demo cmake is not being configured.

[WIP] Error message for unset robot description

4ee8524

Adds an error message if the robot description in the planner manager has not yet been set. Will not proceed to service any motion planning request until robot description has been set.

[BUG] robot description subcriber behaviour test

4dcb216

This commit wants tests if the initialized subscriber's callback is executed to set the robot description.

[WIP] Loads urdf from drake model

2a7b83b

For now, loading urdf from drake models rather than ros subcription

[WIP] Transcribes basic optimization problem

72367d7

This remains untested, build complete

[FT] Basic motion plan success

382bea4

This commit tested a complete motion plan request from the planning pipeline benchmark. It visualises resulting plan on RViZ

cleans up a few comments

cb0e8ca

Cleanup configs

8da844f

Get robot description working

ad817c2

Merge pull request #2 from sjahr/dev

48d7b73

Cleanup configs

[FIX] resolves review comments

dfbebcb

[FIX] review comments and joint velocities

5f81ac7

This commit made a few changes based on the comments fomr reviews. Additionally joint velocties are also set in the robot state.

Merge branch 'moveit:main' into dev

03770c0

[WIP] visualisation attempt

bfc5bc6

Hacked MeshCat together

8a54709

Merge pull request #3 from sea-bass/hack-meshcat

7821927

Hacked MeshCat together

[WIP] adds planning scene block next to robot

616883f

[WIP] collision scene transcription

f3682b0

[WIP] debugging plannign scene

1699412

[WIP] segmentation fault

795acbf

sea-bass reviewed Jul 29, 2024

View reviewed changes

kamiradi added 3 commits July 30, 2024 10:49

[WIP] smart pointers and pointer scoping

f9fbc24

[WIP] sanity check pass

41a5898

Motion Planning with collision checking

829ca42

sea-bass marked this pull request as ready for review August 1, 2024 03:01

sea-bass reviewed Aug 1, 2024

View reviewed changes

review comment refactor

8760c40

sea-bass approved these changes Aug 1, 2024

View reviewed changes

sjahr reviewed Aug 1, 2024

View reviewed changes

kamiradi added 3 commits August 2, 2024 09:00

review comments refactored v2

9f5bd78

Merge branch 'main' into dev

d849a6a

code format check

590d951

sjahr approved these changes Aug 2, 2024

View reviewed changes

pre-commit related changes

fad34b6

sea-bass merged commit 204092d into moveit:main Aug 4, 2024
2 checks passed

sea-bass mentioned this pull request Aug 4, 2024

Optimal SceneGraph pointer #11

Closed

kamiradi deleted the dev branch August 6, 2024 07:24

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Planning scene transcription #23

Planning scene transcription #23

kamiradi commented Jul 23, 2024 •

edited

Loading

kamiradi commented Jul 29, 2024 •

edited

Loading

sea-bass left a comment

sea-bass Jul 29, 2024 •

edited

Loading

kamiradi Jul 29, 2024

sea-bass left a comment •

edited

Loading

sjahr left a comment

sjahr commented Aug 1, 2024

scastro-bdai commented Aug 1, 2024 •

edited

Loading

kamiradi commented Aug 2, 2024

sjahr left a comment

kamiradi commented Aug 3, 2024

sea-bass commented Aug 3, 2024 •

edited

Loading

sea-bass commented Aug 3, 2024

kamiradi commented Aug 3, 2024

kamiradi commented Aug 3, 2024

sea-bass commented Aug 3, 2024 •

edited

Loading

Planning scene transcription #23

Planning scene transcription #23

Conversation

kamiradi commented Jul 23, 2024 • edited Loading

kamiradi commented Jul 29, 2024 • edited Loading

sea-bass left a comment

Choose a reason for hiding this comment

sea-bass Jul 29, 2024 • edited Loading

Choose a reason for hiding this comment

kamiradi Jul 29, 2024

Choose a reason for hiding this comment

sea-bass left a comment • edited Loading

Choose a reason for hiding this comment

sjahr left a comment

Choose a reason for hiding this comment

sjahr commented Aug 1, 2024

scastro-bdai commented Aug 1, 2024 • edited Loading

kamiradi commented Aug 2, 2024

sjahr left a comment

Choose a reason for hiding this comment

kamiradi commented Aug 3, 2024

sea-bass commented Aug 3, 2024 • edited Loading

sea-bass commented Aug 3, 2024

kamiradi commented Aug 3, 2024

kamiradi commented Aug 3, 2024

sea-bass commented Aug 3, 2024 • edited Loading

kamiradi commented Jul 23, 2024 •

edited

Loading

kamiradi commented Jul 29, 2024 •

edited

Loading

sea-bass Jul 29, 2024 •

edited

Loading

sea-bass left a comment •

edited

Loading

scastro-bdai commented Aug 1, 2024 •

edited

Loading

sea-bass commented Aug 3, 2024 •

edited

Loading

sea-bass commented Aug 3, 2024 •

edited

Loading