Experiencing a strange behaviour

[fscafarto]

Hi

I have ROS Noetic installed on my laptop and I have already used Towr in an exam project, in particular I have successfully implemented a Quadruped (DogBot by ReactRobotics) and it was fine. Now, I'm trying to work on AnymalC robot, but when I run my program I am experiencing a strange error, this error says that Ipopt is not able to find a solution because of CPU time exceeded (most of times).

I have tried many workarounds, but no success. For example, I increased CPU time, but no success. The same code for instance has been used with DogBot and it works fine in few steps.
I have properly configured also anymal_model.h header file, changing the mass and the inertia values, and of course the nominal position of the feet, but no success.
About the input values, they are just normal as well as the final ones, I really don't understand why changing the Quadruped (with the proper changes of course) I'm experiencing this problem.

Here is my code, I am sure that the structure of the code is not a problem, since it has been already used and all the quantities suit the formulation, for example the nominal position of feet is correctly taken from the kinematic model provided to Towr, the Center of Mass position and orientation is something like: 0.0, 0.0, 0.47, 0.0, 0.0, 0.0. Initial velocity is all zeros. Moreover, I tried to modify the inputs, for example by assigning a zero initial velocity rather than something like 0.009e-10 which is given by the functions I have used from iDynTree library, but no success, it seems that the problem is not represented by the input data (both initial and final position/orientation). So, I guess the error is somewhere in anymal_model.h, because there is something that make the optimization problem solved by iPopt fail unconditionally, but since anymal_model.h is a tiny piece of code, I don't know where the issue could be...thanks in advance for any suggestion or any idea about what to be checked you can give me since I am stucked on this problem and I can't figure it out

void PLANNING::compute_planning(int flag){

auto gategen = towr::GaitGenerator::MakeGaitGenerator(4);

towr::NlpFormulation formulation;
_formulation=formulation;
_formulation.terrain_ = std::make_shared<towr::FlatGround>(0.0); 

_formulation.model_ = towr::RobotModel(towr::RobotModel::Anymal);  

auto stance = _formulation.model_.kinematic_model_->GetNominalStanceInBase();
_formulation.initial_ee_W_ = stance;
  
_formulation.initial_base_.lin.at(towr::kPos) << dogbot->get_init_pos()(0,0), dogbot->get_init_pos()(1,0), 0.47;
_formulation.initial_base_.ang.at(towr::kPos) << dogbot->get_init_pos()(3,0), dogbot->get_init_pos()(4,0), dogbot->get_init_pos()(5,0);
_formulation.initial_base_.lin.at(towr::kVel) << dogbot->get_init_vel()(0,0), dogbot->get_init_vel()(1,0), dogbot->get_init_vel()(2,0); 
_formulation.initial_base_.ang.at(towr::kVel) << dogbot->get_init_vel()(3,0), dogbot->get_init_vel()(4,0), dogbot->get_init_vel()(5,0); 

std::for_each(_formulation.initial_ee_W_.begin(), _formulation.initial_ee_W_.end(),
              [&](Eigen::Vector3d& p){ p.z() = 0.0; });


    _formulation.final_base_.lin.at(towr::kPos) << _formulation.initial_base_.lin.at(towr::kPos)[0],_formulation.initial_base_.lin.at(towr::kPos)[1]-0.04, 0.47;
    _formulation.final_base_.ang.at(towr::kPos) << 0.0, -0.0, -0.0;
  
Eigen::Vector3d pos_ee;
for (int ee=0; ee<4; ee++){

    switch(ee){
        case 0: pos_ee=dogbot->getBLpos();
        break;
        case 1: pos_ee=dogbot->getBRpos();
        break;
        case 2: pos_ee=dogbot->getFLpos();
        break;
        case 3: pos_ee=dogbot->getFRpos();
        break;
    }

    _formulation.initial_ee_W_.at(ee)[0]=pos_ee[0];
    _formulation.initial_ee_W_.at(ee)[1]=pos_ee[1];
}

 

if(flag==0){
    auto gait_type = static_cast<towr::GaitGenerator::Combos>(towr::GaitGenerator::C5);
    gategen->SetCombo(gait_type);
}
else if(flag==1){
    auto gait_type = static_cast<towr::GaitGenerator::Combos>(towr::GaitGenerator::C6);
    gategen->SetCombo(gait_type);
}
else if(flag==2){
    auto gait_type = static_cast<towr::GaitGenerator::Combos>(towr::GaitGenerator::C7);
    gategen->SetCombo(gait_type);
}
   

//gategen->SetGaits({_stand, _run}); // Stand + Run1

_formulation.params_.ee_phase_durations_.clear();
for(int i=0;i<4;++i){
    _formulation.params_.ee_phase_durations_.push_back(gategen->GetPhaseDurations(0.5,i)); 
    _formulation.params_.ee_in_contact_at_start_.push_back(gategen->IsInContactAtStart(i));
}


ifopt::Problem nlp;

towr::SplineHolder solution;

_solution=solution;

//Init nonlinear programming with vars, constraints and costs
for(auto c: _formulation.GetVariableSets(_solution))
nlp.AddVariableSet(c);

for(auto c: _formulation.GetConstraints(_solution))
nlp.AddConstraintSet(c);

for(auto c: _formulation.GetCosts())
nlp.AddCostSet(c);


auto solver = std::make_shared<ifopt::IpoptSolver>();
solver -> SetOption("jacobian_approximation","exact");
solver -> SetOption("max_cpu_time",20.0); 
solver -> SetOption("print_level", 5);
solver -> Solve(nlp);

}

******************************************************************************/

#ifndef TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_ANYMAL_MODEL_H_
#define TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_ANYMAL_MODEL_H_

#include <towr/models/kinematic_model.h>
#include <towr/models/single_rigid_body_dynamics.h>
#include <towr/models/endeffector_mappings.h>

namespace towr {

/**

• @brief The Kinematics of the quadruped robot ANYmal.
  */
  class AnymalKinematicModel : public KinematicModel {
  public:
  AnymalKinematicModel () : KinematicModel(4)
  {

  const double x_nominal_b = 0.42;
  const double y_nominal_b = 0.28;
  const double z_nominal_b = -0.07;

  nominal_stance_.at(LF) << x_nominal_b, y_nominal_b, z_nominal_b;
  nominal_stance_.at(RF) << x_nominal_b, -y_nominal_b, z_nominal_b;
  nominal_stance_.at(LH) << -x_nominal_b, y_nominal_b, z_nominal_b;
  nominal_stance_.at(RH) << -x_nominal_b, -y_nominal_b, z_nominal_b;

  max_dev_from_nominal_ << 0.15, 0.15, 0.15;
  }
  };

/**

• @brief The Dynamics of the quadruped robot ANYmal.
  */
  class AnymalDynamicModel : public SingleRigidBodyDynamics {
  public:
  AnymalDynamicModel()
  : SingleRigidBodyDynamics(52.1348,
  0.216712, 1.78516, 1.8337, 0.000922381, 0.0589332, -0.000163778,
  4) {}
  };

} // namespace towr

#endif /* TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_ANYMAL_MODEL_H_ */

[fscafarto]

I managed to solve this problem, even if I don't know why the previous code was wrong, anyway it works now. The problem is represented by this piece of code:

Eigen::Vector3d pos_ee;
for (int ee=0; ee<4; ee++){

switch(ee){
    case 0: pos_ee=dogbot->getBLpos();
    break;
    case 1: pos_ee=dogbot->getBRpos();
    break;
    case 2: pos_ee=dogbot->getFLpos();
    break;
    case 3: pos_ee=dogbot->getFRpos();
    break;
}

_formulation.initial_ee_W_.at(ee)[0]=pos_ee[0];
_formulation.initial_ee_W_.at(ee)[1]=pos_ee[1];

}

Once removed it, it works. This code just assigns the x and y component of the position to _formulation.initial (which is filled with the nominal positions passed by the kinematic model in anymal_model.h), these positions are something like 0.2831 rather than 0.28, but Ipopt gets mad with these precise values, so I removed this assignment, but remember that this piece of code works fine with Dogbot, hope this issue is useful for someone.