From e8516767cc6f6fea6e5528a1d0b41f905a411166 Mon Sep 17 00:00:00 2001 From: Evelyn D'Elia <17877131+evelyd@users.noreply.github.com> Date: Fri, 20 Dec 2024 03:07:06 +0100 Subject: [PATCH] implement foot tracker task --- bindings/python/IK/src/InverseKinematics.cpp | 4 +- .../IK/InverseKinematics.h | 19 +-- src/IK/src/InverseKinematics.cpp | 140 ++++++++++++------ 3 files changed, 108 insertions(+), 55 deletions(-) diff --git a/bindings/python/IK/src/InverseKinematics.cpp b/bindings/python/IK/src/InverseKinematics.cpp index 98d79a2..cf652d2 100644 --- a/bindings/python/IK/src/InverseKinematics.cpp +++ b/bindings/python/IK/src/InverseKinematics.cpp @@ -59,13 +59,13 @@ void CreateInverseKinematics(pybind11::module& module) .def("updatePoseTask", &HumanIK::updatePoseTask, py::arg("node"), py::arg("I_position"), py::arg("I_R_IMU"), py::arg("I_linearVelocity"), py::arg("I_omega_IMU")) .def("updateOrientationTask", &HumanIK::updateOrientationTask, py::arg("node"), py::arg("I_R_IMU"), py::arg("I_omega_IMU")) .def("updateGravityTask", &HumanIK::updateGravityTask, py::arg("node"), py::arg("I_R_IMU")) - .def("updateFloorContactTask", &HumanIK::updateFloorContactTask, py::arg("node"), py::arg("verticalForce"), py::arg("linkHeight")) + .def("updateFloorContactTask", &HumanIK::updateFloorContactTask, py::arg("node"), py::arg("verticalForce"), py::arg("I_position"), py::arg("I_R_IMU"), py::arg("I_linearVelocity"), py::arg("I_omega_IMU"), py::arg("linkHeight")) .def("clearCalibrationMatrices", &HumanIK::clearCalibrationMatrices) .def("calibrateWorldYaw", &HumanIK::calibrateWorldYaw, py::arg("nodeStruct")) .def("calibrateAllWithWorld", &HumanIK::calibrateAllWithWorld, py::arg("nodeStruct"), py::arg("frameName")) .def("updatePoseTasks", &HumanIK::updatePoseTasks, py::arg("nodeStruct")) .def("updateOrientationGravityTasks", &HumanIK::updateOrientationAndGravityTasks, py::arg("nodeStruct")) - .def("updateFloorContactTasks", &HumanIK::updateFloorContactTasks, py::arg("wrenchMap"), py::arg("linkHeight")) + .def("updateFloorContactTasks", &HumanIK::updateFloorContactTasks, py::arg("wrenchMap"), py::arg("I_position"), py::arg("I_R_IMU"), py::arg("I_linearVelocity"), py::arg("I_omega_IMU"), py::arg("linkHeight")) .def("updateJointRegularizationTask", &HumanIK::updateJointRegularizationTask) .def("updateJointConstraintsTask", &HumanIK::updateJointConstraintsTask) .def("advance", &HumanIK::advance) diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h index ef3338d..ac49db3 100644 --- a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h +++ b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include @@ -77,10 +76,10 @@ class HumanIK const std::shared_ptr taskHandler); /** - * initialize the R3 task + * initialize the SE3 task * @param taskName name of the task * @param handler pointer to the parameters handler - * @return true if the R3 task is initialized correctly + * @return true if the SE3 task is initialized correctly */ bool initializeFloorContactTask(const std::string& taskName, const std::shared_ptr taskHandler); @@ -192,16 +191,18 @@ class HumanIK }; /** - * Struct containing the R3 task from the BipedalLocomotion IK, the node number and the + * Struct containing the SE3 task from the BipedalLocomotion IK, the node number and the * multiple state weight provider */ struct FloorContactTaskStruct { - std::shared_ptr task; - Eigen::Vector3d weight; + std::shared_ptr task; + Eigen::VectorXd weight; int nodeNumber; bool footInContact{false}; - Eigen::Vector3d setPointPosition; + manif::SE3d IMU_H_link; // Transformation matrix from the IMU to related link + manif::SE3d IMU_H_link_init; // Initial value of the transformation matrix from the IMU to related link, set through config + // file std::string taskName; std::string frameName; double verticalForceThreshold; @@ -463,7 +464,7 @@ class HumanIK * @param verticalForce vertical force * @return true if the orientation setpoint is set correctly */ - bool updateFloorContactTask(const int node, const double verticalForce, const double linkHeight = 0.0); + bool updateFloorContactTask(const int node, const double verticalForce, const Eigen::Vector3d& I_position = Eigen::Vector3d::Zero(), const manif::SO3d& I_R_IMU = manif::SO3d::Identity(), const Eigen::Vector3d& I_linearVelocity = Eigen::Vector3d::Zero(), const manif::SO3Tangentd& I_omega_IMU = manif::SO3d::Tangent::Zero(), const double linkHeight = 0.0); /** * set the setpoint for the joint regularization task. @@ -503,7 +504,7 @@ class HumanIK * @param footInContact unordered map containing the node number and the vertical force * @return true if the calibration matrix is set correctly */ - bool updateFloorContactTasks(const std::unordered_map>& wrenchMap, const double linkHeight = 0.0); + bool updateFloorContactTasks(const std::unordered_map>& wrenchMap, const Eigen::Vector3d& I_position = Eigen::Vector3d::Zero(), const manif::SO3d& I_R_IMU = manif::SO3d::Identity(), const Eigen::Vector3d& I_linearVelocity = Eigen::Vector3d::Zero(), const manif::SO3Tangentd& I_omega_IMU = manif::SO3d::Tangent::Zero(), const double linkHeight = 0.0); /** * clear the calibration matrices W_R_WIMU and IMU_R_link of all the orientation and gravity tasks diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp index b04be32..dbaa653 100644 --- a/src/IK/src/InverseKinematics.cpp +++ b/src/IK/src/InverseKinematics.cpp @@ -279,7 +279,7 @@ bool HumanIK::updateGravityTask(const int node, const manif::SO3d& I_R_IMU) return m_GravityTasks[node].task->setSetPoint((I_R_link.rotation().transpose().rightCols(1))); } -bool HumanIK::updateFloorContactTask(const int node, const double verticalForce, const double linkHeight) +bool HumanIK::updateFloorContactTask(const int node, const double verticalForce, const Eigen::Vector3d& I_position, const manif::SO3d& I_R_IMU, const Eigen::Vector3d& I_linearVelocity, const manif::SO3Tangentd& I_omega_IMU, const double linkHeight) { bool ok{true}; // check if the node number is valid @@ -300,30 +300,33 @@ bool HumanIK::updateFloorContactTask(const int node, const double verticalForce, m_kinDyn->setRobotState(m_basePose, jointPositions, baseVelocity, m_jointVelocities, m_gravity); } - // if the vertical force is greater than the threshold and if the foot is not yet in contact, - // set the weight of the associated task to the weight of the task and set the set point of the - // task to the position of the frame computed with the legged odometry - if (verticalForce > m_FloorContactTasks[node].verticalForceThreshold && !m_FloorContactTasks[node].footInContact) - { - m_qpIK.setTaskWeight(m_FloorContactTasks[node].taskName, m_FloorContactTasks[node].weight); - m_FloorContactTasks[node].footInContact = true; - m_FloorContactTasks[node].setPointPosition - = iDynTree::toEigen(m_kinDyn->getWorldTransform(m_FloorContactTasks[node].frameName).getPosition()); - m_FloorContactTasks[node].setPointPosition(2) = linkHeight; - } else if (verticalForce < m_FloorContactTasks[node].verticalForceThreshold && m_FloorContactTasks[node].footInContact) - { - // if the foot is not more in contact, set the weight of the associated task to zero - m_qpIK.setTaskWeight(m_FloorContactTasks[node].taskName, Eigen::Vector3d::Zero()); - m_FloorContactTasks[node].footInContact = false; - } + // Compute the rotation matrix from the world to the link frame as: + // W_R_link = W_R_WIMU * WIMU_R_IMU * IMU_R_link + manif::SE3d I_H_IMU(I_position, I_R_IMU); - // if the foot is in contact, set the set point of the task - if (m_FloorContactTasks[node].footInContact) - { - ok = m_FloorContactTasks[node].task->setSetPoint(m_FloorContactTasks[node].setPointPosition); + // Create the transformation matrix from the world to the link frame + manif::SE3d I_H_link = I_H_IMU * m_FloorContactTasks[node].IMU_H_link; + + // Compute the linear velocity in the link frame (left trivialized) + // I_linearVelocity_link = link_R_W * W_R_WIMU * WIMU_linearVelocity + Eigen::Vector3d I_linearVelocity_link = I_H_link.rotation().transpose() * I_linearVelocity; + + // Compute the angular velocity of the link frame (right trivialized) + Eigen::Vector3d I_omega_link = I_omega_IMU.coeffs(); + + Eigen::VectorXd mixedVelocityVector(6); + mixedVelocityVector << I_linearVelocity_link, I_omega_link; + + // Create mixed velocity vector + manif::SE3d::Tangent mixedVelocity(mixedVelocityVector); + + if (verticalForce > m_FloorContactTasks[node].verticalForceThreshold) + { // if the vertical force is greater than the threshold, set the foot height to 0 + I_H_link.translation(Eigen::Vector3d(I_H_link.translation().x(), I_H_link.translation().y(), linkHeight)); } - return ok; + // then set the set point for the task regardless of whether the foot is in contact + return m_FloorContactTasks[node].task->setSetPoint(I_H_link, mixedVelocity); } bool HumanIK::updateJointRegularizationTask() @@ -412,11 +415,11 @@ bool HumanIK::updateOrientationAndGravityTasks(const std::unordered_map>& wrenchMap, const double linkHeight) +bool HumanIK::updateFloorContactTasks(const std::unordered_map>& wrenchMap, const Eigen::Vector3d& I_position, const manif::SO3d& I_R_IMU, const Eigen::Vector3d& I_linearVelocity, const manif::SO3Tangentd& I_omega_IMU, const double linkHeight) { for (const auto& [node, data] : wrenchMap) { - if (!updateFloorContactTask(node, data(WRENCH_FORCE_Z), linkHeight)) + if (!updateFloorContactTask(node, data(WRENCH_FORCE_Z), I_position, I_R_IMU, I_linearVelocity, I_omega_IMU, linkHeight)) { BiomechanicalAnalysis::log()->error("[HumanIK::updateFloorContactTasks] Error in updating " "the floor contact task of node {}", @@ -443,6 +446,10 @@ bool HumanIK::clearCalibrationMatrices() data.calibrationMatrix = manif::SO3d::Identity(); data.IMU_R_link = m_GravityTasks[node].IMU_R_link_init; } + for (auto& [frameName, data] : m_FloorContactTasks) + { + data.IMU_H_link = m_FloorContactTasks[frameName].IMU_H_link_init; + } return true; } @@ -994,24 +1001,73 @@ bool HumanIK::initializeFloorContactTask(const std::string& taskName, return false; } - // Retrieve weight parameter from the task handler + // Set node number and task name for the FloorContactTask + m_FloorContactTasks[nodeNumber].nodeNumber = nodeNumber; + m_FloorContactTasks[nodeNumber].taskName = taskName; + + // Retrieve the mask parameter from config file, using the task handler + std::vector mask; + size_t numTrue; + if (!taskHandler->getParameter("mask", mask)) + { + numTrue = 6; + } + else + { + numTrue = std::count(mask.begin(), mask.end(), true) + 3; + } + + // Retrieve weight parameter from config file, using the task handler std::vector weight; if (!taskHandler->getParameter("weight", weight)) { BiomechanicalAnalysis::log()->error("{} Parameter weight of the {} task is missing", logPrefix, taskName); return false; + + if (weight.size() != numTrue) + { + BiomechanicalAnalysis::log()->error("{} The size of the parameter weight of the {} task is " + "{}, it should be {}", + logPrefix, + taskName, + weight.size(), + numTrue); + return false; + } + } - // Check that the weight is a 3D vector - if (weight.size() != 3) + m_FloorContactTasks[nodeNumber].weight.resize(weight.size()); + m_FloorContactTasks[nodeNumber].weight = Eigen::Map(weight.data(), weight.size()); + + // Create an SE3Task object for the pose task + m_FloorContactTasks[nodeNumber].task = std::make_shared(); + + //***************************************************************************************************** + // Retrieve Transformation matrix IMU-to-link from configuration file exampleIK.ini + //***************************************************************************************************** + + std::vector transformation_matrix; + if (taskHandler->getParameter("transformation_matrix", transformation_matrix)) { - BiomechanicalAnalysis::log()->error("{} The size of the parameter weight of the {} task is " - "{}, it should be 3", - logPrefix, - taskName, - weight.size()); - return false; + // Convert transformation matrix to ManifRot and assign it to IMU_R_link + Eigen::Matrix4d transformation_matrix_eigen = Eigen::Map>(transformation_matrix.data()); + m_FloorContactTasks[nodeNumber].IMU_H_link_init = manif::SE3d(transformation_matrix_eigen.block<3, 1>(0, 3), + BipedalLocomotion::Conversions::toManifRot(transformation_matrix_eigen.block<3, 3>(0, 0))); + } else + { + // If transformation_matrix parameter is missing, set IMU_H_link to identity + std::string frame_name; + taskHandler->getParameter("frame_name", frame_name); + BiomechanicalAnalysis::log()->warn("{} Parameter transformation_matrix of the {} task is " + "missing, setting the transformation " + "matrix from the IMU to the frame {} to identity", + logPrefix, + taskName, + frame_name); + m_FloorContactTasks[nodeNumber].IMU_H_link_init.setIdentity(); } + m_FloorContactTasks[nodeNumber].IMU_H_link = m_FloorContactTasks[nodeNumber].IMU_H_link_init; // Retrieve vertical force threshold parameter from the task handler and assign it to the // corresponding FloorContactTask @@ -1024,17 +1080,7 @@ bool HumanIK::initializeFloorContactTask(const std::string& taskName, return false; } - // Map weight vector to Eigen::Vector3d and assign it to the corresponding FloorContactTask - m_FloorContactTasks[nodeNumber].weight = Eigen::Map(weight.data()); - - // Set node number and task name for the FloorContactTask - m_FloorContactTasks[nodeNumber].nodeNumber = nodeNumber; - m_FloorContactTasks[nodeNumber].taskName = taskName; - - // Create an R3Task object for the floor contact task - m_FloorContactTasks[nodeNumber].task = std::make_shared(); - - // Initialize the R3Task object + // Initialize the SE3Task object ok = ok && m_FloorContactTasks[nodeNumber].task->setKinDyn(m_kinDyn); ok = ok && m_FloorContactTasks[nodeNumber].task->initialize(taskHandler); @@ -1042,6 +1088,12 @@ bool HumanIK::initializeFloorContactTask(const std::string& taskName, ok = ok && m_qpIK.addTask(m_FloorContactTasks[nodeNumber].task, taskName, 1, m_FloorContactTasks[nodeNumber].weight); // Check if initialization was successful + if (!ok) + { + BiomechanicalAnalysis::log()->error("{} Error in the initialization of the {} task", logPrefix, taskName); + return false; + } + return ok; }