diff --git a/CHANGELOG.md b/CHANGELOG.md index cf50029..c812289 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,114 @@ to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). ## [Unreleased] +### Added +- Joint trajectories now accept `JointGripperForceTarget(force)` alongside the + legacy exact-width target. It dispatches signed native + `Gripper.Grasp(force)` at the segment boundary: positive closes, negative + opens, and measured width remains state rather than an action. + +### Changed +- The strict trajectory RPC/schema is v4. Gripper targets are explicitly tagged + as `move` or `force`, preventing a force action from being decoded as a width. + +## [0.2.4] - 2026-08-01 + +### Fixed +- Blocking `Gripper.Move` calls now wait for the commanded width instead of + accepting pre-motion stillness during GN01 actuation latency. Obstructed + stillness remains a valid completion condition only for `Gripper.Grasp`. + +## [0.2.3] - 2026-07-31 + +### Added +- Joint trajectories can carry an explicit actuator-target knot 0, exact + per-waypoint gripper targets, and strict authoritative `n_frames` timing. +- The server records acknowledged joint and gripper endpoints as provenance + while rebasing each feedback-MPC prefix to current measured state. + +### Changed +- The joint trajectory wire schema and trajectory RPC identity are v3 and + refuse older or structurally loose payloads before motion. +- Strict joint trajectories are fully prevalidated against effective runtime + joint and `Gripper.params` limits; violations reject instead of clipping or + time-stretching. `Gripper.Move` dispatches fire-and-forget at each segment + boundary while arm streaming continues in the same RPC. + +### Fixed +- Strict execution now bounds the actual first emitted joint target from + measured state on every RPC and derives each prefix's first gripper ramp from + measured width; it repeats both checks immediately before dispatch so state + drift across a mode transition cannot bypass the bound. Prior acknowledged + targets are diagnostic provenance only. +- Fault/contact gates run before mode or actuator writes, pure prevalidation + failures preserve the acknowledged provenance cache, and server lifecycle + boundaries clear it. +- Joint v3 numeric fields reject JSON strings and booleans instead of coercing + them into numbers. + +## [0.2.2] - 2026-07-31 + +### Changed +- Bumped the trajectory RPC identity and server-info schema to v2. The + lease-free identity response now binds control rate, active safety profile, + connected hardware facts, runtime gripper limits, and the effective joint + limit contract. +- Joint trajectories now carry an explicit linear/cosine interpolation law, + honour per-trajectory speed scaling against per-joint runtime limits, and + allocate controller ticks from cumulative waypoint time. +- Flexiv RDK state and gripper-limit reads now fail closed instead of + fabricating zero-valued observations or accepting unchecked commands. + +## [0.2.1] - 2026-07-30 + +### Changed +- Unified the v0.2.0 SpaceMouse/ROS gripper and twist fixes with the canonical + trajectory API used by ActAhead (`JointTrajectory`, + `execute_joint_trajectory`, and the corresponding wire protocol). +- Client and robot-control server deployments can now bind to one exact source + commit instead of combining the trajectory client from a post-v0.1.8 branch + with a divergent v0.2.0 action-chunk server. + +## [0.2.0] - 2026-07-09 + +Fixes from the July 2026 cross-repo audit against `flexiv-spacemouse-teleop`. + +### Fixed +- **SpaceMouse gripper toggle was level-triggered**: holding the button flipped + the gripper open/closed once per control tick (~100 Hz on the default loop). + It now toggles only on the button's rising edge, tracked every tick (also + while the deadman is released and inside `intervention()`), matching the ROS + teleop bridge's behaviour. +- **ROS bridge `~/delta_twist_cmds` treated unitless commands as m/s**: the + node advertises itself as a drop-in target for a MoveIt-Servo teleop + pipeline, but such pipelines publish joystick-style values in `[-1, 1]` that + Servo scales down (0.4 m/s / 0.8 rad/s in the flexiv_ros2 config). Feeding + them straight in executed ~2.5x faster than the Servo path. The node now + defaults to `twist_in_type: unitless` with matching `twist_scale_linear` / + `twist_scale_rotational` parameters (`speed_units` restores the old + behaviour), and drops commands older than `twist_max_age` (default 0.25 s). + +### Changed +- Gripper toggle conventions now match the lab's ROS teleop bridge: GN01 + widths 0.09 m open / 0.01 m close (was 0.08 / 0.0). The initial open/closed + state is inferred from the robot's reported gripper width on first use + (fallback: closed, so the first press opens); override with `initial_open`. +- The gripper button is no longer deadman-gated in `run()`: a rising edge with + the deadman released actuates the gripper with zero motion (previously the + edge was consumed and the internal state flipped while the command was + dropped, desynchronizing state from the physical gripper). +- The ROS bridge integrates twists over the actual inter-message interval + (capped at `twist_max_age`) instead of a fixed `robot.dt`, so the realized + speed no longer scales with the publisher's rate; jog parameters are + validated before the robot is connected. +- `SpaceMouseTeleop` accepts `signs` (six +/-1 values) to calibrate device + axes to the robot frame without a custom source subclass. + +### Housekeeping +- Versions 0.1.6-0.1.8 were lab-internal iterations installed on the control + PC without changelog entries or tags; their changes are folded into this + release. + ## [0.1.5] - 2026-06-19 ### Fixed: 3 more flexiv_rdk-backend bugs from the first real-hardware session diff --git a/CITATION.cff b/CITATION.cff index 807833f..67c21d3 100644 --- a/CITATION.cff +++ b/CITATION.cff @@ -29,4 +29,4 @@ keywords: - real2sim2real - LeRobot license: Apache-2.0 -version: 0.1.0 +version: 0.2.3 diff --git a/docs/action_contract.md b/docs/action_contract.md index 673ee4f..fe19c60 100644 --- a/docs/action_contract.md +++ b/docs/action_contract.md @@ -180,13 +180,16 @@ JointTrajectory( ) ``` -## `GripperCommand` +## `GripperCommand` and direct effort MPC -A parallel-jaw gripper is **continuous**, not binary: the canonical/hardware -command is an opening `width` in metres (+ `force`, `velocity`), matching Flexiv -RDK `Gripper.Move(width, velocity, force_limit)` / `Gripper.Grasp(force)`. The -`0/1` you see in learning benchmarks is a normalized *abstraction* on top of this -continuous width — not a hardware limit. +The library exposes two explicit, non-interchangeable gripper contracts. Legacy +position trajectories use opening `width` through Flexiv RDK +`Gripper.Move(width, velocity, force_limit)`. Direct-effort MPC uses +`JointGripperForceTarget(force)` through `Gripper.Grasp(force)`: positive Newtons +close, negative Newtons open, and measured width is state rather than an action. +The runtime `GripperParams.min_force/max_force` values are authoritative; a +device that does not support a requested signed force is rejected before any +trajectory write. Neither contract is an implicit binary open/close flag. ```python GripperCommand( @@ -200,6 +203,25 @@ GripperCommand( GripperCommand.from_normalized(0.7, span=0.08) # width = 0.7 * 0.08 m ``` +For an effort trajectory, use the separately tagged action: + +```python +JointGripperForceTarget(force=-20.0) # direct opening force, if live limits allow +JointGripperForceTarget(force=35.0) # direct closing force +``` + +If a planner samples a dimensionless signed-effort latent, decode it exactly +once before constructing the RPC payload: + +```python +JointGripperForceTarget.from_signed_effort_latent(z, force_limit=80.0) +# z in [-1,1]: positive closes, negative opens; payload is physical Newtons +``` + +This is intentionally different from the legacy positional +`GripperCommand.from_signed_action`, where positive means open and the decoded +quantity is a width in metres. Neither latent convention appears on the wire. + **`grasp=True` ignores `width` on hardware**: the RDK backend calls `Gripper.Grasp(force)` and the fingers close until contact at the force limit -- a planner that encodes close-intent by thresholding width must not expect diff --git a/docs/index.html b/docs/index.html index def13e6..c8fc90e 100644 --- a/docs/index.html +++ b/docs/index.html @@ -399,8 +399,8 @@

-

Continuous, not binary

-

A parallel-jaw gripper is continuous width (0–0.10 m), not 0/1. The 4-bar actuator is ctrl = 9.404·w − 0.155; GripperCommand(width=…) maps straight through.

+

Continuous position or direct effort

+

Width trajectories use Gripper.Move. Physics-sampling MPC can instead send signed Newton targets with JointGripperForceTarget and Gripper.Grasp; measured width remains state, not the sampled action.

diff --git a/docs/integration_mpc.md b/docs/integration_mpc.md index 3e040a5..b8215c1 100644 --- a/docs/integration_mpc.md +++ b/docs/integration_mpc.md @@ -99,6 +99,75 @@ with RemoteRobot("ROBOT_HOST_IP", 8766, owner="mpc") as r: `RemoteRobot` mirrors the `Robot` API over newline-JSON/TCP, holds the lease with a heartbeat, and installs with only numpy. +## Strict receding-horizon joint prefixes + +For physics-sampling MPC whose action is generalized effort, use +`JointTorqueTrajectory` instead of the position-target example below. It streams +seven additional joint torques at 1 kHz with RDK nonlinear-dynamics compensation +and firmware soft limits enabled, and dispatches an optional +`JointGripperForceTarget` at the same segment boundaries. This API is disabled by +default and is admitted only when the deployment, live `RobotInfo.tau_max`, +safety profile, continuity anchor, and gripper force limits all agree. See +[`joint_torque_mpc.md`](joint_torque_mpc.md). + +Use one atomic `execute_joint_trajectory` call for a multi-segment actuator +prefix. Knot 0 is the previous commanded target, not a fresh measured-state +sample; normal physical tracking lag therefore does not bend the next spline. + +```python +import numpy as np +from flexiv_control import JointGripperTarget, JointTrajectory, JointWaypoint + +knot0 = np.asarray(previous_ack_target, float) # [q0..q6, gripper_width] +knots = np.asarray(prefix_targets, float) # shape (N, 8) +frames = [32, 1] + +traj = JointTrajectory( + initial_positions=knot0[:7], + initial_gripper_width=float(knot0[7]), + waypoints=[ + JointWaypoint( + positions=knot[:7], + gripper=JointGripperTarget( + width=float(knot[7]), + force=20.0, + velocity=None, # derive abs(delta_width) / segment duration + ), + n_frames=n_frames, + ) + for knot, n_frames in zip(knots, frames) + ], + interpolation="linear", + strict_timing=True, + max_joint_speed_scale=0.3, +) +result = robot.execute_joint_trajectory(traj) +``` + +Every strict segment is prevalidated against the effective runtime joint rates +and cached `Gripper.params` before any backend write. Requested `n_frames` are +authoritative: a violation raises instead of clipping or time-stretching. A +provided gripper velocity must equal the velocity that realizes its width delta +in the same segment; `None` derives it. Position-mode `Gripper.Move` is issued +fire-and-forget at the segment boundary, then the arm streams concurrently. + +Knot 0 need not equal the prior commanded endpoint because it is not streamed. +Every feedback-MPC call rebases its joint safety filter and first gripper ramp to +current measured state. The actual first interpolated joint target must be +reachable from that measurement in one controller tick under the effective +per-joint rate limits; gripper velocity is likewise derived and validated from +current measured width. This permits the next plan's knot 0 to reflect ordinary +tracking error without hiding a command jump. The previous acknowledged joint +and gripper targets remain in the result log as provenance, not an admission +gate. The executor repeats the first-setpoint check on the freshest snapshot +after any mode transition, immediately before actuator writes, and recomputes +the first gripper-event ramp from width measured at dispatch. Stop, fault, lease +changes, mode changes, and other mutating RPCs clear +that provenance cache. A payload rejected completely during prevalidation +leaves it intact for diagnosis. The result log also includes requested/scheduled +segment and total ticks, interpolation and execution anchors, gripper events, +and measured gripper tracking. + ## Safety notes for MPC - Keep the safety filter on (it is, by default). A misbehaving solver that diff --git a/docs/integration_teleop.md b/docs/integration_teleop.md index 9b38ce6..bc75ca0 100644 --- a/docs/integration_teleop.md +++ b/docs/integration_teleop.md @@ -100,6 +100,16 @@ ROS-native teleop stack and you do not need to replace it. repo and your RL/MPC code share one controller and one safety profile instead of two parallel control paths. + Mind the **unit semantics** when you rewire: a MoveIt-Servo-style joystick + pipeline publishes *unitless* values in `[-1, 1]` that Servo scales to real + speeds. The bringup node's default (`twist_in_type: unitless`, + `twist_scale_linear: 0.4` m/s, `twist_scale_rotational: 0.8` rad/s) matches + the flexiv_ros2 Servo config, so the same command stream moves at the same + speed. If your publisher sends real m/s / rad/s instead, set + `twist_in_type: speed_units`. Stale commands older than `twist_max_age` + (default 0.25 s) are dropped. Re-verify speeds on the real arm after any + rewiring. + Either way, the win is a single execution-and-safety layer underneath teleop, RL, MPC, and a high-level planner — not three different ways to command the same arm. diff --git a/docs/joint_torque_mpc.md b/docs/joint_torque_mpc.md new file mode 100644 index 0000000..485eecb --- /dev/null +++ b/docs/joint_torque_mpc.md @@ -0,0 +1,55 @@ +# Direct joint-torque MPC execution + +`flexiv-control` exposes `JointTorqueTrajectory` for controllers whose sampled +action is physical joint effort rather than a joint-position or velocity +target. It is intentionally separate from `JointTrajectory`: callers cannot +accidentally reinterpret torque as position. + +Hardware execution is fail closed unless all of the following hold: + +- the robot configuration explicitly sets `allow_joint_torque: true`; +- the server runs at exactly 1 kHz; +- Flexiv RDK exposes `RT_JOINT_TORQUE` and `StreamJointTorque`; +- live `RobotInfo.tau_max` exists and has one positive value per joint; +- the requested `max_joint_torque_scale` does not exceed the active profile; +- every initial/waypoint torque is within the scaled live limits; +- the normal lease, E-stop, fault, wrench, cancellation, and soft-limit gates + remain active. + +The RDK call enables nonlinear dynamics compensation and firmware soft limits. +Waypoint endpoints are linearly connected at 1 kHz because Flexiv requires +smooth continuous torque commands. Gripper force events are synchronized with +the same segment boundaries and remain physical Newton commands, not width +targets. + +Keep the planner's dimensionless semantic coordinate separate from the wire +action. For ActAhead's convention, `latent=+1` means maximum closing force, +`latent=-1` maximum opening force, and decoding happens exactly once: + +```python +target = JointGripperForceTarget.from_signed_effort_latent( + latent, + force_limit=80.0, # deployment value; still checked against live limits +) +``` + +The resulting target stores and transmits only `force` in Newtons. It does not +transmit the latent or a desired width. Do not use +`GripperCommand.from_signed_action` for this path: that legacy positional API +uses the opposite sign convention (`+1` means open) and decodes to metres. + +This matches the public Flexiv RDK semantics: `Gripper.Grasp(force)` is direct +force control, with positive force closing and negative force opening. Admission +is always checked against the connected gripper's live `min_force` and +`max_force`; the controller never assumes that a particular GN01 firmware +supports the full signed range. + +This API supplies an execution mechanism, not a planner objective. Sampling a +torque and penalizing squared torque in a cost function are independent design +choices. + +On success, `ExecutionResult.log` acknowledges the physical command endpoints +as `acknowledged_ending_joint_torque_nm` and +`acknowledged_ending_gripper_force_n`. `ExecutionResult.final_state` remains +the measured end state, including gripper width and measured force; callers +must not reconstruct measured aperture from the semantic latent. diff --git a/pyproject.toml b/pyproject.toml index 564b2a2..6c79aa4 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -79,3 +79,8 @@ addopts = "-q" [tool.ruff] line-length = 100 target-version = "py38" + +[tool.ruff.lint] +# Pin the repository's historical Ruff default instead of inheriting changes +# to the tool's default rule set across releases. +select = ["E4", "E7", "E9", "F"] diff --git a/ros2/flexiv_control_bringup/flexiv_control_bringup/control_node.py b/ros2/flexiv_control_bringup/flexiv_control_bringup/control_node.py index 9113e61..8eef879 100644 --- a/ros2/flexiv_control_bringup/flexiv_control_bringup/control_node.py +++ b/ros2/flexiv_control_bringup/flexiv_control_bringup/control_node.py @@ -37,7 +37,6 @@ """ from __future__ import annotations -import numpy as np import rclpy from rclpy.action import ActionServer, CancelResponse, GoalResponse from rclpy.callback_groups import ReentrantCallbackGroup @@ -71,12 +70,30 @@ def __init__(self) -> None: self.declare_parameter("safety_profile", "tabletop_safe") self.declare_parameter("state_rate", 50.0) self.declare_parameter("joint_names", [""]) + # ~/delta_twist_cmds semantics. "unitless" matches a MoveIt-Servo-style + # joystick pipeline (values in [-1, 1] scaled by twist_scale_*, the + # same convention as flexiv_ros2's Servo config); "speed_units" treats + # the twist directly as m/s and rad/s. + self.declare_parameter("twist_in_type", "unitless") + self.declare_parameter("twist_scale_linear", 0.4) # m/s at full deflection + self.declare_parameter("twist_scale_rotational", 0.8) # rad/s at full deflection + self.declare_parameter("twist_max_age", 0.25) # s; 0 disables the check backend = self.get_parameter("backend").value serial = self.get_parameter("robot_serial").value control_hz = float(self.get_parameter("control_hz").value) profile = self.get_parameter("safety_profile").value + # Validate jog parameters before touching the hardware so a config + # error cannot leave a connected robot in servo mode behind a crash. + self.twist_in_type = str(self.get_parameter("twist_in_type").value) + if self.twist_in_type not in ("unitless", "speed_units"): + raise ValueError(f"twist_in_type must be 'unitless' or 'speed_units', got {self.twist_in_type!r}") + self.twist_scale_linear = float(self.get_parameter("twist_scale_linear").value) + self.twist_scale_rotational = float(self.get_parameter("twist_scale_rotational").value) + self.twist_max_age = float(self.get_parameter("twist_max_age").value) + self._last_twist_time = None + self.get_logger().info( f"starting flexiv_control bridge: backend={backend} profile={profile}" ) @@ -152,11 +169,38 @@ def _publish_state(self) -> None: # -- jog (MoveIt-Servo-compatible) -------------------------------------- def _on_twist(self, msg: TwistStamped) -> None: - # Treat the twist as a per-tick velocity; integrate over one control dt. - dt = self.robot.dt + # Drop stale commands: replaying an old twist after a hiccup would jerk + # the arm. A zero stamp is allowed (some publishers do not stamp). + if self.twist_max_age > 0.0 and (msg.header.stamp.sec or msg.header.stamp.nanosec): + age = (self.get_clock().now() - rclpy.time.Time.from_msg(msg.header.stamp)).nanoseconds * 1e-9 + if age > self.twist_max_age: + self.get_logger().warn( + f"Ignoring stale twist command ({age:.2f}s old)", throttle_duration_sec=2.0 + ) + return + lin = msg.twist.linear ang = msg.twist.angular - delta = [lin.x * dt, lin.y * dt, lin.z * dt, ang.x * dt, ang.y * dt, ang.z * dt] + v = [lin.x, lin.y, lin.z, ang.x, ang.y, ang.z] + if self.twist_in_type == "unitless": + # Joystick convention: clip to [-1, 1], then scale to real speeds. + v = [max(-1.0, min(1.0, x)) for x in v] + v = [x * self.twist_scale_linear for x in v[:3]] + [ + x * self.twist_scale_rotational for x in v[3:] + ] + + # Integrate the velocity over the ACTUAL time since the previous + # command (like MoveIt Servo), so the realized speed does not depend + # on the publisher's rate. The interval is capped so a long gap after + # a pause cannot produce one large jump. + now_s = self.get_clock().now().nanoseconds * 1e-9 + max_gap = self.twist_max_age if self.twist_max_age > 0.0 else 0.1 + if self._last_twist_time is None: + dt = self.robot.dt + else: + dt = min(max(now_s - self._last_twist_time, 0.0), max_gap) + self._last_twist_time = now_s + delta = [x * dt for x in v] if any(abs(d) > 1e-9 for d in delta): try: self.robot.servo_cartesian_delta(delta, duration=dt) diff --git a/src/flexiv_control/__init__.py b/src/flexiv_control/__init__.py index 3baba42..4979adc 100644 --- a/src/flexiv_control/__init__.py +++ b/src/flexiv_control/__init__.py @@ -24,7 +24,7 @@ from __future__ import annotations -__version__ = "0.1.8" +__version__ = "0.4.0.dev0" # --- core data types from .types import ( # noqa: F401 @@ -45,8 +45,12 @@ CartesianWaypoint, TrajectoryRepresentation, ExecutionResult, + JointGripperForceTarget, + JointGripperTarget, JointTrajectory, JointWaypoint, + JointTorqueTrajectory, + JointTorqueWaypoint, ) # --- safety + config @@ -80,8 +84,12 @@ "CartesianWaypoint", "TrajectoryRepresentation", "ExecutionResult", + "JointGripperForceTarget", + "JointGripperTarget", "JointTrajectory", "JointWaypoint", + "JointTorqueTrajectory", + "JointTorqueWaypoint", # safety + config "SafetyFilter", "SafetyProfile", diff --git a/src/flexiv_control/backends/base.py b/src/flexiv_control/backends/base.py index 6a1bab8..aad5d94 100644 --- a/src/flexiv_control/backends/base.py +++ b/src/flexiv_control/backends/base.py @@ -37,6 +37,16 @@ class RobotBackend(abc.ABC): n_joints: int = 7 + def runtime_info(self) -> dict: + """Return immutable facts cached from the connected backend. + + The control server exposes this snapshot without touching hardware on + each identity request. Backends that cannot provide hardware identity + return an empty mapping; hardware backends should fail during connect + rather than fabricate required fields. + """ + return {} + # -- lifecycle ---------------------------------------------------------- @abc.abstractmethod def connect(self) -> None: ... @@ -74,6 +84,11 @@ def stream_cartesian( @abc.abstractmethod def stream_joint(self, q: np.ndarray) -> None: ... + @abc.abstractmethod + def stream_joint_torque(self, tau: np.ndarray) -> None: + """Stream one gravity-compensated joint-torque command in Nm.""" + ... + # -- gripper ------------------------------------------------------------ @abc.abstractmethod def move_gripper(self, cmd: GripperCommand) -> None: ... diff --git a/src/flexiv_control/backends/fake.py b/src/flexiv_control/backends/fake.py index 3c0eb08..51a15df 100644 --- a/src/flexiv_control/backends/fake.py +++ b/src/flexiv_control/backends/fake.py @@ -68,6 +68,7 @@ def __init__( # Recorded command log (for tests / debugging). self.cartesian_log: List[np.ndarray] = [] self.joint_log: List[np.ndarray] = [] + self.joint_torque_log: List[np.ndarray] = [] self.gripper_log: List[GripperCommand] = [] self.mode_log: List[ControlMode] = [] @@ -151,6 +152,14 @@ def stream_joint(self, q: np.ndarray) -> None: a = self._alpha self._q = (1 - a) * self._q + a * q + def stream_joint_torque(self, tau: np.ndarray) -> None: + if self._mode != ControlMode.RT_JOINT_TORQUE: + raise RuntimeError( + "stream_joint_torque requires RT_JOINT_TORQUE mode" + ) + command = np.asarray(tau, dtype=float).reshape(self.n_joints) + self.joint_torque_log.append(command.copy()) + def set_contact_wrench_limit(self, wrench: np.ndarray) -> None: # Mirror the firmware guard so tests can assert apply/restore. self._max_wrench = np.asarray(wrench, float).copy() diff --git a/src/flexiv_control/backends/flexiv_rdk.py b/src/flexiv_control/backends/flexiv_rdk.py index c1fca34..19a881b 100644 --- a/src/flexiv_control/backends/flexiv_rdk.py +++ b/src/flexiv_control/backends/flexiv_rdk.py @@ -19,6 +19,7 @@ from __future__ import annotations +import os import time from typing import Any, Optional @@ -83,6 +84,49 @@ def _get(obj: Any, *names: str, default=None): return default +def _required_attr(obj: Any, *names: str, context: str) -> Any: + """Read a required cross-version RDK field without a synthetic default.""" + sentinel = object() + value = _get(obj, *names, default=sentinel) + if value is sentinel: + raise RuntimeError( + f"Flexiv RDK {context} is missing required field " + f"{'/'.join(names)}" + ) + return value + + +def _required_vector( + obj: Any, + *names: str, + size: int, + context: str, +) -> np.ndarray: + value = np.asarray( + _required_attr(obj, *names, context=context), + dtype=float, + ).reshape(-1) + if value.shape != (size,): + raise RuntimeError( + f"Flexiv RDK {context}.{names[0]} has shape {value.shape}, " + f"expected ({size},)" + ) + if not np.all(np.isfinite(value)): + raise RuntimeError( + f"Flexiv RDK {context}.{names[0]} contains non-finite values" + ) + return value + + +def _required_float(obj: Any, name: str, *, context: str) -> float: + value = float(_required_attr(obj, name, context=context)) + if not np.isfinite(value): + raise RuntimeError( + f"Flexiv RDK {context}.{name} is not finite" + ) + return value + + def _rdk_coord(frame: str): """Map a frame name to ``flexivrdk.CoordType``. @@ -142,6 +186,8 @@ def __init__( self._gripper = None self._mode = ControlMode.IDLE self._connected = False + self._runtime_info: dict = {} + self._gripper_limits: Optional[dict[str, float | str]] = None # The active Cartesian force-control params, kept so stream_cartesian can # actually command the configured target_wrench (not just a zero default). self._force_control: Optional[ForceControlParams] = None @@ -185,6 +231,149 @@ def connect(self) -> None: except Exception: pass + # Cache immutable hardware facts from the already-connected RDK owner. + # get_server_info serves only this snapshot; it never opens a second + # robot connection or performs a state read. + robot_info = self._robot.info() + actual_serial = str( + _required_attr( + robot_info, + "serial_num", + context="RobotInfo", + ) + ).strip() + if not actual_serial: + raise RuntimeError("Flexiv RDK RobotInfo.serial_num is empty") + if self.robot_sn and actual_serial != self.robot_sn: + raise RuntimeError( + f"configured robot_sn={self.robot_sn!r}, but RDK reports " + f"serial_num={actual_serial!r}" + ) + dof = int( + _required_attr(robot_info, "DoF", context="RobotInfo") + ) + if dof != self.n_joints: + raise RuntimeError( + f"RDK RobotInfo.DoF={dof} disagrees with n_joints=" + f"{self.n_joints}" + ) + robot_q_min = _required_vector( + robot_info, + "q_min", + size=dof, + context="RobotInfo", + ) + robot_q_max = _required_vector( + robot_info, + "q_max", + size=dof, + context="RobotInfo", + ) + robot_dq_max = _required_vector( + robot_info, + "dq_max", + size=dof, + context="RobotInfo", + ) + robot_tau_max = _required_vector( + robot_info, + "tau_max", + size=dof, + context="RobotInfo", + ) + if np.any(robot_q_min >= robot_q_max): + raise RuntimeError( + "Flexiv RDK RobotInfo joint position limits are not ordered" + ) + if np.any(robot_dq_max <= 0.0): + raise RuntimeError( + "Flexiv RDK RobotInfo joint velocity limits must be > 0" + ) + if np.any(robot_tau_max <= 0.0): + raise RuntimeError( + "Flexiv RDK RobotInfo joint torque limits must be > 0" + ) + tool = flexivrdk.Tool(self._robot) + tool_name = str(tool.name()).strip() + if not tool_name: + raise RuntimeError( + "Flexiv RDK current tool profile name is empty" + ) + self._runtime_info = { + "runtime_hardware_identity": { + "robot_serial": actual_serial, + "robot_model": str( + _required_attr( + robot_info, + "model_name", + context="RobotInfo", + ) + ), + "robot_software_version": str( + _required_attr( + robot_info, + "software_ver", + context="RobotInfo", + ) + ), + "flexivrdk_version": str( + getattr(flexivrdk, "__version__", "unknown") + ), + "tool_profile": tool_name, + }, + "joint_limits": { + "source": "flexivrdk.Robot.info", + "position_min_rad": robot_q_min.tolist(), + "position_max_rad": robot_q_max.tolist(), + "velocity_max_rad_s": robot_dq_max.tolist(), + "torque_max_nm": robot_tau_max.tolist(), + }, + } + + # Safety.current_limits() is read-only, but constructing Safety requires + # the configured safety password. Never hard-code or expose it. If the + # deployment supplies FLEXIV_RDK_SAFETY_PASSWORD, cache the actual + # active firmware limits; otherwise RobotInfo limits remain available. + safety_password = os.environ.get("FLEXIV_RDK_SAFETY_PASSWORD") + if safety_password: + safety = flexivrdk.Safety(self._robot, safety_password) + current = safety.current_limits() + current_q_min = _required_vector( + current, + "q_min", + size=dof, + context="SafetyLimits", + ) + current_q_max = _required_vector( + current, + "q_max", + size=dof, + context="SafetyLimits", + ) + current_dq_normal = _required_vector( + current, + "dq_max_normal", + size=dof, + context="SafetyLimits", + ) + current_dq_reduced = _required_vector( + current, + "dq_max_reduced", + size=dof, + context="SafetyLimits", + ) + self._runtime_info["current_safety_limits"] = { + "source": "flexivrdk.Safety.current_limits", + "position_min_rad": current_q_min.tolist(), + "position_max_rad": current_q_max.tolist(), + "velocity_max_normal_rad_s": ( + current_dq_normal.tolist() + ), + "velocity_max_reduced_rad_s": ( + current_dq_reduced.tolist() + ), + } + # Force-control modes (NRT_CARTESIAN_MOTION_FORCE -- our cartesian impedance) # REQUIRE the 6-DoF F/T sensor to be zeroed first, else SwitchMode faults with # event 301004 ("FT sensor is not calibrated using primitive [ZeroFTSensor]"). @@ -214,30 +403,94 @@ def connect(self) -> None: # Calling Init() WITHOUT Enable(name) first fails with # "[flexiv::rdk::Gripper::Init] No gripper enabled" and leaves every # gripper command a silent no-op -- which is exactly the trap this used - # to fall into (it tried Init() first and never reached Enable). Surface - # any failure as a warning rather than swallowing it. + # to fall into (it tried Init() first and never reached Enable). A + # configured gripper is part of the hardware contract, so fail connect. try: if hasattr(self._gripper, "Enable"): self._gripper.Enable(self._gripper_name) if hasattr(self._gripper, "Init"): self._gripper.Init() + params = self._gripper.params() + limits: dict[str, float | str] = { + "source": "flexivrdk.Gripper.params", + "device_name": str( + _required_attr( + params, + "name", + context="GripperParams", + ) + ), + "min_width_m": _required_float( + params, + "min_width", + context="GripperParams", + ), + "max_width_m": _required_float( + params, + "max_width", + context="GripperParams", + ), + "min_velocity_m_s": _required_float( + params, + "min_vel", + context="GripperParams", + ), + "max_velocity_m_s": _required_float( + params, + "max_vel", + context="GripperParams", + ), + "min_force_n": _required_float( + params, + "min_force", + context="GripperParams", + ), + "max_force_n": _required_float( + params, + "max_force", + context="GripperParams", + ), + } + for lo, hi in ( + ("min_width_m", "max_width_m"), + ("min_velocity_m_s", "max_velocity_m_s"), + ("min_force_n", "max_force_n"), + ): + if float(limits[lo]) > float(limits[hi]): + raise RuntimeError( + f"Flexiv RDK gripper limits {lo}/{hi} are " + "not ordered" + ) + self._gripper_limits = limits + self._runtime_info["gripper_limits"] = dict(limits) + self._runtime_info[ + "runtime_hardware_identity" + ]["gripper_device"] = str(limits["device_name"]) except Exception as e: # pragma: no cover - hardware-only path - import warnings - - warnings.warn( - f"gripper init failed for device {self._gripper_name!r} ({e}); " - f"gripper commands will be no-ops. Check the device name against " - f"Flexiv Elements -> Settings -> Device.", - RuntimeWarning, - stacklevel=2, - ) self._gripper = None + self._gripper_limits = None + raise RuntimeError( + f"gripper init failed for configured device " + f"{self._gripper_name!r}; refusing to start without its " + "runtime state and limits" + ) from e elif self._gripper_name is not None: # Explicit empty name ("") = no gripper configured; skip cleanly (no # scary warning) rather than attempting an init that cannot succeed. self._gripper = None self._connected = True + def runtime_info(self) -> dict: + """Return connect-time hardware facts without touching the robot.""" + return { + key: ( + dict(value) + if isinstance(value, dict) + else value + ) + for key, value in self._runtime_info.items() + } + def disconnect(self) -> None: if self._robot is not None: try: @@ -265,32 +518,60 @@ def read_state(self) -> RobotState: except Exception: # pragma: no cover - defensive: mode() is optional info pass s = self._robot.states() # VERIFY: states() returns RobotStates - q = np.asarray(_get(s, "q", default=np.zeros(self.n_joints)), float) - dq = np.asarray(_get(s, "dq", "dtheta", default=np.zeros(self.n_joints)), float) - tau = np.asarray(_get(s, "tau", default=np.zeros(self.n_joints)), float) - tcp_pose = np.asarray(_get(s, "tcp_pose", "tcpPose", default=[0, 0, 0, 1, 0, 0, 0]), float) - tcp_vel = np.asarray(_get(s, "tcp_vel", "tcpVel", default=np.zeros(CART_DOF)), float) - wrench = np.asarray( - _get(s, "ext_wrench_in_tcp", "ext_wrench_in_world", "extWrenchInTcp", - default=np.zeros(CART_DOF)), - float, + q = _required_vector( + s, "q", size=self.n_joints, context="RobotStates" + ) + dq = _required_vector( + s, + "dq", + "dtheta", + size=self.n_joints, + context="RobotStates", + ) + tau = _required_vector( + s, "tau", size=self.n_joints, context="RobotStates" + ) + tcp_pose = _required_vector( + s, + "tcp_pose", + "tcpPose", + size=7, + context="RobotStates", + ) + tcp_vel = _required_vector( + s, + "tcp_vel", + "tcpVel", + size=CART_DOF, + context="RobotStates", + ) + wrench = _required_vector( + s, + "ext_wrench_in_tcp", + "ext_wrench_in_world", + "extWrenchInTcp", + size=CART_DOF, + context="RobotStates", ) gw, gf, gm = 0.0, 0.0, False + if self._gripper is None and self._gripper_name: + raise RuntimeError( + f"configured gripper {self._gripper_name!r} is unavailable; " + "refusing to fabricate gripper state" + ) if self._gripper is not None: - try: - gs = self._gripper.states() # VERIFY - gw = float(_get(gs, "width", default=0.0)) - gf = float(_get(gs, "force", default=0.0)) - # v1.x exposes motion via the Gripper.moving() METHOD, not a - # states() field (GripperStates has no is_moving in v1.x, so the - # old field read silently always returned False). - if hasattr(self._gripper, "moving"): - gm = bool(self._gripper.moving()) - else: - gm = bool(_get(gs, "is_moving", "isMoving", default=False)) - except Exception: - pass + gs = self._gripper.states() # VERIFY + gw = _required_float(gs, "width", context="GripperStates") + gf = _required_float(gs, "force", context="GripperStates") + gm = bool( + _required_attr( + gs, + "is_moving", + "isMoving", + context="GripperStates", + ) + ) # Surface a robot-side fault through the state so the control loop (and # any client polling get_state) sees it -- previously hardcoded OK. @@ -308,10 +589,12 @@ def read_state(self) -> RobotState: def in_fault(self) -> bool: """True if the robot reports a fault / protective stop (RDK ``fault()``).""" fault_fn = _get(self._robot, "fault", default=None) + if not callable(fault_fn): + return True try: - return bool(fault_fn()) if callable(fault_fn) else False + return bool(fault_fn()) except Exception: - return False + return True # -- mode --------------------------------------------------------------- def set_mode( @@ -399,6 +682,19 @@ def stream_joint(self, q: np.ndarray) -> None: max_acc = [1.0] * self.n_joints self._robot.SendJointPosition(q, zeros, zeros, max_vel, max_acc) + def stream_joint_torque(self, tau: np.ndarray) -> None: + if self._mode != ControlMode.RT_JOINT_TORQUE: + raise RuntimeError( + "stream_joint_torque requires RT_JOINT_TORQUE mode" + ) + command = [ + float(value) + for value in np.asarray(tau, dtype=float).reshape(self.n_joints) + ] + # RDK v1.x: nonlinear dynamics compensation and firmware soft limits + # remain enabled. The command is the additional generalized torque. + self._robot.StreamJointTorque(command, True, True) + # -- gripper ------------------------------------------------------------ def move_gripper(self, cmd: GripperCommand) -> None: if self._gripper is None: @@ -416,6 +712,37 @@ def move_gripper(self, cmd: GripperCommand) -> None: f"gripper_name: '' for a gripper-less config." ) return + if self._gripper_limits is None: + raise RuntimeError( + "runtime gripper limits are unavailable; refusing command" + ) + checks = [("force", cmd.force, "min_force_n", "max_force_n")] + if not cmd.grasp: + checks.extend( + [ + ( + "width", + cmd.width, + "min_width_m", + "max_width_m", + ), + ( + "velocity", + cmd.velocity, + "min_velocity_m_s", + "max_velocity_m_s", + ), + ] + ) + for field, raw, lo_key, hi_key in checks: + value = float(raw) + lo = float(self._gripper_limits[lo_key]) + hi = float(self._gripper_limits[hi_key]) + if not np.isfinite(value) or not lo <= value <= hi: + raise ValueError( + f"gripper {field}={value!r} outside runtime " + f"[{lo}, {hi}] from flexivrdk.Gripper.params" + ) if cmd.grasp: self._gripper.Grasp(cmd.force) # VERIFY else: diff --git a/src/flexiv_control/backends/mujoco.py b/src/flexiv_control/backends/mujoco.py index 053f1fc..2590329 100644 --- a/src/flexiv_control/backends/mujoco.py +++ b/src/flexiv_control/backends/mujoco.py @@ -197,6 +197,19 @@ def stream_joint(self, q: np.ndarray) -> None: d.ctrl[self._act] = q self._step_n(self._substeps()) + def stream_joint_torque(self, tau: np.ndarray) -> None: + if self._mode != ControlMode.RT_JOINT_TORQUE: + raise RuntimeError( + "stream_joint_torque requires RT_JOINT_TORQUE mode" + ) + m, d = self._m, self._d + command = np.asarray(tau, dtype=float).reshape(self.n_joints) + lo = m.actuator_ctrlrange[self._act, 0] + hi = m.actuator_ctrlrange[self._act, 1] + command = np.where(hi > lo, np.clip(command, lo, hi), command) + d.ctrl[self._act] = command + self._step_n(self._substeps()) + def stream_cartesian(self, pose: np.ndarray, wrench: Optional[np.ndarray] = None) -> None: mj, m, d = self._mj, self._m, self._d pose = np.asarray(pose, float).reshape(7) diff --git a/src/flexiv_control/client/remote_robot.py b/src/flexiv_control/client/remote_robot.py index 850cbf1..b41a252 100644 --- a/src/flexiv_control/client/remote_robot.py +++ b/src/flexiv_control/client/remote_robot.py @@ -117,11 +117,19 @@ def __exit__(self, *exc) -> None: # Methods whose server-side handler can legitimately block for the duration # of a real robot motion: read them with `motion_timeout` instead of the # short default. - _MOTION_METHODS = frozenset({ - "execute_cartesian_trajectory", "execute_joint_trajectory", "move_joint", - "servo_cartesian_delta", "servo_cartesian_pose", - "command_gripper", "home", "go_home_safe", "zero_ft_sensor", - }) + _MOTION_METHODS = frozenset( + { + "execute_cartesian_trajectory", + "execute_joint_trajectory", + "move_joint", + "servo_cartesian_delta", + "servo_cartesian_pose", + "command_gripper", + "home", + "go_home_safe", + "zero_ft_sensor", + } + ) def _call(self, method: str, **params) -> dict: if self._wfile is None or self._rfile is None: @@ -216,6 +224,13 @@ def _heartbeat(self) -> None: break # -- mirror of the Robot API -------------------------------------------- + def get_server_info(self) -> dict: + """Return the remote package/protocol identity without taking a lease.""" + info = self._call("get_server_info") + if not isinstance(info, dict): + raise RemoteRobotError("get_server_info returned a non-object response") + return info + def set_safety_profile(self, name: str) -> None: self._call("set_safety_profile", owner=self.owner, name=name) @@ -302,8 +317,23 @@ def execute_cartesian_trajectory( def execute_joint_trajectory( self, traj: JointTrajectory, *, raise_on_stop: bool = False ) -> ExecutionResult: + # Fail closed before motion when an older server would silently ignore + # explicit knot 0 or strict timing fields. + info = self.get_server_info() + if ( + info.get("protocol_id") != P.PROTOCOL_ID + or info.get("protocol_fingerprint_sha256") != P.PROTOCOL_FINGERPRINT_SHA256 + ): + raise RemoteRobotError( + "joint trajectory protocol mismatch: client requires " + f"{P.PROTOCOL_ID} fingerprint {P.PROTOCOL_FINGERPRINT_SHA256}" + ) r = self._call( - "execute_joint_trajectory", owner=self.owner, traj=P.joint_trajectory_to_dict(traj) + "execute_joint_trajectory", + owner=self.owner, + protocol_id=P.PROTOCOL_ID, + protocol_fingerprint_sha256=P.PROTOCOL_FINGERPRINT_SHA256, + traj=P.joint_trajectory_to_dict(traj), ) result = P.result_from_dict(r["result"]) if raise_on_stop and not result.success: @@ -312,6 +342,32 @@ def execute_joint_trajectory( raise TrajectoryStoppedError(result) return result + def execute_joint_torque_trajectory( + self, traj, *, raise_on_stop: bool = False + ) -> ExecutionResult: + info = self.get_server_info() + if ( + info.get("protocol_id") != P.PROTOCOL_ID + or info.get("protocol_fingerprint_sha256") + != P.PROTOCOL_FINGERPRINT_SHA256 + ): + raise RemoteRobotError( + "joint torque trajectory protocol mismatch" + ) + response = self._call( + "execute_joint_torque_trajectory", + owner=self.owner, + protocol_id=P.PROTOCOL_ID, + protocol_fingerprint_sha256=P.PROTOCOL_FINGERPRINT_SHA256, + traj=P.joint_torque_trajectory_to_dict(traj), + ) + result = P.result_from_dict(response["result"]) + if raise_on_stop and not result.success: + from ..robot import TrajectoryStoppedError + + raise TrajectoryStoppedError(result) + return result + def move_joint( self, q_target, @@ -408,7 +464,8 @@ def start_servo_loop(self, *, control_hz: Optional[float] = None) -> None: def servo_stream(self, pose, gripper: Optional[GripperCommand] = None) -> None: self._call( - "servo_stream", owner=self.owner, + "servo_stream", + owner=self.owner, pose=np.asarray(pose, float).reshape(7).tolist(), gripper=P.gripper_to_dict(gripper), ) diff --git a/src/flexiv_control/config.py b/src/flexiv_control/config.py index fd3ae4f..9ab57fa 100644 --- a/src/flexiv_control/config.py +++ b/src/flexiv_control/config.py @@ -101,6 +101,10 @@ class RobotConfig: gripper_name: Optional[str] = None n_joints: int = 7 control_hz: float = 100.0 + # Direct joint torque is an expert/research interface. It is disabled by + # default and must be enabled by the robot deployment config before the + # facade can enter RT_JOINT_TORQUE or emit one torque command. + allow_joint_torque: bool = False default_safety_profile: str = "tabletop_safe" q_home: np.ndarray = field( default_factory=lambda: np.array([0.0, -0.7, 0.0, 1.6, 0.0, 0.9, 0.0], float) @@ -132,6 +136,9 @@ def load(cls, path_or_name: str) -> "RobotConfig": c.gripper_name = d.get("gripper_name", c.gripper_name) c.n_joints = int(d.get("n_joints", c.n_joints)) c.control_hz = float(d.get("control_hz", c.control_hz)) + c.allow_joint_torque = bool( + d.get("allow_joint_torque", c.allow_joint_torque) + ) c.default_safety_profile = d.get("default_safety_profile", c.default_safety_profile) if "q_home" in d: c.q_home = np.asarray(d["q_home"], float) diff --git a/src/flexiv_control/configs/robots/rizon4s_actahead_lab.yaml b/src/flexiv_control/configs/robots/rizon4s_actahead_lab.yaml index 309f345..f23cacf 100644 --- a/src/flexiv_control/configs/robots/rizon4s_actahead_lab.yaml +++ b/src/flexiv_control/configs/robots/rizon4s_actahead_lab.yaml @@ -7,7 +7,9 @@ robot_sn: "Rizon4s-062626" # GN01 gripper: the device name Flexiv Elements -> Settings -> Device reports for it. gripper_name: "Flexiv-GN01" n_joints: 7 -control_hz: 100.0 +# Flexiv RT_JOINT_TORQUE consumes a smooth command every 1 ms. +control_hz: 1000.0 +allow_joint_torque: true default_safety_profile: actahead_lab # Restore/initial pose used in the lab. Source of truth: ActAhead # config/lab_home_posture.json (read-only RDK probe of the real arm); the diff --git a/src/flexiv_control/configs/safety/actahead_lab.yaml b/src/flexiv_control/configs/safety/actahead_lab.yaml index ac36fad..3e3a9a6 100644 --- a/src/flexiv_control/configs/safety/actahead_lab.yaml +++ b/src/flexiv_control/configs/safety/actahead_lab.yaml @@ -31,6 +31,8 @@ tcp_limits: joint_limits: margin_rad: 0.08 max_joint_speed_scale: 0.30 + # Fraction of the connected robot's RobotInfo.tau_max available to MPC. + max_joint_torque_scale: 0.30 contact: max_wrench: [40, 40, 40, 5, 5, 5] diff --git a/src/flexiv_control/interpolation.py b/src/flexiv_control/interpolation.py index 81cd880..37c43da 100644 --- a/src/flexiv_control/interpolation.py +++ b/src/flexiv_control/interpolation.py @@ -112,25 +112,99 @@ def __init__( start_q: np.ndarray, control_hz: float, *, - max_joint_speed: Optional[float] = None, + max_joint_speed: Optional[np.ndarray | float] = None, ): self.traj = traj self.hz = float(control_hz) self.dt = 1.0 / self.hz - self.start_q = np.asarray(start_q, float) - self.max_joint_speed = max_joint_speed + self.start_q = np.asarray(start_q, float).reshape(-1).copy() + self.max_joint_speed: Optional[np.ndarray] + if max_joint_speed is None: + self.max_joint_speed = None + else: + speed = np.asarray(max_joint_speed, dtype=float) + if speed.ndim == 0: + speed = np.full(self.start_q.shape, float(speed)) + else: + speed = speed.reshape(-1) + if speed.shape != self.start_q.shape: + raise ValueError( + "max_joint_speed must be scalar or match start_q shape " + f"{self.start_q.shape}, got {speed.shape}" + ) + if not np.all(np.isfinite(speed)) or np.any(speed <= 0.0): + raise ValueError("max_joint_speed values must be finite and > 0") + self.max_joint_speed = speed + + cumulative_requested_ticks = 0.0 + cumulative_scheduled_ticks = 0 + self.requested_duration_s = 0.0 + self.requested_segment_ticks: list[int] = [] + for wp in self.traj.waypoints: + duration_s = float(wp.resolve_duration(self.hz)) + if not np.isfinite(duration_s) or duration_s <= 0.0: + raise ValueError("JointWaypoint duration must be finite and > 0") + self.requested_duration_s += duration_s + cumulative_requested_ticks += duration_s * self.hz + boundary = max( + cumulative_scheduled_ticks + 1, + int(np.floor(cumulative_requested_ticks + 0.5)), + ) + self.requested_segment_ticks.append(boundary - cumulative_scheduled_ticks) + cumulative_scheduled_ticks = boundary + self.requested_total_ticks = cumulative_scheduled_ticks + # Backward-compatible names used by callers/tests. + self.nominal_segment_ticks = list(self.requested_segment_ticks) + self.nominal_total_ticks = self.requested_total_ticks + + peak_factor = 1.0 if traj.interpolation == "linear" else _BLEND_PEAK + self.scheduled_segment_ticks: list[int] = [] + prev = self.start_q + for segment_index, (wp, requested_n) in enumerate( + zip(self.traj.waypoints, self.requested_segment_ticks) + ): + tgt = wp.positions + if tgt.shape != self.start_q.shape: + raise ValueError( + f"JointWaypoint {segment_index} positions must match " + f"start_q shape {self.start_q.shape}, got {tgt.shape}" + ) + n = int(requested_n) + if self.max_joint_speed is not None: + dq = np.abs(tgt - prev) + required = peak_factor * dq / (n * self.dt) + too_fast = required > self.max_joint_speed + 1e-12 + if traj.strict_timing and np.any(too_fast): + joint = int(np.flatnonzero(too_fast)[0]) + raise ValueError( + "strict_timing joint rate exceeds effective runtime " + f"limit at segment {segment_index}, joint {joint}: " + f"required {required[joint]:.9g} rad/s > " + f"{self.max_joint_speed[joint]:.9g} rad/s; " + f"requested n_frames={n} is authoritative" + ) + if not traj.strict_timing: + n = max( + n, + int(np.max(np.ceil(peak_factor * dq / (self.max_joint_speed * self.dt)))), + ) + self.scheduled_segment_ticks.append(max(1, n)) + prev = tgt + self.scheduled_total_ticks = int(sum(self.scheduled_segment_ticks)) + self.current_segment = 0 + self.current_segment_tick = 0 def __iter__(self) -> Iterator[np.ndarray]: prev = self.start_q.copy() - for wp in self.traj.waypoints: + for segment_index, (wp, n) in enumerate( + zip(self.traj.waypoints, self.scheduled_segment_ticks) + ): + self.current_segment = segment_index tgt = wp.positions - n = max(1, int(round(wp.resolve_duration(self.hz) * self.hz))) - if self.max_joint_speed and self.max_joint_speed > 0: - dq = float(np.max(np.abs(tgt - prev))) - n = max(n, int(np.ceil(_BLEND_PEAK * dq / (self.max_joint_speed * self.dt)))) - n = max(1, n) for k in range(1, n + 1): - s = _cosine_blend(k / n) + self.current_segment_tick = k + phase = k / n + s = phase if self.traj.interpolation == "linear" else _cosine_blend(phase) yield prev + s * (tgt - prev) prev = tgt.copy() @@ -138,6 +212,30 @@ def setpoints(self) -> List[np.ndarray]: return list(iter(self)) +class JointTorqueTrajectoryInterpolator: + """Linear interpolation of direct torque endpoints at controller rate.""" + + def __init__(self, traj, control_hz: float): + del control_hz # n_frames is authoritative by contract. + self.traj = traj + self.current_segment = 0 + self.current_segment_tick = 0 + self.scheduled_total_ticks = int( + sum(waypoint.n_frames for waypoint in traj.waypoints) + ) + + def __iter__(self): + previous = self.traj.initial_torques.copy() + for segment, waypoint in enumerate(self.traj.waypoints): + self.current_segment = segment + for tick in range(1, waypoint.n_frames + 1): + self.current_segment_tick = tick + yield previous + (tick / waypoint.n_frames) * ( + waypoint.torques - previous + ) + previous = waypoint.torques.copy() + + def delta_to_target_pose(delta: CartesianDelta, current_pose: np.ndarray) -> np.ndarray: """Integrate a relative delta onto the current pose -> absolute target. diff --git a/src/flexiv_control/robot.py b/src/flexiv_control/robot.py index 4c4890f..1f8059b 100644 --- a/src/flexiv_control/robot.py +++ b/src/flexiv_control/robot.py @@ -18,6 +18,8 @@ from __future__ import annotations +import hashlib +import json import threading import time from typing import Optional @@ -29,6 +31,8 @@ CartesianDelta, CartesianWaypoint, ExecutionResult, + JointGripperForceTarget, + JointTorqueTrajectory, JointTrajectory, JointWaypoint, ) @@ -36,6 +40,7 @@ from .config import RobotConfig, load_safety_profile from .interpolation import ( CartesianTrajectoryInterpolator, + JointTorqueTrajectoryInterpolator, JointTrajectoryInterpolator, delta_to_target_pose, ) @@ -47,6 +52,7 @@ ImpedanceParams, JointImpedanceParams, RobotState, + SafetyStatus, StopReason, ) @@ -74,6 +80,10 @@ def __init__(self, result: ExecutionResult): self.result = result +class TrajectoryPrevalidationError(ValueError): + """A joint trajectory was rejected before any backend write.""" + + class Robot: def __init__( self, @@ -91,6 +101,23 @@ def __init__( safety_profile or self.cfg.default_safety_profile ) self.filter = SafetyFilter(self.profile, self.dt) + self._joint_velocity_limits = np.full( + self.cfg.n_joints, + 2.0, + dtype=float, + ) + self._joint_torque_limits: Optional[np.ndarray] = None + # Torque streaming is stateful across receding-horizon RPCs. The next + # trajectory must begin at the last command actually acknowledged by + # this Robot instance (zero after connect, stop, or a mode switch), so + # a client cannot introduce an unverified first-tick torque jump. + self._last_joint_torque_command = np.zeros( + self.cfg.n_joints, + dtype=float, + ) + self._active_stream_mode: Optional[ControlMode] = None + self._effective_joint_contract: Optional[dict] = None + self._apply_runtime_joint_contract() # Cooperative cancel: another thread (e.g. the server's stop handler) # sets this and the executing traj loop aborts at its next tick. self._cancel = threading.Event() @@ -102,7 +129,11 @@ def _backend_kwargs(self) -> dict: """Per-backend construction kwargs drawn from the config.""" b = self.cfg.backend.lower() if b in ("flexiv_rdk", "rdk", "flexiv"): - return dict(robot_sn=self.cfg.robot_sn, gripper_name=self.cfg.gripper_name) + return dict( + robot_sn=self.cfg.robot_sn, + gripper_name=self.cfg.gripper_name, + allow_torque=self.cfg.allow_joint_torque, + ) if b in ("mujoco", "mjx"): return dict( model_path=self.cfg.model_path, @@ -126,8 +157,175 @@ def from_config(cls, path_or_name: str, **overrides) -> "Robot": # -- lifecycle ----------------------------------------------------------- def connect(self) -> None: self.backend.connect() + self._reset_joint_torque_continuity() + self._apply_runtime_joint_contract() + + def _reset_joint_torque_continuity(self) -> None: + self._last_joint_torque_command = np.zeros( + self.cfg.n_joints, + dtype=float, + ) + self._active_stream_mode = None + + def _apply_runtime_joint_contract(self) -> None: + """Intersect configured limits with cached hardware/firmware facts.""" + backend_info = dict(self.backend.runtime_info()) + runtime = backend_info.get("joint_limits") + lower = np.asarray(self.profile.joint_lower, dtype=float).reshape(-1) + upper = np.asarray(self.profile.joint_upper, dtype=float).reshape(-1) + velocity = np.full(lower.shape, 2.0, dtype=float) + torque: Optional[np.ndarray] = None + sources = ["configured_safety_profile"] + + if runtime is not None: + runtime_lower = np.asarray( + runtime["position_min_rad"], + dtype=float, + ).reshape(-1) + runtime_upper = np.asarray( + runtime["position_max_rad"], + dtype=float, + ).reshape(-1) + runtime_velocity = np.asarray( + runtime["velocity_max_rad_s"], + dtype=float, + ).reshape(-1) + runtime_torque_raw = runtime.get("torque_max_nm") + runtime_torque = ( + None + if runtime_torque_raw is None + else np.asarray(runtime_torque_raw, dtype=float).reshape(-1) + ) + if not ( + runtime_lower.shape == runtime_upper.shape == runtime_velocity.shape == lower.shape + ): + raise RuntimeError( + "runtime RobotInfo joint limit dimensions do not match " + "the configured safety profile" + ) + lower = np.maximum(lower, runtime_lower) + upper = np.minimum(upper, runtime_upper) + velocity = runtime_velocity + if runtime_torque is not None: + if ( + runtime_torque.shape != lower.shape + or not np.all(np.isfinite(runtime_torque)) + or np.any(runtime_torque <= 0.0) + ): + raise RuntimeError( + "runtime RobotInfo joint torque limits are invalid" + ) + torque = runtime_torque + sources.append(str(runtime.get("source", "runtime_joint_limits"))) + elif self.cfg.backend.lower() in ("flexiv_rdk", "rdk", "flexiv"): + # A hardware deployment must never fall back to the old uniform + # 2 rad/s assumption. + if self.backend.is_connected: + raise RuntimeError( + "connected Flexiv RDK backend did not provide runtime joint limits" + ) + + current = backend_info.get("current_safety_limits") + if current is not None: + current_lower = np.asarray( + current["position_min_rad"], + dtype=float, + ).reshape(-1) + current_upper = np.asarray( + current["position_max_rad"], + dtype=float, + ).reshape(-1) + current_normal = np.asarray( + current["velocity_max_normal_rad_s"], + dtype=float, + ).reshape(-1) + current_reduced = np.asarray( + current["velocity_max_reduced_rad_s"], + dtype=float, + ).reshape(-1) + if not ( + current_lower.shape + == current_upper.shape + == current_normal.shape + == current_reduced.shape + == lower.shape + ): + raise RuntimeError( + "runtime SafetyLimits dimensions do not match the configured safety profile" + ) + lower = np.maximum(lower, current_lower) + upper = np.minimum(upper, current_upper) + # Which firmware state is active can change asynchronously; the + # smaller ceiling is safe in both normal and reduced operation. + velocity = np.minimum( + velocity, + np.minimum(current_normal, current_reduced), + ) + sources.append(str(current.get("source", "current_safety_limits"))) + + if ( + not np.all(np.isfinite(lower)) + or not np.all(np.isfinite(upper)) + or np.any(lower >= upper) + ): + raise RuntimeError("effective joint position limit intersection is invalid") + if not np.all(np.isfinite(velocity)) or np.any(velocity <= 0.0): + raise RuntimeError("effective per-joint velocity limits are invalid") + + self.profile.joint_lower = lower + self.profile.joint_upper = upper + self._joint_velocity_limits = velocity + self._joint_torque_limits = torque + self.filter.set_profile(self.profile) + self.filter.set_joint_velocity_limits(velocity) + enforced_lower = lower + self.profile.joint_margin_rad + enforced_upper = upper - self.profile.joint_margin_rad + if np.any(enforced_lower >= enforced_upper): + raise RuntimeError("joint margin collapses the effective position interval") + contract = { + "sources": sources, + "hard_position_min_rad": lower.tolist(), + "hard_position_max_rad": upper.tolist(), + "enforced_position_min_rad": enforced_lower.tolist(), + "enforced_position_max_rad": enforced_upper.tolist(), + "base_velocity_max_rad_s": velocity.tolist(), + "max_joint_speed_scale": float(self.profile.max_joint_speed_scale), + "max_joint_torque_scale": float( + self.profile.max_joint_torque_scale + ), + } + if torque is not None: + contract["base_torque_max_nm"] = torque.tolist() + payload = json.dumps( + contract, + sort_keys=True, + separators=(",", ":"), + ).encode("utf-8") + contract["sha256"] = hashlib.sha256(payload).hexdigest() + self._effective_joint_contract = contract + + def server_runtime_info(self) -> dict: + """Return the server's cached runtime contract without a state read.""" + backend_info = dict(self.backend.runtime_info()) + info = { + "control_hz": float(self.control_hz), + "active_safety_profile": self.profile.name, + } + for key in ( + "runtime_hardware_identity", + "gripper_limits", + "joint_limits", + "current_safety_limits", + ): + value = backend_info.get(key) + if value is not None: + info[key] = value + if self._effective_joint_contract is not None: + info["effective_joint_limits"] = dict(self._effective_joint_contract) + return info def disconnect(self) -> None: + self._reset_joint_torque_continuity() self.backend.disconnect() def __enter__(self) -> "Robot": @@ -157,8 +355,9 @@ def _check_lease(self) -> None: # -- safety profile ------------------------------------------------------ def set_safety_profile(self, name_or_path: str) -> None: self.profile = load_safety_profile(name_or_path) - # Update in place so a running control loop / server keeps its reference. - self.filter.set_profile(self.profile) + # Recompute the intersection; changing a profile can tighten but never + # expand beyond connected hardware/firmware facts. + self._apply_runtime_joint_contract() # -- state --------------------------------------------------------------- def get_state(self) -> RobotState: @@ -196,6 +395,8 @@ def start_cartesian_impedance( nullspace_q=nullspace_q if nullspace_q is not None else self.cfg.q_home, max_contact_wrench=self.profile.max_contact_wrench, ) + self._reset_joint_torque_continuity() + self._active_stream_mode = mode def start_joint_impedance( self, @@ -205,6 +406,26 @@ def start_joint_impedance( ) -> None: mode = ControlMode.RT_JOINT_IMPEDANCE if realtime else ControlMode.NRT_JOINT_IMPEDANCE self.backend.set_mode(mode, joint_impedance=joint_impedance or JointImpedanceParams()) + self._reset_joint_torque_continuity() + self._active_stream_mode = mode + + def start_joint_torque(self) -> None: + """Enter the explicitly enabled 1 kHz gravity-compensated torque mode.""" + if not self.cfg.allow_joint_torque: + raise RuntimeError( + "joint torque control is disabled by RobotConfig" + ) + if abs(self.control_hz - 1000.0) > 1e-9: + raise RuntimeError( + "RT_JOINT_TORQUE requires control_hz=1000" + ) + if self._active_stream_mode != ControlMode.RT_JOINT_TORQUE: + self._last_joint_torque_command = np.zeros( + self.cfg.n_joints, + dtype=float, + ) + self.backend.set_mode(ControlMode.RT_JOINT_TORQUE) + self._active_stream_mode = ControlMode.RT_JOINT_TORQUE # -- the RL / MPC / teleop workhorse ------------------------------------ def servo_cartesian_delta( @@ -224,20 +445,28 @@ def servo_cartesian_delta( state = self.get_state() target = delta_to_target_pose(delta, state.tcp_pose) wp = CartesianWaypoint( - position=target[:3], quaternion=target[3:7], - gripper=delta.gripper, duration=delta.duration, frame=delta.frame, + position=target[:3], + quaternion=target[3:7], + gripper=delta.gripper, + duration=delta.duration, + frame=delta.frame, + ) + traj = CartesianTrajectory( + waypoints=[wp], frame=delta.frame, safety_profile=self.profile.name ) - traj = CartesianTrajectory(waypoints=[wp], frame=delta.frame, - safety_profile=self.profile.name) return self.execute_cartesian_trajectory(traj, blocking=True) def servo_cartesian_pose( - self, pose: np.ndarray, *, duration: float = 0.2, + self, + pose: np.ndarray, + *, + duration: float = 0.2, gripper: Optional[GripperCommand] = None, ) -> ExecutionResult: pose = np.asarray(pose, float).reshape(7) - wp = CartesianWaypoint(position=pose[:3], quaternion=pose[3:7], - gripper=gripper, duration=duration) + wp = CartesianWaypoint( + position=pose[:3], quaternion=pose[3:7], gripper=gripper, duration=duration + ) return self.execute_cartesian_trajectory( CartesianTrajectory(waypoints=[wp], safety_profile=self.profile.name), blocking=True ) @@ -334,8 +563,7 @@ def execute_cartesian_trajectory( # Held-payload headroom: server-clamped to the profile's granted # ceiling (default zero), then ADDED to the profile cap. The traj # request is a request, never an override. - allow = np.minimum(traj.contact_wrench_allowance, - self.profile.max_wrench_allowance) + allow = np.minimum(traj.contact_wrench_allowance, self.profile.max_wrench_allowance) if np.any(allow > 0): wrench_cap = wrench_cap + allow wrench_relaxed = True @@ -378,8 +606,11 @@ def _try_fire_close(g, state) -> None: healthy arm converges to a few mm while a contact-stalled arm still shows the full remaining descend distance.""" nonlocal close_aborted, close_issued - err_now = (float(np.linalg.norm(prev_cmd_pos - state.tcp_position)) - if prev_cmd_pos is not None else 0.0) + err_now = ( + float(np.linalg.norm(prev_cmd_pos - state.tcp_position)) + if prev_cmd_pos is not None + else 0.0 + ) if close_aborted or result.clipped or err_now > traj.grip_tracking_gate_m: if not close_aborted: close_aborted = True @@ -417,8 +648,7 @@ def _try_fire_close(g, state) -> None: result.success = False result.stop_reason = StopReason.CONTACT_WRENCH.value break - sr = self.filter.filter_cartesian(pose, state, - max_contact_wrench=wrench_cap) + sr = self.filter.filter_cartesian(pose, state, max_contact_wrench=wrench_cap) if not sr.ok: self.backend.stop() result.success = False @@ -448,7 +678,8 @@ def _try_fire_close(g, state) -> None: # reading. closing = bool(grip.grasp) or ( float(state.gripper_width) > 1e-6 - and grip.width < float(state.gripper_width) - 1e-3) + and grip.width < float(state.gripper_width) - 1e-3 + ) # NEVER gate a grasp=True SUSTAIN once a close was actually # issued this traj: the object is (potentially) between the # fingers, and skipping the force-closure hand-off leaves @@ -456,8 +687,9 @@ def _try_fire_close(g, state) -> None: # failure -- while mislabeling a physical grasp as # "no close fired". The gate exists to prevent closing at an # UNPLANNED height; after a close it has done its job. - gate_active = (traj.grip_tracking_gate_m is not None - and not (grip.grasp and close_issued)) + gate_active = traj.grip_tracking_gate_m is not None and not ( + grip.grasp and close_issued + ) if closing and gate_active: if close_aborted: pass # sticky: a skipped close invalidates the rest @@ -472,15 +704,20 @@ def _try_fire_close(g, state) -> None: _try_fire_close(g, state) settle = min( int(round(GRIP_GATE_SETTLE_S * self.control_hz)), - max(1, int(interp.current_segment_ticks) // 2)) + max(1, int(interp.current_segment_ticks) // 2), + ) pending_close = (grip, tick_idx + settle) else: self.backend.move_gripper(grip) close_issued = close_issued or closing if trajectory is not None: trajectory.append( - [time.perf_counter() - t0, *sr.pose.tolist(), - *state.tcp_pose.tolist(), *state.wrench.tolist()] + [ + time.perf_counter() - t0, + *sr.pose.tolist(), + *state.tcp_pose.tolist(), + *state.wrench.tolist(), + ] ) # bookkeeping. path_tracking_error is the lag between the PREVIOUS @@ -593,13 +830,36 @@ def move_joint( return self.execute_joint_trajectory(traj) def execute_joint_trajectory( - self, traj: JointTrajectory, *, raise_on_stop: bool = False + self, + traj: JointTrajectory, + *, + raise_on_stop: bool = False, ) -> ExecutionResult: self._check_lease() result = ExecutionResult(success=True) + + def reject_unsafe_entry(state: RobotState) -> Optional[ExecutionResult]: + reason = StopReason.NONE + if self.backend.in_fault() or state.safety_status != SafetyStatus.OK: + reason = ( + state.stop_reason + if state.stop_reason != StopReason.NONE + else StopReason.BACKEND_FAULT + ) + elif np.any(np.abs(state.wrench) > self.profile.max_contact_wrench): + reason = StopReason.CONTACT_WRENCH + if reason == StopReason.NONE: + return None + result.success = False + result.stop_reason = reason.value + result.final_state = state + result.log["prewrite_safety_rejection"] = True + result.log["entry_safety_status"] = state.safety_status.value + if raise_on_stop: + raise TrajectoryStoppedError(result) + return result + if self._cancel.is_set(): - # A pending stop aborts THIS traj rather than being silently - # erased (same consume-on-abort semantics as the Cartesian path). self._cancel.clear() result.success = False result.stop_reason = StopReason.USER.value @@ -608,20 +868,252 @@ def execute_joint_trajectory( if raise_on_stop: raise TrajectoryStoppedError(result) return result - self._verify_trajectory_profile(traj.safety_profile, result) - start = self.get_state() - if start.control_mode.is_cartesian or start.control_mode == ControlMode.IDLE: + try: + self._verify_trajectory_profile(traj.safety_profile, result) + requested_speed_scale = float(traj.max_joint_speed_scale) + active_speed_scale = float(self.profile.max_joint_speed_scale) + result.log["requested_max_joint_speed_scale"] = requested_speed_scale + result.log["active_max_joint_speed_scale"] = active_speed_scale + if requested_speed_scale > active_speed_scale + 1e-12: + raise ValueError( + "JointTrajectory max_joint_speed_scale " + f"{requested_speed_scale:.9g} exceeds active safety-profile " + f"ceiling {active_speed_scale:.9g}" + ) + effective_speed_scale = requested_speed_scale + result.log["effective_max_joint_speed_scale"] = effective_speed_scale + result.log["joint_interpolation"] = traj.interpolation + result.log["strict_timing"] = traj.strict_timing + except ValueError as exc: + raise TrajectoryPrevalidationError(str(exc)) from exc + + # Read measured state once, then prevalidate the entire atomic RPC before + # any mode switch, gripper Move, or arm setpoint can reach the backend. + start_state = self.get_state() + entry_rejection = reject_unsafe_entry(start_state) + if entry_rejection is not None: + return entry_rejection + try: + initial_q = ( + start_state.q.copy() + if traj.initial_positions is None + else np.asarray(traj.initial_positions, float).reshape(-1).copy() + ) + if initial_q.shape != start_state.q.shape: + raise ValueError( + f"initial_positions must match measured q shape {start_state.q.shape}, " + f"got {initial_q.shape}" + ) + joint_filter_anchor = start_state.q.copy() + if joint_filter_anchor.shape != start_state.q.shape: + raise ValueError( + "joint filter anchor must match measured q shape " + f"{start_state.q.shape}, got {joint_filter_anchor.shape}" + ) + if not np.all(np.isfinite(joint_filter_anchor)): + raise ValueError("joint filter anchor must be finite") + effective_joint_speed = self._joint_velocity_limits * effective_speed_scale + interp = JointTrajectoryInterpolator( + traj, + initial_q, + self.control_hz, + max_joint_speed=effective_joint_speed, + ) + first_q = next(iter(interp)) if traj.strict_timing else None + + def first_joint_bound_error(anchor_q: np.ndarray) -> Optional[str]: + if first_q is None: + return None + first_step = np.abs(first_q - anchor_q) + first_step_limit = effective_joint_speed * self.dt + too_fast = first_step > first_step_limit + 1e-12 + if not np.any(too_fast): + return None + joint = int(np.flatnonzero(too_fast)[0]) + return ( + "strict_timing first emitted joint target exceeds the " + "effective one-tick rate bound; " + f"joint {joint}: step {first_step[joint]:.9g} rad > " + f"{first_step_limit[joint]:.9g} rad at " + f"control_hz={self.control_hz:.9g}" + ) + + first_error = first_joint_bound_error(joint_filter_anchor) + if first_error is not None: + raise ValueError(first_error) + + joint_lo = self.profile.joint_lower + self.profile.joint_margin_rad + joint_hi = self.profile.joint_upper - self.profile.joint_margin_rad + if traj.strict_timing: + for label, target in [ + ("initial_positions", initial_q), + *[ + (f"waypoints[{i}].positions", wp.positions) + for i, wp in enumerate(traj.waypoints) + ], + ]: + if np.any(target < joint_lo) or np.any(target > joint_hi): + raise ValueError( + f"strict_timing {label} exceeds effective joint position limits" + ) + + initial_gripper = ( + start_state.gripper_width + if traj.initial_gripper_width is None + else float(traj.initial_gripper_width) + ) + gripper_execution_anchor = float(start_state.gripper_width) + if not np.isfinite(gripper_execution_anchor): + raise ValueError("gripper execution anchor must be finite") + runtime_gripper = self.backend.runtime_info().get("gripper_limits") + if runtime_gripper is not None: + width_lo = float(runtime_gripper["min_width_m"]) + width_hi = float(runtime_gripper["max_width_m"]) + for label, width in ( + ("initial_gripper_width", initial_gripper), + ("gripper execution anchor", gripper_execution_anchor), + ): + if not width_lo <= width <= width_hi: + raise ValueError( + f"{label}={width!r} outside runtime [{width_lo}, {width_hi}] " + "from flexivrdk.Gripper.params" + ) + gripper_events: list[dict] = [] + previous_width = gripper_execution_anchor + previous_force: Optional[float] = None + boundary_tick = 0 + for segment, (wp, requested_ticks, scheduled_ticks) in enumerate( + zip( + traj.waypoints, + interp.requested_segment_ticks, + interp.scheduled_segment_ticks, + ) + ): + boundary_tick += scheduled_ticks + if wp.gripper is None: + continue + target = wp.gripper + if isinstance(target, JointGripperForceTarget): + if runtime_gripper is not None: + force_lo = float(runtime_gripper["min_force_n"]) + force_hi = float(runtime_gripper["max_force_n"]) + if not force_lo <= target.force <= force_hi: + raise ValueError( + f"JointWaypoint {segment} gripper force={target.force!r} " + f"outside runtime [{force_lo}, {force_hi}] from " + "flexivrdk.Gripper.params" + ) + gripper_events.append( + { + "mode": "force", + "segment": segment, + "requested_segment_ticks": requested_ticks, + "scheduled_segment_ticks": scheduled_ticks, + "start_tick": boundary_tick - scheduled_ticks, + "end_tick": boundary_tick, + "force_n": target.force, + "dispatch": previous_force != target.force, + } + ) + previous_force = target.force + continue + delta_width = abs(target.width - previous_width) + segment_duration = scheduled_ticks * self.dt + required_velocity = delta_width / segment_duration + velocity = required_velocity if target.velocity is None else target.velocity + if delta_width > 1e-12 and target.velocity is not None: + if not np.isclose(target.velocity, required_velocity, rtol=1e-6, atol=1e-9): + raise ValueError( + f"JointWaypoint {segment} gripper velocity {target.velocity:.9g} " + f"m/s does not realize width delta {delta_width:.9g} m in " + f"scheduled n_frames={scheduled_ticks}; required " + f"{required_velocity:.9g} m/s" + ) + if runtime_gripper is not None: + checks = [ + ("width", target.width, "min_width_m", "max_width_m"), + ("force", target.force, "min_force_n", "max_force_n"), + ] + if delta_width > 1e-12: + checks.append( + ("velocity", velocity, "min_velocity_m_s", "max_velocity_m_s") + ) + for field, value, lo_key, hi_key in checks: + lo = float(runtime_gripper[lo_key]) + hi = float(runtime_gripper[hi_key]) + if not np.isfinite(value) or not lo <= value <= hi: + raise ValueError( + f"JointWaypoint {segment} gripper {field}={value!r} " + f"outside runtime [{lo}, {hi}] from " + "flexivrdk.Gripper.params" + ) + gripper_events.append( + { + "mode": "move", + "segment": segment, + "requested_segment_ticks": requested_ticks, + "scheduled_segment_ticks": scheduled_ticks, + "start_tick": boundary_tick - scheduled_ticks, + "end_tick": boundary_tick, + "start_width_m": previous_width, + "target_width_m": target.width, + "velocity_m_s": velocity, + "requested_velocity_m_s": target.velocity, + "force_n": target.force, + "dispatch": delta_width > 1e-12, + } + ) + previous_width = target.width + previous_force = None + except ValueError as exc: + raise TrajectoryPrevalidationError(str(exc)) from exc + + result.log["base_joint_velocity_limits_rad_s"] = self._joint_velocity_limits.tolist() + if self._effective_joint_contract is not None: + result.log["effective_joint_limits_sha256"] = self._effective_joint_contract["sha256"] + result.log["control_hz"] = float(self.control_hz) + result.log["requested_duration_s"] = interp.requested_duration_s + result.log["requested_segment_ticks"] = list(interp.requested_segment_ticks) + result.log["requested_total_ticks"] = int(interp.requested_total_ticks) + result.log["scheduled_segment_ticks"] = list(interp.scheduled_segment_ticks) + result.log["scheduled_total_ticks"] = int(interp.scheduled_total_ticks) + result.log["scheduled_duration_s"] = float(interp.scheduled_total_ticks * self.dt) + result.log["initial_joint_target"] = initial_q.tolist() + result.log["joint_filter_anchor"] = joint_filter_anchor.tolist() + result.log["ending_joint_target"] = traj.waypoints[-1].positions.tolist() + result.log["initial_gripper_target_m"] = initial_gripper + result.log["gripper_execution_anchor_m"] = gripper_execution_anchor + result.log["ending_gripper_target_m"] = previous_width + result.log["ending_gripper_force_n"] = previous_force + result.log["gripper_events"] = gripper_events + result.log["gripper_tracking"] = [] + + start_state = self.get_state() + entry_rejection = reject_unsafe_entry(start_state) + if entry_rejection is not None: + return entry_rejection + if start_state.control_mode.is_cartesian or start_state.control_mode == ControlMode.IDLE: self.start_joint_impedance() result.log["mode_autostarted"] = True - start = self.get_state() - self.filter.reset(start) - interp = JointTrajectoryInterpolator( - traj, - start.q, - self.control_hz, - max_joint_speed=2.0 * self.profile.max_joint_speed_scale, - ) + start_state = self.get_state() + # Re-anchor after a possible mode transition. The first loop snapshot + # below refreshes this once more immediately before any actuator write. + joint_filter_anchor = start_state.q.copy() + gripper_execution_anchor = float(start_state.gripper_width) + result.log["joint_filter_anchor"] = joint_filter_anchor.tolist() + result.log["gripper_execution_anchor_m"] = gripper_execution_anchor + self.filter.reset(start_state, q=joint_filter_anchor) + + events_by_segment = {event["segment"]: event for event in gripper_events} + active_gripper_target: Optional[float] = gripper_execution_anchor + active_gripper_force: Optional[float] = None + first_gripper_event_checked = False t_loop = time.perf_counter() + execution_started = t_loop + streamed_ticks = 0 + max_joint_speed = 0.0 + max_tracking_error = 0.0 + previous_command = joint_filter_anchor for q in interp: if self._cancel.is_set(): self._cancel.clear() @@ -630,9 +1122,6 @@ def execute_joint_trajectory( result.stop_reason = StopReason.USER.value break state = self.get_state() - # The recovery/home path is the one most likely to run from an - # abnormal pose: give joint moves the same robot-fault and - # contact-wrench gates the Cartesian path has. if self.backend.in_fault(): self.backend.stop() result.success = False @@ -643,20 +1132,348 @@ def execute_joint_trajectory( result.success = False result.stop_reason = StopReason.CONTACT_WRENCH.value break + if streamed_ticks == 0: + # This is the freshest telemetry available before the first + # Gripper.Move or joint stream. Recheck instead of trusting the + # earlier atomic-prevalidation snapshot across a mode switch. + joint_filter_anchor = state.q.copy() + joint_error = first_joint_bound_error(joint_filter_anchor) + if joint_error is not None: + self.backend.stop() + result.success = False + result.stop_reason = StopReason.JOINT_LIMIT.value + result.log["predispatch_joint_rejection"] = joint_error + break + gripper_execution_anchor = float(state.gripper_width) + result.log["joint_filter_anchor"] = joint_filter_anchor.tolist() + result.log["gripper_execution_anchor_m"] = gripper_execution_anchor + self.filter.reset(state, q=joint_filter_anchor) + previous_command = joint_filter_anchor.copy() + active_gripper_target = gripper_execution_anchor + active_gripper_force = None + if interp.current_segment_tick == 1: + event = events_by_segment.get(interp.current_segment) + if event is not None: + event["measured_width_at_dispatch_m"] = state.gripper_width + if event["mode"] == "move" and not first_gripper_event_checked: + first_gripper_event_checked = True + measured_width = float(state.gripper_width) + delta_width = abs(event["target_width_m"] - measured_width) + segment_duration = event["scheduled_segment_ticks"] * self.dt + required_velocity = delta_width / segment_duration + requested_velocity = event["requested_velocity_m_s"] + velocity = ( + required_velocity if requested_velocity is None else requested_velocity + ) + gripper_error = None + if delta_width > 1e-12 and requested_velocity is not None: + if not np.isclose( + requested_velocity, + required_velocity, + rtol=1e-6, + atol=1e-9, + ): + gripper_error = ( + "first gripper event velocity no longer realizes " + "the measured width delta over the remaining " + f"segment: requested {requested_velocity:.9g} m/s, " + f"required {required_velocity:.9g} m/s" + ) + if runtime_gripper is not None and gripper_error is None: + width_lo = float(runtime_gripper["min_width_m"]) + width_hi = float(runtime_gripper["max_width_m"]) + if not width_lo <= measured_width <= width_hi: + gripper_error = ( + f"measured gripper width {measured_width:.9g} m " + f"outside runtime [{width_lo}, {width_hi}]" + ) + elif delta_width > 1e-12: + velocity_lo = float(runtime_gripper["min_velocity_m_s"]) + velocity_hi = float(runtime_gripper["max_velocity_m_s"]) + if not velocity_lo <= velocity <= velocity_hi: + gripper_error = ( + f"first gripper event velocity {velocity:.9g} m/s " + f"outside runtime [{velocity_lo}, {velocity_hi}] " + "after measured-width rebase" + ) + if gripper_error is not None: + self.backend.stop() + result.success = False + result.stop_reason = StopReason.GRIPPER_LIMIT.value + result.log["predispatch_gripper_rejection"] = gripper_error + break + event["prevalidated_start_width_m"] = event["start_width_m"] + event["start_width_m"] = measured_width + event["velocity_m_s"] = velocity + event["dispatch"] = delta_width > 1e-12 + gripper_execution_anchor = measured_width + result.log["gripper_execution_anchor_m"] = measured_width + if event["mode"] == "force" and event["dispatch"]: + self.backend.move_gripper( + GripperCommand( + width=float(state.gripper_width), + force=event["force_n"], + grasp=True, + ) + ) + active_gripper_target = None + active_gripper_force = event["force_n"] + elif event["mode"] == "move" and event["dispatch"]: + self.backend.move_gripper( + GripperCommand( + width=event["target_width_m"], + force=event["force_n"], + velocity=event["velocity_m_s"], + grasp=False, + ) + ) + active_gripper_target = event["target_width_m"] + active_gripper_force = None + if not result.success: + break sr = self.filter.filter_joint(q, state) - if not sr.ok: + if not sr.ok or (traj.strict_timing and sr.clipped): self.backend.stop() result.success = False - result.stop_reason = sr.reason.value + result.stop_reason = sr.reason.value if not sr.ok else StopReason.JOINT_LIMIT.value + result.log["strict_runtime_clip_refused"] = bool(sr.clipped) break if sr.clipped: result.clipped = True self.backend.stream_joint(sr.q) + streamed_ticks += 1 + max_joint_speed = max( + max_joint_speed, + float(np.max(np.abs(sr.q - previous_command)) / self.dt), + ) + max_tracking_error = max(max_tracking_error, float(np.linalg.norm(sr.q - state.q))) + previous_command = sr.q.copy() + result.log["gripper_tracking"].append( + { + "tick": streamed_ticks, + "target_width_m": active_gripper_target, + "target_force_n": active_gripper_force, + "measured_width_m": state.gripper_width, + "error_m": ( + None + if active_gripper_target is None + else active_gripper_target - state.gripper_width + ), + } + ) t_loop += self.dt sleep = t_loop - time.perf_counter() if sleep > 0: time.sleep(sleep) + result.executed_duration = time.perf_counter() - execution_started + result.log["streamed_ticks"] = int(streamed_ticks) + result.path_tracking_error = max_tracking_error + result.max_joint_speed = max_joint_speed + result.final_state = self.get_state() + result.gripper_width_final = result.final_state.gripper_width + result.log["final_gripper_tracking_error_m"] = ( + None + if previous_force is not None + else previous_width - result.final_state.gripper_width + ) + if raise_on_stop and not result.success: + raise TrajectoryStoppedError(result) + return result + + def execute_joint_torque_trajectory( + self, + traj: JointTorqueTrajectory, + *, + raise_on_stop: bool = False, + ) -> ExecutionResult: + """Stream a prevalidated smooth torque trajectory at exactly 1 kHz.""" + self._check_lease() + result = ExecutionResult(success=True) + try: + self._verify_trajectory_profile(traj.safety_profile, result) + except ValueError as exc: + raise TrajectoryPrevalidationError(str(exc)) from exc + if not self.cfg.allow_joint_torque: + raise TrajectoryPrevalidationError( + "joint torque control is disabled by RobotConfig" + ) + if abs(self.control_hz - 1000.0) > 1e-9: + raise TrajectoryPrevalidationError( + "JointTorqueTrajectory requires control_hz=1000" + ) + if self._joint_torque_limits is None: + raise TrajectoryPrevalidationError( + "backend did not publish RobotInfo.tau_max" + ) + requested_scale = float(traj.max_joint_torque_scale) + active_scale = float(self.profile.max_joint_torque_scale) + if requested_scale > active_scale + 1e-12: + raise TrajectoryPrevalidationError( + "JointTorqueTrajectory torque scale exceeds active profile" + ) + command_limit = self._joint_torque_limits * requested_scale + expected_initial = ( + self._last_joint_torque_command + if self._active_stream_mode == ControlMode.RT_JOINT_TORQUE + else np.zeros(self.cfg.n_joints, dtype=float) + ) + if ( + traj.initial_torques.shape != expected_initial.shape + or not np.array_equal(traj.initial_torques, expected_initial) + ): + raise TrajectoryPrevalidationError( + "initial_torques do not match the last acknowledged torque " + "command (zero is required after connect, stop, or mode switch)" + ) + commands = JointTorqueTrajectoryInterpolator(traj, self.control_hz) + segment_ticks = [waypoint.n_frames for waypoint in traj.waypoints] + for label, value in [ + ("initial_torques", traj.initial_torques), + *[ + (f"waypoints[{index}].torques", waypoint.torques) + for index, waypoint in enumerate(traj.waypoints) + ], + ]: + if value.shape != command_limit.shape: + raise TrajectoryPrevalidationError( + f"{label} must have {command_limit.size} values" + ) + if np.any(np.abs(value) > command_limit + 1e-12): + raise TrajectoryPrevalidationError( + f"{label} exceeds scaled RobotInfo.tau_max" + ) + + runtime_gripper = self.backend.runtime_info().get("gripper_limits") + for index, waypoint in enumerate(traj.waypoints): + target = waypoint.gripper + if isinstance(target, JointGripperForceTarget) and runtime_gripper: + if not ( + float(runtime_gripper["min_force_n"]) + <= target.force + <= float(runtime_gripper["max_force_n"]) + ): + raise TrajectoryPrevalidationError( + f"waypoints[{index}] gripper force outside runtime limits" + ) + + state = self.get_state() + if ( + self.backend.in_fault() + or state.safety_status != SafetyStatus.OK + or np.any(np.abs(state.wrench) > self.profile.max_contact_wrench) + ): + result.success = False + result.stop_reason = StopReason.BACKEND_FAULT.value + result.final_state = state + return result + self.start_joint_torque() + events = { + sum(item.n_frames for item in traj.waypoints[:index]): item.gripper + for index, item in enumerate(traj.waypoints) + if item.gripper is not None + } + start = time.perf_counter() + next_tick = start + streamed = 0 + last_gripper_force_commanded: Optional[float] = None + dispatched_gripper_events: list[dict] = [] + for command in commands: + if self._cancel.is_set(): + self._cancel.clear() + self.backend.stop() + self._reset_joint_torque_continuity() + result.success = False + result.stop_reason = StopReason.USER.value + break + state = self.get_state() + if self.backend.in_fault() or np.any( + np.abs(state.wrench) > self.profile.max_contact_wrench + ): + self.backend.stop() + self._reset_joint_torque_continuity() + result.success = False + result.stop_reason = ( + StopReason.BACKEND_FAULT.value + if self.backend.in_fault() + else StopReason.CONTACT_WRENCH.value + ) + break + target = events.get(streamed) + if isinstance(target, JointGripperForceTarget): + self.backend.move_gripper( + GripperCommand( + width=state.gripper_width, + force=target.force, + grasp=True, + ) + ) + last_gripper_force_commanded = float(target.force) + dispatched_gripper_events.append({ + "mode": "force", + "segment": int(commands.current_segment), + "force_n": float(target.force), + }) + elif target is not None: + self.backend.move_gripper( + GripperCommand( + width=target.width, + force=target.force, + velocity=target.velocity or 0.1, + grasp=False, + ) + ) + # The last gripper action is now an exact-width Move, so there + # is no longer an acknowledged direct-force endpoint. + last_gripper_force_commanded = None + dispatched_gripper_events.append({ + "mode": "move", + "segment": int(commands.current_segment), + "width_m": float(target.width), + "force_limit_n": float(target.force), + }) + self.backend.stream_joint_torque(command) + self._last_joint_torque_command = np.asarray( + command, + dtype=float, + ).copy() + streamed += 1 + next_tick += self.dt + delay = next_tick - time.perf_counter() + if delay > 0.0: + time.sleep(delay) + result.executed_duration = time.perf_counter() - start + result.log.update({ + "control_mode": ControlMode.RT_JOINT_TORQUE.value, + "streamed_ticks": streamed, + "gravity_compensation": True, + "soft_limits": True, + "base_torque_max_nm": self._joint_torque_limits.tolist(), + "requested_max_joint_torque_scale": requested_scale, + "effective_torque_max_nm": command_limit.tolist(), + "requested_segment_ticks": segment_ticks, + "scheduled_segment_ticks": segment_ticks, + "requested_total_ticks": sum(segment_ticks), + "scheduled_total_ticks": sum(segment_ticks), + "strict_timing": True, + "acknowledged_ending_joint_torque_nm": ( + self._last_joint_torque_command.tolist() + if result.success else None + ), + # Both names carry a physical Newton command, never a planner + # latent or measured aperture. Keep the historical alias while + # exposing the acknowledgement semantics explicitly. + "ending_gripper_force_n": ( + last_gripper_force_commanded if result.success else None + ), + "acknowledged_ending_gripper_force_n": ( + last_gripper_force_commanded if result.success else None + ), + # Report events actually dispatched before a stop, rather than all + # events merely present in the requested trajectory. + "gripper_events": dispatched_gripper_events, + }) result.final_state = self.get_state() + result.gripper_width_final = result.final_state.gripper_width if raise_on_stop and not result.success: raise TrajectoryStoppedError(result) return result @@ -672,11 +1489,12 @@ def command_gripper( which is why every consumer that needs "open, then proceed" used to fabricate a do-nothing motion traj just to ride its blocking executor. - Settle detection: reaching the commanded width (non-grasp), or width - unchanged while not moving -- the latter only counts after motion has - been OBSERVED or a 0.5 s dwell has passed, because real hardware has an - actuation-latency window after the command in which the unchanged OLD - width would otherwise read as "settled".""" + Settle detection: a ``Move`` command must reach its commanded width. + Only a true ``Grasp`` command may settle at an unchanged, obstructed + width. Treating stillness as success for ``Move`` is unsafe on GN01: + the hardware can report ``is_moving=False`` during its actuation + latency, which previously let an open command return while the fingers + were still closed.""" self._check_lease() initial = self.get_state().gripper_width self.backend.move_gripper(cmd) @@ -693,13 +1511,14 @@ def command_gripper( if state.gripper_is_moving or abs(state.gripper_width - initial) > 1e-3: moved = True settled_target = (not cmd.grasp) and abs(state.gripper_width - cmd.width) < 2e-3 - settled_still = ( - prev_width is not None + settled_grasp = ( + cmd.grasp + and prev_width is not None and abs(state.gripper_width - prev_width) < 5e-4 and not state.gripper_is_moving and (moved or time.time() - t0 >= 0.5) ) - if settled_target or settled_still: + if settled_target or settled_grasp: break prev_width = state.gripper_width time.sleep(0.05) @@ -806,7 +1625,8 @@ def go_home_safe( result = ExecutionResult( success=False, stop_reason=( - StopReason.USER.value if self._cancel.is_set() + StopReason.USER.value + if self._cancel.is_set() else StopReason.CONTACT_WRENCH.value ), ) @@ -840,6 +1660,7 @@ def clear_stop(self) -> None: def stop(self) -> None: self._cancel.set() self.backend.stop() + self._reset_joint_torque_continuity() def _traj_wrench(traj: CartesianTrajectory): diff --git a/src/flexiv_control/safety.py b/src/flexiv_control/safety.py index 676c59f..dbd2963 100644 --- a/src/flexiv_control/safety.py +++ b/src/flexiv_control/safety.py @@ -50,6 +50,7 @@ class SafetyProfile: # Joint limits (margin shrinks the hard limits; speed scale caps velocity). joint_margin_rad: float = 0.08 max_joint_speed_scale: float = 0.30 + max_joint_torque_scale: float = 0.30 # Hard joint position limits (Rizon 4/4s nominal; override per robot). joint_lower: np.ndarray = field( default_factory=lambda: np.array( @@ -114,6 +115,9 @@ def from_dict(cls, d: dict) -> "SafetyProfile": jl = d.get("joint_limits", {}) p.joint_margin_rad = jl.get("margin_rad", p.joint_margin_rad) p.max_joint_speed_scale = jl.get("max_joint_speed_scale", p.max_joint_speed_scale) + p.max_joint_torque_scale = jl.get( + "max_joint_torque_scale", p.max_joint_torque_scale + ) if "lower" in jl: p.joint_lower = np.asarray(jl["lower"], float) if "upper" in jl: @@ -153,6 +157,7 @@ def to_config_dict(self) -> dict: "joint_limits": { "margin_rad": self.joint_margin_rad, "max_joint_speed_scale": self.max_joint_speed_scale, + "max_joint_torque_scale": self.max_joint_torque_scale, "lower": self.joint_lower.tolist(), "upper": self.joint_upper.tolist(), }, @@ -259,9 +264,17 @@ class SafetyFilter: used as the reference, which keeps the guard meaningful in unit tests. """ - def __init__(self, profile: SafetyProfile, control_dt: float): + def __init__( + self, + profile: SafetyProfile, + control_dt: float, + *, + joint_velocity_max: Optional[np.ndarray] = None, + ): self.p = profile self.dt = control_dt + self._joint_velocity_max: Optional[np.ndarray] = None + self.set_joint_velocity_limits(joint_velocity_max) self._prev_pose: Optional[np.ndarray] = None self._prev_q: Optional[np.ndarray] = None self._prev_lin_vel: Optional[np.ndarray] = None # for the accel cap @@ -292,6 +305,26 @@ def reset( def set_profile(self, profile: SafetyProfile) -> None: self.p = profile + def set_joint_velocity_limits( + self, + limits: Optional[np.ndarray], + ) -> None: + """Set runtime per-joint manufacturer/firmware velocity ceilings.""" + if limits is None: + self._joint_velocity_max = None + return + values = np.asarray(limits, dtype=float).reshape(-1) + if ( + values.shape != self.p.joint_lower.shape + or not np.all(np.isfinite(values)) + or np.any(values <= 0.0) + ): + raise ValueError( + "joint_velocity_max must match joint limits and contain " + "finite positive values" + ) + self._joint_velocity_max = values.copy() + # -- Cartesian ----------------------------------------------------------- def filter_cartesian( self, @@ -412,7 +445,12 @@ def filter_joint(self, target_q: np.ndarray, state: RobotState) -> SafetyResult: # Per-tick step limit from a coarse joint-speed cap (rad/s), referenced # to the previous joint command (see filter_cartesian for rationale). ref_q = self._prev_q if self._prev_q is not None else cur_q - max_step = 2.0 * p.max_joint_speed_scale * self.dt + base_velocity = ( + self._joint_velocity_max + if self._joint_velocity_max is not None + else np.full(target_q.shape, 2.0, dtype=float) + ) + max_step = base_velocity * p.max_joint_speed_scale * self.dt step = target_q - ref_q big = np.abs(step) > max_step if np.any(big): diff --git a/src/flexiv_control/server/protocol.py b/src/flexiv_control/server/protocol.py index c49caaa..b18afc0 100644 --- a/src/flexiv_control/server/protocol.py +++ b/src/flexiv_control/server/protocol.py @@ -22,18 +22,25 @@ from __future__ import annotations +import hashlib import json +from pathlib import Path from typing import Any, Optional import numpy as np +from .. import __version__ from ..trajectory import ( CartesianTrajectory, CartesianWaypoint, TrajectoryRepresentation, ExecutionResult, + JointGripperForceTarget, + JointGripperTarget, JointTrajectory, JointWaypoint, + JointTorqueTrajectory, + JointTorqueWaypoint, ) from ..types import ( ControlMode, @@ -46,6 +53,148 @@ ) DEFAULT_PORT = 8766 +SERVER_INFO_SCHEMA = "flexiv-control.server-info.v5" +PROTOCOL_ID = "flexiv-control.trajectory-rpc.v5" +JOINT_TRAJECTORY_SCHEMA = "flexiv-control.joint-trajectory.v4" +JOINT_TORQUE_TRAJECTORY_SCHEMA = "flexiv-control.joint-torque-trajectory.v1" + +# Canonical, path-independent description of the wire seam that must agree +# across the planner client and robot-side server. In particular, this pins +# the trajectory RPC names and payload key that differ from the incompatible +# pre-0.2.1 ``*_chunk`` protocol. +PROTOCOL_CONTRACT = { + "protocol_id": PROTOCOL_ID, + "transport": "newline-delimited-json-request-response-v1", + "identity_rpc": { + "method": "get_server_info", + "lease_required": False, + "runtime_fields": { + "required": ["control_hz", "active_safety_profile"], + "hardware_when_available": [ + "runtime_hardware_identity", + "gripper_limits", + "joint_limits", + "current_safety_limits", + "effective_joint_limits", + ], + }, + }, + "trajectory_rpcs": { + "execute_cartesian_trajectory": "traj", + "execute_joint_trajectory": "traj", + "execute_joint_torque_trajectory": "traj", + }, + "joint_trajectory_contract": { + "schema": JOINT_TRAJECTORY_SCHEMA, + "trajectory_fields": [ + "schema", + "waypoints", + "initial_positions", + "initial_gripper_width", + "max_joint_speed_scale", + "interpolation", + "strict_timing", + "safety_profile", + ], + "waypoint_fields": ["positions", "n_frames", "duration", "gripper"], + "gripper_target_variants": { + "move": ["mode", "width", "force_limit", "velocity"], + "force": ["mode", "force"], + }, + "rpc_identity_fields": ["protocol_id", "protocol_fingerprint_sha256"], + "explicit_initial_target": ["initial_positions", "initial_gripper_width"], + "strict_timing": "authoritative-n_frames-reject-no-clip-or-time-stretch", + "continuity": "measured-rebase-every-rpc-prior-ack-provenance-only", + "first_emitted_joint_bound": "effective-runtime-rate-times-control-period", + "gripper_execution_anchor": "current-measured-width-every-rpc", + "predispatch_revalidation": [ + "first-joint-setpoint-after-mode-transition", + "first-gripper-event-from-current-measured-width", + ], + "numeric_json_types": "numbers-and-arrays-only-no-strings-or-booleans", + "gripper": "explicit-Move-or-signed-Grasp-concurrent-at-segment-boundary", + "interpolation": ["cosine", "linear"], + "max_joint_speed_scale": "finite-(0,1]-active-profile-ceiling", + }, + "joint_torque_trajectory_contract": { + "schema": JOINT_TORQUE_TRAJECTORY_SCHEMA, + "action": "gravity-compensated-joint-torque-nm", + "action_units": {"arm": "N.m", "gripper": "N"}, + "semantic_gripper_latent_on_wire": False, + "rate_hz": 1000, + "interpolation": "linear", + "limits": "RobotInfo.tau_max-times-active-profile-scale", + "firmware_soft_limits": True, + "gripper": "synchronized-signed-force-at-segment-boundary", + "acknowledged_endpoints": [ + "acknowledged_ending_joint_torque_nm", + "acknowledged_ending_gripper_force_n", + ], + "measured_endpoint": "ExecutionResult.final_state", + }, +} + + +def _canonical_sha256(value: Any) -> str: + payload = json.dumps( + value, + sort_keys=True, + separators=(",", ":"), + ).encode("utf-8") + return hashlib.sha256(payload).hexdigest() + + +def _source_fingerprint_sha256() -> str: + """Hash installed package source bytes without depending on install path.""" + package_root = Path(__file__).resolve().parents[1] + digest = hashlib.sha256() + for path in sorted(package_root.rglob("*")): + if not path.is_file() or "__pycache__" in path.parts or path.suffix in {".pyc", ".pyo"}: + continue + relative = path.relative_to(package_root).as_posix() + digest.update(relative.encode("utf-8")) + digest.update(b"\0") + digest.update(path.read_bytes()) + digest.update(b"\0") + return digest.hexdigest() + + +PROTOCOL_FINGERPRINT_SHA256 = _canonical_sha256(PROTOCOL_CONTRACT) +SOURCE_FINGERPRINT_SHA256 = _source_fingerprint_sha256() + + +def server_info(**runtime: Any) -> dict[str, Any]: + """Return process identity plus cached, lease-free runtime facts.""" + required = {"control_hz", "active_safety_profile"} + missing = sorted(required.difference(runtime)) + if missing: + raise ValueError("server_info missing required runtime fields: " + ", ".join(missing)) + control_hz = float(runtime["control_hz"]) + if not np.isfinite(control_hz) or control_hz <= 0.0: + raise ValueError("server_info control_hz must be finite and > 0") + active_profile = str(runtime["active_safety_profile"]).strip() + if not active_profile: + raise ValueError("server_info active_safety_profile must be non-empty") + info: dict[str, Any] = { + "schema": SERVER_INFO_SCHEMA, + "package": "flexiv-control", + "package_version": __version__, + "protocol_id": PROTOCOL_ID, + "protocol_fingerprint_sha256": PROTOCOL_FINGERPRINT_SHA256, + "source_fingerprint_sha256": SOURCE_FINGERPRINT_SHA256, + "control_hz": control_hz, + "active_safety_profile": active_profile, + } + for key in ( + "runtime_hardware_identity", + "gripper_limits", + "joint_limits", + "current_safety_limits", + "effective_joint_limits", + ): + if key in runtime: + info[key] = runtime[key] + return info # --------------------------------------------------------------------------- @@ -279,32 +428,253 @@ def trajectory_from_dict(d: dict) -> CartesianTrajectory: # --------------------------------------------------------------------------- # JointTrajectory # --------------------------------------------------------------------------- +def _require_exact_keys(value: dict, expected: set[str], *, context: str) -> None: + if not isinstance(value, dict): + raise ValueError(f"{context} must be an object") + actual = set(value) + if actual != expected: + raise ValueError( + f"{context} keys do not match {JOINT_TRAJECTORY_SCHEMA}: " + f"missing={sorted(expected - actual)}, extra={sorted(actual - expected)}" + ) + + +def _require_json_number(value: Any, *, context: str) -> float: + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise ValueError(f"{context} must be a JSON number (not a string or boolean)") + return float(value) + + +def _require_optional_json_number(value: Any, *, context: str) -> Optional[float]: + if value is None: + return None + return _require_json_number(value, context=context) + + +def _require_json_number_array(value: Any, *, context: str) -> list[float]: + if not isinstance(value, list): + raise ValueError(f"{context} must be a JSON array") + return [ + _require_json_number(item, context=f"{context}[{index}]") + for index, item in enumerate(value) + ] + + +def joint_gripper_target_to_dict( + g: Optional[JointGripperTarget | JointGripperForceTarget], +) -> Optional[dict]: + if g is None: + return None + if isinstance(g, JointGripperForceTarget): + return {"mode": "force", "force": g.force} + return { + "mode": "move", + "width": g.width, + "force_limit": g.force, + "velocity": g.velocity, + } + + +def joint_gripper_target_from_dict( + d: Optional[dict], +) -> Optional[JointGripperTarget | JointGripperForceTarget]: + if d is None: + return None + if not isinstance(d, dict): + raise ValueError("JointWaypoint.gripper must be an object or null") + mode = d.get("mode") + if mode == "force": + _require_exact_keys(d, {"mode", "force"}, context="JointGripperForceTarget") + return JointGripperForceTarget( + force=_require_json_number( + d["force"], context="JointGripperForceTarget.force" + ) + ) + if mode != "move": + raise ValueError("JointWaypoint.gripper.mode must be 'move' or 'force'") + _require_exact_keys( + d, + {"mode", "width", "force_limit", "velocity"}, + context="JointGripperTarget", + ) + return JointGripperTarget( + width=_require_json_number(d["width"], context="JointGripperTarget.width"), + force=_require_json_number( + d["force_limit"], context="JointGripperTarget.force_limit" + ), + velocity=_require_optional_json_number( + d["velocity"], context="JointGripperTarget.velocity" + ), + ) + + def joint_trajectory_to_dict(c: JointTrajectory) -> dict: return { + "schema": JOINT_TRAJECTORY_SCHEMA, "waypoints": [ { "positions": w.positions.tolist(), "n_frames": w.n_frames, "duration": w.duration, + "gripper": joint_gripper_target_to_dict(w.gripper), } for w in c.waypoints ], + "initial_positions": ( + None if c.initial_positions is None else c.initial_positions.tolist() + ), + "initial_gripper_width": c.initial_gripper_width, "max_joint_speed_scale": c.max_joint_speed_scale, + "interpolation": c.interpolation, + "strict_timing": c.strict_timing, "safety_profile": c.safety_profile, } def joint_trajectory_from_dict(d: dict) -> JointTrajectory: - wpts = [ - JointWaypoint( - positions=np.asarray(w["positions"], float), - n_frames=w.get("n_frames"), - duration=w.get("duration"), + _require_exact_keys( + d, + { + "schema", + "waypoints", + "initial_positions", + "initial_gripper_width", + "max_joint_speed_scale", + "interpolation", + "strict_timing", + "safety_profile", + }, + context="JointTrajectory", + ) + if d["schema"] != JOINT_TRAJECTORY_SCHEMA: + raise ValueError( + f"unsupported JointTrajectory schema {d['schema']!r}; " + f"expected {JOINT_TRAJECTORY_SCHEMA!r}" + ) + if not isinstance(d["waypoints"], list): + raise ValueError("JointTrajectory.waypoints must be a list") + if not isinstance(d["strict_timing"], bool): + raise ValueError("JointTrajectory.strict_timing must be a boolean") + if not isinstance(d["interpolation"], str): + raise ValueError("JointTrajectory.interpolation must be a string") + if not isinstance(d["safety_profile"], str): + raise ValueError("JointTrajectory.safety_profile must be a string") + initial_positions = ( + None + if d["initial_positions"] is None + else _require_json_number_array( + d["initial_positions"], context="JointTrajectory.initial_positions" + ) + ) + initial_gripper_width = _require_optional_json_number( + d["initial_gripper_width"], context="JointTrajectory.initial_gripper_width" + ) + max_joint_speed_scale = _require_json_number( + d["max_joint_speed_scale"], context="JointTrajectory.max_joint_speed_scale" + ) + wpts = [] + for index, w in enumerate(d["waypoints"]): + _require_exact_keys( + w, + {"positions", "n_frames", "duration", "gripper"}, + context=f"JointWaypoint[{index}]", + ) + positions = _require_json_number_array( + w["positions"], context=f"JointWaypoint[{index}].positions" + ) + n_frames = w["n_frames"] + if n_frames is not None and (isinstance(n_frames, bool) or not isinstance(n_frames, int)): + raise ValueError(f"JointWaypoint[{index}].n_frames must be a JSON integer or null") + duration = _require_optional_json_number( + w["duration"], context=f"JointWaypoint[{index}].duration" + ) + wpts.append( + JointWaypoint( + positions=np.asarray(positions, float), + n_frames=n_frames, + duration=duration, + gripper=joint_gripper_target_from_dict(w["gripper"]), + ) ) - for w in d["waypoints"] - ] return JointTrajectory( waypoints=wpts, - max_joint_speed_scale=float(d.get("max_joint_speed_scale", 0.3)), - safety_profile=d.get("safety_profile", ""), + initial_positions=( + None if initial_positions is None else np.asarray(initial_positions, float) + ), + initial_gripper_width=initial_gripper_width, + max_joint_speed_scale=max_joint_speed_scale, + interpolation=d["interpolation"], + strict_timing=d["strict_timing"], + safety_profile=d["safety_profile"], + ) + + +def joint_torque_trajectory_to_dict(c: JointTorqueTrajectory) -> dict: + return { + "schema": JOINT_TORQUE_TRAJECTORY_SCHEMA, + "initial_torques": c.initial_torques.tolist(), + "waypoints": [ + { + "torques": waypoint.torques.tolist(), + "n_frames": waypoint.n_frames, + "gripper": joint_gripper_target_to_dict(waypoint.gripper), + } + for waypoint in c.waypoints + ], + "max_joint_torque_scale": c.max_joint_torque_scale, + "safety_profile": c.safety_profile, + } + + +def joint_torque_trajectory_from_dict(d: dict) -> JointTorqueTrajectory: + _require_exact_keys( + d, + { + "schema", + "initial_torques", + "waypoints", + "max_joint_torque_scale", + "safety_profile", + }, + context="JointTorqueTrajectory", + ) + if d["schema"] != JOINT_TORQUE_TRAJECTORY_SCHEMA: + raise ValueError("unsupported JointTorqueTrajectory schema") + if not isinstance(d["waypoints"], list) or not d["waypoints"]: + raise ValueError("JointTorqueTrajectory.waypoints must be non-empty") + waypoints = [] + for index, waypoint in enumerate(d["waypoints"]): + _require_exact_keys( + waypoint, + {"torques", "n_frames", "gripper"}, + context=f"JointTorqueTrajectory.waypoints[{index}]", + ) + frames = waypoint["n_frames"] + if isinstance(frames, bool) or not isinstance(frames, int): + raise ValueError("JointTorqueWaypoint.n_frames must be an integer") + waypoints.append( + JointTorqueWaypoint( + torques=np.asarray( + _require_json_number_array( + waypoint["torques"], + context=f"waypoints[{index}].torques", + ), + dtype=float, + ), + n_frames=frames, + gripper=joint_gripper_target_from_dict(waypoint["gripper"]), + ) + ) + return JointTorqueTrajectory( + waypoints=waypoints, + initial_torques=np.asarray( + _require_json_number_array( + d["initial_torques"], context="initial_torques" + ), + dtype=float, + ), + max_joint_torque_scale=_require_json_number( + d["max_joint_torque_scale"], context="max_joint_torque_scale" + ), + safety_profile=str(d["safety_profile"]), ) diff --git a/src/flexiv_control/server/server.py b/src/flexiv_control/server/server.py index 1be0829..30de6e5 100644 --- a/src/flexiv_control/server/server.py +++ b/src/flexiv_control/server/server.py @@ -37,7 +37,7 @@ import numpy as np from ..config import RobotConfig -from ..robot import Robot +from ..robot import Robot, TrajectoryPrevalidationError from ..types import GripperCommand, ImpedanceParams from . import protocol as P from .control_loop import ReactiveServoLoop @@ -66,16 +66,19 @@ def __init__( self.lease = Lease(ttl_seconds=lease_ttl) # Host-wide single-owner lock (across OS processes), in addition to the # in-process client Lease. None to disable (e.g. tests / multi-arm hosts). - self._host_lock = ( - HostLock(self.robot.cfg.robot_id, owner="server") if host_lock else None - ) + self._host_lock = HostLock(self.robot.cfg.robot_id, owner="server") if host_lock else None self._robot_lock = threading.Lock() self._servo_loop: Optional[ReactiveServoLoop] = None # single-writer streaming self._tcp: Optional[socketserver.ThreadingTCPServer] = None + # Last target acknowledged by a successful joint-trajectory RPC. + self._last_ack_joint_target: Optional[np.ndarray] = None + self._last_ack_gripper_target: Optional[float] = None + self._joint_target_owner: str = "" self._handlers: Dict[str, Callable[[dict], Any]] = self._build_handlers() # -- lifecycle ----------------------------------------------------------- def start(self) -> None: + self._reset_joint_target_continuity() # Host-wide arbitration: refuse to start if another *live* process holds # this robot (a second server, or a direct script). Reclaimed automatically # if the previous holder crashed (its PID is dead). @@ -130,6 +133,7 @@ def serve_in_thread(self) -> threading.Thread: return t def shutdown(self) -> None: + self._reset_joint_target_continuity() with self._robot_lock: loop, self._servo_loop = self._servo_loop, None if loop is not None: @@ -144,6 +148,72 @@ def shutdown(self) -> None: self.robot.disconnect() if self._host_lock is not None: self._host_lock.release() + self._reset_joint_target_continuity() + + def _reset_joint_target_continuity(self) -> None: + self._last_ack_joint_target = None + self._last_ack_gripper_target = None + self._joint_target_owner = "" + + def _joint_target_execution_context(self, owner: str, traj) -> tuple[str, dict[str, Any]]: + """Validate shape and retain prior targets as provenance.""" + if traj.initial_positions is None: + # Explicit legacy mode deliberately starts from measured q and + # cannot participate in acknowledged-target provenance. + self._reset_joint_target_continuity() + return "legacy_measured", {} + + state = self.robot.get_state() + initial = np.asarray(traj.initial_positions, float) + if initial.shape != state.q.shape: + raise ValueError( + f"initial_positions shape {initial.shape} does not match measured " + f"q shape {state.q.shape}" + ) + + evidence: dict[str, Any] = {} + source = "measured_rebase" + if self._last_ack_joint_target is not None: + if self._joint_target_owner != owner: + # Lease acquisition normally clears this already; fail closed + # against stale cross-owner provenance without rejecting a safe + # measured rebase for the current owner. + self._reset_joint_target_continuity() + else: + source = "measured_rebase_with_prior_ack" + evidence["previous_acknowledged_joint_target"] = ( + self._last_ack_joint_target.tolist() + ) + evidence["measured_joint_delta_from_previous_ack_rad"] = ( + state.q - self._last_ack_joint_target + ).tolist() + evidence["initial_joint_delta_from_previous_ack_rad"] = ( + initial - self._last_ack_joint_target + ).tolist() + if self._last_ack_gripper_target is not None: + evidence["previous_acknowledged_gripper_target_m"] = float( + self._last_ack_gripper_target + ) + evidence["measured_gripper_delta_from_previous_ack_m"] = float( + state.gripper_width - self._last_ack_gripper_target + ) + if traj.initial_gripper_width is not None: + evidence["initial_gripper_delta_from_previous_ack_m"] = float( + traj.initial_gripper_width - self._last_ack_gripper_target + ) + + # Knot 0 is an interpolation origin, not a streamed command. Every + # feedback-MPC RPC therefore rebases the filter and first gripper ramp + # to current telemetry, while the executor validates the actual first + # emitted command against the effective one-tick/runtime bounds. + return source, evidence + + def _ack_joint_target(self, owner: str, traj, ending_gripper_target: Optional[float]) -> None: + self._last_ack_joint_target = traj.waypoints[-1].positions.copy() + self._last_ack_gripper_target = ( + None if ending_gripper_target is None else float(ending_gripper_target) + ) + self._joint_target_owner = owner # -- dispatch ------------------------------------------------------------ def _dispatch(self, req: dict) -> dict: @@ -204,6 +274,7 @@ def _motion_lock(self, owner: str = ""): def _build_handlers(self) -> Dict[str, Callable[[dict], Any]]: return { "ping": lambda p: {"pong": True}, + "get_server_info": self._h_get_server_info, "acquire_lease": self._h_acquire_lease, "release_lease": self._h_release_lease, "heartbeat": self._h_heartbeat, @@ -217,6 +288,9 @@ def _build_handlers(self) -> Dict[str, Callable[[dict], Any]]: "servo_cartesian_pose": self._h_servo_cartesian_pose, "execute_cartesian_trajectory": self._h_execute_cartesian_trajectory, "execute_joint_trajectory": self._h_execute_joint_trajectory, + "execute_joint_torque_trajectory": ( + self._h_execute_joint_torque_trajectory + ), "move_joint": self._h_move_joint, "command_gripper": self._h_command_gripper, "home": self._h_home, @@ -230,6 +304,10 @@ def _build_handlers(self) -> Dict[str, Callable[[dict], Any]]: } # -- handlers ------------------------------------------------------------ + def _h_get_server_info(self, p: dict) -> dict: + """Return identity plus connect-time facts, without a lease/state read.""" + return P.server_info(**self.robot.server_runtime_info()) + def _h_acquire_lease(self, p: dict) -> dict: force = bool(p.get("force", False)) prev = self.lease.owner @@ -238,8 +316,10 @@ def _h_acquire_lease(self, p: dict) -> dict: # motion was in flight, cancel that motion (next tick) -- otherwise the # victim's traj keeps streaming to completion under the thief's lease. if force and prev and prev != info.owner: + self._reset_joint_target_continuity() self.robot.request_stop() elif prev != info.owner: + self._reset_joint_target_continuity() # A FRESH owner must not inherit the cancel latched by the # previous session (a client disconnect requests a safety stop; # with no motion in flight nothing consumes it, and it would @@ -250,6 +330,7 @@ def _h_acquire_lease(self, p: dict) -> dict: def _h_release_lease(self, p: dict) -> dict: self.lease.release(p.get("owner", "")) + self._reset_joint_target_continuity() # Releasing the lease must also tear down an always-on servo loop, else # it would keep writing to the arm with no lease holder (orphan writer). with self._robot_lock: @@ -258,6 +339,7 @@ def _h_release_lease(self, p: dict) -> dict: loop.stop() with self._robot_lock: self.robot.stop() + self._reset_joint_target_continuity() return {"released": True} def _h_heartbeat(self, p: dict) -> dict: @@ -267,6 +349,7 @@ def _h_heartbeat(self, p: dict) -> dict: def _h_set_safety_profile(self, p: dict) -> dict: self._require_lease(p) with self._robot_lock: + self._reset_joint_target_continuity() self.robot.set_safety_profile(p["name"]) return {"profile": self.robot.profile.name} @@ -309,12 +392,11 @@ def _h_start_cartesian_impedance(self, p: dict) -> dict: if "stiffness" in p: imp = ImpedanceParams( stiffness=np.asarray(p["stiffness"], float), - damping_ratio=np.asarray( - p.get("damping_ratio", [0.7] * 6), float - ), + damping_ratio=np.asarray(p.get("damping_ratio", [0.7] * 6), float), ) ns = None if p.get("nullspace_q") is None else np.asarray(p["nullspace_q"], float) with self._motion_lock(owner): + self._reset_joint_target_continuity() self.robot.start_cartesian_impedance( impedance=imp, realtime=bool(p.get("realtime", False)), nullspace_q=ns ) @@ -323,12 +405,14 @@ def _h_start_cartesian_impedance(self, p: dict) -> dict: def _h_start_joint_impedance(self, p: dict) -> dict: owner = self._require_lease(p) with self._motion_lock(owner): + self._reset_joint_target_continuity() self.robot.start_joint_impedance(realtime=bool(p.get("realtime", False))) return {"started": True} def _h_servo_cartesian_delta(self, p: dict) -> dict: owner = self._require_lease(p) with self._motion_lock(owner): + self._reset_joint_target_continuity() r = self.robot.servo_cartesian_delta( np.asarray(p["delta"], float), duration=p.get("duration"), @@ -340,6 +424,7 @@ def _h_servo_cartesian_delta(self, p: dict) -> dict: def _h_servo_cartesian_pose(self, p: dict) -> dict: owner = self._require_lease(p) with self._motion_lock(owner): + self._reset_joint_target_continuity() r = self.robot.servo_cartesian_pose( np.asarray(p["pose"], float), duration=float(p.get("duration", 0.2)), @@ -351,19 +436,66 @@ def _h_execute_cartesian_trajectory(self, p: dict) -> dict: owner = self._require_lease(p) traj = P.trajectory_from_dict(p["traj"]) with self._motion_lock(owner): + self._reset_joint_target_continuity() r = self.robot.execute_cartesian_trajectory(traj, blocking=True) return {"result": P.result_to_dict(r)} def _h_execute_joint_trajectory(self, p: dict) -> dict: owner = self._require_lease(p) + if p.get("protocol_id") != P.PROTOCOL_ID: + raise ValueError(f"protocol_id must be {P.PROTOCOL_ID!r}; old joint payload refused") + if p.get("protocol_fingerprint_sha256") != P.PROTOCOL_FINGERPRINT_SHA256: + raise ValueError( + "protocol_fingerprint_sha256 mismatch; joint contract must match exactly" + ) traj = P.joint_trajectory_from_dict(p["traj"]) with self._motion_lock(owner): - r = self.robot.execute_joint_trajectory(traj) + continuity, continuity_evidence = self._joint_target_execution_context(owner, traj) + try: + r = self.robot.execute_joint_trajectory(traj) + except TrajectoryPrevalidationError: + # No backend write occurred, so retain prior acknowledged + # targets as provenance for a corrected retry. + raise + except Exception: + self._reset_joint_target_continuity() + raise + r.log["continuity_source"] = continuity + r.log.update(continuity_evidence) + if r.success and traj.initial_positions is not None: + self._ack_joint_target(owner, traj, r.log.get("ending_gripper_target_m")) + r.log["acknowledged_ending_joint_target"] = self._last_ack_joint_target.tolist() + r.log["acknowledged_ending_gripper_target_m"] = self._last_ack_gripper_target + elif r.success: + self._reset_joint_target_continuity() + r.log["continuity_reset"] = "legacy_measured" + else: + self._reset_joint_target_continuity() + r.log["continuity_reset"] = r.stop_reason return {"result": P.result_to_dict(r)} + def _h_execute_joint_torque_trajectory(self, p: dict) -> dict: + owner = self._require_lease(p) + if p.get("protocol_id") != P.PROTOCOL_ID: + raise ValueError( + f"protocol_id must be {P.PROTOCOL_ID!r}; old torque payload refused" + ) + if ( + p.get("protocol_fingerprint_sha256") + != P.PROTOCOL_FINGERPRINT_SHA256 + ): + raise ValueError("protocol fingerprint mismatch") + traj = P.joint_torque_trajectory_from_dict(p["traj"]) + with self._motion_lock(owner): + self._reset_joint_target_continuity() + result = self.robot.execute_joint_torque_trajectory(traj) + self._reset_joint_target_continuity() + return {"result": P.result_to_dict(result)} + def _h_move_joint(self, p: dict) -> dict: owner = self._require_lease(p) with self._motion_lock(owner): + self._reset_joint_target_continuity() r = self.robot.move_joint( np.asarray(p["q"], float), duration=None if p.get("duration") is None else float(p["duration"]), @@ -379,15 +511,15 @@ def _h_command_gripper(self, p: dict) -> dict: g = P.gripper_from_dict(p["gripper"]) or GripperCommand() wait = bool(p.get("wait", False)) with self._motion_lock(owner): - w = self.robot.command_gripper( - g, wait=wait, timeout=float(p.get("timeout", 5.0)) - ) + self._reset_joint_target_continuity() + w = self.robot.command_gripper(g, wait=wait, timeout=float(p.get("timeout", 5.0))) return {"ok": True, "final_width": w} def _h_home(self, p: dict) -> dict: owner = self._require_lease(p) q = None if p.get("q") is None else np.asarray(p["q"], float) with self._motion_lock(owner): + self._reset_joint_target_continuity() self.robot.home( q, max_joint_speed=( @@ -407,13 +539,12 @@ def _h_go_home_safe(self, p: dict) -> dict: owner = self._require_lease(p) q = None if p.get("q_home") is None else np.asarray(p["q_home"], float) with self._motion_lock(owner): + self._reset_joint_target_continuity() r = self.robot.go_home_safe( q_home=q, lift_m=float(p.get("lift_m", 0.10)), open_gripper_width=( - None - if p.get("open_gripper_width") is None - else float(p["open_gripper_width"]) + None if p.get("open_gripper_width") is None else float(p["open_gripper_width"]) ), max_tcp_speed=float(p.get("max_tcp_speed", 0.10)), max_joint_speed=float(p.get("max_joint_speed", 0.3)), @@ -421,6 +552,7 @@ def _h_go_home_safe(self, p: dict) -> dict: return {"result": P.result_to_dict(r)} def _h_stop(self, p: dict) -> dict: + self._reset_joint_target_continuity() # stop does not require the lease -- anyone may e-stop. First request a # cooperative cancel so an in-flight blocking traj aborts at its next # tick (the executing thread performs the backend stop itself -- we never @@ -449,6 +581,7 @@ def _h_stop(self, p: dict) -> dict: # Lock busy: an executing traj will see the cancel at its next # tick. Re-set it in case a traj entry consumed it racing us. self.robot.request_stop() + self._reset_joint_target_continuity() return {"stopped": True} # -- always-on single-writer streaming loop (hold-on-stale) ------------- @@ -456,6 +589,7 @@ def _h_start_servo_loop(self, p: dict) -> dict: self._require_lease(p) with self._robot_lock: if self._servo_loop is None: + self._reset_joint_target_continuity() # Share _robot_lock with the loop so its per-tick writes and the # handlers' writes are mutually exclusive (single writer). loop = ReactiveServoLoop( diff --git a/src/flexiv_control/teleop/spacemouse.py b/src/flexiv_control/teleop/spacemouse.py index aee83e7..003103c 100644 --- a/src/flexiv_control/teleop/spacemouse.py +++ b/src/flexiv_control/teleop/spacemouse.py @@ -118,6 +118,10 @@ def __init__( deadband: float = 0.05, deadman_button: Optional[int] = 0, gripper_button: int = 1, + gripper_open_width: float = 0.09, + gripper_close_width: float = 0.01, + initial_open: Optional[bool] = None, + signs: Optional[List[float]] = None, frame: str = "base", owner: str = "spacemouse", ): @@ -132,7 +136,16 @@ def __init__( self.gripper_button = gripper_button self.frame = frame self.owner = owner - self._gripper_open = True + self.gripper_open_width = float(gripper_open_width) + self.gripper_close_width = float(gripper_close_width) + # None: infer from the robot's reported gripper width on first use + # (falls back to closed, matching the ROS teleop bridge convention that + # the first press opens). + self._gripper_open: Optional[bool] = None if initial_open is None else bool(initial_open) + # Optional per-axis sign flips [x, y, z, rx, ry, rz] so the device's + # axes can be calibrated to the robot frame (all +1 by default). + self.signs = np.ones(6) if signs is None else np.asarray(signs, float).reshape(6) + self._prev_gripper_button = 0 # -- mapping ------------------------------------------------------------- def to_delta(self, st: SpaceMouseState) -> np.ndarray: @@ -141,8 +154,8 @@ def to_delta(self, st: SpaceMouseState) -> np.ndarray: trans = self._apply_deadband(st.translation) rot = self._apply_deadband(st.rotation) delta = np.empty(6) - delta[:3] = trans * self.pos_scale * self.dt - delta[3:] = rot * self.rot_scale * self.dt + delta[:3] = trans * self.signs[:3] * self.pos_scale * self.dt + delta[3:] = rot * self.signs[3:] * self.rot_scale * self.dt return delta def _apply_deadband(self, v: np.ndarray) -> np.ndarray: @@ -151,6 +164,15 @@ def _apply_deadband(self, v: np.ndarray) -> np.ndarray: out[np.abs(v) < self.deadband] = 0.0 return out + def _gripper_is_open(self) -> bool: + if self._gripper_open is None: + try: + width = float(self.robot.get_state().gripper_width) + self._gripper_open = width > 0.04 + except Exception: + self._gripper_open = False + return self._gripper_open + def _deadman_ok(self, st: SpaceMouseState) -> bool: if self.deadman_button is None: return True @@ -160,11 +182,21 @@ def _deadman_ok(self, st: SpaceMouseState) -> bool: ) def _gripper_from_buttons(self, st: SpaceMouseState) -> Optional[GripperCommand]: - if len(st.buttons) > self.gripper_button and st.buttons[self.gripper_button]: - self._gripper_open = not self._gripper_open - width = 0.08 if self._gripper_open else 0.0 - return GripperCommand(width=width, force=20.0, grasp=not self._gripper_open) - return None + """Toggle on the button's RISING EDGE only. + + A held button lasts many control ticks; toggling on level would flip + the gripper open/closed at the loop rate for as long as it is pressed. + """ + pressed = int( + len(st.buttons) > self.gripper_button and bool(st.buttons[self.gripper_button]) + ) + rising = pressed == 1 and self._prev_gripper_button == 0 + self._prev_gripper_button = pressed + if not rising: + return None + self._gripper_open = not self._gripper_is_open() + width = self.gripper_open_width if self._gripper_open else self.gripper_close_width + return GripperCommand(width=width, force=20.0, grasp=not self._gripper_open) # -- RL intervention (SERL / HIL-SERL pattern) --------------------------- def intervention(self, policy_action: np.ndarray) -> Tuple[np.ndarray, bool]: @@ -176,13 +208,14 @@ def intervention(self, policy_action: np.ndarray) -> Tuple[np.ndarray, bool]: the human took over (so the RL loop can label the transition). """ st = self.source.read() + self._gripper_from_buttons(st) # keep the toggle state fresh moving = st.magnitude() > self.deadband and self._deadman_ok(st) if not moving: return np.asarray(policy_action, float).reshape(7), False a = np.zeros(7) a[:3] = np.clip(self._apply_deadband(st.translation), -1, 1) a[3:6] = np.clip(self._apply_deadband(st.rotation), -1, 1) - a[6] = -1.0 if not self._gripper_open else 1.0 + a[6] = 1.0 if self._gripper_is_open() else -1.0 return a, True # -- teleop loop --------------------------------------------------------- @@ -201,11 +234,17 @@ def run(self, duration: Optional[float] = None, *, max_ticks: Optional[int] = No if max_ticks is not None and ticks >= max_ticks: break st = self.source.read() + # Button edges are tracked every tick; the gripper is NOT + # deadman-gated (matching the ROS bridge), so a press with the + # deadman released still actuates it, with zero motion. + grip = self._gripper_from_buttons(st) if self._deadman_ok(st): - delta = self.to_delta(st) - grip = self._gripper_from_buttons(st) self.robot.servo_cartesian_delta( - delta, duration=self.dt, frame=self.frame, gripper=grip + self.to_delta(st), duration=self.dt, frame=self.frame, gripper=grip + ) + elif grip is not None: + self.robot.servo_cartesian_delta( + np.zeros(6), duration=self.dt, frame=self.frame, gripper=grip ) ticks += 1 next_tick += self.dt diff --git a/src/flexiv_control/trajectory.py b/src/flexiv_control/trajectory.py index 5156421..2290899 100644 --- a/src/flexiv_control/trajectory.py +++ b/src/flexiv_control/trajectory.py @@ -35,7 +35,7 @@ from dataclasses import dataclass, field, replace from enum import Enum -from typing import List, Optional +from typing import List, Optional, Union import numpy as np @@ -60,10 +60,10 @@ class CartesianWaypoint: """ position: np.ndarray - quaternion: Optional[np.ndarray] = None # (w, x, y, z); None -> hold + quaternion: Optional[np.ndarray] = None # (w, x, y, z); None -> hold gripper: Optional[GripperCommand] = None # None -> hold gripper - n_frames: Optional[int] = None # number of low-level control frames - duration: Optional[float] = None # seconds (alternative to n_frames) + n_frames: Optional[int] = None # number of low-level control frames + duration: Optional[float] = None # seconds (alternative to n_frames) frame: str = "base" def __post_init__(self) -> None: @@ -122,12 +122,12 @@ class CartesianTrajectory: # Kinematic SPEED envelope. Enforced as TIGHTENING-ONLY at execution: the # interpolator runs at min(traj cap, active profile cap), so a traj may # slow itself below the profile but can never relax the profile's limits. - max_tcp_linear_speed: float = 0.25 # m/s - max_tcp_angular_speed: float = 0.60 # rad/s + max_tcp_linear_speed: float = 0.25 # m/s + max_tcp_angular_speed: float = 0.60 # rad/s # Acceleration fields are ADVISORY metadata only (logged, not enforced); # the profile's opt-in per-tick accel cap (max_linear_accel) is what binds. - max_tcp_linear_acc: float = 1.0 # m/s^2 - max_tcp_angular_acc: float = 2.0 # rad/s^2 + max_tcp_linear_acc: float = 1.0 # m/s^2 + max_tcp_angular_acc: float = 2.0 # rad/s^2 # Contact envelope (None -> use the safety profile default). Like the speed # caps, applied as min(traj, profile) -- tightening only. @@ -228,8 +228,12 @@ def resolve_to_absolute(self, start_pose: np.ndarray) -> "CartesianTrajectory": ) new_wps.append( CartesianWaypoint( - position=abs_pos, quaternion=abs_quat, gripper=w.gripper, - n_frames=w.n_frames, duration=w.duration, frame=w.frame, + position=abs_pos, + quaternion=abs_quat, + gripper=w.gripper, + n_frames=w.n_frames, + duration=w.duration, + frame=w.frame, ) ) return replace(self, waypoints=new_wps, representation=TrajectoryRepresentation.ABSOLUTE) @@ -413,9 +417,9 @@ class CartesianDelta: The translation is taken in ``frame`` ("base" or "tcp"). """ - delta: np.ndarray # length-6, base or tcp frame + delta: np.ndarray # length-6, base or tcp frame gripper: Optional[GripperCommand] = None - duration: float = 0.05 # 20 Hz default control step + duration: float = 0.05 # 20 Hz default control step frame: str = "base" def __post_init__(self) -> None: @@ -425,16 +429,108 @@ def __post_init__(self) -> None: # ---------------------------------------------------------------------------- # Joint space (reset / home / MoveIt-plan execution) # ---------------------------------------------------------------------------- +@dataclass +class JointGripperTarget: + """Exact gripper target synchronized with a joint segment. + + ``velocity=None`` derives the velocity needed to cover the width delta in + the segment's scheduled ticks. A supplied velocity asserts the same timing + contract. This contract uses fire-and-forget ``Gripper.Move`` only. + """ + + width: float + force: float = 20.0 + velocity: Optional[float] = None + + def __post_init__(self) -> None: + self.width = float(self.width) + self.force = float(self.force) + if not np.isfinite(self.width): + raise ValueError("JointGripperTarget.width must be finite") + if not np.isfinite(self.force): + raise ValueError("JointGripperTarget.force must be finite") + if self.velocity is not None: + self.velocity = float(self.velocity) + if not np.isfinite(self.velocity) or self.velocity <= 0.0: + raise ValueError("JointGripperTarget.velocity must be finite and > 0") + + +@dataclass +class JointGripperForceTarget: + """Signed native gripper-force target synchronized with a joint segment. + + The backend dispatches ``Gripper.Grasp(force)``. Positive force closes and + negative force opens, exactly matching Flexiv RDK. No desired width is part + of this action; measured width remains observable robot state. + """ + + force: float + + def __post_init__(self) -> None: + self.force = float(self.force) + if not np.isfinite(self.force): + raise ValueError("JointGripperForceTarget.force must be finite") + + @classmethod + def from_signed_effort_latent( + cls, + latent: float, + *, + force_limit: float, + ) -> "JointGripperForceTarget": + """Decode one planner latent into a physical signed-force target. + + The semantic coordinate is dimensionless and bounded by ``[-1, 1]``: + positive closes, negative opens, and zero requests zero force. Only + the decoded Newton value is retained by this object or sent over the + trajectory RPC. This is deliberately *not* the legacy positional + :meth:`GripperCommand.from_signed_action` convention, whose positive + endpoint means open. + + ``force_limit`` is explicit rather than a library constant because the + connected gripper's runtime limits remain authoritative. + """ + value = float(latent) + limit = float(force_limit) + if not np.isfinite(value) or not -1.0 <= value <= 1.0: + raise ValueError("signed effort latent must be finite and in [-1, 1]") + if not np.isfinite(limit) or limit <= 0.0: + raise ValueError("force_limit must be finite and > 0") + return cls(force=value * limit) + + @dataclass class JointWaypoint: positions: np.ndarray n_frames: Optional[int] = None duration: Optional[float] = None + gripper: Optional[ + Union[JointGripperTarget, JointGripperForceTarget, GripperCommand] + ] = None def __post_init__(self) -> None: - self.positions = np.asarray(self.positions, float) + self.positions = np.asarray(self.positions, float).reshape(-1) + if not np.all(np.isfinite(self.positions)): + raise ValueError("JointWaypoint.positions must be finite") if self.n_frames is None and self.duration is None: raise ValueError("JointWaypoint needs n_frames or duration") + if self.n_frames is not None: + if isinstance(self.n_frames, (bool, np.bool_)) or int(self.n_frames) != self.n_frames: + raise ValueError("JointWaypoint.n_frames must be an integer") + self.n_frames = int(self.n_frames) + if self.n_frames <= 0: + raise ValueError("JointWaypoint.n_frames must be positive") + if self.gripper is not None and isinstance(self.gripper, GripperCommand): + if self.gripper.grasp: + raise ValueError( + "JointWaypoint requires an exact Gripper.Move target; " + "grasp=True has no exact width" + ) + self.gripper = JointGripperTarget( + width=self.gripper.width, + force=self.gripper.force, + velocity=self.gripper.velocity, + ) def resolve_duration(self, control_hz: float) -> float: if self.duration is not None: @@ -445,14 +541,116 @@ def resolve_duration(self, control_hz: float) -> float: @dataclass class JointTrajectory: waypoints: List[JointWaypoint] - max_joint_speed_scale: float = 0.3 # fraction of joint vel limits - # Same semantics as CartesianTrajectory.safety_profile: "" = use the active - # profile; a non-empty name must match the active profile or execution raises. + # Preserve the legacy positional field order; v0.2.3 fields are appended. + max_joint_speed_scale: float = 0.3 + interpolation: str = "cosine" safety_profile: str = "" + # Actuator-target knot 0 is intentionally distinct from measured state. + initial_positions: Optional[np.ndarray] = None + initial_gripper_width: Optional[float] = None + # Authoritative n_frames: reject rate violations, never time-stretch. + strict_timing: bool = False def __post_init__(self) -> None: if not self.waypoints: raise ValueError("JointTrajectory needs at least one waypoint") + if self.initial_positions is not None: + self.initial_positions = np.asarray(self.initial_positions, float).reshape(-1) + if not np.all(np.isfinite(self.initial_positions)): + raise ValueError("initial_positions must be finite") + if self.initial_gripper_width is not None: + self.initial_gripper_width = float(self.initial_gripper_width) + if not np.isfinite(self.initial_gripper_width): + raise ValueError("initial_gripper_width must be finite") + self.max_joint_speed_scale = float(self.max_joint_speed_scale) + if ( + not np.isfinite(self.max_joint_speed_scale) + or not 0.0 < self.max_joint_speed_scale <= 1.0 + ): + raise ValueError("max_joint_speed_scale must be finite and in (0, 1]") + if self.interpolation not in {"cosine", "linear"}: + raise ValueError("JointTrajectory.interpolation must be 'cosine' or 'linear'") + self.strict_timing = bool(self.strict_timing) + if self.strict_timing: + if self.initial_positions is None: + raise ValueError("strict_timing requires explicit initial_positions") + for wp in self.waypoints: + if wp.n_frames is None or wp.duration is not None: + raise ValueError( + "strict_timing requires n_frames (and no duration) on every JointWaypoint" + ) + if ( + any(isinstance(wp.gripper, JointGripperTarget) for wp in self.waypoints) + and self.initial_gripper_width is None + ): + raise ValueError("strict_timing gripper waypoints require initial_gripper_width") + + +@dataclass +class JointTorqueWaypoint: + """One smooth arm-torque endpoint and its exact controller duration.""" + + torques: np.ndarray + n_frames: int + gripper: Optional[ + Union[JointGripperTarget, JointGripperForceTarget, GripperCommand] + ] = None + + def __post_init__(self) -> None: + self.torques = np.asarray(self.torques, dtype=float).reshape(-1) + if not np.all(np.isfinite(self.torques)): + raise ValueError("JointTorqueWaypoint.torques must be finite") + if isinstance(self.n_frames, (bool, np.bool_)): + raise ValueError("JointTorqueWaypoint.n_frames must be an integer") + self.n_frames = int(self.n_frames) + if self.n_frames <= 0: + raise ValueError("JointTorqueWaypoint.n_frames must be positive") + if isinstance(self.gripper, GripperCommand): + if self.gripper.grasp: + self.gripper = JointGripperForceTarget( + force=self.gripper.force + ) + else: + self.gripper = JointGripperTarget( + width=self.gripper.width, + force=self.gripper.force, + velocity=self.gripper.velocity, + ) + + +@dataclass +class JointTorqueTrajectory: + """Gravity-compensated arm torque plus synchronized gripper effort. + + ``initial_torques`` is the command immediately before the first emitted + tick. Every segment linearly approaches its endpoint, satisfying Flexiv's + requirement that streamed torque commands remain smooth and continuous. + """ + + waypoints: List[JointTorqueWaypoint] + initial_torques: np.ndarray + max_joint_torque_scale: float = 0.30 + safety_profile: str = "" + + def __post_init__(self) -> None: + if not self.waypoints: + raise ValueError("JointTorqueTrajectory needs at least one waypoint") + self.initial_torques = np.asarray( + self.initial_torques, dtype=float + ).reshape(-1) + if not np.all(np.isfinite(self.initial_torques)): + raise ValueError("initial_torques must be finite") + if any( + waypoint.torques.shape != self.initial_torques.shape + for waypoint in self.waypoints + ): + raise ValueError("all torque waypoints must match initial_torques") + self.max_joint_torque_scale = float(self.max_joint_torque_scale) + if ( + not np.isfinite(self.max_joint_torque_scale) + or not 0.0 < self.max_joint_torque_scale <= 1.0 + ): + raise ValueError("max_joint_torque_scale must be in (0, 1]") # ---------------------------------------------------------------------------- @@ -473,7 +671,7 @@ class ExecutionResult: clipped: bool = False stop_reason: str = "none" executed_duration: float = 0.0 - path_tracking_error: float = 0.0 # max ||pose_cmd - pose_meas|| over run + path_tracking_error: float = 0.0 # max ||pose_cmd - pose_meas|| over run max_tcp_speed: float = 0.0 max_joint_speed: float = 0.0 max_wrench: float = 0.0 diff --git a/src/flexiv_control/types.py b/src/flexiv_control/types.py index ef41803..7062a6e 100644 --- a/src/flexiv_control/types.py +++ b/src/flexiv_control/types.py @@ -82,6 +82,7 @@ class StopReason(str, Enum): STALE_STATE = "stale_state" WORKSPACE = "workspace_limit" JOINT_LIMIT = "joint_limit" + GRIPPER_LIMIT = "gripper_limit" TCP_SPEED = "tcp_speed_limit" POSE_JUMP = "pose_jump_limit" CONTACT_WRENCH = "contact_wrench" diff --git a/tests/test_conformance.py b/tests/test_conformance.py index a2b4ed5..3321019 100644 --- a/tests/test_conformance.py +++ b/tests/test_conformance.py @@ -47,17 +47,23 @@ def test_mujoco_backend_fails_loudly_without_model(): # -- torque gate ------------------------------------------------------------- def test_rt_joint_torque_is_gated_off(): - pytest.importorskip("flexivrdk") + flexivrdk = pytest.importorskip("flexivrdk") from flexiv_control.backends.flexiv_rdk import FlexivRdkBackend b = FlexivRdkBackend("X") # allow_torque defaults to False b._robot = type("R", (), {"SwitchMode": staticmethod(lambda *_a, **_k: None)})() with pytest.raises(RuntimeError): b.set_mode(ControlMode.RT_JOINT_TORQUE) - # opt-in lets it through + # Opt-in lets it through only when the installed RDK exposes that mode. + # Older supported RDK builds intentionally fail loudly instead of + # pretending that an unavailable real-time mode was selected. b2 = FlexivRdkBackend("X", allow_torque=True) b2._robot = type("R", (), {"SwitchMode": staticmethod(lambda *_a, **_k: None)})() - b2.set_mode(ControlMode.RT_JOINT_TORQUE) # must not raise + if hasattr(flexivrdk.Mode, "RT_JOINT_TORQUE"): + b2.set_mode(ControlMode.RT_JOINT_TORQUE) + else: + with pytest.raises(RuntimeError, match="does not expose"): + b2.set_mode(ControlMode.RT_JOINT_TORQUE) # -- SpaceMouse intervention (the previously-untested HIL-SERL path) ---------- diff --git a/tests/test_flexiv_rdk_runtime_contract.py b/tests/test_flexiv_rdk_runtime_contract.py new file mode 100644 index 0000000..1f3a998 --- /dev/null +++ b/tests/test_flexiv_rdk_runtime_contract.py @@ -0,0 +1,185 @@ +from types import SimpleNamespace + +import numpy as np +import pytest + +from flexiv_control.backends.flexiv_rdk import FlexivRdkBackend +from flexiv_control.types import ControlMode, GripperCommand + + +class _Robot: + def __init__(self, states): + self._states = states + + def states(self): + return self._states + + def fault(self): + return False + + +class _Gripper: + def __init__(self, states=None): + self._states = states + self.calls = [] + + def states(self): + return self._states + + def Move(self, width, velocity, force): + self.calls.append(("move", width, velocity, force)) + + def Grasp(self, force): + self.calls.append(("grasp", force)) + + +def _backend(*, robot_states=None, gripper_states=None): + backend = object.__new__(FlexivRdkBackend) + backend.robot_sn = "Rizon4s-test" + backend.n_joints = 7 + backend._gripper_name = "GN01" + backend._allow_torque = False + if robot_states is None: + robot_states = SimpleNamespace( + q=np.zeros(7), + dq=np.zeros(7), + tau=np.zeros(7), + tcp_pose=np.array([0, 0, 0, 1, 0, 0, 0], float), + tcp_vel=np.zeros(6), + ext_wrench_in_tcp=np.zeros(6), + ) + backend._robot = _Robot(robot_states) + backend._gripper = _Gripper( + gripper_states + if gripper_states is not None + else SimpleNamespace(width=0.08, force=0.0, is_moving=False) + ) + backend._mode = ControlMode.IDLE + backend._connected = True + backend._runtime_info = {} + backend._gripper_limits = { + "source": "flexivrdk.Gripper.params", + "device_name": "GN01", + "min_width_m": 0.0, + "max_width_m": 0.085, + "min_velocity_m_s": 0.005, + "max_velocity_m_s": 0.2, + "min_force_n": 1.0, + "max_force_n": 80.0, + } + return backend + + +def test_read_state_rejects_missing_robot_field(): + states = SimpleNamespace( + dq=np.zeros(7), + tau=np.zeros(7), + tcp_pose=np.array([0, 0, 0, 1, 0, 0, 0], float), + tcp_vel=np.zeros(6), + ext_wrench_in_tcp=np.zeros(6), + ) + with pytest.raises(RuntimeError, match="RobotStates.*q"): + _backend(robot_states=states).read_state() + + +def test_read_state_rejects_bad_shape_and_nonfinite_values(): + backend = _backend() + backend._robot._states.dq = np.zeros(6) + with pytest.raises(RuntimeError, match="shape"): + backend.read_state() + backend = _backend() + backend._robot._states.tcp_pose[0] = np.nan + with pytest.raises(RuntimeError, match="non-finite"): + backend.read_state() + + +def test_read_state_rejects_missing_gripper_field(): + backend = _backend( + gripper_states=SimpleNamespace(width=0.08, force=0.0) + ) + with pytest.raises(RuntimeError, match="GripperStates.*is_moving"): + backend.read_state() + + +def test_read_state_rejects_configured_but_unavailable_gripper(): + backend = _backend() + backend._gripper = None + with pytest.raises(RuntimeError, match="refusing to fabricate"): + backend.read_state() + + +def test_fault_query_is_fail_closed_when_missing_or_raising(): + backend = _backend() + backend._robot.fault = None + assert backend.in_fault() is True + + def failed_fault_read(): + raise RuntimeError("transport failed") + + backend._robot.fault = failed_fault_read + assert backend.in_fault() is True + + +def test_gripper_move_is_checked_against_runtime_params(): + backend = _backend() + backend.move_gripper( + GripperCommand( + width=0.08, + velocity=0.1, + force=40.0, + grasp=False, + ) + ) + assert backend._gripper.calls == [("move", 0.08, 0.1, 40.0)] + + for command, field in ( + ( + GripperCommand( + width=0.09, + velocity=0.1, + force=40.0, + grasp=False, + ), + "width", + ), + ( + GripperCommand( + width=0.08, + velocity=0.001, + force=40.0, + grasp=False, + ), + "velocity", + ), + ( + GripperCommand( + width=0.08, + velocity=0.1, + force=100.0, + grasp=False, + ), + "force", + ), + ): + with pytest.raises(ValueError, match=field): + backend.move_gripper(command) + + +def test_grasp_checks_only_the_parameter_rdk_consumes(): + backend = _backend() + backend.move_gripper( + GripperCommand( + width=999.0, + velocity=999.0, + force=20.0, + grasp=True, + ) + ) + assert backend._gripper.calls == [("grasp", 20.0)] + + +def test_gripper_command_fails_closed_without_runtime_limits(): + backend = _backend() + backend._gripper_limits = None + with pytest.raises(RuntimeError, match="limits are unavailable"): + backend.move_gripper(GripperCommand()) diff --git a/tests/test_friction_fixes.py b/tests/test_friction_fixes.py index 3c89cfd..f300042 100644 --- a/tests/test_friction_fixes.py +++ b/tests/test_friction_fixes.py @@ -20,6 +20,7 @@ import threading import time +from types import SimpleNamespace import numpy as np import pytest @@ -189,6 +190,30 @@ def test_command_gripper_wait_returns_width(): assert r.command_gripper(GripperCommand(width=0.08)) is None # fire-and-forget +def test_move_wait_does_not_accept_pre_motion_stillness(monkeypatch): + """GN01 may report stillness during command latency; wait for Move target.""" + r = _robot() + reads = 0 + + def delayed_state(): + nonlocal reads + reads += 1 + return SimpleNamespace( + gripper_width=0.001 if reads < 18 else 0.1, + gripper_is_moving=False, + ) + + monkeypatch.setattr(r.backend, "move_gripper", lambda _cmd: None) + monkeypatch.setattr(r, "get_state", delayed_state) + + width = r.command_gripper( + GripperCommand(width=0.1), wait=True, timeout=2.0 + ) + + assert width == pytest.approx(0.1) + assert reads >= 18 + + def test_home_restores_joints_and_gripper(): cfg = RobotConfig(backend="fake", gripper_home_width=0.085) r = Robot(cfg) diff --git a/tests/test_interpolation.py b/tests/test_interpolation.py index 7dea617..7940d0b 100644 --- a/tests/test_interpolation.py +++ b/tests/test_interpolation.py @@ -1,8 +1,17 @@ import numpy as np +import pytest -from flexiv_control import CartesianTrajectory, CartesianWaypoint +from flexiv_control import ( + CartesianTrajectory, + CartesianWaypoint, + JointTrajectory, + JointWaypoint, +) from flexiv_control import transforms as T -from flexiv_control.interpolation import CartesianTrajectoryInterpolator +from flexiv_control.interpolation import ( + CartesianTrajectoryInterpolator, + JointTrajectoryInterpolator, +) def test_quat_mul_identity(): @@ -54,3 +63,86 @@ def test_interpolator_holds_orientation(): interp = CartesianTrajectoryInterpolator(CartesianTrajectory(waypoints=[wp]), start, 100.0) for pose, _ in interp: assert np.allclose(pose[3:7], [1, 0, 0, 0], atol=1e-9) + + +def test_joint_linear_interpolation_uses_cumulative_tick_rounding(): + start = np.zeros(7) + first = np.ones(7) + second = np.full(7, 2.0) + traj = JointTrajectory( + waypoints=[ + JointWaypoint(positions=first, duration=0.1632), + JointWaypoint(positions=second, duration=0.1632), + ], + interpolation="linear", + ) + interp = JointTrajectoryInterpolator(traj, start, control_hz=100.0) + setpoints = interp.setpoints() + + assert interp.nominal_segment_ticks == [16, 17] + assert interp.nominal_total_ticks == 33 + assert interp.scheduled_segment_ticks == [16, 17] + assert len(setpoints) == 33 + assert np.allclose(setpoints[0], first / 16.0) + assert np.allclose(setpoints[15], first) + assert np.allclose(setpoints[16], first + (second - first) / 17.0) + assert np.allclose(setpoints[-1], second) + + +def test_joint_cosine_remains_the_backward_compatible_default(): + traj = JointTrajectory( + waypoints=[JointWaypoint(positions=np.ones(7), n_frames=4)] + ) + setpoints = JointTrajectoryInterpolator( + traj, + np.zeros(7), + control_hz=100.0, + ).setpoints() + + assert traj.interpolation == "cosine" + assert np.allclose(setpoints[0], np.full(7, 0.1464466094067262)) + assert np.allclose(setpoints[-1], np.ones(7)) + + +def test_joint_speed_limit_is_enforced_per_joint(): + target = np.array([0.5, 0.5, 0, 0, 0, 0, 0], float) + limits = np.array([1.0, 0.25, 1, 1, 1, 1, 1], float) + traj = JointTrajectory( + waypoints=[ + JointWaypoint(positions=target, duration=0.1) + ], + interpolation="linear", + ) + interp = JointTrajectoryInterpolator( + traj, + np.zeros(7), + control_hz=100.0, + max_joint_speed=limits, + ) + points = interp.setpoints() + + # Joint 2 is limiting: 0.5 rad / 0.25 rad/s = 2 seconds. + assert interp.scheduled_total_ticks == 200 + stream = np.vstack([np.zeros(7), points]) + observed = np.max(np.abs(np.diff(stream, axis=0)), axis=0) * 100.0 + assert np.all(observed <= limits + 1e-12) + + +def test_joint_speed_vector_validation_is_fail_closed(): + traj = JointTrajectory( + waypoints=[JointWaypoint(positions=np.zeros(7), duration=0.1)] + ) + with pytest.raises(ValueError, match="match start_q shape"): + JointTrajectoryInterpolator( + traj, + np.zeros(7), + control_hz=100.0, + max_joint_speed=np.ones(6), + ) + with pytest.raises(ValueError, match="finite and > 0"): + JointTrajectoryInterpolator( + traj, + np.zeros(7), + control_hz=100.0, + max_joint_speed=np.zeros(7), + ) diff --git a/tests/test_joint_gripper_force_v4.py b/tests/test_joint_gripper_force_v4.py new file mode 100644 index 0000000..82d36b4 --- /dev/null +++ b/tests/test_joint_gripper_force_v4.py @@ -0,0 +1,143 @@ +from __future__ import annotations + +import copy + +import numpy as np +import pytest + +from flexiv_control import ( + JointGripperForceTarget, + JointGripperTarget, + JointTrajectory, + JointWaypoint, + Robot, + RobotConfig, +) +from flexiv_control.robot import TrajectoryPrevalidationError +from flexiv_control.server import protocol as P + + +def _trajectory(*forces: float, q0: np.ndarray | None = None) -> JointTrajectory: + q0 = np.zeros(7) if q0 is None else np.asarray(q0, float) + return JointTrajectory( + initial_positions=q0, + waypoints=[ + JointWaypoint( + positions=q0.copy(), + n_frames=2, + gripper=JointGripperForceTarget(force=force), + ) + for force in forces + ], + interpolation="linear", + strict_timing=True, + max_joint_speed_scale=0.3, + ) + + +def test_v4_force_and_move_targets_are_explicitly_tagged() -> None: + force_payload = P.joint_trajectory_to_dict(_trajectory(20.0)) + assert force_payload["schema"] == "flexiv-control.joint-trajectory.v4" + assert force_payload["waypoints"][0]["gripper"] == { + "mode": "force", + "force": 20.0, + } + restored = P.joint_trajectory_from_dict(force_payload) + assert restored.initial_gripper_width is None + assert isinstance(restored.waypoints[0].gripper, JointGripperForceTarget) + + move = JointTrajectory( + initial_positions=np.zeros(7), + initial_gripper_width=0.08, + waypoints=[ + JointWaypoint( + positions=np.zeros(7), + n_frames=2, + gripper=JointGripperTarget(width=0.07, force=15.0), + ) + ], + interpolation="linear", + strict_timing=True, + ) + move_payload = P.joint_trajectory_to_dict(move) + assert move_payload["waypoints"][0]["gripper"] == { + "mode": "move", + "width": 0.07, + "force_limit": 15.0, + "velocity": None, + } + + +def test_signed_effort_latent_decodes_once_to_physical_newtons() -> None: + assert JointGripperForceTarget.from_signed_effort_latent( + 0.5, force_limit=80.0 + ).force == pytest.approx(40.0) + assert JointGripperForceTarget.from_signed_effort_latent( + -1.0, force_limit=80.0 + ).force == pytest.approx(-80.0) + with pytest.raises(ValueError, match=r"\[-1, 1\]"): + JointGripperForceTarget.from_signed_effort_latent( + 1.01, force_limit=80.0 + ) + with pytest.raises(ValueError, match="force_limit"): + JointGripperForceTarget.from_signed_effort_latent( + 0.0, force_limit=0.0 + ) + + +def test_v4_rejects_untagged_or_structurally_ambiguous_gripper() -> None: + payload = P.joint_trajectory_to_dict(_trajectory(20.0)) + untagged = copy.deepcopy(payload) + untagged["waypoints"][0]["gripper"].pop("mode") + with pytest.raises(ValueError, match="mode"): + P.joint_trajectory_from_dict(untagged) + mixed = copy.deepcopy(payload) + mixed["waypoints"][0]["gripper"]["width"] = 0.02 + with pytest.raises(ValueError, match="extra"): + P.joint_trajectory_from_dict(mixed) + + +def test_force_trajectory_dispatches_signed_grasp_at_segment_boundaries() -> None: + robot = Robot(RobotConfig(backend="fake", control_hz=1000.0)) + with robot: + robot.acquire_lease("test") + robot.start_joint_impedance() + result = robot.execute_joint_trajectory( + _trajectory(17.0, -9.0, q0=robot.get_state().q) + ) + assert result.success + assert [cmd.force for cmd in robot.backend.gripper_log] == [17.0, -9.0] + assert all(cmd.grasp for cmd in robot.backend.gripper_log) + assert [event["mode"] for event in result.log["gripper_events"]] == [ + "force", + "force", + ] + assert result.log["ending_gripper_target_m"] == pytest.approx(0.08) + assert result.log["ending_gripper_force_n"] == -9.0 + force_rows = [ + row for row in result.log["gripper_tracking"] if row["target_force_n"] is not None + ] + assert force_rows + assert all(row["target_width_m"] is None for row in force_rows) + assert all(row["error_m"] is None for row in force_rows) + + +def test_force_trajectory_fails_closed_against_runtime_limits() -> None: + robot = Robot(RobotConfig(backend="fake", control_hz=1000.0)) + with robot: + robot.acquire_lease("test") + robot.start_joint_impedance() + robot.backend.runtime_info = lambda: { + "gripper_limits": { + "min_width_m": 0.0, + "max_width_m": 0.1, + "min_velocity_m_s": 0.001, + "max_velocity_m_s": 0.2, + "min_force_n": -80.0, + "max_force_n": 80.0, + } + } + with pytest.raises(TrajectoryPrevalidationError, match="outside runtime"): + robot.execute_joint_trajectory(_trajectory(81.0, q0=robot.get_state().q)) + assert robot.backend.gripper_log == [] + assert robot.backend.joint_log == [] diff --git a/tests/test_joint_gripper_sync_v023.py b/tests/test_joint_gripper_sync_v023.py new file mode 100644 index 0000000..b506fbb --- /dev/null +++ b/tests/test_joint_gripper_sync_v023.py @@ -0,0 +1,524 @@ +import copy + +import numpy as np +import pytest + +from flexiv_control import ( + ControlMode, + FakeBackend, + GripperCommand, + JointGripperTarget, + JointTrajectory, + JointWaypoint, + Robot, + RobotConfig, +) +from flexiv_control.client import RemoteRobot, RemoteRobotError +from flexiv_control.interpolation import JointTrajectoryInterpolator +from flexiv_control.server import FlexivControlServer +from flexiv_control.server import protocol as P + + +class OrderedFakeBackend(FakeBackend): + def __init__(self, **kwargs): + super().__init__(**kwargs) + self.write_events = [] + + def move_gripper(self, cmd): + self.write_events.append(("gripper", float(cmd.width), float(cmd.velocity))) + super().move_gripper(cmd) + + def stream_joint(self, q): + self.write_events.append(("joint", np.asarray(q, float).copy())) + super().stream_joint(q) + + +class LimitedOrderedFakeBackend(OrderedFakeBackend): + def runtime_info(self): + return { + "gripper_limits": { + "source": "flexivrdk.Gripper.params", + "device_name": "fake-limited", + "min_width_m": 0.0, + "max_width_m": 0.1, + "min_velocity_m_s": 0.06, + "max_velocity_m_s": 0.2, + "min_force_n": 5.0, + "max_force_n": 40.0, + } + } + + +class ModeDriftBackend(LimitedOrderedFakeBackend): + def __init__(self, *, q_after_mode=None, gripper_after_mode=None, **kwargs): + super().__init__(**kwargs) + self.q_after_mode = q_after_mode + self.gripper_after_mode = gripper_after_mode + + def set_mode(self, mode, **kwargs): + super().set_mode(mode, **kwargs) + if self.q_after_mode is not None: + self._q = np.asarray(self.q_after_mode, float).copy() + if self.gripper_after_mode is not None: + self._gripper_width = float(self.gripper_after_mode) + + +def strict_traj(initial, targets, frames, grippers=None): + grippers = grippers or [None] * len(targets) + return JointTrajectory( + initial_positions=np.asarray(initial, float), + initial_gripper_width=0.08 if any(g is not None for g in grippers) else None, + waypoints=[ + JointWaypoint( + positions=np.asarray(q, float), + n_frames=n, + gripper=g, + ) + for q, n, g in zip(targets, frames, grippers) + ], + interpolation="linear", + strict_timing=True, + max_joint_speed_scale=0.3, + ) + + +def test_strict_prefix_32_1_is_authoritative_and_never_stretched(): + q0 = np.zeros(7) + q1 = q0.copy() + q1[0] = 0.1 + q2 = q1.copy() + q2[0] += 0.005 + traj = strict_traj(q0, [q1, q2], [32, 1]) + interp = JointTrajectoryInterpolator(traj, q0, 100.0, max_joint_speed=np.full(7, 1.0)) + assert interp.requested_segment_ticks == [32, 1] + assert interp.scheduled_segment_ticks == [32, 1] + assert interp.requested_total_ticks == interp.scheduled_total_ticks == 33 + assert len(interp.setpoints()) == 33 + + too_fast = strict_traj(q0, [np.full(7, 0.2)], [1]) + with pytest.raises(ValueError, match="n_frames=1 is authoritative"): + JointTrajectoryInterpolator(too_fast, q0, 100.0, max_joint_speed=np.full(7, 1.0)) + + +def test_joint_v4_schema_round_trip_and_old_or_loose_payload_refusal(): + q0 = np.zeros(7) + traj = strict_traj(q0, [q0], [2], [JointGripperTarget(width=0.079, velocity=None)]) + payload = P.joint_trajectory_to_dict(traj) + restored = P.joint_trajectory_from_dict(payload) + assert restored.strict_timing is True + assert restored.waypoints[0].gripper.velocity is None + + old = {"waypoints": [{"positions": q0.tolist(), "n_frames": 2, "duration": None}]} + with pytest.raises(ValueError, match="keys do not match"): + P.joint_trajectory_from_dict(old) + extra = copy.deepcopy(payload) + extra["legacy"] = True + with pytest.raises(ValueError, match=r"extra=\['legacy'\]"): + P.joint_trajectory_from_dict(extra) + wrong = copy.deepcopy(payload) + wrong["schema"] = "flexiv-control.joint-trajectory.v2" + with pytest.raises(ValueError, match="unsupported JointTrajectory schema"): + P.joint_trajectory_from_dict(wrong) + + invalid_numeric_payloads = [] + for mutate in ( + lambda d: d.__setitem__("max_joint_speed_scale", "0.3"), + lambda d: d["initial_positions"].__setitem__(0, "0.0"), + lambda d: d.__setitem__("initial_gripper_width", "0.08"), + lambda d: d["waypoints"][0]["positions"].__setitem__(0, True), + lambda d: d["waypoints"][0].__setitem__("n_frames", 2.0), + lambda d: d["waypoints"][0]["gripper"].__setitem__("width", "0.079"), + lambda d: d["waypoints"][0]["gripper"].__setitem__("force_limit", True), + lambda d: d["waypoints"][0]["gripper"].__setitem__("velocity", "0.05"), + ): + invalid = copy.deepcopy(payload) + mutate(invalid) + invalid_numeric_payloads.append(invalid) + for invalid in invalid_numeric_payloads: + with pytest.raises(ValueError, match="JSON"): + P.joint_trajectory_from_dict(invalid) + + +def test_first_strict_call_bounds_actual_first_setpoint_from_measured_state(): + q0 = np.zeros(7) + backend = OrderedFakeBackend(start_q=q0) + robot = Robot( + config=RobotConfig(backend="fake", control_hz=100.0), + backend=backend, + control_hz=100.0, + ) + server = FlexivControlServer( + robot=robot, host="127.0.0.1", port=0, lease_ttl=1.0, host_lock=False + ) + server.start() + port = server._tcp.server_address[1] + server.serve_in_thread() + try: + with RemoteRobot("127.0.0.1", port, owner="prefix") as remote: + initial = q0.copy() + initial[0] = 0.006 + unsafe_target = initial.copy() + unsafe_target[0] = 0.012 + with pytest.raises(RemoteRobotError, match="first emitted joint target"): + remote.execute_joint_trajectory(strict_traj(initial, [unsafe_target], [1])) + assert backend.write_events == [] + assert backend.mode_log == [] + + # Knot 0 need not equal telemetry: only the first actual command is + # constrained, here exactly 0.6 rad/s * 10 ms from measured q. + safe = remote.execute_joint_trajectory(strict_traj(initial, [initial], [1])) + assert safe.success + assert safe.log["joint_filter_anchor"] == q0.tolist() + assert backend.joint_log[-1][0] == pytest.approx(0.006) + finally: + server.shutdown() + + +def test_predispatch_joint_recheck_uses_state_after_mode_switch(): + q0 = np.zeros(7) + drifted_q = q0.copy() + drifted_q[0] = -0.004 + backend = ModeDriftBackend(start_q=q0, q_after_mode=drifted_q) + robot = Robot( + config=RobotConfig(backend="fake", control_hz=100.0), + backend=backend, + control_hz=100.0, + ) + robot.connect() + target = q0.copy() + target[0] = 0.005 + result = robot.execute_joint_trajectory(strict_traj(q0, [target], [1])) + assert not result.success + assert result.stop_reason == "joint_limit" + assert "predispatch_joint_rejection" in result.log + assert backend.mode_log == [ControlMode.NRT_JOINT_IMPEDANCE] + assert backend.joint_log == [] + assert backend.gripper_log == [] + + +def test_first_gripper_event_rechecks_mode_switch_width_drift(): + q0 = np.zeros(7) + backend = ModeDriftBackend(start_q=q0, gripper_after_mode=0.075) + robot = Robot( + config=RobotConfig(backend="fake", control_hz=100.0), + backend=backend, + control_hz=100.0, + ) + robot.connect() + traj = strict_traj( + q0, + [q0], + [1], + [JointGripperTarget(width=0.079, force=15.0)], + ) + result = robot.execute_joint_trajectory(traj) + assert not result.success + assert result.stop_reason == "gripper_limit" + assert "predispatch_gripper_rejection" in result.log + assert backend.joint_log == [] + assert backend.gripper_log == [] + + +def test_gripper_move_dispatches_before_arm_and_streams_concurrently(): + q0 = np.zeros(7) + backend = OrderedFakeBackend(start_q=q0) + robot = Robot( + config=RobotConfig(backend="fake", control_hz=100.0), + backend=backend, + control_hz=100.0, + ) + robot.connect() + traj = strict_traj( + q0, + [np.full(7, 0.002), np.full(7, 0.003)], + [2, 1], + [ + JointGripperTarget(width=0.079, force=15.0), + JointGripperTarget(width=0.0785, force=15.0), + ], + ) + result = robot.execute_joint_trajectory(traj) + assert result.success + assert result.log["requested_segment_ticks"] == [2, 1] + assert result.log["scheduled_segment_ticks"] == [2, 1] + assert [event["velocity_m_s"] for event in result.log["gripper_events"]] == pytest.approx( + [0.05, 0.05] + ) + assert [kind for kind, *_ in backend.write_events] == [ + "gripper", + "joint", + "joint", + "gripper", + "joint", + ] + assert result.log["initial_joint_target"] == q0.tolist() + assert result.log["ending_joint_target"] == pytest.approx(np.full(7, 0.003).tolist()) + assert len(result.log["gripper_tracking"]) == 3 + + +def test_explicit_gripper_velocity_must_realize_same_segment_duration(): + q0 = np.zeros(7) + backend = OrderedFakeBackend(start_q=q0) + robot = Robot( + config=RobotConfig(backend="fake", control_hz=100.0), + backend=backend, + control_hz=100.0, + ) + robot.connect() + traj = strict_traj( + q0, + [q0], + [2], + [JointGripperTarget(width=0.079, velocity=0.1)], + ) + with pytest.raises(ValueError, match="does not realize width delta"): + robot.execute_joint_trajectory(traj) + assert backend.write_events == [] + + +def test_runtime_gripper_params_are_prevalidated_before_any_write(): + q0 = np.zeros(7) + backend = LimitedOrderedFakeBackend(start_q=q0) + robot = Robot( + config=RobotConfig(backend="fake", control_hz=100.0), + backend=backend, + control_hz=100.0, + ) + robot.connect() + # 1 mm / 20 ms = 0.05 m/s, below the runtime 0.06 m/s floor. + traj = strict_traj(q0, [q0], [2], [JointGripperTarget(width=0.079)]) + with pytest.raises(ValueError, match="Gripper.params"): + robot.execute_joint_trajectory(traj) + assert backend.write_events == [] + + +def test_first_call_gripper_ramp_uses_measured_width_not_declared_knot_zero(): + q0 = np.zeros(7) + backend = LimitedOrderedFakeBackend(start_q=q0) + robot = Robot( + config=RobotConfig(backend="fake", control_hz=100.0), + backend=backend, + control_hz=100.0, + ) + server = FlexivControlServer( + robot=robot, host="127.0.0.1", port=0, lease_ttl=1.0, host_lock=False + ) + server.start() + port = server._tcp.server_address[1] + server.serve_in_thread() + try: + with RemoteRobot("127.0.0.1", port, owner="prefix") as remote: + traj = strict_traj( + q0, + [q0], + [1], + [JointGripperTarget(width=0.0786, force=15.0)], + ) + traj.initial_gripper_width = 0.07 + result = remote.execute_joint_trajectory(traj) + event = result.log["gripper_events"][0] + assert result.success + assert result.log["gripper_execution_anchor_m"] == pytest.approx(0.08) + assert event["start_width_m"] == pytest.approx(0.08) + assert event["velocity_m_s"] == pytest.approx(0.14) + finally: + server.shutdown() + + +def test_server_ack_is_provenance_and_prevalidation_preserves_it(): + q0 = np.zeros(7) + backend = FakeBackend(start_q=q0, tracking_alpha=0.1) + robot = Robot( + config=RobotConfig(backend="fake", control_hz=100.0), + backend=backend, + control_hz=100.0, + ) + server = FlexivControlServer( + robot=robot, host="127.0.0.1", port=0, lease_ttl=1.0, host_lock=False + ) + server.start() + port = server._tcp.server_address[1] + server.serve_in_thread() + try: + with RemoteRobot("127.0.0.1", port, owner="prefix") as remote: + first_end = q0.copy() + first_end[0] = 0.02 + first = strict_traj(q0, [first_end], [10]) + r1 = remote.execute_joint_trajectory(first) + assert r1.log["continuity_source"] == "measured_rebase" + assert server._last_ack_joint_target.tolist() == first_end.tolist() + assert remote.get_state().q[0] < first_end[0] + + writes_before_old_rpc = len(backend.joint_log) + old_rpc = server._dispatch( + { + "id": 99, + "method": "execute_joint_trajectory", + "params": { + "owner": "prefix", + "traj": P.joint_trajectory_to_dict(first), + }, + } + ) + assert old_rpc["ok"] is False + assert "old joint payload refused" in old_rpc["error"] + assert len(backend.joint_log) == writes_before_old_rpc + assert server._last_ack_joint_target.tolist() == first_end.tolist() + + measured = remote.get_state().q + rebased_end = measured.copy() + rebased_end[0] += 0.002 + rebased = strict_traj(measured, [rebased_end], [1]) + r2 = remote.execute_joint_trajectory(rebased) + assert r2.log["continuity_source"] == "measured_rebase_with_prior_ack" + assert r2.log["previous_acknowledged_joint_target"] == first_end.tolist() + assert np.max(np.abs(r2.log["initial_joint_delta_from_previous_ack_rad"])) > 1e-3 + assert server._last_ack_joint_target.tolist() == rebased_end.tolist() + + current = remote.get_state().q + too_fast_end = current.copy() + too_fast_end[0] += 0.5 + too_fast = strict_traj(current, [too_fast_end], [1]) + writes_before_prevalidation = len(backend.joint_log) + with pytest.raises(RemoteRobotError, match="strict_timing joint rate"): + remote.execute_joint_trajectory(too_fast) + assert len(backend.joint_log) == writes_before_prevalidation + assert server._last_ack_joint_target.tolist() == rebased_end.tolist() + + corrected_q = remote.get_state().q + corrected = remote.execute_joint_trajectory( + strict_traj(corrected_q, [corrected_q], [1]) + ) + assert corrected.log["continuity_source"] == "measured_rebase_with_prior_ack" + + remote.command_gripper(GripperCommand(width=0.07)) + assert server._last_ack_joint_target is None + post_reset_q = remote.get_state().q + post_reset = remote.execute_joint_trajectory( + strict_traj(post_reset_q, [post_reset_q], [1]) + ) + assert post_reset.log["continuity_source"] == "measured_rebase" + finally: + server.shutdown() + + +def test_second_feedback_prefix_rebases_joint_and_gripper_to_current_measurement(): + q0 = np.zeros(7) + backend = LimitedOrderedFakeBackend(start_q=q0, tracking_alpha=0.1) + robot = Robot( + config=RobotConfig(backend="fake", control_hz=100.0), + backend=backend, + control_hz=100.0, + ) + server = FlexivControlServer( + robot=robot, host="127.0.0.1", port=0, lease_ttl=1.0, host_lock=False + ) + server.start() + port = server._tcp.server_address[1] + server.serve_in_thread() + try: + with RemoteRobot("127.0.0.1", port, owner="prefix") as remote: + first_end = q0.copy() + first_end[0] = 0.02 + first = strict_traj( + q0, + [first_end], + [10], + [JointGripperTarget(width=0.068, force=15.0)], + ) + assert remote.execute_joint_trajectory(first).success + assert backend._q[0] < first_end[0] + + # Model feedback between horizons: the next plan starts at current + # telemetry, not the previous commanded endpoint. + backend._gripper_width = 0.077 + measured = remote.get_state() + assert measured.q[0] < first_end[0] + assert measured.gripper_width != pytest.approx(0.068) + second_end = measured.q.copy() + second_end[0] += 0.003 + second = strict_traj( + measured.q, + [second_end], + [1], + [JointGripperTarget(width=0.0784, force=15.0)], + ) + second.initial_gripper_width = measured.gripper_width + result = remote.execute_joint_trajectory(second) + event = result.log["gripper_events"][0] + assert result.success + assert result.log["continuity_source"] == "measured_rebase_with_prior_ack" + assert result.log["joint_filter_anchor"] == pytest.approx(measured.q.tolist()) + assert result.log["gripper_execution_anchor_m"] == pytest.approx(0.077) + assert result.log["previous_acknowledged_gripper_target_m"] == pytest.approx(0.068) + assert event["start_width_m"] == pytest.approx(0.077) + assert event["velocity_m_s"] == pytest.approx(0.14) + finally: + server.shutdown() + + +def test_stop_and_failed_execution_clear_ack_cache(): + q0 = np.zeros(7) + backend = FakeBackend(start_q=q0) + robot = Robot( + config=RobotConfig(backend="fake", control_hz=100.0), + backend=backend, + control_hz=100.0, + ) + server = FlexivControlServer( + robot=robot, host="127.0.0.1", port=0, lease_ttl=1.0, host_lock=False + ) + server.start() + port = server._tcp.server_address[1] + server.serve_in_thread() + try: + with RemoteRobot("127.0.0.1", port, owner="prefix") as remote: + traj = strict_traj(q0, [q0], [1]) + assert remote.execute_joint_trajectory(traj).success + assert server._last_ack_joint_target is not None + + backend._fault = True + backend._mode = ControlMode.IDLE + backend.mode_log.clear() + joint_writes_before_fault = len(backend.joint_log) + failed = remote.execute_joint_trajectory(traj) + assert not failed.success + assert failed.stop_reason == "backend_fault" + assert failed.log["prewrite_safety_rejection"] is True + assert backend.mode_log == [] + assert len(backend.joint_log) == joint_writes_before_fault + assert server._last_ack_joint_target is None + backend._fault = False + + assert remote.execute_joint_trajectory(traj).success + assert server._last_ack_joint_target is not None + remote.stop() + assert server._last_ack_joint_target is None + finally: + server.shutdown() + + +def test_server_start_and_shutdown_reset_continuity_cache(): + q0 = np.zeros(7) + robot = Robot( + config=RobotConfig(backend="fake", control_hz=100.0), + backend=FakeBackend(start_q=q0), + control_hz=100.0, + ) + server = FlexivControlServer( + robot=robot, host="127.0.0.1", port=0, lease_ttl=1.0, host_lock=False + ) + server._last_ack_joint_target = np.ones(7) + server._last_ack_gripper_target = 0.05 + server._joint_target_owner = "stale" + server.start() + assert server._last_ack_joint_target is None + assert server._last_ack_gripper_target is None + server.serve_in_thread() + server._last_ack_joint_target = np.ones(7) + server._last_ack_gripper_target = 0.05 + server._joint_target_owner = "stale" + server.shutdown() + assert server._last_ack_joint_target is None + assert server._last_ack_gripper_target is None + assert server._joint_target_owner == "" diff --git a/tests/test_joint_torque_v5.py b/tests/test_joint_torque_v5.py new file mode 100644 index 0000000..c21c16a --- /dev/null +++ b/tests/test_joint_torque_v5.py @@ -0,0 +1,218 @@ +from __future__ import annotations + +import numpy as np +import pytest + +from flexiv_control import ( + JointGripperForceTarget, + JointTorqueTrajectory, + JointTorqueWaypoint, + Robot, + RobotConfig, +) +from flexiv_control.backends.fake import FakeBackend +from flexiv_control.client import RemoteRobot +from flexiv_control.robot import TrajectoryPrevalidationError +from flexiv_control.server import FlexivControlServer +from flexiv_control.server import protocol as P + + +class TorqueRuntimeFake(FakeBackend): + def runtime_info(self) -> dict: + return { + "joint_limits": { + "source": "fake.RobotInfo", + "position_min_rad": [-2.0] * 7, + "position_max_rad": [2.0] * 7, + "velocity_max_rad_s": [2.0] * 7, + "torque_max_nm": [100.0] * 7, + }, + "gripper_limits": { + "min_force_n": 0.0, + "max_force_n": 80.0, + }, + } + + +def trajectory(value: float = 10.0) -> JointTorqueTrajectory: + return JointTorqueTrajectory( + initial_torques=np.zeros(7), + waypoints=[ + JointTorqueWaypoint( + torques=np.full(7, value), + n_frames=2, + gripper=JointGripperForceTarget(force=40.0), + ) + ], + max_joint_torque_scale=0.3, + safety_profile="tabletop_safe", + ) + + +def test_protocol_v5_roundtrip_and_identity() -> None: + original = trajectory() + restored = P.joint_torque_trajectory_from_dict( + P.joint_torque_trajectory_to_dict(original) + ) + np.testing.assert_array_equal(restored.initial_torques, np.zeros(7)) + np.testing.assert_array_equal(restored.waypoints[0].torques, 10.0) + assert P.PROTOCOL_ID == "flexiv-control.trajectory-rpc.v5" + assert P.PROTOCOL_CONTRACT["trajectory_rpcs"][ + "execute_joint_torque_trajectory" + ] == "traj" + + +def test_torque_execution_interpolates_and_uses_runtime_limits() -> None: + backend = TorqueRuntimeFake() + robot = Robot( + RobotConfig( + backend="fake", + control_hz=1000.0, + allow_joint_torque=True, + ), + backend=backend, + ) + robot.connect() + result = robot.execute_joint_torque_trajectory(trajectory()) + assert result.success + np.testing.assert_allclose(backend.joint_torque_log, [[5.0] * 7, [10.0] * 7]) + assert result.log["gravity_compensation"] is True + assert result.log["soft_limits"] is True + assert result.log["effective_torque_max_nm"] == [30.0] * 7 + assert result.log["acknowledged_ending_gripper_force_n"] == 40.0 + assert result.final_state is not None + assert result.final_state.gripper_force == 40.0 + + +def test_torque_ack_reports_last_dispatched_force_not_last_waypoint_shape() -> None: + backend = TorqueRuntimeFake() + robot = Robot( + RobotConfig( + backend="fake", control_hz=1000.0, allow_joint_torque=True + ), + backend=backend, + ) + robot.connect() + traj = JointTorqueTrajectory( + initial_torques=np.zeros(7), + waypoints=[ + JointTorqueWaypoint( + torques=np.zeros(7), + n_frames=1, + gripper=JointGripperForceTarget(force=20.0), + ), + JointTorqueWaypoint(torques=np.zeros(7), n_frames=1), + ], + max_joint_torque_scale=0.3, + safety_profile="tabletop_safe", + ) + result = robot.execute_joint_torque_trajectory(traj) + assert result.success + assert result.log["ending_gripper_force_n"] == 20.0 + assert result.log["acknowledged_ending_gripper_force_n"] == 20.0 + assert result.log["gripper_events"] == [ + {"mode": "force", "segment": 0, "force_n": 20.0} + ] + + +def test_remote_torque_result_preserves_physical_ack_and_measured_end_state() -> None: + local = Robot( + RobotConfig( + backend="fake", control_hz=1000.0, allow_joint_torque=True + ), + backend=TorqueRuntimeFake(), + ) + server = FlexivControlServer( + robot=local, + host="127.0.0.1", + port=0, + host_lock=False, + ) + server.start() + assert server._tcp is not None + port = server._tcp.server_address[1] + server.serve_in_thread() + try: + with RemoteRobot("127.0.0.1", port, owner="torque-test") as remote: + result = remote.execute_joint_torque_trajectory(trajectory()) + assert result.success + assert result.log["acknowledged_ending_joint_torque_nm"] == [10.0] * 7 + assert result.log["acknowledged_ending_gripper_force_n"] == 40.0 + assert result.final_state is not None + assert result.final_state.gripper_width == pytest.approx(0.08) + assert result.final_state.gripper_force == pytest.approx(40.0) + finally: + server.shutdown() + + +def test_torque_execution_requires_and_preserves_command_continuity() -> None: + backend = TorqueRuntimeFake() + robot = Robot( + RobotConfig( + backend="fake", + control_hz=1000.0, + allow_joint_torque=True, + ), + backend=backend, + ) + robot.connect() + first = robot.execute_joint_torque_trajectory(trajectory()) + assert first.success + second = JointTorqueTrajectory( + initial_torques=np.full(7, 10.0), + waypoints=[JointTorqueWaypoint(torques=np.zeros(7), n_frames=2)], + max_joint_torque_scale=0.3, + safety_profile="tabletop_safe", + ) + assert robot.execute_joint_torque_trajectory(second).success + np.testing.assert_allclose( + backend.joint_torque_log[-2:], + [[5.0] * 7, [0.0] * 7], + ) + + with pytest.raises(TrajectoryPrevalidationError, match="acknowledged"): + robot.execute_joint_torque_trajectory( + JointTorqueTrajectory( + initial_torques=np.ones(7), + waypoints=[JointTorqueWaypoint(torques=np.ones(7), n_frames=1)], + max_joint_torque_scale=0.3, + safety_profile="tabletop_safe", + ) + ) + robot.stop() + robot.clear_stop() + assert robot.execute_joint_torque_trajectory(trajectory()).success + + +def test_torque_execution_fails_closed_without_opt_in_or_tau_max() -> None: + disabled = Robot( + RobotConfig(backend="fake", control_hz=1000.0), + backend=TorqueRuntimeFake(), + ) + disabled.connect() + with pytest.raises(TrajectoryPrevalidationError, match="disabled"): + disabled.execute_joint_torque_trajectory(trajectory()) + + missing = Robot( + RobotConfig( + backend="fake", control_hz=1000.0, allow_joint_torque=True + ), + backend=FakeBackend(), + ) + missing.connect() + with pytest.raises(TrajectoryPrevalidationError, match="tau_max"): + missing.execute_joint_torque_trajectory(trajectory()) + + +def test_torque_execution_rejects_scaled_limit_before_write() -> None: + backend = TorqueRuntimeFake() + robot = Robot( + RobotConfig( + backend="fake", control_hz=1000.0, allow_joint_torque=True + ), + backend=backend, + ) + robot.connect() + with pytest.raises(TrajectoryPrevalidationError, match="tau_max"): + robot.execute_joint_torque_trajectory(trajectory(31.0)) + assert backend.joint_torque_log == [] diff --git a/tests/test_runtime_joint_contract.py b/tests/test_runtime_joint_contract.py new file mode 100644 index 0000000..3f32f58 --- /dev/null +++ b/tests/test_runtime_joint_contract.py @@ -0,0 +1,73 @@ +import re + +import numpy as np + +from flexiv_control import Robot, RobotConfig +from flexiv_control.backends.fake import FakeBackend + + +class _RuntimeLimitsBackend(FakeBackend): + def runtime_info(self) -> dict: + return { + "joint_limits": { + "source": "test.RobotInfo", + "position_min_rad": [-1.0] * 7, + "position_max_rad": [1.0] * 7, + "velocity_max_rad_s": [ + 1.0, + 1.1, + 1.2, + 1.3, + 1.4, + 1.5, + 1.6, + ], + }, + "current_safety_limits": { + "source": "test.Safety.current_limits", + "position_min_rad": [-0.9] * 7, + "position_max_rad": [0.8] * 7, + "velocity_max_normal_rad_s": [0.8] * 7, + "velocity_max_reduced_rad_s": [0.4] * 7, + }, + } + + +def test_runtime_joint_contract_takes_intersection_and_has_digest(): + robot = Robot( + config=RobotConfig(backend="fake"), + backend=_RuntimeLimitsBackend(), + ) + contract = robot.server_runtime_info()["effective_joint_limits"] + + assert np.allclose(robot.profile.joint_lower, -0.9) + assert np.allclose(robot.profile.joint_upper, 0.8) + assert np.allclose(robot._joint_velocity_limits, 0.4) + assert np.allclose(contract["hard_position_min_rad"], -0.9) + assert np.allclose(contract["hard_position_max_rad"], 0.8) + assert np.allclose( + contract["enforced_position_min_rad"], + -0.9 + robot.profile.joint_margin_rad, + ) + assert np.allclose( + contract["enforced_position_max_rad"], + 0.8 - robot.profile.joint_margin_rad, + ) + assert re.fullmatch(r"[0-9a-f]{64}", contract["sha256"]) + assert contract["sources"] == [ + "configured_safety_profile", + "test.RobotInfo", + "test.Safety.current_limits", + ] + + +def test_profile_change_cannot_expand_beyond_runtime_limits(): + robot = Robot( + config=RobotConfig(backend="fake"), + backend=_RuntimeLimitsBackend(), + ) + robot.set_safety_profile("free_space_fast") + + assert np.all(robot.profile.joint_lower >= -0.9) + assert np.all(robot.profile.joint_upper <= 0.8) + assert np.allclose(robot._joint_velocity_limits, 0.4) diff --git a/tests/test_safety.py b/tests/test_safety.py index 2d82406..3da616e 100644 --- a/tests/test_safety.py +++ b/tests/test_safety.py @@ -48,6 +48,33 @@ def test_joint_clip_to_limits(): assert np.all(res.q <= p.joint_upper - p.joint_margin_rad + 1e-9) +def test_joint_speed_filter_uses_per_joint_runtime_limits(): + p = SafetyProfile(max_joint_speed_scale=0.5) + limits = np.array([2.0, 0.2, 2, 2, 2, 2, 2], float) + f = SafetyFilter( + p, + control_dt=0.01, + joint_velocity_max=limits, + ) + s = RobotState() + s.q = np.zeros(7) + res = f.filter_joint(np.full(7, 0.1), s) + expected = limits * 0.5 * 0.01 + assert np.allclose(res.q, expected) + + +def test_joint_speed_filter_rejects_invalid_runtime_limits(): + with np.testing.assert_raises_regex( + ValueError, + "joint_velocity_max", + ): + SafetyFilter( + SafetyProfile(), + control_dt=0.01, + joint_velocity_max=np.ones(6), + ) + + def test_command_age_watchdog(): p = SafetyProfile(command_timeout_ms=100) f = SafetyFilter(p, control_dt=0.01) diff --git a/tests/test_server_client.py b/tests/test_server_client.py index ae39c4d..540cfb9 100644 --- a/tests/test_server_client.py +++ b/tests/test_server_client.py @@ -1,18 +1,31 @@ +from importlib.metadata import version as distribution_version +import re import time import numpy as np import pytest -from flexiv_control import CartesianTrajectory, RobotConfig +from flexiv_control import ( + CartesianTrajectory, + JointTrajectory, + JointWaypoint, + RobotConfig, + __version__, +) from flexiv_control.client import RemoteRobot, RemoteRobotError from flexiv_control.server import FlexivControlServer +from flexiv_control.server import protocol as P @pytest.fixture() def server(): # port 0 lets the OS pick a free port - srv = FlexivControlServer(config=RobotConfig(backend="fake", control_hz=200.0), - host="127.0.0.1", port=0, lease_ttl=1.0) + srv = FlexivControlServer( + config=RobotConfig(backend="fake", control_hz=200.0), + host="127.0.0.1", + port=0, + lease_ttl=1.0, + ) srv.start() port = srv._tcp.server_address[1] srv.serve_in_thread() @@ -27,13 +40,108 @@ def test_remote_state_and_chunk(server): assert s.q.shape == (7,) robot.start_cartesian_impedance() res = robot.execute_cartesian_trajectory( - CartesianTrajectory.from_waypoint_array([[0.45, 0.0, 0.30, 1.0, 20], - [0.48, 0.0, 0.28, 0.0, 20]]) + CartesianTrajectory.from_waypoint_array( + [[0.45, 0.0, 0.30, 1.0, 20], [0.48, 0.0, 0.28, 0.0, 20]] + ) ) assert res.success assert np.allclose(res.final_state.tcp_position, [0.48, 0.0, 0.28], atol=1e-3) +def test_server_info_is_read_only_and_pins_trajectory_protocol(server, monkeypatch): + srv, port = server + + def backend_access_forbidden(*args, **kwargs): + raise AssertionError("get_server_info must not access the robot") + + monkeypatch.setattr(srv.robot, "get_state", backend_access_forbidden) + robot = RemoteRobot("127.0.0.1", port, owner="identity-probe").connect() + try: + assert srv.lease.owner == "" + assert robot._has_lease is False + first = robot.get_server_info() + second = robot.get_server_info() + assert first == second == P.server_info(**srv.robot.server_runtime_info()) + assert srv.lease.owner == "" + assert robot._has_lease is False + finally: + robot.close() + + assert first["schema"] == "flexiv-control.server-info.v5" + assert first["package"] == "flexiv-control" + assert first["package_version"] == __version__ + assert first["protocol_id"] == "flexiv-control.trajectory-rpc.v5" + assert first["protocol_fingerprint_sha256"] == P.PROTOCOL_FINGERPRINT_SHA256 + assert first["source_fingerprint_sha256"] == P.SOURCE_FINGERPRINT_SHA256 + assert first["control_hz"] == pytest.approx(200.0) + assert first["active_safety_profile"] == srv.robot.profile.name + assert re.fullmatch(r"[0-9a-f]{64}", first["protocol_fingerprint_sha256"]) + assert re.fullmatch(r"[0-9a-f]{64}", first["source_fingerprint_sha256"]) + assert P.PROTOCOL_CONTRACT["trajectory_rpcs"] == { + "execute_cartesian_trajectory": "traj", + "execute_joint_trajectory": "traj", + "execute_joint_torque_trajectory": "traj", + } + assert P.PROTOCOL_CONTRACT["identity_rpc"]["runtime_fields"] == { + "required": ["control_hz", "active_safety_profile"], + "hardware_when_available": [ + "runtime_hardware_identity", + "gripper_limits", + "joint_limits", + "current_safety_limits", + "effective_joint_limits", + ], + } + assert P.PROTOCOL_CONTRACT["joint_trajectory_contract"] == { + "schema": P.JOINT_TRAJECTORY_SCHEMA, + "trajectory_fields": [ + "schema", + "waypoints", + "initial_positions", + "initial_gripper_width", + "max_joint_speed_scale", + "interpolation", + "strict_timing", + "safety_profile", + ], + "waypoint_fields": ["positions", "n_frames", "duration", "gripper"], + "gripper_target_variants": { + "move": ["mode", "width", "force_limit", "velocity"], + "force": ["mode", "force"], + }, + "rpc_identity_fields": ["protocol_id", "protocol_fingerprint_sha256"], + "explicit_initial_target": ["initial_positions", "initial_gripper_width"], + "strict_timing": "authoritative-n_frames-reject-no-clip-or-time-stretch", + "continuity": "measured-rebase-every-rpc-prior-ack-provenance-only", + "first_emitted_joint_bound": "effective-runtime-rate-times-control-period", + "gripper_execution_anchor": "current-measured-width-every-rpc", + "predispatch_revalidation": [ + "first-joint-setpoint-after-mode-transition", + "first-gripper-event-from-current-measured-width", + ], + "numeric_json_types": "numbers-and-arrays-only-no-strings-or-booleans", + "gripper": "explicit-Move-or-signed-Grasp-concurrent-at-segment-boundary", + "interpolation": ["cosine", "linear"], + "max_joint_speed_scale": "finite-(0,1]-active-profile-ceiling", + } + + +def test_distribution_metadata_matches_runtime_version(): + assert distribution_version("flexiv-control") == __version__ + + +def test_server_info_rejects_missing_or_invalid_runtime_fields(): + with pytest.raises(ValueError, match="missing required runtime"): + P.server_info() + with pytest.raises(ValueError, match="control_hz"): + P.server_info( + control_hz=0.0, + active_safety_profile="tabletop_safe", + ) + with pytest.raises(ValueError, match="active_safety_profile"): + P.server_info(control_hz=100.0, active_safety_profile="") + + def test_remote_servo_delta(server): srv, port = server with RemoteRobot("127.0.0.1", port, owner="tester") as robot: @@ -44,6 +152,53 @@ def test_remote_servo_delta(server): assert after[1] > before[1] +def test_joint_trajectory_contract_is_enforced_before_streaming(server): + srv, port = server + with RemoteRobot("127.0.0.1", port, owner="joint-contract") as robot: + q0 = robot.get_state().q.copy() + with pytest.raises(RemoteRobotError, match="exceeds active"): + robot.execute_joint_trajectory( + JointTrajectory( + waypoints=[JointWaypoint(positions=q0, duration=0.02)], + max_joint_speed_scale=0.31, + interpolation="linear", + ) + ) + assert np.array_equal(robot.get_state().q, q0) + + result = robot.execute_joint_trajectory( + JointTrajectory( + waypoints=[ + JointWaypoint(positions=q0, duration=0.016), + JointWaypoint(positions=q0, duration=0.016), + ], + max_joint_speed_scale=0.29, + interpolation="linear", + ) + ) + assert result.success + assert result.log["joint_interpolation"] == "linear" + assert result.log["effective_max_joint_speed_scale"] == pytest.approx(0.29) + # 2 x 3.2 requested ticks at the 200 Hz fixture -> 3 + 3 = 6. + assert result.log["scheduled_segment_ticks"] == [3, 3] + assert result.log["scheduled_total_ticks"] == 6 + + +def test_joint_interpolation_round_trips_and_old_payload_is_refused(): + traj = JointTrajectory( + waypoints=[JointWaypoint(np.zeros(7), duration=0.1)], + interpolation="linear", + ) + payload = P.joint_trajectory_to_dict(traj) + assert payload["schema"] == P.JOINT_TRAJECTORY_SCHEMA + assert payload["interpolation"] == "linear" + assert P.joint_trajectory_from_dict(payload).interpolation == "linear" + + del payload["interpolation"] + with pytest.raises(ValueError, match="keys do not match"): + P.joint_trajectory_from_dict(payload) + + def test_lease_blocks_second_client(server): srv, port = server a = RemoteRobot("127.0.0.1", port, owner="alice").connect() @@ -84,7 +239,7 @@ def test_fresh_lease_owner_does_not_inherit_stale_stop(server): srv, port = server a = RemoteRobot("127.0.0.1", port, owner="alice").connect() a.acquire_lease() - a.stop() # latched; no motion in flight consumes it + a.stop() # latched; no motion in flight consumes it a.release_lease() a.close() @@ -92,7 +247,8 @@ def test_fresh_lease_owner_does_not_inherit_stale_stop(server): b.acquire_lease() b.start_cartesian_impedance() res = b.execute_cartesian_trajectory( - CartesianTrajectory.from_waypoint_array([[0.45, 0.0, 0.30, 1.0, 20]])) + CartesianTrajectory.from_waypoint_array([[0.45, 0.0, 0.30, 1.0, 20]]) + ) assert res.success, f"first traj of a fresh session aborted: {res.stop_reason}" assert str(res.stop_reason) != "user" b.close() diff --git a/tests/test_spacemouse_gripper_toggle.py b/tests/test_spacemouse_gripper_toggle.py new file mode 100644 index 0000000..e22e758 --- /dev/null +++ b/tests/test_spacemouse_gripper_toggle.py @@ -0,0 +1,105 @@ +"""Gripper toggle must fire on the button's rising edge, not its level. + +Regression test for the bug where holding the gripper button flipped the +gripper open/closed once per control tick (~100 Hz) for as long as it was held. +""" + +import numpy as np + +from flexiv_control.teleop import ScriptedSpaceMouseSource, SpaceMouseState, SpaceMouseTeleop + + +def _teleop(**kw): + return SpaceMouseTeleop(robot=object(), source=ScriptedSpaceMouseSource(), **kw) + + +def _press(t, buttons): + return SpaceMouseState(buttons=buttons) + + +def test_held_button_toggles_exactly_once(): + t = _teleop() + held = SpaceMouseState(buttons=[0, 1]) + commands = [t._gripper_from_buttons(held) for _ in range(50)] + fired = [c for c in commands if c is not None] + assert len(fired) == 1, "a held button must toggle exactly once, not per tick" + + +def test_release_then_press_toggles_again(): + t = _teleop() + held = SpaceMouseState(buttons=[0, 1]) + released = SpaceMouseState(buttons=[0, 0]) + + first = t._gripper_from_buttons(held) + assert first is not None + assert t._gripper_from_buttons(held) is None # still held: no re-fire + assert t._gripper_from_buttons(released) is None + second = t._gripper_from_buttons(held) + assert second is not None + # Open and close alternate, using the GN01 widths. + assert {round(first.width, 3), round(second.width, 3)} == {0.09, 0.01} + + +def test_first_press_opens_by_default(): + t = _teleop() + cmd = t._gripper_from_buttons(SpaceMouseState(buttons=[0, 1])) + assert cmd is not None + assert cmd.width == t.gripper_open_width + assert cmd.grasp is False + + +def test_signs_flip_axes(): + t = _teleop(signs=[-1, 1, 1, 1, 1, 1], deadband=0.0) + st = SpaceMouseState(translation=np.array([0.5, 0.5, 0.0]), rotation=np.zeros(3)) + delta = t.to_delta(st) + assert delta[0] == -delta[1] + + +class _FakeRobot: + """Records servo calls; reports a wide-open gripper.""" + + def __init__(self, width=0.09): + self._width = width + self.calls = [] + + def get_state(self): + from types import SimpleNamespace + return SimpleNamespace(gripper_width=self._width) + + def acquire_lease(self, *a, **k): + pass + + def start_cartesian_impedance(self, *a, **k): + pass + + def servo_cartesian_delta(self, delta, duration=None, frame=None, gripper=None): + self.calls.append((np.asarray(delta, float).copy(), gripper)) + + +def test_initial_state_inferred_from_robot_width(): + robot = _FakeRobot(width=0.09) # physically open + t = SpaceMouseTeleop(robot=robot, source=ScriptedSpaceMouseSource()) + cmd = t._gripper_from_buttons(SpaceMouseState(buttons=[0, 1])) + assert cmd is not None + assert cmd.width == t.gripper_close_width # open -> first press closes + assert cmd.grasp is True + + +def test_gripper_actuates_with_deadman_released(): + robot = _FakeRobot() + presses = iter([ + SpaceMouseState(buttons=[0, 0]), + SpaceMouseState(buttons=[0, 1]), # gripper press, deadman released + SpaceMouseState(buttons=[0, 1]), + SpaceMouseState(buttons=[0, 0]), + ]) + t = SpaceMouseTeleop( + robot=robot, + source=ScriptedSpaceMouseSource(lambda _t: next(presses)), + publish_hz=1000.0, + ) + t.run(max_ticks=4) + grips = [g for _d, g in robot.calls if g is not None] + assert len(grips) == 1, "one rising edge -> exactly one gripper command" + deltas = [d for d, g in robot.calls if g is not None] + assert np.allclose(deltas[0], 0.0), "no motion while the deadman is released" diff --git a/tests/test_trajectory.py b/tests/test_trajectory.py index 6110901..32e09e6 100644 --- a/tests/test_trajectory.py +++ b/tests/test_trajectory.py @@ -1,7 +1,14 @@ import numpy as np import pytest -from flexiv_control import CartesianTrajectory, CartesianDelta, CartesianWaypoint, ExecutionResult +from flexiv_control import ( + CartesianDelta, + CartesianTrajectory, + CartesianWaypoint, + ExecutionResult, + JointTrajectory, + JointWaypoint, +) def test_waypoint_requires_duration_or_frames(): @@ -57,3 +64,34 @@ def test_cartesian_delta_shape(): def test_execution_result_defaults(): r = ExecutionResult() assert r.success and not r.clipped and r.stop_reason == "none" + + +@pytest.mark.parametrize("scale", [0.0, -0.1, 1.1, np.nan, np.inf]) +def test_joint_trajectory_rejects_invalid_speed_scale(scale): + with pytest.raises(ValueError, match="max_joint_speed_scale"): + JointTrajectory( + waypoints=[JointWaypoint(np.zeros(7), duration=0.1)], + max_joint_speed_scale=scale, + ) + + +def test_joint_trajectory_rejects_unknown_interpolation(): + with pytest.raises(ValueError, match="interpolation"): + JointTrajectory( + waypoints=[JointWaypoint(np.zeros(7), duration=0.1)], + interpolation="cubic", + ) + + +def test_joint_trajectory_preserves_legacy_positional_field_order(): + traj = JointTrajectory( + [JointWaypoint(np.zeros(7), duration=0.1)], + 0.2, + "linear", + "tabletop_safe", + ) + assert traj.max_joint_speed_scale == pytest.approx(0.2) + assert traj.interpolation == "linear" + assert traj.safety_profile == "tabletop_safe" + assert traj.initial_positions is None + assert traj.strict_timing is False