def FromRaveTraj(robot, traj):
N = traj.GetNumWaypoints()
if N < 2:
print "RAVE trajectory has less than 2 waypoints"
return None
timespec = openravepy.ConfigurationSpecification()
timespec.AddGroup('deltatime', 1, 'linear')
posspec = robot.GetActiveConfigurationSpecification()
velspec = posspec.ConvertToVelocitySpecification()
chunkslist = []
vel = traj.GetWaypoint(0, velspec)
ndof = len(vel)
for i in range(N - 1):
pos = traj.GetWaypoint(i, posspec)
deltatime = traj.GetWaypoint(i + 1, timespec)[0]
if deltatime < 1e-5:
continue
nextvel = traj.GetWaypoint(i + 1, velspec)
polynomialslist = []
for j in range(ndof):
x = pos[j]
v = vel[j]
a = (nextvel[j] - vel[j]) / deltatime
polynomialslist.append(Trajectory.Polynomial([x, v, a / 2]))
chunkslist.append(Trajectory.Chunk(deltatime, polynomialslist))
vel = nextvel
return Trajectory.PiecewisePolynomialTrajectory(chunkslist)
def MakeTOPPChunk(vchunk, q0):
polylist = []
for i in range(vchunk.ndof):
polylist.append(
Trajectory.Polynomial([
q0[i], vchunk.v0[i],
0.5 * (vchunk.v1[i] - vchunk.v0[i]) / vchunk.T
]))
return Trajectory.Chunk(vchunk.T, polylist)
def InterpolateViapoints(path):
nviapoints = len(path[0, :])
tv = linspace(0, 1, nviapoints)
for i in range(nviapoints - 1):
tv[i + 1] = tv[i] + linalg.norm(path[:, i] - path[:, i + 1])
tcklist = []
for idof in range(0, path.shape[0]):
tcklist.append(interpolate.splrep(tv, path[idof, :], s=0))
t = tcklist[0][0]
chunkslist = []
for i in range(len(t) - 1):
polylist = []
if abs(t[i + 1] - t[i]) > 1e-5:
for tck in tcklist:
a = 1 / 6. * interpolate.splev(t[i], tck, der=3)
b = 0.5 * interpolate.splev(t[i], tck, der=2)
c = interpolate.splev(t[i], tck, der=1)
d = interpolate.splev(t[i], tck, der=0)
polylist.append(Trajectory.Polynomial([d, c, b, a]))
chunkslist.append(Trajectory.Chunk(t[i + 1] - t[i], polylist))
return Trajectory.PiecewisePolynomialTrajectory(chunkslist)
