def interpolate_pose_linear(pose0, pose1, t):
"""
Linear pose interpolation.
INPUT:
- ``pose0`` -- first pose
- ``pose1`` -- end pose
- ``t`` -- floating number between 0. and 1.
OUTPUT:
Pose linearly interpolated between ``pose0`` (with weight ``t``) and
``pose1`` (with weight ``1 - t``).
"""
pos = pose0[4:] + t * (pose1[4:] - pose0[4:])
quat = quat_slerp(pose0[:4], pose1[:4], t)
return hstack([quat, pos])
def interpolate_pose_linear(pose0, pose1, t):
"""
Linear pose interpolation.
Parameters
----------
pose0 : array
First pose.
pose1 : array
Second pose.
t : scalar
Any number between 0 and 1.
Returns
pose : array
Linear interpolation between the first two arguments.
"""
pos = pose0[4:] + t * (pose1[4:] - pose0[4:])
quat = quat_slerp(pose0[:4], pose1[:4], t)
return hstack([quat, pos])
def update_pose(self, s):
"""
Update pose to a given index ``s`` in the swing-foot motion.
INPUT:
- ``s`` -- index between 0 and 1 in the swing-foot motion.
"""
if s >= 1.:
return
elif s <= .5:
pose0 = self.start_pose
pose1 = self.mid_pose
y = 2. * s
else: # .5 < x < 1
pose0 = self.mid_pose
pose1 = self.end_pose
y = 2. * s - 1.
pos = (1. - y) * pose0[4:] + y * pose1[4:]
quat = quat_slerp(pose0[:4], pose1[:4], y)
self.set_pose(hstack([quat, pos]))