diff --git a/CHANGELOG.md b/CHANGELOG.md index 0ce6aec3f6..77bab0dce7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,8 @@ All notable changes to this project are documented in this file. - Add the support of `std::chrono` in The text logging (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) - Add the possibility to retrieve and set duration from the `IParametersHandler` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) - Add the possibility to update the contact list in the swing foot trajectory planner (https://github.com/ami-iit/bipedal-locomotion-framework/pull/637) +- Implement state dynamic models needed to define an UKF process model (https://github.com/ami-iit/bipedal-locomotion-framework/pull/615) +- Implement measurement dynamic models needed to define an UKF measurement model (https://github.com/ami-iit/bipedal-locomotion-framework/pull/640) ### Changed - Update the `IK tutorial` to use `QPInverseKinematics::build` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/621) @@ -26,7 +28,7 @@ All notable changes to this project are documented in this file. ### Fixed - Return an error if an invalid `KinDynComputations` object is passed to `QPInverseKinematics::build()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/622) -- Fix `QPTSIF` documentation (https://github.com/ami-iit/bipedal-locomotion-framework/pull/634) +- Fix `QPTSID` documentation (https://github.com/ami-iit/bipedal-locomotion-framework/pull/634) - Fix error messages in `QPTSID` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/639) ## [0.12.0] - 2023-03-07 diff --git a/CHANGELOG.md.orig b/CHANGELOG.md.orig new file mode 100644 index 0000000000..e94747b297 --- /dev/null +++ b/CHANGELOG.md.orig @@ -0,0 +1,388 @@ +# Changelog +All notable changes to this project are documented in this file. + +## [unreleased] +### Added +- Implement the `DiscreteGeometryContact` in Contacts component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/626) +- Implement the `SchmittTrigger` in component `Math` and the associated python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/624) +- Add the support of `std::chrono` in The text logging (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) +- Add the possibility to retrieve and set duration from the `IParametersHandler` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) +- Add the possibility to update the contact list in the swing foot trajectory planner (https://github.com/ami-iit/bipedal-locomotion-framework/pull/637) +- Implement state dynamic models needed to define an UKF process model (https://github.com/ami-iit/bipedal-locomotion-framework/pull/615) + +### Changed +- Update the `IK tutorial` to use `QPInverseKinematics::build` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/621) +- Handle case where no FT sensors are specified to split the model (https://github.com/ami-iit/bipedal-locomotion-framework/pull/625) +- General restructure of the `ContactDetector`and the derived classes (`SchmittTriggerDetector` and `FixedFootDetector`) (https://github.com/ami-iit/bipedal-locomotion-framework/pull/624) + Thanks to this refactory the `FixedFootDetector` usage becomes similar to the others `advanceable`. + Indeed now `FixedFootDetector::advace()` considers the input set by the user and provides the corresponding output. + ⚠️ Even if this modification do not break the API the user may notice some strange behavior if `advance` was called after getting the output of the detector. +- Restructure the `Contacts` component to handle time with `std::chrono::nanoseconds` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) +- Restructure the `Planners` component to handle time with `std::chrono::nanoseconds` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) +- Restructure the `FloatingBaseEstimator` component to handle time with `std::chrono::nanoseconds`(https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) +- Update the `blf-position-tracking` to handle time with `std::chrono::nanoseconds` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) +- Update the python bindings to consider the time with `std::chrono::nanoseconds` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/630) +- Robustify SubModelCreator and SubModelKinDynWrapper tests (https://github.com/ami-iit/bipedal-locomotion-framework/pull/631) +- `SwingFootTrajectoryPlanner::advance()` must be called before getting the output (https://github.com/ami-iit/bipedal-locomotion-framework/pull/637) + +### Fixed +- Return an error if an invalid `KinDynComputations` object is passed to `QPInverseKinematics::build()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/622) +- Fix `QPTSID` documentation (https://github.com/ami-iit/bipedal-locomotion-framework/pull/634) +<<<<<<< HEAD +- Fix error messages in `QPTSID` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/639) +======= +>>>>>>> Update CHANGELOG + +## [0.12.0] - 2023-03-07 +### Added +- Add the possibility to attach all the multiple analog sensor clients (https://github.com/ami-iit/bipedal-locomotion-framework/pull/569) +- Add a tutorial for the inverse kinematics (https://github.com/ami-iit/bipedal-locomotion-framework/pull/596) +- Implement the ROS2 sink for the `TextLogging` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/587) +- Implement the `QPFixedBaseInverseKinematics` in the `IK` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/599) +- 🤖 [ergoCubSN000] Add configuration files for the YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/600) +- Add functions to split a model in a set of submodels in the Estimator component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/604) +- Add the possibity to call the advanceable capabilities of the `QuinticSpline` from the python (https://github.com/ami-iit/bipedal-locomotion-framework/pull/609) +- Implement the `CubicSpline` python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/609) +- Implement the python bindings for the iDynTree to manif conversions (https://github.com/ami-iit/bipedal-locomotion-framework/pull/610) +- Implement `blf-balancing-position-control` application (https://github.com/ami-iit/bipedal-locomotion-framework/pull/611) +- Implement the python bindings for `YarpTextLogging` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/611) +- Add SubModelKinDynWrapper class to handle the `KinDynComputation` object of a sub-model (https://github.com/ami-iit/bipedal-locomotion-framework/pull/605) +- Implement the `JointLimitsTask` for the IK (https://github.com/ami-iit/bipedal-locomotion-framework/pull/603) +- Add the possibility to programmatically build an IK problem from a configuration file (https://github.com/ami-iit/bipedal-locomotion-framework/pull/614, https://github.com/ami-iit/bipedal-locomotion-framework/pull/619) + +### Changed +- Ask for `toml++ v3.0.1` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/581) +- The YarpRobotLogger will now automatically connect to the exogenous signal port if available (https://github.com/ami-iit/bipedal-locomotion-framework/pull/570/) +- 🤖 [iCubGenova09] Add the left and right hands skin (raw and filtered) data acquisition (https://github.com/ami-iit/bipedal-locomotion-framework/pull/570/) +- Add informative prints `YarpSensorBridge::Impl` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/569) +- The minimum version of iDynTree now supported is iDynTree 4.3.0 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/588). +- Allow using the `iDynTree swig` bindings in `QPFixedBaseTSID` for the kindyncomputation object (https://github.com/ami-iit/bipedal-locomotion-framework/pull/599) +- Add the possibility to customize the video codec in the `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/607) + +### Fix +- Return an invalid `PolyDriverDescriptor` if `description` is not found in `constructMultipleAnalogSensorsRemapper()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/569) +- Fix compatibility with OpenCV 4.7.0 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/589) +- Fix `attachRemappedRemoteControlBoard` in `YarpSensorBridge` when the `RemoteControlBoard` is not the first polydriver in the polydriverlist (https://github.com/ami-iit/bipedal-locomotion-framework/pull/608) +- Fix race condition in System::ClockBuilder (https://github.com/ami-iit/bipedal-locomotion-framework/pull/618) + +## [0.11.1] - 2022-12-19 +### Fix +- Fix the compilation of the `YarpRobotLoggerDevice` in `Windows` and `macOS` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/580) + +## [0.11.0] - 2022-12-17 +### Added +- Log the status of the system in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/571) +- Add the `ROS2` implementation for Clock class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/575) + +### Changed +- YARP devices are now enabled by default if YARP is found (https://github.com/ami-iit/bipedal-locomotion-framework/pull/576). +- Restructure the python bindings to support _official_ `iDynTree` bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/578) +- Remove _unofficial_ `iDynTree` bindings based on pybind11 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/578) + +### Fix +- Fix compatibility with YARP 3.8 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/577). + +## [0.10.0] - 2022-09-23 +### Added +- Add the possibility to set the exogenous feedback for the `IK::SE3Task` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/567) +- Implement `RobotInterface::constructMultipleAnalogSensorsClient()` and `RobotInterface::constructMultipleAnalogsensorsRemapper()` methods (https://github.com/ami-iit/bipedal-locomotion-framework/pull/568) + +### Changed +- Add the possibility to log only a subset of text logging ports in `YarpRobotLogger` device (https://github.com/ami-iit/bipedal-locomotion-framework/pull/561) +- Accept boolean as integer while getting an element from searchable in `YarpUtilities` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/566) + +### Fix +- Fix typo in the `RobotInterface::constructGenericSensorClient()` documentation (https://github.com/ami-iit/bipedal-locomotion-framework/pull/568) +- Fix compatibility with qhull installed by vcpkg `2022.07.25` and robotology-superbuild-dependencies-vcpkg `0.10.1` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/565). + +## [0.9.0] - 2022-09-09 +### Added +- Implement the `MultiStateWeightProvider` in `ContinuousDynamicalSystem` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/555) +- Implement `PortInput` and `PortOutput`in `System` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/555) +- Implement `toManifTwist` in `Conversions` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/557) +- Add the default value for the desired spatial and angular velocity to the `IK::SO3Task` and `IK::SE3Task` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/557) +- Implement the `IK::R3Task` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/559) + +### Changed +- Now `Advanceable` inherits from `PortInput` and `PortOutput` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/555) + +### Fix +- Fix the dependency required to compile the YarpRobotLogger device (https://github.com/ami-iit/bipedal-locomotion-framework/pull/554) +- Fix the compatibility with fmt v9.0.0 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/556) + +## [0.8.0] - 2022-07-29 +### Added +- Add the possibility to log the YarpTextLogging in the YarpRobotLogger (https://github.com/ami-iit/bipedal-locomotion-framework/pull/541) +- Enable the logging FTs and IMU logging of iCubGenova09 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/546/) +### Changed +- Ported `YarpLoggerDevice` to `robometry`(https://github.com/ami-iit/bipedal-locomotion-framework/pull/533) +- bipedal locomotion framework now depends on YARP 3.7.0 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/541) +### Fix +- Avoid to use deprecated function cv::aruco::drawAxis in ArucoDetector to fix compilation with OpenCV 4.6.0 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/552) + +## [0.7.0] - 2022-06-21 +### Added +- Implement the python bindings for the clock machinery and for the yarp clock (https://github.com/ami-iit/bipedal-locomotion-framework/pull/500) +- Implement the `IWeightProvider` interface and the `ConstantWeightProvider` class in the System component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/506) +- Add installation of pip metadata files for when blf python bindings are installed only via CMake (https://github.com/ami-iit/bipedal-locomotion-framework/pull/508) +- Implement the python bindings for the VectorsCollection message and the associated buffered port (https://github.com/ami-iit/bipedal-locomotion-framework/pull/511) +- Implement the `VectorsCollectionWrapper` device for collection of arbitrary vector ports (https://github.com/ami-iit/bipedal-locomotion-framework/pull/512) +- Add reading of right upper leg FT for `iCubGenova09` and associated cartesian wrench in `YarpRobotLoggerDevice` configuration files (https://github.com/ami-iit/bipedal-locomotion-framework/pull/513) +- Add reading of right and left arms FT for `iCubGenova09` in `YarpRobotLoggerDevice` configuration files (https://github.com/ami-iit/bipedal-locomotion-framework/pull/515) +- Add reading of right and left arms and right upper leg FTs and cartesian wrenches for `iCubGazeboV3` in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/525) +- Add the possibility to retrieve the temperature sensor from `SensorBridge` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/519) +- Add the possibility to set only the velocity in `CubicSpline::setInitialConditions` and `CubicSpline::setFinalConditions` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/528) +- Implement the python bindings for the `ContinuousDynamicalSystem` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/532) +- Add the possibility to log the video in the `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/516) + +### Changed +- An error it will be returned if the user tries to change the clock type once the `clock()` has been already called once (https://github.com/ami-iit/bipedal-locomotion-framework/pull/500) +- Log the arms external wrenches on the YarpRobotLogger for iCubGenova09 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/502) +- IK and TSID now uses the weight provider to specify the weight associated to a task (https://github.com/ami-iit/bipedal-locomotion-framework/pull/506) +- The `Planners`, `System`, `RobotInterface` and `YarpImplementation` components are no more mandatory to compile the python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/511) +- Reorganize the multiple FT sensor and external wrench files into a single file in the YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/525) +- Save the robot name and the names of the channel's elements in the YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/522) +- Use icub-models to get the urdf models for the tests (https://github.com/ami-iit/bipedal-locomotion-framework/pull/526) +- The FT sensor are now considered as `multianalogsensor` in `YarpSensorBridge` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/519) +- Make `YarpRobotLogger` compatible with `yarp-telemetry` v0.5.1 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/535) +- Set for `yarp-telemetry` minimum version to v0.5.1 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/535) +- Make `YarpCameraBridge::getColorImage()` and `YarpCameraBridge::getDepthImage()` thread safe (https://github.com/ami-iit/bipedal-locomotion-framework/pull/516) +- Deprecate `YarpCameraBridge::get()` in favor of `YarpCameraBridge::getMetaData()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/516) +- Move from LGPL to BSD3 license (https://github.com/ami-iit/bipedal-locomotion-framework/pull/550) + +### Fix +- Remove outdated includes in YarpRobotLoggerDevice.cpp (https://github.com/ami-iit/bipedal-locomotion-framework/pull/502) + +## [0.6.0] - 2022-01-10 +### Added +- Add the reading of the orientation of the head IMU in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/471) +- Add the possibility to change the weight in TSID/IK (https://github.com/ami-iit/bipedal-locomotion-framework/pull/475) +- Implement a `FirstOrderSmoother` class in `ContinuousDynamicalSystem` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/476) +- Implement `getIntegrationStep` in `FixedIntegration` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/476) +- Add the possibility to create custom `LinearTasks` in python (https://github.com/ami-iit/bipedal-locomotion-framework/pull/480) +- Implement the possibility to compute the residual terms in the `LinearTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/483) +- Define `VectorsCollection` message in `YarpUtilities` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/483) +- Add the reading of the joint and motor acceleration in `YarpSensorBridge` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/492) + +### Changed +- Use yarp clock instead of system clock in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/473) +- Reduce code duplication in python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/484) +- Use `TextLogger` in `YarpRobotLoggerDevice` instead of `yarp` commands (https://github.com/ami-iit/bipedal-locomotion-framework/pull/486) +- Ask for `osqp-eigen 0.6.4.100`(https://github.com/ami-iit/bipedal-locomotion-framework/pull/490) +- Use enum underlying type to convert `TextLogging` verbosity level to `spdlog` verbosity level (https://github.com/ami-iit/bipedal-locomotion-framework/pull/495) +- `yarp-telemetry` is now a dependency of the `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/487) +- Fix deprecated `YARP` functions in `YarpUtilities` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/491) + +### Fix +- Fix the population of the jointAccelerations and baseAcceleration variables in QPTSID (https://github.com/ami-iit/bipedal-locomotion-framework/pull/478) +- Fix the documentation in the `Advanceable` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/476) +- Add virtual destrutors in `System::Sink`, `System::Source`, `System::LinearTask`, +`System::ITaskControlMode`, `TSID::TSIDLinearTask` and `IK::IKLinearTask` classes (https://github.com/ami-iit/bipedal-locomotion-framework/pull/480) +- The joint torques is now correctly retrieved in QPTSID class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/482) +- The motor velocity and positions are now returned in rad/s and rad (https://github.com/ami-iit/bipedal-locomotion-framework/pull/489) +- Fix `YarpRobotLoggerDevice` documentation (https://github.com/ami-iit/bipedal-locomotion-framework/pull/472) + +## [0.5.0] - 2021-11-30 +### Added +- Implement Python bindings for the TSID component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/428) +- Add the possibility to set the name of each element of a variable stored in the variables handler (https://github.com/ami-iit/bipedal-locomotion-framework/pull/429) +- Develop the python bindings for toml implementation of the parameters handler (https://github.com/ami-iit/bipedal-locomotion-framework/pull/432) +- Implement the VariableRegularizationTask in TSID (https://github.com/ami-iit/bipedal-locomotion-framework/pull/431) +- Implement `create_tsid` utility function for the python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/433) +- Implement the `AngularMomentumTask` in the `TSID` component and the associated python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/436) +- Implement `QPTSID::toString` method and the associated python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/440) +- Implement `ContactWrench` python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/441) +- Implement AngularMomentum task in the IK component and the associated bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/443) +- Implement `create_ik` utility function for the python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/444) +- Add the possibility to set the task controller mode for the SE3Task in the TSID component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/445) +- Expose the `ITaskControlMode` class in the python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/445) +- Add the possibility to enable/disable the joints and motors state logging in the `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/453) +- Implement `QPInverseKinematics::toString` method and the associated python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/461) +- Add the cartesian wrenches logging in `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/447) +- Implement the python bindings for the manif conversions methods (https://github.com/ami-iit/bipedal-locomotion-framework/pull/465) + +### Changed +- Inherits all the `Eigen::Matrix` constructors in the `Wrenchd` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/441) +- Bump the minimum `cmake` version to `3.16.0` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/468) + +### Fix +- Fix Analog FT Sensor reading in `YarpSensorBridgeImpl` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/459) +- Fix config files in `YarpRobotLoggerDevice` for iCub3 head IMU reading (https://github.com/ami-iit/bipedal-locomotion-framework/pull/467) + +## [0.4.0] - 2021-10-15 +### Added +- Implement `AdvanceableRunner::isRunning()` method (https://github.com/ami-iit/bipedal-locomotion-framework/pull/395) +- Implement `ContactPhaseList::getPresentPhase()` method (https://github.com/ami-iit/bipedal-locomotion-framework/pull/396) +- Add a synchronization mechanism for the `AdvanceableRunner` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/403) +- Add the possibility to use spdlog with YARP (https://github.com/ami-iit/bipedal-locomotion-framework/pull/408) +- Add new Advanceable exposing `UnicyclePlanner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/320) + +### Changed +- Add `name` parameter to the `AdvanceableRunner` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/406) +- Set the required `spdlog` version in the cmake file (https://github.com/ami-iit/bipedal-locomotion-framework/pull/415) +- Add features to FTIMULoggerDevice and rename it in YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/405) + +### Fix +- Fix missing components dependencies in the `CMake` machinery (https://github.com/ami-iit/bipedal-locomotion-framework/pull/414) +- Fixed missing include in `FloatingBaseEstimatorIO.h` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/417) + +## [0.3.0] - 2021-08-12 +### Added +- Implement `CubicSpline` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/344) +- Implement `PWM` control in RobotControl class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/346) +- Implement `ContactWrenchCone` class in Math component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/352) +- Implement `skew` function in Math component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/352) +- Implement `QPTSID` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/366) +- Implement motor pwm, motor encoders, wbd joint torque estimates, pid reading in `YarpSensorBridge`(https://github.com/ami-iit/bipedal-locomotion-framework/pull/359). +- Implement FeasibleContactWrenchTask for TSID component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/369). +- Implement python bindings for QPInverseKinematics class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/303) +- Implement `ControlTask` in for System component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/373). +- Allow changing the log verbosity (https://github.com/ami-iit/bipedal-locomotion-framework/pull/385) +- Implement the CoMZMP controller (https://github.com/ami-iit/bipedal-locomotion-framework/pull/387) + +### Changed +- Add common Python files to gitignore (https://github.com/ami-iit/bipedal-locomotion-framework/pull/338) +- General improvements of `blf-calibration-delta-updater` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/361) +- Add the possibility to control a subset of coordinates in `IK::SE3Task` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/356) +- Add the possibility to control a subset of coordinates in `IK::CoMTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/357) +- Reduce the duplicate code in IK and TSID (https://github.com/ami-iit/bipedal-locomotion-framework/pull/364) +- `QPFixedBaseTSID` now inherits from `QPTSID` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/366) +- Enable the Current control in `RobotInterface` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/375) +- Add the possibility to disable and enable the PD controllers in `IK::SE3Task` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/373). +- Add the possibility to use manif objects in the ForwardEuler integrator (https://github.com/ami-iit/bipedal-locomotion-framework/pull/379). + +### Fix +- Fixed the crashing of `YarpSensorBridge` while trying to access unconfigured control board sensors data by adding some checks (https://github.com/ami-iit/bipedal-locomotion-framework/pull/378) +- Fixed the compilation of Python bindings (enabled by the `FRAMEWORK_COMPILE_PYTHON_BINDINGS` CMake option) when compiling with Visual Studio (https://github.com/ami-iit/bipedal-locomotion-framework/pull/380). +- Fixed the `TOML` and `YARP` implementation of the parameters handler when a `std::vector` is passed to the `setParameter()` method (https://github.com/ami-iit/bipedal-locomotion-framework/pull/390). + +## [0.2.0] - 2021-06-15 +### Added +- Implement IRobotControl python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/200) +- Implement ISensorBridge python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/203) +- Implement `LeggedOdometry` class as a part of `FloatingBaseEstimators` library and handle arbitrary contacts in `FloatingBaseEstimator`. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/151) +- Implement the possibility to set a desired reference trajectory in the TimeVaryingDCMPlanner. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/208) +- Implement SchmittTriggerDetector python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/213) +- Implement ModelComputationsHelper for quick construction of KinDynComputations object using parameters handler (https://github.com/ami-iit/bipedal-locomotion-framework/pull/216) +- Implement FloatingBaseEstimator and LeggedOdometry python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/218) +- Add spdlog as mandatory dependency of the project (https://github.com/ami-iit/bipedal-locomotion-framework/pull/225) +- Implement `ICameraBridge` and `IPointCloudBridge` interface classes as a part of `PerceptionInterface` library. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/165) +- Implement `RealSense` driver class as a part of `PerceptionCapture` library. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/165) +- Implement `realsense-test` utility application. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/165) +- Implement the inverse kinematics component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/229) +- Implement LinearizedFrictionCone class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/244) +- Added a check on whether the installed public headers have the correct folder structure (https://github.com/ami-iit/bipedal-locomotion-framework/pull/247) +- Implement python bindings for VariablesHandler class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/234) +- Implement `PerceptionFeatures` library and implement `ArucoDetector`. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/159) +- Implement FixedBaseDynamics class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/242) +- Implemented Sink and Source classes (https://github.com/ami-iit/bipedal-locomotion-framework/pull/267) +- Implement the IClock, StdClock and YarpClock classes (https://github.com/ami-iit/bipedal-locomotion-framework/pull/269) +- Implement `YarpCameraBridge` class for Yarp implementation of ICameraBridge (https://github.com/ami-iit/bipedal-locomotion-framework/pull/237) +- Implement `PointCloudProcessor` class and modify `realsense-test` to test point clouds handling with Realsense. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/236) +- Implement `AdvanceableRunner` and `SharedResource` classes in System component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/272) +- Implement `handleQuitSignals()` function in System component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/277) +- Implement TaskSpaceInverseDynamics interface (https://github.com/ami-iit/bipedal-locomotion-framework/pull/279) +- Implement `Wrench` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/279) +- Implement `SO3Task` in `TSID` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/281) +- Implement clone method in ParametersHandler classes (https://github.com/ami-iit/bipedal-locomotion-framework/pull/288) +- Implement `VariablesHandler::clear()` and `VariablesHandler::initialize()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/291) +- Implement the possibility to set the default contact in the `ContactList` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/297) +- Implement `FixedFootDetector` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/284) +- Implement QPFixedBaseTSID class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/251) +- Implement `YarpImplementation::setFromFile()` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/307) +- Implement `CoMTask` in TSID (https://github.com/ami-iit/bipedal-locomotion-framework/pull/304) +- Implement `YarpParametersHandler` bindings (https://github.com/ami-iit/bipedal-locomotion-framework/pull/309) +- Implement `contactListMapFromJson()` and `contactListMapToJson()` methods and python bindings (https://github.com/ami-iit/bipedal-locomotion-framework/issues/316) +- Implement a matioCpp-based strain2 sensors' FT-IMU logger example device (https://github.com/ami-iit/bipedal-locomotion-framework/pull/326) +- Implement `TomlImplementation` in `ParametersHandler` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/328) +- Implement blf_calibration_delta_updater.py application (https://github.com/ami-iit/bipedal-locomotion-framework/pull/332) + +### Changed +- Move all the Contacts related classes in Contacts component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/204) +- Move all the ContactDetectors related classes in Contacts component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/209) +- The DCMPlanner and TimeVaryingDCMPlanner initialize functions take as input an std::weak_ptr. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/208) +- Use `Math::StandardAccelerationOfGravitation` instead of hardcoding 9.81. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/211) +- Convert iDynTree types in FloatingBaseEstimators component to Eigen/manif types (https://github.com/ami-iit/bipedal-locomotion-framework/pull/215) +- Use std::optional instead of raw pointer in ISensorBridge. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/226) +- Use `System::LinearTask` in TSID component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/240) +- Restructure python bindings in submodules (https://github.com/ami-iit/bipedal-locomotion-framework/pull/238) +- Integrators and DynamicalSystems are now in the `ContinuousDynamicalSystem` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/242) +- Add Input template class to `System::Advanceable` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/267) +- Add support for landmarks and kinematics-free estimation in `FloatingBaseEstimators`. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/254) +- If FRAMEWORK_DETECT_ACTIVE_PYTHON_SITEPACKAGES is OFF, for Python bindings use installation directory provided by sysconfig Python module. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/274) +- Reduce memory allocation in `YarpSensorBridge` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/278) +- Use `TextLogging` in `VariablesHandler` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/291) +- Fix `YarpImplementation::setParameterPrivate()` when a boolean or a vector of boolean is passed (https://github.com/ami-iit/bipedal-locomotion-framework/pull/311) +- Add `foot_take_off_acceleration` and `foot_take_off_velocity` parameters in the `SwingFootPlanner` class (https://github.com/ami-iit/bipedal-locomotion-framework/issues/323) +- Change the parameters handler verbosity (https://github.com/ami-iit/bipedal-locomotion-framework/pull/330) +- Restore backward compatibility of SwingFootPlanner parameters (https://github.com/ami-iit/bipedal-locomotion-framework/pull/334) +- Bump manif version to 0.0.4 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/339) + +### Fixed +- Fix missing implementation of `YarpSensorBridge::getFailedSensorReads()`. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/202) +- Fixed `mas-imu-test` configuration files after FW fix. +- Fixed the implementation ``YarpSensorBridge::attachAllSixAxisForceTorqueSensors()`. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/231) +- Avoid the "Generating the Urdf Model from" message to appear when doing ccmake. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/243) +- Fixed the installation path of public headers related to perception libraries. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/245) +- Fixed InstallBasicPackageFiles to avoid the same problem of https://github.com/ami-iit/matio-cpp/pull/41 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/253) +- Call `positionInterface->setRefSpeeds()` only once when a position reference is set in `YarpRobotControl` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/271) +- Fix initialization of reference frame for world in `LeggedOdometry` class. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/289) +- `LeggedOdometry::Impl::updateInternalContactStates()` is now called even if the legged odometry is not initialize. This was required to have a meaningful base estimation the first time `LeggedOdometry::changeFixedFrame()` is called. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/292) +- Avoid to use the default copy-constructor and copy-assignment operator in `ContactPhaseList` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/295) +- Fix `toString()` method of `VariablesHandler` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/302) +- Fix in `YarpUtilities::getVectorFromSearchable` when a vector of boolean is passed as input (https://github.com/ami-iit/bipedal-locomotion-framework/pull/313) +- Various fixes for the yarp devices (https://github.com/ami-iit/bipedal-locomotion-framework/pull/337) + +## [0.1.1] - 2021-05-08 +### Fix +- Fix the documentation in `TemplateHelpers.h` + +## [0.1.0] - 2021-02-22 +### Added +- The `CHANGELOG.md` file +- The `cmake/BipedalLocomotionControllersFindDepencies.cmake` file +- The `cmake/AddInstallRPATHSupport.cmake` file +- The `cmake/AddUninstallTarget.cmake` file +- The `cmake/FindEigen3.cmake` file +- The `cmake/InstallBasicPackageFiles.cmake` file +- Implement the first version of the `BipedalLocomotionControllers` interface +- Implement the first version of the `YarpUtilities` library +- Implement `ParametersHandler` library (https://github.com/ami-iit/bipedal-locomotion-controllers/pull/13) +- Implement `GenericContainer::Vector` (https://github.com/ami-iit/bipedal-locomotion-controllers/pull/29) +- Implement `Estimators` library (https://github.com/ami-iit/bipedal-locomotion-controllers/pull/23) +- Implement `Contact` library. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/43 and https://github.com/ami-iit/bipedal-locomotion-framework/pull/45) +- Implement the first version of the `TimeVaryingDCMPlanner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/61) +- Implement the Quintic Spline class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/83) +- Implement the `ConvexHullHelper` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/51) +- Implement the `DynamicalSystem` and `Integrator` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/46) +- Implement the `IRobotControl` interface and the YARP specialization (https://github.com/ami-iit/bipedal-locomotion-framework/pull/97, https://github.com/ami-iit/bipedal-locomotion-framework/pull/192) +- Add `SensorBridge` interface (https://github.com/ami-iit/bipedal-locomotion-framework/pull/87) +- Add the `YarpSensorBridge` Implementation (https://github.com/ami-iit/bipedal-locomotion-framework/pull/106) +- Added `CommonConversions`, `ManifConversions`, and `matioCppConversions` libraries to handle type conversions. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/138 and https://github.com/ami-iit/bipedal-locomotion-framework/pull/143) +- Implement the `JointPositionTracking` application. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/136) +- Initial implementation of Python bindings using pybind11 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/134) +- Implement `FloatingBaseEstimatorDevice` YARP device for wrapping floating base estimation algorithms. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/130) +- Implement Continuous algebraic Riccati equation function (https://github.com/ami-iit/bipedal-locomotion-framework/pull/157) +- Implement YARP based `ROSPublisher` in the `YarpUtilities` library. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/156) +- Implement example YARP device `ROSPublisherTestDevice` for understanding the usage of `ROSPublisher`. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/160) +- Implement `TSID` library. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/167, https://github.com/ami-iit/bipedal-locomotion-framework/pull/170, https://github.com/ami-iit/bipedal-locomotion-framework/pull/178) +- Implement the `JointTrajectoryPlayer` application. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/169)29ed234a1c +- Implement `ContactDetectors` library. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/142) +- Added `mas-imu-test` application to check the output of MAS IMUs (https://github.com/ami-iit/bipedal-locomotion-framework/pull/62) +- Implement motor currents reading in `YarpSensorBridge`. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/187) + +[unreleased]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.12.0...master +[0.12.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.11.1...v0.12.0 +[0.11.1]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.11.0...v0.11.1 +[0.11.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.10.0...v0.11.0 +[0.10.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.9.0...v0.10.0 +[0.9.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.8.0...v0.9.0 +[0.8.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.7.0...v0.8.0 +[0.7.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.6.0...v0.7.0 +[0.6.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.5.0...v0.6.0 +[0.5.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.4.0...v0.5.0 +[0.4.0]: https://github.com/ami-iit/bipedal-locomotion-framework/compare/v0.3.0...v0.4.0 +[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 diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index 4c80b9edf3..b998804ab7 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -16,6 +16,7 @@ add_subdirectory(TextLogging) add_subdirectory(Conversions) add_subdirectory(YarpUtilities) add_subdirectory(ContinuousDynamicalSystem) +add_subdirectory(RobotDynamicsEstimator) include(ConfigureFileWithCMakeIf) diff --git a/bindings/python/RobotDynamicsEstimator/CMakeLists.txt b/bindings/python/RobotDynamicsEstimator/CMakeLists.txt new file mode 100644 index 0000000000..9cef2a3969 --- /dev/null +++ b/bindings/python/RobotDynamicsEstimator/CMakeLists.txt @@ -0,0 +1,16 @@ +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# BSD-3-Clause license. + +if(FRAMEWORK_COMPILE_RobotDynamicsEstimator) + + set(H_PREFIX include/BipedalLocomotion/bindings/RobotDynamicsEstimator) + + add_bipedal_locomotion_python_module( + NAME RobotDynamicsEstimatorBindings + SOURCES src/RobotDynamicsEstimator.cpp src/Module.cpp + HEADERS ${H_PREFIX}/RobotDynamicsEstimator.h ${H_PREFIX}/Module.h + LINK_LIBRARIES BipedalLocomotion::RobotDynamicsEstimator + ) + +endif() diff --git a/bindings/python/RobotDynamicsEstimator/include/BipedalLocomotion/bindings/RobotDynamicsEstimator/Module.h b/bindings/python/RobotDynamicsEstimator/include/BipedalLocomotion/bindings/RobotDynamicsEstimator/Module.h new file mode 100644 index 0000000000..6ab90185ec --- /dev/null +++ b/bindings/python/RobotDynamicsEstimator/include/BipedalLocomotion/bindings/RobotDynamicsEstimator/Module.h @@ -0,0 +1,26 @@ +/** + * @file Module.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_DYNAMICS_ESTIMATOR_MODULE_H +#define BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_DYNAMICS_ESTIMATOR_MODULE_H + +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace RobotDynamicsEstimator +{ + +void CreateModule(pybind11::module& module); + +} // namespace RobotDynamicsEstimator +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_DYNAMICS_ESTIMATOR_MODULE_H diff --git a/bindings/python/RobotDynamicsEstimator/include/BipedalLocomotion/bindings/RobotDynamicsEstimator/RobotDynamicsEstimator.h b/bindings/python/RobotDynamicsEstimator/include/BipedalLocomotion/bindings/RobotDynamicsEstimator/RobotDynamicsEstimator.h new file mode 100644 index 0000000000..6ba0f96744 --- /dev/null +++ b/bindings/python/RobotDynamicsEstimator/include/BipedalLocomotion/bindings/RobotDynamicsEstimator/RobotDynamicsEstimator.h @@ -0,0 +1,46 @@ +/** + * @file RobotDynamicsEstimator.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_DYNAMICS_ESTIMATOR_ROBOT_DYNAMICS_ESTIMATOR_H +#define BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_DYNAMICS_ESTIMATOR_ROBOT_DYNAMICS_ESTIMATOR_H + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace RobotDynamicsEstimator +{ + +template bool setKinDyn(T& myclass, ::pybind11::object& obj) +{ + std::shared_ptr* cls + = pybind11::detail::swig_wrapped_pointer_to_pybind< + std::shared_ptr>(obj); + + if (cls == nullptr) + { + throw ::pybind11::value_error("Invalid input for the function. Please provide a valid object."); + } + + return myclass.setKinDyn(*cls); +}; + +void CreateRobotDynamicsEstimator(pybind11::module& module); +void CreateSubModel(pybind11::module& module); +void CreateSubModelCreator(pybind11::module& module); +void CreateSubModelKinDynWrapper(pybind11::module& module); + +} // namespace RobotDynamicsEstimator +} // namespace bindings +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_BINDINGS_ROBOT_DYNAMICS_ESTIMATOR_ROBOT_DYNAMICS_ESTIMATOR_H + diff --git a/bindings/python/RobotDynamicsEstimator/src/Module.cpp b/bindings/python/RobotDynamicsEstimator/src/Module.cpp new file mode 100644 index 0000000000..1e71855590 --- /dev/null +++ b/bindings/python/RobotDynamicsEstimator/src/Module.cpp @@ -0,0 +1,31 @@ +/** + * @file Module.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace RobotDynamicsEstimator +{ +void CreateModule(pybind11::module& module) +{ + module.doc() = "Robot Dynamics Estimator module."; + + CreateRobotDynamicsEstimator(module); + CreateSubModel(module); + CreateSubModelCreator(module); + CreateSubModelKinDynWrapper(module); + +} +} // namespace RobotDynamicsEstimator +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp b/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp new file mode 100644 index 0000000000..e5d3393061 --- /dev/null +++ b/bindings/python/RobotDynamicsEstimator/src/RobotDynamicsEstimator.cpp @@ -0,0 +1,233 @@ +/** + * @file RobotDynamicsEstimator.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace bindings +{ +namespace RobotDynamicsEstimator +{ + +void CreateRobotDynamicsEstimator(pybind11::module& module) +{ + namespace py = ::pybind11; + + using namespace BipedalLocomotion::System; + using namespace BipedalLocomotion::ParametersHandler; + using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; + + py::class_(module, "RobotDynamicsEstimatorInput") + .def(py::init()) + .def_readwrite("basePose", &RobotDynamicsEstimatorInput::basePose) + .def_readwrite("baseVelocity", &RobotDynamicsEstimatorInput::baseVelocity) + .def_readwrite("baseAcceleration", &RobotDynamicsEstimatorInput::baseAcceleration) + .def_readwrite("jointPositions", &RobotDynamicsEstimatorInput::jointPositions) + .def_readwrite("jointVelocities", &RobotDynamicsEstimatorInput::jointVelocities) + .def_readwrite("motorCurrents", &RobotDynamicsEstimatorInput::motorCurrents) + .def_readwrite("ftWrenches", &RobotDynamicsEstimatorInput::ftWrenches) + .def_readwrite("linearAccelerations", &RobotDynamicsEstimatorInput::linearAccelerations) + .def_readwrite("angularVelocities", &RobotDynamicsEstimatorInput::angularVelocities); + + py::class_(module, "RobotDynamicsEstimatorOutput") + .def(py::init()) + .def_readwrite("ds", &RobotDynamicsEstimatorOutput::ds) + .def_readwrite("tau_m", &RobotDynamicsEstimatorOutput::tau_m) + .def_readwrite("tau_F", &RobotDynamicsEstimatorOutput::tau_F) + .def_readwrite("ftWrenches", &RobotDynamicsEstimatorOutput::ftWrenches) + .def_readwrite("ftWrenchesBiases", &RobotDynamicsEstimatorOutput::ftWrenchesBiases) + .def_readwrite("accelerometerBiases", &RobotDynamicsEstimatorOutput::accelerometerBiases) + .def_readwrite("gyroscopeBiases", &RobotDynamicsEstimatorOutput::gyroscopeBiases) + .def(py::pickle([](const RobotDynamicsEstimatorOutput &output) { //__getstate__ + return py::make_tuple(output.ds, + output.tau_m, + output.tau_F, + output.ftWrenches, + output.ftWrenchesBiases, + output.accelerometerBiases, + output.gyroscopeBiases); + }, + [](py::tuple t) { // __setstate__ + if(t.size() != 7) + throw std::runtime_error("Invalid state!"); + RobotDynamicsEstimatorOutput rde; + rde.ds = t[0].cast(); + rde.tau_m = t[1].cast(); + rde.tau_F = t[2].cast(); + rde.ftWrenches = t[3].cast>(); + rde.ftWrenchesBiases = t[4].cast>(); + rde.accelerometerBiases = t[5].cast>(); + rde.gyroscopeBiases = t[6].cast>(); + + return rde; + } + )); + + BipedalLocomotion::bindings::System::CreateAdvanceable + (module, "RobotDynamicsEstimator"); + + py::class_> + (module, "RobotDynamicsEstimator") + .def(py::init()) + .def("finalize", + &BipedalLocomotion::Estimators::RobotDynamicsEstimator::RobotDynamicsEstimator::finalize, + py::arg("stateVariableHandler"), + py::arg("measurementVariableHandler"), + py::arg("kinDynFullModel")) + .def_static("build", + [](std::shared_ptr handler, + py::object& obj, + const std::vector& subModelList, + const std::vector>& kinDynWrapperList) -> + std::unique_ptr { + // get the kindyn computation object from the swig bindings + std::shared_ptr* cls + = py::detail::swig_wrapped_pointer_to_pybind< + std::shared_ptr>(obj); + if (cls == nullptr) + { + throw ::pybind11::value_error("Invalid input for the function. Please provide " + "an iDynTree::KinDynComputations object."); + } + + auto estimator = + BipedalLocomotion::Estimators::RobotDynamicsEstimator::RobotDynamicsEstimator::build(handler, + *cls, + subModelList, + kinDynWrapperList); + + return estimator; + }, + py::arg("handler"), + py::arg("kinDynFullModel"), + py::arg("subModelList"), + py::arg("kinDynWrapperList")) + .def("setInitialState", + [](BipedalLocomotion::Estimators::RobotDynamicsEstimator::RobotDynamicsEstimator& obj, + const BipedalLocomotion::Estimators::RobotDynamicsEstimator::RobotDynamicsEstimatorOutput& state) -> bool + { + return obj.setInitialState(state); + }, + py::arg("initialState")); +} + +void CreateSubModel(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; + using namespace BipedalLocomotion::System; + + py::class_(module, "SubModel") + .def(py::init()); +} + +void CreateSubModelCreator(pybind11::module& module) +{ + namespace py = ::pybind11; + + auto setModelAndSensors = [] (BipedalLocomotion::Estimators::RobotDynamicsEstimator::SubModelCreator& subModelCreator, + ::pybind11::object& model, + ::pybind11::object& sensors) + { + iDynTree::Model* modelPtr + = pybind11::detail::swig_wrapped_pointer_to_pybind< + iDynTree::Model>(model); + + if (modelPtr == nullptr) + { + throw ::pybind11::value_error("Invalid input for the function. Please provide an " + "iDynTree::Model object."); + } + + iDynTree::SensorsList* sensorsPtr + = pybind11::detail::swig_wrapped_pointer_to_pybind< + iDynTree::SensorsList>(sensors); + + if (sensorsPtr == nullptr) + { + throw ::pybind11::value_error("Invalid input for the function. Please provide an " + "iDynTree::SensorsList object."); + } + + subModelCreator.setModelAndSensors(*modelPtr, *sensorsPtr); + }; + + auto createSubModels = [] (BipedalLocomotion::Estimators::RobotDynamicsEstimator::SubModelCreator& subModelCreator, + std::shared_ptr& parameterHandler) + { + subModelCreator.createSubModels(parameterHandler); + }; + + py::class_(module, "SubModelCreator") + .def(py::init()) + .def("createSubModels", + createSubModels, + py::arg("parameterHandler")) + .def("setModelAndSensors", + setModelAndSensors, + py::arg("model"), + py::arg("sensors")) + .def("setKinDyn", + BipedalLocomotion::bindings::RobotDynamicsEstimator::setKinDyn< + BipedalLocomotion::Estimators::RobotDynamicsEstimator::SubModelCreator>, + py::arg("kinDyn")) + .def("getNrOfSubModels", + &BipedalLocomotion::Estimators::RobotDynamicsEstimator::SubModelCreator::getNrOfSubModels) + .def("getSubModelList", + &BipedalLocomotion::Estimators::RobotDynamicsEstimator::SubModelCreator::getSubModelList) + .def("getSubModel", + &BipedalLocomotion::Estimators::RobotDynamicsEstimator::SubModelCreator::getSubModel, + py::arg("index")); +} + +void CreateSubModelKinDynWrapper(pybind11::module& module) +{ + namespace py = ::pybind11; + using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; + using namespace BipedalLocomotion::System; + + py::class_>(module, "SubModelKinDynWrapper") + .def(py::init()) + .def("setKinDyn", + BipedalLocomotion::bindings::RobotDynamicsEstimator::setKinDyn, + py::arg("kinDyn")) + .def("initialize", &SubModelKinDynWrapper::initialize) + .def("updateState", &SubModelKinDynWrapper::updateState) + .def("forwardDynamics", &SubModelKinDynWrapper::forwardDynamics) + .def("getBaseAcceleration", &SubModelKinDynWrapper::getBaseAcceleration) + .def("getBaseVelocity", &SubModelKinDynWrapper::getBaseVelocity) + .def("getBaseFrameName", &SubModelKinDynWrapper::getBaseFrameName) + .def("getMassMatrix", &SubModelKinDynWrapper::getMassMatrix) + .def("getGeneralizedForces", &SubModelKinDynWrapper::getGeneralizedForces) + .def("getFTJacobian", &SubModelKinDynWrapper::getFTJacobian) + .def("getAccelerometerJacobian", &SubModelKinDynWrapper::getAccelerometerJacobian) + .def("getGyroscopeJacobian", &SubModelKinDynWrapper::getGyroscopeJacobian) + .def("getExtContactJacobian", &SubModelKinDynWrapper::getExtContactJacobian) + .def("getAccelerometerBiasAcceleration", &SubModelKinDynWrapper::getAccelerometerBiasAcceleration) + .def("getAccelerometerRotation", &SubModelKinDynWrapper::getAccelerometerRotation); +} + +} // namespace RobotDynamicsEstimator +} // namespace bindings +} // namespace BipedalLocomotion diff --git a/bindings/python/bipedal_locomotion_framework.cpp.in b/bindings/python/bipedal_locomotion_framework.cpp.in index e724644cfe..0e1c3b3436 100644 --- a/bindings/python/bipedal_locomotion_framework.cpp.in +++ b/bindings/python/bipedal_locomotion_framework.cpp.in @@ -82,6 +82,10 @@ #include @endcmakeif FRAMEWORK_COMPILE_ContinuousDynamicalSystem +@cmakeif FRAMEWORK_COMPILE_RobotDynamicsEstimator +#include +@endcmakeif FRAMEWORK_COMPILE_RobotDynamicsEstimator + // Create the Python module PYBIND11_MODULE(bindings, m) { @@ -182,4 +186,9 @@ PYBIND11_MODULE(bindings, m) py::module continuousDynamicalSystemModule = m.def_submodule("continuous_dynamical_system"); bindings::ContinuousDynamicalSystem::CreateModule(continuousDynamicalSystemModule); @endcmakeif FRAMEWORK_COMPILE_ContinuousDynamicalSystem + + @cmakeif FRAMEWORK_COMPILE_RobotDynamicsEstimator + py::module robotDynamicsEstimatorModule = m.def_submodule("robot_dynamics_estimator"); + bindings::RobotDynamicsEstimator::CreateModule(robotDynamicsEstimatorModule); + @endcmakeif FRAMEWORK_COMPILE_RobotDynamicsEstimator } diff --git a/cmake/BipedalLocomotionFrameworkDependencies.cmake b/cmake/BipedalLocomotionFrameworkDependencies.cmake index a00fa4a60d..0b8fac5646 100644 --- a/cmake/BipedalLocomotionFrameworkDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkDependencies.cmake @@ -86,6 +86,10 @@ find_package(robometry 1.1.0 QUIET) checkandset_dependency(robometry MINIMUM_VERSION 1.1.0) dependency_classifier(robometry MINIMUM_VERSION 1.1.0 IS_USED ${FRAMEWORK_USE_robometry}) +find_package(BayesFilters QUIET) +checkandset_dependency(BayesFilters) +dependency_classifier(BayesFilters IS_USED ${FRAMEWORK_USE_BayesFilters}) + # required only for some tests find_package(icub-models 1.23.3 QUIET) checkandset_dependency(icub-models) @@ -170,7 +174,7 @@ framework_dependent_option(FRAMEWORK_COMPILE_FloatingBaseEstimators framework_dependent_option(FRAMEWORK_COMPILE_RobotDynamicsEstimator "Compile RobotDynamicsEstimator libraries?" ON - "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_System;FRAMEWORK_COMPILE_ManifConversions;FRAMEWORK_USE_manif" OFF) + "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_System;FRAMEWORK_COMPILE_ManifConversions;FRAMEWORK_USE_manif;FRAMEWORK_USE_BayesFilters" OFF) framework_dependent_option(FRAMEWORK_COMPILE_ManifConversions "Compile manif Conversions libraries?" ON diff --git a/src/Estimators/CMakeLists.txt b/src/Estimators/CMakeLists.txt index ec0785b758..8640170bc6 100644 --- a/src/Estimators/CMakeLists.txt +++ b/src/Estimators/CMakeLists.txt @@ -27,19 +27,17 @@ if(FRAMEWORK_COMPILE_RobotDynamicsEstimator) set(H_PREFIX include/BipedalLocomotion/RobotDynamicsEstimator) add_bipedal_locomotion_library( NAME RobotDynamicsEstimator - SOURCES src/SubModel.cpp - src/SubModelKinDynWrapper.cpp + SOURCES src/SubModel.cpp src/SubModelKinDynWrapper.cpp src/Dynamics.cpp src/ZeroVelocityDynamics.cpp src/FrictionTorqueStateDynamics.cpp + src/JointVelocityStateDynamics.cpp src/UkfState.cpp src/MotorCurrentMeasurementDynamics.cpp + src/UkfMeasurement.cpp src/AccelerometerMeasurementDynamics.cpp src/GyroscopeMeasurementDynamics.cpp + src/RobotDynamicsEstimator.cpp SUBDIRECTORIES tests/RobotDynamicsEstimator - PUBLIC_HEADERS ${H_PREFIX}/SubModel.h - ${H_PREFIX}/SubModelKinDynWrapper.h - PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler - BipedalLocomotion::TextLogging - BipedalLocomotion::ManifConversions - iDynTree::idyntree-high-level - iDynTree::idyntree-core - iDynTree::idyntree-model - iDynTree::idyntree-modelio - Eigen3::Eigen - MANIF::manif -) + PUBLIC_HEADERS ${H_PREFIX}/SubModel.h ${H_PREFIX}/SubModelKinDynWrapper.h ${H_PREFIX}/Dynamics.h ${H_PREFIX}/ZeroVelocityDynamics.h + ${H_PREFIX}/FrictionTorqueStateDynamics.h ${H_PREFIX}/JointVelocityStateDynamics.h ${H_PREFIX}/AccelerometerMeasurementDynamics.h + ${H_PREFIX}/UkfState.h ${H_PREFIX}/MotorCurrentMeasurementDynamics.h ${H_PREFIX}/UkfMeasurement.h + ${H_PREFIX}/GyroscopeMeasurementDynamics.h ${H_PREFIX}/RobotDynamicsEstimator.h + PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler MANIF::manif BipedalLocomotion::System BipedalLocomotion::Math iDynTree::idyntree-high-level + iDynTree::idyntree-core iDynTree::idyntree-model iDynTree::idyntree-modelio Eigen3::Eigen MANIF::manif BayesFilters::BayesFilters + PRIVATE_LINK_LIBRARIES BipedalLocomotion::TextLogging BipedalLocomotion::ManifConversions + ) endif() diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h new file mode 100644 index 0000000000..e1afeeebf5 --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/AccelerometerMeasurementDynamics.h @@ -0,0 +1,134 @@ +/** + * @file AccelerometerMeasurementDynamics.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_ACCELEROMETER_MEASUREMENT_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_ACCELEROMETER_MEASUREMENT_DYNAMICS_H + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * The AccelerometerMeasurementDynamics class is a concrete implementation of the Dynamics. + * Please use this element if you want to use the model dynamics of an accelerometer defined, + * using the kinematics, as the time derivative of the frame linear velocity: + * \f[ + * v = J \nu + * \f] + * The AccelerometerMeasurementDynamics represents the following equation in the continuous time: + * \f[ + * \dot{v}^{accelerometer} = \dot{J} \nu + J \dot{\nu} = \dot{J} \nu + J^{base} \dot{v}^{base} + J^{joints} \ddot{s} + * \f] + * where the joint acceleration is given by the forward dynamics equation. + */ + +class AccelerometerMeasurementDynamics : public Dynamics +{ + bool m_useBias{false}; /**< If true the dynamics depends on a bias additively. */ + Eigen::VectorXd m_bias; /**< The bias is initialized and used only if m_useBias is true. False if not specified. */ + std::string m_biasVariableName; /**< Name of the variable containing the bias in the variable handler. */ + std::vector m_subModelList; /** List of SubModel objects. */ + std::vector> m_kinDynWrapperList; /**< List of pointers to SubModelKinDynWrapper objects. */ + bool m_isSubModelListSet{false}; /**< Boolean flag saying if the sub-model list has been set. */ + double m_dT; /**< Sampling time. */ + std::vector m_subModelJointAcc; /**< Updated joint acceleration of each sub-model. */ + Eigen::Vector3d m_gravity; /**< Gravitational acceleration. */ + std::vector m_subModelsWithAccelerometer; /**< List of indeces saying which sub-model in the m_subDynamics list containa the accelerometer. */ + manif::SE3d::Tangent m_accFrameVel; /**< Velocity at the accelerometer frame. */ + +protected: + Eigen::VectorXd m_covSingleVar; + Eigen::VectorXd m_JdotNu; + Eigen::VectorXd m_JvdotBase; + Eigen::VectorXd m_Jsdotdot; + Eigen::Vector3d m_accRg; + Eigen::Vector3d m_vCrossW; + Eigen::Vector3d m_linVel; + Eigen::Vector3d m_angVel; + +public: + /* + * Constructor + */ + AccelerometerMeasurementDynamics(); + + /* + * Destructor + */ + virtual ~AccelerometerMeasurementDynamics(); + + /** + * Initialize the state dynamics. + * @param paramHandler pointer to the parameters handler. + * @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 | + * | `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 | + * | `dT` | `double` | Sampling time. | Yes | + * @return True in case of success, false otherwise. + */ + bool initialize(std::weak_ptr paramHandler) override; + + /** + * Finalize the Dynamics. + * @param stateVariableHandler object describing the variables in the state vector. + * @note You should call this method after you add ALL the state dynamics to the state variable handler. + * @return true in case of success, false otherwise. + */ + bool finalize(const System::VariablesHandler& stateVariableHandler) override; + + /** + * Set the SubModelKinDynWrapper object. + * @param kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return True in case of success, false otherwise. + */ + bool setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) override; + + /** + * Controls whether the variable handler contains the variables on which the dynamics depend. + * @return True in case all the dependencies are contained in the variable handler, false otherwise. + */ + bool checkStateVariableHandler() override; + + /** + * Update the content of the element. + * @return True in case of success, false otherwise. + */ + bool update() override; + + /** + * Set the state of the ukf needed to update the dynamics. + * @param ukfState reference to the ukf state. + */ + void setState(const Eigen::Ref ukfState) override; + + /** + * Set a `UKFInput` object. + * @param ukfInput reference to the UKFInput struct. + */ + void setInput(const UKFInput & ukfInput) override; + +}; // class AccelerometerMeasurementDynamics + +BLF_REGISTER_DYNAMICS(AccelerometerMeasurementDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics); + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_ACCELEROMETER_MEASUREMENT_DYNAMICS_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h new file mode 100644 index 0000000000..1b9d220d02 --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/Dynamics.h @@ -0,0 +1,206 @@ +/** + * @file Dynamics.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_DYNAMICS_H + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +/** + * BLF_REGISTER_DYNAMICS is a macro that can be used to register a Dynamics. The key of + * the dynamics will be the stringified version of the Dynamics C++ Type + * @param _model the model of the dynamics + * @param _baseModel the base model from which the _model inherits. + */ +#define BLF_REGISTER_DYNAMICS(_model, _baseModel) \ + static std::shared_ptr<_baseModel> _model##FactoryBuilder() \ +{ \ + return std::make_shared<_model>(); \ + }; \ + static std::string _model##BuilderAutoRegHook \ + = ::BipedalLocomotion::Estimators:: \ + RobotDynamicsEstimator::DynamicsFactory::registerBuilder \ + (#_model, _model##FactoryBuilder); + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * @brief The UKFInput struct represents the input of the ukf needed to update the dynamics. + */ +struct UKFInput +{ + Eigen::VectorXd robotJointPositions; /**< Vector of joint positions. */ + Eigen::VectorXd robotJointAccelerations; /**< Vector of joint accelerations. */ + manif::SE3d robotBasePose; /**< Robot base position and orientation. */ + manif::SE3d::Tangent robotBaseVelocity; /**< Robot base velocity. */ + manif::SE3d::Tangent robotBaseAcceleration; /**< Robot base acceleration. */ +}; + +/** + * UkfInputProvider describes the provider for the inputs of the ukf + */ +class UkfInputProvider : public System::Advanceable +{ +private: + UKFInput m_ukfInput; + +public: + /** + * Get the input + * @return A struct containing the inputs for the ukf + */ + const UKFInput& getOutput() const override; + + /** + * Set the state which represents the state of the provider. + * @param input is a struct containing the input of the ukf. + */ + bool setInput(const UKFInput& input) override; + + /** + * @brief Advance the internal state. This may change the value retrievable from getOutput(). + * @return True if the advance is successfull. + */ + bool advance() override; + + /** + * @brief Determines the validity of the object retrieved with getOutput() + * @return True if the object is valid, false otherwise. + */ + bool isOutputValid() const override; + +}; // class UkfProcessInputProvider + +/** + * Dynamics describes a generic dynamic model for a UKF state or measurement. + */ +class Dynamics +{ +protected: + int m_size; /**< Size of the variable associate to the Dynamics object. */ + Eigen::VectorXd m_updatedVariable; /**< Updated variable computed using the dynamic model. */ + std::string m_description{"Generic Dynamics Element"}; /**< String describing the type of the dynamics */ + std::string m_name; /**< Name of dynamics. */ + Eigen::VectorXd m_covariances; /**< Vector of covariances. */ + std::vector m_elements = {}; /**< Elements composing the variable vector. */ + std::string m_dynamicModel; + bool m_isInitialized{false}; /**< True if the dynamics has been initialized. */ + System::VariablesHandler m_stateVariableHandler; /**< Variable handler describing the variables and the sizes in the ukf state vector. */ + bool m_isStateVariableHandlerSet{false}; /**< True if setVariableHandler is called. */ + UKFInput m_ukfInput; + Eigen::VectorXd m_initialCovariances; /**< Vector of initial covariances. */ + + /** + * Controls whether the variable handler contains the variables on which the dynamics depend. + * @return True in case all the dependencies are contained in the variable handler, false otherwise. + */ + virtual bool checkStateVariableHandler(); + + +public: + /** + * Initialize the task. + * @param paramHandler pointer to the parameters handler. + * @return True in case of success, false otherwise. + */ + virtual bool + initialize(std::weak_ptr paramHandler); + + /** + * Finalize the Dynamics. + * @param handler object describing the variables in the vector for which all the dynamics are defined. + * @note You should call this method after you add ALL the dynamics to the variable handler. + * @return true in case of success, false otherwise. + */ + virtual bool finalize(const System::VariablesHandler& handler); + + /** + * Set the SubModelKinDynWrapper object. + * @param kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return True in case of success, false otherwise. + */ + virtual bool setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList); + + /** + * Update the dynamics of the variable. + * @return True in case of success, false otherwise. + */ + virtual bool update(); + + /** + * Get the next value m_updatedVariable. + * @return a const reference to the next variable value. + */ + Eigen::Ref getUpdatedVariable() const; + + /** + * Get the size of the state. + * @return the size of the state. + */ + int size() const; + + /** + * Set the state of the ukf needed to update the dynamics. + * @param ukfState reference to the ukf state. + */ + virtual void setState(const Eigen::Ref ukfState) = 0; + + /** + * Set a `UKFInput` object. + * @param ukfInput reference to the UKFInput struct. + */ + virtual void setInput(const UKFInput & ukfInput) = 0; + + /** + * @brief getCovariance access the covariance `Eigen::VectorXd` associated to the variables described by this dynamics. + * @return the vector of covariances. + */ + Eigen::Ref getCovariance(); + + /** + * @brief getInitialStateCovariance access the covariance `Eigen::VectorXd` associated to the initial state. + * @return the vector of covariances. + */ + Eigen::Ref getInitialStateCovariance(); + + /** + * Destructor. + */ + virtual ~Dynamics() = default; +}; + +/** + * DynamicsFactory implements the factory design patter for constructing a Dynamics given + * its model. + */ +class DynamicsFactory : public System::Factory +{ + +public: + virtual ~DynamicsFactory() = default; +}; + +} // RobotDynamicsEstimator +} // Estimators +} // BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_DYNAMICS_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h new file mode 100644 index 0000000000..efa966e0b8 --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/FrictionTorqueStateDynamics.h @@ -0,0 +1,128 @@ +/** + * @file FrictionTorqueStateDynamics.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_FRICTION_TORQUE_STATE_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_FRICTION_TORQUE_STATE_DYNAMICS_H + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * The FrictionTorqueDynamics class is a concrete implementation of the Dynamics. + * Please use this element if you want to use a specific friction torque model dynamics. + * The FrictionTorqueDynamics represents the following equation in the continuous time: + * \f[ + * \dot{\tau_{F}} = \ddot{s} ( k_{2} + k_{0} k_{1} (1 - tanh^{2} (k_{1} \dot{s})) ) + * \f] + * since the friction model implemented by this class is defined as: + * \f[ + * \tau_{F} = k_{0} tanh(k_{1} \dot{s}) + k_{2} \dot{s} + * \f] + * In the discrete time the dynamics is defined as: + * \f[ + * \tau_{F,k+1} = \tau_{F,k} + \Delta T \ddot{s} ( k_{2} + k_{0} k_{1} / cosh^{2}(k_{1} \dot{s,k}) ) + * \f] + */ + +class FrictionTorqueStateDynamics : public Dynamics +{ + Eigen::VectorXd m_jointVelocityFullModel; /**< Vector of joint velocities. */ + Eigen::VectorXd m_k0, m_k1, m_k2; /**< Friction parameters (see class description). */ + double m_dT; /**< Sampling time. */ + Eigen::VectorXd m_frictionTorqueFullModel; /**< Friction torque vector of full-model. */ + +protected: + Eigen::VectorXd m_coshArgument; + Eigen::VectorXd m_coshsquared; + Eigen::VectorXd m_k0k1; + Eigen::VectorXd m_dotTauF; + +public: + /* + * Constructor + */ + FrictionTorqueStateDynamics(); + + /* + * Destructor + */ + virtual ~FrictionTorqueStateDynamics(); + + /** + * Initialize the state dynamics. + * @param paramHandler pointer to the parameters handler. + * @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 | + * | `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 paramHandler) override; + + /** + * Finalize the Dynamics. + * @param stateVariableHandler object describing the variables in the state vector. + * @note You should call this method after you add ALL the state dynamics to the state variable handler. + * @return true in case of success, false otherwise. + */ + bool finalize(const System::VariablesHandler& stateVariableHandler) override; + + /** + * Set the SubModelKinDynWrapper object. + * @param kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return True in case of success, false otherwise. + */ + bool setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) override; + + /** + * Controls whether the variable handler contains the variables on which the dynamics depend. + * @return True in case all the dependencies are contained in the variable handler, false otherwise. + */ + bool checkStateVariableHandler() override; + + /** + * Update the content of the element. + * @return True in case of success, false otherwise. + */ + bool update() override; + + /** + * Set the state of the ukf needed to update the dynamics. + * @param ukfState reference to the ukf state. + */ + void setState(const Eigen::Ref ukfState) override; + + /** + * Set a `UKFInput` object. + * @param ukfInput reference to the UKFInput struct. + */ + void setInput(const UKFInput & ukfInput) override; +}; + +BLF_REGISTER_DYNAMICS(FrictionTorqueStateDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics); + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_FRICTION_TORQUE_STATE_DYNAMICS_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/GyroscopeMeasurementDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/GyroscopeMeasurementDynamics.h new file mode 100644 index 0000000000..82e384a856 --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/GyroscopeMeasurementDynamics.h @@ -0,0 +1,126 @@ +/** + * @file GyroscopeMeasurementDynamics.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_ACCELEROMETER_MEASUREMENT_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_ACCELEROMETER_MEASUREMENT_DYNAMICS_H + +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * The GyroscopeMeasurementDynamics class is a concrete implementation of the Dynamics. + * Please use this element if you want to use the model dynamics of a gyroscope. + * The GyroscopeMeasurementDynamics represents the following equation in the continuous time: + * \f[ + * \omega^{gyroscope} = J \nu = J^{base} v^{base} + J^{joints} \dot{s} + * \f] + * where the joint velocity is estimated by the ukf. + */ + +class GyroscopeMeasurementDynamics : public Dynamics +{ + bool m_useBias{false}; /**< If true the dynamics depends on a bias additively. */ + Eigen::VectorXd m_bias; /**< The bias is initialized and used only if m_useBias is true. False if not specified. */ + std::string m_biasVariableName; /**< Name of the variable containing the bias in the variable handler. */ + bool m_isSubModelListSet{false}; /**< Boolean flag saying if the sub-model list has been set. */ + double m_dT; /**< Sampling time. */ + int m_nrOfSubDynamics; /**< Number of sub-dynamics which corresponds to the number of sub-models. */ + std::vector> m_subModelKinDynList; /**< Vector of SubModelKinDynWrapper objects. */ + std::vector m_subModelList; /**< Vector of SubModel objects. */ + std::vector m_subModelJointVel; /**< Updated joint velocities of each sub-model. */ + std::vector m_subModelWithGyro; /**< List of indeces saying which sub-model in the m_subDynamics list containa the gyroscope. */ + Eigen::VectorXd m_jointVelocityFullModel; /**< Vector of joint velocities. */ + +protected: + Eigen::VectorXd m_covSingleVar; + manif::SE3d::Tangent m_subModelBaseVel; + Eigen::VectorXd m_JvBase; + Eigen::VectorXd m_Jsdot; + +public: + /* + * Constructor + */ + GyroscopeMeasurementDynamics(); + + /* + * Destructor + */ + virtual ~GyroscopeMeasurementDynamics(); + + /** + * Initialize the state dynamics. + * @param paramHandler pointer to the parameters handler. + * @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 | + * | `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 | + * | `dT` | `double` | Sampling time. | Yes | + * @return True in case of success, false otherwise. + */ + bool initialize(std::weak_ptr paramHandler) override; + + /** + * Finalize the Dynamics. + * @param stateVariableHandler object describing the variables in the state vector. + * @note You should call this method after you add ALL the state dynamics to the state variable handler. + * @return true in case of success, false otherwise. + */ + bool finalize(const System::VariablesHandler& stateVariableHandler) override; + + /** + * Set the SubModelKinDynWrapper object. + * @param kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return True in case of success, false otherwise. + */ + bool setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) override; + + /** + * Controls whether the variable handler contains the variables on which the dynamics depend. + * @return True in case all the dependencies are contained in the variable handler, false otherwise. + */ + bool checkStateVariableHandler() override; + + /** + * Update the content of the element. + * @return True in case of success, false otherwise. + */ + bool update() override; + + /** + * Set the state of the ukf needed to update the dynamics. + * @param ukfState reference to the ukf state. + */ + void setState(const Eigen::Ref ukfState) override; + + /** + * Set a `UKFInput` object. + * @param ukfInput reference to the UKFInput struct. + */ + void setInput(const UKFInput & ukfInput) override; + +}; // class GyroscopeMeasurementDynamics + +BLF_REGISTER_DYNAMICS(GyroscopeMeasurementDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics); + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_ACCELEROMETER_MEASUREMENT_DYNAMICS_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h new file mode 100644 index 0000000000..6a459671fa --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/JointVelocityStateDynamics.h @@ -0,0 +1,115 @@ +/** + * @file JointVelocityStateDynamics.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_JOINT_VELOCITY_STATE_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_JOINT_VELOCITY_STATE_DYNAMICS_H + +#include +#include + +#include +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * The JointVelocityDynamics class is a concrete implementation of the Dynamics. + * Please use this element if you want to use the robot dynaic model to update the joint dynamics. + * The JointVelocityDynamics represents the following equation in the continuous time: + * \f[ + * \ddot{s} = H^{-1} [\tau_{m} - \tau_{F} + (\sum J^T_{FT} f_{FT}) + + * + (\sum J^T_{ext} f_{ext}) - F^T {}^B \dot v - h ] + * \f] + */ + +class JointVelocityStateDynamics : public Dynamics +{ + double m_dT; /**< Sampling time. */ + Eigen::VectorXd m_jointVelocityFullModel; /**< Joint velocities of full-model. */ + +public: + /* + * Constructor + */ + JointVelocityStateDynamics(); + + /* + * Destructor + */ + virtual ~JointVelocityStateDynamics(); + + /** + * Set the SubModelKinDynWrapper object. + * @param kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return True in case of success, false otherwise. + */ + bool setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) override; + + /** + * Initialize the state dynamics. + * @param paramHandler pointer to the parameters handler. + * @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 | + * | `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` | `double` | Sampling time. | Yes | + * @return True in case of success, false otherwise. + */ + bool initialize(std::weak_ptr paramHandler) override; + + /** + * Finalize the Dynamics. + * @param stateVariableHandler object describing the variables in the state vector. + * @note You should call this method after you add ALL the state dynamics to the state variable handler. + * @return true in case of success, false otherwise. + */ + bool finalize(const System::VariablesHandler& stateVariableHandler) override; + + /** + * Controls whether the variable handler contains the variables on which the dynamics depend. + * @return True in case all the dependencies are contained in the variable handler, false otherwise. + */ + bool checkStateVariableHandler() override; + + /** + * Update the state. + * @return True in case of success, false otherwise. + */ + bool update() override; + + /** + * Set the state of the ukf needed to update the dynamics. + * @param ukfState reference to the ukf state. + */ + void setState(const Eigen::Ref ukfState) override; + + /** + * Set a `UKFInput` object. + * @param ukfInput reference to the UKFInput struct. + */ + void setInput(const UKFInput & ukfInput) override; +}; + +BLF_REGISTER_DYNAMICS(JointVelocityStateDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics); + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_JOINT_VELOCITY_STATE_DYNAMICS_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/MotorCurrentMeasurementDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/MotorCurrentMeasurementDynamics.h new file mode 100644 index 0000000000..74437f3b9c --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/MotorCurrentMeasurementDynamics.h @@ -0,0 +1,117 @@ +/** + * @file MotorCurrentMeasurementDynamics.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_MOTOR_CURRENT_MEASUREMENT_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_MOTOR_CURRENT_MEASUREMENT_DYNAMICS_H + +#include + +// Eigen +#include + +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * The MotorCurrentMeasurementDynamics class is a concrete implementation of the Dynamics. + * Please use this element to define the measurement of the motor current. + * The MotorCurrentMeasurementDynamics represents the following equation in the continuous time: + * \f[ + * \dot{i_{m}} = \tau_{m} / (k_{\tau} r) + * \f] + */ + +class MotorCurrentMeasurementDynamics : public Dynamics +{ +protected: + Eigen::VectorXd m_motorTorque; /**< Vector of joint velocities. */ + Eigen::VectorXd m_currentFrictionTorque; /**< Vector of friction torques. */ + Eigen::VectorXd m_kTau; /**< Torque constant. */ + Eigen::VectorXd m_gearRatio; /**< Gearbox reduction ratio. */ + +public: + /* + * Constructor + */ + MotorCurrentMeasurementDynamics(); + + /* + * Destructor + */ + virtual ~MotorCurrentMeasurementDynamics(); + + /** + * Initialize the measurement object. + * @param paramHandler pointer to the parameters handler. + * @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 | + * | `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 paramHandler) override; + + /** + * Finalize the Dynamics. + * @param measurementVariableHandler object describing the variables in the measurement vector. + * @note You should call this method after you add ALL the measurement dynamics to the measurement variable handler. + * @return true in case of success, false otherwise. + */ + bool finalize(const System::VariablesHandler& measurementVariableHandler) override; + + /** + * Set the SubModelKinDynWrapper object. + * @param kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return True in case of success, false otherwise. + */ + bool setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) override; + + /** + * Controls whether the variable handler contains the variables on which the dynamics depend. + * @return True in case all the dependencies are contained in the variable handler, false otherwise. + */ + bool checkStateVariableHandler() override; + + /** + * Update the content of the element. + * @return True in case of success, false otherwise. + */ + bool update() override; + + /** + * Set the state of the ukf needed to update the dynamics. + * @param ukfState reference to the ukf state. + * @return true if the current state has been updated correctly. + */ + void setState(const Eigen::Ref ukfState) override; + + /** + * Set a `UKFInput` object. + * @param ukfInput reference to the UKFInput struct. + */ + void setInput(const UKFInput & ukfInput) override; + +}; + +BLF_REGISTER_DYNAMICS(MotorCurrentMeasurementDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics); + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_MOTOR_CURRENT_MEASUREMENT_DYNAMICS_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h new file mode 100644 index 0000000000..abdb1464a0 --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/RobotDynamicsEstimator.h @@ -0,0 +1,170 @@ +/** + * @file RobotDynamicsEstimator.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_ROBOT_DYNAMICS_ESTIMATOR_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_ROBOT_DYNAMICS_ESTIMATOR_H + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * RobotDynamicsEstimatorInput of the RobotDynamicsEstimator containing both the inputs and the + * measurements for the Unscented Kalman Filter + */ +struct RobotDynamicsEstimatorInput +{ + manif::SE3d basePose; /**< Pose describing the robot base position and orientation. */ + manif::SE3d::Tangent baseVelocity; /**< Velocity of the robot base. */ + manif::SE3d::Tangent baseAcceleration; /**< Mixed acceleration of the robot base. */ + Eigen::VectorXd jointPositions; /**< Joints positions in rad. */ + Eigen::VectorXd jointVelocities; /**< Joints velocities in rad per sec. */ + Eigen::VectorXd motorCurrents; /**< Motor currents in Ampere. */ + std::map ftWrenches; /**< Wrenches measured by force/torque sensors. */ + std::map linearAccelerations; /**< Linear accelerations measured by accelerometer sensors. */ + std::map angularVelocities; /**< Angular velocities measured by gyroscope sensors. */ +}; + +/** + * RobotDynamicsEstimatorOutput of the RobotDynamicsEstimator which represents the estimation of the + * Unscented Kalman Filter + */ +struct RobotDynamicsEstimatorOutput +{ + Eigen::VectorXd ds; /**< Joints velocities in rad per sec. */ + Eigen::VectorXd tau_m; /**< Motor toruqes in Nm. */ + Eigen::VectorXd tau_F; /**< Motor friction torques in Nm. */ + std::map ftWrenches; /**< Wrenches at the force/torque sensors. */ + std::map ftWrenchesBiases; /**< Biases of the force/torque sensors. */ + std::map accelerometerBiases; /**< Biases of the accelerometer sensors. */ + std::map gyroscopeBiases; /**< Biases of the gyroscope sensors. */ +}; + +/** + * RobotDynamicsEstimator is a concrete class and implements a robot dynamics estimator. + * The estimator is here implemented as an Unscented Kalman Filter. The user can build the estimator + * by using the build method or can initialize the RobotDynamicsEstimator object and then create the + * UkfState model and UkfMeasurement model which are then passed to the `bfl::UKFPrediction` and + * `bfl::UKFCorrection` objects respectively. To run an estimation step the user should set the + * input using `setInput`, call the `advance` method to perform an estimation step, use `getOutput` + * to get the estimation result. + */ +class RobotDynamicsEstimator : public System::Advanceable +{ + /** + * Private implementation + */ + struct Impl; + std::unique_ptr m_pimpl; + +public: + /** + * Constructor. + */ + RobotDynamicsEstimator(); + + /** + * Destructor. + */ + virtual ~RobotDynamicsEstimator(); + + /** + * Initialize the RobotDynamicsEstimator. + * @param handler pointer to the IParametersHandler + * @note + * | Group | Parameter Name | Type | Description | Mandatory | + * |:---------:|:------------------------------:|:---------------:|:----------------------------------------------------------------------------------------------:|:---------:| + * | `GENERAL` | `sampling_time` | `double` | Sampling time | Yes | + * | `UKF` | `alpha` | `double` | `alpha` parameter of the unscented kalman filter | Yes | + * | `UKF` | `beta` | `double` | `beta` parameter of the unscented kalman filter | Yes | + * | `UKF` | `kappa` | `double` | `kappa` parameter of the unscented kalman filter | Yes | + * @return True in case of success, false otherwise. + */ + bool initialize(std::weak_ptr handler) override; + + /** + * @brief Finalize the estimator. + * @param stateVariableHandler reference to a `System::VariablesHandler` object describing the ukf state. + * @param measurementVariableHandler reference to a `System::VariablesHandler` object describing the ukf measurement. + * @param kinDynFullModel a pointer to an iDynTree::KinDynComputations object. + * @note You should call this method after initialized the estimator and created the UkfState model + * which builds the handler needed in input to this method. + * @return true in case of success, false otherwise. + */ + bool finalize(const System::VariablesHandler& stateVariableHandler, + const System::VariablesHandler& measurementVariableHandler, + std::shared_ptr kinDynFullModel); + + /** + * @brief build a robot dynamics estimator implementing an unscented kalman filter. + * @param handler pointer to the IParametersHandler interface. + * @param kinDynFullModel a pointer to an iDynTree::KinDynComputations object that will be shared among + * all the dynamics. + * @param subModelList a list of SubModel objects + * @param kinDynWrapperList a list of pointers to a `SubModelKinDynWrapper` objects that will be shared among + * all the dynamics + * @return a RobotDynamicsEstimator. In case of issues an invalid RobotDynamicsEstimator + * will be returned. + */ + static std::unique_ptr + build(std::weak_ptr handler, + std::shared_ptr kinDynFullModel, + const std::vector& subModelList, + const std::vector>& kinDynWrapperList); + + /** + * @brief set the initial state of the estimator. + * @param initialState a reference to an `Output` object. + * @return true in case of success, false otherwise. + */ + bool setInitialState(const RobotDynamicsEstimatorOutput& initialState); + + /** + * @brief Determines the validity of the object retrieved with getOutput() + * @return True if the object is valid, false otherwise. + */ + bool isOutputValid() const override; + + /** + * @brief Advance the internal state. This may change the value retrievable from getOutput(). + * @return True if the advance is successfull. + */ + bool advance() override; + + /** + * Set the input for the estimator. + * @param input is a struct containing the input of the estimator. + */ + bool setInput(const RobotDynamicsEstimatorInput& input) override; + + /** + * Get the output of the ukf + * @return A struct containing the ukf estimation result. + */ + const RobotDynamicsEstimatorOutput& getOutput() const override; + +}; // class RobotDynamicsEstimator + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_ROBOT_DYNAMICS_ESTIMATOR_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h index ddae85d386..97893b9ceb 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModel.h @@ -56,6 +56,7 @@ struct FT : Sensor Direction forceDirection = Direction::NotSpecified; /**< Force direction depending on which side of the sensor is considered (+1 or -1)*/ + std::string associatedJoint; /**< Name of the fixd joint used to represent the ft sensor in the model. */ }; /** @@ -147,24 +148,70 @@ class SubModel /** * @brief Access an element of the force/torque sensor list. + * @param index is the index of the force/torque sensor in the submodel * @return FT object associated with the specified index. */ const FT& getFTSensor(const int index) const; + /** + * @brief Access an element of the force/torque sensor list. + * @param is the name of the force/torque sensor. + * @return FT object associated with the specified name. + */ + const FT& getFTSensor(const std::string name) const; + + /** + * @brief hasFTSensor check if the force/torque sensor is part of the sub-model + * @param name is the name of the ft sensor + * @return true if the sensor is found, false otherwise + */ + bool hasFTSensor(const std::string name) const; + /** * @brief Access an element of the accelerometer list. + * @param index is the index of the accelerometer in the submodel * @return a Sensor object corresponding to the accelerometer associated with the specified index. */ const Sensor& getAccelerometer(const int index) const; + /** + * @brief Access an element of the accelerometer list. + * @param is the name of the accelerometer. + * @return a Sensor object corresponding to the accelerometer associated with the specified name. + */ + const Sensor& getAccelerometer(const std::string name) const; + + /** + * @brief hasAccelerometer check if the accelerometer is part of the sub-model + * @param name is the name of the accelerometer + * @return true if the sensor is found, false otherwise + */ + bool hasAccelerometer(const std::string name) const; + /** * @brief Access an element of the gyroscope list. + * @param index is the index of the gyroscope in the submodel * @return a Sensor object corresponding to the gyroscope associated with the specified index. */ const Sensor& getGyroscope(const int index) const; + /** + * @brief Access an element of the gyroscope list. + * @param is the name of the gyroscoper. + * @return a Sensor object corresponding to the gyroscope associated with the specified name. + */ + const Sensor& getGyroscope(const std::string name) const; + + /** + * @brief hasAccelerometer check if the gyroscope is part of the sub-model + * @param name is the name of the gyroscope + * @return true if the sensor is found, false otherwise + */ + bool hasGyroscope(const std::string name) const; + /** * @brief access an element of the contact frame list. + * @param index is the index of the external contact in the submodel. * @return a string corresponding to the external contact frame associated with the specified index. */ const std::string& getExternalContact(const int index) const; @@ -265,13 +312,13 @@ class SubModelCreator /** * @brief createSubModels splits the model in SubModel objects cutting the model at the * force/torque sensors specified by the parameterHandler. - * @param ftSensorList list of Sensor structs. + * @param ftSensorList list of FT structs. * @param accList list of Sensor structs. * @param gyroList list of Sensor structs. * @param externalContacts list of strings. * @return a boolean value saying if the subModelList has been created correctly. */ - bool createSubModels(const std::vector& ftSensorList, + bool createSubModels(const std::vector& ftSensorList, const std::vector& accList, const std::vector& gyroList, const std::vector& externalContacts); diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelDynamics.h new file mode 100644 index 0000000000..b29b7e0e2e --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelDynamics.h @@ -0,0 +1,114 @@ +/** + * @file SubModelDynamics.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_SUB_MODEL_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_SUB_MODEL_DYNAMICS_H + +#include +#include +#include + +#include + +#include +#include + +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +struct SubModelDynamics +{ + std::shared_ptr kinDynWrapper; /**< object containing the kinematics and dynamics properties of a sub-model. */ + SubModel subModel; /**< Sub-model object storing information about the model (joints, sensors, applied wrenches) */ + std::map FTMap; /**< The map contains names of the ft sensors and values of the wrench */ + std::map extContactMap; /**< The map contains names of the ft sensors and values of the wrench */ + manif::SE3d::Tangent baseAcceleration; /**< Velocity of the base of the sub-model. */ + Eigen::VectorXd motorTorque; /**< Motor torque vector of sub-model. */ + Eigen::VectorXd frictionTorque; /**< Friction torque vector of sub-model. */ + Eigen::VectorXd jointVelocity; /**< Joint velocities of sub-model. */ + Eigen::VectorXd totalTorqueFromContacts; /**< Joint torques due to known and unknown contacts on the sub-model. */ + Math::Wrenchd wrench; /**< Joint torques due to a specific contact. */ + Eigen::VectorXd torqueFromContact; /**< Joint torques due to a specific contact. */ + + /* + * Constructor + */ + SubModelDynamics(); + + /* + * Destructor + */ + virtual ~SubModelDynamics(); + + /** + * @brief Set the SubModelKinDynWrapper object. + * @param kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return true in case of success, false otherwise. + */ + bool setKinDynWrapper(std::shared_ptr kinDynWrapper); + + /** + * @brief Set the SubModel object. + * @param subModel pointer to a SubModel object. + * @return true in case of success, false otherwise. + */ + bool setSubModel(const SubModel& subModel); + + /** + * @brief Initialize the SubDynamics. + * @return true in case of success, false otherwise. + */ + bool initialize(); + + /** + * @brief setState set the state of the variables used to compute the inverse dynamics. + * @param ukfState an Eigen::Vector representing the state computed by the ukf estimator. + * @param jointVelocityFullModel `Eigen::Vector` representing the velocity of the robot joints. + * @param motorTorqueFullModel `Eigen::Vector` representing the torque of the robot motors. + * @param frictionTorqueFullModel `Eigen::Vector` representing the friction torque at the robot motors. + * @param variableHandler handler of the state of the ukf. + */ + void setState(const Eigen::Ref ukfState, + const Eigen::Ref jointVelocityFullModel, + const Eigen::Ref motorTorqueFullModel, + const Eigen::Ref frictionTorqueFullModel, + const System::VariablesHandler& variableHandler); + + /** + * @brief update computes the joint accelerations given by the forward dynamics and the joint velocities given + * by the numerical integration of the joint accelerations. + * @param fullModelBaseAcceleration is a `manif::SE3d::Tangent` representing the acceleration of the base of the full model. + * @param fullModelJointAcceleration is a `Eigen::VectorXd` representing the acceleration of the joints of the full model. + * @param updatedJointAcceleration is a `Eigen::VectorXd` representing the joint accelerations of the sub-model. + * @return true in case of success, false otherwise. + * @note this method computes the joint acceleration of the sub-model, but it requires the joint acceleration of the full model. + * Actually the only joint accelerations needed to compute the ones of this sub-model are the joint accelerations + * of the previous sub-models as they are used to compute the acceleration of the base of this sub-model. The rest of the joint + * accelerations, which are computed here, can be set to zero as those values are not used. + */ + bool update(manif::SE3d::Tangent& fullModelBaseAcceleration, + Eigen::Ref fullModelJointAcceleration, + Eigen::Ref updatedJointAcceleration); + + /** + * @brief Compute the contribution of external contacts on the joint torques. + */ + void computeTotalTorqueFromContacts(); +}; + +} // RobotDynamicsEstimator +} // Estimators +} // BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_SUB_MODEL_DYNAMICS_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h index f2692e34c0..723be4feec 100644 --- a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/SubModelKinDynWrapper.h @@ -53,11 +53,15 @@ class SubModelKinDynWrapper */ std::map m_accRworldList; /**< Rotation matrix of the accelerometer frame wrt world */ + std::map m_accVelList; /**< Acceleration of the + accelerometers */ manif::SE3d::Tangent m_baseVelocity; /**< Velocity of the base of the sub-model */ std::string m_baseFrame; /**< Name of the base frame of the sub-model */ std::shared_ptr m_kinDynFullModel; + manif::SE3d::Tangent m_subModelBaseAcceleration; /** Acceleration of the sub-model base. */ + protected: int m_numOfJoints; /**< Number of joints in the sub-model */ Eigen::MatrixXd m_FTranspose; /**< It is the bottom-left block of the mass matrix, that is, @@ -81,7 +85,12 @@ class SubModelKinDynWrapper * updateDynamicsVariableState updates the value of all the member variables containing * information about the robot kinematics and dynamics */ - bool updateDynamicsVariableState(); + bool updateDynamicsVariableState(bool updateRobotDynamicsOnly); + + /** + * @brief Compute the contribution of external contacts on the joint torques. + */ + void computeTotalTorqueFromContacts(); public: /** @@ -102,10 +111,15 @@ class SubModelKinDynWrapper initialize(const SubModel& subModel); /** - * @brief updateInternalKinDynState updates the state of the KinDynWrapper object. + * @brief updateState updates the state of the KinDynWrapper object. + * @param robotBaseAcceleration is a manif::SE3d::Tangent representing the robot base acceleration. + * @param robotJointAcceleration is a Eigen reference to a Eigen::VectorXd containing the joint accelerations. + * @param isCorrectStep is a boolean saying if the method is called during the predict step or the correct step. * @return a boolean value saying if the subModelList has been created correctly. */ - bool updateInternalKinDynState(); + bool updateState(manif::SE3d::Tangent& robotBaseAcceleration, + Eigen::Ref robotJointAcceleration, + bool isCorrectStep); /** * @brief forwardDynamics computes the free floaing forward dynamics @@ -125,18 +139,13 @@ class SubModelKinDynWrapper /** * @brief getBaseAcceleration gets the acceleration of the sub-model base. - * @param robotBaseAcceleration the acceleration of the robot base. - * @param robotJointAcceleration the acceleration of the robot joints. - * @param subModelBaseAcceleration the base acceleration of the sub-model. - * @return a boolean value. + * @return subModelBaseAcceleration the acceleration of the sub-model base. */ - bool getBaseAcceleration(manif::SE3d::Tangent& robotBaseAcceleration, - Eigen::Ref robotJointAcceleration, - manif::SE3d::Tangent& subModelBaseAcceleration); + const manif::SE3d::Tangent& getBaseAcceleration(); /** * @brief getBaseVelocity gets the acceleration of the sub-model base. - * @return baseVelocity the the base velocity of the sub-model. + * @return baseVelocity the velocity of the sub-model base. */ const manif::SE3d::Tangent& getBaseVelocity(); @@ -204,6 +213,13 @@ class SubModelKinDynWrapper * @return a boolean value saying if the rotation matrix is found. */ const manif::SO3d& getAccelerometerRotation(const std::string& accName) const; + + /** + * @brief getAccelerometerVelocity access the velocity of the accelerometer specified by the input param. + * @param accName is the name of the accelerometer. + * @return the velocity of the accelerometer. + */ + const manif::SE3d::Tangent& getAccelerometerVelocity(const std::string& accName); }; } // namespace RobotDynamicsEstimator diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h new file mode 100644 index 0000000000..79971b8bde --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfMeasurement.h @@ -0,0 +1,240 @@ +/** + * @file UkfMeasurement.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_MEASUREMENT_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_MEASUREMENT_H + +#include +#include +#include + +// BayesFilters +#include + +// BLF +#include +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * UkfMeasurement is a concrete class that represents the Measurement of the estimator. + * The user should build the dynamic model of the measurement, setting a variable handler + * describing the variables composing the measurement vector, the list of the dynamic models + * associated to each variable, and the matrix of covariances associated to the variable in the + * measurement vector. The user should set also a ukf input provider + * which provides the inputs needed to update the dynamics. + */ + +class UkfMeasurement : public bfl::AdditiveMeasurementModel +{ + /** + * Private implementation + */ + struct Impl; + std::unique_ptr m_pimpl; + +public: + /** + * Constructor. + */ + UkfMeasurement(); + + /** + * Destructor. + */ + virtual ~UkfMeasurement(); + + /** + * Build the ukf measurement model + * @param kinDyn a pointer to an iDynTree::KinDynComputations object that will be shared among + * all the dynamics. + * @param subModelList a vector of SubModel objects. + * @param kinDynWrapperList a vector of pointers to SubModelKinDynWrapper objects + * @param handler pointer to the IParametersHandler interface. + * @param stateVariableHandler a variable handler describing the variables in the state vector of the ukf. + * @note the following parameters are required by the class + * | Parameter Name | Type | Description | Mandatory | + * |:------------------------------:|:---------------:|:----------------------------------------------------------------------------------------------:|:---------:| + * | `sampling_time` | `double` | Sampling time. | Yes | + * | `dynamics_list` |`vector` | List of dynamics composing the measurement model. | Yes | + * For **each** dynamics listed in the parameter `dynamics_list` the user must specified all the parameters + * required by the dynamics itself but `dT` since is already specified in the parent group. + * Moreover the following parameters are required for each dynamics. + * | Group | Parameter Name | Type | Description | Mandatory | + * |:-------------:|:------------------------------:|:--------------------:|:----------------------------------------------------------------------------------------------:|:---------:| + * |`DYNAMICS_NAME`| `name` | `string` | String representing the name of the dynamics. | Yes | + * |`DYNAMICS_NAME`| `elements` |`std::vector` | Vector of strings representing the elements composing the specific dynamics. | No | + * |`DYNAMICS_NAME`| `covariance` |`std::vector` | Vector of double containing the covariance associated to each element. | Yes | + * |`DYNAMICS_NAME`| `dynamic_model` | `string` | String representing the type of dynamics. The string should match the name of the C++ class. | Yes | + * |`DYNAMICS_NAME`| `use_bias` | `boolean` | Boolean saying if an additive bias must be used in the dynamic model. | No | + * |`DYNAMICS_NAME`| `use_bias` | `boolean` | Boolean saying if an additive bias must be used in the dynamic model. | No | + * `DYNAMICS_NAME` is a placeholder for the name of the dynamics contained in the `dynamics_list` list. + * `name` can contain only the following values ("ds", "i_m", "*_ft_sensor", "*_ft_acc", "*_ft_gyro"). + * @note The following `ini` file presents an example of the configuration that can be used to + * build the UkfMeasurement. + * + * UkfMeasurement.ini + * + * dynamics_list ("JOINT_VELOCITY", "MOTOR_CURRENT", "RIGHT_LEG_FT", "RIGHT_FOOT_REAR_GYRO") + * + * [JOINT_VELOCITY] + * name "ds" + * elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + * covariance (1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3) + * dynamic_model "JointVelocityDynamics" + * + * [FRICTION_TORQUE] + * name "tau_F" + * elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + * covariance (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-1) + * dynamic_model "FrictionTorqueDynamics" + * friction_k0 (9.106, 5.03, 4.93, 12.88, 14.34, 1.12) + * friction_k1 (200.0, 6.9, 200.0, 59.87, 200.0, 200.0) + * friction_k2 (1.767, 5.64, 0.27, 2.0, 3.0, 0.0) + * + * [RIGHT_LEG_FT] + * name "r_leg_ft_sensor" + * elements ("fx", "fy", "fz", "mx", "my", "mz") + * covariance (1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4) + * dynamic_model "ZeroVelocityDynamics" + * + * [RIGHT_FOOT_REAR_GYRO_BIAS] + * name "r_foot_rear_ft_gyro_bias" + * elements ("x", "y", "z") + * covariance (8.2e-8, 1e-2, 9.3e-3) + * dynamic_model "ZeroVelocityDynamics" + * + * ~~~~~ + * @return a std::unique_ptr to the UkfMeasurement. + * In case of issues, an empty BipedalLocomotion::System::VariablesHandler + * and an invalid pointer will be returned. + */ + static std::unique_ptr build(std::weak_ptr handler, + System::VariablesHandler& stateVariableHandler, + std::shared_ptr kinDynFullModel, + const std::vector& subModelList, + const std::vector>& kinDynWrapperList); + + /** + * Initialize the ukf measurement model. + * @param handler pointer to the IParametersHandler interface. + * @note the following parameters are required by the class + * | Parameter Name | Type | Description | Mandatory | + * |:------------------------------:|:---------------:|:----------------------------------------------------------------------------------------------:|:---------:| + * | `sampling_time` | `double` | Sampling time. | Yes | + * @return True in case of success, false otherwise. + */ + bool initialize(std::weak_ptr handler); + + /** + * Finalize the UkfMeasurement. + * @param handler variable handler. + * @note You should call this method after you initialize the UkfMeasurement. + * @return true in case of success, false otherwise. + */ + bool finalize(const System::VariablesHandler& handler); + + /** + * @brief setUkfInputProvider set the provider for the ukf input. + * @param ukfInputProvider a pointer to a structure containing the joint positions and the robot base pose, velocity and acceleration. + */ + void setUkfInputProvider(std::shared_ptr ukfInputProvider); + + /** + * @brief getMeasurementVariableHandler access the `System::VariablesHandler` instance created during the initialization phase. + * @return the measurement variable handler containing all the measurement variables and their sizes and offsets. + */ + System::VariablesHandler& getMeasurementVariableHandler(); + + /** + * @brief predictedMeasure predict the new measurement depending on the state computed by the predict step. + * @param cur_states is the state computed by the prediction phase. + * @return a std::pair where the bool value says if the measurement + * prediciton is done correctly and the bfl::Data is the predicted measure. + */ + std::pair predictedMeasure(const Eigen::Ref& cur_states) const override; + + /** + * @brief getNoiseCovarianceMatrix access the `Eigen::MatrixXd` representing the process covariance matrix. + * @return a boolean value and the measurement noise covariance matrix. + */ + std::pair getNoiseCovarianceMatrix() const override; + + /** + * @brief setProperty is not implemented. + * @param property is a string. + * @return false as it is not implemented. + */ + bool setProperty(const std::string& property) override { return false; }; + + /** + * @brief getMeasurementDescription access the `bfl::VectorDescription`. + * @return the measurement vector description. + */ + bfl::VectorDescription getMeasurementDescription() const override; + + /** + * @brief getInputDescription access the `bfl::VectorDescription`. + * @return the input vector description. + */ + bfl::VectorDescription getInputDescription() const override; + + /** + * @brief getMeasurementSize access the length of the measurement vector + * @return the length of measurement vector + */ + std::size_t getMeasurementSize(); + + /** + * Set a `System::VariableHandler` describing the variables composing the state. + * @param stateVariableHandler is the variable handler + */ + void setStateVariableHandler(System::VariablesHandler stateVariableHandler) const; + + /** + * @brief innovation computes the innovation step of the ukf update as the difference between the predicted_measurement + * and the measurement. + * @param predicted_measurements is a `blf::Data` reference representing the measurement predicted in the update step. + * @param measurements is a `blf::Data` reference representing the measurements coming from the user. + * @return a `std::pair` where the boolean value is always true and the `bfl::Data` is the innovation term. + */ + std::pair innovation(const bfl::Data& predicted_measurements, const bfl::Data& measurements) const override; + + /** + * @brief measure get the updated measurement. + * @param data is a `const blf::Data` reference and is optional parameter. + * @return a pair of a boolean value which is always true and the measurements. + */ + std::pair measure(const bfl::Data& data = bfl::Data()) const override; + + /** + * @brief freeze update the measurement using data from sensors. + * @param data is a generic object representing data coming from sensors. + * @return true. + * @note data in this case must be a `std::map` where + * the first element represents the name of each measurement dynamics + * and the second element is the vector containing the measurement + * of the sensor associated to that variable. + */ + bool freeze(const bfl::Data& data = bfl::Data()) override; + +}; // classUkfMeasurement + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_MEASUREMENT_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h new file mode 100644 index 0000000000..8fa255077e --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/UkfState.h @@ -0,0 +1,205 @@ +/** + * @file UkfState.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_STATE_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_STATE_H + +#include +#include +#include + +// BayesFilters +#include + +// BLF +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * UkfState is a concrete class that represents the State of the estimator. + * The user should build the dynamic model of the state setting a variable handler + * describing the variables composing the state, the list of the dynamic model associated + * to each variable in the variable handler, and the matrix of covariances + * associated to the state. The user should set also a ukf input provider + * which provides the inputs needed to update the dynamics. + */ +class UkfState : public bfl::AdditiveStateModel +{ + /** + * Private implementation + */ + struct Impl; + std::unique_ptr m_pimpl; + +// void unpackState(); +// void updateJointAccelerationState(); + +public: + /** + * Constructor. + */ + UkfState(); + + /** + * Destructor. + */ + virtual ~UkfState(); + + /** + * Build the ukf state model + * @param kinDyn a pointer to an iDynTree::KinDynComputations object that will be shared among + * all the dynamics. + * @param subModelList a vector of pairs (SubModel, SubModelKinDynWrapper) that will be shared among all the dynamics. + * @param handler pointer to the IParametersHandler interface. + * @note the following parameters are required by the class + * | Parameter Name | Type | Description | Mandatory | + * |:------------------------------:|:---------------:|:----------------------------------------------------------------------------------------------:|:---------:| + * | `dT` | `double` | Sampling time. | Yes | + * | `dynamics_list` |`vector` | List of dynamics composing the state model. | Yes | + * For **each** dynamics listed in the parameter `dynamics_list` the user must specified all the parameters + * required by the dynamics itself but `dT` since is already specified in the parent group. + * Moreover the following parameters are required for each dynamics. + * | Group | Parameter Name | Type | Description | Mandatory | + * |:-------------:|:------------------------------:|:--------------------:|:----------------------------------------------------------------------------------------------:|:---------:| + * |`DYNAMICS_NAME`| `name` | `string` | String representing the name of the dynamics. | Yes | + * |`DYNAMICS_NAME`| `elements` |`std::vector` | Vector of strings representing the elements composing the specific dynamics. | No | + * |`DYNAMICS_NAME`| `covariance` |`std::vector` | Vector of double containing the covariance associated to each element. | Yes | + * |`DYNAMICS_NAME`| `dynamic_model` | `string` | String representing the type of dynamics. The string should match the name of the C++ class. | Yes | + * |`DYNAMICS_NAME`| `friction_k0` |`std::vector` | Vector of double containing the coefficient k0 of the friction model of each element. | No | + * |`DYNAMICS_NAME`| `friction_k1` |`std::vector` | Vector of double containing the coefficient k1 of the friction model of each element. | No | + * |`DYNAMICS_NAME`| `friction_k2` |`std::vector` | Vector of double containing the coefficient k2 of the friction model of each element. | No | + * `DYNAMICS_NAME` is a placeholder for the name of the dynamics contained in the `dynamics_list` list. + * `name` can contain only the following values ("ds", "tau_m", "tau_F", "*_ft_sensor", "*_ft_sensor_bias", "*_ft_acc_bias", "*_ftgyro_bias"). + * @note The following `ini` file presents an example of the configuration that can be used to + * build the UkfState. + * + * UkfState.ini + * + * dynamics_list ("JOINT_VELOCITY", "FRICTION_TORQUE", "RIGHT_LEG_FT", "RIGHT_FOOT_REAR_GYRO_BIAS") + * + * [JOINT_VELOCITY] + * name "ds" + * elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + * covariance (1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3) + * dynamic_model "JointVelocityDynamics" + * + * [FRICTION_TORQUE] + * name "tau_F" + * elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + * covariance (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-1) + * dynamic_model "FrictionTorqueDynamics" + * friction_k0 (9.106, 5.03, 4.93, 12.88, 14.34, 1.12) + * friction_k1 (200.0, 6.9, 200.0, 59.87, 200.0, 200.0) + * friction_k2 (1.767, 5.64, 0.27, 2.0, 3.0, 0.0) + * + * [RIGHT_LEG_FT] + * name "r_leg_ft_sensor" + * elements ("fx", "fy", "fz", "mx", "my", "mz") + * covariance (1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4) + * dynamic_model "ZeroVelocityDynamics" + * + * [RIGHT_FOOT_REAR_GYRO_BIAS] + * name "r_foot_rear_ft_gyro_bias" + * elements ("x", "y", "z") + * covariance (8.2e-8, 1e-2, 9.3e-3) + * dynamic_model "ZeroVelocityDynamics" + * + * ~~~~~ + * @return a std::unique_ptr to the UkfState. + * In case of issues, an empty BipedalLocomotion::System::VariablesHandler + * and an invalid pointer will be returned. + */ + static std::unique_ptr build(std::weak_ptr handler, + std::shared_ptr kinDynFullModel, + const std::vector& subModelList, + const std::vector>& kinDynWrapperList); + + /** + * Initialize the ukf state model. + * @param handler pointer to the IParametersHandler interface. + * @note the following parameters are required by the class + * | Parameter Name | Type | Description | Mandatory | + * |:------------------------------:|:---------------:|:----------------------------------------------------------------------------------------------:|:---------:| + * | `dT` | `double` | Sampling time. | Yes | + * @return True in case of success, false otherwise. + */ + bool initialize(std::weak_ptr handler); + + /** + * Finalize the UkfState. + * @param handler variable handler. + * @note You should call this method after you initialize the UkfState. + * @return true in case of success, false otherwise. + */ + bool finalize(const System::VariablesHandler& handler); + + /** + * @brief setUkfInputProvider set the provider for the ukf input. + * @param ukfInputProvider a pointer to a structure containing the joint positions and the robot base pose, velocity and acceleration. + */ + void setUkfInputProvider(std::shared_ptr ukfInputProvider); + + /** + * @brief getStateVariableHandler access the `System::VariablesHandler` instance created during the initialization phase. + * @return the state variable handler containing all the state variables and their sizes and offsets. + */ + System::VariablesHandler& getStateVariableHandler(); + + /** + * @brief propagate implements the predict of the ukf + * @param cur_states is the state computed at the previous step + * @param prop_states is the predicted state + */ + void propagate(const Eigen::Ref& cur_states, Eigen::Ref prop_states) override; + + /** + * @brief getNoiseCovarianceMatrix access the `Eigen::MatrixXd` representing the process covariance matrix. + * @return the process noise covariance matrix. + */ + Eigen::MatrixXd getNoiseCovarianceMatrix() override; + + /** + * @brief setProperty is not implemented. + * @param property is a string. + * @return false as it is not implemented. + */ + bool setProperty(const std::string& property) override { return false; }; + + /** + * @brief getStateDescription access the `bfl::VectorDescription`. + * @return the state vector description. + */ + bfl::VectorDescription getStateDescription() override; + + /** + * @brief getStateSize access the length of the state vector. + * @return the length of state vector. + */ + std::size_t getStateSize(); + + /** + * @brief getInitialStateCovarianceMatrix access the `Eigen::MatrixXd` representing the initial state covariance matrix. + * @return a Eigen reference to the Eigen Matrix covariance. + */ + Eigen::Ref getInitialStateCovarianceMatrix(); + +}; // class UKFModel + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_UKF_STATE_H diff --git a/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h new file mode 100644 index 0000000000..ef8b3e04f4 --- /dev/null +++ b/src/Estimators/include/BipedalLocomotion/RobotDynamicsEstimator/ZeroVelocityDynamics.h @@ -0,0 +1,106 @@ +/** + * @file ZeroVelocityDynamics.h + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_ZERO_VELOCITY_DYNAMICS_H +#define BIPEDAL_LOCOMOTION_ESTIMATORS_ZERO_VELOCITY_DYNAMICS_H + +#include +#include + +namespace BipedalLocomotion +{ +namespace Estimators +{ +namespace RobotDynamicsEstimator +{ + +/** + * The ZeroVelocityDynamics class is a concrete implementation of the Dynamics. + * Please use this element if you do not know the specific dynamics of a state variable. + * The ZeroVelocityDynamics represents the following equation in the continuous time: + * \f[ + * \dot{x} = 0 + * \f] + * In the discrete time the following dynamics assigns the current state to the next state: + * \f[ + * x_{k+1} = x_{k} + * \f] + */ + +class ZeroVelocityDynamics : public Dynamics +{ +protected: + Eigen::VectorXd m_currentState; /**< Current state. */ + bool m_useBias{false}; /**< If true the dynamics depends on a bias additively. */ + Eigen::VectorXd m_bias; /**< The bias is initialized and used only if m_useBias is true. False if not specified. */ + std::string m_biasVariableName; /**< Name of the variable containing the bias in the variable handler. */ + + /** + * Controls whether the variable handler contains the variables on which the dynamics depend. + * @return True in case all the dependencies are contained in the variable handler, false otherwise. + */ + bool checkStateVariableHandler() override; + +public: + /** + * Initialize the state dynamics. + * @param paramHandler pointer to the parameters handler. + * @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 | + * | `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 | + * | `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 paramHandler) override; + + /** + * Finalize the Dynamics. + * @param stateVariableHandler object describing the variables in the state vector. + * @note You should call this method after you add ALL the state dynamics to the state variable handler. + * @return true in case of success, false otherwise. + */ + bool finalize(const System::VariablesHandler& stateVariableHandler) override; + + /** + * Set the SubModelKinDynWrapper object. + * @param kinDynWrapper pointer to a SubModelKinDynWrapper object. + * @return True in case of success, false otherwise. + */ + bool setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) override; + + /** + * Update the content of the element. + * @return True in case of success, false otherwise. + */ + bool update() override; + + /** + * Set the state of the ukf needed to update the dynamics. + * @param ukfState reference to the ukf state. + */ + void setState(const Eigen::Ref ukfState) override; + + /** + * Set a `UKFInput` object. + * @param ukfInput reference to the UKFInput struct. + */ + void setInput(const UKFInput & ukfInput) override; + +}; // class ZeroVelocityDynamics + +BLF_REGISTER_DYNAMICS(ZeroVelocityDynamics, ::BipedalLocomotion::Estimators::RobotDynamicsEstimator::Dynamics); + +} // namespace RobotDynamicsEstimator +} // namespace Estimators +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_ESTIMATORS_ZERO_VELOCITY_DYNAMICS_H diff --git a/src/Estimators/src/AccelerometerMeasurementDynamics.cpp b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp new file mode 100644 index 0000000000..a8f482eca1 --- /dev/null +++ b/src/Estimators/src/AccelerometerMeasurementDynamics.cpp @@ -0,0 +1,318 @@ +/** + * @file AccelerometerMeasurementDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +RDE::AccelerometerMeasurementDynamics::AccelerometerMeasurementDynamics() = default; + +RDE::AccelerometerMeasurementDynamics::~AccelerometerMeasurementDynamics() = default; + +bool RDE::AccelerometerMeasurementDynamics::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[AccelerometerMeasurementDynamics::initialize]"; + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + // Set the state dynamics name + if (!ptr->getParameter("name", m_name)) + { + log()->error("{} Error while retrieving the name variable.", errorPrefix); + return false; + } + + // Set the state process covariance + if (!ptr->getParameter("covariance", m_covSingleVar)) + { + log()->error("{} Error while retrieving the covariance variable.", errorPrefix); + return false; + } + + // Set the dynamic model type + if (!ptr->getParameter("dynamic_model", m_dynamicModel)) + { + log()->error("{} Error while retrieving the dynamic_model variable.", errorPrefix); + return false; + } + + // Set the list of elements if it exists + if (!ptr->getParameter("elements", m_elements)) + { + log()->info("{} Variable elements not found.", errorPrefix); + m_elements = {}; + } + + if (!ptr->getParameter("sampling_time", m_dT)) + { + log()->error("{} Error while retrieving the sampling_time variable.", errorPrefix); + return false; + } + + // Set the bias related variables if use_bias is true + if (!ptr->getParameter("use_bias", m_useBias)) + { + log()->info("{} Variable use_bias not found. Set to false by default.", errorPrefix); + } + else + { + m_biasVariableName = m_name + "_bias"; + } + + m_gravity.setZero(); + m_gravity[2] = - BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + m_JdotNu.resize(6); + m_JvdotBase.resize(6); + m_Jsdotdot.resize(6); + + m_description = "Accelerometer measurement dynamics"; + + m_isInitialized = true; + + return true; +} + +bool RDE::AccelerometerMeasurementDynamics::finalize(const System::VariablesHandler &stateVariableHandler) +{ + constexpr auto errorPrefix = "[AccelerometerMeasurementDynamics::finalize]"; + + if (!m_isInitialized) + { + log()->error("{} Please initialize the dynamics before calling finalize.", errorPrefix); + return false; + } + + if (!m_isSubModelListSet) + { + log()->error("{} Please call `setSubModels` before finalizing.", errorPrefix); + return false; + } + + if (stateVariableHandler.getNumberOfVariables() == 0) + { + log()->error("{} The state variable handler is empty.", errorPrefix); + return false; + } + + m_stateVariableHandler = stateVariableHandler; + + if (!checkStateVariableHandler()) + { + log()->error("{} The state variable handler is not valid.", errorPrefix); + return false; + } + + // Search and save all the submodels containing the sensor + for (int submodelIndex = 0; submodelIndex < m_subModelList.size(); submodelIndex++) + { + if (m_subModelList[submodelIndex].hasAccelerometer(m_name)) + { + m_subModelsWithAccelerometer.push_back(submodelIndex); + } + } + + m_covariances.resize(m_covSingleVar.size() * m_subModelsWithAccelerometer.size()); + for (int index = 0; index < m_subModelsWithAccelerometer.size(); index++) + { + m_covariances.segment(index*m_covSingleVar.size(), m_covSingleVar.size()) = m_covSingleVar; + } + + m_size = m_covariances.size(); + + m_subModelJointAcc.resize(m_subModelList.size()); + + for (int idx = 0; idx < m_subModelList.size(); idx++) + { + m_subModelJointAcc[idx].resize(m_subModelList[idx].getJointMapping().size()); + m_subModelJointAcc[idx].setZero(); + } + + m_bias.resize(m_covSingleVar.size()); + m_bias.setZero(); + + m_updatedVariable.resize(m_size); + m_updatedVariable.setZero(); + + m_accFrameVel.setZero(); + + m_linVel.setZero(); + m_angVel.setZero(); + + return true; +} + +bool RDE::AccelerometerMeasurementDynamics::setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) +{ + constexpr auto errorPrefix = "[AccelerometerMeasurementDynamics::setSubModels]"; + + m_subModelList = subModelList; + + m_kinDynWrapperList = kinDynWrapperList; + + m_isSubModelListSet = true; + + return true; +} + +bool RDE::AccelerometerMeasurementDynamics::checkStateVariableHandler() +{ + constexpr auto errorPrefix = "[AccelerometerMeasurementDynamics::checkStateVariableHandler]"; + + if (!m_isSubModelListSet) + { + log()->error("{} Set the sub-model list before setting the variable handler", errorPrefix); + return false; + } + + if (!m_stateVariableHandler.getVariable("tau_m").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `tau_m`.", errorPrefix); + return false; + } + + // Check if the variable handler contains the variables used by this dynamics + if (!m_stateVariableHandler.getVariable("tau_F").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `tau_F`.", errorPrefix); + return false; + } + + if (!m_stateVariableHandler.getVariable("ds").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `ds`.", errorPrefix); + return false; + } + + if (m_useBias) + { + if (!m_stateVariableHandler.getVariable(m_biasVariableName).isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_biasVariableName); + return false; + } + } + + // check if all the sensors are part of the sub-model + for (int subModelIdx = 0; subModelIdx < m_subModelList.size(); subModelIdx++) + { + for (int ftIdx = 0; ftIdx < m_subModelList[subModelIdx].getNrOfFTSensor(); ftIdx++) + { + if (!m_stateVariableHandler.getVariable(m_subModelList[subModelIdx].getFTSensor(ftIdx).name).isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subModelList[subModelIdx].getFTSensor(ftIdx).name); + return false; + } + } + + for (int contactIdx = 0; contactIdx < m_subModelList[subModelIdx].getNrOfExternalContact(); contactIdx++) + { + if (!m_stateVariableHandler.getVariable(m_subModelList[subModelIdx].getExternalContact(contactIdx)).isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_subModelList[subModelIdx].getExternalContact(contactIdx)); + return false; + } + } + } + + return true; +} + +bool RDE::AccelerometerMeasurementDynamics::update() +{ + constexpr auto errorPrefix = "[AccelerometerMeasurementDynamics::update]"; + + // compute joint acceleration per each sub-model containing the accelerometer + for (int subDynamicsIdx = 0; subDynamicsIdx < m_subModelList.size(); subDynamicsIdx++) + { + if (m_subModelList[subDynamicsIdx].getModel().getNrOfDOFs() > 0) + { + for (int jointIdx = 0; jointIdx < m_subModelList[subDynamicsIdx].getJointMapping().size(); jointIdx++) + { + m_subModelJointAcc[subDynamicsIdx][jointIdx] = m_ukfInput.robotJointAccelerations[m_subModelList[subDynamicsIdx].getJointMapping()[jointIdx]]; + } + } + } + + // J_dot nu + base_J v_dot_base + joint_J s_dotdot - acc_Rot_world gravity + bias + for (int index = 0; index < m_subModelsWithAccelerometer.size(); index++) + { +// std::cout << "------------------------------------------------------- Submodel index " << m_subModelsWithAccelerometer[index] << ", acc name " << m_name << std::endl; + + m_JdotNu = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerBiasAcceleration(m_name); + +// std::cout << "m_JdotNu" << std::endl; +// std::cout << m_JdotNu << std::endl; + + m_JvdotBase = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerJacobian(m_name).block(0, 0, 6, 6) * + m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getBaseAcceleration().coeffs(); + +// std::cout << "m_JvdotBase" << std::endl; +// std::cout << m_JvdotBase << std::endl; + + m_accRg = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerRotation(m_name).rotation() * m_gravity; + +// std::cout << "m_accRg" << std::endl; +// std::cout << m_accRg << std::endl; + +// std::cout << "accelerometer velocity\n" << m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs() << std::endl; + + m_linVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(0, 3); + m_angVel = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerVelocity(m_name).coeffs().segment(3, 3); + +// std::cout << "Lin vel" << std::endl; +// std::cout << m_linVel << std::endl; +// std::cout << "Ang vel" << std::endl; +// std::cout << m_angVel << std::endl; + + m_vCrossW = m_linVel.cross(m_angVel); + +// std::cout << "m_vCrossW" << std::endl; +// std::cout << m_vCrossW << std::endl; + + m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) = m_JdotNu.segment(0, 3) + m_JvdotBase.segment(0, 3) - m_vCrossW - m_accRg; + + if (m_useBias) + { + m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) = m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) + m_bias; + } + + if (m_subModelList[m_subModelsWithAccelerometer[index]].getJointMapping().size() > 0) + { + m_Jsdotdot = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerJacobian(m_name). + block(0, 6, 6, m_subModelJointAcc[m_subModelsWithAccelerometer[index]].size()) * + m_subModelJointAcc[m_subModelsWithAccelerometer[index]]; + + m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) += m_Jsdotdot.segment(0, 3); + } + } + + return true; +} + +void RDE::AccelerometerMeasurementDynamics::setState(const Eigen::Ref ukfState) +{ + if (m_useBias) + { + m_bias = ukfState.segment(m_stateVariableHandler.getVariable(m_biasVariableName).offset, + m_stateVariableHandler.getVariable(m_biasVariableName).size); + } +} + +void RDE::AccelerometerMeasurementDynamics::setInput(const UKFInput& ukfInput) +{ + m_ukfInput = ukfInput; +} diff --git a/src/Estimators/src/Dynamics.cpp b/src/Estimators/src/Dynamics.cpp new file mode 100644 index 0000000000..e489d5715f --- /dev/null +++ b/src/Estimators/src/Dynamics.cpp @@ -0,0 +1,79 @@ +/** + * @file Dynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::ParametersHandler; + +const RDE::UKFInput& RDE::UkfInputProvider::getOutput() const +{ + return m_ukfInput; +} + +bool RDE::UkfInputProvider::advance() +{ + return true; +} + +bool RDE::UkfInputProvider::setInput(const UKFInput& input) +{ + m_ukfInput = input; + + return true; +} + +bool RDE::UkfInputProvider::isOutputValid() const +{ + return m_ukfInput.robotJointPositions.size() != 0; +} + +bool RDE::Dynamics::initialize(std::weak_ptr paramHandler) +{ + return true; +} + +bool RDE::Dynamics::finalize(const System::VariablesHandler& stateVariableHandler) +{ + return true; +} + +bool RDE::Dynamics::setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) +{ + return true; +} + +bool RDE::Dynamics::update() +{ + return true; +} + +Eigen::Ref RDE::Dynamics::getUpdatedVariable() const +{ + return m_updatedVariable; +} + +int RDE::Dynamics::size() const +{ + return m_size; +} + +Eigen::Ref RDE::Dynamics::getCovariance() +{ + return m_covariances; +} + +bool RDE::Dynamics::checkStateVariableHandler() +{ + return true; +} + +Eigen::Ref RDE::Dynamics::getInitialStateCovariance() +{ + return m_initialCovariances; +} diff --git a/src/Estimators/src/FrictionTorqueStateDynamics.cpp b/src/Estimators/src/FrictionTorqueStateDynamics.cpp new file mode 100644 index 0000000000..ed5516fbe1 --- /dev/null +++ b/src/Estimators/src/FrictionTorqueStateDynamics.cpp @@ -0,0 +1,212 @@ +/** + * @file FrictionTorqueStateDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +RDE::FrictionTorqueStateDynamics::FrictionTorqueStateDynamics() = default; + +RDE::FrictionTorqueStateDynamics::~FrictionTorqueStateDynamics() = default; + +bool RDE::FrictionTorqueStateDynamics::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::initialize]"; + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + // Set the state dynamics name + if (!ptr->getParameter("name", m_name)) + { + log()->error("{} Error while retrieving the name variable.", errorPrefix); + return false; + } + + // Set the state process covariance + if (!ptr->getParameter("covariance", m_covariances)) + { + log()->error("{} Error while retrieving the covariance variable.", errorPrefix); + return false; + } + + // Set the state initial covariance + if (!ptr->getParameter("initial_covariance", m_initialCovariances)) + { + log()->error("{} Error while retrieving the initial_covariance variable.", errorPrefix); + return false; + } + + // Set the dynamic model type + if (!ptr->getParameter("dynamic_model", m_dynamicModel)) + { + log()->error("{} Error while retrieving the dynamic_model variable.", errorPrefix); + return false; + } + + // Set the list of elements if it exists + if (!ptr->getParameter("elements", m_elements)) + { + log()->info("{} Variable elements not found.", errorPrefix); + m_elements = {}; + } + + // Set the friction parameters + if (!ptr->getParameter("friction_k0", m_k0)) + { + log()->error("{} Error while retrieving the friction_k0 variable.", errorPrefix); + return false; + } + + if (!ptr->getParameter("friction_k1", m_k1)) + { + log()->error("{} Error while retrieving the friction_k1 variable.", errorPrefix); + return false; + } + + if (!ptr->getParameter("friction_k2", m_k2)) + { + log()->error("{} Error while retrieving the friction_k2 variable.", errorPrefix); + return false; + } + + if (!ptr->getParameter("sampling_time", m_dT)) + { + log()->error("{} Error while retrieving the sampling_time variable.", errorPrefix); + return false; + } + + m_description = "Friction torque state dynamics"; + + m_isInitialized = true; + + return true; +} + +bool RDE::FrictionTorqueStateDynamics::finalize(const System::VariablesHandler &stateVariableHandler) +{ + constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::finalize]"; + + if (!m_isInitialized) + { + log()->error("{} Please initialize the dynamics before calling finalize.", errorPrefix); + return false; + } + + if (stateVariableHandler.getNumberOfVariables() == 0) + { + log()->error("{} The state variable handler is empty.", errorPrefix); + return false; + } + + m_stateVariableHandler = stateVariableHandler; + + if (!checkStateVariableHandler()) + { + log()->error("{} The state variable handler is not valid.", errorPrefix); + return false; + } + + m_size = m_covariances.size(); + + m_frictionTorqueFullModel.resize(m_stateVariableHandler.getVariable("tau_F").size); + m_frictionTorqueFullModel.setZero(); + + m_jointVelocityFullModel.resize(m_stateVariableHandler.getVariable("ds").size); + m_jointVelocityFullModel.setZero(); + + m_updatedVariable.resize(m_size); + m_updatedVariable.setZero(); + + m_coshArgument.resize(m_size); + m_coshArgument.setZero(); + + m_coshsquared.resize(m_size); + m_coshsquared.setZero(); + + m_k0k1.resize(m_size); + m_k0k1.setZero(); + + m_dotTauF.resize(m_size); + m_dotTauF.setZero(); + + return true; +} + +bool RDE::FrictionTorqueStateDynamics::setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) +{ + return true; +} + +bool RDE::FrictionTorqueStateDynamics::checkStateVariableHandler() +{ + constexpr auto errorPrefix = "[FrictionTorqueStateDynamics::checkStateVariableHandler]"; + + if (!m_stateVariableHandler.getVariable("tau_m").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `tau_m`.", errorPrefix); + return false; + } + + // Check if the variable handler contains the variables used by this dynamics + if (!m_stateVariableHandler.getVariable("tau_F").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `tau_F`.", errorPrefix); + return false; + } + + if (!m_stateVariableHandler.getVariable("ds").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `ds`.", errorPrefix); + return false; + } + + return true; +} + +// TODO +// Change the model +bool RDE::FrictionTorqueStateDynamics::update() +{ + // k_{1} \dot{s,k} + m_coshArgument = m_k1.array() * m_jointVelocityFullModel.array(); + + // tanh (k_{1} \dot{s,k})) + m_coshsquared = m_coshArgument.array().cosh().square(); + + // k_{0} k_{1} + m_k0k1 = m_k0.array() * m_k1.array(); + + // \ddot{s,k} ( k_{2} + k_{0} k_{1} (1 - tanh^{2} (k_{1} \dot{s,k})) ) + m_dotTauF = m_ukfInput.robotJointAccelerations.array() * ( m_k2.array() + m_k0k1.array() / m_coshsquared.array() ); + + // \tau_{F,k+1} = \tau_{F,k} + \Delta T * \dot{\tau_{F,k}} + m_updatedVariable = m_frictionTorqueFullModel + m_dT * m_dotTauF; + + return true; +} + +void RDE::FrictionTorqueStateDynamics::setState(const Eigen::Ref ukfState) +{ + m_jointVelocityFullModel = ukfState.segment(m_stateVariableHandler.getVariable("ds").offset, + m_stateVariableHandler.getVariable("ds").size); + + m_frictionTorqueFullModel = ukfState.segment(m_stateVariableHandler.getVariable("tau_F").offset, + m_stateVariableHandler.getVariable("tau_F").size); +} + +void RDE::FrictionTorqueStateDynamics::setInput(const UKFInput& ukfInput) +{ + m_ukfInput = ukfInput; +} diff --git a/src/Estimators/src/GyroscopeMeasurementDynamics.cpp b/src/Estimators/src/GyroscopeMeasurementDynamics.cpp new file mode 100644 index 0000000000..ba2fee4d4c --- /dev/null +++ b/src/Estimators/src/GyroscopeMeasurementDynamics.cpp @@ -0,0 +1,245 @@ +/** + * @file GyroscopeMeasurementDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +RDE::GyroscopeMeasurementDynamics::GyroscopeMeasurementDynamics() = default; + +RDE::GyroscopeMeasurementDynamics::~GyroscopeMeasurementDynamics() = default; + +bool RDE::GyroscopeMeasurementDynamics::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[GyroscopeMeasurementDynamics::initialize]"; + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + // Set the state dynamics name + if (!ptr->getParameter("name", m_name)) + { + log()->error("{} Error while retrieving the name variable.", errorPrefix); + return false; + } + + // Set the state process covariance + if (!ptr->getParameter("covariance", m_covSingleVar)) + { + log()->error("{} Error while retrieving the covariance variable.", errorPrefix); + return false; + } + + // Set the dynamic model type + if (!ptr->getParameter("dynamic_model", m_dynamicModel)) + { + log()->error("{} Error while retrieving the dynamic_model variable.", errorPrefix); + return false; + } + + // Set the list of elements if it exists + if (!ptr->getParameter("elements", m_elements)) + { + log()->info("{} Variable elements not found.", errorPrefix); + m_elements = {}; + } + + if (!ptr->getParameter("sampling_time", m_dT)) + { + log()->error("{} Error while retrieving the sampling_time variable.", errorPrefix); + return false; + } + + // Set the bias related variables if use_bias is true + if (!ptr->getParameter("use_bias", m_useBias)) + { + log()->info("{} Variable use_bias not found. Set to false by default.", errorPrefix); + } + else + { + m_biasVariableName = m_name + "_bias"; + } + + m_description = "Gyroscope measurement dynamics"; + + m_isInitialized = true; + + return true; +} + +bool RDE::GyroscopeMeasurementDynamics::finalize(const System::VariablesHandler &stateVariableHandler) +{ + constexpr auto errorPrefix = "[GyroscopeMeasurementDynamics::finalize]"; + + if (!m_isInitialized) + { + log()->error("{} Please initialize the dynamics before calling finalize.", errorPrefix); + return false; + } + + if (stateVariableHandler.getNumberOfVariables() == 0) + { + log()->error("{} The state variable handler is empty.", errorPrefix); + return false; + } + + m_stateVariableHandler = stateVariableHandler; + + if (!checkStateVariableHandler()) + { + log()->error("{} The state variable handler is not valid.", errorPrefix); + return false; + } + + // Search and save all the submodels containing the sensor + for (int submodelIndex = 0; submodelIndex < m_nrOfSubDynamics; submodelIndex++) + { + if (m_subModelList[submodelIndex].hasGyroscope(m_name)) + { + m_subModelWithGyro.push_back(submodelIndex); + } + } + + m_covariances.resize(m_covSingleVar.size() * m_subModelWithGyro.size()); + for (int index = 0; index < m_subModelWithGyro.size(); index++) + { + m_covariances.segment(index*m_covSingleVar.size(), m_covSingleVar.size()) = m_covSingleVar; + } + + m_size = m_covariances.size(); + + m_jointVelocityFullModel.resize(m_stateVariableHandler.getVariable("ds").size); + m_jointVelocityFullModel.setZero(); + + m_subModelJointVel.resize(m_nrOfSubDynamics); + + for (int idx = 0; idx < m_nrOfSubDynamics; idx++) + { + m_subModelJointVel[idx].resize(m_subModelList[idx].getJointMapping().size()); + m_subModelJointVel[idx].setZero(); + } + + m_bias.resize(m_covSingleVar.size()); + m_bias.setZero(); + + m_updatedVariable.resize(m_size); + m_updatedVariable.setZero(); + + return true; +} + +bool RDE::GyroscopeMeasurementDynamics::setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) +{ + constexpr auto errorPrefix = "[GyroscopeMeasurementDynamics::setSubModels]"; + + m_subModelList = subModelList; + m_subModelKinDynList = kinDynWrapperList; + + if (m_subModelList.size() == 0 || m_subModelKinDynList.size() == 0 || m_subModelList.size() != m_subModelKinDynList.size()) + { + log()->error("{} Wrong size of input parameters", errorPrefix); + return false; + } + + m_nrOfSubDynamics = m_subModelList.size(); + + m_isSubModelListSet = true; + + return true; +} + +bool RDE::GyroscopeMeasurementDynamics::checkStateVariableHandler() +{ + constexpr auto errorPrefix = "[GyroscopeMeasurementDynamics::checkStateVariableHandler]"; + + if (!m_isSubModelListSet) + { + log()->error("{} Set the sub-model list before setting the variable handler", errorPrefix); + return false; + } + + if (!m_stateVariableHandler.getVariable("ds").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `ds`.", errorPrefix); + return false; + } + + if (m_useBias) + { + if (!m_stateVariableHandler.getVariable(m_biasVariableName).isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_biasVariableName); + return false; + } + } + + return true; +} + +bool RDE::GyroscopeMeasurementDynamics::update() +{ + constexpr auto errorPrefix = "[GyroscopeMeasurementDynamics::update]"; + + // base_J v_base + joint_J s_dot + bias + for (int index = 0; index < m_subModelWithGyro.size(); index++) + { + m_subModelBaseVel = m_subModelKinDynList[m_subModelWithGyro[index]]->getBaseVelocity(); + + m_JvBase = m_subModelKinDynList[m_subModelWithGyro[index]]->getGyroscopeJacobian(m_name).block(0, 0, 6, 6) * m_subModelBaseVel.coeffs(); + + m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) = m_JvBase.segment(3, 3); + + if (m_useBias) + { + m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) += m_bias; + } + + if (m_subModelList[m_subModelWithGyro[index]].getJointMapping().size() > 0) + { + m_Jsdot = m_subModelKinDynList[m_subModelWithGyro[index]]->getGyroscopeJacobian(m_name).block(0, 6, 6, m_subModelList[m_subModelWithGyro[index]].getModel().getNrOfDOFs()) * + m_subModelJointVel[m_subModelWithGyro[index]]; + + m_updatedVariable.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) += m_Jsdot.segment(3, 3); + } + } + + return true; +} + +void RDE::GyroscopeMeasurementDynamics::setState(const Eigen::Ref ukfState) +{ + m_jointVelocityFullModel = ukfState.segment(m_stateVariableHandler.getVariable("ds").offset, + m_stateVariableHandler.getVariable("ds").size); + + for (int smIndex = 0; smIndex < m_subModelList.size(); smIndex++) + { + for (int jntIndex = 0; jntIndex < m_subModelList[smIndex].getModel().getNrOfDOFs(); jntIndex++) + { + m_subModelJointVel[smIndex][jntIndex] = + m_jointVelocityFullModel[m_subModelList[smIndex].getJointMapping()[jntIndex]]; + } + } + + if (m_useBias) + { + m_bias = ukfState.segment(m_stateVariableHandler.getVariable(m_biasVariableName).offset, + m_stateVariableHandler.getVariable(m_biasVariableName).size); + } +} + +void RDE::GyroscopeMeasurementDynamics::setInput(const UKFInput& ukfInput) +{ + m_ukfInput = ukfInput; +} diff --git a/src/Estimators/src/JointVelocityStateDynamics.cpp b/src/Estimators/src/JointVelocityStateDynamics.cpp new file mode 100644 index 0000000000..c9f25a4328 --- /dev/null +++ b/src/Estimators/src/JointVelocityStateDynamics.cpp @@ -0,0 +1,154 @@ +/** + * @file JointVelocityStateDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +RDE::JointVelocityStateDynamics::JointVelocityStateDynamics() = default; + +RDE::JointVelocityStateDynamics::~JointVelocityStateDynamics() = default; + +bool RDE::JointVelocityStateDynamics::setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) +{ + return true; +} + +bool RDE::JointVelocityStateDynamics::checkStateVariableHandler() +{ + constexpr auto errorPrefix = "[JointVelocityStateDynamics::checkStateVariableHandler]"; + + // Check if the variable handler contains the variables used by this dynamics + if (!m_stateVariableHandler.getVariable("tau_m").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `tau_m`.", errorPrefix); + return false; + } + + if (!m_stateVariableHandler.getVariable("tau_F").isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `tau_F`.", errorPrefix); + return false; + } + + return true; +} + +bool RDE::JointVelocityStateDynamics::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[JointVelocityStateDynamics::initialize]"; + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + // Set the state dynamics name + if (!ptr->getParameter("name", m_name)) + { + log()->error("{} Error while retrieving the name variable.", errorPrefix); + return false; + } + + // Set the state process covariance + if (!ptr->getParameter("covariance", m_covariances)) + { + log()->error("{} Error while retrieving the covariance variable.", errorPrefix); + return false; + } + + // Set the state initial covariance + if (!ptr->getParameter("initial_covariance", m_initialCovariances)) + { + log()->error("{} Error while retrieving the initial_covariance variable.", errorPrefix); + return false; + } + + // Set the dynamic model type + if (!ptr->getParameter("dynamic_model", m_dynamicModel)) + { + log()->error("{} Error while retrieving the dynamic_model variable.", errorPrefix); + return false; + } + + // Set the list of elements if it exists + if (!ptr->getParameter("elements", m_elements)) + { + log()->info("{} Variable elements not found.", errorPrefix); + m_elements = {}; + } + + if (!ptr->getParameter("sampling_time", m_dT)) + { + log()->info("{} Error while retrieving the sampling_time variable.", errorPrefix); + m_elements = {}; + } + + m_description = "Joint velocity state dynamics depending on the robot dynamic model"; + + m_isInitialized = true; + + return true; +} + +bool RDE::JointVelocityStateDynamics::finalize(const System::VariablesHandler &stateVariableHandler) +{ + constexpr auto errorPrefix = "[JointVelocityStateDynamics::finalize]"; + + if (!m_isInitialized) + { + log()->error("{} Please initialize the dynamics before calling finalize.", errorPrefix); + return false; + } + + if (stateVariableHandler.getNumberOfVariables() == 0) + { + log()->error("{} The state variable handler is empty.", errorPrefix); + return false; + } + + m_stateVariableHandler = stateVariableHandler; + + if (!checkStateVariableHandler()) + { + log()->error("{} The state variable handler is not valid.", errorPrefix); + return false; + } + + m_size = m_covariances.size(); + + m_jointVelocityFullModel.resize(m_stateVariableHandler.getVariable("ds").size); + m_jointVelocityFullModel.setZero(); + + m_updatedVariable.resize(m_stateVariableHandler.getVariable("ds").size); + m_updatedVariable.setZero(); + + return true; +} + + +bool RDE::JointVelocityStateDynamics::update() +{ + m_updatedVariable = m_jointVelocityFullModel + m_dT * m_ukfInput.robotJointAccelerations; + + return true; +} + +void RDE::JointVelocityStateDynamics::setState(const Eigen::Ref ukfState) +{ + m_jointVelocityFullModel = ukfState.segment(m_stateVariableHandler.getVariable("ds").offset, + m_stateVariableHandler.getVariable("ds").size); +} + +void RDE::JointVelocityStateDynamics::setInput(const UKFInput& ukfInput) +{ + m_ukfInput = ukfInput; +} diff --git a/src/Estimators/src/MotorCurrentMeasurementDynamics.cpp b/src/Estimators/src/MotorCurrentMeasurementDynamics.cpp new file mode 100644 index 0000000000..2452d9d7ce --- /dev/null +++ b/src/Estimators/src/MotorCurrentMeasurementDynamics.cpp @@ -0,0 +1,149 @@ +/** + * @file MotorCurrentMeasurementDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +RDE::MotorCurrentMeasurementDynamics::MotorCurrentMeasurementDynamics() = default; + +RDE::MotorCurrentMeasurementDynamics::~MotorCurrentMeasurementDynamics() = default; + +bool RDE::MotorCurrentMeasurementDynamics::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[MotorCurrentMeasurementDynamics::initialize]"; + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + // Set the state dynamics name + if (!ptr->getParameter("name", m_name)) + { + log()->error("{} Error while retrieving the name variable.", errorPrefix); + return false; + } + + // Set the state process covariance + if (!ptr->getParameter("covariance", this->m_covariances)) + { + log()->error("{} Error while retrieving the covariance variable.", errorPrefix); + return false; + } + + // Set the dynamic model type + if (!ptr->getParameter("dynamic_model", m_dynamicModel)) + { + log()->error("{} Error while retrieving the dynamic_model variable.", errorPrefix); + return false; + } + + // Set the list of elements if it exists + if (!ptr->getParameter("elements", m_elements)) + { + log()->info("{} Variable elements not found.", errorPrefix); + m_elements = {}; + } + + // Set the torque constants + if (!ptr->getParameter("torque_constant", m_kTau)) + { + log()->error("{} Error while retrieving the torque_constant variable.", errorPrefix); + return false; + } + + // Set the gearbox reduction ratio + if (!ptr->getParameter("gear_ratio", m_gearRatio)) + { + log()->error("{} Error while retrieving the gear_ratio variable.", errorPrefix); + return false; + } + + m_description = "Motor current measurement dynamics"; + + m_isInitialized = true; + + return true; +} + +bool RDE::MotorCurrentMeasurementDynamics::finalize(const System::VariablesHandler &stateVariableHandler) +{ + constexpr auto errorPrefix = "[MotorCurrentMeasurementDynamics::finalize]"; + + if (!m_isInitialized) + { + log()->error("{} Please initialize the dynamics before calling finalize.", errorPrefix); + return false; + } + + if (stateVariableHandler.getNumberOfVariables() == 0) + { + log()->error("{} The variable handler is empty.", errorPrefix); + return false; + } + + m_stateVariableHandler = stateVariableHandler; + + if (!checkStateVariableHandler()) + { + log()->error("{} The variable handler is not valid.", errorPrefix); + return false; + } + + m_size = m_covariances.size(); + + m_updatedVariable.resize(m_size); + m_updatedVariable.setZero(); + + m_motorTorque.resize(m_size); + m_motorTorque.setZero(); + + return true; +} + +bool RDE::MotorCurrentMeasurementDynamics::setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) +{ + return true; +} + +bool RDE::MotorCurrentMeasurementDynamics::checkStateVariableHandler() +{ + constexpr auto errorPrefix = "[MotorCurrentMeasurementDynamics::checkStateVariableHandler]"; + + // Check if the variable handler contains the variables used by this dynamics + if (!m_stateVariableHandler.getVariable("tau_m").isValid()) + { + log()->error("{} The variable handler does not contain the expected measurement name {}.", errorPrefix, "tau_m"); + return false; + } + + return true; +} + +bool RDE::MotorCurrentMeasurementDynamics::update() +{ + m_updatedVariable = m_motorTorque.array() / (m_kTau.array() * m_gearRatio.array()); + + return true; +} + +void RDE::MotorCurrentMeasurementDynamics::setState(const Eigen::Ref ukfState) +{ + m_motorTorque = ukfState.segment(m_stateVariableHandler.getVariable("tau_m").offset, + m_stateVariableHandler.getVariable("tau_m").size); +} + +void RDE::MotorCurrentMeasurementDynamics::setInput(const UKFInput& ukfInput) +{ + return; +} diff --git a/src/Estimators/src/RobotDynamicsEstimator.cpp b/src/Estimators/src/RobotDynamicsEstimator.cpp new file mode 100644 index 0000000000..da35ce6bfc --- /dev/null +++ b/src/Estimators/src/RobotDynamicsEstimator.cpp @@ -0,0 +1,513 @@ +/** + * @file RobotDynamicsEstimator.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion; + +struct RobotDynamicsEstimator::Impl +{ + RobotDynamicsEstimatorOutput estimatorOutput; /**< Output of the estimator. */ + + UKFInput ukfInput; /**< Object to set the provider. */ + std::shared_ptr inputProvider; /**< Shared pointer used by all the dynamics. It needs to be updated here. */ + + std::map ukfMeasurementFromSensors; /**< This map containes the measurement coming from the sensors + and needed for the correction phase of the estimation. */ + + bfl::Gaussian predictedState; /**< Predicted state computed by the `predict` method. */ + bfl::Gaussian correctedState; /**< Corrected state computed by the `correct` method. */ + + double dT; /**< Sampling time. */ + double alpha, beta, kappa; /**< Parameters of the unscented kalman filter. */ + + std::unique_ptr stateModel; /**< UKF state model. */ + std::unique_ptr measurementModel; /**< UKF measurement model. */ + + std::unique_ptr ukfPrediction; + std::unique_ptr ukfCorrection; + + System::VariablesHandler stateHandler; /**< Handler of the ukf state. */ + System::VariablesHandler measurementHandler; /**< Handler of the ukf measurement. */ + + Eigen::MatrixXd initialStateCovariance; + + bool isInitialized{false}; + bool isFinalized{false}; + + bool isValid{false}; + + bool isInitialStateSet{false}; +}; + +RobotDynamicsEstimator::RobotDynamicsEstimator() +{ + m_pimpl = std::make_unique(); +} + +RobotDynamicsEstimator::~RobotDynamicsEstimator() = default; + +bool RobotDynamicsEstimator::initialize(std::weak_ptr handler) +{ + constexpr auto logPrefix = "[RobotDynamicsEstimator::initialize]"; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", logPrefix); + return false; + } + + auto groupGeneral = ptr->getGroup("GENERAL").lock(); + if (groupGeneral == nullptr) + { + log()->error("{} Error while retrieving the group GENERAL.", logPrefix); + return false; + } + + if (!groupGeneral->getParameter("sampling_time", m_pimpl->dT)) + { + log()->error("{} Error while retrieving the sampling time variable.", logPrefix); + return false; + } + + auto groupUkf = ptr->getGroup("UKF").lock(); + if (groupUkf == nullptr) + { + log()->error("{} Error while retrieving the group UKF.", logPrefix); + return false; + } + + if (!groupUkf->getParameter("alpha", m_pimpl->alpha)) + { + log()->error("{} Error while retrieving the alpha variable.", logPrefix); + return false; + } + + if (!groupUkf->getParameter("beta", m_pimpl->beta)) + { + log()->error("{} Error while retrieving the beta variable.", logPrefix); + return false; + } + + if (!groupUkf->getParameter("kappa", m_pimpl->kappa)) + { + log()->error("{} Error while retrieving the kappa variable.", logPrefix); + return false; + } + + m_pimpl->inputProvider = std::make_shared(); + + m_pimpl->isInitialized = true; + + return true; +} + +bool RobotDynamicsEstimator::finalize(const System::VariablesHandler& stateVariableHandler, + const System::VariablesHandler& measurementVariableHandler, + std::shared_ptr kinDynFullModel) +{ + // Resize variables for the estimation + m_pimpl->predictedState.resize(stateVariableHandler.getNumberOfVariables()); + m_pimpl->predictedState.mean().setZero(); + m_pimpl->predictedState.covariance() = m_pimpl->initialStateCovariance; + + m_pimpl->correctedState.resize(stateVariableHandler.getNumberOfVariables()); + m_pimpl->correctedState.mean().setZero(); + m_pimpl->correctedState.covariance() = m_pimpl->initialStateCovariance; + + // Set the provider initial state + m_pimpl->ukfInput.robotBasePose.setIdentity(); + m_pimpl->ukfInput.robotBaseVelocity.setZero(); + m_pimpl->ukfInput.robotBaseAcceleration.setZero(); + m_pimpl->ukfInput.robotJointPositions.resize(kinDynFullModel->model().getNrOfDOFs()); + m_pimpl->inputProvider->setInput(m_pimpl->ukfInput); + + m_pimpl->isFinalized = true; + + return m_pimpl->isFinalized; +} + +std::unique_ptr RobotDynamicsEstimator::build(std::weak_ptr handler, + std::shared_ptr kinDynFullModel, + const std::vector& subModelList, + const std::vector>& kinDynWrapperList) +{ + constexpr auto logPrefix = "[RobotDynamicsEstimator::build]"; + + auto ptr = handler.lock(); + + if (ptr == nullptr) + { + log()->error("{} Invalid parameter handler.", logPrefix); + return nullptr; + } + + if (!kinDynFullModel->setFrameVelocityRepresentation(iDynTree::BODY_FIXED_REPRESENTATION)) + { + log()->error("{} Cannot set the frame velocity representation on the `iDynTree::KinDynComputation` " + "object describing the full robot model.", logPrefix); + return nullptr; + } + + std::unique_ptr estimator = std::make_unique(); + + if (!estimator->initialize(handler)) + { + log()->error("{} Error initializing the RobotDynamicsEstimator.", logPrefix); + return nullptr; + } + + auto groupUKF = ptr->getGroup("UKF").lock(); + if (groupUKF == nullptr) + { + log()->error("{} Error while retrieving the group UKF.", logPrefix); + return nullptr; + } + + auto groupUKFStateTmp = groupUKF->getGroup("UKF_STATE").lock(); + if (groupUKFStateTmp == nullptr) + { + log()->error("{} Error while retrieving the group UKF_STATE.", logPrefix); + return nullptr; + } + + auto groupUKFState = groupUKFStateTmp->clone(); + groupUKFState->setParameter("sampling_time", estimator->m_pimpl->dT); + + // Step 1 + // Create the state model for the ukf + estimator->m_pimpl->stateModel = UkfState::build(groupUKFState, + kinDynFullModel, + subModelList, + kinDynWrapperList); + if (estimator->m_pimpl->stateModel == nullptr) + { + log()->error("{} Error while creating the ukf state model.", logPrefix); + return nullptr; + } + + // Save a copy of the state variable handler + estimator->m_pimpl->stateHandler = estimator->m_pimpl->stateModel->getStateVariableHandler(); + + // Set the input provider + estimator->m_pimpl->stateModel->setUkfInputProvider(estimator->m_pimpl->inputProvider); + + // Get initial state covariance + estimator->m_pimpl->initialStateCovariance = estimator->m_pimpl->stateModel->getInitialStateCovarianceMatrix(); + + // Step 2 + // Initialize the unscented Kalman filter prediction step and pass the ownership of the state model + std::unique_ptr stateModelTemp = std::move(estimator->m_pimpl->stateModel); + estimator->m_pimpl->ukfPrediction = std::make_unique(std::move(stateModelTemp), + estimator->m_pimpl->alpha, + estimator->m_pimpl->beta, + estimator->m_pimpl->kappa); + + // Step 3 + // Create the measurement model for the ukf + auto groupUKFMeasTmp = groupUKF->getGroup("UKF_MEASUREMENT").lock(); + if (groupUKFMeasTmp == nullptr) + { + log()->error("{} Error while retrieving the group UKF_MEASUREMENT.", logPrefix); + return nullptr; + } + + auto groupUKFMeas = groupUKFMeasTmp->clone(); + groupUKFMeas->setParameter("sampling_time", estimator->m_pimpl->dT); + + // Create the measurement model for the ukf + estimator->m_pimpl->measurementModel = UkfMeasurement::build(groupUKFMeas, + estimator->m_pimpl->stateHandler, + kinDynFullModel, + subModelList, + kinDynWrapperList); + if (estimator->m_pimpl->measurementModel == nullptr) + { + log()->error("{} Error while creating the ukf measurement model.", logPrefix); + return nullptr; + } + + // Save a copy of the measurement variable handler + estimator->m_pimpl->measurementHandler = estimator->m_pimpl->measurementModel->getMeasurementVariableHandler(); + + // Set the input provider + estimator->m_pimpl->measurementModel->setUkfInputProvider(estimator->m_pimpl->inputProvider); + + // Step 4 + // Initialize the unscented Kalman filter correction step and pass the ownership of the measurement model. + std::unique_ptr measurementModelTemp = std::move(estimator->m_pimpl->measurementModel); + estimator->m_pimpl->ukfCorrection = std::make_unique(std::move(measurementModelTemp), + estimator->m_pimpl->alpha, + estimator->m_pimpl->beta, + estimator->m_pimpl->kappa); + + // Finalize the estimator + estimator->finalize(estimator->m_pimpl->stateHandler, estimator->m_pimpl->measurementHandler, kinDynFullModel); + + return estimator; +} + +bool RobotDynamicsEstimator::setInitialState(const RobotDynamicsEstimatorOutput& initialState) +{ + constexpr auto logPrefix = "[RobotDynamicsEstimator::setInitialState]"; + + System::VariablesHandler::VariableDescription variable; + + if (m_pimpl->stateHandler.getVariable("ds", variable)) + { + if (initialState.ds.size() != variable.size) + { + log()->error("{} Wrong size of variable `ds`. Found {}, expected {}.", logPrefix, initialState.ds.size(), variable.size); + return false; + } + m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = initialState.ds; + } + + if (m_pimpl->stateHandler.getVariable("tau_m", variable)) + { + if (initialState.tau_m.size() != variable.size) + { + log()->error("{} Wrong size of variable `tau_m`. Found {}, expected {}.", logPrefix, initialState.tau_m.size(), variable.size); + return false; + } + m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = initialState.tau_m; + } + + if (m_pimpl->stateHandler.getVariable("tau_F", variable)) + { + if (initialState.tau_F.size() != variable.size) + { + log()->error("{} Wrong size of variable `tau_F`. Found {}, expected {}.", logPrefix, initialState.tau_F.size(), variable.size); + return false; + } + m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = initialState.tau_F; + } + + for (auto const& [key, val] : initialState.ftWrenches) + { + if (m_pimpl->stateHandler.getVariable(key, variable)) + { + if (val.size() != variable.size) + { + log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, val.size(), variable.size); + return false; + } + m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; + } + } + + for (auto const& [key, val] : initialState.ftWrenchesBiases) + { + if (m_pimpl->stateHandler.getVariable(key, variable)) + { + if (val.size() != variable.size) + { + log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, val.size(), variable.size); + return false; + } + m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; + } + } + + for (auto const& [key, val] : initialState.gyroscopeBiases) + { + if (m_pimpl->stateHandler.getVariable(key, variable)) + { + if (val.size() != variable.size) + { + log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, val.size(), variable.size); + return false; + } + m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; + } + } + + for (auto const& [key, val] : initialState.accelerometerBiases) + { + if (m_pimpl->stateHandler.getVariable(key, variable)) + { + if (val.size() != variable.size) + { + log()->error("{} Wrong size of variable `{}`. Found {}, expected {}.", logPrefix, val, val.size(), variable.size); + return false; + } + m_pimpl->correctedState.mean().segment(variable.offset, variable.size) = val; + } + } + + m_pimpl->estimatorOutput = initialState; + + m_pimpl->isInitialStateSet = true; + + return m_pimpl->isInitialStateSet; +} + +bool RobotDynamicsEstimator::isOutputValid() const +{ + return m_pimpl->isValid; +} + +bool RobotDynamicsEstimator::advance() +{ + constexpr auto logPrefix = "[RobotDynamicsEstimator::advance]"; + + // when advance is called the previous solution is no more valid + m_pimpl->isValid = false; + + if (!m_pimpl->isInitialized) + { + log()->error("{} Please call initialize() before advance().", logPrefix); + return false; + } + + if (!m_pimpl->isFinalized) + { + log()->error("{} Please call finalize() before advance().", logPrefix); + return false; + } + + if (!m_pimpl->isInitialStateSet) + { + log()->error("{} Please set the initial state before advance().", logPrefix); + return false; + } + + // Step 1 --> Predict + m_pimpl->ukfPrediction->predict(m_pimpl->correctedState, m_pimpl->predictedState); + + // Step 2 --> Set measurement + if (!m_pimpl->ukfCorrection->freeze_measurements(m_pimpl->ukfMeasurementFromSensors)) + { + log()->error("{} Cannot set the measurement to the UkfCorrection object.", logPrefix); + return false; + } + + // Step 3 --> Correct + m_pimpl->ukfCorrection->correct(m_pimpl->predictedState, m_pimpl->correctedState); + + m_pimpl->isValid = true; + + return true; +} + +bool RobotDynamicsEstimator::setInput(const RobotDynamicsEstimatorInput & input) +{ + constexpr auto logPrefix = "[RobotDynamicsEstimator::setInput]"; + + // Set the input provider state + m_pimpl->ukfInput.robotBasePose = input.basePose; + m_pimpl->ukfInput.robotBaseVelocity = input.baseVelocity; + m_pimpl->ukfInput.robotBaseAcceleration = input.baseAcceleration; + m_pimpl->ukfInput.robotJointPositions = input.jointPositions; + + if (!m_pimpl->inputProvider->setInput(m_pimpl->ukfInput)) + { + log()->error("{} Cannot set the input of the input provider.", logPrefix); + return false; + } + + // Set the `std::map` used as measurement object + // for the freeze method of the UkfCorrection + m_pimpl->ukfMeasurementFromSensors["ds"] = input.jointVelocities; + m_pimpl->ukfMeasurementFromSensors["i_m"] = input.motorCurrents; + for (auto & [key, value] : input.ftWrenches) + { + m_pimpl->ukfMeasurementFromSensors[key] = value; + } + for (auto & [key, value] : input.linearAccelerations) + { + m_pimpl->ukfMeasurementFromSensors[key] = value; + } + for (auto & [key, value] : input.angularVelocities) + { + m_pimpl->ukfMeasurementFromSensors[key] = value; + } + + return true; +} + +const RobotDynamicsEstimatorOutput& RobotDynamicsEstimator::getOutput() const +{ + constexpr auto logPrefix = "[RobotDynamicsEstimator::getOutput]"; + + if (m_pimpl->isValid) + { + m_pimpl->estimatorOutput.ds = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable("ds").offset, + m_pimpl->stateHandler.getVariable("ds").size); + + m_pimpl->estimatorOutput.tau_m = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable("tau_m").offset, + m_pimpl->stateHandler.getVariable("tau_m").size); + + m_pimpl->estimatorOutput.tau_F = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable("tau_F").offset, + m_pimpl->stateHandler.getVariable("tau_F").size); + + for (auto & [key, value] : m_pimpl->estimatorOutput.ftWrenches) + { + if (m_pimpl->stateHandler.getVariable(key).size > 0) + { + m_pimpl->estimatorOutput.ftWrenches[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, + m_pimpl->stateHandler.getVariable(key).size); + } + else + { + log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); + } + } + + for (auto & [key, value] : m_pimpl->estimatorOutput.ftWrenchesBiases) + { + if (m_pimpl->stateHandler.getVariable(key).size > 0) + { + m_pimpl->estimatorOutput.ftWrenchesBiases[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, + m_pimpl->stateHandler.getVariable(key).size); + } + else + { + log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); + } + } + + for (auto & [key, value] : m_pimpl->estimatorOutput.accelerometerBiases) + { + if (m_pimpl->stateHandler.getVariable(key).size > 0) + { + m_pimpl->estimatorOutput.accelerometerBiases[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, + m_pimpl->stateHandler.getVariable(key).size); + } + else + { + log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); + } + } + + for (auto & [key, value] : m_pimpl->estimatorOutput.gyroscopeBiases) + { + if (m_pimpl->stateHandler.getVariable(key).size > 0) + { + m_pimpl->estimatorOutput.gyroscopeBiases[key] = m_pimpl->correctedState.mean().segment(m_pimpl->stateHandler.getVariable(key).offset, + m_pimpl->stateHandler.getVariable(key).size); + } + else + { + log()->debug("{} Variable {} not found in the state vector.", logPrefix, key); + } + } + } + + return m_pimpl->estimatorOutput; +} diff --git a/src/Estimators/src/SubModel.cpp b/src/Estimators/src/SubModel.cpp index 46147e5490..1985e91404 100644 --- a/src/Estimators/src/SubModel.cpp +++ b/src/Estimators/src/SubModel.cpp @@ -257,23 +257,23 @@ RDE::SubModelCreator::populateSubModel(iDynTree::Model& idynSubModel, return subModel; } -bool RDE::SubModelCreator::createSubModels(const std::vector& ftSensorList, +bool RDE::SubModelCreator::createSubModels(const std::vector& ftSensorList, const std::vector& accList, const std::vector& gyroList, const std::vector& externalContacts) { constexpr auto logPrefix = "[BipedalLocomotion::RobotDynamicsEstimator::SubModelCreator::" - "getSubModels]"; + "createSubModels]"; // Split model in submodels - std::vector ftNameList; + std::vector ftList; for (auto idx = 0; idx < ftSensorList.size(); idx++) { - ftNameList.push_back(ftSensorList[idx].frame); + ftList.push_back(ftSensorList[idx].associatedJoint); } std::vector idynSubModels; - if (!this->splitModel(ftNameList, idynSubModels)) + if (!this->splitModel(ftList, idynSubModels)) { blf::log()->error("{} Unable to split the model in submodels.", logPrefix); return false; @@ -318,8 +318,8 @@ bool RDE::SubModelCreator::createSubModels( } auto populateSensorParameters = [&ptr, logPrefix](const std::string& groupName, - std::vector& names, - std::vector& frames) -> bool { + std::vector& names, + std::vector& frames) -> bool { auto group = ptr->getGroup(groupName).lock(); if (group == nullptr) { @@ -362,15 +362,24 @@ bool RDE::SubModelCreator::createSubModels( logPrefix); } - std::vector ftNames, ftFrames; + std::vector ftNames, ftFrames, ftAssociatedJoints; bool ok = populateSensorParameters("FT", ftNames, ftFrames); - std::vector ftList; + auto ftGroup = ptr->getGroup("FT").lock(); + if (ftGroup == nullptr) + { + blf::log()->error("{} Unable to get the group names 'FT'.", logPrefix); + return false; + } + ok = ok && ftGroup->getParameter("associated_joints", ftAssociatedJoints); + + std::vector ftList; for (auto idx = 0; idx < ftNames.size(); idx++) { - RDE::Sensor ft; + RDE::FT ft; ft.name = ftNames[idx]; ft.frame = ftFrames[idx]; + ft.associatedJoint = ftAssociatedJoints[idx]; ftList.push_back(std::move(ft)); } @@ -399,7 +408,7 @@ bool RDE::SubModelCreator::createSubModels( } ok = ok - && RDE::SubModelCreator::createSubModels(ftList, accList, gyroList, extContactFrames); + && RDE::SubModelCreator::createSubModels(ftList, accList, gyroList, extContactFrames); return ok; } @@ -494,16 +503,100 @@ const RDE::FT& RDE::SubModel::getFTSensor(const int index) const return m_ftList.at(index); } +const RDE::FT& RDE::SubModel::getFTSensor(const std::string name) const +{ + for (int ftIndex = 0; ftIndex < m_ftList.size(); ftIndex++) + { + if (m_ftList[ftIndex].name == name) + { + return m_ftList[ftIndex]; + } + } + + static const RDE::FT ft; + + return ft; +} + +bool RDE::SubModel::hasFTSensor(const std::string name) const +{ + for (int ftIndex = 0; ftIndex < m_ftList.size(); ftIndex++) + { + if (m_ftList[ftIndex].name == name) + { + return true; + } + } + + return false; +} + const RDE::Sensor& RDE::SubModel::getAccelerometer(const int index) const { return m_accelerometerList.at(index); } +const RDE::Sensor& RDE::SubModel::getAccelerometer(const std::string name) const +{ + for (int accIndex = 0; accIndex < m_accelerometerList.size(); accIndex++) + { + if (m_accelerometerList[accIndex].name == name) + { + return m_accelerometerList[accIndex]; + } + } + + static const RDE::Sensor acc; + + return acc; +} + +bool RDE::SubModel::hasAccelerometer(const std::string name) const +{ + for (int accIndex = 0; accIndex < m_accelerometerList.size(); accIndex++) + { + if (m_accelerometerList[accIndex].name == name) + { + return true; + } + } + + return false; +} + const RDE::Sensor& RDE::SubModel::getGyroscope(const int index) const { return m_gyroscopeList.at(index); } +const RDE::Sensor& RDE::SubModel::getGyroscope(const std::string name) const +{ + for (int gyroIndex = 0; gyroIndex < m_gyroscopeList.size(); gyroIndex++) + { + if (m_gyroscopeList[gyroIndex].name == name) + { + return m_gyroscopeList[gyroIndex]; + } + } + + static const RDE::Sensor gyro; + + return gyro; +} + +bool RDE::SubModel::hasGyroscope(const std::string name) const +{ + for (int gyroIndex = 0; gyroIndex < m_gyroscopeList.size(); gyroIndex++) + { + if (m_gyroscopeList[gyroIndex].name == name) + { + return true; + } + } + + return false; +} + const std::string& RDE::SubModel::getExternalContact(const int index) const { return m_externalContactList.at(index); diff --git a/src/Estimators/src/SubModelDynamics.cpp b/src/Estimators/src/SubModelDynamics.cpp new file mode 100644 index 0000000000..eb6cb154c1 --- /dev/null +++ b/src/Estimators/src/SubModelDynamics.cpp @@ -0,0 +1,174 @@ +/** + * @file SubModelDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +RDE::SubModelDynamics::SubModelDynamics() = default; + +RDE::SubModelDynamics::~SubModelDynamics() = default; + +/** + * Set the SubModelKinDynWrapper object. + * Parameter kinDynWrapper pointer to a SubModelKinDynWrapper object. + * Return true in case of success, false otherwise. + */ +bool RDE::SubModelDynamics::setKinDynWrapper(std::shared_ptr kinDynWrapper) +{ + constexpr auto errorPrefix = "[SubModelDynamics::setKinDynWrapper]"; + + this->kinDynWrapper = kinDynWrapper; + + if (kinDynWrapper == nullptr) + { + log()->error("{} Error while setting the `SubModelKinDynWrapper` object.", errorPrefix); + return false; + } + + return true; +} + +bool RDE::SubModelDynamics::setSubModel(const RDE::SubModel& subModel) +{ + constexpr auto errorPrefix = "[SubModelDynamics::setSubModel]"; + + this->subModel = subModel; + + if (!subModel.isValid()) + { + log()->error("{} Set a valid `SubModel` object.", errorPrefix); + return false; + } + + return true; +} + + +bool RDE::SubModelDynamics::initialize() +{ + constexpr auto errorPrefix = "[SubModelDynamics::initialize]"; + + // Inialize map of the ft sensors + if (!subModel.isValid()) + { + log()->error("{} Set a valid `SubModel` object before calling initialize.", errorPrefix); + return false; + } + + for (int idx = 0; idx < subModel.getNrOfFTSensor(); idx++) + { + FTMap[subModel.getFTSensor(idx).name].setZero(); + } + + // Inialize map of the external contacts + for (int idx = 0; idx < subModel.getNrOfExternalContact(); idx++) + { + extContactMap[subModel.getExternalContact(idx)].setZero(); + } + + // Resize and initialize variables + baseAcceleration.setZero(); + + motorTorque.resize(subModel.getModel().getNrOfDOFs()); + motorTorque.setZero(); + + frictionTorque.resize(subModel.getModel().getNrOfDOFs()); + frictionTorque.setZero(); + + jointVelocity.resize(subModel.getModel().getNrOfDOFs()); + jointVelocity.setZero(); + + totalTorqueFromContacts.resize(subModel.getModel().getNrOfDOFs()); + totalTorqueFromContacts.setZero(); + + torqueFromContact.resize(subModel.getModel().getNrOfDOFs()); + torqueFromContact.setZero(); + + wrench.setZero(); + + return true; +} + +void RDE::SubModelDynamics::setState(const Eigen::Ref ukfState, + const Eigen::Ref jointVelocityFullModel, + const Eigen::Ref motorTorqueFullModel, + const Eigen::Ref frictionTorqueFullModel, + const System::VariablesHandler& variableHandler) +{ + for (int idx = 0; idx < subModel.getModel().getNrOfDOFs(); idx++) + { + jointVelocity(idx) = jointVelocityFullModel(subModel.getJointMapping().at(idx)); + motorTorque(idx) = motorTorqueFullModel(subModel.getJointMapping().at(idx)); + frictionTorque(idx) = frictionTorqueFullModel(subModel.getJointMapping().at(idx)); + } + + for (int idx = 0; idx < subModel.getNrOfFTSensor(); idx++) + { + FTMap[subModel.getFTSensor(idx).name] = ukfState.segment(variableHandler.getVariable(subModel.getFTSensor(idx).name).offset, + variableHandler.getVariable(subModel.getFTSensor(idx).name).size); + } + + for (int idx = 0; idx < subModel.getNrOfExternalContact(); idx++) + { + extContactMap[subModel.getExternalContact(idx)] = ukfState.segment(variableHandler.getVariable(subModel.getExternalContact(idx)).offset, + variableHandler.getVariable(subModel.getExternalContact(idx)).size); + } +} + +bool RDE::SubModelDynamics::update(manif::SE3d::Tangent& fullModelBaseAcceleration, + Eigen::Ref fullModelJointAcceleration, + Eigen::Ref updatedJointAcceleration) +{ + constexpr auto errorPrefix = "[SubModelDynamics::update]"; + + computeTotalTorqueFromContacts(); + + if (!kinDynWrapper->getBaseAcceleration(fullModelBaseAcceleration, fullModelJointAcceleration, baseAcceleration)) + { + log()->error("{} Cannot compute the sub-model base acceleration.", errorPrefix); + return false; + } + + if (!kinDynWrapper->forwardDynamics(motorTorque, + frictionTorque, + totalTorqueFromContacts, + baseAcceleration.coeffs(), + updatedJointAcceleration)) + { + log()->error("{} Cannot compute the inverse dynamics.", errorPrefix); + return false; + } + + return true; +} + +void RDE::SubModelDynamics::computeTotalTorqueFromContacts() +{ + totalTorqueFromContacts.setZero(); + + // Contribution of FT measurements + for (int idx = 0; idx < subModel.getNrOfFTSensor(); idx++) + { + wrench = (int)subModel.getFTSensor(idx).forceDirection * FTMap[subModel.getFTSensor(idx).name].array(); + + torqueFromContact = kinDynWrapper->getFTJacobian(subModel.getFTSensor(idx).name).block(0, 6, 6, subModel.getModel().getNrOfDOFs()).transpose() * wrench; + + totalTorqueFromContacts = totalTorqueFromContacts.array() + torqueFromContact.array(); + } + + // Contribution of unknown external contacts + for (int idx = 0; idx < subModel.getNrOfExternalContact(); idx++) + { + torqueFromContact = kinDynWrapper->getExtContactJacobian(subModel.getExternalContact(idx)).block(0, 6, 6, subModel.getModel().getNrOfDOFs()).transpose() * extContactMap[subModel.getExternalContact(idx)]; + + totalTorqueFromContacts = totalTorqueFromContacts.array() + torqueFromContact.array(); + } +} diff --git a/src/Estimators/src/SubModelKinDynWrapper.cpp b/src/Estimators/src/SubModelKinDynWrapper.cpp index 53560dbafb..defc59c2d4 100644 --- a/src/Estimators/src/SubModelKinDynWrapper.cpp +++ b/src/Estimators/src/SubModelKinDynWrapper.cpp @@ -69,6 +69,8 @@ bool RDE::SubModelKinDynWrapper::initialize(const RDE::SubModel& subModel) m_FTBaseAcc.resize(m_numOfJoints); m_totalTorques.resize(m_numOfJoints); + m_subModelBaseAcceleration.setZero(); + // Initialize attributes with correct sizes m_massMatrix.resize(6 + m_subModel.getModel().getNrOfDOFs(), 6 + m_subModel.getModel().getNrOfDOFs()); @@ -97,6 +99,10 @@ bool RDE::SubModelKinDynWrapper::initialize(const RDE::SubModel& subModel) manif::SO3d rotMatrix; rotMatrix.coeffs().setZero(); m_accRworldList[m_subModel.getAccelerometerList()[idx].name] = std::move(rotMatrix); + + manif::SE3d::Tangent vel; + vel.coeffs().setZero(); + m_accVelList[m_subModel.getAccelerometerList()[idx].name] = std::move(vel); } for (int idx = 0; idx < m_subModel.getGyroscopeList().size(); idx++) @@ -116,7 +122,9 @@ bool RDE::SubModelKinDynWrapper::initialize(const RDE::SubModel& subModel) return true; } -bool RDE::SubModelKinDynWrapper::updateInternalKinDynState() +bool RDE::SubModelKinDynWrapper::updateState(manif::SE3d::Tangent& robotBaseAcceleration, + Eigen::Ref robotJointAcceleration, + bool updateRobotDynamicsOnly) { constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::" "updateKinDynState]"; @@ -148,10 +156,23 @@ bool RDE::SubModelKinDynWrapper::updateInternalKinDynState() return false; } - return updateDynamicsVariableState(); + if (!m_kinDynFullModel->getFrameAcc(m_baseFrame, + iDynTree::make_span(robotBaseAcceleration.data(), + manif::SE3d::Tangent::DoF), + robotJointAcceleration, + iDynTree::make_span(m_subModelBaseAcceleration.data(), + manif::SE3d::Tangent::DoF))) + { + blf::log()->error("{} Unable to get the acceleration of the frame {}.", + logPrefix, + m_baseFrame); + return false; + } + + return updateDynamicsVariableState(updateRobotDynamicsOnly); } -bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState() +bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState(bool updateRobotDynamicsOnly) { constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::" "updateDynamicsVariableState]"; @@ -185,45 +206,52 @@ bool RDE::SubModelKinDynWrapper::updateDynamicsVariableState() } } - // Update accelerometer jacobians, dJnu, rotMatrix - for (int idx = 0; idx < m_subModel.getAccelerometerList().size(); idx++) + if (!updateRobotDynamicsOnly) { - // Update jacobian - if (!m_kinDyn - .getFrameFreeFloatingJacobian(m_subModel.getAccelerometerList()[idx].frame, - m_jacAccList[m_subModel.getAccelerometerList()[idx] - .name])) + // Update accelerometer jacobians, dJnu, rotMatrix + for (int idx = 0; idx < m_subModel.getAccelerometerList().size(); idx++) { - blf::log()->error("{} Unable to get the compute the free floating jacobian of the " - "frame {}.", - logPrefix, - m_subModel.getAccelerometerList()[idx].frame); - return false; + // Update jacobian + if (!m_kinDyn + .getFrameFreeFloatingJacobian(m_subModel.getAccelerometerList()[idx].frame, + m_jacAccList[m_subModel.getAccelerometerList()[idx] + .name])) + { + blf::log()->error("{} Unable to get the compute the free floating jacobian of the " + "frame {}.", + logPrefix, + m_subModel.getAccelerometerList()[idx].frame); + return false; + } + + // Update dJnu + m_dJnuList[m_subModel.getAccelerometerList()[idx].name] = iDynTree::toEigen( + m_kinDyn.getFrameBiasAcc(m_subModel.getAccelerometerList()[idx].frame)); + + // Update rotMatrix + m_accRworldList[m_subModel.getAccelerometerList()[idx].name] = blf::Conversions::toManifRot( + m_kinDyn.getWorldTransform(m_subModel.getAccelerometerList()[idx].frame) + .getRotation() + .inverse()); + + // Update accelerometer velocity + m_accVelList[m_subModel.getAccelerometerList()[idx].name] = iDynTree::toEigen( + m_kinDyn.getFrameVel(m_subModel.getAccelerometerList()[idx].frame)); } - // Update dJnu - m_dJnuList[m_subModel.getAccelerometerList()[idx].name] = iDynTree::toEigen( - m_kinDyn.getFrameBiasAcc(m_subModel.getAccelerometerList()[idx].frame)); - - // Update rotMatrix - m_accRworldList[m_subModel.getAccelerometerList()[idx].name] = blf::Conversions::toManifRot( - m_kinDyn.getWorldTransform(m_subModel.getAccelerometerList()[idx].frame) - .getRotation() - .inverse()); - } - - // Update gyroscope jacobians - for (int idx = 0; idx < m_subModel.getGyroscopeList().size(); idx++) - { - if (!m_kinDyn.getFrameFreeFloatingJacobian(m_subModel.getGyroscopeList()[idx].frame, - m_jacGyroList[m_subModel.getGyroscopeList()[idx] - .name])) + // Update gyroscope jacobians + for (int idx = 0; idx < m_subModel.getGyroscopeList().size(); idx++) { - blf::log()->error("{} Unable to get the compute the free floating jacobian of the " - "frame {}.", - logPrefix, - m_subModel.getGyroscopeList()[idx].frame); - return false; + if (!m_kinDyn.getFrameFreeFloatingJacobian(m_subModel.getGyroscopeList()[idx].frame, + m_jacGyroList[m_subModel.getGyroscopeList()[idx] + .name])) + { + blf::log()->error("{} Unable to get the compute the free floating jacobian of the " + "frame {}.", + logPrefix, + m_subModel.getGyroscopeList()[idx].frame); + return false; + } } } @@ -277,35 +305,16 @@ bool RDE::SubModelKinDynWrapper::forwardDynamics(Eigen::Ref mot m_FTBaseAcc = m_FTranspose * baseAcceleration; m_totalTorques - = motorTorqueAfterGearbox - frictionTorques - m_genBiasJointTorques + tauExt + m_FTBaseAcc; + = motorTorqueAfterGearbox - frictionTorques - m_genBiasJointTorques + tauExt - m_FTBaseAcc; jointAcceleration = m_pseudoInverseH * m_totalTorques; return true; } -bool RDE::SubModelKinDynWrapper::getBaseAcceleration( - manif::SE3d::Tangent& robotBaseAcceleration, - Eigen::Ref robotJointAcceleration, - manif::SE3d::Tangent& subModelBaseAcceleration) +const manif::SE3d::Tangent& RDE::SubModelKinDynWrapper::getBaseAcceleration() { - constexpr auto logPrefix = "[BipedalLocomotion::Estimators::SubModelKinDynWrapper::" - "getBaseAcceleration]"; - - if (!m_kinDynFullModel->getFrameAcc(m_baseFrame, - iDynTree::make_span(robotBaseAcceleration.data(), - manif::SE3d::Tangent::DoF), - robotJointAcceleration, - iDynTree::make_span(subModelBaseAcceleration.data(), - manif::SE3d::Tangent::DoF))) - { - blf::log()->error("{} Unable to get the acceleration of the frame {}.", - logPrefix, - m_baseFrame); - return false; - } - - return true; + return m_subModelBaseAcceleration; } const manif::SE3d::Tangent& RDE::SubModelKinDynWrapper::getBaseVelocity() @@ -364,3 +373,9 @@ RDE::SubModelKinDynWrapper::getAccelerometerRotation(const std::string& accName) { return m_accRworldList.at(accName); } + +const manif::SE3d::Tangent& +RDE::SubModelKinDynWrapper::getAccelerometerVelocity(const std::string& accName) +{ + return m_accVelList[accName]; +} diff --git a/src/Estimators/src/UkfMeasurement.cpp b/src/Estimators/src/UkfMeasurement.cpp new file mode 100644 index 0000000000..7a120193d5 --- /dev/null +++ b/src/Estimators/src/UkfMeasurement.cpp @@ -0,0 +1,583 @@ +/** + * @file UkfMeasurement.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion; + +using namespace std::chrono; + +struct RDE::UkfMeasurement::Impl +{ + bool isInitialized{false}; + bool isFinalized{false}; + + bfl::VectorDescription measurementDescription; + bfl::VectorDescription inputDescription; + + Eigen::Vector3d gravity{0, 0, -Math::StandardAccelerationOfGravitation}; /**< Gravity vector. */ + + Eigen::MatrixXd covarianceR; /**< Covariance matrix. */ + int measurementSize{0}; /**< Length of the measurement vector. */ + double dT; /**< Sampling time */ + + std::vector>> dynamicsList; +// std::map> dynamicsList; /**< List of the dynamics composing the process model. */ + + System::VariablesHandler measurementVariableHandler; /**< Variable handler describing the measurement vector. */ + System::VariablesHandler stateVariableHandler; /**< Variable handler describing the state vector. */ + + std::shared_ptr kinDynFullModel; /**< KinDynComputation object for the full model. */ + std::vector subModelList; /**< List of SubModel object describing the sub-models composing the full model. */ + std::vector> kinDynWrapperList; /**< List of SubModelKinDynWrapper objects containing kinematic and dynamic information specific of each sub-model. */ + + std::shared_ptr ukfInputProvider; /**< Provider containing the updated robot state. */ + UKFInput ukfInput; /**< Struct containing the inputs for the ukf populated by the ukfInputProvider. */ + + Eigen::VectorXd jointVelocityState; /**< Joint velocity computed by the ukf. */ + Eigen::VectorXd jointAccelerationState; /**< Joint acceleration computed from forward dynamics which depends on the current ukf state. */ + + Eigen::VectorXd currentState; /**< State estimated in the previous step. */ + Eigen::VectorXd tempPredictedMeas; + Eigen::MatrixXd predictedMeasurement; /**< Vector containing the updated measurement. */ + + std::vector subModelJointVel; /**< List of sub-model joint velocities. */ + std::vector subModelJointAcc; /**< List of sub-model joint accelerations. */ + std::vector subModelJointMotorTorque; /**< List of sub-model joint velocities. */ + std::vector subModelFrictionTorque; /**< List of sub-model joint velocities. */ + std::map FTMap; /**< The map contains names of the ft sensors and values of the wrench */ + std::map extContactMap; /**< The map contains names of the ft sensors and values of the wrench */ + manif::SE3d::Tangent tempSubModelBaseAcc; /**< Acceleration of the base of the sub-model. */ + + // Support variables + std::vector totalTorqueFromContacts; /**< Joint torques due to known and unknown contacts on the sub-model. */ + std::vector torqueFromContact; /**< Joint torques due to a specific contact. */ + Math::Wrenchd wrench; /**< Joint torques due to a specific contact. */ + + + Eigen::VectorXd measurement; /**< Measurements coming from the sensors. */ + + std::map measurementMap; /**< Measurement map . */ + + int offsetMeasurement; /**< Offset used to fill the measurement vector. */ + + bool updateRobotDynamicsOnly{false}; + + void unpackState() + { + jointVelocityState = currentState.segment(stateVariableHandler.getVariable("ds").offset, + stateVariableHandler.getVariable("ds").size); + + for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) + { + // Take sub-model joint velocities, motor torques, friction torques, ft wrenches, ext contact wrenches + for (int jointIdx = 0; jointIdx < subModelList[subModelIdx].getModel().getNrOfDOFs(); jointIdx++) + { + subModelJointVel[subModelIdx](jointIdx) = + jointVelocityState(subModelList[subModelIdx].getJointMapping()[jointIdx]); + + subModelJointMotorTorque[subModelIdx](jointIdx) = + currentState[stateVariableHandler.getVariable("tau_m").offset + + subModelList[subModelIdx].getJointMapping()[jointIdx]]; + + subModelFrictionTorque[subModelIdx](jointIdx) = + currentState[stateVariableHandler.getVariable("tau_F").offset + + subModelList[subModelIdx].getJointMapping()[jointIdx]]; + } + + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfFTSensor(); idx++) + { + FTMap[subModelList[subModelIdx].getFTSensor(idx).name] = + currentState.segment(stateVariableHandler.getVariable(subModelList[subModelIdx].getFTSensor(idx).name).offset, + stateVariableHandler.getVariable(subModelList[subModelIdx].getFTSensor(idx).name).size); + } + + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfExternalContact(); idx++) + { + extContactMap[subModelList[subModelIdx].getExternalContact(idx)] = + currentState.segment(stateVariableHandler.getVariable(subModelList[subModelIdx].getExternalContact(idx)).offset, + stateVariableHandler.getVariable(subModelList[subModelIdx].getExternalContact(idx)).size); + } + } + } + + bool updateState() + { + // Update kindyn full model + kinDynFullModel->setRobotState(ukfInput.robotBasePose.transform(), + ukfInput.robotJointPositions, + iDynTree::make_span(ukfInput.robotBaseVelocity.data(), manif::SE3d::Tangent::DoF), + jointVelocityState, + gravity); + + // compute joint acceleration per each sub-model containing the accelerometer + for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) + { + // Update the kindyn wrapper object of the submodel + kinDynWrapperList[subModelIdx]->updateState(ukfInput.robotBaseAcceleration, + jointAccelerationState, + updateRobotDynamicsOnly); + + if (subModelList[subModelIdx].getModel().getNrOfDOFs() > 0) + { + totalTorqueFromContacts[subModelIdx].setZero(); + + // Contribution of FT measurements + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfFTSensor(); idx++) + { + wrench = (int)subModelList[subModelIdx].getFTSensor(idx).forceDirection * + FTMap[subModelList[subModelIdx].getFTSensor(idx).name].array(); + + torqueFromContact[subModelIdx] = kinDynWrapperList[subModelIdx]-> + getFTJacobian(subModelList[subModelIdx].getFTSensor(idx).name). + block(0, 6, 6, subModelList[subModelIdx].getModel().getNrOfDOFs()).transpose() * wrench; + + totalTorqueFromContacts[subModelIdx] = totalTorqueFromContacts[subModelIdx].array() + torqueFromContact[subModelIdx].array(); + } + + // Contribution of unknown external contacts + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfExternalContact(); idx++) + { + torqueFromContact[subModelIdx] = kinDynWrapperList[subModelIdx]->getExtContactJacobian(subModelList[subModelIdx].getExternalContact(idx)).block(0, 6, 6, subModelList[subModelIdx].getModel().getNrOfDOFs()).transpose() * extContactMap[subModelList[subModelIdx].getExternalContact(idx)]; + + totalTorqueFromContacts[subModelIdx] = totalTorqueFromContacts[subModelIdx].array() + torqueFromContact[subModelIdx].array(); + } + + tempSubModelBaseAcc = kinDynWrapperList[subModelIdx]->getBaseAcceleration(); + + if (!kinDynWrapperList[subModelIdx]->forwardDynamics(subModelJointMotorTorque[subModelIdx], + subModelFrictionTorque[subModelIdx], + totalTorqueFromContacts[subModelIdx], + tempSubModelBaseAcc.coeffs(), + subModelJointAcc[subModelIdx])) + { + BipedalLocomotion::log()->error("Cannot compute the inverse dynamics."); + return false; + } + + // Assign joint acceleration using the correct indeces + for (int jointIdx = 0; jointIdx < subModelList[subModelIdx].getJointMapping().size(); jointIdx++) + { + jointAccelerationState[subModelList[subModelIdx].getJointMapping()[jointIdx]] = subModelJointAcc[subModelIdx][jointIdx]; + } + } + } + + return true; + } +}; + +RDE::UkfMeasurement::UkfMeasurement() +{ + m_pimpl = std::make_unique(); +} + +RDE::UkfMeasurement::~UkfMeasurement() = default; + +bool RDE::UkfMeasurement::initialize(std::weak_ptr handler) +{ + constexpr auto logPrefix = "[UkfMeasurement::initialize]"; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + BipedalLocomotion::log()->error("{} The parameter handler is not valid.", logPrefix); + return false; + } + + if (!ptr->getParameter("sampling_time", m_pimpl->dT)) + { + BipedalLocomotion::log()->error("{} Unable to find the `sampling_time` variable", logPrefix); + return false; + } + + m_pimpl->isInitialized = true; + + return true; +} + +bool RDE::UkfMeasurement::finalize(const System::VariablesHandler& handler) +{ + constexpr auto logPrefix = "[UkfMeasurement::finalize]"; + + if (!m_pimpl->isInitialized) + { + BipedalLocomotion::log()->error("{} Please call initialize() before finalize().", logPrefix); + return false; + } + + m_pimpl->isFinalized = false; + + m_pimpl->measurementSize = 0; + + // finalize all the dynamics + for (int indexDyn1 = 0; indexDyn1 < m_pimpl->dynamicsList.size(); indexDyn1++) + { + if(!m_pimpl->dynamicsList[indexDyn1].second->finalize(handler)) + { + BipedalLocomotion::log()->error("{} Error while finalizing the dynamics named {}", logPrefix, m_pimpl->dynamicsList[indexDyn1].first); + return false; + } + + m_pimpl->measurementSize += m_pimpl->dynamicsList[indexDyn1].second->size(); + } + + // Set value of measurement covariance matrix + m_pimpl->covarianceR.resize(m_pimpl->measurementSize, m_pimpl->measurementSize); + m_pimpl->covarianceR.setZero(); + + for (int indexDyn2 = 0; indexDyn2 < m_pimpl->dynamicsList.size(); indexDyn2++) + { + m_pimpl->measurementVariableHandler.addVariable(m_pimpl->dynamicsList[indexDyn2].first, m_pimpl->dynamicsList[indexDyn2].second->getCovariance().size()); + + m_pimpl->covarianceR.block(m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, + m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size, m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size) = m_pimpl->dynamicsList[indexDyn2].second->getCovariance().asDiagonal(); + } + + m_pimpl->jointVelocityState.resize(m_pimpl->kinDynFullModel->model().getNrOfDOFs()); + m_pimpl->jointAccelerationState.resize(m_pimpl->kinDynFullModel->model().getNrOfDOFs()); + + for (int idx = 0; idx < m_pimpl->subModelList.size(); idx++) + { + m_pimpl->subModelJointVel.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->subModelJointAcc.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->subModelJointMotorTorque.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->subModelFrictionTorque.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->totalTorqueFromContacts.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->torqueFromContact.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + } + + m_pimpl->currentState.resize(m_pimpl->measurementSize); + m_pimpl->currentState.setZero(); + + m_pimpl->measurement.resize(m_pimpl->measurementSize); + m_pimpl->measurement.setZero(); + + m_pimpl->tempPredictedMeas.resize(m_pimpl->measurementSize); + + m_pimpl->measurementDescription = bfl::VectorDescription(m_pimpl->measurementSize); + + m_pimpl->predictedMeasurement.resize(m_pimpl->measurementSize, 2*m_pimpl->stateVariableHandler.getNumberOfVariables()+1); + + m_pimpl->isFinalized = true; + + return true; +} + +std::unique_ptr RDE::UkfMeasurement::build(std::weak_ptr handler, + System::VariablesHandler& stateVariableHandler, + std::shared_ptr kinDynFullModel, + const std::vector& subModelList, + const std::vector>& kinDynWrapperList) +{ + constexpr auto logPrefix = "[UkfMeasurement::build]"; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + BipedalLocomotion::log()->error("{} Invalid parameter handler.", logPrefix); + return nullptr; + } + + std::unique_ptr measurement = std::make_unique(); + + if (!measurement->initialize(ptr)) + { + BipedalLocomotion::log()->error("{} Unable to initialize the measurement object of the ukf.", logPrefix); + return nullptr; + } + + std::vector dynamicsList; + if (!ptr->getParameter("dynamics_list", dynamicsList)) + { + BipedalLocomotion::log()->error("{} Unable to find the parameter 'dynamics_list'.", logPrefix); + return nullptr; + } + + if (kinDynFullModel == nullptr) + { + BipedalLocomotion::log()->error("{} The pointer to the `KinDynComputation` object is not valid.", logPrefix); + return nullptr; + } + + measurement->m_pimpl->stateVariableHandler = stateVariableHandler; + + // Set KinDynComputation for the full model + measurement->m_pimpl->kinDynFullModel = kinDynFullModel; + + // Set the list of SubModel + measurement->m_pimpl->subModelList.reserve(subModelList.size()); + measurement->m_pimpl->subModelList = subModelList; + + // set the list of SubModelKinDynWrapper + measurement->m_pimpl->kinDynWrapperList.reserve(kinDynWrapperList.size()); + measurement->m_pimpl->kinDynWrapperList = kinDynWrapperList; + + // per each dynamics add variable to the variableHandler + // and add the dynamics to the list + for (const auto& dynamicsGroupName : dynamicsList) + { + auto dynamicsGroupTmp = ptr->getGroup(dynamicsGroupName).lock(); + if (dynamicsGroupTmp == nullptr) + { + BipedalLocomotion::log()->error("{} Unable to find the group '{}'.", logPrefix, dynamicsGroupName); + return nullptr; + } + + // add dT parameter since it is required by all the dynamics + auto dynamicsGroup = dynamicsGroupTmp->clone(); + dynamicsGroup->setParameter("sampling_time", measurement->m_pimpl->dT); + + // create variable handler + std::string dynamicsName; + std::vector covariances; + std::vector dynamicsElements; + if (!dynamicsGroup->getParameter("name", dynamicsName)) + { + BipedalLocomotion::log()->error("{} Unable to find the parameter 'name'.", logPrefix); + return nullptr; + } + if (!dynamicsGroup->getParameter("covariance", covariances)) + { + BipedalLocomotion::log()->error("{} Unable to find the parameter 'covariance'.", logPrefix); + return nullptr; + } + if (!dynamicsGroup->getParameter("elements", dynamicsElements)) + { + BipedalLocomotion::log()->error("{} Unable to find the parameter 'elements'.", logPrefix); + return nullptr; + } + + std::string dynamicModel; + if (!dynamicsGroup->getParameter("dynamic_model", dynamicModel)) + { + BipedalLocomotion::log()->error("{} Unable to find the parameter 'dynamic_model'.", logPrefix); + return nullptr; + } + + std::shared_ptr dynamicsInstance = RDE::DynamicsFactory::createInstance(dynamicModel); + if (dynamicsInstance == nullptr) + { + BipedalLocomotion::log()->error("{} The dynamic model '{}' has not been registered.", logPrefix, dynamicModel); + return nullptr; + } + + dynamicsInstance->setSubModels(subModelList, kinDynWrapperList); + + dynamicsInstance->initialize(dynamicsGroup); + + // add dynamics to the list +// measurement->m_pimpl->dynamicsList.insert({dynamicsName, dynamicsInstance}); + measurement->m_pimpl->dynamicsList.emplace_back(dynamicsName, dynamicsInstance); + } + + measurement->m_pimpl->inputDescription = bfl::VectorDescription(stateVariableHandler.getNumberOfVariables()); + + // finalize estimator + if (!measurement->finalize(measurement->m_pimpl->stateVariableHandler)) + { + BipedalLocomotion::log()->error("{} Unable to finalize the RobotDynamicsEstimator.", logPrefix); + return nullptr; + } + + return measurement; +} + +void RDE::UkfMeasurement::setUkfInputProvider(std::shared_ptr ukfInputProvider) +{ + m_pimpl->ukfInputProvider = ukfInputProvider; +} + +std::pair RDE::UkfMeasurement::getNoiseCovarianceMatrix() const +{ + return std::make_pair(true, m_pimpl->covarianceR); +} + +bfl::VectorDescription RDE::UkfMeasurement::getMeasurementDescription() const +{ + return m_pimpl->measurementDescription; +} + +bfl::VectorDescription RDE::UkfMeasurement::getInputDescription() const +{ + return m_pimpl->inputDescription; +} + +// TODO +// Here the cur_state has size state_size x n_sigma_points +// this means that the computation can be parallelized +std::pair RDE::UkfMeasurement::predictedMeasure(const Eigen::Ref& cur_states) const +{ + constexpr auto logPrefix = "[UkfMeasurement::propagate]"; + + // Check that everything is initialized and set + if (!m_pimpl->isFinalized) + { + BipedalLocomotion::log()->error("{} The ukf state is not well initialized.", logPrefix); + throw std::runtime_error("Error"); + } + + if (m_pimpl->ukfInputProvider == nullptr) + { + BipedalLocomotion::log()->error("{} The ukf input provider is not set.", logPrefix); + throw std::runtime_error("Error"); + } + + // Get input of ukf from provider + m_pimpl->ukfInput = m_pimpl->ukfInputProvider->getOutput(); + + for (int sample = 0; sample < cur_states.cols(); sample++) + { + m_pimpl->currentState = cur_states.block(0, sample, cur_states.rows(), 1);; + + m_pimpl->unpackState(); + + // Update kindyn full model + m_pimpl->kinDynFullModel->setRobotState(m_pimpl->ukfInput.robotBasePose.transform(), + m_pimpl->ukfInput.robotJointPositions, + iDynTree::make_span(m_pimpl->ukfInput.robotBaseVelocity.data(), manif::SE3d::Tangent::DoF), + m_pimpl->jointVelocityState, + m_pimpl->gravity); + + if (!m_pimpl->updateState()) + { + BipedalLocomotion::log()->error("{} The joint accelerations are not updated.", logPrefix); + throw std::runtime_error("Error"); + } + + m_pimpl->ukfInput.robotJointAccelerations = m_pimpl->jointAccelerationState; + + // TODO + // This could be parallelized + + // Update all the dynamics +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// std::cout << "variable name = " << name << " , variable offset = " << m_pimpl->measurementVariableHandler.getVariable(name).offset << std::endl; + +// dynamics->setState(m_pimpl->currentState); + +// dynamics->setInput(m_pimpl->ukfInput); + +// if (!dynamics->update()) +// { +// BipedalLocomotion::log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); +// throw std::runtime_error("Error"); +// } + +// m_pimpl->tempPredictedMeas.segment(m_pimpl->measurementVariableHandler.getVariable(name).offset, +// m_pimpl->measurementVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); +// } + for (int indexDyn = 0; indexDyn < m_pimpl->dynamicsList.size(); indexDyn++) + { + m_pimpl->dynamicsList[indexDyn].second->setState(m_pimpl->currentState); + + m_pimpl->dynamicsList[indexDyn].second->setInput(m_pimpl->ukfInput); + + if (!m_pimpl->dynamicsList[indexDyn].second->update()) + { + BipedalLocomotion::log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, m_pimpl->dynamicsList[indexDyn].first); + throw std::runtime_error("Error"); + } + + m_pimpl->tempPredictedMeas.segment(m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn].first).offset, + m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn].first).size) = m_pimpl->dynamicsList[indexDyn].second->getUpdatedVariable(); + } + m_pimpl->predictedMeasurement.block(0, sample, m_pimpl->measurementSize, 1) = m_pimpl->tempPredictedMeas; + } + + return std::make_pair(true, m_pimpl->predictedMeasurement); +} + +std::pair RDE::UkfMeasurement::innovation(const bfl::Data& predicted_measurements, const bfl::Data& measurements) const +{ + Eigen::MatrixXd innovation = -(bfl::any::any_cast(predicted_measurements).colwise() - bfl::any::any_cast(measurements)); + + return std::make_pair(true, std::move(innovation)); +} + +std::size_t RDE::UkfMeasurement::getMeasurementSize() +{ + return m_pimpl->measurementSize; +} + +BipedalLocomotion::System::VariablesHandler& RDE::UkfMeasurement::getMeasurementVariableHandler() +{ + return m_pimpl->measurementVariableHandler; +} + +void RDE::UkfMeasurement::setStateVariableHandler(System::VariablesHandler stateVariableHandler) const +{ + m_pimpl->stateVariableHandler = stateVariableHandler; +} + +std::pair RDE::UkfMeasurement::measure(const bfl::Data& data) const +{ + return std::make_pair(true, m_pimpl->measurement); +} + +bool RDE::UkfMeasurement::freeze(const bfl::Data& data) +{ + constexpr auto logPrefix = "[UkfMeasurement::freeze]"; + + m_pimpl->measurementMap = bfl::any::any_cast>(data); + +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// m_pimpl->offsetMeasurement = m_pimpl->measurementVariableHandler.getVariable(name).offset; + +// if(m_pimpl->measurementMap.count(name) == 0) +// { +// BipedalLocomotion::log()->error("{} Measurement with name `{}` not found.", logPrefix, name); +// return false; +// } + +// // If more sub-models share the same accelerometer or gyroscope sensor, the measurement vector is concatenated +// // a number of times equal to the number of sub-models using the sensor. +// while(m_pimpl->offsetMeasurement < +// (m_pimpl->measurementVariableHandler.getVariable(name).offset + m_pimpl->measurementVariableHandler.getVariable(name).size)) +// { +// m_pimpl->measurement.segment(m_pimpl->offsetMeasurement, m_pimpl->measurementMap[name].size()) +// = m_pimpl->measurementMap[name]; + +// m_pimpl->offsetMeasurement += m_pimpl->measurementMap[name].size(); +// } +// } + for (int index = 0; index < m_pimpl->dynamicsList.size(); index++) + { + m_pimpl->offsetMeasurement = m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[index].first).offset; + + if(m_pimpl->measurementMap.count(m_pimpl->dynamicsList[index].first) == 0) + { + BipedalLocomotion::log()->error("{} Measurement with name `{}` not found.", logPrefix, m_pimpl->dynamicsList[index].first); + return false; + } + + // If more sub-models share the same accelerometer or gyroscope sensor, the measurement vector is concatenated + // a number of times equal to the number of sub-models using the sensor. + while(m_pimpl->offsetMeasurement < + (m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[index].first).offset + m_pimpl->measurementVariableHandler.getVariable(m_pimpl->dynamicsList[index].first).size)) + { + m_pimpl->measurement.segment(m_pimpl->offsetMeasurement, m_pimpl->measurementMap[m_pimpl->dynamicsList[index].first].size()) + = m_pimpl->measurementMap[m_pimpl->dynamicsList[index].first]; + + m_pimpl->offsetMeasurement += m_pimpl->measurementMap[m_pimpl->dynamicsList[index].first].size(); + } + } + + return true; +} diff --git a/src/Estimators/src/UkfState.cpp b/src/Estimators/src/UkfState.cpp new file mode 100644 index 0000000000..b0f0ca6991 --- /dev/null +++ b/src/Estimators/src/UkfState.cpp @@ -0,0 +1,515 @@ +/** + * @file UkfState.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace BipedalLocomotion; +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +using namespace std::chrono; + +struct RDE::UkfState::Impl +{ + bool isInitialized{false}; + bool isFinalized{false}; + + Eigen::Vector3d gravity{0, 0, -Math::StandardAccelerationOfGravitation}; /**< Gravity vector. */ + + Eigen::MatrixXd covarianceQ; /**< Covariance matrix. */ + Eigen::MatrixXd initialCovariance; /**< Initial covariance matrix. */ + std::size_t stateSize; /**< Length of the state vector. */ + double dT; /**< Sampling time */ + + std::vector>> dynamicsList; /**< List of the dynamics composing the process model. */ + + System::VariablesHandler stateVariableHandler; /**< Variable handler describing the state vector. */ + + std::shared_ptr kinDynFullModel; /**< KinDynComputation object for the full model. */ + std::vector subModelList; /**< List of SubModel object describing the sub-models composing the full model. */ + std::vector> kinDynWrapperList; /**< List of SubModelKinDynWrapper objects containing kinematic and dynamic information specific of each sub-model. */ + + std::shared_ptr ukfInputProvider; /**< Provider containing the updated robot state. */ + UKFInput ukfInput; /**< Struct containing the inputs for the ukf populated by the ukfInputProvider. */ + + Eigen::VectorXd jointVelocityState; /**< Joint velocity computed by the ukf. */ + Eigen::VectorXd jointAccelerationState; /**< Joint acceleration computed from forward dynamics which depends on the current ukf state. */ + Eigen::VectorXd currentState; /**< State estimated in the previous step. */ + Eigen::VectorXd nextState; /**< Vector containing all the updated states. */ + + std::vector subModelJointVel; /**< List of sub-model joint velocities. */ + std::vector subModelJointAcc; /**< List of sub-model joint accelerations. */ + std::vector subModelJointMotorTorque; /**< List of sub-model joint motor torques. */ + std::vector subModelFrictionTorque; /**< List of sub-model friction torques. */ + std::map FTMap; /**< The map contains names of the ft sensors and values of the wrench */ + std::map extContactMap; /**< The map contains names of the ft sensors and values of the wrench */ + manif::SE3d::Tangent tempSubModelBaseAcc; /**< Acceleration of the base of the sub-model. */ + + // Support variables + std::vector totalTorqueFromContacts; /**< Joint torques due to known and unknown contacts on the sub-model. */ + std::vector torqueFromContact; /**< Joint torques due to a specific contact. */ + Math::Wrenchd wrench; /**< Joint torques due to a specific contact. */ + + bool updateRobotDynamicsOnly{true}; + + void unpackState() + { + jointVelocityState = currentState.segment(stateVariableHandler.getVariable("ds").offset, + stateVariableHandler.getVariable("ds").size); + + for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) + { + // Take sub-model joint velocities, motor torques, friction torques, ft wrenches, ext contact wrenches + for (int jointIdx = 0; jointIdx < subModelList[subModelIdx].getModel().getNrOfDOFs(); jointIdx++) + { + subModelJointVel[subModelIdx](jointIdx) = + jointVelocityState(subModelList[subModelIdx].getJointMapping()[jointIdx]); + + subModelJointMotorTorque[subModelIdx](jointIdx) = + currentState[stateVariableHandler.getVariable("tau_m").offset + + subModelList[subModelIdx].getJointMapping()[jointIdx]]; + + subModelFrictionTorque[subModelIdx](jointIdx) = + currentState[stateVariableHandler.getVariable("tau_F").offset + + subModelList[subModelIdx].getJointMapping()[jointIdx]]; + } + + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfFTSensor(); idx++) + { + FTMap[subModelList[subModelIdx].getFTSensor(idx).name] = + currentState.segment(stateVariableHandler.getVariable(subModelList[subModelIdx].getFTSensor(idx).name).offset, + stateVariableHandler.getVariable(subModelList[subModelIdx].getFTSensor(idx).name).size); + } + + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfExternalContact(); idx++) + { + extContactMap[subModelList[subModelIdx].getExternalContact(idx)] = + currentState.segment(stateVariableHandler.getVariable(subModelList[subModelIdx].getExternalContact(idx)).offset, + stateVariableHandler.getVariable(subModelList[subModelIdx].getExternalContact(idx)).size); + } + } + } + + bool updateState() + { + // Update kindyn full model + kinDynFullModel->setRobotState(ukfInput.robotBasePose.transform(), + ukfInput.robotJointPositions, + iDynTree::make_span(ukfInput.robotBaseVelocity.data(), manif::SE3d::Tangent::DoF), + jointVelocityState, + gravity); + + // compute joint acceleration per each sub-model + for (int subModelIdx = 0; subModelIdx < subModelList.size(); subModelIdx++) + { + // Update the kindyn wrapper object of the submodel + kinDynWrapperList[subModelIdx]->updateState(ukfInput.robotBaseAcceleration, + jointAccelerationState, + updateRobotDynamicsOnly); + + if (subModelList[subModelIdx].getModel().getNrOfDOFs() > 0) + { + totalTorqueFromContacts[subModelIdx].setZero(); + + // Contribution of FT measurements + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfFTSensor(); idx++) + { + wrench = (int)subModelList[subModelIdx].getFTSensor(idx).forceDirection * + FTMap[subModelList[subModelIdx].getFTSensor(idx).name].array(); + + torqueFromContact[subModelIdx] = kinDynWrapperList[subModelIdx]-> + getFTJacobian(subModelList[subModelIdx].getFTSensor(idx).name). + block(0, 6, 6, subModelList[subModelIdx].getModel().getNrOfDOFs()).transpose() * wrench; + + totalTorqueFromContacts[subModelIdx] = totalTorqueFromContacts[subModelIdx].array() + torqueFromContact[subModelIdx].array(); + } + + // Contribution of unknown external contacts + for (int idx = 0; idx < subModelList[subModelIdx].getNrOfExternalContact(); idx++) + { + torqueFromContact[subModelIdx] = kinDynWrapperList[subModelIdx]->getExtContactJacobian(subModelList[subModelIdx].getExternalContact(idx)).block(0, 6, 6, subModelList[subModelIdx].getModel().getNrOfDOFs()).transpose() * extContactMap[subModelList[subModelIdx].getExternalContact(idx)]; + + totalTorqueFromContacts[subModelIdx] = totalTorqueFromContacts[subModelIdx].array() + torqueFromContact[subModelIdx].array(); + } + + tempSubModelBaseAcc = kinDynWrapperList[subModelIdx]->getBaseAcceleration(); + + if (!kinDynWrapperList[subModelIdx]->forwardDynamics(subModelJointMotorTorque[subModelIdx], + subModelFrictionTorque[subModelIdx], + totalTorqueFromContacts[subModelIdx], + tempSubModelBaseAcc.coeffs(), + subModelJointAcc[subModelIdx])) + { + log()->error("Cannot compute the inverse dynamics."); + return false; + } + + // Assign joint acceleration using the correct indeces + for (int jointIdx = 0; jointIdx < subModelList[subModelIdx].getJointMapping().size(); jointIdx++) + { + jointAccelerationState[subModelList[subModelIdx].getJointMapping()[jointIdx]] = subModelJointAcc[subModelIdx][jointIdx]; + } + } + } + + return true; + } +}; + +RDE::UkfState::UkfState() +{ + m_pimpl = std::make_unique(); +} + +RDE::UkfState::~UkfState() = default; + +bool RDE::UkfState::initialize(std::weak_ptr handler) +{ + constexpr auto logPrefix = "[UkfState::initialize]"; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", logPrefix); + return false; + } + + if (!ptr->getParameter("sampling_time", m_pimpl->dT)) + { + log()->error("{} Unable to find the `sampling_time` variable", logPrefix); + return false; + } + + m_pimpl->isInitialized = true; + + return true; +} + +bool RDE::UkfState::finalize(const System::VariablesHandler& handler) +{ + constexpr auto logPrefix = "[UkfState::finalize]"; + + if (!m_pimpl->isInitialized) + { + log()->error("{} Please call initialize() before finalize().", logPrefix); + return false; + } + + m_pimpl->isFinalized = false; + + m_pimpl->stateSize = 0; + + // finalize all the dynamics +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// if(!dynamics->finalize(handler)) +// { +// log()->error("{} Error while finalizing the dynamics named {}", logPrefix, name); +// return false; +// } + +// m_pimpl->stateSize += dynamics->size(); +// } + for (int indexDyn1 = 0; indexDyn1 < m_pimpl->dynamicsList.size(); indexDyn1++) + { + if(!m_pimpl->dynamicsList[indexDyn1].second->finalize(handler)) + { + log()->error("{} Error while finalizing the dynamics named {}", logPrefix, m_pimpl->dynamicsList[indexDyn1].first); + return false; + } + + m_pimpl->stateSize += m_pimpl->dynamicsList[indexDyn1].second->size(); + } + + + // Set value of process covariance matrix + m_pimpl->covarianceQ.resize(m_pimpl->stateSize, m_pimpl->stateSize); + m_pimpl->covarianceQ.setZero(); + + m_pimpl->initialCovariance.resize(m_pimpl->stateSize, m_pimpl->stateSize); + m_pimpl->initialCovariance.setZero(); + +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// m_pimpl->covarianceQ.block(handler.getVariable(name).offset, handler.getVariable(name).offset, +// handler.getVariable(name).size, handler.getVariable(name).size) = dynamics->getCovariance().asDiagonal(); + +// m_pimpl->initialCovariance.block(handler.getVariable(name).offset, handler.getVariable(name).offset, +// handler.getVariable(name).size, handler.getVariable(name).size) = dynamics->getInitialStateCovariance().asDiagonal(); + +// } + for (int indexDyn2 = 0; indexDyn2 < m_pimpl->dynamicsList.size(); indexDyn2++) + { + m_pimpl->covarianceQ.block(handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, + handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size, handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size) = m_pimpl->dynamicsList[indexDyn2].second->getCovariance().asDiagonal(); + + m_pimpl->initialCovariance.block(handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).offset, + handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size, handler.getVariable(m_pimpl->dynamicsList[indexDyn2].first).size) = m_pimpl->dynamicsList[indexDyn2].second->getInitialStateCovariance().asDiagonal(); + + } + + m_pimpl->jointVelocityState.resize(m_pimpl->kinDynFullModel->model().getNrOfDOFs()); + m_pimpl->jointAccelerationState.resize(m_pimpl->kinDynFullModel->model().getNrOfDOFs()); + + for (int idx = 0; idx < m_pimpl->subModelList.size(); idx++) + { + m_pimpl->subModelJointVel.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->subModelJointAcc.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->subModelJointMotorTorque.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->subModelFrictionTorque.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->totalTorqueFromContacts.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + m_pimpl->torqueFromContact.emplace_back(Eigen::VectorXd(m_pimpl->subModelList[idx].getModel().getNrOfDOFs())); + } + + m_pimpl->currentState.resize(m_pimpl->stateSize); + m_pimpl->currentState.setZero(); + + m_pimpl->nextState.resize(m_pimpl->stateSize); + m_pimpl->nextState.setZero(); + + m_pimpl->isFinalized = true; + + return true; +} + +std::unique_ptr RDE::UkfState::build(std::weak_ptr handler, + std::shared_ptr kinDynFullModel, + const std::vector& subModelList, + const std::vector>& kinDynWrapperList) +{ + constexpr auto logPrefix = "[UkfState::build]"; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + log()->error("{} Invalid parameter handler.", logPrefix); + return nullptr; + } + + std::unique_ptr state = std::make_unique(); + + if (!state->initialize(ptr)) + { + log()->error("{} Unable to initialize the state object of the ukf.", logPrefix); + return nullptr; + } + + std::vector dynamicsList; + if (!ptr->getParameter("dynamics_list", dynamicsList)) + { + log()->error("{} Unable to find the parameter 'dynamics_list'.", logPrefix); + return nullptr; + } + + if (kinDynFullModel == nullptr) + { + log()->error("{} The pointer to the `KinDynComputation` object is not valid.", logPrefix); + return nullptr; + } + + // Set KinDynComputation for the full model + state->m_pimpl->kinDynFullModel = kinDynFullModel; + + // Set the list of SubModel + state->m_pimpl->subModelList.reserve(subModelList.size()); + state->m_pimpl->subModelList = subModelList; + + // set the list of SubModelKinDynWrapper + state->m_pimpl->kinDynWrapperList.reserve(kinDynWrapperList.size()); + state->m_pimpl->kinDynWrapperList = kinDynWrapperList; + + // per each dynamics add variable to the variableHandler + // and add the dynamics to the list + for (const auto& dynamicsGroupName : dynamicsList) + { + auto dynamicsGroupTmp = ptr->getGroup(dynamicsGroupName).lock(); + if (dynamicsGroupTmp == nullptr) + { + log()->error("{} Unable to find the group '{}'.", logPrefix, dynamicsGroupName); + return nullptr; + } + + // add dT parameter since it is required by all the dynamics + auto dynamicsGroup = dynamicsGroupTmp->clone(); + dynamicsGroup->setParameter("sampling_time", state->m_pimpl->dT); + + // create variable handler + std::string dynamicsName; + std::vector covariances; + std::vector dynamicsElements; + if (!dynamicsGroup->getParameter("name", dynamicsName)) + { + log()->error("{} Unable to find the parameter 'name'.", logPrefix); + return nullptr; + } + if (!dynamicsGroup->getParameter("covariance", covariances)) + { + log()->error("{} Unable to find the parameter 'covariance'.", logPrefix); + return nullptr; + } + if (!dynamicsGroup->getParameter("initial_covariance", covariances)) + { + log()->error("{} Unable to find the parameter 'initial_covariance'.", logPrefix); + return nullptr; + } + if (!dynamicsGroup->getParameter("elements", dynamicsElements)) + { + log()->error("{} Unable to find the parameter 'elements'.", logPrefix); + return nullptr; + } + state->m_pimpl->stateVariableHandler.addVariable(dynamicsName, covariances.size(), dynamicsElements); + + std::string dynamicModel; + if (!dynamicsGroup->getParameter("dynamic_model", dynamicModel)) + { + log()->error("{} Unable to find the parameter 'dynamic_model'.", logPrefix); + return nullptr; + } + + std::shared_ptr dynamicsInstance = RDE::DynamicsFactory::createInstance(dynamicModel); + if (dynamicsInstance == nullptr) + { + log()->error("{} The dynamic model '{}' has not been registered.", logPrefix, dynamicModel); + return nullptr; + } + + dynamicsInstance->setSubModels(subModelList, kinDynWrapperList); + + dynamicsInstance->initialize(dynamicsGroup); + + // add dynamics to the list + state->m_pimpl->dynamicsList.emplace_back(dynamicsName, dynamicsInstance); +// state->m_pimpl->dynamicsList.insert({dynamicsName, dynamicsInstance}); + } + + // finalize estimator + if (!state->finalize(state->m_pimpl->stateVariableHandler)) + { + log()->error("{} Unable to finalize the RobotDynamicsEstimator.", logPrefix); + return nullptr; + } + + return state; +} + +void RDE::UkfState::setUkfInputProvider(std::shared_ptr ukfInputProvider) +{ + m_pimpl->ukfInputProvider = ukfInputProvider; +} + +Eigen::MatrixXd RDE::UkfState::getNoiseCovarianceMatrix() +{ + return m_pimpl->covarianceQ; +} + +System::VariablesHandler& RDE::UkfState::getStateVariableHandler() +{ + return m_pimpl->stateVariableHandler; +} + +// TODO +// Here the cur_state has size state_size x n_sigma_points +// this means that the computation can be parallelized +void RDE::UkfState::propagate(const Eigen::Ref& cur_states, Eigen::Ref prop_states) +{ + constexpr auto logPrefix = "[UkfState::propagate]"; + + // Check that everything is initialized and set + if (!m_pimpl->isFinalized) + { + log()->error("{} The ukf state is not well initialized.", logPrefix); + throw std::runtime_error("Error"); + } + + if (m_pimpl->ukfInputProvider == nullptr) + { + log()->error("{} The ukf input provider is not set.", logPrefix); + throw std::runtime_error("Error"); + } + + // Get input of ukf from provider + m_pimpl->ukfInput = m_pimpl->ukfInputProvider->getOutput(); + + prop_states.resize(cur_states.rows(), cur_states.cols()); + + for (int sample = 0; sample < cur_states.cols(); sample++) + { + m_pimpl->currentState = cur_states.block(0, sample, cur_states.rows(), 1); + + m_pimpl->unpackState(); + + if (!m_pimpl->updateState()) + { + log()->error("{} The joint accelerations are not updated.", logPrefix); + throw std::runtime_error("Error"); + } + + m_pimpl->ukfInput.robotJointAccelerations = m_pimpl->jointAccelerationState; + + // TODO + // This could be parallelized + + // Update all the dynamics +// for (auto& [name, dynamics] : m_pimpl->dynamicsList) +// { +// dynamics->setState(m_pimpl->currentState); + +// dynamics->setInput(m_pimpl->ukfInput); + +// if (!dynamics->update()) +// { +// log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, name); +// throw std::runtime_error("Error"); +// } + +// m_pimpl->nextState.segment(m_pimpl->stateVariableHandler.getVariable(name).offset, +// m_pimpl->stateVariableHandler.getVariable(name).size) = dynamics->getUpdatedVariable(); +// } + + for (int indexDyn = 0; indexDyn < m_pimpl->dynamicsList.size(); indexDyn++) + { + m_pimpl->dynamicsList[indexDyn].second->setState(m_pimpl->currentState); + + m_pimpl->dynamicsList[indexDyn].second->setInput(m_pimpl->ukfInput); + + if (!m_pimpl->dynamicsList[indexDyn].second->update()) + { + log()->error("{} Cannot update the dynamics with name `{}`.", logPrefix, m_pimpl->dynamicsList[indexDyn].first); + throw std::runtime_error("Error"); + } + + m_pimpl->nextState.segment(m_pimpl->stateVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn].first).offset, + m_pimpl->stateVariableHandler.getVariable(m_pimpl->dynamicsList[indexDyn].first).size) = m_pimpl->dynamicsList[indexDyn].second->getUpdatedVariable(); + } + + prop_states.block(0, sample, cur_states.rows(), 1) = m_pimpl->nextState; + } +} + +bfl::VectorDescription RDE::UkfState::getStateDescription() +{ + return bfl::VectorDescription(m_pimpl->stateSize); +} + +std::size_t RDE::UkfState::getStateSize() +{ + return m_pimpl->stateSize; +} + +Eigen::Ref RDE::UkfState::getInitialStateCovarianceMatrix() +{ + return m_pimpl->initialCovariance; +} diff --git a/src/Estimators/src/ZeroVelocityDynamics.cpp b/src/Estimators/src/ZeroVelocityDynamics.cpp new file mode 100644 index 0000000000..ca87553cdd --- /dev/null +++ b/src/Estimators/src/ZeroVelocityDynamics.cpp @@ -0,0 +1,174 @@ +/** + * @file ZeroVelocityDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include + +namespace RDE = BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +bool RDE::ZeroVelocityDynamics::initialize(std::weak_ptr paramHandler) +{ + constexpr auto errorPrefix = "[ZeroVelocityDynamics::initialize]"; + + auto ptr = paramHandler.lock(); + if (ptr == nullptr) + { + log()->error("{} The parameter handler is not valid.", errorPrefix); + return false; + } + + // Set the state dynamics name + if (!ptr->getParameter("name", m_name)) + { + log()->error("{} Error while retrieving the name variable.", errorPrefix); + return false; + } + + // Set the state process covariance + if (!ptr->getParameter("covariance", m_covariances)) + { + log()->error("{} Error while retrieving the covariance variable.", errorPrefix); + return false; + } + + // Set the state initial covariance + if (!ptr->getParameter("initial_covariance", m_initialCovariances)) + { + log()->debug("{} Variable initial_covariance not found.", errorPrefix); + } + + // Set the dynamic model type + if (!ptr->getParameter("dynamic_model", m_dynamicModel)) + { + log()->error("{} Error while retrieving the dynamic_model variable.", errorPrefix); + return false; + } + + // Set the list of elements if it exists + if (!ptr->getParameter("elements", m_elements)) + { + log()->debug("{} Variable elements not found.", errorPrefix); + m_elements = {}; + } + + // Set the bias related variables if use_bias is true + if (!ptr->getParameter("use_bias", m_useBias)) + { + log()->debug("{} Variable use_bias not found. Set to false by default.", errorPrefix); + } + else + { + m_biasVariableName = m_name + "_bias"; + } + + m_description = "Zero velocity dynamics"; + + m_isInitialized = true; + + return true; +} + +bool RDE::ZeroVelocityDynamics::finalize(const System::VariablesHandler &stateVariableHandler) +{ + constexpr auto errorPrefix = "[ZeroVelocityDynamics::finalize]"; + + if (!m_isInitialized) + { + log()->error("{} Please initialize the dynamics before calling finalize.", errorPrefix); + return false; + } + + if (stateVariableHandler.getNumberOfVariables() == 0) + { + log()->error("{} The state variable handler is empty.", errorPrefix); + return false; + } + + m_stateVariableHandler = stateVariableHandler; + + if (!checkStateVariableHandler()) + { + log()->error("{} The state variable handler is not valid.", errorPrefix); + return false; + } + + m_size = m_covariances.size(); + + m_currentState.resize(m_size); + m_currentState.setZero(); + + m_updatedVariable.resize(m_size); + m_updatedVariable.setZero(); + + // Set the bias related variables if use_bias is true + if (m_useBias) + { + m_bias.resize(m_size); + m_bias.setZero(); + } + + return true; +} + +bool RDE::ZeroVelocityDynamics::setSubModels(const std::vector& subModelList, const std::vector>& kinDynWrapperList) +{ + return true; +} + +bool RDE::ZeroVelocityDynamics::checkStateVariableHandler() +{ + constexpr auto errorPrefix = "[ZeroVelocityDynamics::checkStateVariableHandler]"; + + // Check if the variable handler contains the variables used by this dynamics + if (!m_stateVariableHandler.getVariable(m_name).isValid()) + { + log()->error("{} The variable handler does not contain the expected state with name `{}`.", errorPrefix, m_name); + return false; + } + + if (m_useBias) + { + if (!m_stateVariableHandler.getVariable(m_biasVariableName).isValid()) + { + log()->error("{} The variable handler does not contain the expected state name {}.", errorPrefix, m_biasVariableName); + return false; + } + } + + return true; +} + +bool RDE::ZeroVelocityDynamics::update() +{ + if (m_useBias) + { + m_updatedVariable = m_currentState + m_bias; + } + else + { + m_updatedVariable = m_currentState; + } + + return true; +} + +void RDE::ZeroVelocityDynamics::setState(const Eigen::Ref ukfState) +{ + m_currentState = ukfState.segment(m_stateVariableHandler.getVariable(m_name).offset, + m_stateVariableHandler.getVariable(m_name).size); + + if (m_useBias) + { + m_bias = ukfState.segment(m_stateVariableHandler.getVariable(m_biasVariableName).offset, + m_stateVariableHandler.getVariable(m_biasVariableName).size); + } +} + +void RDE::ZeroVelocityDynamics::setInput(const UKFInput& ukfInput) +{ + return; +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp new file mode 100644 index 0000000000..bc69ea486a --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/AccelerometerMeasurementDynamicsTest.cpp @@ -0,0 +1,238 @@ +/** + * @file AccelerometerMeasurementDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; + +void createModelLoader(IParametersHandler::shared_ptr group, + iDynTree::ModelLoader& mdlLdr) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group->getParameter("joint_list", jointList)); + + std::vector ftFramesList; + auto ftGroup = group->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); +// jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +void createSubModels(iDynTree::ModelLoader& mdlLdr, + std::shared_ptr kinDynFullModel, + IParametersHandler::shared_ptr group, + SubModelCreator& subModelCreator) +{ + subModelCreator.setModelAndSensors(mdlLdr.model(), mdlLdr.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDynFullModel)); + + REQUIRE(subModelCreator.createSubModels(group)); +} + +bool setStaticState(std::shared_ptr kinDyn) +{ + size_t dofs = kinDyn->getNrOfDegreesOfFreedom(); + iDynTree::Transform worldTbase; + Eigen::VectorXd baseVel(6); + Eigen::Vector3d gravity; + + Eigen::VectorXd qj(dofs), dqj(dofs), ddqj(dofs); + + worldTbase = iDynTree::Transform(iDynTree::Rotation::Identity(), iDynTree::Position::Zero()); + + Eigen::Matrix4d transform = toEigen(worldTbase.asHomogeneousTransform()); + + gravity.setZero(); + gravity(2) = - BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + baseVel.setZero(); + + for (size_t dof = 0; dof < dofs; dof++) + { + qj(dof) = 0.0; + dqj(dof) = 0.0; + ddqj(dof) = 0.0; + } + + return kinDyn->setRobotState(transform, + iDynTree::make_span(qj.data(), qj.size()), + iDynTree::make_span(baseVel.data(), baseVel.size()), + iDynTree::make_span(dqj.data(), dqj.size()), + iDynTree::make_span(gravity.data(), gravity.size())); +} + +TEST_CASE("Friction Torque Dynamics") +{ + // Create parameter handler + auto parameterHandler = std::make_shared(); + + const std::string name = "r_leg_ft_acc"; + Eigen::VectorXd covariance(3); + covariance << 2.3e-3, 1.9e-3, 3.1e-3; + const std::string model = "AccelerometerMeasurementDynamics"; + const bool useBias = true; + + double dT = 0.01; + + const std::vector jointList = {"r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll"}; + + parameterHandler->setParameter("name", name); + parameterHandler->setParameter("covariance", covariance); + parameterHandler->setParameter("dynamic_model", model); + parameterHandler->setParameter("sampling_time", dT); + parameterHandler->setParameter("use_bias", useBias); + + // Create state variable handler + constexpr size_t sizeVariable = 6; + VariablesHandler variableHandler; + REQUIRE(variableHandler.addVariable("ds", sizeVariable)); + REQUIRE(variableHandler.addVariable("tau_m", sizeVariable)); + REQUIRE(variableHandler.addVariable("tau_F", sizeVariable)); + REQUIRE(variableHandler.addVariable("r_leg_ft_acc_bias", 3)); + + // Create model variable handler to load the robot model + auto modelParamHandler = std::make_shared(); + auto emptyGroupNamesFrames = std::make_shared(); + std::vector emptyVectorString; + emptyGroupNamesFrames->setParameter("names", emptyVectorString); + emptyGroupNamesFrames->setParameter("frames", emptyVectorString); + emptyGroupNamesFrames->setParameter("associated_joints", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("FT", emptyGroupNamesFrames)); + REQUIRE(modelParamHandler->setGroup("GYROSCOPE", emptyGroupNamesFrames)); + + auto accGroup = std::make_shared(); + std::vector accNameList = {"r_leg_ft_acc"}; + std::vector accFrameList = {"r_leg_ft"}; + accGroup->setParameter("names", accNameList); + accGroup->setParameter("frames", accFrameList); + REQUIRE(modelParamHandler->setGroup("ACCELEROMETER", accGroup)); + + auto emptyGroupFrames = std::make_shared(); + emptyGroupFrames->setParameter("frames", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("EXTERNAL_CONTACT", emptyGroupFrames)); + + modelParamHandler->setParameter("joint_list", jointList); + + // Load model + iDynTree::ModelLoader modelLoader; + createModelLoader(modelParamHandler, modelLoader); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::BODY_FIXED_REPRESENTATION)); + + REQUIRE(setStaticState(kinDyn)); + + SubModelCreator subModelCreator; + createSubModels(modelLoader, kinDyn, modelParamHandler, subModelCreator); + + std::vector> kinDynWrapperList; + const auto & subModelList = subModelCreator.getSubModelList(); + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); + } + + AccelerometerMeasurementDynamics accDynamics; + REQUIRE(accDynamics.setSubModels(subModelList, kinDynWrapperList)); + REQUIRE(accDynamics.initialize(parameterHandler)); + REQUIRE(accDynamics.finalize(variableHandler)); + + manif::SE3d::Tangent robotBaseAcceleration; + robotBaseAcceleration.setZero(); + + Eigen::VectorXd robotJointAcceleration(kinDyn->model().getNrOfDOFs()); + robotJointAcceleration.setZero(); + + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); + extWrench.zero(); + + // Compute joint torques in static configuration from inverse dynamics on the full model + iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); + + kinDyn->inverseDynamics(iDynTree::make_span(robotBaseAcceleration.data(), + manif::SE3d::Tangent::DoF), + robotJointAcceleration, + extWrench, + jointTorques); + + Eigen::VectorXd state; + state.resize(variableHandler.getNumberOfVariables()); + state.setZero(); + + int offset = variableHandler.getVariable("tau_m").offset; + int size = variableHandler.getVariable("tau_m").size; + for (int jointIndex = 0; jointIndex < size; jointIndex++) + { + state[offset+jointIndex] = jointTorques.jointTorques()[jointIndex]; + } + + // Create an input for the ukf state + UKFInput input; + + // Define joint positions + Eigen::VectorXd jointPos; + jointPos.resize(kinDyn->model().getNrOfDOFs()); + jointPos.setZero(); + input.robotJointPositions = jointPos; + + // Define base pose + manif::SE3d basePose; + basePose.setIdentity(); + input.robotBasePose = basePose; + + // Define base velocity and acceleration + manif::SE3d::Tangent baseVelocity, baseAcceleration; + baseVelocity.setZero(); + baseAcceleration.setZero(); + input.robotBaseVelocity = baseVelocity; + input.robotBaseAcceleration = baseAcceleration; + + input.robotJointAccelerations = Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(); + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(baseAcceleration, + input.robotJointAccelerations, + true)); + } + + accDynamics.setInput(input); + accDynamics.setState(state); + + REQUIRE(accDynamics.update()); + for (int idx = 0; idx < accDynamics.getUpdatedVariable().size(); idx++) + { + REQUIRE(std::abs(accDynamics.getUpdatedVariable()(idx)) < 10); + } +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt index 5631deab95..a783e50060 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt +++ b/src/Estimators/tests/RobotDynamicsEstimator/CMakeLists.txt @@ -20,8 +20,69 @@ if(FRAMEWORK_USE_icub-models AND FRAMEWORK_COMPILE_YarpImplementation) BipedalLocomotion::Math icub-models::icub-models Eigen3::Eigen - MANIF::manif - ) + MANIF::manif) + + add_bipedal_test(NAME ZeroVelocityDynamics + SOURCES ZeroVelocityDynamicsTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator + BipedalLocomotion::ParametersHandler + BipedalLocomotion::ParametersHandlerYarpImplementation + icub-models::icub-models + Eigen3::Eigen) + + add_bipedal_test(NAME FrictionTorqueStateDynamics + SOURCES FrictionTorqueStateDynamicsTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator + BipedalLocomotion::ParametersHandler + BipedalLocomotion::ParametersHandlerYarpImplementation + BipedalLocomotion::System + icub-models::icub-models + Eigen3::Eigen) + + add_bipedal_test(NAME JointVelocityStateDynamics + SOURCES JointVelocityStateDynamicsTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator + BipedalLocomotion::ParametersHandler + BipedalLocomotion::ParametersHandlerYarpImplementation + BipedalLocomotion::System + icub-models::icub-models + Eigen3::Eigen + MANIF::manif) + + add_bipedal_test(NAME AccelerometerMeasurementDynamics + SOURCES AccelerometerMeasurementDynamicsTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator + BipedalLocomotion::ParametersHandler + BipedalLocomotion::ParametersHandlerYarpImplementation + BipedalLocomotion::System + icub-models::icub-models + Eigen3::Eigen + MANIF::manif) + + add_bipedal_test(NAME GyroscopeMeasurementDynamics + SOURCES GyroscopeMeasurementDynamicsTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator + BipedalLocomotion::ParametersHandler + BipedalLocomotion::ParametersHandlerYarpImplementation + BipedalLocomotion::System + icub-models::icub-models + Eigen3::Eigen + MANIF::manif) + + add_bipedal_test(NAME UkfState + SOURCES UkfStateTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator BipedalLocomotion::ManifConversions BipedalLocomotion::ParametersHandlerYarpImplementation + icub-models::icub-models) + + add_bipedal_test(NAME UkfMeasurement + SOURCES UkfMeasurementTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator BipedalLocomotion::ManifConversions BipedalLocomotion::ParametersHandlerYarpImplementation + icub-models::icub-models) + + add_bipedal_test(NAME RobotDynamicsEstimator + SOURCES RobotDynamicsEstimatorTest.cpp + LINKS BipedalLocomotion::RobotDynamicsEstimator BipedalLocomotion::ManifConversions BipedalLocomotion::ParametersHandlerYarpImplementation + icub-models::icub-models) include_directories(${CMAKE_CURRENT_BINARY_DIR}) configure_file("${CMAKE_CURRENT_SOURCE_DIR}/ConfigFolderPath.h.in" "${CMAKE_CURRENT_BINARY_DIR}/ConfigFolderPath.h" @ONLY) diff --git a/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp new file mode 100644 index 0000000000..29dd42d193 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueStateDynamicsTest.cpp @@ -0,0 +1,257 @@ +/** + * @file FrictionTorqueDynamicsTest.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; + +void createModelLoader(IParametersHandler::shared_ptr group, + iDynTree::ModelLoader& mdlLdr) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group->getParameter("joint_list", jointList)); + + std::vector ftFramesList; + auto ftGroup = group->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); +// jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +void createSubModels(iDynTree::ModelLoader& mdlLdr, + std::shared_ptr kinDynFullModel, + IParametersHandler::shared_ptr group, + SubModelCreator& subModelCreator) +{ + subModelCreator.setModelAndSensors(mdlLdr.model(), mdlLdr.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDynFullModel)); + + REQUIRE(subModelCreator.createSubModels(group)); +} + +bool setStaticState(std::shared_ptr kinDyn) +{ + size_t dofs = kinDyn->getNrOfDegreesOfFreedom(); + iDynTree::Transform worldTbase; + Eigen::VectorXd baseVel(6); + Eigen::Vector3d gravity; + + Eigen::VectorXd qj(dofs), dqj(dofs), ddqj(dofs); + + worldTbase = iDynTree::Transform(iDynTree::Rotation::Identity(), iDynTree::Position::Zero()); + + Eigen::Matrix4d transform = toEigen(worldTbase.asHomogeneousTransform()); + + gravity.setZero(); + gravity(2) = - BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + baseVel.setZero(); + + for (size_t dof = 0; dof < dofs; dof++) + { + qj(dof) = 0.0; + dqj(dof) = 0.0; + ddqj(dof) = 0.0; + } + + return kinDyn->setRobotState(transform, + iDynTree::make_span(qj.data(), qj.size()), + iDynTree::make_span(baseVel.data(), baseVel.size()), + iDynTree::make_span(dqj.data(), dqj.size()), + iDynTree::make_span(gravity.data(), gravity.size())); +} + +TEST_CASE("Friction Torque Dynamics") +{ + // Create parameter handler + auto parameterHandler = std::make_shared(); + + const std::string name = "tau_F"; + Eigen::VectorXd covariance(6); + covariance << 1e-3, 1e-3, 5e-3, 5e-3, 5e-3, 5e-3; + const std::string model = "FrictionTorqueStateDynamics"; + const std::vector elements = {"r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll"}; + Eigen::VectorXd k0(6); + k0 << 9.106, 5.03, 4.93, 12.88, 14.34, 1.12; + Eigen::VectorXd k1(6); + k1 << 200.0, 6.9, 200.0, 59.87, 200.0, 200.0; + Eigen::VectorXd k2(6); + k2 << 1.767, 5.64, 0.27, 2.0, 3.0, 0.0; + double dT = 0.01; + + parameterHandler->setParameter("name", name); + parameterHandler->setParameter("covariance", covariance); + parameterHandler->setParameter("initial_covariance", covariance); + parameterHandler->setParameter("dynamic_model", model); + parameterHandler->setParameter("elements", elements); + parameterHandler->setParameter("friction_k0", k0); + parameterHandler->setParameter("friction_k1", k1); + parameterHandler->setParameter("friction_k2", k2); + parameterHandler->setParameter("sampling_time", dT); + + // Create variable handler + constexpr size_t sizeVariable = 6; + VariablesHandler variableHandler; + REQUIRE(variableHandler.addVariable("ds", sizeVariable)); + REQUIRE(variableHandler.addVariable("tau_m", sizeVariable)); + REQUIRE(variableHandler.addVariable("tau_F", sizeVariable)); + + auto modelParamHandler = std::make_shared(); + auto emptyGroupNamesFrames = std::make_shared(); + std::vector emptyVectorString; + emptyGroupNamesFrames->setParameter("names", emptyVectorString); + emptyGroupNamesFrames->setParameter("frames", emptyVectorString); + emptyGroupNamesFrames->setParameter("associated_joints", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("FT", emptyGroupNamesFrames)); + REQUIRE(modelParamHandler->setGroup("ACCELEROMETER", emptyGroupNamesFrames)); + REQUIRE(modelParamHandler->setGroup("GYROSCOPE", emptyGroupNamesFrames)); + auto emptyGroupFrames = std::make_shared(); + emptyGroupFrames->setParameter("frames", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("EXTERNAL_CONTACT", emptyGroupFrames)); + modelParamHandler->setParameter("joint_list", elements); + + iDynTree::ModelLoader modelLoader; + createModelLoader(modelParamHandler, modelLoader); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::BODY_FIXED_REPRESENTATION)); + + REQUIRE(setStaticState(kinDyn)); + + SubModelCreator subModelCreator; + createSubModels(modelLoader, kinDyn, modelParamHandler, subModelCreator); + + std::vector> kinDynWrapperList; + const auto & subModelList = subModelCreator.getSubModelList(); + + manif::SE3d::Tangent baseVelocity, baseAcceleration; + baseVelocity.setZero(); + baseAcceleration.setZero(); + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); + REQUIRE(kinDynWrapperList.at(idx)->updateState(baseAcceleration, Eigen::VectorXd(subModelList[idx].getModel().getNrOfDOFs()).setZero(), true)); + } + + FrictionTorqueStateDynamics tau_F_dynamics; + + REQUIRE(tau_F_dynamics.setSubModels(subModelList, kinDynWrapperList)); + REQUIRE(tau_F_dynamics.initialize(parameterHandler)); + REQUIRE(tau_F_dynamics.finalize(variableHandler)); + + manif::SE3d::Tangent robotBaseAcceleration; + robotBaseAcceleration.setZero(); + + Eigen::VectorXd robotJointAcceleration(kinDyn->model().getNrOfDOFs()); + robotJointAcceleration.setZero(); + + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); + extWrench.zero(); + + // Compute joint torques in static configuration from inverse dynamics on the full model + iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); + + kinDyn->inverseDynamics(iDynTree::make_span(robotBaseAcceleration.data(), + manif::SE3d::Tangent::DoF), + robotJointAcceleration, + extWrench, + jointTorques); + + Eigen::VectorXd state; + state.resize(variableHandler.getVariable("tau_F").offset + variableHandler.getVariable("tau_F").size + 1); + state.setZero(); + + int offset = variableHandler.getVariable("tau_m").offset; + int size = variableHandler.getVariable("tau_m").size; + for (int jointIndex = 0; jointIndex < size; jointIndex++) + { + state[offset+jointIndex] = jointTorques.jointTorques()[jointIndex]; + } + + // Create an input for the ukf state + UKFInput input; + + // Define joint positions + Eigen::VectorXd jointPos; + jointPos.resize(kinDyn->model().getNrOfDOFs()); + jointPos.setZero(); + input.robotJointPositions = jointPos; + + // Define base pose + manif::SE3d basePose; + basePose.setIdentity(); + input.robotBasePose = basePose; + + // Define base velocity and acceleration + input.robotBaseVelocity = baseVelocity; + input.robotBaseAcceleration = baseAcceleration; + + input.robotJointAccelerations = Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(); + + input.robotJointAccelerations = Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(); + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(input.robotBaseAcceleration, input.robotJointAccelerations, true)); + } + + tau_F_dynamics.setInput(input); + tau_F_dynamics.setState(state); + + REQUIRE(tau_F_dynamics.update()); + for (int idx = 0; idx < tau_F_dynamics.getUpdatedVariable().size(); idx++) + { + REQUIRE(std::abs(tau_F_dynamics.getUpdatedVariable()(idx)) < 0.1); + } + + // Set random friction torque + Eigen::VectorXd currentStateTauF(covariance.size()); + for (int index = 0; index < currentStateTauF.size(); index++) + { + currentStateTauF(index) = GENERATE(take(1, random(-30, 30))); + } + + state.segment(variableHandler.getVariable("tau_F").offset, variableHandler.getVariable("tau_F").size) = currentStateTauF; + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(input.robotBaseAcceleration, input.robotJointAccelerations, true)); + } + + tau_F_dynamics.setInput(input); + + tau_F_dynamics.setState(state); + + REQUIRE(tau_F_dynamics.update()); +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp new file mode 100644 index 0000000000..34d32f5533 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/GyroscopeMeasurementDynamicsTest.cpp @@ -0,0 +1,237 @@ +/** + * @file GyroscopeMeasurementDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; + +void createModelLoader(IParametersHandler::shared_ptr group, + iDynTree::ModelLoader& mdlLdr) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group->getParameter("joint_list", jointList)); + + std::vector ftFramesList; + auto ftGroup = group->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); +// jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +void createSubModels(iDynTree::ModelLoader& mdlLdr, + std::shared_ptr kinDynFullModel, + IParametersHandler::shared_ptr group, + SubModelCreator& subModelCreator) +{ + subModelCreator.setModelAndSensors(mdlLdr.model(), mdlLdr.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDynFullModel)); + + REQUIRE(subModelCreator.createSubModels(group)); +} + +bool setStaticState(std::shared_ptr kinDyn) +{ + size_t dofs = kinDyn->getNrOfDegreesOfFreedom(); + iDynTree::Transform worldTbase; + Eigen::VectorXd baseVel(6); + Eigen::Vector3d gravity; + + Eigen::VectorXd qj(dofs), dqj(dofs), ddqj(dofs); + + worldTbase = iDynTree::Transform(iDynTree::Rotation::Identity(), iDynTree::Position::Zero()); + + Eigen::Matrix4d transform = toEigen(worldTbase.asHomogeneousTransform()); + + gravity.setZero(); + gravity(2) = - BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + baseVel.setZero(); + + for (size_t dof = 0; dof < dofs; dof++) + { + qj(dof) = 0.0; + dqj(dof) = 0.0; + ddqj(dof) = 0.0; + } + + return kinDyn->setRobotState(transform, + iDynTree::make_span(qj.data(), qj.size()), + iDynTree::make_span(baseVel.data(), baseVel.size()), + iDynTree::make_span(dqj.data(), dqj.size()), + iDynTree::make_span(gravity.data(), gravity.size())); +} + +TEST_CASE("Gyroscope Measurement Dynamics") +{ + // Create parameter handler + auto parameterHandler = std::make_shared(); + + const std::string name = "r_leg_ft_gyro"; + Eigen::VectorXd covariance(3); + covariance << 2.3e-3, 1.9e-3, 3.1e-3; + const std::string model = "GyroscopeMeasurementDynamics"; + const bool useBias = true; + + double dT = 0.01; + + const std::vector jointList = {"r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll"}; + + parameterHandler->setParameter("name", name); + parameterHandler->setParameter("covariance", covariance); + parameterHandler->setParameter("dynamic_model", model); + parameterHandler->setParameter("sampling_time", dT); + parameterHandler->setParameter("use_bias", useBias); + + // Create state variable handler + constexpr size_t sizeVariable = 6; + VariablesHandler variableHandler; + REQUIRE(variableHandler.addVariable("ds", sizeVariable)); + REQUIRE(variableHandler.addVariable("tau_m", sizeVariable)); + REQUIRE(variableHandler.addVariable("tau_F", sizeVariable)); + REQUIRE(variableHandler.addVariable("r_leg_ft_gyro_bias", 3)); + + // Create model variable handler to load the robot model + auto modelParamHandler = std::make_shared(); + auto emptyGroupNamesFrames = std::make_shared(); + std::vector emptyVectorString; + emptyGroupNamesFrames->setParameter("names", emptyVectorString); + emptyGroupNamesFrames->setParameter("frames", emptyVectorString); + emptyGroupNamesFrames->setParameter("associated_joints", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("FT", emptyGroupNamesFrames)); + REQUIRE(modelParamHandler->setGroup("ACCELEROMETER", emptyGroupNamesFrames)); + + auto accGroup = std::make_shared(); + std::vector accNameList = {"r_leg_ft_gyro"}; + std::vector accFrameList = {"r_leg_ft_sensor"}; + accGroup->setParameter("names", accNameList); + accGroup->setParameter("frames", accFrameList); + REQUIRE(modelParamHandler->setGroup("GYROSCOPE", accGroup)); + + auto emptyGroupFrames = std::make_shared(); + emptyGroupFrames->setParameter("frames", emptyVectorString); + REQUIRE(modelParamHandler->setGroup("EXTERNAL_CONTACT", emptyGroupFrames)); + + modelParamHandler->setParameter("joint_list", jointList); + + // Load model + iDynTree::ModelLoader modelLoader; + createModelLoader(modelParamHandler, modelLoader); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::BODY_FIXED_REPRESENTATION)); + + REQUIRE(setStaticState(kinDyn)); + + SubModelCreator subModelCreator; + createSubModels(modelLoader, kinDyn, modelParamHandler, subModelCreator); + + std::vector> kinDynWrapperList; + const auto & subModelList = subModelCreator.getSubModelList(); + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); + } + + GyroscopeMeasurementDynamics gyroDynamics; + REQUIRE(gyroDynamics.setSubModels(subModelList, kinDynWrapperList)); + REQUIRE(gyroDynamics.initialize(parameterHandler)); + REQUIRE(gyroDynamics.finalize(variableHandler)); + + manif::SE3d::Tangent robotBaseAcceleration; + robotBaseAcceleration.setZero(); + + Eigen::VectorXd robotJointAcceleration(kinDyn->model().getNrOfDOFs()); + robotJointAcceleration.setZero(); + + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); + extWrench.zero(); + + // Compute joint torques in static configuration from inverse dynamics on the full model + iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); + + kinDyn->inverseDynamics(iDynTree::make_span(robotBaseAcceleration.data(), + manif::SE3d::Tangent::DoF), + robotJointAcceleration, + extWrench, + jointTorques); + + Eigen::VectorXd state; + state.resize(variableHandler.getNumberOfVariables()); + state.setZero(); + + int offset = variableHandler.getVariable("tau_m").offset; + int size = variableHandler.getVariable("tau_m").size; + for (int jointIndex = 0; jointIndex < size; jointIndex++) + { + state[offset+jointIndex] = jointTorques.jointTorques()[jointIndex]; + } + + // Create an input for the ukf state + UKFInput input; + + // Define joint positions + Eigen::VectorXd jointPos; + jointPos.resize(kinDyn->model().getNrOfDOFs()); + jointPos.setZero(); + input.robotJointPositions = jointPos; + + // Define base pose + manif::SE3d basePose; + basePose.setIdentity(); + input.robotBasePose = basePose; + + // Define base velocity and acceleration + manif::SE3d::Tangent baseVelocity, baseAcceleration; + baseVelocity.setZero(); + baseAcceleration.setZero(); + input.robotBaseVelocity = baseVelocity; + input.robotBaseAcceleration = baseAcceleration; + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(baseAcceleration, + Eigen::VectorXd(subModelList[idx].getModel().getNrOfDOFs()).setZero(), + true)); + } + + gyroDynamics.setInput(input); + + gyroDynamics.setState(state); + + REQUIRE(gyroDynamics.update()); + for (int idx = 0; idx < gyroDynamics.getUpdatedVariable().size(); idx++) + { + REQUIRE(std::abs(gyroDynamics.getUpdatedVariable()(idx)) < 0.1); + } +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp new file mode 100644 index 0000000000..565638d35d --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/JointVelocityStateDynamicsTest.cpp @@ -0,0 +1,421 @@ +/** + * @file FrictionTorqueDynamicsTest.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include +#include + +// iDynTree +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; + +IParametersHandler::shared_ptr loadParameterHandler() +{ + std::shared_ptr originalHandler = std::make_shared(); + IParametersHandler::shared_ptr parameterHandler = originalHandler; + + yarp::os::ResourceFinder& rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + rf.setDefaultConfigFile("/config/model.ini"); + + std::vector arguments = {" ", "--from ", getConfigPath()}; + + std::vector argv; + for (const auto& arg : arguments) + argv.push_back((char*)arg.data()); + argv.push_back(nullptr); + + rf.configure(argv.size() - 1, argv.data()); + + REQUIRE_FALSE(rf.isNull()); + parameterHandler->clear(); + REQUIRE(parameterHandler->isEmpty()); + originalHandler->set(rf); + + auto group = parameterHandler->getGroup("MODEL").lock(); + REQUIRE(group != nullptr); + + return group; +} + +void createModelLoader(IParametersHandler::shared_ptr group, + iDynTree::ModelLoader& mdlLdr, + bool useFT) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group->getParameter("joint_list", jointList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); + + if (useFT) + { + std::vector ftFramesList; + auto ftGroup = group->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); + jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + } + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +void createSubModels(iDynTree::ModelLoader& mdlLdr, + std::shared_ptr kinDynFullModel, + IParametersHandler::shared_ptr group, + bool useFTSensors, + SubModelCreator& subModelCreator) +{ + subModelCreator.setModelAndSensors(mdlLdr.model(), mdlLdr.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDynFullModel)); + + if (useFTSensors) + { + REQUIRE(subModelCreator.createSubModels(group)); + } + else + { + auto groupEmpty = group->clone(); + groupEmpty->clear(); + + std::vector jointList; + group->getParameter("joint_list", jointList); + groupEmpty->setParameter("joint_list", jointList); + + std::shared_ptr emptySubGroup = groupEmpty->clone(); + emptySubGroup->clear(); + std::vector emptyVector; + emptySubGroup->setParameter("names", emptyVector); + emptySubGroup->setParameter("frames", emptyVector); + emptySubGroup->setParameter("associated_joints", emptyVector); + groupEmpty->setGroup("FT", emptySubGroup); + groupEmpty->setGroup("ACCELEROMETER", emptySubGroup); + groupEmpty->setGroup("GYROSCOPE", emptySubGroup); + groupEmpty->setGroup("EXTERNAL_CONTACT", emptySubGroup); + + REQUIRE(subModelCreator.createSubModels(groupEmpty)); + } +} + +bool setStaticState(std::shared_ptr kinDyn) +{ + size_t dofs = kinDyn->getNrOfDegreesOfFreedom(); + iDynTree::Transform worldTbase; + Eigen::VectorXd baseVel(6); + Eigen::Vector3d gravity; + + Eigen::VectorXd qj(dofs), dqj(dofs), ddqj(dofs); + + worldTbase = iDynTree::Transform(iDynTree::Rotation::Identity(), iDynTree::Position::Zero()); + + Eigen::Matrix4d transform = toEigen(worldTbase.asHomogeneousTransform()); + + gravity.setZero(); + gravity(2) = -BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + baseVel.setZero(); + + for (size_t dof = 0; dof < dofs; dof++) + { + qj(dof) = 0.0; + dqj(dof) = 0.0; + ddqj(dof) = 0.0; + } + + return kinDyn->setRobotState(transform, + iDynTree::make_span(qj.data(), qj.size()), + iDynTree::make_span(baseVel.data(), baseVel.size()), + iDynTree::make_span(dqj.data(), dqj.size()), + iDynTree::make_span(gravity.data(), gravity.size())); +} + +TEST_CASE("Joint Velocity Dynamics Without FT") +{ + // Create parameter handler + auto parameterHandler = std::make_shared(); + + const std::string name = "ds"; + Eigen::VectorXd covariance(6); + covariance << 1e-3, 1e-3, 5e-3, 5e-3, 5e-3, 5e-3; + std::string model = "JointVelocityStateDynamics"; + const std::vector elements = {"r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll"}; + double dT = 0.01; + + parameterHandler->setParameter("name", name); + parameterHandler->setParameter("covariance", covariance); + parameterHandler->setParameter("initial_covariance", covariance); + parameterHandler->setParameter("dynamic_model", model); + parameterHandler->setParameter("elements", elements); + parameterHandler->setParameter("sampling_time", dT); + + // Create variable handler + constexpr size_t sizeVariable = 6; + VariablesHandler variableHandler; + REQUIRE(variableHandler.addVariable("ds", sizeVariable)); + REQUIRE(variableHandler.addVariable("tau_m", sizeVariable)); + REQUIRE(variableHandler.addVariable("tau_F", sizeVariable)); + REQUIRE(variableHandler.addVariable("r_leg_ft_acc_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft_acc_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_rear_ft_acc_bias", 3)); + REQUIRE(variableHandler.addVariable("r_leg_ft_gyro_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft_gyro_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_rear_ft_gyro_bias", 3)); + + auto handlerFromConfig = loadParameterHandler(); + + bool useFT = false; + + iDynTree::ModelLoader modelLoader; + createModelLoader(handlerFromConfig, modelLoader, useFT); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + + REQUIRE(setStaticState(kinDyn)); + + SubModelCreator subModelCreatorWithFT; + createSubModels(modelLoader, kinDyn, handlerFromConfig, useFT, subModelCreatorWithFT); + + std::vector> kinDynWrapperList; + const auto & subModelListWithFT = subModelCreatorWithFT.getSubModelList(); + + for (int idx = 0; idx < subModelCreatorWithFT.getSubModelList().size(); idx++) + { + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelListWithFT[idx])); + } + + manif::SE3d::Tangent robotBaseAcceleration; + robotBaseAcceleration.setZero(); + + Eigen::VectorXd robotJointAcceleration(kinDyn->model().getNrOfDOFs()); + robotJointAcceleration.setZero(); + + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); + extWrench.zero(); + + // Compute joint torques in static configuration from inverse dynamics on the full model + iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); + + kinDyn->inverseDynamics(iDynTree::make_span(robotBaseAcceleration.data(), + manif::SE3d::Tangent::DoF), + robotJointAcceleration, + extWrench, + jointTorques); + + JointVelocityStateDynamics ds; + model = "JointVelocityStateDynamics"; + parameterHandler->setParameter("dynamic_model", model); + REQUIRE(ds.setSubModels(subModelListWithFT, kinDynWrapperList)); + REQUIRE(ds.initialize(parameterHandler)); + REQUIRE(ds.finalize(variableHandler)); + + Eigen::VectorXd state; + state.resize(variableHandler.getNumberOfVariables()); + state.setZero(); + + int offset = variableHandler.getVariable("tau_m").offset; + int size = variableHandler.getVariable("tau_m").size; + for (int jointIndex = 0; jointIndex < size; jointIndex++) + { + state[offset+jointIndex] = jointTorques.jointTorques()[jointIndex]; + } + + // Create an input for the ukf state + UKFInput input; + + // Define joint positions + Eigen::VectorXd jointPos; + jointPos.resize(kinDyn->model().getNrOfDOFs()); + jointPos.setZero(); + input.robotJointPositions = jointPos; + + // Define base pose + manif::SE3d basePose; + basePose.setIdentity(); + input.robotBasePose = basePose; + + // Define base velocity and acceleration + manif::SE3d::Tangent baseVelocity, baseAcceleration; + baseVelocity.setZero(); + baseAcceleration.setZero(); + input.robotBaseVelocity = baseVelocity; + input.robotBaseAcceleration = baseAcceleration; + input.robotJointAccelerations = Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(); + + for (int idx = 0; idx < subModelCreatorWithFT.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(baseAcceleration, + input.robotJointAccelerations, + true)); + } + + ds.setInput(input); + + ds.setState(state); + + REQUIRE(ds.update()); +} + +TEST_CASE("Joint Velocity Dynamics With FT") +{ + // Create parameter handler + auto parameterHandler = std::make_shared(); + + const std::string name = "ds"; + Eigen::VectorXd covariance(6); + covariance << 1e-3, 1e-3, 5e-3, 5e-3, 5e-3, 5e-3; + std::string model = "JointVelocityStateDynamics"; + const std::vector elements = {"r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll"}; + double dT = 0.01; + + parameterHandler->setParameter("name", name); + parameterHandler->setParameter("covariance", covariance); + parameterHandler->setParameter("initial_covariance", covariance); + parameterHandler->setParameter("dynamic_model", model); + parameterHandler->setParameter("elements", elements); + parameterHandler->setParameter("sampling_time", dT); + + // Create variable handler + constexpr size_t sizeVariable = 6; + VariablesHandler variableHandler; + REQUIRE(variableHandler.addVariable("ds", sizeVariable)); + REQUIRE(variableHandler.addVariable("tau_m", sizeVariable)); + REQUIRE(variableHandler.addVariable("tau_F", sizeVariable)); + REQUIRE(variableHandler.addVariable("r_leg_ft", 6)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft", 6)); + REQUIRE(variableHandler.addVariable("r_foot_rear_ft", 6)); + REQUIRE(variableHandler.addVariable("r_leg_ft_bias", 6)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft_bias", 6)); + REQUIRE(variableHandler.addVariable("r_foot_rear_ft_bias", 6)); + REQUIRE(variableHandler.addVariable("r_leg_ft_acc_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft_acc_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_rear_ft_acc_bias", 3)); + REQUIRE(variableHandler.addVariable("r_leg_ft_gyro_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft_gyro_bias", 3)); + REQUIRE(variableHandler.addVariable("r_foot_rear_ft_gyro_bias", 3)); + + auto handlerFromConfig = loadParameterHandler(); + + iDynTree::ModelLoader modelLoader; + createModelLoader(handlerFromConfig, modelLoader, true); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + + REQUIRE(setStaticState(kinDyn)); + + bool useFT = true; + + SubModelCreator subModelCreatorWithFT; + createSubModels(modelLoader, kinDyn, handlerFromConfig, useFT, subModelCreatorWithFT); + + std::vector> kinDynWrapperListWithFT; + const auto & subModelListWithFT = subModelCreatorWithFT.getSubModelList(); + + for (int idx = 0; idx < subModelCreatorWithFT.getSubModelList().size(); idx++) + { + kinDynWrapperListWithFT.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperListWithFT.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperListWithFT.at(idx)->initialize(subModelListWithFT[idx])); + } + + manif::SE3d::Tangent robotBaseAcceleration; + robotBaseAcceleration.setZero(); + + Eigen::VectorXd robotJointAcceleration(kinDyn->model().getNrOfDOFs()); + robotJointAcceleration.setZero(); + + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); + extWrench.zero(); + + // Compute joint torques in static configuration from inverse dynamics on the full model + iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); + + kinDyn->inverseDynamics(iDynTree::make_span(robotBaseAcceleration.data(), + robotBaseAcceleration.coeffs().size()), + robotJointAcceleration, + extWrench, + jointTorques); + + JointVelocityStateDynamics dsSplit; + model = "JointVelocityStateDynamics"; + parameterHandler->setParameter("dynamic_model", model); + REQUIRE(dsSplit.setSubModels(subModelListWithFT, kinDynWrapperListWithFT)); + REQUIRE(dsSplit.initialize(parameterHandler)); + REQUIRE(dsSplit.finalize(variableHandler)); + + Eigen::VectorXd state; + state.resize(variableHandler.getNumberOfVariables()); + state.setZero(); + + int offset = variableHandler.getVariable("tau_m").offset; + int size = variableHandler.getVariable("tau_m").size; + for (int jointIndex = 0; jointIndex < size; jointIndex++) + { + state[offset+jointIndex] = jointTorques.jointTorques()[jointIndex]; + } + + auto massSecondSubModel = subModelListWithFT.at(1).getModel().getTotalMass(); + state(variableHandler.getVariable("r_leg_ft").offset+2) = massSecondSubModel * BipedalLocomotion::Math::StandardAccelerationOfGravitation; + + // Create an input for the ukf state + UKFInput input; + + // Define joint positions + Eigen::VectorXd jointPos; + jointPos.resize(kinDyn->model().getNrOfDOFs()); + jointPos.setZero(); + input.robotJointPositions = jointPos; + + // Define base pose + manif::SE3d basePose; + basePose.setIdentity(); + input.robotBasePose = basePose; + + // Define base velocity and acceleration + manif::SE3d::Tangent baseVelocity, baseAcceleration; + baseVelocity.setZero(); + baseAcceleration.setZero(); + input.robotBaseVelocity = baseVelocity; + input.robotBaseAcceleration = baseAcceleration; + + for (int idx = 0; idx < subModelCreatorWithFT.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperListWithFT.at(idx)->updateState(baseAcceleration, + Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(), + true)); + } + + input.robotJointAccelerations = Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(); + + dsSplit.setInput(input); + + dsSplit.setState(state); + + REQUIRE(dsSplit.update()); +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp new file mode 100644 index 0000000000..beb57a0fa9 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/RobotDynamicsEstimatorTest.cpp @@ -0,0 +1,309 @@ +/** + * @file RobotDynamicsEstimatorTest.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include +#include + +//BLF +#include +#include + +#include +#include +#include + +// iDynTree +#include +#include + +// RDE +#include +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +IParametersHandler::shared_ptr loadParameterHandler() +{ + std::shared_ptr originalHandler = std::make_shared(); + IParametersHandler::shared_ptr parameterHandler = originalHandler; + + yarp::os::ResourceFinder& rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + rf.setDefaultConfigFile("/config/config.ini"); + + std::vector arguments = {" ", "--from ", getConfigPath()}; + + std::vector argv; + for (const auto& arg : arguments) + argv.push_back((char*)arg.data()); + argv.push_back(nullptr); + + rf.configure(argv.size() - 1, argv.data()); + + REQUIRE_FALSE(rf.isNull()); + parameterHandler->clear(); + REQUIRE(parameterHandler->isEmpty()); + + originalHandler->set(rf); + + return parameterHandler; +} + +void createModelLoader(IParametersHandler::weak_ptr group, + iDynTree::ModelLoader& mdlLdr) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group.lock()->getParameter("joint_list", jointList)); + + std::vector ftFramesList; + auto ftGroup = group.lock()->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); + jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +void createInitialState(std::shared_ptr kinDynFullModel, + Eigen::Ref jointTorques, + IParametersHandler::weak_ptr modelHandler, + const std::vector& subModelList, + RobotDynamicsEstimatorOutput& initialState) +{ + // All the values set here come from an experiment on the real robot + + // Resize variables + initialState.ds.resize(kinDynFullModel->model().getNrOfDOFs()); // ds + initialState.tau_m.resize(kinDynFullModel->model().getNrOfDOFs()); // tau_m + initialState.tau_F.resize(kinDynFullModel->model().getNrOfDOFs()); // tau_F + std::vector ftList; + auto ftGroup = modelHandler.lock()->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("names", ftList)); + for (auto ft : ftList) + { + initialState.ftWrenches[ft] = Eigen::VectorXd(6).setZero(); // FT + + std::string ftBias = ft + "_bias"; + initialState.ftWrenchesBiases[ftBias] = Eigen::VectorXd(6).setZero(); // FT bias + } + std::vector accList; + auto accGroup = modelHandler.lock()->getGroup("ACCELEROMETER").lock(); + REQUIRE(accGroup->getParameter("names", accList)); + for (auto acc : accList) + { + std::string accBias = acc + "_bias"; + initialState.accelerometerBiases[accBias] = Eigen::VectorXd(3).setZero(); // ACC BIAS + } + std::vector gyroList; + auto gyroGroup = modelHandler.lock()->getGroup("GYROSCOPE").lock(); + REQUIRE(gyroGroup->getParameter("names", gyroList)); + for (auto gyro : gyroList) + { + std::string gyroBias = gyro + "_bias"; + initialState.gyroscopeBiases[gyroBias] = Eigen::VectorXd(3).setZero(); // GYRO BIAS + } + + + // Set values + initialState.ds.setZero(); +// initialState.tau_m << -1.24764e+01, 1.03400e-01, -4.70000e-03, -3.16350e+00, 4.21800e-01, 4.85000e-01; + initialState.tau_m << 22.644 , 13.5454, -7.6093, 19.3029, -15.0072, -0.805; + initialState.tau_F.setZero(); +// initialState.ftWrenches["r_leg_ft"] << -3.83709731e+01, -8.45653659e+00, 9.25208925e+01, 3.12676978e+00, -9.79714657e+00, 4.01285264e-01; +// initialState.ftWrenches["r_foot_front_ft"] << 5.22305136e-01, -4.88067107e-01, 1.61011089e+00, -9.13330381e-03, -8.39265034e-03, 4.18725767e-04; +// initialState.ftWrenches["r_foot_rear_ft"] << 5.19688377e-01, -4.85621881e-01, 1.60204420e+00, -8.32204636e-03, -9.16952530e-03, -7.99299795e-05; +// initialState.ftWrenchesBiases["r_leg_ft_bias"] << -1.89410386e+01, 7.33674253e+01, -6.04774355e+01, 1.34890092e-02, -3.02023625e+00, 7.01925185e-01; +// initialState.ftWrenchesBiases["r_foot_front_ft_bias"] << -4.85874907e+01, -3.90627141e+01, -3.89636265e+01, -1.83127438e-01, 9.51385814e-01, 2.97127661e-01; +// initialState.ftWrenchesBiases["r_foot_rear_ft_bias"] << -3.75985458e+01, -6.87282453e+01, -1.41893873e+00, -2.24845286e+00, 1.18104453e+00, 3.75446141e-01; + + initialState.ftWrenches["r_leg_ft"] << 49.45480758, 73.91349929, 46.85057916, -15.38562865, 9.04317083, 1.97395434; + initialState.ftWrenches["r_foot_front_ft"] << 1.38697938e-01, 8.71035288e-01, 1.52496873e+00, 1.36409975e-02, -1.99708849e-03, -9.99651158e-05; + initialState.ftWrenches["r_foot_rear_ft"] << 1.38003059e-01, 8.66671384e-01, 1.51732861e+00, 1.70397864e-02, -2.03056058e-03, -3.89970831e-04; +} + +void createInput(std::shared_ptr kinDynFullModel, + Eigen::Ref jointTorques, + IParametersHandler::weak_ptr modelHandler, + const std::vector& subModelList, + RobotDynamicsEstimatorInput& input) +{ + // All the values set here come from an experiment on the real robot + + manif::SE3d basePose; + basePose.setIdentity(); + input.basePose = basePose; + + manif::SE3d::Tangent baseVel; + baseVel.setZero(); + input.baseVelocity = baseVel; + + manif::SE3d::Tangent baseAcc; + baseAcc.setZero(); + input.baseAcceleration = baseAcc; + + input.jointPositions.resize(kinDynFullModel->model().getNrOfDOFs()); +// input.jointPositions << -3.81866275e-01, 1.27512464e-01, 3.83496133e-04, -2.67488553e-02, -9.77915140e-03, 9.58740333e-05; + input.jointPositions << 0.91693925, 0.69777121, 0.00249272, -0.68166438, -0.07698685, -0.08628663; + + input.jointVelocities.resize(kinDynFullModel->model().getNrOfDOFs()); + input.jointVelocities.setZero(); + + // Use gearbox ratio and torque constants to convert joint torques into motor currents + Eigen::VectorXd gearRatio(kinDynFullModel->model().getNrOfDOFs()); + gearRatio << 100.0, -100.0, 100.0, 100.0, 100.0, 100.0; + + Eigen::VectorXd torqueConstant(kinDynFullModel->model().getNrOfDOFs()); + torqueConstant << 0.111, 0.047, 0.047, 0.111, 0.111, 0.025; + +// input.motorCurrents = jointTorques.array() / (gearRatio.array() * torqueConstant.array()); + input.motorCurrents.resize(kinDynFullModel->model().getNrOfDOFs()); +// input.motorCurrents << -1.124e+00, -2.200e-02, -1.000e-03, -2.850e-01, 3.800e-02, 1.940e-01; + input.motorCurrents << 2.04 , -2.882, -1.619, 1.739, -1.352, -0.322; + + input.ftWrenches["r_leg_ft"] = Eigen::VectorXd(6).setZero(); +// input.ftWrenches["r_leg_ft"] << -57.31201172, 64.91088867, 32.04345703, 3.14025879, -12.81738281, 1.10321045; + input.ftWrenches["r_leg_ft"] << 49.45480758, 73.91349929, 46.85057916, -15.38562865, 9.04317083, 1.97395434; + + input.ftWrenches["r_foot_front_ft"] = Eigen::VectorXd(6).setZero(); +// input.ftWrenches["r_foot_front_ft"] << -48.06518555, -39.55078125, -37.35351562, -0.19226074, 0.94299316, 0.29754639; + input.ftWrenches["r_foot_front_ft"] << 1.38697938e-01, 8.71035288e-01, 1.52496873e+00, 1.36409975e-02, -1.99708849e-03, -9.99651158e-05; + + input.ftWrenches["r_foot_rear_ft"] = Eigen::VectorXd(6).setZero(); +// input.ftWrenches["r_foot_rear_ft"] << -37.07885742, -69.21386719, 0.18310547, -2.2567749, 1.171875, 0.37536621; + input.ftWrenches["r_foot_rear_ft"] << 1.38003059e-01, 8.66671384e-01, 1.51732861e+00, 1.70397864e-02, -2.03056058e-03, -3.89970831e-04; + + std::vector accList; + auto accGroup = modelHandler.lock()->getGroup("ACCELEROMETER").lock(); + REQUIRE(accGroup->getParameter("names", accList)); + for (auto acc : accList) + { + input.linearAccelerations[acc] = Eigen::VectorXd(3).setZero(); + input.linearAccelerations[acc](2) = - BipedalLocomotion::Math::StandardAccelerationOfGravitation; + } +// input.linearAccelerations["r_leg_ft_acc"] << 3.72, 0.8, -9.33; +// input.linearAccelerations["r_foot_front_ft_acc"] << 4.01, -2.38, 8.74; +// input.linearAccelerations["r_foot_rear_ft_acc"] << 3.77, -2.39, 8.94; + input.linearAccelerations["r_leg_ft_acc"] << -3.76, -7.65, -4.32; + input.linearAccelerations["r_foot_front_ft_acc"] << 1.44, 5.08, 8.11; + input.linearAccelerations["r_foot_rear_ft_acc"] << 2.12, 5.23, 8.13; + + std::vector gyroList; + auto gyroGroup = modelHandler.lock()->getGroup("GYROSCOPE").lock(); + REQUIRE(gyroGroup->getParameter("names", gyroList)); + for (auto gyro : gyroList) + { + input.angularVelocities[gyro] = Eigen::VectorXd(3).setZero(); + } +// input.angularVelocities["r_leg_ft_gyro"] << 0.03708825, -0.06654068, -0.00109083; +// input.angularVelocities["r_foot_front_ft_gyro"] << 0.0567232 , 0.04799655, -0.01308997; +// input.angularVelocities["r_foot_rear_ft_gyro"] << 0.06108652, 0.04690572, -0.01090831; + input.angularVelocities["r_leg_ft_gyro"] << -0.00654498, 0.0, 0.00109083; + input.angularVelocities["r_foot_front_ft_gyro"] << -0.00327249, 0.0, 0.00218166; + input.angularVelocities["r_foot_rear_ft_gyro"] << -0.00545415, 0.00109083, 0.00218166; +} + +TEST_CASE("RobotDynamicsEstimator") +{ + auto parameterHandler = loadParameterHandler(); + + iDynTree::ModelLoader modelLoader; + auto modelHandler = parameterHandler->getGroup("MODEL").lock(); + + REQUIRE_FALSE(modelHandler == nullptr); + + createModelLoader(modelHandler, modelLoader); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + REQUIRE(kinDyn->setFrameVelocityRepresentation(iDynTree::BODY_FIXED_REPRESENTATION)); + + SubModelCreator subModelCreator; + subModelCreator.setModelAndSensors(kinDyn->model(), modelLoader.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDyn)); + + REQUIRE(subModelCreator.createSubModels(modelHandler)); + + std::vector> kinDynWrapperList; + + const auto & subModelList = subModelCreator.getSubModelList(); + + for (int idx = 0; idx < subModelList.size(); idx++) + { + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); + } + + // automatic build the Estimator from parameter handler + auto estimator = RobotDynamicsEstimator::build(parameterHandler, kinDyn, subModelList, kinDynWrapperList); + REQUIRE_FALSE(estimator == nullptr); + + manif::SE3d::Tangent robotBaseAcceleration; + robotBaseAcceleration.setZero(); + + Eigen::VectorXd robotJointAcceleration(kinDyn->model().getNrOfDOFs()); + robotJointAcceleration.setZero(); + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(robotBaseAcceleration, + robotJointAcceleration, + true)); + } + + iDynTree::LinkNetExternalWrenches extWrench(kinDyn->model()); + extWrench.zero(); + + // Compute joint torques in static configuration from inverse dynamics on the full model + iDynTree::FreeFloatingGeneralizedTorques jointTorques(kinDyn->model()); + + kinDyn->inverseDynamics(iDynTree::make_span(robotBaseAcceleration.data(), + manif::SE3d::Tangent::DoF), + robotJointAcceleration, + extWrench, + jointTorques); + + RobotDynamicsEstimatorOutput initialState; + createInitialState(kinDyn, + iDynTree::toEigen(jointTorques.jointTorques()), + modelHandler, + subModelList, + initialState); + + REQUIRE(estimator->setInitialState(initialState)); + + RobotDynamicsEstimatorInput measurement; + createInput(kinDyn, + iDynTree::toEigen(jointTorques.jointTorques()), + modelHandler, + subModelList, + measurement); + +// for (int i = 0; i<700; i++) +// { +// std::cout << "index = " << i << std::endl; + auto tic = BipedalLocomotion::clock().now(); + REQUIRE(estimator->setInput(measurement)); + REQUIRE(estimator->advance()); + auto toc = BipedalLocomotion::clock().now(); + + BipedalLocomotion::log()->error("{}", toc - tic); + + RobotDynamicsEstimatorOutput result = estimator->getOutput(); +// } +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/SubModelCreatorTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/SubModelCreatorTest.cpp index 1ef8503470..4c8fded0c4 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/SubModelCreatorTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/SubModelCreatorTest.cpp @@ -63,7 +63,7 @@ TEST_CASE("SubModel Creation") std::vector ftFramesList; auto ftGroup = groupModel->getGroup("FT").lock(); - REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); std::vector jointsAndFTs; jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); @@ -92,6 +92,7 @@ TEST_CASE("SubModel Creation") std::vector emptyVector; groupFT->setParameter("names", emptyVector); groupFT->setParameter("frames", emptyVector); + groupFT->setParameter("associated_joints", emptyVector); groupModel->setGroup("FT", groupFT); REQUIRE(subModelCreatorWithoutFT.createSubModels(groupModel)); diff --git a/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp index 0c5ccc7561..b636edff19 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp +++ b/src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp @@ -156,7 +156,7 @@ TEST_CASE("SubModelKinDynWrapper") std::vector ftFramesList; auto ftGroup = group->getGroup("FT").lock(); - REQUIRE(ftGroup->getParameter("frames", ftFramesList)); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); std::vector jointsAndFTs; jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); @@ -219,7 +219,9 @@ TEST_CASE("SubModelKinDynWrapper") REQUIRE(kinDynSubModel.initialize(subModelList[idx])); - REQUIRE(kinDynSubModel.updateInternalKinDynState()); + REQUIRE(kinDynSubModel.updateState(robotBaseAcceleration, + robotJointAcceleration, + true)); int numberOfJoints = subModelList[idx].getModel().getNrOfDOFs(); @@ -236,9 +238,7 @@ TEST_CASE("SubModelKinDynWrapper") const std::string baseFrame = kinDynSubModel.getBaseFrameName(); manif::SE3d::Tangent baseAcceleration; - REQUIRE(kinDynSubModel.getBaseAcceleration(robotBaseAcceleration, - robotJointAcceleration, - baseAcceleration)); + baseAcceleration = kinDynSubModel.getBaseAcceleration(); manif::SE3d::Tangent baseAccelerationFromFullModel; REQUIRE(kinDyn->getFrameAcc(baseFrame, diff --git a/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp new file mode 100644 index 0000000000..3d67d676c3 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/UkfMeasurementTest.cpp @@ -0,0 +1,258 @@ +/** + * @file UkfMeasurementTest.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include +#include + +//BLF +#include +#include +#include + +// iDynTree +#include +#include + +//LIBRARYTOTEST +#include +#include +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +IParametersHandler::shared_ptr loadParameterHandler() +{ + std::shared_ptr originalHandler = std::make_shared(); + IParametersHandler::shared_ptr parameterHandler = originalHandler; + + yarp::os::ResourceFinder& rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + rf.setDefaultConfigFile("/config/config.ini"); + + std::vector arguments = {" ", "--from ", getConfigPath()}; + + std::vector argv; + for (const auto& arg : arguments) + argv.push_back((char*)arg.data()); + argv.push_back(nullptr); + + rf.configure(argv.size() - 1, argv.data()); + + REQUIRE_FALSE(rf.isNull()); + parameterHandler->clear(); + REQUIRE(parameterHandler->isEmpty()); + + originalHandler->set(rf); + + return parameterHandler; +} + +void createModelLoader(IParametersHandler::weak_ptr group, + iDynTree::ModelLoader& mdlLdr) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group.lock()->getParameter("joint_list", jointList)); + + std::vector ftFramesList; + auto ftGroup = group.lock()->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); + jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +TEST_CASE("UkfMeasurement") +{ + auto parameterHandler = loadParameterHandler(); + + iDynTree::ModelLoader modelLoader; + auto modelHandler = parameterHandler->getGroup("MODEL").lock(); + + REQUIRE_FALSE(modelHandler == nullptr); + + createModelLoader(modelHandler, modelLoader); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + + std::vector, std::shared_ptr>> subModelMap; + + SubModelCreator subModelCreator; + subModelCreator.setModelAndSensors(kinDyn->model(), modelLoader.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDyn)); + + REQUIRE(subModelCreator.createSubModels(modelHandler)); + + std::vector> kinDynWrapperList; + + const auto & subModelList = subModelCreator.getSubModelList(); + + for (int idx = 0; idx < subModelList.size(); idx++) + { + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); + } + + // Build the UkfState + auto groupUKF = parameterHandler->getGroup("UKF").lock(); + REQUIRE_FALSE(groupUKF == nullptr); + + auto groupUKFMeasurementTmp = groupUKF->getGroup("UKF_MEASUREMENT").lock(); + REQUIRE_FALSE(groupUKFMeasurementTmp == nullptr); + + auto groupUKFMeasurement = groupUKFMeasurementTmp->clone(); + double dT; + REQUIRE(parameterHandler->getGroup("GENERAL").lock()->getParameter("sampling_time", dT)); + groupUKFMeasurement->setParameter("sampling_time", dT); + + // Create the state variable handler + constexpr size_t sizeVariable = 6; + VariablesHandler stateVariableHandler; + REQUIRE(stateVariableHandler.addVariable("ds", sizeVariable)); + REQUIRE(stateVariableHandler.addVariable("tau_m", sizeVariable)); + REQUIRE(stateVariableHandler.addVariable("tau_F", sizeVariable)); + REQUIRE(stateVariableHandler.addVariable("r_leg_ft", 6)); + REQUIRE(stateVariableHandler.addVariable("r_foot_front_ft", 6)); + REQUIRE(stateVariableHandler.addVariable("r_foot_rear_ft", 6)); + REQUIRE(stateVariableHandler.addVariable("r_leg_ft_bias", 6)); + REQUIRE(stateVariableHandler.addVariable("r_foot_front_ft_bias", 6)); + REQUIRE(stateVariableHandler.addVariable("r_foot_rear_ft_bias", 6)); + REQUIRE(stateVariableHandler.addVariable("r_leg_ft_acc_bias", 3)); + REQUIRE(stateVariableHandler.addVariable("r_foot_front_ft_acc_bias", 3)); + REQUIRE(stateVariableHandler.addVariable("r_foot_rear_ft_acc_bias", 3)); + REQUIRE(stateVariableHandler.addVariable("r_leg_ft_gyro_bias", 3)); + REQUIRE(stateVariableHandler.addVariable("r_foot_front_ft_gyro_bias", 3)); + REQUIRE(stateVariableHandler.addVariable("r_foot_rear_ft_gyro_bias", 3)); + + std::unique_ptr measurementModel = UkfMeasurement::build(groupUKFMeasurement, + stateVariableHandler, + kinDyn, + subModelList, + kinDynWrapperList); + + REQUIRE(measurementModel != nullptr); + + // Create an input for the ukf state + UKFInput input; + + // Define joint positions + Eigen::VectorXd jointPos; + jointPos.resize(kinDyn->model().getNrOfDOFs()); + jointPos.setZero(); + input.robotJointPositions = jointPos; + + // Define base pose + manif::SE3d basePose; + basePose.setIdentity(); + input.robotBasePose = basePose; + + // Define base velocity and acceleration + manif::SE3d::Tangent baseVelocity, baseAcceleration; + baseVelocity.setZero(); + baseAcceleration.setZero(); + input.robotBaseVelocity = baseVelocity; + input.robotBaseAcceleration = baseAcceleration; + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(baseAcceleration, + Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(), + true)); + } + + std::shared_ptr inputProvider = std::make_shared(); + + BipedalLocomotion::System::VariablesHandler measurementHandler = measurementModel->getMeasurementVariableHandler(); +// int measurementSize = measurementModel->getMeasurementSize(); + int stateSize = stateVariableHandler.getNumberOfVariables(); + + measurementModel->setUkfInputProvider(inputProvider); + + Eigen::VectorXd currentState; + currentState.resize(stateSize); + currentState.setZero(); + + Eigen::VectorXd motorTorques; + motorTorques.resize(kinDyn->model().getNrOfDOFs()); + motorTorques << -1.6298, -1.10202, 0, -0.74, 0.0877, -0.00173; + Eigen::VectorXd wrenchFTtLeg; + wrenchFTtLeg.resize(6); + wrenchFTtLeg << 0, 0, 100.518, 0.748, 0.91, 0; + Eigen::VectorXd wrenchFTFootFront; + wrenchFTFootFront.resize(6); + wrenchFTFootFront << 0, 0, 1.761, -0.001, 0.0003, 0; + Eigen::VectorXd wrenchFTFootRear; + wrenchFTFootRear.resize(6); + wrenchFTFootRear << 0, 0, 1.752, 0.000876, 0.000649, 0; + + currentState.segment(stateVariableHandler.getVariable("tau_m").offset, stateVariableHandler.getVariable("tau_m").size) = motorTorques; + currentState.segment(stateVariableHandler.getVariable("r_leg_ft").offset, stateVariableHandler.getVariable("r_leg_ft").size) = wrenchFTtLeg; + currentState.segment(stateVariableHandler.getVariable("r_foot_front_ft").offset, stateVariableHandler.getVariable("r_foot_front_ft").size) = wrenchFTFootFront; + currentState.segment(stateVariableHandler.getVariable("r_foot_rear_ft").offset, stateVariableHandler.getVariable("r_foot_rear_ft").size) = wrenchFTFootRear; + currentState.segment(stateVariableHandler.getVariable("r_foot_rear_ft").offset, stateVariableHandler.getVariable("r_foot_rear_ft").size) = wrenchFTFootRear; + + REQUIRE(inputProvider->setInput(input)); + + std::map measurement; + + Eigen::VectorXd i_m; + i_m.resize(kinDyn->model().getNrOfDOFs()); + i_m << -0.1468, 0.2345, 0, -0.0667, 0.008, -0.000692; + measurement["i_m"] = i_m; + measurement["r_leg_ft"] = wrenchFTtLeg; + measurement["r_foot_front_ft"] = wrenchFTFootFront; + measurement["r_foot_rear_ft"] = wrenchFTFootRear; + + Eigen::Vector3d zeroVec3; + zeroVec3.setZero(); + + measurement["r_leg_ft_gyro"] = zeroVec3; + measurement["r_foot_front_ft_gyro"] = zeroVec3; + measurement["r_foot_rear_ft_gyro"] = zeroVec3; + + Eigen::Vector3d gravVec; + gravVec.setZero(); + gravVec(2) = 9.8066; + + measurement["r_leg_ft_acc"] = gravVec; + measurement["r_foot_front_ft_acc"] = gravVec; + measurement["r_foot_rear_ft_acc"] = gravVec; + + Eigen::VectorXd zeroVector6(stateVariableHandler.getVariable("tau_m").size); + zeroVector6.setZero(); + measurement["ds"] = zeroVector6; + + bfl::Data updatedMeasurementTmp; + + bool resultOk; + + REQUIRE(measurementModel->freeze(measurement)); + + std::tie(resultOk, updatedMeasurementTmp) = measurementModel->predictedMeasure(currentState); + + REQUIRE(resultOk); + + Eigen::VectorXd updatedMeasurement = bfl::any::any_cast(std::move(updatedMeasurementTmp)); + + for (int idx = 0; idx < updatedMeasurement.size(); idx++) + { + REQUIRE(std::abs(updatedMeasurement[idx]) < 200); + } +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp new file mode 100644 index 0000000000..b83b44bf7c --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp @@ -0,0 +1,198 @@ +/** + * @file UkfStateTest.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include +#include +#include + +//BLF +#include +#include +#include + +// iDynTree +#include +#include + +//LIBRARYTOTEST +#include +#include +#include +#include + +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; + +IParametersHandler::shared_ptr loadParameterHandler() +{ + std::shared_ptr originalHandler = std::make_shared(); + IParametersHandler::shared_ptr parameterHandler = originalHandler; + + yarp::os::ResourceFinder& rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + rf.setDefaultConfigFile("/config/config.ini"); + + std::vector arguments = {" ", "--from ", getConfigPath()}; + + std::vector argv; + for (const auto& arg : arguments) + argv.push_back((char*)arg.data()); + argv.push_back(nullptr); + + rf.configure(argv.size() - 1, argv.data()); + + REQUIRE_FALSE(rf.isNull()); + parameterHandler->clear(); + REQUIRE(parameterHandler->isEmpty()); + + originalHandler->set(rf); + + return parameterHandler; +} + +void createModelLoader(IParametersHandler::weak_ptr group, + iDynTree::ModelLoader& mdlLdr) +{ + // List of joints and fts to load the model + std::vector subModelList; + + const std::string modelPath = iCubModels::getModelFile("iCubGenova09"); + + std::vector jointList; + REQUIRE(group.lock()->getParameter("joint_list", jointList)); + + std::vector ftFramesList; + auto ftGroup = group.lock()->getGroup("FT").lock(); + REQUIRE(ftGroup->getParameter("associated_joints", ftFramesList)); + + std::vector jointsAndFTs; + jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); + jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); + + REQUIRE(mdlLdr.loadReducedModelFromFile(modelPath, jointsAndFTs)); +} + +TEST_CASE("UkfState") +{ + auto parameterHandler = loadParameterHandler(); + + iDynTree::ModelLoader modelLoader; + auto modelHandler = parameterHandler->getGroup("MODEL").lock(); + + REQUIRE_FALSE(modelHandler == nullptr); + + createModelLoader(modelHandler, modelLoader); + + auto kinDyn = std::make_shared(); + REQUIRE(kinDyn->loadRobotModel(modelLoader.model())); + + std::vector, std::shared_ptr>> subModelMap; + + SubModelCreator subModelCreator; + subModelCreator.setModelAndSensors(kinDyn->model(), modelLoader.sensors()); + REQUIRE(subModelCreator.setKinDyn(kinDyn)); + + REQUIRE(subModelCreator.createSubModels(modelHandler)); + + std::vector> kinDynWrapperList; + + const auto & subModelList = subModelCreator.getSubModelList(); + + for (int idx = 0; idx < subModelList.size(); idx++) + { + kinDynWrapperList.emplace_back(std::make_shared()); + REQUIRE(kinDynWrapperList.at(idx)->setKinDyn(kinDyn)); + REQUIRE(kinDynWrapperList.at(idx)->initialize(subModelList[idx])); + } + + // Build the UkfState + auto groupUKF = parameterHandler->getGroup("UKF").lock(); + REQUIRE_FALSE(groupUKF == nullptr); + + auto groupUKFStateTmp = groupUKF->getGroup("UKF_STATE").lock(); + REQUIRE_FALSE(groupUKFStateTmp == nullptr); + + auto groupUKFState = groupUKFStateTmp->clone(); + double dT; + REQUIRE(parameterHandler->getGroup("GENERAL").lock()->getParameter("sampling_time", dT)); + groupUKFState->setParameter("sampling_time", dT); + + std::unique_ptr stateModel = UkfState::build(groupUKFState, + kinDyn, + subModelList, + kinDynWrapperList); + + // Create an input for the ukf state + UKFInput input; + + // Define joint positions + Eigen::VectorXd jointPos; + jointPos.resize(kinDyn->model().getNrOfDOFs()); + jointPos.setZero(); + input.robotJointPositions = jointPos; + + // Define base pose + manif::SE3d basePose; + basePose.setIdentity(); + input.robotBasePose = basePose; + + // Define base velocity and acceleration + manif::SE3d::Tangent baseVelocity, baseAcceleration; + baseVelocity.setZero(); + baseAcceleration.setZero(); + input.robotBaseVelocity = baseVelocity; + input.robotBaseAcceleration = baseAcceleration; + + for (int idx = 0; idx < subModelCreator.getSubModelList().size(); idx++) + { + REQUIRE(kinDynWrapperList.at(idx)->updateState(baseAcceleration, + Eigen::VectorXd(kinDyn->model().getNrOfDOFs()).setZero(), + true)); + } + + std::shared_ptr inputProvider = std::make_shared(); + + BipedalLocomotion::System::VariablesHandler stateHandler = stateModel->getStateVariableHandler(); + int stateSize = stateModel->getStateSize(); + + stateModel->setUkfInputProvider(inputProvider); + + Eigen::VectorXd currentState; + currentState.resize(stateSize); + currentState.setZero(); + + Eigen::VectorXd motorTorques; + motorTorques.resize(kinDyn->model().getNrOfDOFs()); + motorTorques << -1.6298, -1.10202, 0, -0.74, 0.0877, -0.00173; + Eigen::VectorXd wrenchFTtLeg; + wrenchFTtLeg.resize(6); + wrenchFTtLeg << 0, 0, 100.518, 0.748, 0.91, 0; + Eigen::VectorXd wrenchFTFootFront; + wrenchFTFootFront.resize(6); + wrenchFTFootFront << 0, 0, 1.761, -0.001, 0.0003, 0; + Eigen::VectorXd wrenchFTFootRear; + wrenchFTFootRear.resize(6); + wrenchFTFootRear << 0, 0, 1.752, 0.000876, 0.000649, 0; + + currentState.segment(stateHandler.getVariable("tau_m").offset, stateHandler.getVariable("tau_m").size) = motorTorques; + currentState.segment(stateHandler.getVariable("r_leg_ft").offset, stateHandler.getVariable("r_leg_ft").size) = wrenchFTtLeg; + currentState.segment(stateHandler.getVariable("r_foot_front_ft").offset, stateHandler.getVariable("r_foot_front_ft").size) = wrenchFTFootFront; + currentState.segment(stateHandler.getVariable("r_foot_rear_ft").offset, stateHandler.getVariable("r_foot_rear_ft").size) = wrenchFTFootRear; + + Eigen::VectorXd updatedState; + updatedState.resize(stateSize); + + REQUIRE(inputProvider->setInput(input)); + + stateModel->propagate(currentState, updatedState); + + for (int idx = 0; idx < updatedState.size(); idx++) + { + REQUIRE(std::abs(updatedState[idx]) < 200); + } +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp b/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp new file mode 100644 index 0000000000..b370ed9a57 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp @@ -0,0 +1,103 @@ +/** + * @file ZeroVelocityDynamics.cpp + * @authors Ines Sorrentino + * @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include + +#include +#include +#include + +#include +#include +#include + +#include + +using namespace BipedalLocomotion::Estimators::RobotDynamicsEstimator; +using namespace BipedalLocomotion::ParametersHandler; +using namespace BipedalLocomotion::System; + +TEST_CASE("Zero Velocity Dynamics") +{ + auto parameterHandler = std::make_shared(); + + // Test without bias + const std::string name0 = "tau_m"; + Eigen::VectorXd covariance0(6); + covariance0 << 1e-3, 1e-3, 5e-3, 5e-3, 5e-3, 5e-3; + const std::string model0 = "ZeroVelocityDynamics"; + + parameterHandler->setParameter("name", name0); + parameterHandler->setParameter("covariance", covariance0); + parameterHandler->setParameter("initial_covariance", covariance0); + parameterHandler->setParameter("dynamic_model", model0); + parameterHandler->setParameter("sampling_time", 0.01); + + // Create variable handler + constexpr size_t sizeVariable = 6; + VariablesHandler variableHandler; + REQUIRE(variableHandler.addVariable("ds", sizeVariable)); + REQUIRE(variableHandler.addVariable("tau_m", sizeVariable)); + REQUIRE(variableHandler.addVariable("tau_F", sizeVariable)); + REQUIRE(variableHandler.addVariable("r_leg_ft_sensor", sizeVariable)); + REQUIRE(variableHandler.addVariable("r_leg_ft_sensor_bias", sizeVariable)); + REQUIRE(variableHandler.addVariable("r_foot_front_ft_sensor", sizeVariable)); + + ZeroVelocityDynamics tau_m; + + REQUIRE(tau_m.initialize(parameterHandler)); + REQUIRE(tau_m.finalize(variableHandler)); + + Eigen::VectorXd currentState(sizeVariable); + for (int index = 0; index < sizeVariable; index++) + { + currentState(index) = GENERATE(take(1, random(-100, 100))); + } + + Eigen::VectorXd state; + state.resize(sizeVariable * variableHandler.getNumberOfVariables()); + state.setZero(); + state.segment(variableHandler.getVariable("tau_m").offset, variableHandler.getVariable("tau_m").size) = currentState; + + tau_m.setState(state); + + REQUIRE(tau_m.update()); + + Eigen::VectorXd nextState(covariance0.size()); + nextState = tau_m.getUpdatedVariable(); + + REQUIRE(currentState == nextState); + + // Test with bias + const std::string name1 = "r_leg_ft_sensor"; + Eigen::VectorXd covariance1(6); + covariance1 << 1e-7, 1e-2, 5e0, 5e-3, 5e-1, 5e-10; + const std::string model1 = "ZeroVelocityDynamics"; + const bool useBias = true; + + parameterHandler->clear(); + parameterHandler->setParameter("name", name1); + parameterHandler->setParameter("covariance", covariance1); + parameterHandler->setParameter("initial_covariance", covariance1); + parameterHandler->setParameter("dynamic_model", model1); + parameterHandler->setParameter("sampling_time", 0.01); + parameterHandler->setParameter("use_bias", useBias); + + ZeroVelocityDynamics ft; + + REQUIRE(ft.initialize(parameterHandler)); + REQUIRE(ft.finalize(variableHandler)); + + state.segment(variableHandler.getVariable("r_leg_ft_sensor_bias").offset, variableHandler.getVariable("r_leg_ft_sensor_bias").size) = currentState; + + ft.setState(state); + REQUIRE(ft.update()); + + Eigen::VectorXd updatedVariable = ft.getUpdatedVariable(); + Eigen::VectorXd ftPre = state.segment(variableHandler.getVariable("r_leg_ft_sensor").offset, variableHandler.getVariable("r_leg_ft_sensor").size); + REQUIRE(std::abs(((currentState.array() + ftPre.array()) - updatedVariable.array()).array().sum()) == 0.0); +} diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/config.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/config.ini index 01410fd898..fb6521d62e 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/config.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/config.ini @@ -4,3 +4,4 @@ sampling_time 0.01 [include MODEL "model.ini"] +[include UKF "ukf.ini"] diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini index ce16c383df..66a320395c 100644 --- a/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/model.ini @@ -4,13 +4,14 @@ joint_list ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", frames ("") [FT] -names ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor") -frames ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor") +names ("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft") +frames ("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft") +associated_joints ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor") [ACCELEROMETER] names ("r_leg_ft_acc", "r_foot_front_ft_acc", "r_foot_rear_ft_acc") -frames ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor") +frames ("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft") [GYROSCOPE] names ("r_leg_ft_gyro", "r_foot_front_ft_gyro", "r_foot_rear_ft_gyro") -frames ("r_leg_ft_sensor", "r_foot_front_ft_sensor", "r_foot_rear_ft_sensor") +frames ("r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft") diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf.ini new file mode 100644 index 0000000000..6805ff7bd6 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf.ini @@ -0,0 +1,8 @@ +alpha 1.0 +beta 2.0 +kappa 0.0 + +[include UKF_STATE "ukf/ukf_state.ini"] + +[include UKF_MEASUREMENT "ukf/ukf_measurement.ini"] + diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini new file mode 100644 index 0000000000..a708005f91 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_measurement.ini @@ -0,0 +1,83 @@ +dynamics_list ("JOINT_VELOCITY", "MOTOR_CURRENT", + "RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT", + "RIGHT_LEG_ACC", "RIGHT_FOOT_FRONT_ACC", "RIGHT_FOOT_REAR_ACC", + "RIGHT_LEG_GYRO", "RIGHT_FOOT_FRONT_GYRO", "RIGHT_FOOT_REAR_GYRO") + +# Available dynamics = ["ZeroVelocityDynamics", "AccelerometerMeasurementDynamics", "GyroscopeMeasurementDynamics", "MotorCurrentMeasurementDynamics"] + +[JOINT_VELOCITY] +name "ds" +elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +covariance (1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4) +dynamic_model "ZeroVelocityDynamics" + +[MOTOR_CURRENT] +name "i_m" +elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +covariance (7.5e-4, 7.6e-4, 1.6e-3, 1.4e-3, 1.3e-4, 7.4e-4) +gear_ratio (100.0, -100.0, 100.0, 100.0, 100.0, 100.0) +torque_constant (0.111, 0.047, 0.047, 0.111, 0.111, 0.025) +dynamic_model "MotorCurrentMeasurementDynamics" + +[RIGHT_LEG_FT] +name "r_leg_ft" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-2, 1e-2, 1e-2, 1e-3, 1e-3, 1e-3) +use_bias 1 +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_FRONT_FT] +name "r_foot_front_ft" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5) +use_bias 1 +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_REAR_FT] +name "r_foot_rear_ft" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5) +use_bias 1 +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_LEG_ACC] +name "r_leg_ft_acc" +elements ("x", "y", "z") +covariance (2.3e-3, 1.9e-3, 3.1e-3) +use_bias 1 +dynamic_model "AccelerometerMeasurementDynamics" + +[RIGHT_FOOT_FRONT_ACC] +name "r_foot_front_ft_acc" +elements ("x", "y", "z") +covariance (2.3e-3, 1.9e-3, 3.1e-3) +use_bias 1 +dynamic_model "AccelerometerMeasurementDynamics" + +[RIGHT_FOOT_REAR_ACC] +name "r_foot_rear_ft_acc" +elements ("x", "y", "z") +covariance (2.5e-3, 2.3e-3, 3e-3) +use_bias 1 +dynamic_model "AccelerometerMeasurementDynamics" + +[RIGHT_LEG_GYRO] +name "r_leg_ft_gyro" +elements ("x", "y", "z") +covariance (0.000000283, 0.0127, 0.0000001) +use_bias 1 +dynamic_model "GyroscopeMeasurementDynamics" + +[RIGHT_FOOT_FRONT_GYRO] +name "r_foot_front_ft_gyro" +elements ("x", "y", "z") +covariance (4.1e-4, 3.3e-4, 3.2e-4) +use_bias 1 +dynamic_model "GyroscopeMeasurementDynamics" + +[RIGHT_FOOT_REAR_GYRO] +name "r_foot_rear_ft_gyro" +elements ("x", "y", "z") +covariance (4.9e-4, 4.2e-4, 3.3e-4) +use_bias 1 +dynamic_model "GyroscopeMeasurementDynamics" diff --git a/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini new file mode 100644 index 0000000000..d182e02031 --- /dev/null +++ b/src/Estimators/tests/RobotDynamicsEstimator/config/ukf/ukf_state.ini @@ -0,0 +1,115 @@ +dynamics_list ("JOINT_VELOCITY", "MOTOR_TORQUE", "FRICTION_TORQUE", "RIGHT_LEG_FT", "RIGHT_FOOT_FRONT_FT", "RIGHT_FOOT_REAR_FT", + "RIGHT_LEG_FT_BIAS", "RIGHT_FOOT_FRONT_FT_BIAS", "RIGHT_FOOT_REAR_FT_BIAS", + "RIGHT_LEG_ACC_BIAS", "RIGHT_FOOT_FRONT_ACC_BIAS", "RIGHT_FOOT_REAR_ACC_BIAS", + "RIGHT_LEG_GYRO_BIAS", "RIGHT_FOOT_FRONT_GYRO_BIAS", "RIGHT_FOOT_REAR_GYRO_BIAS") + +# Available dynamics = ["ZeroVelocityDynamics", "JointVelocityStateDynamics", "FrictionTorqueStateDynamics"] + +[JOINT_VELOCITY] +name "ds" +elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +covariance (1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) +dynamic_model "JointVelocityStateDynamics" + +[MOTOR_TORQUE] +name "tau_m" +elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +covariance (1e-3, 1e-3, 5e-3, 5e-3, 5e-3, 5e-3) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) +dynamic_model "ZeroVelocityDynamics" + +[FRICTION_TORQUE] +name "tau_F" +elements ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +covariance (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-1) +dynamic_model "ZeroVelocityDynamics" +friction_k0 (9.106, 5.03, 4.93, 12.88, 14.34, 1.12) +friction_k1 (200.0, 6.9, 200.0, 59.87, 200.0, 200.0) +friction_k2 (1.767, 5.64, 0.27, 2.0, 3.0, 0.0) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) + +[RIGHT_LEG_FT] +name "r_leg_ft" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-3, 1e-3, 1e-3, 1e-4, 1e-4, 1e-4) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_FRONT_FT] +name "r_foot_front_ft" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_REAR_FT] +name "r_foot_rear_ft" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-3, 1e-3, 1e-3, 1e-6, 1e-6, 1e-6) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_LEG_FT_BIAS] +name "r_leg_ft_bias" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_FRONT_FT_BIAS] +name "r_foot_front_ft_bias" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_REAR_FT_BIAS] +name "r_foot_rear_ft_bias" +elements ("fx", "fy", "fz", "mx", "my", "mz") +covariance (1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6) +initial_covariance (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_LEG_ACC_BIAS] +name "r_leg_ft_acc_bias" +elements ("x", "y", "z") +covariance (7.9e-5, 1.9e-5, 4.4e-5) +initial_covariance (0.01, 0.01, 0.01) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_FRONT_ACC_BIAS] +name "r_foot_front_ft_acc_bias" +elements ("x", "y", "z") +covariance (7.4e-5, 1.2e-5, 4.4e-5) +initial_covariance (0.01, 0.01, 0.01) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_REAR_ACC_BIAS] +name "r_foot_rear_ft_acc_bias" +elements ("x", "y", "z") +covariance (5.5e-5, 1.1e-5, 1.7e-5) +initial_covariance (0.01, 0.01, 0.01) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_LEG_GYRO_BIAS] +name "r_leg_ft_gyro_bias" +elements ("x", "y", "z") +covariance (4e-4, 4.7e-4, 4.7e-4) +initial_covariance (0.01, 0.01, 0.01) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_FRONT_GYRO_BIAS] +name "r_foot_front_ft_gyro_bias" +elements ("x", "y", "z") +covariance (1.3e-2, 9.9e-3, 9e-3) +initial_covariance (0.01, 0.01, 0.01) +dynamic_model "ZeroVelocityDynamics" + +[RIGHT_FOOT_REAR_GYRO_BIAS] +name "r_foot_rear_ft_gyro_bias" +elements ("x", "y", "z") +covariance (8.2e-8, 1e-2, 9.3e-3) +initial_covariance (0.01, 0.01, 0.01) +dynamic_model "ZeroVelocityDynamics" +