griip-core-dev 0.5.6.dev25546__py3-none-any.whl

griip_core/hardware/vrmcs_client.pyi

@@ -0,0 +1,147 @@
+"""Typing stub for the vendored VRMCS transport.
+
+``vrmcs_client.py`` is vendored verbatim from VRMCS-py-client and a handful of
+its methods carry no return annotation. While it arrived inside the
+``vention-firmware-grpc-client`` wheel -- which ships no ``py.typed`` -- mypy
+treated the whole client as ``Any`` and never looked inside. griip-core *does*
+ship ``py.typed``, so without a stub mypy starts analysing upstream code we do
+not maintain and reports every call into the unannotated methods as untyped.
+
+Generated with ``stubgen`` from the vendored module and kept beside it, so
+callers get the real request/response types rather than ``Any``. Regenerate it
+whenever the vendored copy is refreshed:
+
+    uv run stubgen -o <tmp> src/griip_core/hardware/vrmcs_client.py
+
+Annotating the client itself belongs upstream in VRMCS-py-client -- edits to
+the vendored copy are lost on the next refresh, which is why the types live
+here instead.
+"""
+
+import threading
+from _typeshed import Incomplete
+from typing import Callable
+from vention.robots.v1.move_interpolation_request_pb2 import (
+    MoveInterpolationRequest as MoveInterpolationRequest,
+    MoveInterpolationResponse as MoveInterpolationResponse,
+)
+from vention.robots.v1.move_request_pb2 import (
+    MoveRequest as MoveRequest,
+    MoveResponse as MoveResponse,
+    WaitForMoveCompletionRequest as WaitForMoveCompletionRequest,
+    WaitForMoveCompletionResponse as WaitForMoveCompletionResponse,
+)
+from vention.robots.v1.move_until_pb2 import MoveUntilRequest as MoveUntilRequest, MoveUntilResponse as MoveUntilResponse
+from vention.robots.v1.plan_request_pb2 import PlanRequest as PlanRequest, PlanResponse as PlanResponse
+from vention.robots.v1.robot_alarm_pb2 import RobotAlarm as RobotAlarm, RobotAlarmStreamRequest as RobotAlarmStreamRequest
+from vention.robots.v1.robot_configuration_pb2 import (
+    GetTcpRequest as GetTcpRequest,
+    GetTcpResponse as GetTcpResponse,
+    SetPayloadRequest as SetPayloadRequest,
+    SetPayloadResponse as SetPayloadResponse,
+    SetTcpRequest as SetTcpRequest,
+    SetTcpResponse as SetTcpResponse,
+)
+from vention.robots.v1.robot_feedback_pb2 import (
+    Feedback as Feedback,
+    FeedbackStreamRequest as FeedbackStreamRequest,
+    GetFeedbackRequest as GetFeedbackRequest,
+    GetFeedbackResponse as GetFeedbackResponse,
+)
+from vention.robots.v1.robot_kinematics_pb2 import (
+    ForwardKinematicsRequest as ForwardKinematicsRequest,
+    ForwardKinematicsResponse as ForwardKinematicsResponse,
+    InverseKinematicsBatchRequest as InverseKinematicsBatchRequest,
+    InverseKinematicsBatchResponse as InverseKinematicsBatchResponse,
+    InverseKinematicsRequest as InverseKinematicsRequest,
+    InverseKinematicsResponse as InverseKinematicsResponse,
+)
+from vention.robots.v1.robot_management_pb2 import (
+    ListRobotsResponse as ListRobotsResponse,
+    RegisterRobotRequest as RegisterRobotRequest,
+    RegisterRobotResponse,
+    RobotInfo as RobotInfo,
+    UnregisterRobotResponse,
+)
+from vention.robots.v1.robot_requests_pb2 import (
+    GetRobotInfoRequest as GetRobotInfoRequest,
+    JointStatesStreamRequest as JointStatesStreamRequest,
+    PositionStreamRequest as PositionStreamRequest,
+    StopRequest as StopRequest,
+    UnregisterRobotRequest as UnregisterRobotRequest,
+)
+from vention.robots.v1.robot_state_pb2 import (
+    CommandState as CommandState,
+    CommandStateStreamRequest as CommandStateStreamRequest,
+    GetCommandStateRequest as GetCommandStateRequest,
+    GetCommandStateResponse as GetCommandStateResponse,
+    GetJointStatesRequest as GetJointStatesRequest,
+    GetJointStatesResponse as GetJointStatesResponse,
+    GetPositionStateRequest as GetPositionStateRequest,
+    GetPositionStateResponse as GetPositionStateResponse,
+    GetRobotStatesRequest as GetRobotStatesRequest,
+    GetRobotStatesResponse as GetRobotStatesResponse,
+    JointStates as JointStates,
+    PositionState as PositionState,
+    RobotStates as RobotStates,
+    RobotStatesStreamRequest as RobotStatesStreamRequest,
+    SetToFreedriveRequest as SetToFreedriveRequest,
+    SetToFreedriveResponse as SetToFreedriveResponse,
+    SetToNormalRequest as SetToNormalRequest,
+    SetToNormalResponse as SetToNormalResponse,
+)
+from vention.robots.v1.stop_request_pb2 import StopResponse as StopResponse
+
+logger: Incomplete
+
+class VRMCSClient:
+    channel: Incomplete
+    stub: Incomplete
+    joint_stream_thread: threading.Thread | None
+    position_stream_thread: threading.Thread | None
+    states_stream_thread: threading.Thread | None
+    command_state_stream_thread: threading.Thread | None
+    feedback_stream_thread: threading.Thread | None
+    robot_alarm_stream_thread: threading.Thread | None
+    def __init__(self, channel_url: str) -> None: ...
+    def register_robot(self, register_robot_req: RegisterRobotRequest) -> RegisterRobotResponse: ...
+    def stop(self, stop_req: StopRequest) -> StopResponse: ...
+    def set_to_normal(self, set_to_normal_req: SetToNormalRequest) -> SetToNormalResponse: ...
+    def set_to_freedrive(self, set_to_freedrive_req: SetToFreedriveRequest) -> SetToFreedriveResponse: ...
+    def move(self, move_req: MoveRequest) -> MoveResponse: ...
+    def plan(self, plan_req: PlanRequest) -> PlanResponse: ...
+    def wait_for_move_completion(self, request: WaitForMoveCompletionRequest) -> WaitForMoveCompletionResponse: ...
+    def move_until(self, move_until_req: MoveUntilRequest) -> MoveUntilResponse: ...
+    def joint_stream_subscribe(self, stream_req: JointStatesStreamRequest, on_data: Callable[[JointStates], None]): ...
+    joint_stream_iterator: Incomplete
+    def joint_stream_unsubscribe(self) -> None: ...
+    def position_stream_subscribe(self, stream_req: PositionStreamRequest, on_data: Callable[[PositionState], None]): ...
+    position_stream_iterator: Incomplete
+    def position_stream_unsubscribe(self) -> None: ...
+    def states_stream_subscribe(self, stream_req: RobotStatesStreamRequest, on_data: Callable[[RobotStates], None]): ...
+    states_stream_iterator: Incomplete
+    def states_stream_unsubscribe(self) -> None: ...
+    def move_interpolation(self, move_interp_req: MoveInterpolationRequest) -> MoveInterpolationResponse: ...
+    def list_robots(self) -> ListRobotsResponse: ...
+    def set_payload(self, set_payload_req: SetPayloadRequest) -> SetPayloadResponse: ...
+    def unregister_robot(self, unregister_robot_req: UnregisterRobotRequest) -> UnregisterRobotResponse: ...
+    def get_robot_info(self, get_robot_info_req: GetRobotInfoRequest) -> RobotInfo: ...
+    def command_state_stream_subscribe(self, stream_req: CommandStateStreamRequest, on_data: Callable[[CommandState], None]): ...
+    command_state_stream_iterator: Incomplete
+    def command_state_stream_unsubscribe(self) -> None: ...
+    def inverse_kinematics_batch(self, ik_req: InverseKinematicsBatchRequest) -> InverseKinematicsBatchResponse: ...
+    def inverse_kinematics(self, ik_req: InverseKinematicsRequest) -> InverseKinematicsResponse: ...
+    def forward_kinematics(self, fk_req: ForwardKinematicsRequest) -> ForwardKinematicsResponse: ...
+    def set_tcp(self, set_tcp_req: SetTcpRequest) -> SetTcpResponse: ...
+    def get_tcp(self, get_tcp_req: GetTcpRequest) -> GetTcpResponse: ...
+    def get_joint_states(self, get_joint_states_req: GetJointStatesRequest) -> GetJointStatesResponse: ...
+    def get_position_state(self, get_position_state_req: GetPositionStateRequest) -> GetPositionStateResponse: ...
+    def get_robot_states(self, get_robot_states_req: GetRobotStatesRequest) -> GetRobotStatesResponse: ...
+    def get_command_state(self, get_command_state_req: GetCommandStateRequest) -> GetCommandStateResponse: ...
+    def get_feedback(self, get_feedback_req: GetFeedbackRequest) -> GetFeedbackResponse: ...
+    def feedback_stream_subscribe(self, stream_req: FeedbackStreamRequest, on_data: Callable[[Feedback], None]): ...
+    feedback_stream_iterator: Incomplete
+    def feedback_stream_unsubscribe(self) -> None: ...
+    def robot_alarm_stream_subscribe(self, stream_req: RobotAlarmStreamRequest, on_data: Callable[[RobotAlarm], None]): ...
+    robot_alarm_stream_iterator: Incomplete
+    def robot_alarm_stream_unsubscribe(self) -> None: ...

griip_core/hardware/vrmcs_registration.py

@@ -0,0 +1,129 @@
+"""Shared helper for adopting existing VRMCS robot registrations.
+
+VRMCS self-registers robots at id='0' on startup and holds the UR's RTDE
+registers. Re-registering the same id with different driver params is
+rejected by the server, and tearing down to re-register causes RTDE
+contention. Adopting the existing registration sidesteps that fight.
+"""
+
+import logging
+from typing import Any, Callable, Optional
+
+from google.protobuf import empty_pb2
+
+logger = logging.getLogger(__name__)
+
+
+def find_compatible_registration(
+    vrmcs_client: Any,
+    robot_id: str,
+    robot_ip: str,
+    quiet: bool = False,
+    require_ip_match: bool = True,
+    allow_legacy_zero_id_match: bool = False,
+) -> Optional[Any]:
+    """Return a registered VRMCS RobotInfo we should adopt, or None.
+
+    Resolution order:
+      1. Same id as ``robot_id``. When ``require_ip_match=True`` (default),
+         the registration's IP must also match ``robot_ip`` (or be empty,
+         treated as wildcard for legacy registrations that don't populate
+         driver_params.ip).
+      2. Different id, IP matches, registered id is non-"0". This is the
+         execution-engine UUID case — VRMCS' EE registers under a UUID
+         while older clients still pass id="0".
+      3. Different id, IP matches, registered id is "0". Legacy single-
+         robot fallback. Opt-in via ``allow_legacy_zero_id_match`` because
+         a stray id="0" with the same IP is ambiguous in multi-robot cells.
+
+    A registration that matches neither the id nor the IP is never
+    adopted — silently attaching to a different robot on the network is
+    unsafe. Callers should fall back to fresh registration when this
+    returns None.
+
+    If ListRobots fails (e.g. pydantic-conversion issues in certain
+    deployed VRMCS client builds), logs a warning and returns None so
+    the caller can fall back to fresh registration.
+
+    Set ``quiet=True`` to suppress the library's warnings; useful when
+    the caller polls and handles its own dedup.
+
+    Set ``require_ip_match=False`` to opt out of the step-1 IP guard
+    when the caller knows the id is authoritative regardless of IP
+    (e.g. tooling that targets a specific registration by id).
+
+    Set ``allow_legacy_zero_id_match=True`` to enable step 3. Pass this
+    only when the caller knows the network is single-robot.
+    """
+    try:
+        registered = list(vrmcs_client.stub.ListRobots(empty_pb2.Empty()).robots)
+    except Exception as list_error:
+        if not quiet:
+            logger.warning("ListRobots failed, cannot adopt existing registration: %s", list_error)
+        return None
+
+    def ip_of(info: Any) -> str:
+        return str(info.driver_params.params.get("ip", ""))
+
+    # Step 1: same id as caller. IP must match (or be wildcard) unless caller opts out.
+    for info in registered:
+        if info.id != robot_id:
+            continue
+        if not require_ip_match or ip_of(info) in ("", robot_ip):
+            return info
+        # Same id, wrong IP — defer to ip-based resolution.
+        break
+
+    # Step 2: different id, IP matches, non-"0" id (execution-engine UUID).
+    for info in registered:
+        if ip_of(info) == robot_ip and info.id != "0":
+            return info
+
+    # Step 3: different id, IP matches, id="0" (legacy single-robot fallback).
+    if allow_legacy_zero_id_match:
+        for info in registered:
+            if ip_of(info) == robot_ip and info.id == "0":
+                return info
+
+    return None
+
+
+def adopt_existing_registration(
+    vrmcs_client: Any,
+    robot_instance: Any,
+    *,
+    log: Callable[[str], None] = logger.info,
+    allow_legacy_zero_id_match: bool = False,
+    prefer_ip_match: bool = False,
+) -> bool:
+    """Look up an adoptable VRMCS registration for ``robot_instance`` and adopt it.
+
+    Mutates ``robot_instance.id`` in place when the adopted registration's id
+    differs from the caller's id (the execution-engine UUID case). Returns
+    ``True`` when a registration was adopted so the caller can skip the fresh
+    registration path; ``False`` when no compatible registration exists.
+
+    ``log`` is invoked with the adoption message; defaults to module logger
+    info. Pass a wrapper for callers with custom logging interfaces.
+
+    Set ``prefer_ip_match=True`` when the caller has no explicit robot id. This
+    skips exact id matching for placeholder id "0", allowing the non-zero
+    execution-engine registration with the same IP to be adopted. If no such
+    registration exists, callers can still fall back to registering id "0".
+    """
+    robot_ip = robot_instance.driver_params.params.get("ip", "")
+    query_id = "__auto_by_ip__" if prefer_ip_match else robot_instance.id
+    existing = find_compatible_registration(
+        vrmcs_client,
+        robot_id=query_id,
+        robot_ip=robot_ip,
+        allow_legacy_zero_id_match=allow_legacy_zero_id_match,
+    )
+    if existing is None:
+        return False
+    if existing.id != robot_instance.id:
+        log(f"Adopting existing VRMCS registration id={existing.id!r} " f"(was {robot_instance.id!r}) for robot at {robot_ip}")
+        robot_instance.id = existing.id
+    else:
+        log("Robot already registered with VRMCS, proceeding.")
+    return True

griip_core/robot_drivers.py

@@ -0,0 +1,6 @@
+"""Robot driver names shared by the SDK cell config and the server's InitializeRequest."""
+
+# The app owns the robot (e.g. a Fanuc over RMI). Nothing registers it in VRMCS,
+# the server skips IK and path planning, and the SDK reads pose, joints and IK
+# from a robot object the app passes in.
+EXTERNAL_ROBOT_DRIVER = "external"

griip_core/utils/joint_math.py

@@ -0,0 +1,90 @@
+"""Joint-angle math helpers used internally by the SDK.
+
+Kept here (rather than in ``mmai_griip_api.utils.joint_math``) so the
+published griip-sdk wheel can be imported without the API app installed.
+``mmai_griip_api.utils.joint_math`` re-exports these for backward compat.
+"""
+
+from __future__ import annotations
+
+import logging
+from typing import Optional, TypeVar, Union
+
+import numpy as np
+
+logger = logging.getLogger(__name__)
+
+JointConfiguration = list[float]
+JointConfigurations = Union[list[float], list[list[float]]]
+
+_JointConfigurationsT = TypeVar("_JointConfigurationsT", list[float], list[list[float]])
+
+
+def normalize_joint_configurations_to_reference(
+    joint_configurations: _JointConfigurationsT,
+    reference_joint_angles: list[float],
+    joint_lower_limits: Optional[list[float]] = None,
+    joint_upper_limits: Optional[list[float]] = None,
+) -> _JointConfigurationsT:
+    """Wrap each joint to the equivalent angle closest to the reference configuration.
+
+    Accepts a single configuration (1-D) or a batch (2-D); returns the same shape.
+
+    Pass the joint limits whenever they are known. The closest equivalent is the
+    shortest move but not always a legal one: from a reference already part-way
+    round, it can land a further revolution out and drive the joint into its hard
+    stop. With limits supplied, such a joint falls back to the opposite
+    revolution instead, which is the longer move but a reachable one.
+    """
+    reference = np.asarray(reference_joint_angles, dtype=float)
+    joint_positions = np.asarray(joint_configurations, dtype=float)
+
+    if joint_positions.ndim == 1:
+        is_compatible = joint_positions.shape == reference.shape
+    elif joint_positions.ndim == 2:
+        is_compatible = joint_positions.shape[1] == reference.shape[0]
+    else:
+        is_compatible = False
+
+    if not is_compatible:
+        raise ValueError(
+            f"joint_configurations has shape {joint_positions.shape}, " f"but reference_joint_angles has length {reference.shape[0]}"
+        )
+
+    two_pi = 2.0 * np.pi
+    delta = (joint_positions - reference + np.pi) % two_pi - np.pi
+    normalized = reference + delta
+
+    if joint_lower_limits is not None and joint_upper_limits is not None:
+        normalized = _prefer_reachable_revolution(
+            normalized,
+            np.asarray(joint_lower_limits, dtype=float),
+            np.asarray(joint_upper_limits, dtype=float),
+        )
+
+    result: _JointConfigurationsT = normalized.tolist()
+    return result
+
+
+def _prefer_reachable_revolution(
+    normalized: "np.ndarray",
+    lower_limits: "np.ndarray",
+    upper_limits: "np.ndarray",
+) -> "np.ndarray":
+    """Swap out-of-limit angles for their other revolution when that one fits."""
+    outside = (normalized < lower_limits) | (normalized > upper_limits)
+    if not outside.any():
+        return normalized
+
+    two_pi = 2.0 * np.pi
+    alternate = normalized + np.where(normalized < lower_limits, two_pi, -two_pi)
+    alternate_fits = (alternate >= lower_limits) & (alternate <= upper_limits)
+
+    unreachable = outside & ~alternate_fits
+    if unreachable.any():
+        logger.warning(
+            "Joint angles outside limits in both revolutions, leaving them as-is: %s",
+            np.asarray(normalized)[unreachable].tolist(),
+        )
+
+    return np.where(outside & alternate_fits, alternate, normalized)

griip_core/utils/tests/test_joint_math.py

@@ -0,0 +1,116 @@
+"""Goal normalization must not wind a joint into its hard stop.
+
+The degree values below are the ones VRMCS was actually commanded on the Timken
+cell on 2026-08-17, recovered from vrmcs.log. Without limits the normalization
+walked J6 from -117.5 deg to -286.9 deg and then to -362.5 deg, past the UR's
++/-360 deg travel, and the arm tripped into RECOVERABLE_FAULT mid-trajectory.
+"""
+
+import math
+
+import pytest
+
+from griip_core.utils.joint_math import normalize_joint_configurations_to_reference
+
+UR_LOWER = [math.radians(-300.0)] * 6
+UR_UPPER = [math.radians(300.0)] * 6
+# Single-joint cases need single-joint limits; numpy would broadcast a 6-wide
+# limit array against a 1-wide configuration and silently return six angles.
+ONE_JOINT_LOWER = [math.radians(-300.0)]
+ONE_JOINT_UPPER = [math.radians(300.0)]
+
+
+def degrees(joints_rad: list[float]) -> list[float]:
+    return [round(math.degrees(value), 1) for value in joints_rad]
+
+
+def radians(joints_deg: list[float]) -> list[float]:
+    return [math.radians(value) for value in joints_deg]
+
+
+def test_without_limits_it_still_takes_the_nearest_revolution() -> None:
+    """The documented behaviour when no limits are known, kept intact."""
+    goal = radians([73.1])
+    reference = radians([-117.5])
+
+    result = normalize_joint_configurations_to_reference(goal, reference)
+
+    assert degrees(result) == [-286.9]
+
+
+def test_the_capture_move_that_wound_j6() -> None:
+    """home -> apriltag capture: nearest is -286.9, outside +/-300 it must not be used."""
+    goal = radians([15.5, -33.0, 72.5, -139.5, -147.6, 73.1])
+    reference = radians([60.0, -38.9, 65.4, 245.3, -84.2, -117.5])
+
+    unlimited = degrees(normalize_joint_configurations_to_reference(goal, reference))
+    limited = degrees(
+        normalize_joint_configurations_to_reference(goal, reference, joint_lower_limits=UR_LOWER, joint_upper_limits=UR_UPPER)
+    )
+
+    assert unlimited[5] == -286.9
+    # -286.9 is inside +/-300, so J6 is left alone; J4's 245.3-relative nearest is not.
+    assert limited[5] == -286.9
+    assert abs(limited[3]) <= 300.0
+
+
+def test_the_calibration_move_that_hit_the_hard_stop() -> None:
+    """capture -> calibration: nearest equivalent is -362.5 deg, past the hard stop."""
+    goal = radians([-2.46])
+    reference = radians([-286.9])
+
+    unlimited = degrees(normalize_joint_configurations_to_reference(goal, reference))
+    limited = degrees(
+        normalize_joint_configurations_to_reference(goal, reference, joint_lower_limits=ONE_JOINT_LOWER, joint_upper_limits=ONE_JOINT_UPPER)
+    )
+
+    assert unlimited == [-362.5], "reproduces the pre-fix command that faulted the arm"
+    assert limited == [-2.5], "falls back to the reachable revolution"
+
+
+def test_the_reachable_revolution_is_the_same_physical_angle() -> None:
+    goal = radians([-2.46])
+    reference = radians([-286.9])
+
+    limited = normalize_joint_configurations_to_reference(
+        goal, reference, joint_lower_limits=ONE_JOINT_LOWER, joint_upper_limits=ONE_JOINT_UPPER
+    )
+
+    offset = abs(limited[0] - goal[0])
+    assert offset % (2 * math.pi) == pytest.approx(0.0, abs=1e-9)
+
+
+def test_in_range_goals_are_untouched() -> None:
+    goal = radians([10.0, -20.0, 30.0, -40.0, 50.0, -60.0])
+    reference = radians([12.0, -18.0, 33.0, -44.0, 55.0, -66.0])
+
+    limited = normalize_joint_configurations_to_reference(goal, reference, joint_lower_limits=UR_LOWER, joint_upper_limits=UR_UPPER)
+
+    assert degrees(limited) == [10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
+
+
+def test_a_batch_of_configurations_keeps_its_shape() -> None:
+    goals = [radians([-2.46]), radians([73.1])]
+    reference = radians([-286.9])
+
+    limited = normalize_joint_configurations_to_reference(
+        goals, reference, joint_lower_limits=[math.radians(-300.0)], joint_upper_limits=[math.radians(300.0)]
+    )
+
+    assert len(limited) == 2
+    assert degrees(limited[0]) == [-2.5]
+    assert all(abs(math.degrees(value)) <= 300.0 for row in limited for value in row)
+
+
+def test_unreachable_in_both_revolutions_is_left_alone_and_warned(caplog: pytest.LogCaptureFixture) -> None:
+    """A goal no revolution can satisfy stays put so the planner can reject it."""
+    goal = radians([0.0])
+    reference = radians([0.0])
+
+    with caplog.at_level("WARNING"):
+        limited = normalize_joint_configurations_to_reference(
+            goal, reference, joint_lower_limits=[math.radians(100.0)], joint_upper_limits=[math.radians(120.0)]
+        )
+
+    assert degrees(limited) == [0.0]
+    assert "outside limits in both revolutions" in caplog.text

griip_core/utils/transforms.py

@@ -0,0 +1,42 @@
+"""SE3/SO3 transform helpers used internally by the SDK.
+
+Kept here (rather than in ``mmai_griip_api.utils.transforms``) so the
+published griip-sdk wheel can be imported without the API app installed.
+``mmai_griip_api.utils.transforms`` re-exports these for backward compat.
+"""
+
+from __future__ import annotations
+
+import numpy as np
+import numpy.typing as npt
+from spatialmath import SE3, SO3, UnitQuaternion
+
+
+def normalize_rotation_matrix_to_so3(rotation: npt.NDArray[np.float64]) -> SO3:
+    """Re-normalize a 3x3 rotation matrix to a valid SO3 via quaternion round-trip."""
+    raw_so3 = SO3(rotation, check=False)
+    quaternion_xyzs = raw_so3.UnitQuaternion().vec_xyzs
+    x, y, z, scalar = quaternion_xyzs / np.linalg.norm(quaternion_xyzs)
+    return UnitQuaternion(s=scalar, v=[x, y, z]).SO3()
+
+
+def normalize_rotation_matrix_to_se3(pose: npt.NDArray[np.float64]) -> SE3:
+    """Build an SE3 from a raw 4x4 matrix, re-normalizing the rotation to be a valid SO3."""
+    rotation = normalize_rotation_matrix_to_so3(pose[:3, :3])
+    translation = pose[:3, 3]
+    return SE3.Rt(R=rotation, t=translation)
+
+
+def se3_to_pose_list(pose: SE3) -> list[float]:
+    """Convert SE3 to [x, y, z, qw, qx, qy, qz] format for IK solving."""
+    translation = pose.t
+    quaternion_xyzs = UnitQuaternion(pose.R).vec_xyzs
+    return [
+        float(translation[0]),
+        float(translation[1]),
+        float(translation[2]),
+        float(quaternion_xyzs[3]),
+        float(quaternion_xyzs[0]),
+        float(quaternion_xyzs[1]),
+        float(quaternion_xyzs[2]),
+    ]

griip_core_dev-0.5.6.dev25546.dist-info/METADATA

@@ -0,0 +1,12 @@
+Metadata-Version: 2.5
+Name: griip-core-dev
+Version: 0.5.6.dev25546
+Summary: Dev build of griip-core from commit 0af7ee671 (PR #6192). Not a release.
+Author: VentionCo
+License: Proprietary
+Requires-Python: <3.13,>=3.10
+Requires-Dist: grpcio>=1.78.0
+Requires-Dist: numpy>=1.24
+Requires-Dist: protobuf>=4.0
+Requires-Dist: spatialmath-python<2.0.0,>=1.1.15
+Requires-Dist: vention-grpc-py-sdk>=0.0.72

griip_core_dev-0.5.6.dev25546.dist-info/RECORD

@@ -0,0 +1,22 @@
+griip_core/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+griip_core/py.typed,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+griip_core/robot_drivers.py,sha256=koVucKBYTaI1ve7PSxUQhuBcdlj7kBOeSsBTENc9e8Q,328
+griip_core/generated/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+griip_core/generated/griip_pb2.py,sha256=cn3UZ_3fkaAcDtNzQVc5DqxnJzeEemdVJNBPZkalwYI,22782
+griip_core/generated/griip_pb2.pyi,sha256=26QhSFqaElTEQSPLYZ7sXRyebZemJxDiQm8kwIryd-Q,39704
+griip_core/generated/griip_pb2_grpc.py,sha256=sdjto2CXVNDk0GU4GkRi5T5UhjpKeKnsq-byNFfzj4U,27263
+griip_core/generated/griip_pb2_grpc.pyi,sha256=YsnkviRAliqP1f-jRk7pC_t8ai88UefUYBQ7sB4mYAY,20330
+griip_core/hardware/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+griip_core/hardware/vrmcs_client.py,sha256=Ks1j3L8czl1S5O_YmKvVQKZPCz8ZnWWHP0Xv9l0Hau4,54216
+griip_core/hardware/vrmcs_client.pyi,sha256=qzIlnh1h3f1e8wbMxyiSj8VA7CuIOvbD6Uu8TwSA_fo,8215
+griip_core/hardware/vrmcs_registration.py,sha256=7lwtyyGlcJgVx9sGPMU2RwKcYN0vTAgT3RryH4we_pA,5239
+griip_core/hardware/tests/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+griip_core/hardware/tests/test_vrmcs_registration.py,sha256=75uBoWTyNGXyr2cABjwBJrMqoD4ujBEJpC8JuMClRl8,6325
+griip_core/utils/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+griip_core/utils/joint_math.py,sha256=KYLXGpiPr8qx36JT0gSh77czTvqrBw3iFoKJgsw6DxU,3373
+griip_core/utils/transforms.py,sha256=SlFwNhYTq2LmpaLP8aiw7h5F1PIHyvxDAX1qPlXfq5g,1567
+griip_core/utils/tests/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
+griip_core/utils/tests/test_joint_math.py,sha256=P0d3eFhj-x8ebTGoCRkY5ZNmgh_Td00cWmDSKVIUbug,4548
+griip_core_dev-0.5.6.dev25546.dist-info/METADATA,sha256=1GNlcFLl4IEIr8oWi9RMkvHwPAavWLBvJgNRrRFMMzQ,396
+griip_core_dev-0.5.6.dev25546.dist-info/WHEEL,sha256=W3fkpkm7-wf9vBI5Z-7s0eWkeM-spu78I8Neb98DeEg,87
+griip_core_dev-0.5.6.dev25546.dist-info/RECORD,,

griip_core_dev-0.5.6.dev25546.dist-info/WHEEL

@@ -0,0 +1,4 @@
+Wheel-Version: 1.0
+Generator: hatchling 1.32.4
+Root-Is-Purelib: true
+Tag: py3-none-any