From b0e48737e619f43b2b3169a8f6c875033bc9cc99 Mon Sep 17 00:00:00 2001 From: DarksaCY Date: Tue, 8 Sep 2026 10:18:26 +0300 Subject: [PATCH 1/3] Share one MJCF kinematics between the drivers that solve for themselves `yam._Kinematics` is `MjcfKinematics` in `kinematics.py`, beside the placo solver a URDF driver uses, and it takes the model, the site, the joints and the warm starts. #517 names that file as the home. It gains `max_jump`, which a streamed setpoint is solved under: no seed but the live posture, and no solution further from it than the cap. Without it the class runs the YAM's steps in the YAM's order, and `yam_kinematics_goldens.npz` -- ten joint vectors, the pose each puts `DEFAULT_FRAME` at, the joints IK finds for it, and two targets out of reach -- pins that it answers what the YAM answered, to 1e-9. The goldens were generated from the driver before the move, off hardware, and the fake smoke passes. --- positronic/drivers/roboarm/kinematics.py | 95 ++++++++++++++++++ .../drivers/roboarm/tests/test_kinematics.py | 75 ++++++++++++++ .../roboarm/tests/yam_kinematics_goldens.npz | Bin 0 -> 4354 bytes positronic/drivers/roboarm/yam.py | 64 +----------- 4 files changed, 173 insertions(+), 61 deletions(-) create mode 100644 positronic/drivers/roboarm/tests/test_kinematics.py create mode 100644 positronic/drivers/roboarm/tests/yam_kinematics_goldens.npz diff --git a/positronic/drivers/roboarm/kinematics.py b/positronic/drivers/roboarm/kinematics.py index c09f44004..79874986e 100644 --- a/positronic/drivers/roboarm/kinematics.py +++ b/positronic/drivers/roboarm/kinematics.py @@ -1,7 +1,19 @@ +"""Solving an arm's kinematics: the placo solver a URDF driver uses, and FK/IK on an MJCF model. + +An arm whose controller solves Cartesian goals itself does not need either. One whose controller does not, +or whose firmware solves them in a way the driver cannot accept, carries its own model and solves here. +""" + +from collections.abc import Callable, Sequence + +import mujoco as mj import numpy as np from positronic import geom from positronic.drivers import vendor_import +from positronic.utils import package_assets_path + +from .ik import qpos_from_site_pose with vendor_import('placo', 'Kinematics support'): import placo @@ -54,3 +66,86 @@ def inverse( @property def joint_limits(self) -> np.ndarray: return np.array([self.robot.get_joint_limits(joint_name) for joint_name in self.joint_names]) + + +_POS_TOL = 1e-3 # meters; FK-verify acceptance for an IK solution after wrapping and clamping +_ROT_TOL = 1e-2 # radians + + +class MjcfKinematics: + """FK and IK on an MJCF model at one site, in the model's own base frame. + + ``mujoco`` exports every symbol below from a compiled extension, so a type checker cannot see them. + + :param mjcf_path: the model, relative to the package assets. + :param site: the site FK reports and IK aims at. + :param joint_names: the joints, in the order a caller states them. + :param reach_postures: warm starts for a target at base-frame ``(x, y)``, tried after the live posture. + The arms this drives have a 6-DoF wrist, which gives LM no null space to escape a bad basin, so a + seed near the goal is what makes limit-clamped IK reliable. + """ + + def __init__( + self, + mjcf_path: str, + site: str, + joint_names: Sequence[str], + reach_postures: Callable[[float, float], list[np.ndarray]], + ): + self._model = mj.MjModel.from_xml_path(package_assets_path(mjcf_path)) + self._data = mj.MjData(self._model) + self._site_id = mj.mj_name2id(self._model, mj.mjtObj.mjOBJ_SITE, site) + self._qpos_ids = np.array([self._model.joint(name).qposadr.item() for name in joint_names]) + self._dof_ids = np.array([self._model.joint(name).dofadr.item() for name in joint_names]) + ranges = np.array([self._model.joint(name).range for name in joint_names]) + self.lower, self.upper = ranges[:, 0], ranges[:, 1] + self._reach_postures = reach_postures + + def fk(self, q: np.ndarray) -> geom.Transform3D: + self._data.qpos[self._qpos_ids] = q + mj.mj_kinematics(self._model, self._data) + quat = np.empty(4) + mj.mju_mat2Quat(quat, self._data.site_xmat[self._site_id].copy()) + return geom.Transform3D(self._data.site_xpos[self._site_id].copy(), geom.Rotation.from_quat(quat)) + + def ik( + self, target: geom.Transform3D, current_q: np.ndarray, max_jump: float | np.ndarray | None = None + ) -> np.ndarray | None: + """The joints that reach ``target``, warm-started from where the arm stands, or ``None``. + + ``max_jump`` bounds how far the solution may sit from ``current_q``, per joint or over all of them, + and the search stops at the live posture: the arm keeps the shape it has, and a pose it can reach + only in another one comes back as nothing. Without it the reach postures are tried too, so the arm + may change shape to get there -- which swings the end effector, and is for a move somebody waits on. + + A solution is wrapped and clamped into joint range and then FK-verified, so a target the arm cannot + reach comes back as nothing rather than as the nearest thing the solver stopped at. + """ + seeds = (current_q,) if max_jump is not None else (current_q, *self._reach_postures(*target.translation[:2])) + for start in seeds: + self._data.qpos[:] = 0.0 + self._data.qpos[self._qpos_ids] = start + qpos, _, success = qpos_from_site_pose( + self._model, + self._data, + self._site_id, + self._dof_ids, + target.translation, + target.rotation.as_quat, + rot_weight=0.5, + ) + if not success: + continue + q = qpos[self._qpos_ids].copy() + # A revolute joint at q ± 2π is the same pose; wrap out-of-range entries back in when they fit. + q = np.where(q > self.upper, q - 2 * np.pi, q) + q = np.where(q < self.lower, q + 2 * np.pi, q) + q = np.clip(q, self.lower, self.upper) + if max_jump is not None and np.any(np.abs(q - current_q) > max_jump): + continue + reached = self.fk(q) + rot_err = (reached.rotation.inv * target.rotation).angle + rot_err = min(rot_err, 2 * np.pi - rot_err) + if np.linalg.norm(reached.translation - target.translation) < _POS_TOL and rot_err < _ROT_TOL: + return q + return None diff --git a/positronic/drivers/roboarm/tests/test_kinematics.py b/positronic/drivers/roboarm/tests/test_kinematics.py new file mode 100644 index 000000000..f7c89673b --- /dev/null +++ b/positronic/drivers/roboarm/tests/test_kinematics.py @@ -0,0 +1,75 @@ +"""What ``MjcfKinematics`` answers, pinned to what the YAM driver answered before it shared the class. + +``yam_kinematics_goldens.npz`` was generated from the YAM's own ``_Kinematics`` at `cab986af`: ten random +joint vectors inside the model's range, the pose each puts ``DEFAULT_FRAME`` at, the joints IK finds for +that pose from a seed 0.15 rad away, and two targets outside the arm's reach. Regenerating it is a change +of behaviour, not of test data. +""" + +from pathlib import Path + +import numpy as np +import pytest + +from positronic import geom +from positronic.drivers.roboarm.kinematics import MjcfKinematics +from positronic.drivers.roboarm.models import DEFAULT_FRAME + +_YAM_MJCF = 'assets/mujoco/i2rt_yam/yam.xml' +_YAM_JOINTS = ('joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6') +_GOLDENS = Path(__file__).with_name('yam_kinematics_goldens.npz') + + +def _yam_reach_postures(x: float, y: float) -> list[np.ndarray]: + """The YAM's own warm starts, copied from its driver so the goldens meet the seeds that made them.""" + az = np.arctan2(y, x) + return [np.array([az, 1.8, 2.2, 0.0, -0.9, 0.0]), np.array([az, 1.2, 1.2, 0.0, 0.6, 0.0])] + + +@pytest.fixture(scope='module') +def yam() -> MjcfKinematics: + return MjcfKinematics(_YAM_MJCF, DEFAULT_FRAME, _YAM_JOINTS, _yam_reach_postures) + + +@pytest.fixture(scope='module') +def goldens() -> dict[str, np.ndarray]: + return dict(np.load(_GOLDENS)) + + +def _pose(row: np.ndarray) -> geom.Transform3D: + return geom.Transform3D(row[:3], geom.Rotation.from_quat(row[3:])) + + +def test_forward_kinematics_answer_what_the_yam_driver_answered(yam, goldens): + for q, want in zip(goldens['q'], goldens['fk'], strict=True): + got = yam.fk(q) + np.testing.assert_allclose(got.translation, want[:3], atol=1e-9) + assert geom.quat_closest(got.rotation, geom.Rotation.from_quat(want[3:])) == geom.Rotation.from_quat(want[3:]) + + +def test_inverse_kinematics_answer_what_the_yam_driver_answered(yam, goldens): + for target, seed, want, solved in zip( + goldens['ik_target'], goldens['ik_seed'], goldens['ik_q'], goldens['ik_solved'], strict=True + ): + got = yam.ik(_pose(target), seed) + if not solved: + assert got is None + continue + assert got is not None + np.testing.assert_allclose(got, want, atol=1e-9) + + +def test_a_capped_search_keeps_the_shape_the_arm_stands_in(yam, goldens): + """``max_jump`` is what a streamed setpoint is solved under: no seed but the live posture, and no + solution further from it than the cap.""" + target, seed = _pose(goldens['ik_target'][0]), goldens['ik_seed'][0] + + assert yam.ik(target, seed, max_jump=1e-6) is None + + got = yam.ik(target, seed, max_jump=1.0) + assert got is not None and np.all(np.abs(got - seed) <= 1.0) + + +def test_a_target_beyond_the_arm_is_refused_rather_than_approached(yam): + """The solver stops wherever it got to, so an unverified answer would be a pose the arm cannot hold.""" + assert yam.ik(geom.Transform3D(np.array([2.0, 0.0, 0.3])), np.zeros(6)) is None diff --git a/positronic/drivers/roboarm/tests/yam_kinematics_goldens.npz b/positronic/drivers/roboarm/tests/yam_kinematics_goldens.npz new file mode 100644 index 0000000000000000000000000000000000000000..f2c516728005fbabd3ca13b94a8e2e094fc822a1 GIT binary patch literal 4354 zcmeH~cTm(x7RM(M1PM#hMO_Bdun34KL2-FM&^3Sx9)<%IK|v6Z=qX5WR0LMha)31; z0s^8=830A(P1#a)JeN{cLrt9~9-TmwDd;MuU z8!AnoG-BGMSy$8>KVy?3sge8*eEfpRBxRB_W#kcwk6W{TwVeZ*w1*VJaCHmZ9>7>c zXDr@f#?YfPcK8PD4sh|=>KowdChlM6;vMKF>JRjA@pBXX*O_OiM>qLij~+(OBlIX`fH{BlwRag5f*NK zbqHmuZ+Uvo^#gdV&X;=hkxa0*1Wj4q_6Dj7x{Yiu_o1Bz0l^w=y>NfdN(-9WYp@$7 z%XKP6-$-OBEt2I9FNrQQu`VTk+`2R!pDsNXW%nV?CR91uI`T$v9SBblf}X4fcyAV_ zoZZI<{mkbSnrRF2Ka^s^a4tp0SIf`p3mc){PbxGw{SLZqAFZnxSpv+JQ(=A8HfW5! zrOQhY1f*3wyPH*QvSUu+f(kNXM0&Lu|4%D z*6UG+k|qmv7!GW4Ufv1mDODdKjSo=X_Je;h7o8><@GQuDRL#5cK6hg?Bs|bp zKA1k!OVeHa4>*pkR}$iu1`f^l^^;$sIR}P_q0(PrqOU3(Hu33$)AOQWU#TIIkWQ zW=My1u=!9{Va2AocfrQbtimwMdnmj(|L*DRC{$_uNo#sy6KYSGmAdwMF?=XHXP;wr z0J`NVCWq_pqAcU@vY0oTL5fw9WP6hj3#YyJmbQ5Tc^xeFYK>;N>FVTa#La;{ldW$2 zC|?a_9Yb+~`m;cyFE~;Cq5)z{GbSz?u0zw9#H5YctuS0;q@+jVq0u^zag_6y=v-c; z^POH>1Kr$QC1U4bu}l2;#BMb1BD_R-c-M+U6yn{21;-@!oGAQqT|adnbOY&7^TuGxdg}UF3)vE44zJD+XYZ4yY)XbOG1;Vd#;`4N!JS zoz=Rl2DPp-X(>NIBTfb+#nXZ&5QZ8)Ct6%&i1V(_SM#Pe!Srud&ei4?LO{zj$GkE= zQaV4!_<58N$z>mHY-RKVwcxhja4eNrHk9)d+hYiolvny_Sn(i3J<0KXULPc>ZB0Kr zqZVb_UJj{loItE{%jETiQivnfYppe-dr_p*B3bhX z6KWG{!7^3v`v+t)aj|`c!DsDZ=t-{4vCI%a7BBDMzK8+v-q79n`2dMiZ`STKF(zMB z3_l;!o=+v_wyU@LXEq~FuC*2OM{6k6@=R_t=!Ja6sg{3LrE)mOl=W)`6N#WFnboZs zFTr4^;xu~|0Xo_1JnAvE%y-;W!eUMtUCaF?W)>_~cjrF+SNBJtzS ztp5?+cVW73$8(|sGJd%$$R4b%N!Fx|hb8%_5KnC?9=-4A2BKaT01jp?3^>7I`1{xPQe zB24$@nC?w6-G_~#`!r1Vb1~hY!E~>J>D~y_{k}1D&&PD1i|L+)>3%z=`z%cNo|x{B zVY;vU9l9S;?$G`}%8yq5L7|JAMEUq&3H0^e^9xtS;Ea^`-;{sowt4^F<<>}VTvrG> zU5@AB%c!XjNGNh z#)wH&Tn)rcUJ@YkO!9uwsK4UUv4P@KTzo)_o5HBTP}whmzntk~L&X(JGN$O+uf` geom.Transform3D: - self._data.qpos[self._qpos_ids] = q - mj.mj_kinematics(self._model, self._data) - quat = np.empty(4) - mj.mju_mat2Quat(quat, self._data.site_xmat[self._site_id].copy()) - return geom.Transform3D(self._data.site_xpos[self._site_id].copy(), geom.Rotation.from_quat(quat)) - - def ik(self, target: geom.Transform3D, current_q: np.ndarray) -> np.ndarray | None: - """Multi-start LM IK: the live posture first, then the reach postures toward the target's azimuth. - Solutions are wrapped and clamped into joint range, then FK-verified before acceptance.""" - for start in (current_q, *_reach_postures(*target.translation[:2])): - self._data.qpos[:] = 0.0 - self._data.qpos[self._qpos_ids] = start - qpos, _, success = qpos_from_site_pose( - self._model, - self._data, - self._site_id, - self._dof_ids, - target.translation, - target.rotation.as_quat, - rot_weight=0.5, - ) - if not success: - continue - q = qpos[self._qpos_ids].copy() - # A revolute joint at q ± 2π is the same pose; wrap out-of-range entries back in when they fit. - q = np.where(q > self._upper, q - 2 * np.pi, q) - q = np.where(q < self._lower, q + 2 * np.pi, q) - q = np.clip(q, self._lower, self._upper) - reached = self.fk(q) - rot_err = (reached.rotation.inv * target.rotation).angle - rot_err = min(rot_err, 2 * np.pi - rot_err) - if np.linalg.norm(reached.translation - target.translation) < _IK_POS_TOL and rot_err < _IK_ROT_TOL: - return q - return None - - class _Chain(DriverRun[command.CommandType]): """The chain the driver drives: the vendor handle, and the state and moves that go with it.""" @@ -181,7 +123,7 @@ def __init__( self.grip_out = grip_out self.state = YamState() self._base_pose = base_pose - self._kin = _Kinematics() + self._kin = MjcfKinematics(_MJCF_PATH, DEFAULT_FRAME, _JOINT_NAMES, _reach_postures) def __enter__(self) -> '_Chain': return self @@ -461,7 +403,7 @@ def pump(seconds: float): pump(0.1) # the opening move ramps the chain to the park pose over a couple of seconds assert state.value.status == RobotStatus.AVAILABLE, state.value.status - kin = _Kinematics() + kin = MjcfKinematics(_MJCF_PATH, DEFAULT_FRAME, _JOINT_NAMES, _reach_postures) if fake is not None: # State round-trip: the parked chain comes back through the driver's FK. From 3828c2d1af2a3ff146393f8443e55c422f9239ae Mon Sep 17 00:00:00 2001 From: DarksaCY Date: Tue, 8 Sep 2026 11:07:37 +0300 Subject: [PATCH 2/3] Keep the MJCF kinematics out of the placo module `kinematics.py` imports `placo` at module scope, and only the `hardware` extra carries it. Putting the MJCF solver there gave the `yam` extra an undeclared dependency on another one: `uv run --locked --exact --extra yam` could no longer import the YAM driver at all. It has its own module now, and the two files say why they are two. The site is checked: `mj_name2id` answers -1 for a name the model does not carry, and indexing with it reads the model's last site, so the arm would solve for another frame. `reach_postures` takes the whole target rather than its x and y. Where a seed comes from is the arm's business, and the shared class has no reason to say it is planar. The tolerances are the class's own constants. --- positronic/drivers/roboarm/kinematics.py | 95 ---------------- positronic/drivers/roboarm/mjcf_kinematics.py | 106 ++++++++++++++++++ .../drivers/roboarm/tests/test_kinematics.py | 19 ++-- positronic/drivers/roboarm/yam.py | 12 +- 4 files changed, 122 insertions(+), 110 deletions(-) create mode 100644 positronic/drivers/roboarm/mjcf_kinematics.py diff --git a/positronic/drivers/roboarm/kinematics.py b/positronic/drivers/roboarm/kinematics.py index 79874986e..c09f44004 100644 --- a/positronic/drivers/roboarm/kinematics.py +++ b/positronic/drivers/roboarm/kinematics.py @@ -1,19 +1,7 @@ -"""Solving an arm's kinematics: the placo solver a URDF driver uses, and FK/IK on an MJCF model. - -An arm whose controller solves Cartesian goals itself does not need either. One whose controller does not, -or whose firmware solves them in a way the driver cannot accept, carries its own model and solves here. -""" - -from collections.abc import Callable, Sequence - -import mujoco as mj import numpy as np from positronic import geom from positronic.drivers import vendor_import -from positronic.utils import package_assets_path - -from .ik import qpos_from_site_pose with vendor_import('placo', 'Kinematics support'): import placo @@ -66,86 +54,3 @@ def inverse( @property def joint_limits(self) -> np.ndarray: return np.array([self.robot.get_joint_limits(joint_name) for joint_name in self.joint_names]) - - -_POS_TOL = 1e-3 # meters; FK-verify acceptance for an IK solution after wrapping and clamping -_ROT_TOL = 1e-2 # radians - - -class MjcfKinematics: - """FK and IK on an MJCF model at one site, in the model's own base frame. - - ``mujoco`` exports every symbol below from a compiled extension, so a type checker cannot see them. - - :param mjcf_path: the model, relative to the package assets. - :param site: the site FK reports and IK aims at. - :param joint_names: the joints, in the order a caller states them. - :param reach_postures: warm starts for a target at base-frame ``(x, y)``, tried after the live posture. - The arms this drives have a 6-DoF wrist, which gives LM no null space to escape a bad basin, so a - seed near the goal is what makes limit-clamped IK reliable. - """ - - def __init__( - self, - mjcf_path: str, - site: str, - joint_names: Sequence[str], - reach_postures: Callable[[float, float], list[np.ndarray]], - ): - self._model = mj.MjModel.from_xml_path(package_assets_path(mjcf_path)) - self._data = mj.MjData(self._model) - self._site_id = mj.mj_name2id(self._model, mj.mjtObj.mjOBJ_SITE, site) - self._qpos_ids = np.array([self._model.joint(name).qposadr.item() for name in joint_names]) - self._dof_ids = np.array([self._model.joint(name).dofadr.item() for name in joint_names]) - ranges = np.array([self._model.joint(name).range for name in joint_names]) - self.lower, self.upper = ranges[:, 0], ranges[:, 1] - self._reach_postures = reach_postures - - def fk(self, q: np.ndarray) -> geom.Transform3D: - self._data.qpos[self._qpos_ids] = q - mj.mj_kinematics(self._model, self._data) - quat = np.empty(4) - mj.mju_mat2Quat(quat, self._data.site_xmat[self._site_id].copy()) - return geom.Transform3D(self._data.site_xpos[self._site_id].copy(), geom.Rotation.from_quat(quat)) - - def ik( - self, target: geom.Transform3D, current_q: np.ndarray, max_jump: float | np.ndarray | None = None - ) -> np.ndarray | None: - """The joints that reach ``target``, warm-started from where the arm stands, or ``None``. - - ``max_jump`` bounds how far the solution may sit from ``current_q``, per joint or over all of them, - and the search stops at the live posture: the arm keeps the shape it has, and a pose it can reach - only in another one comes back as nothing. Without it the reach postures are tried too, so the arm - may change shape to get there -- which swings the end effector, and is for a move somebody waits on. - - A solution is wrapped and clamped into joint range and then FK-verified, so a target the arm cannot - reach comes back as nothing rather than as the nearest thing the solver stopped at. - """ - seeds = (current_q,) if max_jump is not None else (current_q, *self._reach_postures(*target.translation[:2])) - for start in seeds: - self._data.qpos[:] = 0.0 - self._data.qpos[self._qpos_ids] = start - qpos, _, success = qpos_from_site_pose( - self._model, - self._data, - self._site_id, - self._dof_ids, - target.translation, - target.rotation.as_quat, - rot_weight=0.5, - ) - if not success: - continue - q = qpos[self._qpos_ids].copy() - # A revolute joint at q ± 2π is the same pose; wrap out-of-range entries back in when they fit. - q = np.where(q > self.upper, q - 2 * np.pi, q) - q = np.where(q < self.lower, q + 2 * np.pi, q) - q = np.clip(q, self.lower, self.upper) - if max_jump is not None and np.any(np.abs(q - current_q) > max_jump): - continue - reached = self.fk(q) - rot_err = (reached.rotation.inv * target.rotation).angle - rot_err = min(rot_err, 2 * np.pi - rot_err) - if np.linalg.norm(reached.translation - target.translation) < _POS_TOL and rot_err < _ROT_TOL: - return q - return None diff --git a/positronic/drivers/roboarm/mjcf_kinematics.py b/positronic/drivers/roboarm/mjcf_kinematics.py new file mode 100644 index 000000000..65794f8b1 --- /dev/null +++ b/positronic/drivers/roboarm/mjcf_kinematics.py @@ -0,0 +1,106 @@ +"""FK and IK on an MJCF model, for a driver that solves for its own arm. + +An arm whose controller solves Cartesian goals itself does not need this. One whose controller does not, +or whose firmware solves them in a way the driver cannot accept, carries its own model and solves here. + +Its own module rather than beside the placo ``Kinematics``: that one imports ``placo``, which only the +``hardware`` extra carries, and a driver that solves against an MJCF must not need it. +""" + +from collections.abc import Callable, Sequence + +import mujoco as mj +import numpy as np + +from positronic import geom +from positronic.utils import package_assets_path + +from .ik import qpos_from_site_pose + + +class MjcfKinematics: + """FK and IK on an MJCF model at one site, in the model's own base frame. + + ``mujoco`` exports every symbol below from a compiled extension, so a type checker cannot see them. + + :param mjcf_path: the model, relative to the package assets. + :param site: the site FK reports and IK aims at. + :param joint_names: the joints, in the order a caller states them. + :param reach_postures: warm starts for a target, tried after the live posture. The arms this drives + have a 6-DoF wrist, which gives LM no null space to escape a bad basin, so a seed near the goal is + what makes limit-clamped IK reliable. + """ + + # FK-verify acceptance for a solution, after wrapping and clamping it into joint range + _POS_TOL = 1e-3 # meters + _ROT_TOL = 1e-2 # radians + + def __init__( + self, + mjcf_path: str, + site: str, + joint_names: Sequence[str], + reach_postures: Callable[[geom.Transform3D], list[np.ndarray]], + ): + # rules-allow: primitive-type — `package_assets_path(relative_path: str) -> str` owns the join, and + # every caller spells the model this way + self._model = mj.MjModel.from_xml_path(package_assets_path(mjcf_path)) + self._data = mj.MjData(self._model) + self._site_id = mj.mj_name2id(self._model, mj.mjtObj.mjOBJ_SITE, site) + if self._site_id < 0: + # Indexing with -1 reads the model's last site, so the arm would solve for another frame. + raise ValueError(f'{mjcf_path} names no site {site!r}') + self._qpos_ids = np.array([self._model.joint(name).qposadr.item() for name in joint_names]) + self._dof_ids = np.array([self._model.joint(name).dofadr.item() for name in joint_names]) + ranges = np.array([self._model.joint(name).range for name in joint_names]) + self.lower, self.upper = ranges[:, 0], ranges[:, 1] + self._reach_postures = reach_postures + + def fk(self, q: np.ndarray) -> geom.Transform3D: + self._data.qpos[self._qpos_ids] = q + mj.mj_kinematics(self._model, self._data) + quat = np.empty(4) + mj.mju_mat2Quat(quat, self._data.site_xmat[self._site_id].copy()) + return geom.Transform3D(self._data.site_xpos[self._site_id].copy(), geom.Rotation.from_quat(quat)) + + def ik( + self, target: geom.Transform3D, current_q: np.ndarray, max_jump: float | np.ndarray | None = None + ) -> np.ndarray | None: + """The joints that reach ``target``, warm-started from where the arm stands, or ``None``. + + ``max_jump`` bounds how far the solution may sit from ``current_q``, per joint or over all of them, + and the search stops at the live posture: the arm keeps the shape it has, and a pose it can reach + only in another one comes back as nothing. Without it the reach postures are tried too, so the arm + may change shape to get there -- which swings the end effector, and is for a move somebody waits on. + + A solution is wrapped and clamped into joint range and then FK-verified, so a target the arm cannot + reach comes back as nothing rather than as the nearest thing the solver stopped at. + """ + seeds = (current_q,) if max_jump is not None else (current_q, *self._reach_postures(target)) + for start in seeds: + self._data.qpos[:] = 0.0 + self._data.qpos[self._qpos_ids] = start + qpos, _, success = qpos_from_site_pose( + self._model, + self._data, + self._site_id, + self._dof_ids, + target.translation, + target.rotation.as_quat, + rot_weight=0.5, + ) + if not success: + continue + q = qpos[self._qpos_ids].copy() + # A revolute joint at q ± 2π is the same pose; wrap out-of-range entries back in when they fit. + q = np.where(q > self.upper, q - 2 * np.pi, q) + q = np.where(q < self.lower, q + 2 * np.pi, q) + q = np.clip(q, self.lower, self.upper) + if max_jump is not None and np.any(np.abs(q - current_q) > max_jump): + continue + reached = self.fk(q) + rot_err = (reached.rotation.inv * target.rotation).angle + rot_err = min(rot_err, 2 * np.pi - rot_err) + if np.linalg.norm(reached.translation - target.translation) < self._POS_TOL and rot_err < self._ROT_TOL: + return q + return None diff --git a/positronic/drivers/roboarm/tests/test_kinematics.py b/positronic/drivers/roboarm/tests/test_kinematics.py index f7c89673b..4249ca00c 100644 --- a/positronic/drivers/roboarm/tests/test_kinematics.py +++ b/positronic/drivers/roboarm/tests/test_kinematics.py @@ -1,9 +1,9 @@ -"""What ``MjcfKinematics`` answers, pinned to what the YAM driver answered before it shared the class. +"""What ``MjcfKinematics`` answers on the YAM model, held to a fixed corpus. -``yam_kinematics_goldens.npz`` was generated from the YAM's own ``_Kinematics`` at `cab986af`: ten random -joint vectors inside the model's range, the pose each puts ``DEFAULT_FRAME`` at, the joints IK finds for -that pose from a seed 0.15 rad away, and two targets outside the arm's reach. Regenerating it is a change -of behaviour, not of test data. +``yam_kinematics_goldens.npz`` holds ten joint vectors inside the model's range, the pose each puts +``DEFAULT_FRAME`` at, the joints IK finds for that pose from a seed 0.15 rad away, and two targets outside +the arm's reach. The numbers are the answers, not a sample of them: a run that disagrees with the file has +changed what the class solves, so regenerating it is a change of behaviour rather than of test data. """ from pathlib import Path @@ -12,7 +12,7 @@ import pytest from positronic import geom -from positronic.drivers.roboarm.kinematics import MjcfKinematics +from positronic.drivers.roboarm.mjcf_kinematics import MjcfKinematics from positronic.drivers.roboarm.models import DEFAULT_FRAME _YAM_MJCF = 'assets/mujoco/i2rt_yam/yam.xml' @@ -20,9 +20,10 @@ _GOLDENS = Path(__file__).with_name('yam_kinematics_goldens.npz') -def _yam_reach_postures(x: float, y: float) -> list[np.ndarray]: - """The YAM's own warm starts, copied from its driver so the goldens meet the seeds that made them.""" - az = np.arctan2(y, x) +def _yam_reach_postures(target: geom.Transform3D) -> list[np.ndarray]: + """The YAM's warm starts: joint1 at the target's azimuth, elbow folded down at two heights. The corpus + is answers from these seeds, so it is read with them.""" + az = np.arctan2(target.translation[1], target.translation[0]) return [np.array([az, 1.8, 2.2, 0.0, -0.9, 0.0]), np.array([az, 1.2, 1.2, 0.0, 0.6, 0.0])] diff --git a/positronic/drivers/roboarm/yam.py b/positronic/drivers/roboarm/yam.py index ab6efce5d..94c57a18d 100644 --- a/positronic/drivers/roboarm/yam.py +++ b/positronic/drivers/roboarm/yam.py @@ -27,7 +27,7 @@ from positronic.drivers.utils import DriverRun, MoveAbandoned, MoveStatus, log_failure from . import RobotStatus, State, command -from .kinematics import MjcfKinematics +from .mjcf_kinematics import MjcfKinematics from .models import DEFAULT_FRAME # i2rt lives in the `yam` extra, which the type-check environment does not install. @@ -47,11 +47,11 @@ _JOINT_POS, _JOINT_VEL, _GRIPPER_POS = 'joint_pos', 'joint_vel', 'gripper_pos' -def _reach_postures(x: float, y: float) -> list[np.ndarray]: - """IK warm-start candidates for reaching toward arm-base-frame point (x, y): joint1 swung to the target's - azimuth, elbow folded down at two heights. The 6-DoF wrist gives LM no null space to escape bad basins, - so seeding near the goal is what makes limit-clamped IK reliable.""" - az = np.arctan2(y, x) +def _reach_postures(target: geom.Transform3D) -> list[np.ndarray]: + """IK warm-start candidates for reaching toward ``target``: joint1 swung to its azimuth, elbow folded + down at two heights. The 6-DoF wrist gives LM no null space to escape bad basins, so seeding near the + goal is what makes limit-clamped IK reliable.""" + az = np.arctan2(target.translation[1], target.translation[0]) return [np.array([az, 1.8, 2.2, 0.0, -0.9, 0.0]), np.array([az, 1.2, 1.2, 0.0, 0.6, 0.0])] From 2f718f77258b2a51d82f61220ae158e23321e096 Mon Sep 17 00:00:00 2001 From: DarksaCY Date: Tue, 8 Sep 2026 11:28:01 +0300 Subject: [PATCH 3/3] Take any iterable of reach postures, and drop the caller narrative `reach_postures` is expanded once, so a tuple or a generator serves as well as a list. The docstring also said what an uncapped search is used for rather than what it does. --- positronic/drivers/roboarm/mjcf_kinematics.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/positronic/drivers/roboarm/mjcf_kinematics.py b/positronic/drivers/roboarm/mjcf_kinematics.py index 65794f8b1..8aef4018c 100644 --- a/positronic/drivers/roboarm/mjcf_kinematics.py +++ b/positronic/drivers/roboarm/mjcf_kinematics.py @@ -7,7 +7,7 @@ ``hardware`` extra carries, and a driver that solves against an MJCF must not need it. """ -from collections.abc import Callable, Sequence +from collections.abc import Callable, Iterable, Sequence import mujoco as mj import numpy as np @@ -40,7 +40,7 @@ def __init__( mjcf_path: str, site: str, joint_names: Sequence[str], - reach_postures: Callable[[geom.Transform3D], list[np.ndarray]], + reach_postures: Callable[[geom.Transform3D], Iterable[np.ndarray]], ): # rules-allow: primitive-type — `package_assets_path(relative_path: str) -> str` owns the join, and # every caller spells the model this way @@ -71,7 +71,7 @@ def ik( ``max_jump`` bounds how far the solution may sit from ``current_q``, per joint or over all of them, and the search stops at the live posture: the arm keeps the shape it has, and a pose it can reach only in another one comes back as nothing. Without it the reach postures are tried too, so the arm - may change shape to get there -- which swings the end effector, and is for a move somebody waits on. + may change shape to get there. A solution is wrapped and clamped into joint range and then FK-verified, so a target the arm cannot reach comes back as nothing rather than as the nearest thing the solver stopped at.