General improvements for FloatingBaseEstimators library

CHANGELOG.md

@@ -36,6 +36,7 @@ All notable changes to this project are documented in this file.
 - Restructure python bindings in submodules (https://github.com/dic-iit/bipedal-locomotion-framework/pull/238)
 - Integrators and DynamicalSystems are now in the `ContinuousDynamicalSystem` component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/242)
 - Add Input template class to `System::Advanceable` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/267)
+- Add support for landmarks and kinematics-free estimation in `FloatingBaseEstimators`. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/254)
 
 ### Fixed
 - Fix missing implementation of `YarpSensorBridge::getFailedSensorReads()`. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/202)

src/Contacts/include/BipedalLocomotion/Contacts/Contact.h

@@ -111,6 +111,7 @@ struct EstimatedContact : ContactBase
 
 };
 
+using EstimatedLandmark = EstimatedContact;
 
 }
 }

src/Estimators/CMakeLists.txt

@@ -19,6 +19,6 @@ if(FRAMEWORK_COMPILE_FloatingBaseEstimators)
  SOURCES src/ModelComputationsHelper.cpp src/FloatingBaseEstimator.cpp src/InvariantEKFBaseEstimator.cpp src/LeggedOdometry.cpp
  SUBDIRECTORIES tests/FloatingBaseEstimators
  PUBLIC_HEADERS ${H_PREFIX}/FloatingBaseEstimatorParams.h ${H_PREFIX}/FloatingBaseEstimatorIO.h ${H_PREFIX}/FloatingBaseEstimator.h ${H_PREFIX}/InvariantEKFBaseEstimator.h ${H_PREFIX}/LeggedOdometry.h ${H_PREFIX}/ModelComputationsHelper.h
- PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler BipedalLocomotion::ManifConversions iDynTree::idyntree-high-level iDynTree::idyntree-core iDynTree::idyntree-model BipedalLocomotion::System Eigen3::Eigen BipedalLocomotion::Contacts iDynTree::idyntree-modelio-urdf
+ PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler BipedalLocomotion::ManifConversions iDynTree::idyntree-high-level iDynTree::idyntree-core iDynTree::idyntree-model BipedalLocomotion::System Eigen3::Eigen BipedalLocomotion::Contacts iDynTree::idyntree-modelio-urdf BipedalLocomotion::TextLogging
  PRIVATE_LINK_LIBRARIES MANIF::manif)
 endif()

src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimator.h

@@ -5,8 +5,8 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_H
-#define BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_H
+#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_H
+#define BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_H
 
 #include <BipedalLocomotion/System/Source.h>
 #include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
@@ -161,6 +161,13 @@ class FloatingBaseEstimator : public BipedalLocomotion::System::Source<FloatingB
  std::shared_ptr<iDynTree::KinDynComputations> kindyn);
 
 
+ /**
+ * Configure generic parameters, calling this overloaded method assumes model information is not going to be used.
+ * @param[in] handler configure the generic parameters for the estimator
+ * @return True in case of success, false otherwise.
+ */
+ bool initialize(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler);
+
  /**
  * Set the polled IMU measurement
  * @param[in] accMeas linear accelerometer measurement expressed in the local IMU frame (m/s^2)
@@ -201,6 +208,20 @@ class FloatingBaseEstimator : public BipedalLocomotion::System::Source<FloatingB
  bool setKinematics(const Eigen::VectorXd& encoders,
  const Eigen::VectorXd& encoderSpeeds);
 
+ /**
+ * Set the relative pose of a landmark relative to the base link
+ *
+ * @param[in] landmarkID unique landmark ID
+ * @param[in] quat relative orientation of the landmark as a quaternion
+ * @param[in] pos relative position of the landmark
+ * @param[in] timeNow relative position of the landmark
+ * @return true in case of success, false otherwise
+ */
+ bool setLandmarkRelativePose(const int& landmarkID,
+ const Eigen::Quaterniond& quat,
+ const Eigen::Vector3d& pos,
+ const double& timeNow);
+
  /**
  * Compute one step of the estimator
  * @return True in case of success, false otherwise.
@@ -247,13 +268,6 @@ class FloatingBaseEstimator : public BipedalLocomotion::System::Source<FloatingB
  ModelComputations& modelComputations();
 
 protected:
- /**
- * Configure generic parameters
- * @param[in] handler configure the generic parameters for the estimator
- * @return True in case of success, false otherwise.
- */
- bool initialize(std::weak_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler);
-
  /**
  * These custom parameter specifications should be specified by the derived class.
  * - If setupOptions() is called from within customInitialization(), ensure the group "Options" exist.
@@ -289,6 +303,15 @@ class FloatingBaseEstimator : public BipedalLocomotion::System::Source<FloatingB
  virtual bool updateKinematics(FloatingBaseEstimators::Measurements& meas,
  const double& dt) { return true; };
 
+ /**
+ * Update the predicted state estimates using relative pose measurements of static landmarks in the environment
+ * @param[in] meas measurements to update the predicted states
+ * @param[in] dt sampling period in seconds
+ * @return True in case of success, false otherwise.
+ */
+ virtual bool updateLandmarkRelativePoses(FloatingBaseEstimators::Measurements& meas,
+ const double& dt) { return true; };
+
  /**
  * Setup estimator options. The parameters in the Options group are,
  * - enable_imu_bias_estimation [PARAMETER|-|Enable/disable IMU bias estimation]
@@ -381,6 +404,8 @@ class FloatingBaseEstimator : public BipedalLocomotion::System::Source<FloatingB
  double m_dt{0.01}; /**< Fixed time step of the estimator, in seconds */
  bool m_useIMUForAngVelEstimate{true}; /**< Use IMU measurements as internal state imu angular velocity by default set to true for strap down IMU based EKF implementations, if IMU measurements not used, corresponding impl can set to false */
  bool m_useIMUVelForBaseVelComputation{true}; /**< Compute base velocity using inertnal state IMU velocity. by default set to true for strap down IMU based EKF implementations, if IMU measurements not used, corresponding impl can set to false */
+ bool m_useModelInfo{true}; /**< Flag to enable running the estimator without using the URDF model information*/
+ bool m_isInvEKF{false}; /**< Flag to maintain soon to be deprecated functionalities currently existing only in InvEKFBaseEstimator */
 private:
  /**
  * Setup model related parameters
@@ -420,4 +445,4 @@ class FloatingBaseEstimator : public BipedalLocomotion::System::Source<FloatingB
 } // namespace Estimators
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_H
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_H

src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimatorIO.h

@@ -5,13 +5,14 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_IO_H
-#define BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_IO_H
+#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_IO_H
+#define BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_IO_H
 
 #include <Eigen/Dense>
 #include <manif/manif.h>
 
 #include <BipedalLocomotion/Contacts/Contact.h>
+#include <manif/manif.h>
 #include <map>
 
 namespace BipedalLocomotion
@@ -38,11 +39,12 @@ struct InternalState
  Eigen::Vector3d gyroscopeBias; /**< Bias of the gyroscope expressed in the IMU frame */
 
  Eigen::Vector3d imuAngularVelocity; /**< angular velocity of the IMU with respect to the inertial frame expressed in inertial frame, typically unused for strap-down IMU based EKF implementations*/
- std::map<int, BipedalLocomotion::Contacts::EstimatedContact> supportFrameData; /**< contact measurements */
+ std::map<int, BipedalLocomotion::Contacts::EstimatedContact> supportFrameData; /**< estimated contacts */
+ std::map<int, BipedalLocomotion::Contacts::EstimatedLandmark> landmarkData; /**< estimated landmarks */
 };
 
 /**
-* @brief Struct holding the elements of the state representation
+* @brief Struct holding the elements of the estimator output
 *
 */
 struct Output
@@ -55,7 +57,7 @@ struct Output
 };
 
 /**
-* @brief Struct holding the elements of the state representation
+* @brief Struct holding the elements of the measurements data
 *
 */
 struct Measurements
@@ -65,17 +67,37 @@ struct Measurements
  bool lfInContact{false}; /**< left foot contact state */
  bool rfInContact{false}; /**< right foot contact state */
 
+ /** stamped relative poses,
+ *
+ * @note at the implementation level of FloatingBaseEstimator,
+ * the usage of this map is in a way
+ * that at every iteration, at the end of the
+ * advance() call of the estimator, this map is cleared
+ * so as to not use delayed or outdated measurments
+ *
+ * @note we maintain a map (sorted elements) in order to
+ * maintain accessibility of incrementally augmented variables
+ * from the state and the covariance matrix
+ */
+ std::map<int, BipedalLocomotion::Contacts::EstimatedContact > stampedRelLandmarkPoses;
+
  /** stamped contact status,
- * the usage of this map must be in a way
- * that every time an element is used,
- * it must be erased from the map
- */
+ *
+ * @note at the implementation level of FloatingBaseEstimator,
+ * the usage of this map is in a way
+ * that at every iteration, at the end of the
+ * advance() call of the estimator, this map is cleared
+ * so as to not use delayed or outdated measurments
+ *
+ * @note we maintain a map (sorted elements) in order to
+ * maintain accessibility of incrementally augmented variables
+ * from the state and the covariance matrix
+ */
  std::map<int, BipedalLocomotion::Contacts::EstimatedContact > stampedContactsStatus;
 };
 
 } // namespace FloatingBaseEstimators
 } // namespace Estimators
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_IO_H
-
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_IO_H

src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimatorParams.h

@@ -5,17 +5,23 @@
  * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
  */
 
-#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_PARAMS_H
-#define BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_PARAMS_H
+#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_PARAMS_H
+#define BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_PARAMS_H
 
 #include <Eigen/Dense>
+#include <map>
 
 namespace BipedalLocomotion
 {
 namespace Estimators
 {
 namespace FloatingBaseEstimators
 {
+
+// see https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html
+using PoseCovariance = std::map<int, Eigen::Matrix<double, 6, 1>, std::less<int>,
+ Eigen::aligned_allocator<std::pair<const int, Eigen::Matrix<double, 6, 1> > > >;
+
 /**
 * @brief Struct containing sensor measurement deviation parameters of floating base estimators
 *
@@ -80,6 +86,28 @@ struct SensorsStdDev
  * @brief White Gaussian noise deviation for encoder measurements in continuous time
  */
  Eigen::VectorXd encodersNoise;
+
+ /**
+ * @brief White Gaussian noise deviation for landmark relative pose predictions in continuous time
+ */
+ Eigen::Matrix<double, 6, 1> landmarkPredictionNoise;
+
+ /**
+ * @brief White Gaussian noise deviation for landmark relative pose measurments in continuous time
+ */
+ Eigen::Matrix<double, 6, 1> landmarkMeasurementNoise;
+
+ /**
+ * @brief White Gaussian noise deviation for linear velocity of IMU frame wrt inertial frame
+ * Expressed in local frame as m/s, in continuous time
+ */
+ Eigen::Vector3d imuFrameLinearVelocityNoise;
+
+ /**
+ * @brief White Gaussian noise deviation for angular velocity of IMU frame wrt inertial frame
+ * Expressed in local frame as rad/s, in continuous time
+ */
+ Eigen::Vector3d imuFrameAngularVelocityNoise;
 };
 
 /**
@@ -106,6 +134,12 @@ struct StateStdDev
  */
  Eigen::Vector3d imuLinearVelocity;
 
+ /**
+ * @brief Prior deviation of mixed-trivialized IMU angular velocity
+ * expressed in rad/s
+ */
+ Eigen::Vector3d imuAngularVelocity;
+
  /**
  * @brief Prior deviation of left foot contact frame orientation in inertial frame
  * expressed in radians as rpy
@@ -142,6 +176,16 @@ struct StateStdDev
  * expressed in rad/s
  */
  Eigen::Vector3d gyroscopeBias;
+
+ /**
+ * @brief Container of deviations of support frame pose in inertial frame
+ */
+ PoseCovariance supportFramePose;
+
+ /**
+ * @brief Container of deviations of landmark pose in inertial frame
+ */
+ PoseCovariance landmarkPose;
 };
 
 
@@ -178,6 +222,17 @@ struct Options
  */
  bool ekfUpdateEnabled{true};
 
+ /**
+ * @brief Enable/disable kinematics based correction updates
+ */
+ bool kinematicsUpdateEnabled{true};
+
+ /**
+ * @brief Enable/disable landmarks based correction updates
+ */
+ bool staticLandmarksUpdateEnabled{false};
+
+
  /**
  * @brief Acceleration vector due to gravity
  *
@@ -189,4 +244,4 @@ struct Options
 } // namespace Estimators
 } // namespace BipedalLocomotion
 
-#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FBE_PARAMS_H
+#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FLOATING_BASE_ESTIMATOR_PARAMS_H