Notes on OCP definition

[moorepants]

Here are some thoughts on a minimal set of constraints and objective elements that may be needed to solve the lane change.

The cyclist starts out riding at speed at its upright equilibrium point. They try to follow a line on the ground with a step "lane change" with the rear wheel by only applying steering or elbow torques. After completing the lane change the cyclist attempts to return to the equilibrium state at a slightly different speed.

• We have to have a fixed duration so we should not select too little time for any end constraints to be satisfied, i.e. give plenty of time after the lane change.

Constraints

• No need to apply pedaling torque, start with an initial forward speed and let the end speed be positive (or greater than initial speed - 10% or something reasonable).
• At time = 0 all bicycle states are zero except front and rear wheel rates and the rider's arms are set for the zero steer config.
• At time = final all bicycle states are zero +/- constraint tolerance except front and rear wheel rates. Front and rear wheel rates should result in a positive forward motion and be within ~10% of the initial values for the wheel rates.

Objective

• Minimize steering (or elbow) torque: minimal input energy formulation.
• Rear contact point lateral location should be minimized wrt to the lateral location of the lane line on the ground at whatever longitudinal position it is at: [y_rear(x) - y_ground_line(x)]^2.
• Minimize change in speed: abs(v(t_final) - v_(t_initial))

[moorepants]

If we include pedaling torque, then you can have a constraint: v(t_final) = v(t_initial) and minimize effort in pedaling torque. That is what we have and may be easier to set up.

[tjstienstra]

If we include pedaling torque, then you can have a constraint: v(t_final) = v(t_initial) and minimize effort in pedaling torque. That is what we have and may be easier to set up.

Correct, that is what we currently have and if we want to minimize changes I would suggest trying to stick with it.

[moorepants]

Is there anything else we have that differs from the above?

[tjstienstra]

Yes, there are quite some differences. We have more constraints on the initial state some of which should be removed. I hope to send an overview of my current progress shortly.

[tjstienstra]

The initial state constraints bicycle.q[0] = 0 and bicycle.q[1] = 0 ensure that the bicycle starts with the rear wheel at the start of the path.
The final state constraints bicycle.q[0] = longitudinal_displacement and bicycle.q[1] = lateral_displacement ensure that the bicycle ends up with the rear wheel at the end of the maneuver at t = duration.
The initial state constraints bicycle.q[5] = 0 and bicycle.q[7] = 0 force a reset of the wheel angles to zero. This makes sure that there are not infinite different solutions by just changing the meaningless offset.

bicycle.front_frame.q[0] = 0.0018 and bicycle.front_frame.u[0] = 0 are applied in optimization 4 to set the front suspension to rest at the start of the maneuver. Not doing so will allow storing energy in a cheating way. The suspension should be rather minimal as we are riding on flat ground.

There are several periodic constraints on the velocity of which are set up such that they would assume that the bicycle may do the lanechange in the opposite direction afterwards resulting in a take-over.
These constraints ensure periodicity of the translating velocity:

• bicycle.u[0].replace(t, t0) - bicycle.u[0].replace(t, tf) longitudinal velocity
• bicycle.u[1].replace(t, t0) + bicycle.u[1].replace(t, tf) lateral velocity
• bicycle.u[5].replace(t, t0) - bicycle.u[5].replace(t, tf) rear wheel angular rate
• bicycle.u[7].replace(t, t0) - bicycle.u[7].replace(t, tf) front wheel angular rate

I would say that the above is formulated as overconstrained. We could choose to just enforce periodicity on the rear wheel angular rate.

Officially I would say that the above is the bare minimum what one needs to get sensible results. In fact I did run optimization 1 with just the above (using just a periodic rear wheel angular rate) and it could solve with rather bad convergence. Including the periodic front wheel angular rate did seem to slightly improve convergence to the order of 300-400 iterations, which is higher than preferred. Including lateral and transversal velocity decreases the convergence rate. Therefore, I decided to continue from this point with only a periodic angular velocity on both wheels to ensure periodicity in the translating velocity. Good thing is that this completely maneuver independent.

The general shape of the trajectory is:

                  ------
                 -      --
                -
               -   
              -
             -
 ---        -
-   -     -
     -----

Two problems of the above minimal description are:

• Relatively high initial angular rates (safes torque, pedalling and steering torque are both zero at the start).
• A quite far from zero initial configuration
• Convergence is not as fast as hoped. Though it should be noted that Revise results #6 solved succesfully after quite some iterations.

There are two approaches to either setting initial states to zero or enforcing periodicity (as it were a take-over). In a way I would say that initial state constraints is more sensible as we are officially not dealing with a periodic movement. However, I would argue that the good thing of periodicity is that it enforces sensible values at the start and end without specifying them to be 0 by definition.

Just enforcing to start in a zero configuration by setting the yaw, roll, and steer angles to zero actually seems to still give pretty good convergence (at least for backward Euler).

• bicycle.q[2] = 0 yaw
• bicycle.q[3] = 0 roll
• bicycle.q[6] = 0 steer

At this point I would say most of the previous problems seem to be fixed. Setting the angular rates to zero, will enforce peak torques at the start of the optimization, which is higly undesirable.

The only problem I am seeing in the current solution is that there is no reason for the bicycle to remain upright and stable at the end of the path. Therefore, #5 and #6 actually have a roll angle of over -20 degrees at the end. However, this can be solved by setting the final state constraints:

• bicycle.q[2] = 0 yaw
• bicycle.q[3] = 0 roll
• bicycle.q[6] = 0 steer

Overview of the current constraints:

initial_state_constraints = {
    bicycle.q[0]: 0.0,
    bicycle.q[1]: 0.0,
    bicycle.q[2]: 0.0,
    bicycle.q[3]: 0.0,
    bicycle.q[5]: 0.0,
    bicycle.q[6]: 0.0,
    bicycle.q[7]: 0.0,
}
final_state_constraints = {
    bicycle.q[0]: data.metadata.longitudinal_displacement,
    bicycle.q[1]: data.metadata.lateral_displacement,
    bicycle.q[2]: 0.0,
    bicycle.q[3]: 0.0,
    bicycle.q[6]: 0.0,
}
instance_constraints = (
    # Periodic velocities.
    bicycle.u[5].replace(t, t0) - bicycle.u[5].replace(t, tf),
    bicycle.u[7].replace(t, t0) - bicycle.u[7].replace(t, tf),
)

This seems to give desirable results (further testing remains). However, this is where this is where my investigation for now, as I have to go. The current problem is that if I use backward Euler then it can actually cheat and start with a nonzero initial state. And if I use midpoint, then it is sensitive to result in a saw pattern.

[moorepants]

The final state constraints bicycle.q[0] = longitudinal_displacement and bicycle.q[1] = lateral_displacement ensure that the bicycle ends up with the rear wheel at the end of the maneuver at t = duration.

This shouldn't be needed if we have a tracking term in the objective.

There are several periodic constraints on the velocity of which are set up such that they would assume that the bicycle may do the lanechange in the opposite direction afterwards resulting in a take-over.

I don't know what this means.

bicycle.u[0].replace(t, t0) - bicycle.u[0].replace(t, tf) lateral velocity

Lateral velocity should be zero at start and end, this says that they should simply be the same value even if zero, right?

bicycle.u[1].replace(t, t0) + bicycle.u[1].replace(t, tf) transversal velocity

Longitudinal velocity should be the same non-zero value at start and end. Why is there a + and not a minus? v(t_0) = v(t_f).

You use "period" often, but what is periodic about this problem?

Setting the angular rates to zero, will enforce peak torques at the start of the optimization, which is higly undesirable.

Why is that undesirable? It is fine if the torque is non-zero at the initial time value. If we minimize the torque in the objective function, the optimizer will tame the torque.

however, this can be solved by setting the final state constraints:

Yes, I think that is what you should do. If you don' then the cyclist can "go through the finish" in any state, which can be realistic for a certain problem definition, but I think we should stick to: upright equilibrium and start and end to keep it simple and comparable.

[tjstienstra]

Oops swapped lateral in my comment. The reason for the plus is the lateral velocity bicycle.u[1] is that this gives a mirroring effect. What I try to explain here:

There are several periodic constraints on the velocity of which are set up such that they would assume that the bicycle may do the lanechange in the opposite direction afterwards resulting in a take-over.

This simply means that this is like periodicity of walking a step with one feet and assuming we can mirror our result to get the other one. In this lane change one option would be to use u1(tf) - u1(t0), but that would assume that you will do the same lane change to the left again. If you use u1(tf) + u1(t0) you kind of say we are assuming you will do a lane change to the right back to the original lateral position.

You use "period" often, but what is periodic about this problem?

In the final version I propose, I do not use real periodicity other than for the forward velocity. The thing why I like periodicity is that it just enforces taking reasonable values at the start and the end.

Why is that undesirable? It is fine if the torque is non-zero at the initial time value. If we minimize the torque in the objective function, the optimizer will tame the torque.

Peak torques make the solution dependent on the sample rate. Just being able to give that peak a little earlier may result in a vastly different result. This also causes unexplainable differences between optimizations like 3 and 4.

This shouldn't be needed if we have a tracking term in the objective.

True, we could leave the lateral out, but in a way, I would say that it is also nice to force the solution. This is similar to why I added initial state constraints on the yaw, roll, and steer angle. Otherwise, it will just start in a far-from-straight solution. As for the longitudinal position, that must be enforced because staying in a zero-configuration with zero velocity would otherwise be the optimal solution.

[tjstienstra]

The current problem is that if I use backward Euler then it can actually cheat and start with a nonzero initial state. And if I use midpoint, then it is sensitive to result in a saw pattern.

Do you know any good fixes for one of these problems. I would be highly interested in those.

[tjstienstra]

Do you know any good fixes for one of these problems. I would be highly interested in those.

For backward Euler I tried moving the initial state constraints to the second node, and that just works perfectly. Best convergence I have seen so far on the project and mostly reasonable results.

The reason for the move is simple to explain. The second node is used for the dynamics between the first and second nodes. The first node is in that sense not used at all.