small fixes

ami-iit · Oct 7, 2024 · 6740fbf · 6740fbf
1 parent afbecfa
commit 6740fbf
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 5 deletions.
diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
@@ -439,7 +439,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::VectorXd jointPositions);
+    bool calibrateWorldYaw(std::unordered_map<int, nodeData> nodeStruct, Eigen::Ref<const Eigen::VectorXd> jointPositions);
 
     /**
      * compute the calibration matrix between the IMU frame and the associated link frame
@@ -449,7 +449,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::VectorXd jointPositions, std::string frameRef = "");
+    bool calibrateAllWithWorld(std::unordered_map<int, nodeData> nodeStruct, Eigen::Ref<const Eigen::VectorXd> jointPositions, 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
@@ -337,7 +337,7 @@ bool HumanIK::clearCalibrationMatrices()
     return true;
 }
 
-bool HumanIK::calibrateWorldYaw(std::unordered_map<int, nodeData> nodeStruct, Eigen::VectorXd jointPositions)
+bool HumanIK::calibrateWorldYaw(std::unordered_map<int, nodeData> nodeStruct, Eigen::Ref<const Eigen::VectorXd> jointPositions)
 {
     // reset the robot state
     Eigen::VectorXd jointVelocities;
@@ -381,7 +381,7 @@ bool HumanIK::calibrateWorldYaw(std::unordered_map<int, nodeData> nodeStruct, Ei
     return true;
 }
 
-bool HumanIK::calibrateAllWithWorld(std::unordered_map<int, nodeData> nodeStruct, Eigen::VectorXd jointPositions, std::string refFrame)
+bool HumanIK::calibrateAllWithWorld(std::unordered_map<int, nodeData> nodeStruct, Eigen::Ref<const Eigen::VectorXd> jointPositions, std::string refFrame)
 {
     // reset the robot state
     Eigen::VectorXd jointVelocities;

diff --git a/src/examples/IK/exampleIK.cpp b/src/examples/IK/exampleIK.cpp
@@ -410,6 +410,9 @@ int main()
     // Start T-pose calibration thread
     std::thread tPoseThread = std::thread(setTPoseThread);
 
+    // Set foot height
+    const double linkHeight = 0.0;
+
     // Continuosly read from the sensor measurements (span time instants) and compute the IK
 
     // Cycle over the time instants
@@ -491,7 +494,7 @@ int main()
             getNodeVerticalForce(ifeel_data, node, ii, force);
 
             // IK solver: Update floor contact task for the current node
-            ik.updateFloorContactTask(node, force, 0.0);
+            ik.updateFloorContactTask(node, force, linkHeight);
 
             // Convert orientation to a manif object
             manif::SO3d I_R_IMU_manif = manif::SO3d(fromiDynTreeToEigenQuatConversion(I_R_IMU));