Add usefull methods to SubModel and SubModelKinDynWrapper classes.

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<Sensor>& ftSensorList,
+ bool createSubModels(const std::vector<FT>& ftSensorList,
  const std::vector<Sensor>& accList,
  const std::vector<Sensor>& gyroList,
  const std::vector<std::string>& externalContacts);

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

@@ -53,11 +53,15 @@ class SubModelKinDynWrapper
  */
  std::map<const std::string, manif::SO3d> m_accRworldList; /**< Rotation matrix of the
  accelerometer frame wrt world */
+ std::map<const std::string, manif::SE3d::Tangent> 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<iDynTree::KinDynComputations> 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<const Eigen::VectorXd> 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<const Eigen::VectorXd> 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

src/Estimators/src/SubModel.cpp

@@ -257,23 +257,23 @@ RDE::SubModelCreator::populateSubModel(iDynTree::Model& idynSubModel,
  return subModel;
 }
 
-bool RDE::SubModelCreator::createSubModels(const std::vector<RDE::Sensor>& ftSensorList,
+bool RDE::SubModelCreator::createSubModels(const std::vector<RDE::FT>& ftSensorList,
  const std::vector<RDE::Sensor>& accList,
  const std::vector<RDE::Sensor>& gyroList,
  const std::vector<std::string>& externalContacts)
 {
  constexpr auto logPrefix = "[BipedalLocomotion::RobotDynamicsEstimator::SubModelCreator::"
- "getSubModels]";
+ "createSubModels]";
 
  // Split model in submodels
- std::vector<std::string> ftNameList;
+ std::vector<std::string> ftList;
  for (auto idx = 0; idx < ftSensorList.size(); idx++)
  {
- ftNameList.push_back(ftSensorList[idx].frame);
+ ftList.push_back(ftSensorList[idx].associatedJoint);
  }
 
  std::vector<iDynTree::Model> 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<std::string>& names,
-  std::vector<std::string>& frames) -> bool {
+ std::vector<std::string>& names,
+ std::vector<std::string>& frames) -> bool {
  auto group = ptr->getGroup(groupName).lock();
  if (group == nullptr)
  {
@@ -362,15 +362,24 @@ bool RDE::SubModelCreator::createSubModels(
  logPrefix);
  }
 
- std::vector<std::string> ftNames, ftFrames;
+ std::vector<std::string> ftNames, ftFrames, ftAssociatedJoints;
  bool ok = populateSensorParameters("FT", ftNames, ftFrames);
 
- std::vector<RDE::Sensor> 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<RDE::FT> 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);