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_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 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 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()
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)) #x,y,z = tips #mlab.points3d(x,y,z,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 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)
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)) #x,y,z = tips
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)) colors = colorSeq(100) x,y,z = thread.getXYZ()
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]
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)) colors = colorSeq(100) x, y, z = thread.getXYZ()