ethz-adrl / towr

I am trying to reproduce the bipedal results for traversing the terrain with a gap, as presented in the paper and its supplementary video.

I am on the master branch. I have set the time to 4.4 s, the gait to number 7, and the goal to [3.7, 0.0, 0.66] (all of these using the console interface).

Furthermore, since this part of the code was hardcoded, I modified towr_ros.cc like follows:

-  std::vector<Eigen::Vector3d> ee_pos(4);
+  std::vector<Eigen::Vector3d> ee_pos(2);

-  ee_pos.at(0) <<  0.31,  0.29, 0.0; // LF
-  ee_pos.at(1) <<  0.31, -0.29, 0.0; // RF
-  ee_pos.at(2) << -0.31,  0.29, 0.0; // LH
-  ee_pos.at(3) << -0.31, -0.29, 0.0; // RH
+  ee_pos.at(0) << 0.0,  0.2, 0.0; // L
+  ee_pos.at(1) << 0.0, -0.2, 0.0; // R

-  model_.dynamic_model_   = std::make_shared<towr::HyqDynamicModel>();
-  model_.kinematic_model_ = std::make_shared<towr::HyqKinematicModel>();
-  gait_                   = std::make_shared<towr::QuadrupedGaitGenerator>();
+  model_.dynamic_model_   = std::make_shared<towr::BipedDynamicModel>();
+  model_.kinematic_model_ = std::make_shared<towr::BipedKinematicModel>();
+  gait_                   = std::make_shared<towr::BipedGaitGenerator>();

This is the result I get:

I also tried switching between "ma27", "ma57", and "ma86" but without luck.
I haven't tried using any other solver besides Ipopt.

I don't know if the diff above contains all the required changes to run the biped robot on the gap-crossing environment. Particularly, I am not sure whether [0.0, 0.2, 0.0] and [0.0, -0.2, 0.0] are the correct ee_pos for the initial state.

Thank you very much,
- Henrique Ferrolho

Hi @ferrolho, yes, this looks good, i think the optimizer is doing exactly as it's supposed to and only the visualization in rviz is off.

The height map the optimizer user internally, with a width of 1m is encoded here:

towr/towr_ros/include/towr_ros/height_map_examples.h

Line 105 in dee989c

const double w = 1.0; // gap width or 0.5 for ANYmal

The class that tries to replicate the internal height map with rviz shapes is faultily using only 0.5m and is coded here:

towr/towr_ros/src/rviz_terrain_builder.cc

Line 163 in dee989c

double l_gap = 0.5; // was 0.5m or 1m for biped for anymal motions

So for now just make sure these coincide and I'm actually working on how not to duplicate that information, but at the same time introduce no dependencies on rviz in the core solver class. I'll also fix this bug in that PR and update this issue then.

Hey @awinkler, thank you for the prompt reply!

This was indeed an issue and after setting double l_gap = 1.0; result looks better:

However, I am still a bit confused... This is the log of the optimiser I get on my slow laptop:

Number of Iterations....: 69

                                   (scaled)                 (unscaled)
Objective...............:   0.0000000000000000e+00    0.0000000000000000e+00
Dual infeasibility......:   7.9408063850289635e-06    7.9408063850289635e-06
Constraint violation....:   5.2894000180652583e-12    1.2967404927621828e-11
Complementarity.........:   1.8847416957485208e-06    1.8847416957485208e-06
Overall NLP error.......:   7.9408063850289635e-06    7.9408063850289635e-06


Number of objective function evaluations             = 225
Number of objective gradient evaluations             = 64
Number of equality constraint evaluations            = 225
Number of inequality constraint evaluations          = 225
Number of equality constraint Jacobian evaluations   = 71
Number of inequality constraint Jacobian evaluations = 71
Number of Lagrangian Hessian evaluations             = 0
Total CPU secs in IPOPT (w/o function evaluations)   =      1.221
Total CPU secs in NLP function evaluations           =     25.192

EXIT: Optimal Solution Found.

It says "Optimal Solution Found" and does not report any constraint to have been violated. However, in the animation above we can see that, at least visually, this does not seem to be the case:

Good question. Exactly, this is also related to the way rviz shapes are being used to approximate the actual height map used by the optimizer. If you look at

towr/towr_ros/src/height_map_examples.cc

Line 112 in dee989c

h = a*x*x + b*x + c;

you see that for the optimizer the gap is modeled as a parabola. This parabola cannot be easily represented with rviz shapes, but the foothold you show is placed exactly on this parabola.

PS: You can also model the gap directly as the rviz picture, with non-continuous heights. This can work as well, however having the gradient information of the parabola available helps the solver converge quicker.

Trying to replicate the Biped Gap Crossing scenario