diff --git a/CHANGELOG.md b/CHANGELOG.md index 643ae129bf..581ce3919a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,7 @@ All notable changes to this project are documented in this file. - Implement `ContactList::getNextContact` and `ContactList::getActiveContact` in `Contact` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/764) - Implement `MANN::generateDummyMANNOutput` and `MANN::generateDummyMANNInput` in `ML` component (https://github.com/ami-iit/bipedal-locomotion-framework/pull/771) - Add `MANNAutoregressive` and `MANNTrajectoryGenerator` examples (https://github.com/ami-iit/bipedal-locomotion-framework/pull/771) +- Add `System::TimeProfiler` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/826) ### Changed - Restructure of the `CentroidalMPC` class in `ReducedModelControllers` component. Specifically, the `CentroidalMPC` now provides a contact phase list instead of indicating the next active contact. Additionally, users can now switch between `ipopt` and `sqpmethod` to solve the optimization problem. Furthermore, the update allows for setting the warm-start for the non-linear solver. (https://github.com/ami-iit/bipedal-locomotion-framework/pull/766) diff --git a/bindings/python/YarpUtilities/src/VectorsCollection.cpp b/bindings/python/YarpUtilities/src/VectorsCollection.cpp index 7fbf32e03b..848b6b0019 100644 --- a/bindings/python/YarpUtilities/src/VectorsCollection.cpp +++ b/bindings/python/YarpUtilities/src/VectorsCollection.cpp @@ -7,11 +7,12 @@ #include #include +#include -#include +#include -#include #include +#include namespace BipedalLocomotion { @@ -19,19 +20,31 @@ namespace bindings { namespace YarpUtilities { -void CreateVectorsCollection(pybind11::module& module) +void CreateVectorsCollectionServer(pybind11::module& module) { namespace py = ::pybind11; using namespace ::BipedalLocomotion::YarpUtilities; - py::class_(module, "VectorsCollection") + py::class_(module, "VectorsCollectionServer") .def(py::init()) - .def_readwrite("vectors", &VectorsCollection::vectors) - .def("__repr__", &VectorsCollection::toString) - .def("to_string", &VectorsCollection::toString); - - CreateBufferedPort(module, "BufferedPortVectorsCollection"); + .def( + "initialize", + [](VectorsCollectionServer& impl, + std::shared_ptr + handler) -> bool { return impl.initialize(handler); }, + py::arg("handler")) + .def("populate_metadata", &VectorsCollectionServer::populateMetadata) + .def("finalize_metadata", &VectorsCollectionServer::finalizeMetadata) + .def("clear_data", &VectorsCollectionServer::clearData) + .def("send_data", &VectorsCollectionServer::sendData, py::arg("force_strict") = false) + .def("populate_data", + [](VectorsCollectionServer& impl, + const std::string& key, + Eigen::Ref data) -> bool { + return impl.populateData(key, data); + }) + .def("prepare_data", &VectorsCollectionServer::prepareData); } } // namespace YarpUtilities } // namespace bindings diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/all_joints_mc.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/all_joints_mc.xml new file mode 100644 index 0000000000..653344d6c9 --- /dev/null +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/all_joints_mc.xml @@ -0,0 +1,13 @@ + + + + ("/ergocubSim/head", "/ergocubSim/torso", "/ergocubSim/left_arm", "/ergocubSim/right_arm", "/ergocubSim/left_leg", "/ergocubSim/right_leg") + ("neck_pitch", "neck_roll", "neck_yaw", "camera_tilt", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_roll", "l_wrist_pitch", "l_wrist_yaw", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_roll", "r_wrist_pitch", "r_wrist_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + /yarp_robot_logger/joints + + + udp + + diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/ft_clients.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/ft_clients.xml new file mode 100644 index 0000000000..4c9e9179db --- /dev/null +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/ft_clients.xml @@ -0,0 +1,36 @@ + + + + + + + /ergocubSim/left_arm/FT + /yarp_robot_logger/left_arm + 0.5 + fast_tcp + + + + /ergocubSim/right_arm/FT + /yarp_robot_logger/right_arm + 0.5 + fast_tcp + + + + /ergocubSim/left_leg/FT + /yarp_robot_logger/left_leg + 0.5 + fast_tcp + + + + /ergocubSim/right_leg/FT + /yarp_robot_logger/right_leg + 0.5 + fast_tcp + + + diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/imu_clients.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/imu_clients.xml new file mode 100644 index 0000000000..8cf2a94a44 --- /dev/null +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/imu_clients.xml @@ -0,0 +1,22 @@ + + + + + + + /ergocubSim/left_foot_heel_tiptoe/imu + /yarp_robot_logger/left_foot/imu + 0.5 + fast_tcp + + + + /ergocubSim/right_foot_heel_tiptoe/imu + /yarp_robot_logger/right_foot/imu + 0.5 + fast_tcp + + + diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/mas-remapper.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/mas-remapper.xml new file mode 100644 index 0000000000..4226efde35 --- /dev/null +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/mas-remapper.xml @@ -0,0 +1,36 @@ + + + + + 10 + + (l_arm_ft, r_arm_ft, l_leg_ft, l_foot_front_ft, l_foot_rear_ft, r_leg_ft, r_foot_front_ft, r_foot_rear_ft) + + + (l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu) + + + (l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu) + + + (l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu) + + + (l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu) + + + + + left_arm_ft_client + right_arm_ft_client + left_leg_ft_client + right_leg_ft_client + left_foot_imu_client + right_foot_imu_client + + + + + diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/wrench_clients.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/wrench_clients.xml new file mode 100644 index 0000000000..3fef18f098 --- /dev/null +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/wrench_clients.xml @@ -0,0 +1,44 @@ + + + + + + + /wholeBodyDynamics/left_arm/cartesianEndEffectorWrench:o + /yarp_robot_logger/left_arm_wrench + udp + + + + /wholeBodyDynamics/right_arm/cartesianEndEffectorWrench:o + /yarp_robot_logger/right_arm_wrench + udp + + + + /wholeBodyDynamics/left_foot_front/cartesianEndEffectorWrench:o + /yarp_robot_logger/left_front_wrench + udp + + + + /wholeBodyDynamics/left_foot_rear/cartesianEndEffectorWrench:o + /yarp_robot_logger/left_rear_wrench + udp + + + + /wholeBodyDynamics/right_foot_front/cartesianEndEffectorWrench:o + /yarp_robot_logger/right_front_wrench + udp + + + + /wholeBodyDynamics/right_foot_rear/cartesianEndEffectorWrench:o + /yarp_robot_logger/right_rear_wrench + udp + + + diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/launch-yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/launch-yarp-robot-logger.xml new file mode 100644 index 0000000000..66ae83b6e0 --- /dev/null +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/launch-yarp-robot-logger.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml new file mode 100644 index 0000000000..f66bd5c55b --- /dev/null +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml @@ -0,0 +1,104 @@ + + + + + ergocubSim + 0.002 + (30) + mp4v + /yarp-robot-logger + ("ergoCubGazeboV1_1/yarprobotinterface") + + + 600.0 + + + + ("walking", "fee") + () + + + + + "/yarp-robot-logger/exogenous_signals/walking" + "/walking-coordinator/logger" + "walking" + "udp" + + + + "/yarp-robot-logger/exogenous_signals/fee" + "/fee-estimator/data" + "fee" + "udp" + + + + + + + + + false + + () + + + + + false + true + true + false + true + false + false + false + + + ("neck_pitch", "neck_roll", "neck_yaw", "camera_tilt", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_roll", "l_wrist_pitch", "l_wrist_yaw", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_roll", "r_wrist_pitch", "r_wrist_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") + + + + ("l_arm_ft", "r_arm_ft", "l_leg_ft", "l_foot_front_ft", "l_foot_rear_ft", "r_leg_ft", "r_foot_front_ft", "r_foot_rear_ft") + + + + (l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu) + (l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu) + (l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu) + (l_foot_front_ft_sensor_imu, l_foot_rear_ft_sensor_imu, r_foot_front_ft_sensor_imu, r_foot_rear_ft_sensor_imu) + + + + + + + + all_joints_mc + mas-remapper + + + + + + + + diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index 22abc4998d..25e043dd39 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -30,6 +30,13 @@ struct BaseEstimatorFromFootIMUState { manif::SE3d basePose; /**< final output of the estimator - pose of the robot root link. */ + manif::SE3d::Tangent baseVelocity; /**< velocity of the robot root link. */ + manif::SE3d footPose_L; /**< pose of the left foot */ + manif::SE3d footPose_R; /**< pose of the right foot */ + Eigen::Vector3d centerOfMassPosition; + std::vector stanceFootShadowCorners; + std::vector stanceFootCorners; + int supportCornerIndex; }; /** @@ -37,12 +44,20 @@ struct BaseEstimatorFromFootIMUState */ struct BaseEstimatorFromFootIMUInput { + bool isLeftStance; /**< true if the left foot is in contact with the ground */ + bool isRightStance; /**< true if the right foot is in contact with the ground */ Eigen::VectorXd jointPositions; /**< vector of the robot joint positions */ Eigen::VectorXd jointVelocities; /**< vector of the robot joint velocities */ - manif::SE3d desiredFootPose; /**< desired orientation and position of the foot - as per footstep planner output */ - manif::SO3d measuredRotation; /**< actual orientation of the foot measured by - on-board IMU */ + manif::SE3d offsetStanceFootPose; /**< Optional offset orientation and position of the foot. + E.g. as per footstep planner output */ + manif::SO3d measuredRotation_L; /**< actual orientation of the left foot measured by + on-board IMU */ + manif::SO3d measuredRotation_R; /**< actual orientation of the right foot measured by + on-board IMU */ + manif::SO3Tangentd measuredAngularVelocity_L; /**< actual angular velocity of the left foot + measured by on-board IMU */ + manif::SO3Tangentd measuredAngularVelocity_R; /**< actual angular velocity of the right foot + measured by on-board IMU */ }; /** @@ -50,13 +65,17 @@ struct BaseEstimatorFromFootIMUInput * kinematic chain given by the leg joints positions. * * This class assumes that the foot has a rectangular shape as shown in the following schematics - * + - * p2 __|__ p1 __ - * | | | | - * ___|__|__|___+ | FOOT - * | | | | LENGTH - * |__|__| __| - * p3 | p4 + * + * FRONT + * +X + * p1 __|__ p0 __ + * | | | | + * +Y___|__|__|___ | FOOT + * | | | | LENGTH + * |__|__| __| + * p2 | p3 + * + * HIND * * |_____| * FOOT @@ -77,7 +96,7 @@ class BaseEstimatorFromFootIMU // clang-format off /** * Initialize the BaseEstimatorFromFootIMU block. - * The setModel and setStanceFootRF methods must be called before initialization. + * The setModel method must be called before initialization. * @param handler pointer to the parameter handler. * @note The following parameters are required * | Parameter Name | Type | Description | Mandatory | @@ -120,6 +139,8 @@ class BaseEstimatorFromFootIMU */ const BaseEstimatorFromFootIMUState& getOutput() const override; + void resetBaseEstimatorFromFootIMU(); + /** * Set the state of the BaseEstimatorFromFootIMU. * @param state of the BaseEstimatorFromFootIMU @@ -139,50 +160,71 @@ class BaseEstimatorFromFootIMU /** * Define the 4 foot vertices in World reference frame: * - * + - * m_p2 __|__ m_p1 __ - * | | | | - * ___|__|__|___+ | FOOT - * | | | | LENGTH - * |__|__| __| - * m_p3 | m_p4 - * - * |_____| - * FOOT - * WIDTH + * +x FRONT + * m_p1 __|__ m_p0 __ + * | | | | + * +y___|__|__|___ | FOOT + * | | | | LENGTH + * |__|__| __| + * m_p2 | m_p3 + * HIND + * |_____| + * FOOT + * WIDTH * */ - std::vector m_cornersInLocalFrame; /**< this implementation is considering + std::vector m_cornersInInertialFrame; /**< this implementation is considering rectangular feet (4 corners) */ - std::vector m_transformedFootCorners; /**< vector of the foot corners + std::vector m_tiltedFootCorners; /**< vector of the foot corners transformed into the inertial frame */ - Eigen::Vector3d m_desiredTranslation; /**< the position vector extracted from the - `desiredFootPose` */ - Eigen::Matrix3d m_desiredRotation; /**< the rotation matrix extracted from the `desiredFootPose` - */ - Eigen::Vector3d m_desiredRPY; /**< the rotation matrix extracted from the `desiredFootPose` + Eigen::Vector3d m_offsetTranslation; /**< the position vector extracted from the + `offsetFootPose` */ + Eigen::Matrix3d m_offsetRotation; /**< the rotation matrix extracted from the `offsetFootPose` + */ + Eigen::Vector3d m_offsetRPY; /**< the rotation matrix extracted from the `offsetFootPose` converted into Euler angles */ Eigen::Matrix3d m_measuredRotation; /**< the measured foot orientation casted manif::SO3d --> Eigen::Matrix3d */ + manif::SO3Tangentd m_measuredAngularVelocity; /**< the measured stance foot angular velocity */ Eigen::Vector3d m_measuredRPY; /**< the measured foot orientation converted into Euler angles */ manif::SO3d m_measuredRotationCorrected; /**< rotation matrix that employs: measured Roll, - measured Pitch, desired Yaw */ + measured Pitch, offset Yaw */ + manif::SO3d m_offsetRotationCasted; + manif::SO3d m_measuredTilt; manif::SE3d m_measuredFootPose; /**< the final foot pose matrix obtained through measured and - desired quantities */ - manif::SE3d m_frame_H_link; /**< coordinate change matrix from foot link frame to foot sole frame - */ + offset quantities */ + manif::SE3d m_T_walk; + manif::SE3d m_T_yawDrift; + double m_yawOld; + manif::SE3d m_footFrame_H_link_L; /**< coordinate change matrix from left foot link frame to + * left foot sole frame + */ + manif::SE3d m_footFrame_H_link_R; /**< coordinate change matrix from right foot link frame to + * right foot sole frame + */ Eigen::Vector3d m_gravity; + const Eigen::Vector3d m_noTras{0.0, 0.0, 0.0}; iDynTree::KinDynComputations m_kinDyn; iDynTree::Model m_model; - iDynTree::LinkIndex m_linkIndex; - int m_frameIndex; - std::string m_frameName; - int m_baseFrame; /**< Index of the frame whose pose needs to be estimated */ - std::string m_footFrameName; /**< reference frames of the possible stance feet (whose - orientations are measured)*/ - bool m_isOutputValid{false}; - bool m_isModelSet{false}; + iDynTree::LinkIndex m_stanceLinkIndex; + + std::string m_baseFrameName; /**< Base link of the robot (whose pose must be estimated) */ + int m_baseFrameIndex; /**< Index of the frame whose pose needs to be estimated */ + + std::string m_footFrameName_L; /**< reference frame of the left stance foot (whose + orientation is measured)*/ + int m_footFrameIndex_L; /**< Index of the left foot frame */ + + std::string m_footFrameName_R; /**< reference frame of the right stance foot (whose + orientation is measured)*/ + int m_footFrameIndex_R; /**< Index of the right foot frame */ + + bool m_isLastStanceFoot_L{false}; /**< true if the last stance foot was the left one */ + bool m_isLastStanceFoot_R{false}; /**< true if the last stance foot was the right one */ + bool m_isInitialized{false}; + bool m_isInputSet{false}; + bool m_isOutputValid{false}; }; } // namespace Estimators diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 9bb484f846..a61a5bef76 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -11,12 +11,12 @@ using namespace BipedalLocomotion; -Eigen::Vector3d toZYX(const Eigen::Matrix3d& r) +Eigen::Vector3d toXYZ(const Eigen::Matrix3d& r) { Eigen::Vector3d output; - double& thetaZ = output[0]; // Roll + double& thetaX = output[0]; // Roll double& thetaY = output[1]; // Pitch - double& thetaX = output[2]; // Yaw + double& thetaZ = output[2]; // Yaw if (r(2, 0) < +1) { @@ -49,17 +49,13 @@ bool BaseEstimatorFromFootIMU::setModel(const iDynTree::Model& model) { constexpr auto logPrefix = "[BaseEstimatorFromFootIMU::setModel]"; - m_isModelSet = false; - - m_model = model; - if (!m_kinDyn.loadRobotModel(model)) { log()->error("{} Unable to load the model.", logPrefix); return false; } - m_isModelSet = true; + m_model = model; return true; } @@ -71,7 +67,7 @@ bool BaseEstimatorFromFootIMU::initialize( m_isInitialized = false; - if (!m_isModelSet) + if (!m_model.isValid()) { log()->error("{} No iDynTree::Model has been set.", logPrefix); return false; @@ -111,47 +107,80 @@ bool BaseEstimatorFromFootIMU::initialize( return true; }; - // Base link of the robot (whose pose must be estimated) - std::string baseFrameName; - bool ok = populateParameter(ptr->getGroup("MODEL_INFO"), "base_frame", baseFrameName); - - // Frame associated to the foot of the robot (whose orientation is measured) - ok = populateParameter(ptr->getGroup("MODEL_INFO"), "foot_frame", m_footFrameName); - - ok = ok && populateParameter(ptr, "foot_width_in_m", m_footWidth); - ok = ok && populateParameter(ptr, "foot_length_in_m", m_footLength); + auto baseEstimatorPtr = ptr->getGroup("BASE_ESTIMATOR").lock(); + if (baseEstimatorPtr == nullptr) + { + log()->error("{} Invalid parameter handler for the group 'BASE_ESTIMATOR'", logPrefix); + return false; + } - // Set the 4 foot vertices in World reference frame [dimensions in meters] - m_cornersInLocalFrame.emplace_back(+m_footWidth / 2, +m_footLength / 2, 0); - m_cornersInLocalFrame.emplace_back(-m_footWidth / 2, +m_footLength / 2, 0); - m_cornersInLocalFrame.emplace_back(-m_footWidth / 2, -m_footLength / 2, 0); - m_cornersInLocalFrame.emplace_back(+m_footWidth / 2, -m_footLength / 2, 0); + // Frame associated to the base of the robot (whose pose is estimated) + bool ok = populateParameter(baseEstimatorPtr->getGroup("MODEL_INFO"), + "base_frame", + m_baseFrameName); + + // Frame associated to the left foot of the robot (whose orientation is measured) + ok = ok + && populateParameter(baseEstimatorPtr->getGroup("MODEL_INFO"), + "foot_frame_L", + m_footFrameName_L); + // Frame associated to the right foot of the robot (whose orientation is measured) + ok = ok + && populateParameter(baseEstimatorPtr->getGroup("MODEL_INFO"), + "foot_frame_R", + m_footFrameName_R); + // Foot dimensions. Same for both feet. + ok = ok && populateParameter(baseEstimatorPtr, "foot_width_in_m", m_footWidth); + ok = ok && populateParameter(baseEstimatorPtr, "foot_length_in_m", m_footLength); + + if (!ok) + { + log()->error("{} Unable to retrieve all the parameters from the parameter handler.", logPrefix); + return false; + } m_gravity << 0, 0, -BipedalLocomotion::Math::StandardAccelerationOfGravitation; - m_frameIndex = m_kinDyn.getFrameIndex(m_footFrameName); - if (m_frameIndex == iDynTree::FRAME_INVALID_INDEX) + + m_baseFrameIndex = m_kinDyn.getFrameIndex(m_baseFrameName); + if (m_baseFrameIndex == iDynTree::FRAME_INVALID_INDEX) { - log()->error("{} Invalid frame named: {}", logPrefix, m_footFrameName); + log()->error("{} Invalid frame named: {}", logPrefix, m_baseFrameName); return false; } - m_baseFrame = m_kinDyn.getFrameIndex(baseFrameName); - if (m_baseFrame == iDynTree::FRAME_INVALID_INDEX) + + m_footFrameIndex_L = m_kinDyn.getFrameIndex(m_footFrameName_L); + if (m_footFrameIndex_L == iDynTree::FRAME_INVALID_INDEX) { - log()->error("{} Invalid frame named: {}", logPrefix, baseFrameName); + log()->error("{} Invalid frame named: {}", logPrefix, m_footFrameName_L); return false; } + iDynTree::Transform frame_H_link_L = m_model.getFrameTransform(m_footFrameIndex_L).inverse(); + m_footFrame_H_link_L = Conversions::toManifPose(frame_H_link_L); - m_frameName = m_kinDyn.getFrameName(m_frameIndex); - m_linkIndex = m_model.getFrameLink(m_frameIndex); - - if (!m_kinDyn.setFloatingBase(m_model.getLinkName(m_linkIndex))) + m_footFrameIndex_R = m_kinDyn.getFrameIndex(m_footFrameName_R); + if (m_footFrameIndex_R == iDynTree::FRAME_INVALID_INDEX) { - log()->error("{} Unable to set the floating base.", logPrefix); + log()->error("{} Invalid frame named: {}", logPrefix, m_footFrameName_R); return false; } + iDynTree::Transform frame_H_link_R = m_model.getFrameTransform(m_footFrameIndex_R).inverse(); + m_footFrame_H_link_R = Conversions::toManifPose(frame_H_link_R); + + // resetting the vector of foot corners to be sure it is correctly initialized. + m_cornersInInertialFrame.clear(); - iDynTree::Transform frame_H_link = m_model.getFrameTransform(m_frameIndex).inverse(); - m_frame_H_link = Conversions::toManifPose(frame_H_link); + // Set the 4 foot vertices in World reference frame [dimensions in meters] + m_cornersInInertialFrame.emplace_back(+m_footLength / 2, -m_footWidth / 2, 0); + m_cornersInInertialFrame.emplace_back(+m_footLength / 2, +m_footWidth / 2, 0); + m_cornersInInertialFrame.emplace_back(-m_footLength / 2, +m_footWidth / 2, 0); + m_cornersInInertialFrame.emplace_back(-m_footLength / 2, -m_footWidth / 2, 0); + + m_yawOld = 0.0; + m_T_yawDrift.setIdentity(); + m_measuredFootPose.setIdentity(); + m_T_walk.setIdentity(); + m_state.stanceFootShadowCorners.resize(m_cornersInInertialFrame.size()); + m_state.stanceFootCorners.resize(m_cornersInInertialFrame.size()); m_isInitialized = true; @@ -160,7 +189,17 @@ bool BaseEstimatorFromFootIMU::initialize( bool BaseEstimatorFromFootIMU::setInput(const BaseEstimatorFromFootIMUInput& input) { + constexpr auto logPrefix = "[BaseEstimatorFromFootIMU::setInput]"; + m_isInputSet = false; + + if (!m_isInitialized) + { + log()->error("{} The estimator must be initialized before setting the input.", logPrefix); + return false; + } + m_input = input; + m_isInputSet = true; return true; } @@ -170,100 +209,254 @@ bool BaseEstimatorFromFootIMU::advance() m_isOutputValid = false; - if (!m_isInitialized) + if (!m_isInputSet) + { + log()->error("{} The estimator input has not been set.", logPrefix); + return false; + } + + // checking the stance foot + if (m_input.isLeftStance && m_input.isRightStance) + { + log()->error("{} Both feet are stance feet. The estimator accept only one stance foot.", + logPrefix); + return false; + } + if (!m_input.isLeftStance && !m_input.isRightStance) + { + log()->error("{} No stance foot set. The estimator needs one stance foot in input.", + logPrefix); + return false; + } + manif::SE3d stanceFootFrame_H_link = manif::SE3d::Identity(); + int stanceFootFrameIndex = iDynTree::FRAME_INVALID_INDEX; + + if (m_input.isLeftStance) + { + stanceFootFrame_H_link = m_footFrame_H_link_L; + stanceFootFrameIndex = m_footFrameIndex_L; + // casting the measured foot orientation manif::SO3d --> Eigen::Matrix3d. + m_measuredRotation = m_input.measuredRotation_L.rotation(); + m_measuredAngularVelocity = m_input.measuredAngularVelocity_L; + // has the stance foot just changed? + if (m_isLastStanceFoot_R) + { + // updating the yawOld value. + Eigen::Vector3d lastRPY_R = toXYZ(m_state.footPose_R.rotation()); + // Eigen::Vector3d lastRPY_L = toXYZ(m_input.measuredRotation_L.rotation()); + m_yawOld = lastRPY_R(2); + + // updating the m_T_walk matrix. + Eigen::Vector3d stepTras = (m_state.footPose_L.translation() - m_state.footPose_R.translation()); + stepTras(2) = 0.0; + manif::SE3d T_walkTras(stepTras, manif::SO3d::Identity()); + + // Eigen::Matrix3d stepRot = m_state.footPose_R.rotation().inverse() * m_state.footPose_L.rotation(); + // Eigen::Vector3d stepRotRPY = toXYZ(stepRot); + // manif::SO3d stepRotManif = manif::SO3d(stepRotRPY(0), stepRotRPY(1), stepRotRPY(2)); + // manif::SE3d T_walkRot(m_noTras, stepRotManif); + // manif::SE3d temp = T_walkTras * T_walkRot * m_T_walk * m_T_yawDrift; + + manif::SE3d temp = T_walkTras * m_T_walk; + m_T_walk = temp; + + // m_T_yawDrift.setIdentity(); + } + } + + if (m_input.isRightStance) + { + stanceFootFrame_H_link = m_footFrame_H_link_R; + stanceFootFrameIndex = m_footFrameIndex_R; + // casting the measured foot orientation manif::SO3d --> Eigen::Matrix3d. + m_measuredRotation = m_input.measuredRotation_R.rotation(); + m_measuredAngularVelocity = m_input.measuredAngularVelocity_R; + // has the stance foot just changed? + if (m_isLastStanceFoot_L) + { + // updating the yawOld value. + Eigen::Vector3d lastRPY_L = toXYZ(m_state.footPose_L.rotation()); + // Eigen::Vector3d lastRPY_R = toXYZ(m_input.measuredRotation_R.rotation()); + m_yawOld = lastRPY_L(2); + + // updating the m_T_walk matrix. + Eigen::Vector3d stepTras = (m_state.footPose_R.translation() - m_state.footPose_L.translation()); + stepTras(2) = 0.0; + manif::SE3d T_walkTras(stepTras, manif::SO3d::Identity()); + + // Eigen::Matrix3d stepRot = m_state.footPose_L.rotation().inverse() * m_state.footPose_R.rotation(); + // Eigen::Vector3d stepRotRPY = toXYZ(stepRot); + // manif::SO3d stepRotManif = manif::SO3d(stepRotRPY(0), stepRotRPY(1), stepRotRPY(2)); + // manif::SE3d T_walkRot(m_noTras, stepRotManif); + // manif::SE3d temp = T_walkTras * T_walkRot * m_T_walk * m_T_yawDrift; + + manif::SE3d temp = T_walkTras * m_T_walk; + m_T_walk = temp; + + // m_T_yawDrift.setIdentity(); + } + } + + m_stanceLinkIndex = m_model.getFrameLink(stanceFootFrameIndex); + if (!m_kinDyn.setFloatingBase(m_model.getLinkName(m_stanceLinkIndex))) { - log()->error("{} The estimator is not initialized.", logPrefix); + log()->error("{} Unable to set the stance foot as floating base.", logPrefix); return false; } - // `desiredFootPose` is intended as the output of the footstep planner. + // `offsetStanceFootPose` is intended as the output of the footstep planner. - // extracting the position part of the `desiredFootPose`. - m_desiredTranslation = m_input.desiredFootPose.translation(); + // extracting the position part of the `offsetStanceFootPose`. + m_offsetTranslation = m_input.offsetStanceFootPose.translation(); - // extracting the orientation part of the `desiredFootPose` and expressing it + // extracting the orientation part of the `offsetStanceFootPose` and expressing it // through RPY Euler angles. - m_desiredRotation = m_input.desiredFootPose.rotation(); - m_desiredRPY = toZYX(m_desiredRotation); + m_offsetRotation = m_input.offsetStanceFootPose.rotation(); + m_offsetRPY = toXYZ(m_offsetRotation); + m_offsetRotationCasted = manif::SO3d(m_offsetRPY(0), m_offsetRPY(1), m_offsetRPY(2)); + + // expressing measured orientation through RPY Euler angles. + m_measuredRPY = toXYZ(m_measuredRotation); - // casting the measured foot orientation manif::SO3d --> Eigen::Matrix3d. - m_measuredRotation = m_input.measuredRotation.rotation(); - // expressing this orientation through RPY Euler angles. - m_measuredRPY = toZYX(m_measuredRotation); + // offset Yaw is used instead of measured Yaw. + // m_measuredRPY(2) = m_offsetRPY(2); - // desired Yaw is used instead of measured Yaw. - m_measuredRPY(2) = m_desiredRPY(2); + // MANUAL CORRECTION: measured Roll, Pitch and Yaw. + // double temp_roll = -m_measuredRPY(1); + // double temp_pitch = -m_measuredRPY(0); + // double temp_yaw = -m_measuredRPY(2); + // m_measuredRPY(0) = temp_roll; + // m_measuredRPY(1) = temp_pitch; + // m_measuredRPY(2) = temp_yaw; - // manif::SO3d rotation matrix that employs: measured Roll, measured Pitch, - // desired Yaw. + // manif::SO3d rotation matrix that employs: measured Roll, measured Pitch, offset Yaw. m_measuredRotationCorrected = manif::SO3d(m_measuredRPY(0), m_measuredRPY(1), m_measuredRPY(2)); + m_measuredTilt = manif::SO3d(m_measuredRPY(0), m_measuredRPY(1), 0.0); + double measuredYaw = m_measuredRPY(2); - // manif::SE3d pose matrix that employs: desired Position, measured Roll, - // measured Pitch, desired Yaw. - manif::SE3d T_foot_raw(m_desiredTranslation, m_measuredRotationCorrected); + // manif::SE3d pose matrix that employs: offset Position, measured Roll, measured Pitch, offset + // Yaw. + manif::SE3d T_foot_imu(m_noTras, m_measuredRotationCorrected); + manif::SE3d T_foot_tilt(m_noTras, m_measuredTilt); + manif::SE3d T_foot_offset(m_offsetTranslation, m_offsetRotationCasted); - // finding the positions of the foot corners in world frame given `T_foot_raw` + // finding the positions of the foot corners in world frame given `T_foot_imu` // pose matrix. + // resetting the vector of transformed foot corners from previous iteration. + m_tiltedFootCorners.clear(); + // for each corner we compute the position in the inertial frame - for (const auto& corner : m_cornersInLocalFrame) + for (const auto& corner : m_cornersInInertialFrame) { - m_transformedFootCorners.emplace_back(T_foot_raw.act(corner)); + m_tiltedFootCorners.emplace_back(T_foot_tilt.act(corner)); } // The center of the foot sole is at ground level --> some corners may be - // under ground level. The foot may need to be lifted by an offset value. + // under ground level. The foot may need to be translated by a 3D offset value. // extracting the vertical quotes of the foot corners and finding the lowest // corner. - auto lowestCorner = std::min_element(m_transformedFootCorners.begin(), - m_transformedFootCorners.end(), - [](const Eigen::Vector3d& a, const Eigen::Vector3d& b) { - return a[2] < b[2]; - }); + Eigen::VectorXd cornersZ; + cornersZ.resize(m_cornersInInertialFrame.size()); + cornersZ.setZero(); + int index = 0; + for (const auto& corner : m_tiltedFootCorners) + { + cornersZ[index] = corner[2]; + index++; + } // finding the index of the lowest corner. - int supportCornerIndex = std::distance(m_transformedFootCorners.begin(), lowestCorner); - - Eigen::Vector3d p_desired(0, 0, 0); - Eigen::Vector3d vertexOffset(0, 0, 0); + double minZ = cornersZ[0]; + int indexMinZ = 0; + for (int i = 1; i < cornersZ.size(); i++) + { + if (cornersZ[i] < minZ) + { + minZ = cornersZ[i]; + indexMinZ = i; + } + } + m_state.supportCornerIndex = indexMinZ; - // finding the offset vector needed to bring the lowest corner back to its - // desired position. - switch (supportCornerIndex) + // checking that the index of the lowest corner is within the range [0, 3]. + if (!(0 <= m_state.supportCornerIndex <= 3)) { - case 0: - p_desired = m_input.desiredFootPose.act(m_cornersInLocalFrame[supportCornerIndex]); - vertexOffset = p_desired - m_transformedFootCorners[supportCornerIndex]; - break; - case 1: - p_desired = m_input.desiredFootPose.act(m_cornersInLocalFrame[supportCornerIndex]); - vertexOffset = p_desired - m_transformedFootCorners[supportCornerIndex]; - break; - case 2: - p_desired = m_input.desiredFootPose.act(m_cornersInLocalFrame[supportCornerIndex]); - vertexOffset = p_desired - m_transformedFootCorners[supportCornerIndex]; - break; - case 3: - p_desired = m_input.desiredFootPose.act(m_cornersInLocalFrame[supportCornerIndex]); - vertexOffset = p_desired - m_transformedFootCorners[supportCornerIndex]; - break; - default: log()->error("{} Foot vertex index out of bounds (0, 3): {}.", logPrefix, - supportCornerIndex); + m_state.supportCornerIndex); return false; } + // finding the index of the highest corner. + // double maxZ = cornersZ[0]; + // int indexMaxZ = 0; + // for (int i = 1; i < cornersZ.size(); i++) + // { + // if (cornersZ[i] > maxZ) + // { + // maxZ = cornersZ[i]; + // indexMaxZ = i; + // } + // } + + // double deltaZ = cornersZ[indexMaxZ] - cornersZ[m_state.supportCornerIndex]; + // std::cerr << "Foot deltaZ: " << deltaZ << std::endl; + + // finding the translation vector needed to bring the lowest corner back to its + // untilted position. + Eigen::Vector3d p_untilted(0, 0, 0); + Eigen::Vector3d supportCornerTranslation(0, 0, 0); + p_untilted = (m_cornersInInertialFrame[m_state.supportCornerIndex]); + supportCornerTranslation + = p_untilted - (m_tiltedFootCorners[m_state.supportCornerIndex]); // TODO: change if + // flat ground + // assumption is + // removed + // transforming the offset vector into a translation matrix. - manif::SE3d T_vertexOffset(vertexOffset, manif::SO3d::Identity()); + manif::SE3d T_supportCornerTranslation(supportCornerTranslation, manif::SO3d::Identity()); + + // calculating the yaw drift - VALID FOR BOTH FEET ONLY IF FRAMES ARE ORIENTED IN THE SAME WAY. + double deltaYaw = 0.0; + deltaYaw = measuredYaw - m_yawOld; + m_yawOld = measuredYaw; + // manif::SO3d rotation matrix that employs: zero Roll, zero Pitch, measured Yaw variation. + auto R_deltaYaw = manif::SO3d(0.0, 0.0, deltaYaw); + manif::SE3d T_deltaYaw(m_noTras, R_deltaYaw); + + std::vector tempCorners; + tempCorners.resize(m_cornersInInertialFrame.size()); + int j = 0; + for (const auto& corner : m_cornersInInertialFrame) + { + tempCorners[j] = T_deltaYaw.act(corner); + j++; + } + Eigen::Vector3d yawDrift(0, 0, 0); + yawDrift = m_cornersInInertialFrame[m_state.supportCornerIndex] + - tempCorners[m_state.supportCornerIndex]; + // manif::SE3d T_yawDrift(yawDrift, R_deltaYaw); + manif::SE3d T_yawDrift(m_noTras, R_deltaYaw); + + manif::SE3d temp = T_yawDrift * m_T_yawDrift; + m_T_yawDrift = temp; - // obtaining the final foot pose using both measured and desired quantities. + // obtaining the final foot pose using both measured and offset quantities. // cordinate change is performed from foot sole frame to foot link frame. - m_measuredFootPose = T_vertexOffset * T_foot_raw * m_frame_H_link; + m_measuredFootPose = T_foot_offset * m_T_walk * m_T_yawDrift * T_supportCornerTranslation + * T_foot_tilt * stanceFootFrame_H_link; Eigen::VectorXd baseVelocity(6); - baseVelocity.setZero(); + // baseVelocity.setZero(); + Eigen::Vector3d angularVelocityInLinkFrame = stanceFootFrame_H_link.asSO3().inverse().act(m_measuredAngularVelocity.coeffs()); + Eigen::Vector3d cornerInLinkFrame = stanceFootFrame_H_link.inverse().act(m_cornersInInertialFrame[m_state.supportCornerIndex]); + + baseVelocity.head<3>() = m_measuredFootPose.asSO3().act(cornerInLinkFrame.cross(angularVelocityInLinkFrame)); + + manif::SO3d measuredRotationManif = Conversions::toManifRot(m_measuredRotation); + baseVelocity.tail<3>() = measuredRotationManif.act(m_measuredAngularVelocity.coeffs()); // setting the robot state in terms of stance foot pose and joint positions. if (!m_kinDyn.setRobotState(m_measuredFootPose.transform(), @@ -272,12 +465,56 @@ bool BaseEstimatorFromFootIMU::advance() m_input.jointVelocities, m_gravity)) { - log()->error("{} Unable to set the robot state.", logPrefix); + log()->error("{} Unable to set the robot state from the stance foot pose.", logPrefix); return false; } - // calculating the pose of the root link given the robot state. - m_state.basePose = Conversions::toManifPose(m_kinDyn.getWorldTransform(m_baseFrame)); + // calculating the output of the estimator given the robot state. + m_state.basePose = Conversions::toManifPose(m_kinDyn.getWorldTransform(m_baseFrameIndex)); + m_state.footPose_L = Conversions::toManifPose(m_kinDyn.getWorldTransform(m_footFrameIndex_L)); + m_state.footPose_R = Conversions::toManifPose(m_kinDyn.getWorldTransform(m_footFrameIndex_R)); + m_state.baseVelocity = Conversions::toManifTwist(m_kinDyn.getFrameVel(m_baseFrameIndex)); + m_kinDyn.getCenterOfMassPosition(m_state.centerOfMassPosition); + + for (int i = 0; i < m_cornersInInertialFrame.size(); i++) + { + m_state.stanceFootShadowCorners[i] + = m_T_walk.act(m_T_yawDrift.act(m_cornersInInertialFrame[i])); + m_state.stanceFootCorners[i] = m_T_walk.act( + m_T_yawDrift.act(T_supportCornerTranslation.act(m_tiltedFootCorners[i]))); + } + + double orientationError_L = (toXYZ(m_state.footPose_L.rotation()) - toXYZ(m_input.measuredRotation_L.rotation())).norm(); + orientationError_L = std::fmod(orientationError_L, 2.0 * M_PI) * 180.0 / M_PI; + double orientationError_R = (toXYZ(m_state.footPose_R.rotation()) - toXYZ(m_input.measuredRotation_R.rotation())).norm(); + orientationError_R = std::fmod(orientationError_R, 2.0 * M_PI) * 180.0 / M_PI; + double orientationErrorThreshold = 1.5; // [deg]. TODO: get parameter from config file. 1,5 deg = 0,0261799 rad. + + if ( ((orientationError_L > orientationErrorThreshold) && (orientationError_L < (360.0 - orientationErrorThreshold))) || + ((orientationError_R > orientationErrorThreshold) && (orientationError_R < (360.0 - orientationErrorThreshold)))) + { + log()->warn("{} Foot orientation error above {} [deg] threshold: {} Left, {} Right.", + logPrefix, + orientationErrorThreshold, + orientationError_L, + orientationError_R); + } + // std::cerr << "L FOOT ROTATION IN: " << toXYZ(m_input.measuredRotation_L.rotation()).transpose() << std::endl; + // std::cerr << "L FOOT ROTATION OUT: " << toXYZ(m_state.footPose_L.rotation()).transpose() << std::endl; + // std::cerr << "R FOOT ROTATION IN: " << toXYZ(m_input.measuredRotation_R.rotation()).transpose() << std::endl; + // std::cerr << "R FOOT ROTATION OUT: " << toXYZ(m_state.footPose_R.rotation()).transpose() << std::endl; + + // updating the stance foot flags. + if (m_input.isLeftStance) + { + m_isLastStanceFoot_L = true; + m_isLastStanceFoot_R = false; + } + if (m_input.isRightStance) + { + m_isLastStanceFoot_L = false; + m_isLastStanceFoot_R = true; + } m_isOutputValid = true; @@ -301,6 +538,16 @@ const BaseEstimatorFromFootIMUState& BaseEstimatorFromFootIMU::getOutput() const return m_state; // m_state.basePose is the actual output } +void BaseEstimatorFromFootIMU::resetBaseEstimatorFromFootIMU() +{ + m_isOutputValid = false; + m_yawOld = 0.0; + m_T_yawDrift.setIdentity(); + m_T_walk.setIdentity(); + m_isLastStanceFoot_L = false; + m_isLastStanceFoot_R = false; +} + // OPTIONAL METHOD - TO FORCE THE STATE void BaseEstimatorFromFootIMU::setState(const BaseEstimatorFromFootIMUState& state) diff --git a/src/System/CMakeLists.txt b/src/System/CMakeLists.txt index 2bfd417a50..dd30345c6f 100644 --- a/src/System/CMakeLists.txt +++ b/src/System/CMakeLists.txt @@ -17,11 +17,11 @@ if(FRAMEWORK_COMPILE_System) ${H_PREFIX}/IClock.h ${H_PREFIX}/StdClock.h ${H_PREFIX}/Clock.h ${H_PREFIX}/SharedResource.h ${H_PREFIX}/AdvanceableRunner.h ${H_PREFIX}/QuitHandler.h - ${H_PREFIX}/Barrier.h + ${H_PREFIX}/Barrier.h ${H_PREFIX}/TimeProfiler.h ${H_PREFIX}/WeightProvider.h ${H_PREFIX}/ConstantWeightProvider.h SOURCES src/VariablesHandler.cpp src/LinearTask.cpp src/StdClock.cpp src/Clock.cpp src/QuitHandler.cpp src/Barrier.cpp - src/ConstantWeightProvider.cpp + src/ConstantWeightProvider.cpp src/TimeProfiler.cpp PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler Eigen3::Eigen SUBDIRECTORIES tests YarpImplementation RosImplementation ) diff --git a/src/System/include/BipedalLocomotion/System/TimeProfiler.h b/src/System/include/BipedalLocomotion/System/TimeProfiler.h new file mode 100644 index 0000000000..41bf8a605a --- /dev/null +++ b/src/System/include/BipedalLocomotion/System/TimeProfiler.h @@ -0,0 +1,103 @@ +/** + * @file TimeProfiler.h + * @authors Guglielmo Cervettini, Giulio Romualdi + * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#ifndef BIPEDAL_LOCOMOTION_SYSTEM_TIME_PROFILER_H +#define BIPEDAL_LOCOMOTION_SYSTEM_TIME_PROFILER_H + +// std +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace System +{ +/** + * Timer is a simple class that can be used to measure the time between two events. + */ +class Timer +{ + std::chrono::time_point m_initTime; /**< Init time. */ + std::chrono::time_point m_endTime; /**< End time. */ + std::chrono::nanoseconds m_averageDuration; /**< Average duration. */ + +public: + /** + * Reset the average duration. + */ + void resetAverageDuration(); + + /** + * Set initial time. + */ + void setInitTime(); + + /** + * Set final time. + */ + void setEndTime(); + + /** + * Evaluate the average duration. + */ + void evaluateDuration(); + + /** + * Get the average duration. + * @return average duration. + */ + const std::chrono::nanoseconds& getAverageDuration() const; +}; + +/** + * TimeProfiler is a simple class that can be used to profile the code. + */ +class TimeProfiler +{ + int m_counter; /**< Counter useful to print the profiling quantities only every m_maxCounter + times. */ + int m_maxCounter; /**< The profiling quantities will be printed every maxCounter cycles. */ + std::map m_timers; /**< Dictionary that contains all the timers. */ +public: + /** + * Set the output period. + * @param maxCounter is the period (expressed in cycles). + */ + void setPeriod(int maxCounter); + + /** + * Add a new timer + * @param key is the name of the timer. + * @return true/false in case of success/failure. + */ + bool addTimer(const std::string& key); + + /** + * Set the init time for the timer named "key" + * @param key is the name of the timer. + * @return true/false in case of success/failure. + */ + bool setInitTime(const std::string& key); + + /** + * Set the end time for the timer named "key" + * @param key is the name of the timer. + * @return true/false in case of success/failure. + */ + bool setEndTime(const std::string& key); + + /** + * Print the profiling quantities. + */ + void profiling(); +}; + +}; // namespace System +}; // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_SYSTEM_TIME_PROFILER_H diff --git a/src/System/src/TimeProfiler.cpp b/src/System/src/TimeProfiler.cpp new file mode 100644 index 0000000000..af220ffeae --- /dev/null +++ b/src/System/src/TimeProfiler.cpp @@ -0,0 +1,114 @@ +/** + * @file TimeProfiler.cpp + * @authors Guglielmo Cervettini, Giulio Romualdi + * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the BSD-3-Clause license. + */ + +#include +#include + +#include +#include + +using namespace BipedalLocomotion::System; + +const std::chrono::nanoseconds& Timer::getAverageDuration() const +{ + return m_averageDuration; +} + +void Timer::resetAverageDuration() +{ + m_averageDuration = std::chrono::nanoseconds(0); +} + +void Timer::setInitTime() +{ + m_initTime = std::chrono::steady_clock::now(); +} + +void Timer::setEndTime() +{ + m_endTime = std::chrono::steady_clock::now(); +} + +void Timer::evaluateDuration() +{ + const std::chrono::nanoseconds duration = m_endTime - m_initTime; + m_averageDuration += duration; +} + +void TimeProfiler::setPeriod(int maxCounter) +{ + m_maxCounter = maxCounter; +} + +bool TimeProfiler::addTimer(const std::string& key) +{ + auto timer = m_timers.find(key); + if (timer != m_timers.end()) + { + log()->error("[TimeProfiler::addTimer] The timer named {} already exists.", key); + return false; + } + + m_timers.insert(std::make_pair(key, Timer())); + return true; +} + +bool TimeProfiler::setInitTime(const std::string& key) +{ + auto timer = m_timers.find(key); + if (timer == m_timers.end()) + { + log()->error("[TimeProfiler::setInitTime] Unable to find the timer named {}.", key); + return false; + } + + timer->second.setInitTime(); + return true; +} + +bool TimeProfiler::setEndTime(const std::string& key) +{ + auto timer = m_timers.find(key); + if (timer == m_timers.end()) + { + log()->error("[TimeProfiler::setEndTime] Unable to find the timer named {}.", key); + return false; + } + + timer->second.setEndTime(); + return true; +} + +void TimeProfiler::profiling() +{ + std::string infoStream; + m_counter++; + for (auto& [key, timer] : m_timers) + { + timer.evaluateDuration(); + if (m_counter != m_maxCounter) + { + continue; + } + + const std::chrono::nanoseconds& durationInNs = timer.getAverageDuration(); + + // convert the duration in ns to ms + const auto durationInMs + = std::chrono::duration_cast>(durationInNs) + / double(m_counter); + + infoStream += key + ": " + std::to_string(durationInMs.count()) + " ms "; + timer.resetAverageDuration(); + } + + if (m_counter == m_maxCounter) + { + m_counter = 0; + log()->info("[TimeProfiler::profiling] {}", infoStream); + } +} diff --git a/src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/VectorsCollectionServer.h b/src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/VectorsCollectionServer.h index ffbcfccf0a..8bff73f642 100644 --- a/src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/VectorsCollectionServer.h +++ b/src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/VectorsCollectionServer.h @@ -41,6 +41,8 @@ namespace YarpUtilities * server.populateMetadata("key2", {"metadata4", "metadata5", "metadata6"}); * server.finalizeMetadata(); * + * // prepare the data + * server.prepareData(); * server.clearData(); // optional * server.populateData("key1", {1.0, 2.0, 3.0}); * server.populateData("key2", {4.0, 5.0, 6.0}); @@ -94,7 +96,8 @@ class VectorsCollectionServer : public VectorsCollectionMetadataService * @param key key of the data. * @param data data. * @return true if the data has been set successfully, false otherwise. - * @note this function should be called after the metadata has been finalized. + * @note this function should be called after the metadata has been finalized and after the + * prepareData function has been called. */ bool populateData(const std::string& key, const iDynTree::Span& data); @@ -111,6 +114,12 @@ class VectorsCollectionServer : public VectorsCollectionMetadataService */ bool areMetadataReady() override; + /** + * Prepare the data. + * @note this function should be called before the data is populated. + */ + void prepareData(); + /** * Send the data filled with populateData * @param forceStrict If this is true, wait until any previous sends are complete. If false, the @@ -124,8 +133,9 @@ class VectorsCollectionServer : public VectorsCollectionMetadataService * to reuse it without reallocating memory, you may skip calling this function. Otherwise, use * VectorsCollection::clearData to free the memory allocated in the internal buffer. * @note Note that this function only clears the data and does not affect the metadata. + * @return true if the data has been cleared successfully, false otherwise. */ - void clearData(); + bool clearData(); private: struct Impl; diff --git a/src/YarpUtilities/src/VectorsCollectionServer.cpp b/src/YarpUtilities/src/VectorsCollectionServer.cpp index 07a7b11faa..0284760bd2 100644 --- a/src/YarpUtilities/src/VectorsCollectionServer.cpp +++ b/src/YarpUtilities/src/VectorsCollectionServer.cpp @@ -11,6 +11,8 @@ #include #include +#include +#include #include using namespace BipedalLocomotion::YarpUtilities; @@ -25,8 +27,21 @@ struct VectorsCollectionServer::Impl std::atomic isMetadataFinalized{false}; /**< True if the metadata has been finalized. */ std::unordered_set setOfKeys; /**< Set of keys. */ + std::optional> collection; /**< Reference to the + collection. */ + + /** + * Check if the collection is valid. + * @return True if the collection is valid. + */ + [[nodiscard]] bool isCollectionValid() const; }; +bool VectorsCollectionServer::Impl::isCollectionValid() const +{ + return collection.has_value(); +} + VectorsCollectionServer::VectorsCollectionServer() { m_pimpl = std::make_unique(); @@ -129,6 +144,11 @@ bool VectorsCollectionServer::finalizeMetadata() return true; } +void VectorsCollectionServer::prepareData() +{ + m_pimpl->collection = m_pimpl->port.prepare(); +} + bool VectorsCollectionServer::populateData(const std::string& key, const iDynTree::Span& data) { @@ -148,9 +168,15 @@ bool VectorsCollectionServer::populateData(const std::string& key, return false; } - // prepare the data - BipedalLocomotion::YarpUtilities::VectorsCollection& collection = m_pimpl->port.prepare(); - collection.vectors[key].assign(data.begin(), data.end()); + if (!m_pimpl->isCollectionValid()) + { + log()->error("{} The data collection is not valid. Please call prepareData before " + "calling this function.", + logPrefix); + return false; + } + + m_pimpl->collection.value().get().vectors[key].assign(data.begin(), data.end()); return true; } @@ -160,9 +186,18 @@ void VectorsCollectionServer::sendData(bool forceStrict /*= false */) m_pimpl->port.write(forceStrict); } -void VectorsCollectionServer::clearData() +bool VectorsCollectionServer::clearData() { - m_pimpl->port.prepare().vectors.clear(); + // check if the reference to the collection is valid + if (!m_pimpl->isCollectionValid()) + { + log()->error("[VectorsCollectionServer::clearData] The reference to the collection is " + "invalid. Please call prepareData before calling this function."); + return false; + } + + m_pimpl->collection.value().get().vectors.clear(); + return true; } bool VectorsCollectionServer::areMetadataReady()