Quick examples

Table of contents

Run ipython and copy-paste the codes below into the ipython command line.

• Kinematic limits
• Torque limits

Kinematic limits

TOPP with velocity and acceleration limits. Set uselegacy to True to use the legacy (and faster) KinematicConstraints and to False to use the more general QuadraticConstraints.

import string, time
from pylab import *
from numpy import *
from TOPP import TOPPbindings
from TOPP import TOPPpy
from TOPP import Trajectory

# Trajectory
ndof = 5
trajectorystring = """1.0
5
-0.495010 1.748820 -2.857899 1.965396
0.008319 0.004494 1.357524 -1.289918
-0.354081 1.801074 -1.820616 0.560944
0.221734 -1.320792 3.297177 -2.669786
-0.137741 0.105246 0.118968 -0.051712
1.0
5
0.361307 1.929207 -4.349490 2.275776
0.080419 -1.150212 2.511645 -1.835906
0.187321 -0.157326 -0.355785 0.111770
-0.471667 -2.735793 7.490559 -4.501124
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.001
uselegacy = True
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

# Display results
ion()
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
TOPPpy.PlotKinematics(traj0,traj1,dtplot,vmax,amax)
print "Trajectory duration before TOPP: ", traj0.duration
print "Trajectory duration after TOPP: ", traj1.duration

Torque limits

TOPP with velocity and torque limits (requires OpenRAVE). Use a model of the 7-DOF Barrett WAM. Set uselegacy to True to use the legacy (and faster) TorqueConstraints and to False to use the more general QuadraticConstraints.

import string,time
from pylab import *
from numpy import *
from openravepy import *
from TOPP import TOPPbindings
from TOPP import TOPPpy
from TOPP import TOPPopenravepy
from TOPP import Trajectory
from TOPP import Utilities

# Robot (OpenRAVE)
env = Environment()
env.Load("robots/barrettwam.robot.xml")
env.SetViewer('qtcoin')
env.GetViewer().SetCamera(array([[ 0.92038107,  0.00847738, -0.39093071,  0.69997793],
       [ 0.39101295, -0.02698477,  0.91998951, -1.71402919],
       [-0.00275007, -0.9995999 , -0.02815103,  0.40470174],
       [ 0.        ,  0.        ,  0.        ,  1.        ]]))
robot=env.GetRobots()[0]
grav=[0,0,-9.8]
ndof=robot.GetDOF()
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.01
uselegacy = False
t0 = time.time()
if uselegacy: #Using the legacy TorqueLimits (faster but not fully supported)
    constraintstring = str(discrtimestep)
    constraintstring += "\n" + string.join([str(v) for v in vmax])
    constraintstring += "\n" + string.join([str(t) for t in taumin]) 
    constraintstring += "\n" + string.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" + string.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
print "Setup TOPP:", t1-t0
print "Run TOPP:", t2-t1
print "Total:", t2-t0

# Display results
ion()
x.WriteProfilesList()
x.WriteSwitchPointsList()
profileslist = TOPPpy.ProfilesFromString(x.resprofilesliststring)
switchpointslist = TOPPpy.SwitchPointsFromString(x.switchpointsliststring)
TOPPpy.PlotProfiles(profileslist,switchpointslist,4)
if(ret == 1):
    x.WriteResultTrajectory()
    traj1 = Trajectory.PiecewisePolynomialTrajectory.FromString(x.restrajectorystring)
    dtplot = 0.01
    TOPPpy.PlotKinematics(traj0,traj1,dtplot,vmax)
    TOPPopenravepy.PlotTorques(robot,traj0,traj1,dtplot,taumin,taumax,3)
    print "Trajectory duration before TOPP: ", traj0.duration
    print "Trajectory duration after TOPP: ", traj1.duration
else:
    print "Trajectory is not time-parameterizable"

# Execute trajectory
if(ret == 1):
    TOPPopenravepy.Execute(robot,traj1)