Add Placo IK solver and associated tests.
This commit is contained in:
@@ -0,0 +1,453 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
|
||||
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||
<robot
|
||||
name="RM75-B">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00049987 5.2709E-05 0.060019"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.862" />
|
||||
<inertia
|
||||
ixx="0.0017232"
|
||||
ixy="-3.1058E-06"
|
||||
ixz="-3.7924E-05"
|
||||
iyy="0.0017051"
|
||||
iyz="1.3691E-06"
|
||||
izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000241 -0.013273 -0.00995"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.574" />
|
||||
<inertia
|
||||
ixx="0.002487573"
|
||||
ixy="0.000009663"
|
||||
ixz="-0.000007909"
|
||||
iyy="0.002321038"
|
||||
iyz="0.000179393"
|
||||
izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="link_1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000357 -0.106789 0.005329"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.217" />
|
||||
<inertia
|
||||
ixx="0.003494121"
|
||||
ixy="0.000002921"
|
||||
ixz="-0.000005613"
|
||||
iyy="0.000892721"
|
||||
iyz="-0.000583884"
|
||||
izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_1" />
|
||||
<child
|
||||
link="link_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.2689"
|
||||
upper="2.2689"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000003 -0.01398 -0.011324"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.11" />
|
||||
<inertia
|
||||
ixx="0.001836663"
|
||||
ixy="0.000002259"
|
||||
ixz="-0.000004216"
|
||||
iyy="0.001498875"
|
||||
iyz="0.000037167"
|
||||
izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.256 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_2" />
|
||||
<child
|
||||
link="link_3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000005 -0.084658 0.004747"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.685" />
|
||||
<inertia
|
||||
ixx="0.001282444"
|
||||
ixy="-0.000000551"
|
||||
ixz="-0.000000630"
|
||||
iyy="0.000373013"
|
||||
iyz="-0.000232084"
|
||||
izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_3" />
|
||||
<child
|
||||
link="link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.356"
|
||||
upper="2.356"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000078 -0.012937 -0.008781"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.619" />
|
||||
<inertia
|
||||
ixx="0.000627336"
|
||||
ixy="0.000001636"
|
||||
ixz="-0.000001345"
|
||||
iyy="0.000542455"
|
||||
iyz="0.000034970"
|
||||
izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_4" />
|
||||
<child
|
||||
link="link_5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000014 -0.078524 0.002819"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.602" />
|
||||
<inertia
|
||||
ixx="0.000780774"
|
||||
ixy="-0.000000121"
|
||||
ixz="-0.000000469"
|
||||
iyy="0.000289973"
|
||||
iyz="-0.000120513"
|
||||
izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_5" />
|
||||
<child
|
||||
link="link_6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.234"
|
||||
upper="2.234"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_7">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001094 -0.000077 -0.010119"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.107" />
|
||||
<inertia
|
||||
ixx="0.000044123"
|
||||
ixy="-0.000000064"
|
||||
ixz="0.0000003"
|
||||
iyy="0.000035078"
|
||||
iyz="-0.000000029"
|
||||
izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_7"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_6" />
|
||||
<child
|
||||
link="link_7" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
</robot>
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
+11
-1
@@ -1,8 +1,10 @@
|
||||
"""xr_rm_teleop 包安装配置。
|
||||
|
||||
该包提供基于 XR 相对位姿的 RM75 笛卡尔位姿透传遥操作节点。
|
||||
该包提供基于 XR 相对位姿和 Placo QP 的 RM75 遥操作节点。
|
||||
"""
|
||||
|
||||
from glob import glob
|
||||
|
||||
from setuptools import setup
|
||||
|
||||
package_name = "xr_rm_teleop"
|
||||
@@ -14,6 +16,14 @@ setup(
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
|
||||
(f"share/{package_name}", ["package.xml"]),
|
||||
(
|
||||
f"share/{package_name}/models/rm75",
|
||||
["models/rm75/RM75-B.urdf"],
|
||||
),
|
||||
(
|
||||
f"share/{package_name}/models/rm75/meshes",
|
||||
glob("models/rm75/meshes/*.STL"),
|
||||
),
|
||||
],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from xr_rm_teleop.placo_ik_solver import PlacoIkSolver
|
||||
from xr_rm_teleop.realman_adapter import ArmPose
|
||||
|
||||
|
||||
CASES = {
|
||||
"left": (
|
||||
[-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
|
||||
[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],
|
||||
),
|
||||
"right": (
|
||||
[-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04],
|
||||
[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],
|
||||
),
|
||||
}
|
||||
|
||||
|
||||
def angle_error(actual: list[float], target: list[float]) -> float:
|
||||
deltas = [
|
||||
math.atan2(math.sin(a - b), math.cos(a - b))
|
||||
for a, b in zip(actual, target)
|
||||
]
|
||||
return math.sqrt(sum(value * value for value in deltas))
|
||||
|
||||
|
||||
def main() -> None:
|
||||
urdf_path = Path(sys.argv[1]).resolve()
|
||||
for arm, (joint_degrees, tool_pose) in CASES.items():
|
||||
solver = PlacoIkSolver(str(urdf_path), tool_pose, 1.0 / 90.0)
|
||||
joints = np.deg2rad(joint_degrees).tolist()
|
||||
current = solver.update_joint_state(joints)
|
||||
target = ArmPose(
|
||||
current.x + 0.01,
|
||||
current.y,
|
||||
current.z,
|
||||
current.rx,
|
||||
current.ry,
|
||||
current.rz + 0.05,
|
||||
)
|
||||
|
||||
solve_durations = []
|
||||
for _ in range(45):
|
||||
solver.update_joint_state(joints)
|
||||
started_at = time.perf_counter()
|
||||
joints = solver.solve(target)
|
||||
solve_durations.append(time.perf_counter() - started_at)
|
||||
|
||||
actual = solver.update_joint_state(joints)
|
||||
position_error = np.linalg.norm(
|
||||
np.asarray(actual.xyz()) - np.asarray(target.xyz())
|
||||
)
|
||||
orientation_error = angle_error(actual.rpy(), target.rpy())
|
||||
assert len(joints) == 7
|
||||
assert np.isfinite(joints).all()
|
||||
assert np.allclose(
|
||||
solver.base_configuration,
|
||||
[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
|
||||
)
|
||||
assert position_error <= 0.005
|
||||
assert orientation_error <= math.radians(2.0)
|
||||
print(
|
||||
f"{arm}: position_error={position_error:.6f}m, "
|
||||
f"orientation_error={math.degrees(orientation_error):.3f}deg, "
|
||||
f"solve_avg={1000.0 * np.mean(solve_durations):.3f}ms, "
|
||||
f"solve_max={1000.0 * max(solve_durations):.3f}ms, "
|
||||
f"solve_overruns={sum(value > 1.0 / 90.0 for value in solve_durations)}"
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,4 +1,10 @@
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from xr_rm_teleop.realman_adapter import RealManAdapter
|
||||
from xr_rm_teleop.realman_adapter import MockRealManAdapter
|
||||
from xr_rm_teleop.fun_peripheral import PeripheralConfig
|
||||
|
||||
|
||||
def test_initial_pose_uses_joint_move_only() -> None:
|
||||
@@ -17,3 +23,66 @@ def test_initial_pose_uses_joint_move_only() -> None:
|
||||
adapter._move_to_initial_pose()
|
||||
|
||||
assert adapter._arm.calls == [(joints, 20, 0, 0, 1)]
|
||||
|
||||
|
||||
def test_peripheral_config_exposes_selected_tool() -> None:
|
||||
config = PeripheralConfig(
|
||||
scissorgripper=1,
|
||||
tools_in_ee={
|
||||
"first": [[0.0] * 7, [0.0] * 7],
|
||||
"second": [[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0], [0.0] * 7],
|
||||
},
|
||||
)
|
||||
|
||||
assert config.tool_name == "second"
|
||||
assert config.tool_pose == [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0]
|
||||
|
||||
|
||||
def test_joint_feedback_is_cached_in_radians() -> None:
|
||||
class FakeArm:
|
||||
def rm_get_joint_degree(self):
|
||||
return 0, [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
|
||||
|
||||
adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
|
||||
adapter._arm = FakeArm()
|
||||
|
||||
adapter._read_joint_state_once()
|
||||
snapshot = adapter.get_latest_joint_state()
|
||||
|
||||
assert snapshot is not None
|
||||
assert snapshot.positions == pytest.approx(
|
||||
[math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
|
||||
)
|
||||
|
||||
|
||||
def test_joint_target_uses_movej_canfd_in_degrees() -> None:
|
||||
class FakeArm:
|
||||
def __init__(self) -> None:
|
||||
self.calls = []
|
||||
|
||||
def rm_movej_canfd(self, *args):
|
||||
self.calls.append(args)
|
||||
return 0
|
||||
|
||||
adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
|
||||
adapter._arm = FakeArm()
|
||||
target = [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
|
||||
|
||||
adapter.send_joint_target(target, follow=False)
|
||||
|
||||
assert len(adapter._arm.calls) == 1
|
||||
degrees, follow, expand, trajectory_mode, radio = adapter._arm.calls[0]
|
||||
assert degrees == pytest.approx([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
|
||||
assert (follow, expand, trajectory_mode, radio) == (False, 0, 2, 0)
|
||||
|
||||
|
||||
def test_mock_joint_feedback_is_available_without_vendor_sdk() -> None:
|
||||
adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
|
||||
|
||||
adapter.connect()
|
||||
snapshot = adapter.get_latest_joint_state()
|
||||
|
||||
assert snapshot is not None
|
||||
assert snapshot.positions == pytest.approx(
|
||||
[math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
|
||||
)
|
||||
|
||||
@@ -0,0 +1,164 @@
|
||||
import time
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
|
||||
from xr_rm_teleop.single_arm_velocity_teleop import SingleArmVelocityTeleop
|
||||
|
||||
|
||||
class FakeLogger:
|
||||
def warn(self, *args, **kwargs):
|
||||
del args, kwargs
|
||||
|
||||
def error(self, *args, **kwargs):
|
||||
del args, kwargs
|
||||
|
||||
|
||||
class FakeTime:
|
||||
def __sub__(self, other):
|
||||
del other
|
||||
return SimpleNamespace(nanoseconds=0)
|
||||
|
||||
|
||||
def test_missing_or_stale_feedback_does_not_enable_qp() -> None:
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._command_timeout_sec = 0.12
|
||||
teleop._adapter = SimpleNamespace(get_latest_joint_state=lambda: None)
|
||||
|
||||
assert teleop._fresh_joint_state() is None
|
||||
|
||||
teleop._adapter = SimpleNamespace(
|
||||
get_latest_joint_state=lambda: JointStateSnapshot(
|
||||
[0.0] * 7,
|
||||
time.monotonic() - 1.0,
|
||||
)
|
||||
)
|
||||
assert teleop._fresh_joint_state() is None
|
||||
|
||||
|
||||
def test_stale_feedback_stops_before_active_control() -> None:
|
||||
stopped = []
|
||||
entered = []
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._adapter = SimpleNamespace(
|
||||
get_latest_joint_state=lambda: JointStateSnapshot(
|
||||
[0.0] * 7,
|
||||
time.monotonic() - 1.0,
|
||||
)
|
||||
)
|
||||
teleop._command_timeout_sec = 0.12
|
||||
teleop._joint_feedback_ready = True
|
||||
teleop._arm_name = "right_rm75"
|
||||
teleop._last_msg = SimpleNamespace(
|
||||
grip=True,
|
||||
pose=SimpleNamespace(
|
||||
position=SimpleNamespace(x=0.0, y=0.0, z=0.0),
|
||||
orientation=SimpleNamespace(x=0.0, y=0.0, z=0.0, w=1.0),
|
||||
),
|
||||
)
|
||||
teleop._last_msg_time = FakeTime()
|
||||
teleop._active = False
|
||||
teleop._enable_orientation_control = False
|
||||
teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
|
||||
teleop.get_logger = lambda: FakeLogger()
|
||||
teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
|
||||
teleop._enter_active_control = lambda *args: entered.append(args)
|
||||
|
||||
teleop._control_tick()
|
||||
|
||||
assert stopped == [True]
|
||||
assert entered == []
|
||||
|
||||
|
||||
def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
|
||||
class FakeSolver:
|
||||
def __init__(self) -> None:
|
||||
self.solve_calls = 0
|
||||
|
||||
def update_joint_state(self, joints):
|
||||
assert joints == [0.1] * 7
|
||||
return ArmPose(0.3, 0.0, 0.2)
|
||||
|
||||
def solve(self, target):
|
||||
del target
|
||||
self.solve_calls += 1
|
||||
return [0.2] * 7
|
||||
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._ik_solver = FakeSolver()
|
||||
teleop._active = False
|
||||
teleop._last_valid_joint_target = None
|
||||
teleop._last_current_pose = None
|
||||
|
||||
pose = teleop._sync_joint_feedback(
|
||||
JointStateSnapshot([0.1] * 7, time.monotonic())
|
||||
)
|
||||
|
||||
assert pose == ArmPose(0.3, 0.0, 0.2)
|
||||
assert teleop._last_valid_joint_target == [0.1] * 7
|
||||
assert teleop._ik_solver.solve_calls == 0
|
||||
|
||||
|
||||
def test_qp_failure_returns_last_known_good_target() -> None:
|
||||
class FailingSolver:
|
||||
def solve(self, target):
|
||||
del target
|
||||
raise RuntimeError("NaN in QP solution")
|
||||
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._ik_solver = FailingSolver()
|
||||
teleop._last_valid_joint_target = [0.1] * 7
|
||||
teleop._arm_name = "right_rm75"
|
||||
teleop.get_logger = lambda: FakeLogger()
|
||||
|
||||
target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
|
||||
|
||||
assert target == pytest.approx([0.1] * 7)
|
||||
assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
|
||||
|
||||
|
||||
def test_qp_success_updates_last_known_good_target() -> None:
|
||||
class SuccessfulSolver:
|
||||
def solve(self, target):
|
||||
del target
|
||||
return [0.2] * 7
|
||||
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._ik_solver = SuccessfulSolver()
|
||||
teleop._last_valid_joint_target = [0.1] * 7
|
||||
teleop._arm_name = "left_rm75"
|
||||
teleop.get_logger = lambda: FakeLogger()
|
||||
|
||||
target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
|
||||
|
||||
assert target == pytest.approx([0.2] * 7)
|
||||
assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
|
||||
|
||||
|
||||
def test_joint_send_failure_requests_slow_stop_and_resets_control() -> None:
|
||||
class FailingAdapter:
|
||||
def __init__(self) -> None:
|
||||
self.stop_calls = 0
|
||||
|
||||
def send_joint_target(self, joints, follow):
|
||||
del joints, follow
|
||||
raise RuntimeError("send failed")
|
||||
|
||||
def stop(self):
|
||||
self.stop_calls += 1
|
||||
|
||||
reset_calls = []
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._adapter = FailingAdapter()
|
||||
teleop._follow = False
|
||||
teleop._arm_name = "left_rm75"
|
||||
teleop._stop_sent = False
|
||||
teleop.get_logger = lambda: FakeLogger()
|
||||
teleop._safe_stop = lambda reset_active: reset_calls.append(reset_active)
|
||||
|
||||
sent = teleop._send_joint_target([0.1] * 7)
|
||||
|
||||
assert not sent
|
||||
assert teleop._adapter.stop_calls == 1
|
||||
assert reset_calls == [True]
|
||||
@@ -1,9 +1,10 @@
|
||||
import math
|
||||
import time
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from xr_rm_teleop.realman_adapter import ArmPose, MockRealManAdapter
|
||||
from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
|
||||
from xr_rm_teleop.single_arm_velocity_teleop import (
|
||||
SingleArmVelocityTeleop,
|
||||
_euler_to_quaternion,
|
||||
@@ -95,6 +96,19 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
|
||||
teleop._arm_name = "test_rm75"
|
||||
teleop._command_timeout_sec = 0.12
|
||||
teleop._enable_orientation_control = True
|
||||
teleop._adapter = SimpleNamespace(
|
||||
get_latest_joint_state=lambda: JointStateSnapshot(
|
||||
[0.1] * 7,
|
||||
time.monotonic(),
|
||||
)
|
||||
)
|
||||
teleop._ik_solver = SimpleNamespace(
|
||||
update_joint_state=lambda joints: ArmPose(0.3, 0.0, 0.2)
|
||||
)
|
||||
teleop._active = False
|
||||
teleop._last_valid_joint_target = None
|
||||
teleop._last_current_pose = None
|
||||
teleop._joint_feedback_ready = True
|
||||
stopped = []
|
||||
teleop.get_clock = lambda: FakeClock()
|
||||
teleop.get_logger = lambda: FakeLogger()
|
||||
@@ -113,12 +127,3 @@ def test_quaternion_roundtrip_for_small_rpy() -> None:
|
||||
def test_zero_quaternion_is_invalid() -> None:
|
||||
with pytest.raises(ValueError):
|
||||
_normalize_quaternion([0.0, 0.0, 0.0, 0.0])
|
||||
|
||||
|
||||
def test_mock_adapter_uses_shortest_angular_velocity() -> None:
|
||||
adapter = MockRealManAdapter([0.0, 0.0, 0.0, 3.13, 0.0, -3.13], 0.1)
|
||||
|
||||
adapter.send_cartesian_target(ArmPose(0.0, 0.0, 0.0, -3.13, 0.0, 3.13), False)
|
||||
|
||||
assert abs(adapter.last_velocity[3]) < 1.0
|
||||
assert abs(adapter.last_velocity[5]) < 1.0
|
||||
|
||||
@@ -0,0 +1,49 @@
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from xr_rm_teleop.placo_ik_solver import (
|
||||
PlacoIkSolver,
|
||||
_arm_pose_to_transform,
|
||||
_tool_pose_to_transform,
|
||||
_transform_to_arm_pose,
|
||||
)
|
||||
from xr_rm_teleop.realman_adapter import ArmPose
|
||||
|
||||
|
||||
def test_tool_offset_rotates_with_flange_and_roundtrips() -> None:
|
||||
flange_pose = ArmPose(0.30, -0.10, 0.20, 0.0, math.pi / 2.0, 0.0)
|
||||
tool_pose = [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
|
||||
|
||||
base_to_flange = _arm_pose_to_transform(flange_pose)
|
||||
flange_to_tool = _tool_pose_to_transform(tool_pose)
|
||||
base_to_tool = base_to_flange @ flange_to_tool
|
||||
recovered_flange = base_to_tool @ np.linalg.inv(flange_to_tool)
|
||||
|
||||
assert base_to_tool[:3, 3] == pytest.approx([0.49, -0.10, 0.20])
|
||||
assert recovered_flange == pytest.approx(base_to_flange)
|
||||
|
||||
|
||||
def test_transform_to_arm_pose_roundtrip() -> None:
|
||||
expected = ArmPose(0.25, -0.30, 0.40, 0.20, -0.30, 0.40)
|
||||
|
||||
actual = _transform_to_arm_pose(_arm_pose_to_transform(expected))
|
||||
|
||||
assert actual.xyz() == pytest.approx(expected.xyz())
|
||||
assert actual.rpy() == pytest.approx(expected.rpy())
|
||||
|
||||
|
||||
def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
|
||||
solver = object.__new__(PlacoIkSolver)
|
||||
solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
|
||||
solver._velocity_limits = np.ones(7)
|
||||
solver._dt = 0.1
|
||||
solver._actual_joints = np.zeros(7)
|
||||
|
||||
with pytest.raises(ValueError, match="finite"):
|
||||
solver._validate_result(np.full(7, np.nan))
|
||||
with pytest.raises(ValueError, match="position"):
|
||||
solver._validate_result(np.full(7, 2.0))
|
||||
with pytest.raises(ValueError, match="velocity"):
|
||||
solver._validate_result(np.full(7, 0.2))
|
||||
@@ -25,6 +25,14 @@ class PeripheralConfig:
|
||||
tools_in_ee: dict[str, list[list[float]]]
|
||||
set_initial_tool_state: bool = False
|
||||
|
||||
@property
|
||||
def tool_name(self) -> str:
|
||||
return list(self.tools_in_ee)[self.scissorgripper]
|
||||
|
||||
@property
|
||||
def tool_pose(self) -> list[float]:
|
||||
return list(self.tools_in_ee[self.tool_name][0])
|
||||
|
||||
|
||||
def load_peripheral_config(config_file: str, arm: str) -> PeripheralConfig:
|
||||
"""从 bringup YAML 读取指定左右臂的外设配置。"""
|
||||
|
||||
@@ -0,0 +1,209 @@
|
||||
"""RM75 的 Placo 0.9.4 单步 QP 逆解。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from importlib.metadata import PackageNotFoundError, version
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .realman_adapter import ArmPose
|
||||
|
||||
|
||||
EXPECTED_PLACO_VERSION = "0.9.4"
|
||||
RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)]
|
||||
RM75_Q_SLICE = slice(7, 14)
|
||||
|
||||
|
||||
def _rpy_to_rotation(roll: float, pitch: float, yaw: float) -> np.ndarray:
|
||||
cr, sr = math.cos(roll), math.sin(roll)
|
||||
cp, sp = math.cos(pitch), math.sin(pitch)
|
||||
cy, sy = math.cos(yaw), math.sin(yaw)
|
||||
return np.array(
|
||||
[
|
||||
[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
|
||||
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
|
||||
[-sp, cp * sr, cp * cr],
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
|
||||
|
||||
def _rotation_to_rpy(rotation: np.ndarray) -> tuple[float, float, float]:
|
||||
pitch = math.asin(-float(np.clip(rotation[2, 0], -1.0, 1.0)))
|
||||
if abs(math.cos(pitch)) > 1e-9:
|
||||
roll = math.atan2(float(rotation[2, 1]), float(rotation[2, 2]))
|
||||
yaw = math.atan2(float(rotation[1, 0]), float(rotation[0, 0]))
|
||||
else:
|
||||
roll = math.atan2(-float(rotation[1, 2]), float(rotation[1, 1]))
|
||||
yaw = 0.0
|
||||
return roll, pitch, yaw
|
||||
|
||||
|
||||
def _arm_pose_to_transform(pose: ArmPose) -> np.ndarray:
|
||||
transform = np.eye(4)
|
||||
transform[:3, :3] = _rpy_to_rotation(pose.rx, pose.ry, pose.rz)
|
||||
transform[:3, 3] = pose.xyz()
|
||||
return transform
|
||||
|
||||
|
||||
def _tool_pose_to_transform(tool_pose: list[float]) -> np.ndarray:
|
||||
values = np.asarray(tool_pose, dtype=float)
|
||||
if values.shape != (7,) or not np.isfinite(values).all():
|
||||
raise ValueError("tool pose must contain 7 finite values")
|
||||
x, y, z, qx, qy, qz, qw = values
|
||||
norm = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)
|
||||
if norm <= 1e-9:
|
||||
raise ValueError("tool quaternion norm must be positive")
|
||||
qx, qy, qz, qw = qx / norm, qy / norm, qz / norm, qw / norm
|
||||
transform = np.eye(4)
|
||||
transform[:3, :3] = np.array(
|
||||
[
|
||||
[
|
||||
1 - 2 * (qy * qy + qz * qz),
|
||||
2 * (qx * qy - qz * qw),
|
||||
2 * (qx * qz + qy * qw),
|
||||
],
|
||||
[
|
||||
2 * (qx * qy + qz * qw),
|
||||
1 - 2 * (qx * qx + qz * qz),
|
||||
2 * (qy * qz - qx * qw),
|
||||
],
|
||||
[
|
||||
2 * (qx * qz - qy * qw),
|
||||
2 * (qy * qz + qx * qw),
|
||||
1 - 2 * (qx * qx + qy * qy),
|
||||
],
|
||||
]
|
||||
)
|
||||
transform[:3, 3] = [x, y, z]
|
||||
return transform
|
||||
|
||||
|
||||
def _transform_to_arm_pose(transform: np.ndarray) -> ArmPose:
|
||||
roll, pitch, yaw = _rotation_to_rpy(transform[:3, :3])
|
||||
return ArmPose(
|
||||
float(transform[0, 3]),
|
||||
float(transform[1, 3]),
|
||||
float(transform[2, 3]),
|
||||
roll,
|
||||
pitch,
|
||||
yaw,
|
||||
)
|
||||
|
||||
|
||||
class PlacoIkSolver:
|
||||
def __init__(
|
||||
self,
|
||||
urdf_path: str,
|
||||
tool_pose: list[float],
|
||||
dt: float,
|
||||
) -> None:
|
||||
if dt <= 0.0:
|
||||
raise ValueError("dt must be positive")
|
||||
try:
|
||||
installed_version = version("placo")
|
||||
import placo
|
||||
except (ImportError, PackageNotFoundError) as exc:
|
||||
raise RuntimeError(
|
||||
"Placo 0.9.4 must come from "
|
||||
"/home/robot/miniconda3/envs/xr"
|
||||
) from exc
|
||||
if installed_version != EXPECTED_PLACO_VERSION:
|
||||
raise RuntimeError(
|
||||
f"Placo {EXPECTED_PLACO_VERSION} is required, got {installed_version}"
|
||||
)
|
||||
|
||||
model_path = Path(urdf_path).expanduser().resolve()
|
||||
if not model_path.is_file():
|
||||
raise FileNotFoundError(f"RM75 URDF not found: {model_path}")
|
||||
|
||||
self._dt = dt
|
||||
self._robot = placo.RobotWrapper(str(model_path))
|
||||
if self._robot.state.q.shape != (14,):
|
||||
raise RuntimeError(
|
||||
f"expected Placo q shape (14,), got {self._robot.state.q.shape}"
|
||||
)
|
||||
if list(self._robot.joint_names()) != RM75_JOINT_NAMES:
|
||||
raise RuntimeError(
|
||||
f"unexpected RM75 joint order: {list(self._robot.joint_names())}"
|
||||
)
|
||||
offsets = [
|
||||
self._robot.get_joint_offset(name) for name in RM75_JOINT_NAMES
|
||||
]
|
||||
if offsets != list(range(7, 14)):
|
||||
raise RuntimeError(f"unexpected RM75 q offsets: {offsets}")
|
||||
|
||||
self._joint_limits = np.asarray(
|
||||
[self._robot.get_joint_limits(name) for name in RM75_JOINT_NAMES]
|
||||
)
|
||||
velocity_offsets = [
|
||||
self._robot.get_joint_v_offset(name) for name in RM75_JOINT_NAMES
|
||||
]
|
||||
self._velocity_limits = np.asarray(
|
||||
[
|
||||
self._robot.model.velocityLimit[index]
|
||||
for index in velocity_offsets
|
||||
]
|
||||
)
|
||||
self._tool_transform = _tool_pose_to_transform(tool_pose)
|
||||
self._tool_inverse = np.linalg.inv(self._tool_transform)
|
||||
self._actual_joints: np.ndarray | None = None
|
||||
|
||||
self._solver = placo.KinematicsSolver(self._robot)
|
||||
self._solver.dt = dt
|
||||
self._solver.mask_fbase(True)
|
||||
self._solver.enable_velocity_limits(True)
|
||||
self._frame_task = self._solver.add_frame_task("link_7", np.eye(4))
|
||||
self._frame_task.configure("rm75_frame", "soft", 1.0)
|
||||
manipulability = self._solver.add_manipulability_task(
|
||||
"link_7",
|
||||
"both",
|
||||
1.0,
|
||||
)
|
||||
manipulability.configure("rm75_manipulability", "soft", 5e-2)
|
||||
self._solver.add_kinetic_energy_regularization_task(1e-6)
|
||||
|
||||
@property
|
||||
def base_configuration(self) -> list[float]:
|
||||
return self._robot.state.q[:7].tolist()
|
||||
|
||||
def update_joint_state(self, joints: list[float]) -> ArmPose:
|
||||
values = np.asarray(joints, dtype=float)
|
||||
if values.shape != (7,) or not np.isfinite(values).all():
|
||||
raise ValueError("joint state must contain 7 finite values")
|
||||
is_first_feedback = self._actual_joints is None
|
||||
self._actual_joints = values.copy()
|
||||
self._robot.state.q[RM75_Q_SLICE] = values
|
||||
self._robot.update_kinematics()
|
||||
base_to_flange = self._robot.get_T_world_frame("link_7")
|
||||
if is_first_feedback:
|
||||
self._frame_task.T_world_frame = base_to_flange.copy()
|
||||
base_to_tool = base_to_flange @ self._tool_transform
|
||||
return _transform_to_arm_pose(base_to_tool)
|
||||
|
||||
def solve(self, target_tool_pose: ArmPose) -> list[float]:
|
||||
if self._actual_joints is None:
|
||||
raise RuntimeError("joint state must be initialized before QP solve")
|
||||
self._frame_task.T_world_frame = (
|
||||
_arm_pose_to_transform(target_tool_pose) @ self._tool_inverse
|
||||
)
|
||||
self._solver.solve(True)
|
||||
result = np.asarray(
|
||||
self._robot.state.q[RM75_Q_SLICE],
|
||||
dtype=float,
|
||||
).copy()
|
||||
self._validate_result(result)
|
||||
return result.tolist()
|
||||
|
||||
def _validate_result(self, result: np.ndarray) -> None:
|
||||
if result.shape != (7,) or not np.isfinite(result).all():
|
||||
raise ValueError("QP result must contain 7 finite values")
|
||||
lower = self._joint_limits[:, 0]
|
||||
upper = self._joint_limits[:, 1]
|
||||
if np.any(result < lower - 1e-9) or np.any(result > upper + 1e-9):
|
||||
raise ValueError("QP result violates RM75 joint position limits")
|
||||
max_step = self._velocity_limits * self._dt + 1e-9
|
||||
if np.any(np.abs(result - self._actual_joints) > max_step):
|
||||
raise ValueError("QP result violates RM75 one-cycle velocity limits")
|
||||
@@ -1,21 +1,15 @@
|
||||
"""RM75 机械臂适配层。
|
||||
|
||||
对上提供统一的当前位姿读取、笛卡尔位姿目标发送和停止接口；对下根据配置
|
||||
选择 mock 积分模拟器或睿尔曼 Python API2 真机通信。
|
||||
"""
|
||||
"""RM75 机械臂关节反馈、关节透传和停止适配层。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import threading
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from numbers import Number
|
||||
from typing import Any
|
||||
|
||||
|
||||
def _angle_delta(target: float, current: float) -> float:
|
||||
return math.atan2(math.sin(target - current), math.cos(target - current))
|
||||
|
||||
|
||||
@dataclass
|
||||
class ArmPose:
|
||||
x: float
|
||||
@@ -32,55 +26,64 @@ class ArmPose:
|
||||
return [self.rx, self.ry, self.rz]
|
||||
|
||||
|
||||
class MockRealManAdapter:
|
||||
"""无机械臂时使用的运动学模拟器，用于验证 ROS2 遥操链路。"""
|
||||
@dataclass(frozen=True)
|
||||
class JointStateSnapshot:
|
||||
positions: list[float]
|
||||
received_at: float
|
||||
|
||||
def __init__(self, initial_pose: list[float], dt: float) -> None:
|
||||
self._pose = ArmPose(*initial_pose[:6])
|
||||
self._dt = dt
|
||||
self.last_velocity = [0.0] * 6
|
||||
|
||||
class MockRealManAdapter:
|
||||
"""不导入厂商 SDK 的关节状态 mock。"""
|
||||
|
||||
def __init__(self, initial_joint_degrees: list[float]) -> None:
|
||||
if len(initial_joint_degrees) != 7 or not all(
|
||||
math.isfinite(value) for value in initial_joint_degrees
|
||||
):
|
||||
raise ValueError("initial joint pose must contain 7 finite values")
|
||||
self._joint_positions = [
|
||||
math.radians(value) for value in initial_joint_degrees
|
||||
]
|
||||
self.last_joint_target: list[float] | None = None
|
||||
self.last_tool_open: bool | None = None
|
||||
|
||||
def connect(self) -> None:
|
||||
return
|
||||
|
||||
def get_current_pose(self) -> ArmPose:
|
||||
return self._pose
|
||||
def get_latest_joint_state(self) -> JointStateSnapshot:
|
||||
return JointStateSnapshot(
|
||||
list(self._joint_positions),
|
||||
time.monotonic(),
|
||||
)
|
||||
|
||||
def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
|
||||
def send_joint_target(self, joints: list[float], follow: bool) -> None:
|
||||
del follow
|
||||
self.last_velocity = [
|
||||
(pose.x - self._pose.x) / self._dt,
|
||||
(pose.y - self._pose.y) / self._dt,
|
||||
(pose.z - self._pose.z) / self._dt,
|
||||
_angle_delta(pose.rx, self._pose.rx) / self._dt,
|
||||
_angle_delta(pose.ry, self._pose.ry) / self._dt,
|
||||
_angle_delta(pose.rz, self._pose.rz) / self._dt,
|
||||
]
|
||||
self._pose = pose
|
||||
if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
|
||||
raise ValueError("joint target must contain 7 finite values")
|
||||
self._joint_positions = list(joints)
|
||||
self.last_joint_target = list(joints)
|
||||
|
||||
def stop(self) -> None:
|
||||
self.last_velocity = [0.0] * 6
|
||||
return
|
||||
|
||||
def close(self) -> None:
|
||||
self.stop()
|
||||
|
||||
def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
|
||||
del config_file, peripheral_arm
|
||||
def configure_peripheral(self, config: Any, peripheral_arm: str) -> None:
|
||||
del config, peripheral_arm
|
||||
|
||||
def set_tool_enabled(self, open_tool: bool) -> None:
|
||||
self.last_tool_open = open_tool
|
||||
|
||||
|
||||
class RealManAdapter:
|
||||
"""睿尔曼 Python API2 的笛卡尔位姿透传适配层。"""
|
||||
"""复用一个睿尔曼 Python API2 连接的关节适配层。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
robot_ip: str,
|
||||
robot_port: int,
|
||||
avoid_singularity: int,
|
||||
frame_type: int,
|
||||
feedback_period: float,
|
||||
logger: Any | None = None,
|
||||
configure_safety_limits: bool = True,
|
||||
max_line_speed: float = 1.0,
|
||||
@@ -98,7 +101,9 @@ class RealManAdapter:
|
||||
self._robot_ip = robot_ip
|
||||
self._robot_port = robot_port
|
||||
self._avoid_singularity = avoid_singularity
|
||||
self._frame_type = frame_type
|
||||
if feedback_period <= 0.0:
|
||||
raise ValueError("feedback_period must be positive")
|
||||
self._feedback_period = feedback_period
|
||||
self._logger = logger
|
||||
self._configure_safety_limits = configure_safety_limits
|
||||
self._max_line_speed = max_line_speed
|
||||
@@ -114,6 +119,11 @@ class RealManAdapter:
|
||||
self._canfd_radio = canfd_radio
|
||||
self._scissorgripper: int | None = None
|
||||
self._arm: Any | None = None
|
||||
self._joint_state_lock = threading.Lock()
|
||||
self._latest_joint_state: JointStateSnapshot | None = None
|
||||
self._feedback_stop = threading.Event()
|
||||
self._feedback_thread: threading.Thread | None = None
|
||||
self._feedback_fault_logged = False
|
||||
|
||||
def connect(self) -> None:
|
||||
try:
|
||||
@@ -130,38 +140,48 @@ class RealManAdapter:
|
||||
"RealMan connected: "
|
||||
f"ip={self._robot_ip}, port={self._robot_port}, "
|
||||
f"avoid_singularity={self._avoid_singularity}, "
|
||||
f"frame_type={self._frame_type}, command=rm_movep_canfd"
|
||||
"command=rm_movej_canfd"
|
||||
)
|
||||
if self._configure_safety_limits:
|
||||
self._apply_safety_limits()
|
||||
if self._move_to_initial_pose_on_connect:
|
||||
self._move_to_initial_pose()
|
||||
self._feedback_stop.clear()
|
||||
self._feedback_thread = threading.Thread(
|
||||
target=self._feedback_loop,
|
||||
name=f"rm75_feedback_{self._robot_ip}",
|
||||
daemon=True,
|
||||
)
|
||||
self._feedback_thread.start()
|
||||
|
||||
def get_current_pose(self) -> ArmPose:
|
||||
self._require_arm()
|
||||
state = self._arm.rm_get_current_arm_state()
|
||||
pose = self._find_pose(state)
|
||||
if pose is None:
|
||||
raise RuntimeError(f"无法从睿尔曼状态中解析当前 TCP 位姿：{state!r}")
|
||||
return ArmPose(*pose[:6])
|
||||
def get_latest_joint_state(self) -> JointStateSnapshot | None:
|
||||
with self._joint_state_lock:
|
||||
if self._latest_joint_state is None:
|
||||
return None
|
||||
return JointStateSnapshot(
|
||||
list(self._latest_joint_state.positions),
|
||||
self._latest_joint_state.received_at,
|
||||
)
|
||||
|
||||
def send_cartesian_target(self, pose: ArmPose, follow: bool) -> None:
|
||||
def send_joint_target(self, joints: list[float], follow: bool) -> None:
|
||||
self._require_arm()
|
||||
ret = self._arm.rm_movep_canfd(
|
||||
[pose.x, pose.y, pose.z, pose.rx, pose.ry, pose.rz],
|
||||
if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
|
||||
raise ValueError("joint target must contain 7 finite values")
|
||||
ret = self._arm.rm_movej_canfd(
|
||||
[math.degrees(value) for value in joints],
|
||||
follow,
|
||||
0,
|
||||
self._canfd_trajectory_mode,
|
||||
self._canfd_radio,
|
||||
)
|
||||
self._check_return(ret, "rm_movep_canfd")
|
||||
self._check_return(ret, "rm_movej_canfd")
|
||||
|
||||
def configure_peripheral(self, config_file: str, peripheral_arm: str) -> None:
|
||||
def configure_peripheral(self, config: Any, peripheral_arm: str) -> None:
|
||||
self._require_arm()
|
||||
from .fun_peripheral import load_peripheral_config, peripheral_cfg
|
||||
from .fun_peripheral import peripheral_cfg
|
||||
|
||||
config = load_peripheral_config(config_file, peripheral_arm)
|
||||
self._scissorgripper = config.scissorgripper
|
||||
tool_name = list(config.tools_in_ee.keys())[config.scissorgripper]
|
||||
tool_name = config.tool_name
|
||||
self._log_info(
|
||||
"开始配置 RealMan 末端外设："
|
||||
f"arm={peripheral_arm}, scissorgripper={config.scissorgripper}, "
|
||||
@@ -196,6 +216,12 @@ class RealManAdapter:
|
||||
if self._arm is None:
|
||||
return
|
||||
self.stop()
|
||||
self._feedback_stop.set()
|
||||
if self._feedback_thread is not None:
|
||||
self._feedback_thread.join(timeout=3.0)
|
||||
if self._feedback_thread.is_alive():
|
||||
self._log_warn("RealMan 关节反馈线程未在 3 秒内退出。")
|
||||
self._feedback_thread = None
|
||||
try:
|
||||
self._arm.rm_delete_robot_arm()
|
||||
finally:
|
||||
@@ -205,6 +231,37 @@ class RealManAdapter:
|
||||
if self._arm is None:
|
||||
raise RuntimeError("睿尔曼机械臂尚未连接")
|
||||
|
||||
def _feedback_loop(self) -> None:
|
||||
while not self._feedback_stop.is_set():
|
||||
try:
|
||||
self._read_joint_state_once()
|
||||
self._feedback_fault_logged = False
|
||||
except Exception as exc:
|
||||
if not self._feedback_fault_logged:
|
||||
self._log_warn(f"RealMan 关节反馈读取失败：{exc}")
|
||||
self._feedback_fault_logged = True
|
||||
self._feedback_stop.wait(self._feedback_period)
|
||||
|
||||
def _read_joint_state_once(self) -> None:
|
||||
self._require_arm()
|
||||
result = self._arm.rm_get_joint_degree()
|
||||
self._check_return(result, "rm_get_joint_degree")
|
||||
if not isinstance(result, tuple) or len(result) < 2:
|
||||
raise RuntimeError(f"rm_get_joint_degree 返回格式错误：{result!r}")
|
||||
degrees = result[1]
|
||||
if (
|
||||
not isinstance(degrees, (list, tuple))
|
||||
or len(degrees) != 7
|
||||
or not all(isinstance(value, Number) for value in degrees)
|
||||
):
|
||||
raise RuntimeError(f"RM75 关节反馈必须包含 7 个数值：{degrees!r}")
|
||||
positions = [math.radians(float(value)) for value in degrees]
|
||||
if not all(math.isfinite(value) for value in positions):
|
||||
raise RuntimeError("RM75 关节反馈包含 NaN/Inf")
|
||||
snapshot = JointStateSnapshot(positions, time.monotonic())
|
||||
with self._joint_state_lock:
|
||||
self._latest_joint_state = snapshot
|
||||
|
||||
def _apply_safety_limits(self) -> None:
|
||||
# 真机安全限幅尽量下发到控制器；不支持的 SDK 接口会在 _try_call 中降级为警告。
|
||||
self._try_call("rm_set_avoid_singularity_mode", int(self._avoid_singularity))
|
||||
@@ -260,74 +317,3 @@ class RealManAdapter:
|
||||
@staticmethod
|
||||
def _return_code(ret: Any) -> Any:
|
||||
return ret[0] if isinstance(ret, tuple) and ret else ret
|
||||
|
||||
@classmethod
|
||||
def _find_pose(cls, obj: Any) -> list[float] | None:
|
||||
# 不同 SDK 版本返回字段可能略有差异，因此递归查找常见 TCP 位姿字段。
|
||||
if isinstance(obj, dict):
|
||||
for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
|
||||
pose = cls._as_pose(obj.get(key))
|
||||
if pose is not None:
|
||||
return pose
|
||||
for value in obj.values():
|
||||
pose = cls._find_pose(value)
|
||||
if pose is not None:
|
||||
return pose
|
||||
elif isinstance(obj, (list, tuple)):
|
||||
pose = cls._as_pose(obj)
|
||||
if pose is not None:
|
||||
return pose
|
||||
for value in obj:
|
||||
pose = cls._find_pose(value)
|
||||
if pose is not None:
|
||||
return pose
|
||||
elif hasattr(obj, "to_dictionary"):
|
||||
try:
|
||||
return cls._find_pose(obj.to_dictionary(7))
|
||||
except TypeError:
|
||||
return cls._find_pose(obj.to_dictionary())
|
||||
elif hasattr(obj, "to_dict"):
|
||||
return cls._find_pose(obj.to_dict())
|
||||
else:
|
||||
for key in ("pose", "tool_pose", "tcp_pose", "current_pose"):
|
||||
if hasattr(obj, key):
|
||||
pose = cls._as_pose(getattr(obj, key))
|
||||
if pose is not None:
|
||||
return pose
|
||||
return None
|
||||
|
||||
@staticmethod
|
||||
def _as_pose(value: Any) -> list[float] | None:
|
||||
if isinstance(value, (list, tuple)) and len(value) >= 6:
|
||||
if all(isinstance(item, Number) for item in value[:6]):
|
||||
return [float(item) for item in value[:6]]
|
||||
if isinstance(value, dict):
|
||||
position = value.get("position")
|
||||
euler = value.get("euler")
|
||||
if isinstance(position, dict) and isinstance(euler, dict):
|
||||
keys = ("x", "y", "z")
|
||||
rpy_keys = ("rx", "ry", "rz")
|
||||
if all(key in position for key in keys) and all(key in euler for key in rpy_keys):
|
||||
return [
|
||||
float(position["x"]),
|
||||
float(position["y"]),
|
||||
float(position["z"]),
|
||||
float(euler["rx"]),
|
||||
float(euler["ry"]),
|
||||
float(euler["rz"]),
|
||||
]
|
||||
if all(hasattr(value, attr) for attr in ("position", "euler")):
|
||||
position = getattr(value, "position")
|
||||
euler = getattr(value, "euler")
|
||||
if all(hasattr(position, key) for key in ("x", "y", "z")) and all(
|
||||
hasattr(euler, key) for key in ("rx", "ry", "rz")
|
||||
):
|
||||
return [
|
||||
float(position.x),
|
||||
float(position.y),
|
||||
float(position.z),
|
||||
float(euler.rx),
|
||||
float(euler.ry),
|
||||
float(euler.rz),
|
||||
]
|
||||
return None
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
"""RM75 单臂 XR 相对位姿透传遥操作节点。
|
||||
"""RM75 单臂 XR 相对位姿 QP 遥操作节点。
|
||||
|
||||
节点订阅左/右手柄位姿，在 grip 按下时锁定手柄和 TCP 起点，把手柄相对位姿
|
||||
映射成机器人坐标系中的目标 TCP，并通过 rm_movep_canfd 持续下发目标位姿。
|
||||
映射成机器人坐标系中的目标 TCP，再通过 Placo 和 rm_movej_canfd 下发关节目标。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
@@ -9,6 +9,7 @@ from __future__ import annotations
|
||||
import math
|
||||
import queue
|
||||
import threading
|
||||
import time
|
||||
from typing import Iterable
|
||||
|
||||
import rclpy
|
||||
@@ -19,7 +20,14 @@ from std_msgs.msg import Bool
|
||||
|
||||
from xr_rm_interfaces.msg import XrController
|
||||
|
||||
from .realman_adapter import ArmPose, MockRealManAdapter, RealManAdapter
|
||||
from .fun_peripheral import load_peripheral_config
|
||||
from .placo_ik_solver import PlacoIkSolver
|
||||
from .realman_adapter import (
|
||||
ArmPose,
|
||||
JointStateSnapshot,
|
||||
MockRealManAdapter,
|
||||
RealManAdapter,
|
||||
)
|
||||
|
||||
|
||||
def _norm(values: Iterable[float]) -> float:
|
||||
@@ -176,13 +184,11 @@ class SingleArmVelocityTeleop(Node):
|
||||
self.declare_parameter("low_z_threshold", 0.20)
|
||||
self.declare_parameter("low_z_min_radius", 0.21)
|
||||
self.declare_parameter("xr_to_robot_matrix", [0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0])
|
||||
self.declare_parameter("current_pose_poll_hz", 10.0)
|
||||
self.declare_parameter("use_mock", True)
|
||||
self.declare_parameter("mock_initial_pose", [0.35, 0.0, 0.30, 0.0, 0.0, 0.0])
|
||||
self.declare_parameter("robot_urdf_path", "")
|
||||
self.declare_parameter("robot_ip", "192.168.1.18")
|
||||
self.declare_parameter("robot_port", 8080)
|
||||
self.declare_parameter("avoid_singularity", 1)
|
||||
self.declare_parameter("frame_type", 1)
|
||||
self.declare_parameter("follow", False)
|
||||
self.declare_parameter("configure_safety_limits", True)
|
||||
self.declare_parameter("max_line_speed", 1.0)
|
||||
@@ -232,7 +238,6 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._low_z_threshold = float(self.get_parameter("low_z_threshold").value)
|
||||
self._low_z_min_radius = float(self.get_parameter("low_z_min_radius").value)
|
||||
self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
|
||||
self._current_pose_poll_hz = float(self.get_parameter("current_pose_poll_hz").value)
|
||||
self._follow = self._bool_parameter("follow")
|
||||
self._enable_tool_control = self._bool_parameter("enable_tool_control")
|
||||
self._enable_trigger_gripper_control = self._bool_parameter("enable_trigger_gripper_control")
|
||||
@@ -254,7 +259,8 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._last_sent_orientation: list[float] | None = None
|
||||
self._last_command_time: Time | None = None
|
||||
self._last_current_pose: ArmPose | None = None
|
||||
self._last_current_pose_time: Time | None = None
|
||||
self._last_valid_joint_target: list[float] | None = None
|
||||
self._joint_feedback_ready = False
|
||||
self._stop_sent = True
|
||||
self._trigger_tool_open = True
|
||||
self._last_trigger_pressed: bool | None = None
|
||||
@@ -262,6 +268,17 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._tool_worker_stop = threading.Event()
|
||||
self._tool_worker_thread: threading.Thread | None = None
|
||||
|
||||
peripheral_arm = self._peripheral_arm_name()
|
||||
config_file = str(self.get_parameter("peripheral_config_file").value)
|
||||
self._peripheral_config = load_peripheral_config(
|
||||
config_file,
|
||||
peripheral_arm,
|
||||
)
|
||||
self._ik_solver = PlacoIkSolver(
|
||||
str(self.get_parameter("robot_urdf_path").value),
|
||||
self._peripheral_config.tool_pose,
|
||||
self._dt,
|
||||
)
|
||||
self._adapter = self._make_adapter()
|
||||
self._adapter.connect()
|
||||
self._setup_tool_control()
|
||||
@@ -276,24 +293,24 @@ class SingleArmVelocityTeleop(Node):
|
||||
self.create_subscription(XrController, topic, self._on_controller, 10)
|
||||
self.create_timer(self._dt, self._control_tick)
|
||||
self.get_logger().info(
|
||||
f"{self._arm_name} 位姿透传遥操节点已启动，监听话题：{topic}, "
|
||||
f"{self._arm_name} Placo QP 遥操节点已启动，监听话题：{topic}, "
|
||||
f"dt={self._dt:.4f}s, follow={self._follow}, "
|
||||
f"orientation_control={self._enable_orientation_control}"
|
||||
)
|
||||
|
||||
def _make_adapter(self):
|
||||
# mock 和真机共享同一位姿目标链路，只在适配层切换执行方式。
|
||||
initial_joint_pose = self._float_list_parameter(
|
||||
"initial_joint_pose",
|
||||
7,
|
||||
)
|
||||
if self._bool_parameter("use_mock"):
|
||||
return MockRealManAdapter(
|
||||
[float(v) for v in self.get_parameter("mock_initial_pose").value],
|
||||
self._dt,
|
||||
)
|
||||
return MockRealManAdapter(initial_joint_pose)
|
||||
|
||||
return RealManAdapter(
|
||||
robot_ip=self.get_parameter("robot_ip").value,
|
||||
robot_port=int(self.get_parameter("robot_port").value),
|
||||
avoid_singularity=int(self.get_parameter("avoid_singularity").value),
|
||||
frame_type=int(self.get_parameter("frame_type").value),
|
||||
feedback_period=self._dt,
|
||||
logger=self.get_logger(),
|
||||
configure_safety_limits=self._bool_parameter("configure_safety_limits"),
|
||||
max_line_speed=float(self.get_parameter("max_line_speed").value),
|
||||
@@ -303,13 +320,20 @@ class SingleArmVelocityTeleop(Node):
|
||||
joint_max_speed=float(self.get_parameter("joint_max_speed").value),
|
||||
joint_max_acc=float(self.get_parameter("joint_max_acc").value),
|
||||
move_to_initial_pose_on_connect=self._bool_parameter("move_to_initial_pose_on_connect"),
|
||||
initial_joint_pose=self._float_list_parameter("initial_joint_pose", 7),
|
||||
initial_joint_pose=initial_joint_pose,
|
||||
init_move_speed=int(self.get_parameter("init_move_speed").value),
|
||||
canfd_trajectory_mode=int(self.get_parameter("canfd_trajectory_mode").value),
|
||||
canfd_radio=int(self.get_parameter("canfd_radio").value),
|
||||
)
|
||||
|
||||
def _setup_tool_control(self) -> None:
|
||||
peripheral_arm = self._peripheral_arm_name()
|
||||
if self._bool_parameter("configure_peripheral_on_connect"):
|
||||
self._adapter.configure_peripheral(
|
||||
self._peripheral_config,
|
||||
peripheral_arm,
|
||||
)
|
||||
|
||||
if not self._enable_tool_control:
|
||||
if self._enable_trigger_gripper_control:
|
||||
self.get_logger().warn(
|
||||
@@ -317,11 +341,6 @@ class SingleArmVelocityTeleop(Node):
|
||||
)
|
||||
return
|
||||
|
||||
peripheral_arm = self._peripheral_arm_name()
|
||||
config_file = str(self.get_parameter("peripheral_config_file").value)
|
||||
if self._bool_parameter("configure_peripheral_on_connect"):
|
||||
self._adapter.configure_peripheral(config_file, peripheral_arm)
|
||||
|
||||
self._start_tool_worker()
|
||||
|
||||
topic = str(self.get_parameter("tool_command_topic").value).strip()
|
||||
@@ -436,6 +455,32 @@ class SingleArmVelocityTeleop(Node):
|
||||
|
||||
def _control_tick(self) -> None:
|
||||
now = self.get_clock().now()
|
||||
snapshot = self._fresh_joint_state()
|
||||
if snapshot is None:
|
||||
if self._joint_feedback_ready:
|
||||
self.get_logger().warn(
|
||||
f"{self._arm_name} 关节反馈缺失或过期，机械臂停止。",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
self._joint_feedback_ready = False
|
||||
self._safe_stop(reset_active=True)
|
||||
return
|
||||
try:
|
||||
current_pose = self._sync_joint_feedback(snapshot)
|
||||
except Exception as exc:
|
||||
self.get_logger().error(
|
||||
f"{self._arm_name} 关节反馈同步到 Placo 失败：{exc}",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
self._joint_feedback_ready = False
|
||||
self._safe_stop(reset_active=True)
|
||||
return
|
||||
if not self._joint_feedback_ready:
|
||||
self.get_logger().info(
|
||||
f"{self._arm_name} 已收到首帧有效关节反馈，QP 可以启用。"
|
||||
)
|
||||
self._joint_feedback_ready = True
|
||||
|
||||
if self._last_msg is None or self._last_msg_time is None:
|
||||
self._safe_stop(reset_active=True)
|
||||
return
|
||||
@@ -469,13 +514,17 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._safe_stop(reset_active=True)
|
||||
return
|
||||
if not self._active:
|
||||
self._enter_active_control(controller_now, controller_quat, now)
|
||||
self._enter_active_control(
|
||||
controller_now,
|
||||
controller_quat,
|
||||
current_pose,
|
||||
now,
|
||||
)
|
||||
return
|
||||
|
||||
assert self._controller_start is not None
|
||||
assert self._robot_start_pose is not None
|
||||
|
||||
self._maybe_refresh_current_pose(now)
|
||||
raw_target_xyz = self._raw_target_from_controller(controller_now)
|
||||
raw_target_rpy = self._raw_orientation_from_controller(controller_quat)
|
||||
workspace_target, workspace_clamped = self._clamp_workspace_with_flag(raw_target_xyz)
|
||||
@@ -507,7 +556,8 @@ class SingleArmVelocityTeleop(Node):
|
||||
)
|
||||
|
||||
self._publish_debug(raw_target_pose, target_pose, velocity, target_clamped)
|
||||
if self._send_cartesian_target(target_pose):
|
||||
joint_target = self._solve_joint_target(target_pose)
|
||||
if self._send_joint_target(joint_target):
|
||||
self._last_sent_target = sent_target
|
||||
self._last_sent_orientation = sent_orientation
|
||||
self._last_command_time = now
|
||||
@@ -517,18 +567,9 @@ class SingleArmVelocityTeleop(Node):
|
||||
self,
|
||||
controller_now: list[float],
|
||||
controller_quat: tuple[float, float, float, float],
|
||||
robot_pose: ArmPose,
|
||||
now: Time,
|
||||
) -> None:
|
||||
try:
|
||||
robot_pose = self._read_current_pose_for_control(now)
|
||||
except Exception as exc:
|
||||
self.get_logger().error(
|
||||
f"{self._arm_name} 读取 TCP 位姿失败，停止输出：{exc}",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
self._safe_stop(reset_active=True)
|
||||
return
|
||||
|
||||
robot_xyz = robot_pose.xyz()
|
||||
self._active = True
|
||||
self._controller_start = controller_now
|
||||
@@ -755,27 +796,45 @@ class SingleArmVelocityTeleop(Node):
|
||||
for i in range(3)
|
||||
]
|
||||
|
||||
def _read_current_pose_for_control(self, now: Time) -> ArmPose:
|
||||
pose = self._adapter.get_current_pose()
|
||||
self._last_current_pose = pose
|
||||
self._last_current_pose_time = now
|
||||
return pose
|
||||
def _fresh_joint_state(self) -> JointStateSnapshot | None:
|
||||
snapshot = self._adapter.get_latest_joint_state()
|
||||
if snapshot is None:
|
||||
return None
|
||||
age = time.monotonic() - snapshot.received_at
|
||||
if age < 0.0 or age > self._command_timeout_sec:
|
||||
return None
|
||||
if (
|
||||
len(snapshot.positions) != 7
|
||||
or not all(math.isfinite(value) for value in snapshot.positions)
|
||||
):
|
||||
return None
|
||||
return snapshot
|
||||
|
||||
def _maybe_refresh_current_pose(self, now: Time) -> None:
|
||||
if self._current_pose_poll_hz <= 0.0:
|
||||
return
|
||||
if self._last_current_pose_time is not None:
|
||||
age = (now - self._last_current_pose_time).nanoseconds * 1e-9
|
||||
if age < 1.0 / self._current_pose_poll_hz:
|
||||
return
|
||||
def _sync_joint_feedback(
|
||||
self,
|
||||
snapshot: JointStateSnapshot,
|
||||
) -> ArmPose:
|
||||
current_pose = self._ik_solver.update_joint_state(
|
||||
snapshot.positions
|
||||
)
|
||||
self._last_current_pose = current_pose
|
||||
if not self._active or self._last_valid_joint_target is None:
|
||||
self._last_valid_joint_target = list(snapshot.positions)
|
||||
return current_pose
|
||||
|
||||
def _solve_joint_target(self, target_pose: ArmPose) -> list[float]:
|
||||
if self._last_valid_joint_target is None:
|
||||
raise RuntimeError("valid joint feedback has not been initialized")
|
||||
try:
|
||||
self._read_current_pose_for_control(now)
|
||||
result = self._ik_solver.solve(target_pose)
|
||||
except Exception as exc:
|
||||
self.get_logger().warn(
|
||||
f"{self._arm_name} 低频读取 TCP 位姿失败，继续透传目标：{exc}",
|
||||
f"{self._arm_name} QP 求解失败，保持上一组关节目标：{exc}",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
return list(self._last_valid_joint_target)
|
||||
self._last_valid_joint_target = list(result)
|
||||
return list(result)
|
||||
|
||||
def _safe_stop(self, reset_active: bool) -> None:
|
||||
if not self._stop_sent:
|
||||
@@ -818,16 +877,16 @@ class SingleArmVelocityTeleop(Node):
|
||||
return ArmPose(*self._last_sent_target, *rpy)
|
||||
return None
|
||||
|
||||
def _send_cartesian_target(self, pose: ArmPose) -> bool:
|
||||
def _send_joint_target(self, joints: list[float]) -> bool:
|
||||
try:
|
||||
self._adapter.send_cartesian_target(pose, self._follow)
|
||||
self._adapter.send_joint_target(joints, self._follow)
|
||||
except Exception as exc:
|
||||
self.get_logger().error(
|
||||
f"{self._arm_name} 发送位姿透传命令失败：{exc}",
|
||||
f"{self._arm_name} 发送关节透传命令失败：{exc}",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
self._active = False
|
||||
self._send_stop_once()
|
||||
self._safe_stop(reset_active=True)
|
||||
return False
|
||||
return True
|
||||
|
||||
@@ -917,8 +976,6 @@ class SingleArmVelocityTeleop(Node):
|
||||
raise ValueError("target_filter_fast_threshold_m must be >= 0")
|
||||
if self._max_linear_speed <= 0.0:
|
||||
raise ValueError("max_linear_speed must be > 0")
|
||||
if self._current_pose_poll_hz < 0.0:
|
||||
raise ValueError("current_pose_poll_hz must be >= 0")
|
||||
if self._orientation_deadband_rad < 0.0:
|
||||
raise ValueError("orientation_deadband_rad must be >= 0")
|
||||
if not 0.0 <= self._orientation_filter_alpha <= 1.0:
|
||||
|
||||
Reference in New Issue
Block a user