use `DynamicsPointMass2DSpeedGamma` to compute `brachistochrone`

[Zcaic]

Hello, when I use DynamicsPointMass2DSpeedGamma to compute brachistochrone, I found the result is error. The brachistochrone question is

The initial conditions are:
$$x_0$$=0, $$y_0$$=0, $$v_0$$=0
The final conditions are:
$$x_f$$=10, $$y_f$$=5

The control variable is "gamma". I want to minimize time.
I use the code below, but it doen't work:

N = 100
g = 9.81

opti = asb.Opti()

time_final = opti.variable(init_guess=5.0, lower_bound=0.01)
time = anp.linspace(0, time_final, N)

dyn = asb.DynamicsPointMass2DSpeedGamma(
    mass_props=asb.MassProperties(mass=1.0),
    x_e=opti.variable(init_guess=anp.linspace(0, 10, N), lower_bound=0.0),
    z_e=opti.variable(init_guess=anp.linspace(0, 5, N), lower_bound=0.0),
    speed=opti.variable(init_guess=1, n_vars=N),
    gamma=opti.variable(init_guess=0, n_vars=N, lower_bound=-anp.pi / 2, upper_bound=anp.pi / 2),
    alpha=0.0,
)

opti.subject_to(
    [dyn.x_e[0] == 0.0, dyn.x_e[-1] == 10.0, dyn.z_e[0] == 0.0, dyn.z_e[-1] == 5, dyn.speed[0] == 0.0]
)

dyn.add_force(Fx=-anp.sin(dyn.gamma) * g, axes="body")
dyn.constrain_derivatives(opti, time)

opti.minimize(time_final)
sol = opti.solve()

dyn = sol(dyn)

fig = plt.figure(0)
ax = fig.add_subplot(111)
ax.plot(dyn.x_e, -dyn.z_e)
plt.show()

the result is:

it's wrong.

And I write my code as below, it works well:

N = 200
g = 9.81

opti = asb.Opti()

time_final = opti.variable(init_guess=5, lower_bound=0)
theta = opti.variable(init_guess=0, n_vars=N, lower_bound=0, upper_bound=anp.pi)

dt = time_final / (N - 1)

v = opti.variable(init_guess=0, n_vars=N, freeze=True)
x = opti.variable(init_guess=0, n_vars=N, freeze=True)
y = opti.variable(init_guess=0, n_vars=N, freeze=True)

for i in range(1, N):
    theta_mid = 0.5 * (theta[i - 1] + theta[i])

    v[i] = v[i - 1] + g * anp.cos(theta_mid) * dt
    x[i] = x[i - 1] + 0.5 * (v[i] + v[i - 1]) * anp.sin(theta_mid) * dt
    y[i] = y[i - 1] + 0.5 * (v[i] + v[i - 1]) * anp.cos(theta_mid) * dt

opti.subject_to([x[-1] == 10.0, y[-1] == 5.0])

opti.minimize(time_final)
sol = opti.solve()
fig = plt.figure(0)
ax = fig.add_subplot(111)
ax.plot(sol(x), -sol(y))
plt.show()

[Zcaic]

I don't know where is wrong when I use DynamicsPointMass2DSpeedGamma to solve this question.

[peterdsharpe]

Hi @Zcaic ,

Thanks for sharing! The DynamicsPointMass2DSpeedGamma module uses a polar parameterization of the velocity (i.e., it's stored as speed + gamma, or equivalently, magnitude + direction; not Cartesian V_x and V_y components). This is useful because it creates less integration error on kinetic energy (which maps purely to speed) - a particular relevant property for trajectory optimization, as it makes it harder for an optimizer to exploit integration error to get more kinetic energy.

However, it has the drawback that the equations of motion blow up when the speed goes to zero - you get a division by zero.

For example, consider the state derivative $d\gamma/dt$: when the speed is near zero, an infinitesimal force can cause an infinite change in the direction of motion.

Because this Brachistochrone problem starts at a speed of 0, the first integration point yields NaN state derivatives.

Try replacing asb.DynamicsPointMass2DSpeedGamma with asb.DynamicsPointMass2DCartesian, and see if that fixes the issue.

[peterdsharpe]

Specifically, this is where the division-by-zero is happening:

AeroSandbox/aerosandbox/dynamics/point_mass/point_3D/speed_gamma_track.py

Line 106 in f290df9

d_gamma = force_gamma_direction / self.mass_props.mass / self.speed

Another possible fix would be to replace your dyn.speed[0] == 0 constraint with something like dyn.speed[0] == 0.1 or so, which should eliminate the singularity. You may also want to log-transform speed (by setting speed=opti.variable(init_guess=1, n_vars=N, log_transform=True), or to set a lower_bound=0.01 on speed -- both to prevent the speed == 0 singularity from being hit.

But this, of course, is a fundamentally different optimization problem - so the best bet is likely just to use a Cartesian velocity parameterization here.

[Zcaic]

Thanks for replying, I have try it as you mentioned. The code is

N = 100

opti = asb.Opti()

time_final = opti.variable(init_guess=5.0, lower_bound=0.01)
time = anp.linspace(0, time_final, N)

dyn = asb.DynamicsPointMass2DCartesian(
    mass_props=asb.MassProperties(mass=1.0),
    x_e=opti.variable(init_guess=anp.linspace(0, 10, N), lower_bound=0.0),
    z_e=opti.variable(init_guess=anp.linspace(0, 5, N), lower_bound=0.0),
    u_e=opti.variable(init_guess=1, n_vars=N, lower_bound=0.0),
    w_e=opti.variable(init_guess=1, n_vars=N),
    alpha=0.0,
)

opti.subject_to(
    [
        dyn.x_e[0] == 0.0,
        dyn.x_e[-1] == 10.0,
        dyn.z_e[0] == 0.0,
        dyn.z_e[-1] == 5,
        dyn.u_e[0] == 0.0,
        dyn.w_e[0] == 0.0,
    ]
)

dyn.add_force(Fx=opti.variable(init_guess=1, n_vars=N), axes="body")
dyn.constrain_derivatives(opti, time)

opti.minimize(time_final)
sol = opti.solve()

It raises：

It seems to have some problems.

[peterdsharpe]

In this latest code, you are allowing the optimizer to add an arbitrary force in the x-direction:

dyn.add_force(Fx=opti.variable(init_guess=1, n_vars=N), axes="body")

This results in a problem that does not have a unique local optimum - there are an infinite number of ways that you could use an unbounded force function to achieve time_final == 0.01, which is the only lower bound that has been set here. Therefore, the optimality conditions (dual feasibility) never get satisfied, and therefore IPOPT will keep iterating to try to satisfy this.

[Zcaic]

It‘s my new code：

N = 100
g = 9.81

opti = asb.Opti()

time_final = opti.variable(init_guess=5.0, lower_bound=0.01)
time = anp.linspace(0, time_final, N)

dyn = asb.DynamicsPointMass2DCartesian(
    mass_props=asb.MassProperties(mass=1.0),
    x_e=opti.variable(init_guess=anp.linspace(0, 10, N), lower_bound=0.0),
    z_e=opti.variable(init_guess=anp.linspace(0, 5, N), lower_bound=0.0),
    u_e=opti.variable(init_guess=1, n_vars=N, lower_bound=0.0),
    w_e=opti.variable(init_guess=1, n_vars=N),
    alpha=0.0,
)

opti.subject_to(
    [
        dyn.x_e[0] == 0.0,
        dyn.x_e[-1] == 10.0,
        dyn.z_e[0] == 0.0,
        dyn.z_e[-1] == 5,
        dyn.u_e[0] == 0.0,
        dyn.w_e[0] == 0.0,
    ]
)

gamma = dyn.gamma
dyn.add_gravity_force(g=g)
T = opti.variable(init_guess=1, n_vars=N, lower_bound=0.0)
dyn.add_force(Fz=-T, axes="body")
opti.subject_to(g * anp.sin(gamma) == -T)

dyn.constrain_derivatives(opti, time)

opti.minimize(time_final)
sol = opti.solve()

But it raises:
return_status is 'Not_Enough_Degrees_Of_Freedom'