towr
towr copied to clipboard
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?
For the records... problem solved with
formulation.params_.force_polynomials_per_stance_phase_ = 1;
I have an intuition of why that is happening, but still looks weird to me.
Another problem, apparently, was to have a stance phase time exactly equal to:
formulation.params_.dt_constraint_dynamic_
(0.1 by default)
Is the latter a bug, maybe?