def reset(self, start_pose, end_pose):
mid_pose = pose_interp(start_pose, end_pose, .5)
mid_n = rotation_matrix_from_quat(mid_pose[:4])[0:3, 2]
mid_pose[4:] += 0.2 * mid_n
self.set_pose(start_pose)
self.start_pose = start_pose
self.mid_pose = mid_pose
self.end_pose = end_pose
def interpolate_foot_path(self):
p0 = self.foot.p
p1 = self.end_pose[4:]
R = rotation_matrix_from_quat(self.end_pose[:4])
t, n = R[0:3, 0], R[0:3, 2]
v1 = 0.5 * t - 0.5 * n
if norm(self.foot_vel) > 1e-4:
v0 = self.foot_vel
self.path = interpolate_uab_hermite_topp(p0, v0, p1, v1)
self.sd_beg = norm(v0) / norm(self.path.Evald(0.))
else: # choose initial direction
v0 = 0.3 * self.foot.t + 0.7 * self.foot.n
self.path = interpolate_uab_hermite_topp(p0, v0, p1, v1)
self.sd_beg = 0.
def reset(self, start_pose, end_pose):
"""
Reset both end poses of the interpolation.
Parameters
----------
start_pose : array
New start pose.
end_pose : array
New end pose.
"""
mid_pose = interpolate_pose_linear(start_pose, end_pose, .5)
mid_n = rotation_matrix_from_quat(mid_pose[:4])[0:3, 2]
mid_pose[4:] += self.swing_height * mid_n
self.set_pose(start_pose)
self.start_pose = start_pose
self.mid_pose = mid_pose
self.end_pose = end_pose
def interpolate_path(self):
target_pose = self.body.end_pose
target_p = target_pose[4:]
R = rotation_matrix_from_quat(target_pose[:4])
t, n = R[0:3, 0], R[0:3, 2]
target_pd = 0.5 * t - 0.5 * n
p0 = self.body.p
p1 = target_p
if norm(p1 - p0) < 1e-3:
return None, None
v1 = target_pd
if norm(self.body.pd) > 1e-4:
v0 = self.body.pd
path = interpolate_houba_topp(p0, p1, v0, v1)
sd_beg = norm(v0) / norm(path.Evald(0.))
else: # choose initial direction
v0 = 0.3 * self.body.t + 0.7 * self.body.n
path = interpolate_houba_topp(p0, p1, v0, v1)
sd_beg = 0.
return path, sd_beg