From 4d8aeaacaea1f9c3a8e6efc278ec0136f01ccf26 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Thu, 7 Sep 2023 18:07:49 +0200 Subject: [PATCH 01/45] Fixed an issue with baseEstimatorFromFootIMU wich did not properly reset the orientation info before each advance call --- .../BaseEstimatorFromFootIMU.h | 5 +++-- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 13 +++++++++---- .../JointPositionTracking/Module.h | 2 +- utilities/joint-position-tracking/src/Module.cpp | 12 ++++++------ 4 files changed, 19 insertions(+), 13 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index 22abc4998d..f6da7e22de 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -77,7 +77,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 | @@ -169,7 +169,8 @@ class BaseEstimatorFromFootIMU measured Pitch, desired Yaw */ 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 + manif::SE3d m_frame_H_link; /**< coordinate change matrix from foot link frame to foot sole + * frame */ Eigen::Vector3d m_gravity; iDynTree::KinDynComputations m_kinDyn; diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 9bb484f846..d34b8ce0c9 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -113,13 +113,15 @@ bool BaseEstimatorFromFootIMU::initialize( // Base link of the robot (whose pose must be estimated) std::string baseFrameName; - bool ok = populateParameter(ptr->getGroup("MODEL_INFO"), "base_frame", baseFrameName); + auto baseEstimatorPtr = ptr->getGroup("BASE_ESTIMATOR").lock(); + bool ok + = populateParameter(baseEstimatorPtr->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 = populateParameter(baseEstimatorPtr->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); + ok = ok && populateParameter(baseEstimatorPtr, "foot_width_in_m", m_footWidth); + ok = ok && populateParameter(baseEstimatorPtr, "foot_length_in_m", m_footLength); // Set the 4 foot vertices in World reference frame [dimensions in meters] m_cornersInLocalFrame.emplace_back(+m_footWidth / 2, +m_footLength / 2, 0); @@ -205,6 +207,9 @@ bool BaseEstimatorFromFootIMU::advance() // finding the positions of the foot corners in world frame given `T_foot_raw` // pose matrix. + // resetting the vector of transformed foot corners from previous iteration. + m_transformedFootCorners.clear(); + // for each corner we compute the position in the inertial frame for (const auto& corner : m_cornersInLocalFrame) { diff --git a/utilities/joint-position-tracking/include/BipedalLocomotion/JointPositionTracking/Module.h b/utilities/joint-position-tracking/include/BipedalLocomotion/JointPositionTracking/Module.h index 8928bac923..d392be73da 100644 --- a/utilities/joint-position-tracking/include/BipedalLocomotion/JointPositionTracking/Module.h +++ b/utilities/joint-position-tracking/include/BipedalLocomotion/JointPositionTracking/Module.h @@ -19,9 +19,9 @@ #include #include +#include #include #include -#include namespace BipedalLocomotion { diff --git a/utilities/joint-position-tracking/src/Module.cpp b/utilities/joint-position-tracking/src/Module.cpp index b4cc748815..927fedc688 100644 --- a/utilities/joint-position-tracking/src/Module.cpp +++ b/utilities/joint-position-tracking/src/Module.cpp @@ -10,9 +10,9 @@ #include #include +#include #include #include -#include #include @@ -113,27 +113,27 @@ bool Module::configure(yarp::os::ResourceFinder& rf) auto parametersHandler = std::make_shared(rf); std::string name; - if(!parametersHandler->getParameter("name", name)) + if (!parametersHandler->getParameter("name", name)) return false; this->setName(name.c_str()); - if(!parametersHandler->getParameter("sampling_time", m_dT)) + if (!parametersHandler->getParameter("sampling_time", m_dT)) return false; double maxValue = 0; - if(!parametersHandler->getParameter("max_angle_deg", maxValue)) + if (!parametersHandler->getParameter("max_angle_deg", maxValue)) return false; maxValue *= M_PI / 180; double minValue = 0; - if(!parametersHandler->getParameter("min_angle_deg", minValue)) + if (!parametersHandler->getParameter("min_angle_deg", minValue)) return false; minValue *= M_PI / 180; double trajectoryDuration = 5; - if(!parametersHandler->getParameter("trajectory_duration", trajectoryDuration)) + if (!parametersHandler->getParameter("trajectory_duration", trajectoryDuration)) return false; this->createPolydriver(parametersHandler); From 4bae7e69afe3944f1326c454e2cdf61003dc7c3e Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Thu, 14 Sep 2023 15:52:12 +0200 Subject: [PATCH 02/45] Using IMU yaw insted of planned yaw --- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index d34b8ce0c9..014bf6d7a6 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -194,7 +194,7 @@ bool BaseEstimatorFromFootIMU::advance() m_measuredRPY = toZYX(m_measuredRotation); // desired Yaw is used instead of measured Yaw. - m_measuredRPY(2) = m_desiredRPY(2); + // m_measuredRPY(2) = m_desiredRPY(2); // manif::SO3d rotation matrix that employs: measured Roll, measured Pitch, // desired Yaw. From 7f613e7d132c2fabcfb9734abd69972c17afe5a6 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Thu, 14 Sep 2023 16:10:13 +0200 Subject: [PATCH 03/45] Reverting last commit, using planned yaw to replace imu yaw --- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 014bf6d7a6..d34b8ce0c9 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -194,7 +194,7 @@ bool BaseEstimatorFromFootIMU::advance() m_measuredRPY = toZYX(m_measuredRotation); // desired Yaw is used instead of measured Yaw. - // m_measuredRPY(2) = m_desiredRPY(2); + m_measuredRPY(2) = m_desiredRPY(2); // manif::SO3d rotation matrix that employs: measured Roll, measured Pitch, // desired Yaw. From ae5f3f95ab87ec16fa706a070c1aa81b74f7db73 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Thu, 14 Sep 2023 18:58:09 +0200 Subject: [PATCH 04/45] Fixed rotation bug and using IMU yaw again --- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index d34b8ce0c9..ec529f2642 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -14,9 +14,9 @@ using namespace BipedalLocomotion; Eigen::Vector3d toZYX(const Eigen::Matrix3d& r) { Eigen::Vector3d output; - double& thetaZ = output[0]; // Roll + double& thetaZ = output[0]; // Yaw double& thetaY = output[1]; // Pitch - double& thetaX = output[2]; // Yaw + double& thetaX = output[2]; // Roll if (r(2, 0) < +1) { @@ -194,11 +194,11 @@ bool BaseEstimatorFromFootIMU::advance() m_measuredRPY = toZYX(m_measuredRotation); // desired Yaw is used instead of measured Yaw. - m_measuredRPY(2) = m_desiredRPY(2); + // m_measuredRPY(2) = m_desiredRPY(2); // manif::SO3d rotation matrix that employs: measured Roll, measured Pitch, // desired Yaw. - m_measuredRotationCorrected = manif::SO3d(m_measuredRPY(0), m_measuredRPY(1), m_measuredRPY(2)); + m_measuredRotationCorrected = manif::SO3d(m_measuredRPY(2), m_measuredRPY(1), m_measuredRPY(0)); // manif::SE3d pose matrix that employs: desired Position, measured Roll, // measured Pitch, desired Yaw. From c20e623269222c507c6ff938401069502e062834 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Thu, 14 Sep 2023 19:04:26 +0200 Subject: [PATCH 05/45] Fixed bug regarding the use of IMU yaw --- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index ec529f2642..812b5c9b9b 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -194,7 +194,7 @@ bool BaseEstimatorFromFootIMU::advance() m_measuredRPY = toZYX(m_measuredRotation); // desired Yaw is used instead of measured Yaw. - // m_measuredRPY(2) = m_desiredRPY(2); + // m_measuredRPY(0) = m_desiredRPY(0); // manif::SO3d rotation matrix that employs: measured Roll, measured Pitch, // desired Yaw. From b149e985b3e53893a41784fb41319f7231ac36d0 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Fri, 15 Sep 2023 18:32:14 +0200 Subject: [PATCH 06/45] Fixed foot sole local frame axes --- .../src/BaseEstimatorFromFootIMU.cpp | 25 +++++++++++-------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 812b5c9b9b..1569de252b 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]; // Yaw + double& thetaX = output[0]; // Roll double& thetaY = output[1]; // Pitch - double& thetaX = output[2]; // Roll + double& thetaZ = output[2]; // Yaw if (r(2, 0) < +1) { @@ -123,11 +123,14 @@ bool BaseEstimatorFromFootIMU::initialize( ok = ok && populateParameter(baseEstimatorPtr, "foot_width_in_m", m_footWidth); ok = ok && populateParameter(baseEstimatorPtr, "foot_length_in_m", m_footLength); + // resetting the vector of foot corners to be sure it is correctly initialized. + m_cornersInLocalFrame.clear(); + // 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); + m_cornersInLocalFrame.emplace_back(+m_footLength / 2, -m_footWidth / 2, 0); + m_cornersInLocalFrame.emplace_back(+m_footLength / 2, +m_footWidth / 2, 0); + m_cornersInLocalFrame.emplace_back(-m_footLength / 2, +m_footWidth / 2, 0); + m_cornersInLocalFrame.emplace_back(-m_footLength / 2, -m_footWidth / 2, 0); m_gravity << 0, 0, -BipedalLocomotion::Math::StandardAccelerationOfGravitation; m_frameIndex = m_kinDyn.getFrameIndex(m_footFrameName); @@ -186,19 +189,19 @@ bool BaseEstimatorFromFootIMU::advance() // extracting the orientation part of the `desiredFootPose` and expressing it // through RPY Euler angles. m_desiredRotation = m_input.desiredFootPose.rotation(); - m_desiredRPY = toZYX(m_desiredRotation); + m_desiredRPY = toXYZ(m_desiredRotation); // 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); + m_measuredRPY = toXYZ(m_measuredRotation); // desired Yaw is used instead of measured Yaw. - // m_measuredRPY(0) = m_desiredRPY(0); + // m_measuredRPY(2) = m_desiredRPY(2); // manif::SO3d rotation matrix that employs: measured Roll, measured Pitch, // desired Yaw. - m_measuredRotationCorrected = manif::SO3d(m_measuredRPY(2), m_measuredRPY(1), m_measuredRPY(0)); + m_measuredRotationCorrected = manif::SO3d(m_measuredRPY(0), m_measuredRPY(1), m_measuredRPY(2)); // manif::SE3d pose matrix that employs: desired Position, measured Roll, // measured Pitch, desired Yaw. From 2fbd087290a08f8472c51588ccd9d5b5824c98fd Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Tue, 19 Sep 2023 20:32:46 +0200 Subject: [PATCH 07/45] Code semplification in the translation calculation to spot the translation discontinuities bug --- .../src/BaseEstimatorFromFootIMU.cpp | 59 +++++++++---------- 1 file changed, 29 insertions(+), 30 deletions(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 1569de252b..1c12ce32ea 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -220,49 +220,48 @@ bool BaseEstimatorFromFootIMU::advance() } // 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_transformedFootCorners.size()); + cornersZ.setZero(); + int index = 0; + for (const auto& corner : m_transformedFootCorners) + { + 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 supportCornerIndex = 0; + for (int i = 1; i < cornersZ.size(); i++) + { + if (cornersZ[i] < minZ) + { + minZ = cornersZ[i]; + supportCornerIndex = i; + } + } - // 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 <= 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); return false; } + // finding the offset vector needed to bring the lowest corner back to its + // desired position. + Eigen::Vector3d p_desired(0, 0, 0); + Eigen::Vector3d vertexOffset(0, 0, 0); + p_desired = m_input.desiredFootPose.act(m_cornersInLocalFrame[supportCornerIndex]); + vertexOffset = p_desired - m_transformedFootCorners[supportCornerIndex]; + // transforming the offset vector into a translation matrix. manif::SE3d T_vertexOffset(vertexOffset, manif::SO3d::Identity()); From ad0225b99ee24e61b55a08c42b78640b56107c5b Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Wed, 20 Sep 2023 01:19:41 +0200 Subject: [PATCH 08/45] updating foot corners in inertial frame when foot is flat --- .../BaseEstimatorFromFootIMU.h | 4 +- .../src/BaseEstimatorFromFootIMU.cpp | 57 ++++++++++++++++--- 2 files changed, 53 insertions(+), 8 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index f6da7e22de..4116e79412 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -152,7 +152,7 @@ class BaseEstimatorFromFootIMU * 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 transformed into the inertial frame */ @@ -169,6 +169,8 @@ class BaseEstimatorFromFootIMU measured Pitch, desired Yaw */ manif::SE3d m_measuredFootPose; /**< the final foot pose matrix obtained through measured and desired quantities */ + manif::SE3d m_resetFootCorners; /**< the final foot pose matrix used to reset corners positions + in inertial frame when the foot is flat */ manif::SE3d m_frame_H_link; /**< coordinate change matrix from foot link frame to foot sole * frame */ diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 1c12ce32ea..b5804d9510 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -124,13 +124,13 @@ bool BaseEstimatorFromFootIMU::initialize( ok = ok && populateParameter(baseEstimatorPtr, "foot_length_in_m", m_footLength); // resetting the vector of foot corners to be sure it is correctly initialized. - m_cornersInLocalFrame.clear(); + m_cornersInInertialFrame.clear(); // Set the 4 foot vertices in World reference frame [dimensions in meters] - m_cornersInLocalFrame.emplace_back(+m_footLength / 2, -m_footWidth / 2, 0); - m_cornersInLocalFrame.emplace_back(+m_footLength / 2, +m_footWidth / 2, 0); - m_cornersInLocalFrame.emplace_back(-m_footLength / 2, +m_footWidth / 2, 0); - m_cornersInLocalFrame.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_cornersInInertialFrame.emplace_back(-m_footLength / 2, -m_footWidth / 2, 0); m_gravity << 0, 0, -BipedalLocomotion::Math::StandardAccelerationOfGravitation; m_frameIndex = m_kinDyn.getFrameIndex(m_footFrameName); @@ -214,7 +214,7 @@ bool BaseEstimatorFromFootIMU::advance() m_transformedFootCorners.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)); } @@ -246,6 +246,21 @@ bool BaseEstimatorFromFootIMU::advance() } } + // finding the index of the highest corner. + double maxZ = cornersZ[0]; + int highestCornerIndex = 0; + for (int i = 1; i < cornersZ.size(); i++) + { + if (cornersZ[i] > maxZ) + { + maxZ = cornersZ[i]; + highestCornerIndex = i; + } + } + + double deltaZ = cornersZ[highestCornerIndex] - cornersZ[supportCornerIndex]; + // std::cerr << "Foot deltaZ: " << deltaZ << std::endl; + // checking that the index of the lowest corner is within the range [0, 3]. if (!(0 <= supportCornerIndex <= 3)) { @@ -259,7 +274,7 @@ bool BaseEstimatorFromFootIMU::advance() // desired position. Eigen::Vector3d p_desired(0, 0, 0); Eigen::Vector3d vertexOffset(0, 0, 0); - p_desired = m_input.desiredFootPose.act(m_cornersInLocalFrame[supportCornerIndex]); + p_desired = m_input.desiredFootPose.act(m_cornersInInertialFrame[supportCornerIndex]); vertexOffset = p_desired - m_transformedFootCorners[supportCornerIndex]; // transforming the offset vector into a translation matrix. @@ -268,6 +283,7 @@ bool BaseEstimatorFromFootIMU::advance() // obtaining the final foot pose using both measured and desired 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_resetFootCorners = T_vertexOffset * T_foot_raw; Eigen::VectorXd baseVelocity(6); baseVelocity.setZero(); @@ -286,6 +302,33 @@ bool BaseEstimatorFromFootIMU::advance() // calculating the pose of the root link given the robot state. m_state.basePose = Conversions::toManifPose(m_kinDyn.getWorldTransform(m_baseFrame)); + // updating m_cornersInLocalFrame when the foot is nearly flat + double flatnessThreshold = 0.0001; + if (deltaZ <= flatnessThreshold) + { + // extracting the position part of the reset corners pose. + auto tras = m_resetFootCorners.translation(); + // foot z is completely canceled + tras(2) = 0.0; + // extracting the orientation part of the reset corners pose and expressing it through RPY + // Euler angles. + auto rot = m_resetFootCorners.rotation(); + auto rpy = toXYZ(rot); + // Roll and Pitch are completely canceled. + rpy(0) = 0.0; + rpy(1) = 0.0; + // manif::SO3d rotation matrix that employs: zero Roll, zero Pitch, measured Yaw. + auto rotFlat = manif::SO3d(rpy(0), rpy(1), rpy(2)); + // manif::SE3d reser corners pose matrix. + manif::SE3d T_reset_corners(tras, rotFlat); + int j = 0; + for (const auto& corner : m_cornersInInertialFrame) + { + m_cornersInInertialFrame[j] = T_reset_corners.act(corner); + j++; + } + } + m_isOutputValid = true; return true; From fed49d99eb600d54663f53681fa68f2a605dee65 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Thu, 21 Sep 2023 18:41:11 +0200 Subject: [PATCH 09/45] Modified the estimator to export foot corners position for debug --- .../BaseEstimatorFromFootIMU.h | 28 ++-- .../src/BaseEstimatorFromFootIMU.cpp | 129 ++++++++++++------ 2 files changed, 103 insertions(+), 54 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index 4116e79412..a10ff6f220 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -30,6 +30,9 @@ struct BaseEstimatorFromFootIMUState { manif::SE3d basePose; /**< final output of the estimator - pose of the robot root link. */ + std::vector sphereShadowCorners; + std::vector sphereFootCorners; + int supportCornerIndex; }; /** @@ -139,17 +142,17 @@ 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_p2 __|__ m_p1 __ + * | | | | + * +y___|__|__|___ | FOOT + * | | | | LENGTH + * |__|__| __| + * m_p3 | m_p4 + * REAR + * |_____| + * FOOT + * WIDTH * */ std::vector m_cornersInInertialFrame; /**< this implementation is considering @@ -171,6 +174,9 @@ class BaseEstimatorFromFootIMU desired quantities */ manif::SE3d m_resetFootCorners; /**< the final foot pose matrix used to reset corners positions in inertial frame when the foot is flat */ + manif::SE3d m_yawDrift; + Eigen::Vector3d m_trasOld; + Eigen::Vector3d m_rpyOld; manif::SE3d m_frame_H_link; /**< coordinate change matrix from foot link frame to foot sole * frame */ diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index b5804d9510..82dcdb7b64 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -158,6 +158,12 @@ bool BaseEstimatorFromFootIMU::initialize( iDynTree::Transform frame_H_link = m_model.getFrameTransform(m_frameIndex).inverse(); m_frame_H_link = Conversions::toManifPose(frame_H_link); + m_trasOld.setZero(); + m_rpyOld.setZero(); + m_yawDrift.setIdentity(); + m_state.sphereShadowCorners.resize(m_cornersInInertialFrame.size()); + m_state.sphereFootCorners.resize(m_cornersInInertialFrame.size()); + m_isInitialized = true; return ok; @@ -236,37 +242,37 @@ bool BaseEstimatorFromFootIMU::advance() // finding the index of the lowest corner. double minZ = cornersZ[0]; - int supportCornerIndex = 0; + m_state.supportCornerIndex = 0; for (int i = 1; i < cornersZ.size(); i++) { if (cornersZ[i] < minZ) { minZ = cornersZ[i]; - supportCornerIndex = i; + m_state.supportCornerIndex = i; } } // finding the index of the highest corner. - double maxZ = cornersZ[0]; - int highestCornerIndex = 0; - for (int i = 1; i < cornersZ.size(); i++) - { - if (cornersZ[i] > maxZ) - { - maxZ = cornersZ[i]; - highestCornerIndex = i; - } - } - - double deltaZ = cornersZ[highestCornerIndex] - cornersZ[supportCornerIndex]; + // double maxZ = cornersZ[0]; + // int highestCornerIndex = 0; + // for (int i = 1; i < cornersZ.size(); i++) + // { + // if (cornersZ[i] > maxZ) + // { + // maxZ = cornersZ[i]; + // highestCornerIndex = i; + // } + // } + + // double deltaZ = cornersZ[highestCornerIndex] - cornersZ[m_state.supportCornerIndex]; // std::cerr << "Foot deltaZ: " << deltaZ << std::endl; // checking that the index of the lowest corner is within the range [0, 3]. - if (!(0 <= supportCornerIndex <= 3)) + if (!(0 <= m_state.supportCornerIndex <= 3)) { log()->error("{} Foot vertex index out of bounds (0, 3): {}.", logPrefix, - supportCornerIndex); + m_state.supportCornerIndex); return false; } @@ -274,16 +280,32 @@ bool BaseEstimatorFromFootIMU::advance() // desired position. Eigen::Vector3d p_desired(0, 0, 0); Eigen::Vector3d vertexOffset(0, 0, 0); - p_desired = m_input.desiredFootPose.act(m_cornersInInertialFrame[supportCornerIndex]); - vertexOffset = p_desired - m_transformedFootCorners[supportCornerIndex]; + p_desired = m_yawDrift.act(m_cornersInInertialFrame[m_state.supportCornerIndex]); + vertexOffset = p_desired - m_yawDrift.act(m_transformedFootCorners[m_state.supportCornerIndex]); + // std::cerr << "shadowCorners: " << p_desired.transpose() << std::endl; + // std::cerr << "footCorners: " + // << m_yawDrift.act(m_transformedFootCorners[m_state.supportCornerIndex]).transpose() + // << std::endl; + // std::cerr << "vertexOffset: " << vertexOffset.transpose() << std::endl; + // if (vertexOffset.norm() > 0.1) + // { + // log()->error("{} Foot vertex offset too large: {}.", logPrefix, vertexOffset.norm()); + // return false; + // } + for (int i = 0; i < m_cornersInInertialFrame.size(); i++) + { + m_state.sphereShadowCorners[i] = m_yawDrift.act(m_cornersInInertialFrame[i]); + m_state.sphereFootCorners[i] = m_yawDrift.act(m_transformedFootCorners[i]); + } // transforming the offset vector into a translation matrix. manif::SE3d T_vertexOffset(vertexOffset, manif::SO3d::Identity()); + // manif::SE3d T_vertexOffset(manif::SE3d::Identity()); // obtaining the final foot pose using both measured and desired 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_resetFootCorners = T_vertexOffset * T_foot_raw; + m_measuredFootPose = m_yawDrift * T_vertexOffset * T_foot_raw * m_frame_H_link; + m_resetFootCorners = T_foot_raw; Eigen::VectorXd baseVelocity(6); baseVelocity.setZero(); @@ -303,31 +325,52 @@ bool BaseEstimatorFromFootIMU::advance() m_state.basePose = Conversions::toManifPose(m_kinDyn.getWorldTransform(m_baseFrame)); // updating m_cornersInLocalFrame when the foot is nearly flat - double flatnessThreshold = 0.0001; - if (deltaZ <= flatnessThreshold) + // double flatnessThreshold = 0.0001; + // if (deltaZ <= flatnessThreshold) + // extracting the position part of the reset corners pose. + Eigen::Vector3d deltaTras(0, 0, 0); + Eigen::Vector3d deltaRPY(0, 0, 0); + auto tras = m_resetFootCorners.translation(); + // foot z is completely canceled + tras(2) = 0.0; + deltaTras = tras - m_trasOld; + m_trasOld = tras; + // extracting the orientation part of the reset corners pose and expressing it through RPY + // Euler angles. + auto rot = m_resetFootCorners.rotation(); + auto rpy = toXYZ(rot); + // Roll and Pitch are completely canceled. + rpy(0) = 0.0; + rpy(1) = 0.0; + deltaRPY = rpy - m_rpyOld; + m_rpyOld = rpy; + // manif::SO3d rotation matrix that employs: zero Roll, zero Pitch, measured Yaw. + auto deltaYaw = manif::SO3d(deltaRPY(0), deltaRPY(1), deltaRPY(2)); + // manif::SE3d reser corners pose matrix. + manif::SE3d T_reset_corners(deltaTras, deltaYaw); + std::vector tempCorners; + tempCorners.resize(m_cornersInInertialFrame.size()); + int j = 0; + for (const auto& corner : m_cornersInInertialFrame) { - // extracting the position part of the reset corners pose. - auto tras = m_resetFootCorners.translation(); - // foot z is completely canceled - tras(2) = 0.0; - // extracting the orientation part of the reset corners pose and expressing it through RPY - // Euler angles. - auto rot = m_resetFootCorners.rotation(); - auto rpy = toXYZ(rot); - // Roll and Pitch are completely canceled. - rpy(0) = 0.0; - rpy(1) = 0.0; - // manif::SO3d rotation matrix that employs: zero Roll, zero Pitch, measured Yaw. - auto rotFlat = manif::SO3d(rpy(0), rpy(1), rpy(2)); - // manif::SE3d reser corners pose matrix. - manif::SE3d T_reset_corners(tras, rotFlat); - int j = 0; - for (const auto& corner : m_cornersInInertialFrame) - { - m_cornersInInertialFrame[j] = T_reset_corners.act(corner); - j++; - } + tempCorners[j] = T_reset_corners.act(corner); + j++; } + Eigen::Vector3d resetOffset(0, 0, 0); + resetOffset = m_cornersInInertialFrame[m_state.supportCornerIndex] + - tempCorners[m_state.supportCornerIndex]; + // std::cerr << "resetOffset: " << resetOffset.transpose() << std::endl; + manif::SE3d T_resetOffset(resetOffset, manif::SO3d::Identity()); + manif::SE3d temp; + temp = T_resetOffset * m_yawDrift; + m_yawDrift = temp; + + // j = 0; + // for (const auto& corner : tempCorners) + // { + // m_cornersInInertialFrame[j] = T_resetOffset.act(corner); + // j++; + // } m_isOutputValid = true; From 9a5a13bfbc9c678543c318cbcc6416def56ff331 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Thu, 28 Sep 2023 15:17:46 +0200 Subject: [PATCH 10/45] Typo in comments --- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 82dcdb7b64..5df46861ce 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -330,6 +330,7 @@ bool BaseEstimatorFromFootIMU::advance() // extracting the position part of the reset corners pose. Eigen::Vector3d deltaTras(0, 0, 0); Eigen::Vector3d deltaRPY(0, 0, 0); + auto tras = m_resetFootCorners.translation(); // foot z is completely canceled tras(2) = 0.0; @@ -346,7 +347,7 @@ bool BaseEstimatorFromFootIMU::advance() m_rpyOld = rpy; // manif::SO3d rotation matrix that employs: zero Roll, zero Pitch, measured Yaw. auto deltaYaw = manif::SO3d(deltaRPY(0), deltaRPY(1), deltaRPY(2)); - // manif::SE3d reser corners pose matrix. + // manif::SE3d reset corners pose matrix. manif::SE3d T_reset_corners(deltaTras, deltaYaw); std::vector tempCorners; tempCorners.resize(m_cornersInInertialFrame.size()); From a0e0157277077cb40d5cd8e01c47b035cbb0dad6 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Thu, 28 Sep 2023 19:10:50 +0200 Subject: [PATCH 11/45] It's finally working! --- .../BaseEstimatorFromFootIMU.h | 2 ++ .../src/BaseEstimatorFromFootIMU.cpp | 29 ++++++++++++++----- 2 files changed, 24 insertions(+), 7 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index a10ff6f220..f3d06d1f1c 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -170,6 +170,8 @@ class BaseEstimatorFromFootIMU 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 */ + manif::SO3d m_desiredRotationCasted; + manif::SO3d m_measuredTilt; manif::SE3d m_measuredFootPose; /**< the final foot pose matrix obtained through measured and desired quantities */ manif::SE3d m_resetFootCorners; /**< the final foot pose matrix used to reset corners positions diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 5df46861ce..07146d5bc8 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -196,6 +196,7 @@ bool BaseEstimatorFromFootIMU::advance() // through RPY Euler angles. m_desiredRotation = m_input.desiredFootPose.rotation(); m_desiredRPY = toXYZ(m_desiredRotation); + m_desiredRotationCasted = manif::SO3d(m_desiredRPY(0), m_desiredRPY(1), m_desiredRPY(2)); // casting the measured foot orientation manif::SO3d --> Eigen::Matrix3d. m_measuredRotation = m_input.measuredRotation.rotation(); @@ -208,10 +209,14 @@ bool BaseEstimatorFromFootIMU::advance() // manif::SO3d rotation matrix that employs: measured Roll, measured Pitch, // desired 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); // manif::SE3d pose matrix that employs: desired Position, measured Roll, // measured Pitch, desired Yaw. - manif::SE3d T_foot_raw(m_desiredTranslation, m_measuredRotationCorrected); + Eigen::Vector3d noTras(0, 0, 0); + manif::SE3d T_foot_raw(noTras, m_measuredRotationCorrected); + manif::SE3d T_foot_tilt(noTras, m_measuredTilt); + manif::SE3d T_foot(m_desiredTranslation, m_desiredRotationCasted); // finding the positions of the foot corners in world frame given `T_foot_raw` // pose matrix. @@ -222,7 +227,7 @@ bool BaseEstimatorFromFootIMU::advance() // for each corner we compute the position in the inertial frame for (const auto& corner : m_cornersInInertialFrame) { - m_transformedFootCorners.emplace_back(T_foot_raw.act(corner)); + m_transformedFootCorners.emplace_back(T_foot_tilt.act(corner)); } // The center of the foot sole is at ground level --> some corners may be @@ -280,8 +285,13 @@ bool BaseEstimatorFromFootIMU::advance() // desired position. Eigen::Vector3d p_desired(0, 0, 0); Eigen::Vector3d vertexOffset(0, 0, 0); - p_desired = m_yawDrift.act(m_cornersInInertialFrame[m_state.supportCornerIndex]); - vertexOffset = p_desired - m_yawDrift.act(m_transformedFootCorners[m_state.supportCornerIndex]); + // p_desired = m_yawDrift.act(m_cornersInInertialFrame[m_state.supportCornerIndex]); + // vertexOffset = p_desired - + // m_yawDrift.act(m_transformedFootCorners[m_state.supportCornerIndex]); + + p_desired = (m_cornersInInertialFrame[m_state.supportCornerIndex]); + vertexOffset = p_desired - (m_transformedFootCorners[m_state.supportCornerIndex]); + // std::cerr << "shadowCorners: " << p_desired.transpose() << std::endl; // std::cerr << "footCorners: " // << m_yawDrift.act(m_transformedFootCorners[m_state.supportCornerIndex]).transpose() @@ -304,7 +314,7 @@ bool BaseEstimatorFromFootIMU::advance() // obtaining the final foot pose using both measured and desired quantities. // cordinate change is performed from foot sole frame to foot link frame. - m_measuredFootPose = m_yawDrift * T_vertexOffset * T_foot_raw * m_frame_H_link; + m_measuredFootPose = T_foot * m_yawDrift * T_vertexOffset * T_foot_tilt * m_frame_H_link; m_resetFootCorners = T_foot_raw; Eigen::VectorXd baseVelocity(6); @@ -348,7 +358,10 @@ bool BaseEstimatorFromFootIMU::advance() // manif::SO3d rotation matrix that employs: zero Roll, zero Pitch, measured Yaw. auto deltaYaw = manif::SO3d(deltaRPY(0), deltaRPY(1), deltaRPY(2)); // manif::SE3d reset corners pose matrix. - manif::SE3d T_reset_corners(deltaTras, deltaYaw); + // manif::SE3d T_reset_corners(deltaTras, deltaYaw); + Eigen::Vector3d tr(0, 0, 0); + manif::SE3d T_reset_corners(tr, deltaYaw); + std::vector tempCorners; tempCorners.resize(m_cornersInInertialFrame.size()); int j = 0; @@ -361,7 +374,9 @@ bool BaseEstimatorFromFootIMU::advance() resetOffset = m_cornersInInertialFrame[m_state.supportCornerIndex] - tempCorners[m_state.supportCornerIndex]; // std::cerr << "resetOffset: " << resetOffset.transpose() << std::endl; - manif::SE3d T_resetOffset(resetOffset, manif::SO3d::Identity()); + // manif::SE3d T_resetOffset(resetOffset, manif::SO3d::Identity()); + manif::SE3d T_resetOffset(resetOffset, deltaYaw); + manif::SE3d temp; temp = T_resetOffset * m_yawDrift; m_yawDrift = temp; From 159004d76fd8b5f285a9c9a678f1fad24bb65cb4 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Thu, 28 Sep 2023 19:52:07 +0200 Subject: [PATCH 12/45] Adding a translation to cyan dots so that they are fixed to the robot foot --- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 07146d5bc8..59e210a2b0 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -302,16 +302,16 @@ bool BaseEstimatorFromFootIMU::advance() // log()->error("{} Foot vertex offset too large: {}.", logPrefix, vertexOffset.norm()); // return false; // } + // transforming the offset vector into a translation matrix. + manif::SE3d T_vertexOffset(vertexOffset, manif::SO3d::Identity()); + // manif::SE3d T_vertexOffset(manif::SE3d::Identity()); for (int i = 0; i < m_cornersInInertialFrame.size(); i++) { m_state.sphereShadowCorners[i] = m_yawDrift.act(m_cornersInInertialFrame[i]); - m_state.sphereFootCorners[i] = m_yawDrift.act(m_transformedFootCorners[i]); + m_state.sphereFootCorners[i] + = T_vertexOffset.act(m_yawDrift.act(m_transformedFootCorners[i])); } - // transforming the offset vector into a translation matrix. - manif::SE3d T_vertexOffset(vertexOffset, manif::SO3d::Identity()); - // manif::SE3d T_vertexOffset(manif::SE3d::Identity()); - // obtaining the final foot pose using both measured and desired quantities. // cordinate change is performed from foot sole frame to foot link frame. m_measuredFootPose = T_foot * m_yawDrift * T_vertexOffset * T_foot_tilt * m_frame_H_link; From 497e50dc423a4a1a7613122b568f637480ae2d2d Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Wed, 4 Oct 2023 14:57:05 +0200 Subject: [PATCH 13/45] Code updates related to first test on the robot --- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 59e210a2b0..790be2f156 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -205,11 +205,20 @@ bool BaseEstimatorFromFootIMU::advance() // desired Yaw is used instead of measured Yaw. // m_measuredRPY(2) = m_desiredRPY(2); + // double temp_roll = -m_measuredRPY(1); + // double temp_pitch = -m_measuredRPY(0); + // m_measuredRPY(0) = temp_roll; + // m_measuredRPY(1) = temp_pitch; + // m_measuredRPY(2) = -m_measuredRPY(2); // manif::SO3d rotation matrix that employs: measured Roll, measured Pitch, // desired 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); + // Dani's idea: + // consider either roll or pitch, whichever is larger in magnitude, and set the + // other to zero. + // m_measuredTilt = manif::SO3d(m_measuredRPY(0), 0.0, 0.0); // manif::SE3d pose matrix that employs: desired Position, measured Roll, // measured Pitch, desired Yaw. From 8f187341a06dd930a3bfe7bd977dfd346ba30764 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Wed, 4 Oct 2023 14:59:28 +0200 Subject: [PATCH 14/45] Comment updates related to first test on the robot --- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 790be2f156..6036126fdf 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -205,6 +205,8 @@ bool BaseEstimatorFromFootIMU::advance() // desired Yaw is used instead of measured Yaw. // m_measuredRPY(2) = m_desiredRPY(2); + + // Manual correction of the measured Roll, Pitch and Yaw. // double temp_roll = -m_measuredRPY(1); // double temp_pitch = -m_measuredRPY(0); // m_measuredRPY(0) = temp_roll; From fb4ecddc9d0a79eec9b142ca6f6e2d9610a286a6 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Wed, 1 Nov 2023 19:32:56 +0100 Subject: [PATCH 15/45] Better contact corner update, get CoM, code refactoring --- .../BaseEstimatorFromFootIMU.h | 6 ++-- .../src/BaseEstimatorFromFootIMU.cpp | 30 +++++++++++++------ 2 files changed, 25 insertions(+), 11 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index f3d06d1f1c..279b4742d0 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -30,6 +30,7 @@ struct BaseEstimatorFromFootIMUState { manif::SE3d basePose; /**< final output of the estimator - pose of the robot root link. */ + iDynTree::Span centerOfMassPosition; std::vector sphereShadowCorners; std::vector sphereFootCorners; int supportCornerIndex; @@ -186,14 +187,15 @@ class BaseEstimatorFromFootIMU iDynTree::KinDynComputations m_kinDyn; iDynTree::Model m_model; iDynTree::LinkIndex m_linkIndex; - int m_frameIndex; + int m_footFrameIndex; 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}; 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 6036126fdf..c5ed1d6194 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -133,8 +133,8 @@ bool BaseEstimatorFromFootIMU::initialize( m_cornersInInertialFrame.emplace_back(-m_footLength / 2, -m_footWidth / 2, 0); m_gravity << 0, 0, -BipedalLocomotion::Math::StandardAccelerationOfGravitation; - m_frameIndex = m_kinDyn.getFrameIndex(m_footFrameName); - if (m_frameIndex == iDynTree::FRAME_INVALID_INDEX) + m_footFrameIndex = m_kinDyn.getFrameIndex(m_footFrameName); + if (m_footFrameIndex == iDynTree::FRAME_INVALID_INDEX) { log()->error("{} Invalid frame named: {}", logPrefix, m_footFrameName); return false; @@ -146,8 +146,8 @@ bool BaseEstimatorFromFootIMU::initialize( return false; } - m_frameName = m_kinDyn.getFrameName(m_frameIndex); - m_linkIndex = m_model.getFrameLink(m_frameIndex); + m_frameName = m_kinDyn.getFrameName(m_footFrameIndex); + m_linkIndex = m_model.getFrameLink(m_footFrameIndex); if (!m_kinDyn.setFloatingBase(m_model.getLinkName(m_linkIndex))) { @@ -155,7 +155,7 @@ bool BaseEstimatorFromFootIMU::initialize( return false; } - iDynTree::Transform frame_H_link = m_model.getFrameTransform(m_frameIndex).inverse(); + iDynTree::Transform frame_H_link = m_model.getFrameTransform(m_footFrameIndex).inverse(); m_frame_H_link = Conversions::toManifPose(frame_H_link); m_trasOld.setZero(); @@ -171,7 +171,17 @@ bool BaseEstimatorFromFootIMU::initialize( bool BaseEstimatorFromFootIMU::setInput(const BaseEstimatorFromFootIMUInput& input) { + constexpr auto logPrefix = "[BaseEstimatorFromFootIMU::setInput]"; + m_isInputSet = false; m_input = input; + + if (!m_isInitialized) + { + log()->error("{} The estimator is not initialized.", logPrefix); + return false; + } + + m_isInputSet = true; return true; } @@ -181,9 +191,9 @@ bool BaseEstimatorFromFootIMU::advance() m_isOutputValid = false; - if (!m_isInitialized) + if (!m_isInputSet) { - log()->error("{} The estimator is not initialized.", logPrefix); + log()->error("{} The estimator input has not been set.", logPrefix); return false; } @@ -258,15 +268,16 @@ bool BaseEstimatorFromFootIMU::advance() // finding the index of the lowest corner. double minZ = cornersZ[0]; - m_state.supportCornerIndex = 0; + int indexMinZ = 0; for (int i = 1; i < cornersZ.size(); i++) { if (cornersZ[i] < minZ) { minZ = cornersZ[i]; - m_state.supportCornerIndex = i; + indexMinZ = i; } } + m_state.supportCornerIndex = indexMinZ; // finding the index of the highest corner. // double maxZ = cornersZ[0]; @@ -344,6 +355,7 @@ bool BaseEstimatorFromFootIMU::advance() // calculating the pose of the root link given the robot state. m_state.basePose = Conversions::toManifPose(m_kinDyn.getWorldTransform(m_baseFrame)); + m_kinDyn.getCenterOfMassPosition(m_state.centerOfMassPosition); // updating m_cornersInLocalFrame when the foot is nearly flat // double flatnessThreshold = 0.0001; From 3c7ca6f5a0cbfc151fa3e227bf49f4973b672f6a Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Fri, 3 Nov 2023 12:57:37 +0100 Subject: [PATCH 16/45] Minor code refactoring --- .../BaseEstimatorFromFootIMU.h | 27 +++++++++++-------- .../src/BaseEstimatorFromFootIMU.cpp | 23 +++++++++------- 2 files changed, 30 insertions(+), 20 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index 279b4742d0..cf1ed23ab2 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -30,7 +30,8 @@ struct BaseEstimatorFromFootIMUState { manif::SE3d basePose; /**< final output of the estimator - pose of the robot root link. */ - iDynTree::Span centerOfMassPosition; + // iDynTree::Span centerOfMassPosition; + Eigen::Vector3d centerOfMassPosition; std::vector sphereShadowCorners; std::vector sphereFootCorners; int supportCornerIndex; @@ -54,13 +55,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 @@ -144,13 +149,13 @@ class BaseEstimatorFromFootIMU * Define the 4 foot vertices in World reference frame: * * +x FRONT - * m_p2 __|__ m_p1 __ + * m_p1 __|__ m_p0 __ * | | | | * +y___|__|__|___ | FOOT * | | | | LENGTH * |__|__| __| - * m_p3 | m_p4 - * REAR + * m_p2 | m_p3 + * HIND * |_____| * FOOT * WIDTH diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index c5ed1d6194..ea3c061b7b 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -279,6 +279,15 @@ bool BaseEstimatorFromFootIMU::advance() } m_state.supportCornerIndex = indexMinZ; + // checking that the index of the lowest corner is within the range [0, 3]. + if (!(0 <= m_state.supportCornerIndex <= 3)) + { + log()->error("{} Foot vertex index out of bounds (0, 3): {}.", + logPrefix, + m_state.supportCornerIndex); + return false; + } + // finding the index of the highest corner. // double maxZ = cornersZ[0]; // int highestCornerIndex = 0; @@ -294,15 +303,6 @@ bool BaseEstimatorFromFootIMU::advance() // double deltaZ = cornersZ[highestCornerIndex] - cornersZ[m_state.supportCornerIndex]; // std::cerr << "Foot deltaZ: " << deltaZ << std::endl; - // checking that the index of the lowest corner is within the range [0, 3]. - if (!(0 <= m_state.supportCornerIndex <= 3)) - { - log()->error("{} Foot vertex index out of bounds (0, 3): {}.", - logPrefix, - m_state.supportCornerIndex); - return false; - } - // finding the offset vector needed to bring the lowest corner back to its // desired position. Eigen::Vector3d p_desired(0, 0, 0); @@ -336,7 +336,12 @@ bool BaseEstimatorFromFootIMU::advance() // obtaining the final foot pose using both measured and desired quantities. // cordinate change is performed from foot sole frame to foot link frame. + m_measuredFootPose = T_foot * m_yawDrift * T_vertexOffset * T_foot_tilt * m_frame_H_link; + + // Example if we want to switch off the yaw drift correction + // m_measuredFootPose = T_foot * T_vertexOffset * T_foot_tilt * m_frame_H_link; + m_resetFootCorners = T_foot_raw; Eigen::VectorXd baseVelocity(6); From cbb5a457c9b8749289c96d5a1181fbd6060732b3 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Fri, 3 Nov 2023 19:54:46 +0100 Subject: [PATCH 17/45] Minor visualization changes --- .../FloatingBaseEstimators/BaseEstimatorFromFootIMU.h | 2 ++ src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 4 ++++ 2 files changed, 6 insertions(+) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index cf1ed23ab2..b8299eebf2 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -30,6 +30,8 @@ struct BaseEstimatorFromFootIMUState { manif::SE3d basePose; /**< final output of the estimator - pose of the robot root link. */ + manif::SE3d footPose_R; /**< pose of the right foot */ + manif::SE3d footPose_L; /**< pose of the left foot */ // iDynTree::Span centerOfMassPosition; Eigen::Vector3d centerOfMassPosition; std::vector sphereShadowCorners; diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index ea3c061b7b..ec3d9ab813 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -360,6 +360,10 @@ bool BaseEstimatorFromFootIMU::advance() // calculating the pose of the root link given the robot state. m_state.basePose = Conversions::toManifPose(m_kinDyn.getWorldTransform(m_baseFrame)); + m_state.footPose_R = Conversions::toManifPose(m_kinDyn.getWorldTransform(m_footFrameIndex)); + m_state.footPose_L = Conversions::toManifPose( + m_kinDyn.getWorldTransform(m_kinDyn.getFrameIndex("l_sole"))); // TODO: make it generic + m_kinDyn.getCenterOfMassPosition(m_state.centerOfMassPosition); // updating m_cornersInLocalFrame when the foot is nearly flat From 4524af45299d6ff757a86d1dc983ac683d351556 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Tue, 14 Nov 2023 19:34:40 +0100 Subject: [PATCH 18/45] Edited class BaseEstimatorFromFootIMU to enable stance foot switch --- .../BaseEstimatorFromFootIMU.h | 74 ++-- .../src/BaseEstimatorFromFootIMU.cpp | 355 ++++++++++-------- 2 files changed, 240 insertions(+), 189 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index b8299eebf2..03f1affe7b 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -32,10 +32,9 @@ struct BaseEstimatorFromFootIMUState root link. */ manif::SE3d footPose_R; /**< pose of the right foot */ manif::SE3d footPose_L; /**< pose of the left foot */ - // iDynTree::Span centerOfMassPosition; Eigen::Vector3d centerOfMassPosition; - std::vector sphereShadowCorners; - std::vector sphereFootCorners; + std::vector stanceFootShadowCorners; + std::vector stanceFootCorners; int supportCornerIndex; }; @@ -44,12 +43,16 @@ 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 */ }; /** @@ -165,40 +168,51 @@ class BaseEstimatorFromFootIMU */ 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 */ 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 */ - manif::SO3d m_desiredRotationCasted; + 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_resetFootCorners; /**< the final foot pose matrix used to reset corners positions - in inertial frame when the foot is flat */ - manif::SE3d m_yawDrift; - Eigen::Vector3d m_trasOld; - Eigen::Vector3d m_rpyOld; - 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; iDynTree::KinDynComputations m_kinDyn; iDynTree::Model m_model; - iDynTree::LinkIndex m_linkIndex; - int m_footFrameIndex; - 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)*/ + 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_isModelSet{false}; bool m_isInitialized{false}; bool m_isInputSet{false}; diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index ec3d9ab813..87056e104b 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -111,58 +111,74 @@ bool BaseEstimatorFromFootIMU::initialize( return true; }; - // Base link of the robot (whose pose must be estimated) - std::string baseFrameName; auto baseEstimatorPtr = ptr->getGroup("BASE_ESTIMATOR").lock(); - bool ok - = populateParameter(baseEstimatorPtr->getGroup("MODEL_INFO"), "base_frame", baseFrameName); - - // Frame associated to the foot of the robot (whose orientation is measured) - ok = populateParameter(baseEstimatorPtr->getGroup("MODEL_INFO"), "foot_frame", m_footFrameName); + if (baseEstimatorPtr == nullptr) + { + log()->error("{} Invalid parameter handler for the group 'BASE_ESTIMATOR'", logPrefix); + return false; + } + // 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); - // resetting the vector of foot corners to be sure it is correctly initialized. - m_cornersInInertialFrame.clear(); - - // 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_gravity << 0, 0, -BipedalLocomotion::Math::StandardAccelerationOfGravitation; - m_footFrameIndex = m_kinDyn.getFrameIndex(m_footFrameName); - if (m_footFrameIndex == 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_footFrameIndex); - m_linkIndex = m_model.getFrameLink(m_footFrameIndex); - - 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_footFrameIndex).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_trasOld.setZero(); - m_rpyOld.setZero(); - m_yawDrift.setIdentity(); - m_state.sphereShadowCorners.resize(m_cornersInInertialFrame.size()); - m_state.sphereFootCorners.resize(m_cornersInInertialFrame.size()); + 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; @@ -173,14 +189,14 @@ bool BaseEstimatorFromFootIMU::setInput(const BaseEstimatorFromFootIMUInput& inp { constexpr auto logPrefix = "[BaseEstimatorFromFootIMU::setInput]"; m_isInputSet = false; - m_input = input; if (!m_isInitialized) { - log()->error("{} The estimator is not initialized.", logPrefix); + log()->error("{} The estimator must be initialized before setting the input.", logPrefix); return false; } + m_input = input; m_isInputSet = true; return true; } @@ -197,58 +213,114 @@ bool BaseEstimatorFromFootIMU::advance() return false; } - // `desiredFootPose` is intended as the output of the footstep planner. + // 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(); + // has the stance foot just changed? + if (m_isLastStanceFoot_R) + { + // updating the yawOld value. + Eigen::Vector3d lastRPY_L = toXYZ(m_state.footPose_L.rotation()); + m_yawOld = lastRPY_L(2); + + // updating the m_T_walk matrix. + manif::SE3d T_walk = m_state.footPose_L; + manif::SE3d temp = T_walk * m_T_walk; + m_T_walk = temp; + } + } + 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(); + // has the stance foot just changed? + if (m_isLastStanceFoot_L) + { + // updating the yawOld value. + Eigen::Vector3d lastRPY_R = toXYZ(m_state.footPose_R.rotation()); + m_yawOld = lastRPY_R(2); + + // updating the m_T_walk matrix. + manif::SE3d T_walk = m_state.footPose_R; + manif::SE3d temp = T_walk * m_T_walk; + m_T_walk = temp; + } + } + + m_stanceLinkIndex = m_model.getFrameLink(stanceFootFrameIndex); + if (!m_kinDyn.setFloatingBase(m_model.getLinkName(m_stanceLinkIndex))) + { + log()->error("{} Unable to set the stance foot as floating base.", logPrefix); + return false; + } + + // `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 = toXYZ(m_desiredRotation); - m_desiredRotationCasted = manif::SO3d(m_desiredRPY(0), m_desiredRPY(1), m_desiredRPY(2)); + m_offsetRotation = m_input.offsetStanceFootPose.rotation(); + m_offsetRPY = toXYZ(m_offsetRotation); + m_offsetRotationCasted = manif::SO3d(m_offsetRPY(0), m_offsetRPY(1), m_offsetRPY(2)); - // casting the measured foot orientation manif::SO3d --> Eigen::Matrix3d. - m_measuredRotation = m_input.measuredRotation.rotation(); - // expressing this orientation through RPY Euler angles. + // expressing measured orientation through RPY Euler angles. m_measuredRPY = toXYZ(m_measuredRotation); - // desired Yaw is used instead of measured Yaw. - // m_measuredRPY(2) = m_desiredRPY(2); + // offset Yaw is used instead of measured Yaw. + // m_measuredRPY(2) = m_offsetRPY(2); - // Manual correction of the measured Roll, Pitch and Yaw. + // 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) = -m_measuredRPY(2); + // 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); - // Dani's idea: - // consider either roll or pitch, whichever is larger in magnitude, and set the - // other to zero. - // m_measuredTilt = manif::SO3d(m_measuredRPY(0), 0.0, 0.0); - - // manif::SE3d pose matrix that employs: desired Position, measured Roll, - // measured Pitch, desired Yaw. - Eigen::Vector3d noTras(0, 0, 0); - manif::SE3d T_foot_raw(noTras, m_measuredRotationCorrected); + double measuredYaw = m_measuredRPY(2); + + // manif::SE3d pose matrix that employs: offset Position, measured Roll, measured Pitch, offset + // Yaw. + const Eigen::Vector3d noTras(0, 0, 0); + manif::SE3d T_foot_imu(noTras, m_measuredRotationCorrected); manif::SE3d T_foot_tilt(noTras, m_measuredTilt); - manif::SE3d T_foot(m_desiredTranslation, m_desiredRotationCasted); + 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_transformedFootCorners.clear(); + m_tiltedFootCorners.clear(); // for each corner we compute the position in the inertial frame for (const auto& corner : m_cornersInInertialFrame) { - m_transformedFootCorners.emplace_back(T_foot_tilt.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 @@ -257,10 +329,10 @@ bool BaseEstimatorFromFootIMU::advance() // extracting the vertical quotes of the foot corners and finding the lowest // corner. Eigen::VectorXd cornersZ; - cornersZ.resize(m_transformedFootCorners.size()); + cornersZ.resize(m_cornersInInertialFrame.size()); cornersZ.setZero(); int index = 0; - for (const auto& corner : m_transformedFootCorners) + for (const auto& corner : m_tiltedFootCorners) { cornersZ[index] = corner[2]; index++; @@ -290,59 +362,37 @@ bool BaseEstimatorFromFootIMU::advance() // finding the index of the highest corner. // double maxZ = cornersZ[0]; - // int highestCornerIndex = 0; + // int indexMaxZ = 0; // for (int i = 1; i < cornersZ.size(); i++) // { // if (cornersZ[i] > maxZ) // { // maxZ = cornersZ[i]; - // highestCornerIndex = i; + // indexMaxZ = i; // } // } - // double deltaZ = cornersZ[highestCornerIndex] - cornersZ[m_state.supportCornerIndex]; + // double deltaZ = cornersZ[indexMaxZ] - cornersZ[m_state.supportCornerIndex]; // std::cerr << "Foot deltaZ: " << deltaZ << std::endl; - // finding the offset vector needed to bring the lowest corner back to its - // desired position. - Eigen::Vector3d p_desired(0, 0, 0); - Eigen::Vector3d vertexOffset(0, 0, 0); - // p_desired = m_yawDrift.act(m_cornersInInertialFrame[m_state.supportCornerIndex]); - // vertexOffset = p_desired - - // m_yawDrift.act(m_transformedFootCorners[m_state.supportCornerIndex]); - - p_desired = (m_cornersInInertialFrame[m_state.supportCornerIndex]); - vertexOffset = p_desired - (m_transformedFootCorners[m_state.supportCornerIndex]); - - // std::cerr << "shadowCorners: " << p_desired.transpose() << std::endl; - // std::cerr << "footCorners: " - // << m_yawDrift.act(m_transformedFootCorners[m_state.supportCornerIndex]).transpose() - // << std::endl; - // std::cerr << "vertexOffset: " << vertexOffset.transpose() << std::endl; - // if (vertexOffset.norm() > 0.1) - // { - // log()->error("{} Foot vertex offset too large: {}.", logPrefix, vertexOffset.norm()); - // return false; - // } + // 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_vertexOffset(manif::SE3d::Identity()); - for (int i = 0; i < m_cornersInInertialFrame.size(); i++) - { - m_state.sphereShadowCorners[i] = m_yawDrift.act(m_cornersInInertialFrame[i]); - m_state.sphereFootCorners[i] - = T_vertexOffset.act(m_yawDrift.act(m_transformedFootCorners[i])); - } + manif::SE3d T_supportCornerTranslation(supportCornerTranslation, manif::SO3d::Identity()); - // 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_foot * m_yawDrift * T_vertexOffset * T_foot_tilt * m_frame_H_link; - - // Example if we want to switch off the yaw drift correction - // m_measuredFootPose = T_foot * T_vertexOffset * T_foot_tilt * m_frame_H_link; - - m_resetFootCorners = T_foot_raw; + m_measuredFootPose = m_T_walk * T_foot_offset * m_T_yawDrift * T_supportCornerTranslation + * T_foot_tilt * stanceFootFrame_H_link; Eigen::VectorXd baseVelocity(6); baseVelocity.setZero(); @@ -354,71 +404,58 @@ 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)); - m_state.footPose_R = Conversions::toManifPose(m_kinDyn.getWorldTransform(m_footFrameIndex)); - m_state.footPose_L = Conversions::toManifPose( - m_kinDyn.getWorldTransform(m_kinDyn.getFrameIndex("l_sole"))); // TODO: make it generic - + // 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_kinDyn.getCenterOfMassPosition(m_state.centerOfMassPosition); - // updating m_cornersInLocalFrame when the foot is nearly flat - // double flatnessThreshold = 0.0001; - // if (deltaZ <= flatnessThreshold) - // extracting the position part of the reset corners pose. - Eigen::Vector3d deltaTras(0, 0, 0); - Eigen::Vector3d deltaRPY(0, 0, 0); - - auto tras = m_resetFootCorners.translation(); - // foot z is completely canceled - tras(2) = 0.0; - deltaTras = tras - m_trasOld; - m_trasOld = tras; - // extracting the orientation part of the reset corners pose and expressing it through RPY - // Euler angles. - auto rot = m_resetFootCorners.rotation(); - auto rpy = toXYZ(rot); - // Roll and Pitch are completely canceled. - rpy(0) = 0.0; - rpy(1) = 0.0; - deltaRPY = rpy - m_rpyOld; - m_rpyOld = rpy; - // manif::SO3d rotation matrix that employs: zero Roll, zero Pitch, measured Yaw. - auto deltaYaw = manif::SO3d(deltaRPY(0), deltaRPY(1), deltaRPY(2)); - // manif::SE3d reset corners pose matrix. - // manif::SE3d T_reset_corners(deltaTras, deltaYaw); - Eigen::Vector3d tr(0, 0, 0); - manif::SE3d T_reset_corners(tr, deltaYaw); + for (int i = 0; i < m_cornersInInertialFrame.size(); i++) + { + m_state.stanceFootShadowCorners[i] = m_T_yawDrift.act(m_cornersInInertialFrame[i]); + m_state.stanceFootCorners[i] + = T_supportCornerTranslation.act(m_T_yawDrift.act(m_tiltedFootCorners[i])); + } + + // calculating the yaw drift. + 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(noTras, R_deltaYaw); std::vector tempCorners; tempCorners.resize(m_cornersInInertialFrame.size()); int j = 0; for (const auto& corner : m_cornersInInertialFrame) { - tempCorners[j] = T_reset_corners.act(corner); + tempCorners[j] = T_deltaYaw.act(corner); j++; } - Eigen::Vector3d resetOffset(0, 0, 0); - resetOffset = m_cornersInInertialFrame[m_state.supportCornerIndex] - - tempCorners[m_state.supportCornerIndex]; - // std::cerr << "resetOffset: " << resetOffset.transpose() << std::endl; - // manif::SE3d T_resetOffset(resetOffset, manif::SO3d::Identity()); - manif::SE3d T_resetOffset(resetOffset, deltaYaw); - - manif::SE3d temp; - temp = T_resetOffset * m_yawDrift; - m_yawDrift = temp; - - // j = 0; - // for (const auto& corner : tempCorners) - // { - // m_cornersInInertialFrame[j] = T_resetOffset.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 temp = T_yawDrift * m_T_yawDrift; + m_T_yawDrift = temp; + + // 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; From 82a63c33f923d8a826c6e7327fc5b89253f1cbb2 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Tue, 21 Nov 2023 19:04:07 +0100 Subject: [PATCH 19/45] Pose update fix. Calculating the offset at each step. --- .../FloatingBaseEstimators/BaseEstimatorFromFootIMU.h | 2 +- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 10 +++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index 03f1affe7b..59bded71ba 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -30,8 +30,8 @@ struct BaseEstimatorFromFootIMUState { manif::SE3d basePose; /**< final output of the estimator - pose of the robot root link. */ - manif::SE3d footPose_R; /**< pose of the right foot */ 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; diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 87056e104b..4e06cb8450 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -242,7 +242,9 @@ bool BaseEstimatorFromFootIMU::advance() m_yawOld = lastRPY_L(2); // updating the m_T_walk matrix. - manif::SE3d T_walk = m_state.footPose_L; + manif::SE3d T_walk((m_state.footPose_L.translation() + - m_state.footPose_R.translation()), + manif::SO3d::Identity()); manif::SE3d temp = T_walk * m_T_walk; m_T_walk = temp; } @@ -261,7 +263,9 @@ bool BaseEstimatorFromFootIMU::advance() m_yawOld = lastRPY_R(2); // updating the m_T_walk matrix. - manif::SE3d T_walk = m_state.footPose_R; + manif::SE3d T_walk((m_state.footPose_R.translation() + - m_state.footPose_L.translation()), + manif::SO3d::Identity()); manif::SE3d temp = T_walk * m_T_walk; m_T_walk = temp; } @@ -421,7 +425,7 @@ bool BaseEstimatorFromFootIMU::advance() = T_supportCornerTranslation.act(m_T_yawDrift.act(m_tiltedFootCorners[i])); } - // calculating the yaw drift. + // 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; From 134015f5afeca8b0941de2fc839d608ce1575f13 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Thu, 23 Nov 2023 16:38:21 +0100 Subject: [PATCH 20/45] Fixed stance foot corners visualization --- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 4e06cb8450..144aeb6400 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -420,9 +420,10 @@ bool BaseEstimatorFromFootIMU::advance() for (int i = 0; i < m_cornersInInertialFrame.size(); i++) { - m_state.stanceFootShadowCorners[i] = m_T_yawDrift.act(m_cornersInInertialFrame[i]); - m_state.stanceFootCorners[i] - = T_supportCornerTranslation.act(m_T_yawDrift.act(m_tiltedFootCorners[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]))); } // calculating the yaw drift - VALID FOR BOTH FEET ONLY IF FRAMES ARE ORIENTED IN THE SAME WAY. From 840d3740405a28311ab473462e5e5581a278f51f Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Mon, 18 Dec 2023 16:32:57 +0100 Subject: [PATCH 21/45] Fix discontinuity in the contact foot yaw angle while turning --- .../src/BaseEstimatorFromFootIMU.cpp | 51 ++++++++++--------- 1 file changed, 27 insertions(+), 24 deletions(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 144aeb6400..d182afdfcc 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -239,6 +239,7 @@ bool BaseEstimatorFromFootIMU::advance() { // updating the yawOld value. Eigen::Vector3d lastRPY_L = toXYZ(m_state.footPose_L.rotation()); + // Eigen::Vector3d lastRPY_L = toXYZ(m_input.measuredRotation_L.rotation()); m_yawOld = lastRPY_L(2); // updating the m_T_walk matrix. @@ -260,6 +261,7 @@ bool BaseEstimatorFromFootIMU::advance() { // updating the yawOld value. Eigen::Vector3d lastRPY_R = toXYZ(m_state.footPose_R.rotation()); + // Eigen::Vector3d lastRPY_R = toXYZ(m_input.measuredRotation_R.rotation()); m_yawOld = lastRPY_R(2); // updating the m_T_walk matrix. @@ -393,6 +395,31 @@ bool BaseEstimatorFromFootIMU::advance() // transforming the offset vector into a translation matrix. 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(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(noTras, R_deltaYaw); + + manif::SE3d temp = T_yawDrift * m_T_yawDrift; + m_T_yawDrift = temp; + // 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 = m_T_walk * T_foot_offset * m_T_yawDrift * T_supportCornerTranslation @@ -426,30 +453,6 @@ bool BaseEstimatorFromFootIMU::advance() m_T_yawDrift.act(T_supportCornerTranslation.act(m_tiltedFootCorners[i]))); } - // 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(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 temp = T_yawDrift * m_T_yawDrift; - m_T_yawDrift = temp; - // updating the stance foot flags. if (m_input.isLeftStance) { From d84c6963c1a3390d2e0cd610161320425e860976 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Wed, 3 Jan 2024 16:25:59 +0100 Subject: [PATCH 22/45] Ignore delta Z between steps. --- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index d182afdfcc..b185ecd703 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -243,9 +243,9 @@ bool BaseEstimatorFromFootIMU::advance() m_yawOld = lastRPY_L(2); // updating the m_T_walk matrix. - manif::SE3d T_walk((m_state.footPose_L.translation() - - m_state.footPose_R.translation()), - manif::SO3d::Identity()); + Eigen::Vector3d step = (m_state.footPose_L.translation() - m_state.footPose_R.translation()); + step(2) = 0.0; + manif::SE3d T_walk(step, manif::SO3d::Identity()); manif::SE3d temp = T_walk * m_T_walk; m_T_walk = temp; } @@ -265,9 +265,9 @@ bool BaseEstimatorFromFootIMU::advance() m_yawOld = lastRPY_R(2); // updating the m_T_walk matrix. - manif::SE3d T_walk((m_state.footPose_R.translation() - - m_state.footPose_L.translation()), - manif::SO3d::Identity()); + Eigen::Vector3d step = (m_state.footPose_R.translation() - m_state.footPose_L.translation()); + step(2) = 0.0; + manif::SE3d T_walk(step, manif::SO3d::Identity()); manif::SE3d temp = T_walk * m_T_walk; m_T_walk = temp; } From 825010ba0e892c5c2da82d85c9c769f620d206a7 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Thu, 4 Jan 2024 14:35:30 +0100 Subject: [PATCH 23/45] Added a zero translation vector as class member. --- .../BaseEstimatorFromFootIMU.h | 1 + src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index 59bded71ba..6d344333b6 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -195,6 +195,7 @@ class BaseEstimatorFromFootIMU * 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_stanceLinkIndex; diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index b185ecd703..25054c383b 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -228,6 +228,7 @@ bool BaseEstimatorFromFootIMU::advance() } 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; @@ -312,9 +313,8 @@ bool BaseEstimatorFromFootIMU::advance() // manif::SE3d pose matrix that employs: offset Position, measured Roll, measured Pitch, offset // Yaw. - const Eigen::Vector3d noTras(0, 0, 0); - manif::SE3d T_foot_imu(noTras, m_measuredRotationCorrected); - manif::SE3d T_foot_tilt(noTras, m_measuredTilt); + 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_imu` @@ -401,7 +401,7 @@ bool BaseEstimatorFromFootIMU::advance() 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(noTras, R_deltaYaw); + manif::SE3d T_deltaYaw(m_noTras, R_deltaYaw); std::vector tempCorners; tempCorners.resize(m_cornersInInertialFrame.size()); @@ -415,14 +415,14 @@ bool BaseEstimatorFromFootIMU::advance() yawDrift = m_cornersInInertialFrame[m_state.supportCornerIndex] - tempCorners[m_state.supportCornerIndex]; // manif::SE3d T_yawDrift(yawDrift, R_deltaYaw); - manif::SE3d T_yawDrift(noTras, 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 offset quantities. // cordinate change is performed from foot sole frame to foot link frame. - m_measuredFootPose = m_T_walk * T_foot_offset * m_T_yawDrift * T_supportCornerTranslation + m_measuredFootPose = T_foot_offset * m_T_walk * m_T_yawDrift * T_supportCornerTranslation * T_foot_tilt * stanceFootFrame_H_link; Eigen::VectorXd baseVelocity(6); From 63d6af4418b34b67b8f7ffe98974c452c63820a0 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Thu, 4 Jan 2024 20:25:59 +0100 Subject: [PATCH 24/45] Improved setModel method, added debug console prints. --- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 25054c383b..619ded90b2 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -51,14 +51,14 @@ bool BaseEstimatorFromFootIMU::setModel(const iDynTree::Model& model) m_isModelSet = false; - m_model = model; - if (!m_kinDyn.loadRobotModel(model)) { log()->error("{} Unable to load the model.", logPrefix); return false; } + m_model = model; + m_isModelSet = true; return true; @@ -453,6 +453,13 @@ bool BaseEstimatorFromFootIMU::advance() m_T_yawDrift.act(T_supportCornerTranslation.act(m_tiltedFootCorners[i]))); } + + 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) { From 69a19f8266e9e731ecc9932c95655f833643bdcd Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Fri, 5 Jan 2024 16:34:48 +0100 Subject: [PATCH 25/45] Added warning for error between input and output feet rotations above threshold. --- .../src/BaseEstimatorFromFootIMU.cpp | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 619ded90b2..b491b5accb 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -453,12 +453,22 @@ bool BaseEstimatorFromFootIMU::advance() 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(); + double orientationError_R = (toXYZ(m_state.footPose_R.rotation()) - toXYZ(m_input.measuredRotation_R.rotation())).norm(); + double orientationErrorThreshold = 0.001; // [rad]. TODO: get parameter from config file. 0,001 rad = 0,05729578 deg. - 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; - + if ( (orientationError_L > orientationErrorThreshold) || (orientationError_R > orientationErrorThreshold) ) + { + log()->warn("{} Foot orientation error above {} threshold: {} Left, {} Right.", + logPrefix, + orientationErrorThreshold, + orientationError_L, + orientationError_R); + } + // err << "L FOOT ROTATION IN: " << toXYZ(m_input.measuredRotation_L.rotation()).transpose() << std::endl; + // err << "L FOOT ROTATION OUT: " << toXYZ(m_state.footPose_L.rotation()).transpose() << std::endl; + // err << "R FOOT ROTATION IN: " << toXYZ(m_input.measuredRotation_R.rotation()).transpose() << std::endl; + // err << "R FOOT ROTATION OUT: " << toXYZ(m_state.footPose_R.rotation()).transpose() << std::endl; // updating the stance foot flags. if (m_input.isLeftStance) From 9e4434be6499b383281aefb41330fe28b58f8653 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Sat, 6 Jan 2024 23:02:28 +0100 Subject: [PATCH 26/45] Bug fix: yaw discontynuity at stance switch for the first of a direction change. --- .../src/BaseEstimatorFromFootIMU.cpp | 43 +++++++++++++------ 1 file changed, 31 insertions(+), 12 deletions(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index b491b5accb..732d57433c 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -239,18 +239,28 @@ bool BaseEstimatorFromFootIMU::advance() if (m_isLastStanceFoot_R) { // updating the yawOld value. - Eigen::Vector3d lastRPY_L = toXYZ(m_state.footPose_L.rotation()); + Eigen::Vector3d lastRPY_R = toXYZ(m_state.footPose_R.rotation()); // Eigen::Vector3d lastRPY_L = toXYZ(m_input.measuredRotation_L.rotation()); - m_yawOld = lastRPY_L(2); + m_yawOld = lastRPY_R(2); // updating the m_T_walk matrix. - Eigen::Vector3d step = (m_state.footPose_L.translation() - m_state.footPose_R.translation()); - step(2) = 0.0; - manif::SE3d T_walk(step, manif::SO3d::Identity()); - manif::SE3d temp = T_walk * m_T_walk; + 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; @@ -261,16 +271,25 @@ bool BaseEstimatorFromFootIMU::advance() if (m_isLastStanceFoot_L) { // updating the yawOld value. - Eigen::Vector3d lastRPY_R = toXYZ(m_state.footPose_R.rotation()); + Eigen::Vector3d lastRPY_L = toXYZ(m_state.footPose_L.rotation()); // Eigen::Vector3d lastRPY_R = toXYZ(m_input.measuredRotation_R.rotation()); - m_yawOld = lastRPY_R(2); + m_yawOld = lastRPY_L(2); // updating the m_T_walk matrix. - Eigen::Vector3d step = (m_state.footPose_R.translation() - m_state.footPose_L.translation()); - step(2) = 0.0; - manif::SE3d T_walk(step, manif::SO3d::Identity()); - manif::SE3d temp = T_walk * m_T_walk; + 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(); } } From 3ebecdacc1ddfa4eaafd761149f388a870979cf9 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Tue, 9 Jan 2024 14:08:43 +0100 Subject: [PATCH 27/45] Typo fix. --- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 732d57433c..99a2dd2618 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -484,10 +484,10 @@ bool BaseEstimatorFromFootIMU::advance() orientationError_L, orientationError_R); } - // err << "L FOOT ROTATION IN: " << toXYZ(m_input.measuredRotation_L.rotation()).transpose() << std::endl; - // err << "L FOOT ROTATION OUT: " << toXYZ(m_state.footPose_L.rotation()).transpose() << std::endl; - // err << "R FOOT ROTATION IN: " << toXYZ(m_input.measuredRotation_R.rotation()).transpose() << std::endl; - // err << "R FOOT ROTATION OUT: " << toXYZ(m_state.footPose_R.rotation()).transpose() << std::endl; + // 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) From 9619d08d0dba6c5817a9edf784b90f25c393865b Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 8 Jan 2024 12:54:39 +0100 Subject: [PATCH 28/45] Avoid to call BufferedPort::prepare every time VectorsCollectionServer::VectorsCollectionServer is called --- .../YarpUtilities/src/VectorsCollection.cpp | 31 +++++++++---- .../YarpUtilities/VectorsCollectionServer.h | 14 +++++- .../src/VectorsCollectionServer.cpp | 45 ++++++++++++++++--- 3 files changed, 74 insertions(+), 16 deletions(-) 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/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() From b1f01e43644ba7008b311d3040fa04272cf0f22d Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Fri, 12 Jan 2024 11:49:58 +0100 Subject: [PATCH 29/45] Revert unwanted changes. --- .../BipedalLocomotion/JointPositionTracking/Module.h | 2 +- utilities/joint-position-tracking/src/Module.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/utilities/joint-position-tracking/include/BipedalLocomotion/JointPositionTracking/Module.h b/utilities/joint-position-tracking/include/BipedalLocomotion/JointPositionTracking/Module.h index d392be73da..8928bac923 100644 --- a/utilities/joint-position-tracking/include/BipedalLocomotion/JointPositionTracking/Module.h +++ b/utilities/joint-position-tracking/include/BipedalLocomotion/JointPositionTracking/Module.h @@ -19,9 +19,9 @@ #include #include -#include #include #include +#include namespace BipedalLocomotion { diff --git a/utilities/joint-position-tracking/src/Module.cpp b/utilities/joint-position-tracking/src/Module.cpp index 927fedc688..b4cc748815 100644 --- a/utilities/joint-position-tracking/src/Module.cpp +++ b/utilities/joint-position-tracking/src/Module.cpp @@ -10,9 +10,9 @@ #include #include -#include #include #include +#include #include @@ -113,27 +113,27 @@ bool Module::configure(yarp::os::ResourceFinder& rf) auto parametersHandler = std::make_shared(rf); std::string name; - if (!parametersHandler->getParameter("name", name)) + if(!parametersHandler->getParameter("name", name)) return false; this->setName(name.c_str()); - if (!parametersHandler->getParameter("sampling_time", m_dT)) + if(!parametersHandler->getParameter("sampling_time", m_dT)) return false; double maxValue = 0; - if (!parametersHandler->getParameter("max_angle_deg", maxValue)) + if(!parametersHandler->getParameter("max_angle_deg", maxValue)) return false; maxValue *= M_PI / 180; double minValue = 0; - if (!parametersHandler->getParameter("min_angle_deg", minValue)) + if(!parametersHandler->getParameter("min_angle_deg", minValue)) return false; minValue *= M_PI / 180; double trajectoryDuration = 5; - if (!parametersHandler->getParameter("trajectory_duration", trajectoryDuration)) + if(!parametersHandler->getParameter("trajectory_duration", trajectoryDuration)) return false; this->createPolydriver(parametersHandler); From 23e9ae42a9081604a02b342d1ebc36d6bbeed65b Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Mon, 15 Jan 2024 17:54:45 +0100 Subject: [PATCH 30/45] Writing estimated pose to yarp port for the WalkingModule. --- .../BaseEstimatorFromFootIMU.h | 8 ++++++++ .../src/BaseEstimatorFromFootIMU.cpp | 19 +++++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index 6d344333b6..dd11268629 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -16,6 +16,11 @@ #include #include +// YARP +#include +#include +#include + #include namespace BipedalLocomotion @@ -146,6 +151,9 @@ class BaseEstimatorFromFootIMU BaseEstimatorFromFootIMUInput m_input; /**< Last input stored in the estimator */ BaseEstimatorFromFootIMUState m_state; /**< Current state stored in the estimator */ + yarp::os::BufferedPort m_port; /**< Port used to send the output of the + estimator to the WalkingModule */ + // Geometric quantities of the foot double m_footWidth; /**< Lateral dimension of the robot foot */ double m_footLength; /**< Frontal dimension of the robot foot */ diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 99a2dd2618..fe31121a94 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -180,6 +180,8 @@ bool BaseEstimatorFromFootIMU::initialize( m_state.stanceFootShadowCorners.resize(m_cornersInInertialFrame.size()); m_state.stanceFootCorners.resize(m_cornersInInertialFrame.size()); + m_port.open("/baseEstimatorFromFootIMU/state:o"); + m_isInitialized = true; return ok; @@ -472,6 +474,23 @@ bool BaseEstimatorFromFootIMU::advance() m_T_yawDrift.act(T_supportCornerTranslation.act(m_tiltedFootCorners[i]))); } + // yarp write the basePose in the port + yarp::os::Bottle& output = m_port.prepare(); + output.clear(); + output.addFloat64(m_state.basePose.translation().x()); + output.addFloat64(m_state.basePose.translation().y()); + output.addFloat64(m_state.basePose.translation().z()); + output.addFloat64(toXYZ(m_state.basePose.rotation())[0]); + output.addFloat64(toXYZ(m_state.basePose.rotation())[1]); + output.addFloat64(toXYZ(m_state.basePose.rotation())[2]); + output.addFloat64(0.0); + output.addFloat64(0.0); + output.addFloat64(0.0); + output.addFloat64(0.0); + output.addFloat64(0.0); + output.addFloat64(0.0); + m_port.write(); + double orientationError_L = (toXYZ(m_state.footPose_L.rotation()) - toXYZ(m_input.measuredRotation_L.rotation())).norm(); double orientationError_R = (toXYZ(m_state.footPose_R.rotation()) - toXYZ(m_input.measuredRotation_R.rotation())).norm(); double orientationErrorThreshold = 0.001; // [rad]. TODO: get parameter from config file. 0,001 rad = 0,05729578 deg. From 9b71f824b1c4a2719ee17bbc03206f8027deefeb Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Tue, 16 Jan 2024 11:54:58 +0100 Subject: [PATCH 31/45] Changed from Bottle to yarp::sig::vector. --- .../BaseEstimatorFromFootIMU.h | 6 ++-- .../src/BaseEstimatorFromFootIMU.cpp | 28 ++++++++++--------- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index dd11268629..73dd49d988 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -17,7 +17,7 @@ #include // YARP -#include +#include #include #include @@ -151,8 +151,8 @@ class BaseEstimatorFromFootIMU BaseEstimatorFromFootIMUInput m_input; /**< Last input stored in the estimator */ BaseEstimatorFromFootIMUState m_state; /**< Current state stored in the estimator */ - yarp::os::BufferedPort m_port; /**< Port used to send the output of the - estimator to the WalkingModule */ + yarp::os::BufferedPort m_port; /**< Port used to send the output of the + estimator to the WalkingModule */ // Geometric quantities of the foot double m_footWidth; /**< Lateral dimension of the robot foot */ diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index fe31121a94..c7f77de7cc 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -475,20 +475,22 @@ bool BaseEstimatorFromFootIMU::advance() } // yarp write the basePose in the port - yarp::os::Bottle& output = m_port.prepare(); + yarp::sig::Vector output; output.clear(); - output.addFloat64(m_state.basePose.translation().x()); - output.addFloat64(m_state.basePose.translation().y()); - output.addFloat64(m_state.basePose.translation().z()); - output.addFloat64(toXYZ(m_state.basePose.rotation())[0]); - output.addFloat64(toXYZ(m_state.basePose.rotation())[1]); - output.addFloat64(toXYZ(m_state.basePose.rotation())[2]); - output.addFloat64(0.0); - output.addFloat64(0.0); - output.addFloat64(0.0); - output.addFloat64(0.0); - output.addFloat64(0.0); - output.addFloat64(0.0); + // output.resize(12); + output.push_back(m_state.basePose.translation().x()); + output.push_back(m_state.basePose.translation().y()); + output.push_back(m_state.basePose.translation().z()); + output.push_back(toXYZ(m_state.basePose.rotation())[0]); + output.push_back(toXYZ(m_state.basePose.rotation())[1]); + output.push_back(toXYZ(m_state.basePose.rotation())[2]); + output.push_back(0.0); + output.push_back(0.0); + output.push_back(0.0); + output.push_back(0.0); + output.push_back(0.0); + output.push_back(0.0); + m_port.prepare() = output; m_port.write(); double orientationError_L = (toXYZ(m_state.footPose_L.rotation()) - toXYZ(m_input.measuredRotation_L.rotation())).norm(); From afbb6099d5499720072ab57eeef38e219d14efa5 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Thu, 18 Jan 2024 12:54:24 +0100 Subject: [PATCH 32/45] Added YarpRobotLoggerDevice config files for ergoCubGazeboV1_1. --- .../all_joints_mc.xml | 13 +++ .../ft_clients.xml | 36 +++++++ .../imu_clients.xml | 43 ++++++++ .../mas-remapper.xml | 22 +++++ .../wrench_clients.xml | 44 +++++++++ .../launch-yarp-robot-logger.xml | 16 +++ .../ergoCubGazeboV1_1/yarp-robot-logger.xml | 97 +++++++++++++++++++ 7 files changed, 271 insertions(+) create mode 100644 devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/all_joints_mc.xml create mode 100644 devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/ft_clients.xml create mode 100644 devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/imu_clients.xml create mode 100644 devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/mas-remapper.xml create mode 100644 devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/wrench_clients.xml create mode 100644 devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/launch-yarp-robot-logger.xml create mode 100644 devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml 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..014860d483 --- /dev/null +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/imu_clients.xml @@ -0,0 +1,43 @@ + + + + + + + /ergocub/head/inertials + /yarp_robot_logger/head/inertials + 0.5 + fast_tcp + + + + /ergocub/left_arm/imu + /yarp_robot_logger/left_arm/imu + 0.5 + fast_tcp + + + + /ergocub/right_arm/imu + /yarp_robot_logger/right_arm/imu + 0.5 + fast_tcp + + + + /ergocub/left_foot/imu + /yarp_robot_logger/left_foot/imu + 0.5 + fast_tcp + + + + /ergocub/right_foot/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..d7bff95da3 --- /dev/null +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/blf-yarp-robot-logger-interfaces/mas-remapper.xml @@ -0,0 +1,22 @@ + + + + + 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) + + + + + left_arm_ft_client + right_arm_ft_client + left_leg_ft_client + right_leg_ft_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..96c0f9e4fc --- /dev/null +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/launch-yarp-robot-logger.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + 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..1a764cb731 --- /dev/null +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml @@ -0,0 +1,97 @@ + + + + + ergocubSim + 0.002 + (30) + mp4v + /yarp-robot-logger + ("ergoCubGazeboV1/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 + false + 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") + + + + + + + + all_joints_mc + mas-remapper + + + + + + + + From 5cb0a2fa0e7c4b51380efdb23bd913683a41e764 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Tue, 6 Feb 2024 12:54:47 +0100 Subject: [PATCH 33/45] Updated YarpRobotLogger for ergoCubGazeboV1_1 --- .../robots/ergoCubGazeboV1_1/launch-yarp-robot-logger.xml | 2 +- .../app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) 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 index 96c0f9e4fc..55f28a9b11 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/launch-yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/launch-yarp-robot-logger.xml @@ -5,7 +5,7 @@ BSD-3-Clause license. --> - + diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml index 1a764cb731..64d7882cb7 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml @@ -9,14 +9,14 @@ BSD-3-Clause license. --> (30) mp4v /yarp-robot-logger - ("ergoCubGazeboV1/yarprobotinterface") + ("ergoCubGazeboV1_1/yarprobotinterface") 600.0 - ("Walking", "fee") + ("walking", "fee") () "udp" - + "/yarp-robot-logger/exogenous_signals/walking" "/walking-coordinator/logger" "walking" From e7da2a1cc3fd1b2b66f4b78100b1c0bc863a306d Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Tue, 6 Feb 2024 12:55:54 +0100 Subject: [PATCH 34/45] Increased error threshold to limit warning messages. --- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index c7f77de7cc..5b54dd57d5 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -495,7 +495,7 @@ bool BaseEstimatorFromFootIMU::advance() double orientationError_L = (toXYZ(m_state.footPose_L.rotation()) - toXYZ(m_input.measuredRotation_L.rotation())).norm(); double orientationError_R = (toXYZ(m_state.footPose_R.rotation()) - toXYZ(m_input.measuredRotation_R.rotation())).norm(); - double orientationErrorThreshold = 0.001; // [rad]. TODO: get parameter from config file. 0,001 rad = 0,05729578 deg. + double orientationErrorThreshold = 0.01; // [rad]. TODO: get parameter from config file. 0,01 rad = 0,5729578 deg. if ( (orientationError_L > orientationErrorThreshold) || (orientationError_R > orientationErrorThreshold) ) { From 367fc406ad79e1d58ff31140325a06022aacfc39 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Tue, 13 Feb 2024 13:12:33 +0100 Subject: [PATCH 35/45] Adding feet IMU to yarp logger device. --- .../imu_clients.xml | 25 ++----------------- .../mas-remapper.xml | 14 +++++++++++ 2 files changed, 16 insertions(+), 23 deletions(-) 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 index 014860d483..8cf2a94a44 100644 --- 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 @@ -5,36 +5,15 @@ - - /ergocub/head/inertials - /yarp_robot_logger/head/inertials - 0.5 - fast_tcp - - - - /ergocub/left_arm/imu - /yarp_robot_logger/left_arm/imu - 0.5 - fast_tcp - - - - /ergocub/right_arm/imu - /yarp_robot_logger/right_arm/imu - 0.5 - fast_tcp - - - /ergocub/left_foot/imu + /ergocubSim/left_foot_heel_tiptoe/imu /yarp_robot_logger/left_foot/imu 0.5 fast_tcp - /ergocub/right_foot/imu + /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 index d7bff95da3..4226efde35 100644 --- 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 @@ -8,6 +8,18 @@ (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) + @@ -15,6 +27,8 @@ right_arm_ft_client left_leg_ft_client right_leg_ft_client + left_foot_imu_client + right_foot_imu_client From 894209c7aa66db6d21cc26f1b0c53479e9a9ec5e Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Tue, 13 Feb 2024 16:04:15 +0100 Subject: [PATCH 36/45] Additional changes needed to actually log the imu data through the yarp logger. --- .../ergoCubGazeboV1_1/launch-yarp-robot-logger.xml | 1 + .../app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml | 9 ++++++++- 2 files changed, 9 insertions(+), 1 deletion(-) 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 index 55f28a9b11..66ae83b6e0 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/launch-yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/launch-yarp-robot-logger.xml @@ -10,6 +10,7 @@ BSD-3-Clause license. --> + diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml index 64d7882cb7..f66bd5c55b 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml @@ -65,7 +65,7 @@ BSD-3-Clause license. --> false true - false + true false true false @@ -80,6 +80,13 @@ BSD-3-Clause license. --> ("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) + + From 0447966bdc09af4e4ec26dc95dcc50d86f302631 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Mon, 19 Feb 2024 13:15:39 +0100 Subject: [PATCH 37/45] Removed yarp write of the output from the estimator. This must be perforned by the RFModule. --- .../BaseEstimatorFromFootIMU.h | 9 +------- .../src/BaseEstimatorFromFootIMU.cpp | 21 ------------------- 2 files changed, 1 insertion(+), 29 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index 73dd49d988..97a17d9b7f 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -16,11 +16,7 @@ #include #include -// YARP -#include -#include -#include - +// std #include namespace BipedalLocomotion @@ -151,9 +147,6 @@ class BaseEstimatorFromFootIMU BaseEstimatorFromFootIMUInput m_input; /**< Last input stored in the estimator */ BaseEstimatorFromFootIMUState m_state; /**< Current state stored in the estimator */ - yarp::os::BufferedPort m_port; /**< Port used to send the output of the - estimator to the WalkingModule */ - // Geometric quantities of the foot double m_footWidth; /**< Lateral dimension of the robot foot */ double m_footLength; /**< Frontal dimension of the robot foot */ diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 5b54dd57d5..a89602b22b 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -180,8 +180,6 @@ bool BaseEstimatorFromFootIMU::initialize( m_state.stanceFootShadowCorners.resize(m_cornersInInertialFrame.size()); m_state.stanceFootCorners.resize(m_cornersInInertialFrame.size()); - m_port.open("/baseEstimatorFromFootIMU/state:o"); - m_isInitialized = true; return ok; @@ -474,25 +472,6 @@ bool BaseEstimatorFromFootIMU::advance() m_T_yawDrift.act(T_supportCornerTranslation.act(m_tiltedFootCorners[i]))); } - // yarp write the basePose in the port - yarp::sig::Vector output; - output.clear(); - // output.resize(12); - output.push_back(m_state.basePose.translation().x()); - output.push_back(m_state.basePose.translation().y()); - output.push_back(m_state.basePose.translation().z()); - output.push_back(toXYZ(m_state.basePose.rotation())[0]); - output.push_back(toXYZ(m_state.basePose.rotation())[1]); - output.push_back(toXYZ(m_state.basePose.rotation())[2]); - output.push_back(0.0); - output.push_back(0.0); - output.push_back(0.0); - output.push_back(0.0); - output.push_back(0.0); - output.push_back(0.0); - m_port.prepare() = output; - m_port.write(); - double orientationError_L = (toXYZ(m_state.footPose_L.rotation()) - toXYZ(m_input.measuredRotation_L.rotation())).norm(); double orientationError_R = (toXYZ(m_state.footPose_R.rotation()) - toXYZ(m_input.measuredRotation_R.rotation())).norm(); double orientationErrorThreshold = 0.01; // [rad]. TODO: get parameter from config file. 0,01 rad = 0,5729578 deg. From 815413be8dd9922d3d5aebcad880f10d0dff75b0 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Thu, 29 Feb 2024 12:07:51 +0100 Subject: [PATCH 38/45] Added estimation of the floating base twist to the BaseEstimatorFromFootIMU. --- .../BaseEstimatorFromFootIMU.h | 6 ++++++ src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 12 +++++++++++- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index 97a17d9b7f..86fc2663e4 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -31,6 +31,7 @@ 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; @@ -54,6 +55,10 @@ struct BaseEstimatorFromFootIMUInput 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 */ }; /** @@ -179,6 +184,7 @@ class BaseEstimatorFromFootIMU 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, offset Yaw */ diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index a89602b22b..98a0f23537 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -235,6 +235,7 @@ bool BaseEstimatorFromFootIMU::advance() 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) { @@ -267,6 +268,7 @@ bool BaseEstimatorFromFootIMU::advance() 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) { @@ -445,7 +447,14 @@ bool BaseEstimatorFromFootIMU::advance() * T_foot_tilt * stanceFootFrame_H_link; Eigen::VectorXd baseVelocity(6); - baseVelocity.setZero(); + // baseVelocity.setZero(); + auto angularVelocityInLinkFrame = stanceFootFrame_H_link.asSO3().inverse().act(m_measuredAngularVelocity.coeffs()); + auto 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(), @@ -462,6 +471,7 @@ bool BaseEstimatorFromFootIMU::advance() 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++) From c59414c19f00e28290f5241a2195b56175ef43d8 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Fri, 1 Mar 2024 16:02:54 +0100 Subject: [PATCH 39/45] Code refactoring for improved error handling. --- .../BaseEstimatorFromFootIMU.h | 2 -- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 12 +++++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index 86fc2663e4..cf49e317ab 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -16,7 +16,6 @@ #include #include -// std #include namespace BipedalLocomotion @@ -221,7 +220,6 @@ class BaseEstimatorFromFootIMU 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_isModelSet{false}; bool m_isInitialized{false}; bool m_isInputSet{false}; bool m_isOutputValid{false}; diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 98a0f23537..6ce313d858 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -49,8 +49,6 @@ bool BaseEstimatorFromFootIMU::setModel(const iDynTree::Model& model) { constexpr auto logPrefix = "[BaseEstimatorFromFootIMU::setModel]"; - m_isModelSet = false; - if (!m_kinDyn.loadRobotModel(model)) { log()->error("{} Unable to load the model.", logPrefix); @@ -59,8 +57,6 @@ bool BaseEstimatorFromFootIMU::setModel(const iDynTree::Model& model) m_model = model; - m_isModelSet = true; - 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; @@ -137,6 +133,12 @@ bool BaseEstimatorFromFootIMU::initialize( 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_baseFrameIndex = m_kinDyn.getFrameIndex(m_baseFrameName); From f0295bc2ce37380bb983ee5b13d0742ac252fe58 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Fri, 1 Mar 2024 18:00:22 +0100 Subject: [PATCH 40/45] Improved orientation error warning logic. --- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 6ce313d858..2823595838 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -485,12 +485,15 @@ bool BaseEstimatorFromFootIMU::advance() } 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(); - double orientationErrorThreshold = 0.01; // [rad]. TODO: get parameter from config file. 0,01 rad = 0,5729578 deg. + 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_R > orientationErrorThreshold) ) + if ( ((orientationError_L > orientationErrorThreshold) && (orientationError_L < (360.0 - orientationErrorThreshold))) || + ((orientationError_R > orientationErrorThreshold) && (orientationError_R < (360.0 - orientationErrorThreshold)))) { - log()->warn("{} Foot orientation error above {} threshold: {} Left, {} Right.", + log()->warn("{} Foot orientation error above {} [deg] threshold: {} Left, {} Right.", logPrefix, orientationErrorThreshold, orientationError_L, From 2a8f896d865b35f67579f5c441d2ed23aacc539a Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Wed, 6 Mar 2024 23:14:46 +0100 Subject: [PATCH 41/45] Added a reset method for the BaseEstimatorFromFootIMU. --- .../FloatingBaseEstimators/BaseEstimatorFromFootIMU.h | 2 ++ src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 10 ++++++++++ 2 files changed, 12 insertions(+) diff --git a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h index cf49e317ab..25e043dd39 100644 --- a/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h +++ b/src/Estimators/include/BipedalLocomotion/FloatingBaseEstimators/BaseEstimatorFromFootIMU.h @@ -139,6 +139,8 @@ class BaseEstimatorFromFootIMU */ const BaseEstimatorFromFootIMUState& getOutput() const override; + void resetBaseEstimatorFromFootIMU(); + /** * Set the state of the BaseEstimatorFromFootIMU. * @param state of the BaseEstimatorFromFootIMU diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 2823595838..96e2fb5cce 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -538,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) From ca8280e3c8c9702879877f2c2d24a3ec0efc01f5 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Wed, 6 Mar 2024 23:19:59 +0100 Subject: [PATCH 42/45] Specified type into variable declaration. --- src/Estimators/src/BaseEstimatorFromFootIMU.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp index 96e2fb5cce..a61a5bef76 100644 --- a/src/Estimators/src/BaseEstimatorFromFootIMU.cpp +++ b/src/Estimators/src/BaseEstimatorFromFootIMU.cpp @@ -450,8 +450,8 @@ bool BaseEstimatorFromFootIMU::advance() Eigen::VectorXd baseVelocity(6); // baseVelocity.setZero(); - auto angularVelocityInLinkFrame = stanceFootFrame_H_link.asSO3().inverse().act(m_measuredAngularVelocity.coeffs()); - auto cornerInLinkFrame = stanceFootFrame_H_link.inverse().act(m_cornersInInertialFrame[m_state.supportCornerIndex]); + 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)); From e8911ca3fe47dc700419fa4793e2bfa46df0a534 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Thu, 28 Mar 2024 17:10:38 +0100 Subject: [PATCH 43/45] Added TimeProfiler to BLF. --- src/CMakeLists.txt | 1 + src/TimeProfiler/CMakeLists.txt | 8 ++ .../TimeProfiler/TimeProfiler.h | 103 +++++++++++++++++ src/TimeProfiler/src/TimeProfiler.cpp | 107 ++++++++++++++++++ 4 files changed, 219 insertions(+) create mode 100644 src/TimeProfiler/CMakeLists.txt create mode 100644 src/TimeProfiler/include/BipedalLocomotion/TimeProfiler/TimeProfiler.h create mode 100644 src/TimeProfiler/src/TimeProfiler.cpp diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3d421bed86..5848dc3097 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -22,3 +22,4 @@ add_subdirectory(IK) add_subdirectory(SimplifiedModelControllers) add_subdirectory(ML) add_subdirectory(ReducedModelControllers) +add_subdirectory(TimeProfiler) diff --git a/src/TimeProfiler/CMakeLists.txt b/src/TimeProfiler/CMakeLists.txt new file mode 100644 index 0000000000..31476661ec --- /dev/null +++ b/src/TimeProfiler/CMakeLists.txt @@ -0,0 +1,8 @@ +# Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia (IIT) +# All Rights Reserved. +# Authors: Giulio Romualdi + +add_bipedal_locomotion_library( + NAME TimeProfiler + SOURCES src/TimeProfiler.cpp + PUBLIC_HEADERS include/BipedalLocomotion/TimeProfiler/TimeProfiler.h) diff --git a/src/TimeProfiler/include/BipedalLocomotion/TimeProfiler/TimeProfiler.h b/src/TimeProfiler/include/BipedalLocomotion/TimeProfiler/TimeProfiler.h new file mode 100644 index 0000000000..6f6c4e58e4 --- /dev/null +++ b/src/TimeProfiler/include/BipedalLocomotion/TimeProfiler/TimeProfiler.h @@ -0,0 +1,103 @@ +/** + * @file TimeProfiler.h + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +#ifndef BIPEDAL_LOCOMOTION_TIME_PROFILER_H +#define BIPEDAL_LOCOMOTION_TIME_PROFILER_H + +// std +#include +#include + +namespace BipedalLocomotion +{ + + /** + * Simple timer. + */ + class Timer + { + clock_t m_initTime; /**< Init time. */ + clock_t m_endTime; /**< End time. */ + double m_averageDuration; /**< Average duration. */ + + public: + + /** + * Reset the average duration. + */ + void resetAverageDuration(); + + /** + * Set initial time. + */ + void setInitTime(); + + /** + * Set final time. + */ + void setEndTime(); + + /** + * Evalyate the average duration. + */ + void evaluateDuration(); + + /** + * Get the average duration. + * @return average duration. + */ + const double& getAverageDuration() const; + + }; + + /** + * Simple Time profiler class + */ + 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(); + }; +}; + +#endif diff --git a/src/TimeProfiler/src/TimeProfiler.cpp b/src/TimeProfiler/src/TimeProfiler.cpp new file mode 100644 index 0000000000..156bd42463 --- /dev/null +++ b/src/TimeProfiler/src/TimeProfiler.cpp @@ -0,0 +1,107 @@ +/** + * @file TimeProfiler.cpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +#include +#include +#include + +#include + +using namespace BipedalLocomotion; + +const double& Timer::getAverageDuration() const +{ + return m_averageDuration; +} + +void Timer::resetAverageDuration() +{ + m_averageDuration = 0; +} + +void Timer::setInitTime() +{ + m_initTime = clock(); +} + +void Timer::setEndTime() +{ + m_endTime = clock(); +} + +void Timer::evaluateDuration() +{ + clock_t duration = m_endTime - m_initTime; + m_averageDuration += static_cast(duration) / CLOCKS_PER_SEC * 1000.0; +} + +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()) + { + std::cerr << "[TimeProfiler::addTimer] This timer already exist." <())); + return true; +} + +bool TimeProfiler::setInitTime(const std::string& key) +{ + auto timer = m_timers.find(key); + if(timer == m_timers.end()) + { + std::cerr << "[TimeProfiler::setInitTime] Unable to find the timer." <second->setInitTime(); + return true; +} + +bool TimeProfiler::setEndTime(const std::string& key) +{ + auto timer = m_timers.find(key); + if(timer == m_timers.end()) + { + std::cerr << "[TimeProfiler::setEndTime] Unable to find the timer." <second->setEndTime(); + return true; +} + +void TimeProfiler::profiling() +{ + std::string infoStream; + m_counter++; + for(auto timer = m_timers.begin(); timer != m_timers.end(); timer++) + { + timer->second->evaluateDuration(); + if(m_counter == m_maxCounter) + { + infoStream += timer->first + ": " + + std::to_string((timer->second->getAverageDuration())/m_counter) + + " ms "; + timer->second->resetAverageDuration(); + } + } + if(m_counter == m_maxCounter) + { + m_counter = 0; + std::cout << infoStream << std::endl; + } +} From 393a672b4f479269f9937dc151506d13affff812 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Fri, 29 Mar 2024 19:26:33 +0100 Subject: [PATCH 44/45] Added time profiler inside System in BLF. --- src/CMakeLists.txt | 1 - src/System/CMakeLists.txt | 4 ++-- .../BipedalLocomotion/System}/TimeProfiler.h | 17 ++++++++------ .../src/TimeProfiler.cpp | 23 ++++++++++--------- src/TimeProfiler/CMakeLists.txt | 8 ------- 5 files changed, 24 insertions(+), 29 deletions(-) rename src/{TimeProfiler/include/BipedalLocomotion/TimeProfiler => System/include/BipedalLocomotion/System}/TimeProfiler.h (82%) rename src/{TimeProfiler => System}/src/TimeProfiler.cpp (76%) delete mode 100644 src/TimeProfiler/CMakeLists.txt diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 5848dc3097..3d421bed86 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -22,4 +22,3 @@ add_subdirectory(IK) add_subdirectory(SimplifiedModelControllers) add_subdirectory(ML) add_subdirectory(ReducedModelControllers) -add_subdirectory(TimeProfiler) 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/TimeProfiler/include/BipedalLocomotion/TimeProfiler/TimeProfiler.h b/src/System/include/BipedalLocomotion/System/TimeProfiler.h similarity index 82% rename from src/TimeProfiler/include/BipedalLocomotion/TimeProfiler/TimeProfiler.h rename to src/System/include/BipedalLocomotion/System/TimeProfiler.h index 6f6c4e58e4..bd29477772 100644 --- a/src/TimeProfiler/include/BipedalLocomotion/TimeProfiler/TimeProfiler.h +++ b/src/System/include/BipedalLocomotion/System/TimeProfiler.h @@ -6,24 +6,26 @@ * @date 2018 */ -#ifndef BIPEDAL_LOCOMOTION_TIME_PROFILER_H -#define BIPEDAL_LOCOMOTION_TIME_PROFILER_H +#ifndef BIPEDAL_LOCOMOTION_SYSTEM_TIME_PROFILER_H +#define BIPEDAL_LOCOMOTION_SYSTEM_TIME_PROFILER_H // std #include #include +#include namespace BipedalLocomotion { - +namespace System +{ /** * Simple timer. */ class Timer { - clock_t m_initTime; /**< Init time. */ - clock_t m_endTime; /**< End time. */ - double m_averageDuration; /**< Average duration. */ + std::chrono::time_point m_initTime; /**< Init time. */ + std::chrono::time_point m_endTime; /**< End time. */ + std::chrono::nanoseconds m_averageDuration; /**< Average duration. */ public: @@ -51,7 +53,7 @@ namespace BipedalLocomotion * Get the average duration. * @return average duration. */ - const double& getAverageDuration() const; + const std::chrono::nanoseconds& getAverageDuration() const; }; @@ -99,5 +101,6 @@ namespace BipedalLocomotion void profiling(); }; }; +}; #endif diff --git a/src/TimeProfiler/src/TimeProfiler.cpp b/src/System/src/TimeProfiler.cpp similarity index 76% rename from src/TimeProfiler/src/TimeProfiler.cpp rename to src/System/src/TimeProfiler.cpp index 156bd42463..727f5849a3 100644 --- a/src/TimeProfiler/src/TimeProfiler.cpp +++ b/src/System/src/TimeProfiler.cpp @@ -10,34 +10,35 @@ #include #include -#include +#include +#include -using namespace BipedalLocomotion; +using namespace BipedalLocomotion::System; -const double& Timer::getAverageDuration() const +const std::chrono::nanoseconds& Timer::getAverageDuration() const { return m_averageDuration; } void Timer::resetAverageDuration() { - m_averageDuration = 0; + m_averageDuration = std::chrono::nanoseconds(0); } void Timer::setInitTime() { - m_initTime = clock(); + m_initTime = std::chrono::steady_clock::now(); } void Timer::setEndTime() { - m_endTime = clock(); + m_endTime = std::chrono::steady_clock::now(); } void Timer::evaluateDuration() { - clock_t duration = m_endTime - m_initTime; - m_averageDuration += static_cast(duration) / CLOCKS_PER_SEC * 1000.0; + auto duration = m_endTime - m_initTime; + m_averageDuration += duration; } void TimeProfiler::setPeriod(int maxCounter) @@ -94,14 +95,14 @@ void TimeProfiler::profiling() if(m_counter == m_maxCounter) { infoStream += timer->first + ": " - + std::to_string((timer->second->getAverageDuration())/m_counter) - + " ms "; + + std::to_string(double(timer->second->getAverageDuration().count())/m_counter) + + " ns "; timer->second->resetAverageDuration(); } } if(m_counter == m_maxCounter) { m_counter = 0; - std::cout << infoStream << std::endl; + log()->warn("{}", infoStream); } } diff --git a/src/TimeProfiler/CMakeLists.txt b/src/TimeProfiler/CMakeLists.txt deleted file mode 100644 index 31476661ec..0000000000 --- a/src/TimeProfiler/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -# Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia (IIT) -# All Rights Reserved. -# Authors: Giulio Romualdi - -add_bipedal_locomotion_library( - NAME TimeProfiler - SOURCES src/TimeProfiler.cpp - PUBLIC_HEADERS include/BipedalLocomotion/TimeProfiler/TimeProfiler.h) From e01f71553807883be8f5a50b0dd7553823fd5fc3 Mon Sep 17 00:00:00 2001 From: Guglielmo Date: Sun, 31 Mar 2024 14:21:19 +0200 Subject: [PATCH 45/45] Final changes to TimeProfiler: changelog, clang-format and removed std::cerr. --- CHANGELOG.md | 1 + .../BipedalLocomotion/System/TimeProfiler.h | 149 +++++++++--------- src/System/src/TimeProfiler.cpp | 56 ++++--- 3 files changed, 105 insertions(+), 101 deletions(-) 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/src/System/include/BipedalLocomotion/System/TimeProfiler.h b/src/System/include/BipedalLocomotion/System/TimeProfiler.h index bd29477772..41bf8a605a 100644 --- a/src/System/include/BipedalLocomotion/System/TimeProfiler.h +++ b/src/System/include/BipedalLocomotion/System/TimeProfiler.h @@ -1,106 +1,103 @@ /** * @file TimeProfiler.h - * @authors Giulio Romualdi - * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia - * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT - * @date 2018 + * @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 -#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: /** - * Simple timer. + * Reset the average duration. */ - 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(); + void resetAverageDuration(); - /** - * Set final time. - */ - void setEndTime(); - - /** - * Evalyate the average duration. - */ - void evaluateDuration(); - - /** - * Get the average duration. - * @return average duration. - */ - const std::chrono::nanoseconds& getAverageDuration() const; + /** + * Set initial time. + */ + void setInitTime(); - }; + /** + * Set final time. + */ + void setEndTime(); /** - * Simple Time profiler class + * Evaluate the average duration. */ - 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. */ + void evaluateDuration(); - public: + /** + * Get the average duration. + * @return average duration. + */ + const std::chrono::nanoseconds& getAverageDuration() const; +}; - /** - * Set the output period. - * @param maxCounter is the period (expressed in cycles). - */ - void setPeriod(int maxCounter); +/** + * 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); + /** + * 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 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); + /** + * 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(); - }; -}; + /** + * Print the profiling quantities. + */ + void profiling(); }; -#endif +}; // 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 index 727f5849a3..af220ffeae 100644 --- a/src/System/src/TimeProfiler.cpp +++ b/src/System/src/TimeProfiler.cpp @@ -1,13 +1,11 @@ /** * @file TimeProfiler.cpp - * @authors Giulio Romualdi - * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia - * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT - * @date 2018 + * @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 #include @@ -37,7 +35,7 @@ void Timer::setEndTime() void Timer::evaluateDuration() { - auto duration = m_endTime - m_initTime; + const std::chrono::nanoseconds duration = m_endTime - m_initTime; m_averageDuration += duration; } @@ -49,39 +47,39 @@ void TimeProfiler::setPeriod(int maxCounter) bool TimeProfiler::addTimer(const std::string& key) { auto timer = m_timers.find(key); - if(timer != m_timers.end()) + if (timer != m_timers.end()) { - std::cerr << "[TimeProfiler::addTimer] This timer already exist." <error("[TimeProfiler::addTimer] The timer named {} already exists.", key); return false; } - m_timers.insert(std::make_pair(key, std::make_unique())); + 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()) + if (timer == m_timers.end()) { - std::cerr << "[TimeProfiler::setInitTime] Unable to find the timer." <error("[TimeProfiler::setInitTime] Unable to find the timer named {}.", key); return false; } - timer->second->setInitTime(); + timer->second.setInitTime(); return true; } bool TimeProfiler::setEndTime(const std::string& key) { auto timer = m_timers.find(key); - if(timer == m_timers.end()) + if (timer == m_timers.end()) { - std::cerr << "[TimeProfiler::setEndTime] Unable to find the timer." <error("[TimeProfiler::setEndTime] Unable to find the timer named {}.", key); return false; } - timer->second->setEndTime(); + timer->second.setEndTime(); return true; } @@ -89,20 +87,28 @@ void TimeProfiler::profiling() { std::string infoStream; m_counter++; - for(auto timer = m_timers.begin(); timer != m_timers.end(); timer++) + for (auto& [key, timer] : m_timers) { - timer->second->evaluateDuration(); - if(m_counter == m_maxCounter) + timer.evaluateDuration(); + if (m_counter != m_maxCounter) { - infoStream += timer->first + ": " - + std::to_string(double(timer->second->getAverageDuration().count())/m_counter) - + " ns "; - timer->second->resetAverageDuration(); + 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) + + if (m_counter == m_maxCounter) { m_counter = 0; - log()->warn("{}", infoStream); + log()->info("[TimeProfiler::profiling] {}", infoStream); } }