Planning Scene transcription V2 #31

kamiradi · 2024-08-06T11:12:14Z

@sea-bass @sjahr. This is at a reviewable stage. The planning scene transcription contains the following

Transcribes Box, Sphere, Cylinder primitives directly from the planning scene object (tested on box, yet to test on sphere and cylinder)
I tried going down the octomap road, but I could not see an obvious way of transcribing it in drake. Any thoughts?

I'll do the pre-commit stuff after any other changes you guys have in mind

sea-bass

Changes look good!

I think all I see is cleaning up some of the debug prints and the hard-coded names -- ensure the planning scene object names match those specified here? But maybe I am misunderstanding how the Drake scene graph works.

.docker/Dockerfile

sea-bass · 2024-08-12T22:43:17Z

src/ktopt_planning_context.cpp

      continue;
    }
+    RCLCPP_INFO(getLogger(), "iterating inside collision object's shapes");


Remove before merging

sea-bass · 2024-08-12T22:43:41Z

src/ktopt_planning_context.cpp

      const auto& pose = collision_object->shape_poses_[0];
+      const auto& shape_type = collision_object->shapes_[0]->type;
+      RCLCPP_INFO(getLogger(), "Shape type: %d, object id: %s, shape: %s", shape_type, object, shape);


Remove before merging, or make into debug? Might still be useful, long as it doesn't spam by default

sea-bass · 2024-08-12T22:44:22Z

src/ktopt_planning_context.cpp

+        case shapes::ShapeType::BOX: {
+
+          const auto objectptr = std::dynamic_pointer_cast<const shapes::Box>(shape);
+          RCLCPP_INFO(getLogger(), "Box, size: %f", objectptr->size[0]); // shape.size


Same as other comment. In general, should clean up what to print. So I'll stop commenting the same thing

sea-bass · 2024-08-12T22:46:00Z

src/ktopt_planning_context.cpp

+
+          const auto objectptr = std::dynamic_pointer_cast<const shapes::Box>(shape);
+          RCLCPP_INFO(getLogger(), "Box, size: %f", objectptr->size[0]); // shape.size
+          const SourceId box_source_id = scene_graph.RegisterSource("template_box");


The name should probably not be "template_SHAPE" but rather the actual entity name from the planning scene?

sea-bass · 2024-08-12T22:46:18Z

src/ktopt_planning_context.cpp

+          break;
+        }
+        case shapes::ShapeType::UNKNOWN_SHAPE: {
+          RCLCPP_INFO(getLogger(), "Unknown shape, ignoring in scene graph");


Maybe this should be a warning instead

sjahr

Make sure to run pre-commit run -a between the iterations on the code. It makes it also easier to read and review

sjahr · 2024-08-16T14:28:11Z

.docker/Dockerfile

@@ -26,6 +26,7 @@ RUN apt update && \
    apt install -y \
        build-essential \
        clang-format-14 \
+        clangd-12 \


Similar to Sebastian's comment: Why do we need this?

I added this for linting and LSP support, so that vim has access to the symbols for functionality like Goto definition/declaration etc.

sjahr · 2024-08-16T14:28:35Z

src/ktopt_planning_context.cpp

+  // NOTE: Information tree, to be deleted after implementation
+  // Planning scene -> World (getWorld, WorldConstPtr)
+  // objectIds -> World.getObjectIds()
+  // Object -> World.getObject(objectIds)


sjahr · 2024-08-16T14:29:02Z

src/ktopt_planning_context.cpp

@@ -248,29 +253,83 @@ void KTOptPlanningContext::transcribePlanningScene(const planning_scene::Plannin
    }
    if (object == OCTOMAP_NS)
    {
-      RCLCPP_ERROR(getLogger(), "skipping octomap for now ...");
+      RCLCPP_INFO(getLogger(), "skipping octomap for now ...");


Make this a warning and say explicitly: Octomap parsing not supported at the moment

sjahr · 2024-08-16T14:29:38Z

src/ktopt_planning_context.cpp

+              RigidTransformd(pose),
+              std::make_unique<Sphere>(
+                objectptr->radius), object));
+          RCLCPP_INFO(getLogger(), "Sphere");


kamiradi · 2024-08-18T10:29:46Z

@sea-bass @sjahr So I may have celebrated to soon,

There seems to be some thing I'm missing when it comes to reading the poses of the objects within planning scene into the scene_graph. Is there a transformation I'm missing between the robot and the world. As far as the scene_graph is concerned, I make the robot spawn at world origin. There could be an initial transformation that I am missing?

…rake into pr_scene_transcription

sea-bass · 2024-08-18T12:01:13Z

Is there a transformation I'm missing between the robot and the world. As far as the scene_graph is concerned, I make the robot spawn at world origin. There could be an initial transformation that I am missing?

You could check the TF widget in RViz to see if there is something amiss there? But as far as world-robot transform is concerned, this should anyways come from the URDF and I would be surprised if this was it.

As another thing to look form maybe the scene objects are expressed with some other transform relative to world? @sjahr would know this better.

sea-bass · 2024-08-18T12:13:23Z

Actually from the screenshot it just seems like no transforms at all are being applied to the boxes in the Drake scene graph. Everything is at the origin with identity transform, far as I can tell?

kamiradi · 2024-08-18T15:58:28Z

I think the poses of individual "boxes" (four in this case) w.r.t the collision object frame of reference may be correct. I think there is some global transformation that is missing here. If that makes sense?

kamiradi · 2024-08-18T16:00:01Z

Is there a transformation I'm missing between the robot and the world. As far as the scene_graph is concerned, I make the robot spawn at world origin. There could be an initial transformation that I am missing?

You could check the TF widget in RViz to see if there is something amiss there? But as far as world-robot transform is concerned, this should anyways come from the URDF and I would be surprised if this was it.

As another thing to look form maybe the scene objects are expressed with some other transform relative to world? @sjahr would know this better.

Yup I was thinking that it could be the second reason. Maybe somewhere in the planning scene there is object level pose as well?

sjahr · 2024-08-19T08:27:54Z

Is there a transformation I'm missing between the robot and the world. As far as the scene_graph is concerned, I make the robot spawn at world origin. There could be an initial transformation that I am missing?

You could check the TF widget in RViz to see if there is something amiss there? But as far as world-robot transform is concerned, this should anyways come from the URDF and I would be surprised if this was it.

As another thing to look form maybe the scene objects are expressed with some other transform relative to world? @sjahr would know this better.

Not sure about that because it works with the MoveIt planning scene, so I guess all transform should be known. Does the function you added fix the issue @kamiradi ?

kamiradi · 2024-08-19T09:13:11Z

The function I added, changes Moveit/ROS coordinate system to Drake. I thought this would, but it didn't. I'll try printing the positions and debugging.

kamiradi · 2024-08-19T15:54:26Z

All right good news, the pose object coming in from the planning scene itself has pose.x, y, z = 0.0. Can you point me towards where in moveit's planning scene the object markers are being set. Because in RViZ it visualises it correctly, so maybe I can use whichever pose to do the same here?

kamiradi · 2024-08-20T07:41:00Z

Ok, solved! There was indeed an object pose that I was missing.

sjahr

Nice, I'll test it before our meeting and I have some small clean-up comments and then this is good to go

sjahr · 2024-08-20T09:20:34Z

src/ktopt_planning_context.cpp

@@ -248,33 +249,113 @@ void KTOptPlanningContext::transcribePlanningScene(const planning_scene::Plannin
    }
    if (object == OCTOMAP_NS)
    {
-      RCLCPP_ERROR(getLogger(), "skipping octomap for now ...");
+      RCLCPP_INFO(getLogger(), "Octomap not supported for now ... ");


Suggested change

RCLCPP_INFO(getLogger(), "Octomap not supported for now ... ");

RCLCPP_WARN(getLogger(), "Octomap not supported for now ... ");

sjahr · 2024-08-20T09:24:39Z

src/ktopt_planning_context.cpp

+        {
+          RCLCPP_WARN(getLogger(), "Cone not supported in drake");
+          break;
+        }


Are these all possible shape types? If not, you should add a default like this at the end of the switch statement

default: { RCLCPP_WARN(getLogger(), "Shape TYPE conversation to drake is not implemented"); }

sjahr · 2024-08-20T09:25:06Z

src/ktopt_planning_context.cpp

    }
  }
 }

+RigidTransformd KTOptPlanningContext::convertToDrakeFrame(const Eigen::Affine3d& ros_pose)


Remove the whole function since it is not needed?

sea-bass · 2024-08-20T12:19:12Z

README.md

+Tips for when inside the container
+
+    # Can be added to the entrypoint
+    git config --global --add safe.directory /root/workspace/src/moveit_drake/


I think I already added this to the entrypoint!

sea-bass · 2024-08-20T12:20:07Z

src/ktopt_planning_context.cpp

@@ -160,7 +160,7 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
    visualizer_->ForcedPublish(vis_context);

    // Without these sleeps, the visualizer won't give you time to load your browser
-    std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(params_.trajectory_time_step * 1000.0)));
+    std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(params_.trajectory_time_step * 10000.0)));


Are you wanting to merge with this?

I had an idea which I could try -- basically have the motion planning terminate but starting the visualization separately in a thread so Meshcat doesn't die instantly, but also doesn't hold up the actual planning?

Not for this PR, but at least the delay should be reduced

sea-bass · 2024-08-20T12:20:34Z

src/ktopt_planning_context.cpp

@@ -228,6 +228,7 @@ void KTOptPlanningContext::setRobotDescription(std::string robot_description)

 void KTOptPlanningContext::transcribePlanningScene(const planning_scene::PlanningScene& planning_scene)
 {
+  // Transcribed the planning scene into a drake scene graph


Suggested change

// Transcribed the planning scene into a drake scene graph

// Transcribe the planning scene into a drake scene graph

sjahr

Let's merge once SebC's comments are addressed 😄

kamiradi added 4 commits August 6, 2024 12:11

[WIP] adds box case

0b10366

Merge branch 'moveit:main' into pr_scene_transcription

808ed9a

[WIP] generalised sphere and box

996476a

basic primitives added

9506420

sea-bass reviewed Aug 12, 2024

View reviewed changes

sjahr and others added 4 commits August 15, 2024 20:12

Merge branch 'main' into pr_scene_transcription

204b105

switch moveit_benchmark_resources from pr to main

1a24acf

tested on new planning scenes

ecd3376

Merge branch 'main' into pr_scene_transcription

a565092

sjahr reviewed Aug 16, 2024

View reviewed changes

kamiradi added 2 commits August 18, 2024 11:34

Moveit to drake transform

77d6f01

Merge branch 'pr_scene_transcription' of github.com:kamiradi/moveit_d…

34598ae

…rake into pr_scene_transcription

sjahr and others added 2 commits August 18, 2024 19:30

Format

59fa46b

review comment changes

f10ff7a

planning scene debug [WIP]

c891ed9

planning scene works!

c215249

kamiradi requested review from sjahr and sea-bass August 20, 2024 07:54

review comment changes

418a107

kamiradi marked this pull request as ready for review August 20, 2024 07:57

sjahr reviewed Aug 20, 2024

View reviewed changes

review comment changes

b4b02e8

sea-bass reviewed Aug 20, 2024

View reviewed changes

sjahr approved these changes Aug 20, 2024

View reviewed changes

review comment changes

eedf50d

sjahr merged commit c2ff3b1 into moveit:main Aug 20, 2024
2 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Planning Scene transcription V2 #31

Planning Scene transcription V2 #31

kamiradi commented Aug 6, 2024 •

edited

Loading

sea-bass left a comment

sea-bass Aug 12, 2024

sea-bass Aug 12, 2024

sea-bass Aug 12, 2024

sea-bass Aug 12, 2024

sea-bass Aug 12, 2024

sjahr left a comment

sjahr Aug 16, 2024

kamiradi Aug 17, 2024

sjahr Aug 16, 2024

sjahr Aug 16, 2024

sjahr Aug 16, 2024

kamiradi commented Aug 18, 2024 •

edited

Loading

sea-bass commented Aug 18, 2024

sea-bass commented Aug 18, 2024

kamiradi commented Aug 18, 2024

kamiradi commented Aug 18, 2024

sjahr commented Aug 19, 2024

kamiradi commented Aug 19, 2024

kamiradi commented Aug 19, 2024

kamiradi commented Aug 20, 2024 •

edited

Loading

sjahr left a comment

sjahr Aug 20, 2024

sjahr Aug 20, 2024

sjahr Aug 20, 2024

sea-bass Aug 20, 2024

sea-bass Aug 20, 2024

sea-bass Aug 20, 2024

sjahr left a comment

	RCLCPP_INFO(getLogger(), "Octomap not supported for now ... ");
	RCLCPP_WARN(getLogger(), "Octomap not supported for now ... ");

	// Transcribed the planning scene into a drake scene graph
	// Transcribe the planning scene into a drake scene graph

Planning Scene transcription V2 #31

Planning Scene transcription V2 #31

Conversation

kamiradi commented Aug 6, 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

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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kamiradi commented Aug 18, 2024 • edited Loading

sea-bass commented Aug 18, 2024

sea-bass commented Aug 18, 2024

kamiradi commented Aug 18, 2024

kamiradi commented Aug 18, 2024

sjahr commented Aug 19, 2024

kamiradi commented Aug 19, 2024

kamiradi commented Aug 19, 2024

kamiradi commented Aug 20, 2024 • edited Loading

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

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

kamiradi commented Aug 6, 2024 •

edited

Loading

kamiradi commented Aug 18, 2024 •

edited

Loading

kamiradi commented Aug 20, 2024 •

edited

Loading