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)
