def InterpolateViapointsLinear(path):
nviapoints = len(path[0, :])
### assumption: distance between waypoints is always the same and can be scaled to 1.0/nviapoints
tv = np.zeros(nviapoints)
for i in range(nviapoints - 1):
tv[i + 1] = tv[i] + np.linalg.norm(path[:, i] - path[:, i + 1])
tv = linspace(0, 1, nviapoints)
tcklist = []
for idof in range(0, path.shape[0]):
tcklist.append(interpolate.splrep(tv, path[idof, :], s=0, k=1))
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 = 0
b = 0
c = interpolate.splev(t[i], tck, der=1)
d = interpolate.splev(t[i], tck, der=0)
polylist.append(TOPPTrajectory.Polynomial([d, c, b, a]))
chunkslist.append(TOPPTrajectory.Chunk(t[i + 1] - t[i], polylist))
return TOPPTrajectory.PiecewisePolynomialTrajectory(chunkslist)
def CompensateTrajectory(robot, T1, T2, freedofs1, freedofs2, dependentdofs1,
dependentdofs2, constrainedlink, freetraj, dependent0,
nchunks, chunksubdiv, tol_jacobian):
# Compensate trajectory
nsubdiv = nchunks * chunksubdiv
dt = freetraj.duration / nsubdiv
dependentvalues = array(dependent0)
dependentv = []
dependentv.append(array(dependentvalues))
tv = [0]
dofvalues1 = zeros(robot.GetDOF())
dofvalues2 = zeros(robot.GetDOF())
for ichunk in range(nsubdiv):
t = ichunk * dt
freevalues = freetraj.Eval(t)
freed = freetraj.Evald(t)
if len(freedofs1) > 0:
dofvalues1[freedofs1] = freevalues[list(range(len(freedofs1)))]
if len(dependentdofs1) > 0:
dofvalues1[dependentdofs1] = dependentvalues[list(
range(len(dependentdofs1)))]
if len(freedofs2) > 0:
dofvalues2[freedofs2] = freevalues[
len(freedofs1) + array(list(range(len(freedofs2))))]
if len(dependentdofs2) > 0:
dofvalues2[dependentdofs2] = dependentvalues[
len(dependentdofs1) + array(list(range(len(dependentdofs2))))]
try:
dependentd = Compensate(robot, T1, T2, dofvalues1, dofvalues2,
freedofs1, freedofs2, dependentdofs1,
dependentdofs2, constrainedlink, freed,
tol_jacobian)
except Exception as inst:
print("Could not interpolate : Compensate failed")
return None
dependentvalues += dependentd * dt
dependentv.append(array(dependentvalues))
tv.append(t + dt)
dependentv = array(dependentv)
# Interpolate dependent values with splines
tcklist = []
for idof in range(0, dependentv.shape[1]):
tcklist.append(
scipy.interpolate.splrep(tv[::chunksubdiv],
dependentv[::chunksubdiv, idof],
s=0))
t = tcklist[0][0]
chunkslist = []
for i in range(len(t) - 1):
dependentpolylist = []
if abs(t[i + 1] - t[i]) > 1e-5:
for tck in tcklist:
a = 1 / 6. * scipy.interpolate.splev(t[i], tck, der=3)
b = 0.5 * scipy.interpolate.splev(t[i], tck, der=2)
c = scipy.interpolate.splev(t[i], tck, der=1)
d = scipy.interpolate.splev(t[i], tck, der=0)
dependentpolylist.append(Trajectory.Polynomial([d, c, b, a]))
chunkslist.append(
Trajectory.Chunk(t[i + 1] - t[i], dependentpolylist))
return Trajectory.PiecewisePolynomialTrajectory(chunkslist)
def reverse_traj(trajectroy):
newchunkslist = []
for chunk in trajectroy.chunkslist:
T = chunk.duration
newpoly_list = []
for p in chunk.polynomialsvector:
# Perform variable changing of p(x) = a_n(x)^n + a_(n-1)(x)^(n-1) + ...
# by x = T - y
a = p.q # coefficient vector with python convention (highest degree first)
# a is a poly1d object
r = a.r
newr = [T - k for k in r]
b = np.poly1d(newr,
True) # reconstruct a new polynomial from roots
b = b * a.coeffs[0] # multiply back by a_n
# *** this multiplication does not commute
if (b(0) * a(T) < 0):
# correct the coeffs if somehow the polynomial is flipped
b = b * -1.0
# TOPP convention is weak-term-first
newpoly = Trajectory.Polynomial(b.coeffs.tolist()[::-1])
newpoly_list.append(newpoly)
newchunk = Trajectory.Chunk(chunk.duration, newpoly_list)
newchunkslist.insert(0, newchunk)
return Trajectory.PiecewisePolynomialTrajectory(newchunkslist)
def InsertTrajectory(q0, freetraj, freedofs):
polyarray = array([Trajectory.Polynomial([q]) for q in q0])
chunkslist = []
for chunk in freetraj.chunkslist:
polyarray[freedofs] = chunk.polynomialsvector
chunkslist.append(Trajectory.Chunk(chunk.duration, list(polyarray)))
return Trajectory.PiecewisePolynomialTrajectory(chunkslist)
def SplitTraj2(Rlist, traj):
trajlist = []
chunkindex = 0
clist = []
for i in range(len(Rlist) - 1):
while chunkindex < len(traj.chunkslist):
chunkcur = traj.chunkslist[chunkindex]
chunknext = traj.chunkslist[chunkindex + 1]
clist.append(chunkcur)
chunkindex += 1
if (norm(
dot(Rlist[i], expmat(chunkcur.Eval(chunkcur.duration))) -
dot(Rlist[i + 1], expmat(chunknext.Eval(0))))) < 1e-1:
trajlist.append(
Trajectory.PiecewisePolynomialTrajectory(clist))
clist = []
break
# Last traj
clist = []
while chunkindex < len(traj.chunkslist):
clist.append(traj.chunkslist[chunkindex])
chunkindex += 1
trajlist.append(Trajectory.PiecewisePolynomialTrajectory(clist))
return LieTraj(Rlist, trajlist)
def InterpolateSO3(R0, R1, omega0, omega1, T):
r1 = logvect(dot(R0.T, R1))
u = linalg.solve(Amat(r1), omega1 * T)
c = omega0 * T
M = array([[1, 0, 0, 1, 0, 0], [0, 1, 0, 0, 1, 0], [0, 0, 1, 0, 0, 1],
[3, 0, 0, 2, 0, 0], [0, 3, 0, 0, 2, 0], [0, 0, 3, 0, 0, 2]])
y = array([
r1[0] - c[0], r1[1] - c[1], r1[2] - c[2], u[0] - c[0], u[1] - c[1],
u[2] - c[2]
])
x = linalg.solve(M, y)
a = x[:3]
b = x[3:]
T2 = T * T
T3 = T2 * T
polylist = []
for i in range(3):
polylist.append(
Trajectory.Polynomial([0, c[i] / T, b[i] / T2, a[i] / T3]))
chunk = Trajectory.Chunk(T, polylist)
return Trajectory.PiecewisePolynomialTrajectory([chunk])
def ReplaceTransTrajectorySegment(originaltranstraj, transtrajsegment, t0, t1):
"""ReplaceTransTrajectorySegment replaces the segment (t0, t1) in the (arbitrary degree) originaltranstraj
with an (arbitrary degree) transtrajsegment.
"""
assert (t1 > t0)
newchunkslist = []
i0, rem0 = originaltranstraj.FindChunkIndex(t0)
i1, rem1 = originaltranstraj.FindChunkIndex(t1)
## check if t0 falls in the first chunk.
## if not, insert chunk 0 to chunk i0 - 1 into newchunkslist
if i0 > 0:
for c in originaltranstraj.chunkslist[0:i0]:
newchunkslist.append(c)
## remainderchunk0
remchunk0 = Trajectory.Chunk(
rem0, originaltranstraj.chunkslist[i0].polynomialsvector)
newchunkslist.append(remchunk0)
## insert transtrajsegment
for c in transtrajsegment.chunkslist:
newchunkslist.append(c)
## remainderchunk1
newpoly_list = []
for p in originaltranstraj.chunkslist[i1].polynomialsvector:
## perform variable changing of p(x) = a_n(x)^n + a_(n-1)(x)^(n-1) + ...
## by x = y + rem1
a = p.q ## coefficient vector with python convention (highest degree first)
## a is a poly1d object
r = a.r ## polynomial roots
for i in range(len(r)):
r[i] = r[i] - rem1
b = np.poly1d(r, True) ## reconstruct a new polynomial from roots
## b is a poly1d object
b = b * a.coeffs[
0] ## multiply back by a_n *** this multiplication does not commute
newpoly = Trajectory.Polynomial(
b.coeffs.tolist()[::-1]) ## TOPP convention is weak-term-first
newpoly_list.append(newpoly)
remchunk1 = Trajectory.Chunk(
originaltranstraj.chunkslist[i1].duration - rem1, newpoly_list)
newchunkslist.append(remchunk1)
## insert remaining chunks
if i1 < len(originaltranstraj.chunkslist) - 1:
for c in originaltranstraj.chunkslist[i1 + 1:len(originaltranstraj.
chunkslist)]:
newchunkslist.append(c)
return Trajectory.PiecewisePolynomialTrajectory(newchunkslist)
def TransRotTrajFromSE3Traj(SE3traj):
transclist = []
rclist = []
for c in SE3traj.chunkslist:
transchunk = Trajectory.Chunk(c.duration, c.polynomialsvector[:3])
transclist.append(transchunk)
rchunk = Trajectory.Chunk(c.duration, c.polynomialsvector[3:])
rclist.append(rchunk)
transtraj = Trajectory.PiecewisePolynomialTrajectory(transclist)
rtraj = Trajectory.PiecewisePolynomialTrajectory(rclist)
return transtraj, rtraj
def InterpolateSO3ZeroOmega(R0, R1, T):
r = logvect(dot(R0.T, R1))
a = ones(3) * (-2)
b = ones(3) * 3
T2 = T * T
T3 = T2 * T
polylist = []
for i in range(3):
polylist.append(
Trajectory.Polynomial([0, 0, r[i] * b[i] / T2, r[i] * a[i] / T3]))
chunk = Trajectory.Chunk(T, polylist)
return Trajectory.PiecewisePolynomialTrajectory([chunk])
def SplitTrajectories(trajtotal, freedofs, dependentdofs):
nchunks = len(trajtotal.chunkslist)
freechunkslist = []
dependentchunkslist = []
for chunk in trajtotal.chunkslist:
freepolylist = array(chunk.polynomialsvector)[freedofs]
dependentpolylist = array(chunk.polynomialsvector)[dependentdofs]
freechunkslist.append(Trajectory.Chunk(chunk.duration, freepolylist))
dependentchunkslist.append(
Trajectory.Chunk(chunk.duration, dependentpolylist))
return Trajectory.PiecewisePolynomialTrajectory(
freechunkslist), Trajectory.PiecewisePolynomialTrajectory(
dependentchunkslist)
def SE3TrajFromTransandSO3(transtraj, rtraj): # same chunk.duration
#return duration-dimension-trans polynomial- rot polynomial
if len(transtraj.chunkslist) != len(rtraj.chunkslist):
print 'error'
return 0
clist = []
for c in transtraj.chunkslist:
plist = []
for i in range(3):
plist.append(c.polynomialsvector[i])
for i in range(3):
rc = rtraj.chunkslist[len(clist)]
plist.append(rc.polynomialsvector[i])
chunk = Trajectory.Chunk(c.duration, plist)
clist.append(chunk)
return Trajectory.PiecewisePolynomialTrajectory(clist)
def InterpolateViapointsLinearFixedDerivative(path):
nviapoints = len(path[0, :])
tv = linspace(0, 1, nviapoints)
tcklist = []
for idof in range(0, path.shape[0]):
tcklist.append(interpolate.splrep(tv, path[idof, :], s=0, k=1))
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 = 0
b = 0
c = interpolate.splev(t[i], tck, der=1)
d = interpolate.splev(t[i], tck, der=0)
polylist.append(TOPPTrajectory.Polynomial([d, c, b, a]))
chunkslist.append(TOPPTrajectory.Chunk(t[i + 1] - t[i], polylist))
return TOPPTrajectory.PiecewisePolynomialTrajectory(chunkslist)
def InterpolateViapointsCustom(path):
nviapoints = len(path[0, :])
tv = np.zeros(nviapoints)
for i in range(nviapoints - 1):
tv[i + 1] = tv[i] + np.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, k=3))
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(TOPPTrajectory.Polynomial([d, c, b, a]))
chunkslist.append(TOPPTrajectory.Chunk(t[i + 1] - t[i], polylist))
return TOPPTrajectory.PiecewisePolynomialTrajectory(chunkslist)
def ShortcutTrajectory(traj, ccConfigFunction, ccSegmentFunction, numIter=100):
"""Shortcut a CD trajectory.
ccConfigFunction: collision checking function for a configuration
This function has template ccConfigFunction(config, activeIndices).
ccSegmentFunction: collision checking function for a segment
This function has template ccSegmentFunction(config1, config2, activeIndices)
"""
nsuccessful = 0
originalDuration = traj.duration
newTraj = traj
for i in xrange(numIter):
T = newTraj.duration
print "Shortcut iteration {0}:".format(i + 1),
t0 = T * np.random.random_sample()
t1 = T * np.random.random_sample()
if t0 > t1:
_t = t1
t1 = t0
t0 = _t
c0 = Config(newTraj.Eval(t0))
c1 = Config(newTraj.Eval(t1))
if Distance(c0, c1) >= (t1 - t0):
# Not shorter
print "not shorter"
continue
if ccSegmentFunction(c0, c1, c0.activeIndices):
# Segment in collision
continue
print "successful"
nsuccessful += 1
newSegment = CDPath.FromConfigsList([c0, c1])
newTraj = Trajectory.InsertIntoTrajectory(newTraj,
newSegment,
t0,
t1,
order=1)
print "Successful shortcuts = {0} times".format(nsuccessful)
print "Original duration = {0}".format(originalDuration)
print " New duration = {0}".format(newTraj.duration)
print " diff = {0}".format(originalDuration - newTraj.duration)
return newTraj
def MergeTrajectories(q0, freetraj, dependenttraj, freedofs, dependentdofs):
nchunks = len(dependenttraj.chunkslist)
ti = 0
chunkslist = []
polyarray = array([Trajectory.Polynomial([q]) for q in q0])
for i in range(nchunks):
freepolylist = []
dependentchunk = dependenttraj.chunkslist[i]
freevalues = freetraj.Eval(ti)
freed = freetraj.Evald(ti)
freedd = freetraj.Evaldd(ti)
freeddd = freetraj.Evaldn(ti, 3)
for ifree in range(freetraj.dimension):
a = 1 / 6. * freeddd[ifree]
b = 0.5 * freedd[ifree]
c = freed[ifree]
d = freevalues[ifree]
freepolylist.append(Trajectory.Polynomial([d, c, b, a]))
polyarray[freedofs] = freepolylist
polyarray[dependentdofs] = dependentchunk.polynomialsvector
chunkslist.append(
Trajectory.Chunk(dependentchunk.duration, list(polyarray)))
ti += dependentchunk.duration
return Trajectory.PiecewisePolynomialTrajectory(chunkslist)
def ReplaceTrajectorySegment(originallietraj, trajsegment, t0, t1):
"""ReplaceTrajectorySegment replaces the segment (t0, t1), it returns a LieTraj variable (Rotationlist and Trajectorylist) """
assert (t1 > t0)
newtrajlist = []
newRlist = []
i0, rem0 = originallietraj.FindTrajIndex(t0)
i1, rem1 = originallietraj.FindTrajIndex(t1)
# print "t" , originallietraj.FindTrajIndex(t0) ##
# print "t" , originallietraj.FindTrajIndex(t1) ##
## check if t0 falls in the first traj.
## if not, insert traj 0 to traj i0 - 1 into newtrajlist
if i0 > 0:
for i in range(0, i0):
newtrajlist.append(originallietraj.trajlist[i])
newRlist.append(originallietraj.Rlist[i])
## remaindertraj0
newchunkslist = []
ic0, remc0 = originallietraj.trajlist[i0].FindChunkIndex(rem0)
# print "c0", originallietraj.trajlist[i0].FindChunkIndex(rem0) ##
# check if rem0 falls in the first chunk, if not, ...
if ic0 > 0:
for c in originallietraj.trajlist[i0].chunkslist[0:ic0]:
newchunkslist.append(c)
# remainderchunk0
remchunk0 = Trajectory.Chunk(
remc0, originallietraj.trajlist[i0].chunkslist[ic0].polynomialsvector)
newchunkslist.append(remchunk0)
remtraj0 = Trajectory.PiecewisePolynomialTrajectory(newchunkslist)
newtrajlist.append(remtraj0)
newRlist.append(originallietraj.Rlist[i0])
## insert trajsegment
newtrajlist.append(trajsegment)
newRlist.append(originallietraj.EvalRotation(t0))
######################################
## For the traj right after the trajsegment
## remaindertraj1
newchunkslist = []
ic1, remc1 = originallietraj.trajlist[i1].FindChunkIndex(rem1)
newpoly_list = []
for p in originallietraj.trajlist[i1].chunkslist[ic1].polynomialsvector:
## perform variable changing of p(x) = a_n(x)^n + a_(n-1)(x)^(n-1) + ...
## by x = y + remc1
a = p.q ## coefficient vector with python convention (highest degree first)
## a is a poly1d object
r = a.r ## polynomial roots
for i in range(len(r)):
r[i] = r[i] - remc1
b = np.poly1d(r, True) ## reconstruct a new polynomial from roots
## b is a poly1d object
b = b * a.coeffs[
0] ## multiply back by a_n *** this multiplication does not commute
newpoly = Trajectory.Polynomial(
b.coeffs.tolist()[::-1]) ## TOPP convention is weak-term-first
newpoly_list.append(newpoly)
remchunk1 = Trajectory.Chunk(
originallietraj.trajlist[i1].chunkslist[ic1].duration - remc1,
newpoly_list)
newchunkslist.append(remchunk1)
## insert remaining chunk
if ic1 < len(originallietraj.trajlist[i1].chunkslist) - 1:
for c in originallietraj.trajlist[i1].chunkslist[
ic1 + 1:len(originallietraj.trajlist[i1].chunkslist)]:
newchunkslist.append(c)
## insert
remtraj1 = Trajectory.PiecewisePolynomialTrajectory(newchunkslist)
newtrajlist.append(remtraj1)
newRlist.append(originallietraj.Rlist[i1]
) ##ROTATION Should be at originallietraj.Rlist[i1] ##
###############################
# insert the remainder trajectoris
if i1 < len(originallietraj.trajlist) - 1:
Rindex = i1 + 1
for t in originallietraj.trajlist[i1 +
1:len(originallietraj.trajlist)]:
newtrajlist.append(t)
newRlist.append(originallietraj.Rlist[Rindex])
Rindex += 1
return lie.LieTraj(newRlist, newtrajlist)
def CompensateTrajectory(robot,
qstart,
freedofs,
dependentdofs,
constrainedlinks,
freetraj,
dependent0,
nchunks,
chunksubdiv,
toljacobian=0.01,
slidedofs=None,
slidelinks=None,
slidedelta=None):
# Compensate trajectory
nsubdiv = nchunks * chunksubdiv
dt = freetraj.duration / nsubdiv
dependentvalues = array(dependent0)
dependentv = []
dependentv.append(array(dependentvalues))
tv = [0]
dofvalues = array(qstart)
for ichunk in range(nsubdiv):
t = ichunk * dt
freevalues = freetraj.Eval(t)
dofvalues[freedofs] = freevalues
if slidedofs is None:
dofvalues[dependentdofs] = dependentvalues
else:
dofvalues[dependentdofs + slidedofs] = dependentvalues
freedelta = freetraj.Evald(t)
if slidelinks is None:
dependentdelta = Compensate(robot, freedofs, dependentdofs,
constrainedlinks, dofvalues, freedelta,
toljacobian)
else:
dependentdelta = Compensate(robot, freedofs, dependentdofs,
constrainedlinks, dofvalues, freedelta,
toljacobian, slidedofs, slidelinks,
slidedelta)
if dependentdelta is None:
print "t=", t
return None
dependentvalues += dependentdelta * dt
dependentv.append(array(dependentvalues))
tv.append(t + dt)
dependentv = array(dependentv)
tv = array(tv)
# Interpolate dependent values with splines
tcklist = []
for idof in range(0, dependentv.shape[1]):
tcklist.append(
scipy.interpolate.splrep(tv[::chunksubdiv],
dependentv[::chunksubdiv, idof],
s=0))
t = tcklist[0][0]
chunkslist = []
for i in range(len(t) - 1):
dependentpolylist = []
if abs(t[i + 1] - t[i]) > 1e-5:
for tck in tcklist:
a = 1 / 6. * scipy.interpolate.splev(t[i], tck, der=3)
b = 0.5 * scipy.interpolate.splev(t[i], tck, der=2)
c = scipy.interpolate.splev(t[i], tck, der=1)
d = scipy.interpolate.splev(t[i], tck, der=0)
dependentpolylist.append(Trajectory.Polynomial([d, c, b, a]))
chunkslist.append(
Trajectory.Chunk(t[i + 1] - t[i], dependentpolylist))
return Trajectory.PiecewisePolynomialTrajectory(chunkslist)
def replace_lie_traj_segment(original_lie_traj, lie_traj_segment, t0, t1):
"""
Replace the lie trajectory segment in time interval (C{t0}, C{t1})
in {original_lie_traj} with the given C{lie_traj_segment}.
@type original_traj: lie.LieTraj
@param original_traj: Original lie trajectory.
@type traj_segment: lie.LieTraj
@param traj_segment: New lie trajectory segment.
@type t0: float
@param t0: Start time of the time interval.
@type t1: float
@param t1: End time of the time interval.
@rtype: str
@return: New lie trajectory after replacement.
"""
assert (t1 > t0)
new_traj_list = []
new_R_list = []
i0, rem0 = original_lie_traj.FindTrajIndex(t0)
i1, rem1 = original_lie_traj.FindTrajIndex(t1)
## check if t0 falls in the first traj.
## if not, insert traj 0 to traj i0 - 1 into new_traj_list
if i0 > 0:
for i in range(0, i0):
new_traj_list.append(original_lie_traj.trajlist[i])
new_R_list.append(original_lie_traj.Rlist[i])
## remaindertraj0
new_chunk_list = []
ic0, remc0 = original_lie_traj.trajlist[i0].FindChunkIndex(rem0)
# print "c0", original_lie_traj.trajlist[i0].FindChunkIndex(rem0) ##
# check if rem0 falls in the first chunk, if not, ...
if ic0 > 0:
for c in original_lie_traj.trajlist[i0].chunkslist[0:ic0]:
new_chunk_list.append(c)
# remainderchunk0
rem_chunk0 = Trajectory.Chunk(remc0, original_lie_traj.trajlist[i0].\
chunkslist[ic0].polynomialsvector)
new_chunk_list.append(rem_chunk0)
rem_traj0 = Trajectory.PiecewisePolynomialTrajectory(new_chunk_list)
new_traj_list.append(rem_traj0)
new_R_list.append(original_lie_traj.Rlist[i0])
## insert lie_traj_segment
new_traj_list.append(lie_traj_segment)
new_R_list.append(original_lie_traj.EvalRotation(t0))
## For the traj right after the lie_traj_segment
## remaindertraj1
new_chunk_list = []
ic1, remc1 = original_lie_traj.trajlist[i1].FindChunkIndex(rem1)
new_poly_list = []
for p in original_lie_traj.trajlist[i1].chunkslist[ic1].polynomialsvector:
## perform variable changing of p(x) = a_n(x)^n + a_(n-1)(x)^(n-1) + ...
## by x = y + remc1
a = p.q ## coefficient vector with python convention (highest degree first)
## a is a poly1d object
r = a.r ## polynomial roots
for i in range(len(r)):
r[i] = r[i] - remc1
b = np.poly1d(r, True) ## reconstruct a new polynomial from roots
## b is a poly1d object
b = b * a.coeffs[
0] ## multiply back by a_n *** this multiplication does not commute
new_poly = Trajectory.Polynomial(
b.coeffs.tolist()[::-1]) ## TOPP convention is weak-term-first
new_poly_list.append(new_poly)
rem_chunk1 = Trajectory.Chunk(
original_lie_traj.trajlist[i1].chunkslist[ic1].duration - remc1,
new_poly_list)
new_chunk_list.append(rem_chunk1)
## insert remaining chunk
if ic1 < len(original_lie_traj.trajlist[i1].chunkslist) - 1:
for c in original_lie_traj.trajlist[i1].chunkslist[ic1 + 1: \
len(original_lie_traj.trajlist[i1].chunkslist)]:
new_chunk_list.append(c)
## insert
rem_traj1 = Trajectory.PiecewisePolynomialTrajectory(new_chunk_list)
new_traj_list.append(rem_traj1)
new_R_list.append(original_lie_traj.Rlist[i1]
) ##ROTATION Should be at original_lie_traj.Rlist[i1] ##
# insert the remainder trajectoris
if i1 < len(original_lie_traj.trajlist) - 1:
R_index = i1 + 1
for t in original_lie_traj.trajlist[i1+1: \
len(original_lie_traj.trajlist)]:
new_traj_list.append(t)
new_R_list.append(original_lie_traj.Rlist[R_index])
R_index += 1
return lie.LieTraj(new_R_list, new_traj_list)
def replace_traj_segment(original_traj, traj_segment, t0, t1):
"""
Replace the segment in time interval (C{t0}, C{t1}) in C{original_traj}
with the given C{traj_segment}.
NB: The trajectory is of type TOPP.Trajectory.PiecewisePolynomialTrajectory.
@type original_traj: TOPP.Trajectory.PiecewisePolynomialTrajectory
@param original_traj: Original trajectory.
@type traj_segment: TOPP.Trajectory.PiecewisePolynomialTrajectory
@param traj_segment: New trajectory segment.
@type t0: float
@param t0: Start time of the time interval.
@type t1: float
@param t1: End time of the time interval.
@rtype: str
@return: New trajectory after replacement.
"""
assert (t1 > t0)
new_chunk_list = []
i0, rem0 = original_traj.FindChunkIndex(t0)
i1, rem1 = original_traj.FindChunkIndex(t1)
## check if t0 falls in the first chunk.
## if not, insert chunk 0 to chunk i0 - 1 into new_chunk_list
if i0 > 0:
for c in original_traj.chunkslist[0:i0]:
new_chunk_list.append(c)
## remainderchunk0
rem_chunk0 = Trajectory.Chunk(
rem0, original_traj.chunkslist[i0].polynomialsvector)
new_chunk_list.append(rem_chunk0)
## insert traj_segment
for c in traj_segment.chunkslist:
new_chunk_list.append(c)
## remainderchunk1
new_poly_list = []
for p in original_traj.chunkslist[i1].polynomialsvector:
## perform variable changing of p(x) = a_n(x)^n + a_(n-1)(x)^(n-1) + ...
## by x = y + rem1
a = p.q ## coefficient vector with python convention (highest degree first)
## a is a poly1d object
r = a.r ## polynomial roots
for i in range(len(r)):
r[i] = r[i] - rem1
b = np.poly1d(r, True) ## reconstruct a new polynomial from roots
## b is a poly1d object
b = b * a.coeffs[
0] ## multiply back by a_n *** this multiplication does not commute
new_poly = Trajectory.Polynomial(
b.coeffs.tolist()[::-1]) ## TOPP convention is weak-term-first
new_poly_list.append(new_poly)
rem_chunk1 = Trajectory.Chunk(original_traj.chunkslist[i1].duration - rem1,
new_poly_list)
new_chunk_list.append(rem_chunk1)
## insert remaining chunks
if i1 < len(original_traj.chunkslist) - 1:
for c in original_traj.chunkslist[i1 +
1:len(original_traj.chunkslist)]:
new_chunk_list.append(c)
return Trajectory.PiecewisePolynomialTrajectory(new_chunk_list)