def poly2_interpolate(q0, q1, qd0=None, qd1=None, **kwargs):
dofs = xrange(q0.shape[0])
if qd0 is not None:
C0, C1, C2 = q0, qd0, q1 - q0 - qd0
elif qd1 is not None:
Delta_q = center_angle_vect(q1 - q0)
C0, C1, C2 = q0, -qd1 + 2 * Delta_q, qd1 - Delta_q
else:
raise Exception("please provide either qd0 or qd1")
q_polynoms = [poly1d([C2[i], C1[i], C0[i]]) for i in dofs]
traj = PolynomialChunk(1.0, q_polynoms, **kwargs)
return traj
def bezier_interpolate(q_init, qd_init, q_dest, qd_dest, **kwargs):
Delta_q = center_angle_vect(q_dest - q_init)
q0 = q_init
q3 = q_init + Delta_q
q1 = q0 + qd_init / 3.0
q2 = q3 - qd_dest / 3.0
C0 = q0
C1 = 3 * (q1 - q0)
C2 = 3 * (q2 - 2 * q1 + q0)
C3 = -q0 + 3 * q1 - 3 * q2 + q3
dofs = xrange(len(q0))
q_polynoms = [poly1d([C3[i], C2[i], C1[i], C0[i]]) for i in dofs]
traj = PolynomialChunk(1.0, q_polynoms, **kwargs)
return traj
