def __init__(self, robot, traj, taumin, taumax, vmax, discrtimestep=None, integrationtimestep=None): constring = str(discrtimestep) + "\n" constring += vect2str(taumin) + "\n" constring += vect2str(taumax) + "\n" constring += vect2str([0, 0]) # TODO: non-zero vmax trajstring = str(traj) super(TorqueLimits, self).__init__( robot, "TorqueLimitsRave", constring, trajstring)
def ComputeKinematicConstraints(traj, amax, discrtimestep): # Sample the dynamics constraints ndiscrsteps = int((traj.duration + 1e-10) / discrtimestep) + 1 constraintstring = "" for i in range(ndiscrsteps): t = i * discrtimestep qd = traj.Evald(t) qdd = traj.Evaldd(t) constraintstring += "\n" + vect2str(+qd) + " " + vect2str(-qd) constraintstring += "\n" + vect2str(+qdd) + " " + vect2str(-qdd) constraintstring += "\n" + vect2str(-amax) + " " + vect2str(-amax) return constraintstring