Add state/measurement models and RDE main class - PR4 #744
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
isorrentino
requested review from
GiulioRomualdi and
S-Dafarra
as code owners
isorrentino
force-pushed
the
RDE/PR4
branch
2 times, most recently
from
08659c6
to
91d6234
Compare
isorrentino
changed the title
isorrentino
force-pushed
the
RDE/PR4
branch
2 times, most recently
from
2039816
to
8a61c53
Compare
|
Hi @GiulioRomualdi and @S-Dafarra, the PR is now ready for review. |
|
I see a memcheck error but if I run valgrind offline I do not see the error. See the output I have offline: isorrentino@IITICUBLAP263:~/dev/robotology-superbuild/build/src/bipedal-locomotion-framework (master)$ valgrind ./bin/RobotDynamicsEstimationUnitTests
==22595== Memcheck, a memory error detector
==22595== Copyright (C) 2002-2017, and GNU GPL'd, by Julian Seward et al.
==22595== Using Valgrind-3.18.1 and LibVEX; rerun with -h for copyright info
==22595== Command: ./bin/RobotDynamicsEstimationUnitTests
==22595==
Randomness seeded to: 645987130
[2023-11-07 17:47:08.240] [thread: 22595] [blf] [info] Unpacking data...
[2023-11-07 17:47:08.302] [thread: 22595] [blf] [info] Size of s: 500x4
[2023-11-07 17:47:08.306] [thread: 22595] [blf] [info] Size of ds: 500x4
[2023-11-07 17:47:08.309] [thread: 22595] [blf] [info] Size of dds: 500x4
[2023-11-07 17:47:08.311] [thread: 22595] [blf] [info] Size of im: 500x4
[2023-11-07 17:47:08.313] [thread: 22595] [blf] [info] Size of expected_tauj: 500x4
[2023-11-07 17:47:08.315] [thread: 22595] [blf] [info] Size of expected_taum: 500x4
[2023-11-07 17:47:08.318] [thread: 22595] [blf] [info] Size of expected_tauF: 500x4
[2023-11-07 17:47:08.339] [thread: 22595] [blf] [info] Size of fts[ft_1]: 500x6
[2023-11-07 17:47:08.342] [thread: 22595] [blf] [info] Size of fts[ft_2]: 500x6
[2023-11-07 17:47:08.346] [thread: 22595] [blf] [info] Size of accs[ft_1_acc]: 500x3
[2023-11-07 17:47:08.347] [thread: 22595] [blf] [info] Size of accs[ft_2_acc]: 500x3
[2023-11-07 17:47:08.351] [thread: 22595] [blf] [info] Size of gyros[ft_1_gyro]: 500x3
[2023-11-07 17:47:08.352] [thread: 22595] [blf] [info] Size of gyros[ft_2_gyro]: 500x3
[2023-11-07 17:47:08.430] [thread: 22595] [blf] [info] Starting estimation...
===============================================================================
All tests passed (4018 assertions in 1 test case)
==22595==
==22595== HEAP SUMMARY:
==22595== in use at exit: 51,292 bytes in 263 blocks
==22595== total heap usage: 1,275,387 allocs, 1,275,124 frees, 445,230,829 bytes allocated
==22595==
==22595== LEAK SUMMARY:
==22595== definitely lost: 0 bytes in 0 blocks
==22595== indirectly lost: 0 bytes in 0 blocks
==22595== possibly lost: 0 bytes in 0 blocks
==22595== still reachable: 49,276 bytes in 242 blocks
==22595== suppressed: 0 bytes in 0 blocks
==22595== Rerun with --leak-check=full to see details of leaked memory
==22595==
==22595== For lists of detected and suppressed errors, rerun with: -s
==22595== ERROR SUMMARY: 0 errors from 0 contexts (suppressed: 0 from 0)The code is compiled in |
|
The error was about the time. The test was taking too long. I reduced the number of samples. |
GiulioRomualdi
requested changes
Nov 8, 2023
Comment on lines
44
to
48
| /** | ||
| * Private implementation | ||
| */ | ||
| // struct Impl; | ||
| // std::unique_ptr<Impl> m_pimpl; |
There was a problem hiding this comment.
Since you removed pimpl you can delete these lines
Comment on lines
59
to
67
| // /** | ||
| // * Constructor. | ||
| // */ | ||
| // UkfMeasurement(); | ||
|
|
||
| // /** | ||
| // * Destructor. | ||
| // */ | ||
| // virtual ~UkfMeasurement(); |
There was a problem hiding this comment.
You can remove these lines
Comment on lines
97
to
100
| * | ||
| * UkfMeasurement.ini | ||
| * | ||
| * dynamics_list ("JOINT_VELOCITY", "MOTOR_CURRENT", "RIGHT_LEG_FT", "RIGHT_FOOT_REAR_GYRO") |
There was a problem hiding this comment.
Suggested change
| * | |
| * UkfMeasurement.ini | |
| * | |
| * dynamics_list ("JOINT_VELOCITY", "MOTOR_CURRENT", "RIGHT_LEG_FT", "RIGHT_FOOT_REAR_GYRO") | |
| * \code{.ini} | |
| * # UkfMeasurement.ini | |
| * | |
| * dynamics_list ("JOINT_VELOCITY", "MOTOR_CURRENT", "RIGHT_LEG_FT", "RIGHT_FOOT_REAR_GYRO") |
| * covariance (8.2e-8, 1e-2, 9.3e-3) | ||
| * dynamic_model "ConstantMeasurementModel" | ||
| * | ||
| * ~~~~~ |
There was a problem hiding this comment.
Suggested change
| * ~~~~~ | |
| * \endcode |
| * @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(); |
There was a problem hiding this comment.
Can make this function const? returning a const ref
src/Estimators/src/UkfState.cpp
Outdated
Comment on lines
341
to
344
| Eigen::Ref<Eigen::MatrixXd> RDE::UkfState::getInitialStateCovarianceMatrix() | ||
| { | ||
| return m_initialCovariance; | ||
| } |
Comment on lines
20
to
33
| set(CUSTOM_MODEL_EXPECTED_MD5 33b903fe87939e2e4932564e45741047) | ||
| if (EXISTS "${CMAKE_CURRENT_BINARY_DIR}/model.urdf") | ||
| file(MD5 "${CMAKE_CURRENT_BINARY_DIR}/model.urdf" CUSTOM_MODEL_CHECKSUM_VARIABLE) | ||
| string(COMPARE EQUAL ${CUSTOM_MODEL_CHECKSUM_VARIABLE} ${CUSTOM_MODEL_EXPECTED_MD5} CUSTOM_MODEL_UPDATED) | ||
| else() | ||
| set(CUSTOM_MODEL_UPDATED FALSE) | ||
| endif() | ||
|
|
||
| add_bipedal_test(NAME SubModelCreator | ||
| SOURCES SubModelCreatorTest.cpp | ||
| LINKS BipedalLocomotion::RobotDynamicsEstimator | ||
| BipedalLocomotion::ParametersHandler | ||
| BipedalLocomotion::ParametersHandlerYarpImplementation | ||
| icub-models::icub-models) | ||
| endif() | ||
| if(NOT ${CUSTOM_MODEL_UPDATED}) | ||
| message(STATUS "Fetching CUSTOM model from github.com/ami-iit/paper_sorrentino_2024_icra_robot-dynamics-estimation/...") | ||
| file(DOWNLOAD https://raw.githubusercontent.com/ami-iit/paper_sorrentino_2024_icra_robot-dynamics-estimation/577c9d734ff0cb7128274b3880106da11678538c/urdf/four_joints_two_sensors.urdf | ||
| ${CMAKE_CURRENT_BINARY_DIR}/model.urdf | ||
| EXPECTED_MD5 ${CUSTOM_MODEL_EXPECTED_MD5}) | ||
| endif() |
There was a problem hiding this comment.
You should do this only if test is enabled
| sampling_time 0.01 | ||
|
|
||
| [include MODEL "model.ini"] | ||
|
|
||
| [include UKF "ukf.ini"] | ||
| [include UKF "ukf.ini"] |
There was a problem hiding this comment.
Suggested change
| [include UKF "ukf.ini"] | |
| [include UKF "ukf.ini"] | |
|
|
||
| covariance (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2) | ||
| initial_covariance (1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2) | ||
| dynamic_model "ZeroVelocityStateDynamics" |
There was a problem hiding this comment.
Suggested change
| dynamic_model "ZeroVelocityStateDynamics" | |
| dynamic_model "ZeroVelocityStateDynamics" | |
| @@ -40,6 +40,7 @@ TEST_CASE("MANNTrajectoryGenerator") | |||
|
|
|||
| iDynTree::ModelLoader ml; | |||
| REQUIRE(ml.loadReducedModelFromFile(getRobotModelPath(), jointsList)); | |||
| BipedalLocomotion::log()->info("{}", getRobotModelPath()); | |||
|
@GiulioRomualdi Changes applied. |
GiulioRomualdi
approved these changes
Nov 8, 2023
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
This branch is rebased on https://github.com/isorrentino/bipedal-locomotion-framework/tree/RDE/PR3. I convert the PR to draft until #743 is merged.