fix jogging stepping by holding one velocity instead of re-arming
`set_cartesian_velocity` passed `duration=velocity_command_ttl_s` on every message, so each one re-armed a timed segment and the controller re-planned the same motion. On a real Lite6 that is not smooth travel: the arm moves in steps. Reducing how often the velocity is sent does not help — re-arming four times a second still steps — so it now uses `duration=0`, the SDK's "always effective, will not stop automatically", and skips a velocity that has not changed. One command stands until a different one replaces it. That removes the controller-side expiry, which `_expire_jog` was relying on: it flipped `_jogging` and reported `ready` without commanding anything, because the controller's own timer had already stopped the arm. With the velocity held indefinitely that would leave an arm travelling while the node reported idle, so `_expire_jog` now queues the stop itself. It goes through the job queue rather than the driver, since it runs on the reporter thread and the control thread owns the driver. `VELOCITY_COMMAND_TTL_S` is unchanged in value and meaning to a caller, but it is now enforced here rather than by the controller. Both halves are one commit on purpose: `duration=0` without the stop in `_expire_jog` is an arm that never stops when its sender dies. Adds tests/test_driver.py — the driver had no direct coverage, because FakeArm replaces it everywhere else, which is why this was invisible to the suite.
This commit is contained in:
@@ -0,0 +1,104 @@
|
||||
"""The driver's half of velocity control: what actually reaches the controller.
|
||||
|
||||
`FakeArm` stands in for this whole class everywhere else, so nothing else in the
|
||||
suite can see what the driver says to the SDK. That is the one thing worth
|
||||
pinning here: a velocity is a set-point the controller holds, and how often it is
|
||||
re-sent is the difference between an arm that travels and an arm that steps.
|
||||
"""
|
||||
|
||||
import pytest
|
||||
|
||||
from dora_ulite6.config import Config
|
||||
from dora_ulite6.driver import ULite6
|
||||
|
||||
MOVE = [20.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
BACK = [-20.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
STILL = [0.0] * 6
|
||||
|
||||
|
||||
class FakeSDK:
|
||||
"""Just enough XArmAPI to build a driver and watch it command a velocity."""
|
||||
|
||||
def __init__(self, *args, **kwargs):
|
||||
self.connected = True
|
||||
self.mode = None
|
||||
self.velocities = []
|
||||
|
||||
def connect(self):
|
||||
self.connected = True
|
||||
|
||||
def disconnect(self):
|
||||
self.connected = False
|
||||
|
||||
def motion_enable(self, enable=True):
|
||||
pass
|
||||
|
||||
def set_mode(self, mode):
|
||||
self.mode = mode
|
||||
|
||||
def set_state(self, state):
|
||||
pass
|
||||
|
||||
def vc_set_cartesian_velocity(self, speeds, is_radian=None, is_tool_coord=False, duration=-1):
|
||||
self.velocities.append((list(speeds), duration))
|
||||
return 0
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def robot(monkeypatch):
|
||||
"""Return a driver over a fake SDK, with the calls it made on `arm`."""
|
||||
monkeypatch.setattr("dora_ulite6.driver.XArmAPI", FakeSDK)
|
||||
return ULite6(Config())
|
||||
|
||||
|
||||
def commanded(robot):
|
||||
"""Return every velocity the driver put on the controller."""
|
||||
return [speeds for speeds, _ in robot.arm.velocities]
|
||||
|
||||
|
||||
def test_a_velocity_is_never_given_a_duration(robot):
|
||||
# `duration` makes the controller stop the arm on its own, which means every
|
||||
# message re-arms a timed segment and the controller re-plans the same
|
||||
# motion. The SDK reads 0 as "always effective, will not stop
|
||||
# automatically", which is what lets one command stand.
|
||||
robot.set_cartesian_velocity(MOVE)
|
||||
|
||||
assert [duration for _, duration in robot.arm.velocities] == [0]
|
||||
|
||||
|
||||
def test_a_held_velocity_reaches_the_controller_once(robot):
|
||||
# The arm holds it, so saying it again says nothing new — and saying it
|
||||
# again several times a second is what an operator feels as stepping.
|
||||
for _ in range(5):
|
||||
robot.set_cartesian_velocity(MOVE)
|
||||
|
||||
assert commanded(robot) == [MOVE]
|
||||
|
||||
|
||||
def test_a_changed_velocity_reaches_the_controller(robot):
|
||||
# Sending once is worth nothing if a new direction is taken for the old one.
|
||||
robot.set_cartesian_velocity(MOVE)
|
||||
robot.set_cartesian_velocity(BACK)
|
||||
|
||||
assert commanded(robot) == [MOVE, BACK]
|
||||
|
||||
|
||||
def test_a_zero_velocity_stops_the_arm_and_leaves_velocity_mode(robot):
|
||||
robot.set_cartesian_velocity(MOVE)
|
||||
|
||||
robot.set_cartesian_velocity(STILL)
|
||||
|
||||
assert commanded(robot) == [MOVE, STILL]
|
||||
assert robot._velocity_mode is False
|
||||
|
||||
|
||||
def test_the_same_velocity_is_sent_again_after_leaving_velocity_mode(robot):
|
||||
# Leaving the mode clears what the controller was holding, so the velocity
|
||||
# it had before means nothing on the way back in. Remembering it across the
|
||||
# gap would leave the arm still while the operator held a direction.
|
||||
robot.set_cartesian_velocity(MOVE)
|
||||
robot.set_cartesian_velocity(STILL)
|
||||
|
||||
robot.set_cartesian_velocity(MOVE)
|
||||
|
||||
assert commanded(robot) == [MOVE, STILL, MOVE]
|
||||
+17
-2
@@ -205,8 +205,7 @@ def test_a_zero_velocity_jog_does_not_make_a_running_motion_look_idle(monkeypatc
|
||||
|
||||
|
||||
def test_a_sender_that_goes_quiet_stops_being_called_jogging(monkeypatch):
|
||||
# The controller expires the command on its own; nothing tells us, so the
|
||||
# clock has to.
|
||||
# Nothing tells us a sender died, so the clock has to.
|
||||
control = build(monkeypatch, motion=MotionConfig(velocity_command_ttl_s=0.02))
|
||||
try:
|
||||
wait_for(lambda: READY in control.take_state_changes())
|
||||
@@ -218,6 +217,22 @@ def test_a_sender_that_goes_quiet_stops_being_called_jogging(monkeypatch):
|
||||
control.shutdown()
|
||||
|
||||
|
||||
def test_a_sender_that_goes_quiet_actually_stops_the_arm(monkeypatch):
|
||||
# The velocity is held by the controller indefinitely, so the expiry is the
|
||||
# deadman itself and not a note about one. Reporting `ready` without
|
||||
# commanding the stop would leave the arm travelling while the node said it
|
||||
# was idle, which is worse than not expiring it at all.
|
||||
control = build(monkeypatch, motion=MotionConfig(velocity_command_ttl_s=0.02))
|
||||
try:
|
||||
wait_for(lambda: READY in control.take_state_changes())
|
||||
control.start_jog([20.0, 0, 0, 0, 0, 0])
|
||||
wait_for(lambda: ("jog", [20.0, 0, 0, 0, 0, 0]) in control.arms[0].calls)
|
||||
|
||||
wait_for(lambda: ("jog", [0.0] * 6) in control.arms[0].calls)
|
||||
finally:
|
||||
control.shutdown()
|
||||
|
||||
|
||||
def test_a_jog_preempts_a_motion_that_is_running(monkeypatch):
|
||||
control = build(monkeypatch)
|
||||
control.arms[0].block = threading.Event()
|
||||
|
||||
Reference in New Issue
Block a user