cristhian
a95d804f0b
`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.
105 lines
3.3 KiB
Python
105 lines
3.3 KiB
Python
"""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]
|