123 lines
6.7 KiB
Python
123 lines
6.7 KiB
Python
"""Offline trajectory reference and nonblocking execution supervisor.
|
|
|
|
No ROS/CAN/network/enable interface. Returned actions are simulation intents,
|
|
NOT MoveJ commands. Hardware needs a verified timed-trajectory/stop adapter.
|
|
"""
|
|
import numpy as np
|
|
from elbow_raise_preflight import timing_preview
|
|
|
|
|
|
class Trajectory:
|
|
def __init__(self, points):
|
|
self.points=np.asarray(points,float)
|
|
preview=timing_preview(self.points)
|
|
self.times=np.asarray(preview['time_from_start_s'])
|
|
self.duration=float(self.times[-1])
|
|
|
|
def sample(self, elapsed):
|
|
if not np.isfinite(elapsed):raise ValueError('Nonfinite trajectory time')
|
|
t=float(np.clip(elapsed,0,self.duration))
|
|
i=min(int(np.searchsorted(self.times,t,side='right')-1),len(self.points)-2)
|
|
dt=self.times[i+1]-self.times[i];s=(t-self.times[i])/dt
|
|
delta=self.points[i+1]-self.points[i]
|
|
return dict(position=(self.points[i]+(3*s*s-2*s**3)*delta).tolist(),
|
|
velocity=((6*s-6*s*s)*delta/dt).tolist(),
|
|
acceleration=((6-12*s)*delta/dt**2).tolist())
|
|
|
|
|
|
class Supervisor:
|
|
"""tick() never blocks; caller supplies distinct sensor observations.
|
|
|
|
emit must also be nonblocking. Sensor timestamps use this process's
|
|
monotonic time domain. seq advances only for genuinely new observations.
|
|
Stop acceptance alone is NOT stop completion; stationary feedback needed.
|
|
"""
|
|
TERMINAL={'DONE','FAULT_STOPPED','FAULT_STOP_UNCONFIRMED'}
|
|
|
|
def __init__(self, trajectory, emit=None):
|
|
self.trajectory=trajectory;self.emit=emit or (lambda event:None)
|
|
self.state='IDLE';self.last_tick=None;self.last_feedback=None
|
|
self.last_seq=-1;self.position=None;self.velocity=None
|
|
self.reason=None;self.started=None;self.last_progress=-float('inf')
|
|
self.stop_accepted=False;self.stationary_since=None
|
|
|
|
def event(self, now, kind, **data):
|
|
self.emit(dict(time=now,event=kind,state=self.state,**data))
|
|
|
|
def feedback(self, now, observation):
|
|
if observation is None:return
|
|
seq=observation['seq'];stamp=observation['received']
|
|
q=np.asarray(observation['position'],float);v=np.asarray(observation['velocity'],float)
|
|
if (type(seq) is not int or not np.isfinite(stamp) or stamp>now or
|
|
q.shape!=(7,) or v.shape!=(7,) or not np.isfinite(q).all() or not np.isfinite(v).all()):
|
|
raise ValueError('invalid_feedback')
|
|
if seq<=self.last_seq:return
|
|
if self.last_feedback is not None and stamp<self.last_feedback:raise ValueError('feedback_time_regressed')
|
|
self.last_seq=seq;self.last_feedback=stamp;self.position=q;self.velocity=v
|
|
|
|
def abort(self, now, reason):
|
|
if self.state in self.TERMINAL or self.state=='STOPPING':return []
|
|
self.reason=reason;self.state='STOPPING';self.stop_deadline=now+1.
|
|
self.stationary_since=None;self.stop_accepted=False
|
|
self.event(now,'alarm',reason=reason,stop_confirmed=False)
|
|
self.event(now,'stop_requested',enable_change_requested=False)
|
|
return [dict(action='request_controlled_stop',reason=reason)]
|
|
|
|
def tick(self, now, observation=None, acknowledgement=None, stop_ack=None, cancel=False):
|
|
if not np.isfinite(now):raise ValueError('Nonfinite supervisor time')
|
|
if self.state in self.TERMINAL:return []
|
|
if self.last_tick is not None and now<self.last_tick:raise ValueError('Monotonic clock regressed')
|
|
gap=0 if self.last_tick is None else now-self.last_tick
|
|
self.last_tick=now
|
|
try:self.feedback(now,observation)
|
|
except (ValueError,KeyError,TypeError) as exc:
|
|
if self.state!='STOPPING':return self.abort(now,str(exc))
|
|
# Bad packets must not prevent the stop deadline from expiring.
|
|
self.last_feedback=None
|
|
self.event(now,'invalid_feedback_during_stop',reason=str(exc))
|
|
fresh=self.last_feedback is not None and now-self.last_feedback<=.2
|
|
if self.state=='STOPPING':
|
|
if stop_ack is True:self.stop_accepted=True
|
|
if stop_ack is False:
|
|
self.state='FAULT_STOP_UNCONFIRMED';self.event(now,'stop_unconfirmed',reason='stop_rejected');return []
|
|
stationary=fresh and np.max(np.abs(self.velocity))<=.002
|
|
if stationary and self.stop_accepted:
|
|
if self.stationary_since is None:self.stationary_since=now
|
|
if now-self.stationary_since>=.2:
|
|
self.state='FAULT_STOPPED';self.event(now,'stop_confirmed');return []
|
|
else:self.stationary_since=None
|
|
if now>=self.stop_deadline:
|
|
self.state='FAULT_STOP_UNCONFIRMED';self.event(now,'stop_unconfirmed',reason='stop_timeout_or_feedback_missing')
|
|
return []
|
|
if cancel:return self.abort(now,'user_cancel')
|
|
if gap>.1 and self.state!='IDLE':return self.abort(now,'scheduler_deadline_missed')
|
|
if not fresh:return self.abort(now,'feedback_timeout')
|
|
if self.state=='IDLE':
|
|
if np.max(np.abs(self.position-self.trajectory.points[0]))>.002:return self.abort(now,'start_mismatch')
|
|
if np.max(np.abs(self.velocity))>.002:return self.abort(now,'start_not_stationary')
|
|
self.state='WAIT_ACK';self.ack_deadline=now+.5
|
|
self.event(now,'submission_requested',duration_s=self.trajectory.duration)
|
|
return [dict(action='submit_timed_trajectory')]
|
|
if self.state=='WAIT_ACK':
|
|
if acknowledgement is False:return self.abort(now,'trajectory_rejected')
|
|
if now>=self.ack_deadline:return self.abort(now,'ack_timeout')
|
|
if acknowledgement is not True:return []
|
|
self.started=now;self.state='RUNNING';self.event(now,'started')
|
|
elapsed=now-self.started
|
|
reference=self.trajectory.sample(elapsed)
|
|
tracking_error=float(np.max(np.abs(self.position-reference['position'])))
|
|
if tracking_error>.01:return self.abort(now,'tracking_error')
|
|
if now-self.last_progress>=1.:
|
|
self.event(now,'progress',fraction=min(elapsed/self.trajectory.duration,1.),tracking_error_rad=tracking_error)
|
|
self.last_progress=now
|
|
if elapsed>=self.trajectory.duration:
|
|
self.state='SETTLING'
|
|
settled=tracking_error<=.002 and np.max(np.abs(self.velocity))<=.002
|
|
if settled:
|
|
if self.stationary_since is None:self.stationary_since=now
|
|
if now-self.stationary_since>=.2:
|
|
self.state='DONE';self.event(now,'completed');return []
|
|
else:self.stationary_since=None
|
|
if elapsed>=self.trajectory.duration+2.:return self.abort(now,'arrival_timeout')
|
|
return [dict(action='reference_only',elapsed_s=elapsed,**reference)]
|