Exemple #1
0
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
Exemple #2
0
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