Simple biped gap crossing example in casadi

[EnricoMingo]

I just added compass_gap_crossing.pdf and compass_gap_crossing.py which represents a simple example of a biped traversing a gap using optimal control, implemented in casadi. This work started by @matteoparigi may be a good starting point for the quadruped case.

[matteosodano]

We have downloaded and run the code, and read the documentation. For the run, we have commented the lines relative to the "logger" since it called an external library that we did not have.

We have a couple of questions about the code:

• Lines 220-222: we did not understand what the constraint is about.
• Lines 228-230: it seems that this constraint is already included in the previous one (lines 224-226), relative to the friction cones. Why is it repeated (if it actually is)?

[matteoparigi]

Hi,

Lines 220-222: we did not understand what the constraint is about.

In my code the "normal vectors" (i.e. the normal vector to the environment function, related to each contact point) are optimization variables. Thus you need the constraint 220-222 to enforce that the normal vectors are actually normal to the environment function. The expression of the environment normal, given the environment function, is computed at line 63-64:

n_env = jacobian(Sc, point)/norm_1(jacobian(Sc, point))

Lines 228-230: it seems that this constraint is already included in the previous one (lines 224-226), relative to the friction cones. Why is it repeated (if it actually is)?

You are right, you can comment out either one or the other. And see what is the difference in doing so if any.

[matteosodano]

Ok, clear. Thank you.

[francesco-roscia]

We have expanded the code to a quadruped. We modeled it as two bipeds linked by a prismatic joint. We required to the robot to move of 0.5 m along the x axis (forward movement) taking at most 10 steps, each of maximum length of 0.3 m. Only one foot at time can move. It seems that ipopt it is not able to solve the problem if the centroidal statics constraint is enabled. You can find the code and a rough documentation (it contains the cost index and all the constraints) in the folder Dummy_Quadruped_in_CasADi. Notice that in the plots, axes have not the same scale.

[EnricoMingo]

Hi Francesco, did you updated the centroidal static constraint as well to take into account more than two contacts?

[francesco-roscia]

We have made some modifications in the centroidal statics constraint and now it seems to work well!

[francesco-roscia]

It seems that we solved the problem in presence of a gap. We modeled the environment simply as a flat ground and we impose additional constraints to the contact points. We considered the gap as an infinite strip of width 10 cm and we imposed a clearance of 1 cm from each edge of the gap. The gap is orthogonal to x-axis. Denoting the x-coordinates of the gap as gap_min and gap_max (which include the clearance), the x-coordinates of the i-th feet at time k as x(i, k), the gap constraint is: (x(i, k) - gap_min) (x(i, k) - gap_max) >= 0. The constraint is satisfied if x(i, k) =< gap_min or if x(i, k) >= gap_max, that is the foot is in one of the "good" portions of the plane. In the zip folder, you can find plots of a simulation in which the robot have to move of 3 m forward and of 1 m to left, crossing the gap with every leg, in 40 movements. The optimal solution is found. Figure.zip

Sometimes, the algorithm does not converge to an optimal solution (this is not the case of the pictures), but it returns "restoration failed". It says

Restoration phase converged to a feasible point that is unacceptable to the filter for the original problem. Restoration phase in the restoration phase failed.

Ipopt documentation states that

This indicates that the restoration phase failed to find a feasible point that was acceptable to the filter line search for the original problem. This could happen if the problem is highly degenerate, does not satisfy the constraint qualification, or if your NLP code provides incorrect derivative information.

Do you have any suggestions?

[EnricoMingo]

Great, tomorrow I am going to download the code and test it. I think we can almost consider this first MILESTONE done.

Il giorno mar 5 mag 2020 alle ore 19:03 francesco-roscia < notifications@github.com> ha scritto:

It seems that we solved the problem in presence of a gap. We modeled the environment simply as a flat ground and we impose additional constraints to the contact points. We considered the gap as an infinite strip of width 10 cm and we imposed a clearance of 1 cm from each edge of the gap. The gap is orthogonal to x-axis. Denoting the x-coordinates of the gap as gap_min and gap_max (which include the clearance), the x-coordinates of the i-th feet at time k as x(i, k), the gap constraint is: (x(i, k) - gap_min) (x(i, k) - gap_max) >= 0. The constraint is satisfied if x(i, k) =< gap_min or if x(i, k) >= gap_max, that is the foot is in one of the "good" portions of the plane. In the zip folder, you can find plots of a simulation in which the robot have to move of 3 m forward and of 1 m to left, crossing the gap with every leg, in 40 movements. The optimal solution is found. Figure.zip https://github.com/ADVRHumanoids/gap_crossing/files/4582294/Figure.zip

Sometimes, the algorithm does not converge to an optimal solution (this is not the case of the pictures), but it returns "restoration failed". It says

Restoration phase converged to a feasible point that is unacceptable to the filter for the original problem. Restoration phase in the restoration phase failed. Ipopt documentation states that This indicates that the restoration phase failed to find a feasible point that was acceptable to the filter line search for the original problem. This could happen if the problem is highly degenerate, does not satisfy the constraint qualification, or if your NLP code provides incorrect derivative information.

— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub https://github.com/ADVRHumanoids/gap_crossing/issues/3#issuecomment-624180712, or unsubscribe https://github.com/notifications/unsubscribe-auth/ABKBVSVFTJPT3KC3UZC43O3RQBBFXANCNFSM4LQJJUYA .

-- PhD Mingo Hoffman Enrico Istituto Italiano di Tecnologia, Genova

[francesco-roscia]

Hi to all! Do you know which is paper the pdf at https://github.com/ADVRHumanoids/gap_crossing/blob/master/docs/compass_gap_crossing.pdf refers to?

[matteoparigi]

Hi! this one here

[francesco-roscia]

Hi! this one here

Thank you @matteoparigi