def update_quat(self, setpoint, dt):
self.quat = pa.quat_integrate(self.quat, self.vel, dt)
self.vel += dt * self.accel
err_quat = pa.quat_wrap_shortest(pa.quat_inv_comp(setpoint, self.quat))
self.accel = -2. * self.xi * self.omega * self.vel \
- 2. * self.omega * self.omega * err_quat[1:]
return self.quat, self.vel, self.accel
def update_quat(self, sp, dt):
err_quat = 2. * np.array(pa.quat_wrap_shortest(pa.quat_inv_comp(sp, self.quat)))
self.e = [err_quat[1:], self.vel, self.accel]
self.quat = pa.quat_integrate(self.quat, self.vel, dt)
self.vel += dt * self.accel
self.accel = np.zeros(3)
self.accel = self.M[0] * np.clip(self.K[0] / self.CM[0] * self.e[0] + self.accel, -1., 1.)
self.accel = self.M[1] * np.clip(self.K[1] / self.CM[1] * self.e[1] + self.accel, -1., 1.)
# pdb.set_trace()
return self.quat, self.vel, self.accel
