-
Notifications
You must be signed in to change notification settings - Fork 76
Quick examples
Cuong edited this page Apr 25, 2014
·
59 revisions
Run ipython and copy-paste the codes below into the ipython command line.
TOPP with velocity and acceleration limits.
import string
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"""
# Constraints
discrtimestep = 0.001
vmax = 2*ones(ndof) # Velocity limits
amax = 10*ones(ndof) # Acceleration limits
constraintstring = str(discrtimestep) + "\n"; # Discretization time step
constraintstring += string.join([str(v) for v in vmax]) + "\n"
constraintstring += string.join([str(a) for a in amax])
# Run TOPP
x = TOPPbindings.TOPPInstance(None,"KinematicLimits",constraintstring,trajectorystring);
ret = x.RunComputeProfiles(0,0)
x.ReparameterizeTrajectory()
# 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)
traj0 = Trajectory.PiecewisePolynomialTrajectory.FromString(trajectorystring)
dtplot = 0.01
TOPPpy.PlotKinematics(traj0,traj1,dtplot,vmax,amax)
print "Trajectory duration before TOPP: ", traj0.duration
print "Trajectory duration after TOPP: ", traj1.duration
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)
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)
- Plan a trajectory with OpenRAVE Bi-RRT planner and constraintparabolic smoothing
- 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)
-> <-