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

feat: 优化双臂采摘QP稳健性

Browse Source
This commit is contained in:
YikaiFu-cart
2026-08-13 10:23:35 +08:00
parent 807374c9fd
commit 7a5c27d6b9
10 changed files with 1078 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
xr_rm_teleop/test/test_initial_joint_pose.py
+37
View File
@@ -100,6 +100,43 @@ def test_deployed_workspace_is_in_front_of_robot(config_name, node_names) -> Non
assert parameters["workspace_max"] == [0.70, 0.10, 0.75]
@pytest.mark.parametrize(
"arm,single_config,dual_node,j3_reference_deg,j3_weight",
[
("left", "left_arm_rm75.yaml", "left_arm_teleop", 67.96, 1e-5),
("right", "right_arm_rm75.yaml", "right_arm_teleop", -89.57, 1e-4),
],
)
def test_qp_optimization_parameters_match_single_and_dual_configs(
arm,
single_config,
dual_node,
j3_reference_deg,
j3_weight,
) -> None:
del arm
with (CONFIG_DIR / single_config).open(encoding="utf-8") as stream:
single = yaml.safe_load(stream)["single_arm_velocity_teleop"][
"ros__parameters"
]
with (CONFIG_DIR / "dual_arm_rm75.yaml").open(encoding="utf-8") as stream:
dual = yaml.safe_load(stream)[dual_node]["ros__parameters"]
expected = {
"qp_j3_reference_deg": j3_reference_deg,
"qp_j3_weight": j3_weight,
"qp_j4_min_deg": 10.0,
"qp_j4_warn_deg": 25.0,
"qp_j4_weight": 1e-4,
"qp_manipulability_sigma_stop": 0.01,
"qp_manipulability_sigma_warn": 0.04,
"qp_manipulability_weight": 1e-4,
}
for name, value in expected.items():
assert single[name] == pytest.approx(value)
assert dual[name] == pytest.approx(value)
@pytest.mark.parametrize(
("existing", "expected_operation"),
[(False, "create"), (True, "update")],
xr_rm_teleop/test/test_joint_control.py
+52 -4
View File
@@ -11,6 +11,7 @@ from xr_rm_teleop.single_arm_velocity_teleop import (
SingleArmVelocityTeleop,
_make_transform,
_so3_exp,
_so3_log,
)
@@ -610,7 +611,7 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
assert teleop._ik_solver.solve_calls == 0
def test_qp_failure_returns_last_known_good_target() -> None:
def test_qp_failure_returns_none_and_keeps_last_known_good_target() -> None:
class FailingSolver:
def solve(self, target):
del target
@@ -624,11 +625,11 @@ def test_qp_failure_returns_last_known_good_target() -> None:
target = teleop._solve_joint_target(np.eye(4))
assert target == pytest.approx([0.1] * 7)
assert target is None
assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
def test_qp_success_updates_last_known_good_target() -> None:
def test_qp_success_waits_for_send_before_updating_last_known_good_target() -> None:
class SuccessfulSolver:
def solve(self, target):
del target
@@ -643,7 +644,54 @@ def test_qp_success_updates_last_known_good_target() -> None:
target = teleop._solve_joint_target(np.eye(4))
assert target == pytest.approx([0.2] * 7)
assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
def test_target_filters_do_not_commit_candidate_state() -> None:
teleop = object.__new__(SingleArmVelocityTeleop)
teleop._filtered_target = [0.0, 0.0, 0.0]
teleop._filtered_orientation_target = np.eye(3)
teleop._target_filter_alpha = 0.5
teleop._target_filter_alpha_fast = 0.5
teleop._target_filter_fast_threshold_m = 1.0
teleop._orientation_filter_alpha = 0.5
position = teleop._filter_target([0.2, 0.0, 0.0])
orientation = teleop._filter_orientation_target(
_so3_exp(np.asarray([0.0, 0.0, 0.2]))
)
assert position == pytest.approx([0.1, 0.0, 0.0])
assert teleop._filtered_target == pytest.approx([0.0, 0.0, 0.0])
assert teleop._filtered_orientation_target == pytest.approx(np.eye(3))
assert np.linalg.norm(_so3_log(orientation)) == pytest.approx(0.1)
def test_failed_send_does_not_commit_cartesian_reference_state() -> None:
teleop = object.__new__(SingleArmVelocityTeleop)
teleop._last_valid_joint_target = [0.1] * 7
teleop._filtered_target = [0.2, 0.0, 0.0]
teleop._filtered_orientation_target = np.eye(3)
teleop._last_sent_target = [0.2, 0.0, 0.0]
teleop._last_sent_orientation = np.eye(3)
teleop._last_command_time = FakeTime()
teleop._send_joint_target = lambda joints: False
sent = teleop._send_and_commit_joint_target(
[0.3] * 7,
[0.3, 0.0, 0.0],
_so3_exp(np.asarray([0.0, 0.0, 0.1])),
[0.3, 0.0, 0.0],
_so3_exp(np.asarray([0.0, 0.0, 0.1])),
FakeTime(),
)
assert not sent
assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
assert teleop._filtered_target == pytest.approx([0.2, 0.0, 0.0])
assert teleop._filtered_orientation_target == pytest.approx(np.eye(3))
assert teleop._last_sent_target == pytest.approx([0.2, 0.0, 0.0])
assert teleop._last_sent_orientation == pytest.approx(np.eye(3))
def test_enter_active_control_initializes_se3_orientation_state() -> None:
xr_rm_teleop/test/test_placo_transforms.py
+129
View File
@@ -6,6 +6,7 @@ from xml.etree import ElementTree
import numpy as np
import pytest
from xr_rm_teleop import placo_ik_solver
from xr_rm_teleop.placo_ik_solver import (
QP_ORIENTATION_TOLERANCE_RAD,
QP_POSITION_TOLERANCE_M,
@@ -242,6 +243,7 @@ def test_qp_solve_rejects_position_error_above_two_millimeters() -> None:
solver._frame_task = SimpleNamespace(T_a_b=None)
solver._solver = SimpleNamespace(solve=lambda update: None)
solver._validate_result = lambda result, previous: None
solver._update_auxiliary_task_weights = lambda: None
solver._target_errors = lambda: (2.1e-3, 0.0)
with pytest.raises(RuntimeError, match="QP did not converge after 30"):
@@ -285,3 +287,130 @@ def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
solver._validate_result(np.full(7, 2.0))
with pytest.raises(ValueError, match="velocity"):
solver._validate_result(np.full(7, 0.2))
def test_lower_margin_activation_is_clamped_and_linear() -> None:
activation = placo_ik_solver._lower_margin_activation
assert activation(0.05, 0.01, 0.04) == 0.0
assert activation(0.025, 0.01, 0.04) == pytest.approx(0.5)
assert activation(0.005, 0.01, 0.04) == 1.0
@pytest.mark.parametrize(
"arm,joint_degrees,j3_reference_deg",
[
("left", ARM_CASES[0][1], 67.96),
("right", ARM_CASES[1][1], -89.57),
],
)
def test_solver_configures_auxiliary_qp_tasks(
arm: str,
joint_degrees: list[float],
j3_reference_deg: float,
) -> None:
pytest.importorskip("placo")
solver = PlacoIkSolver(
str(DUAL_URDF_PATH),
1.0 / 90.0,
arm,
j3_reference_deg=j3_reference_deg,
j3_weight=1e-5,
j4_min_deg=10.0,
j4_warn_deg=25.0,
j4_weight=1e-4,
manipulability_sigma_stop=0.01,
manipulability_sigma_warn=0.04,
manipulability_weight=1e-4,
)
joints = np.radians(joint_degrees).tolist()
solver.update_joint_state(joints)
assert solver._j3_task.get_joint(
solver._joint_names[2]
) == pytest.approx(math.radians(j3_reference_deg))
assert np.asarray(solver._j4_constraint.A)[
solver._q_offsets[3]
] == pytest.approx(-1.0)
assert np.asarray(solver._j4_constraint.b) == pytest.approx(
[-math.radians(10.0)]
)
assert solver._j4_constraint.priority == "hard"
jacobian = solver._active_tcp_jacobian()
assert jacobian.shape == (6, 7)
assert np.isfinite(jacobian).all()
assert np.linalg.svd(jacobian, compute_uv=False)[-1] > 0.0
def test_failed_qp_restores_internal_state_to_actual_feedback() -> None:
solver, joints = _dual_placo_solver("left", ARM_CASES[0][1])
current_pose = solver.update_joint_state(joints)
unreachable = current_pose.copy()
unreachable[2, 3] += 10.0
with pytest.raises((RuntimeError, ValueError)):
solver.solve(unreachable)
assert solver._robot.state.q[solver._q_offsets] == pytest.approx(joints)
def test_solver_rejects_non_positive_manipulability_threshold() -> None:
pytest.importorskip("placo")
with pytest.raises(ValueError, match="manipulability thresholds"):
PlacoIkSolver(
str(DUAL_URDF_PATH),
1.0 / 90.0,
"left",
manipulability_sigma_stop=0.0,
manipulability_sigma_warn=0.04,
)
@pytest.mark.parametrize(
"q4_deg,sigma_min,expected_activation",
[
(25.0, 0.04, 0.0),
(17.5, 0.025, 0.5),
(10.0, 0.01, 1.0),
],
)
def test_auxiliary_weights_activate_only_inside_warning_margins(
q4_deg: float,
sigma_min: float,
expected_activation: float,
) -> None:
class TaskSpy:
def __init__(self) -> None:
self.calls = []
def configure(self, name, priority, weight) -> None:
self.calls.append((name, priority, weight))
solver = object.__new__(PlacoIkSolver)
solver._q_offsets = np.arange(7, 14)
solver._robot = SimpleNamespace(
state=SimpleNamespace(q=np.zeros(21))
)
solver._robot.state.q[solver._q_offsets[3]] = math.radians(q4_deg)
solver._j4_task = TaskSpy()
solver._j4_min = math.radians(10.0)
solver._j4_warn = math.radians(25.0)
solver._j4_weight = 1e-4
solver._manipulability_task = TaskSpy()
solver._manipulability_sigma_stop = 0.01
solver._manipulability_sigma_warn = 0.04
solver._manipulability_weight = 1e-4
jacobian = np.zeros((6, 7))
jacobian[:, :6] = np.diag([1.0] * 5 + [sigma_min])
solver._active_tcp_jacobian = lambda: jacobian
solver._update_auxiliary_task_weights()
expected_weight = 1e-4 * expected_activation
assert solver._j4_task.calls == [
("j4_soft_buffer", "soft", pytest.approx(expected_weight))
]
assert solver._manipulability_task.calls == [
("tcp_6d_manipulability", "soft", pytest.approx(expected_weight))
]
xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py
+166 -25
View File
@@ -31,6 +31,14 @@ QP_POSITION_TOLERANCE_M = 2e-3
QP_ORIENTATION_TOLERANCE_RAD = 5e-3
def _lower_margin_activation(value: float, stop: float, warn: float) -> float:
if not all(np.isfinite(item) for item in (value, stop, warn)):
raise ValueError("activation values must be finite")
if stop >= warn:
raise ValueError("activation stop must be smaller than warn")
return float(np.clip((warn - value) / (warn - stop), 0.0, 1.0))
def _validated_transform(transform: np.ndarray) -> np.ndarray:
values = np.asarray(transform, dtype=float)
if values.shape != (4, 4) or not np.isfinite(values).all():
@@ -60,6 +68,15 @@ class PlacoIkSolver:
urdf_path: str,
dt: float,
arm: str,
*,
j3_reference_deg: float | None = None,
j3_weight: float = 1e-5,
j4_min_deg: float | None = None,
j4_warn_deg: float | None = None,
j4_weight: float = 1e-4,
manipulability_sigma_stop: float = 0.01,
manipulability_sigma_warn: float = 0.04,
manipulability_weight: float = 0.0,
) -> None:
if dt <= 0.0:
raise ValueError("dt must be positive")
@@ -134,12 +151,37 @@ class PlacoIkSolver:
]
)
self._actual_joints: np.ndarray | None = None
weights = (j3_weight, j4_weight, manipulability_weight)
if not all(np.isfinite(value) and value >= 0.0 for value in weights):
raise ValueError("QP auxiliary weights must be finite and non-negative")
if j3_reference_deg is not None and not np.isfinite(j3_reference_deg):
raise ValueError("J3 reference must be finite")
if (j4_min_deg is None) != (j4_warn_deg is None):
raise ValueError("J4 minimum and warning angles must be configured together")
if j4_min_deg is not None:
if not all(np.isfinite(value) for value in (j4_min_deg, j4_warn_deg)):
raise ValueError("J4 angles must be finite")
if j4_warn_deg <= j4_min_deg:
raise ValueError("J4 warning angle must exceed its minimum")
j4_limits_deg = np.degrees(self._joint_limits[3])
if j4_min_deg < j4_limits_deg[0] or j4_warn_deg > j4_limits_deg[1]:
raise ValueError("J4 safety angles must stay within URDF limits")
if not (
np.isfinite(manipulability_sigma_stop)
and np.isfinite(manipulability_sigma_warn)
and 0.0 < manipulability_sigma_stop
< manipulability_sigma_warn
):
raise ValueError(
"manipulability thresholds must satisfy 0 < stop < warn"
)
self._solver = placo.KinematicsSolver(self._robot)
self._solver.dt = dt
self._solver.mask_fbase(True)
for name in inactive_joint_names:
self._solver.mask_dof(name)
self._solver.enable_joint_limits(True)
self._solver.enable_velocity_limits(True)
self._frame_task = self._solver.add_relative_frame_task(
self._base_frame,
@@ -149,6 +191,53 @@ class PlacoIkSolver:
self._frame_task.configure("rm75_relative_frame", "soft", 1.0)
self._solver.add_kinetic_energy_regularization_task(1e-6)
self._j3_task = None
if j3_reference_deg is not None and j3_weight > 0.0:
self._j3_task = self._solver.add_joints_task()
self._j3_task.set_joints(
{self._joint_names[2]: np.deg2rad(j3_reference_deg)}
)
self._j3_task.configure("j3_reference", "soft", j3_weight)
self._j4_task = None
self._j4_constraint = None
self._j4_min = None
self._j4_warn = None
self._j4_weight = j4_weight
if j4_min_deg is not None:
self._j4_min = float(np.deg2rad(j4_min_deg))
self._j4_warn = float(np.deg2rad(j4_warn_deg))
self._j4_task = self._solver.add_joints_task()
self._j4_task.set_joints(
{self._joint_names[3]: self._j4_warn}
)
self._j4_task.configure("j4_soft_buffer", "soft", 0.0)
matrix = np.zeros((1, self._robot.state.q.size))
matrix[0, self._q_offsets[3]] = -1.0
self._j4_constraint = (
self._solver.add_joint_space_half_spaces_constraint(
matrix,
np.asarray([-self._j4_min]),
)
)
self._j4_constraint.configure("j4_lower_bound", "hard")
self._manipulability_task = None
self._manipulability_sigma_stop = manipulability_sigma_stop
self._manipulability_sigma_warn = manipulability_sigma_warn
self._manipulability_weight = manipulability_weight
if manipulability_weight > 0.0:
self._manipulability_task = self._solver.add_manipulability_task(
self._tcp_frame,
"both",
1.0,
)
self._manipulability_task.configure(
"tcp_6d_manipulability",
"soft",
0.0,
)
@property
def joint_names(self) -> list[str]:
return list(self._joint_names)
@@ -183,32 +272,64 @@ class PlacoIkSolver:
float(orientation_task.error_norm()),
)
def _active_tcp_jacobian(self) -> np.ndarray:
jacobian = np.asarray(
self._robot.frame_jacobian(
self._tcp_frame,
"local_world_aligned",
),
dtype=float,
)[:, self._v_offsets]
if jacobian.shape != (6, 7) or not np.isfinite(jacobian).all():
raise ValueError("TCP Jacobian must be a finite 6x7 matrix")
return jacobian
def _update_auxiliary_task_weights(self) -> None:
if self._j4_task is not None:
q4 = float(self._robot.state.q[self._q_offsets[3]])
activation = _lower_margin_activation(
q4,
self._j4_min,
self._j4_warn,
)
self._j4_task.configure(
"j4_soft_buffer",
"soft",
self._j4_weight * activation,
)
if self._manipulability_task is not None:
sigma_min = float(
np.linalg.svd(
self._active_tcp_jacobian(),
compute_uv=False,
)[-1]
)
activation = _lower_margin_activation(
sigma_min,
self._manipulability_sigma_stop,
self._manipulability_sigma_warn,
)
self._manipulability_task.configure(
"tcp_6d_manipulability",
"soft",
self._manipulability_weight * activation,
)
def _restore_actual_joint_state(self) -> None:
self._robot.state.q[self._q_offsets] = self._actual_joints
self._robot.update_kinematics()
def solve(self, target_tool_pose: np.ndarray) -> list[float]:
if self._actual_joints is None:
raise RuntimeError("joint state must be initialized before QP solve")
self._frame_task.T_a_b = _validated_transform(
target_tool_pose
)
result = np.asarray(
self._robot.state.q[self._q_offsets],
dtype=float,
).copy()
position_error, orientation_error = self._target_errors()
if (
position_error <= QP_POSITION_TOLERANCE_M
and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
):
return result.tolist()
for _ in range(QP_MAX_ITERATIONS):
previous = result
self._solver.solve(True)
self._robot.update_kinematics()
try:
self._frame_task.T_a_b = _validated_transform(
target_tool_pose
)
result = np.asarray(
self._robot.state.q[self._q_offsets],
dtype=float,
).copy()
self._validate_result(result, previous)
position_error, orientation_error = self._target_errors()
if (
position_error <= QP_POSITION_TOLERANCE_M
@@ -216,12 +337,32 @@ class PlacoIkSolver:
):
return result.tolist()
raise RuntimeError(
"QP did not converge after "
f"{QP_MAX_ITERATIONS} iterations: "
f"position_error={position_error:.6f} m, "
f"orientation_error={orientation_error:.6f} rad"
)
for _ in range(QP_MAX_ITERATIONS):
previous = result
self._update_auxiliary_task_weights()
self._solver.solve(True)
self._robot.update_kinematics()
result = np.asarray(
self._robot.state.q[self._q_offsets],
dtype=float,
).copy()
self._validate_result(result, previous)
position_error, orientation_error = self._target_errors()
if (
position_error <= QP_POSITION_TOLERANCE_M
and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
):
return result.tolist()
raise RuntimeError(
"QP did not converge after "
f"{QP_MAX_ITERATIONS} iterations: "
f"position_error={position_error:.6f} m, "
f"orientation_error={orientation_error:.6f} rad"
)
except Exception:
self._restore_actual_joint_state()
raise
def _validate_result(
self,
xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py
+108 -14
View File
@@ -201,6 +201,14 @@ class SingleArmVelocityTeleop(Node):
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("use_mock", True)
self.declare_parameter("robot_urdf_path", "")
self.declare_parameter("qp_j3_reference_deg", 0.0)
self.declare_parameter("qp_j3_weight", 1e-5)
self.declare_parameter("qp_j4_min_deg", 10.0)
self.declare_parameter("qp_j4_warn_deg", 25.0)
self.declare_parameter("qp_j4_weight", 1e-4)
self.declare_parameter("qp_manipulability_sigma_stop", 0.01)
self.declare_parameter("qp_manipulability_sigma_warn", 0.04)
self.declare_parameter("qp_manipulability_weight", 1e-4)
self.declare_parameter("robot_ip", "192.168.1.18")
self.declare_parameter("robot_port", 8080)
self.declare_parameter("realtime_push_host_ip", "")
@@ -260,6 +268,30 @@ class SingleArmVelocityTeleop(Node):
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._use_mock = self._bool_parameter("use_mock")
self._qp_j3_reference_deg = float(
self.get_parameter("qp_j3_reference_deg").value
)
self._qp_j3_weight = float(
self.get_parameter("qp_j3_weight").value
)
self._qp_j4_min_deg = float(
self.get_parameter("qp_j4_min_deg").value
)
self._qp_j4_warn_deg = float(
self.get_parameter("qp_j4_warn_deg").value
)
self._qp_j4_weight = float(
self.get_parameter("qp_j4_weight").value
)
self._qp_manipulability_sigma_stop = float(
self.get_parameter("qp_manipulability_sigma_stop").value
)
self._qp_manipulability_sigma_warn = float(
self.get_parameter("qp_manipulability_sigma_warn").value
)
self._qp_manipulability_weight = float(
self.get_parameter("qp_manipulability_weight").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")
@@ -328,6 +360,18 @@ class SingleArmVelocityTeleop(Node):
str(self.get_parameter("robot_urdf_path").value),
self._dt,
peripheral_arm,
j3_reference_deg=self._qp_j3_reference_deg,
j3_weight=self._qp_j3_weight,
j4_min_deg=self._qp_j4_min_deg,
j4_warn_deg=self._qp_j4_warn_deg,
j4_weight=self._qp_j4_weight,
manipulability_sigma_stop=(
self._qp_manipulability_sigma_stop
),
manipulability_sigma_warn=(
self._qp_manipulability_sigma_warn
),
manipulability_weight=self._qp_manipulability_weight,
)
debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
self._joint_state_pub = self.create_publisher(
@@ -730,12 +774,16 @@ class SingleArmVelocityTeleop(Node):
joint_target = self._solve_joint_target(target_pose)
qp_ms = (time.perf_counter_ns() - qp_started_ns) * 1e-6
send_started_ns = time.perf_counter_ns()
sent = self._send_joint_target(joint_target)
sent = self._send_and_commit_joint_target(
joint_target,
filtered_target,
filtered_orientation,
sent_target,
sent_orientation,
now,
)
send_ms = (time.perf_counter_ns() - send_started_ns) * 1e-6
if sent:
self._last_sent_target = sent_target
self._last_sent_orientation = sent_orientation.copy()
self._last_command_time = now
self._stop_sent = False
total_ms = (time.perf_counter_ns() - tick_started_ns) * 1e-6
try:
@@ -867,17 +915,15 @@ class SingleArmVelocityTeleop(Node):
def _filter_target(self, target: list[float]) -> list[float]:
if self._filtered_target is None:
self._filtered_target = list(target)
return list(target)
delta = [target[i] - self._filtered_target[i] for i in range(3)]
distance = _norm(delta)
alpha = self._adaptive_filter_alpha(distance)
self._filtered_target = [
return [
alpha * target[i] + (1.0 - alpha) * self._filtered_target[i]
for i in range(3)
]
return list(self._filtered_target)
def _adaptive_filter_alpha(self, distance: float) -> float:
if self._target_filter_fast_threshold_m <= 1e-9:
@@ -916,17 +962,15 @@ class SingleArmVelocityTeleop(Node):
def _filter_orientation_target(self, target_rotation: np.ndarray) -> np.ndarray:
if self._filtered_orientation_target is None:
self._filtered_orientation_target = _project_rotation(target_rotation)
return self._filtered_orientation_target.copy()
return _project_rotation(target_rotation)
error = _so3_log(
target_rotation @ self._filtered_orientation_target.T
)
self._filtered_orientation_target = _project_rotation(
return _project_rotation(
_so3_exp(self._orientation_filter_alpha * error)
@ self._filtered_orientation_target
)
return self._filtered_orientation_target.copy()
def _limit_orientation_step(
self,
@@ -1196,7 +1240,10 @@ class SingleArmVelocityTeleop(Node):
self._last_valid_joint_target = list(snapshot.positions)
return current_pose
def _solve_joint_target(self, target_pose: np.ndarray) -> list[float]:
def _solve_joint_target(
self,
target_pose: np.ndarray,
) -> list[float] | None:
if self._last_valid_joint_target is None:
raise RuntimeError("valid joint feedback has not been initialized")
try:
@@ -1206,10 +1253,28 @@ class SingleArmVelocityTeleop(Node):
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 None
return list(result)
def _send_and_commit_joint_target(
self,
joint_target: list[float] | None,
filtered_target: list[float],
filtered_orientation: np.ndarray,
sent_target: list[float],
sent_orientation: np.ndarray,
now: Time,
) -> bool:
if joint_target is None or not self._send_joint_target(joint_target):
return False
self._last_valid_joint_target = list(joint_target)
self._filtered_target = list(filtered_target)
self._filtered_orientation_target = filtered_orientation.copy()
self._last_sent_target = list(sent_target)
self._last_sent_orientation = sent_orientation.copy()
self._last_command_time = now
return True
def _safe_stop(self, reset_active: bool) -> None:
if not self._stop_sent:
self._send_stop_once()
@@ -1461,6 +1526,35 @@ class SingleArmVelocityTeleop(Node):
raise ValueError("joint_max_speed must be > 0")
if self._joint_command_max_acceleration <= 0.0:
raise ValueError("joint_max_acc must be > 0")
qp_weights = (
self._qp_j3_weight,
self._qp_j4_weight,
self._qp_manipulability_weight,
)
if not all(
math.isfinite(value) and value >= 0.0
for value in qp_weights
):
raise ValueError("QP auxiliary weights must be finite and non-negative")
if not math.isfinite(self._qp_j3_reference_deg):
raise ValueError("qp_j3_reference_deg must be finite")
if not all(
math.isfinite(value)
for value in (self._qp_j4_min_deg, self._qp_j4_warn_deg)
):
raise ValueError("QP J4 angles must be finite")
if self._qp_j4_warn_deg <= self._qp_j4_min_deg:
raise ValueError("qp_j4_warn_deg must exceed qp_j4_min_deg")
if not (
math.isfinite(self._qp_manipulability_sigma_stop)
and math.isfinite(self._qp_manipulability_sigma_warn)
and 0.0 < self._qp_manipulability_sigma_stop
< self._qp_manipulability_sigma_warn
):
raise ValueError(
"QP manipulability sigma thresholds must satisfy "
"0 < stop < warn"
)
def _shutdown_tool_worker(self) -> None:
if self._tool_worker_thread is None or self._tool_command_queue is None: