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_load():
t = Thread()
print "before:"
t.printVertices()
t.load(np.random.randn(3, 11), 2)
print "after:"
t.printVertices()
def test_load():
t = Thread()
print "before:"
t.printVertices()
t.load(np.random.randn(3,11),2)
print "after:"
t.printVertices()
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_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_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())
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)
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 doo.doo import Thread
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
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
T = 15
BEND = pi/6
TWIST = 2*pi
thread = Thread()
x,y,z = thread.getXYZ()
#mlab.plot3d(x,y,z,tube_radius=.4,color=(1,1,1))
mlab.figure(1); mlab.clf()
for t in xrange(T):
twists.twist_around_axis(thread,BEND/T,-BEND/T,[0,1,0])
x,y,z = thread.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.1,color=(1-t/T,0,t/T))
mlab.figure(2); mlab.clf()
for t in xrange(T):
#twists.twist_ctrl(thread,2*np.pi/T,0)
twists.twist_around_axis(thread,TWIST/T,TWIST/T,[1,0,0])
x,y,z = thread.getXYZ()
mlab.plot3d(x,y,z,tube_radius=.1,color=(1-t/T,0,t/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()
import sys
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()
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)
import mayavi.mlab as mlab import sys from doo import USCALE from mp.simplectrl2 import * import mp.plots as plots # seed with 0 and final config is the same # seed with 1 and it's way different ########## MAKE TARGET STATE T = 30 scale = 5 thread_start = Thread() cons_start = thread_start.getConstraints() 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()
import sys
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()
from numpy.random import randn
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))
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))
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
from doo.doo import Thread
from mp.utils import upsample2D,normr,almostEq,ndinterp,colorSeq
from simplectrl2 import getState,setOrs,applyTwist,infTwist
import plots
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)
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]
from numpy.random import randn import mayavi.mlab as mlab import sys from doo import USCALE from mp.simplectrl2 import * import mp.plots as plots # seed with 0 and final config is the same # seed with 1 and it's way different ########## MAKE TARGET STATE T = 30 scale = 5 thread_start = Thread() cons_start = thread_start.getConstraints() 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))
from doo import *
from doo.doo import Thread
from sqp import frPath, calcStates, evalDiff
from numpy.random import randn
from utils import upsample2D, colorSeq
T = 20
thread = Thread()
scale = .25
cons_start = thread.getConstraints()
cons_1 = cons_start + scale * np.r_[
randn(3) * 10, randn(3) * .5,
randn(3) * 10, randn(3) * .5]
cons_2 = cons_start + scale * np.r_[
randn(3) * 10, randn(3) * .5,
randn(3) * 10, randn(3) * .5]
cons_end = cons_start + scale * np.r_[
randn(3) * 10, randn(3) * .5,
randn(3) * 10, randn(3) * .5]
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:
thread.setConstraints(cons)
print "moving it along path 1->2->goal"
cons_t = upsample2D(np.array([cons_1, cons_2, cons_end]), T)
import mayavi.mlab as mlab
from doo import *
from doo.doo import Thread
from sqp import frPath,calcStates,evalDiff
from numpy.random import randn
from utils import upsample2D,colorSeq
T = 20
thread = Thread()
scale=.25
cons_start = thread.getConstraints()
cons_1 = cons_start + scale*np.r_[randn(3)*10,randn(3)*.5,randn(3)*10,randn(3)*.5]
cons_2 = cons_start + scale*np.r_[randn(3)*10,randn(3)*.5,randn(3)*10,randn(3)*.5]
cons_end = cons_start + scale*np.r_[randn(3)*10,randn(3)*.5,randn(3)*10,randn(3)*.5]
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:
thread.setConstraints(cons)
print "moving it along path 1->2->goal"
cons_t = upsample2D(np.array([cons_1,cons_2,cons_end]),T)
import mayavi.mlab as mlab
colors = colorSeq(100)
xyzs_fwd = []
for cons in cons_t[1:]:
thread.setConstraints(cons)
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
