Esempio n. 1
0
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)    
Esempio n. 2
0
    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])

Esempio n. 3
0
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)
Esempio n. 4
0
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))
Esempio n. 5
0
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)
Esempio n. 6
0
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
Esempio n. 7
0
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()
Esempio n. 8
0
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))

Esempio n. 9
0
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)
Esempio n. 10
0
    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)
Esempio n. 11
0
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))