def test_ref(r, time, setpoint):
ref = np.zeros((len(time), 9))
for i in range(1, time.size):
sp_quat = pa.quat_of_euler(setpoint[i])
r.update_quat(sp_quat, time[i] - time[i - 1])
euler = pa.euler_of_quat(r.quat)
ref[i] = np.concatenate((euler, r.vel, r.accel))
return ref
def test_ref(r, time, setpoint):
ref = np.zeros((len(time), 9))
for i in range(1, time.size):
sp_quat = pa.quat_of_euler(setpoint[i])
r.update_quat(sp_quat, time[i] - time[i - 1])
euler = pa.euler_of_quat(r.quat)
ref[i] = np.concatenate((euler, r.vel, r.accel))
return ref
def update(self, type, duration, step_duration, step_ampl):
self.type = type
self.duration, self.step_duration, self.step_ampl = duration, step_duration, step_ampl
self.time = np.arange(0., self.duration, self.dt)
self.euler = np.zeros((len(self.time), pa.e_size))
try:
i = [Setpoint.t_step_phi, Setpoint.t_step_theta, Setpoint.t_step_psi].index(self.type)
self.euler[:, i] = step_ampl / 2 * scipy.signal.square(math.pi / step_duration * self.time)
except Exception as e:
print(e)
pass
self.quat = np.zeros((len(self.time), pa.q_size))
for i in range(0, len(self.time)):
self.quat[i] = pa.quat_of_euler(self.euler[i])
def set_euler(self, euler, vel=np.zeros(3), accel=np.zeros(3)):
self.euler = euler
self.vel = vel
self.accel = accel
self.quat = pa.quat_of_euler(self.euler)
def update_euler(self, setpoint, dt):
self.update_quat(pa.quat_of_euler(setpoint), dt)
self.euler = pa.euler_of_quat(self.quat)
