Add accelerometer and gyroscope dynamic models #666
Merged
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
force-pushed
the
mergeRobotDynamicsEstim/pr3
branch
from
7e6d846
to
f721f26
Compare
| m_JvdotBase = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerJacobian(m_name).block(0, 0, 6, 6) * | ||
| m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getBaseAcceleration().coeffs(); | ||
|
|
||
| m_accRg = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerRotation(m_name).rotation() * m_gravity; |
There was a problem hiding this comment.
Suggested change
| m_accRg = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerRotation(m_name).rotation() * m_gravity; | |
| m_accRg = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerRotation(m_name).act(m_gravity); |
Comment on lines
187
to
188
| 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); |
There was a problem hiding this comment.
you can use manif function instead of segment
| { | ||
| m_JdotNu = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerBiasAcceleration(m_name); | ||
|
|
||
| m_JvdotBase = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerJacobian(m_name).block(0, 0, 6, 6) * |
There was a problem hiding this comment.
Suggested change
| m_JvdotBase = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerJacobian(m_name).block(0, 0, 6, 6) * | |
| m_JvdotBase = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerJacobian(m_name).leftCols<6>() * |
Comment on lines
201
to
202
| m_Jsdotdot = m_kinDynWrapperList[m_subModelsWithAccelerometer[index]]->getAccelerometerJacobian(m_name). | ||
| block(0, 6, 6, m_subModelJointAcc[m_subModelsWithAccelerometer[index]].size()) * |
There was a problem hiding this comment.
Also here you may use a smarter block operation: https://eigen.tuxfamily.org/dox/group__TutorialBlockOperations.html
| 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; |
There was a problem hiding this comment.
Suggested change
| m_covariances.segment(index*m_covSingleVar.size(), m_covSingleVar.size()) = m_covSingleVar; | |
| m_covariances.segment(index * m_covSingleVar.size(), m_covSingleVar.size()) = m_covSingleVar; |
Comment on lines
32
to
58
| 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. */ | ||
| int m_nrOfSubDynamics; /**< Number of sub-dynamics which corresponds to the number of sub-models. */ | ||
| std::vector<std::shared_ptr<SubModelKinDynWrapper>> m_subModelKinDynList; /**< Vector of SubModelKinDynWrapper objects. */ | ||
| std::vector<SubModel> m_subModelList; /**< Vector of SubModel objects. */ | ||
| std::vector<Eigen::VectorXd> m_subModelJointVel; /**< Updated joint velocities of each sub-model. */ | ||
| std::vector<std::size_t> 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. */ | ||
| UKFInput m_ukfInput; /**< Input of the UKF used to update the dynamics. */ | ||
| std::string m_name; /**< Name of dynamics. */ | ||
| std::vector<std::string> m_elements = {}; /**< Elements composing the variable vector. */ | ||
| System::VariablesHandler m_stateVariableHandler; /**< Variable handler describing the variables and the sizes in the ukf state vector. */ | ||
|
|
||
| Eigen::VectorXd m_covSingleVar; /**< Covariance of the gyroscope measurement from configuration. */ | ||
| manif::SE3d::Tangent m_subModelBaseVel; /**< Submodel base velocity. */ | ||
| Eigen::VectorXd m_JvBase; /**< Jacobian times base velocity. */ | ||
| Eigen::VectorXd m_Jsdot; /**< Jacobian times joint velocity. */ |
There was a problem hiding this comment.
Could yu please call clang-format in this part?
|
|
||
| std::vector<std::string> jointsAndFTs; | ||
| jointsAndFTs.insert(jointsAndFTs.begin(), jointList.begin(), jointList.end()); | ||
| // jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); |
There was a problem hiding this comment.
Suggested change
| // jointsAndFTs.insert(jointsAndFTs.end(), ftFramesList.begin(), ftFramesList.end()); |
| @@ -52,6 +52,28 @@ if(FRAMEWORK_USE_icub-models AND FRAMEWORK_COMPILE_YarpImplementation) | |||
| MANIF::manif | |||
| BipedalLocomotion::ManifConversions) | |||
|
|
|||
| add_bipedal_test(NAME AccelerometerMeasurementDynamics | |||
There was a problem hiding this comment.
Suggested change
| add_bipedal_test(NAME AccelerometerMeasurementDynamics | |
| add_bipedal_test(NAME AccelerometerMeasurementDynamics |
| @@ -0,0 +1,236 @@ | |||
| /** | |||
| @@ -0,0 +1,235 @@ | |||
| /** | |||
isorrentino
force-pushed
the
mergeRobotDynamicsEstim/pr3
branch
from
f721f26
to
af0923f
Compare
GiulioRomualdi
approved these changes
May 6, 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.
The PR implements two classes
AccelerometerMeasurementDynamicsandGyroscopeMeasurementDynamicsdescribing the measurement models for the accelerometer and the gyroscope sensors.