r1-python-sdk 0.2.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -0,0 +1,125 @@
1
+ """Gripper (end board: 0x7F1 command / 0x7F2 status).
2
+
3
+ The gripper belongs to the end board: a Damiao CAN gripper homes itself on power-up and manages its own enable and
4
+ fault state; an AgiBot OmniPicker RS-485 gripper is driven by the end board acting as Modbus master and counts as
5
+ ready once the handshake succeeds. What the PC can do: home / re-handshake, stop, go to a percentage opening, set
6
+ speed / acc / dec / current limit (0x7F1 PARAM; speed applies on the next POS, the rest are written immediately).
7
+ """
8
+
9
+ from __future__ import annotations
10
+
11
+ from typing import Optional
12
+
13
+ from . import protocol as pr
14
+ from .code import R1State
15
+ from .decorator import r1_catch, r1_connected
16
+ from .transport import CanFrame
17
+ from .base import Base
18
+
19
+
20
+ class Gripper(Base):
21
+
22
+ @r1_catch("set")
23
+ @r1_connected("set")
24
+ def set_gripper_position(self, pos: float, wait: bool = False, timeout: Optional[float] = None,
25
+ tol: int = 3) -> int:
26
+ """Opening 0~100 % (0 fully closed, 100 fully open). wait=True waits until within +/-tol and status READY."""
27
+ if not isinstance(pos, (int, float)) or isinstance(pos, bool) or not (0 <= pos <= 100):
28
+ return R1State.OUT_OF_RANGE
29
+ g = self._gripper_fresh()
30
+ if g is None:
31
+ return R1State.GRIPPER_NO_FEEDBACK
32
+ if g.status == pr.GRIP_ST_FAULT:
33
+ self._set_last_error("gripper fault: call set_gripper_init() to home it again and recover")
34
+ return R1State.GRIPPER_FAULT
35
+ if not g.homed:
36
+ return R1State.GRIPPER_NOT_INIT
37
+ code = self._grip_cmd(pr.GRIP_CMD_POS, int(round(pos)))
38
+ if code != R1State.NORMAL or not wait:
39
+ return code
40
+ tmo = self._cfg.timeout * 5 if timeout is None else float(timeout)
41
+ p = int(round(pos))
42
+
43
+ def reached() -> bool:
44
+ gg = self._gripper
45
+ return gg is not None and gg.status == pr.GRIP_ST_READY and abs(gg.percent - p) <= tol
46
+
47
+ if self._wait_until(reached, tmo):
48
+ return R1State.NORMAL
49
+ gg = self._gripper
50
+ if gg is not None and gg.status == pr.GRIP_ST_FAULT:
51
+ return R1State.GRIPPER_FAULT
52
+ return R1State.WAIT_FINISH_TIMEOUT
53
+
54
+ def open_gripper(self, wait: bool = False, timeout: Optional[float] = None) -> int:
55
+ return self.set_gripper_position(100, wait=wait, timeout=timeout)
56
+
57
+ def close_gripper(self, wait: bool = False, timeout: Optional[float] = None) -> int:
58
+ return self.set_gripper_position(0, wait=wait, timeout=timeout)
59
+
60
+ @r1_connected("get")
61
+ def get_gripper_position(self):
62
+ """(code, percent). When not homed, code=GRIPPER_NOT_INIT but the value is still returned."""
63
+ g = self._gripper_fresh()
64
+ if g is None:
65
+ return R1State.GRIPPER_NO_FEEDBACK, None
66
+ if not g.homed:
67
+ return R1State.GRIPPER_NOT_INIT, g.percent
68
+ return R1State.NORMAL, g.percent
69
+
70
+ @r1_connected("get")
71
+ def get_gripper_status(self):
72
+ """(code, status) status: 0 not homed, 1 homing, 2 ready, 3 fault. See the ``gripper_status`` property for details."""
73
+ g = self._gripper_fresh()
74
+ if g is None:
75
+ return R1State.GRIPPER_NO_FEEDBACK, None
76
+ return R1State.NORMAL, g.status
77
+
78
+ @r1_catch("set")
79
+ @r1_connected("set")
80
+ def set_gripper_init(self) -> int:
81
+ """Re-home (0x7F1 HOME). The end board homes on power-up so this is rarely needed; it is the way to recover from FAULT."""
82
+ return self._grip_cmd(pr.GRIP_CMD_HOME)
83
+
84
+ @r1_catch("set")
85
+ @r1_connected("set")
86
+ def stop_gripper(self) -> int:
87
+ return self._grip_cmd(pr.GRIP_CMD_STOP)
88
+
89
+ # ------------------------------------------------------------------ parameters
90
+ @r1_catch("set")
91
+ @r1_connected("set")
92
+ def set_gripper_param(self, speed: Optional[int] = None, acc: Optional[int] = None,
93
+ dec: Optional[int] = None, current: Optional[int] = None,
94
+ save: bool = False) -> int:
95
+ """Speed / acceleration / deceleration / current limit, each 0~255; None leaves it unchanged.
96
+ save=True writes to the end board's Flash so it survives power-off.
97
+ AgiBot OmniPicker: acc/dec/current are written to the gripper immediately, speed goes with the next
98
+ set_gripper_position. Damiao CAN gripper: the end board stores the values; MIT behaviour is unchanged for now."""
99
+ vals = {"speed": speed, "acc": acc, "dec": dec, "current": current}
100
+ if all(v is None for v in vals.values()) and not save:
101
+ return R1State.OUT_OF_RANGE
102
+ for name, v in vals.items():
103
+ if v is None:
104
+ continue
105
+ if not isinstance(v, (int, float)) or isinstance(v, bool) or not (0 <= v <= 255):
106
+ self._set_last_error(f"set_gripper_param: {name}={v} is outside 0~255")
107
+ return R1State.OUT_OF_RANGE
108
+ frame = pr.grip_param_frame(speed=speed, acc=acc, dec=dec, current=current, save=save)
109
+ return self._send(CanFrame(pr.CAN_ID_GRIP_CMD, frame), confirm=True)
110
+
111
+ def set_gripper_speed(self, speed: int, save: bool = False) -> int:
112
+ """Target speed 0~255, applied on the next set_gripper_position."""
113
+ return self.set_gripper_param(speed=speed, save=save)
114
+
115
+ def set_gripper_force(self, force: int, save: bool = False) -> int:
116
+ """Grip force = current limit 0~255 (AgiBot register 12), written to the gripper immediately."""
117
+ return self.set_gripper_param(current=force, save=save)
118
+
119
+ def set_gripper_enable(self, enable: bool = True) -> int:
120
+ """CMD_NOT_EXIST: the end board manages gripper enable itself; there is no such command on CAN."""
121
+ return R1State.CMD_NOT_EXIST
122
+
123
+ # ------------------------------------------------------------------ internal
124
+ def _grip_cmd(self, cmd: int, pos: int = 0) -> int:
125
+ return self._send(CanFrame(pr.CAN_ID_GRIP_CMD, pr.grip_cmd_frame(cmd, pos)), confirm=True)
@@ -0,0 +1,19 @@
1
+ """PC-side kinematics for the R1 arm: FK, geometric Jacobian, damped-least-squares IK. Pure numpy.
2
+
3
+ from r1_python_sdk.kin import R1_CHAIN, IKSolver, pose_to_T
4
+ T = R1_CHAIN.fk([0, -0.96, -1.12, -1.57, 0.94, 1.57])
5
+ res = IKSolver(R1_CHAIN).solve(T, q_init=[0] * 6)
6
+ """
7
+
8
+ from .chain import R1_CHAIN, R1_JOINTS, R1_T_FLANGE, Chain, JointSpec
9
+ from .ik import IKOptions, IKResult, IKSolver
10
+ from .transform import (T_to_pose, axis_angle_to_mat, inv_T, make_T, mat_to_rpy, min_jerk, pose_to_T,
11
+ rot_angle_between, rot_exp, rot_interp, rot_log, rot_x, rot_y, rot_z, rpy_to_mat)
12
+
13
+ __all__ = [
14
+ "Chain", "JointSpec", "R1_CHAIN", "R1_JOINTS", "R1_T_FLANGE",
15
+ "IKSolver", "IKOptions", "IKResult",
16
+ "rpy_to_mat", "mat_to_rpy", "pose_to_T", "T_to_pose", "make_T", "inv_T",
17
+ "rot_x", "rot_y", "rot_z", "axis_angle_to_mat", "rot_log", "rot_exp", "rot_interp", "rot_angle_between",
18
+ "min_jerk",
19
+ ]
@@ -0,0 +1,149 @@
1
+ """Serial kinematic chain of the R1 arm: forward kinematics and geometric Jacobian (numpy only).
2
+
3
+ The default chain ``R1_CHAIN`` is transcribed from ``R1_Arm_description/urdf/R1.urdf`` (joint1..joint6 + the fixed
4
+ ``end_joint``), so the SDK needs neither the URDF file nor MuJoCo / pinocchio at run time. ``Chain.from_urdf`` can
5
+ rebuild it from a URDF if the hardware description ever changes.
6
+
7
+ Frames:
8
+ - base frame = ``base_link`` (the URDF root, arm mounting plane, +z up);
9
+ - flange frame = ``end_link`` (gripper fingertip plane; +x points along the gripper, fingers open along +/-y);
10
+ - TCP = flange frame shifted by an optional tool transform.
11
+ """
12
+
13
+ from __future__ import annotations
14
+
15
+ import math
16
+ import xml.etree.ElementTree as ET
17
+ from dataclasses import dataclass
18
+ from pathlib import Path
19
+ from typing import List, Optional, Sequence, Tuple
20
+
21
+ import numpy as np
22
+
23
+ from .transform import axis_angle_to_mat, make_T, mat_to_rpy, pose_to_T
24
+
25
+
26
+ @dataclass(frozen=True)
27
+ class JointSpec:
28
+ """One revolute joint: fixed transform from the parent joint frame, then rotation about ``axis``."""
29
+ name: str
30
+ xyz: Tuple[float, float, float]
31
+ rpy: Tuple[float, float, float]
32
+ axis: Tuple[float, float, float]
33
+ lower: float
34
+ upper: float
35
+
36
+ def T_fixed(self) -> np.ndarray:
37
+ return pose_to_T(self.xyz, self.rpy)
38
+
39
+
40
+ class Chain:
41
+ def __init__(self, joints: Sequence[JointSpec], T_flange: np.ndarray, name: str = "R1") -> None:
42
+ if len(joints) == 0:
43
+ raise ValueError("chain needs at least one joint")
44
+ self.name = name
45
+ self.joints: List[JointSpec] = list(joints)
46
+ self.n = len(self.joints)
47
+ self._T_fixed = [j.T_fixed() for j in self.joints]
48
+ self._axes = [np.asarray(j.axis, dtype=float) / np.linalg.norm(j.axis) for j in self.joints]
49
+ self.T_flange = np.asarray(T_flange, dtype=float).reshape(4, 4)
50
+ self.lower = np.array([j.lower for j in self.joints])
51
+ self.upper = np.array([j.upper for j in self.joints])
52
+
53
+ # ------------------------------------------------------------------ FK
54
+ def fk_frames(self, q: Sequence[float]) -> Tuple[List[np.ndarray], np.ndarray]:
55
+ """Return (joint frames, flange frame). ``frames[i]`` is the pose of joint i *after* its rotation; its
56
+ origin and rotated axis define the i-th Jacobian column."""
57
+ q = np.asarray(q, dtype=float)
58
+ if q.shape != (self.n,):
59
+ raise ValueError(f"q must have {self.n} values")
60
+ T = np.eye(4)
61
+ frames: List[np.ndarray] = []
62
+ for i in range(self.n):
63
+ T = T @ self._T_fixed[i]
64
+ T = T @ make_T(axis_angle_to_mat(self._axes[i], float(q[i])), (0.0, 0.0, 0.0))
65
+ frames.append(T)
66
+ return frames, T @ self.T_flange
67
+
68
+ def fk(self, q: Sequence[float], T_tool: Optional[np.ndarray] = None) -> np.ndarray:
69
+ """4x4 pose of the TCP in the base frame. ``T_tool`` is the tool transform relative to the flange."""
70
+ _, T = self.fk_frames(q)
71
+ return T if T_tool is None else T @ T_tool
72
+
73
+ # ------------------------------------------------------------------ Jacobian
74
+ def jacobian(self, q: Sequence[float], T_tool: Optional[np.ndarray] = None,
75
+ frames: Optional[List[np.ndarray]] = None) -> Tuple[np.ndarray, np.ndarray]:
76
+ """Geometric Jacobian in the base frame: rows 0..2 linear, 3..5 angular, columns = joints.
77
+ Returns (J 6xn, T_tcp 4x4). Pass ``frames`` from fk_frames to avoid recomputing."""
78
+ if frames is None:
79
+ frames, T_fl = self.fk_frames(q)
80
+ else:
81
+ T_fl = frames[-1] @ self.T_flange
82
+ T_tcp = T_fl if T_tool is None else T_fl @ T_tool
83
+ p_e = T_tcp[:3, 3]
84
+ J = np.zeros((6, self.n))
85
+ for i, Ti in enumerate(frames):
86
+ z = Ti[:3, :3] @ self._axes[i]
87
+ J[:3, i] = np.cross(z, p_e - Ti[:3, 3])
88
+ J[3:, i] = z
89
+ return J, T_tcp
90
+
91
+ # ------------------------------------------------------------------ limits
92
+ def clamp(self, q: np.ndarray) -> np.ndarray:
93
+ return np.minimum(np.maximum(q, self.lower), self.upper)
94
+
95
+ def within_limits(self, q: Sequence[float], tol: float = 1e-9) -> bool:
96
+ q = np.asarray(q, dtype=float)
97
+ return bool(np.all(q >= self.lower - tol) and np.all(q <= self.upper + tol))
98
+
99
+ # ------------------------------------------------------------------ construction from URDF
100
+ @classmethod
101
+ def from_urdf(cls, path: "str | Path", base: str = "base_link", tip: str = "end_link") -> "Chain":
102
+ """Walk the URDF from ``base`` to ``tip`` collecting revolute joints; fixed joints are folded into the
103
+ next joint's transform (or into the flange transform after the last revolute joint)."""
104
+ root = ET.parse(str(path)).getroot()
105
+ by_parent = {}
106
+ for j in root.findall("joint"):
107
+ by_parent[j.find("parent").get("link")] = j
108
+
109
+ joints: List[JointSpec] = []
110
+ T_acc = np.eye(4)
111
+ link = base
112
+ while link != tip:
113
+ j = by_parent.get(link)
114
+ if j is None:
115
+ raise ValueError(f"URDF: no joint leaves link {link!r} on the way to {tip!r}")
116
+ o = j.find("origin")
117
+ xyz = tuple(float(v) for v in (o.get("xyz", "0 0 0") if o is not None else "0 0 0").split())
118
+ rpy = tuple(float(v) for v in (o.get("rpy", "0 0 0") if o is not None else "0 0 0").split())
119
+ T_acc = T_acc @ pose_to_T(xyz, rpy)
120
+ if j.get("type") in ("revolute", "continuous"):
121
+ ax = j.find("axis")
122
+ axis = tuple(float(v) for v in (ax.get("xyz", "0 0 1") if ax is not None else "0 0 1").split())
123
+ lim = j.find("limit")
124
+ lo = float(lim.get("lower", -math.pi)) if lim is not None else -math.pi
125
+ hi = float(lim.get("upper", math.pi)) if lim is not None else math.pi
126
+ r, p, y = mat_to_rpy(T_acc[:3, :3])
127
+ joints.append(JointSpec(j.get("name"), tuple(T_acc[:3, 3]), (r, p, y), axis, lo, hi))
128
+ T_acc = np.eye(4)
129
+ elif j.get("type") != "fixed":
130
+ raise ValueError(f"URDF: unsupported joint type {j.get('type')!r} ({j.get('name')})")
131
+ link = j.find("child").get("link")
132
+ return cls(joints, T_acc, name=root.get("name", "robot"))
133
+
134
+
135
+ # ---------------------------------------------------------------------- R1 (from R1.urdf)
136
+ _PI2 = 1.5708
137
+
138
+ R1_JOINTS: Tuple[JointSpec, ...] = (
139
+ JointSpec("joint1", (0.0, 0.0, 0.08465), (0.0, 0.0, 0.0), (0.0, 0.0, 1.0), -2.8, 2.8),
140
+ JointSpec("joint2", (0.020084, 0.031625, 0.05555), (-_PI2, 0.0, 0.0), (0.0, 0.0, -1.0), -3.14, 0.0),
141
+ JointSpec("joint3", (-0.244, 0.0, 0.0), (0.0, 0.0, 0.0), (0.0, 0.0, 1.0), -3.14, 0.0),
142
+ JointSpec("joint4", (0.228, -0.059, -0.031625), (0.0, _PI2, 0.0), (0.0, 0.0, 1.0), -3.1416, 3.1416),
143
+ JointSpec("joint5", (0.0, -0.02275, 0.046), (_PI2, -_PI2, 0.0), (0.0, 0.0, 1.0), -1.57, 1.57),
144
+ JointSpec("joint6", (0.090, 0.0, -0.02275), (0.0, -_PI2, 3.1416), (0.0, 0.0, 1.0), -3.14, 3.14),
145
+ )
146
+ # end_joint (fixed): link6 -> end_link
147
+ R1_T_FLANGE = pose_to_T((0.0, 0.0, 0.17889), (0.0, -_PI2, 3.1416))
148
+
149
+ R1_CHAIN = Chain(R1_JOINTS, R1_T_FLANGE, name="R1")
@@ -0,0 +1,180 @@
1
+ """Damped-least-squares inverse kinematics (numpy only).
2
+
3
+ Same algorithm family as ``open/xyz-test/xyz_sim.py`` (MuJoCo) and the reBotArm CLIK solver (pinocchio), but with
4
+ the FK / Jacobian coming from ``Chain`` so the SDK carries no simulator dependency.
5
+
6
+ Task modes:
7
+ - full: position + orientation (6 rows);
8
+ - position: position only (3 rows), orientation free;
9
+ - free_axis: position + orientation, but rotation about one world axis is left free (5 rows).
10
+ With axis = +z this is the "follow the base yaw" mode of xyz-test: the tool keeps its tilt but is
11
+ allowed to swing with joint1, which gives a much larger lateral workspace.
12
+ """
13
+
14
+ from __future__ import annotations
15
+
16
+ from dataclasses import dataclass
17
+ from typing import Optional, Sequence, Tuple
18
+
19
+ import numpy as np
20
+
21
+ from .chain import Chain
22
+ from .transform import rot_log
23
+
24
+
25
+ @dataclass
26
+ class IKOptions:
27
+ max_iter: int = 100
28
+ tol_pos: float = 1e-4 # m
29
+ tol_rot: float = 1e-3 # rad
30
+ damping: float = 0.05 # lambda of the DLS step (adaptively scaled by the error)
31
+ rot_weight: float = 0.5 # orientation rows weight relative to position rows
32
+ max_dq: float = 0.25 # per-iteration cap on any joint increment (rad)
33
+ position_only: bool = False
34
+ free_axis: Optional[Sequence[float]] = None # world axis whose rotation is unconstrained (None = constrain all)
35
+
36
+
37
+ @dataclass
38
+ class IKResult:
39
+ q: np.ndarray
40
+ success: bool
41
+ pos_err: float # m
42
+ rot_err: float # rad (0 when position_only)
43
+ iterations: int
44
+ at_limit: list # 0-based indices of joints sitting on a limit in the returned q
45
+ limit_blocked: bool = False # during the solve a joint on its limit wanted to move further out (frozen column)
46
+
47
+
48
+ def _rot_basis_without(axis: np.ndarray) -> np.ndarray:
49
+ """2x3 orthonormal rows spanning the plane perpendicular to ``axis``."""
50
+ a = axis / np.linalg.norm(axis)
51
+ helper = np.array([1.0, 0.0, 0.0]) if abs(a[0]) < 0.9 else np.array([0.0, 1.0, 0.0])
52
+ u = np.cross(a, helper)
53
+ u /= np.linalg.norm(u)
54
+ v = np.cross(a, u)
55
+ return np.vstack([u, v])
56
+
57
+
58
+ class IKSolver:
59
+ def __init__(self, chain: Chain, options: Optional[IKOptions] = None) -> None:
60
+ self.chain = chain
61
+ self.opt = options or IKOptions()
62
+
63
+ # ------------------------------------------------------------------ error
64
+ def _task_error(self, T_cur: np.ndarray, T_tgt: np.ndarray, opt: IKOptions):
65
+ """Return (e, P) where e is the stacked task error and P selects / weights the Jacobian rows."""
66
+ e_pos = T_tgt[:3, 3] - T_cur[:3, 3]
67
+ if opt.position_only:
68
+ return e_pos, None, 0.0
69
+ # rotation error in the world frame: rotate the local log into world
70
+ R_cur = T_cur[:3, :3]
71
+ e_rot_world = R_cur @ rot_log(R_cur.T @ T_tgt[:3, :3])
72
+ rot_err = float(np.linalg.norm(e_rot_world))
73
+ if opt.free_axis is not None:
74
+ B = _rot_basis_without(np.asarray(opt.free_axis, dtype=float))
75
+ e_rot = B @ e_rot_world
76
+ rot_err = float(np.linalg.norm(e_rot))
77
+ else:
78
+ B = None
79
+ e_rot = e_rot_world
80
+ return np.concatenate([e_pos, opt.rot_weight * e_rot]), B, rot_err
81
+
82
+ def _task_jacobian(self, J: np.ndarray, B: Optional[np.ndarray], opt: IKOptions) -> np.ndarray:
83
+ if opt.position_only:
84
+ return J[:3]
85
+ Jr = J[3:] if B is None else B @ J[3:]
86
+ return np.vstack([J[:3], opt.rot_weight * Jr])
87
+
88
+ # ------------------------------------------------------------------ solve
89
+ def solve(self, T_target: np.ndarray, q_init: Sequence[float], T_tool: Optional[np.ndarray] = None,
90
+ lower: Optional[np.ndarray] = None, upper: Optional[np.ndarray] = None,
91
+ options: Optional[IKOptions] = None) -> IKResult:
92
+ """Solve for q such that fk(q) @ T_tool == T_target. ``lower``/``upper`` override the chain limits
93
+ (e.g. to apply tighter soft limits)."""
94
+ opt = options or self.opt
95
+ ch = self.chain
96
+ lo = ch.lower if lower is None else np.asarray(lower, dtype=float)
97
+ hi = ch.upper if upper is None else np.asarray(upper, dtype=float)
98
+ q = np.minimum(np.maximum(np.asarray(q_init, dtype=float).copy(), lo), hi)
99
+ T_target = np.asarray(T_target, dtype=float)
100
+
101
+ frames, _ = ch.fk_frames(q)
102
+ J, T_cur = ch.jacobian(q, T_tool, frames)
103
+ e, B, rot_err = self._task_error(T_cur, T_target, opt)
104
+ pos_err = float(np.linalg.norm(e[:3]))
105
+ it = 0
106
+ blocked_any = False
107
+ for it in range(1, opt.max_iter + 1):
108
+ if pos_err < opt.tol_pos and rot_err < opt.tol_rot:
109
+ break
110
+ Jt = self._task_jacobian(J, B, opt)
111
+ # large damping far from the target (robust), small damping close to it (fast final convergence)
112
+ lam = opt.damping * min(1.0, max(0.02, 10.0 * float(np.linalg.norm(e))))
113
+ dq, blocked = self._dls_step(Jt, e, lam, q, lo, hi)
114
+ blocked_any = blocked_any or blocked
115
+ m = float(np.max(np.abs(dq)))
116
+ if m > opt.max_dq:
117
+ dq *= opt.max_dq / m
118
+ if m < 1e-12:
119
+ break
120
+ # backtracking: accept the step only if the task error decreases
121
+ prev = float(np.linalg.norm(e))
122
+ alpha = 1.0
123
+ accepted = False
124
+ for _ in range(6):
125
+ q_new = np.minimum(np.maximum(q + alpha * dq, lo), hi)
126
+ frames_n, _ = ch.fk_frames(q_new)
127
+ J_n, T_n = ch.jacobian(q_new, T_tool, frames_n)
128
+ e_n, B_n, rot_n = self._task_error(T_n, T_target, opt)
129
+ if float(np.linalg.norm(e_n)) < prev:
130
+ q, J, T_cur, e, B, rot_err = q_new, J_n, T_n, e_n, B_n, rot_n
131
+ pos_err = float(np.linalg.norm(e[:3]))
132
+ accepted = True
133
+ break
134
+ alpha *= 0.5
135
+ if not accepted:
136
+ break
137
+
138
+ ok = pos_err < opt.tol_pos * 3.0 and rot_err < opt.tol_rot * 3.0
139
+ at_lim = [i for i in range(ch.n) if q[i] <= lo[i] + 1e-6 or q[i] >= hi[i] - 1e-6]
140
+ return IKResult(q=q, success=bool(ok), pos_err=pos_err, rot_err=rot_err, iterations=it, at_limit=at_lim,
141
+ limit_blocked=blocked_any)
142
+
143
+ @staticmethod
144
+ def _dls_step(J: np.ndarray, e: np.ndarray, lam: float, q: np.ndarray,
145
+ lo: np.ndarray, hi: np.ndarray) -> Tuple[np.ndarray, bool]:
146
+ """DLS step with active joint limits: a joint sitting on a limit and asked to move further out is frozen
147
+ (its column removed) and the step is re-solved, so the remaining joints keep a consistent direction.
148
+ Returns (dq, any_joint_was_frozen)."""
149
+ n = J.shape[1]
150
+ free = np.ones(n, dtype=bool)
151
+ dq = np.zeros(n)
152
+ frozen = False
153
+ for _ in range(n + 1):
154
+ Jf = J[:, free]
155
+ if Jf.shape[1] == 0:
156
+ return np.zeros(n), True
157
+ A = Jf @ Jf.T + (lam * lam) * np.eye(J.shape[0])
158
+ dq.fill(0.0)
159
+ dq[free] = Jf.T @ np.linalg.solve(A, e)
160
+ blocked = ((q <= lo + 1e-9) & (dq < 0.0)) | ((q >= hi - 1e-9) & (dq > 0.0))
161
+ blocked &= free
162
+ if not np.any(blocked):
163
+ return dq, frozen
164
+ frozen = True
165
+ free[blocked] = False
166
+ return dq, frozen
167
+
168
+ def solve_multi_seed(self, T_target: np.ndarray, seeds: Sequence[Sequence[float]],
169
+ T_tool: Optional[np.ndarray] = None, lower=None, upper=None,
170
+ options: Optional[IKOptions] = None) -> IKResult:
171
+ """Try several seeds, return the first success or else the best (smallest error) attempt."""
172
+ best: Optional[IKResult] = None
173
+ for s in seeds:
174
+ r = self.solve(T_target, s, T_tool, lower, upper, options)
175
+ if r.success:
176
+ return r
177
+ if best is None or (r.pos_err + 0.1 * r.rot_err) < (best.pos_err + 0.1 * best.rot_err):
178
+ best = r
179
+ assert best is not None
180
+ return best
@@ -0,0 +1,133 @@
1
+ """SO(3) / SE(3) helpers (numpy only).
2
+
3
+ Conventions:
4
+ - RPY = fixed-axis XYZ (URDF / ROS): R = Rz(yaw) @ Ry(pitch) @ Rx(roll).
5
+ - Homogeneous transforms are 4x4 float64 arrays.
6
+ """
7
+
8
+ from __future__ import annotations
9
+
10
+ import math
11
+ from typing import Sequence, Tuple
12
+
13
+ import numpy as np
14
+
15
+ Vec3 = Sequence[float]
16
+
17
+
18
+ def rot_x(a: float) -> np.ndarray:
19
+ c, s = math.cos(a), math.sin(a)
20
+ return np.array([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]])
21
+
22
+
23
+ def rot_y(a: float) -> np.ndarray:
24
+ c, s = math.cos(a), math.sin(a)
25
+ return np.array([[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]])
26
+
27
+
28
+ def rot_z(a: float) -> np.ndarray:
29
+ c, s = math.cos(a), math.sin(a)
30
+ return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
31
+
32
+
33
+ def rpy_to_mat(roll: float, pitch: float, yaw: float) -> np.ndarray:
34
+ return rot_z(yaw) @ rot_y(pitch) @ rot_x(roll)
35
+
36
+
37
+ def mat_to_rpy(R: np.ndarray) -> Tuple[float, float, float]:
38
+ """Inverse of rpy_to_mat. Near the pitch = +/-90 deg singularity roll is set to 0 and yaw absorbs the rest."""
39
+ sy = -R[2, 0]
40
+ sy = max(-1.0, min(1.0, sy))
41
+ pitch = math.asin(sy)
42
+ if abs(sy) > 1.0 - 1e-7:
43
+ roll = 0.0
44
+ yaw = math.atan2(-R[0, 1], R[1, 1])
45
+ else:
46
+ roll = math.atan2(R[2, 1], R[2, 2])
47
+ yaw = math.atan2(R[1, 0], R[0, 0])
48
+ return roll, pitch, yaw
49
+
50
+
51
+ def axis_angle_to_mat(axis: Vec3, angle: float) -> np.ndarray:
52
+ """Rodrigues formula; axis is normalised internally."""
53
+ k = np.asarray(axis, dtype=float)
54
+ n = np.linalg.norm(k)
55
+ if n < 1e-12:
56
+ return np.eye(3)
57
+ k = k / n
58
+ K = skew(k)
59
+ return np.eye(3) + math.sin(angle) * K + (1.0 - math.cos(angle)) * (K @ K)
60
+
61
+
62
+ def skew(v: np.ndarray) -> np.ndarray:
63
+ return np.array([[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]])
64
+
65
+
66
+ def rot_log(R: np.ndarray) -> np.ndarray:
67
+ """so(3) logarithm: rotation vector (axis * angle) of R."""
68
+ tr = (np.trace(R) - 1.0) * 0.5
69
+ tr = max(-1.0, min(1.0, tr))
70
+ angle = math.acos(tr)
71
+ if angle < 1e-9:
72
+ return np.zeros(3)
73
+ if math.pi - angle < 1e-6:
74
+ # near pi: axis from the symmetric part
75
+ A = (R + np.eye(3)) * 0.5
76
+ axis = np.sqrt(np.clip(np.diag(A), 0.0, None))
77
+ # fix signs using off-diagonals
78
+ if axis[0] > 1e-6:
79
+ axis[1] = math.copysign(axis[1], A[0, 1])
80
+ axis[2] = math.copysign(axis[2], A[0, 2])
81
+ elif axis[1] > 1e-6:
82
+ axis[2] = math.copysign(axis[2], A[1, 2])
83
+ n = np.linalg.norm(axis)
84
+ return axis / n * angle if n > 0 else np.zeros(3)
85
+ w = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])
86
+ return w * (angle / (2.0 * math.sin(angle)))
87
+
88
+
89
+ def rot_exp(w: np.ndarray) -> np.ndarray:
90
+ w = np.asarray(w, dtype=float)
91
+ a = float(np.linalg.norm(w))
92
+ if a < 1e-12:
93
+ return np.eye(3)
94
+ return axis_angle_to_mat(w / a, a)
95
+
96
+
97
+ def rot_interp(R0: np.ndarray, R1: np.ndarray, t: float) -> np.ndarray:
98
+ """Geodesic interpolation on SO(3) (equivalent to quaternion slerp along the short arc)."""
99
+ return R0 @ rot_exp(t * rot_log(R0.T @ R1))
100
+
101
+
102
+ def rot_angle_between(R0: np.ndarray, R1: np.ndarray) -> float:
103
+ return float(np.linalg.norm(rot_log(R0.T @ R1)))
104
+
105
+
106
+ def make_T(R: np.ndarray, p: Vec3) -> np.ndarray:
107
+ T = np.eye(4)
108
+ T[:3, :3] = R
109
+ T[:3, 3] = np.asarray(p, dtype=float)
110
+ return T
111
+
112
+
113
+ def inv_T(T: np.ndarray) -> np.ndarray:
114
+ R = T[:3, :3]
115
+ p = T[:3, 3]
116
+ out = np.eye(4)
117
+ out[:3, :3] = R.T
118
+ out[:3, 3] = -R.T @ p
119
+ return out
120
+
121
+
122
+ def pose_to_T(xyz: Vec3, rpy: Vec3) -> np.ndarray:
123
+ return make_T(rpy_to_mat(*rpy), xyz)
124
+
125
+
126
+ def T_to_pose(T: np.ndarray) -> Tuple[np.ndarray, Tuple[float, float, float]]:
127
+ return T[:3, 3].copy(), mat_to_rpy(T[:3, :3])
128
+
129
+
130
+ def min_jerk(tau: float) -> float:
131
+ """Minimum-jerk time scaling s(tau) on [0, 1]: zero velocity and acceleration at both ends."""
132
+ tau = max(0.0, min(1.0, tau))
133
+ return tau * tau * tau * (10.0 + tau * (-15.0 + 6.0 * tau))