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
