Use IMU measurements to set submodel base state in RDE (#793)

CHANGELOG.md

@@ -3,6 +3,8 @@ All notable changes to this project are documented in this file.
 
 ## [unreleased]
 ### Added
+- Set submodel states from IMUs in RDE and add friction torques as measurement (https://github.com/ami-iit/bipedal-locomotion-framework/pull/793)
+
 ### Changed
 ### Fixed
 ### Removed
@@ -552,4 +554,4 @@ All notable changes to this project are documented in this file.
 [0.3.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.2.0...v0.3.0
 [0.2.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.1.1...v0.2.0
 [0.1.1]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.1.0...v0.1.1
-[0.1.0]: https://github.com/ami-iit/bipedal-locomotion-framework/releases/tag/v0.1.0
+[0.1.0]: https://github.com/ami-iit/bipedal-locomotion-framework/releases/tag/v0.1.0

bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp

@@ -49,7 +49,7 @@ void CreateRobotDynamicsEstimator(pybind11::module& module)
  .def_readwrite("ft_wrenches", &RobotDynamicsEstimatorInput::ftWrenches)
  .def_readwrite("linear_accelerations", &RobotDynamicsEstimatorInput::linearAccelerations)
  .def_readwrite("angular_velocities", &RobotDynamicsEstimatorInput::angularVelocities)
- .def_readwrite("friction", &RobotDynamicsEstimatorInput::friction);
+ .def_readwrite("friction_torques", &RobotDynamicsEstimatorInput::frictionTorques);
 
  py::class_<RobotDynamicsEstimatorOutput>(module, "RobotDynamicsEstimatorOutput")
  .def(py::init())
@@ -58,7 +58,9 @@ void CreateRobotDynamicsEstimator(pybind11::module& module)
  .def_readwrite("tau_F", &RobotDynamicsEstimatorOutput::tau_F)
  .def_readwrite("ft_wrenches", &RobotDynamicsEstimatorOutput::ftWrenches)
  .def_readwrite("ft_wrenches_biases", &RobotDynamicsEstimatorOutput::ftWrenchesBiases)
+ .def_readwrite("linear_accelerations", &RobotDynamicsEstimatorOutput::linearAccelerations)
  .def_readwrite("accelerometer_biases", &RobotDynamicsEstimatorOutput::accelerometerBiases)
+ .def_readwrite("angular_velocities", &RobotDynamicsEstimatorOutput::angularVelocities)
  .def_readwrite("gyroscope_biases", &RobotDynamicsEstimatorOutput::gyroscopeBiases)
  .def_readwrite("contact_wrenches", &RobotDynamicsEstimatorOutput::contactWrenches)
  .def(py::pickle([](const RobotDynamicsEstimatorOutput &output) { //__getstate__
@@ -67,7 +69,9 @@ void CreateRobotDynamicsEstimator(pybind11::module& module)
  output.tau_F,
  output.ftWrenches,
  output.ftWrenchesBiases,
+ output.linearAccelerations,
  output.accelerometerBiases,
+ output.angularVelocities,
  output.gyroscopeBiases,
  output.contactWrenches);
  },
@@ -80,9 +84,11 @@ void CreateRobotDynamicsEstimator(pybind11::module& module)
  rde.tau_F = t[2].cast<Eigen::VectorXd>();
  rde.ftWrenches = t[3].cast<std::map<std::string, Eigen::VectorXd>>();
  rde.ftWrenchesBiases = t[4].cast<std::map<std::string, Eigen::VectorXd>>();
- rde.accelerometerBiases = t[5].cast<std::map<std::string, Eigen::VectorXd>>();
- rde.gyroscopeBiases = t[6].cast<std::map<std::string, Eigen::VectorXd>>();
- rde.contactWrenches = t[7].cast<std::map<std::string, Eigen::VectorXd>>();
+ rde.linearAccelerations = t[5].cast<std::map<std::string, Eigen::VectorXd>>();
+ rde.accelerometerBiases = t[6].cast<std::map<std::string, Eigen::VectorXd>>();
+ rde.angularVelocities = t[7].cast<std::map<std::string, Eigen::VectorXd>>();
+ rde.gyroscopeBiases = t[8].cast<std::map<std::string, Eigen::VectorXd>>();
+ rde.contactWrenches = t[9].cast<std::map<std::string, Eigen::VectorXd>>();
 
  return rde;
  }

src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h

@@ -82,17 +82,18 @@ class AccelerometerMeasurementDynamics : public Dynamics
  /**
  * Initialize the state dynamics.
  * @param paramHandler pointer to the parameters handler.
+ * @param name name of the dynamics.
  * @note the following parameters are required by the class
  * | Parameter Name | Type | Description | Mandatory |
  * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:|
- * |  `name`  | `string` | Name of the state contained in the `VariablesHandler` describing the state associated to this dynamics| Yes |
+ * | `input_name` | `string` |  Name of the variable in input to the main application corresponding to this dynamics | Yes |
  * | `covariance` | `vector` | Process covariances | Yes |
  * | `dynamic_model` | `string` | Type of dynamic model describing the state dynamics. | Yes |
  * | `use_bias` |`boolean` | Boolean saying if the dynamics depends on a bias. False if not specified. | No |
  * @return True in case of success, false otherwise.
  */
  bool
- initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
+ initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler, const std::string& name) override;
 
  /**
  * Finalize the Dynamics.

src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ConstantMeasurementModel.h

@@ -50,17 +50,18 @@ class ConstantMeasurementModel : public Dynamics
  /**
  * Initialize the state dynamics.
  * @param paramHandler pointer to the parameters handler.
+ * @param name name of the dynamics.
  * @note the following parameters are required by the class
  * | Parameter Name | Type | Description | Mandatory |
  * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:|
- * | `name` | `string` | Name of the state contained in the `VariablesHandler` describing the state associated to this dynamics| Yes |
- * | `covariance` | `vector` | Process covariances | Yes |
+ * | `input_name` | `string` | Name of the variable in input to the main application corresponding to this dynamics. | Yes |
+ * | `associated_state` | `string` | Name of the variable in the state vector corresponding to this dynamics. | Yes |
+ * | `covariance` | `vector` | Process covariances. | Yes |
  * | `dynamic_model` | `string` | Type of dynamic model describing the state dynamics. | Yes |
- * | `elements` | `vector` | Vector of strings describing the list of sub variables composing the state associated to this dynamics.| No |
  * | `use_bias` |`boolean` | Boolean saying if the dynamics depends on a bias. False if not specified. | No |
  * @return True in case of success, false otherwise.
  */
- bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
+ bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler, const std::string& name) override;
 
  /**
  * Finalize the Dynamics.

src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h

@@ -118,10 +118,11 @@ class Dynamics
  /**
  * Initialize the task.
  * @param paramHandler pointer to the parameters handler.
+ * @param name name of the dynamics.
  * @return True in case of success, false otherwise.
  */
  virtual bool
- initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler);
+ initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler, const std::string& name);
 
  /**
  * Finalize the Dynamics.

src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ExternalContactStateDynamics.h

@@ -51,17 +51,17 @@ class ExternalContactStateDynamics : public Dynamics
  /**
  * Initialize the state dynamics.
  * @param paramHandler pointer to the parameters handler.
+ * @param name name of the dynamics.
  * @note the following parameters are required by the class
  * | Parameter Name | Type | Description | Mandatory |
  * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:|
- * |  `name`  | `string` | Name of the state contained in the `VariablesHandler` describing the state associated to this dynamics| Yes |
+ * | `input_name` | `string` |  Name of the variable in input to the main application corresponding to this dynamics | Yes |
  * | `covariance` | `vector` | Process covariances | Yes |
  * | `initial_covariance` | `vector` | Initial state covariances | Yes |
  * | `dynamic_model` | `string` | Type of dynamic model describing the state dynamics. | Yes |
- * | `elements` | `vector` | Vector of strings describing the list of sub variables composing the state associated to this dynamics.| No |
  * @return True in case of success, false otherwise.
  */
- bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
+ bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler, const std::string& name) override;
 
  /**
  * Finalize the Dynamics.

src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h

@@ -64,22 +64,22 @@ class FrictionTorqueStateDynamics : public Dynamics
  /**
  * Initialize the state dynamics.
  * @param paramHandler pointer to the parameters handler.
+ * @param name name of the dynamics.
  * @note the following parameters are required by the class
  * | Parameter Name | Type | Description | Mandatory |
  * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:|
- * |  `name`  | `string` | Name of the state contained in the `VariablesHandler` describing the state associated to this dynamics| Yes |
+ * | `input_name` | `string` |  Name of the variable in input to the main application corresponding to this dynamics | Yes |
  * | `covariance` | `vector` | Process covariances | Yes |
  * | `initial_covariance` | `vector` | Initial state covariances | Yes |
  * | `dynamic_model` | `string` | Type of dynamic model describing the state dynamics. | Yes |
- * | `elements` | `vector` | Vector of strings describing the list of sub variables composing the state associated to this dynamics.| No |
  * | `friction_k0` | `vector` | Vector of coefficients k0. For more info check the class description. | Yes |
  * | `friction_k1` | `vector` | Vector of coefficients k1. For more info check the class description. | Yes |
  * | `friction_k2` | `vector` | Vector of coefficients k2. For more info check the class description. | Yes |
  * | `dT` | `double` | Sampling time. | Yes |
  * @return True in case of success, false otherwise.
  */
  bool
- initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
+ initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler, const std::string& name) override;
 
  /**
  * Finalize the Dynamics.

src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/GyroscopeMeasurementDynamics.h

@@ -69,17 +69,18 @@ class GyroscopeMeasurementDynamics : public Dynamics
  /**
  * Initialize the state dynamics.
  * @param paramHandler pointer to the parameters handler.
+ * @param name name of the dynamics.
  * @note the following parameters are required by the class
  * | Parameter Name | Type | Description | Mandatory |
  * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:|
- * |  `name`  | `string` | Name of the state contained in the `VariablesHandler` describing the state associated to this dynamics| Yes |
+ * | `input_name` | `string` |  Name of the variable in input to the main application corresponding to this dynamics | Yes |
  * | `covariance` | `vector` | Process covariances | Yes |
  * | `dynamic_model` | `string` | Type of dynamic model describing the state dynamics. | Yes |
  * | `use_bias` |`boolean` | Boolean saying if the dynamics depends on a bias. False if not specified. | No |
  * @return True in case of success, false otherwise.
  */
  bool
- initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
+ initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler, const std::string& name) override;
 
  /**
  * Finalize the Dynamics.

src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h

@@ -69,18 +69,18 @@ class JointVelocityStateDynamics : public Dynamics
  /**
  * Initialize the state dynamics.
  * @param paramHandler pointer to the parameters handler.
+ * @param name name of the dynamics.
  * @note the following parameters are required by the class
  * | Parameter Name | Type | Description | Mandatory |
  * |:----------------------------------:|:---------------------:|:-------------------------------------------------------------------------------------------------------:|:---------:|
- * |  `name`  | `string` | Name of the state contained in the `VariablesHandler` describing the state associated to this dynamics| Yes |
+ * | `input_name` | `string` |  Name of the variable in input to the main application corresponding to this dynamics | Yes |
  * | `covariance` | `vector` | Process covariances | Yes |
  * | `initial_covariance` | `vector` | Initial state covariances | Yes |
  * | `dynamic_model` | `string` | Type of dynamic model describing the state dynamics. | Yes |
- * | `elements` | `vector` | Vector of strings describing the list of sub variables composing the state associated to this dynamics.| No |
  * | `dT` | `chrono::nanoseconds` | Sampling time. | Yes |
  * @return True in case of success, false otherwise.
  */
- bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
+ bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler, const std::string& name) override;
 
  /**
  * Finalize the Dynamics.

src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/MotorCurrentMeasurementDynamics.h

@@ -56,19 +56,19 @@ class MotorCurrentMeasurementDynamics : public Dynamics
  /**
  * Initialize the measurement object.
  * @param paramHandler pointer to the parameters handler.
+ * @param name name of the dynamics.
  * @note the following parameters are required by the class
  * | Parameter Name | Type | Description | Mandatory |
  * |:----------------------------------:|:--------:|:-------------------------------------------------------------------------------------------------------:|:---------:|
- * |  `name`  | `string` |Name of the measurement variable contained in the `VariablesHandler` describing this dynamics | Yes |
+ * | `input_name` | `string` | Name of the variable in input to the main application corresponding to this dynamics  | Yes |
  * | `covariance` | `vector` | Process covariances | Yes |
  * | `dynamic_model` | `string` | Type of dynamic model describing the measurement dynamics. | Yes |
- * | `elements` | `vector` |Vector of strings describing the list of variables composing the measurement dynamics. | No |
  * | `torque_constant` | `vector` | Vector of coefficients k0. For more info check the class description. | Yes |
  * | `gear_ratio` | `vector` | Vector of coefficients k1. For more info check the class description. | Yes |
  * @return True in case of success, false otherwise.
  */
  bool
- initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;
+ initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler, const std::string& name) override;
 
  /**
  * Finalize the Dynamics.