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

[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.

[davetcoleman]

it passes!! thanks for your help @harrison-bcr