ZMP in MPC during stair climbing

[Saeed-Mansouri]

Dear Dr. Caron,

I have found that the plane of the support foot is considered to calculate the zmp of the pendulum (pendulum.zpm()) for both SSP and DSP. So the zmp is estimated on different planes when the walking state is switched from DSP to SSP. I know that the set of zero-tilting moment points (ZMPs) of a wrench is an axis, and for practical purposes, we can intersect it with a plane (#12 (comment)). But I couldn't understand how we can consider some different planes for walking path planning. Would you please clear it for me?

[stephane-caron]

But I couldn't understand how we can consider some different planes for walking path planning.

I'm not sure I understand your question. As you have seen, the ZMP is computed in the foot frame during SSP, no question there. Hence I guess your question is: during DSP, the ZMP transitions from one plane to the next, how are these ZMP height variations taken care of in the MPC optimization?

(You refer to this optimization which you refer to as walking path planning, although note that contrary to a trajectory a path doesn't include timing information, so this optimization is rather trajectory planning.)

The short answer to this question is they are not taken care of (i.e. not modeled) by this MPC. Like all unmodeled phenomena, they will result in extra errors for the stabilizer when the robot climbs stairs. If these errors are eating your robustness margin too much, there is a great way to extend the MPC to take care of them described in this paper.

[Saeed-Mansouri]

During the stair climbing, because of the transition of the zmp from one plane to the next higher one at the beginning of the SSP and the axis of the zmp is not necessarily vertical, I expected that there would be a jump (discontinuity) in the trajectory of pendulum.zpm() in the x or y directions. I don't see any jump in the pendulum.zpm() trajectory. Why are there no jumps?

[stephane-caron]

That's a consequence of two things:

• the normal vector n is vertical in both Pendulum::completeIPM and Pendulum::resetCoMHeight
• the CoM height is forced to the nominal h in Pendulum::resetCoMHeight
  Let's take a look at

void Pendulum::completeIPM(const Contact & plane)
{
  auto n = plane.normal();
  auto gravitoInertial = world::gravity - comdd_;
  double lambda = n.dot(gravitoInertial) / n.dot(plane.p() - com_);
  zmp_ = com_ + gravitoInertial / lambda;
  zmpd_ = comd_ - comddd_ / lambda;
  omega_ = std::sqrt(lambda);
}

When this function is called after resetCoMHeight, lambda is equal to g / h, i.e. the desired natural frequency omega^2. If we look at the horizontal coordinates of the ZMP (cross product by n), we then get:

zmp x n = com x n + (g - comdd) x n / lambda = com x n + 0 - comdd x n / lambda = (com - comdd / omega^2) x n

Overall, we see that the combination of these two functions leaves the horizontal coordinates (i.e. the CoM, its acceleration, and ZMP output from the MPC trajectory generator) untouched. That's why we only see the jump vertically.

[Saeed-Mansouri]

Thanks Dr. Caron. It was helpful for me.