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

TODO

• I had removed all the kids' blocks from the mujoco world, but when I was asked to split the PR some of those changes were lost. Please re-remove the blocks, if necessary, and rename waypoints, objectives, etc to no longer use the "blocks" concept. Some of the Objectives that really depend on blocks can just be deleted
• I moved the entire table forward, closer to the robot, to improve reachability. I think I updated nearly all the waypoints, but some may still be invalid
• With the table moved, some objectives might get into collision with the back shelf of the table. In some of the objectives I programmatically added a collision object to represent that back shelf, you might need to do more of that.
• In general, test all the objectives per Wrock's list below

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.

Passing Objectives (per @MikeWrock's test)

Application - Advanced Examples

• [FAIL] Grasp Pose Tuning With April Tag
  • Adjust TransformPose Z 0.15-0.25
• [FAIL] Grasp Pose Using Yaml
  • Adjust TransformPose Z 0.15-0.25
  • Loaded offset was incorrectly applied
• [FAIL] Pick And Place Example
  • Does not pick/place, target object is missing
• [FAIL] Pick April Tag Labeled Object With Approval
  • Adjust TransformPose Z 0.15-0.25
  • Tool path plans beyond pariltag reach point. I wonder if the size of the apriltag is wrong and it's messing with the depth perception?
• Record and Replay Scanning Motion
• Record Square Trajectory
• [FAIL] Register CAD Part
  • No cube
• Show grasp_link
• [FLAKEY] Stationary Admittance
  • FT limit exceeds immediately with interactive viewer
    Application - Basic Examples
• [FAIL] Classical Pick and Place
  • It failed to pick or place the test tubes
  • The path planned looked entirely incorrect (no cartesian approach, horizontal motion at the end)
• Cycle Between Waypoints
• Joint Diagnostic
• Load and Execute Joint Trajectory
• Look at Table
• [FAIL] Pick up Cube
  • No cube
• [FAIL] Place Object
  • Collides with shelf
• Plan and Save Trajectory
• [FAIL] Playback Square Trajectory
  • Collides with shelf
• [FAIL] Stitch Multiple Point Clouds Together
  • Collides with coloured test tubes
• [FAIL] Take Scene Camera Snapshot
  • Scene camera points topic is not published (wrist camera points, and scene camera/color is)
• Take Wrist Camera Snapshot
  Application - ML (GPU Recommended)
• ML Auto Grasp Object from Clicked Point
• ML Find Bottles on Table from Image Exemplar
• ML Find Objects on Table
• ML Segment Bottles from File
• ML Segment Image
• ML Segment Point Cloud
• ML Segment Point Cloud from Clicked Point
• Pick 1 Pill Bottle with ML
  Application - MPC Examples
• [FAIL] MPC Pose Tracking
  • Missing waypoint
• [FAIL] MPC Pose Tracking Dynamic Point Cloud Avoidance with Sphere Down Sample
  • Missing waypoint
  • pose_stamped->stamped_pose convention change
• [FAIL] MPC Pose Tracking Static Point Cloud Avoidance with Sphere Down Sample
  • pose_stamped->stamped_pose convention change
• MPC Pose Tracking With Point Cloud Avoidance
• [FAIL] MPC Pose Tracking With Static Sphere Point Cloud Avoidance
  • No scene camera points topic
    Grasping
• Close Gripper
• Open Gripper
  Motion - Execute
• [FAIL] Push Button With a Trajectory
  • Trajectory does not align with button
    Motion - Planning
• [FAIL] Grasp Planning
  • Passes but does not plan to a useful grasp point
    MuJoCo Simulation
• Load MuJoCo Robot State
• Reset MuJoCo Robot
• Reset MuJoCo Sim
• Save Robot Joint State
  Perception - 3D Point Cloud
• Clear Snapshot
• [FAIL] Octomap Example
  • Scene camera points topic is not published (wrist camera is)
    Perception - Planning Scene
• Add Table to Planning Scene
• AddBottlesToPlanningScene
  • Succeeds but throws tf errors
• Reset Planning Scene
  Perception - Scene Constraints
• Clear Previous Obstacles
  Training Examples
• [FAIL] Stack Blocks with ICP
  • Blocks no longer present, need replacement ICP STL and example
    User Input
• Teleoperate
  Vector Handling
• CreateVector
• Vector and String Example
• Vector Subtrees Example
  Visualization
• Marker Visualization Example

[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

[rlpratt12]

This includes custom behaviors, mujoco scene changes, and modified/new objectives. Please break these changes in to multiple PRs per our best practices.

[davetcoleman]

@rlpratt12 as stated in the comment above (#512 (comment)) and in the PR desc, I have this effort broken into 4 PRs already, two of which are in this combined PR temporarily. I can break it into more, but I first need the earlier PRs merged in. Managing 4 PRs across 3 repos is a huge pain, and you are unfairly characterizing my efforts here.

[davetcoleman]

I've had claude split this out into more PRs, that are all straight forward and ready to merge asap:

• Inline colored point cloud subtrees into register_cad_part #527
• Clean up and improve lab_sim objectives #526
• Add CLAUDE.md and ignore MUJOCO_LOG.TXT #525

This final PR I've marked as draft until those other ones are merged.

[JWhitleyWork]

Rebased to pick up CI fix.

[davetcoleman]

@nbbrooks merged the last blocker PR (of several) to this late yesterday, so can we merge this now?

[nbbrooks]

@rlpratt12 can you remove your "requested changes" block?

@davetcoleman am I missing a countertop texture? or is that a followup pr?

[nbbrooks]

My laptop is having performance issues and I'm seeing a 50% grasp success rate.

simplescreenrecorder-2026-03-13_08.44.45.mp4

This could be any number of things

• Increased cpu load from additional free joints
• New bottles need friction tuning
• Robotiq grpper always needed better tuning

I am concerned people in training will run into this, but I don't have a good grasp on the success rate for the current set of objectives. I suspect the EGL rendering will vastly improve this by taking off CPU load.

[MikeWrock]

I wasn't able to get ML working on my GPU so I haven't tested the ML objectives, but I've updated the PR summary to include a list of objectives and whether they still run successfully or not. I've made some easy fixes, but some of the objectives require larger efforts to fix (new ICP/registration examples, update/remove/replace hardcoded pick and place objectives, shift pose-based paths so they dont collide with new desk location). My latest changes are pushed, but I'd like someone else to confirm the failing objectives are now passing for them as well

[davetcoleman]

@davetcoleman am I missing a countertop texture? or is that a followup pr?

This is the texture Claude gave me for the countertop, and I was actually happy with it. You're hoping for a non-checkerboard black pattern? How about we leave that for future work.

My laptop is having performance issues and I'm seeing a 50% grasp success rate.

This is why I had added ForceSuccess, and I had tuned the gripper config further, and I had changed the close amount to be less than the full close distance. But those changes were asked to be reverted through the review process thus far.

I've made some easy fixes, but some of the objectives require larger efforts to fix

I don't see your fixes in this PR's force push history, are you sure you pushed them @MikeWrock ?

[nbbrooks]

This is why I had added ForceSuccess

Let's separate out physical grasping success concerns from MoveGripperAction behavior success/failure concerns. I was talking about physical grasping success - if it actually was able to lift the bottle up.