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

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

[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
• [FAIL] MPC Pose Tracking With Static Sphere Point Cloud Avoidance
  • Scan scene returns failure always with this error, "SendPointCloudToUI Error: No points left in the point cloud after removing NaN points."

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.