def test_rotate_until_planes_aligned():
mlab.figure(1); mlab.clf()
thread = Thread()
thread_target = thread.clone()
for cons in C.seq_controllers(C.translate_ends(thread_target,[0,30,0],20),
C.bend_ends(thread_target,pi/4,.1),
C.jumprope(thread_target,pi/3,.1)):
thread_target.setConstraints(cons)
x,y,z = thread_target.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.1,color=(1,1,1))
colors = colorSeq(100)
for cons in C.match_cons(thread,thread_target.getConstraints()):
thread.setConstraints(cons)
x,y,z = thread.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.1,color=colors.next(),opacity=.5)
cons_t = upsample2D(
np.r_[atleast_2d(cons_start),
atleast_2d(cons_mid),
atleast_2d(cons_end)], T)
return cons_t
def flattenWP(cons_t):
return cons_t.flatten()
def unflattenWP(x):
return x.reshape(-1, 12)
goal_thread = start_thread.clone()
t_start = time()
for cons in makeTraj(randCons(1)):
goal_thread.setConstraints(cons)
print "time:", t_start - time()
def sayhi(_):
print "hi"
x0 = flattenWP(
upsample2D(np.array([cons_start.flatten(),
cons_end.flatten()]), n_wp + 2)[1:-1])
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()
t.setConstraints(con+pert)
xyzs.append(t.getXYZ().flatten())
xyzs = array(xyzs)
cxyzs = xyzs - xyzs.mean(axis=0)
figure(a)
plot(steps,cxyzs)
try:
seed = int(sys.argv[-1])
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))
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)
cons_end = cons_start + scale*USCALE*randn(12)
cons_mid = cons_start + scale*USCALE*randn(12)
cons_t = upsample2D(array([cons_start,cons_mid,cons_end]),T)
diffs = cons_t[:,6:9] - cons_t[:,0:3]
diffs = truncNorm(diffs,18)
cons_t[:,6:9] = cons_t[:,0:3] + diffs
mlab.figure(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))
thread = thread_start.clone()
for (t,cons) in enumerate(cons_t):
print t
thread.setConstraints(cons)
#x,y,z = thread.getXYZ()
xyz = x,y,z = thread.getXYZ()
mlab.plot3d(x,y,z,color=(1,0,0),tube_radius=.4)
thread_targ = thread
####### GOTO ARCH
# guess a good arch
try:
seed = int(sys.argv[-1])
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()
import mayavi.mlab as mlab
import sys
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))
cons_end = cons_start + scale * USCALE * randn(12)
cons_mid = cons_start + scale * USCALE * randn(12)
cons_t = upsample2D(array([cons_start, cons_mid, cons_end]), T)
diffs = cons_t[:, 6:9] - cons_t[:, 0:3]
diffs = truncNorm(diffs, 18)
cons_t[:, 6:9] = cons_t[:, 0:3] + diffs
mlab.figure(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))
thread = thread_start.clone()
for (t, cons) in enumerate(cons_t):
print t
thread.setConstraints(cons)
#x,y,z = thread.getXYZ()
xyz = x, y, z = thread.getXYZ()
mlab.plot3d(x, y, z, color=(1, 0, 0), tube_radius=.4)
thread_targ = thread
####### GOTO ARCH
# guess a good arch
center = (xyz.T[0] + xyz.T[-1]) / 2
mainax = 10 * normr(xyz.T[-1] - center)
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()
t.setConstraints(con + pert)
xyzs.append(t.getXYZ().flatten())
xyzs = array(xyzs)
cxyzs = xyzs - xyzs.mean(axis=0)
figure(a)
plot(steps, cxyzs)
import sys
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))
