Добавлено управление роботом
This commit is contained in:
Даниил Грабарь
@@ -0,0 +1,800 @@
|
||||
import numpy as np
|
||||
import pytest
|
||||
import pickle
|
||||
from multiprocessing import get_context
|
||||
from queue import Empty, Queue
|
||||
from types import SimpleNamespace
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
from iiwa_genesis.control import (
|
||||
CartesianCommand,
|
||||
JointCommand,
|
||||
RobotController,
|
||||
VelocityLimitedMotion,
|
||||
configure_robot_control,
|
||||
base_pose_to_world,
|
||||
parse_command,
|
||||
GuiCommandQueue,
|
||||
)
|
||||
from iiwa_genesis.control_gui import ControlGui, _observe_parent_close, command_from_fields
|
||||
from iiwa_genesis.scene import SceneMotionController
|
||||
|
||||
|
||||
IIWA_VELOCITY_LIMITS = (1.71, 1.71, 1.75, 2.27, 2.44, 3.14, 3.14)
|
||||
|
||||
|
||||
class FakeRobot:
|
||||
def __init__(self):
|
||||
self.calls = []
|
||||
self.position = np.zeros(7)
|
||||
|
||||
def control_dofs_position(self, position, dofs_idx_local=None):
|
||||
self.position = np.asarray(position, dtype=float)
|
||||
self.calls.append(("control", self.position.copy(), dofs_idx_local))
|
||||
|
||||
def get_dofs_position(self, dofs_idx_local=None):
|
||||
self.calls.append(("position", dofs_idx_local))
|
||||
return self.position
|
||||
|
||||
def get_link(self, name):
|
||||
self.calls.append(("link", name))
|
||||
return name
|
||||
|
||||
def inverse_kinematics(self, **kwargs):
|
||||
self.calls.append(("ik", kwargs))
|
||||
qpos = np.arange(7, dtype=float) * .2
|
||||
return (qpos, np.zeros(6)) if kwargs.get("return_error") else qpos
|
||||
|
||||
def set_dofs_kp(self, kp, dofs_idx_local=None):
|
||||
self.calls.append(("kp", np.asarray(kp), dofs_idx_local))
|
||||
|
||||
def set_dofs_kv(self, kv, dofs_idx_local=None):
|
||||
self.calls.append(("kv", np.asarray(kv), dofs_idx_local))
|
||||
|
||||
def set_dofs_force_range(self, lower, upper, dofs_idx_local=None):
|
||||
self.calls.append(("force", np.asarray(lower), np.asarray(upper), dofs_idx_local))
|
||||
|
||||
|
||||
def test_parses_joint_mode_and_seven_angles():
|
||||
command = parse_command("joints 0 0.2 0 1.0 0 -0.3 0.1")
|
||||
assert isinstance(command, JointCommand)
|
||||
np.testing.assert_allclose(command.angles, [0, 0.2, 0, 1.0, 0, -0.3, 0.1])
|
||||
|
||||
|
||||
def test_set_uses_all_seven_values_after_selecting_joints_mode():
|
||||
command = parse_command("set 0 0.2 0 1.0 0 0 0", mode="joints")
|
||||
assert isinstance(command, JointCommand)
|
||||
np.testing.assert_allclose(command.angles, [0, 0.2, 0, 1.0, 0, 0, 0])
|
||||
|
||||
|
||||
def test_selected_joints_mode_accepts_bare_angle_values():
|
||||
command = parse_command("0 0.2 0 1.0 0 0 0", mode="joints")
|
||||
assert isinstance(command, JointCommand)
|
||||
np.testing.assert_allclose(command.angles, [0, 0.2, 0, 1.0, 0, 0, 0])
|
||||
|
||||
|
||||
def test_parses_cartesian_tcp_pose_in_robot_base_frame():
|
||||
command = parse_command("cartesian 0.35 0 0.55 0 1.57 0")
|
||||
assert isinstance(command, CartesianCommand)
|
||||
np.testing.assert_allclose(command.position, [0.35, 0, 0.55])
|
||||
np.testing.assert_allclose(command.rpy, [0, 1.57, 0])
|
||||
|
||||
|
||||
def test_cartesian_target_is_transformed_from_robot_base_to_world():
|
||||
pose = base_pose_to_world([1, 2, 3], [0, 0, 90], [0.5, 0, 0.2], [0, 0, 0])
|
||||
np.testing.assert_allclose(pose[:3, 3], [1, 2.5, 3.2], atol=1e-7)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("line", ["joints 0 0 0", "cartesian 0 0 0 0", "mode unknown"])
|
||||
def test_rejects_invalid_commands(line):
|
||||
with pytest.raises(ValueError):
|
||||
parse_command(line)
|
||||
|
||||
|
||||
def test_controller_sends_joint_target_to_position_controller():
|
||||
robot = FakeRobot()
|
||||
controller = RobotController(robot, [1, 2, 3, 4, 5, 6, 7],
|
||||
{"position": [1, 2, 3], "euler": [0, 0, 0]})
|
||||
controller.apply(JointCommand((0, 1, 2, 3, 4, 5, 6)))
|
||||
assert robot.calls[0][0] == "control"
|
||||
np.testing.assert_array_equal(robot.calls[0][1], [0, 1, 2, 3, 4, 5, 6])
|
||||
|
||||
|
||||
def test_configures_position_controller_for_all_arm_dofs():
|
||||
robot = FakeRobot()
|
||||
configure_robot_control(robot, [1, 2, 3, 4, 5, 6, 7])
|
||||
assert [call[0] for call in robot.calls] == ["kp", "kv", "force"]
|
||||
|
||||
|
||||
def test_controller_sends_cartesian_target_to_ik_in_world_frame():
|
||||
robot = FakeRobot()
|
||||
controller = RobotController(robot, list(range(7)),
|
||||
{"position": [1, 2, 3], "euler": [0, 0, 0]}, "tcp")
|
||||
controller.apply(CartesianCommand((0.5, 0, 0.2), (0, 0, 0)))
|
||||
ik = next(call[1] for call in robot.calls if call[0] == "ik")
|
||||
np.testing.assert_allclose(ik["pos"], [1.5, 2, 3.2])
|
||||
assert ik["link"] == "tcp"
|
||||
|
||||
|
||||
def test_cartesian_ik_accepts_componentwise_genesis_tolerances():
|
||||
robot = FakeRobot()
|
||||
robot.inverse_kinematics = lambda **kwargs: (np.zeros(7), np.full(6, 0.0004))
|
||||
controller = SceneMotionController(robot, list(range(7)), motion_config())
|
||||
controller.apply(CartesianCommand((0.5, 0, 0.2), (0, 0, 0)), 50)
|
||||
|
||||
|
||||
def test_scene_motion_controller_plans_gui_cartesian_command_at_selected_speed():
|
||||
robot = FakeRobot()
|
||||
controller = SceneMotionController(
|
||||
robot,
|
||||
list(range(7)),
|
||||
{
|
||||
"scene": {"mode": "with_gripper", "dt": 0.1},
|
||||
"robot": {
|
||||
"urdf": "iiwa_genesis_description/urdf/iiwa_with_gripper.urdf",
|
||||
"position": [1, 2, 3],
|
||||
"euler": [0, 0, 0],
|
||||
},
|
||||
},
|
||||
)
|
||||
|
||||
controller.apply_gui({
|
||||
"type": "cartesian", "values": [0.5, 0, 0.2, 0, 0, 0],
|
||||
"speed_percent": 50,
|
||||
})
|
||||
controller.step()
|
||||
|
||||
ik = next(call[1] for call in robot.calls if call[0] == "ik")
|
||||
np.testing.assert_allclose(ik["pos"], [1.5, 2, 3.2])
|
||||
assert ik["link"] == "tcp"
|
||||
np.testing.assert_allclose(
|
||||
robot.calls[-1][1], [0, .0855, .0875, .1135, .122, .157, .157])
|
||||
|
||||
|
||||
class LaggingRobot(FakeRobot):
|
||||
def control_dofs_position(self, position, dofs_idx_local=None):
|
||||
self.calls.append(("control", np.asarray(position).copy(), dofs_idx_local))
|
||||
|
||||
|
||||
def motion_config(mode="with_gripper"):
|
||||
return {
|
||||
"scene": {"mode": mode, "dt": .1},
|
||||
"robot": {
|
||||
"urdf": f"iiwa_genesis_description/urdf/iiwa_{mode}.urdf",
|
||||
"position": [1, 2, 3], "euler": [0, 0, 90],
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
@pytest.mark.parametrize("mode,link", [("with_gripper", "tcp"), ("without_gripper", "link7")])
|
||||
def test_queued_cartesian_motion_advances_despite_lagging_feedback(mode, link):
|
||||
robot = LaggingRobot()
|
||||
controller = SceneMotionController(robot, list(range(7)), motion_config(mode))
|
||||
commands = Queue()
|
||||
commands.put({"type": "cartesian", "values": [.5, 0, .2, 0, 0, 0],
|
||||
"speed_percent": 50})
|
||||
controller.apply_gui(commands.get_nowait())
|
||||
for _ in range(3):
|
||||
controller.step()
|
||||
targets = [call[1] for call in robot.calls if call[0] == "control"]
|
||||
np.testing.assert_allclose(targets[1], [0, .171, .175, .227, .244, .314, .314])
|
||||
np.testing.assert_allclose(targets[2], [0, .2, .2625, .3405, .366, .471, .471])
|
||||
ik = next(call[1] for call in robot.calls if call[0] == "ik")
|
||||
assert ik["link"] == link
|
||||
assert ik["return_error"] is True
|
||||
assert ik["pos_tol"] == .0005
|
||||
assert ik["rot_tol"] == .005
|
||||
np.testing.assert_allclose(ik["pos"], [1, 2.5, 3.2])
|
||||
np.testing.assert_allclose(ik["quat"], [2**-.5, 0, 0, 2**-.5], atol=1e-7)
|
||||
|
||||
|
||||
def test_queued_stop_freezes_commanded_target_and_next_move_starts_there():
|
||||
robot = LaggingRobot()
|
||||
controller = SceneMotionController(robot, list(range(7)), motion_config())
|
||||
commands = Queue()
|
||||
commands.put({"type": "joints", "values": [1] * 7, "speed_percent": 50})
|
||||
controller.apply_gui(commands.get_nowait())
|
||||
controller.step()
|
||||
controller.step()
|
||||
frozen = np.array([.171, .171, .175, .227, .244, .314, .314])
|
||||
commands.put({"type": "stop"})
|
||||
controller.apply_gui(commands.get_nowait())
|
||||
for _ in range(3):
|
||||
assert controller.step() is True
|
||||
np.testing.assert_allclose(robot.calls[-1][1], frozen)
|
||||
controller.apply(JointCommand((0,) * 7))
|
||||
controller.step()
|
||||
np.testing.assert_allclose(robot.calls[-1][1], np.zeros(7), atol=1e-15)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("target", [[4] * 7, [-4] * 7, [0] * 6, [np.nan] * 7, [np.inf] * 7])
|
||||
@pytest.mark.parametrize("mode", ["with_gripper", "without_gripper"])
|
||||
def test_invalid_joint_target_preserves_active_motion(target, mode):
|
||||
robot = LaggingRobot()
|
||||
controller = SceneMotionController(robot, list(range(7)), motion_config(mode))
|
||||
controller.apply(JointCommand((1,) * 7), 50)
|
||||
controller.step()
|
||||
with pytest.raises(ValueError):
|
||||
controller.apply_gui({"type": "joints", "values": target, "speed_percent": 100})
|
||||
controller.step()
|
||||
np.testing.assert_allclose(robot.calls[-1][1], [.171, .171, .175, .227, .244, .314, .314])
|
||||
|
||||
|
||||
@pytest.mark.parametrize("index", range(7))
|
||||
@pytest.mark.parametrize("boundary", [-.25, .5])
|
||||
def test_selected_urdf_position_limits_apply_to_each_joint(tmp_path, index, boundary):
|
||||
cfg = motion_config()
|
||||
tree = ET.parse(cfg["robot"]["urdf"])
|
||||
limit = tree.find(f"joint[@name='joint{index + 1}']/limit")
|
||||
limit.set("lower", "-.25")
|
||||
limit.set("upper", ".5")
|
||||
cfg["robot"]["urdf"] = tmp_path / "robot.urdf"
|
||||
tree.write(cfg["robot"]["urdf"])
|
||||
robot = LaggingRobot()
|
||||
controller = SceneMotionController(robot, list(range(7)), cfg)
|
||||
target = np.zeros(7)
|
||||
target[index] = boundary
|
||||
controller.apply(JointCommand(tuple(target)))
|
||||
for _ in range(4):
|
||||
controller.step()
|
||||
np.testing.assert_allclose(robot.calls[-1][1], target)
|
||||
invalid = target.copy()
|
||||
invalid[index] += -.001 if boundary < 0 else .001
|
||||
with pytest.raises(ValueError, match="URDF"):
|
||||
controller.apply(JointCommand(tuple(invalid)))
|
||||
assert controller.step() is True
|
||||
np.testing.assert_allclose(robot.calls[-1][1], target)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("result", [
|
||||
(np.zeros(7), [.0006, 0, 0, 0, 0, 0]),
|
||||
(np.zeros(7), [0, 0, 0, .006, 0, 0]),
|
||||
(np.zeros(7), [np.nan] * 6),
|
||||
(np.zeros(7), [np.inf] * 6),
|
||||
(np.zeros(7), [0] * 5),
|
||||
(np.full(7, 4), np.zeros(6)),
|
||||
(np.full(7, np.nan), np.zeros(6)),
|
||||
Exception("Genesis solver failed"),
|
||||
])
|
||||
def test_failed_ik_is_recoverable_and_preserves_active_motion(result, monkeypatch):
|
||||
robot = LaggingRobot()
|
||||
controller = SceneMotionController(robot, list(range(7)), motion_config())
|
||||
controller.apply(JointCommand((1,) * 7), 50)
|
||||
controller.step()
|
||||
|
||||
def inverse_kinematics(**kwargs):
|
||||
if isinstance(result, Exception):
|
||||
raise result
|
||||
return result
|
||||
|
||||
monkeypatch.setattr(robot, "inverse_kinematics", inverse_kinematics)
|
||||
with pytest.raises(ValueError):
|
||||
controller.apply(CartesianCommand((100, 0, 0), (0, 0, 0)))
|
||||
assert controller.mode == "joints"
|
||||
controller.step()
|
||||
np.testing.assert_allclose(robot.calls[-1][1], [.171, .171, .175, .227, .244, .314, .314])
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def cli_runtime(monkeypatch, tmp_path):
|
||||
from iiwa_genesis import cli, control_gui, preview, runtime, scene
|
||||
|
||||
robot = LaggingRobot()
|
||||
robot.get_joint = lambda name: SimpleNamespace(dofs_idx_local=[int(name[-1]) - 1])
|
||||
terminal_queue = Queue()
|
||||
gui = SimpleNamespace(commands=Queue(), diagnostics=Queue(), status="running")
|
||||
events = []
|
||||
scheduled = {}
|
||||
|
||||
def poll_terminal():
|
||||
try:
|
||||
return terminal_queue.get_nowait()
|
||||
except Empty:
|
||||
return None
|
||||
|
||||
terminal = SimpleNamespace(start=lambda: None, poll=poll_terminal)
|
||||
gui.start = lambda: gui
|
||||
gui.close = lambda: events.append("gui closed")
|
||||
fake_preview = SimpleNamespace(close=lambda: events.append("preview closed"),
|
||||
stop=SimpleNamespace(is_set=lambda: False),
|
||||
submit=lambda *args: None)
|
||||
steps = []
|
||||
|
||||
def step():
|
||||
steps.append(robot.calls[-1][1].copy())
|
||||
if len(steps) in scheduled:
|
||||
scheduled[len(steps)]()
|
||||
|
||||
fake_scene = SimpleNamespace(step=step, viewer=SimpleNamespace(is_alive=lambda: True))
|
||||
camera = SimpleNamespace(name="overhead", cfg={"fps": 10, "max_range": 6},
|
||||
read=lambda timestamp: events.append(("frame", timestamp)))
|
||||
gs = SimpleNamespace(cpu="cpu", backend="cpu", __version__="fake",
|
||||
init=lambda **kwargs: None, destroy=lambda: events.append("destroy"))
|
||||
cfg = motion_config()
|
||||
cfg["scene"].update(viewer=False, backend="cpu", seed=0)
|
||||
cfg["output"] = dict(save_frames=False, preview=True, directory=tmp_path, pointcloud=False)
|
||||
monkeypatch.setattr(cli, "load_config", lambda path: cfg)
|
||||
monkeypatch.setattr(runtime, "import_genesis", lambda: gs)
|
||||
monkeypatch.setattr(scene, "build_scene", lambda *args: (fake_scene, robot, [camera]))
|
||||
monkeypatch.setattr(cli, "TerminalInput", lambda: terminal)
|
||||
monkeypatch.setattr(control_gui, "ControlGui", lambda: gui)
|
||||
monkeypatch.setattr(preview, "Preview", lambda: fake_preview)
|
||||
monkeypatch.setattr(cli.time, "sleep", lambda duration: None)
|
||||
monkeypatch.setattr("sys.argv", ["run_scene.py", "--viewer", "--steps", "3",
|
||||
"--interactive", "--control-window"])
|
||||
return SimpleNamespace(run=cli.main, robot=robot, gui=gui, terminal=terminal_queue,
|
||||
events=events, steps=steps, scheduled=scheduled, preview=fake_preview)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("mode", ["with_gripper", "without_gripper"])
|
||||
@pytest.mark.parametrize("side", ["lower", "upper"])
|
||||
@pytest.mark.parametrize("outward", [False, True])
|
||||
def test_cartesian_ik_clips_float32_joint_boundaries(mode, side, outward, monkeypatch):
|
||||
robot = LaggingRobot()
|
||||
controller = SceneMotionController(robot, list(range(7)), motion_config(mode))
|
||||
boundary = getattr(controller, side).copy()
|
||||
result = boundary.astype(np.float32)
|
||||
if outward:
|
||||
result = np.nextafter(result, np.float32(-np.inf if side == "lower" else np.inf))
|
||||
monkeypatch.setattr(robot, "inverse_kinematics", lambda **kwargs: (result, np.zeros(6)))
|
||||
|
||||
controller.apply(CartesianCommand((.5, 0, .2), (0, 0, 0)))
|
||||
for _ in range(40):
|
||||
controller.step()
|
||||
|
||||
expected = np.array([2.97, 2.09, 2.97, 2.09, 2.97, 2.09, 3.05] if outward else [
|
||||
2.97, 2.0899999141693115, 2.97, 2.0899999141693115,
|
||||
2.97, 2.0899999141693115, 3.049999952316284])
|
||||
if side == "lower":
|
||||
expected = -expected
|
||||
np.testing.assert_array_equal(robot.calls[-1][1], expected)
|
||||
assert (robot.calls[-1][1] >= controller.lower).all()
|
||||
assert (robot.calls[-1][1] <= controller.upper).all()
|
||||
np.testing.assert_array_equal(getattr(controller, side), boundary)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("index", range(7))
|
||||
@pytest.mark.parametrize("side,offset", [("lower", -1e-5), ("upper", 1e-5)])
|
||||
def test_cartesian_ik_rejects_real_boundary_violation(index, side, offset, monkeypatch):
|
||||
robot = LaggingRobot()
|
||||
controller = SceneMotionController(robot, list(range(7)), motion_config())
|
||||
controller.apply(JointCommand((1,) * 7), 50)
|
||||
controller.step()
|
||||
result = np.zeros(7)
|
||||
result[index] = getattr(controller, side)[index] + offset
|
||||
monkeypatch.setattr(robot, "inverse_kinematics", lambda **kwargs: (result, np.zeros(6)))
|
||||
|
||||
with pytest.raises(ValueError, match="URDF"):
|
||||
controller.apply(CartesianCommand((.5, 0, .2), (0, 0, 0)))
|
||||
assert controller.mode == "joints"
|
||||
controller.step()
|
||||
np.testing.assert_allclose(robot.calls[-1][1], [.171, .171, .175, .227, .244, .314, .314])
|
||||
|
||||
|
||||
class SustainedQueue:
|
||||
def __init__(self, app, value):
|
||||
self.app = app
|
||||
self.value = value
|
||||
self.reads = {}
|
||||
|
||||
def get_nowait(self):
|
||||
tick = len(self.app.steps)
|
||||
self.reads[tick] = self.reads.get(tick, 0) + 1
|
||||
assert self.reads[tick] < 1000, "input processing starved physics"
|
||||
return self.value
|
||||
|
||||
|
||||
@pytest.mark.parametrize("channel", ["terminal", "commands", "diagnostics"])
|
||||
def test_cli_sustained_input_yields_to_physics_and_frames(cli_runtime, channel, monkeypatch):
|
||||
from iiwa_genesis import cli
|
||||
|
||||
app = cli_runtime
|
||||
value = "joints 1 1 1 1 1 1 1" if channel == "terminal" else {
|
||||
"type": "joints", "values": [1] * 7, "speed_percent": 100}
|
||||
queue = SustainedQueue(app, value)
|
||||
if channel == "terminal":
|
||||
monkeypatch.setattr(cli, "TerminalInput", lambda: SimpleNamespace(
|
||||
start=lambda: None, poll=queue.get_nowait))
|
||||
else:
|
||||
setattr(app.gui, channel, queue)
|
||||
app.terminal.put("joints 1 1 1 1 1 1 1")
|
||||
|
||||
app.run()
|
||||
|
||||
assert len(app.steps) == 3
|
||||
assert set(queue.reads) == {0, 1, 2}
|
||||
assert len([event for event in app.events if isinstance(event, tuple)]) == 3
|
||||
assert (app.steps[-1] > app.steps[0]).all()
|
||||
|
||||
|
||||
@pytest.mark.parametrize("quit_line", ["quit", " EXIT "])
|
||||
def test_cli_quit_exits_before_pending_movement_gui_or_physics(cli_runtime, quit_line):
|
||||
app = cli_runtime
|
||||
app.terminal.put("joints 1 1 1 1 1 1 1")
|
||||
|
||||
def request_quit():
|
||||
app.terminal.put("cartesian .5 0 .2 0 0 0")
|
||||
app.terminal.put(quit_line)
|
||||
app.gui.commands.put({"type": "joints", "values": [-1] * 7, "speed_percent": 100})
|
||||
app.gui.diagnostics.put({"message": "pending diagnostic"})
|
||||
|
||||
app.scheduled[1] = request_quit
|
||||
app.run()
|
||||
|
||||
assert len(app.steps) == 1
|
||||
assert not any(call[0] == "ik" for call in app.robot.calls)
|
||||
assert app.gui.commands.qsize() == 1
|
||||
assert app.gui.diagnostics.qsize() == 1
|
||||
assert len([event for event in app.events if isinstance(event, tuple)]) == 1
|
||||
assert app.events[-3:] == ["gui closed", "preview closed", "destroy"]
|
||||
|
||||
|
||||
def test_cli_stop_preempts_pending_ik_from_both_inputs_and_allows_later_move(cli_runtime):
|
||||
app = cli_runtime
|
||||
app.terminal.put("cartesian .5 0 .2 0 0 0")
|
||||
app.gui.commands.put({"type": "cartesian", "values": [.5, 0, .2, 0, 0, 0],
|
||||
"speed_percent": 100})
|
||||
app.gui.commands.put({"type": "stop"})
|
||||
app.scheduled[1] = lambda: app.gui.commands.put({
|
||||
"type": "joints", "values": [1] * 7, "speed_percent": 50})
|
||||
app.run()
|
||||
|
||||
assert not any(call[0] == "ik" for call in app.robot.calls)
|
||||
np.testing.assert_array_equal(app.steps[0], np.zeros(7))
|
||||
np.testing.assert_allclose(app.steps[1], [.0855, .0855, .0875, .1135, .122, .157, .157])
|
||||
|
||||
|
||||
def test_cli_stop_defers_already_queued_later_move_until_next_tick(cli_runtime):
|
||||
app = cli_runtime
|
||||
app.gui.commands.put({"type": "stop"})
|
||||
app.gui.commands.put({"type": "joints", "values": [1] * 7, "speed_percent": 50})
|
||||
app.run()
|
||||
|
||||
np.testing.assert_array_equal(app.steps[0], np.zeros(7))
|
||||
np.testing.assert_allclose(app.steps[1], [.0855, .0855, .0875, .1135, .122, .157, .157])
|
||||
np.testing.assert_allclose(app.steps[2], [.171, .171, .175, .227, .244, .314, .314])
|
||||
|
||||
|
||||
@pytest.mark.parametrize("source", ["terminal", "gui"])
|
||||
def test_cli_bounded_batches_preserve_backlogged_commands(cli_runtime, source, monkeypatch):
|
||||
app = cli_runtime
|
||||
for _ in range(100):
|
||||
if source == "terminal":
|
||||
app.terminal.put("help")
|
||||
else:
|
||||
app.gui.commands.put({"type": "close"})
|
||||
if source == "terminal":
|
||||
app.terminal.put("joints 1 1 1 1 1 1 1")
|
||||
else:
|
||||
app.gui.commands.put({"type": "joints", "values": [1] * 7, "speed_percent": 100})
|
||||
|
||||
# Run enough ticks to reach the final command while checking frames continue.
|
||||
monkeypatch.setattr("sys.argv", ["run_scene.py", "--viewer", "--steps", "12",
|
||||
"--interactive", "--control-window"])
|
||||
app.run()
|
||||
|
||||
np.testing.assert_array_equal(app.steps[0], np.zeros(7))
|
||||
assert (app.steps[-1] > 0).all()
|
||||
assert len(app.steps) == 12
|
||||
|
||||
|
||||
def test_cli_batch_parses_set_after_mode_change_and_invalid_quit(cli_runtime, capsys):
|
||||
app = cli_runtime
|
||||
app.terminal.put("quit extra")
|
||||
app.terminal.put("mode cartesian")
|
||||
app.terminal.put("set .5 0 .2 0 0 0")
|
||||
app.run()
|
||||
|
||||
assert len(app.steps) == 3
|
||||
assert len([call for call in app.robot.calls if call[0] == "ik"]) == 1
|
||||
assert "quit takes no values" in capsys.readouterr().out
|
||||
|
||||
|
||||
@pytest.mark.parametrize("side,offset", [("lower", -1e-7), ("upper", 1e-7)])
|
||||
def test_joint_commands_keep_strict_limits_for_tiny_excursions(side, offset):
|
||||
robot = LaggingRobot()
|
||||
controller = SceneMotionController(robot, list(range(7)), motion_config())
|
||||
target = getattr(controller, side) + offset
|
||||
with pytest.raises(ValueError, match="URDF"):
|
||||
controller.apply(JointCommand(tuple(target)))
|
||||
|
||||
|
||||
class BrokenQueue:
|
||||
def __init__(self, error):
|
||||
self.error = error
|
||||
self.reads = 0
|
||||
|
||||
def get_nowait(self):
|
||||
self.reads += 1
|
||||
raise self.error("queue disconnected")
|
||||
|
||||
|
||||
@pytest.mark.parametrize("channel", ["commands", "diagnostics"])
|
||||
@pytest.mark.parametrize("error", [EOFError, OSError, ValueError])
|
||||
def test_cli_disables_only_broken_gui_queue_and_keeps_terminal_and_frames(cli_runtime, channel, error, capsys):
|
||||
app = cli_runtime
|
||||
broken = BrokenQueue(error)
|
||||
setattr(app.gui, channel, broken)
|
||||
app.terminal.put("joints 1 1 1 1 1 1 1")
|
||||
app.scheduled[1] = lambda: app.terminal.put("joints -1 -1 -1 -1 -1 -1 -1")
|
||||
if channel == "diagnostics":
|
||||
app.scheduled[2] = lambda: app.gui.commands.put({"type": "stop"})
|
||||
else:
|
||||
app.gui.diagnostics.put({"message": "child startup failed"})
|
||||
app.run()
|
||||
assert broken.reads == 1
|
||||
assert len(app.steps) == 3
|
||||
np.testing.assert_allclose(app.steps[1], np.zeros(7), atol=1e-15)
|
||||
if channel == "diagnostics":
|
||||
np.testing.assert_allclose(app.steps[2], app.steps[1])
|
||||
else:
|
||||
assert (app.steps[2] < 0).all()
|
||||
assert len([event for event in app.events if isinstance(event, tuple)]) == 3
|
||||
assert app.events[-3:] == ["gui closed", "preview closed", "destroy"]
|
||||
assert "queue disconnected" in capsys.readouterr().out
|
||||
|
||||
|
||||
def test_cli_rejects_gui_command_but_processes_following_valid_command(cli_runtime, capsys):
|
||||
app = cli_runtime
|
||||
app.gui.commands.put({"type": "joints", "values": [4] * 7, "speed_percent": 100})
|
||||
app.gui.commands.put({"type": "joints", "values": [1] * 7, "speed_percent": 50})
|
||||
app.run()
|
||||
np.testing.assert_allclose(app.steps[0], [.0855, .0855, .0875, .1135, .122, .157, .157])
|
||||
np.testing.assert_allclose(app.steps[2], [.2565, .2565, .2625, .3405, .366, .471, .471])
|
||||
assert "Command error" in capsys.readouterr().out
|
||||
|
||||
|
||||
@pytest.mark.parametrize("source", ["terminal", "gui"])
|
||||
def test_cli_recovers_from_genesis_ik_exception_without_losing_motion(cli_runtime, source, monkeypatch, capsys):
|
||||
app = cli_runtime
|
||||
|
||||
def fail(**kwargs):
|
||||
raise Exception("Genesis IK failure")
|
||||
|
||||
monkeypatch.setattr(app.robot, "inverse_kinematics", fail)
|
||||
app.gui.commands.put({"type": "joints", "values": [1] * 7, "speed_percent": 50})
|
||||
if source == "terminal":
|
||||
app.scheduled[1] = lambda: app.terminal.put("cartesian 100 0 0 0 0 0")
|
||||
else:
|
||||
app.scheduled[1] = lambda: app.gui.commands.put({
|
||||
"type": "cartesian", "values": [100, 0, 0, 0, 0, 0], "speed_percent": 100})
|
||||
app.scheduled[2] = lambda: app.gui.commands.put({"type": "stop"})
|
||||
app.run()
|
||||
np.testing.assert_allclose(app.steps[1], [.171, .171, .175, .227, .244, .314, .314])
|
||||
np.testing.assert_allclose(app.steps[2], app.steps[1])
|
||||
assert app.events[-1] == "destroy"
|
||||
output = capsys.readouterr().out
|
||||
assert "Command error" in output
|
||||
assert "Genesis IK failure" in output
|
||||
|
||||
|
||||
@pytest.mark.parametrize("failure", ["startup", "close", "preview_close"])
|
||||
def test_cli_gui_failures_always_destroy_genesis(cli_runtime, failure, capsys):
|
||||
app = cli_runtime
|
||||
|
||||
def fail():
|
||||
raise RuntimeError(failure)
|
||||
|
||||
if failure == "startup":
|
||||
app.gui.start = fail
|
||||
elif failure == "close":
|
||||
app.gui.close = fail
|
||||
else:
|
||||
app.preview.close = fail
|
||||
app.terminal.put("joints 1 1 1 1 1 1 1")
|
||||
app.run()
|
||||
assert len(app.steps) == 3
|
||||
assert app.events[-1] == "destroy"
|
||||
if failure != "preview_close":
|
||||
assert "preview closed" in app.events
|
||||
assert failure in capsys.readouterr().out
|
||||
|
||||
|
||||
def test_velocity_limited_motion_scales_each_urdf_velocity_limit():
|
||||
motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS)
|
||||
|
||||
motion.set_target(np.full(7, 10.0), 50)
|
||||
position, complete = motion.step(np.zeros(7), 1.0)
|
||||
|
||||
np.testing.assert_allclose(position, np.asarray(IIWA_VELOCITY_LIMITS) * 0.5)
|
||||
assert complete is False
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"speed_percent", [np.nan, np.inf, -1, 0, 101, "50", "fast", True,
|
||||
np.bool_(False), 1 + 0j, None])
|
||||
def test_velocity_limited_motion_rejects_invalid_speed(speed_percent):
|
||||
motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS)
|
||||
|
||||
with pytest.raises(ValueError):
|
||||
motion.set_target(np.ones(7), speed_percent)
|
||||
|
||||
|
||||
def test_velocity_limited_motion_rejects_numeric_string_speed_percent():
|
||||
motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS)
|
||||
|
||||
with pytest.raises(ValueError):
|
||||
motion.set_target(np.full(7, 10.0), "50")
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"dt", ["1", "fast", True, np.bool_(False), 1 + 0j, None, np.nan,
|
||||
np.inf, -1])
|
||||
def test_velocity_limited_motion_rejects_invalid_dt(dt):
|
||||
motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS)
|
||||
motion.set_target(np.ones(7), 50)
|
||||
|
||||
with pytest.raises(ValueError):
|
||||
motion.step(np.zeros(7), dt)
|
||||
|
||||
|
||||
def test_velocity_limited_motion_applies_mixed_joint_caps_independently():
|
||||
limits = np.asarray([0.5, 2.0, 1.0, 3.0, 0.25, 4.0, 1.5])
|
||||
motion = VelocityLimitedMotion(limits)
|
||||
motion.set_target(np.full(7, 10.0), 50)
|
||||
|
||||
position, complete = motion.step(np.zeros(7), 1.0)
|
||||
|
||||
np.testing.assert_allclose(position, limits * 0.5)
|
||||
assert complete is False
|
||||
|
||||
|
||||
def test_velocity_limited_motion_clamps_each_joint_and_reports_completion():
|
||||
motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS)
|
||||
target = np.asarray([1, -1, 0.1, -0.1, 0.01, -0.01, 0.001])
|
||||
motion.set_target(target, 100)
|
||||
|
||||
position, complete = motion.step(np.zeros(7), 10.0)
|
||||
|
||||
np.testing.assert_allclose(position, target)
|
||||
assert complete is True
|
||||
assert motion.complete is True
|
||||
|
||||
|
||||
def test_velocity_limited_motion_stop_holds_current_position():
|
||||
motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS)
|
||||
current = np.arange(7, dtype=float)
|
||||
motion.set_target(np.ones(7), 50)
|
||||
|
||||
motion.stop(current)
|
||||
position, complete = motion.step(current + 10, 1.0)
|
||||
|
||||
np.testing.assert_allclose(position, current)
|
||||
assert complete is True
|
||||
|
||||
|
||||
def test_gui_queue_sends_picklable_plain_joint_command_data():
|
||||
queue = GuiCommandQueue(get_context("spawn").Queue())
|
||||
|
||||
queue.send_joint([0, 1, 2, 3, 4, 5, 6], 40)
|
||||
command = queue.poll()
|
||||
|
||||
assert isinstance(command, dict)
|
||||
assert command == {"type": "joints", "values": [0., 1., 2., 3., 4., 5., 6.],
|
||||
"speed_percent": 40.}
|
||||
assert pickle.loads(pickle.dumps(command)) == command
|
||||
queue.close()
|
||||
|
||||
|
||||
def test_gui_queue_sends_stop_and_close_commands():
|
||||
queue = GuiCommandQueue(get_context("spawn").Queue())
|
||||
|
||||
queue.send_stop()
|
||||
queue.send_close()
|
||||
|
||||
assert queue.poll() == {"type": "stop"}
|
||||
assert queue.poll() == {"type": "close"}
|
||||
assert queue.poll() is None
|
||||
queue.close()
|
||||
|
||||
|
||||
def test_gui_command_from_fields_validates_mode_specific_field_counts():
|
||||
command = command_from_fields("cartesian", [".1", ".2", ".3", "0", "1", "2"], "75")
|
||||
|
||||
assert command == {"type": "cartesian", "values": [.1, .2, .3, 0., 1., 2.],
|
||||
"speed_percent": 75.}
|
||||
with pytest.raises(ValueError):
|
||||
command_from_fields("joints", ["0"] * 6, "50")
|
||||
with pytest.raises(ValueError):
|
||||
command_from_fields("other", ["0"] * 7, "50")
|
||||
|
||||
|
||||
def test_gui_zero_speed_send_becomes_stop_command():
|
||||
assert command_from_fields("joints", ["0"] * 7, "0") == {"type": "stop"}
|
||||
|
||||
|
||||
@pytest.mark.parametrize("speed", ["nan", "inf", "-inf", "101", "-1"])
|
||||
def test_gui_command_rejects_nonfinite_or_out_of_range_speed(speed):
|
||||
with pytest.raises(ValueError):
|
||||
command_from_fields("joints", ["0"] * 7, speed)
|
||||
|
||||
|
||||
def test_gui_queue_sends_cartesian_command_data():
|
||||
queue = GuiCommandQueue(get_context("spawn").Queue())
|
||||
|
||||
queue.send_cartesian([.1, .2, .3, 0, 1, 2], 25)
|
||||
|
||||
assert queue.poll() == {"type": "cartesian", "values": [.1, .2, .3, 0., 1., 2.],
|
||||
"speed_percent": 25.}
|
||||
queue.close()
|
||||
|
||||
|
||||
def test_gui_queue_rejects_nonfinite_speed_and_maps_zero_to_stop():
|
||||
queue = GuiCommandQueue(get_context("spawn").Queue())
|
||||
|
||||
with pytest.raises(ValueError):
|
||||
queue.send_joint([0] * 7, float("nan"))
|
||||
queue.send_cartesian([0] * 6, 0)
|
||||
assert queue.poll() == {"type": "stop"}
|
||||
queue.close()
|
||||
|
||||
|
||||
def test_control_gui_can_close_before_start_and_reports_closed_status():
|
||||
gui = ControlGui()
|
||||
|
||||
gui.close()
|
||||
|
||||
assert gui.status == "closed"
|
||||
|
||||
|
||||
class FakeProcess:
|
||||
def __init__(self, alive):
|
||||
self.alive = iter(alive)
|
||||
self.joins = []
|
||||
self.terminates = 0
|
||||
|
||||
def is_alive(self):
|
||||
return next(self.alive)
|
||||
|
||||
def join(self, timeout):
|
||||
self.joins.append(timeout)
|
||||
|
||||
def terminate(self):
|
||||
self.terminates += 1
|
||||
|
||||
|
||||
def test_started_child_observer_destroys_root_after_parent_close():
|
||||
class Root:
|
||||
def __init__(self):
|
||||
self.destroyed = False
|
||||
self.scheduled = []
|
||||
|
||||
def destroy(self):
|
||||
self.destroyed = True
|
||||
|
||||
def after(self, delay, callback):
|
||||
self.scheduled.append((delay, callback))
|
||||
|
||||
root = Root()
|
||||
closed = get_context("spawn").Event()
|
||||
|
||||
_observe_parent_close(root, closed)
|
||||
assert root.destroyed is False
|
||||
assert len(root.scheduled) == 1
|
||||
|
||||
closed.set()
|
||||
root.scheduled[0][1]()
|
||||
|
||||
assert root.destroyed is True
|
||||
|
||||
|
||||
def test_gui_close_joins_started_child_without_terminating_when_it_exits():
|
||||
gui = ControlGui()
|
||||
gui.process = FakeProcess([True, False])
|
||||
gui._started = True
|
||||
|
||||
gui.close()
|
||||
|
||||
assert gui.process.joins == [2]
|
||||
assert gui.process.terminates == 0
|
||||
|
||||
|
||||
def test_gui_close_terminates_started_child_only_as_fallback():
|
||||
gui = ControlGui()
|
||||
gui.process = FakeProcess([True, True, False])
|
||||
gui._started = True
|
||||
|
||||
gui.close()
|
||||
|
||||
assert gui.process.joins == [2, 2]
|
||||
assert gui.process.terminates == 1
|
||||
Reference in New Issue
Block a user