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.
- r1_python_sdk/__init__.py +22 -0
- r1_python_sdk/api.py +40 -0
- r1_python_sdk/base.py +1075 -0
- r1_python_sdk/board.py +71 -0
- r1_python_sdk/code.py +161 -0
- r1_python_sdk/config.py +124 -0
- r1_python_sdk/damiao.py +239 -0
- r1_python_sdk/decorator.py +57 -0
- r1_python_sdk/gripper.py +125 -0
- r1_python_sdk/kin/__init__.py +19 -0
- r1_python_sdk/kin/chain.py +149 -0
- r1_python_sdk/kin/ik.py +180 -0
- r1_python_sdk/kin/transform.py +133 -0
- r1_python_sdk/kinematics.py +342 -0
- r1_python_sdk/logport.py +105 -0
- r1_python_sdk/protocol.py +354 -0
- r1_python_sdk/servo.py +287 -0
- r1_python_sdk/transport.py +246 -0
- r1_python_sdk/utils.py +50 -0
- r1_python_sdk/version.py +1 -0
- r1_python_sdk-0.2.0.dist-info/METADATA +399 -0
- r1_python_sdk-0.2.0.dist-info/RECORD +24 -0
- r1_python_sdk-0.2.0.dist-info/WHEEL +4 -0
- r1_python_sdk-0.2.0.dist-info/licenses/LICENSE +202 -0
r1_python_sdk/gripper.py
ADDED
|
@@ -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")
|
r1_python_sdk/kin/ik.py
ADDED
|
@@ -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))
|