Advice on how to decrease unrealistic contact forces between spline nodes

[ferrolho]

Hi, @awinkler! I am trying to compute a 2-seconds-long jump for a quadruped, but unfortunately the solver converges to a solution with unrealistic contact forces. Please see the animation below.

I am aware that this is because the formulation only ensures unilaterality and forces within the friction cones at the nodes of the polynomials but not in between. You do mention this in the towr::ForceConstraint documentation:

Attention: Constraint is enforced only at the spline nodes. In between violations of this constraint can occur.

However, I am wondering if you have any tips on how to formulate the problem in a way that avoids these unrealistic forces.

For the sake of completion, I will detail the changes I made to the current version of the codebase. I added a new gait for the jump:

case C5: SetGaits({Stand, Hop2, Stand}); break;  // jump

The Stand phase duration is still set to 0.3 (no changes to this line), but I changed the times of the Hop2 (GetStridePronk() method defined here) to:

QuadrupedGaitGenerator::GetStridePronk () const
{
  double push   = 0.5;
  double flight = 0.4;
  double land   = 0.5;
(...)

Therefore, for the new gait C5, the default timings are 0.3 + 0.5 + 0.4 + 0.5 + 0.3 = 2.0. The settings in the interface are:

I have also tried to add more and shorter Stand phases before and after the Hop2, e.g., Stand, Stand, Stand, Hop2, Stand, Stand, Stand and changing the Stand time to 0.1, but I had no luck. I was hoping that having more phases would allow to have more nodes in the same time interval as before, therefore leading to a formulation with more constraints enforcing the unilaterality more often.

I also tried increasing duration_base_polynomial_ slightly, or to decrease force_polynomials_per_stance_phase_ from 3 to 2, as you mention here. I am trying to avoid adding a cost because that slows down convergence, but maybe that is the way to solve this?

Thank you in advance! 🙂

[awinkler]

hmmm, adding multiple Stand phases behind each other will still only be parameterized as one long stand phase if i'm not mistaken. And if that stand phase gets longer, while still being parameterized by the same number of polynomials, the less "freedom per time" that polynomial has. I could imagine in this scenario the solver needs a very abrupte change in the force profile to cause that long of a flight-phase. To give it that freedom, you could try to reduce the stand time before and after, so that the force polynomials don't have to simultaneously model standing in the place and also then changing abruptly. Their expressiveness is sort of concentrated on a shorter time interval.

But as you rightfully investigated, this is not so easy to do, and you already turned on some of the knobs i would have tried as well.

[ferrolho]

adding multiple Stand phases behind each other will still only be parameterized as one long stand phase if i'm not mistaken.

I was not aware of this. I stacked multiple Stand phases similarly to how you stack other types of phases:

towr/towr/src/quadruped_gait_generator.cc

Lines 80 to 84 in a211e01

	case C0: SetGaits({Stand, Walk2, Walk2, Walk2, Walk2E, Stand}); break; // overlap-walk
	case C1: SetGaits({Stand, Run2, Run2, Run2, Run2E, Stand}); break; // fly trot
	case C2: SetGaits({Stand, Run3, Run3, Run3, Run3E, Stand}); break; // pace
	case C3: SetGaits({Stand, Hop1, Hop1, Hop1, Hop1E, Stand}); break; // bound
	case C4: SetGaits({Stand, Hop3, Hop3, Hop3, Hop3E, Stand}); break; // gallop

The goal was to prolong the "standing" time before the jump, similarly to how the lines above prolong locomotion trajectories.

Indeed, if the formulation of multiple Stand phases stacked to each other results in only one very long phase, then it is true that the polynomial will have less "freedom per time" to model the forces required for the jump. But my initial idea for having multiple Stand phases was precisely to allow the solver to have a long polynomial modelling the initial standing time, and then another polynomial to model the abrupt changes in force required for the jump take-off.

I think we should make a conclusion note here for future readers: while it is true that the unilaterality and forces within the friction cones are only enforced at the spline nodes and not in between, that does not seem to be the "crux" for modelling this jumping task; rather, the crux seems to be related to the limited expressiveness of each phase's polynomial.

I will have a look at the codebase myself now, but do you know if it would be easy to change the code to not reduce stacks of Stand phases to only one long Stand phase? I believe that may be the right way to achieve this, rather than trying to turn more knobs. 😛

[ferrolho]

adding multiple Stand phases behind each other will still only be parameterized as one long stand phase if i'm not mistaken.

I can confirm you were not mistaken. The lines below are responsible for merging phases from the pre-specified gait if they do not involve a change in contact state for any of the feet:

towr/towr/src/gait_generator.cc

Lines 90 to 95 in 7193d66

	// if contact will change in next phase, so this phase duration complete
	bool contacts_will_change = curr.at(ee) != next.at(ee);
	if (contacts_will_change) {
	foot_durations.at(ee).push_back(d_accumulated.at(ee));
	d_accumulated.at(ee) = 0.0;
	}

I tried my luck by forcing the body of the if statement to always run:

// bool contacts_will_change = curr.at(ee) != next.at(ee);
bool contacts_will_change = true;

which did stop the merging of the Stand phases.

I verified this by checking the size of params.ee_phase_durations_ for the first foot with

std::cout << "[DEBUG] params.ee_phase_durations_[0]: " << params.ee_phase_durations_[0].size() << std::endl;

just after the for loop

towr/towr_ros/src/towr_ros_app.cc

Lines 73 to 76 in d30c4a0

	for (int ee=0; ee<n_ee; ++ee) {
	params.ee_phase_durations_.push_back(gait_gen_->GetPhaseDurations(msg.total_duration, ee));
	params.ee_in_contact_at_start_.push_back(gait_gen_->IsInContactAtStart(ee));
	}

For the gait sequence Stand, Stand, Hop2, Stand, Stand, the output is

[DEBUG] params.ee_phase_durations_[0]: 7

which is correct because Stand has only one BB_ phase, and Hop2 has three phases: BB_, II_, BB_.

Unfortunately, this still did not produce the trajectory I was hoping for, and there seem to be some extra flight phases that are not specified in the gait sequence. Please see the animation below:

[awinkler]

Well, I don’t think treating multiple stand phases as separate makes sense. This way of specifying the gait was really just a helper class. I think if we want to give more freedom to the polynomials representing the stand phases, we should use more force polynomials per stand phase (as you already correctly tried). The position polynomial for the stance phase is fixed anyway, so there is no need to use multiple of those.

So this really just brings us back to your initial comment comment; don’t see a robust/deterministic fix for that...

[ferrolho]

I agree with your remarks, @awinkler. I will now close this issue. Thank you for your help and feedback.

Final note for future readers: I don't want to leave prospect users of TOWR with a bad impression. Let me emphasise that the issue discussed above is really only a setback if the duration of the trajectory is longer than the expressiveness of the polynomials available. For shorter trajectories, TOWR works really well! See the 1-second-long jump below, with well-behaved forces, and for which the solver converges in ~10 iterations. 🙂