From da4e6635eb43c709a5db812c94bb16003242d969 Mon Sep 17 00:00:00 2001 From: Evelyn D'Elia <17877131+evelyd@users.noreply.github.com> Date: Mon, 7 Oct 2024 14:53:23 +0200 Subject: [PATCH] define calib joint positions in config file and give foot link height a default value --- bindings/python/IK/src/InverseKinematics.cpp | 4 +-- .../IK/InverseKinematics.h | 10 ++++--- src/IK/src/InverseKinematics.cpp | 29 ++++++++++++++++--- src/IK/tests/HumanIKTest.cpp | 9 ++---- src/examples/IK/exampleIK.cpp | 11 ++----- 5 files changed, 37 insertions(+), 26 deletions(-) diff --git a/bindings/python/IK/src/InverseKinematics.cpp b/bindings/python/IK/src/InverseKinematics.cpp index fabb5a4..8bf3051 100644 --- a/bindings/python/IK/src/InverseKinematics.cpp +++ b/bindings/python/IK/src/InverseKinematics.cpp @@ -58,8 +58,8 @@ void CreateInverseKinematics(pybind11::module& module) .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"), py::arg("joint_positions")) - .def("calibrateAllWithWorld", &HumanIK::calibrateAllWithWorld, py::arg("nodeStruct"), py::arg("joint_positions"), py::arg("frameName")) + .def("calibrateWorldYaw", &HumanIK::calibrateWorldYaw, py::arg("nodeStruct")) + .def("calibrateAllWithWorld", &HumanIK::calibrateAllWithWorld, py::arg("nodeStruct"), py::arg("frameName")) .def("updateOrientationGravityTasks", &HumanIK::updateOrientationAndGravityTasks, py::arg("nodeStruct")) .def("updateFloorContactTasks", &HumanIK::updateFloorContactTasks, py::arg("wrenchMap"), py::arg("linkHeight")) .def("updateJointRegularizationTask", &HumanIK::updateJointRegularizationTask) diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h index cee6ef7..31ad9a0 100644 --- a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h +++ b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h @@ -125,6 +125,8 @@ class HumanIK manif::SO3d I_R_link; /** orientation of the link in the inertial frame */ manif::SO3Tangentd I_omega_link; /** angular velocity of the link in the inertial frame */ + Eigen::VectorXd m_calibrationJointPositions; /** Joint positions for calibration */ + /** * Struct containing the SO3 task from the BipedalLocomotion IK, the node number and the * rotation matrix between the IMU and the link @@ -391,7 +393,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); + bool updateFloorContactTask(const int node, const double verticalForce, const double linkHeight = 0.0); /** * set the setpoint for the joint regularization task. @@ -423,7 +425,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); + bool updateFloorContactTasks(const std::unordered_map>& wrenchMap, const double linkHeight = 0.0); /** * clear the calibration matrices W_R_WIMU and IMU_R_link of all the orientation and gravity tasks @@ -439,7 +441,7 @@ class HumanIK * @return true if the calibration matrix is set correctly * @note gravity is expected to be aligned with the z-axis of the IMU frame */ - bool calibrateWorldYaw(std::unordered_map nodeStruct, Eigen::Ref jointPositions); + bool calibrateWorldYaw(std::unordered_map nodeStruct); /** * compute the calibration matrix between the IMU frame and the associated link frame @@ -449,7 +451,7 @@ class HumanIK * @param frameRef reference frame used as world * @return true if the calibration matrix is set correctly */ - bool calibrateAllWithWorld(std::unordered_map nodeStruct, Eigen::Ref jointPositions, std::string frameRef = ""); + bool calibrateAllWithWorld(std::unordered_map nodeStruct, std::string frameRef = ""); /** * this function solves the inverse kinematics problem and integrate the joint velocity to diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp index 9ec3732..c0f2efd 100644 --- a/src/IK/src/InverseKinematics.cpp +++ b/src/IK/src/InverseKinematics.cpp @@ -37,6 +37,7 @@ bool HumanIK::initialize(std::weak_ptrgetNrOfDegreesOfFreedom()); m_jointVelocities.resize(m_kinDyn->getNrOfDegreesOfFreedom()); + m_calibrationJointPositions.resize(m_kinDyn->getNrOfDegreesOfFreedom()); // Retrieve the state of the system if (!kinDyn->getRobotState(m_basePose, m_jointPositions, m_baseVelocity, m_jointVelocities, m_gravity)) @@ -75,6 +76,26 @@ bool HumanIK::initialize(std::weak_ptrgetParameter("robot_velocity_variable_name", variable); m_variableHandler.addVariable(variable, kinDyn->getNrOfDegreesOfFreedom() + 6); + // Retrieve calibration joint positions parameter from config file, using the param handler + Eigen::VectorXd calibrationJointPositions; + if (ptr->getParameter("calibration_joint_positions", calibrationJointPositions)) + { + // Check that the length of the joint positions is correct + if (calibrationJointPositions.size() != m_kinDyn->getNrOfDegreesOfFreedom()) + { + BiomechanicalAnalysis::log()->warn("{} Calibration joint positions vector has wrong size, setting all joints to zero", logPrefix); + m_calibrationJointPositions.setZero(); + } else + { + m_calibrationJointPositions = calibrationJointPositions; + } + } else + { + // If calibration joint positions are missing, set to 0 + BiomechanicalAnalysis::log()->warn("{} Parameter calibration_joint_positions is missing, setting all joints to zero", logPrefix); + m_calibrationJointPositions.setZero(); + } + // Cycle on the tasks to be initialized for (const auto& task : tasks) { @@ -337,7 +358,7 @@ bool HumanIK::clearCalibrationMatrices() return true; } -bool HumanIK::calibrateWorldYaw(std::unordered_map nodeStruct, Eigen::Ref jointPositions) +bool HumanIK::calibrateWorldYaw(std::unordered_map nodeStruct) { // reset the robot state Eigen::VectorXd jointVelocities; @@ -348,7 +369,7 @@ bool HumanIK::calibrateWorldYaw(std::unordered_map nodeStruct, Ei Eigen::VectorXd baseVelocity; baseVelocity.resize(6); baseVelocity.setZero(); - m_kinDyn->setRobotState(basePose, jointPositions, baseVelocity, jointVelocities, m_gravity); + m_kinDyn->setRobotState(basePose, m_calibrationJointPositions, baseVelocity, jointVelocities, m_gravity); // Update the orientation and gravity tasks for (const auto& [node, data] : nodeStruct) { @@ -381,7 +402,7 @@ bool HumanIK::calibrateWorldYaw(std::unordered_map nodeStruct, Ei return true; } -bool HumanIK::calibrateAllWithWorld(std::unordered_map nodeStruct, Eigen::Ref jointPositions, std::string refFrame) +bool HumanIK::calibrateAllWithWorld(std::unordered_map nodeStruct, std::string refFrame) { // reset the robot state Eigen::VectorXd jointVelocities; @@ -392,7 +413,7 @@ bool HumanIK::calibrateAllWithWorld(std::unordered_map nodeStruct Eigen::VectorXd baseVelocity; baseVelocity.resize(6); baseVelocity.setZero(); - m_kinDyn->setRobotState(basePose, jointPositions, baseVelocity, jointVelocities, m_gravity); + m_kinDyn->setRobotState(basePose, m_calibrationJointPositions, baseVelocity, jointVelocities, m_gravity); manif::SO3d secondaryCalib = manif::SO3d::Identity(); // if a reference frame is provided, compute the world rotation matrix of the reference frame diff --git a/src/IK/tests/HumanIKTest.cpp b/src/IK/tests/HumanIKTest.cpp index f67280c..7aeee43 100644 --- a/src/IK/tests/HumanIKTest.cpp +++ b/src/IK/tests/HumanIKTest.cpp @@ -49,11 +49,6 @@ TEST_CASE("InverseKinematics test") qInitial.setConstant(0.0); - // Define the robot state for calibration - Eigen::VectorXd calibJointPositions; - calibJointPositions.resize(kinDyn->getNrOfDegreesOfFreedom()); - calibJointPositions.setZero(); - REQUIRE(ik.initialize(paramHandler, kinDyn)); REQUIRE(ik.setDt(0.1)); REQUIRE(ik.updateOrientationTask(3, I_R_IMU, I_omega_IMU)); @@ -62,8 +57,8 @@ TEST_CASE("InverseKinematics test") REQUIRE(ik.updateOrientationAndGravityTasks(mapNodeData)); REQUIRE(ik.updateJointConstraintsTask()); REQUIRE(ik.updateJointRegularizationTask()); - REQUIRE(ik.calibrateWorldYaw(mapNodeData, calibJointPositions)); - REQUIRE(ik.calibrateAllWithWorld(mapNodeData, calibJointPositions, "link1")); + REQUIRE(ik.calibrateWorldYaw(mapNodeData)); + REQUIRE(ik.calibrateAllWithWorld(mapNodeData, "link1")); REQUIRE(ik.advance()); REQUIRE(ik.getJointPositions(JointPositions)); REQUIRE(ik.getJointVelocities(JointVelocities)); diff --git a/src/examples/IK/exampleIK.cpp b/src/examples/IK/exampleIK.cpp index e7e7f71..f9a2087 100644 --- a/src/examples/IK/exampleIK.cpp +++ b/src/examples/IK/exampleIK.cpp @@ -442,15 +442,8 @@ int main() nodeStruct[node].I_R_IMU = BipedalLocomotion::Conversions::toManifRot(I_R_IMU); } - // Define the robot state for calibration - Eigen::VectorXd calibJointPositions; - calibJointPositions.resize(kinDyn->getNrOfDegreesOfFreedom()); - calibJointPositions.setZero(); - Eigen::Vector3d calibBasePosition; - calibBasePosition.setZero(); - - ik.calibrateWorldYaw(nodeStruct, calibJointPositions); // Calibrate the world yaw - ik.calibrateAllWithWorld(nodeStruct, calibJointPositions, "LeftFoot"); // Calibrate all with world + ik.calibrateWorldYaw(nodeStruct); // Calibrate the world yaw + ik.calibrateAllWithWorld(nodeStruct, "LeftFoot"); // Calibrate all with world BiomechanicalAnalysis::log()->info("T-pose calibration done"); tPoseFlag = false;