Files
a7l20-hug-sim/scripts/trajectory_supervisor.py

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)]