Add dynamic models composing the UKF process model #615

isorrentino · 2023-03-02T11:48:39Z

This PR implements some state dynamics which can be used to build a process model for a UKF.

GiulioRomualdi · 2023-03-02T16:44:36Z

Can you rebase the PR on top of master and remove all the commits already merged in past PR?

isorrentino · 2023-03-03T11:09:30Z

Hi @GiulioRomualdi @S-Dafarra I noticed that the measurement and state dynamics classes are exactly the same. I put the PR in draft in order to change the StateDynamics class name into Dynamics and apply all the needed modifications. I'll ping you as soon as the modifications are pushed and the PR is again ready to be reviewed.

isorrentino · 2023-03-03T11:29:05Z

Hi @GiulioRomualdi @S-Dafarra I noticed that the measurement and state dynamics classes are exactly the same. I put the PR in draft in order to change the StateDynamics class name into Dynamics and apply all the needed modifications. I'll ping you as soon as the modifications are pushed and the PR is again ready to be reviewed.

Changes took less than expected. Changing back to ready.

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

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

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

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

src/Estimators/tests/RobotDynamicsEstimator/UKFModelCreatorTest.cpp

src/Estimators/tests/RobotDynamicsEstimator/FrictionTorqueDynamicsTest.cpp

src/Estimators/tests/RobotDynamicsEstimator/JointVelocityDynamicsTest.cpp

src/Estimators/tests/RobotDynamicsEstimator/ZeroVelocityDynamicsTest.cpp

isorrentino · 2023-03-16T10:59:43Z

Hi @GiulioRomualdi and @S-Dafarra, I modified most of the code because using it I noticed that some things were missing or were uncomfortable to use. I also added some missing tests. I'm going to apply the reviews made by Stefano, but you will probably have to do the review again.

isorrentino · 2023-03-16T14:07:43Z

Sounds good @isorrentino, when you are ready you can re-request the review

I requested the review again but I found a problem and I'm trying to fix it. Let's wait for the review. Thanks. Maybe I can put the PR in draft until I solve the issue.

isorrentino · 2023-03-24T12:29:44Z

Hi @S-Dafarra and @GiulioRomualdi, this PR can be reviewed again. Thank you.

GiulioRomualdi · 2023-03-29T08:28:41Z

I rebased the PR on master. Now the CI should run smoothly

…ss model, the ukfstate model class.

isorrentino · 2023-03-30T09:32:35Z

Friendly ping @S-Dafarra @GiulioRomualdi :)

S-Dafarra

Some preliminary comments. I haven't gone through all the files yet

S-Dafarra · 2023-04-03T14:13:43Z

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

+ */
+#define BLF_REGISTER_DYNAMICS(_model, _baseModel)               \
+    static std::shared_ptr<_baseModel> _model##FactoryBuilder() \
+{                                                               \


Indentation problem

S-Dafarra · 2023-04-03T14:17:46Z

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

+ * @param _model the model of the dynamics
+ * @param _baseModel the base model from which the _model inherits.
+ */
+#define BLF_REGISTER_DYNAMICS(_model, _baseModel)               \


The term "dynamics" is quite abused and omnipresent. While it is safe to have a class named Dynamics in a specific namespace, this macro has to be in the root namespace. I would suggest using a more specific name, maybe adding an ESTIMATOR keyword

S-Dafarra · 2023-04-03T14:18:31Z

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

+{
+
+/**
+ * @brief The UKFInput struct represents the input of the ukf needed to update the dynamics.


Suggested change

* @brief The UKFInput struct represents the input of the ukf needed to update the dynamics.

* @brief The UKFInput struct represents the input of the ukf needed to update the robot dynamics.

S-Dafarra · 2023-04-03T14:20:29Z

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

+/**
+ * UkfInputProvider describes the provider for the inputs of the ukf
+ */
+class UkfInputProvider : public System::Advanceable<UKFInput, UKFInput>


I would suggest adding more details on what this class is needed for. From the documentation only I was not able to understand it.

S-Dafarra · 2023-04-03T14:25:13Z

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

+    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<std::string> 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/measurement vector. */
+    bool m_isStateVariableHandlerSet{false}; /**< True if setVariableHandler is called. */
+    UKFInput m_ukfInput;


Looking at the header only, I find a little suspicious to have all these as protected members. Is it ok for the derived classes to access all these data. I would suggest eventually to keep the data as private as possible, and use protected setters and getters to structure the access to this data.

Then, looking at the implementation, I noticed that almost none of this data is used in the base class. I would suggest leaving the handling of the private attributes directly to the derived class, rather than forcing all the derived classes using some specific variables.

S-Dafarra · 2023-04-03T14:29:41Z

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

+     * Set the SubModelKinDynWrapper object.
+     * @param kinDynWrapper pointer to a SubModelKinDynWrapper object.
+     * @return True in case of success, false otherwise.


Missing documentation for the second input

S-Dafarra · 2023-04-03T14:34:38Z

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

+    /**
+     * Set the SubModelKinDynWrapper object.
+     * @param kinDynWrapper pointer to a SubModelKinDynWrapper object.
+     * @return True in case of success, false otherwise.
+     */
+    bool setSubModels(const std::vector<SubModel>& subModelList, const std::vector<std::shared_ptr<SubModelKinDynWrapper>>& kinDynWrapperList) override;


Same as above, missing doc for the second entry

S-Dafarra · 2023-04-03T14:41:44Z

src/Estimators/src/FrictionTorqueStateDynamics.cpp

+    }
+
+    // Set the dynamic model type
+    if (!ptr->getParameter("dynamic_model", m_dynamicModel))


Is this used anywhere?

S-Dafarra

I should have checked all the files now 😊

S-Dafarra · 2023-04-03T15:36:29Z

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

+     * Set the SubModelKinDynWrapper object.
+     * @param kinDynWrapper pointer to a SubModelKinDynWrapper object.
+     * @return True in case of success, false otherwise.
+     */


As for the others

S-Dafarra · 2023-04-03T15:38:12Z

src/Estimators/src/JointVelocityStateDynamics.cpp

+    if (!ptr->getParameter("dynamic_model", m_dynamicModel))
+    {
+        log()->error("{} Error while retrieving the dynamic_model variable.", errorPrefix);
+        return false;
+    }


S-Dafarra · 2023-04-03T15:42:06Z

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

+namespace RobotDynamicsEstimator
+{
+
+struct SubModelDynamics


I would suggest converting this to a class with the proper setters/getters

S-Dafarra · 2023-04-03T15:46:26Z

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

+    /**
+     * Set the SubModelKinDynWrapper object.
+     * @param kinDynWrapper pointer to a SubModelKinDynWrapper object.
+     * @return True in case of success, false otherwise.
+     */
+    bool setSubModels(const std::vector<SubModel>& subModelList, const std::vector<std::shared_ptr<SubModelKinDynWrapper>>& kinDynWrapperList) override;


S-Dafarra · 2023-04-03T15:49:31Z

src/Estimators/src/ZeroVelocityDynamics.cpp

+    return true;
+}
+
+bool RDE::ZeroVelocityDynamics::setSubModels(const std::vector<RDE::SubModel>& subModelList, const std::vector<std::shared_ptr<RDE::SubModelKinDynWrapper>>& kinDynWrapperList)


Suggested change

bool RDE::ZeroVelocityDynamics::setSubModels(const std::vector<RDE::SubModel>& subModelList, const std::vector<std::shared_ptr<RDE::SubModelKinDynWrapper>>& kinDynWrapperList)

bool RDE::ZeroVelocityDynamics::setSubModels(const std::vector<RDE::SubModel>& /*subModelList*/, const std::vector<std::shared_ptr<RDE::SubModelKinDynWrapper>>& /*kinDynWrapperList*/)

When a parameter is not used, the compiler might generate warnings. In these cases it is sufficient to comment the name, thus indicating to the compiler you are not going to use them.

S-Dafarra · 2023-04-03T15:49:59Z

src/Estimators/src/ZeroVelocityDynamics.cpp

+    }
+}
+
+void RDE::ZeroVelocityDynamics::setInput(const UKFInput& ukfInput)


Also here, you could comment out the name of the parameter not used.

S-Dafarra · 2023-04-03T15:57:53Z

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

+    System::VariablesHandler getStateVariableHandler();
+
+    /**
+     * @brief propagate implements the predict of the ukf


Suggested change

* @brief propagate implements the predict of the ukf

* @brief propagate implements the prediction phase of the ukf

src/Estimators/src/UkfState.cpp

S-Dafarra · 2023-04-03T16:44:21Z

src/Estimators/tests/RobotDynamicsEstimator/UkfStateTest.cpp

+    REQUIRE(inputProvider->setInput(input));
+
+    stateModel->propagate(currentState, updatedState);
+
+    for (int idx = 0; idx < updatedState.size(); idx++)
+    {
+        REQUIRE(std::abs(updatedState[idx]) < 200);
+    }
+}


I would also suggest defining a test case where you expect some specific output, and check that you converge there. You could generate in some way some simple synthetic data (i.e. a set of constant joint torques), and check that the estimator converges to the quantities you used to generate the data (i.e. you check that you are able to reconstruct the initial set of joint torques)

isorrentino · 2023-04-26T15:51:54Z

Hi @GiulioRomualdi @S-Dafarra, I'm finally back to this PR. I know that @S-Dafarra has already spent time on the review, but I thought it might make more sense to close the 4 PRs containing the code of the estimator (#615, #640, #646, #649) and open many much smaller ones to simplify and speed up the review process. Consider also that the code of the last PR (#649) changes also many parts of the code contained in the previous three PRs. What do you think?

S-Dafarra · 2023-04-26T16:04:19Z

Hi @GiulioRomualdi @S-Dafarra, I'm finally back to this PR. I know that @S-Dafarra has already spent time on the review, but I thought it might make more sense to close the 4 PRs containing the code of the estimator (#615, #640, #646, #649) and open many much smaller ones to simplify and speed up the review process. Consider also that the code of the last PR (#649) changes also many parts of the code contained in the previous three PRs. What do you think?

Clearly, smaller PRs are much easier to review, but anyway there is always a tradeoff with the development time. Up to you! I did not check the other PRs yet. If you close this, I would just ask you to consider my previous comments in the new PRs before asking me to review them (only if they still apply) 😊

isorrentino · 2023-04-26T16:07:27Z

If you close this, I would just ask you to consider my previous comments in the new PRs before asking me to review them (only if they still apply)

Of course, I would have not ignored your comments :) Actually, since the second PR is very similar to this one, they will apply also to that one :D

isorrentino · 2023-04-29T10:04:31Z

As mentioned in #615 (comment), I'm going to close this PR without merging it.

isorrentino requested review from GiulioRomualdi and S-Dafarra as code owners March 2, 2023 11:48

isorrentino self-assigned this Mar 2, 2023

github-actions bot added the 🔨 component: Estimators label Mar 2, 2023

isorrentino marked this pull request as draft March 2, 2023 11:54

isorrentino force-pushed the feature/StateDynamics branch 4 times, most recently from af4b6f5 to 66d8547 Compare March 2, 2023 13:54

isorrentino marked this pull request as ready for review March 2, 2023 15:29

isorrentino force-pushed the feature/StateDynamics branch 2 times, most recently from 2536f32 to 1aae054 Compare March 2, 2023 18:58

isorrentino marked this pull request as draft March 3, 2023 11:09

isorrentino force-pushed the feature/StateDynamics branch from 1aae054 to e721bea Compare March 3, 2023 11:28

isorrentino marked this pull request as ready for review March 3, 2023 11:29

isorrentino force-pushed the feature/StateDynamics branch 2 times, most recently from 5cb64d0 to 442173a Compare March 3, 2023 11:33

S-Dafarra reviewed Mar 6, 2023

View reviewed changes

isorrentino force-pushed the feature/StateDynamics branch from a547ac4 to 89c4b8a Compare March 16, 2023 10:19

github-actions bot added 🔨 component: IK 🔨 component: System 🐍 bindings labels Mar 16, 2023

isorrentino force-pushed the feature/StateDynamics branch from 89c4b8a to 8381122 Compare March 16, 2023 10:56

github-actions bot removed 🔨 component: IK 🐍 bindings 🔨 component: System labels Mar 16, 2023

isorrentino force-pushed the feature/StateDynamics branch from c63afdd to 084fe1a Compare March 16, 2023 13:18

isorrentino force-pushed the feature/StateDynamics branch from 7588848 to 7fb5dc5 Compare March 22, 2023 17:38

github-actions bot removed the 🔨 component: Contacts label Mar 22, 2023

isorrentino force-pushed the feature/StateDynamics branch 7 times, most recently from e86444e to adbca58 Compare March 24, 2023 12:26

GiulioRomualdi force-pushed the feature/StateDynamics branch from adbca58 to 625d04e Compare March 29, 2023 08:28

isorrentino added 2 commits March 29, 2023 12:27

Implement the Dynamics base class, the dynamics classes for the proce…

39d981f

…ss model, the ukfstate model class.

Update CHANGELOG

a193884

isorrentino force-pushed the feature/StateDynamics branch from 625d04e to a193884 Compare March 29, 2023 10:27

S-Dafarra reviewed Apr 3, 2023

View reviewed changes

isorrentino closed this Apr 29, 2023

This was referenced Apr 29, 2023

Add dynamic models composing the UKF measurement model #640

Closed

Implement RobotDynamicsEstimator #646

Closed

Implement bindings for RobotDynamicsEstimator #649

Closed

Implement robot dynamics estimator - PR1 #661

Merged

isorrentino deleted the feature/StateDynamics branch September 25, 2023 12:03

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add dynamic models composing the UKF process model #615

Add dynamic models composing the UKF process model #615

isorrentino commented Mar 2, 2023 •

edited

Loading

GiulioRomualdi commented Mar 2, 2023 •

edited

Loading

isorrentino commented Mar 3, 2023

isorrentino commented Mar 3, 2023

isorrentino commented Mar 16, 2023

isorrentino commented Mar 16, 2023 •

edited

Loading

isorrentino commented Mar 24, 2023

GiulioRomualdi commented Mar 29, 2023

isorrentino commented Mar 30, 2023

S-Dafarra left a comment

S-Dafarra Apr 3, 2023

S-Dafarra Apr 3, 2023

S-Dafarra Apr 3, 2023

S-Dafarra Apr 3, 2023

S-Dafarra Apr 3, 2023

S-Dafarra Apr 3, 2023

S-Dafarra Apr 3, 2023

S-Dafarra Apr 3, 2023

S-Dafarra left a comment

S-Dafarra Apr 3, 2023

S-Dafarra Apr 3, 2023

S-Dafarra Apr 3, 2023

S-Dafarra Apr 3, 2023

S-Dafarra Apr 3, 2023

S-Dafarra Apr 3, 2023

S-Dafarra Apr 3, 2023

S-Dafarra Apr 3, 2023

isorrentino commented Apr 26, 2023

S-Dafarra commented Apr 26, 2023

isorrentino commented Apr 26, 2023

isorrentino commented Apr 29, 2023

	* @brief The UKFInput struct represents the input of the ukf needed to update the dynamics.
	* @brief The UKFInput struct represents the input of the ukf needed to update the robot dynamics.

	bool RDE::ZeroVelocityDynamics::setSubModels(const std::vector<RDE::SubModel>& subModelList, const std::vector<std::shared_ptr<RDE::SubModelKinDynWrapper>>& kinDynWrapperList)
	bool RDE::ZeroVelocityDynamics::setSubModels(const std::vector<RDE::SubModel>& /subModelList/, const std::vector<std::shared_ptr<RDE::SubModelKinDynWrapper>>& /kinDynWrapperList/)

	* @brief propagate implements the predict of the ukf
	* @brief propagate implements the prediction phase of the ukf

Add dynamic models composing the UKF process model #615

Add dynamic models composing the UKF process model #615

Conversation

isorrentino commented Mar 2, 2023 • edited Loading

GiulioRomualdi commented Mar 2, 2023 • edited Loading

isorrentino commented Mar 3, 2023

isorrentino commented Mar 3, 2023

isorrentino commented Mar 16, 2023

isorrentino commented Mar 16, 2023 • edited Loading

isorrentino commented Mar 24, 2023

GiulioRomualdi commented Mar 29, 2023

isorrentino commented Mar 30, 2023

S-Dafarra left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

S-Dafarra left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

isorrentino commented Apr 26, 2023

S-Dafarra commented Apr 26, 2023

isorrentino commented Apr 26, 2023

isorrentino commented Apr 29, 2023

isorrentino commented Mar 2, 2023 •

edited

Loading

GiulioRomualdi commented Mar 2, 2023 •

edited

Loading

isorrentino commented Mar 16, 2023 •

edited

Loading