added integrator and setting robot state in advance

src/IK/CMakeLists.txt

@@ -3,6 +3,6 @@ add_biomechanical_analysis_library(
     NAME                   IK
     PUBLIC_HEADERS         include/BiomechanicalAnalysis/IK/InverseKinematics.h
     SOURCES                src/InverseKinematics.cpp
-    PUBLIC_LINK_LIBRARIES  BipedalLocomotion::IK BipedalLocomotion::ParametersHandler
+    PUBLIC_LINK_LIBRARIES  BipedalLocomotion::IK BipedalLocomotion::ParametersHandler BipedalLocomotion::ContinuousDynamicalSystem BipedalLocomotion::CommonConversions
     PRIVATE_LINK_LIBRARIES BiomechanicalAnalysis::Logging
     SUBDIRECTORIES         tests)

src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h

@@ -14,6 +14,8 @@
 #include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
 #include <BipedalLocomotion/System/VariablesHandler.h>
 #include <BipedalLocomotion/IK/SO3Task.h>
+#include <BipedalLocomotion/ContinuousDynamicalSystem/FloatingBaseSystemKinematics.h>
+#include <BipedalLocomotion/ContinuousDynamicalSystem/ForwardEuler.h>
 
 namespace BiomechanicalAnalysis
 {
@@ -24,20 +26,35 @@ namespace IK
 class HumanIK
 {
 private:
-    // Integration time step
-    double m_dtIntegration;
+    // Integration time step in nanoseconds
+    std::chrono::nanoseconds m_dtIntegration;
+
+    // Struct to integrate the base and joint velocities
+    struct System
+    {
+        std::shared_ptr<BipedalLocomotion::ContinuousDynamicalSystem::ForwardEuler<BipedalLocomotion::ContinuousDynamicalSystem::FloatingBaseSystemKinematics>> integrator;
+        std::shared_ptr<BipedalLocomotion::ContinuousDynamicalSystem::FloatingBaseSystemKinematics> dynamics;
+    };
+
+    // System to integrate the base and joint velocities
+    System m_system;    
 
     // Joint positions and velocities
     Eigen::VectorXd m_jointPositions;
     Eigen::VectorXd m_jointVelocities;
-    Eigen::Vector3d m_basePosition;
-    manif::SE3Tangentd m_baseVelocity;
+    Eigen::Matrix4d m_basePose;
+    Eigen::Vector3d m_baseLinearPosition;
+    Eigen::Matrix<double, 6, 1> m_baseVelocity;
     manif::SO3d m_baseOrientation;
     Eigen::Vector3d m_baseAngularVelocity;
+    Eigen::Vector3d m_gravity;
 
     // tasks
     std::shared_ptr<BipedalLocomotion::IK::SO3Task> m_link1OrientationTask;
 
+    // pointer to the KinDynComputations object
+    std::shared_ptr<iDynTree::KinDynComputations> m_kinDyn;
+
     // Number of Joint Degrees of Freedom
     int m_nrDoFs;
 

src/IK/src/InverseKinematics.cpp

@@ -1,9 +1,12 @@
 #include <BiomechanicalAnalysis/IK/InverseKinematics.h>
 #include <BiomechanicalAnalysis/Logging/Logger.h>
+#include <BipedalLocomotion/Conversions/ManifConversions.h>
 #include <iostream>
 
 using namespace BiomechanicalAnalysis::IK;
-
+using namespace BipedalLocomotion::ContinuousDynamicalSystem;
+using namespace BipedalLocomotion::Conversions;
+using namespace std::chrono_literals;
 
 bool HumanIK::initialize(std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler,
                 std::shared_ptr<iDynTree::KinDynComputations> kinDyn)
@@ -12,13 +15,29 @@ bool HumanIK::initialize(std::weak_ptr<const BipedalLocomotion::ParametersHandle
     constexpr std::size_t lowPriority = 1;
     constexpr auto logPrefix = "[HumanIK::initialize]";
 
-    m_jointPositions.resize(kinDyn->getNrOfDegreesOfFreedom());
-    m_jointVelocities.resize(kinDyn->getNrOfDegreesOfFreedom());
+    if ((kinDyn == nullptr) || (!kinDyn->isValid()))
+    {
+        BiomechanicalAnalysis::log()->error("{} Invalid kinDyn object.", logPrefix);
+        return false;
+    }
+
+    m_kinDyn = kinDyn;
+
+    m_jointPositions.resize(m_kinDyn->getNrOfDegreesOfFreedom());
+    m_jointVelocities.resize(m_kinDyn->getNrOfDegreesOfFreedom());
+
+    kinDyn->getRobotState(m_basePose, m_jointPositions, m_baseVelocity, m_jointVelocities, m_gravity);
+
+    m_system.dynamics = std::make_shared<FloatingBaseSystemKinematics>();
+    m_system.dynamics->setState({m_basePose.topRightCorner<3, 1>(), toManifRot(m_basePose.topLeftCorner<3,3>()) , m_jointPositions});
+
+    m_system.integrator = std::make_shared<ForwardEuler<FloatingBaseSystemKinematics>>();
+    m_system.integrator->setDynamicalSystem(m_system.dynamics);
 
     auto ptr = handler.lock();
     if (ptr == nullptr)
     {
-        BiomechanicalAnalysis::log()->error("{} parameters handler is a null pointer.", logPrefix);
+        BiomechanicalAnalysis::log()->error("{} Invalid parameters handler.", logPrefix);
         return false;
     }
 
@@ -40,14 +59,15 @@ bool HumanIK::initialize(std::weak_ptr<const BipedalLocomotion::ParametersHandle
 
 bool HumanIK::setDt(const double dt)
 {
-    m_dtIntegration = dt;
+    dt;
+    m_dtIntegration = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(dt));
 
-    return true;
+    return m_system.integrator->setIntegrationStep(m_dtIntegration);
 }
 
 double HumanIK::getDt() const
 {
-    return m_dtIntegration;
+    return m_dtIntegration.count();
 }
 
 bool HumanIK::setDoFsNumber(const int nrDoFs)
@@ -88,13 +108,31 @@ bool HumanIK::advance()
     if(ok)
     {
         m_jointVelocities = m_qpIK.getOutput().jointVelocity;
-        m_baseVelocity = m_qpIK.getOutput().baseVelocity;
+        m_baseVelocity = m_qpIK.getOutput().baseVelocity.coeffs();
+    }
+    else
+    {
+        BiomechanicalAnalysis::log()->error("[HumanIK::advance] Error in the QP solver.");
     }
 
-    // integrate the joint velocities
-    m_jointPositions += m_dtIntegration * m_jointVelocities;
+    ok = ok && m_system.dynamics->setControlInput({m_baseVelocity, m_jointVelocities});
+    ok = ok && m_system.integrator->integrate(0s, m_dtIntegration);
 
-    return true;
+    if(ok)
+    {
+        const auto& [basePosition, baseRotation, jointPosition]
+                    = m_system.integrator->getSolution();
+        m_basePose.topRightCorner<3, 1>() = basePosition;
+        m_basePose.topLeftCorner<3, 3>() = baseRotation.rotation();
+        m_jointPositions = jointPosition;
+        m_kinDyn->setRobotState(m_basePose, jointPosition, m_baseVelocity, m_jointVelocities, m_gravity);
+    }
+    else
+    {
+        BiomechanicalAnalysis::log()->error("[HumanIK::advance] Error in the integration.");
+    }
+
+    return ok;
 }
 
 bool HumanIK::getJointPositions(Eigen::Ref<Eigen::VectorXd> jointPositions) const
@@ -113,7 +151,7 @@ bool HumanIK::getJointVelocities(Eigen::Ref<Eigen::VectorXd> jointVelocities) co
 
 bool HumanIK::getBasePosition(Eigen::Ref<Eigen::Vector3d> basePosition) const
 {
-    basePosition = m_basePosition;
+    basePosition = m_baseLinearPosition;
 
     return true;
 }