def linear(q_init, q_dest, duration=None): n = len(q_init) C0 = q_init C1 = q_dest - q_init q_polynoms = [poly1d([C1[i], C0[i]]) for i in xrange(n)] traj = PolynomialChunk(1., q_polynoms) if duration is not None: return traj.timescale(duration) return traj
def linear(q_init, q_dest, duration=None): n = len(q_init) C0 = q_init C1 = q_dest - q_init q_polynoms = [poly1d([C1[i], C0[i]]) for i in xrange(n)] traj = PolynomialChunk(1., q_polynoms) if duration is not None: return traj.timescale(duration) return traj
def bezier(q_init, qd_init, q_dest, qd_dest, duration=None): n = len(q_init) Delta_q = vector.center_angle_vect(q_dest - q_init) q0 = q_init q3 = q_init + Delta_q q1 = q0 + qd_init / 3. q2 = q3 - qd_dest / 3. C0 = q0 C1 = 3 * (q1 - q0) C2 = 3 * (q2 - 2 * q1 + q0) C3 = -q0 + 3 * q1 - 3 * q2 + q3 q_polynoms = [poly1d([C3[i], C2[i], C1[i], C0[i]]) for i in xrange(n)] traj = PolynomialChunk(1., q_polynoms) if duration is not None: return traj.timescale(duration) return traj
def bezier(q_init, qd_init, q_dest, qd_dest, duration=None): n = len(q_init) Delta_q = vector.center_angle_vect(q_dest - q_init) q0 = q_init q3 = q_init + Delta_q q1 = q0 + qd_init / 3. q2 = q3 - qd_dest / 3. C0 = q0 C1 = 3 * (q1 - q0) C2 = 3 * (q2 - 2 * q1 + q0) C3 = -q0 + 3 * q1 - 3 * q2 + q3 q_polynoms = [poly1d([C3[i], C2[i], C1[i], C0[i]]) for i in xrange(n)] traj = PolynomialChunk(1., q_polynoms) if duration is not None: return traj.timescale(duration) return traj