def test_load2():
t1 = Thread()
t1.load(np.random.randn(3, 11), 2)
xyz = t1.getXYZ()
t2 = Thread()
t2.load(xyz, 0)
t1.minimizeEnergy()
xyz1 = t1.getXYZ()
t2.minimizeEnergy()
xyz2 = t2.getXYZ()
print xyz1 - xyz2
def test_load2():
t1 = Thread()
t1.load(np.random.randn(3,11),2)
xyz = t1.getXYZ()
t2 = Thread()
t2.load(xyz,0)
t1.minimizeEnergy()
xyz1 = t1.getXYZ()
t2.minimizeEnergy()
xyz2 = t2.getXYZ()
print xyz1-xyz2
def test_bend_ends():
thread = Thread()
mlab.figure(3); mlab.clf()
T = 16
colors = colorSeq(25)
for (t,cons) in enumerate(C.bend_ends(thread,1,.1)):
thread.setConstraints(cons)
x,y,z = thread.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.1,color=colors.next())
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_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)
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())
from mp.utils import upsample2D
import numpy as np
from numpy import pi
from numpy.random import randn
import mayavi.mlab as mlab
import sys
from nibabel.eulerangles import angle_axis2euler
import twists
BEND = pi/6
TWIST = 2*pi
thread = Thread()
xyz0 = thread.getXYZ()
#mlab.plot3d(x,y,z,tube_radius=.4,color=(1,1,1))
mlab.figure(1); mlab.clf()
T = 16
for (t,cons) in enumerate(twists.jumprope2(thread,pi)):
#twists.twist_ctrl(thread,2*np.pi/T,0)
thread.setConstraints(cons)
x,y,z = thread.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.1,color=(1-t/T,0,t/T))
xyz = np.array([x,y,z])
print twists.opt_rot(xyz,xyz0)
#x,y,z = tips
#mlab.points3d(x,y,z,color=(1,1,1))
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()
t.setConstraints(con+pert)
xyzs.append(t.getXYZ().flatten())
xyzs = array(xyzs)
cxyzs = xyzs - xyzs.mean(axis=0)
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))
for (i,thread,color) in zip(xrange(n_clones),threads,colors):
print "thread",i
cons_mid = cons_start + scale*np.r_[randn(3)*10,randn(3)*.5,randn(3)*10,randn(3)*.5]
cons_t = upsample2D(np.array([cons_start,cons_mid,cons_end]),T)
thread = threads[i]
for (t,cons) in enumerate(cons_t):
print t
thread.setConstraints(cons)
x,y,z = threads[i].getXYZ()
if t==T-1: tube_radius = .4
else: tube_radius = .1
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))
for (i, thread, color) in zip(xrange(n_clones), threads, colors):
print "thread", i
cons_mid = cons_start + scale * np.r_[randn(3) * 10,
randn(3) * .5,
randn(3) * 10,
randn(3) * .5]
cons_t = upsample2D(np.array([cons_start, cons_mid, cons_end]), T)
thread = threads[i]
for (t, cons) in enumerate(cons_t):
print t
thread.setConstraints(cons)
"""
Based on expt_fancy_twist, but now we're using the controllers module
"""
from __future__ import division
from doo.doo import Thread
from mp.utils import colorSeq
import numpy as np
from numpy import pi,r_
from numpy.random import randn
import mayavi.mlab as mlab
from simplectrl import makeArch
from time import sleep
from itertools import izip
if __name__ == "__main__":
thread = Thread()
mlab.figure(1); mlab.clf()
T = 20
colors = colorSeq(100)
for (t,cons) in izip(xrange(T),makeArch(thread,r_[0,0,0],r_[0,10,0],r_[0,0,5],0)):
thread.setConstraints(cons)
x,y,z = thread.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.1,color=colors.next())
sleep(.1)
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)
mlab.clf()
if __name__ == "__main__":
colors = colorSeq(100)
T = 30
center = r_[0,0,0]
mainax = r_[10,0,0]
up = r_[0,0,1]
elev = pi/3
thread = Thread()
x,y,z = xyz = thread.getXYZ()
p1_arch = center - mainax
p2_arch = center + mainax
v1_arch = normr(mainax + normr(up)*norm(mainax)*tan(elev))
v2_arch = normr(mainax - normr(up)*norm(mainax)*tan(elev))
p1_now,v1_now,p2_now,v2_now = getState(thread)
p1s = upsample2D(array([p1_now,p1_arch]),T,1)
p2s = upsample2D(array([p2_now,p2_arch]),T,1)
v1s = upsample2D(array([v1_now,v1_arch]),T,1)
v2s = upsample2D(array([v2_now,v2_arch]),T,1)
for (p1,p2,v1,v2) in zip(p1s,p2s,v1s,v2s):
col = colors.next()
