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)