def compute_control_trajectory(q_q_dot_q_dot_dot):

    p, p_dot, p_dot_dot, theta, theta_dot, theta_dot_dot = q_q_dot_q_dot_dot

    num_timesteps = len(p)
    
    u = zeros((num_timesteps,2))

    for ti in range(int(num_timesteps)):

        q_ti         = matrix( [ p[ti,0],         p[ti,1],         theta[ti]         ] ).T
        q_dot_ti     = matrix( [ p_dot[ti,0],     p_dot[ti,1],     theta_dot[ti]     ] ).T
        q_dot_dot_ti = matrix( [ p_dot_dot[ti,0], p_dot_dot[ti,1], theta_dot_dot[ti] ] ).T
        x_ti         = bmat( [[q_ti], [q_dot_ti]] )

        H, C, G, B = compute_manipulator_matrices(x_ti)

        f1      = H.I*(-G-C*q_dot_ti)
        f2      = H.I*B
        f2_pinv = linalg.pinv(f2)
        u_ti    = f2_pinv*(f1 - q_dot_dot_ti)
        u[ti,:] = u_ti.T

    return u
dataset/ETHPy150Open stanford-gfx/Horus/Code/flashlight/quadrotor2d.py/compute_control_trajectory/swapped operands of `-`