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}"