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)