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