async def abort_trajectory(self):
"""Aborts the trajectory transmission."""
cmd = await self.fps.send_command(
"SEND_TRAJECTORY_ABORT",
positioner_ids=list(self.trajectories.keys()),
)
if cmd.status.failed:
raise TrajectoryError("Cannot abort trajectory transmission.",
self)
def validate(self):
"""Validates the trajectory."""
if len(self.trajectories) == 0:
raise TrajectoryError("trajectory is empty.", self)
if len(self.trajectories) != len(numpy.unique(list(
self.trajectories))):
raise TrajectoryError("Duplicate positioner trajectories.", self)
for pid in self.trajectories:
trajectory = self.trajectories[pid]
if "alpha" not in trajectory or "beta" not in trajectory:
self.failed_positioners[pid] = "NO_DATA"
raise TrajectoryError(
f"Positioner {pid} missing alpha or beta data.",
self,
)
for arm in ["alpha", "beta"]:
data = numpy.array(list(zip(*trajectory[arm]))[0])
if arm == "beta":
if config.get("safe_mode", False):
if config["safe_mode"] is True:
min_beta = 160
else:
min_beta = config["safe_mode"]["min_beta"]
if numpy.any(data < min_beta):
self.failed_positioners[pid] = "SAFE_MODE"
raise TrajectoryError(
f"Positioner {pid}: safe mode is on "
f"and beta < {min_beta}.",
self,
)
async def unwind(
current_positions: dict[int, tuple[float | None, float | None]],
collision_buffer: float | None = None,
disabled: list[int] = [],
force: bool = False,
):
"""Folds all the robots to the lattice position.
This coroutine uses a process pool executor to run Kaiju routines.
"""
alpha0, beta0 = config["kaiju"]["lattice_position"]
# We create the data directly since it's simple. This should be a bit faster
# than creating a grid and dumping it.
data = {"collision_buffer": collision_buffer, "grid": {}}
for pid, (alpha, beta) in current_positions.items():
data["grid"][int(pid)] = (alpha, beta, alpha0, beta0, pid in disabled)
(to_destination, _, did_fail, deadlocks) = await run_in_executor(
get_path_pair,
data=data,
ignore_did_fail=force,
stop_if_deadlock=force,
executor="process",
)
if did_fail:
if force is False:
raise TrajectoryError(
"Failed generating a valid unwind trajectory. "
f"{len(deadlocks)} deadlocks were found ({deadlocks}).")
else:
log.warning("Deadlocks found in unwind but proceeding anyway.")
return to_destination
async def goto(
fps: FPS,
new_positions: dict[int, tuple[float, float]],
speed: Optional[float] = None,
relative: bool = False,
use_sync_line: bool = None,
go_cowboy: bool = False,
force: bool = False,
command: CluCommand[JaegerActor] | None = None,
):
"""Send positioners to a given position using a trajectory with ``kaiju`` check.
Parameters
----------
fps
The `.FPS` instance.
new_positions
The new positions as a dictionary of positioner ID to a tuple of new
alpha and beta angles. Positioners not specified will be kept on the
same positions.
speed
The speed to use.
relative
If `True`, ``alpha`` and ``beta`` are considered relative angles.
use_sync_line
Whether to use the SYNC line to start the trajectories.
go_cowboy
If set, does not create a ``kaiju``-safe trajectory. Use at your own risk.
force
If ``go_cowboy=False`` and the trajectory is deadlocked, a `.TrajectoryError`
will be raised. Use ``force=True`` to apply the trajectory anyway.
command
A command to pass to `.send_trajectory` to output additional information.
"""
if fps.locked:
FPSLockedError("The FPS is locked.")
if fps.moving:
raise JaegerError("FPS is moving. Cannot send goto.")
for pid in new_positions:
if pid not in fps.positioners:
raise JaegerError(f"Positioner ID {pid} is not connected.")
speed = float(speed or config["positioner"]["motor_speed"])
if speed < 500 or speed > 5000:
raise JaegerError("Invalid speed.")
positioner_ids = list(new_positions.keys())
await fps.update_position(positioner_ids=positioner_ids)
trajectories = {}
if go_cowboy is True:
for pid in positioner_ids:
pos = fps[pid]
if pos.alpha is None or pos.beta is None:
raise JaegerError(
f"Positioner {pid}: cannot goto with unknown position.")
current_alpha = pos.alpha
current_beta = pos.beta
if relative is True:
alpha_end = current_alpha + new_positions[pid][0]
beta_end = current_beta + new_positions[pid][1]
else:
alpha_end = new_positions[pid][0]
beta_end = new_positions[pid][1]
alpha_delta = abs(alpha_end - current_alpha)
beta_delta = abs(beta_end - current_beta)
time_end = [
get_goto_move_time(alpha_delta, speed=speed),
get_goto_move_time(beta_delta, speed=speed),
]
trajectories[pid] = {
"alpha": [(current_alpha, 0.1),
(alpha_end, time_end[0] + 0.1)],
"beta": [(current_beta, 0.1), (beta_end, time_end[1] + 0.1)],
}
else:
if relative is True:
raise JaegerError("relative is not implemented for kaiju moves.")
data = {"collision_buffer": None, "grid": {}}
for pid, (current_alpha,
current_beta) in fps.get_positions_dict().items():
if current_alpha is None or current_beta is None:
raise JaegerError(
f"Positioner {pid} does not know its position.")
if pid in new_positions:
data["grid"][int(pid)] = (
current_alpha,
current_beta,
new_positions[pid][0],
new_positions[pid][1],
fps[pid].disabled,
)
else:
data["grid"][int(pid)] = (
current_alpha,
current_beta,
current_alpha,
current_beta,
fps[pid].disabled,
)
(to_destination, _, did_fail, deadlocks) = await run_in_executor(
get_path_pair,
data=data,
stop_if_deadlock=force,
executor="process",
ignore_did_fail=force,
)
if did_fail is True:
if force is False:
raise TrajectoryError(
"Cannot execute trajectory. Found "
f"{len(deadlocks)} deadlocks ({deadlocks}).")
else:
warnings.warn(
f"Found {len(deadlocks)} deadlocks but applying trajectory.",
JaegerUserWarning,
)
trajectories = to_destination
return await send_trajectory(
fps,
trajectories,
use_sync_line=use_sync_line,
command=command,
extra_dump_data={"kaiju_trajectory": not go_cowboy},
)
async def send_trajectory(
fps: FPS,
trajectories: str | pathlib.Path | TrajectoryDataType,
use_sync_line: bool | None = None,
send_trajectory: bool = True,
start_trajectory: bool = True,
command: Optional[CluCommand[JaegerActor]] = None,
dump: bool | str = True,
extra_dump_data: dict[str, Any] = {},
) -> Trajectory:
"""Sends a set of trajectories to the positioners.
This is a low-level function that raises errors when a problem is
encountered. Most users should use `.FPS.send_trajectory` instead.
`.send_trajectory` automates `.Trajectory` by calling the different
methods in order, but provides less control.
Parameters
----------
fps
The instance of `.FPS` that will receive the trajectory.
trajectories
Either a path to a YAML file to read or a dictionary with the
trajectories. In either case the format must be a dictionary in
which the keys are the ``positioner_ids`` and each value is a
dictionary containing two keys: ``alpha`` and ``beta``, each
pointing to a list of tuples ``(position, time)``, where
``position`` is in degrees and ``time`` is in seconds.
use_sync_line
If `True`, the SYNC line will be used to synchronise the beginning of
all trajectories. Otherwise a `.START_TRAJECTORY` command will be sent
over the CAN network. If `None`, defaults to the configuration parameter.
send_trajectory
If `True`, sends the trajectory to the positioners and returns the
`.Trajectory` instance.
start_trajectory
If `True`, runs the trajectory after sending it. Otherwise, returns
the `.Trajectory` instance after sending the data. Ignored if
`send_trajectory=False`.
command
A command to which to output informational messages.
dump
Whether to dump a JSON with the trajectory to disk. If `True`,
the trajectory is stored in ``/data/logs/jaeger/trajectories/<MJD>`` with
a unique sequence for each trajectory. A string can be passed with a
custom path. The dump file is created only if `.Trajectory.start` is
called, regardless of whether the trajectory succeeds.
extra_dump_data
A dictionary with additional parameters to add to the dump JSON.
Raises
------
TrajectoryError
If encounters a problem sending the trajectory.
FPSLockedError
If the FPS is locked.
Examples
--------
::
>>> fps = FPS()
>>> await fps.initialise()
# Send a trajectory with two points for positioner 4
>>> await fps.send_trajectory({4: {'alpha': [(90, 0), (91, 3)],
'beta': [(20, 0), (23, 4)]}})
"""
traj = Trajectory(
fps,
trajectories,
dump=dump,
extra_dump_data=extra_dump_data,
)
if use_sync_line is None:
use_sync_line = config["fps"]["use_sync_line"]
if use_sync_line:
if not isinstance(fps.ieb, IEB) or fps.ieb.disabled:
raise TrajectoryError(
"IEB is not connected. Cannot use SYNC line.", traj)
if (await fps.ieb.get_device("sync").read())[0] == "closed":
raise TrajectoryError("The SYNC line is on high.", traj)
if send_trajectory is False:
return traj
msg = "Sending trajectory data."
log.debug(msg)
if command:
command.debug(msg)
try:
await traj.send()
except TrajectoryError as err:
raise TrajectoryError(
f"Something went wrong sending the trajectory: {err}",
err.trajectory,
)
finally:
if traj.dump_file:
traj.dump_trajectory()
if command:
command.debug(trajectory_dump_file=traj.dump_file)
msg = f"Trajectory sent in {traj.data_send_time:.1f} seconds."
log.debug(msg)
if command:
command.debug(msg)
log.info(
f"Expected time to complete trajectory: {traj.move_time:.2f} seconds.")
if command and traj.move_time:
command.info(move_time=round(traj.move_time, 2))
if start_trajectory is False:
return traj
msg = "Starting trajectory ..."
log.info(msg)
if command:
command.info(msg)
try:
await traj.start(use_sync_line=use_sync_line)
except TrajectoryError as err:
if traj.start_time is not None and traj.end_time is not None:
elapsed = traj.end_time - traj.start_time
elapsed_msg = f" Trajectory failed {elapsed:.2f} seconds after start."
else:
elapsed_msg = ""
raise TrajectoryError(
f"Something went wrong during the trajectory: {err}.{elapsed_msg}",
err.trajectory,
)
finally:
if traj.dump_file and command:
command.debug(trajectory_dump_file=traj.dump_file)
if command:
command.info(folded=(await fps.is_folded()))
msg = "All positioners have reached their destinations."
log.info(msg)
if command:
command.info(msg)
return traj
async def start(self, use_sync_line=True):
"""Starts the trajectory."""
if not self._ready_to_start or self.failed:
raise TrajectoryError("The trajectory has not been sent.", self)
if self.move_time is None:
raise TrajectoryError("move_time not set.", self)
self.use_sync_line = use_sync_line
if use_sync_line:
if not isinstance(self.fps.ieb, IEB) or self.fps.ieb.disabled:
raise TrajectoryError(
"IEB is not connected. Cannot use SYNC line.",
self,
)
if (await self.fps.ieb.get_device("sync").read())[0] == "closed":
raise TrajectoryError("The SYNC line is on high.", self)
sync = self.fps.ieb.get_device("sync")
assert isinstance(sync, drift.Relay)
# Set SYNC line to high.
await sync.close()
# Schedule reseting of SYNC line
asyncio.get_event_loop().call_later(0.5, asyncio.create_task,
sync.open())
else:
# Start trajectories
command = await self.fps.send_command(
"START_TRAJECTORY",
positioner_ids=0,
timeout=1,
# All positioners reply, including those not in the trajectory.
n_positioners=len(self.fps.positioners),
)
if command.status.failed:
await self.fps.stop_trajectory()
self.failed = True
raise TrajectoryError("START_TRAJECTORY failed", self)
restart_pollers = True if self.fps.pollers.running else False
await self.fps.pollers.stop()
self.start_time = time.time()
# min_trajectory_time = 5.0
# PS = PositionerStatus
try:
# # The positioners take a bit to report that they are moving so if the
# # move time is too short, we don't try to check if the positioners started
# # moving (but we'll check later that they arrived to their positions.)
# if self.move_time >= min_trajectory_time:
# await asyncio.sleep(min_trajectory_time)
# await self.fps.update_status()
# if self.fps.status & FPSStatus.IDLE:
# raise TrajectoryError("Move failed to start.")
# sbits = numpy.array([p.status for p in self.fps.values()])
# not_moving = numpy.where(sbits & PS.DISPLACEMENT_COMPLETED)
# if not_moving[0].any():
# not_moving_pids = numpy.array(list(self.fps))[not_moving[0]]
# # Before reporting that the positioner is not moving, check if
# # it's already there.
# await self.fps.update_position()
# positions = self.fps.get_positions()
# really_not_moving = []
# for pid in not_moving_pids:
# if pid not in self.trajectories:
# continue
# current = positions[positions[:, 0] == pid][0, 1:]
# alpha = self.trajectories[pid]["alpha"][-1][0]
# beta = self.trajectories[pid]["beta"][-1][0]
# if not numpy.allclose(current - [alpha, beta], 0, atol=0.1):
# really_not_moving.append(pid)
# if len(really_not_moving) > 0:
# not_moving_str = ", ".join(map(str, really_not_moving))
# # Should this be an error?
# warnings.warn(
# "Some positioners do not appear to be "
# f"moving: {not_moving_str}.",
# JaegerUserWarning,
# )
while True:
await asyncio.sleep(1)
if self.fps.locked:
raise TrajectoryError(
"The FPS got locked during the trajectory.",
self,
)
await self.fps.update_status()
if self.fps.status & FPSStatus.IDLE:
self.failed = False
break
elapsed = time.time() - self.start_time
if elapsed > (self.move_time + 3):
raise TrajectoryError(
"Some positioners did not complete the move.",
self,
)
# TODO: There seems to be bug in the firmware. Sometimes when a positioner
# fails to start its trajectory, at the end of the trajectory time it
# does believe it has reached the commanded position, although it's still
# at the initial position. In those cases issuing a SEND_TRAJECTORY_ABORT
# followed by a position update seems to return correct positions.
await self.fps.stop_trajectory()
# The FPS says they have all stopped moving but check that they are
# actually at their positions.
await self.fps.update_position()
failed_reach = False
for pid in self.trajectories:
alpha = self.trajectories[pid]["alpha"][-1][0]
beta = self.trajectories[pid]["beta"][-1][0]
current_position = numpy.array(self.fps[pid].position)
if not numpy.allclose(current_position, [alpha, beta],
atol=0.1):
warnings.warn(
f"Positioner {pid} may not have reached its destination.",
JaegerUserWarning,
)
failed_reach = True
if failed_reach:
raise TrajectoryError(
"Some positioners did not reach their destinations.",
self,
)
except BaseException:
self.failed = True
await self.fps.stop_trajectory()
raise
finally:
# Not explicitely updating the positions here because save_snapshot()
# will do that and no need to waste extra time.
await self.fps.save_snapshot()
if self.dump_file:
self.dump_trajectory()
self.end_time = time.time()
if restart_pollers:
self.fps.pollers.start()
return True
async def send(self):
"""Sends the trajectory but does not start it."""
self.move_time = 0.0
await self.fps.stop_trajectory()
await self.fps.stop_trajectory(clear_flags=True)
if not await self.fps.update_status(
positioner_ids=list(self.trajectories),
timeout=1.0,
):
self.failed = True
raise TrajectoryError("Some positioners did not respond.", self)
# Check that all positioners are ready to receive a new trajectory.
for pos_id in self.trajectories:
positioner = self.fps.positioners[pos_id]
status = positioner.status
if positioner.disabled:
raise TrajectoryError(
f"positioner_id={pos_id} is disabled/offline but was "
"included in the trajectory.",
self,
)
if (positioner.flags.DATUM_ALPHA_INITIALIZED not in status
or positioner.flags.DATUM_BETA_INITIALIZED not in status
or positioner.flags.DISPLACEMENT_COMPLETED not in status):
self.failed = True
self.failed_positioners[pos_id] = "NOT_READY"
raise TrajectoryError(
f"positioner_id={pos_id} is not ready to receive a trajectory.",
self,
)
traj_pos = self.trajectories[pos_id]
self.n_points[pos_id] = (len(traj_pos["alpha"]),
len(traj_pos["beta"]))
# Gets maximum time for this positioner
max_time_pos = max([
max(list(zip(*traj_pos["alpha"]))[1]),
max(list(zip(*traj_pos["beta"]))[1]),
])
# Updates the global trajectory max time.
if max_time_pos > self.move_time:
self.move_time = max_time_pos
new_traj_data = {}
for pos_id in self.trajectories:
data = SendNewTrajectory.get_data(
self.n_points[pos_id][0],
self.n_points[pos_id][1],
)
new_traj_data[pos_id] = data
# Starts trajectory
new_traj_cmd = await self.fps.send_command(
"SEND_NEW_TRAJECTORY",
positioner_ids=list(self.trajectories),
data=new_traj_data,
)
if new_traj_cmd.status.failed or new_traj_cmd.status.timed_out:
self.failed = True
raise TrajectoryError("Failed sending SEND_NEW_TRAJECTORY.", self)
start_trajectory_send_time = time.time()
# How many points from the trajectory are we putting in each command.
n_chunk = config["positioner"]["trajectory_data_n_points"]
# Gets the maximum number of points for each arm for all positioners.
max_points = numpy.max(list(self.n_points.values()), axis=0)
max_points = {"alpha": max_points[0], "beta": max_points[1]}
# Send chunks of size n_chunk to all the positioners in parallel.
# Do alpha first, then beta.
for arm in ["alpha", "beta"]:
for jj in range(0, max_points[arm], n_chunk):
data = {}
send_trajectory_pids = []
for pos_id in self.trajectories:
arm_chunk = self.trajectories[pos_id][arm][jj:jj + n_chunk]
if len(arm_chunk) == 0:
continue
send_trajectory_pids.append(pos_id)
positions = numpy.array(arm_chunk).astype(numpy.float64)
data_pos = SendTrajectoryData.calculate_positions(
positions)
data[pos_id] = data_pos
self.data_send_cmd = await self.fps.send_command(
"SEND_TRAJECTORY_DATA",
positioner_ids=send_trajectory_pids,
data=data,
)
status = self.data_send_cmd.status
if status.failed or status.timed_out:
for reply in self.data_send_cmd.replies:
if reply.response_code != ResponseCode.COMMAND_ACCEPTED:
code = reply.response_code.name
self.failed_positioners[reply.positioner_id] = code
self.failed = True
raise TrajectoryError(
"At least one SEND_TRAJECTORY_COMMAND failed.",
self,
)
# Finalise the trajectories
self.end_traj_cmds = await self.fps.send_command(
"TRAJECTORY_DATA_END",
positioner_ids=list(self.trajectories.keys()),
)
for cmd in self.end_traj_cmds:
if cmd.status.failed:
self.failed = True
raise TrajectoryError("TRAJECTORY_DATA_END failed.", self)
if ResponseCode.INVALID_TRAJECTORY in cmd.replies[0].response_code:
self.failed = True
raise TrajectoryError(
f"positioner_id={cmd.positioner_id} got an "
f"INVALID_TRAJECTORY reply.",
self,
)
self.data_send_time = time.time() - start_trajectory_send_time
self._ready_to_start = True
self.failed = False
return True
def __init__(
self,
fps: FPS,
trajectories: str | pathlib.Path | TrajectoryDataType,
dump: bool | str = True,
extra_dump_data: dict[str, Any] = {},
):
self.fps = fps
self.trajectories: TrajectoryDataType
if self.fps.locked:
raise FPSLockedError(
f"FPS is locked by {fps.locked_by}. Cannot send trajectories.")
if self.fps.moving:
raise TrajectoryError(
"The FPS is moving. Cannot send new trajectory.",
self,
)
if isinstance(trajectories, (str, pathlib.Path)):
path = pathlib.Path(trajectories)
if path.suffix == ".json":
self.trajectories = json.loads(open(path,
"r").read())["trajectory"]
else:
self.trajectories = cast(dict, read_yaml_file(trajectories))
elif isinstance(trajectories, dict):
self.trajectories = trajectories
else:
raise TrajectoryError("invalid trajectory data.", self)
# List of positioners that failed receiving the trajectory and reason.
self.failed_positioners: dict[int, str] = {}
self.validate()
#: Number of points sent to each positioner as a tuple ``(alpha, beta)``.
self.n_points = {}
#: The time required to complete the trajectory.
self.move_time: float | None = None
#: How long it took to send the trajectory.
self.data_send_time: float | None = None
self.failed = False
self.send_new_trajectory_failed = False
# Commands that will be sent. Mostly for inspection if the trajectory fails.
self.data_send_cmd: Command | None = None
self.end_traj_cmds: Command | None = None
self.start_time: float | None = None
self.end_time: float | None = None
self.use_sync_line: bool = True
self._ready_to_start = False
self.dump_data = {
"start_time": time.time(),
"success": False,
"trajectory_send_time": None,
"trajectory_start_time": None,
"end_time": None,
"use_sync_line": True,
"extra": extra_dump_data,
"trajectory": self.trajectories,
"initial_positions": self.fps.get_positions_dict(),
"final_positions": {},
}
if dump is False:
self.dump_file = None
elif isinstance(dump, (str, pathlib.Path)):
self.dump_file = str(dump)
else:
dirname = config["positioner"]["trajectory_dump_path"]
mjd = str(int(Time.now().mjd))
dirname = os.path.join(dirname, mjd)
files = list(sorted(glob(os.path.join(dirname, "*.json"))))
if len(files) == 0:
seq = 1
else:
seq = int(files[-1].split("-")[-1].split(".")[0]) + 1
self.dump_file = os.path.join(dirname,
f"trajectory-{mjd}-{seq:04d}.json")