def update_pose(self, x):
"""Update pose for x \in [0, 1]."""
if x >= 1.:
return
elif x <= .5:
pose0 = self.start_pose
pose1 = self.mid_pose
y = 2. * x
else: # .5 < x < 1
pose0 = self.mid_pose
pose1 = self.end_pose
y = 2. * x - 1.
pos = (1. - y) * pose0[4:] + y * pose1[4:]
quat = quat_slerp(pose0[:4], pose1[:4], y)
self.set_pose(hstack([quat, pos]))
def update_pose(self, s):
"""
Update pose to a given index ``s`` in the swing-foot motion.
Parameters
----------
s : scalar
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]))
def pose_interp(pose0, pose1, t):
"""Linear pose interpolation."""
pos = pose0[4:] + t * (pose1[4:] - pose0[4:])
quat = quat_slerp(pose0[:4], pose1[:4], t)
return hstack([quat, pos])
def update_orientation(self, s):
"""SLERP interpolation of foot orientation (0. <= s <= 1.)."""
self.set_quat(quat_slerp(self.quat, self.end_pose[0:4], s))