Calculate forces during standing

Hi,

I was trying to calculate the inverse dynamic of a robot in static stance position.

Unfortunately, I observed that the generate ee_force splines have wild oscillations (value at time t=0 is correct).

This is the code I used:

using namespace towr;

int main()
{
  NlpFormulation formulation;
  formulation.terrain_ = std::make_shared<FlatGround>(0.0);
  formulation.model_ = RobotModel(RobotModel::Anymal);

  // don't move. 
  formulation.initial_base_.lin.at(kPos).z() = 0.42;
  formulation.final_base_.lin. = formulation.initial_base_.lin;

  auto nominal_stance = formulation.model_.kinematic_model_->GetNominalStanceInBase();

  // Single stance phase
  for (size_t i=0; i<4; i++)  {  
      auto foot = nominal_stance[i];
      foot.z() = 0.0; 
      formulation.initial_ee_W_.push_back(foot);
      formulation.params_.ee_phase_durations_.push_back({0.1});
      formulation.params_.ee_in_contact_at_start_.push_back(true);
  }

  ifopt::Problem nlp;
  SplineHolder solution;

  for (auto c : formulation.GetVariableSets(solution)) {
      nlp.AddVariableSet(c);
  }
  for (auto c : formulation.GetConstraints(solution)){
       nlp.AddConstraintSet(c);
  }
  // Needed to add this, to prevent oscillations of the upper body
  nlp.AddCostSet( std::make_shared<NodeCost>(id::base_lin_nodes, kVel, Z, 1.0) );

  auto solver = std::make_shared<ifopt::IpoptSolver>();
  solver->SetOption("jacobian_approximation", "exact"); // "finite difference-values"
  solver->SetOption("max_cpu_time", 20.0);
  solver->Solve(nlp);

  double t = 0.0;

  while (t<=solution.base_linear_->GetTotalTime() + 1e-5)
  {
    cout << "\nt=" << t << "\n";

    printf("Base linear position z:  %lf \n", solution.base_linear_->GetPoint(t).p().z());
    printf("Feet forces:  %lf\t%lf\t%lf\t%lf\n",
           solution.ee_force_.at(0)->GetPoint(t).p().z(),
           solution.ee_force_.at(1)->GetPoint(t).p().z(),
           solution.ee_force_.at(2)->GetPoint(t).p().z(),
           solution.ee_force_.at(3)->GetPoint(t).p().z() );

    t += 0.01;
  }
  return 0;
}

What do you think it is going on and how can I prevent this from happen?

ethz-adrl / towr

Calculate forces during standing #91