Remove redundant code
This commit is contained in:
Saberlve
parent
99546165ec
commit
b2875851ae
@ -1,43 +0,0 @@
|
||||
robot:
|
||||
type: uf::robot
|
||||
id: "uf_robot_cartesian"
|
||||
robot_dof: 7
|
||||
control_space: "cartesian"
|
||||
robot_ip: "192.168.1.245"
|
||||
gripper_type: 1
|
||||
enable_logs: false
|
||||
max_linear_velocity: 200
|
||||
min_tcp_z_mm: 50
|
||||
gripper_error_log_path: "logs/xarm7_gripper_errors.log"
|
||||
cameras:
|
||||
camera:
|
||||
type: intelrealsense
|
||||
serial_number_or_name: "242622070583"
|
||||
width: 640
|
||||
height: 480
|
||||
fps: 30
|
||||
camera2:
|
||||
type: intelrealsense
|
||||
serial_number_or_name: "148522072685"
|
||||
width: 640
|
||||
height: 480
|
||||
fps: 30
|
||||
|
||||
teleop:
|
||||
type: uf::gello_teleop
|
||||
id: "gello_teleop"
|
||||
port: "/dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTB9HYVD-if00-port0"
|
||||
joint_ids: [1, 2, 3, 4, 5, 6, 7]
|
||||
joint_signs: [1, 1, 1, 1, 1, 1, 1]
|
||||
gripper_id: 8
|
||||
gripper_open_deg: 198.28125
|
||||
gripper_close_deg: 155.75
|
||||
|
||||
dataset:
|
||||
root: "datasets/xarm7_gello_cartesian_datas"
|
||||
repo_id: "ufactory/xarm7_gello_cartesian_datas"
|
||||
single_task: "Pick up the purple grape and drop into the box on the left."
|
||||
fps: 60
|
||||
episode_time_s: 60
|
||||
reset_time_s: 20
|
||||
push_to_hub: False
|
||||
@ -130,7 +130,6 @@ class UFRobot(Robot, Thread):
|
||||
self._local_joint_origins = None
|
||||
self._local_model_fault_count = 0
|
||||
self._last_safe_joint_target = None
|
||||
self._last_safe_cartesian_target = None
|
||||
self._last_guard_path = "not_run"
|
||||
self._tcp_z_is_clamped = False
|
||||
self._tcp_z_last_log_time = 0.0
|
||||
@ -139,8 +138,8 @@ class UFRobot(Robot, Thread):
|
||||
self.report_stop_event = Event()
|
||||
self._rt_report_normal = False
|
||||
self._update_lock = Lock()
|
||||
# The TCP z guard uses the asynchronous RT report to avoid a blocking
|
||||
# FK request on every GELLO servo cycle.
|
||||
# Cartesian observations and the joint-mode TCP z guard use the
|
||||
# asynchronous RT report.
|
||||
self._use_rt_report = (
|
||||
self._control_space == "cartesian" or self._min_tcp_z_mm is not None
|
||||
)
|
||||
@ -310,10 +309,9 @@ class UFRobot(Robot, Thread):
|
||||
|
||||
def _initialize_tcp_z_guard(self) -> None:
|
||||
"""Initialize guard state from the robot's current physical target."""
|
||||
if self._min_tcp_z_mm is None or self.real_arm is None:
|
||||
if self._control_space != "joint" or self._min_tcp_z_mm is None or self.real_arm is None:
|
||||
return
|
||||
|
||||
if self._control_space == "joint":
|
||||
code, states = self.real_arm.get_joint_states(is_radian=True, num=1)
|
||||
if code != 0 or not states or len(states[0]) < self._dof:
|
||||
raise RuntimeError(f"Unable to initialize TCP z guard from joint state, code={code}")
|
||||
@ -330,15 +328,6 @@ class UFRobot(Robot, Thread):
|
||||
f"Current TCP z {current_z:.2f} mm is below the hard floor "
|
||||
f"{self._min_tcp_z_mm:.2f} mm"
|
||||
)
|
||||
else:
|
||||
code, pose = self.real_arm.get_position_aa(is_radian=True)
|
||||
if code != 0 or len(pose) < 6:
|
||||
raise RuntimeError(f"Unable to initialize TCP z guard from TCP pose, code={code}")
|
||||
target = np.asarray(pose[:6], dtype=np.float64)
|
||||
if not np.all(np.isfinite(target)):
|
||||
raise RuntimeError("Unable to initialize TCP z guard from non-finite TCP pose")
|
||||
self._last_safe_cartesian_target = target
|
||||
|
||||
self._tcp_z_is_clamped = False
|
||||
self._tcp_z_last_log_time = 0.0
|
||||
self._tcp_z_last_error_log_time = 0.0
|
||||
@ -673,30 +662,6 @@ class UFRobot(Robot, Thread):
|
||||
and actual_z > self._min_tcp_z_mm + activation_margin
|
||||
)
|
||||
|
||||
def _guard_cartesian_target(self, command: list[float]) -> np.ndarray | None:
|
||||
"""Clamp a Cartesian target without changing its other five components."""
|
||||
target = np.asarray(command, dtype=np.float64)
|
||||
if self._min_tcp_z_mm is None:
|
||||
return target
|
||||
|
||||
fallback = self._last_safe_cartesian_target
|
||||
try:
|
||||
if target.shape != (6,) or not np.all(np.isfinite(target)):
|
||||
raise ValueError("Cartesian target has invalid shape or contains NaN/Inf")
|
||||
requested_z = float(target[2])
|
||||
if requested_z < self._min_tcp_z_mm:
|
||||
target[2] = self._min_tcp_z_mm
|
||||
self._log_tcp_z_clamp(True, requested_z)
|
||||
else:
|
||||
self._log_tcp_z_clamp(False)
|
||||
self._last_safe_cartesian_target = target.copy()
|
||||
return target
|
||||
except Exception as exc:
|
||||
self._log_tcp_z_guard_error(str(exc))
|
||||
if fallback is None:
|
||||
return None
|
||||
return np.asarray(fallback, dtype=np.float64).copy()
|
||||
|
||||
def configure(self) -> None:
|
||||
self.real_arm.motion_enable()
|
||||
self.real_arm.clean_error()
|
||||
@ -804,21 +769,6 @@ class UFRobot(Robot, Thread):
|
||||
self._is_calibrated = True
|
||||
pass # CHECK! currently No-op
|
||||
|
||||
def get_joint_observation(self) -> dict[str, float]:
|
||||
"""Read joint positions for teleoperator alignment in Cartesian mode."""
|
||||
if self.real_arm is None or not self._is_connected:
|
||||
raise ConnectionError("UF Robot is not connected")
|
||||
code, states = self.real_arm.get_joint_states(is_radian=True, num=1)
|
||||
if code != 0 or not states or len(states[0]) < self._dof:
|
||||
raise RuntimeError(f"Failed to read xArm joint states, code={code}")
|
||||
positions = np.asarray(states[0][:self._dof], dtype=np.float64)
|
||||
if not np.all(np.isfinite(positions)):
|
||||
raise RuntimeError("xArm joint states contain NaN or Inf")
|
||||
return {
|
||||
f"{self.prefix}J{i + 1}.pos": float(position)
|
||||
for i, position in enumerate(positions)
|
||||
}
|
||||
|
||||
def get_observation(self) -> dict[str, np.ndarray]:
|
||||
obs_dict = {}
|
||||
self._log_controller_error_if_changed("get_observation")
|
||||
@ -1097,48 +1047,6 @@ class UFRobot(Robot, Thread):
|
||||
if code is not None and code != 0:
|
||||
raise RuntimeError(f"{command} failed, code={code}, {self._motion_status()}")
|
||||
|
||||
def joint_action_to_cartesian(self, action: dict) -> dict:
|
||||
"""Convert an absolute joint action to an xArm axis-angle pose.
|
||||
|
||||
GELLO reports absolute joint positions while Cartesian control expects
|
||||
``pose.x/y/z/rx/ry/rz``. The xArm controller's FK is used so the
|
||||
configured robot model and tool frame stay authoritative. The returned
|
||||
action is passed through ``send_action`` for Cartesian safety checks.
|
||||
"""
|
||||
if self.real_arm is None or not self._is_connected:
|
||||
raise ConnectionError("UF Robot is not connected")
|
||||
|
||||
joint_keys = [f"{self.prefix}J{i}.pos" for i in range(1, self._dof + 1)]
|
||||
try:
|
||||
joints = [float(action[key]) for key in joint_keys]
|
||||
except (KeyError, TypeError, ValueError) as exc:
|
||||
raise ValueError(f"Invalid joint action; expected keys {joint_keys}") from exc
|
||||
|
||||
if not np.all(np.isfinite(joints)):
|
||||
raise ValueError("Joint action contains NaN or Inf")
|
||||
|
||||
code, pose = self.real_arm.get_forward_kinematics(
|
||||
joints,
|
||||
input_is_radian=True,
|
||||
return_is_radian=True,
|
||||
)
|
||||
pose = np.asarray(pose, dtype=np.float64)
|
||||
if code != 0 or pose.shape[0] < 6 or not np.all(np.isfinite(pose[:6])):
|
||||
raise RuntimeError(f"xArm forward kinematics failed, code={code}, pose={pose}")
|
||||
|
||||
cartesian = {
|
||||
f"{self.prefix}pose.x": float(pose[0]),
|
||||
f"{self.prefix}pose.y": float(pose[1]),
|
||||
f"{self.prefix}pose.z": float(pose[2]),
|
||||
f"{self.prefix}pose.rx": float(pose[3]),
|
||||
f"{self.prefix}pose.ry": float(pose[4]),
|
||||
f"{self.prefix}pose.rz": float(pose[5]),
|
||||
}
|
||||
gripper_key = f"{self.prefix}gripper.pos"
|
||||
if gripper_key in action:
|
||||
cartesian[gripper_key] = float(action[gripper_key])
|
||||
return cartesian
|
||||
|
||||
def send_action(self, action: dict) -> np.ndarray:
|
||||
if not self._is_connected:
|
||||
raise ConnectionError()
|
||||
@ -1228,12 +1136,7 @@ class UFRobot(Robot, Thread):
|
||||
if not self._rt_report_normal:
|
||||
raise ConnectionError("RT Report for target robot NOT READY! ")
|
||||
cmd_list = [action[f"{self.prefix}pose.x"], action[f"{self.prefix}pose.y"], action[f"{self.prefix}pose.z"], action[f"{self.prefix}pose.rx"], action[f"{self.prefix}pose.ry"], action[f"{self.prefix}pose.rz"]]
|
||||
safe_cmd = self._guard_cartesian_target(cmd_list)
|
||||
if safe_cmd is not None:
|
||||
safe_cmd = safe_cmd.tolist()
|
||||
for i, key in enumerate(("x", "y", "z", "rx", "ry", "rz")):
|
||||
safe_action[f"{self.prefix}pose.{key}"] = float(safe_cmd[i])
|
||||
self.real_arm.set_position_aa(axis_angle_pose=safe_cmd, speed=lin_spd, is_radian=True, wait=False)
|
||||
self.real_arm.set_position_aa(axis_angle_pose=cmd_list, speed=lin_spd, is_radian=True, wait=False)
|
||||
# self.real_arm.set_position(*cmd_list, radius=0, speed=lin_spd, is_radian=True, wait=False)
|
||||
|
||||
if self._cmd_cnt < 99999:
|
||||
|
||||
@ -8,7 +8,6 @@ from dataclasses import asdict, dataclass
|
||||
from pprint import pformat
|
||||
import numpy as np
|
||||
import lerobot_robot_ufactory # patch
|
||||
from lerobot.scripts.lerobot_record import register_third_party_plugins
|
||||
from lerobot.datasets.pipeline_features import aggregate_pipeline_dataset_features, create_initial_features
|
||||
from lerobot.datasets.utils import build_dataset_frame, combine_feature_dicts
|
||||
from lerobot.policies.utils import make_robot_action
|
||||
@ -56,15 +55,6 @@ def continuous_rotvec(new_rv, prev_rv):
|
||||
new_rv = -(2 * np.pi - angle) * axis
|
||||
return new_rv
|
||||
|
||||
def blend_poses(pose_a, pose_b, alpha):
|
||||
"""位姿混合: (1-alpha)*A + alpha*B, 旋转用 SO(3) 插值。
|
||||
先将 pose_b 的 rotvec 归一化到与 pose_a 同符号半球，避免 ±π 跳变破坏线性混合。"""
|
||||
blended_pos = (1 - alpha) * np.array(pose_a[:3]) + alpha * np.array(pose_b[:3])
|
||||
# 旋转用线性混合 rotvec (delta 很小时近似 SLERP)
|
||||
rot_b = continuous_rotvec(np.array(pose_b[3:6]), np.array(pose_a[3:6]))
|
||||
blended_rot = (1 - alpha) * np.array(pose_a[3:6]) + alpha * rot_b
|
||||
return np.concatenate([blended_pos, blended_rot]).tolist()
|
||||
|
||||
def compute_relative_axis_angle(rot_prev, rot_curr):
|
||||
"""
|
||||
计算两个轴角之间的相对旋转。
|
||||
@ -212,24 +202,6 @@ def eval_loop(cfg: EvalConfig, relative=False, rx_continuous=False, safety_guard
|
||||
device = get_safe_torch_device(policy.config.device, log=True)
|
||||
sleep_time_s = 1 / dataset_metadata.fps
|
||||
|
||||
# Gripper look-ahead: denormalization stats for peeking into the action queue
|
||||
_gripper_mean = dataset_metadata.stats['action']['mean'][-1].item()
|
||||
_gripper_std = dataset_metadata.stats['action']['std'][-1].item()
|
||||
_gripper_min = dataset_metadata.stats['action']['min'][-1].item()
|
||||
_gripper_max = dataset_metadata.stats['action']['max'][-1].item()
|
||||
is_act_policy = hasattr(policy.config, 'chunk_size')
|
||||
# ACT: lookahead 30 (~1s) 补偿 chunk 慢启动; DP: 队列仅 8 步，lookahead 4
|
||||
GRIPPER_LOOKAHEAD = 0 if is_act_policy else 4
|
||||
|
||||
# =====================================================
|
||||
# Chunk boundary smoothing: only damp large discontinuities at action chunk
|
||||
# boundaries (mean ~6mm jump) while preserving smooth within-chunk motion (~1mm).
|
||||
# When step-to-step cmd change exceeds SMOOTH_THRESHOLD, clamp it to that limit.
|
||||
# =====================================================
|
||||
SMOOTH_THRESHOLD = 0 # mm: smooth chunk boundary jumps while preserving trajectory
|
||||
SMOOTH_ROT_THRESHOLD = 0.05 # rad: max allowed rotation jump per step
|
||||
prev_smoothed_pose = None
|
||||
|
||||
print("\n********** Policy Eval Episode Loop Start **********")
|
||||
print(f'relative: {relative}')
|
||||
|
||||
@ -248,10 +220,8 @@ def eval_loop(cfg: EvalConfig, relative=False, rx_continuous=False, safety_guard
|
||||
prev_robot_dict = {}
|
||||
prev_action_dict = {}
|
||||
|
||||
is_multiple_robot = False
|
||||
if hasattr(cfg.robot, 'robots'):
|
||||
keys = cfg.robot.robots.keys()
|
||||
is_multiple_robot = True
|
||||
else:
|
||||
keys = ['']
|
||||
for key in keys:
|
||||
@ -284,7 +254,6 @@ def eval_loop(cfg: EvalConfig, relative=False, rx_continuous=False, safety_guard
|
||||
obs = robot.get_observation()
|
||||
|
||||
curr_robot_dict = {}
|
||||
curr_action_dict = {}
|
||||
for key in keys:
|
||||
prefix = f'.{key}' if key else ''
|
||||
is_tcp = f'{prefix}pose.x' in obs and f'{prefix}pose.y' in obs and f'{prefix}pose.z' in obs and f'{prefix}pose.rx' in obs and f'{prefix}pose.ry' in obs and f'{prefix}pose.rz' in obs
|
||||
@ -348,67 +317,13 @@ def eval_loop(cfg: EvalConfig, relative=False, rx_continuous=False, safety_guard
|
||||
|
||||
# robot_action_to_send[f'{prefix}pose.z'] = max(robot_action_to_send[f'{prefix}pose.z'], 199)
|
||||
|
||||
# Rate-limited smoothing: cap position velocity to reduce chunk boundary jerks
|
||||
# Uses vector-norm clamping to preserve motion direction
|
||||
if SMOOTH_THRESHOLD > 0:
|
||||
pos_keys = [f'{prefix}pose.x', f'{prefix}pose.y', f'{prefix}pose.z']
|
||||
rot_keys = [f'{prefix}pose.rx', f'{prefix}pose.ry', f'{prefix}pose.rz']
|
||||
if prev_smoothed_pose is None:
|
||||
prev_smoothed_pose = {k: robot_action_to_send[k] for k in pos_keys + rot_keys}
|
||||
else:
|
||||
# Vector-norm clamp on position (preserves direction)
|
||||
delta_pos = np.array([robot_action_to_send[k] - prev_smoothed_pose[k] for k in pos_keys])
|
||||
norm = np.linalg.norm(delta_pos)
|
||||
if norm > SMOOTH_THRESHOLD:
|
||||
delta_pos = delta_pos * (SMOOTH_THRESHOLD / norm)
|
||||
for i, k in enumerate(pos_keys):
|
||||
prev_smoothed_pose[k] = prev_smoothed_pose[k] + delta_pos[i]
|
||||
robot_action_to_send[k] = prev_smoothed_pose[k]
|
||||
# Per-axis clamp on rotation
|
||||
for k in rot_keys:
|
||||
delta = robot_action_to_send[k] - prev_smoothed_pose[k]
|
||||
if abs(delta) > SMOOTH_ROT_THRESHOLD:
|
||||
delta = SMOOTH_ROT_THRESHOLD * (1 if delta > 0 else -1)
|
||||
prev_smoothed_pose[k] = prev_smoothed_pose[k] + delta
|
||||
robot_action_to_send[k] = prev_smoothed_pose[k]
|
||||
|
||||
if relative:
|
||||
curr_action_pose = np.array([
|
||||
robot_action_to_send[f'{prefix}pose.x'], robot_action_to_send[f'{prefix}pose.y'], robot_action_to_send[f'{prefix}pose.z'],
|
||||
robot_action_to_send[f'{prefix}pose.rx'], robot_action_to_send[f'{prefix}pose.ry'], robot_action_to_send[f'{prefix}pose.rz']
|
||||
])
|
||||
# 相对增量模式: 漂移修正，将指令位姿温和拉回实际位姿
|
||||
# prev_action_pose = blend_poses(curr_action_pose, curr_robot_pose, 0.05)
|
||||
prev_action_dict[key]['pose'] = curr_action_pose
|
||||
|
||||
# # Gripper look-ahead: peek ahead in the action queue to compensate
|
||||
# # for the slow ramp in the ACT chunk (eliminates 1-2s gripper delay)
|
||||
# gripper_raw = robot_action_to_send.get('left.gripper.pos', 0)
|
||||
# if hasattr(policy, '_action_queue') and len(policy._action_queue) > 0:
|
||||
# # ACT: 队列为 deque of tensors, 归一化方式 MEAN_STD
|
||||
# lookahead_idx = min(GRIPPER_LOOKAHEAD, len(policy._action_queue) - 1)
|
||||
# future_gripper_norm = policy._action_queue[lookahead_idx][0, -1].item()
|
||||
# gripper_raw = future_gripper_norm * _gripper_std + _gripper_mean
|
||||
# elif hasattr(policy, '_queues') and 'action' in policy._queues and len(policy._queues['action']) > 0:
|
||||
# # DP: 队列结构不同, 归一化方式 MIN_MAX → [-1,1] → [min,max]
|
||||
# lookahead_idx = min(GRIPPER_LOOKAHEAD, len(policy._queues['action']) - 1)
|
||||
# future_gripper_norm = policy._queues['action'][lookahead_idx][0, -1].item()
|
||||
# gripper_raw = (future_gripper_norm + 1) / 2 * (_gripper_max - _gripper_min) + _gripper_min
|
||||
# robot_action_to_send['left.gripper.pos'] = 1.0 if gripper_raw > 0.4 else 0.0
|
||||
|
||||
# gripper_raw = robot_action_to_send.get('right.gripper.pos', 0)
|
||||
# if hasattr(policy, '_action_queue') and len(policy._action_queue) > 0:
|
||||
# # ACT: 队列为 deque of tensors, 归一化方式 MEAN_STD
|
||||
# lookahead_idx = min(GRIPPER_LOOKAHEAD, len(policy._action_queue) - 1)
|
||||
# future_gripper_norm = policy._action_queue[lookahead_idx][0, -1].item()
|
||||
# gripper_raw = future_gripper_norm * _gripper_std + _gripper_mean
|
||||
# elif hasattr(policy, '_queues') and 'action' in policy._queues and len(policy._queues['action']) > 0:
|
||||
# # DP: 队列结构不同, 归一化方式 MIN_MAX → [-1,1] → [min,max]
|
||||
# lookahead_idx = min(GRIPPER_LOOKAHEAD, len(policy._queues['action']) - 1)
|
||||
# future_gripper_norm = policy._queues['action'][lookahead_idx][0, -1].item()
|
||||
# gripper_raw = (future_gripper_norm + 1) / 2 * (_gripper_max - _gripper_min) + _gripper_min
|
||||
# robot_action_to_send['right.gripper.pos'] = 1.0 if gripper_raw > 0.4 else 0.0
|
||||
|
||||
# Safety check: any violation triggers an e-stop; the action is
|
||||
# NOT sent and the loop waits for the operator to press right arrow
|
||||
if safety_guard is not None:
|
||||
|
||||
@ -484,23 +484,6 @@ def record_loop(
|
||||
else:
|
||||
act = teleop.get_action()
|
||||
|
||||
# GELLO reports absolute joint positions. In Cartesian robot mode,
|
||||
# convert them with the xArm FK before the normal action pipeline;
|
||||
# send_action() then applies the Cartesian safety guard and sends
|
||||
# the resulting pose with set_position_aa().
|
||||
joint_action_keys = [
|
||||
f"{getattr(robot, 'prefix', '')}J{i}.pos"
|
||||
for i in range(1, getattr(robot, "_dof", 0) + 1)
|
||||
]
|
||||
if (
|
||||
getattr(robot, "_control_space", None) == "cartesian"
|
||||
and hasattr(robot, "joint_action_to_cartesian")
|
||||
and joint_action_keys
|
||||
and act is not None
|
||||
and all(key in act for key in joint_action_keys)
|
||||
):
|
||||
act = robot.joint_action_to_cartesian(act)
|
||||
|
||||
# (space mouse) from delta Cartesian cmd to absolute command
|
||||
if act is not None and "pose.dx" in act:
|
||||
last_robot_cmd.update({"pose.x": last_robot_cmd["pose.x"] + act["pose.dx"], "pose.y": last_robot_cmd["pose.y"] + act["pose.dy"], "pose.z": last_robot_cmd["pose.z"] + act["pose.dz"]})
|
||||
@ -602,11 +585,6 @@ def _prepare_recording_episode(robot, teleop, is_uf_teleop, manual_mode):
|
||||
|
||||
if is_uf_teleop:
|
||||
obs = robot.get_observation()
|
||||
# Cartesian observations expose TCP pose, while GELLO alignment needs
|
||||
# the current absolute joint positions.
|
||||
joint_observation = getattr(robot, "get_joint_observation", None)
|
||||
if joint_observation is not None:
|
||||
obs.update(joint_observation())
|
||||
teleop.set_teleop_enabled(True, obs)
|
||||
|
||||
|
||||
|
||||
@ -10,7 +10,6 @@ from dataclasses import asdict, dataclass
|
||||
from datetime import datetime
|
||||
from pprint import pformat
|
||||
import lerobot_robot_ufactory # patch
|
||||
from lerobot.scripts.lerobot_record import register_third_party_plugins
|
||||
from lerobot.processor import (
|
||||
make_default_processors,
|
||||
)
|
||||
|
||||
@ -43,7 +43,6 @@ def make_guard_robot(arm, min_tcp_z_mm=100.0, control_space="joint"):
|
||||
robot._control_space = control_space
|
||||
robot._min_tcp_z_mm = min_tcp_z_mm
|
||||
robot._last_safe_joint_target = np.asarray([0.25] * 7)
|
||||
robot._last_safe_cartesian_target = np.asarray([250.0, 0.0, 120.0, 0.0, 0.0, 0.0])
|
||||
robot._tcp_z_is_clamped = False
|
||||
robot._tcp_z_last_log_time = 0.0
|
||||
robot._tcp_z_last_error_log_time = 0.0
|
||||
@ -145,14 +144,6 @@ def test_joint_guard_rejects_discontinuous_ik_solution():
|
||||
assert np.allclose(result, [0.25] * 7)
|
||||
|
||||
|
||||
def test_cartesian_guard_preserves_other_axes_and_clamps_z():
|
||||
robot = make_guard_robot(object(), control_space="cartesian")
|
||||
|
||||
result = robot._guard_cartesian_target([300.0, 20.0, 90.0, 0.1, 0.2, 0.3])
|
||||
|
||||
assert result == pytest.approx([300.0, 20.0, 100.0, 0.1, 0.2, 0.3])
|
||||
|
||||
|
||||
def test_send_action_sends_and_returns_clamped_joint_target():
|
||||
arm = FakeKinematicsArm()
|
||||
arm.error_code = 0
|
||||
|
||||
Loading…
Reference in New Issue
Block a user