From e6b9b2f2bdc43bec95e8b85db919b018cd4e33c3 Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Tue, 5 May 2026 17:23:06 -0400 Subject: [PATCH 1/3] 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. 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 with three explicit 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 '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; block replaced with three explicit 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) --- .../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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 @@ + + @@ -234,13 +236,17 @@ - - - - - - - + + + + @@ -414,20 +420,20 @@ - + - - + + + - + 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 @@ + From 80fa06c4602e66bf132368d7709a580f42ed9db8 Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Thu, 7 May 2026 19:29:01 -0400 Subject: [PATCH 2/3] feat(factory_sim): admittance move-to-touch for Vacuum Pick Bracket Wires a wrist force/torque sensor through MuJoCo, ros2_control, and the admittance controller, then rewrites Vacuum Pick Bracket Part Subtree to descend onto the bracket under z-axis admittance instead of an open-loop +0.055 m cartesian push. The FTS reads the weld constraint force at the suction-tool flange interface, so when the cup contacts the bracket the admittance back-drives the trajectory while MuJoCo activates the gripper-to-bracket weld at the actual contact pose. FTS wiring: - Add a at gripper_base origin (suction_tool.xml) and a top-level in scene.xml. MujocoSystem auto-registers these as wrist_ft_sensor/{force,torque}.{x,y,z} state interfaces using the site name. - Add force_torque_sensor_broadcaster to controller_manager and configure it (sensor_name: wrist_ft_sensor, frame_id: tool0). Add it to controllers_active_at_startup. - Set JTAC ft_sensor_name: wrist_ft_sensor and migrate sensor_frame / ee_frame to the plural sensor_frames / ee_frames forms (silences the deprecation warnings). Subtree rewrite (vacuum_pick_bracket_part_subtree.xml): - Drop SetupMTCMoveAlongFrameAxis (the open-loop push the prior comment flagged as wanting force feedback). - Plan the touch as a separate cartesian segment (PlanCartesianPath, +0.07 m along grasp_link Z, position-only) for a 1.5 cm overshoot beyond the prior 5.5 cm push so admittance has room to back off. - Tare the FTS (CallTriggerService /joint_trajectory_admittance_controller/ zero_fts/tool0), enable z-only admittance, ExecuteTrajectory with loose path/goal tolerance and force threshold off. - Restore plain JTAC tracking with an admittance-disable params yaml so downstream MTC motions aren't offset. Two new params yamls: - vacuum_touch_admittance_params.yaml: z-only, 180 N/m stiffness, heavy damping, mirroring lab_sim/objectives/push_button_admittance_params.yaml. - vacuum_touch_admittance_disable.yaml: all axes off. Co-Authored-By: Claude Opus 4.7 (1M context) --- src/factory_sim/config/config.yaml | 1 + .../control/picknik_fanuc.ros2_control.yaml | 27 ++++-- src/factory_sim/description/scene.xml | 8 ++ src/factory_sim/description/suction_tool.xml | 5 ++ .../vacuum_pick_bracket_part_subtree.xml | 90 ++++++++++++++----- .../vacuum_touch_admittance_disable.yaml | 54 +++++++++++ .../vacuum_touch_admittance_params.yaml | 55 ++++++++++++ 7 files changed, 215 insertions(+), 25 deletions(-) create mode 100644 src/factory_sim/objectives/vacuum_touch_admittance_disable.yaml create mode 100644 src/factory_sim/objectives/vacuum_touch_admittance_params.yaml diff --git a/src/factory_sim/config/config.yaml b/src/factory_sim/config/config.yaml index fc7d3c107..30f59ae70 100644 --- a/src/factory_sim/config/config.yaml +++ b/src/factory_sim/config/config.yaml @@ -96,6 +96,7 @@ ros2_control: controllers_active_at_startup: - "joint_trajectory_admittance_controller" - "joint_state_broadcaster" + - "force_torque_sensor_broadcaster" - "tool_attach_controller" - "suction_cup_controller" # Load but do not start these controllers so they can be activated later if needed. 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 ae9a63c13..5c6d1600b 100644 --- a/src/factory_sim/config/control/picknik_fanuc.ros2_control.yaml +++ b/src/factory_sim/config/control/picknik_fanuc.ros2_control.yaml @@ -5,6 +5,8 @@ controller_manager: type: joint_state_broadcaster/JointStateBroadcaster joint_trajectory_admittance_controller: type: joint_trajectory_admittance_controller/JointTrajectoryAdmittanceController + force_torque_sensor_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster # A controller to enable/disable the tool attachment interface at the robot flange. # In sim, this is modeled as a weld constraint in Mujoco. # In a real robot, this controller would activate a mechanism to attach/detach the tool at the robot flange. @@ -48,17 +50,32 @@ joint_state_broadcaster: - position - velocity +force_torque_sensor_broadcaster: + ros__parameters: + # Hardware interface name. Matches the MuJoCo name; MujocoSystem + # exposes "/force.{x,y,z}" and "/torque.{x,y,z}" state interfaces. + sensor_name: wrist_ft_sensor + state_interface_names: + - force.x + - force.y + - force.z + - torque.x + - torque.y + - torque.z + # The frame the sensor reading is published in. + frame_id: tool0 + joint_trajectory_admittance_controller: ros__parameters: # Joint group to control. planning_group_name: manipulator - # Specifies the frame/link name of the force torque sensor. Must exist in the robot description. - sensor_frame: link_6 - # Specifies the frame/link name of the end-effector frame. Must exist in the robot description. - ee_frame: grasp_link + # Specifies the frame/link names of the force torque sensor(s). Must exist in the robot description. + sensor_frames: [tool0] + # Specifies the frame/link names of the end-effector frame(s). Must exist in the robot description. + ee_frames: [grasp_link] # The name of the force/torque ros2_control hardware interface which will be used in the admittance calculation. # If empty, the controller will run without admittance. - ft_sensor_name: "" + ft_sensor_name: wrist_ft_sensor # Joint accelerations to use for immediate stops (e.g. when cancelling a running trajectory). # Chosen to match MoveIt configs. stop_accelerations: [30.0, 30.0, 30.0, 30.0, 30.0, 30.0] diff --git a/src/factory_sim/description/scene.xml b/src/factory_sim/description/scene.xml index e3778f581..a8f1a3661 100644 --- a/src/factory_sim/description/scene.xml +++ b/src/factory_sim/description/scene.xml @@ -419,6 +419,14 @@ + + + + + + diff --git a/src/factory_sim/description/suction_tool.xml b/src/factory_sim/description/suction_tool.xml index 04b27afe3..1146b7d49 100644 --- a/src/factory_sim/description/suction_tool.xml +++ b/src/factory_sim/description/suction_tool.xml @@ -14,6 +14,11 @@ armature="0" /> + + diff --git a/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml b/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml index 044a4d878..105c7ba73 100644 --- a/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml +++ b/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml @@ -1,9 +1,8 @@ - @@ -20,14 +19,17 @@ output_pose="{approach_pose}" translation_xyz="0;0;0.15" quaternion_xyzw="-0.0; -1.0; -0.0; -0.0" - name="Create approach pose before straight pick" + name="Create approach pose above bracket" /> + @@ -53,28 +55,76 @@ trajectory_sampling_rate="100" velocity_scale_factor="1.000000" /> - + + + + + + + + + Date: Thu, 7 May 2026 20:54:07 -0400 Subject: [PATCH 3/3] fix(factory_sim): make admittance move-to-touch run end-to-end Two bugs in the prior commit that prevented Vacuum Pick Bracket Part Subtree from completing under Pick Brackets from Left Bin's Repeat loop: 1. SetAdmittanceParameters reads BT XML attributes as parameter overrides, so the human-readable `name="..."` annotation was being interpreted as a missing parameter name and aborting with "Failed to parse admittance controller parameters: Parameter '' not found". Replaced the BT step labels with XML comments above each Action so intent is still readable. 2. AddPoseStampedToVector appends. The parent BT wraps this subtree in , and the {touch_path} blackboard variable is inherited across SubTree invocations, so iteration 2+ sent a path with stale waypoints to PlanCartesianPath. The planner rejected it: "Bad initial conditions: Waypoint N in path closer than '2 * blending_radius' to neighbors. Distance to next waypoint: ~0". Added ResetPoseStampedVector before the append so the path starts empty each iteration. Co-Authored-By: Claude Opus 4.7 (1M context) --- .../vacuum_pick_bracket_part_subtree.xml | 23 +++++++++++++++---- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml b/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml index 105c7ba73..236adf775 100644 --- a/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml +++ b/src/factory_sim/objectives/vacuum_pick_bracket_part_subtree.xml @@ -65,12 +65,12 @@ the suction cup contacts the bracket. The 7 cm overshoot guarantees contact within the planned path; admittance back-drives the trajectory once the FT sensor reads the contact reaction. --> + + + + + + +