diff --git a/src/Estimators/CMakeLists.txt b/src/Estimators/CMakeLists.txt index 2567991926..6421a9b717 100644 --- a/src/Estimators/CMakeLists.txt +++ b/src/Estimators/CMakeLists.txt @@ -16,9 +16,9 @@ if(FRAMEWORK_COMPILE_FloatingBaseEstimators) set(H_PREFIX include/BipedalLocomotion/FloatingBaseEstimators) add_bipedal_locomotion_library( NAME FloatingBaseEstimators - SOURCES src/ModelComputationsHelper.cpp src/FloatingBaseEstimator.cpp src/InvariantEKFBaseEstimator.cpp src/LeggedOdometry.cpp + SOURCES src/ModelComputationsHelper.cpp src/FloatingBaseEstimator.cpp src/InvariantEKFBaseEstimator.cpp src/LeggedOdometry.cpp src/CenterOfMassKalmanFilter.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_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 ${H_PREFIX}/CenterOfMassKalmanFilter.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 BipedalLocomotion::TextLogging PRIVATE_LINK_LIBRARIES MANIF::manif) endif() diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/CenterOfMassKalmanFilter.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/CenterOfMassKalmanFilter.h new file mode 100644 index 0000000000..5b2ca25e11 --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/CenterOfMassKalmanFilter.h @@ -0,0 +1,96 @@ +/** + * @file CenterOfMassKalmanFilter.h + * @authors Prashanth Ramadoss + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_CENTER_OF_MASS_KALMAN_FILTER_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_CENTER_OF_MASS_KALMAN_FILTER_H + +#include +#include +#include + +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ + +/** + * Internal state of a constant height CoM Kalman Filter + */ +struct CoMKFState +{ + Eigen::Vector2d comPosition; + Eigen::Vector2d comVelocity; + Eigen::Vector2d comOffset; +}; + +struct CoMKFInput +{ + Eigen::Vector2d globalCoP; + Eigen::Vector3d acc, gyro; +}; + +/** + * Implements the constant height LIPM model based Kalman filter + * described in the paper, + * "Center of Mass Estimator for Humanoids and its Application in + * Modelling Error Compensation, Fall Detection and Prevention" + * to estimate the horizontal center of mass position and velocity + * + * assumptions, + * - constant height CoM LIPM model + * - the IMU is rigidly attached to the base link + * - CoM is always close to the same rigid body as the IMU + * - constant angular velocity is assumed, making angular acceleration is zero + */ +class CenterOfMassKalmanFilter : public BipedalLocomotion::System::Advanceable +{ +public: + CenterOfMassKalmanFilter(); + ~CenterOfMassKalmanFilter(); + /** + * Initialize the estimator. + * @param paramHandler pointer to the parameters handler. + * @note the following parameters are required by the class + * | Parameter Name | Type | Description | Mandatory | + * |:-----------------------------------------:|:--------:|:--------------------------------------------------------------------------------------:|:---------:| + * | `sampling_time` | `double` | Sampling period in seconds | Yes | + * | `base_link_imu` | `string` | name of the IMU attached to the base link | Yes | + * | `com_position_measurement_noise_var` | `double` | covariance of center of mass position measurement noise | Yes | + * | `com_acceleration_measurement_noise_var` | `double` | covariance of center of mass acceleration measurement noise | Yes | + * | `com_position_prediction_noise_var` | `double` | covariance of center of mass position prediction noise | Yes | + * | `com_velocity_prediction_noise_var` | `double` | covariance of center of mass velocity prediction noise | Yes | + * | `com_offset_prediction_noise_var` | `double` | covariance of center of mass offset prediction noise | Yes | + */ + bool initialize(std::weak_ptr handler) override; + + /** + * @note: This estimator will not modify the state of the KinDyn. + * Assumes that the kindyn state is set externally before every advance() call of this method + */ + bool setKinDynObject(std::shared_ptr kinDyn); + bool setInitialState(const CoMKFState& initialState); + bool setInput(const CoMKFInput& input) override; + + bool setGlobalCenterOfPressure(const double& copX, const double& copY); + bool setBaseLinkIMUMeasurement(Eigen::Ref acc, + Eigen::Ref gyro); + bool advance() override; + + const CoMKFState& getOutput() const override; + bool isOutputValid() const override; +private: + class Impl; + std::unique_ptr m_pimpl; +}; + +} // namespace Estimators +} // namespace BipedalLocomotion + + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_CENTER_OF_MASS_KALMAN_FILTER_H diff --git a/src/Estimators/src/CenterOfMassKalmanFilter.cpp b/src/Estimators/src/CenterOfMassKalmanFilter.cpp new file mode 100644 index 0000000000..d2a798ebf5 --- /dev/null +++ b/src/Estimators/src/CenterOfMassKalmanFilter.cpp @@ -0,0 +1,366 @@ +/** + * @file CenterOfMassKalmanFilter.cpp + * @authors Prashanth Ramadoss + * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include +#include +#include +#include +#include + +using namespace BipedalLocomotion::Estimators; +using namespace BipedalLocomotion; + +class CenterOfMassKalmanFilter::Impl +{ +public: + // KF matrices + void prepareAandB(); + void prepareCandD(); + void prepareQandR(); + void prepareMeasurementVector(); + + bool predict(); + bool update(); + + Eigen::Matrix x; + + Eigen::Matrix A, I6; + Eigen::Matrix B; + Eigen::Matrix C; + Eigen::Matrix D; + + Eigen::Matrix P; + Eigen::Matrix Q; + Eigen::Matrix R, S; + Eigen::Matrix K, CT; + + Eigen::Vector4d z, y; + double sigma2_y_com, sigma2_y_ddcom; + double sigma2_x_com, sigma2_dx_com, sigma2_xoff_com; + + Eigen::Matrix2d I2; + double dt; + double omegaSquared; + Eigen::Vector3d A_g; + + CoMKFInput input; + CoMKFState state; + std::string imuName; + + std::shared_ptr kinDyn; + + // maintain some iDynTree buffers + // not using manif here because of depends on KinDyn + Eigen::Matrix3d Sw; // S(omega) + iDynTree::Position A_o_G; + iDynTree::Transform A_H_IMU; + iDynTree::Position A_o_IMU; + iDynTree::Rotation A_R_IMU; + iDynTree::Position IMU_o_G; + bool initialized{false}; +}; + +CenterOfMassKalmanFilter::CenterOfMassKalmanFilter() : +m_pimpl(std::make_unique()) +{ + m_pimpl->I2.setIdentity(); + m_pimpl->I6.setIdentity(); + + m_pimpl->A.setIdentity(); + m_pimpl->B.setZero(); + m_pimpl->C.setZero(); + m_pimpl->D.setZero(); + + m_pimpl->P.setZero(); + m_pimpl->Q.setZero(); + m_pimpl->R.setZero(); + + m_pimpl->CT.setZero(); + m_pimpl->S.setZero(); + m_pimpl->K.setZero(); + + m_pimpl->z.setZero(); + m_pimpl->y.setZero(); + + m_pimpl->A_g.setZero(); + m_pimpl->A_g(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation; +} + +CenterOfMassKalmanFilter::~CenterOfMassKalmanFilter() +{ +} + +bool CenterOfMassKalmanFilter::setInitialState(const CoMKFState& initialState) +{ + m_pimpl->x.segment<2>(0) = initialState.comPosition; + m_pimpl->x.segment<2>(2) = initialState.comVelocity; + m_pimpl->x.segment<2>(4) = initialState.comOffset; + return true; +} + +bool CenterOfMassKalmanFilter::initialize(std::weak_ptr handler) +{ + auto handle = handler.lock(); + const std::string printPrefix{"[CenterOfMassKalmanFilter::initialize]"}; + if (handle == nullptr) + { + log()->error("{} The parameter handler has expired. " + "Please check its scope.", printPrefix); + return false; + } + + if (!handle->getParameter("sampling_time", m_pimpl->dt)) + { + log()->error("{} Unable to initialize the sampling time.", printPrefix); + return false; + } + + if (!handle->getParameter("base_link_imu", m_pimpl->imuName)) + { + log()->error("{} Unable to initialize the IMU name.", printPrefix); + return false; + } + + if (!handle->getParameter("com_position_measurement_noise_var", m_pimpl->sigma2_y_com)) + { + log()->error("{} Unable to initialize the com position measurement noise variance.", printPrefix); + return false; + } + + if (!handle->getParameter("com_acceleration_measurement_noise_var", m_pimpl->sigma2_y_ddcom)) + { + log()->error("{} Unable to initialize the IMU name.", printPrefix); + return false; + } + + if (!handle->getParameter("com_position_prediction_noise_var", m_pimpl->sigma2_x_com)) + { + log()->error("{} Unable to initialize the IMU name.", printPrefix); + return false; + } + + if (!handle->getParameter("com_velocity_prediction_noise_var", m_pimpl->sigma2_dx_com)) + { + log()->error("{} Unable to initialize the IMU name.", printPrefix); + return false; + } + + if (!handle->getParameter("com_offset_prediction_noise_var", m_pimpl->sigma2_xoff_com)) + { + log()->error("{} Unable to initialize the IMU name.", printPrefix); + return false; + } + + m_pimpl->prepareQandR(); + + return true; +} + +bool CenterOfMassKalmanFilter::setKinDynObject(std::shared_ptr kinDyn) +{ + const std::string printPrefix{"[CenterOfMassKalmanFilter::setKinDynObject]"}; + if (m_pimpl->initialized) + { + log()->error("{} Please initialize the estimator before calling this method."); + return false; + } + + if (!kinDyn->isValid()) + { + log()->error("{} Invalid KinDynComputation object."); + return false; + } + + auto imuIdx = kinDyn->model().getFrameIndex(m_pimpl->imuName); + if (imuIdx == iDynTree::FRAME_INVALID_INDEX) + { + log()->error("{} The configured IMU does not seem to be available in the loaded model."); + return false; + } + + auto baseLinkIdx = kinDyn->model().getFrameLink(imuIdx); + if (kinDyn->model().getFrameName(baseLinkIdx) != kinDyn->getFloatingBase()) + { + log()->error("{} The configured IMU does not seem to be associated with the base link."); + return false; + } + + m_pimpl->kinDyn = kinDyn; + return true; +} + +bool CenterOfMassKalmanFilter::advance() +{ + if (!isOutputValid()) + { + log()->error("{} Please initialize the estimator and" + "set KinDynComputations object before calling this method."); + return false; + } + + m_pimpl->A_o_G = m_pimpl->kinDyn->getCenterOfMassPosition(); + const auto& g = BipedalLocomotion::Math::StandardAccelerationOfGravitation; + m_pimpl->omegaSquared = g/m_pimpl->A_o_G(2); // g/z_{com} + + if (!m_pimpl->predict()) + { + log()->error("{} Failed to propagate the states."); + return false; + } + + if (!m_pimpl->update()) + { + log()->error("{} Failed to update the states with measurements."); + return false; + } + + // update exposed states + m_pimpl->state.comPosition = m_pimpl->x.head<2>(); + m_pimpl->state.comVelocity = m_pimpl->x.segment<2>(2); + m_pimpl->state.comOffset = m_pimpl->x.tail<2>(); + + return true; +} + +bool CenterOfMassKalmanFilter::Impl::predict() +{ + prepareAandB(); + const Eigen::Vector2d& u = input.globalCoP; + + // KF prediction equations + x = A*x + B*u; + P = A*P*(A.transpose()) + Q; + + return true; +} + +bool CenterOfMassKalmanFilter::Impl::update() +{ + prepareCandD(); + prepareMeasurementVector(); + + // predicted measurement + const Eigen::Vector2d& u = input.globalCoP; + z = C*x + D*u; + + // KF update equations + CT = C.transpose(); + S = C*P*CT + R; + K = P*CT*(S.inverse()); + + x += K*(y - z); + P = (I6 - K*C)*P; + + return true; +} + +void CenterOfMassKalmanFilter::Impl::prepareMeasurementVector() +{ + // assuming z axis of inertial frame perpendicular to gravity + // get com horizontal position from kinematics + // y = [ horizontal com position from kinematics ] + // [horizontal com acceleration from kinematics and IMU] + A_H_IMU = kinDyn->getWorldTransform(imuName); + A_o_IMU = A_H_IMU.getPosition(); + A_R_IMU = A_H_IMU.getRotation(); + + IMU_o_G = A_R_IMU.inverse()*(A_o_G - A_o_IMU); + + Sw = iDynTree::skew(input.gyro); + + // get Eigen refs + auto R_imu = iDynTree::toEigen(A_R_IMU); + auto r = iDynTree::toEigen(IMU_o_G); + auto com = iDynTree::toEigen(A_o_G); + + Eigen::Vector3d A_odoubledot_G; + A_odoubledot_G = R_imu*input.acc + A_g + (R_imu*Sw*Sw*r); + + // serialize measurements + y.head<2>() = com.head<2>(); + y.tail<2>() = A_odoubledot_G.head<2>(); +} + +void CenterOfMassKalmanFilter::Impl::prepareAandB() +{ + // A = [ I + (0.5 w^2 dT^2) I dT I (0.5*w^2 dT^2) I ] + // [ (w^2 dT^2) I I (w^2 dT^2) I ] + // [ 0 0 I ] + // I is a 2x2 identity matrix + // w - LIPM eigenfrequency = sqrt(g/com_height) + // dt - sampling period + + double halfOmegaSq{0.5*omegaSquared}; + double dtSq{dt*dt}; + + // hoping these matrix block assignments are not + // more expensive than scalar assignments + A.topRightCorner<2, 2>() = (halfOmegaSq*dtSq)*I2; + A.topLeftCorner<2, 2>() = I2 + A.topRightCorner<2, 2>(); + A.block<2, 2>(0, 2) = dt*I2; + A.block<2, 2>(2, 0) = A.block<2, 2>(2, 2) = omegaSquared*dtSq*I2; + + // B = [ -(0.5 w^2 dT^2) I ] + // [ -(w^2 dT^2) I ] + // [ 0 ] + B.topLeftCorner<2, 2>() = - A.topRightCorner<2, 2>(); + B.block<2, 2>(2, 0) = - A.block<2, 2>(2, 2); +} + +void CenterOfMassKalmanFilter::Impl::prepareCandD() +{ + // C = [ I 0 0] + // [ w^2 I 0 w^2 I] + C.topLeftCorner<2, 2>() = I2; + C.bottomLeftCorner<2, 2>() = C.bottomRightCorner<2, 2>() = omegaSquared*I2; + + // D = [ 0 ] + // [ - w^2 I ] + D.bottomRightCorner<2, 2>() = -C.bottomRightCorner<2, 2>(); +} + +void CenterOfMassKalmanFilter::Impl::prepareQandR() +{ + Q.topRightCorner<2, 2>() = sigma2_x_com*I2; + Q.block<2, 2>(2, 2) = sigma2_dx_com*I2; + Q.bottomRightCorner<2, 2>() = sigma2_xoff_com*I2; + + R.topLeftCorner<2, 2>() = sigma2_y_com*I2; + R.bottomRightCorner<2, 2>() = sigma2_y_ddcom*I2; +} + +bool CenterOfMassKalmanFilter::setInput(const CoMKFInput& input) +{ + m_pimpl->input = input; + return true; +} + +bool CenterOfMassKalmanFilter::setGlobalCenterOfPressure(const double& copX, const double& copY) +{ + m_pimpl->input.globalCoP << copX, copY; + return true; +} + +bool CenterOfMassKalmanFilter::setBaseLinkIMUMeasurement(Eigen::Ref acc, + Eigen::Ref gyro) +{ + m_pimpl->input.acc = acc; + m_pimpl->input.gyro = gyro; + return true; +} + +const CoMKFState& CenterOfMassKalmanFilter::getOutput() const +{ + return m_pimpl->state; +} + +bool CenterOfMassKalmanFilter::isOutputValid() const +{ + return m_pimpl->initialized && + (m_pimpl->kinDyn != nullptr) && + m_pimpl->kinDyn->isValid(); +}