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/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(): diff --git a/core_ws/src/golem_skills/golem_skills/base.py b/core_ws/src/golem_skills/golem_skills/base.py index 6ae4d84b..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 @@ -65,6 +66,8 @@ ) 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, drive_pose, robot_profile from .run_telemetry import RunRecorder, describe_goal, run_dir @@ -80,10 +83,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,30 +301,42 @@ 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}') + + # 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 = {} @@ -339,14 +350,29 @@ 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 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 + # 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 + # /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) @@ -405,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() @@ -452,8 +484,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: @@ -462,7 +494,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 @@ -540,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: @@ -997,7 +1048,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 @@ -1080,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 @@ -1090,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) @@ -1324,12 +1383,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): """ @@ -1405,8 +1495,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 @@ -1423,7 +1519,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})'): @@ -1514,7 +1619,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 +1647,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/__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 2b19c1e5..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,6 +43,9 @@ class SynthesisInput: plan_grasp: Callable # rclpy logger log: object + # 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 2c42a348..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,7 +40,9 @@ 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', + 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: return None, f'no grasp planned for {inp.obj!r}' 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/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/robots.py b/core_ws/src/golem_skills/golem_skills/robots.py new file mode 100644 index 00000000..122191cf --- /dev/null +++ b/core_ws/src/golem_skills/golem_skills/robots.py @@ -0,0 +1,155 @@ +""" +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, 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.""" + + # 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] + # 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 + # 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( + 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', + hand='magpie', + hand_cameras=True, +) + +# 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' + + +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] + + +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/golem_skills/skills/grasp.py b/core_ws/src/golem_skills/golem_skills/skills/grasp.py index bee0c684..dfd6727b 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 @@ -37,10 +37,12 @@ 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 ..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 @@ -311,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) @@ -323,14 +329,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 +364,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 +384,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, @@ -395,8 +403,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}') @@ -413,7 +420,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.""" @@ -425,12 +451,29 @@ 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( 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 +563,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 +595,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 +625,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 +725,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 +790,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 +830,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_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/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/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_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_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 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..99821a23 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(): @@ -266,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_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 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} 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..fe9a1c60 --- /dev/null +++ b/core_ws/src/golem_skills/test/test_robots.py @@ -0,0 +1,132 @@ +"""Test the per-robot profiles the manipulation skills read (robots.py).""" + +from dataclasses import replace +from types import SimpleNamespace + +import numpy as np +import pytest + +from golem_skills.robots import (ARMS, CONVENTION_CONTACT_M, DEFAULT_ROBOT, ROBOTS, + drive_pose, 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)) + + +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) + + +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 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`) |