define calib joint positions in config file and give foot link height…

… a default value

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)

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<int, Eigen::Matrix<double, 6, 1>>& wrenchMap, const double linkHeight);
+    bool updateFloorContactTasks(const std::unordered_map<int, Eigen::Matrix<double, 6, 1>>& 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<int, nodeData> nodeStruct, Eigen::Ref<const Eigen::VectorXd> jointPositions);
+    bool calibrateWorldYaw(std::unordered_map<int, nodeData> 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<int, nodeData> nodeStruct, Eigen::Ref<const Eigen::VectorXd> jointPositions, std::string frameRef = "");
+    bool calibrateAllWithWorld(std::unordered_map<int, nodeData> nodeStruct, std::string frameRef = "");
 
     /**
      * this function solves the inverse kinematics problem and integrate the joint velocity to

src/IK/src/InverseKinematics.cpp

@@ -37,6 +37,7 @@ bool HumanIK::initialize(std::weak_ptr<const BipedalLocomotion::ParametersHandle
     // resize kinematic variables based on DoF
     m_jointPositions.resize(m_kinDyn->getNrOfDegreesOfFreedom());
     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_ptr<const BipedalLocomotion::ParametersHandle
     group->getParameter("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<int, nodeData> nodeStruct, Eigen::Ref<const Eigen::VectorXd> jointPositions)
+bool HumanIK::calibrateWorldYaw(std::unordered_map<int, nodeData> nodeStruct)
 {
     // reset the robot state
     Eigen::VectorXd jointVelocities;
@@ -348,7 +369,7 @@ bool HumanIK::calibrateWorldYaw(std::unordered_map<int, nodeData> 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<int, nodeData> nodeStruct, Ei
     return true;
 }
 
-bool HumanIK::calibrateAllWithWorld(std::unordered_map<int, nodeData> nodeStruct, Eigen::Ref<const Eigen::VectorXd> jointPositions, std::string refFrame)
+bool HumanIK::calibrateAllWithWorld(std::unordered_map<int, nodeData> nodeStruct, std::string refFrame)
 {
     // reset the robot state
     Eigen::VectorXd jointVelocities;
@@ -392,7 +413,7 @@ bool HumanIK::calibrateAllWithWorld(std::unordered_map<int, nodeData> 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

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));

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;