def _work_step(self, i, sp):
start, stop = int(i * self.n_iter_per_step), int((i + 1) * self.n_iter_per_step)
# print('_work_step of %s: i %i, start %i, stop %i' % (self.impl, i, start, stop))
for j in range(start, stop):
self.impl.update_quat(sp.quat[j], sp.dt)
self.quat[j], self.vel[j], self.accel[j] = self.impl.quat, self.impl.vel, self.impl.accel
self.euler[j] = pa.euler_of_quat(self.quat[j])
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 random_setpoint(time, dt_step=2):
tf = time[0]
sp = np.zeros((len(time), 3))
sp_i = [0, 0, 0]
for i in range(0, len(time)):
if time[i] >= tf:
ui = np.random.rand(3) - [0.5, 0.5, 0.5]
ai = np.random.rand(1)
n = np.linalg.norm(ui)
if n > 0:
ui /= n
sp_i = pa.euler_of_quat(pa.quat_of_axis_angle(ui, ai))
tf += dt_step
sp[i] = sp_i
return sp
def random_setpoint(time, dt_step=2):
tf = time[0]
sp = np.zeros((len(time), 3))
sp_i = [0, 0, 0]
for i in range(0, len(time)):
if time[i] >= tf:
ui = np.random.rand(3) - [0.5, 0.5, 0.5];
ai = np.random.rand(1)
n = np.linalg.norm(ui)
if n > 0:
ui /= n
sp_i = pa.euler_of_quat(pa.quat_of_axis_angle(ui, ai))
tf += dt_step
sp[i] = sp_i
return sp
def set_quat(self, quat, vel=np.zeros(3), accel=np.zeros(3)):
self.quat = quat
self.vel = vel
self.accel = accel
self.euler = pa.euler_of_quat(self.quat)
def update_euler(self, setpoint, dt):
self.update_quat(pa.quat_of_euler(setpoint), dt)
self.euler = pa.euler_of_quat(self.quat)
