# ME-314 - Final Project - Yael Ben Shalom

*In this project, I simulated the dynamics of a planar jack-in-a-box system. I simulated and animated the dynamics of the multi-body system in time, with external forces applied on the box and impact accrues between the two rigid bodies (the jack and the box’s walls).*

*Both bodies in the planar systems are in gravity (in the -y direction of the world frame). In addition, two external forces are applied to the box – the first is an equal and opposite force to gravity (to ensure the box stays at the same position), and the second is a torque to rotate the box. The applied torque has a sinusoidal form to make the box rotate back and forth in a constant frequency.*

***


```python
import numpy as np
import sympy as sym 
from sympy.abc import t
from sympy import sin, cos, symbols, Function, Inverse, Eq, Matrix, Rational, simplify, lambdify, solve
from math import pi
import matplotlib.pyplot as plt
```


```python
def mat_inverse(g):
    ''' A functions for ''invers'' operations.
    '''
    R = Matrix([[g[0,0], g[0,1], g[0,2]],
                [g[1,0], g[1,1], g[1,2]],
                [g[2,0], g[2,1], g[2,2]]])
    R_inv = R.T
    p = Matrix([g[0,3], g[1,3], g[2,3]])
    p_inv = -R_inv * p
    g_inv = Matrix([[R_inv[0,0], R_inv[0,1], R_inv[0,2], p_inv[0]],
                      [R_inv[1,0], R_inv[1,1], R_inv[1,2], p_inv[1]],
                      [R_inv[2,0], R_inv[2,1], R_inv[2,2], p_inv[2]],
                      [         0,          0,         0,        1]])
    return g_inv


def unhat(g):
    ''' A functions for ''unhat'' operations.
    '''
    V = Matrix([0, 0, 0, 0, 0, 0])
    V[0, 0] = g[0, 3]
    V[1, 0] = g[1, 3]
    V[2, 0] = g[2, 3]
    V[3, 0] = g[2, 1]
    V[4, 0] = g[0, 2]
    V[5, 0] = g[1, 0]
    
    return V


def get_se3_np(x, y, theta):
    """
    This function return SE(3) given x, y, theta
    """
    return np.array([[np.cos(theta), -np.sin(theta), 0, x],
                     [np.sin(theta),  np.cos(theta), 0, y],
                     [            0,              0, 1, 0],
                     [            0,              0, 0, 1]])
    
    
def integrate(f, xt, dt, time):
    """
    This function takes in an initial condition x(t) and a timestep dt,
    as well as a dynamical system f(x) that outputs a vector of the
    same dimension as x(t). It outputs a vector x(t+dt) at the future
    time step.
    
    Parameters
    ============
    dyn: Python function
        derivate of the system at a given step x(t), 
        it can considered as \dot{x}(t) = func(x(t))
    x0: NumPy array
        current step x(t)
    dt: 
        step size for integration
    time:
        step time
        
    Return
    ============
    new_x: 
        value of x(t+dt) integrated from x(t)
    """
    k1 = dt * f(xt, time)
    k2 = dt * f(xt+k1/2., time)
    k3 = dt * f(xt+k2/2., time)
    k4 = dt * f(xt+k3, time)
    new_xt = xt + (1/6.) * (k1+2.0*k2+2.0*k3+k4)

    return new_xt


def simulate_impact(f, x0, tspan, dt, integrate):
    """
    This function takes in an initial condition x0, a timestep dt,
    a time span tspan consisting of a list [min_time, max_time],
    as well as a dynamical system f(x) that outputs a vector of the
    same dimension as x0. It outputs a full trajectory simulated
    over the time span of dimensions (xvec_size, time_vec_size).
    
    Parameters
    ============
    f: Python function
        derivate of the system at a given step x(t), 
        it can considered as \dot{x}(t) = func(x(t))
    x0: NumPy array
        initial conditions
    tspan: Python list
        tspan = [min_time, max_time], it defines the start and end
        time of simulation
    dt:
        time step for numerical integration
    integrate: Python function
        numerical integration method used in this simulation

    Return
    ============
    x_traj:
        simulated trajectory of x(t) from t=0 to tf
    """
    N = int((max(tspan)-min(tspan))/dt)
    x = np.copy(x0)
    tvec = np.linspace(min(tspan),max(tspan), N)
    xtraj = np.zeros((len(x0), N))
    time = 0
    for i in range(N):
        time = time + dt
        (impact, impact_num) = impact_condition(x, phi_func, 1e-1)
        if impact is True:
            x = impact_update(x, impact_eqns_list[impact_num], dum_list)
            xtraj[:, i]=integrate(f, x, dt, time)
        else:
            xtraj[:, i]=integrate(f, x, dt, time)
        x = np.copy(xtraj[:,i]) 
    return xtraj
```


```python
# Box parameters:
l_box, M_box = 6, 100
j_box = 4*M_box*l_box**2

# Jack parameters:
l_jack, m_jack = 1, 1
j_jack = 4*m_jack*l_jack**2

# General parameters:
# g = 0 # Without gravity
g = 9.81 # With gravity

# Define box and jack's variable:
x_box = Function(r'x_box')(t)
y_box = Function(r'y_box')(t)
theta_box = Function(r'\theta_box')(t)
x_jack = Function(r'x_jack')(t)
y_jack = Function(r'y_jack')(t)
theta_jack = Function(r'\theta_jack')(t)

# Define the external force:
theta_d_box = sin(2*pi*t/5)
k = 30000
F_y_box = 4*M_box*g
F_theta_box = k*(theta_d_box)
F = Matrix([0, F_y_box, F_theta_box, 0, 0, 0])

# Define the configuration vector:
q = Matrix([x_box, y_box, theta_box, x_jack, y_jack, theta_jack])
qdot = q.diff(t)
qddot = qdot.diff(t)

# Finding the homogeneous representations of the rigid bodies:
g_w_b = Matrix([[cos(theta_box), -sin(theta_box), 0, x_box], [sin(theta_box), cos(theta_box), 0, y_box], [0, 0, 1, 0], [0, 0, 0, 1]])
g_b_b1 = Matrix([[1, 0, 0, l_box], [0, 1, 0, l_box], [0, 0, 1, 0], [0, 0, 0, 1]])
g_b_b2 = Matrix([[1, 0, 0, 0], [0, 1, 0, -l_box], [0, 0, 1, 0], [0, 0, 0, 1]])
g_b_b3 = Matrix([[1, 0, 0, -l_box], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
g_b_b4 = Matrix([[1, 0, 0, 0], [0, 1, 0, l_box], [0, 0, 1, 0], [0, 0, 0, 1]])
g_w_j = Matrix([[cos(theta_jack), -sin(theta_jack), 0, x_jack], [sin(theta_jack), cos(theta_jack), 0, y_jack], [0, 0, 1, 0], [0, 0, 0, 1]])
g_j_j1 = Matrix([[1, 0, 0, l_jack], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
g_j_j2 = Matrix([[1, 0, 0, 0], [0, 1, 0, -l_jack], [0, 0, 1, 0], [0, 0, 0, 1]])
g_j_j3 = Matrix([[1, 0, 0, -l_jack], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
g_j_j4 = Matrix([[1, 0, 0, 0], [0, 1, 0, l_jack], [0, 0, 1, 0], [0, 0, 0, 1]])

g_w_b1 = g_w_b * g_b_b1
g_w_b2 = g_w_b * g_b_b2
g_w_b3 = g_w_b * g_b_b3
g_w_b4 = g_w_b * g_b_b4

g_w_j1 = g_w_j * g_j_j1
g_w_j2 = g_w_j * g_j_j2
g_w_j3 = g_w_j * g_j_j3
g_w_j4 = g_w_j * g_j_j4

g_b1_j1 = mat_inverse(g_w_b1) * g_w_j1
g_b1_j2 = mat_inverse(g_w_b1) * g_w_j2
g_b1_j3 = mat_inverse(g_w_b1) * g_w_j3
g_b1_j4 = mat_inverse(g_w_b1) * g_w_j4

g_b2_j1 = mat_inverse(g_w_b2) * g_w_j1
g_b2_j2 = mat_inverse(g_w_b2) * g_w_j2
g_b2_j3 = mat_inverse(g_w_b2) * g_w_j3
g_b2_j4 = mat_inverse(g_w_b2) * g_w_j4

g_b3_j1 = mat_inverse(g_w_b3) * g_w_j1
g_b3_j2 = mat_inverse(g_w_b3) * g_w_j2
g_b3_j3 = mat_inverse(g_w_b3) * g_w_j3
g_b3_j4 = mat_inverse(g_w_b3) * g_w_j4

g_b4_j1 = mat_inverse(g_w_b4) * g_w_j1
g_b4_j2 = mat_inverse(g_w_b4) * g_w_j2
g_b4_j3 = mat_inverse(g_w_b4) * g_w_j3
g_b4_j4 = mat_inverse(g_w_b4) * g_w_j4

# Calculate the rigid body velocities for the box and the jack:
V_box = unhat(mat_inverse(g_w_b) * g_w_b.diff(t))
V_jack = unhat(mat_inverse(g_w_j) * g_w_j.diff(t))

# Calculate the body inertia matrix:
I_box = Matrix([[4*M_box, 0, 0, 0, 0, 0], [0, 4*M_box, 0, 0, 0, 0], [0 ,0 ,4*M_box ,0 ,0 ,0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, j_box]])
I_jack = Matrix([[4*m_jack, 0, 0, 0, 0, 0], [0, 4*m_jack, 0, 0, 0, 0], [0, 0, 4*m_jack, 0, 0, 0], [0, 0, 0, 0, 0, 0], [0, 0, 0 ,0, 0, 0], [0, 0, 0, 0, 0, j_jack]])

# Calculate the Lagrangian:
KE = simplify(0.5*(V_box.T)*I_box*V_box + 0.5*(V_jack.T)*I_jack*V_jack)[0]
PE = simplify(g*(4*M_box*y_box + 4*m_jack*y_jack))
L = simplify(KE - PE)

# Compute the Euler-Lagrange Equations:
dL_dq = simplify(Matrix([L]).jacobian(q).T)
dL_dqdot = simplify(Matrix([L]).jacobian(qdot).T)
ddL_dqdot_dt = simplify(dL_dqdot.diff(t))

lhs = simplify(ddL_dqdot_dt - dL_dq)
rhs = simplify(F)
EL_Eqs = simplify(Eq(lhs, rhs))
display(EL_Eqs)

# Solve the Euler-Lagrange Equations:
EL_solns = solve(EL_Eqs, qddot, dict=True)
```


```python
x_box_ddot_func = lambdify([q[0], q[1], q[2], q[3], q[4], q[5], qdot[0], qdot[1], qdot[2], qdot[3], qdot[4], qdot[5], t], EL_solns[0][qddot[0]])
y_box_ddot_func = lambdify([q[0], q[1], q[2], q[3], q[4], q[5], qdot[0], qdot[1], qdot[2], qdot[3], qdot[4], qdot[5], t], EL_solns[0][qddot[1]])
theta_box_ddot_func = lambdify([q[0], q[1], q[2], q[3], q[4], q[5], qdot[0], qdot[1], qdot[2], qdot[3], qdot[4], qdot[5], t], EL_solns[0][qddot[2]])
x_jack_ddot_func = lambdify([q[0], q[1], q[2], q[3], q[4], q[5], qdot[0], qdot[1], qdot[2], qdot[3], qdot[4], qdot[5], t], EL_solns[0][qddot[3]])
y_jack_ddot_func = lambdify([q[0], q[1], q[2], q[3], q[4], q[5], qdot[0], qdot[1], qdot[2], qdot[3], qdot[4], qdot[5], t], EL_solns[0][qddot[4]])
theta_jack_ddot_func = lambdify([q[0], q[1], q[2], q[3], q[4], q[5], qdot[0], qdot[1], qdot[2], qdot[3], qdot[4], qdot[5], t], EL_solns[0][qddot[5]])

def dynamics(s, t):
    sdot = np.array([
        s[6],
        s[7],
        s[8],
        s[9],
        s[10],
        s[11],
        x_box_ddot_func(*s, t),
        y_box_ddot_func(*s, t),
        theta_box_ddot_func(*s, t),
        x_jack_ddot_func(*s, t),
        y_jack_ddot_func(*s, t),
        theta_jack_ddot_func(*s, t)
    ])
    return sdot
```


```python
# Define acceleration matrix:
qddot_Matrix = Matrix([qdot[0], EL_solns[0][qddot[0]],
                       qdot[1], EL_solns[0][qddot[1]],
                       qdot[2], EL_solns[0][qddot[2]],
                       qdot[3], EL_solns[0][qddot[3]],
                       qdot[4], EL_solns[0][qddot[4]],
                       qdot[5], EL_solns[0][qddot[5]]])

# Define dummy symbols:
x_b_l, y_b_l, theta_b_l, x_j_l, y_j_l, theta_j_l, x_b_ldot, y_b_ldot, theta_b_ldot, x_j_ldot, y_j_ldot, theta_j_ldot = symbols('x_box_l, y_box_l, theta_box_l, x_jack_l, y_jack_l, theta_jack_l, x_box_ldot, y_box_ldot, theta_box_ldot, x_jack_ldot, y_jack_ldot, theta_jack_ldot')

dummy_dict = {q[0]:x_b_l, q[1]:y_b_l, q[2]:theta_b_l,
              q[3]:x_j_l, q[4]:y_j_l, q[5]:theta_j_l,
              qdot[0]:x_b_ldot, qdot[1]:y_b_ldot, qdot[2]:theta_b_ldot,
              qdot[3]:x_j_ldot, qdot[4]:y_j_ldot, qdot[5]:theta_j_ldot}
qddot_d = qddot_Matrix.subs(dummy_dict)
qddot_lambdify = lambdify([x_b_l, x_b_ldot ,y_b_l, y_b_ldot, theta_b_l, theta_b_ldot,
                           x_j_l, x_j_ldot ,y_j_l, y_j_ldot, theta_j_l, theta_j_ldot, t], qddot_d)
```


```python
r_jack_hat = Matrix([x_jack, y_jack, theta_jack, 1])

# Define impact constraint for wall 1:
phi_b1_j1 = (g_b1_j1[3]).subs(dummy_dict)
phi_b1_j2 = (g_b1_j2[3]).subs(dummy_dict)
phi_b1_j3 = (g_b1_j3[3]).subs(dummy_dict)
phi_b1_j4 = (g_b1_j4[3]).subs(dummy_dict)

# Define impact constraint for wall 2:
phi_b2_j1 = (g_b2_j1[7]).subs(dummy_dict)
phi_b2_j2 = (g_b2_j2[7]).subs(dummy_dict)
phi_b2_j3 = (g_b2_j3[7]).subs(dummy_dict)
phi_b2_j4 = (g_b2_j4[7]).subs(dummy_dict)

# Define impact constraint for wall 3:
phi_b3_j1 = (g_b3_j1[3]).subs(dummy_dict)
phi_b3_j2 = (g_b3_j2[3]).subs(dummy_dict)
phi_b3_j3 = (g_b3_j3[3]).subs(dummy_dict)
phi_b3_j4 = (g_b3_j4[3]).subs(dummy_dict)

# Define impact constraint for wall 4:
phi_b4_j1 = (g_b4_j1[7]).subs(dummy_dict)
phi_b4_j2 = (g_b4_j2[7]).subs(dummy_dict)
phi_b4_j3 = (g_b4_j3[7]).subs(dummy_dict)
phi_b4_j4 = (g_b4_j4[7]).subs(dummy_dict)

# Define impact constraint:
phi_dum = simplify(Matrix([[phi_b1_j1], [phi_b1_j2], [phi_b1_j3], [phi_b1_j4],  # all jack frames in the box1 frame
                           [phi_b2_j1], [phi_b2_j2], [phi_b2_j3], [phi_b2_j4],  # all jack frames in the box2 frame
                           [phi_b3_j1], [phi_b3_j2], [phi_b3_j3], [phi_b3_j4],  # all jack frames in the box3 frame
                           [phi_b4_j1], [phi_b4_j2], [phi_b4_j3], [phi_b4_j4]])) # all jack frames in the box4 frame

# Compute the Hamiltonian:
H = simplify((dL_dqdot.T * qdot)[0] - L)

# Compute expressions:
H_dum = H.subs(dummy_dict)
dL_dqdot_dum = dL_dqdot.subs(dummy_dict)
dPhidq_dum = phi_dum.jacobian([x_b_l, y_b_l, theta_b_l, x_j_l, y_j_l, theta_j_l])

# Define dummy symbols for tau+:
lamb = symbols(r'lambda')
x_b_dot_Plus, y_b_dot_Plus, theta_b_dot_Plus, x_j_dot_Plus, y_j_dot_Plus, theta_j_dot_Plus = symbols(r'x_box_dot_+, y_box_dot_+, theta_box_dot_+, x_jack_dot_+, y_jack_dot_+, theta_jack_dot_+')

impact_dict = {x_b_ldot:x_b_dot_Plus, y_b_ldot:y_b_dot_Plus, theta_b_ldot:theta_b_dot_Plus,
               x_j_ldot:x_j_dot_Plus, y_j_ldot:y_j_dot_Plus, theta_j_ldot:theta_j_dot_Plus}

# Evaluate expressions at tau+:
dL_dqdot_dumPlus = simplify(dL_dqdot_dum.subs(impact_dict))
dPhidq_dumPlus = simplify(dPhidq_dum.subs(impact_dict))
H_dumPlus = simplify(H_dum.subs(impact_dict))

impact_eqns_list = []

# Define equations
lhs = Matrix([dL_dqdot_dumPlus[0] - dL_dqdot_dum[0],
              dL_dqdot_dumPlus[1] - dL_dqdot_dum[1],
              dL_dqdot_dumPlus[2] - dL_dqdot_dum[2],
              dL_dqdot_dumPlus[3] - dL_dqdot_dum[3],
              dL_dqdot_dumPlus[4] - dL_dqdot_dum[4],
              dL_dqdot_dumPlus[5] - dL_dqdot_dum[5],
              H_dumPlus - H_dum])

for i in range(phi_dum.shape[0]):
    rhs = Matrix([lamb*dPhidq_dum[i,0],
                  lamb*dPhidq_dum[i,1],
                  lamb*dPhidq_dum[i,2],
                  lamb*dPhidq_dum[i,3],
                  lamb*dPhidq_dum[i,4],
                  lamb*dPhidq_dum[i,5],
                  0])
    impact_eqns_list.append(simplify(Eq(lhs, rhs)))
```


```python
dum_list = [x_b_dot_Plus, y_b_dot_Plus, theta_b_dot_Plus,
            x_j_dot_Plus, y_j_dot_Plus, theta_j_dot_Plus]

def impact_update(s, impact_eqns, dum_list):
    """ This function updated the system after impact.
    It returns the uptadet s array after impact.
    """
    subs_dict = {x_b_l:s[0], y_b_l:s[1], theta_b_l:s[2],
                 x_j_l:s[3], y_j_l:s[4], theta_j_l:s[5],
                 x_b_ldot:s[6], y_b_ldot:s[7], theta_b_ldot:s[8],
                 x_j_ldot:s[9], y_j_ldot:s[10], theta_j_ldot:s[11]}
    new_impact_eqns = impact_eqns.subs(subs_dict)
    impact_solns = solve(new_impact_eqns, [x_b_dot_Plus, y_b_dot_Plus, theta_b_dot_Plus,
                                           x_j_dot_Plus, y_j_dot_Plus, theta_j_dot_Plus,
                                           lamb], dict=True)
    if len(impact_solns) == 1:
        print("Damn, only one solution")
        
    else:
        for sol in impact_solns:
            lamb_sol = sol[lamb]
            if abs(lamb_sol) < 1e-06:
                pass
            else:
                return np.array([
                    s[0], #q will be the same after impact
                    s[1],
                    s[2],
                    s[3],
                    s[4],
                    s[5],
                    float(sym.N(sol[dum_list[0]])), #q_dot will change after impact
                    float(sym.N(sol[dum_list[1]])),
                    float(sym.N(sol[dum_list[2]])),
                    float(sym.N(sol[dum_list[3]])),
                    float(sym.N(sol[dum_list[4]])),
                    float(sym.N(sol[dum_list[5]])),
                ])
```


```python
phi_func = lambdify([x_b_l, y_b_l, theta_b_l,
                     x_j_l, y_j_l, theta_j_l,
                     x_b_ldot, y_b_ldot, theta_b_ldot,
                     x_j_ldot, y_j_ldot, theta_j_ldot],
                    phi_dum)

def impact_condition(s, phi_func, threshold = 1e-1):
    """ This function checks the systems for impact.
    In the case of an impact (abs(phi_val) < threshold),
    the function returns True and the row number of the
    phi matrix in which the impact accured.
    """
    phi_val = phi_func(*s)
    for i in range(phi_val.shape[0]):
        if (phi_val[i] > -threshold) and (phi_val[i] < threshold):
            return (True, i)
    return (False, None)

# print('term impact condition function:', impact_condition(s_test, phi_func, 0.01))
```


```python
# Simulate the motion:
tspan = [0, 10]
dt = 0.01
s0 = np.array([0, 0, 0, 0, 0, 0, 0, 0, -1.6, 0, 0, 0])

N = int((max(tspan) - min(tspan))/dt)
tvec = np.linspace(min(tspan), max(tspan), N)
traj = simulate_impact(dynamics, s0, tspan, dt, integrate)

plt.figure()
plt.plot(tvec, traj[0], label='x_box')
plt.plot(tvec, traj[1], label='y_box')
plt.plot(tvec, traj[2], label='theta_box')
plt.title('Box Motion Simulation')
plt.xlabel('t')
plt.ylabel('state')
plt.legend(loc="best")
plt.show()

plt.figure()
plt.plot(tvec, traj[3], label='x_jack')
plt.plot(tvec, traj[4], label='y_jack')
plt.plot(tvec, traj[5], label='theta_jack')
plt.title('Jack Motion Simulation')
plt.xlabel('t')
plt.ylabel('state')
plt.legend(loc="best")
plt.show()

plt.figure()
plt.plot(tvec, traj[6], label='x_box_dot')
plt.plot(tvec, traj[7], label='y_box_dot')
plt.plot(tvec, traj[8], label='theta_box_dot')
plt.title('Box Velocity Simulation')
plt.xlabel('t')
plt.ylabel('state')
plt.legend(loc="best")
plt.show()

plt.figure()
plt.plot(tvec, traj[9], label='x_jack_dot')
plt.plot(tvec, traj[10], label='y_jack_dot')
plt.plot(tvec, traj[11], label='theta_jack_dot')
plt.title('Jack Velocity Simulation')
plt.xlabel('t')
plt.ylabel('state')
plt.legend(loc="best")
plt.show()
```


```python
def animate_jack_in_a_box(config_array,l=1,w=0.2,T=10):
    """
    Function to generate web-based animation of the system

    Parameters:
    ================================================
    config_array:
        trajectory of theta1 and theta2, should be a NumPy array with
        shape of (2,N)
    L1:
        length of the first pendulum
    L2:
        length of the second pendulum
    T:
        length/seconds of animation duration

    Returns: None
    """
    ################################
    # Imports required for animation. (leave this part)
    from plotly.offline import init_notebook_mode, iplot
    from IPython.display import display, HTML
    import plotly.graph_objects as go

    #######################
    # Browser configuration. (leave this part)
    def configure_plotly_browser_state():
        import IPython
        display(IPython.core.display.HTML('''
            
            
            '''))
    configure_plotly_browser_state()
    init_notebook_mode(connected=False)

    
    ###############################################
    # Getting data from pendulum angle trajectories.
    N = len(config_array[0])
    
    x_box_array = config_array[0]
    y_box_array = config_array[1]
    theta_box_array = config_array[2]
    x_jack_array = config_array[3]
    y_jack_array = config_array[4]
    theta_jack_array = config_array[5]

    b1_x_array = np.zeros(N, dtype=np.float32)
    b1_y_array = np.zeros(N, dtype=np.float32)
    b2_x_array = np.zeros(N, dtype=np.float32)
    b2_y_array = np.zeros(N, dtype=np.float32)
    b3_x_array = np.zeros(N, dtype=np.float32)
    b3_y_array = np.zeros(N, dtype=np.float32)
    b4_x_array = np.zeros(N, dtype=np.float32)
    b4_y_array = np.zeros(N, dtype=np.float32)
    
    j_x_array = np.zeros(N, dtype=np.float32)
    j_y_array = np.zeros(N, dtype=np.float32)
    j1_x_array = np.zeros(N, dtype=np.float32)
    j1_y_array = np.zeros(N, dtype=np.float32)
    j2_x_array = np.zeros(N, dtype=np.float32)
    j2_y_array = np.zeros(N, dtype=np.float32)
    j3_x_array = np.zeros(N, dtype=np.float32)
    j3_y_array = np.zeros(N, dtype=np.float32)
    j4_x_array = np.zeros(N, dtype=np.float32)
    j4_y_array = np.zeros(N, dtype=np.float32)
    
    for t in range(N):
        g_w_b = get_se3_np(x_box_array[t], y_box_array[t], theta_box_array[t])
        g_w_j = get_se3_np(x_jack_array[t], y_jack_array[t], theta_jack_array[t])

        b1 = g_w_b.dot(np.array([l_box, l_box, 0, 1]))
        b1_x_array[t] = b1[0]
        b1_y_array[t] = b1[1]
        b2 = g_w_b.dot(np.array([l_box, -l_box, 0, 1]))
        b2_x_array[t] = b2[0]
        b2_y_array[t] = b2[1]
        b3 = g_w_b.dot(np.array([-l_box, -l_box, 0, 1]))
        b3_x_array[t] = b3[0]
        b3_y_array[t] = b3[1]
        b4 = g_w_b.dot(np.array([-l_box, l_box, 0, 1]))
        b4_x_array[t] = b4[0]
        b4_y_array[t] = b4[1]    
        
        j = g_w_j.dot(np.array([0, 0, 0, 1]))
        j_x_array[t] = j[0]
        j_y_array[t] = j[1]
        j1 = g_w_j.dot(np.array([l_jack, 0, 0, 1]))
        j1_x_array[t] = j1[0]
        j1_y_array[t] = j1[1]
        j2 = g_w_j.dot(np.array([0, -l_jack, 0, 1]))
        j2_x_array[t] = j2[0]
        j2_y_array[t] = j2[1]
        j3 = g_w_j.dot(np.array([-l_jack, 0, 0, 1]))
        j3_x_array[t] = j3[0]
        j3_y_array[t] = j3[1]
        j4 = g_w_j.dot(np.array([0, l_jack, 0, 1]))
        j4_x_array[t] = j4[0]
        j4_y_array[t] = j4[1]    
    
    ####################################
    # Axis limits.
    xm = -13
    xM = 13
    ym = -13
    yM = 13

    ###########################
    # Defining data dictionary.
    data=[dict(name = 'Box'),
          dict(name = 'Jack'),
          dict(name = 'Mass1_Jack'),
        ]
    
    ################################
    # Preparing simulation layout.
    layout=dict(xaxis=dict(range=[xm, xM], autorange=False, zeroline=False,dtick=1),
                yaxis=dict(range=[ym, yM], autorange=False, zeroline=False,scaleanchor = "x",dtick=1),
                title='Jack in a Box Simulation', 
                hovermode='closest',
                updatemenus= [{'type': 'buttons',
                               'buttons': [{'label': 'Play','method': 'animate',
                                            'args': [None, {'frame': {'duration': T, 'redraw': False}}]},
                                           {'args': [[None], {'frame': {'duration': T, 'redraw': False}, 'mode': 'immediate',
                                            'transition': {'duration': 0}}],'label': 'Pause','method': 'animate'}
                                          ]
                              }]
               )
    
    ########################################
    # Defining the frames of the simulation.
    frames=[dict(data=[
            dict(x=[b1_x_array[k],b2_x_array[k],b3_x_array[k],b4_x_array[k],b1_x_array[k]], 
                 y=[b1_y_array[k],b2_y_array[k],b3_y_array[k],b4_y_array[k],b1_y_array[k]], 
                 mode='lines',
                 line=dict(color='blue', width=3)
                 ),
            dict(x=[j1_x_array[k],j3_x_array[k],j_x_array[k],j2_x_array[k],j4_x_array[k]],
                 y=[j1_y_array[k],j3_y_array[k],j_y_array[k],j2_y_array[k],j4_y_array[k]],
                 mode='lines',
                 line=dict(color='green', width=3)
                 ),
            go.Scatter(
                x=[j1_x_array[k],j2_x_array[k],j3_x_array[k],j4_x_array[k]],
                y=[j1_y_array[k],j2_y_array[k],j3_y_array[k],j4_y_array[k]],
                mode="markers",
                marker=dict(color='darkgreen', size=6)),
                 ]) for k in range(N)]
    
    #######################################
    # Putting it all together and plotting.
    figure1=dict(data=data, layout=layout, frames=frames)           
    iplot(figure1)

##############
# The animation:
animate_jack_in_a_box(traj)
```


```python

```

c0b7262eb2f9d36294e9b8bee60af8a2652be6f9
ipynb
Jupyter Notebook
code/final project.ipynb
YaelBenShalom/Simulating-Jack-in-a-Box
ee3efbc974374f1ea624a05efd23923732d3652b
MIT
code/final project.ipynb
YaelBenShalom/Simulating-Jack-in-a-Box
ee3efbc974374f1ea624a05efd23923732d3652b
MIT
code/final project.ipynb
YaelBenShalom/Simulating-Jack-in-a-Box
ee3efbc974374f1ea624a05efd23923732d3652b
MIT
Qwen/Qwen-72B
1. YES
2. YES