def traj_str_3rd_degree(q_beg, q_end, qd_beg, qd_end, duration):
"""
Return an interpolated 3rd 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 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 = Utilities.Interpolate3rdDegree(q_beg[k], q_end[k],
qd_beg[k], qd_end[k],
duration)
traj_str += "\n%.10f %.10f %.10f %.10f" % (d, c, b, a)
return traj_str
def InterpolateFree(qbeg, qend, qsbeg, qsend, duration):
pathstring = ''
ndof = len(qbeg)
pathstring += "%f\n%d" % (duration, ndof)
for k in range(ndof):
a, b, c, d = Utilities.Interpolate3rdDegree(qbeg[k], qend[k], qsbeg[k],
qsend[k], duration)
pathstring += "\n%f %f %f %f" % (d, c, b, a)
return Trajectory.PiecewisePolynomialTrajectory.FromString(pathstring)
def TrajString3rdDegree(q_beg, q_end, qs_beg, qs_end, duration):
trajectorystring = ''
ndof = len(q_beg)
trajectorystring += "%f\n%d" % (duration, ndof)
for k in range(ndof):
a, b, c, d = Utilities.Interpolate3rdDegree(q_beg[k], q_end[k],
qs_beg[k], qs_end[k],
duration)
trajectorystring += "\n%f %f %f %f" % (d, c, b, a)
return trajectorystring
dof_lim=robot.GetDOFLimits()
vel_lim=robot.GetDOFVelocityLimits()
robot.SetDOFLimits(-10*ones(ndof),10*ones(ndof)) # Overrides robot joint limits for TOPP computations
robot.SetDOFVelocityLimits(100*vel_lim) # Override robot velocity limits for TOPP computations
# Trajectory
q0 = zeros(ndof)
q1 = zeros(ndof)
qd0 = ones(ndof)
qd1 = -ones(ndof)
q0[0:7] = [-2,0.5,1,3,-3,-2,-2]
q1[0:7] = [2,-0.5,-1,-1,1,1,1]
T = 1.5
trajectorystring = "%f\n%d"%(T,ndof)
for i in range(ndof):
a,b,c,d = Utilities.Interpolate3rdDegree(q0[i],q1[i],qd0[i],qd1[i],T)
trajectorystring += "\n%f %f %f %f"%(d,c,b,a)
traj0 = Trajectory.PiecewisePolynomialTrajectory.FromString(trajectorystring)
# Constraints
vmax = zeros(ndof)
taumin = zeros(ndof)
taumax = zeros(ndof)
vmax[0:7] = vel_lim[0:7] # Velocity limits
taumin[0:7] = -robot.GetDOFMaxTorque()[0:7] # Torque limits
taumax[0:7] = robot.GetDOFMaxTorque()[0:7] # Torque limits
# Set up the TOPP problem
discrtimestep = 0.005
uselegacy = False
t0 = time.time()