Inequality constraints under footstep rotations

[Saeed-Mansouri]

Dear Dr. Caron,

I have studied all of the codes in Pymanoid and LIPM admittance control packages as well as the relevant papers during the last few months. I have encountered two problems for which I appreciate your answers:

1. To solve QP during walking pattern generation, the ZMP constraint is defined in the world frame. This constraint works very well for straight and side walking. However, it seems that for moving on a curve or turning motion, the foot (local) frame has a rotation about the normal axis (z-axis) which rotates the support polygon and therefore the bounds of the ZMP cannot be defined properly (see figure). I think to resolve this problem, the ZMP constraint should be defined in the local frame (support foot frame in each step) instead of the world frame. Can I know your opinion?

in LIPM:

Eigen::HrepXd hrep() const
{
  Eigen::Matrix<double, 4, 2> localHrepMat, worldHrepMat;
  Eigen::Matrix<double, 4, 1> localHrepVec, worldHrepVec;
  localHrepMat <<
    +1, 0,
    -1, 0,
    0, +1,
    0, -1;
  localHrepVec <<
    halfLength,
    halfLength,
    halfWidth,
    halfWidth;
  if ((normal() - world::e_z).norm() > 1e-3)
  {
    LOG_WARNING("Contact is not horizontal");
  }
  const sva::PTransformd & X_0_c = pose;
  worldHrepMat = localHrepMat * X_0_c.rotation().topLeftCorner<2, 2>();
  worldHrepVec = worldHrepMat * X_0_c.translation().head<2>() + localHrepVec;
  return Eigen::HrepXd(worldHrepMat, worldHrepVec);
}

and

void ModelPredictiveControl::updateZMPConstraint()
{
hreps_[0] = initContact_.hrep();
hreps_[2] = targetContact_.hrep();
//hreps_[1] = getDoubleSupportHrep(initContact_, targetContact_);
//hreps_[3] = getDoubleSupportHrep(targetContact_, nextContact_);
unsigned totalRows = 0;
for (long i = 0; i <= NB_STEPS; i++)
{
  unsigned hrepIndex = indexToHrep_[i];
  if (hrepIndex % 2 == 0)
  {
    const auto & hrep = hreps_[hrepIndex];
    totalRows += static_cast<unsigned>(hrep.first.rows());
  }
}
Eigen::MatrixXd A{totalRows, STATE_SIZE * (NB_STEPS + 1)};
Eigen::VectorXd b{totalRows};
A.setZero();
long nextRow = 0;
for (long i = 0; i <= NB_STEPS; i++)
{
  unsigned hrepIndex = indexToHrep_[i];
  if (hrepIndex % 2 == 0)
  {
    const auto & hrep = hreps_[indexToHrep_[i]];
    unsigned consRows = static_cast<unsigned>(hrep.first.rows());
    A.block(nextRow, STATE_SIZE * i, consRows, STATE_SIZE) = hrep.first * zmpFromState_;
    b.segment(nextRow, consRows) = hrep.second;
    nextRow += consRows;
  }
}
zmpCons_ = std::make_shared<copra::TrajectoryConstraint>(A, b);

}

in Pymanoid:

def get_scaled_contact_area(self, scale):
    """
    Get the vertices of the scaled contact area.

    Parameters
    ----------
    scale : scalar
        Contact area is scaled by this ratio.

    Returns
    -------
    vertices : list of arrays
        List of vertex coordinates in the world frame.
    """
    X = scale * self.shape[0]
    Y = scale * self.shape[1]
    v1 = dot(self.T, array([+X, +Y, 0., 1.]))[:3]
    v2 = dot(self.T, array([+X, -Y, 0., 1.]))[:3]
    v3 = dot(self.T, array([-X, -Y, 0., 1.]))[:3]
    v4 = dot(self.T, array([-X, +Y, 0., 1.]))[:3]
    return [v1, v2, v3, v4]

and

    cur_vertices = self.stance_foot.get_scaled_contact_area(0.9) 
    next_vertices = self.swing_target.get_scaled_contact_area(0.9)
    for coord in [0, 1]:
        cur_max = max(v[coord] for v in cur_vertices)
        cur_min = min(v[coord] for v in cur_vertices)
        next_max = max(v[coord] for v in next_vertices)
        next_min = min(v[coord] for v in next_vertices)
        e[coord] = [
            array([+1000., +1000.]) if i < nb_init_dsp_steps else
            array([+cur_max, -cur_min]) if i - nb_init_dsp_steps <= nb_init_ssp_steps else
            array([+1000., +1000.]) if i - nb_init_dsp_steps - nb_init_ssp_steps < nb_dsp_steps else
            array([+next_max, -next_min])
            for i in range(nb_preview_steps)]

2. As discussed in “Walking Trajectory Planning on Stairs Using Virtual Slope for Biped Robots”, there are two major issues for motion generation of bipeds on stairs: i) the ZMP equation problem: which is “the ZMP equation on stairs is different from that of flat ground”, and ii) the ZMP definition problem: which is “the ZMP cannot be defined in the double-support phase on stairs because the contact points of the feet do not constitute a plane.” In this paper, a virtual slope method is proposed to resolve these issues. I cannot find any difference in the approach implemented in LIPM to generate straight and stair walking trajectories. Would you please explain how you handle these two issues in the LIPM for stair walking?

[stephane-caron]

Hi Saeed. It is better to open a separate issue for each question. Let me answer your first question here.

which rotates the support polygon and therefore the bounds of the ZMP cannot be defined properly (see figure)

Note that one operation in your notes is incorrect: you cannot multiply by a homogeneous transform (or any matrix) in vector inequalities. To do this properly in general we use the double description method. To learn about this and the more general topic of polyhedral geometry you can start from this page and references therein.

To solve QP during walking pattern generation, the ZMP constraint is defined in the world frame.

To avoid any confusion here: the ZMP constraint has the form A * z <= b where z is indeed the vector of ZMP coordinates in the world frame, but the constraint itself is defined in the local frame. See the variables localHrepMat and localHrepVec in the hrep() function you quoted above: in this function, you can see how the matrix and vector are modified into worldHrepMat and worldHrepVec so that world coordinates on the right hand side are converted to local ones.

This implementation takes foot rotations into account. Here is an example where HRP-4 walks through curved footstep plans in Choreonoid: choreonoid_curved_walk.mp4.zip

[stephane-caron]

I added these two plans curved_15cm_steps and curved_20cm_steps to the configuration file for reference.

[Saeed-Mansouri]

Thank you for your helpful response. I tried to find how the world coordinates are converted to local ones. However, I find a little difference in the formulation. Instead of the rotation matrix, I have seen the inverse of one (see figure). I have been a little bit confused. Is X_0_c the transformation matrix of the foot frame?

const sva::PTransformd & X_0_c = pose;
worldHrepMat = localHrepMat * X_0_c.rotation().topLeftCorner<2, 2>();
worldHrepVec = worldHrepMat * X_0_c.translation().head<2>() + localHrepVec;
return Eigen::HrepXd(worldHrepMat, worldHrepVec);

[stephane-caron]

Good point: X_0_c is not a (4×4) affine transformation matrix, rather it is the (6×6) Plucker transform from the inertial frame (0) to the contact frame (c). This is because our control framework is based on spatial vectors. If you are not familiar with it, I recommend checking out the following introductory resources:

• A short introduction to SpaceVecAlg and RBDyn for the libraries used in this controller
• A Beginner's Guide to 6-D Vectors for the concepts they rely on

Here, the important fact for you is that:

• The rotation matrix R of a usual affine transform T = [R & p \\ 0 & 1] goes from the target frame to the source one. When the source is the inertial frame 0 and the target is the contact frame c, the rotation matrix is R = R_c_0.
• The rotation matrix of a Plücker transform X_0_c goes from the source frame to the target one, i.e. what you get from X_0_c.rotation() is the matrix R_0_c. This is indeed the inverse of the rotation matrix you would get with a usual affine transform.