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
