YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: 发布双臂关节状态与目标

Browse Source
This commit is contained in:
YikaiFu-cart
2026-08-04 13:31:52 +08:00
parent 631e3ee11c
commit 9a00898be3
4 changed files with 127 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xr_rm_teleop/package.xml
+1
View File
@@ -11,6 +11,7 @@
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>python3-yaml</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>xr_rm_interfaces</exec_depend>
xr_rm_teleop/test/test_joint_control.py
+94 -1
View File
@@ -4,6 +4,7 @@ from types import SimpleNamespace
import numpy as np
import pytest
from builtin_interfaces.msg import Time as TimeMsg
from xr_rm_teleop.realman_adapter import JointStateSnapshot
from xr_rm_teleop.single_arm_velocity_teleop import (
@@ -29,6 +30,80 @@ class FakeTime:
del other
return SimpleNamespace(nanoseconds=0)
def to_msg(self):
return TimeMsg()
class FakePublisher:
def __init__(self) -> None:
self.messages = []
def publish(self, message) -> None:
self.messages.append(message)
def _joint_publishing_teleop() -> SingleArmVelocityTeleop:
names = [f"omnipic_joint_{index}" for index in range(1, 8)]
teleop = object.__new__(SingleArmVelocityTeleop)
teleop._ik_solver = SimpleNamespace(
joint_names=names,
update_joint_state=lambda joints: np.eye(4),
)
teleop._joint_state_pub = FakePublisher()
teleop._joint_target_pub = FakePublisher()
teleop._active = False
teleop._last_valid_joint_target = None
teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
return teleop
def test_reset_joint_state_publishes_named_feedback() -> None:
teleop = _joint_publishing_teleop()
positions = [0.1 * index for index in range(7)]
snapshot = JointStateSnapshot(positions, time.monotonic())
teleop._reset_joint_state(snapshot)
message = teleop._joint_state_pub.messages[-1]
assert message.name == teleop._ik_solver.joint_names
assert message.position == pytest.approx(positions)
def test_sync_joint_feedback_publishes_each_sample() -> None:
teleop = _joint_publishing_teleop()
positions = [0.2] * 7
teleop._sync_joint_feedback(
JointStateSnapshot(positions, time.monotonic())
)
assert len(teleop._joint_state_pub.messages) == 1
assert teleop._joint_state_pub.messages[0].position == pytest.approx(positions)
def test_send_joint_target_publishes_limited_command() -> None:
sent = []
teleop = _joint_publishing_teleop()
teleop._adapter = SimpleNamespace(
send_joint_target=lambda joints, follow: sent.append((list(joints), follow))
)
teleop._follow = False
teleop._latest_joint_positions = [0.0] * 7
teleop._last_joint_command_target = [0.0] * 7
teleop._last_joint_command_velocity = [0.0] * 7
teleop._joint_command_max_speed = 1.0
teleop._joint_command_max_acceleration = 100.0
teleop._dt = 0.1
assert teleop._send_joint_target([0.5] * 7)
assert len(sent) == 1
assert sent[0][0] == pytest.approx([0.1] * 7)
assert sent[0][1] is False
message = teleop._joint_target_pub.messages[-1]
assert message.name == teleop._ik_solver.joint_names
assert message.position == pytest.approx(teleop._last_joint_command_target)
def _primary_button_teleop(*, move_error=None):
events = []
@@ -112,8 +187,11 @@ def test_startup_joint_query_initializes_qp_and_command_history() -> None:
)
)
teleop._ik_solver = SimpleNamespace(
update_joint_state=lambda joints: pose
joint_names=[f"omnipic_joint_{index}" for index in range(1, 8)],
update_joint_state=lambda joints: pose,
)
teleop._joint_state_pub = FakePublisher()
teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
teleop.get_logger = lambda: FakeLogger()
teleop._initialize_joint_state()
@@ -178,6 +256,12 @@ def _timeout_teleop(adapter) -> SingleArmVelocityTeleop:
teleop._ik_solver = SimpleNamespace(
update_joint_state=lambda joints: np.eye(4)
)
teleop._ik_solver.joint_names = [
f"omnipic_joint_{index}" for index in range(1, 8)
]
teleop._joint_state_pub = FakePublisher()
teleop._joint_target_pub = FakePublisher()
teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
teleop._stop_sent = False
teleop._feedback_resync_timeout_sec = 0.5
teleop._publish_stop_debug = lambda: None
@@ -433,6 +517,10 @@ def test_feedback_fault_blocks_grip_until_release() -> None:
teleop._ik_solver = SimpleNamespace(
update_joint_state=lambda joints: np.eye(4)
)
teleop._ik_solver.joint_names = [
f"omnipic_joint_{index}" for index in range(1, 8)
]
teleop._joint_state_pub = FakePublisher()
teleop._grip_rearm_required = True
teleop._control_fault_latched = False
teleop._feedback_resync_attempted = False
@@ -459,6 +547,9 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
class FakeSolver:
def __init__(self) -> None:
self.solve_calls = 0
self.joint_names = [
f"omnipic_joint_{index}" for index in range(1, 8)
]
def update_joint_state(self, joints):
assert joints == [0.1] * 7
@@ -476,6 +567,8 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
teleop._active = False
teleop._last_valid_joint_target = None
teleop._last_current_pose = None
teleop._joint_state_pub = FakePublisher()
teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
pose = teleop._sync_joint_feedback(
JointStateSnapshot([0.1] * 7, time.monotonic())
xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py
+4
View File
@@ -149,6 +149,10 @@ class PlacoIkSolver:
self._frame_task.configure("rm75_relative_frame", "soft", 1.0)
self._solver.add_kinetic_energy_regularization_task(1e-6)
@property
def joint_names(self) -> list[str]:
return list(self._joint_names)
@property
def base_configuration(self) -> list[float]:
return self._robot.state.q[:7].tolist()
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
+28 -1
View File
@@ -17,6 +17,7 @@ import rclpy
from geometry_msgs.msg import PoseStamped, TwistStamped
from rclpy.node import Node
from rclpy.time import Time
from sensor_msgs.msg import JointState
from std_msgs.msg import Bool
from xr_rm_interfaces.msg import XrController
@@ -327,12 +328,22 @@ class SingleArmVelocityTeleop(Node):
self._dt,
peripheral_arm,
)
debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
self._joint_state_pub = self.create_publisher(
JointState,
f"{debug_ns}/joint_states",
10,
)
self._joint_target_pub = self.create_publisher(
JointState,
f"{debug_ns}/joint_target",
10,
)
self._adapter = self._make_adapter()
self._adapter.connect()
self._initialize_joint_state()
self._setup_tool_control()
debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
self._current_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/current_pose", 10)
self._raw_target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/raw_target_pose", 10)
self._target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/target_pose", 10)
@@ -393,6 +404,13 @@ class SingleArmVelocityTeleop(Node):
self._adapter.close()
raise
def _publish_joint_positions(self, publisher, positions: list[float]) -> None:
message = JointState()
message.header.stamp = self.get_clock().now().to_msg()
message.name = self._ik_solver.joint_names
message.position = [float(value) for value in positions]
publisher.publish(message)
def _reset_joint_state(
self,
snapshot: JointStateSnapshot,
@@ -406,6 +424,7 @@ class SingleArmVelocityTeleop(Node):
self._last_valid_joint_target = list(positions)
self._last_joint_command_target = list(positions)
self._last_joint_command_velocity = [0.0] * 7
self._publish_joint_positions(self._joint_state_pub, positions)
return current_pose
def _setup_tool_control(self) -> None:
@@ -1166,6 +1185,10 @@ class SingleArmVelocityTeleop(Node):
)
self._latest_joint_positions = list(snapshot.positions)
self._last_current_pose = current_pose
self._publish_joint_positions(
self._joint_state_pub,
list(snapshot.positions),
)
if not self._active or self._last_valid_joint_target is None:
self._last_valid_joint_target = list(snapshot.positions)
return current_pose
@@ -1254,6 +1277,10 @@ class SingleArmVelocityTeleop(Node):
return False
self._last_joint_command_target = limited_target
self._last_joint_command_velocity = limited_velocity
self._publish_joint_positions(
self._joint_target_pub,
limited_target,
)
return True
@staticmethod