Skip to content

Quick examples

Jump to bottom Edit New page
Cuong edited this page May 16, 2014 · 59 revisions

Table of contents

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

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.

MVC Values Velocities Accelerations

Torque limits

TOPP with velocity and torque limits (requires OpenRAVE). Change the path of the file "twodof.robot.xml" if necessary (it is not included in the OpenRAVE distribution and can be found in TOPP/robots/).

import string
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/twodof.robot.xml")
env.SetViewer('qtcoin')
env.GetViewer().SetCamera(array([[ 0.00846067,  0.4334184 , -0.9011531 ,  0.84555054],
       [ 0.99938498,  0.0270039 ,  0.02237072,  0.01155015],
       [ 0.03403053, -0.90078814, -0.43292337,  0.65048862],
       [ 0.        ,  0.        ,  0.        ,  1.        ]]))
robot = env.GetRobots()[0]
robot.SetTransform(array([[0, 0, 1, 0],
                          [0, 1, 0, 0],
                          [-1, 0, 0, 0.3],
                          [0, 0, 0, 1]]))
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
robot.SetDOFVelocityLimits(100 * vel_lim) # Overrides robot velocity limits

# Trajectory
q0 = [0,0]
q1 = [5*pi/4,-pi/2]
qd0 = [1,1]
qd1 = [1,1]
T = 2
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
discrtimestep = 0.002
vmax = array([5, 5])
taumin = array([-25, -10])
taumax = array([25, 10])
constraintstring = str(discrtimestep) + "\n";
constraintstring += string.join([str(v) for v in vmax]) + "\n"
constraintstring += string.join([str(t) for t in taumin]) + "\n" + string.join([str(t) for t in taumax])

#Run TOPP
x = TOPPbindings.TOPPInstance(robot,"TorqueLimitsRave", constraintstring, trajectorystring)
ret = x.RunComputeProfiles(0,0)
if(ret == 1):
    x.ReparameterizeTrajectory()

# 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

General quadratic constraints

TOPP with general quadratic constraints (requires OpenRAVE). See our paper « A general, fast, and robust implementation of the time-optimal path parameterization algorithm » for details about quadratic constraints.

import string
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
robot.SetDOFVelocityLimits(100*vel_lim) # Override robot velocity limits

# Trajectory
q0 = zeros(ndof)
q1 = zeros(ndof)
qd0 = zeros(ndof)
qd1 = zeros(ndof)
q1[0:4] = [2.32883,  1.61082,  0.97706,  1.94169] #Target DOF values for the shoulder and elbow joints
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
discrtimestep = 0.005
vmax = zeros(ndof)
taumin = zeros(ndof)
taumax = zeros(ndof)
vmax[0:4] = [2,2,2,2]  # Velocity limits, only for the shoulder and elbow joints
taumin[0:4] = [-30,-50,-25,-15] # Torque limits, only for the shoulder and elbow joints
taumax[0:4] = [30,50,25,15]
constraintstring = str(discrtimestep) + "\n";
constraintstring += string.join([str(v) for v in vmax])
constraintstring += TOPPopenravepy.ComputeTorquesConstraints(robot,traj0,taumin,taumax,discrtimestep)

# Run TOPP
x = TOPPbindings.TOPPInstance(None,"QuadraticConstraints",constraintstring,trajectorystring);
ret = x.RunComputeProfiles(0,0)
if(ret == 1):
    x.ReparameterizeTrajectory()

# 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

Complete example with OpenRAVE Bi-RRT

  1. Plan a trajectory with OpenRAVE Bi-RRT planner and constraintparabolic smoothing
  2. Reparameterize with TOPP under velocity and torque limits
import string
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

# Robot
env = Environment()
env.SetViewer('qtcoin')
env.Load('data/lab1.env.xml')
robot = env.GetRobots()[0]
RaveSetDebugLevel(DebugLevel.Debug)
ndof = robot.GetDOF()

# Finding a trajectory using OpenRAVE RRT with constraintparabolicsmoothing
vmax = 2 * ones(ndof)
amax = 10* ones(ndof)
robot.SetDOFVelocityLimits(vmax)
robot.SetDOFAccelerationLimits(amax)
robot.SetDOFValues(zeros(ndof))
robot.SetActiveDOFs(range(4)) # set the shoulder and elbow joints to be the only active joints
params = Planner.PlannerParameters()
params.SetRobotActiveJoints(robot)
params.SetGoalConfig([0.1,pi/2,pi/3,pi/2.1])
params.SetExtraParameters("""<_postprocessing planner="constraintparabolicsmoother">
    <_fStepLength>0</_fStepLength>
    <minswitchtime>0.5</minswitchtime>
    <_nmaxiterations>40</_nmaxiterations>
</_postprocessing>""")
planner=RaveCreatePlanner(env,'birrt')
planner.InitPlan(robot, params)
ravetraj0 = RaveCreateTrajectory(env,'')
planner.PlanPath(ravetraj0)
topptraj0 = TOPPopenravepy.FromRaveTraj(robot,ravetraj0)

# Constraints
discrtimestep = 0.005
taumin = zeros(ndof)
taumax = zeros(ndof)
taumin[0:4] = [-30,-50,-25,-15] # Torque limits, only for the shoulder and elbow joints
taumax[0:4] = [30,50,25,15]
constraintstring = str(discrtimestep) + "\n";
constraintstring += string.join([str(v) for v in vmax])
constraintstring += TOPPopenravepy.ComputeTorquesConstraints(robot,topptraj0,taumin,taumax,discrtimestep)

# Run TOPP
x = TOPPbindings.TOPPInstance(None,"QuadraticConstraints",constraintstring,str(topptraj0));
ret = x.RunComputeProfiles(0,0)
if(ret == 1):
    x.ReparameterizeTrajectory()

# 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()
    topptraj1 = Trajectory.PiecewisePolynomialTrajectory.FromString(x.restrajectorystring)
    #topptraj1 = x.GetOpenRAVEResultTrajectory(env)
    dtplot = 0.01
    TOPPpy.PlotKinematics(topptraj0,topptraj1,dtplot,vmax)
    TOPPopenravepy.PlotTorques(robot,topptraj0,topptraj1,dtplot,taumin,taumax,3)
    print "Trajectory duration before TOPP: ", topptraj0.duration
    print "Trajectory duration after TOPP: ", topptraj1.duration
else:
    print "Trajectory is not time-parameterizable"

# Execute trajectory
if(ret == 1):
    spec = ravetraj0.GetConfigurationSpecification()
    ravetraj1 = TOPPopenravepy.ToRaveTraj(robot,spec,topptraj1)
    robot.GetController().SetPath(ravetraj1)

![MVC](https://raw.github.com/quangounet/TOPP/master/tests/fig/birrt_mvc.png) ![Values](https://raw.github.com/quangounet/TOPP/master/tests/fig/birrt_val.png) ![Velocities](https://raw.github.com/quangounet/TOPP/master/tests/fig/birrt_vel.png) ![Accelerations](https://raw.github.com/quangounet/TOPP/master/tests/fig/birrt_acc.png)
Add a custom footer
Add a custom sidebar
Clone this wiki locally