Skip to content

Quick examples

Jump to bottom Edit New page
Cuong edited this page Jul 17, 2017 · 59 revisions

Table of contents

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

Kinematic limits (initial trajectory specified by via-points)

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
from TOPP import Utilities

# A two-dof path going through 5 viapoints (0,1) - (1,1) - (5,1) - (3,2) - (5,4)
path = array([[0,1,5,3,5],[1,1,1,2,4]])
traj0 = Utilities.InterpolateViapoints(path) # Interpolate using splines

# Constraints
vmax = 2*ones(traj0.dimension)  # Velocity limits
amax = 10*ones(traj0.dimension) # Acceleration limits

# Set up the TOPP instance
trajectorystring = str(traj0)
discrtimestep = 0.005
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

Kinematic limits (initial trajectory specified by trajectorystring)

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.01
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)

MVC Values Velocities Accelerations

Kinematic limits for trajectories in SO(3)

TOPP on a SO(3) trajectory with angular velocity and acceleration limits. Note that, when "Inertia" is the identity matrix, "torques" are the same as angular accelerations.

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

def Extractabc(abc):
    lista = [float(x) for x in abc[0].split()]
    listb = [float(x) for x in abc[1].split()]
    listc = [float(x) for x in abc[2].split()]
    n= len(lista)/6
    a = zeros((n,6))
    b = zeros((n,6))
    c = zeros((n,6))
    for i in range(n):
        a[i,:] = lista[i*6:i*6+6]
        b[i,:] = listb[i*6:i*6+6]
        c[i,:] = listc[i*6:i*6+6]
    return a, b, c

trajstr = """0.500000
3
0.0 0.0 -35.9066153846 47.930797594
0.0 0.0 -0.645566686001 1.11351913336
0.0 0.0 8.3609538376 -11.3580450529
0.500000
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

# 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(traj,traj1,dtplot,vmax,accelmax)

raw_input()

MVC Values Velocities Accelerations

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)

MVC Values Velocities Torques

Bimanual manipulation (redundant actuation)

TOPP with velocity and torque limits (requires OpenRAVE) for a redundantly actuated robot. Uses the class PolygonConstraints. See our paper http://www.ntu.edu.sg/home/cuong/docs/overactuated.pdf

import time
import string
from pylab import *
from numpy import *
from openravepy import *
from TOPP import Trajectory
from TOPP import TOPPpy
from TOPP import TOPPopenravepy
from TOPP import TOPPbindings
from TOPP import ClosedChain
from TOPP import Bimanual
ion()

############# Load environment and robot ############
env = Environment()
env.SetViewer('qtcoin')
env.Load('../robots/ArmFull.xml')
env.Load('../robots/ArmCut.xml')
robot1 = env.GetRobots()[0]
robot2 = env.GetRobots()[1]
t = RaveCreateKinBody(env,'')
t.InitFromBoxes(array([array([0.3,0,-0.06,0.5,0.2,0.02])]),True)
t.SetName('Box')
g=t.GetLinks()[0].GetGeometries()[0]
g.SetAmbientColor([0.5,0.5,0.5])
g.SetDiffuseColor([0.5,0.5,0.5])
env.Add(t,True)

M = array([[ 0.99981043, -0.01138393,  0.01579602,  0.23296329],
       [-0.01788201, -0.21589215,  0.97625346, -0.95132697],
       [-0.00770337, -0.97635085, -0.21605479,  0.3695007 ],
       [ 0.        ,  0.        ,  0.        ,  1.        ]])
env.GetViewer().SetCamera(M)

T1 = eye(4)
T2 = array(T1)
T2[0,3] = 0.6
T1dyn = array([[1.,0,0,0],
            [0,0,-1,0],
            [0,1,0,0],
            [0,0,0,1]])
T2dyn = array(T1dyn)
T2dyn[0,3] = 0.6

robot1.SetTransform(T1)
robot2.SetTransform(T2)
constrainedlink = 9
obj = robot1.GetLinks()[constrainedlink]
taumin = -20*ones(6)
taumax = 20*ones(6)
robot = Bimanual.Robot()
robot.robot1 = robot1
robot.robot2 = robot2
robot.T1 = T1
robot.T2 = T2
robot.T1dyn = T1dyn
robot.T2dyn = T2dyn
robot.freedofs = [0,1,2]
robot.dependentdofs = [3,4,5]
robot.constrainedlink = 9
robot.Gdofs = [0,1,2]
robot.Sdofs = [3,4,5]
robot.actuated = [True]*6
robot.taumin = taumin
robot.taumax = taumax
robot.vmax = [3]*6
robot.q_range = [[-pi,pi],[-pi,pi],[-pi,pi]]    
tunings = Bimanual.Tunings()
tunings.duration = 1
tunings.nchunks = 10
tunings.chunksubdiv = 20
tunings.tol_jacobian = 1e-2
tunings.discrtimestep = 5e-3

############# Kinematics to find start and goal configurations ############
# Start configuration
# Robot 1
q_start1 = array([ 1.60299799, -0.98929389, -0.58842164])
robot1.SetTransform(T1)
robot1.SetDOFValues(q_start1)
desiredpose = Bimanual.Getxytheta(obj.GetTransform())
# Robot 2
desiredpose2 = desiredpose + [0,0,-pi]
robot1.SetTransform(T2)
q_start2, error = Bimanual.IK3(robot1,desiredpose2,q_start=array([pi,0,0])-q_start1)
robot1.SetTransform(T1)
robot1.SetDOFValues(q_start1)
robot2.SetDOFValues(q_start2[0:2])
q_start = zeros(6)
q_start[0:3] = q_start1
q_start[3:6] = q_start2
robot2.SetTransform(T2)

# Goal configuration
# Robot 1
q_goal1 = q_start1 + array([0.5,-0.5,-0.1])
robot1.SetTransform(T1)
robot1.SetDOFValues(q_goal1)
desiredpose = Bimanual.Getxytheta(obj.GetTransform())
# Robot 2
desiredpose2 = desiredpose + [0,0,-pi]
robot1.SetTransform(T2)
q_goal2, error = Bimanual.IK3(robot1,desiredpose2,q_start=array([pi,0,0])-q_goal1)
robot1.SetTransform(T1)
robot1.SetDOFValues(q_goal1)
robot2.SetDOFValues(q_goal2[0:2])
q_goal = zeros(6)
q_goal[0:3] = q_goal1
q_goal[3:6] = q_goal2

########### Interpolate between the two configurations ##############
freestartvelocities = array([0.1,0.1,0.1])
freegoalvelocities = array([0.5,0,0])
trajtotal = Bimanual.Interpolate(robot, tunings, q_start, q_goal, freestartvelocities, freegoalvelocities)
trajectorystring = str(trajtotal)

################ Bobrow with actuation redundancy ##################
constraintstring = str(tunings.discrtimestep)
constraintstring += "\n" + string.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()
    x.WriteResultTrajectory()
    trajtotal2 = Trajectory.PiecewisePolynomialTrajectory.FromString(x.restrajectorystring)

    dt=0.01
    robot1.SetTransform(T1dyn)
    robot2.SetTransform(T2dyn)
    traj = trajtotal
    for t in arange(0,traj.duration,dt):
        q = traj.Eval(t)
        robot1.SetDOFValues(q[0:3])
        robot2.SetDOFValues(q[3:5])
        time.sleep(dt)

    TOPPpy.PlotProfiles(profileslist,switchpointslist,4)
    TOPPpy.PlotKinematics(trajtotal,trajtotal2,dt,robot.vmax)
    Bimanual.PlotTorques(robot,trajtotal,trajtotal2,dt)

MVC Values Velocities Torques

Add a custom footer
Add a custom sidebar
Clone this wiki locally