diff --git a/illustrative_example_for_viz/README.rst b/illustrative_example_for_viz/README.rst new file mode 100644 index 0000000..604ef61 --- /dev/null +++ b/illustrative_example_for_viz/README.rst @@ -0,0 +1,25 @@ +To run and see this visualization working, you need to first install PyDyViz. + +Install Instructions for PyDyViz +================================ + +1) Get the source code from repository:: + + $ git clone https://github.com/PythonDynamics/pydy-viz + +2) Add the directory to the PYTHONPATH:: + + $ export PYTHONPATH=$PYTHONPATH:/path/to/directory/pydy-viz + +3) Now you can run the example from this script:: + + $ python illustrative_example.py + + +In case there are any difficulties to get it working, Please feel free to contact our `mailing list`_. + +.. _mailing list: http://groups.google.com/group/pydy + + + + diff --git a/illustrative_example_for_viz/code_gen.py b/illustrative_example_for_viz/code_gen.py new file mode 100644 index 0000000..5206500 --- /dev/null +++ b/illustrative_example_for_viz/code_gen.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python + +from sympy import Dummy, lambdify +from sympy.external import import_module + +np = import_module('numpy') + +def numeric_right_hand_side(kane, parameters): + """Returns the right hand side of the first order ordinary differential + equations from a KanesMethod system which can be evaluated numerically. + + This function probably only works for simple cases (no dependent speeds, + specified functions). + + """ + + dynamic = kane._q + kane._u + dummy_symbols = [Dummy() for i in dynamic] + dummy_dict = dict(zip(dynamic, dummy_symbols)) + kindiff_dict = kane.kindiffdict() + + M = kane.mass_matrix_full.subs(kindiff_dict).subs(dummy_dict) + F = kane.forcing_full.subs(kindiff_dict).subs(dummy_dict) + + M_func = lambdify(dummy_symbols + parameters, M) + F_func = lambdify(dummy_symbols + parameters, F) + + def right_hand_side(x, t, args): + """Returns the derivatives of the states. + + Parameters + ---------- + x : ndarray, shape(n) + The current state vector. + t : float + The current time. + args : ndarray + The constants. + + Returns + ------- + dx : ndarray, shape(2 * (n + 1)) + The derivative of the state. + + """ + arguments = np.hstack((x, args)) + dx = np.array(np.linalg.solve(M_func(*arguments), + F_func(*arguments))).T[0] + + return dx + + return right_hand_side diff --git a/illustrative_example_for_viz/illustrative_example.py b/illustrative_example_for_viz/illustrative_example.py new file mode 100644 index 0000000..7a95b73 --- /dev/null +++ b/illustrative_example_for_viz/illustrative_example.py @@ -0,0 +1,32 @@ +#Export method calls and namespace +# from three_link_pendulum example .. + +from three_link_pendulum import I, O, links, particles, kane +from simulate import params, states, param_vals +from simulate import link_length, link_radius, particle_radius +from pydy_viz.shapes import Cylinder, Sphere +from pydy_viz.scene import Scene +from pydy_viz.visualization_frame import VisualizationFrame + +viz_frames = [] + +for i, (link, particle) in enumerate(zip(links, particles)): + + link_shape = Cylinder('cylinder{}'.format(i), radius=link_radius, + length=link_length, color='red') + viz_frames.append(VisualizationFrame('link_frame{}'.format(i), link, + link_shape)) + + particle_shape = Sphere('sphere{}'.format(i), radius=particle_radius, + color='blue') + viz_frames.append(VisualizationFrame('particle_frame{}'.format(i), + link.frame, particle, + particle_shape)) + +scene = Scene(I, O, *viz_frames) + +print('Generating transform time histories.') +scene.generate_visualization_json(kane._q + kane._u, params, states, + param_vals) +print('Done.') +scene.display() diff --git a/illustrative_example_for_viz/simulate.py b/illustrative_example_for_viz/simulate.py new file mode 100644 index 0000000..3df5cbf --- /dev/null +++ b/illustrative_example_for_viz/simulate.py @@ -0,0 +1,38 @@ +#This script produces the numerical integration for the EoMs +#generated by three_link_pendulum.py + +from three_link_pendulum import l, m, M, Ixx, Iyy, Izz, g, kane +from scipy.integrate import odeint +from code_gen import numeric_right_hand_side +from numpy import radians, linspace, hstack, zeros, ones + +params = list(l) + list(m) + list(M) + list(Ixx) + list(Iyy) + list(Izz) + [g] + +link_length = 10.0 # meters +link_mass = 10.0 # kg +link_radius = 0.5 # meters +link_ixx = 1.0 / 12.0 * link_mass * \ + (3 * link_radius ** 2 + link_length ** 2) +link_iyy = link_mass * link_radius ** 2 +link_izz = link_ixx + +particle_mass = 5.0 # kg +particle_radius = 1.0 # meters + +param_vals = [link_length for x in l] + \ + [particle_mass for x in m] + \ + [link_mass for x in M] + \ + [link_ixx for x in list(Ixx)] + \ + [link_iyy for x in list(Iyy)] + \ + [link_izz for x in list(Izz)] + \ + [9.8] + +print("Generating numeric right hand side.") +right_hand_side = numeric_right_hand_side(kane, params) + +t = linspace(0.0, 60.0, num=6000) +x0 = hstack((ones(6) * radians(10.0), zeros(6))) + +print("Integrating equations of motion.") +states = odeint(right_hand_side, x0, t, args=(param_vals,)) +print("Integration done.") diff --git a/illustrative_example_for_viz/three_link_pendulum.py b/illustrative_example_for_viz/three_link_pendulum.py new file mode 100644 index 0000000..a321b86 --- /dev/null +++ b/illustrative_example_for_viz/three_link_pendulum.py @@ -0,0 +1,129 @@ +#For this one we assume RigidBody links +from sympy import symbols +import sympy.physics.mechanics as me + +print("Defining the problem.") +#Number of links = 3 +N_links = 3 + +#Number of masses = 3 +N_bobs = 3 + +#Defining Dynamic Symbols ................ + +#Generalized coordinates(angular) ... + +alpha = me.dynamicsymbols('alpha:{}'.format(N_links)) +beta = me.dynamicsymbols('beta:{}'.format(N_links)) + +#Generalized speeds(angular) ... +omega = me.dynamicsymbols('omega:{}'.format(N_links)) +delta = me.dynamicsymbols('delta:{}'.format(N_links)) + +#Mass of each bob: +m = symbols('m:' + str(N_bobs)) + +#Length and mass of each link .. +l = symbols('l:' + str(N_links)) +M = symbols('M:' + str(N_links)) +#For storing Inertia for each link : +Ixx = symbols('Ixx:' + str(N_links)) +Iyy = symbols('Iyy:' + str(N_links)) +Izz = symbols('Izz:' + str(N_links)) + +#gravity and time .... +g, t = symbols('g t') + +#Now defining an Inertial ReferenceFrame First .... + +I = me.ReferenceFrame('I') + +#And some other frames ... + +A = me.ReferenceFrame('A') +A.orient(I, 'Space', [alpha[0], beta[0], 0], 'ZXY') +B = me.ReferenceFrame('B') +B.orient(A, 'Space', [alpha[1], beta[1], 0], 'ZXY') +C = me.ReferenceFrame('C') +C.orient(B, 'Space', [alpha[2], beta[2], 0], 'ZXY') + +#Setting angular velocities of new frames ... +A.set_ang_vel(I, omega[0] * I.z + delta[0] * I.x) +B.set_ang_vel(I, omega[1] * I.z + delta[1] * I.x) +C.set_ang_vel(I, omega[2] * I.z + delta[2] * I.x) + +# An Origin point, with velocity = 0 +O = me.Point('O') +O.set_vel(I, 0) + +#Three more points, for masses .. +P1 = O.locatenew('P1', -l[0] * A.y) +P2 = P1.locatenew('P2', -l[1] * B.y) +P3 = P2.locatenew('P3', -l[2] * C.y) + +#Setting velocities of points with v2pt theory ... +P1.v2pt_theory(O, I, A) +P2.v2pt_theory(P1, I, B) +P3.v2pt_theory(P2, I, C) +points = [P1, P2, P3] + +Pa1 = me.Particle('Pa1', points[0], m[0]) +Pa2 = me.Particle('Pa2', points[1], m[1]) +Pa3 = me.Particle('Pa3', points[2], m[2]) +particles = [Pa1, Pa2, Pa3] + +#defining points for links(RigidBodies) +#Assuming CoM as l/2 ... +P_link1 = O.locatenew('P_link1', -l[0] / 2 * A.y) +P_link2 = P1.locatenew('P_link2', -l[1] / 2 * B.y) +P_link3 = P2.locatenew('P_link3', -l[2] / 2 * C.y) + +#setting velocities of these points with v2pt theory ... +P_link1.v2pt_theory(O, I, A) +P_link2.v2pt_theory(P1, I, B) +P_link3.v2pt_theory(P2, I, C) + +points_rigid_body = [P_link1, P_link2, P_link3] + +#defining inertia tensors for links + +inertia_link1 = me.inertia(A, Ixx[0], Iyy[0], Izz[0]) +inertia_link2 = me.inertia(B, Ixx[1], Iyy[1], Izz[1]) +inertia_link3 = me.inertia(C, Ixx[2], Iyy[2], Izz[2]) + +#Defining links as Rigid bodies ... + +link1 = me.RigidBody('link1', P_link1, A, M[0], (inertia_link1, P_link1)) +link2 = me.RigidBody('link2', P_link2, B, M[1], (inertia_link2, P_link2)) +link3 = me.RigidBody('link3', P_link3, C, M[2], (inertia_link3, P_link3)) +links = [link1, link2, link3] + +#Applying forces on all particles , and adding all forces in a list.. +forces = [] +for particle in particles: + mass = particle.get_mass() + point = particle.get_point() + forces.append((point, -mass * g * I.y)) + +#Applying forces on rigidbodies .. +for link in links: + mass = link.get_mass() + point = link.get_masscenter() + forces.append((point, -mass * g * I.y)) + +kinematic_differentials = [] +for i in range(0, N_bobs): + kinematic_differentials.append(omega[i] - alpha[i].diff(t)) + kinematic_differentials.append(delta[i] - beta[i].diff(t)) + +#Adding particles and links in the same system ... +total_system = links + particles + +q = alpha + beta + +u = omega + delta + +print("Generating equations of motion.") +kane = me.KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kinematic_differentials) +fr, frstar = kane.kanes_equations(forces, total_system) +print("Derivation complete.") diff --git a/mass_spring/code_gen.py b/mass_spring/code_gen.py old mode 100755 new mode 100644 diff --git a/mass_spring/viz.py b/mass_spring/viz.py index b87147f..e96ee40 100644 --- a/mass_spring/viz.py +++ b/mass_spring/viz.py @@ -23,4 +23,6 @@ def visualize(coordinate_time): #we will replace the data in the js_data file js_data = re.sub("#\(coordinates_time\)", str(coordinate_time), js) + f = open('output.js','w') + f.write(js_data) display(Javascript(js_data)) diff --git a/three_link_pendulum/README b/three_link_pendulum/README new file mode 100644 index 0000000..e07eeb0 --- /dev/null +++ b/three_link_pendulum/README @@ -0,0 +1,2 @@ +This is a 3 link pendulum problem, assuming links as RigidBodies .. +I will add a fig soon .. diff --git a/three_link_pendulum/three_link_pendulum.py b/three_link_pendulum/three_link_pendulum.py new file mode 100644 index 0000000..169c1cd --- /dev/null +++ b/three_link_pendulum/three_link_pendulum.py @@ -0,0 +1,150 @@ +#For this one we assume RigidBody links +from sympy import symbols,sympify +from sympy.physics.mechanics import * + +#Number of links = 3 +N_links = 3 + +#Number of masses = 3 +N_bobs = 3 + +#Defining Dynamic Symbols ................ + +#Generalized coordinates(angular) ... + +alpha = dynamicsymbols('alpha1 alpha2 alpha3') +beta = dynamicsymbols('beta1 beta2 beta3') + +#Generalized speeds(angular) ... +alphad = dynamicsymbols('alpha1 alpha2 alpha3',1) +betad = dynamicsymbols('beta1 beta2 beta3',1) + +#Mass of each bob: +m = symbols('m:'+str(N_bobs)) + + +#Length and mass of each link .. +l = symbols('l:' + str(N_links)) +M = symbols('M:' + str(N_links)) +#For storing Inertia for each link : +Ixx = symbols('Ixx:'+str(N_links)) +Iyy = symbols('Iyy:'+str(N_links)) + +#gravity and time .... +g, t = symbols('g t') + + +#Now defining an Inertial ReferenceFrame First .... + +I = ReferenceFrame('I') + +#And some other frames ... + +A = ReferenceFrame('A') +A.orient(I,'Body',[alpha[0],beta[0],0],'ZXY') +B = ReferenceFrame('B') +B.orient(I,'Body',[alpha[1],beta[1],0],'ZXY') +C = ReferenceFrame('C') +C.orient(I,'Body',[alpha[2],beta[2],0],'ZXY') + + +#Setting angular velocities of new frames ... +A.set_ang_vel(I, alphad[0] * I.z + betad[0] * I.x) +B.set_ang_vel(I, alphad[1] * I.z + betad[1] * I.x) +C.set_ang_vel(I, alphad[2] * I.z + betad[2] * I.x) + + + +# An Origin point, with velocity = 0 +O = Point('O') +O.set_vel(I,0) + +#Three more points, for masses .. +P1 = O.locatenew('P1', l[0] * A.y) +P2 = O.locatenew('P2', l[1] * B.y) +P3 = O.locatenew('P3', l[2] * C.y) + +#Setting velocities of points with v2pt theory ... +P1.v2pt_theory(O, I, A) +P2.v2pt_theory(P1, I, B) +P3.v2pt_theory(P2, I, C) +points = [P1,P2,P3] + +Pa1 = Particle('Pa1', points[0], m[0]) +Pa2 = Particle('Pa2', points[1], m[1]) +Pa3 = Particle('Pa3', points[2], m[2]) +particles = [Pa1,Pa2,Pa3] + + + +#defining points for links(RigidBodies) +#Assuming CoM as l/2 ... +P_link1 = O.locatenew('P_link1', l[0]/2 * A.y) +P_link2 = O.locatenew('P_link1', l[1]/2 * B.y) +P_link3 = O.locatenew('P_link1', l[2]/2 * C.y) +#setting velocities of these points with v2pt theory ... +P_link1.v2pt_theory(O, I, A) +P_link2.v2pt_theory(P_link1, I, B) +P_link3.v2pt_theory(P_link2, I, C) + +points_rigid_body = [P_link1,P_link2,P_link3] + + +#defining inertia tensors for links + +inertia_link1 = inertia(A,Ixx[0],Iyy[0],0) +inertia_link2 = inertia(B,Ixx[1],Iyy[1],0) +inertia_link3 = inertia(C,Ixx[2],Iyy[2],0) + +#Defining links as Rigid bodies ... + +link1 = RigidBody('link1', P_link1, A, M[0], (inertia_link1, O)) +link2 = RigidBody('link2', P_link2, B, M[1], (inertia_link2, P1)) +link3 = RigidBody('link3', P_link3, C, M[2], (inertia_link3, P2)) +links = [link1,link2,link3] + + +#Applying forces on all particles , and adding all forces in a list.. +forces = [] +for particle in particles: + + mass = particle.get_mass() + point = particle.get_point() + forces.append((point, -mass * g * I.y) ) + +#Applying forces on rigidbodies .. +for link in links: + mass = link.get_mass() + point = link.get_masscenter() + forces.append((point, -mass * g * I.y) ) +kinetic_differentials = [] +for i in range(0,N_bobs): + kinetic_differentials.append(alphad[i] - alpha[i]) + kinetic_differentials.append(betad[i] - beta[i]) + +#Adding particles and links in the same system ... +total_system = [] +for particle in particles: + total_system.append(particle) + +for link in links: + total_system.append(link) + +q = [] +for angle in alpha: + q.append(angle) +for angle in beta: + q.append(angle) +print q +u = [] + +for vel in alphad: + u.append(vel) +for vel in betad: + u.append(vel) + +print u +kane = KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kinetic_differentials) +fr, frstar = kane.kanes_equations(forces, total_system) + +print fr