Merge branch 'whole-body-fixed-base' into whole-body

python/quadruped_reactive_walking/tools/Utils.py

+from example_robot_data import load
+import numpy as np
+import pinocchio as pin
+
+
+def init_robot(q_init, params):
+    """Load the solo model and initialize the Gepetto viewer if it is enabled
+
+    Args:
+        q_init (array): the default position of the robot actuators
+        params (object): store parameters
+    """
+    # Load robot model and data
+    solo = load("solo12")
+    q = solo.q0.reshape((-1, 1))
+
+    # Initialisation of the position of footsteps to be under the shoulder
+    # There is a lateral offset of around 7 centimeters
+    fsteps_under_shoulders = np.zeros((3, 4))
+    indexes = [
+        solo.model.getFrameId("FL_FOOT"),
+        solo.model.getFrameId("FR_FOOT"),
+        solo.model.getFrameId("HL_FOOT"),
+        solo.model.getFrameId("HR_FOOT"),
+    ]
+    q[7:, 0] = 0.0
+    pin.framesForwardKinematics(solo.model, solo.data, q)
+    for i in range(4):
+        fsteps_under_shoulders[:, i] = solo.data.oMf[indexes[i]].translation
+    fsteps_under_shoulders[2, :] = 0.0
+
+    # Initial angular positions of actuators
+    q[7:, 0] = q_init
+
+    # Initialisation of model quantities
+    pin.centerOfMass(solo.model, solo.data, q, np.zeros((18, 1)))
+    pin.updateFramePlacements(solo.model, solo.data)
+    pin.crba(solo.model, solo.data, solo.q0)
+
+    # Initialisation of the position of footsteps
+    fsteps_init = np.zeros((3, 4))
+    h_init = 0.0
+    for i in range(4):
+        fsteps_init[:, i] = solo.data.oMf[indexes[i]].translation
+        h = (solo.data.oMf[1].translation - solo.data.oMf[indexes[i]].translation)[2]
+        if h > h_init:
+            h_init = h
+    fsteps_init[2, :] = 0.0
+
+    # Initialisation of the position of shoulders
+    shoulders_init = np.zeros((3, 4))
+    indexes = [4, 12, 20, 28]  # Shoulder indexes
+    for i in range(4):
+        shoulders_init[:, i] = solo.data.oMf[indexes[i]].translation
+
+    # Saving data
+    params.h_ref = h_init
+    # Mass of the whole urdf model (also = to Ycrb[1].mass)
+    params.mass = solo.data.mass[0]
+    # Composite rigid body inertia in q_init position
+    params.I_mat = solo.data.Ycrb[1].inertia.ravel().tolist()
+    params.CoM_offset = (solo.data.com[0][:3] - q[0:3, 0]).tolist()
+    params.CoM_offset[1] = 0.0
+
+    #  Use initial feet pos as reference
+    for i in range(4):
+        for j in range(3):
+            params.shoulders[3 * i + j] = shoulders_init[j, i]
+            params.footsteps_init[3 * i + j] = fsteps_init[j, i]
+            params.footsteps_under_shoulders[3 * i + j] = fsteps_init[j, i]
+
+
+def quaternionToRPY(quat):
+    """Quaternion (4 x 0) to Roll Pitch Yaw (3 x 1)"""
+
+    qx = quat[0]
+    qy = quat[1]
+    qz = quat[2]
+    qw = quat[3]
+
+    rotateXa0 = 2.0 * (qy * qz + qw * qx)
+    rotateXa1 = qw * qw - qx * qx - qy * qy + qz * qz
+    rotateX = 0.0
+
+    if (rotateXa0 != 0.0) and (rotateXa1 != 0.0):
+        rotateX = np.arctan2(rotateXa0, rotateXa1)
+
+    rotateYa0 = -2.0 * (qx * qz - qw * qy)
+    rotateY = 0.0
+    if rotateYa0 >= 1.0:
+        rotateY = np.pi / 2.0
+    elif rotateYa0 <= -1.0:
+        rotateY = -np.pi / 2.0
+    else:
+        rotateY = np.arcsin(rotateYa0)
+
+    rotateZa0 = 2.0 * (qx * qy + qw * qz)
+    rotateZa1 = qw * qw + qx * qx - qy * qy - qz * qz
+    rotateZ = 0.0
+    if (rotateZa0 != 0.0) and (rotateZa1 != 0.0):
+        rotateZ = np.arctan2(rotateZa0, rotateZa1)
+
+    return np.array([[rotateX], [rotateY], [rotateZ]])

src/ComplementaryFilter.cpp

+#include "qrw/ComplementaryFilter.hpp"
+
+ComplementaryFilter::ComplementaryFilter()
+    : dt_(0.),
+      HighPass_(Vector3::Zero()),
+      LowPass_(Vector3::Zero()),
+      alpha_(Vector3::Zero()),
+      x_(Vector3::Zero()),
+      dx_(Vector3::Zero()),
+      filteredX_(Vector3::Zero()) {}
+
+ComplementaryFilter::ComplementaryFilter(double dt, Vector3 HighPass, Vector3 LowPass)
+    : dt_(dt),
+      HighPass_(HighPass),
+      LowPass_(LowPass),
+      alpha_(Vector3::Zero()),
+      x_(Vector3::Zero()),
+      dx_(Vector3::Zero()),
+      filteredX_(Vector3::Zero()) {}
+
+void ComplementaryFilter::initialize(double dt, Vector3 HighPass, Vector3 LowPass) {
+  dt_ = dt;
+  HighPass_ = HighPass;
+  LowPass_ = LowPass;
+}
+
+Vector3 ComplementaryFilter::compute(Vector3 const& x, Vector3 const& dx, Vector3 const& alpha) {
+  alpha_ = alpha;
+  x_ = x;
+  dx_ = dx;
+
+  HighPass_ = alpha.cwiseProduct(HighPass_ + dx_ * dt_);
+  LowPass_ = alpha.cwiseProduct(LowPass_) + (Vector3::Ones() - alpha).cwiseProduct(x_);
+  filteredX_ = HighPass_ + LowPass_;
+
+  return filteredX_;
+}
\ No newline at end of file

src/Estimator.cpp

+#include "qrw/Estimator.hpp"
+
+#include <example-robot-data/path.hpp>
+
+#include "pinocchio/algorithm/compute-all-terms.hpp"
+#include "pinocchio/algorithm/frames.hpp"
+#include "pinocchio/math/rpy.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+
+Estimator::Estimator()
+    : perfectEstimator_(false),
+      solo3D_(false),
+      dt_(0.0),
+      initialized_(false),
+      feetFrames_(Vector4::Zero()),
+      footRadius_(0.155),
+      alphaPos_({0.995, 0.995, 0.9}),
+      alphaVelMax_(1.),
+      alphaVelMin_(0.97),
+      alphaSecurity_(0.),
+      IMUYawOffset_(0.),
+      IMULinearAcceleration_(Vector3::Zero()),
+      IMUAngularVelocity_(Vector3::Zero()),
+      IMURpy_(Vector3::Zero()),
+      IMUQuat_(pinocchio::SE3::Quaternion(1.0, 0.0, 0.0, 0.0)),
+      qActuators_(Vector12::Zero()),
+      vActuators_(Vector12::Zero()),
+      phaseRemainingDuration_(0),
+      feetStancePhaseDuration_(Vector4::Zero()),
+      feetStatus_(Vector4::Zero()),
+      feetTargets_(Matrix34::Zero()),
+      q_FK_(Vector19::Zero()),
+      v_FK_(Vector18::Zero()),
+      baseVelocityFK_(Vector3::Zero()),
+      basePositionFK_(Vector3::Zero()),
+      b_baseVelocity_(Vector3::Zero()),
+      feetPositionBarycenter_(Vector3::Zero()),
+      qEstimate_(Vector19::Zero()),
+      vEstimate_(Vector18::Zero()),
+      vSecurity_(Vector12::Zero()),
+      windowSize_(0),
+      vFiltered_(Vector6::Zero()),
+      qRef_(Vector18::Zero()),
+      vRef_(Vector18::Zero()),
+      baseVelRef_(Vector6::Zero()),
+      baseAccRef_(Vector6::Zero()),
+      oRh_(Matrix3::Identity()),
+      hRb_(Matrix3::Identity()),
+      oTh_(Vector3::Zero()),
+      h_v_(Vector6::Zero()),
+      h_vFiltered_(Vector6::Zero()) {
+  b_M_IMU_ = pinocchio::SE3(pinocchio::SE3::Quaternion(1.0, 0.0, 0.0, 0.0), Vector3(0.1163, 0.0, 0.02));
+  q_FK_(6) = 1.0;
+  qEstimate_(6) = 1.0;
+}
+
+void Estimator::initialize(Params& params) {
+  dt_ = params.dt_wbc;
+  perfectEstimator_ = params.perfect_estimator;
+  solo3D_ = params.solo3D;
+
+  // Filtering estimated linear velocity
+  int k_mpc = (int)(std::round(params.dt_mpc / params.dt_wbc));
+  windowSize_ = (int)(k_mpc * params.gait.rows() / params.N_periods);
+  vx_queue_.resize(windowSize_, 0.0);  // List full of 0.0
+  vy_queue_.resize(windowSize_, 0.0);  // List full of 0.0
+  vz_queue_.resize(windowSize_, 0.0);  // List full of 0.0
+
+  // Filtering velocities used for security checks
+  double fc = 6.0;
+  double y = 1 - std::cos(2 * M_PI * 6. * dt_);
+  alphaSecurity_ = -y + std::sqrt(y * y + 2 * y);
+
+  // Initialize Quantities
+  basePositionFK_(2) = params.h_ref;
+  velocityFilter_.initialize(dt_, Vector3::Zero(), Vector3::Zero());
+  positionFilter_.initialize(dt_, Vector3::Zero(), basePositionFK_);
+  qRef_(2, 0) = params.h_ref;
+  qRef_.tail(12) = Vector12(params.q_init.data());
+
+  // Initialize Pinocchio
+  const std::string filename = std::string(EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf");
+  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), velocityModel_, false);
+  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(), positionModel_, false);
+  velocityData_ = pinocchio::Data(velocityModel_);
+  positionData_ = pinocchio::Data(positionModel_);
+  pinocchio::computeAllTerms(velocityModel_, velocityData_, qEstimate_, vEstimate_);
+  pinocchio::computeAllTerms(positionModel_, positionData_, qEstimate_, vEstimate_);
+  feetFrames_ << (int)positionModel_.getFrameId("FL_FOOT"), (int)positionModel_.getFrameId("FR_FOOT"),
+      (int)positionModel_.getFrameId("HL_FOOT"), (int)positionModel_.getFrameId("HR_FOOT");
+}
+
+void Estimator::run(MatrixN const& gait, MatrixN const& feetTargets, VectorN const& baseLinearAcceleration,
+                    VectorN const& baseAngularVelocity, VectorN const& baseOrientation, VectorN const& q,
+                    VectorN const& v, VectorN const& perfectPosition, Vector3 const& b_perfectVelocity) {
+  updatFeetStatus(gait, feetTargets);
+  updateIMUData(baseLinearAcceleration, baseAngularVelocity, baseOrientation, perfectPosition);
+  updateJointData(q, v);
+
+  updateForwardKinematics();
+  computeFeetPositionBarycenter();
+
+  estimateVelocity(b_perfectVelocity);
+  estimatePosition(perfectPosition.head(3));
+
+  filterVelocity();
+
+  vSecurity_ = (1 - alphaSecurity_) * vActuators_ + alphaSecurity_ * vSecurity_;
+}
+
+void Estimator::updateReferenceState(VectorN const& vRef) {
+  // Update reference acceleration and velocities
+  Matrix3 Rz = pinocchio::rpy::rpyToMatrix(0., 0., -baseVelRef_[5] * dt_);
+  baseAccRef_.head(3) = (vRef.head(3) - Rz * baseVelRef_.head(3)) / dt_;
+  baseAccRef_.tail(3) = (vRef.tail(3) - Rz * baseVelRef_.tail(3)) / dt_;
+  baseVelRef_ = vRef;
+
+  // Update position and velocity state vectors
+  Rz = pinocchio::rpy::rpyToMatrix(0., 0., qRef_[5]);
+  vRef_.head(2) = Rz.topLeftCorner(2, 2) * baseVelRef_.head(2);
+  vRef_[5] = baseVelRef_[5];
+  vRef_.tail(12) = vActuators_;
+
+  qRef_.head(2) += vRef_.head(2) * dt_;
+  qRef_[2] = qEstimate_[2];
+  qRef_.segment(3, 2) = IMURpy_.head(2);
+  qRef_[5] += baseVelRef_[5] * dt_;
+  qRef_.tail(12) = qActuators_;
+
+  // Transformation matrices
+  hRb_ = pinocchio::rpy::rpyToMatrix(IMURpy_[0], IMURpy_[1], 0.);
+  oRh_ = pinocchio::rpy::rpyToMatrix(0., 0., qRef_[5]);
+  oTh_.head(2) = qRef_.head(2);
+
+  // Express estimated velocity and filtered estimated velocity in horizontal frame
+  h_v_.head(3) = hRb_ * vEstimate_.head(3);
+  h_v_.tail(3) = hRb_ * vEstimate_.segment(3, 3);
+  h_vFiltered_.head(3) = hRb_ * vFiltered_.head(3);
+  h_vFiltered_.tail(3) = hRb_ * vFiltered_.tail(3);
+}
+
+void Estimator::updatFeetStatus(MatrixN const& gait, MatrixN const& feetTargets) {
+  feetStatus_ = gait.row(0);
+  feetTargets_ = feetTargets;
+
+  feetStancePhaseDuration_ += feetStatus_;
+  feetStancePhaseDuration_ = feetStancePhaseDuration_.cwiseProduct(feetStatus_);
+
+  phaseRemainingDuration_ = 1;
+  while (feetStatus_.isApprox((Vector4)gait.row(phaseRemainingDuration_))) {
+    phaseRemainingDuration_++;
+  }
+}
+
+void Estimator::updateIMUData(Vector3 const& baseLinearAcceleration, Vector3 const& baseAngularVelocity,
+                              Vector3 const& baseOrientation, VectorN const& perfectPosition) {
+  IMULinearAcceleration_ = baseLinearAcceleration;
+  IMUAngularVelocity_ = baseAngularVelocity;
+  IMURpy_ = baseOrientation;
+
+  if (!initialized_) {
+    IMUYawOffset_ = IMURpy_(2);
+    initialized_ = true;
+  }
+  IMURpy_(2) -= IMUYawOffset_;
+
+  if (solo3D_) {
+    IMURpy_.tail(1) = perfectPosition.tail(1);
+  }
+
+  IMUQuat_ = pinocchio::SE3::Quaternion(pinocchio::rpy::rpyToMatrix(IMURpy_(0), IMURpy_(1), IMURpy_(2)));
+}
+
+void Estimator::updateJointData(Vector12 const& q, Vector12 const& v) {
+  qActuators_ = q;
+  vActuators_ = v;
+}
+
+void Estimator::updateForwardKinematics() {
+  q_FK_.tail(12) = qActuators_;
+  v_FK_.tail(12) = vActuators_;
+  q_FK_.segment(3, 4) << 0., 0., 0., 1.;
+  pinocchio::forwardKinematics(velocityModel_, velocityData_, q_FK_, v_FK_);
+
+  q_FK_.segment(3, 4) = IMUQuat_.coeffs();
+  pinocchio::forwardKinematics(positionModel_, positionData_, q_FK_);
+
+  int nContactFeet = 0;
+  Vector3 baseVelocityEstimate = Vector3::Zero();
+  Vector3 basePositionEstimate = Vector3::Zero();
+  for (int foot = 0; foot < 4; foot++) {
+    if (feetStatus_(foot) == 1. && feetStancePhaseDuration_[foot] >= 16) {
+      baseVelocityEstimate += computeBaseVelocityFromFoot(foot);
+      basePositionEstimate += computeBasePositionFromFoot(foot);
+      nContactFeet++;
+    }
+  }
+
+  if (nContactFeet > 0) {
+    baseVelocityFK_ = baseVelocityEstimate / nContactFeet;
+    basePositionFK_ = basePositionEstimate / nContactFeet;
+  }
+}
+
+Vector3 Estimator::computeBaseVelocityFromFoot(int footId) {
+  pinocchio::updateFramePlacement(velocityModel_, velocityData_, feetFrames_[footId]);
+  pinocchio::SE3 contactFrame = velocityData_.oMf[feetFrames_[footId]];
+  Vector3 frameVelocity =
+      pinocchio::getFrameVelocity(velocityModel_, velocityData_, feetFrames_[footId], pinocchio::LOCAL).linear();
+
+  return contactFrame.translation().cross(IMUAngularVelocity_) - contactFrame.rotation() * frameVelocity;
+}
+
+Vector3 Estimator::computeBasePositionFromFoot(int footId) {
+  pinocchio::updateFramePlacement(positionModel_, positionData_, feetFrames_[footId]);
+  Vector3 basePosition = -positionData_.oMf[feetFrames_[footId]].translation();
+  basePosition(0) += footRadius_ * (vActuators_(1 + 3 * footId) + vActuators_(2 + 3 * footId));
+
+  return basePosition;
+}
+
+void Estimator::computeFeetPositionBarycenter() {
+  int nContactFeet = 0;
+  Vector3 feetPositions = Vector3::Zero();
+  for (int j = 0; j < 4; j++) {
+    if (feetStatus_(j) == 1.) {
+      feetPositions += feetTargets_.col(j);
+      nContactFeet++;
+    }
+  }
+  if (nContactFeet > 0) feetPositionBarycenter_ = feetPositions / nContactFeet;
+}
+
+double Estimator::computeAlphaVelocity() {
+  double a = std::ceil(feetStancePhaseDuration_.maxCoeff() * 0.1) - 1;
+  double b = static_cast<double>(phaseRemainingDuration_);
+  double c = ((a + b) - 2) * 0.5;
+  if (a <= 0 || b <= 1)
+    return alphaVelMax_;
+  else
+    return alphaVelMin_ + (alphaVelMax_ - alphaVelMin_) * std::abs(c - (a - 1)) / c;
+}
+
+void Estimator::estimateVelocity(Vector3 const& b_perfectVelocity) {
+  Vector3 alpha = Vector3::Ones() * computeAlphaVelocity();
+  Matrix3 oRb = IMUQuat_.toRotationMatrix();
+  Vector3 bTi = (b_M_IMU_.translation()).cross(IMUAngularVelocity_);
+
+  // At IMU location in world frame
+  Vector3 oi_baseVelocityFK = solo3D_ ? oRb * (b_perfectVelocity + bTi) : oRb * (baseVelocityFK_ + bTi);
+  Vector3 oi_baseVelocity = velocityFilter_.compute(oi_baseVelocityFK, oRb * IMULinearAcceleration_, alpha);
+
+  // At base location in base frame
+  b_baseVelocity_ = oRb.transpose() * oi_baseVelocity - bTi;
+
+  vEstimate_.head(3) = perfectEstimator_ ? b_perfectVelocity : b_baseVelocity_;
+  vEstimate_.segment(3, 3) = IMUAngularVelocity_;
+  vEstimate_.tail(12) = vActuators_;
+}
+
+void Estimator::estimatePosition(Vector3 const& perfectPosition) {
+  Matrix3 oRb = IMUQuat_.toRotationMatrix();
+
+  Vector3 basePosition = solo3D_ ? perfectPosition : (basePositionFK_ + feetPositionBarycenter_);
+  qEstimate_.head(3) = positionFilter_.compute(basePosition, oRb * b_baseVelocity_, alphaPos_);
+
+  if (perfectEstimator_ || solo3D_) qEstimate_(2) = perfectPosition(2);
+  qEstimate_.segment(3, 4) = IMUQuat_.coeffs();
+  qEstimate_.tail(12) = qActuators_;
+}
+
+void Estimator::filterVelocity() {
+  vFiltered_ = vEstimate_.head(6);
+  vx_queue_.pop_back();
+  vy_queue_.pop_back();
+  vz_queue_.pop_back();
+  vx_queue_.push_front(vEstimate_(0));
+  vy_queue_.push_front(vEstimate_(1));
+  vz_queue_.push_front(vEstimate_(2));
+  vFiltered_(0) = std::accumulate(vx_queue_.begin(), vx_queue_.end(), 0.) / windowSize_;
+  vFiltered_(1) = std::accumulate(vy_queue_.begin(), vy_queue_.end(), 0.) / windowSize_;
+  vFiltered_(2) = std::accumulate(vz_queue_.begin(), vz_queue_.end(), 0.) / windowSize_;
+}

src/Filter.cpp

+#include "qrw/Filter.hpp"
+
+Filter::Filter()
+    : b_(0.),
+      a_(Vector2::Zero()),
+      x_(Vector6::Zero()),
+      y_(VectorN::Zero(6, 1)),
+      accum_(Vector6::Zero()),
+      init_(false) {
+  // Empty
+}
+
+void Filter::initialize(Params& params) {
+  const double fc = 15.0;
+  b_ = (2 * M_PI * params.dt_wbc * fc) / (2 * M_PI * params.dt_wbc * fc + 1.0);
+
+  a_ << 1.0, -(1.0 - b_);
+
+  x_queue_.resize(1, Vector6::Zero());
+  y_queue_.resize(a_.rows() - 1, Vector6::Zero());
+}
+
+VectorN Filter::filter(Vector6 const& x, bool check_modulo) {
+  // Retrieve measurement
+  x_ = x;
+
+  // Handle modulo for orientation
+  if (check_modulo) {
+    // Handle 2 pi modulo for roll, pitch and yaw
+    // Should happen sometimes for yaw but now for roll and pitch except
+    // if the robot rolls over
+    for (int i = 3; i < 6; i++) {
+      if (std::abs(x_(i, 0) - y_(i, 0)) > 1.5 * M_PI) {
+        handle_modulo(i, x_(i, 0) - y_(i, 0) > 0);
+      }
+    }
+  }
+
+  // Initialisation of the value in the queues to the first measurement
+  if (!init_) {
+    init_ = true;
+    std::fill(x_queue_.begin(), x_queue_.end(), x_.head(6));
+    std::fill(y_queue_.begin(), y_queue_.end(), x_.head(6));
+  }
+
+  // Store measurement in x queue
+  x_queue_.pop_back();
+  x_queue_.push_front(x_.head(6));
+
+  // Compute result (y/x = b/a for the transfert function)
+  accum_ = b_ * x_queue_[0];
+  for (int i = 1; i < a_.rows(); i++) {
+    accum_ -= a_[i] * y_queue_[i - 1];
+  }
+
+  // Store result in y queue for recursion
+  y_queue_.pop_back();
+  y_queue_.push_front(accum_ / a_[0]);
+
+  // Filtered result is stored in y_queue_.front()
+  // Assigned to dynamic-sized vector for binding purpose
+  y_ = y_queue_.front();
+
+  return y_;
+}
+
+void Filter::handle_modulo(int a, bool dir) {
+  // Add or remove 2 PI to all elements in the queues
+  x_queue_[0](a, 0) += dir ? 2.0 * M_PI : -2.0 * M_PI;
+  for (int i = 1; i < a_.rows(); i++) {
+    (y_queue_[i - 1])(a, 0) += dir ? 2.0 * M_PI : -2.0 * M_PI;
+  }
+}

src/Params.cpp

@@ -24,6 +24,7 @@ Params::Params()
       N_periods(0),
       type_MPC(0),
       save_guess(false),
+      movement(""),
       interpolate_mpc(true),
       interpolation_type(0),
       kf_enabled(false),
@@ -144,6 +145,9 @@ void Params::initialize(const std::string& file_path) {
   assert_yaml_parsing(robot_node, "robot", "save_guess");
   save_guess = robot_node["save_guess"].as<bool>();
 
+  assert_yaml_parsing(robot_node, "robot", "movement");
+  movement = robot_node["movement"].as<std::string>();
+
   assert_yaml_parsing(robot_node, "robot", "interpolate_mpc");
   interpolate_mpc = robot_node["interpolate_mpc"].as<bool>();