LeggedEstimator.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <gtsam/base/MatrixConstants.h>
#include <gtsam/geometry/ExtendedPose3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/LeftLinearEKF.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/navigation/PreintegrationParams.h>
#include <gtsam/nonlinear/BatchFixedLagSmoother.h>
 
#include <memory>
#include <optional>
#include <string>
#include <vector>
 
namespace gtsam {

struct ContactMeasurement {
  size_t foot = 0;
  Vector3 bodyPoint = Vector3::Zero();
  bool touchdown = false;
};

struct LeggedEstimatorParams {
  std::shared_ptr<PreintegrationParams> preintegrationParams;
  Pose3 body_P_imu = Pose3(Rot3(), Point3(0.30, 0.0, 0.15));
  double footholdProcessSigma = 1e-4;
  double footholdInitSigma = 5e-1;
  Matrix3 contactCovariance =
      (Vector3(0.03 * 0.03, 0.03 * 0.03, 0.02 * 0.02)).asDiagonal();
  double heightPriorSigma = 0.15;
  bool useRobustContactNoise = false;
  double robustContactHuberK = 2.0;
  imuBias::ConstantBias imuBias;
  double biasAccRandomWalkSigma = 5e-3;
  double biasOmegaRandomWalkSigma = 1e-4;
  bool useFullContactInitialization = true;
  bool marginalizeLeavingFoot = true;
};

class GTSAM_EXPORT LeggedEstimator {
 public:
  virtual ~LeggedEstimator() = default;

  void turnHeightPriorOn(double terrainHeight) {
    terrainHeight_ = terrainHeight;
  }

  void turnHeightPriorOff() { terrainHeight_.reset(); }

  virtual void predict(const Vector3& omegaBody,
                       const Vector3& specificForceBody, double dt) = 0;

  virtual void processContacts(
      const std::vector<ContactMeasurement>& activeContacts) = 0;

  virtual ExtendedPose3d estimate() const = 0;

  virtual imuBias::ConstantBias estimateBias() const = 0;
 
 protected:
  const std::optional<double>& terrainHeight() const { return terrainHeight_; }

  static ExtendedPose3d MakeEstimate(const NavState& navState,
                                     const Matrix& footholds);

  static NavState EstimateNavState(const ExtendedPose3d& estimate) {
    return NavState(estimate.rotation(), estimate.x(0), estimate.x(1));
  }

  static Matrix EstimateFootholds(const ExtendedPose3d& estimate);
 
 private:
  std::optional<double> terrainHeight_;
};

class GTSAM_EXPORT LeggedInvariantEKF : public LeftLinearEKF<ExtendedPose3d>,
                                        public LeggedEstimator {
 public:
  using EkfBase = LeftLinearEKF<ExtendedPose3d>;
  using TangentVector = typename EkfBase::TangentVector;
  using Jacobian = typename EkfBase::Jacobian;
  using Covariance = typename EkfBase::Covariance;

  LeggedInvariantEKF(const NavState& navState0, const Matrix& footholds0,
                     const Matrix& P0, const LeggedEstimatorParams& params,
                     const std::vector<std::string>& footNames = {});

  Matrix covariance() const { return EkfBase::covariance(); }

  size_t numFeet() const { return numFeet_; }

  const std::vector<std::string>& footNames() const { return footNames_; }

  const LeggedEstimatorParams& params() const { return params_; }

  ExtendedPose3d estimate() const override {
    return MakeEstimate(baseState(), footholdMatrix());
  }

  imuBias::ConstantBias estimateBias() const override {
    return params_.imuBias;
  }

  void predict(const Vector3& omegaBody, const Vector3& specificForceBody,
               double dt) override;

  void processContacts(
      const std::vector<ContactMeasurement>& activeContacts) override;

  struct AutonomousFlow {
    explicit AutonomousFlow(size_t numFeet, double dt)
        : numFeet(numFeet), dt(dt) {}

    Matrix dIdentity() const {
      Matrix Phi = Matrix::Identity(9 + 3 * static_cast<int>(numFeet),
                                    9 + 3 * static_cast<int>(numFeet));
      Phi.block(3, 6, 3, 3) = I_3x3 * dt;
      return Phi;
    }

    ExtendedPose3d operator()(const ExtendedPose3d& state) const {
      Matrix blocks = state.xMatrix();
      blocks.col(0) += blocks.col(1) * dt;
      return ExtendedPose3d(state.rotation(), blocks);
    }
    size_t numFeet;
    double dt;
  };

  static ExtendedPose3d MakeState(const NavState& navState,
                                  const Matrix& footholds);

  static ExtendedPose3d GravityIncrement(size_t numFeet, const Vector3& gravity,
                                         double dt);

  static ExtendedPose3d ImuIncrement(size_t numFeet, const Vector3& omegaBody,
                                     const Vector3& specificForceBody,
                                     double dt);

  static size_t FootColumn(size_t foot) { return 2 + foot; }
 
 protected:
  NavState baseState() const {
    return {this->X_.rotation(), this->X_.x(0), this->X_.x(1)};
  }
  Matrix footholdMatrix() const {
    return this->X_.xMatrix().rightCols(static_cast<Eigen::Index>(numFeet()));
  }
  void resetFootToMeasurement(size_t foot, const Vector3& bodyPoint);
  void marginalizeFoot(size_t foot);
  virtual void applyContactUpdate(
      const std::vector<ContactMeasurement>& activeContacts);
  bool awaitingFullContactInitialization() const {
    return params_.useFullContactInitialization && !fullContactInitialized_;
  }
 
 private:
  bool maybeInitializeFromFullContact(
      const std::vector<ContactMeasurement>& activeContacts,
      const std::vector<bool>& activeFeet);
  Covariance processNoise(double dt) const;
  void applySingleContactUpdate(size_t foot, const Vector3& bodyPoint,
                                const Matrix3& covariance);
  void applySingleHeightPrior(size_t foot, double terrainHeight);
 
  size_t numFeet_;
  LeggedEstimatorParams params_;
  std::vector<std::string> footNames_;
  std::vector<bool> inContact_;
  std::vector<bool> initialized_;
  bool fullContactInitialized_ = false;
};

class GTSAM_EXPORT LeggedInvariantIEKF : public LeggedInvariantEKF {
 public:
  LeggedInvariantIEKF(const NavState& navState0, const Matrix& footholds0,
                      const Matrix& P0, const LeggedEstimatorParams& params,
                      const std::vector<std::string>& footNames = {});
 
 protected:
  void applyContactUpdate(
      const std::vector<ContactMeasurement>& activeContacts) override;
};

class GTSAM_EXPORT LeggedFixedLagSmoother : public LeggedEstimator {
 public:
  LeggedFixedLagSmoother(const NavState& navState0, const Matrix& footholds0,
                         const Matrix9& baseCovariance0,
                         const LeggedEstimatorParams& params, double lagSeconds,
                         const std::vector<std::string>& footNames = {});

  ~LeggedFixedLagSmoother() override;

  size_t numFeet() const { return numFeet_; }

  ExtendedPose3d estimate() const override;

  imuBias::ConstantBias estimateBias() const override { return biasEstimate_; }

  void predict(const Vector3& omegaBody, const Vector3& specificForceBody,
               double dt) override;

  void processContacts(
      const std::vector<ContactMeasurement>& activeContacts) override;
 
 private:
  bool maybeInitializeFromFullContact(
      const std::vector<ContactMeasurement>& activeContacts,
      const std::vector<bool>& activeFeet);
  void refreshEstimateFromSmoother();
  NavState currentBaseState() const { return optimizedBaseState_; }
  Key currentBaseKey() const { return MakeBaseKey(step_); }
  static Key MakeBaseKey(size_t step) {
    return Symbol('x', static_cast<uint64_t>(step));
  }
  static Key MakeBiasKey() { return Symbol('b', 0); }
  static Key MakeFootKey(size_t foot, size_t episode) {
    return Symbol('f', static_cast<uint64_t>(1000 * foot + episode));
  }
  bool hasPendingImu() const { return pim_.deltaTij() > 0.0; }
  bool graphInitialized() const {
    return !params_.useFullContactInitialization || fullContactInitialized_;
  }
  bool awaitingFullContactInitialization() const {
    return params_.useFullContactInitialization && !fullContactInitialized_;
  }
 
  size_t numFeet_;
  LeggedEstimatorParams params_;
  std::vector<std::string> footNames_;
  Matrix initialFootholds_;
  Matrix9 baseCovariance0_;
  BatchFixedLagSmoother smoother_;
  PreintegratedImuMeasurements pim_;
  size_t step_ = 0;
  double currentTime_ = 0.0;
  std::vector<bool> inContact_;
  std::vector<bool> initialized_;
  std::vector<size_t> footEpisodes_;
  std::vector<std::optional<Key>> activeFootKeys_;
  NavState optimizedBaseState_;
  NavState deadReckonedState_;
  imuBias::ConstantBias biasEstimate_;
  bool fullContactInitialized_ = false;
};

class GTSAM_EXPORT LeggedCombinedFixedLagSmoother : public LeggedEstimator {
 public:
  LeggedCombinedFixedLagSmoother(
      const NavState& navState0, const Matrix& footholds0,
      const Matrix9& baseCovariance0, const LeggedEstimatorParams& params,
      double lagSeconds, const std::vector<std::string>& footNames = {});

  ~LeggedCombinedFixedLagSmoother() override;

  size_t numFeet() const { return numFeet_; }

  ExtendedPose3d estimate() const override;

  imuBias::ConstantBias estimateBias() const override { return biasEstimate_; }

  void predict(const Vector3& omegaBody, const Vector3& specificForceBody,
               double dt) override;

  void processContacts(
      const std::vector<ContactMeasurement>& activeContacts) override;
 
 private:
  bool maybeInitializeFromFullContact(
      const std::vector<ContactMeasurement>& activeContacts,
      const std::vector<bool>& activeFeet);
  void refreshEstimateFromSmoother();
  NavState currentBaseState() const { return optimizedBaseState_; }
  Key currentPoseKey() const { return MakePoseKey(step_); }
  Key currentVelocityKey() const { return MakeVelocityKey(step_); }
  Key currentBiasKey() const { return MakeBiasKey(step_); }
  static Key MakePoseKey(size_t step) {
    return Symbol('x', static_cast<uint64_t>(step));
  }
  static Key MakeVelocityKey(size_t step) {
    return Symbol('v', static_cast<uint64_t>(step));
  }
  static Key MakeBiasKey(size_t step) {
    return Symbol('b', static_cast<uint64_t>(step));
  }
  static Key MakeFootKey(size_t foot, size_t episode) {
    return Symbol('f', static_cast<uint64_t>(1000 * foot + episode));
  }
  bool hasPendingImu() const { return pim_.deltaTij() > 0.0; }
  bool graphInitialized() const {
    return !params_.useFullContactInitialization || fullContactInitialized_;
  }
  bool awaitingFullContactInitialization() const {
    return params_.useFullContactInitialization && !fullContactInitialized_;
  }
 
  size_t numFeet_;
  LeggedEstimatorParams params_;
  std::vector<std::string> footNames_;
  Matrix initialFootholds_;
  Matrix9 baseCovariance0_;
  BatchFixedLagSmoother smoother_;
  PreintegratedCombinedMeasurements pim_;
  size_t step_ = 0;
  double currentTime_ = 0.0;
  std::vector<bool> inContact_;
  std::vector<bool> initialized_;
  std::vector<size_t> footEpisodes_;
  std::vector<std::optional<Key>> activeFootKeys_;
  NavState optimizedBaseState_;
  NavState deadReckonedState_;
  imuBias::ConstantBias biasEstimate_;
  bool fullContactInitialized_ = false;
};
 
}  // namespace gtsam