def traj_str_5th_degree(q_beg, q_end, qd_beg, qd_end, qdd_beg, qdd_end, duration): """ Return an interpolated 5th degree polynomial trajectory string. It is up to 10 decimal places accuracy to guarantee continuity at trajectory end. @type q_beg: list @param q_beg: Initial configuration. @type q_end: list @param q_end: Final configuration. @type qd_beg: list @param qd_beg: Time derivative of initial configuration. @type qd_end: list @param qd_end: Time derivative of final configuration. @type qdd_beg: list @param qdd_beg: 2nd order time derivative of initial configuration. @type qdd_end: list @param qdd_end: 2nd order time derivative of final configuration. @type duration: float @param duration: Time length of the interpolated trajectory. @rtype: str @return: The interpolated polynomial trajectory string. """ traj_str = '' ndof = len(q_beg) traj_str += "%f\n%d" % (duration, ndof) for k in range(ndof): a, b, c, d, e, f = Utilities.Interpolate5thDegree( q_beg[k], q_end[k], qd_beg[k], qd_end[k], qdd_beg[k], qdd_end[k], duration) traj_str += "\n%.10f %.10f %.10f %.10f %.10f %.10f" % (f, e, d, c, b, a) return traj_str
def TrajString5thDegree(q_beg, q_end, qs_beg, qs_end, qss_beg, qss_end, duration): trajectorystring = '' ndof = len(q_beg) trajectorystring += "%f\n%d" % (duration, ndof) for k in range(ndof): a, b, c, d, e, f = Utilities.Interpolate5thDegree( q_beg[k], q_end[k], qs_beg[k], qs_end[k], qss_beg[k], qss_end[k], duration) trajectorystring += "\n%f %f %f %f %f %f" % (f, e, d, c, b, a) return trajectorystring