-
Notifications
You must be signed in to change notification settings - Fork 76
Quick examples
Run ipython and copy-paste the codes below into the ipython command line.
- Kinematic limits
- Kinematic limits for trajectories in SO(3)
- Torque limits
- Bimanual manipulation (redundant actuation)
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
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)
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()
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)
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)