def GetABCFromSO3Manifold(SO3dimIdx, amin, amax, W, dW, ddW):
Ndim = W.shape[0]
Nwaypoints = W.shape[1]
duration = 1.0 / float(Nwaypoints - 1)
[SO3trajstr,
durationVector] = GetTrajectoryStringSO3(SO3dimIdx, W, dW, ddW)
if SO3trajstr is None:
a = np.zeros((Nwaypoints, 1))
b = np.zeros((Nwaypoints, 1))
c = np.zeros((Nwaypoints, 1))
return [a, b, c]
#SO3traj = Trajectory.PiecewisePolynomialTrajectory.FromString(SO3trajstr)
#SO3traj.Plot(0.05)
#plt.show()
constraintsstr = str(duration)
SO3dim = len(SO3dimIdx)
assert (SO3dim == 3)
#for j in range(SO3dim):
### TODO: make generic amax[1] -> amax[SO3]
constraintsstr += "\n" + ' '.join([str(amax[1]), str(0.0), str(0.0)])
constraintsstr += "\n" + ' '.join([str(amin[1]), str(0.0), str(0.0)])
#print constraintsstr
#print "###########################"
inertia = eye(3)
for v in inertia:
constraintsstr += "\n" + ' '.join([str(i) for i in v])
abc = TOPPbindings.RunComputeSO3Constraints(SO3trajstr, constraintsstr)
a, b, c = Extractabc(abc)
return [a, b, c]
def Shortcut(robot,
taumax,
vmax,
lietraj,
maxiter,
expectedduration=-1,
meanduration=0,
upperlimit=-1,
inertia=None,
trackingplot=None):
if trackingplot == 1:
plt.axis([0, maxiter, 0, lietraj.duration])
plt.ion()
plt.show()
ylabel('Trajectory duration (s)')
xlabel('Iteration')
t_sc_start = time.time()
originalduration = lietraj.duration
#return shortcuted traj
if upperlimit < 0:
dur = lietraj.duration
upperlimit = lietraj.duration
else:
dur = upperlimit
attempt = 0
## for shortcutting
integrationtimestep = 1e-2
reparamtimestep = 1e-2
passswitchpointnsteps = 5
discrtimestep = 1e-2
constraintsstring = str(discrtimestep)
constraintsstring += "\n" + ' '.join([str(v) for v in taumax])
if not (inertia is None):
for v in inertia:
constraintsstring += "\n" + ' '.join([str(i) for i in v])
assert (dur > 10.0 * discrtimestep)
ncollision = 0
nnotretimable = 0
nnotshorter = 0
for it in range(maxiter):
if trackingplot == 1:
plt.scatter(it, lietraj.duration)
plt.draw()
if (
expectedduration > 0
): # check, if newlietraj.duration is short enough, stop SHORTCUTING
if (lietraj.duration < expectedduration):
print "\033[1;32mTrajectory's duration is already shorter than expected time --> stop shortcuting\033[0m"
break
if (dur < discrtimestep):
print "[Utils::Shortcut] trajectory duration is less than discrtimestep.\n"
break ## otherwise, this will cause an error in TOPP
## select an interval for shortcutting
t0 = random.rand() * dur
if meanduration == 0:
meanduration = dur - t0
T = random.rand() * min(meanduration, dur - t0)
t1 = t0 + T
while (T < 2.0 * discrtimestep):
t0 = random.rand() * dur
if meanduration == 0:
meanduration = dur - t0
T = random.rand() * min(meanduration, dur - t0)
t1 = t0 + T
if t1 > upperlimit:
t1 = upperlimit
if (t1 < t0):
temp = t0
t0 = t1
t1 = temp
T = t1 - t0
# print "\n\nShortcutting iteration", it + 1
# print t0, t1, t1- t0
# interpolate from t0 to t1
R_beg = lietraj.EvalRotation(t0)
R_end = lietraj.EvalRotation(t1)
omega0 = lietraj.EvalOmega(t0)
omega1 = lietraj.EvalOmega(t1)
shortcuttraj = lie.InterpolateSO3(R_beg, R_end, omega0, omega1, T)
#check feasibility only for the new portion
isincollision = CheckCollisionTraj(robot, shortcuttraj, R_beg,
discrtimestep)
if (not isincollision):
# a,b,c = lie.ComputeSO3Constraints(shortcuttraj, taumax, discrtimestep)
abc = TOPPbindings.RunComputeSO3Constraints(
str(shortcuttraj), constraintsstring) # discrtimestep)
a, b, c = lie.Extractabc(abc)
topp_inst = TOPP.QuadraticConstraints(shortcuttraj, discrtimestep,
vmax, list(a), list(b),
list(c))
x = topp_inst.solver
ret = x.RunComputeProfiles(1, 1)
if (ret == 1):
x.resduration
## check whether the new one has shorter duration
if (x.resduration + 0.01 < T): #skip if not shorter than 0.3 s
x.ReparameterizeTrajectory()
x.WriteResultTrajectory()
TOPPed_shortcuttraj = Trajectory.PiecewisePolynomialTrajectory.FromString(
x.restrajectorystring)
newlietraj = ReplaceTrajectorySegment(
lietraj, TOPPed_shortcuttraj, t0, t1)
lietraj = newlietraj
dur = lietraj.duration
#print "*******************************************"
print "Success at iteration", it + 1, ":", t0, t1, "Deta_t:", t1 - t0 - x.resduration
attempt += 1
#print "T:", nnotretimable, "; S:", nnotshorter , "; C:", ncollision , "; OK:", attempt
#print "*******************************************"
else:
# print "Not shorter"
nnotshorter += 1
else:
# print "Not retimable"
nnotretimable += 1
else:
# print "Collision"
ncollision += 1
# print "T:", nnotretimable, "; S:", nnotshorter , "; C:", ncollision , "; OK:", attempt
print "\033[1;32mT:", nnotretimable, "; S:", nnotshorter, "; C:", ncollision, "; OK:", attempt, "\033[0m"
print "\033[1;32m", originalduration - lietraj.duration, "sec. shorter\033[0m"
t_sc_end = time.time()
print "\033[1;32mRunning time:", t_sc_end - t_sc_start, "sec.\033[0m"
return lietraj
0.034761 0.188049 -1.298730 1.553443"""
traj0 = Trajectory.PiecewisePolynomialTrajectory.FromString(trajectorystring)
# Constraints
vmax = 2 * ones(ndof) # Velocity limits
amax = 10 * ones(ndof) # Acceleration limits
# Set up the TOPP instance
discrtimestep = 0.01
uselegacy = True
t0 = time.time()
if uselegacy: #Using the legacy KinematicLimits (a bit faster but not fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + " ".join([str(v) for v in vmax])
constraintstring += "\n" + " ".join([str(a) for a in amax])
x = TOPPbindings.TOPPInstance(None, "KinematicLimits", constraintstring,
trajectorystring)
else: #Using the general QuadraticConstraints (fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + " ".join([str(v) for v in vmax])
constraintstring += TOPPpy.ComputeKinematicConstraints(
traj0, amax, discrtimestep)
x = TOPPbindings.TOPPInstance(None, "QuadraticConstraints",
constraintstring, trajectorystring)
# Run TOPP
t1 = time.time()
ret = x.RunComputeProfiles(0, 0)
x.ReparameterizeTrajectory()
t2 = time.time()
print("Using legacy:", uselegacy)
def generateToppTrajectoryCallback(self, req):
print("Generating TOPP trajectory.")
res = GenerateTrajectoryResponse()
dof = len(req.waypoints.points[0].positions)
n = len(req.waypoints.points)
# If there is not enough waypoints to generate a trajectory return false
if (n <= 1 or dof == 0):
print(
"You must provide at least 2 points to generate a valid trajectory."
)
res.trajectory.success = False
return res
# Generate trajectory
# Path is of dimensions DOF x n_waypoints
path = np.zeros([dof, n])
for i in range(0, n):
for j in range(0, dof):
path[j][i] = req.waypoints.points[i].positions[j]
traj0 = Utilities.InterpolateViapoints(
path) # Interpolate using splines
# Constraints
vmax = zeros(dof) # Velocity limits
amax = zeros(dof) # Acceleration limits
for i in range(0, dof):
vmax[i] = req.waypoints.points[0].velocities[i]
amax[i] = req.waypoints.points[0].accelerations[i]
# Set up the TOPP instance
trajectorystring = str(traj0)
discrtimestep = 0.01
uselegacy = False
t0 = time.time()
if uselegacy: #Using the legacy KinematicLimits (a bit faster but not fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + string.join([str(v) for v in vmax])
constraintstring += "\n" + string.join([str(a) for a in amax])
x = TOPPbindings.TOPPInstance(None, "KinematicLimits",
constraintstring, trajectorystring)
else: #Using the general QuadraticConstraints (fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + string.join([str(v) for v in vmax])
constraintstring += TOPPpy.ComputeKinematicConstraints(
traj0, amax, discrtimestep)
x = TOPPbindings.TOPPInstance(None, "QuadraticConstraints",
constraintstring, trajectorystring)
# Run TOPP
t1 = time.time()
ret = x.RunComputeProfiles(0, 0)
x.ReparameterizeTrajectory()
t2 = time.time()
print("Using legacy:", uselegacy)
print("Discretization step:", discrtimestep)
print("Setup TOPP:", t1 - t0)
print("Run TOPP:", t2 - t1)
print("Total:", t2 - t0)
x.WriteProfilesList()
x.WriteSwitchPointsList()
profileslist = TOPPpy.ProfilesFromString(x.resprofilesliststring)
switchpointslist = TOPPpy.SwitchPointsFromString(
x.switchpointsliststring)
TOPPpy.PlotProfiles(profileslist, switchpointslist, 4)
x.WriteResultTrajectory()
traj1 = Trajectory.PiecewisePolynomialTrajectory.FromString(
x.restrajectorystring)
dtplot = 0.01
if self.plot_flag == True:
ion()
TOPPpy.PlotKinematics(traj0, traj1, dtplot, vmax, amax)
res.trajectory = self.TOPP2JointTrajectory(traj1,
req.sampling_frequency)
res.success = True
return res
def call_TOPP(req):
### Get the request path and prepare for TOPP
t0 = time.time()
print("= Get the request path...")
path_points = req.path
vel_limits = req.vel_limits
acc_limits = req.acc_limits
discrtimestep = req.timestep
# create a path going through viapoints
print("== Create a path going through viapoints...")
dim = len(path_points[0].positions)
ndata = len(path_points)
path_array = np.zeros((dim, ndata))
for i in range(ndata):
path_array[:, i] = np.array(path_points[i].positions)
print("=== Interpolate viapoints...")
#import pdb
#pdb.set_trace()
path = Utilities.InterpolateViapoints(
path_array) # Interpolate using splines
# Constraints
vmax = np.array(vel_limits)
amax = np.array(acc_limits)
### Set up the TOPP instance
print("==== Set up a TOPP instance...")
t1 = time.time()
trajectorystring = str(path)
uselegacy = True # True to use KinematicLimits(vel & acc)
if uselegacy: #Using the legacy KinematicLimits (a bit faster but not fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + string.join([str(v) for v in vmax])
constraintstring += "\n" + string.join([str(a) for a in amax])
x = TOPPbindings.TOPPInstance(None, "KinematicLimits",
constraintstring, trajectorystring)
else: #Using the general QuadraticConstraints (fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + string.join([str(v) for v in vmax])
constraintstring += TOPPpy.ComputeKinematicConstraints(
path, amax, discrtimestep)
x = TOPPbindings.TOPPInstance(None, "QuadraticConstraints",
constraintstring, trajectorystring)
### Run TOPP
print("===== Run TOPP...")
t2 = time.time()
#import pdb
#pdb.set_trace()
ret = x.RunComputeProfiles(0, 0)
x.ReparameterizeTrajectory()
### Convert resultant array to plan
print("====== Convert resultant array to plan...")
t3 = time.time()
x.WriteResultTrajectory(
) # be sure to write the result before reading it!!!
traj = Trajectory.PiecewisePolynomialTrajectory.FromString(
x.restrajectorystring)
traj_point = JointTrajectoryPoint()
traj_points = [] #PathToTrajResponse() # just an empty list
t = 0.0
while t < traj.duration:
traj_point.positions = traj.Eval(t).tolist()
traj_point.velocities = traj.Evald(t).tolist()
traj_point.accelerations = traj.Evaldd(t).tolist()
traj_point.time_from_start = rospy.Duration(t)
traj_points.append(copy.deepcopy(traj_point))
t += discrtimestep
# the last point
traj_point.positions = traj.Eval(traj.duration).tolist()
traj_point.velocities = traj.Evald(traj.duration).tolist()
traj_point.accelerations = traj.Evaldd(traj.duration).tolist()
traj_point.time_from_start = rospy.Duration(traj.duration)
traj_points.append(copy.deepcopy(traj_point))
### Display statistics
t4 = time.time()
print(">>>>> Statistics of TOPP <<<<<")
print("Dimension of path point: " + str(path_array.shape[0]))
print("Number of data points: " + str(path_array.shape[1]))
print("Using legacy: " + str(uselegacy))
print("Discretization step: " + str(discrtimestep) + " s")
print("Obtain request path and get prepared: " + str(t1 - t0) + " s")
print("Setup TOPP: " + str(t2 - t1) + " s")
print("Run TOPP: " + str(t3 - t2) + " s")
print("Convert resultant array to plan: " + str(t4 - t3) + " s")
print("Total: " + str(t4 - t0) + " s")
print("Trajectory duration before TOPP: " + str(path.duration) + " s")
print("Trajectory duration after TOPP: " + str(traj.duration) + " s")
print(">>>>> End <<<<<")
### Return result
res = PathToTrajResponse()
res.traj = traj_points
return res
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()
if uselegacy: #Using the legacy TorqueLimits (faster but not fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + " ".join([str(v) for v in vmax])
constraintstring += "\n" + " ".join([str(t) for t in taumin])
constraintstring += "\n" + " ".join([str(t) for t in taumax])
x = TOPPbindings.TOPPInstance(robot,"TorqueLimitsRave", constraintstring, trajectorystring)
else: #Using the general QuadraticConstraints (fully supported)
constraintstring = str(discrtimestep)
constraintstring += "\n" + " ".join([str(v) for v in vmax])
constraintstring += TOPPopenravepy.ComputeTorquesConstraints(robot,traj0,taumin,taumax,discrtimestep)
x = TOPPbindings.TOPPInstance(None,"QuadraticConstraints",constraintstring,trajectorystring);
# Run TOPP
t1 = time.time()
ret = x.RunComputeProfiles(0,0)
if(ret == 1):
x.ReparameterizeTrajectory()
t2 = time.time()
print("Using legacy:", uselegacy)
print("Discretization step:", discrtimestep)
################ Bobrow with actuation redundancy ##################
constraintstring = str(tunings.discrtimestep)
constraintstring += "\n" + " ".join([str(v) for v in robot.vmax])
scaledowncoef = 0.99
robot.taumin = taumin * scaledowncoef # safety bound
robot.taumax = taumax * scaledowncoef
t0 = time.time()
constraintstring += Bimanual.ComputeConstraints(robot,tunings,trajtotal)
robot.taumin = taumin
robot.taumax = taumax
t1 = time.time()
x = TOPPbindings.TOPPInstance(None,"PolygonConstraints",constraintstring,trajectorystring)
x.integrationtimestep = 1e-3
ret = x.RunComputeProfiles(1,1)
t2 = time.time()
print("Compute Polygon constraints:", t1-t0, "seconds")
print("Note : Compute Polygon is faster with the C++ version, checkout branch tomas-develop")
print("Run TOPP:", t2-t1, "seconds")
x.WriteProfilesList()
x.WriteSwitchPointsList()
profileslist = TOPPpy.ProfilesFromString(x.resprofilesliststring)
switchpointslist = TOPPpy.SwitchPointsFromString(x.switchpointsliststring)
if(ret == 1):
x.ReparameterizeTrajectory()
3
0.0 0.1 -0.159247917034 0.0789972227119
0.0 0.1 -32.7720979649 43.5627972865
0.0 0.1 0.958473557774 -1.41129807703"""
traj = Trajectory.PiecewisePolynomialTrajectory.FromString(trajstr)
inertia = eye(3)
vmax = ones(3)
accelmax = ones(3)
discrtimestep = 1e-3
constraintsstr = str(discrtimestep)
constraintsstr += "\n" + " ".join([str(a) for a in accelmax])
for v in inertia:
constraintsstr += "\n" + " ".join([str(i) for i in v])
#When Inertia is an Identity matrix, angular accelerations are the same as torques
t0 = time.time()
abc = TOPPbindings.RunComputeSO3Constraints(trajstr, constraintsstr)
a, b, c = Extractabc(abc)
t1 = time.time()
topp_inst = TOPP.QuadraticConstraints(traj, discrtimestep, vmax, list(a),
list(b), list(c))
x = topp_inst.solver
ret = x.RunComputeProfiles(0, 0)
x.ReparameterizeTrajectory()
t2 = time.time()
print("Compute a,b,c:", t1 - t0)
print("Run TOPP:", t2 - t1)
print("Total:", t2 - t0)
# print "in collision", " ", t, "/" , lietraj.duration
# time.sleep(0.01)
################################# TOPP #############################################
discrtimestep= 1e-2
constraintsstring = str(discrtimestep)
constraintsstring += "\n" + ' '.join([str(v) for v in taumax])
for v in inertia:
constraintsstring += "\n" + ' '.join([str(i) for i in v])
# Note that, when Inertia is an Identity matrix, angular accelerations are the same as torques
print "\033[93mRunning TOPP", "\033[0m"
t_topp_start = time.time()
traj = Trajectory.PiecewisePolynomialTrajectory.FromString(Utils.TrajStringFromTrajList(Trajlist))
abc = TOPPbindings.RunComputeSO3Constraints(str(traj),constraintsstring)
a,b,c = lie.Extractabc(abc)
# a,b,c = lie.ComputeSO3Constraints(traj, taumax, discrtimestep) #This is the implementation of computing SO3Constraints in Python
topp_inst = TOPP.QuadraticConstraints(traj, discrtimestep, vmax, list(a), list(b), list(c))
x = topp_inst.solver
ret = x.RunComputeProfiles(0,0)
if ret == 1:
x.ReparameterizeTrajectory()
x.WriteResultTrajectory()
traj1 = Trajectory.PiecewisePolynomialTrajectory.FromString(x.restrajectorystring)
t_topp_end = time.time()
print "\033[1;32mRunning time:",t_topp_end-t_topp_start, "sec.\033[0m"
