Merge pull request #52 from ami-iit/add_ik_bindings

Add HumanIK python bindings

.github/workflows/ci.yml

@@ -89,6 +89,7 @@ jobs:
                   -DFRAMEWORK_COMPILE_YarpImplementation=ON \
                   -DBUILD_TESTING:BOOL=ON \
                   -DBUILD_EXAMPLES:BOOL=ON \
+                  -DFRAMEWORK_COMPILE_PYTHON_BINDINGS:BOOL=ON \
                   -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install
 
         - name: Configure [Windows]
@@ -103,6 +104,7 @@ jobs:
                   -DFRAMEWORK_COMPILE_YarpImplementation=ON \
                   -DBUILD_TESTING:BOOL=ON \
                   -DBUILD_EXAMPLES:BOOL=ON \
+                  -DFRAMEWORK_COMPILE_PYTHON_BINDINGS:BOOL=ON \
                   -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install
                     
         # Build step          

bindings/python/IK/CMakeLists.txt

@@ -7,6 +7,6 @@ set(H_PREFIX include/BiomechanicalAnalysis/bindings/IK)
 
 add_baf_python_module(
     NAME IKBindings
-    SOURCES src/Module.cpp
-    HEADERS ${H_PREFIX}/Module.h
+    SOURCES src/Module.cpp src/InverseKinematics.cpp
+    HEADERS ${H_PREFIX}/Module.h ${H_PREFIX}/InverseKinematics.h
     LINK_LIBRARIES BiomechanicalAnalysis::IK)

bindings/python/IK/include/BiomechanicalAnalysis/bindings/IK/InverseKinematics.h

@@ -0,0 +1,26 @@
+/**
+ * @file InverseKinematics.h
+ * @authors Evelyn D'Elia
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIOMECHANICAL_ANALYSIS_BINDINGS_IK_INVERSE_KINEMATICS_H
+#define BIOMECHANICAL_ANALYSIS_BINDINGS_IK_INVERSE_KINEMATICS_H
+
+#include <pybind11/pybind11.h>
+
+namespace BiomechanicalAnalysis
+{
+namespace bindings
+{
+namespace IK
+{
+
+void CreateInverseKinematics(pybind11::module& module);
+
+} // namespace IK
+} // namespace bindings
+} // namespace BiomechanicalAnalysis
+
+#endif // BIOMECHANICAL_ANALYSIS_BINDINGS_IK_INVERSE_KINEMATICS_H

bindings/python/IK/src/InverseKinematics.cpp

@@ -0,0 +1,107 @@
+/**
+ * @file InverseKinematics.cpp
+ * @authors Evelyn D'Elia
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <pybind11/chrono.h>
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BiomechanicalAnalysis/IK/InverseKinematics.h>
+#include <BiomechanicalAnalysis/bindings/type_caster/swig.h>
+#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
+
+namespace BiomechanicalAnalysis
+{
+namespace bindings
+{
+namespace IK
+{
+
+void CreateInverseKinematics(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+
+    using namespace BiomechanicalAnalysis::IK;
+    using namespace BipedalLocomotion::ParametersHandler;
+
+    py::class_<nodeData>(module, "nodeData")
+        .def(py::init<>())
+        .def_readwrite("I_R_IMU", &nodeData::I_R_IMU)
+        .def_readwrite("I_omega_IMU", &nodeData::I_omega_IMU);
+
+    py::class_<HumanIK>(module, "HumanIK")
+        .def(py::init())
+        .def(
+            "initialize",
+            [](HumanIK& ik, std::shared_ptr<const IParametersHandler> handler, py::object& obj) -> bool {
+                std::shared_ptr<iDynTree::KinDynComputations>* cls
+                    = py::detail::swig_wrapped_pointer_to_pybind<std::shared_ptr<iDynTree::KinDynComputations>>(obj);
+
+                if (cls == nullptr)
+                {
+                    throw ::pybind11::value_error("Invalid input for the function. Please provide "
+                                                  "an iDynTree::KinDynComputations object.");
+                }
+
+                return ik.initialize(handler, *cls);
+            },
+            py::arg("param_handler"),
+            py::arg("kin_dyn"))
+        .def("setDt", &HumanIK::setDt, py::arg("dt"))
+        .def("getDt", &HumanIK::getDt)
+        .def("getDoFsNumber", &HumanIK::getDoFsNumber)
+        .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("updateOrientationGravityTasks", &HumanIK::updateOrientationAndGravityTasks, py::arg("nodeStruct"))
+        .def("updateFloorContactTasks", &HumanIK::updateFloorContactTasks, py::arg("wrenchMap"), py::arg("linkHeight"))
+        .def("updateJointRegularizationTask", &HumanIK::updateJointRegularizationTask)
+        .def("updateJointConstraintsTask", &HumanIK::updateJointConstraintsTask)
+        .def("advance", &HumanIK::advance)
+        .def("getJointPositions",
+             [](HumanIK& ik) {
+                 Eigen::VectorXd jointPositions(ik.getDoFsNumber());
+                 bool ok = ik.getJointPositions(jointPositions);
+                 return std::make_tuple(ok, jointPositions);
+             })
+        .def("getJointVelocities",
+             [](HumanIK& ik) {
+                 Eigen::VectorXd jointVelocities(ik.getDoFsNumber());
+                 bool ok = ik.getJointVelocities(jointVelocities);
+                 return std::make_tuple(ok, jointVelocities);
+             })
+        .def("getBasePosition",
+             [](HumanIK& ik) {
+                 Eigen::Vector3d basePosition;
+                 bool ok = ik.getBasePosition(basePosition);
+                 return std::make_tuple(ok, basePosition);
+             })
+        .def("getBaseOrientation",
+             [](HumanIK& ik) {
+                 Eigen::Matrix3d baseOrientation;
+                 bool ok = ik.getBaseOrientation(baseOrientation);
+                 return std::make_tuple(ok, baseOrientation);
+             })
+        .def("getBaseLinearVelocity",
+             [](HumanIK& ik) {
+                 Eigen::Vector3d baseVelocity;
+                 bool ok = ik.getBaseLinearVelocity(baseVelocity);
+                 return std::make_tuple(ok, baseVelocity);
+             })
+        .def("getBaseAngularVelocity", [](HumanIK& ik) {
+            Eigen::Vector3d baseAngularVelocity;
+            bool ok = ik.getBaseAngularVelocity(baseAngularVelocity);
+            return std::make_tuple(ok, baseAngularVelocity);
+        });
+}
+
+} // namespace IK
+} // namespace bindings
+} // namespace BiomechanicalAnalysis

bindings/python/IK/src/Module.cpp

@@ -6,6 +6,9 @@
 
 #include <pybind11/pybind11.h>
 
+#include <BiomechanicalAnalysis/bindings/IK/InverseKinematics.h>
+#include <BiomechanicalAnalysis/bindings/IK/Module.h>
+
 namespace BiomechanicalAnalysis
 {
 namespace bindings
@@ -16,6 +19,8 @@ namespace IK
 void CreateModule(pybind11::module& module)
 {
     module.doc() = "IK module.";
+
+    CreateInverseKinematics(module);
 }
 } // namespace IK
 } // namespace bindings

ci_env.yml

@@ -13,3 +13,4 @@ dependencies:
   - catch2>=3.3.0
   - manif
   - bipedal-locomotion-framework>=0.19.0
+  - pybind11

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
@@ -327,7 +329,7 @@ class HumanIK
      * node_number                     2
      * weight                          (1.0 1.0 1.0)
      * vertical_force_threshold        60.0
-     * 
+     *
      * [JOINT_LIMITS_TASK]
      * type                            "JointConstraintTask"
      * robot_velocity_variable_name    "robot_velocity"
@@ -337,7 +339,7 @@ class HumanIK
      * joints_list                     ("jLeftKnee_rotz", "jRightKnee_rotz", "jLeftAnkle_rotz", "jRightAnkle_rotz")
      * upper_bounds                    (0.0, 0.0, 0.0, 0.0)
      * lower_bounds                    (0.0, 0.0, 0.0, 0.0)
-     * 
+     *
      * [JOINT_REG_TASK]
      * type                            "JointRegularizationTask"
      * robot_velocity_variable_name    "robot_velocity"
@@ -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);
+    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);
+    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

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)
     {
@@ -211,7 +232,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)
+bool HumanIK::updateFloorContactTask(const int node, const double verticalForce, const double linkHeight)
 {
     bool ok{true};
     // check if the node number is valid
@@ -243,7 +264,7 @@ bool HumanIK::updateFloorContactTask(const int node, const double verticalForce)
         m_FloorContactTasks[node].footInContact = true;
         m_FloorContactTasks[node].setPointPosition
             = iDynTree::toEigen(m_kinDyn->getWorldTransform(m_FloorContactTasks[node].frameName).getPosition());
-        m_FloorContactTasks[node].setPointPosition(2) = 0.0;
+        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
@@ -319,11 +340,11 @@ bool HumanIK::updateOrientationAndGravityTasks(const std::unordered_map<int, nod
     return true;
 }
 
-bool HumanIK::updateFloorContactTasks(const std::unordered_map<int, Eigen::Matrix<double, 6, 1>>& wrenchMap)
+bool HumanIK::updateFloorContactTasks(const std::unordered_map<int, Eigen::Matrix<double, 6, 1>>& wrenchMap, const double linkHeight)
 {
     for (const auto& [node, data] : wrenchMap)
     {
-        if (!updateFloorContactTask(node, data(WRENCH_FORCE_Z)))
+        if (!updateFloorContactTask(node, data(WRENCH_FORCE_Z), linkHeight))
         {
             BiomechanicalAnalysis::log()->error("[HumanIK::updateFloorContactTasks] Error in updating "
                                                 "the floor contact task of node {}",
@@ -352,18 +373,15 @@ bool HumanIK::clearCalibrationMatrices()
 bool HumanIK::calibrateWorldYaw(std::unordered_map<int, nodeData> nodeStruct)
 {
     // reset the robot state
-    Eigen::VectorXd jointPositions;
     Eigen::VectorXd jointVelocities;
-    jointPositions.resize(this->getDoFsNumber());
-    jointPositions.setZero();
     jointVelocities.resize(this->getDoFsNumber());
     jointVelocities.setZero();
     Eigen::Matrix4d basePose;
     basePose.setIdentity();
     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)
     {
@@ -399,18 +417,15 @@ bool HumanIK::calibrateWorldYaw(std::unordered_map<int, nodeData> nodeStruct)
 bool HumanIK::calibrateAllWithWorld(std::unordered_map<int, nodeData> nodeStruct, std::string refFrame)
 {
     // reset the robot state
-    Eigen::VectorXd jointPositions;
     Eigen::VectorXd jointVelocities;
-    jointPositions.resize(this->getDoFsNumber());
-    jointPositions.setZero();
     jointVelocities.resize(this->getDoFsNumber());
     jointVelocities.setZero();
     Eigen::Matrix4d basePose;
     basePose.setIdentity();
     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
@@ -491,9 +506,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
-        initialJointPositions.resize(this->getDoFsNumber());
-        initialJointPositions.setZero();
-        m_system.dynamics->setState({basePose.topRightCorner<3, 1>(), toManifRot(basePose.topLeftCorner<3, 3>()), initialJointPositions});
+        m_system.dynamics->setState({basePose.topRightCorner<3, 1>(), toManifRot(basePose.topLeftCorner<3, 3>()), m_calibrationJointPositions});
         m_tPose = false;
     }
 

src/examples/IK/exampleIK.cpp

@@ -438,6 +438,7 @@ int main()
                 // Perform T-Pose calibration for each node
                 nodeStruct[node].I_R_IMU = BipedalLocomotion::Conversions::toManifRot(I_R_IMU);
             }
+
             ik.calibrateWorldYaw(nodeStruct); // Calibrate the world yaw
             ik.calibrateAllWithWorld(nodeStruct, "LeftFoot"); // Calibrate all with world