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())
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))
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=(0,t/T,1-t/T))
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)
