forked from YikaiFu-cart/acRealman_xr
Enhance orientation control for RM75 arms in XR teleoperation
This commit is contained in:
@ -3,7 +3,7 @@
|
||||
<package format="3">
|
||||
<name>xr_rm_teleop</name>
|
||||
<version>0.1.0</version>
|
||||
<description>XR relative-motion teleoperation controllers for RealMan RM75 arms.</description>
|
||||
<description>XR relative-pose teleoperation controllers for RealMan RM75 arms.</description>
|
||||
<maintainer email="user@example.com">Yikai Fu</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
"""xr_rm_teleop 包安装配置。
|
||||
|
||||
该包提供基于 XR 相对位移的 RM75 笛卡尔位姿透传遥操作节点。
|
||||
该包提供基于 XR 相对位姿的 RM75 笛卡尔位姿透传遥操作节点。
|
||||
"""
|
||||
|
||||
from setuptools import setup
|
||||
|
||||
124
xr_rm_teleop/test/test_orientation_control.py
Normal file
124
xr_rm_teleop/test/test_orientation_control.py
Normal file
@ -0,0 +1,124 @@
|
||||
import math
|
||||
from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from xr_rm_teleop.realman_adapter import ArmPose, MockRealManAdapter
|
||||
from xr_rm_teleop.single_arm_velocity_teleop import (
|
||||
SingleArmVelocityTeleop,
|
||||
_euler_to_quaternion,
|
||||
_normalize_quaternion,
|
||||
_quaternion_to_euler,
|
||||
)
|
||||
|
||||
|
||||
def _make_teleop_for_orientation() -> SingleArmVelocityTeleop:
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._enable_orientation_control = True
|
||||
teleop._enable_orientation_axes = [True, True, True]
|
||||
teleop._controller_orientation_start = (0.0, 0.0, 0.0, 1.0)
|
||||
teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.1, -0.2, 0.3)
|
||||
teleop._xr_to_robot_matrix = [
|
||||
0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 1.0,
|
||||
1.0, 0.0, 0.0,
|
||||
]
|
||||
return teleop
|
||||
|
||||
|
||||
def assert_angles_close(actual: list[float] | tuple[float, ...], expected: list[float]) -> None:
|
||||
assert len(actual) == len(expected)
|
||||
for actual_value, expected_value in zip(actual, expected):
|
||||
assert math.atan2(math.sin(actual_value - expected_value), math.cos(actual_value - expected_value)) == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_identity_controller_orientation_keeps_tcp_orientation() -> None:
|
||||
teleop = _make_teleop_for_orientation()
|
||||
|
||||
target = teleop._raw_orientation_from_controller((0.0, 0.0, 0.0, 1.0))
|
||||
|
||||
assert_angles_close(target, teleop._robot_start_pose.rpy())
|
||||
|
||||
|
||||
def test_xr_relative_rotation_maps_through_xr_to_robot_matrix() -> None:
|
||||
teleop = _make_teleop_for_orientation()
|
||||
teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.0, 0.0, 0.0)
|
||||
xr_roll = _euler_to_quaternion(0.2, 0.0, 0.0)
|
||||
|
||||
target = teleop._raw_orientation_from_controller(xr_roll)
|
||||
|
||||
assert_angles_close(target, [0.0, 0.0, 0.2])
|
||||
|
||||
|
||||
def test_orientation_deadband_filter_and_speed_limit() -> None:
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._orientation_deadband_rad = 0.01
|
||||
teleop._orientation_filter_alpha = 0.5
|
||||
teleop._max_orientation_speed = 0.5
|
||||
teleop._dt = 0.1
|
||||
teleop._last_sent_orientation = [0.0, 0.0, 0.0]
|
||||
teleop._filtered_orientation_target = [0.0, 0.0, 0.0]
|
||||
|
||||
assert teleop._apply_orientation_deadband([0.001, 0.0, 0.0]) == [0.0, 0.0, 0.0]
|
||||
|
||||
filtered = teleop._filter_orientation_target([0.2, 0.0, 0.0])
|
||||
assert_angles_close(filtered, [0.1, 0.0, 0.0])
|
||||
|
||||
limited, was_limited = teleop._limit_orientation_step([0.2, 0.0, 0.0])
|
||||
assert was_limited
|
||||
assert_angles_close(limited, [0.05, 0.0, 0.0])
|
||||
|
||||
|
||||
def test_invalid_controller_quaternion_stops_current_tick() -> None:
|
||||
class FakeTime:
|
||||
def __sub__(self, other):
|
||||
del other
|
||||
return SimpleNamespace(nanoseconds=0)
|
||||
|
||||
class FakeClock:
|
||||
def now(self):
|
||||
return FakeTime()
|
||||
|
||||
class FakeLogger:
|
||||
def warn(self, *args, **kwargs):
|
||||
del args, kwargs
|
||||
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
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=0.0),
|
||||
),
|
||||
)
|
||||
teleop._last_msg_time = FakeTime()
|
||||
teleop._arm_name = "test_rm75"
|
||||
teleop._command_timeout_sec = 0.12
|
||||
teleop._enable_orientation_control = True
|
||||
stopped = []
|
||||
teleop.get_clock = lambda: FakeClock()
|
||||
teleop.get_logger = lambda: FakeLogger()
|
||||
teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
|
||||
|
||||
teleop._control_tick()
|
||||
|
||||
assert stopped == [True]
|
||||
|
||||
|
||||
def test_quaternion_roundtrip_for_small_rpy() -> None:
|
||||
quat = _normalize_quaternion(_euler_to_quaternion(0.2, -0.1, 0.3))
|
||||
assert_angles_close(_quaternion_to_euler(quat), [0.2, -0.1, 0.3])
|
||||
|
||||
|
||||
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
|
||||
@ -6,11 +6,16 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
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
|
||||
@ -23,6 +28,9 @@ class ArmPose:
|
||||
def xyz(self) -> list[float]:
|
||||
return [self.x, self.y, self.z]
|
||||
|
||||
def rpy(self) -> list[float]:
|
||||
return [self.rx, self.ry, self.rz]
|
||||
|
||||
|
||||
class MockRealManAdapter:
|
||||
"""无机械臂时使用的运动学模拟器，用于验证 ROS2 遥操链路。"""
|
||||
@ -45,9 +53,9 @@ class MockRealManAdapter:
|
||||
(pose.x - self._pose.x) / self._dt,
|
||||
(pose.y - self._pose.y) / self._dt,
|
||||
(pose.z - self._pose.z) / self._dt,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
_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
|
||||
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
"""RM75 单臂 XR 相对位姿透传遥操作节点。
|
||||
|
||||
节点订阅左/右手柄位姿，在 grip 按下时锁定手柄和 TCP 起点，把手柄相对位移
|
||||
节点订阅左/右手柄位姿，在 grip 按下时锁定手柄和 TCP 起点，把手柄相对位姿
|
||||
映射成机器人坐标系中的目标 TCP，并通过 rm_movep_canfd 持续下发目标位姿。
|
||||
"""
|
||||
|
||||
@ -30,8 +30,123 @@ def _clamp(value: float, low: float, high: float) -> float:
|
||||
return min(max(value, low), high)
|
||||
|
||||
|
||||
def _wrap_angle(angle: float) -> float:
|
||||
return math.atan2(math.sin(angle), math.cos(angle))
|
||||
|
||||
|
||||
def _angle_delta(target: float, current: float) -> float:
|
||||
return _wrap_angle(target - current)
|
||||
|
||||
|
||||
def _normalize_quaternion(values: Iterable[float]) -> tuple[float, float, float, float]:
|
||||
x, y, z, w = [float(value) for value in values]
|
||||
norm = math.sqrt(x * x + y * y + z * z + w * w)
|
||||
if norm <= 1e-9 or not math.isfinite(norm):
|
||||
raise ValueError("quaternion norm is zero or non-finite")
|
||||
return x / norm, y / norm, z / norm, w / norm
|
||||
|
||||
|
||||
def _quaternion_conjugate(
|
||||
quat: tuple[float, float, float, float],
|
||||
) -> tuple[float, float, float, float]:
|
||||
x, y, z, w = quat
|
||||
return -x, -y, -z, w
|
||||
|
||||
|
||||
def _quaternion_multiply(
|
||||
left: tuple[float, float, float, float],
|
||||
right: tuple[float, float, float, float],
|
||||
) -> tuple[float, float, float, float]:
|
||||
lx, ly, lz, lw = left
|
||||
rx, ry, rz, rw = right
|
||||
return _normalize_quaternion(
|
||||
(
|
||||
lw * rx + lx * rw + ly * rz - lz * ry,
|
||||
lw * ry - lx * rz + ly * rw + lz * rx,
|
||||
lw * rz + lx * ry - ly * rx + lz * rw,
|
||||
lw * rw - lx * rx - ly * ry - lz * rz,
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
def _quaternion_to_matrix(quat: tuple[float, float, float, float]) -> list[float]:
|
||||
x, y, z, w = quat
|
||||
xx = x * x
|
||||
yy = y * y
|
||||
zz = z * z
|
||||
xy = x * y
|
||||
xz = x * z
|
||||
yz = y * z
|
||||
wx = w * x
|
||||
wy = w * y
|
||||
wz = w * z
|
||||
return [
|
||||
1.0 - 2.0 * (yy + zz),
|
||||
2.0 * (xy - wz),
|
||||
2.0 * (xz + wy),
|
||||
2.0 * (xy + wz),
|
||||
1.0 - 2.0 * (xx + zz),
|
||||
2.0 * (yz - wx),
|
||||
2.0 * (xz - wy),
|
||||
2.0 * (yz + wx),
|
||||
1.0 - 2.0 * (xx + yy),
|
||||
]
|
||||
|
||||
|
||||
def _matrix_multiply(left: list[float], right: list[float]) -> list[float]:
|
||||
return [
|
||||
sum(left[row * 3 + k] * right[k * 3 + col] for k in range(3))
|
||||
for row in range(3)
|
||||
for col in range(3)
|
||||
]
|
||||
|
||||
|
||||
def _matrix_transpose(matrix: list[float]) -> list[float]:
|
||||
return [
|
||||
matrix[0], matrix[3], matrix[6],
|
||||
matrix[1], matrix[4], matrix[7],
|
||||
matrix[2], matrix[5], matrix[8],
|
||||
]
|
||||
|
||||
|
||||
def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]:
|
||||
cy = math.cos(yaw * 0.5)
|
||||
sy = math.sin(yaw * 0.5)
|
||||
cp = math.cos(pitch * 0.5)
|
||||
sp = math.sin(pitch * 0.5)
|
||||
cr = math.cos(roll * 0.5)
|
||||
sr = math.sin(roll * 0.5)
|
||||
|
||||
qw = cr * cp * cy + sr * sp * sy
|
||||
qx = sr * cp * cy - cr * sp * sy
|
||||
qy = cr * sp * cy + sr * cp * sy
|
||||
qz = cr * cp * sy - sr * sp * cy
|
||||
return _normalize_quaternion((qx, qy, qz, qw))
|
||||
|
||||
|
||||
def _euler_to_matrix(roll: float, pitch: float, yaw: float) -> list[float]:
|
||||
return _quaternion_to_matrix(_euler_to_quaternion(roll, pitch, yaw))
|
||||
|
||||
|
||||
def _matrix_to_euler(matrix: list[float]) -> tuple[float, float, float]:
|
||||
sy = -_clamp(matrix[6], -1.0, 1.0)
|
||||
pitch = math.asin(sy)
|
||||
cp = math.cos(pitch)
|
||||
if abs(cp) > 1e-9:
|
||||
roll = math.atan2(matrix[7], matrix[8])
|
||||
yaw = math.atan2(matrix[3], matrix[0])
|
||||
else:
|
||||
roll = math.atan2(-matrix[5], matrix[4])
|
||||
yaw = 0.0
|
||||
return _wrap_angle(roll), _wrap_angle(pitch), _wrap_angle(yaw)
|
||||
|
||||
|
||||
def _quaternion_to_euler(quat: tuple[float, float, float, float]) -> tuple[float, float, float]:
|
||||
return _matrix_to_euler(_quaternion_to_matrix(quat))
|
||||
|
||||
|
||||
class SingleArmVelocityTeleop(Node):
|
||||
"""基于 XR 手柄相对位移的 RM75 单臂位姿透传遥操节点。
|
||||
"""基于 XR 手柄相对位姿的 RM75 单臂位姿透传遥操节点。
|
||||
|
||||
类名和可执行入口沿用旧名称，避免已有 launch、UI 和命令失效。
|
||||
"""
|
||||
@ -50,6 +165,11 @@ class SingleArmVelocityTeleop(Node):
|
||||
self.declare_parameter("target_filter_fast_threshold_m", 0.03)
|
||||
self.declare_parameter("max_linear_speed", 0.25)
|
||||
self.declare_parameter("enable_position_axes", [True, True, True])
|
||||
self.declare_parameter("enable_orientation_control", False)
|
||||
self.declare_parameter("enable_orientation_axes", [True, True, True])
|
||||
self.declare_parameter("orientation_deadband_rad", 0.005)
|
||||
self.declare_parameter("orientation_filter_alpha", 0.65)
|
||||
self.declare_parameter("max_orientation_speed", 0.6)
|
||||
self.declare_parameter("workspace_min", [0.20, -0.35, 0.10])
|
||||
self.declare_parameter("workspace_max", [0.65, 0.35, 0.60])
|
||||
self.declare_parameter("cyl_radius_limit", [0.20, 0.60])
|
||||
@ -102,6 +222,11 @@ class SingleArmVelocityTeleop(Node):
|
||||
)
|
||||
self._max_linear_speed = float(self.get_parameter("max_linear_speed").value)
|
||||
self._enable_position_axes = self._bool_list_parameter("enable_position_axes", 3)
|
||||
self._enable_orientation_control = self._bool_parameter("enable_orientation_control")
|
||||
self._enable_orientation_axes = self._bool_list_parameter("enable_orientation_axes", 3)
|
||||
self._orientation_deadband_rad = float(self.get_parameter("orientation_deadband_rad").value)
|
||||
self._orientation_filter_alpha = float(self.get_parameter("orientation_filter_alpha").value)
|
||||
self._max_orientation_speed = float(self.get_parameter("max_orientation_speed").value)
|
||||
self._workspace_min = self._float_list_parameter("workspace_min", 3)
|
||||
self._workspace_max = self._float_list_parameter("workspace_max", 3)
|
||||
self._cyl_radius_limit = self._float_list_parameter("cyl_radius_limit", 2)
|
||||
@ -122,9 +247,12 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._last_msg_time: Time | None = None
|
||||
self._active = False
|
||||
self._controller_start: list[float] | None = None
|
||||
self._controller_orientation_start: tuple[float, float, float, float] | None = None
|
||||
self._robot_start_pose: ArmPose | None = None
|
||||
self._filtered_target: list[float] | None = None
|
||||
self._filtered_orientation_target: list[float] | None = None
|
||||
self._last_sent_target: list[float] | None = None
|
||||
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
|
||||
@ -150,7 +278,8 @@ class SingleArmVelocityTeleop(Node):
|
||||
self.create_timer(self._dt, self._control_tick)
|
||||
self.get_logger().info(
|
||||
f"{self._arm_name} 位姿透传遥操节点已启动，监听话题：{topic}, "
|
||||
f"dt={self._dt:.4f}s, follow={self._follow}"
|
||||
f"dt={self._dt:.4f}s, follow={self._follow}, "
|
||||
f"orientation_control={self._enable_orientation_control}"
|
||||
)
|
||||
|
||||
def _make_adapter(self):
|
||||
@ -332,39 +461,66 @@ class SingleArmVelocityTeleop(Node):
|
||||
return
|
||||
|
||||
controller_now = self._controller_xyz(self._last_msg)
|
||||
try:
|
||||
controller_quat = self._controller_quaternion(self._last_msg)
|
||||
except ValueError as exc:
|
||||
self.get_logger().warn(
|
||||
f"{self._arm_name} XR 手柄姿态无效，停止输出：{exc}",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
self._safe_stop(reset_active=True)
|
||||
return
|
||||
if not self._active:
|
||||
self._enter_active_control(controller_now, now)
|
||||
self._enter_active_control(controller_now, controller_quat, 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 = self._raw_target_from_controller(controller_now)
|
||||
workspace_target, workspace_clamped = self._clamp_workspace_with_flag(raw_target)
|
||||
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)
|
||||
desired_target = self._apply_deadband(workspace_target)
|
||||
filtered_target = self._filter_target(desired_target)
|
||||
sent_target, step_limited = self._limit_target_step(filtered_target)
|
||||
sent_target, final_clamped = self._clamp_workspace_with_flag(sent_target)
|
||||
desired_orientation = self._apply_orientation_deadband(raw_target_rpy)
|
||||
filtered_orientation = self._filter_orientation_target(desired_orientation)
|
||||
sent_orientation, orientation_step_limited = self._limit_orientation_step(filtered_orientation)
|
||||
|
||||
velocity = self._estimate_command_velocity(sent_target, now)
|
||||
target_clamped = workspace_clamped or step_limited or final_clamped
|
||||
velocity = self._estimate_command_velocity(sent_target, sent_orientation, now)
|
||||
target_clamped = workspace_clamped or step_limited or final_clamped or orientation_step_limited
|
||||
raw_target_pose = ArmPose(
|
||||
x=raw_target_xyz[0],
|
||||
y=raw_target_xyz[1],
|
||||
z=raw_target_xyz[2],
|
||||
rx=raw_target_rpy[0],
|
||||
ry=raw_target_rpy[1],
|
||||
rz=raw_target_rpy[2],
|
||||
)
|
||||
target_pose = ArmPose(
|
||||
x=sent_target[0],
|
||||
y=sent_target[1],
|
||||
z=sent_target[2],
|
||||
rx=self._robot_start_pose.rx,
|
||||
ry=self._robot_start_pose.ry,
|
||||
rz=self._robot_start_pose.rz,
|
||||
rx=sent_orientation[0],
|
||||
ry=sent_orientation[1],
|
||||
rz=sent_orientation[2],
|
||||
)
|
||||
|
||||
self._publish_debug(raw_target, sent_target, velocity, target_clamped)
|
||||
self._publish_debug(raw_target_pose, target_pose, velocity, target_clamped)
|
||||
if self._send_cartesian_target(target_pose):
|
||||
self._last_sent_target = sent_target
|
||||
self._last_sent_orientation = sent_orientation
|
||||
self._last_command_time = now
|
||||
self._stop_sent = False
|
||||
|
||||
def _enter_active_control(self, controller_now: list[float], now: Time) -> None:
|
||||
def _enter_active_control(
|
||||
self,
|
||||
controller_now: list[float],
|
||||
controller_quat: tuple[float, float, float, float],
|
||||
now: Time,
|
||||
) -> None:
|
||||
try:
|
||||
robot_pose = self._read_current_pose_for_control(now)
|
||||
except Exception as exc:
|
||||
@ -378,18 +534,33 @@ class SingleArmVelocityTeleop(Node):
|
||||
robot_xyz = robot_pose.xyz()
|
||||
self._active = True
|
||||
self._controller_start = controller_now
|
||||
self._controller_orientation_start = controller_quat
|
||||
self._robot_start_pose = robot_pose
|
||||
self._filtered_target = robot_xyz
|
||||
self._filtered_orientation_target = robot_pose.rpy()
|
||||
self._last_sent_target = robot_xyz
|
||||
self._last_sent_orientation = robot_pose.rpy()
|
||||
self._last_command_time = now
|
||||
self._stop_sent = True
|
||||
self.get_logger().info(f"{self._arm_name} Grip 按下，已锁定手柄和机械臂初始位姿。")
|
||||
self._publish_debug(robot_xyz, robot_xyz, [0.0] * 6, False)
|
||||
self._publish_debug(robot_pose, robot_pose, [0.0] * 6, False)
|
||||
|
||||
@staticmethod
|
||||
def _controller_xyz(msg: XrController) -> list[float]:
|
||||
return [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]
|
||||
|
||||
def _controller_quaternion(self, msg: XrController) -> tuple[float, float, float, float]:
|
||||
if not self._enable_orientation_control:
|
||||
return 0.0, 0.0, 0.0, 1.0
|
||||
return _normalize_quaternion(
|
||||
(
|
||||
msg.pose.orientation.x,
|
||||
msg.pose.orientation.y,
|
||||
msg.pose.orientation.z,
|
||||
msg.pose.orientation.w,
|
||||
)
|
||||
)
|
||||
|
||||
def _raw_target_from_controller(self, controller_now: list[float]) -> list[float]:
|
||||
assert self._controller_start is not None
|
||||
assert self._robot_start_pose is not None
|
||||
@ -410,6 +581,38 @@ class SingleArmVelocityTeleop(Node):
|
||||
matrix[6] * delta[0] + matrix[7] * delta[1] + matrix[8] * delta[2],
|
||||
]
|
||||
|
||||
def _raw_orientation_from_controller(
|
||||
self,
|
||||
controller_quat: tuple[float, float, float, float],
|
||||
) -> list[float]:
|
||||
assert self._robot_start_pose is not None
|
||||
if not self._enable_orientation_control:
|
||||
return self._robot_start_pose.rpy()
|
||||
|
||||
assert self._controller_orientation_start is not None
|
||||
xr_delta_quat = _quaternion_multiply(
|
||||
controller_quat,
|
||||
_quaternion_conjugate(self._controller_orientation_start),
|
||||
)
|
||||
xr_delta_matrix = _quaternion_to_matrix(xr_delta_quat)
|
||||
matrix = self._xr_to_robot_matrix
|
||||
robot_delta_matrix = _matrix_multiply(
|
||||
_matrix_multiply(matrix, xr_delta_matrix),
|
||||
_matrix_transpose(matrix),
|
||||
)
|
||||
robot_start_matrix = _euler_to_matrix(
|
||||
self._robot_start_pose.rx,
|
||||
self._robot_start_pose.ry,
|
||||
self._robot_start_pose.rz,
|
||||
)
|
||||
target_matrix = _matrix_multiply(robot_delta_matrix, robot_start_matrix)
|
||||
target_rpy = list(_matrix_to_euler(target_matrix))
|
||||
start_rpy = self._robot_start_pose.rpy()
|
||||
return [
|
||||
target_rpy[i] if self._enable_orientation_axes[i] else start_rpy[i]
|
||||
for i in range(3)
|
||||
]
|
||||
|
||||
def _apply_deadband(self, target: list[float]) -> list[float]:
|
||||
if self._deadband_m <= 0.0 or self._last_sent_target is None:
|
||||
return target
|
||||
@ -457,6 +660,51 @@ class SingleArmVelocityTeleop(Node):
|
||||
for i in range(3)
|
||||
], True
|
||||
|
||||
def _apply_orientation_deadband(self, target_rpy: list[float]) -> list[float]:
|
||||
if self._orientation_deadband_rad <= 0.0 or self._last_sent_orientation is None:
|
||||
return [_wrap_angle(value) for value in target_rpy]
|
||||
delta = [
|
||||
_angle_delta(target_rpy[i], self._last_sent_orientation[i])
|
||||
for i in range(3)
|
||||
]
|
||||
if _norm(delta) < self._orientation_deadband_rad:
|
||||
return list(self._last_sent_orientation)
|
||||
return [_wrap_angle(value) for value in target_rpy]
|
||||
|
||||
def _filter_orientation_target(self, target_rpy: list[float]) -> list[float]:
|
||||
if self._filtered_orientation_target is None:
|
||||
self._filtered_orientation_target = [_wrap_angle(value) for value in target_rpy]
|
||||
return list(self._filtered_orientation_target)
|
||||
|
||||
self._filtered_orientation_target = [
|
||||
_wrap_angle(
|
||||
self._filtered_orientation_target[i]
|
||||
+ self._orientation_filter_alpha
|
||||
* _angle_delta(target_rpy[i], self._filtered_orientation_target[i])
|
||||
)
|
||||
for i in range(3)
|
||||
]
|
||||
return list(self._filtered_orientation_target)
|
||||
|
||||
def _limit_orientation_step(self, target_rpy: list[float]) -> tuple[list[float], bool]:
|
||||
if self._last_sent_orientation is None:
|
||||
return [_wrap_angle(value) for value in target_rpy], False
|
||||
|
||||
delta = [
|
||||
_angle_delta(target_rpy[i], self._last_sent_orientation[i])
|
||||
for i in range(3)
|
||||
]
|
||||
distance = _norm(delta)
|
||||
max_step = self._max_orientation_speed * self._dt
|
||||
if distance <= max_step or distance <= 1e-9:
|
||||
return [_wrap_angle(value) for value in target_rpy], False
|
||||
|
||||
scale = max_step / distance
|
||||
return [
|
||||
_wrap_angle(self._last_sent_orientation[i] + delta[i] * scale)
|
||||
for i in range(3)
|
||||
], True
|
||||
|
||||
def _clamp_workspace_with_flag(self, target: list[float]) -> tuple[list[float], bool]:
|
||||
clamped = [
|
||||
_clamp(target[i], self._workspace_min[i], self._workspace_max[i])
|
||||
@ -487,8 +735,13 @@ class SingleArmVelocityTeleop(Node):
|
||||
|
||||
return target
|
||||
|
||||
def _estimate_command_velocity(self, target: list[float], now: Time) -> list[float]:
|
||||
if self._last_sent_target is None:
|
||||
def _estimate_command_velocity(
|
||||
self,
|
||||
target_xyz: list[float],
|
||||
target_rpy: list[float],
|
||||
now: Time,
|
||||
) -> list[float]:
|
||||
if self._last_sent_target is None or self._last_sent_orientation is None:
|
||||
return [0.0] * 6
|
||||
|
||||
dt = self._dt
|
||||
@ -497,9 +750,12 @@ class SingleArmVelocityTeleop(Node):
|
||||
if measured_dt > 1e-6:
|
||||
dt = measured_dt
|
||||
return [
|
||||
(target[i] - self._last_sent_target[i]) / dt
|
||||
(target_xyz[i] - self._last_sent_target[i]) / dt
|
||||
for i in range(3)
|
||||
] + [0.0, 0.0, 0.0]
|
||||
] + [
|
||||
_angle_delta(target_rpy[i], self._last_sent_orientation[i]) / dt
|
||||
for i in range(3)
|
||||
]
|
||||
|
||||
def _read_current_pose_for_control(self, now: Time) -> ArmPose:
|
||||
pose = self._adapter.get_current_pose()
|
||||
@ -529,9 +785,12 @@ class SingleArmVelocityTeleop(Node):
|
||||
if reset_active:
|
||||
self._active = False
|
||||
self._controller_start = None
|
||||
self._controller_orientation_start = None
|
||||
self._robot_start_pose = None
|
||||
self._filtered_target = None
|
||||
self._filtered_orientation_target = None
|
||||
self._last_sent_target = None
|
||||
self._last_sent_orientation = None
|
||||
self._last_command_time = None
|
||||
self._publish_stop_debug()
|
||||
|
||||
@ -549,7 +808,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
pose = self._debug_pose_fallback()
|
||||
if pose is None:
|
||||
return
|
||||
self._publish_debug(pose.xyz(), pose.xyz(), [0.0] * 6, False)
|
||||
self._publish_debug(pose, pose, [0.0] * 6, False)
|
||||
|
||||
def _debug_pose_fallback(self) -> ArmPose | None:
|
||||
if self._last_current_pose is not None:
|
||||
@ -557,7 +816,8 @@ class SingleArmVelocityTeleop(Node):
|
||||
if self._robot_start_pose is not None:
|
||||
return self._robot_start_pose
|
||||
if self._last_sent_target is not None:
|
||||
return ArmPose(*self._last_sent_target)
|
||||
rpy = self._last_sent_orientation or [0.0, 0.0, 0.0]
|
||||
return ArmPose(*self._last_sent_target, *rpy)
|
||||
return None
|
||||
|
||||
def _send_cartesian_target(self, pose: ArmPose) -> bool:
|
||||
@ -575,38 +835,18 @@ class SingleArmVelocityTeleop(Node):
|
||||
|
||||
def _publish_debug(
|
||||
self,
|
||||
raw_target_xyz: list[float],
|
||||
target_xyz: list[float],
|
||||
raw_target_pose: ArmPose,
|
||||
target_pose: ArmPose,
|
||||
command_velocity: list[float],
|
||||
target_clamped: bool,
|
||||
) -> None:
|
||||
stamp = self.get_clock().now().to_msg()
|
||||
current_pose = self._debug_pose_fallback()
|
||||
if current_pose is None:
|
||||
current_pose = ArmPose(*target_xyz)
|
||||
current_pose = target_pose
|
||||
|
||||
raw_pose = self._pose_msg(
|
||||
stamp,
|
||||
ArmPose(
|
||||
x=raw_target_xyz[0],
|
||||
y=raw_target_xyz[1],
|
||||
z=raw_target_xyz[2],
|
||||
rx=current_pose.rx,
|
||||
ry=current_pose.ry,
|
||||
rz=current_pose.rz,
|
||||
),
|
||||
)
|
||||
target_msg = self._pose_msg(
|
||||
stamp,
|
||||
ArmPose(
|
||||
x=target_xyz[0],
|
||||
y=target_xyz[1],
|
||||
z=target_xyz[2],
|
||||
rx=current_pose.rx,
|
||||
ry=current_pose.ry,
|
||||
rz=current_pose.rz,
|
||||
),
|
||||
)
|
||||
raw_pose = self._pose_msg(stamp, raw_target_pose)
|
||||
target_msg = self._pose_msg(stamp, target_pose)
|
||||
velocity_msg = TwistStamped()
|
||||
velocity_msg.header.stamp = stamp
|
||||
velocity_msg.header.frame_id = "rm_base"
|
||||
@ -628,7 +868,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
|
||||
@staticmethod
|
||||
def _pose_msg(stamp, pose: ArmPose) -> PoseStamped:
|
||||
qx, qy, qz, qw = SingleArmVelocityTeleop._euler_to_quaternion(pose.rx, pose.ry, pose.rz)
|
||||
qx, qy, qz, qw = _euler_to_quaternion(pose.rx, pose.ry, pose.rz)
|
||||
msg = PoseStamped()
|
||||
msg.header.stamp = stamp
|
||||
msg.header.frame_id = "rm_base"
|
||||
@ -641,21 +881,6 @@ class SingleArmVelocityTeleop(Node):
|
||||
msg.pose.orientation.w = qw
|
||||
return msg
|
||||
|
||||
@staticmethod
|
||||
def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]:
|
||||
cy = math.cos(yaw * 0.5)
|
||||
sy = math.sin(yaw * 0.5)
|
||||
cp = math.cos(pitch * 0.5)
|
||||
sp = math.sin(pitch * 0.5)
|
||||
cr = math.cos(roll * 0.5)
|
||||
sr = math.sin(roll * 0.5)
|
||||
|
||||
qw = cr * cp * cy + sr * sp * sy
|
||||
qx = sr * cp * cy - cr * sp * sy
|
||||
qy = cr * sp * cy + sr * cp * sy
|
||||
qz = cr * cp * sy - sr * sp * cy
|
||||
return qx, qy, qz, qw
|
||||
|
||||
def _float_list_parameter(self, name: str, expected_length: int) -> list[float]:
|
||||
values = [float(value) for value in self.get_parameter(name).value]
|
||||
if len(values) != expected_length:
|
||||
@ -696,6 +921,12 @@ class SingleArmVelocityTeleop(Node):
|
||||
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:
|
||||
raise ValueError("orientation_filter_alpha must be in [0, 1]")
|
||||
if self._max_orientation_speed <= 0.0:
|
||||
raise ValueError("max_orientation_speed must be > 0")
|
||||
if not 0.0 <= self._trigger_close_threshold <= 1.0:
|
||||
raise ValueError("trigger_close_threshold must be in [0, 1]")
|
||||
for axis, (low, high) in enumerate(zip(self._workspace_min, self._workspace_max)):
|
||||
|
||||
Reference in New Issue
Block a user