From da440f2f5e332185fd40aaeb215cadf055669c76 Mon Sep 17 00:00:00 2001 From: Evelyn D'Elia <17877131+evelyd@users.noreply.github.com> Date: Wed, 16 Oct 2024 10:06:24 +0200 Subject: [PATCH 01/12] add machinery for SE3 task to IK --- .../IK/InverseKinematics.h | 91 +++++++- src/IK/src/InverseKinematics.cpp | 201 +++++++++++++++++- src/IK/tests/HumanIKTest.cpp | 12 +- src/IK/tests/configTestIK.toml | 11 +- 4 files changed, 299 insertions(+), 16 deletions(-) diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h index 31ad9a0..c93a933 100644 --- a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h +++ b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -36,6 +37,8 @@ struct nodeData { manif::SO3d I_R_IMU; manif::SO3Tangentd I_omega_IMU = manif::SO3d::Tangent::Zero(); + Eigen::Vector3d I_position = Eigen::Vector3d::Zero(); + Eigen::Vector3d I_linearVelocity = Eigen::Vector3d::Zero(); }; // clang-format off @@ -46,6 +49,15 @@ struct nodeData class HumanIK { private: + /** + * initialize the SE3 task + * @param taskName name of the task + * @param handler pointer to the parameters handler + * @return true if the SE3 task is initialized correctly + */ + bool initializePoseTask(const std::string& taskName, + const std::shared_ptr taskHandler); + /** * initialize the SO3 task * @param taskName name of the task @@ -127,6 +139,26 @@ class HumanIK Eigen::VectorXd m_calibrationJointPositions; /** Joint positions for calibration */ + /** + * Struct containing the SE3 task from the BipedalLocomotion IK, the node number and the + * rotation matrix between the IMU and the link + */ + struct PoseTaskStruct + { + std::shared_ptr task; + int nodeNumber; + manif::SO3d IMU_R_link; // Rotation matrix from the IMU to related link + manif::SO3d IMU_R_link_init; // Initial value of the rotation matrix from the IMU to related link, set through config + // file + manif::SO3d calibrationMatrix = manif::SO3d::Identity(); // Initialization (to Identity) of + // Rotation matrix from the World + // to the World of the IMU, which + // will be calibrated using Tpose + // script + Eigen::Matrix weight; // Weight of the task + std::string frameName; // Name of the frame in which the task is expressed + }; + /** * Struct containing the SO3 task from the BipedalLocomotion IK, the node number and the * rotation matrix between the IMU and the link @@ -192,6 +224,11 @@ class HumanIK manif::SO3d calib_W_R_link = manif::SO3d::Identity(); /** calibration matrix between the world and the link */ + std::unordered_map m_PoseTasks; /** unordered map of type + PoseTaskStruct, each + element referring to a + node*/ + std::unordered_map m_OrientationTasks; /** unordered map of type OrientationTaskStruct, each element referring to a @@ -240,7 +277,21 @@ class HumanIK * For **each** task listed in the parameter `tasks` the user must specify all the parameters * required by the task itself but `robot_velocity_variable_name` since is already specified in * the `IK` group. Moreover, each task requires a parameter `type` that identifies the type of - * task. Up to now, only the "SO3Task" is implemented. + * task. + * + * * The "SE3Task" requires the following parameters: + * | Group | Parameter Name | Type | Description | Mandatory | + * |:---------:|:------------------------------:|:---------------:|:---------------------------------------------------------------------------------------:|:---------:| + * | `SE3Task` | `type` | `string` | Type of the task. The value to be set is `SE3Task` | Yes | + * | `SE3Task` | `robot_velocity_variable_name` | `string` |Name of the variable contained in `VariablesHandler` describing the generalized robot velocity| Yes | + * | `SE3Task` | `node_number` | `int` | Node number of the task. The node number must be unique. | Yes | + * | `SE3Task` | `rotation_matrix` | `vector`| Rotation matrix between the IMU and the link. By default it set to identity. | No | + * | `SE3Task` | `frame_name` | `string` | Name of the frame in which the task is expressed. | Yes | + * * | `SE3Task` | `kp_linear` | `double` | Value of the gain of the linear velocity feedback. | Yes | + * | `SE3Task` | `kp_angular` | `double` | Value of the gain of the angular velocity feedback. | Yes | + * | `SE3Task` | `weight` | `vector`| Weight of the task. Default value is (1.0, 1.0, 1.0) | yes | + * `SE3Task` is a placeholder for the name of the task contained in the `tasks` list. + * * The "SO3Task" requires the following parameters: * | Group | Parameter Name | Type | Description | Mandatory | * |:---------:|:------------------------------:|:---------------:|:---------------------------------------------------------------------------------------:|:---------:| @@ -296,23 +347,35 @@ class HumanIK * @note The following `ini` file presents an example of the configuration that can be used to * build the HumanIK class. * ~~~~~{.ini} - * tasks ("PELVIS_TASK", "GRAVITY_TASK", "RIGHT_TOE_TASK", "JOINT_LIMITS_TASK", "JOINT_REG_TASK") + * tasks ("PELVIS_TASK", "T8_TASK", "GRAVITY_TASK", "RIGHT_TOE_TASK", "JOINT_LIMITS_TASK", "JOINT_REG_TASK") * * [IK] * robot_velocity_variable_name "robot_velocity" * verbosity false * * [PELVIS_TASK] - * type "SO3Task" + * type "SE3Task" * robot_velocity_variable_name "robot_velocity" * frame_name "Pelvis" - * kp_angular 5.0 + * kp_linear 1.0 + * kp_angular 1.0 * node_number 3 * weight (1.0, 1.0, 1.0) * rotation_matrix (0.0, 1.0, 0.0, * 0.0, 0.0, -1.0, * -1.0, 0.0, 0.0) * + * [T8_TASK] + * type "SO3Task" + * robot_velocity_variable_name "robot_velocity" + * frame_name "T8" + * kp_angular 20.0 + * node_number 6 + * weight (1.0 1.0 1.0) + * rotation_matrix (0.0, 1.0, 0.0, + * 0.0, 0.0, 1.0, + * 1.0, 0.0, 0.0) + * * [GRAVITY_TASK] * type "GravityTask" * robot_velocity_variable_name "robot_velocity" @@ -376,6 +439,18 @@ class HumanIK * @return true if the orientation setpoint is set correctly */ bool + updatePoseTask(const int node, const Eigen::Vector3d& I_position, const manif::SO3d& I_R_IMU, const Eigen::Vector3d& I_linearVelocity = Eigen::Vector3d::Zero(), const manif::SO3Tangentd& I_omega_IMU = manif::SO3d::Tangent::Zero()); + + /** + * set the orientation and the angular velocity for a given node of a SO3 task + * @param node node number + * @param I_position position of the sensor in the world frame + * @param I_R_IMU orientation of the IMU + * @param I_linearVelocity linear velocity of the sensor in the world frame + * @param I_omega_IMU angular velocity of the IMU + * @return true if the orientation setpoint is set correctly + */ + bool updateOrientationTask(const int node, const manif::SO3d& I_R_IMU, const manif::SO3Tangentd& I_omega_IMU = manif::SO3d::Tangent::Zero()); /** @@ -411,6 +486,14 @@ class HumanIK */ bool updateJointConstraintsTask(); + /** + * update the pose for all the nodes of the SE3 tasks + * @param nodeStruct unordered map containing the struct node data + * containing the position, orientation, linear and angular velocity of an IMU, associated to the node number + * @return true if the calibration matrix is set correctly + */ + bool updatePoseTasks(const std::unordered_map& nodeStruct); + /** * update the orientation for all the nodes of the SO3 and gravity tasks * @param nodeStruct unordered map containing the struct node data (see diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp index 41cb108..d558c2a 100644 --- a/src/IK/src/InverseKinematics.cpp +++ b/src/IK/src/InverseKinematics.cpp @@ -116,8 +116,18 @@ bool HumanIK::initialize(std::weak_ptrerror("{} Parameter task_type of the {} task is missing", logPrefix, task); return false; } - // Initialize SO3 task - if (taskType == "SO3Task") + + // Initialize SE3 task + if (taskType == "SE3Task") + { + if (!initializePoseTask(task, taskHandler)) + { + BiomechanicalAnalysis::log()->error("{} Error in the initialization of the {} task", logPrefix, task); + return false; + } + // Initialize SO3Task + } + else if (taskType == "SO3Task") { if (!initializeOrientationTask(task, taskHandler)) { @@ -196,6 +206,40 @@ int HumanIK::getDoFsNumber() const return m_nrDoFs; } +bool HumanIK::updatePoseTask(const int node, const Eigen::Vector3d& I_position, const manif::SO3d& I_R_IMU, const Eigen::Vector3d& I_linearVelocity, const manif::SO3Tangentd& I_omega_IMU) +{ + + // Check if the node number is valid + if (m_PoseTasks.find(node) == m_PoseTasks.end()) + { + BiomechanicalAnalysis::log()->error("[HumanIK::updatePoseTask] Invalid node number {}.", node); + return 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 + I_R_link = m_PoseTasks[node].calibrationMatrix * I_R_IMU * m_PoseTasks[node].IMU_R_link; + + // Create the transformation matrix from the world to the link frame + manif::SE3d I_H_link(manif::SE3d::Translation(I_position), I_R_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_R_link.rotation().transpose() * m_PoseTasks[node].calibrationMatrix.rotation() * I_linearVelocity; + + // Compute the angular velocity of the link frame (right trivialized) + Eigen::Vector3d I_omega_link = m_PoseTasks[node].calibrationMatrix.rotation() * I_omega_IMU.coeffs(); + + Eigen::VectorXd mixedVelocityVector(6); + mixedVelocityVector << I_linearVelocity_link, I_omega_link; + + // Create mixed velocity vector + manif::SE3d::Tangent mixedVelocity(mixedVelocityVector); + + // Set the setpoint for the pose task of the node + return m_PoseTasks[node].task->setSetPoint(I_H_link, mixedVelocity); +} + bool HumanIK::updateOrientationTask(const int node, const manif::SO3d& I_R_IMU, const manif::SO3Tangentd& I_omega_IMU) { // Check if the node number is valid @@ -305,6 +349,33 @@ bool HumanIK::updateJointConstraintsTask() return m_jointConstraintsTask->update(); } +bool HumanIK::updatePoseTasks(const std::unordered_map& nodeStruct) +{ + // Update the pose tasks + for (const auto& [node, data] : nodeStruct) + { + if (m_PoseTasks.find(node) != m_PoseTasks.end()) + { + if (!updatePoseTask(node, data.I_position, data.I_R_IMU, data.I_linearVelocity, data.I_omega_IMU)) + { + BiomechanicalAnalysis::log()->error("[HumanIK::updatePoseTasks] " + "Error in updating the pose task of " + "node {}", + node); + return false; + } + } else + { + BiomechanicalAnalysis::log()->error("[HumanIK::updatePoseTasks] " + "Invalid node number {}.", + node); + return false; + } + } + + return true; +} + bool HumanIK::updateOrientationAndGravityTasks(const std::unordered_map& nodeStruct) { // Update the orientation and gravity tasks @@ -357,6 +428,11 @@ bool HumanIK::updateFloorContactTasks(const std::unordered_map nodeStruct) for (const auto& [node, data] : nodeStruct) { // check if the node number is valid - if (m_OrientationTasks.find(node) == m_OrientationTasks.end() && m_GravityTasks.find(node) == m_GravityTasks.end()) + if (m_PoseTasks.find(node) == m_PoseTasks.end() && m_OrientationTasks.find(node) == m_OrientationTasks.end() && m_GravityTasks.find(node) == m_GravityTasks.end()) { BiomechanicalAnalysis::log()->error("[HumanIK::calibrateWorldYaw] Invalid node number."); return false; } iDynTree::Rotation rpyOffset; - if (m_OrientationTasks.find(node) != m_OrientationTasks.end()) + if (m_PoseTasks.find(node) != m_PoseTasks.end()) + { + // compute the offset between the world and the IMU world as: + // W_R_WIMU = W_R_link * (WIMU_R_IMU * IMU_R_link)^{T} + iDynTree::toEigen(rpyOffset) = iDynTree::toEigen(m_kinDyn->getWorldTransform(m_PoseTasks[node].frameName).getRotation()) + * ((data.I_R_IMU * m_PoseTasks[node].IMU_R_link).inverse()).rotation(); + // set the calibration matrix of the orientation task to the offset in yaw + m_PoseTasks[node].calibrationMatrix = manif::SO3d(0, 0, rpyOffset.asRPY()(2)); + } + else if (m_OrientationTasks.find(node) != m_OrientationTasks.end()) { // compute the offset between the world and the IMU world as: // W_R_WIMU = W_R_link * (WIMU_R_IMU * IMU_R_link)^{T} @@ -438,13 +523,22 @@ bool HumanIK::calibrateAllWithWorld(std::unordered_map nodeStruct for (const auto& [node, data] : nodeStruct) { // check if the node number is valid - if (m_OrientationTasks.find(node) == m_OrientationTasks.end() && m_GravityTasks.find(node) == m_GravityTasks.end()) + if (m_PoseTasks.find(node) == m_PoseTasks.end() && m_OrientationTasks.find(node) == m_OrientationTasks.end() && m_GravityTasks.find(node) == m_GravityTasks.end()) { BiomechanicalAnalysis::log()->error("[HumanIK::calibrateWorldYaw] Invalid node number."); return false; } - if (m_OrientationTasks.find(node) != m_OrientationTasks.end()) + if (m_PoseTasks.find(node) != m_PoseTasks.end()) + { + // compute the rotation matrix from the IMU to the link frame as: + // IMU_R_link = (W_R_WIMU * WIMU_R_IMU)^{T} * W_R_link + Eigen::Matrix3d IMU_R_link = (m_PoseTasks[node].calibrationMatrix * data.I_R_IMU).rotation().transpose() + * iDynTree::toEigen(m_kinDyn->getWorldTransform(m_PoseTasks[node].frameName).getRotation()); + m_PoseTasks[node].IMU_R_link = BipedalLocomotion::Conversions::toManifRot(IMU_R_link); + m_PoseTasks[node].calibrationMatrix = secondaryCalib * m_PoseTasks[node].calibrationMatrix; + } + else if (m_OrientationTasks.find(node) != m_OrientationTasks.end()) { // compute the rotation matrix from the IMU to the link frame as: // IMU_R_link = (W_R_WIMU * WIMU_R_IMU)^{T} * W_R_link @@ -587,6 +681,101 @@ bool HumanIK::getBaseAngularVelocity(Eigen::Ref baseAngularVelo return true; } +bool HumanIK::initializePoseTask(const std::string& taskName, + const std::shared_ptr taskHandler) +{ + // Log prefix for error messages + constexpr auto logPrefix = "[HumanIK::initializePoseTask]"; + + // Variable to store the node number + int nodeNumber; + // Flag to indicate successful initialization + bool ok{true}; + + // Retrieve node number parameter from config file, using the task handler + if (!taskHandler->getParameter("node_number", nodeNumber)) + { + BiomechanicalAnalysis::log()->error("{} Parameter node_number of the {} task is missing", logPrefix, taskName); + return false; + } + + // Retrieve frame name parameter from config file, using the task handler + if (!taskHandler->getParameter("frame_name", m_PoseTasks[nodeNumber].frameName)) + { + BiomechanicalAnalysis::log()->error("{} Parameter frame_name of the {} task is missing", logPrefix, taskName); + return false; + } + + // 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; + } + + // Check that the weight is a 3D vector + if (weight.size() != 6) + { + BiomechanicalAnalysis::log()->error("{} The size of the parameter weight of the {} task is " + "{}, it should be 6", + logPrefix, + taskName, + weight.size()); + return false; + } + + // Map weight vector to Eigen::Vector6d and assign it to the corresponding task + m_PoseTasks[nodeNumber].weight = Eigen::Map(weight.data(), 6); + // Set node number for the pose task + m_PoseTasks[nodeNumber].nodeNumber = nodeNumber; + + // Create an SE3Task object for the pose task + m_PoseTasks[nodeNumber].task = std::make_shared(); + + //***************************************************************************************************** + // Retrieve Rotation matrix IMU-to-link from configuration file exampleIK.ini + //***************************************************************************************************** + + std::vector rotation_matrix; + if (taskHandler->getParameter("rotation_matrix", rotation_matrix)) + { + // Convert rotation matrix to ManifRot and assign it to IMU_R_link + m_PoseTasks[nodeNumber].IMU_R_link_init + = BipedalLocomotion::Conversions::toManifRot(Eigen::Map>(rotation_matrix.data())); + } else + { + // If rotation_matrix parameter is missing, set IMU_R_link to identity + std::string frame_name; + taskHandler->getParameter("frame_name", frame_name); + BiomechanicalAnalysis::log()->warn("{} Parameter rotation_matrix of the {} task is " + "missing, setting the rotation " + "matrix from the IMU to the frame {} to identity", + logPrefix, + taskName, + frame_name); + m_PoseTasks[nodeNumber].IMU_R_link_init.setIdentity(); + } + m_PoseTasks[nodeNumber].IMU_R_link = m_PoseTasks[nodeNumber].IMU_R_link_init; + //***************************************************************************************************** + + // Initialize the SE3Task object + ok = ok && m_PoseTasks[nodeNumber].task->setKinDyn(m_kinDyn); + ok = ok && m_PoseTasks[nodeNumber].task->initialize(taskHandler); + + // Add the orientation task to the QP solver + ok = ok && m_qpIK.addTask(m_PoseTasks[nodeNumber].task, taskName, 1, m_PoseTasks[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; +} + bool HumanIK::initializeOrientationTask(const std::string& taskName, const std::shared_ptr taskHandler) { diff --git a/src/IK/tests/HumanIKTest.cpp b/src/IK/tests/HumanIKTest.cpp index 0fc96d5..b2e69cb 100644 --- a/src/IK/tests/HumanIKTest.cpp +++ b/src/IK/tests/HumanIKTest.cpp @@ -32,6 +32,8 @@ TEST_CASE("InverseKinematics test") Eigen::VectorXd qInitial(kinDyn->getNrOfDegreesOfFreedom()); BiomechanicalAnalysis::IK::HumanIK ik; + Eigen::Vector3d I_position = Eigen::Vector3d::Random(); + Eigen::Vector3d I_linearVelocity = Eigen::Vector3d::Random(); manif::SO3d I_R_IMU; manif::SO3Tangentd I_omega_IMU; I_R_IMU.setRandom(); @@ -47,13 +49,21 @@ TEST_CASE("InverseKinematics test") mapNodeData[7].I_R_IMU = I_R_IMU; mapNodeData[7].I_omega_IMU = I_omega_IMU; + std::unordered_map poseNodeData; + poseNodeData[3].I_R_IMU = I_R_IMU; + poseNodeData[3].I_omega_IMU = I_omega_IMU; + poseNodeData[3].I_position = I_position; + poseNodeData[3].I_linearVelocity = I_linearVelocity; + qInitial.setConstant(0.0); REQUIRE(ik.initialize(paramHandler, kinDyn)); REQUIRE(ik.setDt(0.1)); - REQUIRE(ik.updateOrientationTask(3, I_R_IMU, I_omega_IMU)); + REQUIRE(ik.updatePoseTask(3, I_position, I_R_IMU, I_linearVelocity, I_omega_IMU)); + REQUIRE(ik.updateOrientationTask(4, I_R_IMU, I_omega_IMU)); REQUIRE(ik.updateFloorContactTask(10, 11.0)); REQUIRE(ik.updateGravityTask(10, I_R_IMU)); + REQUIRE(ik.updatePoseTasks(poseNodeData)); REQUIRE(ik.updateOrientationAndGravityTasks(mapNodeData)); REQUIRE(ik.updateJointConstraintsTask()); REQUIRE(ik.updateJointRegularizationTask()); diff --git a/src/IK/tests/configTestIK.toml b/src/IK/tests/configTestIK.toml index 9281540..c62b808 100644 --- a/src/IK/tests/configTestIK.toml +++ b/src/IK/tests/configTestIK.toml @@ -1,7 +1,7 @@ tasks = [ - "PELVIS_TASK", "T8_TASK", "RIGHT_UPPER_ARM_TASK", "RIGHT_FORE_ARM_TASK", - "LEFT_UPPER_ARM_TASK", "LEFT_FORE_ARM_TASK", "RIGHT_UPPER_LEG_TASK", - "RIGHT_LOWER_LEG_TASK", "LEFT_UPPER_LEG_TASK", "LEFT_LOWER_LEG_TASK", + "PELVIS_TASK", "T8_TASK", "RIGHT_UPPER_ARM_TASK", "RIGHT_FORE_ARM_TASK", + "LEFT_UPPER_ARM_TASK", "LEFT_FORE_ARM_TASK", "RIGHT_UPPER_LEG_TASK", + "RIGHT_LOWER_LEG_TASK", "LEFT_UPPER_LEG_TASK", "LEFT_LOWER_LEG_TASK", "GRAVITY_TASK_1", "FLOOR_CONTACT_TASK_1", "JOINT_LIMITS_TASK", "JOINT_REG_TASK", "JOINT_VEL_LIMITS_TASK" ] @@ -10,12 +10,13 @@ robot_velocity_variable_name = "robot_velocity" verbosity = false [PELVIS_TASK] -type = "SO3Task" +type = "SE3Task" robot_velocity_variable_name = "robot_velocity" frame_name = "link0" +kp_linear = 1.0 kp_angular = 1.0 node_number = 3 -weight = [1.0, 1.0, 1.0] +weight = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] [T8_TASK] type = "SO3Task" From 131fa4c4670a82f4107c9863b15f0f70df9b9257 Mon Sep 17 00:00:00 2001 From: Evelyn D'Elia <17877131+evelyd@users.noreply.github.com> Date: Wed, 16 Oct 2024 10:31:13 +0200 Subject: [PATCH 02/12] add bindings for IK pose task --- bindings/python/IK/src/InverseKinematics.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/bindings/python/IK/src/InverseKinematics.cpp b/bindings/python/IK/src/InverseKinematics.cpp index 8bf3051..98d79a2 100644 --- a/bindings/python/IK/src/InverseKinematics.cpp +++ b/bindings/python/IK/src/InverseKinematics.cpp @@ -31,7 +31,9 @@ void CreateInverseKinematics(pybind11::module& module) py::class_(module, "nodeData") .def(py::init<>()) .def_readwrite("I_R_IMU", &nodeData::I_R_IMU) - .def_readwrite("I_omega_IMU", &nodeData::I_omega_IMU); + .def_readwrite("I_omega_IMU", &nodeData::I_omega_IMU) + .def_readwrite("I_position", &nodeData::I_position) + .def_readwrite("I_linearVelocity", &nodeData::I_linearVelocity); py::class_(module, "HumanIK") .def(py::init()) @@ -54,12 +56,14 @@ void CreateInverseKinematics(pybind11::module& module) .def("setDt", &HumanIK::setDt, py::arg("dt")) .def("getDt", &HumanIK::getDt) .def("getDoFsNumber", &HumanIK::getDoFsNumber) + .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("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("updateJointRegularizationTask", &HumanIK::updateJointRegularizationTask) From 29671b336e663f98ba685c1c798431a9bed086c5 Mon Sep 17 00:00:00 2001 From: Evelyn D'Elia <17877131+evelyd@users.noreply.github.com> Date: Fri, 25 Oct 2024 10:49:28 +0200 Subject: [PATCH 03/12] remove calibration for pose tasks and handle mask parameter --- .../IK/InverseKinematics.h | 9 +-- src/IK/src/InverseKinematics.cpp | 70 ++++++++----------- 2 files changed, 30 insertions(+), 49 deletions(-) diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h index c93a933..90a4355 100644 --- a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h +++ b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h @@ -147,14 +147,9 @@ class HumanIK { std::shared_ptr task; int nodeNumber; - manif::SO3d IMU_R_link; // Rotation matrix from the IMU to related link - manif::SO3d IMU_R_link_init; // Initial value of the rotation matrix from the IMU to related link, set through config + 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 - manif::SO3d calibrationMatrix = manif::SO3d::Identity(); // Initialization (to Identity) of - // Rotation matrix from the World - // to the World of the IMU, which - // will be calibrated using Tpose - // script Eigen::Matrix weight; // Weight of the task std::string frameName; // Name of the frame in which the task is expressed }; diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp index d558c2a..ca87671 100644 --- a/src/IK/src/InverseKinematics.cpp +++ b/src/IK/src/InverseKinematics.cpp @@ -218,17 +218,17 @@ bool HumanIK::updatePoseTask(const int node, const Eigen::Vector3d& I_position, // Compute the rotation matrix from the world to the link frame as: // W_R_link = W_R_WIMU * WIMU_R_IMU * IMU_R_link - I_R_link = m_PoseTasks[node].calibrationMatrix * I_R_IMU * m_PoseTasks[node].IMU_R_link; + manif::SE3d I_H_IMU(I_position, I_R_IMU); // Create the transformation matrix from the world to the link frame - manif::SE3d I_H_link(manif::SE3d::Translation(I_position), I_R_link); + manif::SE3d I_H_link = I_H_IMU * m_PoseTasks[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_R_link.rotation().transpose() * m_PoseTasks[node].calibrationMatrix.rotation() * I_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 = m_PoseTasks[node].calibrationMatrix.rotation() * I_omega_IMU.coeffs(); + Eigen::Vector3d I_omega_link = I_omega_IMU.coeffs(); Eigen::VectorXd mixedVelocityVector(6); mixedVelocityVector << I_linearVelocity_link, I_omega_link; @@ -430,8 +430,7 @@ bool HumanIK::clearCalibrationMatrices() { for (auto& [node, data] : m_PoseTasks) { - data.calibrationMatrix = manif::SO3d::Identity(); - data.IMU_R_link = m_PoseTasks[node].IMU_R_link_init; + data.IMU_H_link = m_PoseTasks[node].IMU_H_link_init; } for (auto& [node, data] : m_OrientationTasks) { @@ -462,23 +461,14 @@ bool HumanIK::calibrateWorldYaw(std::unordered_map nodeStruct) for (const auto& [node, data] : nodeStruct) { // check if the node number is valid - if (m_PoseTasks.find(node) == m_PoseTasks.end() && m_OrientationTasks.find(node) == m_OrientationTasks.end() && m_GravityTasks.find(node) == m_GravityTasks.end()) + if (m_OrientationTasks.find(node) == m_OrientationTasks.end() && m_GravityTasks.find(node) == m_GravityTasks.end()) { BiomechanicalAnalysis::log()->error("[HumanIK::calibrateWorldYaw] Invalid node number."); return false; } iDynTree::Rotation rpyOffset; - if (m_PoseTasks.find(node) != m_PoseTasks.end()) - { - // compute the offset between the world and the IMU world as: - // W_R_WIMU = W_R_link * (WIMU_R_IMU * IMU_R_link)^{T} - iDynTree::toEigen(rpyOffset) = iDynTree::toEigen(m_kinDyn->getWorldTransform(m_PoseTasks[node].frameName).getRotation()) - * ((data.I_R_IMU * m_PoseTasks[node].IMU_R_link).inverse()).rotation(); - // set the calibration matrix of the orientation task to the offset in yaw - m_PoseTasks[node].calibrationMatrix = manif::SO3d(0, 0, rpyOffset.asRPY()(2)); - } - else if (m_OrientationTasks.find(node) != m_OrientationTasks.end()) + if (m_OrientationTasks.find(node) != m_OrientationTasks.end()) { // compute the offset between the world and the IMU world as: // W_R_WIMU = W_R_link * (WIMU_R_IMU * IMU_R_link)^{T} @@ -523,22 +513,13 @@ bool HumanIK::calibrateAllWithWorld(std::unordered_map nodeStruct for (const auto& [node, data] : nodeStruct) { // check if the node number is valid - if (m_PoseTasks.find(node) == m_PoseTasks.end() && m_OrientationTasks.find(node) == m_OrientationTasks.end() && m_GravityTasks.find(node) == m_GravityTasks.end()) + if (m_OrientationTasks.find(node) == m_OrientationTasks.end() && m_GravityTasks.find(node) == m_GravityTasks.end()) { BiomechanicalAnalysis::log()->error("[HumanIK::calibrateWorldYaw] Invalid node number."); return false; } - if (m_PoseTasks.find(node) != m_PoseTasks.end()) - { - // compute the rotation matrix from the IMU to the link frame as: - // IMU_R_link = (W_R_WIMU * WIMU_R_IMU)^{T} * W_R_link - Eigen::Matrix3d IMU_R_link = (m_PoseTasks[node].calibrationMatrix * data.I_R_IMU).rotation().transpose() - * iDynTree::toEigen(m_kinDyn->getWorldTransform(m_PoseTasks[node].frameName).getRotation()); - m_PoseTasks[node].IMU_R_link = BipedalLocomotion::Conversions::toManifRot(IMU_R_link); - m_PoseTasks[node].calibrationMatrix = secondaryCalib * m_PoseTasks[node].calibrationMatrix; - } - else if (m_OrientationTasks.find(node) != m_OrientationTasks.end()) + if (m_OrientationTasks.find(node) != m_OrientationTasks.end()) { // compute the rotation matrix from the IMU to the link frame as: // IMU_R_link = (W_R_WIMU * WIMU_R_IMU)^{T} * W_R_link @@ -715,13 +696,17 @@ bool HumanIK::initializePoseTask(const std::string& taskName, } // Check that the weight is a 3D vector - if (weight.size() != 6) + std::vector mask; + taskHandler->getParameter("mask", mask); + size_t numTrue = std::count(mask.begin(), mask.end(), true); + if (weight.size() != numTrue) { BiomechanicalAnalysis::log()->error("{} The size of the parameter weight of the {} task is " - "{}, it should be 6", + "{}, it should be {}", logPrefix, taskName, - weight.size()); + weight.size(), + numTrue); return false; } @@ -737,33 +722,34 @@ bool HumanIK::initializePoseTask(const std::string& taskName, // Retrieve Rotation matrix IMU-to-link from configuration file exampleIK.ini //***************************************************************************************************** - std::vector rotation_matrix; - if (taskHandler->getParameter("rotation_matrix", rotation_matrix)) + std::vector transformation_matrix; + if (taskHandler->getParameter("transformation_matrix", transformation_matrix)) { - // Convert rotation matrix to ManifRot and assign it to IMU_R_link - m_PoseTasks[nodeNumber].IMU_R_link_init - = BipedalLocomotion::Conversions::toManifRot(Eigen::Map>(rotation_matrix.data())); + // Convert transformation matrix to ManifRot and assign it to IMU_R_link + Eigen::Matrix4d transformation_matrix_eigen = Eigen::Map>(transformation_matrix.data()); + m_PoseTasks[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 rotation_matrix parameter is missing, set IMU_R_link to identity + // 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 rotation_matrix of the {} task is " - "missing, setting the rotation " + 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_PoseTasks[nodeNumber].IMU_R_link_init.setIdentity(); + m_PoseTasks[nodeNumber].IMU_H_link_init.setIdentity(); } - m_PoseTasks[nodeNumber].IMU_R_link = m_PoseTasks[nodeNumber].IMU_R_link_init; + m_PoseTasks[nodeNumber].IMU_H_link = m_PoseTasks[nodeNumber].IMU_H_link_init; //***************************************************************************************************** // Initialize the SE3Task object ok = ok && m_PoseTasks[nodeNumber].task->setKinDyn(m_kinDyn); ok = ok && m_PoseTasks[nodeNumber].task->initialize(taskHandler); - // Add the orientation task to the QP solver + // Add the pose task to the QP solver ok = ok && m_qpIK.addTask(m_PoseTasks[nodeNumber].task, taskName, 1, m_PoseTasks[nodeNumber].weight); // Check if initialization was successful From 024cd2943e8b81766a8385d368a2379ee93c4917 Mon Sep 17 00:00:00 2001 From: Evelyn D'Elia <17877131+evelyd@users.noreply.github.com> Date: Fri, 25 Oct 2024 11:23:08 +0200 Subject: [PATCH 04/12] fix the mask parameter check for pose task --- src/IK/src/InverseKinematics.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp index ca87671..d99255f 100644 --- a/src/IK/src/InverseKinematics.cpp +++ b/src/IK/src/InverseKinematics.cpp @@ -697,8 +697,16 @@ bool HumanIK::initializePoseTask(const std::string& taskName, // Check that the weight is a 3D vector std::vector mask; - taskHandler->getParameter("mask", mask); - size_t numTrue = std::count(mask.begin(), mask.end(), true); + size_t numTrue; + if (!taskHandler->getParameter("mask", mask)) + { + numTrue = 6; + } + else + { + numTrue = std::count(mask.begin(), mask.end(), true); + } + if (weight.size() != numTrue) { BiomechanicalAnalysis::log()->error("{} The size of the parameter weight of the {} task is " From 76214314fdf669c324729a821923600095589545 Mon Sep 17 00:00:00 2001 From: Evelyn D'Elia <17877131+evelyd@users.noreply.github.com> Date: Fri, 8 Nov 2024 09:50:48 +0100 Subject: [PATCH 05/12] use frame name for pose task id and fix mask, priority settings --- .../IK/InverseKinematics.h | 9 +- src/IK/src/InverseKinematics.cpp | 108 +++++++++--------- src/IK/tests/HumanIKTest.cpp | 12 +- 3 files changed, 67 insertions(+), 62 deletions(-) diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h index 90a4355..b2c913d 100644 --- a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h +++ b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h @@ -146,11 +146,10 @@ class HumanIK struct PoseTaskStruct { std::shared_ptr task; - int nodeNumber; 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 - Eigen::Matrix weight; // Weight of the task + Eigen::VectorXd weight; // Weight of the task, length depends on mask std::string frameName; // Name of the frame in which the task is expressed }; @@ -219,7 +218,7 @@ class HumanIK manif::SO3d calib_W_R_link = manif::SO3d::Identity(); /** calibration matrix between the world and the link */ - std::unordered_map m_PoseTasks; /** unordered map of type + std::unordered_map m_PoseTasks; /** unordered map of type PoseTaskStruct, each element referring to a node*/ @@ -434,7 +433,7 @@ class HumanIK * @return true if the orientation setpoint is set correctly */ bool - updatePoseTask(const int node, const Eigen::Vector3d& I_position, const manif::SO3d& I_R_IMU, const Eigen::Vector3d& I_linearVelocity = Eigen::Vector3d::Zero(), const manif::SO3Tangentd& I_omega_IMU = manif::SO3d::Tangent::Zero()); + updatePoseTask(const std::string frameName, const Eigen::Vector3d& I_position, const manif::SO3d& I_R_IMU, const Eigen::Vector3d& I_linearVelocity = Eigen::Vector3d::Zero(), const manif::SO3Tangentd& I_omega_IMU = manif::SO3d::Tangent::Zero()); /** * set the orientation and the angular velocity for a given node of a SO3 task @@ -487,7 +486,7 @@ class HumanIK * containing the position, orientation, linear and angular velocity of an IMU, associated to the node number * @return true if the calibration matrix is set correctly */ - bool updatePoseTasks(const std::unordered_map& nodeStruct); + bool updatePoseTasks(const std::unordered_map& nodeStruct); /** * update the orientation for all the nodes of the SO3 and gravity tasks diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp index d99255f..6a06d06 100644 --- a/src/IK/src/InverseKinematics.cpp +++ b/src/IK/src/InverseKinematics.cpp @@ -206,13 +206,13 @@ int HumanIK::getDoFsNumber() const return m_nrDoFs; } -bool HumanIK::updatePoseTask(const int node, const Eigen::Vector3d& I_position, const manif::SO3d& I_R_IMU, const Eigen::Vector3d& I_linearVelocity, const manif::SO3Tangentd& I_omega_IMU) +bool HumanIK::updatePoseTask(const std::string frameName, const Eigen::Vector3d& I_position, const manif::SO3d& I_R_IMU, const Eigen::Vector3d& I_linearVelocity, const manif::SO3Tangentd& I_omega_IMU) { - // Check if the node number is valid - if (m_PoseTasks.find(node) == m_PoseTasks.end()) + // Check if the frame name is valid + if (m_PoseTasks.find(frameName) == m_PoseTasks.end()) { - BiomechanicalAnalysis::log()->error("[HumanIK::updatePoseTask] Invalid node number {}.", node); + BiomechanicalAnalysis::log()->error("[HumanIK::updatePoseTask] Invalid frame name {}.", frameName); return false; } @@ -221,7 +221,7 @@ bool HumanIK::updatePoseTask(const int node, const Eigen::Vector3d& I_position, manif::SE3d I_H_IMU(I_position, I_R_IMU); // Create the transformation matrix from the world to the link frame - manif::SE3d I_H_link = I_H_IMU * m_PoseTasks[node].IMU_H_link; + manif::SE3d I_H_link = I_H_IMU * m_PoseTasks[frameName].IMU_H_link; // Compute the linear velocity in the link frame (left trivialized) // I_linearVelocity_link = link_R_W * W_R_WIMU * WIMU_linearVelocity @@ -237,7 +237,7 @@ bool HumanIK::updatePoseTask(const int node, const Eigen::Vector3d& I_position, manif::SE3d::Tangent mixedVelocity(mixedVelocityVector); // Set the setpoint for the pose task of the node - return m_PoseTasks[node].task->setSetPoint(I_H_link, mixedVelocity); + return m_PoseTasks[frameName].task->setSetPoint(I_H_link, mixedVelocity); } bool HumanIK::updateOrientationTask(const int node, const manif::SO3d& I_R_IMU, const manif::SO3Tangentd& I_omega_IMU) @@ -349,26 +349,26 @@ bool HumanIK::updateJointConstraintsTask() return m_jointConstraintsTask->update(); } -bool HumanIK::updatePoseTasks(const std::unordered_map& nodeStruct) +bool HumanIK::updatePoseTasks(const std::unordered_map& nodeStruct) { // Update the pose tasks - for (const auto& [node, data] : nodeStruct) + for (const auto& [frameName, data] : nodeStruct) { - if (m_PoseTasks.find(node) != m_PoseTasks.end()) + if (m_PoseTasks.find(frameName) != m_PoseTasks.end()) { - if (!updatePoseTask(node, data.I_position, data.I_R_IMU, data.I_linearVelocity, data.I_omega_IMU)) + if (!updatePoseTask(frameName, data.I_position, data.I_R_IMU, data.I_linearVelocity, data.I_omega_IMU)) { BiomechanicalAnalysis::log()->error("[HumanIK::updatePoseTasks] " "Error in updating the pose task of " - "node {}", - node); + "frame {}", + frameName); return false; } } else { BiomechanicalAnalysis::log()->error("[HumanIK::updatePoseTasks] " - "Invalid node number {}.", - node); + "Invalid frame name {}.", + frameName); return false; } } @@ -428,9 +428,9 @@ bool HumanIK::updateFloorContactTasks(const std::unordered_mapgetParameter("node_number", nodeNumber)) - { - BiomechanicalAnalysis::log()->error("{} Parameter node_number of the {} task is missing", logPrefix, taskName); - return false; - } - // Retrieve frame name parameter from config file, using the task handler - if (!taskHandler->getParameter("frame_name", m_PoseTasks[nodeNumber].frameName)) + std::string frameName; + if (!taskHandler->getParameter("frame_name", frameName)) { BiomechanicalAnalysis::log()->error("{} Parameter frame_name of the {} task is missing", logPrefix, taskName); return false; } - // Retrieve weight parameter from config file, using the task handler - std::vector weight; - if (!taskHandler->getParameter("weight", weight)) + // Retrieve priority parameter from config file, using the task handler + int priority; + if (!taskHandler->getParameter("priority", priority)) { - BiomechanicalAnalysis::log()->error("{} Parameter weight of the {} task is missing", logPrefix, taskName); + BiomechanicalAnalysis::log()->error("{} Parameter priority of the {} task is missing", logPrefix, taskName); return false; } - // Check that the weight is a 3D vector + // Retrieve the mask parameter from config file, using the task handler std::vector mask; size_t numTrue; if (!taskHandler->getParameter("mask", mask)) @@ -704,30 +696,37 @@ bool HumanIK::initializePoseTask(const std::string& taskName, } else { - numTrue = std::count(mask.begin(), mask.end(), true); + numTrue = std::count(mask.begin(), mask.end(), true) + 3; } - if (weight.size() != numTrue) + // Retrieve weight parameter from config file, using the task handler + std::vector weight; + if ((!taskHandler->getParameter("weight", weight)) && (priority != 0)) { - BiomechanicalAnalysis::log()->error("{} The size of the parameter weight of the {} task is " - "{}, it should be {}", - logPrefix, - taskName, - weight.size(), - numTrue); + 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; + } + } - // Map weight vector to Eigen::Vector6d and assign it to the corresponding task - m_PoseTasks[nodeNumber].weight = Eigen::Map(weight.data(), 6); - // Set node number for the pose task - m_PoseTasks[nodeNumber].nodeNumber = nodeNumber; + m_PoseTasks[frameName].weight.resize(weight.size()); + m_PoseTasks[frameName].weight = Eigen::Map(weight.data(), weight.size()); // Create an SE3Task object for the pose task - m_PoseTasks[nodeNumber].task = std::make_shared(); + m_PoseTasks[frameName].task = std::make_shared(); //***************************************************************************************************** - // Retrieve Rotation matrix IMU-to-link from configuration file exampleIK.ini + // Retrieve Transformation matrix IMU-to-link from configuration file exampleIK.ini //***************************************************************************************************** std::vector transformation_matrix; @@ -735,7 +734,7 @@ bool HumanIK::initializePoseTask(const std::string& taskName, { // Convert transformation matrix to ManifRot and assign it to IMU_R_link Eigen::Matrix4d transformation_matrix_eigen = Eigen::Map>(transformation_matrix.data()); - m_PoseTasks[nodeNumber].IMU_H_link_init = manif::SE3d(transformation_matrix_eigen.block<3, 1>(0, 3), + m_PoseTasks[frameName].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 { @@ -748,17 +747,24 @@ bool HumanIK::initializePoseTask(const std::string& taskName, logPrefix, taskName, frame_name); - m_PoseTasks[nodeNumber].IMU_H_link_init.setIdentity(); + m_PoseTasks[frameName].IMU_H_link_init.setIdentity(); } - m_PoseTasks[nodeNumber].IMU_H_link = m_PoseTasks[nodeNumber].IMU_H_link_init; + m_PoseTasks[frameName].IMU_H_link = m_PoseTasks[frameName].IMU_H_link_init; //***************************************************************************************************** // Initialize the SE3Task object - ok = ok && m_PoseTasks[nodeNumber].task->setKinDyn(m_kinDyn); - ok = ok && m_PoseTasks[nodeNumber].task->initialize(taskHandler); + ok = ok && m_PoseTasks[frameName].task->setKinDyn(m_kinDyn); + ok = ok && m_PoseTasks[frameName].task->initialize(taskHandler); // Add the pose task to the QP solver - ok = ok && m_qpIK.addTask(m_PoseTasks[nodeNumber].task, taskName, 1, m_PoseTasks[nodeNumber].weight); + if (priority == 0) + { + ok = ok && m_qpIK.addTask(m_PoseTasks[frameName].task, taskName, priority); + } + else + { + ok = ok && m_qpIK.addTask(m_PoseTasks[frameName].task, taskName, priority, m_PoseTasks[frameName].weight); + } // Check if initialization was successful if (!ok) diff --git a/src/IK/tests/HumanIKTest.cpp b/src/IK/tests/HumanIKTest.cpp index b2e69cb..8883b9e 100644 --- a/src/IK/tests/HumanIKTest.cpp +++ b/src/IK/tests/HumanIKTest.cpp @@ -49,17 +49,17 @@ TEST_CASE("InverseKinematics test") mapNodeData[7].I_R_IMU = I_R_IMU; mapNodeData[7].I_omega_IMU = I_omega_IMU; - std::unordered_map poseNodeData; - poseNodeData[3].I_R_IMU = I_R_IMU; - poseNodeData[3].I_omega_IMU = I_omega_IMU; - poseNodeData[3].I_position = I_position; - poseNodeData[3].I_linearVelocity = I_linearVelocity; + std::unordered_map poseNodeData; + poseNodeData["root_link"].I_R_IMU = I_R_IMU; + poseNodeData["root_link"].I_omega_IMU = I_omega_IMU; + poseNodeData["root_link"].I_position = I_position; + poseNodeData["root_link"].I_linearVelocity = I_linearVelocity; qInitial.setConstant(0.0); REQUIRE(ik.initialize(paramHandler, kinDyn)); REQUIRE(ik.setDt(0.1)); - REQUIRE(ik.updatePoseTask(3, I_position, I_R_IMU, I_linearVelocity, I_omega_IMU)); + REQUIRE(ik.updatePoseTask("root_link", I_position, I_R_IMU, I_linearVelocity, I_omega_IMU)); REQUIRE(ik.updateOrientationTask(4, I_R_IMU, I_omega_IMU)); REQUIRE(ik.updateFloorContactTask(10, 11.0)); REQUIRE(ik.updateGravityTask(10, I_R_IMU)); From f2ba4f8e578d295c8b6dd7fd0da9033abe49a856 Mon Sep 17 00:00:00 2001 From: Evelyn D'Elia <17877131+evelyd@users.noreply.github.com> Date: Fri, 8 Nov 2024 09:51:44 +0100 Subject: [PATCH 06/12] set base pose to initial value during t-pose --- src/IK/src/InverseKinematics.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp index 6a06d06..80df5d7 100644 --- a/src/IK/src/InverseKinematics.cpp +++ b/src/IK/src/InverseKinematics.cpp @@ -580,8 +580,7 @@ bool HumanIK::advance() { Eigen::Matrix4d basePose; // Pose of the base Eigen::VectorXd initialJointPositions; // Initial positions of the joints - basePose.setIdentity(); // Set the base pose to the identity matrix - m_system.dynamics->setState({basePose.topRightCorner<3, 1>(), toManifRot(basePose.topLeftCorner<3, 3>()), m_calibrationJointPositions}); + m_system.dynamics->setState({m_basePose.topRightCorner<3, 1>(), toManifRot(m_basePose.topLeftCorner<3, 3>()), m_calibrationJointPositions}); m_tPose = false; } From f3d12cae956e9e3da48d48e3905cf9c0d3da53f6 Mon Sep 17 00:00:00 2001 From: Evelyn D'Elia <17877131+evelyd@users.noreply.github.com> Date: Fri, 8 Nov 2024 23:00:29 +0100 Subject: [PATCH 07/12] use initial base pose to init floor contact and update calib matrix --- examples/test_IK.py | 125 ++++++++++ examples/test_IK.toml | 231 ++++++++++++++++++ .../IK/InverseKinematics.h | 1 + src/IK/src/InverseKinematics.cpp | 11 +- 4 files changed, 363 insertions(+), 5 deletions(-) create mode 100644 examples/test_IK.py create mode 100644 examples/test_IK.toml diff --git a/examples/test_IK.py b/examples/test_IK.py new file mode 100644 index 0000000..484025a --- /dev/null +++ b/examples/test_IK.py @@ -0,0 +1,125 @@ +import biomechanical_analysis_framework as baf +import bipedal_locomotion_framework.bindings as blf +import idyntree.swig as idyn +import icub_models +import numpy as np +import resolve_robotics_uri_py +import pathlib +import h5py +import manifpy as manif + +def get_kindyn(): + + joints_list = ["neck_pitch", "neck_roll", "neck_yaw", + "torso_pitch", "torso_roll", "torso_yaw", + "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", + "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", + "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"] + + model_loader = idyn.ModelLoader() + # assert model_loader.loadReducedModelFromFile(str(icub_models.get_model_file("ergoCubSN001")), joints_list) + + urdf_path = str(resolve_robotics_uri_py.resolve_robotics_uri("package://ergoCub/robots/ergoCubSN001/model.urdf")) + assert model_loader.loadReducedModelFromFile(str(urdf_path), joints_list) + + # create KinDynComputationsDescriptor + kindyn = idyn.KinDynComputations() + assert kindyn.loadRobotModel(model_loader.model()) + + return joints_list, kindyn + +def normalize_quat(quat): + norm = np.linalg.norm(quat) + return [quat[1] / norm, quat[2] / norm, quat[3] / norm, quat[0] / norm] + +print("test_IK") + +joints_list, kindyn = get_kindyn() + +# create KinDynComputationsDescriptor +assert kindyn.getNrOfDegreesOfFreedom() == len(joints_list) + +# Set the joint positions to random values +joint_values = [np.random.uniform(-0.5, 0.5) for _ in range(kindyn.getNrOfDegreesOfFreedom())] +assert kindyn.setJointPos(joint_values) + +# Set the robot state +updated_world_T_base = np.array([[1., 0., 0., 0.],[0., 0., -1., 0.],[0., 1., 0., 0.],[0., 0., 0., 1.]]) +updated_s = [np.random.uniform(-0.5,0.5) for _ in range(kindyn.getNrOfDegreesOfFreedom())] +updated_base_velocity = [np.random.uniform(-0.5,0.5) for _ in range(6)] +updated_s_dot = [np.random.uniform(-0.5,0.5) for _ in range(kindyn.getNrOfDegreesOfFreedom())] +updated_world_gravity = [np.random.uniform(-0.5,0.5) for _ in range(3)] +assert kindyn.setRobotState(updated_world_T_base,updated_s,updated_base_velocity, + updated_s_dot,updated_world_gravity) + +humanIK = baf.ik.HumanIK() +# empty_handler = blf.parameters_handler.TomlParametersHandler() + +# Set the parameters from toml file +qp_ik_params = blf.parameters_handler.TomlParametersHandler() +toml = pathlib.Path("test_IK.toml").expanduser() +assert toml.is_file() +ok = qp_ik_params.set_from_file(str(toml)) +# qp_ik_params.set_parameter_string(name="robot_velocity_variable_name", value="robotVelocity") + +humanIK.initialize(qp_ik_params, kindyn) + +# Create the node struct (dict) +# Read in the data +mocap_filename = "/home/evelyd/pi_learning_for_collaborative_carrying/datasets/collaborative_payload_carrying/leader_backward/follower.mat" +mocap_data = h5py.File(mocap_filename, 'r') +mocap_data_cleaned = mocap_data['robot_logger_device'] + +# Cut the data at the start time +# Get the index of the timestamp closest to the start time +start_time = 12.59 +zeroed_timestamps = np.squeeze(mocap_data_cleaned['shoe1']['FT']['timestamps'][:] - mocap_data_cleaned['shoe1']['FT']['timestamps'][0]) +start_time_index = np.argmin(np.abs(zeroed_timestamps - start_time)) + +# Assign the data for the SO3 and Gravity tasks into a struct +# Create the list of nodes in order of the toml file +pose_nodes = [3] +orientation_nodes = [6, 7, 8, 5, 4, 11, 12, 9, 10] +floor_contact_nodes = [1, 2] + +node_struct = {} +for node in orientation_nodes + floor_contact_nodes: + # Define time series of rotations for this node + I_R_IMU = [manif.SO3(quaternion=normalize_quat([quat[1], quat[2], quat[3], quat[0]])) for quat in np.squeeze(mocap_data_cleaned['node' + str(node)]['orientation']['data'][start_time_index:])] + # Define time series of angular velocities for this node + I_omega_IMU = [manif.SO3Tangent(omega) for omega in np.squeeze(mocap_data_cleaned['node' + str(node)]['angVel']['data'][start_time_index:])] + # Assign these values to the node struct + # node_struct[node] = {'I_R_IMU': I_R_IMU, 'I_omega_IMU': I_omega_IMU} + nodeData = baf.ik.nodeData() + nodeData.I_R_IMU = I_R_IMU[0] + nodeData.I_omega_IMU = I_omega_IMU[0] + nodeData.I_position = [0., 0., 0.] + nodeData.I_linearVelocity = [0., 0., 0.] + node_struct[node] = nodeData + +pose_node_struct = {} +for node in pose_nodes: + # Define time series of rotations for this node + I_R_IMU = [manif.SO3(quaternion=normalize_quat([quat[1], quat[2], quat[3], quat[0]])) for quat in np.squeeze(mocap_data_cleaned['node' + str(node)]['orientation']['data'][start_time_index:])] + # Define time series of angular velocities for this node + I_omega_IMU = [manif.SO3Tangent(omega) for omega in np.squeeze(mocap_data_cleaned['node' + str(node)]['angVel']['data'][start_time_index:])] + # Assign these values to the node struct + # node_struct[node] = {'I_R_IMU': I_R_IMU, 'I_omega_IMU': I_omega_IMU} + nodeData = baf.ik.nodeData() + nodeData.I_R_IMU = I_R_IMU[0] + nodeData.I_omega_IMU = I_omega_IMU[0] + nodeData.I_position = [0., 0., 0.] + nodeData.I_linearVelocity = [0., 0., 0.] + pose_node_struct[node] = nodeData + +# Put robot in T pose +calib_joint_positions = np.array([0.]*len(joints_list)) +calib_joint_positions[joints_list.index("l_shoulder_roll")] = np.pi/2 +calib_joint_positions[joints_list.index("r_shoulder_roll")] = np.pi/2 + +humanIK.calibrateWorldYaw(node_struct) +humanIK.calibrateAllWithWorld(node_struct, "l_sole") + +humanIK.updatePoseTask(3, pose_node_struct[3].I_position, pose_node_struct[3].I_R_IMU, pose_node_struct[3].I_linearVelocity, pose_node_struct[3].I_omega_IMU) +humanIK.updatePoseTasks(pose_node_struct) \ No newline at end of file diff --git a/examples/test_IK.toml b/examples/test_IK.toml new file mode 100644 index 0000000..0f830ed --- /dev/null +++ b/examples/test_IK.toml @@ -0,0 +1,231 @@ +tasks = [ + "PELVIS_TASK", + "T8_TASK", + "RIGHT_UPPER_ARM_TASK", + "RIGHT_FORE_ARM_TASK", + "LEFT_UPPER_ARM_TASK", + "LEFT_FORE_ARM_TASK", + "RIGHT_UPPER_LEG_TASK", + "RIGHT_LOWER_LEG_TASK", + "LEFT_UPPER_LEG_TASK", + "LEFT_LOWER_LEG_TASK", + "RIGHT_TOE_TASK", + "LEFT_TOE_TASK", + "RIGHT_FOOT_TASK", + "LEFT_FOOT_TASK", + "JOINT_LIMITS_TASK", + "JOINT_REG_TASK", + "JOINT_VEL_LIMITS_TASK" +] + +[IK] +verbosity = false +robot_velocity_variable_name = "robot_velocity" + +[PELVIS_TASK] +type = "SE3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 #remove later +frame_name = "root_link" +kp_linear = 20.0 +kp_angular = 20.0 +node_number = 3 +weight = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] +rotation_matrix = [0.0, 1.0, 0.0, + 0.0, 0.0, -1.0, + -1.0, 0.0, 0.0] + +[T8_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "chest" +kp_angular = 20.0 +node_number = 6 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + 1.0, 0.0, 0.0] + +[RIGHT_UPPER_ARM_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "r_upper_arm" +kp_angular = 20.0 +node_number = 7 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, + 0.0, -1.0, 0.0] + +[RIGHT_FORE_ARM_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "r_forearm" +kp_angular = 20.0 +node_number = 8 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, + 0.0, -1.0, 0.0] + +[LEFT_UPPER_ARM_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "l_upper_arm" +kp_angular = 20.0 +node_number = 5 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, -1.0, + 0.0, 1.0, 0.0] + +[LEFT_FORE_ARM_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "l_forearm" +kp_angular = 20.0 +node_number = 4 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, -1.0, + 0.0, 1.0, 0.0] + +[RIGHT_UPPER_LEG_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "r_upper_leg" +kp_angular = 20.0 +node_number = 11 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, + 0.0, -1.0, 0.0] + +[RIGHT_LOWER_LEG_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "r_lower_leg" +kp_angular = 20.0 +node_number = 12 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, + 0.0, -1.0, 0.0] + +[LEFT_UPPER_LEG_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "l_upper_leg" +kp_angular = 20.0 +node_number = 9 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, -1.0, + 0.0, 1.0, 0.0] + +[LEFT_LOWER_LEG_TASK] +type = "SO3Task" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "l_lower_leg" +kp_angular = 20.0 +node_number = 10 +weight = [1.0, 1.0, 1.0] +rotation_matrix = [1.0, 0.0, 0.0, + 0.0, 0.0, -1.0, + 0.0, 1.0, 0.0] + + +[RIGHT_FOOT_TASK] +type = "GravityTask" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +target_frame_name = "r_sole" +kp = 20.0 +node_number = 2 +weight = [1.0, 1.0] +rotation_matrix = [0.0, -1.0, 0.0, + 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0] + +[LEFT_FOOT_TASK] +type = "GravityTask" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +target_frame_name = "l_sole" +kp = 20.0 +node_number = 1 +weight = [1.0, 1.0] +rotation_matrix = [-1.0, 0.0, 0.0, + 0.0, -1.0, 0.0, + 0.0, 0.0, 1.0] + +[RIGHT_TOE_TASK] +type = "FloorContactTask" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "r_foot_front" +kp_linear = 60.0 +node_number = 2 +mask = [true, true, true] +weight = [1.0, 1.0, 1.0] +vertical_force_threshold = 60.0 + +[LEFT_TOE_TASK] +type = "FloorContactTask" +robot_velocity_variable_name = "robot_velocity" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +frame_name = "l_foot_front" +kp_linear = 60.0 +node_number = 1 +mask = [true, true, true] +weight = [1.0, 1.0, 1.0] +vertical_force_threshold = 60.0 + +[JOINT_LIMITS_TASK] +type = "JointConstraintTask" +robot_velocity_variable_name = "robot_velocity" +priority = 0 +use_model_limits = false +sampling_time = 0.017 +k_limits = 1.0 +joints_list = ["l_hip_roll", "r_hip_roll", "l_hip_yaw", "r_hip_yaw"] +upper_bounds = [1.9, 1.9, 0.75, 0.75] +lower_bounds = [0.07, 0.07, -0.25, -0.25] + +[JOINT_REG_TASK] +type = "JointRegularizationTask" +weight_provider_type = "ConstantWeightProvider" +priority = 1 +robot_velocity_variable_name = "robot_velocity" +weight = 0.000001 + + +[JOINT_VEL_LIMITS_TASK] +type = "JointVelocityLimitsTask" +robot_velocity_variable_name = "robot_velocity" +priority = 0 +upper_limit = 5.0 +lower_limit = -5.0 \ No newline at end of file diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h index b2c913d..ef3338d 100644 --- a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h +++ b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h @@ -138,6 +138,7 @@ class HumanIK manif::SO3Tangentd I_omega_link; /** angular velocity of the link in the inertial frame */ Eigen::VectorXd m_calibrationJointPositions; /** Joint positions for calibration */ + Eigen::Matrix4d initialBasePose; /** Initial SO3 pose of the base */ /** * Struct containing the SE3 task from the BipedalLocomotion IK, the node number and the diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp index 80df5d7..ab934f7 100644 --- a/src/IK/src/InverseKinematics.cpp +++ b/src/IK/src/InverseKinematics.cpp @@ -46,6 +46,9 @@ bool HumanIK::initialize(std::weak_ptr(); m_system.dynamics->setState({m_basePose.topRightCorner<3, 1>(), toManifRot(m_basePose.topLeftCorner<3, 3>()), m_jointPositions}); @@ -291,12 +294,10 @@ bool HumanIK::updateFloorContactTask(const int node, const double verticalForce, Eigen::VectorXd jointPositions; jointPositions.resize(this->getDoFsNumber()); jointPositions.setZero(); - Eigen::Matrix4d basePose; - basePose.setIdentity(); Eigen::VectorXd baseVelocity; baseVelocity.resize(6); baseVelocity.setZero(); - m_kinDyn->setRobotState(basePose, jointPositions, baseVelocity, m_jointVelocities, m_gravity); + 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, @@ -526,7 +527,7 @@ bool HumanIK::calibrateAllWithWorld(std::unordered_map nodeStruct Eigen::Matrix3d IMU_R_link = (m_OrientationTasks[node].calibrationMatrix * data.I_R_IMU).rotation().transpose() * iDynTree::toEigen(m_kinDyn->getWorldTransform(m_OrientationTasks[node].frameName).getRotation()); m_OrientationTasks[node].IMU_R_link = BipedalLocomotion::Conversions::toManifRot(IMU_R_link); - m_OrientationTasks[node].calibrationMatrix = secondaryCalib * m_OrientationTasks[node].calibrationMatrix; + m_OrientationTasks[node].calibrationMatrix = toManifRot(initialBasePose.topLeftCorner<3, 3>()) * secondaryCalib * m_OrientationTasks[node].calibrationMatrix; } else { // compute the rotation matrix from the IMU to the link frame as: @@ -534,7 +535,7 @@ bool HumanIK::calibrateAllWithWorld(std::unordered_map nodeStruct Eigen::Matrix3d IMU_R_link = (m_GravityTasks[node].calibrationMatrix * data.I_R_IMU).rotation().transpose() * iDynTree::toEigen(m_kinDyn->getWorldTransform(m_GravityTasks[node].frameName).getRotation()); m_GravityTasks[node].IMU_R_link = BipedalLocomotion::Conversions::toManifRot(IMU_R_link); - m_GravityTasks[node].calibrationMatrix = secondaryCalib * m_GravityTasks[node].calibrationMatrix; + m_GravityTasks[node].calibrationMatrix = toManifRot(initialBasePose.topLeftCorner<3, 3>()) * secondaryCalib * m_GravityTasks[node].calibrationMatrix; } } // set the flag to true to reset the integration From abf8a608e5f8e85637b23b3ec101bf416821369e Mon Sep 17 00:00:00 2001 From: Evelyn D'Elia <17877131+evelyd@users.noreply.github.com> Date: Wed, 13 Nov 2024 12:38:28 +0100 Subject: [PATCH 08/12] small fix for task priority assignment --- src/IK/src/InverseKinematics.cpp | 4 ++-- src/IK/tests/HumanIKTest.cpp | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp index ab934f7..b04be32 100644 --- a/src/IK/src/InverseKinematics.cpp +++ b/src/IK/src/InverseKinematics.cpp @@ -683,8 +683,8 @@ bool HumanIK::initializePoseTask(const std::string& taskName, int priority; if (!taskHandler->getParameter("priority", priority)) { - BiomechanicalAnalysis::log()->error("{} Parameter priority of the {} task is missing", logPrefix, taskName); - return false; + BiomechanicalAnalysis::log()->warn("{} Parameter priority of the {} task is missing, setting to 1 (soft priority)", logPrefix, taskName); + priority = 1; } // Retrieve the mask parameter from config file, using the task handler diff --git a/src/IK/tests/HumanIKTest.cpp b/src/IK/tests/HumanIKTest.cpp index 8883b9e..c8dd424 100644 --- a/src/IK/tests/HumanIKTest.cpp +++ b/src/IK/tests/HumanIKTest.cpp @@ -50,16 +50,16 @@ TEST_CASE("InverseKinematics test") mapNodeData[7].I_omega_IMU = I_omega_IMU; std::unordered_map poseNodeData; - poseNodeData["root_link"].I_R_IMU = I_R_IMU; - poseNodeData["root_link"].I_omega_IMU = I_omega_IMU; - poseNodeData["root_link"].I_position = I_position; - poseNodeData["root_link"].I_linearVelocity = I_linearVelocity; + poseNodeData["link0"].I_R_IMU = I_R_IMU; + poseNodeData["link0"].I_omega_IMU = I_omega_IMU; + poseNodeData["link0"].I_position = I_position; + poseNodeData["link0"].I_linearVelocity = I_linearVelocity; qInitial.setConstant(0.0); REQUIRE(ik.initialize(paramHandler, kinDyn)); REQUIRE(ik.setDt(0.1)); - REQUIRE(ik.updatePoseTask("root_link", I_position, I_R_IMU, I_linearVelocity, I_omega_IMU)); + REQUIRE(ik.updatePoseTask("link0", I_position, I_R_IMU, I_linearVelocity, I_omega_IMU)); REQUIRE(ik.updateOrientationTask(4, I_R_IMU, I_omega_IMU)); REQUIRE(ik.updateFloorContactTask(10, 11.0)); REQUIRE(ik.updateGravityTask(10, I_R_IMU)); 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 09/12] 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; } From b56afb705c2e21045ecc565617091a15f94d2e3d Mon Sep 17 00:00:00 2001 From: Evelyn D'Elia <17877131+evelyd@users.noreply.github.com> Date: Mon, 23 Dec 2024 22:25:00 +0100 Subject: [PATCH 10/12] fix IK test for foot trackers --- src/IK/tests/configTestIK.toml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/IK/tests/configTestIK.toml b/src/IK/tests/configTestIK.toml index c62b808..4a19b59 100644 --- a/src/IK/tests/configTestIK.toml +++ b/src/IK/tests/configTestIK.toml @@ -103,8 +103,9 @@ type = "FloorContactTask" robot_velocity_variable_name = "robot_velocity" frame_name = "link10" kp_linear = 1.0 +kp_angular = 1.0 node_number = 10 -weight = [10.0, 10.0, 10.0] +weight = [10.0, 10.0, 10.0, 10.0, 10.0, 10.0] vertical_force_threshold = 60.0 [JOINT_LIMITS_TASK] From 2c61a3ee6d572a2420f658f5286294d14b750d85 Mon Sep 17 00:00:00 2001 From: Evelyn D'Elia <17877131+evelyd@users.noreply.github.com> Date: Mon, 17 Mar 2025 14:26:47 +0100 Subject: [PATCH 11/12] make base pose tracker usage optional --- .../IK/InverseKinematics.h | 3 +- src/IK/src/InverseKinematics.cpp | 34 ++++++++++++++++--- src/IK/tests/configTestIK.toml | 2 ++ src/examples/IK/exampleIK.ini | 2 ++ 4 files changed, 36 insertions(+), 5 deletions(-) diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h index ac49db3..f67f31f 100644 --- a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h +++ b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h @@ -128,6 +128,7 @@ class HumanIK Eigen::VectorXd m_jointPositions; /** Position of the joints */ Eigen::VectorXd m_jointVelocities; /** Velocity of the joints */ Eigen::Matrix4d m_basePose; /** SO3 pose of the base */ + bool m_useMeasuredBasePose; /** Flag to show that the vive tracker is being used to provide base pose info */ Eigen::Matrix m_baseVelocity; /** Vector containing the linear and angular velocity of the base */ Eigen::Vector3d m_gravity; /** Gravity vector */ @@ -137,7 +138,7 @@ class HumanIK manif::SO3Tangentd I_omega_link; /** angular velocity of the link in the inertial frame */ Eigen::VectorXd m_calibrationJointPositions; /** Joint positions for calibration */ - Eigen::Matrix4d initialBasePose; /** Initial SO3 pose of the base */ + Eigen::Matrix4d m_initialBasePose; /** Initial SO3 pose of the base */ /** * Struct containing the SE3 task from the BipedalLocomotion IK, the node number and the diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp index dbaa653..fab009c 100644 --- a/src/IK/src/InverseKinematics.cpp +++ b/src/IK/src/InverseKinematics.cpp @@ -47,7 +47,7 @@ bool HumanIK::initialize(std::weak_ptr(); m_system.dynamics->setState({m_basePose.topRightCorner<3, 1>(), toManifRot(m_basePose.topLeftCorner<3, 3>()), m_jointPositions}); @@ -73,6 +73,15 @@ bool HumanIK::initialize(std::weak_ptrgetParameter("use_measured_base_pose", useMeasuredBasePose)) + { + BiomechanicalAnalysis::log()->error("{} Parameter use_measured_base_pose is missing", logPrefix); + return false; + } + m_useMeasuredBasePose = useMeasuredBasePose; + bool ok = m_qpIK.initialize(ptr->getGroup("IK")); auto group = ptr->getGroup("IK").lock(); std::string variable; @@ -499,6 +508,9 @@ bool HumanIK::calibrateWorldYaw(std::unordered_map nodeStruct) bool HumanIK::calibrateAllWithWorld(std::unordered_map nodeStruct, std::string refFrame) { + // Set the log prefix + constexpr auto logPrefix = "[HumanIK::calibrateAllWithWorld]"; + // reset the robot state Eigen::VectorXd jointVelocities; jointVelocities.resize(this->getDoFsNumber()); @@ -534,7 +546,7 @@ bool HumanIK::calibrateAllWithWorld(std::unordered_map nodeStruct Eigen::Matrix3d IMU_R_link = (m_OrientationTasks[node].calibrationMatrix * data.I_R_IMU).rotation().transpose() * iDynTree::toEigen(m_kinDyn->getWorldTransform(m_OrientationTasks[node].frameName).getRotation()); m_OrientationTasks[node].IMU_R_link = BipedalLocomotion::Conversions::toManifRot(IMU_R_link); - m_OrientationTasks[node].calibrationMatrix = toManifRot(initialBasePose.topLeftCorner<3, 3>()) * secondaryCalib * m_OrientationTasks[node].calibrationMatrix; + m_OrientationTasks[node].calibrationMatrix = toManifRot(m_initialBasePose.topLeftCorner<3, 3>()) * secondaryCalib * m_OrientationTasks[node].calibrationMatrix; } else { // compute the rotation matrix from the IMU to the link frame as: @@ -542,7 +554,14 @@ bool HumanIK::calibrateAllWithWorld(std::unordered_map nodeStruct Eigen::Matrix3d IMU_R_link = (m_GravityTasks[node].calibrationMatrix * data.I_R_IMU).rotation().transpose() * iDynTree::toEigen(m_kinDyn->getWorldTransform(m_GravityTasks[node].frameName).getRotation()); m_GravityTasks[node].IMU_R_link = BipedalLocomotion::Conversions::toManifRot(IMU_R_link); - m_GravityTasks[node].calibrationMatrix = toManifRot(initialBasePose.topLeftCorner<3, 3>()) * secondaryCalib * m_GravityTasks[node].calibrationMatrix; + if (m_useMeasuredBasePose) + { + m_GravityTasks[node].calibrationMatrix = toManifRot(m_initialBasePose.topLeftCorner<3, 3>()) * secondaryCalib * m_GravityTasks[node].calibrationMatrix; + } else + { + m_GravityTasks[node].calibrationMatrix = secondaryCalib * m_GravityTasks[node].calibrationMatrix; + BiomechanicalAnalysis::log()->info("{} Assuming initial base pose is identity.", logPrefix); + } } } // set the flag to true to reset the integration @@ -556,6 +575,9 @@ bool HumanIK::advance() // Initialize ok flag to true bool ok{true}; + // Log prefix for error messages + constexpr auto logPrefix = "[HumanIK::advance]"; + // Advance the QP solver ok = ok && m_qpIK.advance(); // Check if the output of the QP solver is valid @@ -586,7 +608,11 @@ bool HumanIK::advance() if (m_tPose) { - Eigen::Matrix4d basePose; // Pose of the base + if (!m_useMeasuredBasePose) + { + m_basePose.setIdentity(); + BiomechanicalAnalysis::log()->info("{} Resetting base pose to identity.", logPrefix); + } Eigen::VectorXd initialJointPositions; // Initial positions of the joints m_system.dynamics->setState({m_basePose.topRightCorner<3, 1>(), toManifRot(m_basePose.topLeftCorner<3, 3>()), m_calibrationJointPositions}); m_tPose = false; diff --git a/src/IK/tests/configTestIK.toml b/src/IK/tests/configTestIK.toml index 4a19b59..4d468e4 100644 --- a/src/IK/tests/configTestIK.toml +++ b/src/IK/tests/configTestIK.toml @@ -5,6 +5,8 @@ tasks = [ "GRAVITY_TASK_1", "FLOOR_CONTACT_TASK_1", "JOINT_LIMITS_TASK", "JOINT_REG_TASK", "JOINT_VEL_LIMITS_TASK" ] +use_measured_base_pose = true + [IK] robot_velocity_variable_name = "robot_velocity" verbosity = false diff --git a/src/examples/IK/exampleIK.ini b/src/examples/IK/exampleIK.ini index 0ddf363..226028f 100644 --- a/src/examples/IK/exampleIK.ini +++ b/src/examples/IK/exampleIK.ini @@ -7,6 +7,8 @@ tasks ("PELVIS_TASK", "T8_TASK", "RIGHT_UPPER_ARM_TASK "RIGHT_UPPER_LEG_TASK", "RIGHT_LOWER_LEG_TASK", "LEFT_UPPER_LEG_TASK", "LEFT_LOWER_LEG_TASK", "RIGHT_TOE_TASK", "LEFT_TOE_TASK", "RIGHT_FOOT_TASK", "LEFT_FOOT_TASK", "JOINT_LIMITS_TASK", "JOINT_REG_TASK") +use_measured_base_pose = true + [IK] robot_velocity_variable_name "robot_velocity" verbosity false From 5b501485a66d4bdfd9c570b6870e82cc631337fa Mon Sep 17 00:00:00 2001 From: Evelyn D'Elia <17877131+evelyd@users.noreply.github.com> Date: Mon, 24 Mar 2025 13:47:17 +0100 Subject: [PATCH 12/12] implement base pose calib also for orientation task --- src/IK/src/InverseKinematics.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp index fab009c..76d7669 100644 --- a/src/IK/src/InverseKinematics.cpp +++ b/src/IK/src/InverseKinematics.cpp @@ -546,7 +546,14 @@ bool HumanIK::calibrateAllWithWorld(std::unordered_map nodeStruct Eigen::Matrix3d IMU_R_link = (m_OrientationTasks[node].calibrationMatrix * data.I_R_IMU).rotation().transpose() * iDynTree::toEigen(m_kinDyn->getWorldTransform(m_OrientationTasks[node].frameName).getRotation()); m_OrientationTasks[node].IMU_R_link = BipedalLocomotion::Conversions::toManifRot(IMU_R_link); - m_OrientationTasks[node].calibrationMatrix = toManifRot(m_initialBasePose.topLeftCorner<3, 3>()) * secondaryCalib * m_OrientationTasks[node].calibrationMatrix; + if (m_useMeasuredBasePose) + { + m_OrientationTasks[node].calibrationMatrix = toManifRot(m_initialBasePose.topLeftCorner<3, 3>()) * secondaryCalib * m_OrientationTasks[node].calibrationMatrix; + } else + { + m_OrientationTasks[node].calibrationMatrix = secondaryCalib * m_OrientationTasks[node].calibrationMatrix; + BiomechanicalAnalysis::log()->info("{} Assuming initial base pose is identity for OrientationTask.", logPrefix); + } } else { // compute the rotation matrix from the IMU to the link frame as: @@ -560,7 +567,7 @@ bool HumanIK::calibrateAllWithWorld(std::unordered_map nodeStruct } else { m_GravityTasks[node].calibrationMatrix = secondaryCalib * m_GravityTasks[node].calibrationMatrix; - BiomechanicalAnalysis::log()->info("{} Assuming initial base pose is identity.", logPrefix); + BiomechanicalAnalysis::log()->info("{} Assuming initial base pose is identity for GravityTask.", logPrefix); } } }