def test_move_ends():
thread = Thread()
mlab.figure(1); mlab.clf()
colors = colorSeq(25)
cons_new = thread.getConstraints()
cons_new[0:3] += 4
cons_new[6:9] += 4
for (t,cons) in enumerate(C.match_cons(thread,cons_new)):
thread.setConstraints(cons)
x,y,z = thread.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.1,color=colors.next())
def test_jumprope():
thread = Thread()
mlab.figure(1); mlab.clf()
colors = colorSeq(100)
cons_new = thread.getConstraints()
for cons in C.bend_ends(thread,.25,.1):
thread.setConstraints(cons)
for (t,cons) in enumerate(C.jumprope(thread,pi,.1)):
thread.setConstraints(cons)
x,y,z = thread.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.1,color=colors.next())
def test_running_jumprope():
thread = Thread()
mlab.figure(1); mlab.clf()
colors = colorSeq(100)
cons_new = thread.getConstraints()
cons_new[0:3] += 25
cons_new[6:9] += 25
for cons in C.bend_ends(thread,.25,.1):
thread.setConstraints(cons)
rj = C.add_controllers(thread,C.translate_ends(thread,[0,0,30],20),
C.jumprope(thread,pi,.25))
for (t,cons) in enumerate(rj):
thread.setConstraints(cons)
x,y,z = thread.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.1,color=colors.next())
import scipy.optimize as opt
from pylab import *
from time import time
from doo import *
from doo.doo import Thread
from sqp import frPath, calcStates, evalDiff
from numpy.random import randn
from utils import upsample2D
T = 15
start_thread = Thread()
scale = .25
cons_start = start_thread.getConstraints()
n_wp = 1
def randCons(n):
return cons_start + scale * USCALE[None, :] * randn(n, 12)
cons_end = randCons(1)
def makeTraj(cons_mid):
cons_t = upsample2D(
np.r_[atleast_2d(cons_start),
atleast_2d(cons_mid),
atleast_2d(cons_end)], T)
return cons_t
from pylab import *
from doo.doo import Thread
from control import jacobian,calcFeats2d
from doo import *
import numpy as np
from utils import upsample2D
thread = Thread()
#A,B = thread.makePerts()
scale = .25
cons_start = thread.getConstraints()
cons_1 = cons_start + scale*r_[randn(3)*10,randn(3)*.5,randn(3)*10,randn(3)*.5]
cons_2 = cons_start + scale*r_[randn(3)*10,randn(3)*.5,randn(3)*10,randn(3)*.5]
cons_end = cons_start + scale*r_[randn(3)*10,randn(3)*.5,randn(3)*10,randn(3)*.5]
for i,cons in enumerate(upsample2D(np.array([cons_start,cons_1,cons_2,cons_end]),80)):
print i
thread.setConstraints(cons)
q = thread.getXYZ()
#print "linear approx":
for a in xrange(12):
eps = zeros(12)
eps[a] = USCALE[a]
steps = linspace(-1,1,20)
perts = outer(steps,eps)
con = thread.getConstraints()
xyzs = []
for pert in perts:
t = thread.clone()
np.random.seed(seed)
print "using seed", seed
except Exception:
print "not using seed"
# seed with 0 and final config is the same
# seed with 1 and it's way different
scale = .25
T = 10
thread_start = Thread()
n_clones = 3
threads = [thread_start.clone() for _ in xrange(n_clones)]
cons_start = thread_start.getConstraints()
cons_end = cons_start + scale*np.r_[randn(3)*10,randn(3)*.5,randn(3)*10,randn(3)*.5]
tip1_start,tip2_start = cons_start[0:3],cons_start[6:9]
tip1_end,tip2_end = cons_end[0:3],cons_end[6:9]
tips = np.c_[tip1_start,tip2_start,tip1_end,tip2_end]
colors = [(1,0,0),(0,1,0),(0,0,1)]
mlab.clf()
x,y,z = thread_start.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.4,color=(1,1,1))
#x,y,z = tips
#mlab.points3d(x,y,z,color=(1,1,1))
from doo import *
from doo.doo import Thread
from sqp import frPath,calcStates,evalDiff
from numpy.random import randn
from utils import upsample2D
T = 30
start_thread = Thread()
scale=.2
cons_start = start_thread.getConstraints()
cons_1 = cons_start + scale*USCALE*randn(12)
cons_2 = cons_start + scale*USCALE*randn(12)
cons_end = cons_start + scale*USCALE*randn(12)
print "moving thread to config 1 so it's hopefully stable"
cons_t = upsample2D(np.array([cons_start,cons_1]),T)
for cons in cons_t:
start_thread.setConstraints(cons)
print "moving it along path 1->2->goal"
goal_thread = start_thread.clone()
cons_t = upsample2D(np.array([cons_1,cons_2,cons_end]),T)
for cons in cons_t[1:]:
goal_thread.setConstraints(cons)
#print evalDiff(start_thread,goal_thread,cons_t)
frPath(start_thread,goal_thread,T,None)
np.random.seed(seed)
print "using seed", seed
except Exception:
print "not using seed"
# seed with 0 and final config is the same
# seed with 1 and it's way different
scale = .25
T = 10
thread_start = Thread()
n_clones = 3
threads = [thread_start.clone() for _ in xrange(n_clones)]
cons_start = thread_start.getConstraints()
cons_end = cons_start + scale * np.r_[
randn(3) * 10, randn(3) * .5,
randn(3) * 10, randn(3) * .5]
tip1_start, tip2_start = cons_start[0:3], cons_start[6:9]
tip1_end, tip2_end = cons_end[0:3], cons_end[6:9]
tips = np.c_[tip1_start, tip2_start, tip1_end, tip2_end]
colors = [(1, 0, 0), (0, 1, 0), (0, 0, 1)]
mlab.clf()
x, y, z = thread_start.getXYZ()
mlab.plot3d(x, y, z, tube_radius=.4, color=(1, 1, 1))
#x,y,z = tips
import mp.controllers as C
mlab.figure(1); mlab.clf()
scalar =5
T = 30
n_waypoints = 3
#np.random.seed(23)
np.random.seed(24)
#np.random.seed(28)
thread = Thread()
thread_targ = thread.clone()
cons = thread.getConstraints()
waypoints = cons[None,:] + np.cumsum(np.r_[np.zeros((1,12)),scalar*USCALE*randn(n_waypoints,12)],0)
for cons in upsample2D(waypoints,n_waypoints*T):
thread_targ.setConstraints(cons)
x,y,z = thread_targ.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.05,color=(1,1,1))
mlab.clf()
x,y,z = thread_targ.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.2,color=(1,1,1))
colors = colorSeq(100)
x,y,z = thread.getXYZ()
from pylab import *
from doo.doo import Thread
from control import jacobian, calcFeats2d
from doo import *
import numpy as np
from utils import upsample2D
thread = Thread()
#A,B = thread.makePerts()
scale = .25
cons_start = thread.getConstraints()
cons_1 = cons_start + scale * r_[
randn(3) * 10, randn(3) * .5,
randn(3) * 10, randn(3) * .5]
cons_2 = cons_start + scale * r_[
randn(3) * 10, randn(3) * .5,
randn(3) * 10, randn(3) * .5]
cons_end = cons_start + scale * r_[
randn(3) * 10, randn(3) * .5,
randn(3) * 10, randn(3) * .5]
for i, cons in enumerate(
upsample2D(np.array([cons_start, cons_1, cons_2, cons_end]), 80)):
print i
thread.setConstraints(cons)
q = thread.getXYZ()
#print "linear approx":
for a in xrange(12):
eps = zeros(12)
eps[a] = USCALE[a]
mlab.figure(1)
mlab.clf()
scalar = 5
T = 30
n_waypoints = 3
#np.random.seed(23)
np.random.seed(24)
#np.random.seed(28)
thread = Thread()
thread_targ = thread.clone()
cons = thread.getConstraints()
waypoints = cons[None, :] + np.cumsum(
np.r_[np.zeros((1, 12)), scalar * USCALE * randn(n_waypoints, 12)], 0)
for cons in upsample2D(waypoints, n_waypoints * T):
thread_targ.setConstraints(cons)
x, y, z = thread_targ.getXYZ()
mlab.plot3d(x, y, z, tube_radius=.05, color=(1, 1, 1))
mlab.clf()
x, y, z = thread_targ.getXYZ()
mlab.plot3d(x, y, z, tube_radius=.2, color=(1, 1, 1))
colors = colorSeq(100)
x, y, z = thread.getXYZ()
