克隆下来必须带上正在用的视觉权重和本机 site.yaml，否则感知只能指到一台机器上的路径，别人起不来。

顶视迁臂改走厂家 MoveL，D405 框心对准后再对孔；夹爪停在 caught 时 open 不再把相同目标当成已经张开，init 会真的回零。

Co-authored-by: Cursor <cursoragent@cursor.com>

thirdparty/flexiv_rdk/example_py/utility.py

@@ -0,0 +1,109 @@
+#!/usr/bin/env python
+
+"""utility.py
+
+Utility methods.
+
+Import this module as `import utility` and use functions through the module namespace,
+for example `utility.quat2eulerZYX(...)` and `utility.primitive_state_true_for_groups(...)`.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import math
+
+# pip install scipy
+from scipy.spatial.transform import Rotation as R
+
+
+def quat2eulerZYX(quat, degree=False):
+    """
+    Convert quaternion to Euler angles with ZYX axis rotations.
+
+    Parameters
+    ----------
+    quat : float list
+        Quaternion input in [w,x,y,z] order.
+    degree : bool
+        Return values in degrees, otherwise in radians.
+
+    Returns
+    ----------
+    float list
+        Euler angles in [x,y,z] order, radian by default unless specified otherwise.
+        Returned angles are normalized by scipy to a principal range. In degree mode,
+        this is typically within [-180, 180].
+    """
+
+    # Convert target quaternion to Euler ZYX using scipy package's 'xyz' extrinsic rotation
+    # NOTE: scipy uses [x,y,z,w] order to represent quaternion. The returned Euler values
+    # are a normalized representation of the same physical orientation.
+    eulerZYX = (
+        R.from_quat([quat[1], quat[2], quat[3], quat[0]])
+        .as_euler("xyz", degrees=degree)
+        .tolist()
+    )
+
+    return eulerZYX
+
+
+def primitive_state_true_for_groups(primitive_states, state_name):
+    """
+    Check whether primitive states are true for all groups included in primitive_states.
+
+    Parameters
+    ----------
+    primitive_states : dict
+        Primitive states keyed by joint group.
+    state_name : str or dict
+        Primitive state name to check for all groups, or per-group state names keyed by joint
+        group, e.g. "reachedTarget" or {group: "terminated"}.
+
+    Returns
+    ----------
+    bool
+        True if all included groups contain the requested primitive state and its integer value is
+        non-zero.
+    """
+
+    def primitive_state_true(primitive_state, requested_state_name):
+        if requested_state_name not in primitive_state.names_and_values:
+            return False
+
+        value = primitive_state.names_and_values[requested_state_name]
+        return isinstance(value, int) and value != 0
+
+    if isinstance(state_name, dict):
+        return all(
+            group in state_name
+            and primitive_state_true(primitive_state, state_name[group])
+            for group, primitive_state in primitive_states.items()
+        )
+
+    return all(
+        primitive_state_true(primitive_state, state_name)
+        for primitive_state in primitive_states.values()
+    )
+
+
+def list2str(ls):
+    """
+    Convert a list to a string.
+
+    Parameters
+    ----------
+    ls : list
+        Source list of any size.
+
+    Returns
+    ----------
+    str
+        A string with format "ls[0] ls[1] ... ls[n] ", i.e. each value
+        followed by a space, including the last one.
+    """
+
+    ret_str = ""
+    for i in ls:
+        ret_str += str(i) + " "
+    return ret_str