(
    foot_positions_in_com_frame: Sequence[Sequence[float]],
    stance_foot_ids: Sequence[int],
)
return np.hstack(jacobians)
r"""Computes the 6 x 3n matrix to map contact force to com dynamics.

  This is essentially the vectorized rhs of com dynamics equation:
      ma = \sum f
      I\omega_dot = \sum r \cross f

  where the summation if over all feet in contact with ground.

  Caveats: Current we have not taken the com rotation into account as we assume
  the com rotation would be small. Ideally we should rotate the foot_positions
  to a world frame centered at com. Also, since absolute yaw angles are not
  accurate dute to drifting, we should use (roll, pitch, 0) to do the foot
  position projection. This feature will be quite useful for MPC.
  TODO(b/143378213): Fix this.

  Args:
    foot_positions_in_com_frame: the local position of each foot.
    stance_foot_ids: The stance foot to be used to assemble the matrix.

  Returns:
    The contact force projection matrix.
r"""Computes the 6 x 3n matrix to map contact force to com dynamics.
r Computes the 6 x 3n matrix to map contact force to com dynamics .
def compute_contact_force_projection_matrix(
    foot_positions_in_com_frame: Sequence[Sequence[float]],
    stance_foot_ids: Sequence[int],
) -> np.ndarray:
  r"""Computes the 6 x 3n matrix to map contact force to com dynamics.

  This is essentially the vectorized rhs of com dynamics equation:
      ma = \sum f
      I\omega_dot = \sum r \cross f

  where the summation if over all feet in contact with ground.

  Caveats: Current we have not taken the com rotation into account as we assume
  the com rotation would be small. Ideally we should rotate the foot_positions
  to a world frame centered at com. Also, since absolute yaw angles are not
  accurate dute to drifting, we should use (roll, pitch, 0) to do the foot
  position projection. This feature will be quite useful for MPC.
  TODO(b/143378213): Fix this.

  Args:
    foot_positions_in_com_frame: the local position of each foot.
    stance_foot_ids: The stance foot to be used to assemble the matrix.

  Returns:
    The contact force projection matrix.

  """
  jacobians = []
  for foot_id in stance_foot_ids:
    jv = np.identity(3)
    foot_position = foot_positions_in_com_frame[foot_id]
    x, y, z = foot_position[:3]
    jw = np.array(((0, -z, y), (z, 0, -x), (-y, x, 0)))
    jacobians.append(np.vstack((jv, jw)))

  return np.hstack(jacobians)
def compute_contact_force_projection_matrix ( foot_positions_in_com_frame : Sequence [ Sequence [ float ] ] , stance_foot_ids : Sequence [ int ] , ) -> np . ndarray : jacobians = [ ] for foot_id in stance_foot_ids : jv = np . identity ( 3 ) foot_position = foot_positions_in_com_frame [ foot_id ] x , y , z = foot_position [ : 3 ] jw = np . array ( ( ( 0 , - z , y ) , ( z , 0 , - x ) , ( - y , x , 0 ) ) ) jacobians . append ( np . vstack ( ( jv , jw ) ) ) return np . hstack ( jacobians )