From 406d2e0d3698c76cde33daf1b383de120af65800 Mon Sep 17 00:00:00 2001 From: Davide Date: Thu, 14 Dec 2023 16:48:10 +0100 Subject: [PATCH] added integrator and setting robot state in advance --- src/IK/CMakeLists.txt | 2 +- .../IK/InverseKinematics.h | 25 ++++++-- src/IK/src/InverseKinematics.cpp | 62 +++++++++++++++---- 3 files changed, 72 insertions(+), 17 deletions(-) diff --git a/src/IK/CMakeLists.txt b/src/IK/CMakeLists.txt index edb3f9e..21f064c 100644 --- a/src/IK/CMakeLists.txt +++ b/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) diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h index 7adb730..5803567 100644 --- a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h +++ b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h @@ -14,6 +14,8 @@ #include #include #include +#include +#include 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> integrator; + std::shared_ptr 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 m_baseVelocity; manif::SO3d m_baseOrientation; Eigen::Vector3d m_baseAngularVelocity; + Eigen::Vector3d m_gravity; // tasks std::shared_ptr m_link1OrientationTask; + // pointer to the KinDynComputations object + std::shared_ptr m_kinDyn; + // Number of Joint Degrees of Freedom int m_nrDoFs; diff --git a/src/IK/src/InverseKinematics.cpp b/src/IK/src/InverseKinematics.cpp index e07d87a..4ccc0b1 100644 --- a/src/IK/src/InverseKinematics.cpp +++ b/src/IK/src/InverseKinematics.cpp @@ -1,9 +1,12 @@ #include #include +#include #include using namespace BiomechanicalAnalysis::IK; - +using namespace BipedalLocomotion::ContinuousDynamicalSystem; +using namespace BipedalLocomotion::Conversions; +using namespace std::chrono_literals; bool HumanIK::initialize(std::weak_ptr handler, std::shared_ptr kinDyn) @@ -12,13 +15,29 @@ bool HumanIK::initialize(std::weak_ptrgetNrOfDegreesOfFreedom()); - 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(); + m_system.dynamics->setState({m_basePose.topRightCorner<3, 1>(), toManifRot(m_basePose.topLeftCorner<3,3>()) , m_jointPositions}); + + m_system.integrator = std::make_shared>(); + 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(std::chrono::duration(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 jointPositions) const @@ -113,7 +151,7 @@ bool HumanIK::getJointVelocities(Eigen::Ref jointVelocities) co bool HumanIK::getBasePosition(Eigen::Ref basePosition) const { - basePosition = m_basePosition; + basePosition = m_baseLinearPosition; return true; }