Ball catcher

In [1]:

    

# Imports
from __future__ import division
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d.axes3d import Axes3D
from numpy.random import multivariate_normal as normal
from dynamics import *
from simulation import *
from optimization import *
from plotting import *

# Configuration
%matplotlib
np.set_printoptions(suppress=True, precision=4)


    

    




Using matplotlib backend: Qt4Agg

In [2]:

    

# Initialization
T_sim = 5.4
dt = T_sim/N_sim
[F, h, obs_cov_fcn, EKF, BF, EBF] = dynamics_init(dt)


    

In [8]:

    

ca.arctan2(0,1) * 180 / ca.pi


    

    
Out[8]:





0.0

In [ ]:

    

x0 = normal(mu0, S0)
[x_all, z_all] = simulate_trajectory(x0, u_, F, dt, h, obs_cov_fcn, N_sim)

# Plot
fig, ax = plt.subplots(figsize=(6,6))

# Trajectory
[traj_b, _] = plot_traj(ax, x_all)
# Ball observations
[obs_b] = plot_observations('ball observations', ax,
                            z_all[:,'x_b'], z_all[:,'y_b'])
# Catcher gaze
[arrows_c] = plot_arrows('gaze', ax, x_all[:,'x_c'],
                         x_all[:,'y_c'], x_all[:,'phi'], x_all[:,'theta'])
# Appearance
ax.set_title("A simulated trajectory")
ax.grid(True)
ax.legend(loc=4, handles=[traj_b, obs_b, arrows_c])
ax.set_aspect('auto')
#fig.tight_layout()


    

In [3]:

    

plan = create_simple_plan(m0, F, dt, N_sim)
x_all = plan.prefix['X']


    

    




******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
 Ipopt is released as open source code under the Eclipse Public License (EPL).
         For more information visit http://projects.coin-or.org/Ipopt
******************************************************************************

This is Ipopt version 3.11, running with linear solver ma57.

Number of nonzeros in equality constraint Jacobian...:      536
Number of nonzeros in inequality constraint Jacobian.:       30
Number of nonzeros in Lagrangian Hessian.............:      129

Total number of variables............................:      240
                     variables with only lower bounds:       15
                variables with lower and upper bounds:       45
                     variables with only upper bounds:        0
Total number of equality constraints.................:      180
Total number of inequality constraints...............:       15
        inequality constraints with only lower bounds:        0
   inequality constraints with lower and upper bounds:        0
        inequality constraints with only upper bounds:       15

iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
   0  5,3999892e-05 2,15e+01 5,13e-04  -1,0 0,00e+00    -  0,00e+00 0,00e+00   0
Reallocating memory for MA57: lfact (5773)
   1  1,3604670e+00 2,11e+01 5,81e-01  -1,0 3,64e+01    -  1,37e-02 1,61e-02h  1
   2  1,5134951e+00 2,11e+01 5,49e-01  -1,0 3,58e+01   0,0 3,26e-03 1,11e-03h  1
   3  1,9523733e+00 2,10e+01 7,67e-01  -1,0 3,58e+01   0,4 6,98e-03 2,43e-03h  1
   4  1,9524570e+00 2,10e+01 7,65e-01  -1,0 3,08e+02  -0,1 1,57e-04 1,07e-04h  1
   5  2,4940988e+00 2,10e+01 1,42e+00  -1,0 3,57e+01   0,4 8,35e-03 2,91e-03h  1
   6  4,4638129e+00 2,08e+01 2,14e+00  -1,0 3,56e+01   0,8 2,11e-04 7,20e-03h  1
   7  1,0102850e+01 2,02e+01 4,28e+00  -1,0 3,92e+01   0,3 5,24e-03 2,97e-02h  3
   8  1,4433702e+01 2,00e+01 6,45e+00  -1,0 3,43e+01   0,7 3,47e-02 1,15e-02h  1
   9  2,4702655e+01 1,97e+01 5,40e+01  -1,0 3,39e+01   1,2 2,28e-01 1,58e-02h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  10  1,1838161e+03 8,76e+00 5,86e+02  -1,0 3,33e+01   0,7 4,79e-02 5,55e-01h  1
  11  1,3574113e+03 7,93e+00 5,57e+02  -1,0 1,49e+01   1,1 1,50e-01 9,48e-02h  1
  12  2,1158212e+03 5,69e+00 7,35e+02  -1,0 1,34e+01   1,6 8,08e-03 2,82e-01h  1
  13  2,1475663e+03 5,52e+00 7,05e+02  -1,0 1,31e+01   1,1 3,74e-01 3,08e-02h  1
  14  2,6244017e+03 4,07e+00 2,32e+02  -1,0 9,35e+00   1,5 8,88e-01 2,61e-01h  1
  15  2,7026251e+03 2,01e+00 1,96e+02  -1,0 6,91e+00   1,0 6,55e-02 5,06e-01h  1
  16  2,7068139e+03 1,99e+00 1,51e+02  -1,0 3,41e+00   1,4 4,66e-01 1,31e-02h  1
  17  1,9946079e+03 4,50e-01 7,64e+01  -1,0 4,44e+00   1,0 1,60e-02 1,00e+00f  1
  18  1,8455479e+03 3,02e-01 5,76e+01  -1,0 1,32e+00   1,4 7,41e-01 3,11e-01f  1
  19  9,3274051e+02 1,81e-01 1,03e+02  -1,0 2,91e+00   0,9 3,93e-01 1,00e+00f  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  20  6,9601504e+02 4,97e-02 2,22e+01  -1,0 1,00e+00   1,3 5,55e-01 1,00e+00f  1
  21  3,3209659e+02 2,27e-02 1,43e+01  -1,0 1,94e+00   0,9 8,41e-01 1,00e+00f  1
  22  2,7097444e+02 1,90e-02 1,28e+01  -1,0 2,31e+00   0,4 8,12e-01 1,81e-01f  1
  23  1,7484017e+02 8,94e-03 9,55e+00  -1,0 1,20e+00   0,8 1,00e+00 6,61e-01f  1
  24  9,8584888e+01 1,16e-02 6,38e+00  -1,0 1,60e+00   0,3 1,00e+00 5,23e-01f  1
  25  3,2060003e+01 4,66e-02 1,57e+00  -1,0 1,50e+00  -0,1 1,00e+00 9,10e-01f  1
  26  2,7115436e+01 6,42e-01 1,49e+00  -1,0 1,51e+01    -  7,20e-02 2,01e-01f  1
  27  1,9503967e+01 3,06e+00 2,62e+00  -1,0 8,55e+00    -  1,30e-01 8,87e-01f  1
  28  1,7334482e+01 3,19e+00 2,97e+00  -1,0 2,59e+00  -0,6 7,06e-01 1,00e+00f  1
  29  1,7785727e+01 9,17e-01 3,78e+00  -1,0 1,24e+00  -0,2 1,00e+00 7,22e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  30  1,7959945e+01 5,06e-01 1,59e+00  -1,0 2,09e+00  -0,7 8,75e-01 1,00e+00f  1
  31  1,6062556e+01 1,54e-01 2,72e-01  -1,0 1,69e+00  -1,1 1,00e+00 1,00e+00f  1
  32  1,4253138e+01 2,08e-01 3,31e-01  -1,7 2,94e+00    -  5,11e-01 1,00e+00f  1
  33  1,4138912e+01 2,05e-02 4,27e-02  -1,7 1,04e+00    -  1,00e+00 1,00e+00h  1
  34  1,4137687e+01 3,12e-04 9,17e-04  -2,5 2,22e-01    -  9,99e-01 1,00e+00h  1
  35  1,4115841e+01 1,01e-05 2,72e-03  -3,8 2,24e-01    -  8,60e-01 1,00e+00h  1
  36  1,4107501e+01 3,95e-07 1,05e-06  -3,8 2,20e-01    -  1,00e+00 1,00e+00h  1
  37  1,4104789e+01 5,77e-08 5,60e-04  -5,7 1,30e-01    -  8,86e-01 1,00e+00h  1
  38  1,4104174e+01 3,21e-09 1,29e-09  -5,7 7,16e-02    -  1,00e+00 1,00e+00h  1
  39  1,4104020e+01 2,16e-11 1,93e-11  -5,7 3,78e-02    -  1,00e+00 1,00e+00h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  40  1,4103970e+01 2,19e-12 1,15e-05  -8,6 2,18e-02    -  9,82e-01 1,00e+00h  1
  41  1,4103959e+01 3,55e-15 2,52e-14  -8,6 1,09e-02    -  1,00e+00 1,00e+00h  1
  42  1,4103957e+01 3,55e-15 2,51e-14  -8,6 5,52e-03    -  1,00e+00 1,00e+00f  1
  43  1,4103956e+01 3,55e-15 2,52e-14  -8,6 2,73e-03    -  1,00e+00 1,00e+00f  1
  44  1,4103956e+01 3,55e-15 2,51e-14  -8,6 1,31e-03    -  1,00e+00 1,00e+00h  1
  45  1,4103956e+01 3,55e-15 3,01e-14  -9,0 6,93e-04    -  1,00e+00 1,00e+00h  1

Number of Iterations....: 45

                                   (scaled)                 (unscaled)
Objective...............:   1,4103955648834972e+01    1,4103955648834972e+01
Dual infeasibility......:   3,0087043967341742e-14    3,0087043967341742e-14
Constraint violation....:   3,5527136788005009e-15    3,5527136788005009e-15
Complementarity.........:   3,7994125831145155e-09    3,7994125831145155e-09
Overall NLP error.......:   3,7994125831145155e-09    3,7994125831145155e-09


Number of objective function evaluations             = 50
Number of objective gradient evaluations             = 46
Number of equality constraint evaluations            = 50
Number of inequality constraint evaluations          = 50
Number of equality constraint Jacobian evaluations   = 46
Number of inequality constraint Jacobian evaluations = 46
Number of Lagrangian Hessian evaluations             = 45
Total CPU secs in IPOPT (w/o function evaluations)   =      0,044
Total CPU secs in NLP function evaluations           =      0,008

EXIT: Optimal Solution Found.
time spent in eval_f: 0.000703 s. (50 calls, 0.01406 ms. average)
time spent in eval_grad_f: 0.000975 s. (47 calls, 0.0207447 ms. average)
time spent in eval_g: 0.000913 s. (50 calls, 0.01826 ms. average)
time spent in eval_jac_g: 0.001165 s. (48 calls, 0.0242708 ms. average)
time spent in eval_h: 0.004035 s. (46 calls, 0.0877174 ms. average)
time spent in main loop (proc): 0.054381 s.
time spent in main loop (wall): 0 s.
time spent in callback function: 0 s.
time spent in callback preparation: 0.000242 s.

In [4]:

    

# Plot the optimal trajectory
fig, ax = plt.subplots(figsize=(6,6))

# Trajectory
[traj_b, _] = plot_traj(ax, x_all)
# Catcher gaze
[arrows_c] = plot_arrows('gaze', ax, x_all[:,'x_c'],
                         x_all[:,'y_c'], x_all[:,'phi'], x_all[:,'theta'])
# Appearance
ax.set_title("Optimal trajectory without uncertainty")
ax.grid(True)
ax.legend(loc=2, handles=[traj_b, arrows_c])
ax.set_aspect('auto')
#fig.tight_layout()


    

In [ ]:

    

# Simulate a trajectory
x0 = normal(mu0, S0)
[x_all, z_all] = simulate_trajectory(x0, u_, F, dt, h, obs_cov_fcn, N_sim)

# Filter observations
b_all = filter_observations(b0, z_all, u_, EKF, N_sim)


    

In [ ]:

    

# Plot the true trajectory and the filtered version
fig, ax = plt.subplots(figsize=(10,10))

# Trajectory
[traj_b, _] = plot_traj(ax, x_all)
# Filtered
[filt_mb, filt_Sb, _, _] = plot_filt_bc_mS(ax, b_all)
# Catcher gaze
[arrows_c] = plot_arrows('gaze', ax, x_all[:,'x_c'],
                         x_all[:,'y_c'], x_all[:,'phi'], x_all[:,'theta'])
# Appearance
ax.set_title("Simulated trajectory and filtered observations")
ax.grid(True)
ax.legend(loc=2, handles=[traj_b, filt_mb, filt_Sb, arrows_c])
ax.set_aspect('equal')
#fig.tight_layout()


    

In [ ]:

    

# Simulate an extended belief trajectory
eb_all = simulate_ebelief_trajectory(eb0, u_, EBF, N_sim)

# Simulate a trajectory
x0 = normal(mu0, S0)
[x_all, z_all] = simulate_trajectory(x0, u_, F, dt, h, obs_cov_fcn, N_sim)

# Filter observations
b_all = filter_observations(b0, z_all, u_, EKF, N_sim)


    

In [ ]:

    

# Plot
fig, ax = plt.subplots(figsize=(10,10))

# Plan
[plan_mb, plan_Lb, plan_Sb, plan_SLb,
 _, _, _, _, arrows_c] = plot_plan(ax, eb_all)
# Trajectory
[traj_b, _] = plot_traj(ax, x_all)
# Filtered
[filt_b, _] = plot_filt(ax, b_all)

# Appearance
ax.set_title("Uncertainty propagation, "
             "sampled trajectory and filtered observations")
ax.grid(True)
ax.legend(loc=4,
          handles = [plan_mb, traj_b, filt_b, plan_Lb,
                     plan_Sb, plan_SLb, arrows_c])
ax.set_aspect('equal')
#fig.tight_layout()


    

In [ ]:

    

# Create a plan
sol = create_plan_pc(b0, BF, dt, N_sim)

# Simulate L's for plotting
eb_init = ext_belief(eb0)
eb_init['m'] = sol['X',0]
eb_all = simulate_ebelief_trajectory(eb_init, sol['U'], EBF, N_sim)


    

In [ ]:

    

# Sample several trajectories with optimal controls
b_init = belief(b0)
b_init['m'] = sol['X',0]
[X_all, B_all] = sim_traj(b_init, sol['U'], F, dt, h, obs_cov_fcn, EKF, N_sim)


    

In [ ]:

    

# Plot plan
fig, ax = plt.subplots(figsize=(10,10))

# Plan
[plan_mb, plan_Lb, plan_Sb, plan_SLb,
 _, _, _, _, arrows_c] = plot_plan(ax, eb_all)

# Appearance
ax.set_title("Planned trajectory")
ax.grid(True)
ax.legend(loc=2, handles = [plan_mb, plan_Lb, plan_Sb, plan_SLb, arrows_c])
ax.set_aspect('equal')
#.tight_layout()


    

In [ ]:

    

# Plot plan and sampled trajectories
fig, ax = plt.subplots(1,2,figsize=(20,8))

# True trajectories
plot_plan(ax[0], eb_all)
plot_traj_beam(ax[0], X_all)
# Appearance
ax[0].set_title("True trajectories")
ax[0].grid(True)
ax[0].set_aspect('equal')

# Belief trajectories
plot_plan(ax[1], eb_all)
plot_filt_beam(ax[1], B_all)
# Appearance
ax[1].set_title("Filtered trajectories")
ax[1].grid(True)
ax[1].set_aspect('equal')
#fig.tight_layout()


    

In [ ]:

    

# Plot the trajectory in 3D
ts = np.linspace(0, T_sim, N_sim+1)
fig = plt.figure(figsize=(12,8))
ax = fig.add_subplot(111,projection='3d')
ax.scatter3D(sol['X',:,'x_b'],
             sol['X',:,'y_b'],
             sol['X',:,'z_b'],
             c='b', lw='0.1', label='ball'
            )
ax.scatter3D(sol['X',:,'x_c'],
             sol['X',:,'y_c'],
             ca.DMatrix.zeros(ca.veccat(sol['X',:,'x_c']).size()),
             c='r', lw='0.1', label='catcher'
            )
plot_arrows3D('gaze', ax,
              sol['X',:,'x_c'], sol['X',:,'y_c'],
              sol['X',:,'phi'], sol['X',:,'theta'])
#ax.view_init(30, 20)
ax.set_zlim(0,40)
fig.show()


    

In [ ]:

    

# Simulate
[X_all, B_all, EB_all] = mpc_loop(eb0, T_sim, N_sim, N_ctrl, N_delay)


    

In [ ]:

    

# Plot
mpc_plot(X_all, B_all, EB_all, N_sim, N_ctrl, N_delay)


    

In [ ]:

    




    

In [ ]:

    




    

In [ ]:

    

ca.veccat( state[['vx_b','vy_b','vz_b']] )


    

In [ ]: