def update_target_pose(self, dt):
assert self.retimed_traj is not None
from_start = self.foot_link.p - self.start_pose[4:]
to_goal = self.end_pose[4:] - self.foot_link.p
dist_to_goal = norm(to_goal)
dist_from_start = norm(from_start)
x = dist_to_goal / (dist_from_start + dist_to_goal)
quat = quat_slerp(self.foot.quat, self.end_pose[0:4], 1. - x)
self.foot_vel = self.retimed_traj.Evald(dt)
self.foot.set_pos(self.retimed_traj.Eval(dt))
self.foot.set_quat(quat)
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]))