Add SubModelKinDynWrapper class #605

isorrentino · 2023-02-13T08:25:27Z

Adding the SubModelKinDynWrapper class. It handles the KinDynComputation object of a sub-model and keeps updated the dynamics and kinematics information.

This PR is blocked by #604.

S-Dafarra · 2023-02-13T08:58:19Z

Please, ping me again after #604 is merged and this is ready to be reviewed. Now I can see both PRs code and it is confusing 😊

isorrentino · 2023-02-20T15:11:17Z

Hi @S-Dafarra @GiulioRomualdi, the PR (#604) blocking this one has been merged. It is ready for review.

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

src/Estimators/src/SubModelKinDynWrapper.cpp

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

src/Estimators/src/SubModelKinDynWrapper.cpp

S-Dafarra · 2023-02-20T15:58:08Z

src/Estimators/tests/RobotDynamicsEstimator/SubModelKinDynWrapperTest.cpp

+            Eigen::VectorXd motorTorques(numberOfJoints);
+            Eigen::VectorXd frictionTorques(numberOfJoints);
+            Eigen::VectorXd contactTorques(numberOfJoints);
+
+            Eigen::VectorXd baseAcceleration(6);
+            REQUIRE(kinDynSubModel.getBaseAcceleration(kinDyn,
+                                                       robotBaseAcceleration,
+                                                       robotJointAcceleration,
+                                                       baseAcceleration));
+
+            Eigen::VectorXd baseVelocity(6);
+            baseVelocity = kinDynSubModel.getBaseVelocity(kinDyn);
+
+            Eigen::VectorXd jointAcceleration(numberOfJoints);
+
+            for (int i = 0; i < numberOfJoints; i++)
+            {
+                motorTorques(i) = random_double();
+                frictionTorques(i) = random_double();
+                contactTorques(i) = random_double();
+            }
+
+            REQUIRE(kinDynSubModel.inverseDynamics(motorTorques,
+                                                   frictionTorques,
+                                                   contactTorques,
+                                                   baseAcceleration,
+                                                   jointAcceleration));


Does it make sense to run these tests with meaningful inputs to check if the output makes sense?

GiulioRomualdi · 2023-02-20T19:04:10Z

Tomorrow I will do a better review, but I noticed that you are using idyntree matrices and vectors in the interface can you use eigen or manif?

isorrentino · 2023-02-21T11:23:37Z

@GiulioRomualdi @S-Dafarra I changed the code after your reviews. It's ready to be checked again. Thanks.

src/Estimators/src/SubModelKinDynWrapper.cpp

S-Dafarra

I just noticed a couple of other dynamic memory allocations. Apologies for not noticing it before.

src/Estimators/src/SubModelKinDynWrapper.cpp

S-Dafarra

Minor comment

src/Estimators/src/SubModelKinDynWrapper.cpp

GiulioRomualdi

In general, you may avoid using iDynTree::Vector and Matrices since all the KinDynComputations get and set methods take as input iDynTree::Span and iDynTree::MatrixView

GiulioRomualdi · 2023-02-24T14:59:22Z

src/Estimators/src/SubModelKinDynWrapper.cpp

+    for (int idx = 0; idx < m_subModel.getJointMapping().size(); idx++)
+    {
+        m_qj.setVal(idx, m_jointPositionModel[m_subModel.getJointMapping()[idx]]);
+        m_dqj.setVal(idx, m_jointVelocityModel[m_subModel.getJointMapping()[idx]]);
+    }
+
+    if (!m_kinDyn.setRobotState(m_worldTBase, m_qj, m_baseVelocityiDyn, m_dqj, m_worldGravity))
+    {
+        blf::log()->error("{} Unable to set the robot state.", logPrefix);
+        return false;
+    }


You can use directly eigen vectors here

GiulioRomualdi · 2023-02-24T15:00:01Z

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

+    bool inverseDynamics(Eigen::VectorXd& motorTorqueAfterGearbox,
+                         Eigen::VectorXd& frictionTorques,
+                         Eigen::VectorXd& tauExt,
+                         Eigen::VectorXd& baseAcceleration,
+                         Eigen::VectorXd& jointAcceleration);


you can use the eigen ref

GiulioRomualdi · 2023-02-24T15:00:11Z

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

+                             const Eigen::VectorXd& robotBaseAcceleration,
+                             const Eigen::VectorXd& robotJointAcceleration,


GiulioRomualdi · 2023-02-24T15:00:42Z

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

+     * @param kinDynFullModel is the KinDynComputation object associated to a model.
+     * @return baseVelocity the the base velocity of the sub-model.
+     */
+    Eigen::Ref<Eigen::VectorXd> getBaseVelocity(std::shared_ptr<iDynTree::KinDynComputations> kinDynFullModel);


This gives the user the possibility to change the base velocity is this wanted?

Suggested change

Eigen::Ref<Eigen::VectorXd> getBaseVelocity(std::shared_ptr<iDynTree::KinDynComputations> kinDynFullModel);

Eigen::Ref<const Eigen::VectorXd> getBaseVelocity(std::shared_ptr<iDynTree::KinDynComputations> kinDynFullModel) const;

Actually this should be a manif::SE3d::Tangent object

GiulioRomualdi · 2023-02-24T15:01:16Z

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

+#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_SUBMODELKINDYNWRAPPER_H
+#define BIPEDAL_LOCOMOTION_ESTIMATORS_SUBMODELKINDYNWRAPPER_H


Suggested change

#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_SUBMODELKINDYNWRAPPER_H

#define BIPEDAL_LOCOMOTION_ESTIMATORS_SUBMODELKINDYNWRAPPER_H

#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_SUB_MODEL_KINDYN_WRAPPER_H

#define BIPEDAL_LOCOMOTION_ESTIMATORS_SUB_MODEL_KINDYN_WRAPPER_H

GiulioRomualdi · 2023-02-24T15:02:38Z

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

+    std::map<const std::string, Eigen::VectorXd> m_dJnuList; /**< Accelerometer bias accelerations */
+    std::map<const std::string, Eigen::Matrix3d> m_accRworldList; /**< Rotation matrix of the
+                                                                 accelerometer frame wrt world */
+    Eigen::VectorXd m_baseVelocity; /**< Velocity of the base of the sub-model */


We use manif::SE3Tangentd for the velocity

GiulioRomualdi · 2023-02-24T15:03:01Z

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

+    std::map<const std::string, Eigen::MatrixXd> m_jacContactList; /**< Jacobians of the external
+                                                                      contacts */
+    std::map<const std::string, Eigen::VectorXd> m_dJnuList; /**< Accelerometer bias accelerations */
+    std::map<const std::string, Eigen::Matrix3d> m_accRworldList; /**< Rotation matrix of the


Suggested change

std::map<const std::string, Eigen::Matrix3d> m_accRworldList; /**< Rotation matrix of the

std::map<const std::string, manif::SO3d> m_accRworldList; /**< Rotation matrix of the

GiulioRomualdi · 2023-02-24T15:04:49Z

src/Estimators/src/SubModelKinDynWrapper.cpp

+    if (!m_kinDyn.getFreeFloatingMassMatrix(m_idynMassMatrix))
+    {
+        blf::log()->error("{} Unable to get the mass matrix of the sub-model.", logPrefix);
+        return false;
+    }
+    m_massMatrix = iDynTree::toEigen(m_idynMassMatrix);


Suggested change

if (!m_kinDyn.getFreeFloatingMassMatrix(m_idynMassMatrix))

{

blf::log()->error("{} Unable to get the mass matrix of the sub-model.", logPrefix);

return false;

}

m_massMatrix = iDynTree::toEigen(m_idynMassMatrix);

if (!m_kinDyn.getFreeFloatingMassMatrix(m_massMatrix))

{

blf::log()->error("{} Unable to get the mass matrix of the sub-model.", logPrefix);

return false;

}

GiulioRomualdi · 2023-02-24T15:05:50Z

src/Estimators/src/SubModelKinDynWrapper.cpp

+    m_FTBaseAcc.resize(m_numOfJoints);
+    m_totalTorques.resize(m_numOfJoints);
+
+    m_idynMassMatrix = iDynTree::FreeFloatingMassMatrix(m_subModel.getModel());


Suggested change

m_idynMassMatrix = iDynTree::FreeFloatingMassMatrix(m_subModel.getModel());

GiulioRomualdi · 2023-02-24T15:06:05Z

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

+    iDynTree::VectorDynSize m_qj;
+    iDynTree::VectorDynSize m_dqj;
+
+    iDynTree::FreeFloatingMassMatrix m_idynMassMatrix;


Suggested change

iDynTree::FreeFloatingMassMatrix m_idynMassMatrix;

isorrentino · 2023-02-26T07:45:36Z

@GiulioRomualdi I changed the code following your suggestions. I hope I got how to change it.

…information of a sub-model

isorrentino requested a review from GiulioRomualdi as a code owner February 13, 2023 08:25

isorrentino self-assigned this Feb 13, 2023

isorrentino requested a review from S-Dafarra as a code owner February 13, 2023 08:25

github-actions bot added the 🔨 component: Estimators label Feb 13, 2023

isorrentino force-pushed the feature/kinDynSubModel branch from a56a473 to ef6fc89 Compare February 13, 2023 13:56

isorrentino marked this pull request as draft February 14, 2023 08:43

isorrentino force-pushed the feature/kinDynSubModel branch 5 times, most recently from c19baa4 to 47c6b65 Compare February 20, 2023 15:02

isorrentino marked this pull request as ready for review February 20, 2023 15:09

S-Dafarra reviewed Feb 20, 2023

View reviewed changes

S-Dafarra reviewed Feb 21, 2023

View reviewed changes

src/Estimators/src/SubModelKinDynWrapper.cpp Outdated Show resolved Hide resolved

S-Dafarra reviewed Feb 21, 2023

View reviewed changes

src/Estimators/src/SubModelKinDynWrapper.cpp Outdated Show resolved Hide resolved

src/Estimators/src/SubModelKinDynWrapper.cpp Outdated Show resolved Hide resolved

isorrentino force-pushed the feature/kinDynSubModel branch 2 times, most recently from 1502d99 to 32cfd8c Compare February 21, 2023 13:59

S-Dafarra approved these changes Feb 21, 2023

View reviewed changes

src/Estimators/src/SubModelKinDynWrapper.cpp Outdated Show resolved Hide resolved

isorrentino force-pushed the feature/kinDynSubModel branch from 32cfd8c to a88121d Compare February 21, 2023 19:53

GiulioRomualdi reviewed Feb 24, 2023

View reviewed changes

isorrentino force-pushed the feature/kinDynSubModel branch 2 times, most recently from a9f94be to 1f2067f Compare February 26, 2023 07:04

isorrentino force-pushed the feature/kinDynSubModel branch 2 times, most recently from 1eb1edb to 6861db0 Compare February 26, 2023 08:41

GiulioRomualdi approved these changes Feb 28, 2023

View reviewed changes

isorrentino added 2 commits February 28, 2023 13:04

Add SubModelKinDynWrapper class handling the kinematics and dynamics …

2543a97

…information of a sub-model

Update CHANGELOG.md

9861bdb

GiulioRomualdi force-pushed the feature/kinDynSubModel branch from 6861db0 to 9861bdb Compare February 28, 2023 12:07

GiulioRomualdi merged commit 741c6b9 into ami-iit:master Feb 28, 2023

isorrentino deleted the feature/kinDynSubModel branch March 31, 2023 08:42

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add SubModelKinDynWrapper class #605

Add SubModelKinDynWrapper class #605

isorrentino commented Feb 13, 2023

S-Dafarra commented Feb 13, 2023

isorrentino commented Feb 20, 2023

S-Dafarra Feb 20, 2023

GiulioRomualdi commented Feb 20, 2023

isorrentino commented Feb 21, 2023

S-Dafarra left a comment

S-Dafarra left a comment

GiulioRomualdi left a comment

GiulioRomualdi Feb 24, 2023

GiulioRomualdi Feb 24, 2023

GiulioRomualdi Feb 24, 2023

GiulioRomualdi Feb 24, 2023

GiulioRomualdi Feb 24, 2023

GiulioRomualdi Feb 24, 2023

GiulioRomualdi Feb 24, 2023

GiulioRomualdi Feb 24, 2023

GiulioRomualdi Feb 24, 2023

GiulioRomualdi Feb 24, 2023

GiulioRomualdi Feb 24, 2023

isorrentino commented Feb 26, 2023 •

edited

Loading

		const Eigen::VectorXd& robotBaseAcceleration,
		const Eigen::VectorXd& robotJointAcceleration,

	Eigen::Ref<Eigen::VectorXd> getBaseVelocity(std::shared_ptr<iDynTree::KinDynComputations> kinDynFullModel);
	Eigen::Ref<const Eigen::VectorXd> getBaseVelocity(std::shared_ptr<iDynTree::KinDynComputations> kinDynFullModel) const;

		#ifndef BIPEDAL_LOCOMOTION_ESTIMATORS_SUBMODELKINDYNWRAPPER_H
		#define BIPEDAL_LOCOMOTION_ESTIMATORS_SUBMODELKINDYNWRAPPER_H

	std::map<const std::string, Eigen::Matrix3d> m_accRworldList; /**< Rotation matrix of the
	std::map<const std::string, manif::SO3d> m_accRworldList; /**< Rotation matrix of the

Add SubModelKinDynWrapper class #605

Add SubModelKinDynWrapper class #605

Conversation

isorrentino commented Feb 13, 2023

S-Dafarra commented Feb 13, 2023

isorrentino commented Feb 20, 2023

Choose a reason for hiding this comment

GiulioRomualdi commented Feb 20, 2023

isorrentino commented Feb 21, 2023

S-Dafarra left a comment

Choose a reason for hiding this comment

S-Dafarra left a comment

Choose a reason for hiding this comment

GiulioRomualdi 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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

isorrentino commented Feb 26, 2023 • edited Loading

isorrentino commented Feb 26, 2023 •

edited

Loading