def interpolate_pose_quadratic(pose0, pose1, x): """ Pose interpolation that is linear in orientation (SLERP) and quadratic (Bezier) in position. Parameters ---------- pose0 : (7,) array First pose. pose1 : (7,) array Second pose. x : scalar Number between 0 and 1. Returns ------- pose : (7,) array Linear interpolation between the first two arguments. Note ---- Initial and final linear velocities on the interpolated trajectory are zero. """ pos = x**2 * (3 - 2 * x) * (pose1[4:] - pose0[4:]) + pose0[4:] quat = quat_slerp(pose0[:4], pose1[:4], x) return hstack([quat, pos])
def integrate(self, dt): """ Integrate swing foot motion forward by a given amount of time. Parameters ---------- dt : scalar Duration of forward integration, in [s]. """ self.playback_time += dt s = min(1., self.playback_time / self.duration) quat = quat_slerp(self.start_contact.quat, self.end_contact.quat, s) pos = self.path(s) pose = hstack([quat, pos]) return pose
def interpolate_pose_linear(pose0, pose1, x): """ Standalone function for linear pose interpolation. Parameters ---------- pose0 : (7,) array First pose. pose1 : (7,) array Second pose. x : scalar Number between 0 and 1. Returns ------- pose : (7,) array Linear interpolation between the first two arguments. """ pos = pose0[4:] + x * (pose1[4:] - pose0[4:]) quat = quat_slerp(pose0[:4], pose1[:4], x) return hstack([quat, pos])
def eval_quat(self, s): return quat_slerp(self.start_quat, self.end_quat, s)