Skip to content

Commit

Permalink
added integrator and setting robot state in advance
Browse files Browse the repository at this point in the history
  • Loading branch information
davidegorbani committed Dec 14, 2023
1 parent ddb3095 commit 406d2e0
Show file tree
Hide file tree
Showing 3 changed files with 72 additions and 17 deletions.
2 changes: 1 addition & 1 deletion src/IK/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
25 changes: 21 additions & 4 deletions src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand All @@ -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;

Expand Down
62 changes: 50 additions & 12 deletions src/IK/src/InverseKinematics.cpp
Original file line number Diff line number Diff line change
@@ -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)
Expand All @@ -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;
}

Expand All @@ -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)
Expand Down Expand Up @@ -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
Expand All @@ -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;
}
Expand Down

0 comments on commit 406d2e0

Please sign in to comment.