Compare commits
7
Commits
412aeac955
..
v0.3.1
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
3b941e722e | ||
|
|
1985659abf | ||
|
|
a95d804f0b | ||
|
|
f29265a860 | ||
|
|
8cd4db93f1 | ||
|
|
4427da9c34 | ||
|
|
608e22078e |
@@ -60,7 +60,7 @@ Everything is configured through environment variables — see README.md for the
|
|||||||
## Test
|
## Test
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
uv run pytest # 83 tests, no robot and no dataflow needed
|
uv run pytest # 92 tests, no robot and no dataflow needed
|
||||||
uv run ruff check .
|
uv run ruff check .
|
||||||
```
|
```
|
||||||
|
|
||||||
@@ -85,7 +85,12 @@ while it runs. So:
|
|||||||
dataflow, and so the arm can be told whether this is a restart.
|
dataflow, and so the arm can be told whether this is a restart.
|
||||||
- **`dora_ulite6/control.py`** — `Control`: both arm threads and the mailboxes the loop
|
- **`dora_ulite6/control.py`** — `Control`: both arm threads and the mailboxes the loop
|
||||||
drains. The **control thread** runs one job at a time and is allowed to block: startup,
|
drains. The **control thread** runs one job at a time and is allowed to block: startup,
|
||||||
the `INIT_MODE` move, and every service including the motions that take seconds. The
|
the `INIT_MODE` move, and every service including the motions that take seconds. Its
|
||||||
|
wait between connection attempts serves the queue (`_serve_while_down`) rather than
|
||||||
|
sleeping through it: a request is answered exactly once whatever the arm is doing, and
|
||||||
|
an arm that is down is an outcome to report, not a reason to go quiet. A motion is
|
||||||
|
refused there rather than held, since a held one would fire unattended the moment the
|
||||||
|
link came back. The
|
||||||
**reporter thread** never blocks: it reads the report cache at the telemetry cadence,
|
**reporter thread** never blocks: it reads the report cache at the telemetry cadence,
|
||||||
so `tcp_pose` keeps flowing right through a move. **Nothing outside this file may call
|
so `tcp_pose` keeps flowing right through a move. **Nothing outside this file may call
|
||||||
the driver**, except `stop` and `read_report`, both written for another thread. Same
|
the driver**, except `stop` and `read_report`, both written for another thread. Same
|
||||||
@@ -178,6 +183,15 @@ A typed optional input therefore forces the example dataflow to wire it. `reques
|
|||||||
## Rules
|
## Rules
|
||||||
|
|
||||||
- Do not mention AI tools in commit messages or documentation.
|
- Do not mention AI tools in commit messages or documentation.
|
||||||
|
- **A velocity is set on the controller once and held.** `set_cartesian_velocity` passes no
|
||||||
|
duration and skips an unchanged velocity, because re-arming makes the controller re-plan
|
||||||
|
the same motion and the arm steps instead of travelling — four times a second is enough
|
||||||
|
to feel it. Do not reintroduce a per-message `duration`, and do not "simplify" the
|
||||||
|
unchanged-velocity check away.
|
||||||
|
- **`_expire_jog` is the deadman, not bookkeeping.** Nothing else stops an arm whose sender
|
||||||
|
died mid-jog: the controller holds the velocity indefinitely. It must command the stop,
|
||||||
|
not merely report `ready` — an arm travelling while the node says idle is worse than no
|
||||||
|
expiry at all.
|
||||||
- This controls a real robot. Test changes carefully and preserve the safety checks: the
|
- This controls a real robot. Test changes carefully and preserve the safety checks: the
|
||||||
reachability check before a move, the arrival confirmation after it, and the live error
|
reachability check before a move, the arrival confirmation after it, and the live error
|
||||||
reads that make both of them mean anything.
|
reads that make both of them mean anything.
|
||||||
|
|||||||
@@ -123,8 +123,7 @@ you see `done`. Sending them all at once does not queue them; the last one wins.
|
|||||||
|
|
||||||
## Jogging
|
## Jogging
|
||||||
|
|
||||||
`jog` is a topic, not a service: a velocity command expires on the controller's own TTL,
|
`jog` is a topic, not a service: a sender keeps sending and nobody waits for an answer.
|
||||||
so a sender keeps sending and nobody waits for an answer.
|
|
||||||
|
|
||||||
```python
|
```python
|
||||||
node.send_output("jog", pa.array([{"velocity": [20.0, 0, 0, 0, 0, 0]}]))
|
node.send_output("jog", pa.array([{"velocity": [20.0, 0, 0, 0, 0, 0]}]))
|
||||||
@@ -132,9 +131,18 @@ node.send_output("jog", pa.array([{"velocity": [20.0, 0, 0, 0, 0, 0]}]))
|
|||||||
|
|
||||||
It still drives the arm, so it behaves like a motion where that matters: the first
|
It still drives the arm, so it behaves like a motion where that matters: the first
|
||||||
non-zero jog preempts a motion in play and `robot_state` turns to `moving`. A zero
|
non-zero jog preempts a motion in play and `robot_state` turns to `moving`. A zero
|
||||||
velocity stops it, and so does simply going quiet — the node returns to `ready` once
|
velocity stops it, and so does simply going quiet — the node stops the arm and returns to
|
||||||
`VELOCITY_COMMAND_TTL_S` has passed, because that is when the controller has expired the
|
`ready` once `VELOCITY_COMMAND_TTL_S` has passed.
|
||||||
command.
|
|
||||||
|
**The velocity is set once and held.** The controller is given a velocity with no
|
||||||
|
duration, so it keeps it until a different one arrives: re-sending an unchanged velocity
|
||||||
|
does not reach the arm at all. Sending it repeatedly re-armed a timed segment and made the
|
||||||
|
controller re-plan the same motion, which on a real arm is felt as stepping rather than
|
||||||
|
travel — even at four messages a second.
|
||||||
|
|
||||||
|
That also means **the controller no longer stops the arm on its own**, so
|
||||||
|
`VELOCITY_COMMAND_TTL_S` is enforced here instead. It is a real deadman now rather than a
|
||||||
|
description of one: a sender that dies mid-jog is what it exists for.
|
||||||
|
|
||||||
## Telemetry
|
## Telemetry
|
||||||
|
|
||||||
@@ -157,7 +165,7 @@ Add to your dataflow:
|
|||||||
nodes:
|
nodes:
|
||||||
- id: ulite6
|
- id: ulite6
|
||||||
git: ssh://git@gitea.liria.cl:5001/liria/dora-ulite6.git
|
git: ssh://git@gitea.liria.cl:5001/liria/dora-ulite6.git
|
||||||
tag: v0.3.0
|
tag: v0.3.1
|
||||||
build: pip install .
|
build: pip install .
|
||||||
path: dora-ulite6
|
path: dora-ulite6
|
||||||
inputs:
|
inputs:
|
||||||
@@ -239,13 +247,13 @@ left where it stood.
|
|||||||
| `READY_TIMEOUT_S` | `3.0` | How long `recover` waits for a movable state |
|
| `READY_TIMEOUT_S` | `3.0` | How long `recover` waits for a movable state |
|
||||||
| `HOME_TIMEOUT_S` | `20.0` | How long homing may take |
|
| `HOME_TIMEOUT_S` | `20.0` | How long homing may take |
|
||||||
| `TOOL_SETTLE_S` | `2.0` | How long to wait for suction to build or fingers to close |
|
| `TOOL_SETTLE_S` | `2.0` | How long to wait for suction to build or fingers to close |
|
||||||
| `VELOCITY_COMMAND_TTL_S` | `0.5` | The controller's own expiry for one `jog` command |
|
| `VELOCITY_COMMAND_TTL_S` | `0.5` | How long a `jog` stream may go quiet before this node stops the arm |
|
||||||
|
|
||||||
## Development
|
## Development
|
||||||
|
|
||||||
```bash
|
```bash
|
||||||
uv sync --group dev
|
uv sync --group dev
|
||||||
uv run pytest # 83 tests, no robot needed
|
uv run pytest # 92 tests, no robot needed
|
||||||
uv run ruff check .
|
uv run ruff check .
|
||||||
```
|
```
|
||||||
|
|
||||||
|
|||||||
+10
-4
@@ -49,9 +49,15 @@ inputs:
|
|||||||
description: >-
|
description: >-
|
||||||
A continuous velocity command: a one-row struct {velocity} of 6 numbers
|
A continuous velocity command: a one-row struct {velocity} of 6 numbers
|
||||||
[vx, vy, vz, wx, wy, wz] in mm/s and deg/s. A topic rather than a service
|
[vx, vy, vz, wx, wy, wz] in mm/s and deg/s. A topic rather than a service
|
||||||
because it expires on the controller's own TTL and a sender must keep
|
because a sender keeps sending and nothing is published in answer. It
|
||||||
sending; nothing is published in answer. It drives the arm, so the first
|
drives the arm, so the first non-zero one preempts a motion in play and
|
||||||
non-zero one preempts a motion in play and robot_state turns to moving.
|
robot_state turns to moving.
|
||||||
|
|
||||||
|
The velocity is set on the controller once and held: an unchanged one is
|
||||||
|
not sent to the arm again, because re-arming it makes the controller
|
||||||
|
re-plan the same motion and the arm step rather than travel. A sender that
|
||||||
|
goes quiet for longer than VELOCITY_COMMAND_TTL_S is stopped by this node,
|
||||||
|
which is the deadman behind a sender that dies mid-jog.
|
||||||
|
|
||||||
outputs:
|
outputs:
|
||||||
response:
|
response:
|
||||||
@@ -112,7 +118,7 @@ env:
|
|||||||
example: |
|
example: |
|
||||||
- id: ulite6
|
- id: ulite6
|
||||||
git: ssh://git@gitea.liria.cl:5001/liria/dora-ulite6.git
|
git: ssh://git@gitea.liria.cl:5001/liria/dora-ulite6.git
|
||||||
tag: v0.3.0
|
tag: v0.3.1
|
||||||
build: pip install .
|
build: pip install .
|
||||||
path: dora-ulite6
|
path: dora-ulite6
|
||||||
inputs:
|
inputs:
|
||||||
|
|||||||
@@ -60,9 +60,10 @@ class MotionConfig:
|
|||||||
ready_timeout_s: float = 3.0
|
ready_timeout_s: float = 3.0
|
||||||
home_timeout_s: float = 20.0
|
home_timeout_s: float = 20.0
|
||||||
tool_settle_s: float = 2.0
|
tool_settle_s: float = 2.0
|
||||||
# The controller's own expiry for one jog command: the arm keeps moving at
|
# How long a jog stream may go quiet before this node stops the arm. The
|
||||||
# the commanded velocity until this runs out, so a sender that stops sending
|
# velocity is set on the controller once and held there indefinitely — see
|
||||||
# stops the arm.
|
# `ULite6.set_cartesian_velocity` — so nothing expires it but us: this is
|
||||||
|
# the deadman behind a sender that dies mid-jog, enforced by `_expire_jog`.
|
||||||
velocity_command_ttl_s: float = 0.5
|
velocity_command_ttl_s: float = 0.5
|
||||||
|
|
||||||
@classmethod
|
@classmethod
|
||||||
|
|||||||
+76
-11
@@ -232,10 +232,13 @@ class Control:
|
|||||||
self._preempt_motions("jog")
|
self._preempt_motions("jog")
|
||||||
self._set_state(MOVING)
|
self._set_state(MOVING)
|
||||||
with self._lock:
|
with self._lock:
|
||||||
self._jogging = moving
|
was_jogging, self._jogging = self._jogging, moving
|
||||||
self._last_jog = time.monotonic()
|
self._last_jog = time.monotonic()
|
||||||
if not moving:
|
# Only a jog that was in fact jogging ends one. A zero velocity from a
|
||||||
self._set_state(READY)
|
# teleop source that is merely idling says nothing about a motion that
|
||||||
|
# owns the arm, and must not answer for it.
|
||||||
|
if not moving and was_jogging:
|
||||||
|
self._idle_unless_busy()
|
||||||
self._jobs.put(Jog(velocity=velocity))
|
self._jobs.put(Jog(velocity=velocity))
|
||||||
|
|
||||||
def start_motion(self, call: ServiceCall) -> None:
|
def start_motion(self, call: ServiceCall) -> None:
|
||||||
@@ -336,11 +339,49 @@ class Control:
|
|||||||
except RobotUnavailableError as error:
|
except RobotUnavailableError as error:
|
||||||
log(f"{error}; retrying in {self.config.reconnect_interval_s:.0f}s")
|
log(f"{error}; retrying in {self.config.reconnect_interval_s:.0f}s")
|
||||||
self._set_state(UNAVAILABLE)
|
self._set_state(UNAVAILABLE)
|
||||||
self._stopping.wait(self.config.reconnect_interval_s)
|
self._serve_while_down(self.config.reconnect_interval_s)
|
||||||
return
|
return
|
||||||
self._started = True
|
self._started = True
|
||||||
self._set_state(READY)
|
self._set_state(READY)
|
||||||
|
|
||||||
|
def _serve_while_down(self, seconds: float) -> None:
|
||||||
|
"""Answer what can be answered without a serving arm, until it is time to retry.
|
||||||
|
|
||||||
|
This is the wait between connection attempts, and it serves the queue
|
||||||
|
rather than sleeping through it. Sitting silent is the one thing a caller
|
||||||
|
must never get: an arm that is down is an outcome to report, not a reason
|
||||||
|
to stop answering, and a request left in the queue here would be answered
|
||||||
|
only if the arm ever came back.
|
||||||
|
"""
|
||||||
|
deadline = time.monotonic() + seconds
|
||||||
|
while not self._stopping.is_set():
|
||||||
|
remaining = deadline - time.monotonic()
|
||||||
|
if remaining <= 0:
|
||||||
|
return
|
||||||
|
try:
|
||||||
|
job = self._jobs.get(timeout=min(remaining, 0.1))
|
||||||
|
except queue.Empty:
|
||||||
|
continue
|
||||||
|
if isinstance(job, ServiceCall):
|
||||||
|
self._serve_without_arm(job)
|
||||||
|
|
||||||
|
def _serve_without_arm(self, call: ServiceCall) -> None:
|
||||||
|
"""Answer one request that arrived before the arm was serving.
|
||||||
|
|
||||||
|
A motion is refused outright. Holding it would be worse than silence:
|
||||||
|
the arm is somewhere unknown, nobody is watching, and the motion would
|
||||||
|
fire unattended the moment the link came back.
|
||||||
|
|
||||||
|
Everything else is tried on the arm, because a controller that answers
|
||||||
|
while refusing to serve is exactly what `recover` and `clear` are for,
|
||||||
|
and refusing them here would leave the one state they exist to escape
|
||||||
|
with no way out of it.
|
||||||
|
"""
|
||||||
|
if call.moves_the_arm:
|
||||||
|
self._finish_motion(call, FAILED, False, "the arm is not serving yet")
|
||||||
|
return
|
||||||
|
self._respond(call, self._attempt(lambda: services.run(self.robot, call.row)))
|
||||||
|
|
||||||
def _start_position(self) -> None:
|
def _start_position(self) -> None:
|
||||||
"""Put the arm where INIT_MODE says it should start, if anywhere."""
|
"""Put the arm where INIT_MODE says it should start, if anywhere."""
|
||||||
if self.restarted and self.config.init_mode != "none":
|
if self.restarted and self.config.init_mode != "none":
|
||||||
@@ -403,8 +444,7 @@ class Control:
|
|||||||
self._finish_motion(call, PREEMPTED, False, f"preempted by {preempted_by}")
|
self._finish_motion(call, PREEMPTED, False, f"preempted by {preempted_by}")
|
||||||
else:
|
else:
|
||||||
self._finish_motion(call, DONE if ok else FAILED, ok, fields.get("message", ""), fields)
|
self._finish_motion(call, DONE if ok else FAILED, ok, fields.get("message", ""), fields)
|
||||||
if self._state == MOVING:
|
self._idle_unless_busy()
|
||||||
self._set_state(READY)
|
|
||||||
|
|
||||||
def _leave_jog(self) -> bool:
|
def _leave_jog(self) -> bool:
|
||||||
"""Clear the jogging flag, reporting whether the arm was in fact jogging."""
|
"""Clear the jogging flag, reporting whether the arm was in fact jogging."""
|
||||||
@@ -515,20 +555,45 @@ class Control:
|
|||||||
self._state_changes.append(state)
|
self._state_changes.append(state)
|
||||||
log(f"state: {state}")
|
log(f"state: {state}")
|
||||||
|
|
||||||
|
def _idle_unless_busy(self) -> None:
|
||||||
|
"""Report the arm idle, but only when nothing is still driving it.
|
||||||
|
|
||||||
|
One thing ending is not the arm stopping. A motion may still be winding
|
||||||
|
down inside the SDK after the jog that preempted it went quiet; a jog may
|
||||||
|
have taken the arm over from the motion that just finished. Either way
|
||||||
|
`ready` published over it is the false idle the state exists to rule out.
|
||||||
|
|
||||||
|
The state also stops being ours to set once it has left `moving` — that
|
||||||
|
is `unavailable` or `recovering` reporting something this does not know
|
||||||
|
about, and it must not be overwritten with `ready`.
|
||||||
|
"""
|
||||||
|
with self._lock:
|
||||||
|
if self._running_motion is not None or self._jogging or self._state != MOVING:
|
||||||
|
return
|
||||||
|
self._set_state(READY)
|
||||||
|
|
||||||
# --- the reporter thread ---
|
# --- the reporter thread ---
|
||||||
|
|
||||||
def _expire_jog(self) -> None:
|
def _expire_jog(self) -> None:
|
||||||
"""Stop calling the arm jogging once the controller has expired the command.
|
"""Stop the arm once the sender driving it has gone quiet.
|
||||||
|
|
||||||
A jog lives on the controller's own TTL, so a sender that goes quiet
|
**This is the deadman, not bookkeeping.** The velocity is set on the
|
||||||
leaves an arm that stopped moving a moment later. Nothing tells us; the
|
controller once and held there indefinitely — see
|
||||||
clock does.
|
:meth:`ULite6.set_cartesian_velocity` for why it cannot be given a
|
||||||
|
duration — so a sender that dies mid-jog leaves an arm that travels
|
||||||
|
until something says otherwise. Nothing tells us; the clock does, and
|
||||||
|
this is what says otherwise.
|
||||||
|
|
||||||
|
The stop goes through the job queue rather than the arm directly: this
|
||||||
|
runs on the reporter thread, and the control thread is the only one that
|
||||||
|
may touch the driver.
|
||||||
"""
|
"""
|
||||||
with self._lock:
|
with self._lock:
|
||||||
if not self._jogging or time.monotonic() - self._last_jog < self.config.motion.velocity_command_ttl_s:
|
if not self._jogging or time.monotonic() - self._last_jog < self.config.motion.velocity_command_ttl_s:
|
||||||
return
|
return
|
||||||
self._jogging = False
|
self._jogging = False
|
||||||
self._set_state(READY)
|
self._jobs.put(Jog(velocity=[0.0] * 6))
|
||||||
|
self._idle_unless_busy()
|
||||||
|
|
||||||
def _run_reporter(self) -> None:
|
def _run_reporter(self) -> None:
|
||||||
"""Publish telemetry, whatever the control thread is doing.
|
"""Publish telemetry, whatever the control thread is doing.
|
||||||
|
|||||||
+30
-5
@@ -61,6 +61,10 @@ class ULite6:
|
|||||||
# falls back to a blocking get_position().
|
# falls back to a blocking get_position().
|
||||||
self.arm = XArmAPI(self.ip, do_not_open=True, enable_report=True)
|
self.arm = XArmAPI(self.ip, do_not_open=True, enable_report=True)
|
||||||
self._velocity_mode = False
|
self._velocity_mode = False
|
||||||
|
# The velocity currently standing on the controller. A velocity is a
|
||||||
|
# set-point it holds, so re-sending an unchanged one is not a no-op: see
|
||||||
|
# `set_cartesian_velocity`.
|
||||||
|
self._commanded: list | None = None
|
||||||
self.connect()
|
self.connect()
|
||||||
|
|
||||||
# --- connection ---
|
# --- connection ---
|
||||||
@@ -500,10 +504,24 @@ class ULite6:
|
|||||||
def set_cartesian_velocity(self, velocity: list) -> None:
|
def set_cartesian_velocity(self, velocity: list) -> None:
|
||||||
"""Drive the arm at a constant base-frame velocity (xArm mode 5) and return.
|
"""Drive the arm at a constant base-frame velocity (xArm mode 5) and return.
|
||||||
|
|
||||||
``velocity`` is ``[vx, vy, vz, wx, wy, wz]`` in mm/s and deg/s. The arm
|
``velocity`` is ``[vx, vy, vz, wx, wy, wz]`` in mm/s and deg/s. The
|
||||||
keeps moving until the controller's own TTL expires or a new velocity
|
controller holds it until a different one arrives or an all-zero vector
|
||||||
arrives, so a sender must keep sending; an all-zero vector stops it and
|
stops it and leaves velocity mode.
|
||||||
leaves velocity mode.
|
|
||||||
|
**The velocity is set once and then left alone.** ``duration=0`` is the
|
||||||
|
SDK's "always effective, will not stop automatically", and an unchanged
|
||||||
|
velocity is not sent again at all. Both halves matter: this used to pass
|
||||||
|
``duration=velocity_command_ttl_s`` on every message, which re-armed a
|
||||||
|
timed segment several times a second and made the controller re-plan the
|
||||||
|
same motion over and over. On a real arm that is not smooth travel — an
|
||||||
|
operator feels it as the arm moving in steps. Re-arming even four times a
|
||||||
|
second is enough to produce it, so the answer is not to re-arm less
|
||||||
|
often but not to re-arm at all.
|
||||||
|
|
||||||
|
Dropping ``duration`` means the controller no longer stops the arm by
|
||||||
|
itself, so the expiry that backs a sender going quiet has to be enforced
|
||||||
|
here instead: :meth:`Control._expire_jog` commands the stop that the
|
||||||
|
controller's timer used to.
|
||||||
"""
|
"""
|
||||||
self._require_connection()
|
self._require_connection()
|
||||||
if any(velocity):
|
if any(velocity):
|
||||||
@@ -511,12 +529,19 @@ class ULite6:
|
|||||||
self.arm.set_mode(MODE_CARTESIAN_VELOCITY)
|
self.arm.set_mode(MODE_CARTESIAN_VELOCITY)
|
||||||
self.arm.set_state(0)
|
self.arm.set_state(0)
|
||||||
self._velocity_mode = True
|
self._velocity_mode = True
|
||||||
|
# Every route out of velocity mode comes back through here, so
|
||||||
|
# this is the one place the standing velocity has to be
|
||||||
|
# forgotten — re-entering the mode must always re-send.
|
||||||
|
self._commanded = None
|
||||||
|
if velocity == self._commanded:
|
||||||
|
return
|
||||||
code = self.arm.vc_set_cartesian_velocity(
|
code = self.arm.vc_set_cartesian_velocity(
|
||||||
velocity,
|
velocity,
|
||||||
is_radian=False,
|
is_radian=False,
|
||||||
is_tool_coord=False,
|
is_tool_coord=False,
|
||||||
duration=self.motion.velocity_command_ttl_s,
|
duration=0,
|
||||||
)
|
)
|
||||||
|
self._commanded = list(velocity)
|
||||||
self._check_motion_code(code, "set_cartesian_velocity")
|
self._check_motion_code(code, "set_cartesian_velocity")
|
||||||
return
|
return
|
||||||
if self._velocity_mode:
|
if self._velocity_mode:
|
||||||
|
|||||||
+1
-1
@@ -1,6 +1,6 @@
|
|||||||
[project]
|
[project]
|
||||||
name = "dora-ulite6"
|
name = "dora-ulite6"
|
||||||
version = "0.3.0"
|
version = "0.3.1"
|
||||||
description = "Dora node for a UFactory Lite6 arm: motion and control services, and joint and tool-pose telemetry"
|
description = "Dora node for a UFactory Lite6 arm: motion and control services, and joint and tool-pose telemetry"
|
||||||
readme = "README.md"
|
readme = "README.md"
|
||||||
license = { text = "MIT" }
|
license = { text = "MIT" }
|
||||||
|
|||||||
+6
-1
@@ -30,11 +30,16 @@ class FakeArm:
|
|||||||
self.recover_error = None
|
self.recover_error = None
|
||||||
self.block = None
|
self.block = None
|
||||||
self.stopped = False
|
self.stopped = False
|
||||||
|
self.connected = True
|
||||||
|
|
||||||
# --- reads ---
|
# --- reads ---
|
||||||
|
|
||||||
def is_connected(self):
|
def is_connected(self):
|
||||||
return True
|
return self.connected
|
||||||
|
|
||||||
|
def connect(self):
|
||||||
|
self.calls.append("connect")
|
||||||
|
return self.connected
|
||||||
|
|
||||||
def is_ready(self):
|
def is_ready(self):
|
||||||
return self.ready
|
return self.ready
|
||||||
|
|||||||
@@ -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]
|
||||||
+93
-4
@@ -6,17 +6,18 @@ import time
|
|||||||
import pytest
|
import pytest
|
||||||
|
|
||||||
from dora_ulite6.config import Config, MotionConfig
|
from dora_ulite6.config import Config, MotionConfig
|
||||||
from dora_ulite6.control import MOVING, PREEMPTED, READY, Control, ServiceCall
|
from dora_ulite6.control import MOVING, PREEMPTED, READY, UNAVAILABLE, Control, ServiceCall
|
||||||
from dora_ulite6.pose import RobotPose
|
from dora_ulite6.pose import RobotPose
|
||||||
from tests.conftest import FakeArm, wait_for
|
from tests.conftest import FakeArm, wait_for
|
||||||
|
|
||||||
|
|
||||||
def build(monkeypatch, **config):
|
def build(monkeypatch, connected=True, **config):
|
||||||
"""Return a started Control over a fake arm, and the arms it built."""
|
"""Return a started Control over a fake arm, and the arms it built."""
|
||||||
arms = []
|
arms = []
|
||||||
|
|
||||||
def make(cfg):
|
def make(cfg):
|
||||||
arm = FakeArm(cfg)
|
arm = FakeArm(cfg)
|
||||||
|
arm.connected = connected
|
||||||
arms.append(arm)
|
arms.append(arm)
|
||||||
return arm
|
return arm
|
||||||
|
|
||||||
@@ -87,6 +88,56 @@ def test_a_first_start_still_runs_init_mode(monkeypatch):
|
|||||||
control.shutdown()
|
control.shutdown()
|
||||||
|
|
||||||
|
|
||||||
|
# --- an arm that is not serving yet ---
|
||||||
|
|
||||||
|
|
||||||
|
def messages_on(control, output, seen):
|
||||||
|
"""Collect messages on one output, draining the outbox as the event loop would."""
|
||||||
|
seen.extend(control.take_messages())
|
||||||
|
return [message for message in seen if message.output == output]
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_request_sent_while_the_arm_is_down_is_still_answered(monkeypatch):
|
||||||
|
# An arm that is down is an outcome to report, not a reason to stop
|
||||||
|
# answering. `recover` in particular has to reach a controller that answers
|
||||||
|
# while refusing to serve — that state is the one it exists to escape.
|
||||||
|
control = build(monkeypatch, connected=False, reconnect_interval_s=0.05)
|
||||||
|
seen = []
|
||||||
|
try:
|
||||||
|
wait_for(lambda: UNAVAILABLE in control.take_state_changes())
|
||||||
|
|
||||||
|
control.submit(ServiceCall(request_id="r1", row={"service": "recover"}))
|
||||||
|
|
||||||
|
responses = wait_for(lambda: messages_on(control, "response", seen))
|
||||||
|
assert responses[0].metadata["request_id"] == "r1"
|
||||||
|
assert responses[0].fields["ok"] is True
|
||||||
|
assert "recover" in control.arms[0].calls
|
||||||
|
finally:
|
||||||
|
control.shutdown()
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_motion_sent_while_the_arm_is_down_is_refused_rather_than_held(monkeypatch):
|
||||||
|
# Holding it would be worse than silence: the arm is somewhere unknown and
|
||||||
|
# nobody is watching, so a held motion would fire unattended the moment the
|
||||||
|
# link came back.
|
||||||
|
control = build(monkeypatch, connected=False, reconnect_interval_s=0.05)
|
||||||
|
seen = []
|
||||||
|
try:
|
||||||
|
wait_for(lambda: UNAVAILABLE in control.take_state_changes())
|
||||||
|
|
||||||
|
control.start_motion(ServiceCall(request_id="m1", row={"service": "home"}))
|
||||||
|
|
||||||
|
responses = wait_for(lambda: messages_on(control, "response", seen))
|
||||||
|
assert responses[0].metadata["request_id"] == "m1"
|
||||||
|
assert responses[0].fields["ok"] is False
|
||||||
|
|
||||||
|
control.arms[0].connected = True
|
||||||
|
wait_for(lambda: READY in control.take_state_changes())
|
||||||
|
assert "go_home" not in control.arms[0].calls
|
||||||
|
finally:
|
||||||
|
control.shutdown()
|
||||||
|
|
||||||
|
|
||||||
# --- jogging ---
|
# --- jogging ---
|
||||||
|
|
||||||
|
|
||||||
@@ -130,9 +181,31 @@ def test_a_zero_velocity_jog_puts_the_arm_back_to_ready(monkeypatch):
|
|||||||
control.shutdown()
|
control.shutdown()
|
||||||
|
|
||||||
|
|
||||||
|
def test_a_zero_velocity_jog_does_not_make_a_running_motion_look_idle(monkeypatch):
|
||||||
|
# A teleop source streaming zero velocity says nothing about a motion that
|
||||||
|
# owns the arm, and must not answer for it: an arm that is travelling must
|
||||||
|
# never look idle, and a `ready` here would also stop the motion's own end
|
||||||
|
# from ever being published.
|
||||||
|
control = build(monkeypatch)
|
||||||
|
control.arms[0].block = threading.Event()
|
||||||
|
try:
|
||||||
|
wait_for(lambda: READY in control.take_state_changes())
|
||||||
|
control.submit(ServiceCall(request_id="m1", row={"service": "home"}))
|
||||||
|
wait_for(lambda: MOVING in control.take_state_changes())
|
||||||
|
|
||||||
|
for _ in range(5):
|
||||||
|
control.start_jog([0.0] * 6)
|
||||||
|
|
||||||
|
assert control.take_state_changes() == []
|
||||||
|
|
||||||
|
control.arms[0].block.set()
|
||||||
|
wait_for(lambda: READY in control.take_state_changes())
|
||||||
|
finally:
|
||||||
|
control.shutdown()
|
||||||
|
|
||||||
|
|
||||||
def test_a_sender_that_goes_quiet_stops_being_called_jogging(monkeypatch):
|
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
|
# Nothing tells us a sender died, so the clock has to.
|
||||||
# clock has to.
|
|
||||||
control = build(monkeypatch, motion=MotionConfig(velocity_command_ttl_s=0.02))
|
control = build(monkeypatch, motion=MotionConfig(velocity_command_ttl_s=0.02))
|
||||||
try:
|
try:
|
||||||
wait_for(lambda: READY in control.take_state_changes())
|
wait_for(lambda: READY in control.take_state_changes())
|
||||||
@@ -144,6 +217,22 @@ def test_a_sender_that_goes_quiet_stops_being_called_jogging(monkeypatch):
|
|||||||
control.shutdown()
|
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):
|
def test_a_jog_preempts_a_motion_that_is_running(monkeypatch):
|
||||||
control = build(monkeypatch)
|
control = build(monkeypatch)
|
||||||
control.arms[0].block = threading.Event()
|
control.arms[0].block = threading.Event()
|
||||||
|
|||||||
Reference in New Issue
Block a user