From 0308c58a1ce5420c2d372b4070f281df9729e45d Mon Sep 17 00:00:00 2001 From: Griswald Brooks Date: Wed, 24 Jun 2026 18:39:51 -0400 Subject: [PATCH] fix(hangar_sim): stop base pirouette by biasing pick IK yaw Use the new opt-in IK yaw weighting to keep the omni-base heading near its current value during picks instead of drifting a full revolution (issue #19973): weight rotational_yaw_joint in the pose_ik seed-distance cost, and in the SetupMTCBatchPoseIK solution-selection cost with max_ik_solutions=8 so the selection has yaw alternatives to choose from. Base translation stays free, so reachability is preserved. Co-Authored-By: Claude Opus 4.8 (1M context) --- src/hangar_sim/config/moveit/pose_ik_distance.yaml | 5 +++++ .../objectives/move_boxes_to_loading_zone_from_waypoint.xml | 4 +++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/hangar_sim/config/moveit/pose_ik_distance.yaml b/src/hangar_sim/config/moveit/pose_ik_distance.yaml index 1ad041046..bdb4353fd 100644 --- a/src/hangar_sim/config/moveit/pose_ik_distance.yaml +++ b/src/hangar_sim/config/moveit/pose_ik_distance.yaml @@ -3,6 +3,11 @@ manipulator: target_tolerance: 0.001 solve_mode: "optimize_distance" optimization_timeout: 0.005 + # Issue #19973: weight the redundant mobile-base yaw heavily in the seed-distance cost so the IK resolves the + # redundancy toward keeping the base heading near its current value, instead of letting it drift a full revolution + # across consecutive picks. Base translation stays unweighted (free) so the arm can still reach boxes. + seed_distance_joint_names: ["rotational_yaw_joint"] + seed_distance_joint_weights: [10.0] arm_only: kinematics_solver: pose_ik_plugin/PoseIKPlugin diff --git a/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml b/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml index a6545ba69..a65c39f2b 100644 --- a/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml +++ b/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml @@ -260,7 +260,9 @@ end_effector_link="grasp_link" ik_group="manipulator" ik_timeout_s="0.01" - max_ik_solutions="1" + max_ik_solutions="8" + cost_joint_names="rotational_yaw_joint" + cost_joint_weights="50.0" monitored_stage="allow collision (gripper, <octomap>)" target_poses="{grasp_poses}" task="{task}"