Planning scene transcription

[kamiradi]

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

[kamiradi]

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.

[sjahr]

Oh an it looks like you need to rebase on main

[scastro-bdai]

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

[kamiradi]

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.

[kamiradi]

@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]

@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]

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]

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]

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

[sea-bass]

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