From e6b9b2f2bdc43bec95e8b85db919b018cd4e33c3 Mon Sep 17 00:00:00 2001
From: Griswald Brooks <griswald.brooks@gmail.com>
Date: Tue, 5 May 2026 17:23:06 -0400
Subject: [PATCH] fix(factory_sim): 18180 Pick and Place Brackets sim setup and
 tool-handling cartesian fixes

Addresses two distinct bugs surfaced in the factory_sim package by
PickNikRobotics/moveit_pro#18180.

1. Bracket spawn-position errors at sim reset.
   <replicate count=3> in description/scene.xml interacts badly with
   MuJoCo 3.2.7's keyframe parser, silently corrupting the first
   replica's qpos slot -- bracket-0-0 lands under the pedestal on every
   reset. MuJoCo 3.3.5 hard-errors on the same input
   ("Keyframe 'default' has invalid qpos size, got 48, should be 13").
   Fix replaces <replicate> with three explicit <include> blocks of
   per-bracket geometry files.

2. Tool-handling cartesian rejections at the holder.
   Two cartesian motions in the suction-gripper subtrees deterministi-
   cally land link_6 at the suction_gripper world object (depth = 0):
   - Pick Up Tool from Holder's +0.10m approach push.
   - Place Tool in Tool Holder's 0.35m post-detach Retract.
   The pair link_6 <-> suction_gripper has no ACM entry (default
   NEVER), so the planner rejects both segments. Previously masked by
   <replicate>'s non-deterministic IK ordering. Fix opts each segment
   out of environment-collision checks; the cartesian paths are purely
   vertical along tool0 Z, so wrist swings into other env are
   precluded by the path itself.

Plus three small simulator-side fixes folded in (keyframe qpos row
order corrected to match MuJoCo's body compile order; default joint
path tolerance bumped to 0.65 to clear joint-6 deviation against the
0.6 limit; MuJoCo arena bumped from 15M to 64M to prevent contact-
constraint overflow during the place phase, which had been segfaulting
ros2_control_node via the viewer's next sync).

Edits:

- description/scene.xml -- keyframe qpos rows reordered; <replicate>
  block replaced with three explicit <include>s; arena bumped to 64M.
- description/bracket_collision_geometry_{0,1,2}.xml -- new per-bracket
  geometry files referenced by the includes.
- config/control/picknik_fanuc.ros2_control.yaml -- default_path_-
  tolerance bumped to 0.65.
- objectives/pick_up_tool_from_holder.xml -- ignore_environment_-
  collisions="true" on the +0.10m SetupMTCMoveAlongFrameAxis;
  SetupMTCIgnoreCollisionsBetweenObjects(link_5;link_6;suction_-
  gripper) added matching the sibling subtree pattern.
- objectives/retract.xml -- parameterized ignore_environment_-
  collisions as a new input port with default "false" so existing call
  sites keep their hardcoded behavior; threaded through to the
  underlying SetupMTCMoveAlongFrameAxis.
- objectives/place_tool_in_tool_holder.xml -- passes ignore_-
  environment_collisions="true" at its 0.35m Retract call site so the
  post-detach retreat clears the same depth = 0 contact.

Refs PickNikRobotics/moveit_pro#18180

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
---
 .../control/picknik_fanuc.ros2_control.yaml   |   2 +-
 .../bracket_collision_geometry_0.xml          | 553 ++++++++++++++++++
 .../bracket_collision_geometry_1.xml          | 553 ++++++++++++++++++
 .../bracket_collision_geometry_2.xml          | 553 ++++++++++++++++++
 src/factory_sim/description/scene.xml         |  28 +-
 .../objectives/pick_up_tool_from_holder.xml   |   8 +-
 .../objectives/place_tool_in_tool_holder.xml  |   7 +-
 src/factory_sim/objectives/retract.xml        |   3 +-
 8 files changed, 1692 insertions(+), 15 deletions(-)
 create mode 100644 src/factory_sim/description/bracket_collision_geometry_0.xml
 create mode 100644 src/factory_sim/description/bracket_collision_geometry_1.xml
 create mode 100644 src/factory_sim/description/bracket_collision_geometry_2.xml

diff --git a/src/factory_sim/config/control/picknik_fanuc.ros2_control.yaml b/src/factory_sim/config/control/picknik_fanuc.ros2_control.yaml
index d6d3a08cf..ae9a63c13 100644
--- a/src/factory_sim/config/control/picknik_fanuc.ros2_control.yaml
+++ b/src/factory_sim/config/control/picknik_fanuc.ros2_control.yaml
@@ -81,7 +81,7 @@ joint_trajectory_admittance_controller:
     # provide the gravity vector in the base frame.
     gravity_vector: [0.0, 0.0, -9.8]
     # Default maximum joint-space deviation accepted along the trajectory, if not specified in the goal message.
-    default_path_tolerance: 0.6
+    default_path_tolerance: 0.65
     # Default maximum joint-space deviation accepted at the trajectory end-point, if not specified in the goal message.
     default_goal_tolerance: 0.05
     # Default maximum absolute force torque readings allowed from the force torque sensor along each Cartesian space
diff --git a/src/factory_sim/description/bracket_collision_geometry_0.xml b/src/factory_sim/description/bracket_collision_geometry_0.xml
new file mode 100644
index 000000000..1809bfc36
--- /dev/null
+++ b/src/factory_sim/description/bracket_collision_geometry_0.xml
@@ -0,0 +1,553 @@
+<mujoco model="bracket_collision_geometry_0">
+  <!-- Using these properties to prevent trampoline behavior 
+  when the bracket lands flat on table or flat on another bracket part -->
+  <!-- condim="6" -->
+  <!-- friction="2.0 0.1 0.1" -->
+  <!-- margin="0.005" -->
+  <!-- solref="0.02 2.0" -->
+  <!-- solimp="0.95 0.999 0.01 0.5 2" -->
+  <!-- -->
+  <!-- damping="0.0001" -->
+  <!-- which were experimentally checked. There may be a better way to do it. -->
+
+  <!-- 5 Box collision primitives -->
+  <!-- starting from the left (pointy end) -->
+  <!-- top cheek -->
+  <body name="bracket-0-0" pos="-0.42 0.5 0.35">
+    <joint type="free" damping="0.0001" />
+    <geom
+      type="box"
+      size="0.0530875 0.025 0.00762"
+      pos="0.072240 -0.045944 0.00762"
+      euler="0 0 0.4593"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- bottom cheek -->
+    <geom
+      type="box"
+      size="0.0422435 0.0218295 0.00762"
+      pos="0.054467 -0.100271 0.00762"
+      euler="0 0 -0.7275"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- middle upper box -->
+    <geom
+      type="box"
+      size="0.01565 0.0145 0.00762"
+      pos="0.13365 -0.055718 0.00762"
+      euler="0 0 0"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- middle lower box-->
+    <geom
+      type="box"
+      size="0.024469 0.010 0.00762"
+      pos="0.108831 -0.085 0.00762"
+      euler="0 0 0"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- middle rear box -->
+    <geom
+      type="box"
+      size="0.017113 0.029 0.00762"
+      pos="0.166413 -0.070218 0.00762"
+      euler="0 0 0"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- top butt -->
+    <geom
+      type="box"
+      size="0.027879 0.025 0.00762"
+      pos="0.136622 -0.025 0.00762"
+      euler="0 0 0"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- bottom butt -->
+    <geom
+      type="box"
+      size="0.048879 0.025 0.00762"
+      pos="0.120751 -0.120 0.00762"
+      euler="0 0 0"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+
+    <!-- 3 Cylinder collision primitives -->
+    <!-- size="radius half_length" and default axis along Z -->
+    <!-- pointy side -->
+    <geom
+      type="cylinder"
+      size="0.025 0.00762"
+      pos="0.024654 -0.069478 0.00762"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- butts side -->
+    <geom
+      type="cylinder"
+      size="0.025 0.00762"
+      pos="0.1645 -0.025 0.00762"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="cylinder"
+      size="0.025 0.00762"
+      pos="0.16963 -0.120 0.00762"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+
+    <!-- Holes in part used for placement, approximated by boxes -->
+    <!-- 10 boxes centered at C=(0.076362, -0.079826, 0)-->
+    <geom
+      type="box"
+      pos="0.076362 -0.066326 0.00762"
+      euler="0 0 0.00"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.068518 -0.068839 0.00762"
+      euler="0 0 0.62"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.063594 -0.075441 0.00762"
+      euler="0 0 1.24"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.063423 -0.083676 0.00762"
+      euler="0 0 1.86"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.068068 -0.090478 0.00762"
+      euler="0 0 2.48"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.075801 -0.093314 0.00762"
+      euler="0 0 3.10"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.083742 -0.091130 0.00762"
+      euler="0 0 3.72"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.088937 -0.084738 0.00762"
+      euler="0 0 4.34"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.089450 -0.076517 0.00762"
+      euler="0 0 4.96"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.085092 -0.069528 0.00762"
+      euler="0 0 5.58"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+
+    <!-- 10 boxes centered at C=(0.110, -0.067, 0) -->
+    <geom
+      type="box"
+      pos="0.110000 -0.053500 0.00762"
+      euler="0 0 0.00"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.102156 -0.056013 0.00762"
+      euler="0 0 0.62"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.097232 -0.062615 0.00762"
+      euler="0 0 1.24"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.097061 -0.070850 0.00762"
+      euler="0 0 1.86"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.101706 -0.077652 0.00762"
+      euler="0 0 2.48"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.109439 -0.080488 0.00762"
+      euler="0 0 3.10"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.117380 -0.078304 0.00762"
+      euler="0 0 3.72"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.122575 -0.071912 0.00762"
+      euler="0 0 4.34"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.123088 -0.063691 0.00762"
+      euler="0 0 4.96"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.118730 -0.056702 0.00762"
+      euler="0 0 5.58"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+
+    <!-- 10 boxes centered at C=(0.1413, -0.082661, 0) -->
+    <geom
+      type="box"
+      pos="0.141300 -0.069161 0.00762"
+      euler="0 0 0.00"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.133456 -0.071674 0.00762"
+      euler="0 0 0.62"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.128532 -0.078276 0.00762"
+      euler="0 0 1.24"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.128361 -0.086511 0.00762"
+      euler="0 0 1.86"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.133006 -0.093313 0.00762"
+      euler="0 0 2.48"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.140739 -0.096149 0.00762"
+      euler="0 0 3.10"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.148680 -0.093965 0.00762"
+      euler="0 0 3.72"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.153875 -0.087573 0.00762"
+      euler="0 0 4.34"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.154388 -0.079352 0.00762"
+      euler="0 0 4.96"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.150030 -0.072363 0.00762"
+      euler="0 0 5.58"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+
+    <geom
+      contype="0"
+      conaffinity="0"
+      type="mesh"
+      mesh="bracket"
+      material="sheet_metal"
+    />
+    <site name="bracket_part_pose-0-0" />
+  </body>
+</mujoco>
diff --git a/src/factory_sim/description/bracket_collision_geometry_1.xml b/src/factory_sim/description/bracket_collision_geometry_1.xml
new file mode 100644
index 000000000..e85d60022
--- /dev/null
+++ b/src/factory_sim/description/bracket_collision_geometry_1.xml
@@ -0,0 +1,553 @@
+<mujoco model="bracket_collision_geometry_1">
+  <!-- Using these properties to prevent trampoline behavior 
+  when the bracket lands flat on table or flat on another bracket part -->
+  <!-- condim="6" -->
+  <!-- friction="2.0 0.1 0.1" -->
+  <!-- margin="0.005" -->
+  <!-- solref="0.02 2.0" -->
+  <!-- solimp="0.95 0.999 0.01 0.5 2" -->
+  <!-- -->
+  <!-- damping="0.0001" -->
+  <!-- which were experimentally checked. There may be a better way to do it. -->
+
+  <!-- 5 Box collision primitives -->
+  <!-- starting from the left (pointy end) -->
+  <!-- top cheek -->
+  <body name="bracket-0-1" pos="-0.42 0.56 0.40">
+    <joint type="free" damping="0.0001" />
+    <geom
+      type="box"
+      size="0.0530875 0.025 0.00762"
+      pos="0.072240 -0.045944 0.00762"
+      euler="0 0 0.4593"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- bottom cheek -->
+    <geom
+      type="box"
+      size="0.0422435 0.0218295 0.00762"
+      pos="0.054467 -0.100271 0.00762"
+      euler="0 0 -0.7275"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- middle upper box -->
+    <geom
+      type="box"
+      size="0.01565 0.0145 0.00762"
+      pos="0.13365 -0.055718 0.00762"
+      euler="0 0 0"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- middle lower box-->
+    <geom
+      type="box"
+      size="0.024469 0.010 0.00762"
+      pos="0.108831 -0.085 0.00762"
+      euler="0 0 0"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- middle rear box -->
+    <geom
+      type="box"
+      size="0.017113 0.029 0.00762"
+      pos="0.166413 -0.070218 0.00762"
+      euler="0 0 0"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- top butt -->
+    <geom
+      type="box"
+      size="0.027879 0.025 0.00762"
+      pos="0.136622 -0.025 0.00762"
+      euler="0 0 0"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- bottom butt -->
+    <geom
+      type="box"
+      size="0.048879 0.025 0.00762"
+      pos="0.120751 -0.120 0.00762"
+      euler="0 0 0"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+
+    <!-- 3 Cylinder collision primitives -->
+    <!-- size="radius half_length" and default axis along Z -->
+    <!-- pointy side -->
+    <geom
+      type="cylinder"
+      size="0.025 0.00762"
+      pos="0.024654 -0.069478 0.00762"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- butts side -->
+    <geom
+      type="cylinder"
+      size="0.025 0.00762"
+      pos="0.1645 -0.025 0.00762"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="cylinder"
+      size="0.025 0.00762"
+      pos="0.16963 -0.120 0.00762"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+
+    <!-- Holes in part used for placement, approximated by boxes -->
+    <!-- 10 boxes centered at C=(0.076362, -0.079826, 0)-->
+    <geom
+      type="box"
+      pos="0.076362 -0.066326 0.00762"
+      euler="0 0 0.00"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.068518 -0.068839 0.00762"
+      euler="0 0 0.62"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.063594 -0.075441 0.00762"
+      euler="0 0 1.24"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.063423 -0.083676 0.00762"
+      euler="0 0 1.86"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.068068 -0.090478 0.00762"
+      euler="0 0 2.48"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.075801 -0.093314 0.00762"
+      euler="0 0 3.10"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.083742 -0.091130 0.00762"
+      euler="0 0 3.72"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.088937 -0.084738 0.00762"
+      euler="0 0 4.34"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.089450 -0.076517 0.00762"
+      euler="0 0 4.96"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.085092 -0.069528 0.00762"
+      euler="0 0 5.58"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+
+    <!-- 10 boxes centered at C=(0.110, -0.067, 0) -->
+    <geom
+      type="box"
+      pos="0.110000 -0.053500 0.00762"
+      euler="0 0 0.00"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.102156 -0.056013 0.00762"
+      euler="0 0 0.62"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.097232 -0.062615 0.00762"
+      euler="0 0 1.24"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.097061 -0.070850 0.00762"
+      euler="0 0 1.86"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.101706 -0.077652 0.00762"
+      euler="0 0 2.48"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.109439 -0.080488 0.00762"
+      euler="0 0 3.10"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.117380 -0.078304 0.00762"
+      euler="0 0 3.72"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.122575 -0.071912 0.00762"
+      euler="0 0 4.34"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.123088 -0.063691 0.00762"
+      euler="0 0 4.96"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.118730 -0.056702 0.00762"
+      euler="0 0 5.58"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+
+    <!-- 10 boxes centered at C=(0.1413, -0.082661, 0) -->
+    <geom
+      type="box"
+      pos="0.141300 -0.069161 0.00762"
+      euler="0 0 0.00"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.133456 -0.071674 0.00762"
+      euler="0 0 0.62"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.128532 -0.078276 0.00762"
+      euler="0 0 1.24"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.128361 -0.086511 0.00762"
+      euler="0 0 1.86"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.133006 -0.093313 0.00762"
+      euler="0 0 2.48"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.140739 -0.096149 0.00762"
+      euler="0 0 3.10"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.148680 -0.093965 0.00762"
+      euler="0 0 3.72"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.153875 -0.087573 0.00762"
+      euler="0 0 4.34"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.154388 -0.079352 0.00762"
+      euler="0 0 4.96"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.150030 -0.072363 0.00762"
+      euler="0 0 5.58"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+
+    <geom
+      contype="0"
+      conaffinity="0"
+      type="mesh"
+      mesh="bracket"
+      material="sheet_metal"
+    />
+    <site name="bracket_part_pose-0-1" />
+  </body>
+</mujoco>
diff --git a/src/factory_sim/description/bracket_collision_geometry_2.xml b/src/factory_sim/description/bracket_collision_geometry_2.xml
new file mode 100644
index 000000000..28b6b7470
--- /dev/null
+++ b/src/factory_sim/description/bracket_collision_geometry_2.xml
@@ -0,0 +1,553 @@
+<mujoco model="bracket_collision_geometry_2">
+  <!-- Using these properties to prevent trampoline behavior 
+  when the bracket lands flat on table or flat on another bracket part -->
+  <!-- condim="6" -->
+  <!-- friction="2.0 0.1 0.1" -->
+  <!-- margin="0.005" -->
+  <!-- solref="0.02 2.0" -->
+  <!-- solimp="0.95 0.999 0.01 0.5 2" -->
+  <!-- -->
+  <!-- damping="0.0001" -->
+  <!-- which were experimentally checked. There may be a better way to do it. -->
+
+  <!-- 5 Box collision primitives -->
+  <!-- starting from the left (pointy end) -->
+  <!-- top cheek -->
+  <body name="bracket-0-2" pos="-0.42 0.62 0.45">
+    <joint type="free" damping="0.0001" />
+    <geom
+      type="box"
+      size="0.0530875 0.025 0.00762"
+      pos="0.072240 -0.045944 0.00762"
+      euler="0 0 0.4593"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- bottom cheek -->
+    <geom
+      type="box"
+      size="0.0422435 0.0218295 0.00762"
+      pos="0.054467 -0.100271 0.00762"
+      euler="0 0 -0.7275"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- middle upper box -->
+    <geom
+      type="box"
+      size="0.01565 0.0145 0.00762"
+      pos="0.13365 -0.055718 0.00762"
+      euler="0 0 0"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- middle lower box-->
+    <geom
+      type="box"
+      size="0.024469 0.010 0.00762"
+      pos="0.108831 -0.085 0.00762"
+      euler="0 0 0"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- middle rear box -->
+    <geom
+      type="box"
+      size="0.017113 0.029 0.00762"
+      pos="0.166413 -0.070218 0.00762"
+      euler="0 0 0"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- top butt -->
+    <geom
+      type="box"
+      size="0.027879 0.025 0.00762"
+      pos="0.136622 -0.025 0.00762"
+      euler="0 0 0"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- bottom butt -->
+    <geom
+      type="box"
+      size="0.048879 0.025 0.00762"
+      pos="0.120751 -0.120 0.00762"
+      euler="0 0 0"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+
+    <!-- 3 Cylinder collision primitives -->
+    <!-- size="radius half_length" and default axis along Z -->
+    <!-- pointy side -->
+    <geom
+      type="cylinder"
+      size="0.025 0.00762"
+      pos="0.024654 -0.069478 0.00762"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <!-- butts side -->
+    <geom
+      type="cylinder"
+      size="0.025 0.00762"
+      pos="0.1645 -0.025 0.00762"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="cylinder"
+      size="0.025 0.00762"
+      pos="0.16963 -0.120 0.00762"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+
+    <!-- Holes in part used for placement, approximated by boxes -->
+    <!-- 10 boxes centered at C=(0.076362, -0.079826, 0)-->
+    <geom
+      type="box"
+      pos="0.076362 -0.066326 0.00762"
+      euler="0 0 0.00"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.068518 -0.068839 0.00762"
+      euler="0 0 0.62"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.063594 -0.075441 0.00762"
+      euler="0 0 1.24"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.063423 -0.083676 0.00762"
+      euler="0 0 1.86"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.068068 -0.090478 0.00762"
+      euler="0 0 2.48"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.075801 -0.093314 0.00762"
+      euler="0 0 3.10"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.083742 -0.091130 0.00762"
+      euler="0 0 3.72"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.088937 -0.084738 0.00762"
+      euler="0 0 4.34"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.089450 -0.076517 0.00762"
+      euler="0 0 4.96"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.085092 -0.069528 0.00762"
+      euler="0 0 5.58"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+
+    <!-- 10 boxes centered at C=(0.110, -0.067, 0) -->
+    <geom
+      type="box"
+      pos="0.110000 -0.053500 0.00762"
+      euler="0 0 0.00"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.102156 -0.056013 0.00762"
+      euler="0 0 0.62"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.097232 -0.062615 0.00762"
+      euler="0 0 1.24"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.097061 -0.070850 0.00762"
+      euler="0 0 1.86"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.101706 -0.077652 0.00762"
+      euler="0 0 2.48"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.109439 -0.080488 0.00762"
+      euler="0 0 3.10"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.117380 -0.078304 0.00762"
+      euler="0 0 3.72"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.122575 -0.071912 0.00762"
+      euler="0 0 4.34"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.123088 -0.063691 0.00762"
+      euler="0 0 4.96"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.118730 -0.056702 0.00762"
+      euler="0 0 5.58"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+
+    <!-- 10 boxes centered at C=(0.1413, -0.082661, 0) -->
+    <geom
+      type="box"
+      pos="0.141300 -0.069161 0.00762"
+      euler="0 0 0.00"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.133456 -0.071674 0.00762"
+      euler="0 0 0.62"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.128532 -0.078276 0.00762"
+      euler="0 0 1.24"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.128361 -0.086511 0.00762"
+      euler="0 0 1.86"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.133006 -0.093313 0.00762"
+      euler="0 0 2.48"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.140739 -0.096149 0.00762"
+      euler="0 0 3.10"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.148680 -0.093965 0.00762"
+      euler="0 0 3.72"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.153875 -0.087573 0.00762"
+      euler="0 0 4.34"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.154388 -0.079352 0.00762"
+      euler="0 0 4.96"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+    <geom
+      type="box"
+      pos="0.150030 -0.072363 0.00762"
+      euler="0 0 5.58"
+      size=".0075 .0075 .007"
+      rgba="0 .8 0 1"
+      group="3"
+      condim="6"
+      friction="2.0 0.1 0.1"
+      margin="0.005"
+      solref="0.02 2.0"
+      solimp="0.95 0.999 0.01 0.5 2"
+    />
+
+    <geom
+      contype="0"
+      conaffinity="0"
+      type="mesh"
+      mesh="bracket"
+      material="sheet_metal"
+    />
+    <site name="bracket_part_pose-0-2" />
+  </body>
+</mujoco>
diff --git a/src/factory_sim/description/scene.xml b/src/factory_sim/description/scene.xml
index 8a75412ab..e3778f581 100644
--- a/src/factory_sim/description/scene.xml
+++ b/src/factory_sim/description/scene.xml
@@ -4,6 +4,8 @@
     <flag multiccd="enable" override="enable" />
   </option>
 
+  <size memory="64M" />
+
   <asset>
     <!-- Define textures and materials -->
     <texture name="wood" type="cube" file="assets/wood.png" />
@@ -234,13 +236,17 @@
       </body>
     </body>
 
-    <!-- Bracket parts in left bin -->
-    <!-- 3 1 -->
-    <replicate count="3" sep="-" offset="0 0.06 0.05" euler="0.0 0.0 0.0">
-      <replicate count="1" sep="-" offset="0 0 .18" euler="0.0 0.0 0.0">
-        <include file="bracket_collision_geometry.xml" />
-      </replicate>
-    </replicate>
+    <!-- Bracket parts in left bin.
+         Three explicit bodies instead of <replicate count="3">: with replicate +
+         <keyframe>, MuJoCo 3.2.7 silently corrupts the first replica's freejoint
+         qpos to (0,0,0,1,0,0,0) on key load, dropping bracket-0-0 under the
+         pedestal on "Reset MuJoCo Sim". MuJoCo 3.3.5 surfaces the same bug as
+         "Keyframe 'default' has invalid qpos size, got 48, should be 13".
+         Explicit bodies preserve every keyframe row, so all three brackets reset
+         in their bin. Refs PickNikRobotics/moveit_pro#18180. -->
+    <include file="bracket_collision_geometry_0.xml" />
+    <include file="bracket_collision_geometry_1.xml" />
+    <include file="bracket_collision_geometry_2.xml" />
     <!--  -->
     <!-- ASSEMBLY SCENARIO -->
     <!--  -->
@@ -414,20 +420,20 @@
   <include file="box-welds.xml" />
 
   <keyframe>
-    <!-- qpos layout: -->
+    <!-- qpos layout, in compile order: -->
     <!-- robot [joint_1..joint_6] (6) -->
-    <!-- shelf freejoint [x y z qw qx qy qz] (7) -->
-    <!-- bracket-0 freejoint (7), bracket-1 freejoint (7), bracket-2 freejoint (7) -->
+    <!-- bracket-0 freejoint [x y z qw qx qy qz] (7), bracket-1 (7), bracket-2 (7) -->
+    <!-- shelf freejoint (7) -->
     <!-- suction_tool (gripper_base) freejoint (7) -->
     <!-- inspection_tool freejoint (7) -->
     <key
       name="default"
       qpos="
         0 0 0 0 0 0
-        0.25 -0.4 0.2301 1 0 0 0
         -0.42 0.5 0.35 1 0 0 0
         -0.42 0.56 0.40 1 0 0 0
         -0.42 0.62 0.45 1 0 0 0
+        0.25 -0.4 0.2301 1 0 0 0
         0.25 0.08 0.3 0 1 0 0
         0.25 -0.08 0.3 0 1 0 0"
       ctrl="0 0 0 0 0 0"
diff --git a/src/factory_sim/objectives/pick_up_tool_from_holder.xml b/src/factory_sim/objectives/pick_up_tool_from_holder.xml
index dbe2b9b65..4463f20d3 100644
--- a/src/factory_sim/objectives/pick_up_tool_from_holder.xml
+++ b/src/factory_sim/objectives/pick_up_tool_from_holder.xml
@@ -26,6 +26,12 @@
         trajectory_monitoring="false"
       />
       <Action ID="SetupMTCCurrentState" task="{mtc_task}" />
+      <Action
+        ID="SetupMTCIgnoreCollisionsBetweenObjects"
+        allow_collision="true"
+        task="{mtc_task}"
+        object_names="link_5;link_6;suction_gripper"
+      />
       <Action
         ID="SetupMTCPlanToPose"
         acceleration_scale_factor="0.5"
@@ -52,7 +58,7 @@
         axis_x="0.000000"
         axis_y="0.000000"
         axis_z="1.000000"
-        ignore_environment_collisions="false"
+        ignore_environment_collisions="true"
         planning_group_name="manipulator"
         velocity_scale="0.5"
         task="{mtc_task}"
diff --git a/src/factory_sim/objectives/place_tool_in_tool_holder.xml b/src/factory_sim/objectives/place_tool_in_tool_holder.xml
index e66ae9b70..67b69165b 100644
--- a/src/factory_sim/objectives/place_tool_in_tool_holder.xml
+++ b/src/factory_sim/objectives/place_tool_in_tool_holder.xml
@@ -77,7 +77,12 @@
         timeout="10"
       />
       <Action ID="DetachURDF" urdf_name="{tool_name}" />
-      <SubTree ID="Retract" _collapsed="true" distance="0.35" />
+      <SubTree
+        ID="Retract"
+        _collapsed="true"
+        distance="0.35"
+        ignore_environment_collisions="true"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/factory_sim/objectives/retract.xml b/src/factory_sim/objectives/retract.xml
index 2b332672e..8eed459e3 100644
--- a/src/factory_sim/objectives/retract.xml
+++ b/src/factory_sim/objectives/retract.xml
@@ -24,7 +24,7 @@
         acceleration_scale="0.5"
         axis_x="0.000000"
         axis_y="0.000000"
-        ignore_environment_collisions="false"
+        ignore_environment_collisions="{ignore_environment_collisions}"
         planning_group_name="manipulator"
         velocity_scale="0.5"
         task="{mtc_task}"
@@ -44,6 +44,7 @@
         <Metadata subcategory="Motion - Execute" />
       </MetadataFields>
       <input_port name="distance" default="0.1" />
+      <input_port name="ignore_environment_collisions" default="false" />
     </SubTree>
   </TreeNodesModel>
 </root>