3D ball and 2D catcher simultaneous

A joint model of a 3D ball and a 2D catcher with free final time


In [1]:
# Plotting
%matplotlib inline
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d.axes3d import Axes3D

# Numerics
import numpy as np

# CasADi
import casadi as ca
import casadi.tools as cat

In [2]:
# Simulation parameters
T_sim = 1.0        # simulation time
N_sim = 20         # time steps
h = T_sim/N_sim    # time-step length
N_rk = 1          # number of RK4 steps (for each time-step)

# Model parameters
nx = 12; nu = 3
mu = 10.0/12; g0 = 9.81
a = 7.5; b = 2.5
wmin = -2 * ca.pi
wmax = 2 * ca.pi
thetamin = -ca.pi
thetamax = ca.pi

# Initial conditions
# Catcher
x_c0 = 7
y_c0 = 3
vx_c0 = -2
vy_c0 = 0
phi0 = 6./5 * ca.pi
# Ball
x_b0 = 0
y_b0 = 0
z_b0 = 0
vx_b0 = 5
vy_b0 = 6
vz_b0 = 10

# Create symbolic variables
states = cat.struct_symSX(["x_c","y_c",
                           "vx_c","vy_c","phi",
                           "x_b","y_b","z_b",
                           "vx_b","vy_b","vz_b",
                           "T"])
[x_c,y_c,vx_c,vy_c,phi,
 x_b,y_b,z_b,vx_b,vy_b,vz_b,T] = states[...]
controls = cat.struct_symSX(["u","w","theta"])
[u,w,theta] = controls[...]

# ODE right-hand side
rhs = cat.struct_SX(states)
rhs["x_c"] = vx_c * T
rhs["y_c"] = vy_c * T
rhs["vx_c"] = (u * (ca.cos(theta) * ca.cos(phi) - \
                    ca.sin(theta) * ca.sin(phi)) - mu * vx_c) * T
rhs["vy_c"] = (u * (ca.cos(theta) * ca.sin(phi) + \
                    ca.sin(theta) * ca.cos(phi)) - mu * vy_c) * T
rhs["phi"] = w * T
rhs["x_b"] = vx_b * T
rhs["y_b"] = vy_b * T
rhs["z_b"] = vz_b * T
rhs["vx_b"] = 0
rhs["vy_b"] = 0
rhs["vz_b"] = -g0 * T
rhs["T"] = 0

# Degrees of freedom for the optimizer
V = cat.struct_symSX([
        (
            cat.entry("X",repeat=N_sim+1,struct=states),
            cat.entry("U",repeat=N_sim,struct=controls)
        )
    ])

# Continuous-time dynamics
f = ca.SXFunction([states, controls], [rhs])
f.init()

# Discrete-time dynamics
F = ca.simpleRK(f, N_rk)

In [3]:
# Objective function
J = 1e1 * (V["X",N_sim,"x_c"] - V["X",N_sim,"x_b"]) ** 2 + \
    1e1 * (V["X",N_sim,"y_c"] - V["X",N_sim,"y_b"]) ** 2
# Regularize u
J += 1e-3 * ca.sum_square(ca.veccat(V["U",:,"u"])) * h * V["X",N_sim,"T"]
# Regularize theta
J += 1e-3 * ca.sum_square(ca.veccat(V["U",:,"theta"])) * h * V["X",N_sim,"T"]

# Build non-linear constraints and running costs
g = []
lbg = []
ubg = []
for i in range(N_sim):
    # Multiple shooting
    g.append(V["X",i+1] - \
             F([V["X",i], V["U",i], h])[0])
    lbg.append(ca.DMatrix.zeros(nx))
    ubg.append(ca.DMatrix.zeros(nx))
    
    # Control constraints
    g.append(V["U",i,"u"] - a - b * ca.cos(V["U",i,"theta"]))
    lbg.append(-ca.inf)
    ubg.append(ca.DMatrix([0]))
    
    # Encourage looking at the ball
    d = ca.veccat((ca.cos(V["X",i,"phi"]), ca.sin(V["X",i,"phi"])))
    r = ca.veccat((V["X",i,"x_b"]-V["X",i,"x_c"], V["X",i,"y_b"]-V["X",i,"y_c"]))
    J -= 1e-2 * ca.mul(d.T,r)
g = ca.vertcat(g)
lbg = ca.veccat(lbg)
ubg = ca.veccat(ubg)

In [4]:
# Create NLP
nlp = ca.SXFunction(ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
nlp.init()

# Create solver
solver = ca.NlpSolver("ipopt",nlp)
solver.init()

# Set non-linear bounds
solver.setInput(lbg,"lbg")
solver.setInput(ubg,"ubg")

# Initialize simple bounds
lbx = V(-ca.inf)
ubx = V(ca.inf)

# ---------------------------------------------------------------------
# State limits
# ---------------------------------------------------------------------
# Initial condition of the catcher
lbx["X",0,"x_c"] = ubx["X",0,"x_c"] = x_c0
lbx["X",0,"y_c"] = ubx["X",0,"y_c"] = y_c0
lbx["X",0,"vx_c"] = ubx["X",0,"vx_c"] = vx_c0
lbx["X",0,"vy_c"] = ubx["X",0,"vy_c"] = vy_c0
lbx["X",0,"phi"] = ubx["X",0,"phi"] = phi0

# Initial condition of the ball
lbx["X",0,"x_b"] = ubx["X",0,"x_b"] = x_b0
lbx["X",0,"y_b"] = ubx["X",0,"y_b"] = y_b0
lbx["X",0,"z_b"] = ubx["X",0,"z_b"] = z_b0
lbx["X",0,"vx_b"] = ubx["X",0,"vx_b"] = vx_b0
lbx["X",0,"vy_b"] = ubx["X",0,"vy_b"] = vy_b0
lbx["X",0,"vz_b"] = ubx["X",0,"vz_b"] = vz_b0

# Require time to be strictly positive (>= 0.1)
lbx["X",:,"T"] = 0.1

# Simulation ends when the ball touches the ground
lbx["X",N_sim,"z_b"] = ubx["X",N_sim,"z_b"] = 0

# ---------------------------------------------------------------------
# Control limits
# ---------------------------------------------------------------------
# u >= 0 (the upper bound is set by non-linear constraints)
lbx["U",:,"u"] = 0
# wmin <= w <= wmax
lbx["U",:,"w"] = wmin
ubx["U",:,"w"] = wmax
# thetamin <= theta <= thetamax
lbx["U",:,"theta"] = thetamin
ubx["U",:,"theta"] = thetamax

# Set simple bounds
solver.setInput(lbx,"lbx")
solver.setInput(ubx,"ubx")

# Solve
with cat.nice_stdout():
    solver.evaluate()
#solver.evaluate()
sol = V(solver.getOutput())


******************************************************************************
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.12.1, running with linear solver mumps.
NOTE: Other linear solvers might be more efficient (see Ipopt documentation).

Number of nonzeros in equality constraint Jacobian...:     1079
Number of nonzeros in inequality constraint Jacobian.:       40
Number of nonzeros in Lagrangian Hessian.............:      512

Total number of variables............................:      300
                     variables with only lower bounds:       41
                variables with lower and upper bounds:       40
                     variables with only upper bounds:        0
Total number of equality constraints.................:      240
Total number of inequality constraints...............:       20
        inequality constraints with only lower bounds:        0
   inequality constraints with lower and upper bounds:        0
        inequality constraints with only upper bounds:       20

iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
   0 -7.4264736e-02 9.95e+00 2.39e-05  -1.0 0.00e+00    -  0.00e+00 0.00e+00   0
   1  1.5618454e+01 2.75e+01 2.13e+06  -1.0 1.45e+02    -  6.37e-04 5.00e-01f  2
   2  1.6228998e+01 2.73e+01 2.12e+06  -1.0 4.50e+01    -  2.17e-02 5.77e-03h  1
   3  1.5836326e+01 2.70e+01 2.09e+06  -1.0 3.55e+01    -  1.20e-02 1.12e-02f  1
   4  1.5659256e+01 2.70e+01 2.09e+06  -1.0 3.66e+01    -  1.68e-02 3.16e-03f  1
   5  1.3183926e+01 2.57e+01 1.99e+06  -1.0 3.55e+01    -  6.04e-02 4.68e-02f  1
   6  1.1353994e+01 1.03e+01 8.48e+05  -1.0 3.43e+01    -  2.54e-02 6.95e-01f  1
   7  8.7404785e+00 9.70e+00 7.95e+05  -1.0 2.20e+01   2.0 2.35e-01 6.32e-02f  1
   8  2.6298774e+01 4.18e+00 3.54e+05  -1.0 2.12e+01   1.5 5.16e-01 6.66e-01h  1
   9  6.5945702e+01 3.39e+00 2.88e+05  -1.0 1.36e+01   1.9 2.02e-01 1.96e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  10  7.7386289e+01 3.18e+00 2.70e+05  -1.0 1.21e+01   2.4 4.93e-01 6.51e-02h  1
  11  8.6003468e+01 2.70e+00 2.29e+05  -1.0 1.17e+01   1.9 1.50e-01 1.55e-01h  1
  12  8.6002501e+01 2.69e+00 2.29e+05  -1.0 1.06e+01   2.3 3.21e-01 2.43e-03h  1
  13  7.9640218e+01 2.27e+00 1.93e+05  -1.0 1.06e+01   1.8 4.01e-01 1.63e-01f  1
  14  6.7995879e+01 2.09e+00 1.78e+05  -1.0 9.62e+00   1.4 1.00e+00 7.85e-02f  1
  15  5.3852572e+01 1.74e+00 1.48e+05  -1.0 9.17e+00   1.8 4.58e-01 1.76e-01f  1
  16  2.1417510e+01 1.23e+00 1.05e+05  -1.0 8.19e+00   1.3 1.00e+00 3.12e-01f  1
  17  8.8516012e+01 3.93e-01 3.70e+04  -1.0 7.52e+00   0.8 5.75e-01 7.87e-01h  1
  18  1.2967850e+02 4.30e-02 6.19e+03  -1.0 2.95e+00   0.4 7.86e-01 1.00e+00h  1
  19  6.4420626e+01 2.38e-02 2.19e+03  -1.0 2.19e+00  -0.1 1.00e+00 6.64e-01f  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  20  4.6673717e+01 1.99e-02 1.65e+03  -1.0 3.11e+00  -0.6 2.78e-01 2.44e-01f  1
  21  3.9895377e+01 1.19e-03 1.63e+02  -1.0 2.99e-01   0.7 1.00e+00 9.10e-01f  1
  22  2.7667778e+01 6.48e-04 1.20e+00  -1.0 6.58e-01   0.3 1.00e+00 1.00e+00f  1
  23  1.1482567e+01 4.39e-03 8.06e-01  -1.0 1.31e+00  -0.2 1.00e+00 9.78e-01f  1
  24  2.6577774e+00 4.39e-03 5.78e-01  -1.7 1.48e+00  -0.7 9.65e-01 8.65e-01f  1
  25  1.3659903e+00 2.62e-04 2.49e-01  -1.7 4.49e-01  -0.3 1.00e+00 9.42e-01f  1
  26  9.1172591e-01 4.62e-04 1.76e-01  -1.7 5.94e-01  -0.7 1.00e+00 5.16e-01f  1
  27  2.6907854e-01 2.82e-03 6.47e-02  -2.5 9.41e-01  -1.2 6.28e-01 1.00e+00f  1
  28  1.7385691e-01 6.25e-05 3.96e-02  -2.5 2.47e-01  -0.8 1.00e+00 1.00e+00h  1
  29  1.0414061e-01 1.48e-03 2.24e-02  -2.5 4.20e-01  -1.3 1.00e+00 1.00e+00f  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  30  6.0188629e-02 2.26e-03 2.08e-02  -3.8 5.10e-01  -1.7 6.99e-01 1.00e+00h  1
  31 -4.1557578e-02 1.14e-01 1.31e-02  -3.8 1.14e+00  -2.2 3.74e-01 1.00e+00h  1
  32 -1.0408162e-01 3.46e-01 1.77e-02  -3.8 3.20e+01  -2.7 7.00e-02 2.05e-01h  1
  33 -1.8821421e-01 2.70e-01 1.37e-02  -3.8 1.53e+01    -  2.41e-01 2.13e-01h  1
  34 -2.1631577e-01 2.29e-01 1.16e-02  -3.8 1.17e+01    -  1.52e-01 1.42e-01h  1
  35 -2.3366722e-01 1.82e-01 6.28e-03  -3.8 9.81e+00    -  5.12e-01 1.82e-01h  1
  36 -2.4676570e-01 1.03e-01 3.22e-03  -3.8 7.34e+00    -  7.51e-01 3.92e-01h  1
  37 -2.5306942e-01 5.36e-03 1.91e-03  -3.8 2.18e+00    -  1.00e+00 1.00e+00h  1
  38 -2.5262480e-01 1.87e-04 1.91e-05  -3.8 2.66e-01    -  1.00e+00 1.00e+00h  1
  39 -2.5542764e-01 3.59e-05 1.31e-05  -5.7 1.81e-01    -  9.60e-01 1.00e+00h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  40 -2.5545499e-01 1.57e-05 3.69e-06  -5.7 2.30e-01    -  1.00e+00 1.00e+00h  1
  41 -2.5545604e-01 3.87e-07 5.97e-07  -5.7 4.01e-02    -  1.00e+00 1.00e+00h  1
  42 -2.5549122e-01 8.60e-08 1.58e-07  -8.6 2.03e-02    -  9.96e-01 1.00e+00h  1
  43 -2.5549123e-01 2.00e-09 3.08e-09  -8.6 2.88e-03    -  1.00e+00 1.00e+00h  1

Number of Iterations....: 43

                                   (scaled)                 (unscaled)
Objective...............:  -2.5549122526764212e-01   -2.5549122526764212e-01
Dual infeasibility......:   3.0805886966672434e-09    3.0805886966672434e-09
Constraint violation....:   1.9999093492373277e-09    1.9999093492373277e-09
Complementarity.........:   8.3922900408179666e-09    8.3922900408179666e-09
Overall NLP error.......:   8.3922900408179666e-09    8.3922900408179666e-09


Number of objective function evaluations             = 47
Number of objective gradient evaluations             = 44
Number of equality constraint evaluations            = 47
Number of inequality constraint evaluations          = 47
Number of equality constraint Jacobian evaluations   = 44
Number of inequality constraint Jacobian evaluations = 44
Number of Lagrangian Hessian evaluations             = 43
Total CPU secs in IPOPT (w/o function evaluations)   =      0.137
Total CPU secs in NLP function evaluations           =      0.050

EXIT: Optimal Solution Found.
time spent in eval_f: 0.001763 s. (47 calls, 0.0375106 ms. average)
time spent in eval_grad_f: 0.002237 s. (45 calls, 0.0497111 ms. average)
time spent in eval_g: 0.002306 s. (47 calls, 0.0490638 ms. average)
time spent in eval_jac_g: 0.006495 s. (46 calls, 0.141196 ms. average)
time spent in eval_h: 0.037442 s. (44 calls, 0.850955 ms. average)
time spent in main loop: 0.196375 s.
time spent in callback function: 0 s.
time spent in callback preparation: 0.002044 s.

In [7]:
#%matplotlib inline
# Plot the trajectory in 3D
ts = np.linspace(0, float(sol["X",N_sim,"T"]), N_sim+1)
fig = plt.figure(figsize=(12,8))
ax = fig.add_subplot(111,projection='3d')
ax.set_zlim(0,6)
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'
            )
ax.view_init(30, 20)
fig.show()



In [8]:
%matplotlib inline
# Plot the controls
fig, ax = plt.subplots(figsize=(10,4))
ax.grid(True)
ax.step(ts, ca.veccat((0,sol["U",:,"u"])), label='u')
ax.step(ts, ca.veccat((0,sol["U",:,"w"])), label='w')
ax.step(ts, ca.veccat((0,sol["U",:,"theta"])), label='theta')
ax.legend(loc=1);



In [9]:
# Plot the trajectory in 2D
fig, ax = plt.subplots(figsize=(10,10))
ax.grid(True)
ax.scatter(ca.vertcat(sol["X",:,"x_b"]),
           ca.vertcat(sol["X",:,"y_b"]),
           s=30, lw=0.02, c='b', label='ball'
          )
ax.scatter(ca.vertcat(sol["X",:,"x_c"]),
           ca.vertcat(sol["X",:,"y_c"]),
           s=30, lw=0.02, c='c', label='catcher'
          )

# Plot the arrows
Q = ax.quiver(ca.vertcat(sol["X",:,"x_c"]),
          ca.vertcat(sol["X",:,"y_c"]),
          ca.cos(ca.vertcat(sol["X",:,"phi"])),
          ca.sin(ca.vertcat(sol["X",:,"phi"])),
          units='xy', angles='xy',
          scale=2.5, width=0.04,
          headwidth=4, headlength=6, headaxislength=5,
          color='m', lw='0.1'
         )
qk = ax.quiverkey(Q, 0.04, 0.88, 1, 'gaze direction', labelpos='E',
                  fontproperties={'size': 12}
                 )
ax.legend(loc=2);



In [ ]:


In [ ]:


In [ ]:


In [ ]: