From e3c2abe8426a981dbddc23a55d81d762c83b09bd Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Fri, 20 Nov 2026 20:56:39 -0500 Subject: [PATCH 1/8] golem_skills: read the grasp robot from a per-robot profile The grasp frames, pinch frames, prep and staging configs and GraspGenX gripper the manipulation skills drive were H1-2 constants spread over base.py, grasp.py, pick_place.py and graspgenx.py. They now come from robots.RobotProfile, picked once by a new read-only `robot` parameter (default h12, the only profile so far). No behaviour change on the H1-2. Co-Authored-By: Claude Opus 5.5 --- core_ws/src/golem_skills/golem_skills/base.py | 22 ++++--- .../golem_skills/grasp_synthesis/common.py | 2 + .../golem_skills/grasp_synthesis/graspgenx.py | 2 +- .../src/golem_skills/golem_skills/robots.py | 61 +++++++++++++++++++ .../golem_skills/golem_skills/skills/grasp.py | 43 +++++++------ .../golem_skills/skills/grasp_detection.py | 9 +-- .../golem_skills/skills/grasp_servo.py | 6 +- .../golem_skills/skills/pick_place.py | 34 +++++------ .../golem_skills/test/test_grasp_synthesis.py | 7 ++- core_ws/src/golem_skills/test/test_robots.py | 36 +++++++++++ 10 files changed, 166 insertions(+), 56 deletions(-) create mode 100644 core_ws/src/golem_skills/golem_skills/robots.py create mode 100644 core_ws/src/golem_skills/test/test_robots.py diff --git a/core_ws/src/golem_skills/golem_skills/base.py b/core_ws/src/golem_skills/golem_skills/base.py index 6ae4d84b..2077a3cd 100644 --- a/core_ws/src/golem_skills/golem_skills/base.py +++ b/core_ws/src/golem_skills/golem_skills/base.py @@ -65,6 +65,7 @@ ) from .grasp_synthesis import DEFAULT_METHOD, METHODS, method_name from model_server.model_logging import _log_root +from .robots import ARMS, DEFAULT_ROBOT, ROBOTS, robot_profile from .run_telemetry import RunRecorder, describe_goal, run_dir @@ -80,10 +81,6 @@ 'right': '/realsense/right_hand/color/image_raw/compressed/detections', } -# URDF gripper-BASE frames whose axes match GraspGenX's grasp convention (+Z -# approach, +X finger closing, origin at the magpie gripper base), so a grasp -# pose drives them as given, with no axis permutation or TCP offset. -GRASP_FRAMES = {'left': 'left_graspgenx_frame', 'right': 'right_graspgenx_frame'} # per-arm gripper namespace: the magpie driver and the sims serve /set_position, # /set_force, /open and /close and publish /state; the driver # alone serves /reset_overload @@ -302,6 +299,15 @@ def __init__(self, node_name='golem_skills'): # them, and pick_place can call this node's own /skill/grasp. self._cb_group = ReentrantCallbackGroup() + # The robot the skills drive (robots.py): its grasp frames, named + # configurations and grasp planner gripper. Read once: a running node + # serves one robot. + self.robot = robot_profile(self.declare_parameter( + 'robot', DEFAULT_ROBOT, ParameterDescriptor( + description=f'robot the skills drive, one of {sorted(ROBOTS)}', + read_only=True)).value) + self.get_logger().info(f'golem_skills: robot {self.robot.name}') + self.gripper_clis = { arm: self.create_client(SetGripperPosition, f'{ns}/set_position', callback_group=self._cb_group) @@ -997,7 +1003,7 @@ def move_frame_to(self, frame, pose, """ Drive URDF `frame` to `pose` (pelvis frame) with one /frame_task goal; return reached. - `frame` is GRASP_FRAMES[arm] for grasping. Pass the skill's goal handle + `frame` is the robot's grasp frame (robots.py) for grasping. Pass the skill's goal handle as `outer_gh` so a skill cancel cancels the frame_task goal too. `duration_sec` is a TIMEOUT. `slow_mode` runs the goal at the server's @@ -1514,7 +1520,7 @@ def _grip_hold_loop(self, arm, stop): def _validated_arm(self, goal): """Return goal.arm normalized to 'left' or 'right', or None for any other value.""" arm = goal.arm.strip().lower() - if arm not in GRASP_FRAMES: + if arm not in ARMS: return None return arm @@ -1542,8 +1548,8 @@ def _start_run_telemetry(self, skill, goal_handle): goal.top_down, self.get_parameter('grasp_method').value) try: rec = RunRecorder( - self, skill, out_dir, WORLD_FRAME, GRASP_FRAMES, - arm=arm if arm in GRASP_FRAMES else None, + self, skill, out_dir, WORLD_FRAME, self.robot.grasp_frames, + arm=arm if arm in ARMS else None, request=request).start() # recording must never break a skill except Exception as e: diff --git a/core_ws/src/golem_skills/golem_skills/grasp_synthesis/common.py b/core_ws/src/golem_skills/golem_skills/grasp_synthesis/common.py index 2b19c1e5..fd582365 100644 --- a/core_ws/src/golem_skills/golem_skills/grasp_synthesis/common.py +++ b/core_ws/src/golem_skills/golem_skills/grasp_synthesis/common.py @@ -43,6 +43,8 @@ class SynthesisInput: plan_grasp: Callable # rclpy logger log: object + # gripper_name the graspgen service plans for (robots.RobotProfile.graspgen_gripper) + gripper_name: str @property def centroid(self): diff --git a/core_ws/src/golem_skills/golem_skills/grasp_synthesis/graspgenx.py b/core_ws/src/golem_skills/golem_skills/grasp_synthesis/graspgenx.py index 2c42a348..63f18331 100644 --- a/core_ws/src/golem_skills/golem_skills/grasp_synthesis/graspgenx.py +++ b/core_ws/src/golem_skills/golem_skills/grasp_synthesis/graspgenx.py @@ -40,7 +40,7 @@ def _plan(inp): """Return (graspgen response, None), or (None, reason) when the service gives none.""" - resp = inp.plan_grasp(inp.cloud, gripper_name='magpie', frame='pelvis', + resp = inp.plan_grasp(inp.cloud, gripper_name=inp.gripper_name, frame='pelvis', scene_cloud=inp.scene, arm=inp.arm) if resp is None: return None, f'no grasp planned for {inp.obj!r}' diff --git a/core_ws/src/golem_skills/golem_skills/robots.py b/core_ws/src/golem_skills/golem_skills/robots.py new file mode 100644 index 00000000..03881b1f --- /dev/null +++ b/core_ws/src/golem_skills/golem_skills/robots.py @@ -0,0 +1,61 @@ +""" +Hold what the manipulation skills need to know about each robot they drive. + +The skills node's `robot` parameter (default 'h12') picks one RobotProfile from +ROBOTS, once at start-up. Everything robot-specific the grasp and pick_place +skills read comes from it; the rest of the package is the same for every robot. + +This module imports no ROS package. +""" + +from dataclasses import dataclass +from typing import Optional + + +# the arms a goal may name, and the keys of every per-arm table below +ARMS = ('left', 'right') + + +@dataclass(frozen=True) +class RobotProfile: + """Name what the grasp and pick_place skills drive on one robot.""" + + # golem_upper_ik's robot name, also the node's `robot` parameter value + name: str + # arm -> frame the grasp poses are for: the GraspGenX gripper-base + # convention (+Z the approach, +X the finger-closing axis, the finger + # contact point grasp_synthesis.common.GRIPPER_BASE_TO_CONTACT_M along +Z) + grasp_frames: dict + # arm -> pinch-point frame, at the finger contact point; pick_place's lift + # drives it + pinch_frames: dict + # golem_upper_ik named configuration the arms go to before the head camera + # looks: out of its view + prep_config: str + # arm -> named configuration a staged synthesis method starts from + staging_configs: dict + # gripper_name the graspgen service plans for; None: no GraspGenX model + graspgen_gripper: Optional[str] + + +H12 = RobotProfile( + name='h12', + # URDF frames of the Magpie grippers + grasp_frames={'left': 'left_graspgenx_frame', 'right': 'right_graspgenx_frame'}, + pinch_frames={'left': 'left_grasp_frame', 'right': 'right_grasp_frame'}, + prep_config='prep', + staging_configs={'left': 'staging_left', 'right': 'staging_right'}, + graspgen_gripper='magpie', +) + +ROBOTS = {p.name: p for p in (H12,)} + +DEFAULT_ROBOT = 'h12' + + +def robot_profile(name): + """Return the RobotProfile named `name`; raise ValueError for an unknown robot.""" + key = str(name).strip().lower() + if key not in ROBOTS: + raise ValueError(f'unknown robot {name!r}; golem_skills drives {sorted(ROBOTS)}') + return ROBOTS[key] diff --git a/core_ws/src/golem_skills/golem_skills/skills/grasp.py b/core_ws/src/golem_skills/golem_skills/skills/grasp.py index bee0c684..7485d806 100644 --- a/core_ws/src/golem_skills/golem_skills/skills/grasp.py +++ b/core_ws/src/golem_skills/golem_skills/skills/grasp.py @@ -37,8 +37,8 @@ from custom_ros_messages.action import SkillGrasp -from ..base import (_Run, FRAME_TASK_STABLE_STOP_MSGS, GRASP_FRAMES, - SLOW_MODE_TIME_SCALE, WORLD_FRAME) +from ..base import (_Run, FRAME_TASK_STABLE_STOP_MSGS, SLOW_MODE_TIME_SCALE, + WORLD_FRAME) from ..grasp_synthesis import METHODS, SynthesisInput, method_name from ..grasp_synthesis.common import pose_at from ..perception_utils import (approach_pose, pose_quat, pose_xyz, @@ -323,14 +323,16 @@ def _grasp(self, gh): if not run.phase('detect_object', 0.0): return run.result - # Arms to 'prep' first, out of the head camera's view (an arm across the - # object corrupts the box, mask and cloud); planned, because the arms - # start wherever the last skill left them. The only failure without a - # retreat: the skill has not moved the arm yet. - if not self.goto_named_config('prep', plan=True, slow_mode=True, + # Arms to the robot's prep config ('prep' on the H1-2) first, out of + # the head camera's view (an arm across the object corrupts the box, + # mask and cloud); planned, because the arms start wherever the last + # skill left them. The only failure without a retreat: the skill has + # not moved the arm yet. + prep = self.robot.prep_config + if not self.goto_named_config(prep, plan=True, slow_mode=True, duration_sec=30.0, result_timeout=90.0, outer_gh=gh): - return run.abort("move to 'prep' before detection failed") + return run.abort(f'move to {prep!r} before detection failed') def fail(message): """Lift the hand clear of the workspace, then abort with `message`.""" @@ -356,9 +358,9 @@ def fail(message): # A staged method starts from the arm's joint-space staging config: a # repeatable posture above the workspace for a steep top-down approach, - # reached without an IK solve, with the other arm at 'prep'. Other - # methods start from 'prep'. - stage_cfg = 'staging_right' if arm == 'right' else 'staging_left' + # reached without an IK solve, with the other arm at prep. Other + # methods start from prep. + stage_cfg = self.robot.staging_configs[arm] if method.staged: if not self.goto_named_config(stage_cfg, plan=True, slow_mode=True, duration_sec=30.0, result_timeout=90.0, @@ -376,7 +378,7 @@ def fail(message): # the run_telemetry target is the object centroid in the world frame if targets.centroid_w is not None: self._note_run_target(targets.centroid_w) - reset_cfg = stage_cfg if method.staged else 'prep' + reset_cfg = stage_cfg if method.staged else prep obstacles = scene_obstacles(scene, obj_cloud) if method.reach_check else None contact, err = self._reach_candidate(run, gh, goal, obj, arm, cands, targets, reset_cfg, @@ -430,7 +432,8 @@ def get_grasp_candidates(self, method, goal, obj, arm, obj_cloud, scene): inp = SynthesisInput( obj=obj, arm=arm, cloud=obj_cloud, scene=scene, min_pitch_deg=float(goal.min_downward_pitch_deg or 0.0), - plan_grasp=self.plan_grasp, log=self.get_logger()) + plan_grasp=self.plan_grasp, log=self.get_logger(), + gripper_name=self.robot.graspgen_gripper) cands, err = method.synthesize(inp) if err is None and not cands.grasps: err = f'{method.name} produced no grasp for {obj!r}' @@ -520,7 +523,7 @@ def mark(i, state): f'{cands.tiers[i]}, score {cands.scores[i]:.2f}, ' f'width {width_mm:.1f}mm') contact, why = None, 'pre-grasp unreachable' - if self._approach_pregrasp(GRASP_FRAMES[arm], targets.approaches_p[i], + if self._approach_pregrasp(self.robot.grasp_frames[arm], targets.approaches_p[i], run, gh, pregrasp_w=targets.approaches_w[i]): if goal.visual_servo: contact, err = self._servo_contact(run, gh, arm, obj, targets, i) @@ -552,7 +555,7 @@ def mark(i, state): def _check_candidate_reach(self, arm, targets, idx, obstacles): """Return why candidate `idx`'s approach fails the reach check, or None.""" - frame = GRASP_FRAMES[arm] + frame = self.robot.grasp_frames[arm] cur = self._frame_pose_in_pelvis(frame) if cur is None: return None @@ -582,7 +585,7 @@ def _servo_contact(self, run, gh, arm, obj, targets, idx): self.get_logger().warn( f'grasp: visual servo did not converge ({err}) — closing ' 'anyway at the current pose (best-effort)') - pose_p = self._frame_pose_in_pelvis(GRASP_FRAMES[arm]) + pose_p = self._frame_pose_in_pelvis(self.robot.grasp_frames[arm]) pose_w = (self._transform_pose(pose_p, 'pelvis', WORLD_FRAME) if targets.have_world and pose_p is not None else None) return _Contact(idx, pose_p, pose_w), None @@ -682,7 +685,7 @@ def _move_in_along_axis(self, gh, arm, targets, idx): correction removes the remaining pelvis drift. A cancel or an arm halt misses the candidate. """ - frame = GRASP_FRAMES[arm] + frame = self.robot.grasp_frames[arm] stop_short = float(self.get_parameter('contact_offset').value) contact_p = approach_pose(targets.grasps_p[idx], -stop_short) contact_w = (approach_pose(targets.grasps_w[idx], -stop_short) @@ -747,7 +750,7 @@ def _retreat_after_failure(self, arm, gh): if halt is not None: self.get_logger().warn(f'retreat: skipped, arm halted: {halt}') return - frame = GRASP_FRAMES[arm] + frame = self.robot.grasp_frames[arm] cur = self._frame_pose_in_pelvis(frame) if cur is None: self.get_logger().warn( @@ -787,12 +790,12 @@ def _closest_arm(self, centroid_p): object is on (+Y = left). """ dists = {} - for arm, frame in GRASP_FRAMES.items(): + for arm, frame in self.robot.grasp_frames.items(): p = self._transform_pose(pose_at(centroid_p), 'pelvis', frame) if p is not None: dists[arm] = math.sqrt(p.position.x ** 2 + p.position.y ** 2 + p.position.z ** 2) - if len(dists) == len(GRASP_FRAMES): + if len(dists) == len(self.robot.grasp_frames): arm = min(dists, key=dists.get) self.get_logger().info( 'grasp: auto-selected %s arm (hand-to-object %s)' % ( diff --git a/core_ws/src/golem_skills/golem_skills/skills/grasp_detection.py b/core_ws/src/golem_skills/golem_skills/skills/grasp_detection.py index 81a3c722..57c99c36 100644 --- a/core_ws/src/golem_skills/golem_skills/skills/grasp_detection.py +++ b/core_ws/src/golem_skills/golem_skills/skills/grasp_detection.py @@ -12,7 +12,7 @@ from geometry_msgs.msg import Point from custom_ros_messages.msg import Detection -from ..base import GEMINI_TIMEOUT_SEC, GRASP_FRAMES +from ..base import GEMINI_TIMEOUT_SEC from ..grasp_synthesis.common import pose_at from ..perception_utils import gemini_boxes @@ -64,8 +64,9 @@ def detect_object(self, obj, run, gh, arm=None): if len(boxes) <= 1: return boxes[0] if boxes else self._gemini_box(obj, run, gh) ref = None - if arm in GRASP_FRAMES: - o = self._transform_pose(pose_at((0.0, 0.0, 0.0)), GRASP_FRAMES[arm], 'pelvis') + if arm in self.robot.grasp_frames: + o = self._transform_pose(pose_at((0.0, 0.0, 0.0)), + self.robot.grasp_frames[arm], 'pelvis') if o is not None: ref = np.array([o.position.x, o.position.y, o.position.z]) if ref is None: @@ -83,7 +84,7 @@ def detect_object(self, obj, run, gh, arm=None): f'grasp: {len(boxes)} {obj!r} instances in view but none had ' f'usable head-camera depth; falling back to the single-box query') return self._gemini_box(obj, run, gh) - where = f'the {arm} arm' if arm in GRASP_FRAMES else 'the robot' + where = f'the {arm} arm' if arm in self.robot.grasp_frames else 'the robot' self.get_logger().info( f'grasp: {len(boxes)} {obj!r} instances in view; picked the one at ' f'pelvis ({best_pt[0]:.3f}, {best_pt[1]:.3f}, {best_pt[2]:.3f}), ' diff --git a/core_ws/src/golem_skills/golem_skills/skills/grasp_servo.py b/core_ws/src/golem_skills/golem_skills/skills/grasp_servo.py index 1681c0b9..bf24dd22 100644 --- a/core_ws/src/golem_skills/golem_skills/skills/grasp_servo.py +++ b/core_ws/src/golem_skills/golem_skills/skills/grasp_servo.py @@ -17,7 +17,7 @@ from rclpy.duration import Duration as RclpyDuration from sensor_msgs.msg import Image -from ..base import DEPTH_MAX_M, FRAME_TASK_STABLE_STOP_MSGS, GRASP_FRAMES +from ..base import DEPTH_MAX_M, FRAME_TASK_STABLE_STOP_MSGS from ..perception_utils import deproject_mask, gemini_boxes from .grasp_detection import box_detection from .grasp_markers import approach_target_tf @@ -156,7 +156,7 @@ def servo(self, run, gh, arm, obj, targets, idx): """ Align the gripper on `obj` with the hand camera at the pre-grasp. - Drives GRASP_FRAMES[arm] in PURE TRANSLATION until `obj` sits at + Drives the arm's grasp frame in PURE TRANSLATION until `obj` sits at (VISUAL_SERVO_TARGET_XY_M, depth setpoint) in the hand camera's optical frame; the orientation stays the planned grasp's. The depth setpoint is `servo_depth`, raised to the median servo_depth_floor of @@ -175,7 +175,7 @@ def servo(self, run, gh, arm, obj, targets, idx): Returns (pose, None), the pelvis-frame gripper Pose it converged at, where the caller closes with no contact move, or (None, reason). """ - frame = GRASP_FRAMES[arm] + frame = self.robot.grasp_frames[arm] planned_q = targets.grasps_p[idx].orientation servo_depth = float(self.get_parameter('servo_depth').value) target = np.array([VISUAL_SERVO_TARGET_XY_M[0], diff --git a/core_ws/src/golem_skills/golem_skills/skills/pick_place.py b/core_ws/src/golem_skills/golem_skills/skills/pick_place.py index 1c587da5..8b4e7f13 100644 --- a/core_ws/src/golem_skills/golem_skills/skills/pick_place.py +++ b/core_ws/src/golem_skills/golem_skills/skills/pick_place.py @@ -33,7 +33,7 @@ from custom_ros_messages.action import SkillGrasp, SkillPickPlace -from ..base import _Run, FRAME_TASK_STABLE_STOP_MSGS, GRASP_FRAMES, WORLD_FRAME +from ..base import _Run, FRAME_TASK_STABLE_STOP_MSGS, WORLD_FRAME from ..perception_utils import (matrix_to_pose, pose_to_matrix, transform_to_matrix) from ..grasp_synthesis.common import pose_at @@ -67,13 +67,12 @@ LIFT_STEP = 0.05 # s of commanded motion per rung LIFT_STEP_DURATION_SEC = 4.0 -# The lift drives the PINCH-POINT frame, not the GraspGenX gripper-base frame -# the targets are computed for. The two are GRIPPER_BASE_TO_CONTACT_M (0.1146 m) -# apart along the approach, so nulling the error at the base would let a wrist -# orientation residual swing the fingertips through ~0.1146 x ang_tol -# unnoticed; on a small part held in the fingertips that arc rolls it out of -# the jaws. -PINCH_FRAMES = {'left': 'left_grasp_frame', 'right': 'right_grasp_frame'} +# The lift drives the robot's PINCH-POINT frame (robots.RobotProfile.pinch_frames), +# not the GraspGenX gripper-base frame the targets are computed for. The two +# are GRIPPER_BASE_TO_CONTACT_M (0.1146 m) apart along the approach, so nulling +# the error at the base would let a wrist orientation residual swing the +# fingertips through ~0.1146 x ang_tol unnoticed; on a small part held in the +# fingertips that arc rolls it out of the jaws. # debug TF (RViz) of the detected place point, then the commanded place pose PLACE_TARGET_FRAME = 'pick_place_target_frame' @@ -123,12 +122,13 @@ def fail(message): if not run.phase('detect_place_target', 0.0): return run.result # Arms out of the head camera's view before the detection image (the - # grasp skill's own 'prep' is then a no-op). The skill has not moved - # the arm yet, so there is no retreat. - if not self.goto_named_config('prep', plan=True, slow_mode=True, + # grasp skill's own move to prep is then a no-op). The skill has not + # moved the arm yet, so there is no retreat. + prep = self.robot.prep_config + if not self.goto_named_config(prep, plan=True, slow_mode=True, duration_sec=30.0, result_timeout=90.0, outer_gh=gh): - return run.abort("move to 'prep' before detection failed") + return run.abort(f'move to {prep!r} before detection failed') place_cloud, _, err = self.detect_object_cloud(place, run, gh) if err: return fail(f'place target: {err}') @@ -155,10 +155,10 @@ def fail(message): # The lift rungs are computed for the GraspGenX frame the grasp pose is # in, then restated for the pinch-point frame that holds the object # (to_pinch); see the LIFT_* constants. - lift_frame = PINCH_FRAMES[arm] + lift_frame = self.robot.pinch_frames[arm] T_off = self._pinch_frame_offset(arm) if T_off is None: - return fail(f'TF {GRASP_FRAMES[arm]} -> {lift_frame} unavailable, ' + return fail(f'TF {self.robot.grasp_frames[arm]} -> {lift_frame} unavailable, ' 'so the post-grasp lift target is unknown') def to_pinch(pose_gx): @@ -214,11 +214,11 @@ def to_pinch(pose_gx): place_w if place_w is not None else place_p, self.get_clock().now().to_msg(), child=PLACE_TARGET_FRAME)) self.get_logger().info( - f'pick_place: {GRASP_FRAMES[arm]} to the top-down release pose ' + f'pick_place: {self.robot.grasp_frames[arm]} to the top-down release pose ' f'{PLACE_HEIGHT * 100:.0f}cm above {place!r}') # planned: the hand carries the object across the workspace if not self.servo_frame_to_world( - GRASP_FRAMES[arm], place_w, place_p, outer_gh=gh, + self.robot.grasp_frames[arm], place_w, place_p, outer_gh=gh, duration_sec=APPROACH_DURATION_SEC, max_iter=SERVO_MAX_ITER, lin_tol=SERVO_LIN_TOL, ang_tol=SERVO_ANG_TOL, label='carry to release pose above place target'): @@ -289,7 +289,7 @@ def _pinch_frame_offset(self, arm): """ try: tf = self.tf_buffer.lookup_transform( - GRASP_FRAMES[arm], PINCH_FRAMES[arm], Time(), + self.robot.grasp_frames[arm], self.robot.pinch_frames[arm], Time(), timeout=RclpyDuration(seconds=1.0)) except TransformException: return None diff --git a/core_ws/src/golem_skills/test/test_grasp_synthesis.py b/core_ws/src/golem_skills/test/test_grasp_synthesis.py index d29aa694..bd8e88b5 100644 --- a/core_ws/src/golem_skills/test/test_grasp_synthesis.py +++ b/core_ws/src/golem_skills/test/test_grasp_synthesis.py @@ -44,7 +44,7 @@ def _fake_planner(poses, scores=None, width=0.05): def plan_grasp(cloud, gripper_name, frame, scene_cloud=None, arm=''): """Return a graspgen-like response holding `poses`, scored best-first.""" - calls.append(dict(frame=frame, arm=arm)) + calls.append(dict(frame=frame, arm=arm, gripper_name=gripper_name)) grasps = [] for p in poses: g = PoseStamped() @@ -64,7 +64,8 @@ def _input(cloud, arm='right', min_pitch=0.0, planner=None): """Build a SynthesisInput for the object 'thing'.""" return SynthesisInput(obj='thing', arm=arm, cloud=cloud, scene=None, min_pitch_deg=min_pitch, - plan_grasp=planner or _fake_planner([]), log=_Log()) + plan_grasp=planner or _fake_planner([]), log=_Log(), + gripper_name='magpie') def _contact(pose): @@ -204,7 +205,7 @@ def test_graspgenx_raw_executes_server_order_untouched(): np.testing.assert_allclose(pose_to_matrix(g.pose), pose_to_matrix(p), atol=1e-9) assert cands.scores == sorted(cands.scores, reverse=True) - assert planner.calls[0] == dict(frame='pelvis', arm='right') + assert planner.calls[0] == dict(frame='pelvis', arm='right', gripper_name='magpie') def test_graspgenx_filtered_rerolls_and_drops_cross_body(): diff --git a/core_ws/src/golem_skills/test/test_robots.py b/core_ws/src/golem_skills/test/test_robots.py new file mode 100644 index 00000000..ee0a9175 --- /dev/null +++ b/core_ws/src/golem_skills/test/test_robots.py @@ -0,0 +1,36 @@ +"""Test the per-robot profiles the manipulation skills read (robots.py).""" + +import pytest + +from golem_skills.robots import ARMS, DEFAULT_ROBOT, ROBOTS, robot_profile + + +def test_default_robot_is_the_h12(): + """A node started without `robot` drives the H1-2, as before the parameter existed.""" + assert DEFAULT_ROBOT == 'h12' + p = robot_profile(DEFAULT_ROBOT) + assert p.grasp_frames == {'left': 'left_graspgenx_frame', + 'right': 'right_graspgenx_frame'} + assert p.pinch_frames == {'left': 'left_grasp_frame', 'right': 'right_grasp_frame'} + assert p.prep_config == 'prep' + assert p.staging_configs == {'left': 'staging_left', 'right': 'staging_right'} + assert p.graspgen_gripper == 'magpie' + + +def test_robot_names_are_case_and_space_insensitive(): + """' H12 ' selects the h12 profile.""" + assert robot_profile(' H12 ') is ROBOTS['h12'] + + +def test_unknown_robot_is_refused(): + """An unknown name raises ValueError naming the robots there are.""" + with pytest.raises(ValueError, match='unknown robot'): + robot_profile('h1') + + +@pytest.mark.parametrize('name', sorted(ROBOTS)) +def test_every_profile_covers_both_arms(name): + """Every per-arm table has exactly the arms a goal may name.""" + p = ROBOTS[name] + for table in (p.grasp_frames, p.pinch_frames, p.staging_configs): + assert tuple(sorted(table)) == tuple(sorted(ARMS)) From bd18915cc42f51f1221eb7dfd3f21484b973c0ac Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Fri, 20 Nov 2026 20:58:47 -0500 Subject: [PATCH 2/8] golem_skills: keep /joint_states per joint, merging its publishers SkillsBase kept the last /joint_states message, so with two publishers (golem_upper_ik's body joints and, on the G1, dex_3's finger joints) the grasp reach check could start its IK from a message without the arm joints. Each joint now keeps its latest position and arrival time; latest_joint_state() returns them all as one JointState, and joint_positions_since() waits for named joints published after a time (the Dex3-1 grip check's input). On the H1-2, with one publisher, the reach check sees the same joints as before. Co-Authored-By: Claude Opus 5.5 --- core_ws/src/golem_skills/golem_skills/base.py | 50 +++++++++++++--- .../golem_skills/test/test_joint_states.py | 59 +++++++++++++++++++ 2 files changed, 102 insertions(+), 7 deletions(-) create mode 100644 core_ws/src/golem_skills/test/test_joint_states.py diff --git a/core_ws/src/golem_skills/golem_skills/base.py b/core_ws/src/golem_skills/golem_skills/base.py index 2077a3cd..4d4f6aa1 100644 --- a/core_ws/src/golem_skills/golem_skills/base.py +++ b/core_ws/src/golem_skills/golem_skills/base.py @@ -350,9 +350,14 @@ def __init__(self, node_name='golem_skills'): # mm, the aperture the driver reported after the last set_gripper per # arm; gripper_aperture's fallback before any state message self._gripper_actual_mm = {} - # the latest /joint_states (golem_upper_ik's joint_state_publisher, the - # body joints from /lowstate); the grasp reach check starts its IK there - self._joint_state = None + # joint name -> (position [rad], robot time it arrived [s]) from every + # /joint_states message. More than one node publishes there, each its + # own joints (golem_upper_ik's joint_state_publisher the body from + # /lowstate; on the G1 dex_3's driver the fingers too), so the joints + # are kept per name rather than as the last message. The grasp reach + # check starts its IK at them. + self._joints = {} + self._joints_lock = threading.Lock() self.joint_state_sub = self.create_subscription( JointState, '/joint_states', self._on_joint_state, 10, callback_group=self._cb_group) @@ -1330,12 +1335,43 @@ def _on_gripper_state(self, arm, msg): self._gripper_state_t[arm] = self.robot_now() def _on_joint_state(self, msg): - """Keep the latest /joint_states message.""" - self._joint_state = msg + """Keep each joint's latest /joint_states position and when it arrived.""" + t = self.robot_now() + with self._joints_lock: + for name, position in zip(msg.name, msg.position): + self._joints[name] = (float(position), t) def latest_joint_state(self): - """Return the latest /joint_states message, or None before the first.""" - return self._joint_state + """ + Return every joint heard on /joint_states as one JointState, or None before the first. + + Each joint has its latest position, whichever publisher sent it. + """ + with self._joints_lock: + if not self._joints: + return None + msg = JointState() + msg.name = list(self._joints) + msg.position = [p for p, _ in self._joints.values()] + return msg + + def joint_positions_since(self, names, since, wait_sec): + """ + Return {name: position [rad]} of `names`, each from a message after `since`, or None. + + `since` and `wait_sec` are s of robot time; waits up to `wait_sec` for + every one of `names` to be published after `since`. None when one is + not, e.g. its publisher is down. + """ + deadline = self.robot_now() + wait_sec + while True: + with self._joints_lock: + got = {n: self._joints.get(n) for n in names} + if all(v is not None and v[1] > since for v in got.values()): + return {n: v[0] for n, v in got.items()} + if self.robot_now() >= deadline: + return None + self._sleep_robot(ROBOT_WAIT_POLL_SEC, threading.Event()) def gripper_aperture_since(self, arm, since, wait_sec): """ diff --git a/core_ws/src/golem_skills/test/test_joint_states.py b/core_ws/src/golem_skills/test/test_joint_states.py new file mode 100644 index 00000000..d63bde54 --- /dev/null +++ b/core_ws/src/golem_skills/test/test_joint_states.py @@ -0,0 +1,59 @@ +"""Test that SkillsBase keeps /joint_states per joint, merging its publishers.""" + +import pytest +import rclpy +from sensor_msgs.msg import JointState + +from golem_skills import base +from test_heartbeat_halt import FakeClock +from test_robot_time import _node + + +def _js(names, positions): + """Return a JointState of `names` at `positions`.""" + msg = JointState() + msg.name = list(names) + msg.position = [float(p) for p in positions] + return msg + + +@pytest.fixture +def node(tmp_path, monkeypatch): + """Yield (a SkillsBase without use_sim_time, the fake time.monotonic it reads).""" + n = _node(tmp_path, False) + steady = FakeClock() + monkeypatch.setattr(base.time, 'monotonic', steady) + yield n, steady + n.destroy_node() + rclpy.shutdown() + + +def test_two_publishers_merge(node): + """The body's and the hand driver's messages both stay in the joint state.""" + n, _ = node + assert n.latest_joint_state() is None + n._on_joint_state(_js(['left_elbow_joint', 'right_elbow_joint'], [0.5, -0.5])) + n._on_joint_state(_js(['left_hand_index_0_joint'], [-0.3])) + js = n.latest_joint_state() + assert dict(zip(js.name, js.position)) == { + 'left_elbow_joint': 0.5, 'right_elbow_joint': -0.5, 'left_hand_index_0_joint': -0.3} + # a later message updates its own joints only + n._on_joint_state(_js(['left_elbow_joint'], [0.7])) + js = n.latest_joint_state() + assert dict(zip(js.name, js.position))['left_elbow_joint'] == 0.7 + assert dict(zip(js.name, js.position))['right_elbow_joint'] == -0.5 + + +def test_joint_positions_since_wants_every_joint_newer(node): + """Only positions published after `since` count, and every named joint must have one.""" + n, steady = node + n._on_joint_state(_js(['a', 'b'], [1.0, 2.0])) + since = n.robot_now() + steady.advance(0.1) + n._on_joint_state(_js(['a'], [1.5])) + assert n.joint_positions_since(['a', 'b'], since, 0.0) is None + assert n.joint_positions_since(['a'], since, 0.0) == {'a': 1.5} + assert n.joint_positions_since(['c'], since, 0.0) is None + steady.advance(0.1) + n._on_joint_state(_js(['b'], [2.5])) + assert n.joint_positions_since(['a', 'b'], since, 0.0) == {'a': 1.5, 'b': 2.5} From 4678afa2ae1e1cf3da1efd23b04183c1bb6c72e1 Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Fri, 20 Nov 2026 21:02:12 -0500 Subject: [PATCH 3/8] golem_skills: open, close and grip-check a Dex3-1 hand RobotProfile.hand names the hand a robot grasps with: 'magpie' (the H1-2, unchanged) or 'dex3', dex_3's driver. With dex3 the node makes no Magpie clients; open_gripper and close_gripper call //hand/open and //hand/close, with no force limit and no grip-hold loop (the driver keeps commanding the pose). The grasp's grip check reads how far the six closing finger joints travelled from dex_3's open pose to its close pose (hands.dex3_grip_failure): all of them at 0.9 or more is a close on nothing, the index and middle fingers' mean under 0.2 a close that did not happen. The thresholds are chosen, not measured. golem_skills now exec-depends on dex_3 for its poses. Co-Authored-By: Claude Opus 5.5 --- core_ws/src/golem_skills/golem_skills/base.py | 50 ++++++++++--- .../src/golem_skills/golem_skills/hands.py | 75 +++++++++++++++++++ .../src/golem_skills/golem_skills/robots.py | 3 + .../golem_skills/golem_skills/skills/grasp.py | 31 ++++++-- core_ws/src/golem_skills/package.xml | 1 + core_ws/src/golem_skills/test/test_hands.py | 47 ++++++++++++ 6 files changed, 192 insertions(+), 15 deletions(-) create mode 100644 core_ws/src/golem_skills/golem_skills/hands.py create mode 100644 core_ws/src/golem_skills/test/test_hands.py diff --git a/core_ws/src/golem_skills/golem_skills/base.py b/core_ws/src/golem_skills/golem_skills/base.py index 4d4f6aa1..c50974ed 100644 --- a/core_ws/src/golem_skills/golem_skills/base.py +++ b/core_ws/src/golem_skills/golem_skills/base.py @@ -65,6 +65,7 @@ ) from .grasp_synthesis import DEFAULT_METHOD, METHODS, method_name from model_server.model_logging import _log_root +from .hands import DEX3_NS from .robots import ARMS, DEFAULT_ROBOT, ROBOTS, robot_profile from .run_telemetry import RunRecorder, describe_goal, run_dir @@ -308,30 +309,33 @@ def __init__(self, node_name='golem_skills'): read_only=True)).value) self.get_logger().info(f'golem_skills: robot {self.robot.name}') + # The hand the robot grasps with (hands.py): the Magpie gripper's + # services below, or the Dex3-1 driver's open and close. + magpie_ns = GRIPPER_NS if self.robot.hand == 'magpie' else {} self.gripper_clis = { arm: self.create_client(SetGripperPosition, f'{ns}/set_position', callback_group=self._cb_group) - for arm, ns in GRIPPER_NS.items() + for arm, ns in magpie_ns.items() } self.gripper_open_clis = { arm: self.create_client(Trigger, f'{ns}/open', callback_group=self._cb_group) - for arm, ns in GRIPPER_NS.items() + for arm, ns in magpie_ns.items() } self.gripper_close_clis = { arm: self.create_client(Trigger, f'{ns}/close', callback_group=self._cb_group) - for arm, ns in GRIPPER_NS.items() + for arm, ns in magpie_ns.items() } self.gripper_force_clis = { arm: self.create_client(SetGripperForce, f'{ns}/set_force', callback_group=self._cb_group) - for arm, ns in GRIPPER_NS.items() + for arm, ns in magpie_ns.items() } # clears a Dynamixel overload without opening the fingers; the grip-hold # loop's other call self.gripper_reset_clis = { arm: self.create_client(Trigger, f'{ns}/reset_overload', callback_group=self._cb_group) - for arm, ns in GRIPPER_NS.items() + for arm, ns in magpie_ns.items() } # arm -> (thread, stop Event) of the running grip-hold loop self._grip_hold = {} @@ -345,11 +349,21 @@ def __init__(self, node_name='golem_skills'): GripperState, f'{ns}/state', lambda msg, a=arm: self._on_gripper_state(a, msg), 10, callback_group=self._cb_group) - for arm, ns in GRIPPER_NS.items() + for arm, ns in magpie_ns.items() } # mm, the aperture the driver reported after the last set_gripper per # arm; gripper_aperture's fallback before any state message self._gripper_actual_mm = {} + # the Dex3-1 driver's named poses, per arm + dex3_ns = DEX3_NS if self.robot.hand == 'dex3' else {} + self.hand_open_clis = { + arm: self.create_client(Trigger, f'{ns}/open', callback_group=self._cb_group) + for arm, ns in dex3_ns.items() + } + self.hand_close_clis = { + arm: self.create_client(Trigger, f'{ns}/close', callback_group=self._cb_group) + for arm, ns in dex3_ns.items() + } # joint name -> (position [rad], robot time it arrived [s]) from every # /joint_states message. More than one node publishes there, each its # own joints (golem_upper_ik's joint_state_publisher the body from @@ -463,8 +477,8 @@ def __init__(self, node_name='golem_skills'): lambda msg, a=arm: self._on_arm_detections(a, msg), qos_profile_sensor_data, callback_group=self._cb_group) - # With `wait_for_services`, wait for the H1-2's perception and gripper - # services and the arm actions, 10 s in total; a missing one is not fatal. + # With `wait_for_services`, wait for the perception and hand services + # and the arm actions, 10 s in total; a missing one is not fatal. wait_for_services = bool( self.declare_parameter('wait_for_services', True).value) if wait_for_services: @@ -473,7 +487,8 @@ def __init__(self, node_name='golem_skills'): for cli in (self.gemini_cli, self.sam_cli, self.graspgen_cli, *self.gripper_clis.values(), *self.gripper_open_clis.values(), *self.gripper_close_clis.values(), - *self.gripper_force_clis.values()): + *self.gripper_force_clis.values(), + *self.hand_open_clis.values(), *self.hand_close_clis.values()): rem = max(0.0, deadline - time.monotonic()) if rem <= 0.0: break @@ -1447,8 +1462,14 @@ def open_gripper(self, arm, amount=1.0): A position command, so partial openings hold, followed by GRIPPER_SETTLE_SEC so the fingers stop before the arm moves. Opening - ends the arm's grip-hold loop (through set_gripper). + ends the arm's grip-hold loop (through set_gripper). A Dex3-1 opens + fully whatever `amount` (its driver's `open` pose). """ + if self.robot.hand == 'dex3': + if not self._trigger_gripper(self.hand_open_clis[arm], f'hand open({arm})'): + return False + self.get_clock().sleep_for(RclpyDuration(seconds=GRIPPER_SETTLE_SEC)) + return True amount = min(max(float(amount), 0.0), 1.0) if not self.set_gripper(arm, amount * GRIPPER_MAX_WIDTH_MM): return False @@ -1465,7 +1486,16 @@ def close_gripper(self, arm, force_n=GRIP_FORCE_N): returns before the fingers finish, then starts the grip-hold loop (_grip_hold_loop), which keeps running after this call and after the skill returns, until the gripper is opened. Returns success. + + A Dex3-1 has no force limit or overload: its driver's `close` pose, + which the driver keeps commanding, then GRIPPER_SETTLE_SEC; no + grip-hold loop. """ + if self.robot.hand == 'dex3': + if not self._trigger_gripper(self.hand_close_clis[arm], f'hand close({arm})'): + return False + self.get_clock().sleep_for(RclpyDuration(seconds=GRIPPER_SETTLE_SEC)) + return True if not self.set_gripper_force(arm, force_n): return False if not self._trigger_gripper(self.gripper_close_clis[arm], f'close({arm})'): diff --git a/core_ws/src/golem_skills/golem_skills/hands.py b/core_ws/src/golem_skills/golem_skills/hands.py new file mode 100644 index 00000000..da3a1318 --- /dev/null +++ b/core_ws/src/golem_skills/golem_skills/hands.py @@ -0,0 +1,75 @@ +""" +Name the hands the grasp skills close, and judge a Dex3-1 grip from its finger joints. + +RobotProfile.hand (robots.py) is one of HANDS: + + magpie the H1-2's Magpie gripper (magpie_control and the H1-2 sims): + /set_position, set_force, open, close, reset_overload and the + /state aperture, ns = //gripper; SkillsBase drives it + dex3 the G1's Dex3-1 hand (dex_3's dex3_hands): //hand/open and + //hand/close (std_srvs/Trigger), and the finger joints on + /joint_states. No force limit and no aperture: the grip check reads + how far the fingers closed (dex3_grip_failure). + +This module imports no ROS package. +""" + +import numpy as np + +from dex_3.dex3 import JOINT_NAMES, named_pose + +HANDS = ('magpie', 'dex3') + +# the Dex3-1 driver's per-arm service namespace: /open, /close +DEX3_NS = {'left': '/left/hand', 'right': '/right/hand'} + +# Fraction of each closing joint's travel from `open` to `close` (dex_3's named poses) +# the fingers reached after a close: every one at or above DEX3_EMPTY_FRACTION means +# nothing stopped them (a close on nothing leaves the thumb about 1 cm from the finger +# tips); the index and middle fingers' mean below DEX3_NOT_CLOSED_FRACTION means they +# did not close at all. Between the two, something is held. Chosen, not measured: +# unmeasured on the real G1 and its sim. +DEX3_EMPTY_FRACTION = 0.9 +DEX3_NOT_CLOSED_FRACTION = 0.2 + + +def dex3_closing_joints(arm): + """Return the Dex3-1 joints of `arm` that move between `open` and `close`, in driver order.""" + travel = named_pose(arm, 'close') - named_pose(arm, 'open') + return [n for n, t in zip(JOINT_NAMES[arm], travel) if abs(t) > 1e-9] + + +def dex3_closure(arm, positions): + """ + Return {joint: fraction of its open -> close travel reached} for `arm`'s closing joints. + + `positions` maps joint names to rad (at least dex3_closing_joints(arm)). + 0 is open, 1 the `close` pose; a finger pushed back past open reads below 0. + """ + q_open = dict(zip(JOINT_NAMES[arm], named_pose(arm, 'open'))) + q_close = dict(zip(JOINT_NAMES[arm], named_pose(arm, 'close'))) + return {n: float((positions[n] - q_open[n]) / (q_close[n] - q_open[n])) + for n in dex3_closing_joints(arm)} + + +def dex3_grip_failure(arm, positions): + """ + Return why `arm`'s Dex3-1 holds nothing after a close, given its finger joints, else None. + + `positions` is {joint: rad} after the close, or None when no finger state arrived. + """ + if positions is None: + return 'no hand joint state arrived after the close' + frac = dex3_closure(arm, positions) + fingers = [f for n, f in frac.items() if '_index_' in n or '_middle_' in n] + if float(np.mean(fingers)) < DEX3_NOT_CLOSED_FRACTION: + return f'the fingers did not close ({dex3_closure_text(frac)})' + if min(frac.values()) >= DEX3_EMPTY_FRACTION: + return f'the hand closed on nothing ({dex3_closure_text(frac)})' + return None + + +def dex3_closure_text(frac): + """Return the closure fractions as a short log string, e.g. 'thumb_1 0.95, index_0 0.40'.""" + return ', '.join(f'{n.split("_hand_", 1)[-1].removesuffix("_joint")} {f:.2f}' + for n, f in frac.items()) diff --git a/core_ws/src/golem_skills/golem_skills/robots.py b/core_ws/src/golem_skills/golem_skills/robots.py index 03881b1f..6b895a0e 100644 --- a/core_ws/src/golem_skills/golem_skills/robots.py +++ b/core_ws/src/golem_skills/golem_skills/robots.py @@ -36,6 +36,8 @@ class RobotProfile: staging_configs: dict # gripper_name the graspgen service plans for; None: no GraspGenX model graspgen_gripper: Optional[str] + # the hand the grasp closes, one of hands.HANDS + hand: str H12 = RobotProfile( @@ -46,6 +48,7 @@ class RobotProfile: prep_config='prep', staging_configs={'left': 'staging_left', 'right': 'staging_right'}, graspgen_gripper='magpie', + hand='magpie', ) ROBOTS = {p.name: p for p in (H12,)} diff --git a/core_ws/src/golem_skills/golem_skills/skills/grasp.py b/core_ws/src/golem_skills/golem_skills/skills/grasp.py index 7485d806..967f0541 100644 --- a/core_ws/src/golem_skills/golem_skills/skills/grasp.py +++ b/core_ws/src/golem_skills/golem_skills/skills/grasp.py @@ -11,7 +11,7 @@ in along the grasp axis (or the visual servo); a candidate whose pre-grasp or contact is not reached is dropped and the next one tried - -> force close -> check the gripper holds something + -> close -> check the hand holds something Perception and execution are the same for every synthesis method; only the candidate list differs (grasp_synthesis/__init__.py has the methods and how a @@ -41,6 +41,8 @@ WORLD_FRAME) from ..grasp_synthesis import METHODS, SynthesisInput, method_name from ..grasp_synthesis.common import pose_at +from ..hands import (dex3_closing_joints, dex3_closure, dex3_closure_text, + dex3_grip_failure) from ..perception_utils import (approach_pose, pose_quat, pose_xyz, quat_geodesic, quat_slerp) from .grasp_detection import GraspDetection, perception_name @@ -100,7 +102,8 @@ # wider than this cannot be grasped. OPEN_GRIP_MIN_MM = 90.0 # s of robot time to wait for a gripper state published after the close (the -# driver and the sims publish at 10 Hz) +# Magpie driver and the H1-2 sims publish at 10 Hz, dex_3's driver its finger +# joints at 50 Hz) GRIP_STATE_WAIT_SEC = 1.0 @@ -397,8 +400,7 @@ def fail(message): if not self.close_gripper(arm): return fail('gripper close failed') self.get_clock().sleep_for(RclpyDuration(seconds=POST_CLOSE_SETTLE_SEC)) - aperture = self.gripper_aperture_since(arm, closed_at, GRIP_STATE_WAIT_SEC) - reason = grip_failure(aperture) + reason, held = self._grip_check(arm, closed_at) if reason is not None: return fail(f'grasp of {obj!r} with {method.name} candidate ' f'{cands.labels[contact.idx]} failed: {reason}') @@ -415,7 +417,26 @@ def fail(message): return run.succeed( f'grasped {obj!r} with {method.name} candidate ' f'{cands.labels[contact.idx]} (score {cands.scores[contact.idx]:.2f}, ' - f'gripper at {aperture:.1f} mm)') + f'{held})') + + def _grip_check(self, arm, closed_at): + """ + Return (why `arm`'s hand holds nothing, or None; its state for the result message). + + `closed_at` is the robot time the close was sent; only hand state + published after it counts. The Magpie's aperture (grip_failure), or + how far the Dex3-1's fingers closed (hands.dex3_grip_failure). + """ + if self.robot.hand == 'dex3': + positions = self.joint_positions_since( + dex3_closing_joints(arm), closed_at, GRIP_STATE_WAIT_SEC) + reason = dex3_grip_failure(arm, positions) + held = ('fingers closed ' + dex3_closure_text(dex3_closure(arm, positions)) + if positions is not None else '') + return reason, held + aperture = self.gripper_aperture_since(arm, closed_at, GRIP_STATE_WAIT_SEC) + reason = grip_failure(aperture) + return reason, (f'gripper at {aperture:.1f} mm' if aperture is not None else '') def _resolve_grasp_method(self, goal): """Return (GraspMethod, None) for the goal, or (None, reason) for an unknown name.""" diff --git a/core_ws/src/golem_skills/package.xml b/core_ws/src/golem_skills/package.xml index f100a1ea..f1fe4130 100644 --- a/core_ws/src/golem_skills/package.xml +++ b/core_ws/src/golem_skills/package.xml @@ -10,6 +10,7 @@ rclpy model_server golem_upper_ik + dex_3 rcl_interfaces custom_ros_messages magpie_msgs diff --git a/core_ws/src/golem_skills/test/test_hands.py b/core_ws/src/golem_skills/test/test_hands.py new file mode 100644 index 00000000..0afd3d37 --- /dev/null +++ b/core_ws/src/golem_skills/test/test_hands.py @@ -0,0 +1,47 @@ +"""Test the Dex3-1 grip check (hands.py) on joint positions built from dex_3's poses.""" + +import pytest + +from dex_3.dex3 import JOINT_NAMES, named_pose +from golem_skills.hands import (DEX3_EMPTY_FRACTION, dex3_closing_joints, dex3_closure, + dex3_grip_failure) + + +def _at(arm, fraction, override=None): + """Return {joint: rad} `fraction` of the way from open to close, with `override` fractions.""" + q_open, q_close = named_pose(arm, 'open'), named_pose(arm, 'close') + out = {} + for name, o, c in zip(JOINT_NAMES[arm], q_open, q_close): + f = (override or {}).get(name.split('_hand_')[1], fraction) + out[name] = float(o + f * (c - o)) + return out + + +@pytest.mark.parametrize('arm', ['left', 'right']) +def test_thumb_rotation_is_not_a_closing_joint(arm): + """thumb_0 stays at 0 in both poses, so the check leaves it out; the other six count.""" + joints = dex3_closing_joints(arm) + assert f'{arm}_hand_thumb_0_joint' not in joints + assert len(joints) == 6 + + +@pytest.mark.parametrize('arm', ['left', 'right']) +def test_closure_reads_zero_open_and_one_closed(arm): + """The open pose reads 0 and the close pose 1 on every closing joint, both hands.""" + assert all(abs(f) < 1e-9 for f in dex3_closure(arm, _at(arm, 0.0)).values()) + assert all(abs(f - 1.0) < 1e-9 for f in dex3_closure(arm, _at(arm, 1.0)).values()) + + +@pytest.mark.parametrize('arm', ['left', 'right']) +def test_grip_verdicts(arm): + """Nothing in the way is empty, fingers stopped short hold, fingers that stayed open failed.""" + assert dex3_grip_failure(arm, None) == 'no hand joint state arrived after the close' + assert 'closed on nothing' in dex3_grip_failure(arm, _at(arm, 1.0)) + assert 'closed on nothing' in dex3_grip_failure(arm, _at(arm, DEX3_EMPTY_FRACTION)) + assert 'did not close' in dex3_grip_failure(arm, _at(arm, 0.05)) + # an object stops the index and middle fingers half way; the thumb closes fully + held = _at(arm, 1.0, {'index_0_joint': 0.5, 'index_1_joint': 0.6, + 'middle_0_joint': 0.5, 'middle_1_joint': 0.6}) + assert dex3_grip_failure(arm, held) is None + # one finger stopped by the object is enough + assert dex3_grip_failure(arm, _at(arm, 1.0, {'thumb_2_joint': 0.4})) is None From 96ac99d008ca6c1041a0f47d8ade52338d99b4cb Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Fri, 20 Nov 2026 21:04:11 -0500 Subject: [PATCH 4/8] golem_skills: refuse grasp options the robot lacks, before moving A grasp goal now aborts before the arm moves when its method plans with GraspGenX (GraspMethod.graspgen) and the robot's profile names no GraspGenX gripper (graspgen_gripper None), or when it asks for visual_servo and the profile has no hand cameras (hand_cameras). The message names the methods that do run. graspgenx._plan also refuses a missing gripper model rather than call the service. The H1-2 has both, so nothing changes there. Co-Authored-By: Claude Opus 5.5 --- .../golem_skills/grasp_synthesis/__init__.py | 8 ++++-- .../golem_skills/grasp_synthesis/common.py | 5 ++-- .../golem_skills/grasp_synthesis/graspgenx.py | 2 ++ .../src/golem_skills/golem_skills/robots.py | 4 +++ .../golem_skills/golem_skills/skills/grasp.py | 19 +++++++++++++ .../golem_skills/test/test_grasp_synthesis.py | 16 +++++++++++ core_ws/src/golem_skills/test/test_robots.py | 28 +++++++++++++++++++ 7 files changed, 78 insertions(+), 4 deletions(-) diff --git a/core_ws/src/golem_skills/golem_skills/grasp_synthesis/__init__.py b/core_ws/src/golem_skills/golem_skills/grasp_synthesis/__init__.py index f977816a..f975a23a 100644 --- a/core_ws/src/golem_skills/golem_skills/grasp_synthesis/__init__.py +++ b/core_ws/src/golem_skills/golem_skills/grasp_synthesis/__init__.py @@ -44,6 +44,9 @@ class GraspMethod: # True: the executor runs its reach check (skills/grasp_reach.py) on each # candidate before moving the arm, and skips the ones that fail it reach_check: bool = False + # True: the method plans with the graspgen service, so it runs only on a + # robot whose gripper GraspGenX has a model of (RobotProfile.graspgen_gripper) + graspgen: bool = False METHODS = {m.name: m for m in ( @@ -51,9 +54,10 @@ class GraspMethod: GraspMethod('top_down', top_down.synthesize, staged=True), GraspMethod('centroid', centroid.synthesize), GraspMethod('antipodal', antipodal.synthesize), - GraspMethod('graspgenx', graspgenx.synthesize_raw, reach_check=True), + GraspMethod('graspgenx', graspgenx.synthesize_raw, reach_check=True, + graspgen=True), GraspMethod('graspgenx_filtered', graspgenx.synthesize_filtered, - reach_check=True), + reach_check=True, graspgen=True), )} DEFAULT_METHOD = 'heuristic' diff --git a/core_ws/src/golem_skills/golem_skills/grasp_synthesis/common.py b/core_ws/src/golem_skills/golem_skills/grasp_synthesis/common.py index fd582365..fee05805 100644 --- a/core_ws/src/golem_skills/golem_skills/grasp_synthesis/common.py +++ b/core_ws/src/golem_skills/golem_skills/grasp_synthesis/common.py @@ -43,8 +43,9 @@ class SynthesisInput: plan_grasp: Callable # rclpy logger log: object - # gripper_name the graspgen service plans for (robots.RobotProfile.graspgen_gripper) - gripper_name: str + # gripper_name the graspgen service plans for (robots.RobotProfile.graspgen_gripper); + # None on a robot GraspGenX has no gripper model of + gripper_name: Optional[str] @property def centroid(self): diff --git a/core_ws/src/golem_skills/golem_skills/grasp_synthesis/graspgenx.py b/core_ws/src/golem_skills/golem_skills/grasp_synthesis/graspgenx.py index 63f18331..385e7d67 100644 --- a/core_ws/src/golem_skills/golem_skills/grasp_synthesis/graspgenx.py +++ b/core_ws/src/golem_skills/golem_skills/grasp_synthesis/graspgenx.py @@ -40,6 +40,8 @@ def _plan(inp): """Return (graspgen response, None), or (None, reason) when the service gives none.""" + if inp.gripper_name is None: + return None, "GraspGenX has no model of this robot's gripper" resp = inp.plan_grasp(inp.cloud, gripper_name=inp.gripper_name, frame='pelvis', scene_cloud=inp.scene, arm=inp.arm) if resp is None: diff --git a/core_ws/src/golem_skills/golem_skills/robots.py b/core_ws/src/golem_skills/golem_skills/robots.py index 6b895a0e..aa012733 100644 --- a/core_ws/src/golem_skills/golem_skills/robots.py +++ b/core_ws/src/golem_skills/golem_skills/robots.py @@ -38,6 +38,9 @@ class RobotProfile: graspgen_gripper: Optional[str] # the hand the grasp closes, one of hands.HANDS hand: str + # whether each hand carries a camera (yolo_server's hand-camera bundles), + # which goal.visual_servo needs + hand_cameras: bool H12 = RobotProfile( @@ -49,6 +52,7 @@ class RobotProfile: staging_configs={'left': 'staging_left', 'right': 'staging_right'}, graspgen_gripper='magpie', hand='magpie', + hand_cameras=True, ) ROBOTS = {p.name: p for p in (H12,)} diff --git a/core_ws/src/golem_skills/golem_skills/skills/grasp.py b/core_ws/src/golem_skills/golem_skills/skills/grasp.py index 967f0541..dfd6727b 100644 --- a/core_ws/src/golem_skills/golem_skills/skills/grasp.py +++ b/core_ws/src/golem_skills/golem_skills/skills/grasp.py @@ -314,6 +314,9 @@ def _grasp(self, gh): if arm is None and goal.arm.strip().lower() not in ('', 'none'): return run.abort(f'invalid arm {goal.arm!r}') method, err = self._resolve_grasp_method(goal) + if err: + return run.abort(err) + err = self._robot_refusal(method, goal) if err: return run.abort(err) obj = perception_name(goal.target_object) @@ -448,6 +451,22 @@ def _resolve_grasp_method(self, goal): f'{sorted(METHODS)}') return method, None + def _robot_refusal(self, method, goal): + """ + Return why this robot cannot run `method` or the goal's options, or None. + + Checked before the arm moves: a GraspGenX method needs a gripper + GraspGenX has a model of, and goal.visual_servo needs hand cameras. + """ + name = self.robot.name + if method.graspgen and self.robot.graspgen_gripper is None: + return (f'grasp_method {method.name!r} plans with GraspGenX, which has ' + f'no model of the {name} hand; use one of ' + f'{sorted(m for m, v in METHODS.items() if not v.graspgen)}') + if goal.visual_servo and not self.robot.hand_cameras: + return f'visual_servo needs hand cameras, and the {name} has none' + return None + def get_grasp_candidates(self, method, goal, obj, arm, obj_cloud, scene): """Run `method` on the segmented object; return (Candidates, None) or (None, reason).""" inp = SynthesisInput( diff --git a/core_ws/src/golem_skills/test/test_grasp_synthesis.py b/core_ws/src/golem_skills/test/test_grasp_synthesis.py index bd8e88b5..99821a23 100644 --- a/core_ws/src/golem_skills/test/test_grasp_synthesis.py +++ b/core_ws/src/golem_skills/test/test_grasp_synthesis.py @@ -267,3 +267,19 @@ def no_plan(*_, **__): cands, err = METHODS[name].synthesize( _input(_box_cloud(OBJ, (0.04, 0.04, 0.04)), planner=no_plan)) assert cands is None and 'no grasp planned' in err + + +@pytest.mark.parametrize('name', ['graspgenx', 'graspgenx_filtered']) +def test_graspgenx_without_a_gripper_model_plans_nothing(name): + """With no GraspGenX gripper for the robot, the service is not called and the method fails.""" + planner = _fake_planner(_planner_pool()) + inp = _input(_box_cloud(OBJ, (0.04, 0.04, 0.04)), planner=planner) + inp.gripper_name = None + cands, err = METHODS[name].synthesize(inp) + assert cands is None and 'no model' in err + assert planner.calls == [] + + +def test_only_the_graspgenx_methods_need_graspgen(): + """The heuristic methods run on any robot; the two GraspGenX ones need its model.""" + assert {n for n, m in METHODS.items() if m.graspgen} == {'graspgenx', 'graspgenx_filtered'} diff --git a/core_ws/src/golem_skills/test/test_robots.py b/core_ws/src/golem_skills/test/test_robots.py index ee0a9175..6329dc91 100644 --- a/core_ws/src/golem_skills/test/test_robots.py +++ b/core_ws/src/golem_skills/test/test_robots.py @@ -1,5 +1,8 @@ """Test the per-robot profiles the manipulation skills read (robots.py).""" +from dataclasses import replace +from types import SimpleNamespace + import pytest from golem_skills.robots import ARMS, DEFAULT_ROBOT, ROBOTS, robot_profile @@ -34,3 +37,28 @@ def test_every_profile_covers_both_arms(name): p = ROBOTS[name] for table in (p.grasp_frames, p.pinch_frames, p.staging_configs): assert tuple(sorted(table)) == tuple(sorted(ARMS)) + + +def _refusal(profile, method, visual_servo=False): + """Return GraspSkill._robot_refusal for `profile` on `method` and a goal's visual_servo.""" + from golem_skills.grasp_synthesis import METHODS + from golem_skills.skills.grasp import GraspSkill + node = SimpleNamespace(robot=profile) + goal = SimpleNamespace(visual_servo=visual_servo) + return GraspSkill._robot_refusal(node, METHODS[method], goal) + + +def test_the_h12_refuses_nothing(): + """The H1-2 runs GraspGenX and the visual servo.""" + h12 = robot_profile('h12') + assert _refusal(h12, 'graspgenx_filtered', visual_servo=True) is None + + +def test_a_robot_without_graspgen_or_hand_cameras_is_refused_before_moving(): + """No GraspGenX gripper model refuses its methods; no hand cameras refuse visual_servo.""" + bare = replace(robot_profile('h12'), name='bare', graspgen_gripper=None, + hand_cameras=False) + assert 'GraspGenX' in _refusal(bare, 'graspgenx') + assert 'antipodal' in _refusal(bare, 'graspgenx_filtered') + assert _refusal(bare, 'antipodal') is None + assert 'hand cameras' in _refusal(bare, 'top_down', visual_servo=True) From 86f79033995840d97ec286b16457bdef2d6397fc Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Fri, 20 Nov 2026 21:06:43 -0500 Subject: [PATCH 5/8] golem_skills: a G1 profile, grasping with the Dex3-1 from 'home' robot:=g1 selects it: the Dex3-1 hand (dex_3's driver), golem_upper_ik's 'home' as prep and staging (the G1 has neither), no GraspGenX gripper model and no hand cameras. The G1's URDF has no grasp frame, so the profile defines tool frames on _hand_palm_link: a grasp frame in the GraspGenX convention (+Z the approach, along the palm face's normal; +X the closing axis, the palm's +X; the finger contact point 0.1146 m along +Z) and a pinch frame at that contact point, 80 mm along the palm and 40 mm in front of its face, from the URDF's joint origins and not measured. SkillsBase broadcasts the tool frames on /tf_static and drives a tool frame's frame_task goal through its parent link (robots.drive_pose). The H1-2 has no tool frames, so its goals are unchanged. Co-Authored-By: Claude Opus 5.5 --- core_ws/src/golem_skills/golem_skills/base.py | 41 ++++++++- .../src/golem_skills/golem_skills/robots.py | 91 ++++++++++++++++++- core_ws/src/golem_skills/test/test_g1_node.py | 76 ++++++++++++++++ core_ws/src/golem_skills/test/test_robots.py | 70 +++++++++++++- 4 files changed, 271 insertions(+), 7 deletions(-) create mode 100644 core_ws/src/golem_skills/test/test_g1_node.py diff --git a/core_ws/src/golem_skills/golem_skills/base.py b/core_ws/src/golem_skills/golem_skills/base.py index c50974ed..2d5b4847 100644 --- a/core_ws/src/golem_skills/golem_skills/base.py +++ b/core_ws/src/golem_skills/golem_skills/base.py @@ -39,12 +39,13 @@ from action_msgs.msg import GoalStatus from builtin_interfaces.msg import Duration -from geometry_msgs.msg import Point, Pose, Quaternion, Vector3Stamped +from geometry_msgs.msg import Point, Pose, Quaternion, TransformStamped, Vector3Stamped from sensor_msgs.msg import CameraInfo, CompressedImage, Image, JointState from std_msgs.msg import Bool, Header from sensor_msgs_py import point_cloud2 -from tf2_ros import Buffer, TransformBroadcaster, TransformListener, TransformException +from tf2_ros import (Buffer, StaticTransformBroadcaster, TransformBroadcaster, + TransformListener, TransformException) from custom_ros_messages.msg import DetectionBundle from custom_ros_messages.srv import GeminiQuery, SamSegment, GraspGen @@ -66,7 +67,7 @@ from .grasp_synthesis import DEFAULT_METHOD, METHODS, method_name from model_server.model_logging import _log_root from .hands import DEX3_NS -from .robots import ARMS, DEFAULT_ROBOT, ROBOTS, robot_profile +from .robots import ARMS, DEFAULT_ROBOT, ROBOTS, drive_pose, robot_profile from .run_telemetry import RunRecorder, describe_goal, run_dir @@ -430,6 +431,12 @@ def __init__(self, node_name='golem_skills'): # (publish_tf, _republish_tfs) so they do not expire; the skill that # registered a frame drops it (drop_tf) when its goal ends. self.tf_broadcaster = TransformBroadcaster(self) + # The robot's tool frames (robots.RobotProfile.tool_frames: on the G1 + # the Dex3-1 grasp and pinch frames its URDF lacks), latched on + # /tf_static, so every TF lookup of a grasp or pinch frame works as it + # does on a URDF frame. + self._tool_tf = StaticTransformBroadcaster(self) + self._broadcast_tool_frames() # child_frame_id -> TransformStamped self._persistent_tfs = {} self._tf_lock = threading.Lock() @@ -566,6 +573,24 @@ def publish_tf(self, transform): self._persistent_tfs[transform.child_frame_id] = transform self.tf_broadcaster.sendTransform(transform) + def _broadcast_tool_frames(self): + """Send every tool frame of the robot's profile on /tf_static (none on the H1-2).""" + tfs = [] + stamp = self.get_clock().now().to_msg() + for name, tool in self.robot.tool_frames.items(): + t = TransformStamped() + t.header.stamp = stamp + t.header.frame_id = tool.parent + t.child_frame_id = name + p = matrix_to_pose(tool.matrix()) + t.transform.translation.x = p.position.x + t.transform.translation.y = p.position.y + t.transform.translation.z = p.position.z + t.transform.rotation = p.orientation + tfs.append(t) + if tfs: + self._tool_tf.sendTransform(tfs) + def drop_tf(self, child_frame_id): """Stop re-broadcasting a frame registered with publish_tf.""" with self._tf_lock: @@ -1106,6 +1131,10 @@ def _send_frame_move(self, frame, pose, duration_sec, outer_gh, do_plan, Reached means the goal succeeded (or stopped early as stable) AND the last streamed error is within (`lin_tol`, `ang_tol`). + A tool frame of the robot (robots.drive_pose) is driven through its + URDF parent at the pose that puts it at `pose`; the streamed errors + are then the parent's. + `stable_stop_msgs`: if set, cancel the goal and return once the streamed (lin, ang) feedback stays within FRAME_TASK_STABLE_EPS_* of its running reference for that many messages: the arm has settled and @@ -1116,8 +1145,12 @@ def _send_frame_move(self, frame, pose, duration_sec, outer_gh, do_plan, if halt is not None: self.get_logger().error(f'frame_task[{tag}] not sent: {halt}') return False + drive_frame, T = drive_pose(self.robot, frame, pose_to_matrix(pose)) + if drive_frame != frame: + tag = f'{tag} via {drive_frame}' + pose = matrix_to_pose(T) goal = FrameTask.Goal() - goal.frame_names = [frame] + goal.frame_names = [drive_frame] goal.frame_targets = [pose] goal.plan = do_plan goal.slow_mode = bool(slow_mode) diff --git a/core_ws/src/golem_skills/golem_skills/robots.py b/core_ws/src/golem_skills/golem_skills/robots.py index aa012733..122191cf 100644 --- a/core_ws/src/golem_skills/golem_skills/robots.py +++ b/core_ws/src/golem_skills/golem_skills/robots.py @@ -8,14 +8,35 @@ This module imports no ROS package. """ -from dataclasses import dataclass +from dataclasses import dataclass, field from typing import Optional +import numpy as np + # the arms a goal may name, and the keys of every per-arm table below ARMS = ('left', 'right') +@dataclass(frozen=True) +class ToolFrame: + """A frame the skills define on a URDF link the robot's model has: a fixed pose in it.""" + + # URDF frame it is fixed to, the one frame_task drives for it + parent: str + # m, its origin in `parent` + xyz: tuple + # its rotation in `parent`, a 3x3 matrix by rows (columns: its x, y, z axes) + rot: tuple + + def matrix(self): + """Return its 4x4 pose in `parent`.""" + T = np.eye(4) + T[:3, :3] = np.asarray(self.rot, dtype=float) + T[:3, 3] = np.asarray(self.xyz, dtype=float) + return T + + @dataclass(frozen=True) class RobotProfile: """Name what the grasp and pick_place skills drive on one robot.""" @@ -41,6 +62,10 @@ class RobotProfile: # whether each hand carries a camera (yolo_server's hand-camera bundles), # which goal.visual_servo needs hand_cameras: bool + # frame name -> ToolFrame, for grasp and pinch frames the robot's URDF does + # not have: SkillsBase broadcasts them as static TF and drives frame_task + # through their parent (drive_pose) + tool_frames: dict = field(default_factory=dict) H12 = RobotProfile( @@ -55,7 +80,56 @@ class RobotProfile: hand_cameras=True, ) -ROBOTS = {p.name: p for p in (H12,)} +# m from a grasp frame's origin to its finger contact point along its +Z: the +# GraspGenX convention's (grasp_synthesis.common.GRIPPER_BASE_TO_CONTACT_M, the +# Magpie's), which every synthesis method plans with. This module imports no ROS +# package, so the value is repeated here; test_robots checks the two agree. +CONVENTION_CONTACT_M = 0.1146 + +# The G1's Dex3-1 grasp, in _hand_palm_link (from the URDF joint origins, not +# measured): the index and middle fingers stand along the palm's +X from +# x = 0.078 m, 57 mm apart in z, and curl toward the palm's face (-Y on the left +# hand, +Y on the right); the thumb stands out of that face at x = 0.023 m and +# swings toward +X. The grasp approaches along the face's normal and closes +# along the palm's +X, with the finger contact point DEX3_CONTACT_M in front of +# the face, between thumb and fingers. Unmeasured on the real G1 and its sim. +DEX3_CONTACT_M = (0.08, 0.04, 0.0) + + +def _dex3_frames(arm): + """Return {grasp frame: ToolFrame, pinch frame: ToolFrame} of `arm`'s Dex3-1.""" + side = -1.0 if arm == 'left' else 1.0 + # columns x (closing: palm +X), y (z cross x), z (approach: the palm face's normal) + z = np.array([0.0, side, 0.0]) + x = np.array([1.0, 0.0, 0.0]) + R = np.column_stack([x, np.cross(z, x), z]) + contact = np.array([DEX3_CONTACT_M[0], side * DEX3_CONTACT_M[1], DEX3_CONTACT_M[2]]) + rot = tuple(tuple(float(v) for v in row) for row in R) + parent = f'{arm}_hand_palm_link' + return { + f'{arm}_dex3_grasp_frame': ToolFrame( + parent, tuple(float(v) for v in contact - CONVENTION_CONTACT_M * z), rot), + f'{arm}_dex3_pinch_frame': ToolFrame(parent, tuple(float(v) for v in contact), rot), + } + + +G1 = RobotProfile( + name='g1', + # tool frames on the Dex3-1 palms (DEX3_CONTACT_M): the G1's URDF has no grasp frame + grasp_frames={arm: f'{arm}_dex3_grasp_frame' for arm in ARMS}, + pinch_frames={arm: f'{arm}_dex3_pinch_frame' for arm in ARMS}, + # golem_upper_ik's G1 has no 'prep' or staging posture; 'home' holds the + # forearms forward, the palms about 0.24 m in front of the pelvis + prep_config='home', + staging_configs={arm: 'home' for arm in ARMS}, + # GraspGenX has no Dex3-1 model, and the G1 no hand cameras + graspgen_gripper=None, + hand='dex3', + hand_cameras=False, + tool_frames={**_dex3_frames('left'), **_dex3_frames('right')}, +) + +ROBOTS = {p.name: p for p in (H12, G1)} DEFAULT_ROBOT = 'h12' @@ -66,3 +140,16 @@ def robot_profile(name): if key not in ROBOTS: raise ValueError(f'unknown robot {name!r}; golem_skills drives {sorted(ROBOTS)}') return ROBOTS[key] + + +def drive_pose(profile, frame, T): + """ + Return (URDF frame, its 4x4 pose) that puts `frame` at 4x4 pose `T`. + + A tool frame (profile.tool_frames) is driven through its parent; any other + frame is a URDF frame, returned as given. + """ + tool = profile.tool_frames.get(frame) + if tool is None: + return frame, T + return tool.parent, T @ np.linalg.inv(tool.matrix()) diff --git a/core_ws/src/golem_skills/test/test_g1_node.py b/core_ws/src/golem_skills/test/test_g1_node.py new file mode 100644 index 00000000..b88bf93d --- /dev/null +++ b/core_ws/src/golem_skills/test/test_g1_node.py @@ -0,0 +1,76 @@ +"""Test a SkillsBase started with robot:=g1: its hand clients, tool frames and frame_task goals.""" + +import os + +import numpy as np +import pytest +import rclpy +from rclpy.time import Time + +from golem_skills.base import SkillsBase +from golem_skills.perception_utils import (matrix_to_pose, pose_to_matrix, + transform_to_matrix) +from test_heartbeat_halt import DDS_CONFIG + +TEST_DOMAIN = 94 + + +@pytest.fixture +def g1(tmp_path): + """Yield a SkillsBase with robot:=g1 on a loopback test domain.""" + config = tmp_path / 'cyclonedds.xml' + config.write_text(DDS_CONFIG) + os.environ['CYCLONEDDS_URI'] = 'file://' + str(config) + os.environ['ROS_DOMAIN_ID'] = str(TEST_DOMAIN) + os.environ['ROS_LOCALHOST_ONLY'] = '0' + rclpy.init(args=['--ros-args', '-p', 'wait_for_services:=false', + '-p', 'record_runs:=false', '-p', 'robot:=g1']) + node = SkillsBase('g1_skills_test') + yield node + node.destroy_node() + rclpy.shutdown() + + +def test_g1_node_uses_the_dex3_driver(g1): + """No Magpie clients; open and close clients on dex_3's //hand/ services.""" + assert g1.robot.name == 'g1' + assert g1.gripper_clis == {} and g1.gripper_close_clis == {} + assert {a: c.srv_name for a, c in g1.hand_close_clis.items()} == { + 'left': '/left/hand/close', 'right': '/right/hand/close'} + assert {a: c.srv_name for a, c in g1.hand_open_clis.items()} == { + 'left': '/left/hand/open', 'right': '/right/hand/open'} + + +def test_g1_tool_frames_are_on_tf_static(g1): + """The node's own TF listener resolves the Dex3-1 grasp frame from its palm link.""" + for _ in range(50): + rclpy.spin_once(g1, timeout_sec=0.1) + if g1.tf_buffer.can_transform('left_hand_palm_link', 'left_dex3_grasp_frame', + Time()): + break + tf = g1.tf_buffer.lookup_transform('left_hand_palm_link', 'left_dex3_grasp_frame', + Time()) + np.testing.assert_allclose( + transform_to_matrix(tf.transform), + g1.robot.tool_frames['left_dex3_grasp_frame'].matrix(), atol=1e-6) + + +def test_g1_grasp_frame_moves_go_to_the_palm(g1): + """A frame_task goal for the grasp frame drives the palm link to T inv(offset).""" + sent = [] + + def fake_send(_client, goal, **_kw): + """Record the goal and report no result.""" + sent.append(goal) + return None + + g1._send_action = fake_send + g1._last_beat = g1._monotonic() + T = np.eye(4) + T[:3, 3] = (0.35, 0.15, 0.05) + g1.move_frame_to('left_dex3_grasp_frame', matrix_to_pose(T), duration_sec=1.0) + assert len(sent) == 1 + assert list(sent[0].frame_names) == ['left_hand_palm_link'] + offset = g1.robot.tool_frames['left_dex3_grasp_frame'].matrix() + np.testing.assert_allclose(pose_to_matrix(sent[0].frame_targets[0]) @ offset, T, + atol=1e-9) diff --git a/core_ws/src/golem_skills/test/test_robots.py b/core_ws/src/golem_skills/test/test_robots.py index 6329dc91..fe9a1c60 100644 --- a/core_ws/src/golem_skills/test/test_robots.py +++ b/core_ws/src/golem_skills/test/test_robots.py @@ -3,9 +3,11 @@ from dataclasses import replace from types import SimpleNamespace +import numpy as np import pytest -from golem_skills.robots import ARMS, DEFAULT_ROBOT, ROBOTS, robot_profile +from golem_skills.robots import (ARMS, CONVENTION_CONTACT_M, DEFAULT_ROBOT, ROBOTS, + drive_pose, robot_profile) def test_default_robot_is_the_h12(): @@ -62,3 +64,69 @@ def test_a_robot_without_graspgen_or_hand_cameras_is_refused_before_moving(): assert 'antipodal' in _refusal(bare, 'graspgenx_filtered') assert _refusal(bare, 'antipodal') is None assert 'hand cameras' in _refusal(bare, 'top_down', visual_servo=True) + + +def test_the_convention_contact_distance_is_grasp_synthesis_own(): + """robots.py's copy of the grasp convention's contact distance equals common.py's.""" + from golem_skills.grasp_synthesis.common import GRIPPER_BASE_TO_CONTACT_M + assert CONVENTION_CONTACT_M == GRIPPER_BASE_TO_CONTACT_M + + +@pytest.mark.parametrize('arm', ARMS) +def test_g1_grasp_and_pinch_frames_follow_the_grasp_convention(arm): + """Each Dex3-1 grasp frame approaches along the palm face's normal, its contact the pinch.""" + g1 = robot_profile('g1') + grasp = g1.tool_frames[g1.grasp_frames[arm]] + pinch = g1.tool_frames[g1.pinch_frames[arm]] + assert grasp.parent == pinch.parent == f'{arm}_hand_palm_link' + Tg, Tp = grasp.matrix(), pinch.matrix() + R = Tg[:3, :3] + np.testing.assert_allclose(R @ R.T, np.eye(3), atol=1e-12) + assert np.linalg.det(R) == pytest.approx(1.0) + side = -1.0 if arm == 'left' else 1.0 + # approach (+Z) along the palm face's normal, closing (+X) along the palm's +X + np.testing.assert_allclose(R[:, 2], [0.0, side, 0.0], atol=1e-12) + np.testing.assert_allclose(R[:, 0], [1.0, 0.0, 0.0], atol=1e-12) + # the contact point, CONVENTION_CONTACT_M along +Z, is the pinch frame's origin + np.testing.assert_allclose(Tg[:3, 3] + CONVENTION_CONTACT_M * R[:, 2], Tp[:3, 3], + atol=1e-12) + np.testing.assert_allclose(Tp[:3, :3], R, atol=1e-12) + np.testing.assert_allclose(Tp[:3, 3], [0.08, side * 0.04, 0.0], atol=1e-12) + + +def test_g1_hands_mirror(): + """The right hand's frames are the left's mirrored across the palm's XZ plane.""" + g1 = robot_profile('g1') + M = np.diag([1.0, -1.0, 1.0]) + for kind in ('grasp', 'pinch'): + left = g1.tool_frames[f'left_dex3_{kind}_frame'].matrix() + right = g1.tool_frames[f'right_dex3_{kind}_frame'].matrix() + np.testing.assert_allclose(M @ left[:3, 3], right[:3, 3], atol=1e-12) + # the approach mirrors, the closing axis stays + np.testing.assert_allclose(M @ left[:3, 2], right[:3, 2], atol=1e-12) + np.testing.assert_allclose(left[:3, 0], right[:3, 0], atol=1e-12) + + +def test_drive_pose_goes_through_the_tool_frames_parent(): + """A tool frame is driven through its parent at T inv(offset); a URDF frame as given.""" + g1 = robot_profile('g1') + T = np.eye(4) + T[:3, :3] = np.array([[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]) + T[:3, 3] = (0.4, 0.2, 0.1) + frame, Tp = drive_pose(g1, 'left_dex3_grasp_frame', T) + assert frame == 'left_hand_palm_link' + np.testing.assert_allclose(Tp @ g1.tool_frames['left_dex3_grasp_frame'].matrix(), T, + atol=1e-12) + assert drive_pose(g1, 'left_hand_palm_link', T) == ('left_hand_palm_link', T) + assert drive_pose(robot_profile('h12'), 'left_graspgenx_frame', T)[0] == \ + 'left_graspgenx_frame' + + +def test_g1_profile(): + """The G1 grasps with the Dex3-1, from 'home', with neither GraspGenX nor a visual servo.""" + g1 = robot_profile('g1') + assert (g1.hand, g1.prep_config, g1.graspgen_gripper, g1.hand_cameras) == \ + ('dex3', 'home', None, False) + assert set(g1.staging_configs.values()) == {'home'} + assert 'graspgenx' in _refusal(g1, 'graspgenx') + assert _refusal(g1, 'heuristic') is None From 10bf3ebed28099b477327a24937b8142bc40f2e8 Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Fri, 20 Nov 2026 21:09:20 -0500 Subject: [PATCH 6/8] golem_skills: run the grasp reach check on the node's robot The reach check built golem_upper_ik's H1-2 model and tested the Magpie's volume. ReachModel and its child process now take the node's robot: its IK profile, its hand's boxes (HAND_BOXES_M per hand, the Dex3-1's from the URDF's joint origins) and, for a tool frame such as the G1's Dex3-1 grasp frame, the IK follows the frame's parent link as frame_task does. On the G1 no method runs the check yet (only the GraspGenX methods do, and the G1 refuses them); the H1-2's check is unchanged. Co-Authored-By: Claude Opus 5.5 --- .../golem_skills/golem_skills/reach_model.py | 91 +++++++++++-------- .../golem_skills/skills/grasp_reach.py | 3 +- .../src/golem_skills/test/test_grasp_reach.py | 58 +++++++++--- 3 files changed, 103 insertions(+), 49 deletions(-) diff --git a/core_ws/src/golem_skills/golem_skills/reach_model.py b/core_ws/src/golem_skills/golem_skills/reach_model.py index 5ee82697..057d2633 100644 --- a/core_ws/src/golem_skills/golem_skills/reach_model.py +++ b/core_ws/src/golem_skills/golem_skills/reach_model.py @@ -21,11 +21,11 @@ from golem_upper_ik.core.robot_model import FixedState, RobotModel from golem_upper_ik.profile import load_profile +from .robots import drive_pose, robot_profile + -# robot whose IK model the check runs: the grasp skill drives the H1-2's Magpie frames -REACH_ROBOT = 'h12' # m golem_upper_ik's self-collision barrier keeps between sphere pairs (controller.d_min of -# every H1-2 controller config) +# every H1-2 and G1 controller config) REACH_D_MIN_M = 0.02 # s per simulated IK step; the frame_task server steps faster, but the path it follows # is the same, and a coarser step keeps a check near a second @@ -37,17 +37,29 @@ REACH_STABLE_M = 0.0005 REACH_STABLE_STEPS = 20 -# The hand's volume in the GraspGenX gripper-base frame (+Z the approach, +X the -# finger-closing axis), as axis-aligned boxes (x0, x1, y0, y1, z0, z1) [m], from the magpie -# meshes in CL_Assets/graspgenx/assets/meshes/magpie: the base (base_top, base_bot) and the -# open fingers back to the contact plane (common.GRIPPER_BASE_TO_CONTACT_M). Beyond that -# plane the pads straddle the part and a close stops on whatever they meet, so they are -# not tested. -HAND_BOXES_M = ( - (-0.067, 0.067, -0.019, 0.015, 0.002, 0.085), - (-0.108, -0.054, -0.021, 0.016, 0.066, 0.1146), - (0.054, 0.108, -0.021, 0.016, 0.066, 0.1146), -) +# Each hand's volume in its grasp frame (the GraspGenX gripper-base convention: +Z the +# approach, +X the finger-closing axis), as axis-aligned boxes (x0, x1, y0, y1, z0, z1) +# [m], back to the contact plane (common.GRIPPER_BASE_TO_CONTACT_M). Beyond that plane +# the fingers straddle the part and a close stops on whatever they meet, so they are not +# tested. +HAND_BOXES_M = { + # the base (base_top, base_bot) and the open fingers, from the magpie meshes in + # CL_Assets/graspgenx/assets/meshes/magpie + 'magpie': ( + (-0.067, 0.067, -0.019, 0.015, 0.002, 0.085), + (-0.108, -0.054, -0.021, 0.016, 0.066, 0.1146), + (0.054, 0.108, -0.021, 0.016, 0.066, 0.1146), + ), + # the Dex3-1 open, in robots.py's grasp frame: the palm, the index and middle fingers + # standing straight along +X, and the thumb standing out of the palm toward the part. + # From the URDF's joint origins with links about 25 mm thick, not from the meshes; + # unmeasured on the real G1. + 'dex3': ( + (-0.080, 0.000, -0.045, 0.045, 0.0496, 0.0846), + (-0.010, 0.095, -0.042, 0.042, 0.0626, 0.0866), + (-0.070, -0.040, -0.012, 0.012, 0.0846, 0.1146), + ), +} # m each box is shrunk by on every side, so depth noise on a surface the hand only # touches does not count HAND_MARGIN_M = 0.003 @@ -75,32 +87,32 @@ SUPPORT_DEPTH_M = 0.005 -def hand_hits(obstacles, T): - """Return how many `obstacles` points lie in the hand at gripper-base pose `T` (4x4).""" +def hand_hits(obstacles, T, hand): + """Return how many `obstacles` points lie in `hand` (a HAND_BOXES_M key) at grasp pose `T`.""" if obstacles is None or len(obstacles) == 0: return 0 local = (obstacles - T[:3, 3]) @ T[:3, :3] m = HAND_MARGIN_M inside = np.zeros(len(local), dtype=bool) - for x0, x1, y0, y1, z0, z1 in HAND_BOXES_M: + for x0, x1, y0, y1, z0, z1 in HAND_BOXES_M[hand]: inside |= ((local[:, 0] > x0 + m) & (local[:, 0] < x1 - m) & (local[:, 1] > y0 + m) & (local[:, 1] < y1 - m) & (local[:, 2] > z0 + m) & (local[:, 2] < z1 - m)) return int(inside.sum()) -def _hand_samples(): - """Return points filling HAND_BOXES_M, each shrunk by HAND_MARGIN_M (gripper-base frame).""" +def _hand_samples(boxes): + """Return points filling `boxes`, each shrunk by HAND_MARGIN_M (grasp frame).""" m = HAND_MARGIN_M out = [] - for box in HAND_BOXES_M: + for box in boxes: axes = [np.linspace(lo + m, hi - m, int(np.ceil((hi - lo - 2 * m) / HAND_SAMPLE_M)) + 1) for lo, hi in zip(box[0::2], box[1::2])] out.append(np.stack([a.ravel() for a in np.meshgrid(*axes, indexing='ij')], axis=-1)) return np.concatenate(out) -HAND_SAMPLES = _hand_samples() +HAND_SAMPLES = {hand: _hand_samples(boxes) for hand, boxes in HAND_BOXES_M.items()} def support_edges(obstacles): @@ -126,11 +138,11 @@ def support_edges(obstacles): return edges -def support_depth(edges, T): - """Return how far [m] the hand at gripper-base pose `T` sits below the extended support.""" +def support_depth(edges, T, hand): + """Return how far [m] `hand` at grasp pose `T` sits below the extended support.""" if not edges: return 0.0 - p = HAND_SAMPLES @ T[:3, :3].T + T[:3, 3] + p = HAND_SAMPLES[hand] @ T[:3, :3].T + T[:3, 3] idx = np.floor(p[:, 1] / SUPPORT_SLICE_M).astype(int) depth = 0.0 for i in np.unique(idx): @@ -147,9 +159,11 @@ def support_depth(edges, T): class ReachModel: """Follow pelvis-frame waypoints with golem_upper_ik's IK from a measured pose.""" - def __init__(self): - """Build the robot's IK model (~15 s: the URDF and self-collision model).""" - self.profile = load_profile(REACH_ROBOT) + def __init__(self, robot): + """Build `robot`'s IK model (~15 s: the URDF and self-collision model).""" + # the skills' profile (robots.py): its hand and tool frames + self.robot = robot_profile(robot) + self.profile = load_profile(self.robot.name) self.model = RobotModel(self.profile) self.joints = list(self.profile.body_joints) q = np.zeros(self.model.num_motor) @@ -205,32 +219,35 @@ def check(self, q_body, frame, waypoints, obstacles): Return why the approach along `waypoints` fails from body q `q_body`, or None. `waypoints`: (4x4 pose, label, (lin_tol, ang_tol) or None, scene) in execution - order, pelvis frame, for URDF `frame`; a waypoint with tolerances must be reached - within them, one without only steers the IK, and one with `scene` set must leave - the hand out of `obstacles` ((N, 3) pelvis-frame points, or None) and above the - support extended toward the robot (support_depth). + order, pelvis frame, for the grasp frame `frame` (a URDF frame or a tool frame of + the robot's profile, which the IK follows through its parent, as frame_task + does); a waypoint with tolerances must be reached within them, one without only + steers the IK, and one with `scene` set must leave the hand out of `obstacles` + ((N, 3) pelvis-frame points, or None) and above the support extended toward the + robot (support_depth). """ self.start(q_body) edges = support_edges(obstacles) + hand = self.robot.hand for T, label, tol, scene in waypoints: if scene: - hits = hand_hits(obstacles, T) + hits = hand_hits(obstacles, T, hand) if hits >= HAND_HIT_POINTS: return f'the hand would be in the scene at the {label} ({hits} points)' - below = support_depth(edges, T) + below = support_depth(edges, T, hand) if below > SUPPORT_DEPTH_M: return (f'the hand would be {below * 1000:.0f} mm below the support seen ' f'ahead of it at the {label}') - lin, ang = self.follow(frame, T) + lin, ang = self.follow(*drive_pose(self.robot, frame, T)) if tol is not None and (lin > tol[0] or ang > tol[1]): return (f'the IK stops {lin * 1000:.0f} mm / {np.degrees(ang):.0f} deg ' f'short of the {label}') return None -def serve(conn): +def serve(conn, robot): """ - Build a ReachModel and answer check requests on `conn` until the other end closes. + Build `robot`'s ReachModel and answer check requests on `conn` until the other end closes. Sends ('ready', build s) or ('error', reason) once, then for each request (joint names, positions, frame, waypoints, obstacles) one ('ok', ReachModel.check's @@ -238,7 +255,7 @@ def serve(conn): """ try: t0 = time.monotonic() - model = ReachModel() + model = ReachModel(robot) conn.send(('ready', time.monotonic() - t0)) except Exception as e: # noqa: BLE001 - any failure is reported to the node conn.send(('error', f'{type(e).__name__}: {e}')) diff --git a/core_ws/src/golem_skills/golem_skills/skills/grasp_reach.py b/core_ws/src/golem_skills/golem_skills/skills/grasp_reach.py index 3b32a76e..ab69694b 100644 --- a/core_ws/src/golem_skills/golem_skills/skills/grasp_reach.py +++ b/core_ws/src/golem_skills/golem_skills/skills/grasp_reach.py @@ -71,7 +71,8 @@ def _reach_model_start(self): # spawn, not fork: a forked child would inherit the node's rclpy threads ctx = multiprocessing.get_context('spawn') self._reach_conn, child = ctx.Pipe() - self._reach_proc = ctx.Process(target=reach_model.serve, args=(child,), + self._reach_proc = ctx.Process(target=reach_model.serve, + args=(child, self.robot.name), name='grasp_reach', daemon=True) self._reach_proc.start() child.close() diff --git a/core_ws/src/golem_skills/test/test_grasp_reach.py b/core_ws/src/golem_skills/test/test_grasp_reach.py index 1e9de55c..8e91160c 100644 --- a/core_ws/src/golem_skills/test/test_grasp_reach.py +++ b/core_ws/src/golem_skills/test/test_grasp_reach.py @@ -2,7 +2,7 @@ Test the grasp reach check: the hand-in-scene count, the approach waypoints and the IK replay. The IK tests build golem_upper_ik's H1-2 model (~15 s) once in this process and once in the -check's child process, and need no ROS graph. +check's child process, and its G1 model once, and need no ROS graph. """ import math @@ -20,6 +20,7 @@ approach_waypoints, line_waypoints) from golem_skills.reach_model import (HAND_HIT_POINTS, SUPPORT_DEPTH_M, ReachModel, hand_hits, support_depth, support_edges) +from golem_skills.robots import robot_profile from golem_skills.skills.grasp_reach import (OBJECT_CLEAR_M, SCENE_CROP_M, GraspReach, scene_obstacles) @@ -34,9 +35,9 @@ def test_a_hand_above_a_surface_is_clear_and_one_through_it_is_not(): """A level hand 5 cm above a table holds no table points; one at table height holds many.""" table = plane(0.0) level = pose_to_matrix(level_pose((0.0, 0.0, 0.05), (1.0, 0.0, 0.0))) - assert hand_hits(table, level) == 0 + assert hand_hits(table, level, 'magpie') == 0 level[2, 3] = 0.0 - assert hand_hits(table, level) >= HAND_HIT_POINTS + assert hand_hits(table, level, 'magpie') >= HAND_HIT_POINTS def test_a_top_down_hand_whose_pads_straddle_a_part_is_clear(): @@ -46,7 +47,7 @@ def test_a_top_down_hand_whose_pads_straddle_a_part_is_clear(): down[:3, :3] = np.array([[1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]]) # contact point 5 mm above the table: the base sits 0.1146 m above it down[:3, 3] = (0.0, 0.0, 0.005 + 0.1146) - assert hand_hits(table, down) == 0 + assert hand_hits(table, down, 'magpie') == 0 def seen_support(x0=0.38, y0=-0.3, y1=0.3): @@ -65,27 +66,28 @@ def test_a_rolled_hand_below_the_unseen_table_front_is_caught(): """A vertical-closing hand in front of the seen table, fingers below its top, fails.""" table = seen_support() T = rolled_pregrasp(0.02) - assert hand_hits(table, T) == 0 - assert support_depth(support_edges(table), T) > 0.04 + assert hand_hits(table, T, 'magpie') == 0 + assert support_depth(support_edges(table), T, 'magpie') > 0.04 def test_a_level_hand_above_the_unseen_table_front_is_clear(): """A level hand in front of the seen table, above its top, passes.""" table = seen_support() level = pose_to_matrix(approach_pose(level_pose((0.45, 0.0, 0.05), (1.0, 0.0, 0.0)), -0.10)) - assert support_depth(support_edges(table), level) <= SUPPORT_DEPTH_M + assert support_depth(support_edges(table), level, 'magpie') <= SUPPORT_DEPTH_M def test_the_support_extends_only_in_the_slices_it_is_seen_in(): """The rolled hand passes when the table is seen only beside it, not ahead of it.""" table = seen_support(y0=0.15) - assert support_depth(support_edges(table), rolled_pregrasp(0.02)) <= SUPPORT_DEPTH_M + depth = support_depth(support_edges(table), rolled_pregrasp(0.02), 'magpie') + assert depth <= SUPPORT_DEPTH_M def test_a_hand_behind_the_seen_near_edge_is_left_to_the_point_count(): """Hand points at or past a slice's near edge are not tested against the extension.""" table = seen_support(x0=0.20) - assert support_depth(support_edges(table), rolled_pregrasp(0.02)) == 0.0 + assert support_depth(support_edges(table), rolled_pregrasp(0.02), 'magpie') == 0.0 def test_scene_obstacles_drops_the_object_and_far_points(): @@ -127,7 +129,7 @@ def test_approach_waypoints_judge_the_pregrasp_and_the_first_quarter_of_the_axis @pytest.fixture(scope='module') def reach(): """Return a ReachModel with the arms at 'prep'.""" - model = ReachModel() + model = ReachModel('h12') q = np.zeros(model.model.num_motor) q[model.model.reduced_mask] = model.profile.named_configs['prep'] return model, q @@ -188,9 +190,10 @@ def test_a_cross_body_grasp_is_not(reach): class _Node(GraspReach): """Stand in for the skills node: a logger and a fixed /joint_states.""" - def __init__(self, joint_state): + def __init__(self, joint_state, robot='h12'): """Keep `joint_state` and collect log lines.""" self.joint_state = joint_state + self.robot = robot_profile(robot) self.lines = [] def get_logger(self): @@ -241,3 +244,36 @@ def tick(): node._reach_proc.kill() assert any('IK model built' in line for line in node.lines), node.lines assert max(gaps) < 0.5 + + +def test_a_dex3_hand_above_a_surface_is_clear_and_one_through_it_is_not(): + """A level Dex3-1 with its contact 6 cm above a table holds no table points; at it, many.""" + table = plane(0.0) + level = pose_to_matrix(level_pose((0.0, 0.0, 0.06), (1.0, 0.0, 0.0))) + assert hand_hits(table, level, 'dex3') == 0 + level[2, 3] -= 0.06 + assert hand_hits(table, level, 'dex3') >= HAND_HIT_POINTS + + +@pytest.fixture(scope='module') +def reach_g1(): + """Return the G1's ReachModel with the arms at 'home'.""" + model = ReachModel('g1') + q = np.zeros(model.model.num_motor) + q[model.model.reduced_mask] = model.profile.named_configs['home'] + return model, q + + +def test_the_g1_ik_follows_its_tool_frame_through_the_palm(reach_g1): + """From 'home' the left Dex3-1 grasp frame reaches 5 cm ahead, not 1 m ahead.""" + model, q = reach_g1 + frame = 'left_dex3_grasp_frame' + model.start(q) + cur = model.frame_pose('left_hand_palm_link') @ model.robot.tool_frames[frame].matrix() + near, far = cur.copy(), cur.copy() + near[0, 3] += 0.05 + far[0, 3] += 1.0 + tol = (APPROACH_LIN_TOL, 0.10) + assert model.check(q, frame, [(near, 'pre-grasp', tol, False)], None) is None + why = model.check(q, frame, [(far, 'pre-grasp', tol, False)], None) + assert why is not None and 'short of the pre-grasp' in why From ea7f92aaddb9133bd8795636b0e4fe4c69bbcfe4 Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Fri, 20 Nov 2026 21:10:14 -0500 Subject: [PATCH 7/8] golem_bringup: start the G1's golem_skills on its G1 profile With use_skills:=true, g1_core started golem_skills on its default profile, the H1-2's (Magpie grippers, graspgenx frames). It now passes robot: g1, so /skill/grasp and /skill/pick_place drive the Dex3-1 hands and their palm-link grasp frames. Affects only the G1 bringups with use_skills (sim; the real G1 starts no skills); the H1-2's bringups and /skill/frontier_explore are unchanged. test_nav_launch checks the parameter. Co-Authored-By: Claude Opus 5.5 --- .../src/golem_bringup/launch/g1/g1_core.launch.py | 12 +++++++----- core_ws/src/golem_bringup/test/test_nav_launch.py | 7 ++++++- 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/core_ws/src/golem_bringup/launch/g1/g1_core.launch.py b/core_ws/src/golem_bringup/launch/g1/g1_core.launch.py index 69624ea9..09d50f29 100644 --- a/core_ws/src/golem_bringup/launch/g1/g1_core.launch.py +++ b/core_ws/src/golem_bringup/launch/g1/g1_core.launch.py @@ -63,9 +63,10 @@ FAST-LIO starts while the sim's elastic band still holds the robot up: the leg node releases it once its policy runs and the controller's start-up moves are done (or band.max_wait after the policy started). -use_skills:=true starts golem_skills (serving /skill/*, including -/skill/frontier_explore) without waiting for the H1-2's perception and gripper -services. use_hands starts dex3_hands, the Dex3-1 driver, which writes +use_skills:=true starts golem_skills on its G1 profile (robot: g1; serving +/skill/*, including /skill/frontier_explore and, with the Dex3-1 driver and the +perception services, /skill/grasp and /skill/pick_place) without waiting for +those services. use_hands starts dex3_hands, the Dex3-1 driver, which writes rt/dex3//cmd directly, outside golem_safety. use_rviz starts RViz on rviz/g1/g1.rviz. @@ -217,12 +218,13 @@ def _setup(context): ), ] if _as_bool(context, 'use_skills'): - # The /skill/* action servers, without waiting for the H1-2's services. + # The /skill/* action servers on the G1's profile (the Dex3-1 hands), + # without waiting for the perception and hand services. actions.append(Node( package='golem_skills', executable='skills', name='golem_skills', - parameters=[use_sim_time, {'wait_for_services': False}], + parameters=[use_sim_time, {'robot': ROBOT, 'wait_for_services': False}], output='screen', )) if use_nav and not sim: diff --git a/core_ws/src/golem_bringup/test/test_nav_launch.py b/core_ws/src/golem_bringup/test/test_nav_launch.py index 5f4da965..2f887b1d 100644 --- a/core_ws/src/golem_bringup/test/test_nav_launch.py +++ b/core_ws/src/golem_bringup/test/test_nav_launch.py @@ -170,7 +170,12 @@ def test_use_skills_is_declared_and_starts_golem_skills(): for value in ('true', 'false'): passed = _passed_to_core('g1_sim.launch.py', use_nav='true', use_skills=value) assert passed['use_skills'] == value - assert ('skills' in _core_nodes(**passed)) == (value == 'true') + nodes = _core_nodes(**passed) + assert ('skills' in nodes) == (value == 'true') + if value == 'true': + # golem_skills drives the G1's profile (its Dex3-1 hands), not the default H1-2 + assert nodes['skills'].parameters['robot'] == 'g1' + assert nodes['skills'].parameters['wait_for_services'] is False def test_core_passes_use_sim_time_on(): From d92fece8a91985a7ab13e3c3f4d1f468a26118b1 Mon Sep 17 00:00:00 2001 From: Nikolaus Correll Date: Fri, 20 Nov 2026 21:41:52 -0500 Subject: [PATCH 8/8] docs: golem_skills' robot profiles and the G1's manipulation skills MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit CLAUDE.md §4, §6 and §7 and docs/g1 describe the `robot` parameter, what the G1's profile drives (the Dex3-1 through dex_3, tool frames on the palm links, 'home' as prep), what it refuses (GraspGenX, visual_servo), and what has run: the grasp's execution in the sim from a synthetic object, not a perception-driven grasp or pick_place. Co-Authored-By: Claude Opus 5.5 --- CLAUDE.md | 25 ++++++++++++++++++++----- docs/g1/index.md | 6 ++++-- docs/g1/packages.md | 2 +- 3 files changed, 25 insertions(+), 8 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index 0c6f8c44..5ee2a9b3 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -211,7 +211,7 @@ nests `unitree_api`, `unitree_go`, `unitree_hg`, `unitree_ros2_example`). | `golem_lowerbody_rl` | **controller (lower body, both robots)** | One leg node for both robots, run from config, with no robot or policy name in its code: a robot's `config//` (`robot.yaml`: leg joints and the policies it may run; `sim.yaml` and `real.yaml`: start-up, band, command and fault limits of the run) and each policy's spec in `config//policies/.yaml` (runtime, TorchScript or ONNX; observation terms; command processing; action mapping); joint slots and `mode_machine` come from `golem_safety`'s robot profile. The H1-2 runs the TorchScript walking, FAME RMA stand/squat and ALMI stand/walk policies, one per run (`policy:=`, `almi` by default in every bringup); the G1 runs HOMIE's ONNX walking policy (its weights under their own licence in `policies/homie/`, §8). ROS only (no Unitree SDK): consumes `/lowstate`, `/cmd_vel` and `/lowerbody/height`, publishes the 12 leg joints' PD setpoints on `golem_safety`'s `/safety/lowcmd_lower_in` | `lowerbody_controller_node` (`/lowerbody/confirm_engage`, `/lowerbody/active_policy`); `launch/lowerbody.launch.py` (`robot`, `config`, `policy`, `sim`; refuses a run config for the other platform, a real-robot run config without the operator's confirm, or an unlisted policy before any node starts: `lowerbody: not starting: ...`), which `h12_lowerbody.launch.py` and `g1_core.launch.py` include; the H1-2's top-level files, which include the leg block last, run the same check first (`golem_bringup.h12_legs.leg_check`) | | `lmg_deploy` | controller (whole body, MARL) | The loco-manipulation game's two-agent (legs + IK arms) MARL policy, run by the `lmg_*` bringups. It reads `/livox/imu` in the MID-360's raw, upside-down `livox_imu_link`, as the H1-2's models and RoboCasa sim carry it, and turns it into the upright frame the policies trained on (`livox_imu.livox_to_imu_rotation`). Policies live one per robot, `policies/h12/` and `policies/g1/` (empty), **one iteration at a time: a new export replaces the folder and old ones are deleted** (`policies/README.md`) | `marl_deploy_node` | | `golem_slam` | SLAM + navigation (both robots) | One navigation graph for both robots (FAST-LIO deskewed cloud → self filter → floor alignment (`level_odom`)/OctoMap and `level_cloud` → scan → slam_toolbox → Nav2 MPPI, every Nav2 command through the velocity smoother; the local costmap's footprint widened by the arms, `dynamic_footprint`, the body alone during an arm goal), with the dynamic `body → pelvis` transform (`body_to_pelvis`, following the torso or waist joints) and a static ` → livox_frame`. `navigation.launch.py robot:=h12\|g1` picks the robot's config directory, `config//`: FAST-LIO (`fastlio.yaml`, the G1 sim's `fastlio_sim.yaml`), the MID360 driver config (`MID360_config.json`, real robot, read by the real-driver launches), Nav2 (`nav2.yaml`), OctoMap (`octomap.yaml`) and the pipeline profile (`pipeline.yaml`: lidar link, scan band, `level_odom` values, the arm links of `dynamic_footprint`); slam_toolbox's `config/slam_toolbox.yaml` is shared; `config/example..yaml` annotate every key and are not installed | `launch/navigation.launch.py`, `body_to_pelvis`, `config/` | -| `golem_skills` | **skills (both robots)** | Action servers for the 12 `/skill/*` atomic skills an LLM can call (frontier exploration lives here). See §7. | `skills` (SkillsNode) | +| `golem_skills` | **skills (both robots)** | Action servers for the 12 `/skill/*` atomic skills an LLM can call (frontier exploration lives here); the `robot` parameter picks the H1-2's or the G1's profile (`robots.py`). See §7. | `skills` (SkillsNode) | | `model_server` | model server | ROS service servers wrapping ML models: Gemini VLM, SAM3 segmentation, GraspGenX grasp generation, YOLO detection | `gemini_server`, `sam_server`, `graspgen_server`, `yolo_server` | | `custom_ros_messages` | interfaces (both robots) | 19 actions (13 `Skill*`, `FrameTask`, `NamedConfig`, `DualArm`, …), msgs, srvs (`GeminiQuery`, `SamSegment`, `GraspGen`, `SetHandPose`, …) | rosidl only | | `magpie_control` | driver (gripper/sensors) | Dynamixel gripper node (`/left|right/gripper/*` services, DeliGrasp action), F/T + tactile sensor nodes | `gripper_node`, `ft_sensor_node` | @@ -466,7 +466,7 @@ reference. | Launch file | Scenario | Starts | |---|---|---| -| `g1_sim.launch.py` | **G1 sim** (sim time; `sim_split` for `golem_safety` and the controller, `golem_lowerbody_rl`'s `config/g1/sim.yaml`). Always starts the leg node, running HOMIE. `use_nav` (default true) adds shared navigation (`golem_slam`, FAST-LIO odometry), `use_skills:=true` starts `golem_skills` (including `/skill/frontier_explore`) | `g1_core` | +| `g1_sim.launch.py` | **G1 sim** (sim time; `sim_split` for `golem_safety` and the controller, `golem_lowerbody_rl`'s `config/g1/sim.yaml`). Always starts the leg node, running HOMIE. `use_nav` (default true) adds shared navigation (`golem_slam`, FAST-LIO odometry), `use_skills:=true` starts `golem_skills` on its G1 profile (`robot: g1`; including `/skill/frontier_explore`) | `g1_core` | | `g1_real_robot.launch.py` | **real G1** in Unitree's debug mode, every node in the laptop's `golem_ros` container on domain 0 (`split` and `g1_split`, `golem_lowerbody_rl`'s `config/g1/real.yaml`). The leg node only with `start_position_verified:=true` (default **false**): without it the leg motors stay off and the arms run. `use_nav` (default true) adds the MID-360 driver and `golem_slam`; no skills yet | `g1_core` | | `g1_core.launch.py` | shared core, starting `golem_safety`'s and `golem_upper_ik`'s nodes through their launch files (`safety.launch.py`, `upper_ik.launch.py`, `robot_state.launch.py`), as the H1-2's blocks do: `robot_state_publisher`, `golem_upper_ik`'s `joint_state_publisher` (`robot: g1`), `golem_safety`'s `safety_node` (`robot: g1`, `safety_config`, `sim`, `use_estop: false`: the G1 has no estop node), then `golem_upper_ik`'s `frame_task_server` (`robot: g1`, `controller_config`) after `controller_delay` (2 s), then `golem_lowerbody_rl`'s `lowerbody_controller_node` (its `lowerbody.launch.py`: `robot: g1`, `policy: homie`, run config `lowerbody_config`) after `locomotion_delay` (2 s more: always in the sim, with `start_position_verified` on the real robot); [`dex3_hands`] (`use_hands`), [rviz] (`use_rviz`), [`golem_slam`] (`use_nav`), [`golem_skills`] (`use_skills`) | — | @@ -556,9 +556,24 @@ the `SKILL_ACTIONS` table (`base.py`). All types are `custom_ros_messages` also defines `SkillRetrieveMemory`; no node in this repo serves it yet. On the G1, `golem_skills` starts when `use_skills:=true` -is passed to `g1_sim.launch.py` (with `wait_for_services: false` so it starts -immediately without waiting for H1-2 vision/gripper services), serving -`/skill/frontier_explore`. +is passed to `g1_sim.launch.py` (with `robot: g1` and `wait_for_services: +false`, so it starts immediately without waiting for the perception and hand +services), serving `/skill/frontier_explore` and the manipulation skills. + +The node's read-only `robot` parameter (`h12` by default, `g1`) picks a +profile in `golem_skills/robots.py`: the frames the grasp and pick_place +skills drive, the hand, the named configs and the GraspGenX gripper. On the +G1 the grasp closes the Dex3-1 through `dex_3`'s `//hand/{open,close}` +(the grip check reads the finger joints on `/joint_states`), starts from +`home` (the G1 has no `prep` or staging config), and drives tool frames on +`_hand_palm_link` (`_dex3_grasp_frame`, `_dex3_pinch_frame`, +which the node broadcasts on `/tf_static`; their offsets are from the URDF, +unmeasured). The G1 refuses `graspgenx*` (GraspGenX has no Dex3-1 model) and +`visual_servo` (no hand cameras) before the arm moves, so it runs +`heuristic`, `antipodal`, `centroid` and `top_down`. Its execution was run +in the sim from a synthetic object cloud; a perception-driven grasp has not +run on it (the head camera does not see the RoboCasa counter tops), nor has +`pick_place`. Arm moves (`/frame_task`, `/named_config`) need a live `/safety/heartbeat`: `golem_skills` refuses to send one before the first true beat, after a false diff --git a/docs/g1/index.md b/docs/g1/index.md index cfd68a91..d92b3e3f 100644 --- a/docs/g1/index.md +++ b/docs/g1/index.md @@ -25,8 +25,10 @@ Nothing is installed on, or run from, the robot's own computers. Not done yet: the procedure for entering Unitree's debug mode on our G1, navigation on the real robot, self-collision spheres on the Dex3-1 fingers, -manipulation skills, sensors and skills in the real bringup, and tuning of -gains and limits on our G1. +a perception-driven grasp and `pick_place` (the manipulation skills run on the +G1's profile, but only their execution has run, in the sim, from a synthetic +object), sensors and skills in the real bringup, and tuning of gains and +limits on our G1. ## Selecting the G1 diff --git a/docs/g1/packages.md b/docs/g1/packages.md index c6eb5f7b..8ecff5e0 100644 --- a/docs/g1/packages.md +++ b/docs/g1/packages.md @@ -26,7 +26,7 @@ below has its own README with the full details, except the launch-only | `golem_upper_ik` | the arm IK controller, `frame_task_server` (`/frame_task`, `/named_config`), and `joint_state_publisher` ([README](https://github.com/correlllab/GOLEM/blob/main/core_ws/src/golem_upper_ik/README.md)). | | `golem_lowerbody_rl` | the leg node, running HOMIE on the G1's legs ([README](https://github.com/correlllab/GOLEM/blob/main/core_ws/src/golem_lowerbody_rl/README.md)). | | `golem_slam` | the SLAM and Nav2 graph both robots run, `navigation.launch.py robot:=g1` ([README](https://github.com/correlllab/GOLEM/blob/main/core_ws/src/golem_slam/README.md)). | -| `golem_skills` | action servers for `/skill/*`, including `/skill/frontier_explore`, started by `use_skills:=true` in the G1 bringup. | +| `golem_skills` | action servers for `/skill/*`, including `/skill/frontier_explore`, started by `use_skills:=true` in the G1 bringup on its G1 profile (`robot: g1`): `/skill/grasp` and `/skill/pick_place` close the Dex3-1 hands with the heuristic grasp methods (no GraspGenX, no visual servo). | | `custom_ros_messages` | the actions and services (`FrameTask`, `NamedConfig`, `SetHandPose`, `SkillFrontierExplore`, ...). | | `FAST_LIO` (package `fast_lio`) | lidar-inertial odometry for `golem_slam` | | `livox_ros_driver2` | the `CustomMsg` type of `/livox/lidar`, and the MID-360 driver the real G1's bringup starts with `use_nav` (`launch/g1/g1_real_drivers.launch.py`) |