Add lab desk objects, AprilTag bottle picking, and clean up lab_sim

[davetcoleman]

Summary

• Add realistic lab environment objects to MuJoCo scene: desk lamp, laptop with screen/keyboard textures, potted plant, alligator trophy statue on shelf, test tube rack with 6 colored tubes, and 4 manipulable medicine bottles
• Adds new AprilTag based pick and place example that is more comprehensive: moves items into specific positions on a tray. This is a major part of the new tutorial learning material

Dependencies

• Requires picknik_accessories#feature/lab-sim-desk-objects for test tube stand repositioning. Make sure you run the submodule update command otherwise you'll get qpos errors

Note: I would love to get rid of picknik_accessories repo for good, merging it into this repo. With the advent of AI, we dont need to maintain a reusable set of MuJoCo assets for our customers. The vision was to make this into a sharable library I think. In a future PR, I think we should get rid of this repo because it causes a lot of problems trying to keep changes to moveit_pro, example_ws, and picknik_accessories in sync.

[MikeWrock]

This is failing CI for both integration testing and pre-commit. Please confirm the objectives in lab_sim still run successfully and that the new assets don't introduce any unexpected behaviors (ungraspable objects due to collisions, collisions with new objects due to waypoints, confusing input to ML prompts, poor friction coefficient for new objects). As our flagship config, I wouldn't want to introduce a whole bunch of unpickable objects or add complexity that would make the product look poorly functional.

Please also monitor the change in CPU usage, there are a lot of additional free joints that would significantly increase the CPU load and I want to make sure it's not a huge jump. I noticed some of the free joints (like the alligator) are not even reachable, and would be better as fixed joints.

When this PR is tested and ready for review, please add a story to track your work on the project board

[harrison-bcr]

Passing Objectives (per @harrison-bcr's test)

Application - Advanced Examples

• Grasp Pose Tuning With April Tag
• Grasp Pose Using Yaml
• Pick And Place Example
• Pick April Tag Labeled Object With Approval
• Record and Replay Scanning Motion
• Record Square Trajectory
• Register CAD Part
• Visualize grasp_link
• [FLAKEY] Stationary Admittance
  • FT limit exceeds immediately with interactive viewer

Application - Basic Examples

• Cycle Between Waypoints
• Joint Diagnostic
• Load and Execute Joint Trajectory
• Look at Table
• Pick up Object
• Place Object
• Plan and Save Trajectory
• Playback Square Trajectory
• Stitch Multiple Point Clouds Together
• Take Scene Camera Snapshot
• Take Wrist Camera Snapshot

Application - ML (GPU Recommended)

• ML Auto Grasp Object from Clicked Point
• [FAIL] ML Find Bottles on Table from Image Exemplar
  • Objective Server Error
    Failed to create behavior tree for Objective ML Find Bottles on Table from Image Exemplar. Reason: BehaviorTree error while creating tree: Missing manifest for element_ID: CreateBoundingBoxFromOffset. It shouldn't happen. Please report this issue.
• [FAIL] ML Find Objects on Table
  • Objective Server Error
    Failed to create behavior tree for Objective ML Find Objects on Table. Reason: BehaviorTree error while creating tree: Missing manifest for element_ID: GetMasks2DFromExemplar. It shouldn't happen. Please report this issue.
• [FAIL] ML Segment Bottles from File
  • Objective Server Error
    Failed to create behavior tree for Objective ML Segment Bottles from File. Reason: BehaviorTree error while creating tree: Missing manifest for element_ID: GetMasks2DFromExemplar. It shouldn't happen. Please report this issue.
• ML Segment Image
• ML Segment Point Cloud
• ML Segment Point Cloud from Clicked Point
• Pick 1 Pill Bottle with ML
  • It succeeds. But it segments the flasks as the cylindrical object.
• MPC Pose Tracking
• MPC Pose Tracking Dynamic Point Cloud Avoidance with Sphere Down Sample
• MPC Pose Tracking Static Point Cloud Avoidance with Sphere Down Sample
• MPC Pose Tracking With Point Cloud Avoidance
• MPC Pose Tracking With Static Sphere Point Cloud Avoidance

Grasping

• Close Gripper
• Open Gripper
  Motion - Execute
• Push Button With a Trajectory
  Motion - Planning
• Grasp Planning

MuJoCo Simulation

• Load MuJoCo Robot State
• Reset MuJoCo Robot
• Reset MuJoCo Sim
• Save Robot Joint State

Perception - 3D Point Cloud

• Clear Snapshot
• Octomap Example
• Add Table to Planning Scene
• AddBottlesToPlanningScene
  • Succeeds but throws tf errors
• Reset Planning Scene

Perception - Scene Constraints

• Clear Previous Obstacles

Training Examples

• Stack Blocks with ICP
  • It works without issues, but the bottle when placed down tilts over. The bottle slips when gripped by the robot and
    moved
• Teleoperate

Vector Handling

• CreateVector
• Vector and String Example
• Vector Subtrees Example

Visualization

• Marker Visualization Example

[davetcoleman]

[FAIL] Register CAD Part

Can you replace the boring square (which is barely a "CAD Part") with one of the other objects on the table? Perhaps the microscope on the left? Or something else that is more interesting than a cube, please?

[FAIL] Classical Pick and Place

How about you just delete this objective? we have plenty of other P&P examples now

[FAIL] Stack Blocks with ICP

How about we delete this one too?

The other FAILs - do you see a path to fix them?

Thanks for working on this!

[harrison-bcr]

[FAIL] Register CAD Part

Can you replace the boring square (which is barely a "CAD Part") with one of the other objects on the table? Perhaps the microscope on the left? Or something else that is more interesting than a cube, please?

[FAIL] Classical Pick and Place

How about you just delete this objective? we have plenty of other P&P examples now

[FAIL] Stack Blocks with ICP

How about we delete this one too?

The other FAILs - do you see a path to fix them?

Thanks for working on this!

Hi @davetcoleman . Saw this comment after I put some fixes.

• I changed Register CAD Part to one of the cylinders on the table. I can change it to the microscope.
• I will delete Classical Pick and Place in the next commit
• I fixed Stack Blocks with ICP. But if it is still required to delete it, I can do that

For the remaining FAILS, I am still going through each failed ones and trying to have a fix for it.

[davetcoleman]

I changed Register CAD Part to one of the cylinders on the table. I can change it to the microscope.

Yes I think it would be much more interesting to change it

I will delete Classical Pick and Place in the next commit

Great

I fixed Stack Blocks with ICP. But if it is still required to delete it, I can do that

I like keeping it if you can, but should we remove "Blocks" from the name? What is it stacking? I really want to get rid of kids blocks from the examples

[nbbrooks]

@harrison-bcr how is this coming along? Are you blocked by anything?

[davetcoleman]

@harrison-bcr be sure to pull in latest changes, such as this one: #549 (comment)

[harrison-bcr]

@harrison-bcr how is this coming along? Are you blocked by anything?

Hey @nbbrooks , I just finished updating this tracker.

• Stationary Admittance behavior error is coming same as mentioned before. Somehow when the mujoco viewer is on, the force values exceed the limits.
• 3 ML based behaviors has this issue I am not familiar with (Check tracker for issue log). I will need some help and guidance for the ML based behaviors.
• MPC Pose Tracking With Static Sphere Point Cloud Avoidance scan and merge point clouds somehow fails. Yet to find the reason. I should be able to solve this with some more debugging.

[harrison-bcr]

@harrison-bcr be sure to pull in latest changes, such as this one: #549 (comment)

Just to be sure @davetcoleman, does this mean I should pull rebase changes from v9.1 into this branch ?

[davetcoleman]

Just to be sure @davetcoleman, does this mean I should pull rebase changes from v9.1 into this branch ?

Yea I think that makes sense

What if we merge this and move these 3 issues as separate bug issues?

Stationary Admittance behavior error is coming same as mentioned before. Somehow when the mujoco viewer is on, the force values exceed the limits.

I dont know what this is

3 ML based behaviors has this issue I am not familiar with (Check tracker for issue log). I will need some help and guidance for the ML based behaviors.

If these are the old ML things, these have never really worked in the tutorials - they dont put bounding boxes correctly on anything. I think we should just delete them since @griswaldbrooks has much better new ones merged in. Good issue for a new thread.

MPC Pose Tracking With Static Sphere Point Cloud Avoidance scan and merge point clouds somehow fails. Yet to find the reason. I should be able to solve this with some more debugging.

I don't think my changes would have affected anything here, but good thing to discuss with @bkanator in a new thread

[JWhitleyWork]

Rebased on main.

[davetcoleman]

@harrison-bcr I made some additional tweaks based on the CI feedback.

The remaining issues that I have not addressed, according to CI:

2. Teleoperate — Pre-existing / infrastructure

  - Error: DoTeleoperateAction Error: Action server not available
  - This is a core objective from moveit_pro_objectives (not lab_sim). Teleop requires a UI action server that doesn't exist in CI. Likely should
  be in the skip_objectives list, or is a known flaky test.
  - Not caused by this PR.

  3. Marker Visualization Example — Pre-existing / infrastructure

  - Error: Server not available for service /request_text_from_prompts
  - Another core objective that requires a UI service not present in CI.
  - Not caused by this PR.

  4. MPC Pose Tracking With Static Sphere Point Cloud Avoidance — Likely pre-existing / flaky

  - Error: No points left in the point cloud after removing NaN points
  - The simulated camera returned an empty/NaN point cloud. This is a simulation timing/environment issue, not a code bug.
  - Not caused by this PR.

  5. Playback Square Trajectory — Likely pre-existing / flaky

  - Error: Controller aborted sub-trajectory 0: Path tolerance violated
  - The robot couldn't follow the recorded trajectory closely enough. This is a common flaky test in simulation.
  - Not caused by this PR.

I think some of those are just flakey and would pass if we re-run CI. Others I think might be broken on main. Can you test main (without this PR) and see if they work there? Any progress on these?

[harrison-bcr]

@harrison-bcr I made some additional tweaks based on the CI feedback.

The remaining issues that I have not addressed, according to CI:

2. Teleoperate — Pre-existing / infrastructure

  - Error: DoTeleoperateAction Error: Action server not available
  - This is a core objective from moveit_pro_objectives (not lab_sim). Teleop requires a UI action server that doesn't exist in CI. Likely should
  be in the skip_objectives list, or is a known flaky test.
  - Not caused by this PR.

  3. Marker Visualization Example — Pre-existing / infrastructure

  - Error: Server not available for service /request_text_from_prompts
  - Another core objective that requires a UI service not present in CI.
  - Not caused by this PR.

  4. MPC Pose Tracking With Static Sphere Point Cloud Avoidance — Likely pre-existing / flaky

  - Error: No points left in the point cloud after removing NaN points
  - The simulated camera returned an empty/NaN point cloud. This is a simulation timing/environment issue, not a code bug.
  - Not caused by this PR.

  5. Playback Square Trajectory — Likely pre-existing / flaky

  - Error: Controller aborted sub-trajectory 0: Path tolerance violated
  - The robot couldn't follow the recorded trajectory closely enough. This is a common flaky test in simulation.
  - Not caused by this PR.

I think some of those are just flakey and would pass if we re-run CI. Others I think might be broken on main. Can you test main (without this PR) and see if they work there? Any progress on these?

@davetcoleman I ran these behaviors in main branch and they all seem to work without any issues.

The reason why Playback Square Trajectory is throwing an issue in this branch is because the gripper is hitting the shelf. I had updated the trajectory path to avoid colliding, but somehow it must have merged back from main.
Let me know if I should add this fix back in.

The issue with MPC Pose Tracking With Static Sphere Point Cloud Avoidance is something I still dont understand. The motion and method of capturing and merging point clouds are same in both old desk setup and new desk setup. But somehow in main it works but in this branch it returns zero points.

[harrison-bcr]

On an unrelated note, the package lab_sim_behaviors has build failures. I have been adding COLCON_IGNORE while testing as it doesnt interfere with any of the current tests. Below mentioned is the build output:

--- stderr: lab_sim_behaviors                                 
In file included from /home/harrison/user_ws/src/lab_sim_behaviors/src/compute_tray_place_positions_using_apriltags.cpp:13:
/home/harrison/user_ws/src/lab_sim_behaviors/include/lab_sim_behaviors/compute_tray_place_positions_using_apriltags.hpp:10:10: fatal error: moveit_pro_behavior/behaviors/visualization/ros_publisher_handle.hpp: No such file or directory
   10 | #include <moveit_pro_behavior/behaviors/visualization/ros_publisher_handle.hpp>
      |          ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.
gmake[2]: *** [CMakeFiles/lab_sim_behaviors.dir/build.make:76: CMakeFiles/lab_sim_behaviors.dir/src/compute_tray_place_positions_using_apriltags.cpp.o] Error 1
gmake[2]: *** Waiting for unfinished jobs....
In file included from /home/harrison/user_ws/src/lab_sim_behaviors/src/register_behaviors.cpp:11:
/home/harrison/user_ws/src/lab_sim_behaviors/include/lab_sim_behaviors/compute_tray_place_positions_using_apriltags.hpp:10:10: fatal error: moveit_pro_behavior/behaviors/visualization/ros_publisher_handle.hpp: No such file or directory
   10 | #include <moveit_pro_behavior/behaviors/visualization/ros_publisher_handle.hpp>
      |          ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.

[nbbrooks]

You need a version of 9.1 for those behaviors to build. Give this release candidate a go

curl -Lo /tmp/moveit-pro-9.1.0-rc15-any.deb https://github.com/PickNikRobotics/moveit_pro_deb/raw/main/picknik/moveit-pro-9.1.0-rc15-any.deb
sudo apt install -y /tmp/moveit-pro-9.1.0-rc15-any.deb
moveit_pro run

[davetcoleman]

Let me know if I should add this fix back in.

Yes please!

The issue with MPC Pose Tracking With Static Sphere Point Cloud Avoidance is something I still dont understand. The motion and method of capturing and merging point clouds are same in both old desk setup and new desk setup. But somehow in main it works but in this branch it returns zero points.

@bkanator do you have any thoughts here? You built this right?

[davetcoleman]

@harrison-bcr i tested the remaining issues just now and pushed 2 fixes. i squashed the commits together so we could then have a clean rebase on main, to fix the other issues. hopefully it works this time!

[harrison-bcr]

@harrison-bcr i tested the remaining issues just now and pushed 2 fixes. i squashed the commits together so we could then have a clean rebase on main, to fix the other issues. hopefully it works this time!

@davetcoleman These behaviours are all working now.

2. Teleoperate — Pre-existing / infrastructure

  - Error: DoTeleoperateAction Error: Action server not available
  - This is a core objective from moveit_pro_objectives (not lab_sim). Teleop requires a UI action server that doesn't exist in CI. Likely should
  be in the skip_objectives list, or is a known flaky test.
  - Not caused by this PR.

  3. Marker Visualization Example — Pre-existing / infrastructure

  - Error: Server not available for service /request_text_from_prompts
  - Another core objective that requires a UI service not present in CI.
  - Not caused by this PR.

  4. MPC Pose Tracking With Static Sphere Point Cloud Avoidance — Likely pre-existing / flaky

  - Error: No points left in the point cloud after removing NaN points
  - The simulated camera returned an empty/NaN point cloud. This is a simulation timing/environment issue, not a code bug.
  - Not caused by this PR.

  5. Playback Square Trajectory — Likely pre-existing / flaky

  - Error: Controller aborted sub-trajectory 0: Path tolerance violated
  - The robot couldn't follow the recorded trajectory closely enough. This is a common flaky test in simulation.
  - Not caused by this PR.

[bkanator]

Let me know if I should add this fix back in.

Yes please!

The issue with MPC Pose Tracking With Static Sphere Point Cloud Avoidance is something I still dont understand. The motion and method of capturing and merging point clouds are same in both old desk setup and new desk setup. But somehow in main it works but in this branch it returns zero points.

@bkanator do you have any thoughts here? You built this right?

Yes, I did! that's me! I don't think it has ever 'flaked', but looks like you fixed it with the wrist camera ? and perhaps the other was not giving point clouds with actual numbers. Ping me if you still need me to debug and I can look at this branch. What claude had suggested in that little blurb (sim timing flake) was inaccurate so glad you found it!

[davetcoleman]

Three tests failed in CI for PR #512:

1. Teleoperate — flakey
2. Interactive Marker Visualization Example — flakey
3. Register CAD Part — timed out waiting for /wrist_camera/points (5s default) because MuJoCo wasn't running in realtime on the CI machine. Passed on retry. Just pushed a fix bumping the timeout to 10s.

[rlpratt12]

Removing blocking review since PR was split up.

Removing blocking review since PR was split up.