def drive(Diagram, varList, angleDict):
# Declare nodes
# n1 = Segment(sp.Symbol('x_1'),sp.Symbol('y_1'),sp.Symbol('xd_1'),sp.Symbol('yd_1'),sp.Symbol('th_1'),sp.Symbol('thd_1'),"1", 1.0,1.0,0.5,1.0,True)
# n2 = Segment(sp.Symbol('x_2'),sp.Symbol('y_2'),sp.Symbol('xd_2'),sp.Symbol('yd_2'),sp.Symbol('th_2'),sp.Symbol('thd_2'),"2", 1.0,1.0,0.5,1.0,False)
# Organize nodes into a diagram
# Diagram = {n1:[n2], n2:[n1]}
# Rewrite the coordinates of the nodes in terms of the
# generalized coordinates
# print Diagram
# print angleDict
head = find_head(Diagram)
dfs(Diagram, head)
# print Diagram
#
# for node in Diagram.keys():
# print node.__dict__
#
# print "*****************"
# print n2.y
# print n2.yd
# print "*****************"
# print ""
# Compute the lagrangian
dgm = [n for n in Diagram.keys()]
# print n1.x, "\n", n1.xd
L = sum([lagrange_01(n) for n in dgm])
L = sp.simplify(sp.expand(L))
usedSymbols = L.atoms(sp.Symbol)
coords = filter(lambda x: x[0] in usedSymbols or x[0] in usedSymbols, varList)
# print coords
# print angleDict
# print calcEL(L, coords)
lagrange = calcEL(L, coords)
initialConditions = [(angleDict[q], 0) for (q, qdot) in coords]
trajectory = numTimeEvolve(L, coords, initialConditions, np.linspace(0, 1000, 4000))
return trajectory, lagrange
#print "stuff"
# Organize nodes into a diagram
Diagram = {n1:[n2], n2:[n1, n3], n3:[n2, n4], n4:[n3,n5], n5:[n4,n6], n6:[n5]}
# Rewrite the coordinates of the nodes in terms of the
# generalized coordinates
head = find_head(Diagram)
dfs(Diagram, head)
#print "*****************"
#print n2.xd
#print n2.yd
#print "*****************"
#print ""
#Compute the lagrangian
dgm = [n1, n2, n3, n4, n5, n6]
#print n1.x, "\n", n1.xd
L = sum([lagrange_01(n) for n in dgm])
L = sp.simplify(sp.expand(L))
print "\n\n\nL is \n", L, "\n\n\n"
# Compute and simulate differential equatoins
tAxis=np.linspace(0,50,10000)
(x,y,xdot,ydot)=sp.symbols(['th_1','th_2','thd_1','thd_2'])
EL = calcEL(L, [(x,xdot),(y,ydot)])
print "EL \n", EL, "\n\n\n"
F, out=numTimeEvolve(L,[(x,xdot),(y,ydot)],[(0,0),(3,0)],tAxis)
print F
