Skip to content

Commit

Permalink
implement foot tracker task
Browse files Browse the repository at this point in the history
  • Loading branch information
@evelyd
evelyd committed Dec 20, 2024
1 parent abf8a60 commit e851676
Show file tree
Hide file tree
Showing 3 changed files with 108 additions and 55 deletions.
4 changes: 2 additions & 2 deletions bindings/python/IK/src/InverseKinematics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,13 +59,13 @@ void CreateInverseKinematics(pybind11::module& module)
.def("updatePoseTask", &HumanIK::updatePoseTask, py::arg("node"), py::arg("I_position"), py::arg("I_R_IMU"), py::arg("I_linearVelocity"), py::arg("I_omega_IMU"))
.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("updateFloorContactTask", &HumanIK::updateFloorContactTask, py::arg("node"), py::arg("verticalForce"), py::arg("I_position"), py::arg("I_R_IMU"), py::arg("I_linearVelocity"), py::arg("I_omega_IMU"), 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("updatePoseTasks", &HumanIK::updatePoseTasks, py::arg("nodeStruct"))
.def("updateOrientationGravityTasks", &HumanIK::updateOrientationAndGravityTasks, py::arg("nodeStruct"))
.def("updateFloorContactTasks", &HumanIK::updateFloorContactTasks, py::arg("wrenchMap"), py::arg("linkHeight"))
.def("updateFloorContactTasks", &HumanIK::updateFloorContactTasks, py::arg("wrenchMap"), py::arg("I_position"), py::arg("I_R_IMU"), py::arg("I_linearVelocity"), py::arg("I_omega_IMU"), py::arg("linkHeight"))
.def("updateJointRegularizationTask", &HumanIK::updateJointRegularizationTask)
.def("updateJointConstraintsTask", &HumanIK::updateJointConstraintsTask)
.def("advance", &HumanIK::advance)
Expand Down
19 changes: 10 additions & 9 deletions src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@
#include <BipedalLocomotion/IK/JointTrackingTask.h>
#include <BipedalLocomotion/IK/JointVelocityLimitsTask.h>
#include <BipedalLocomotion/IK/QPInverseKinematics.h>
#include <BipedalLocomotion/IK/R3Task.h>
#include <BipedalLocomotion/IK/SO3Task.h>
#include <BipedalLocomotion/IK/SE3Task.h>
#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
Expand Down Expand Up @@ -77,10 +76,10 @@ class HumanIK
const std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> taskHandler);

/**
* initialize the R3 task
* initialize the SE3 task
* @param taskName name of the task
* @param handler pointer to the parameters handler
* @return true if the R3 task is initialized correctly
* @return true if the SE3 task is initialized correctly
*/
bool initializeFloorContactTask(const std::string& taskName,
const std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> taskHandler);
Expand Down Expand Up @@ -192,16 +191,18 @@ class HumanIK
};

/**
* Struct containing the R3 task from the BipedalLocomotion IK, the node number and the
* Struct containing the SE3 task from the BipedalLocomotion IK, the node number and the
* multiple state weight provider
*/
struct FloorContactTaskStruct
{
std::shared_ptr<BipedalLocomotion::IK::R3Task> task;
Eigen::Vector3d weight;
std::shared_ptr<BipedalLocomotion::IK::SE3Task> task;
Eigen::VectorXd weight;
int nodeNumber;
bool footInContact{false};
Eigen::Vector3d setPointPosition;
manif::SE3d IMU_H_link; // Transformation matrix from the IMU to related link
manif::SE3d IMU_H_link_init; // Initial value of the transformation matrix from the IMU to related link, set through config
// file
std::string taskName;
std::string frameName;
double verticalForceThreshold;
Expand Down Expand Up @@ -463,7 +464,7 @@ class HumanIK
* @param verticalForce vertical force
* @return true if the orientation setpoint is set correctly
*/
bool updateFloorContactTask(const int node, const double verticalForce, const double linkHeight = 0.0);
bool updateFloorContactTask(const int node, const double verticalForce, const Eigen::Vector3d& I_position = Eigen::Vector3d::Zero(), const manif::SO3d& I_R_IMU = manif::SO3d::Identity(), const Eigen::Vector3d& I_linearVelocity = Eigen::Vector3d::Zero(), const manif::SO3Tangentd& I_omega_IMU = manif::SO3d::Tangent::Zero(), const double linkHeight = 0.0);

/**
* set the setpoint for the joint regularization task.
Expand Down Expand Up @@ -503,7 +504,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, const double linkHeight = 0.0);
bool updateFloorContactTasks(const std::unordered_map<int, Eigen::Matrix<double, 6, 1>>& wrenchMap, const Eigen::Vector3d& I_position = Eigen::Vector3d::Zero(), const manif::SO3d& I_R_IMU = manif::SO3d::Identity(), const Eigen::Vector3d& I_linearVelocity = Eigen::Vector3d::Zero(), const manif::SO3Tangentd& I_omega_IMU = manif::SO3d::Tangent::Zero(), const double linkHeight = 0.0);

/**
* clear the calibration matrices W_R_WIMU and IMU_R_link of all the orientation and gravity tasks
Expand Down
140 changes: 96 additions & 44 deletions src/IK/src/InverseKinematics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -279,7 +279,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, const double linkHeight)
bool HumanIK::updateFloorContactTask(const int node, const double verticalForce, const Eigen::Vector3d& I_position, const manif::SO3d& I_R_IMU, const Eigen::Vector3d& I_linearVelocity, const manif::SO3Tangentd& I_omega_IMU, const double linkHeight)
{
bool ok{true};
// check if the node number is valid
Expand All @@ -300,30 +300,33 @@ bool HumanIK::updateFloorContactTask(const int node, const double verticalForce,
m_kinDyn->setRobotState(m_basePose, jointPositions, baseVelocity, m_jointVelocities, m_gravity);
}

// if the vertical force is greater than the threshold and if the foot is not yet in contact,
// set the weight of the associated task to the weight of the task and set the set point of the
// task to the position of the frame computed with the legged odometry
if (verticalForce > m_FloorContactTasks[node].verticalForceThreshold && !m_FloorContactTasks[node].footInContact)
{
m_qpIK.setTaskWeight(m_FloorContactTasks[node].taskName, m_FloorContactTasks[node].weight);
m_FloorContactTasks[node].footInContact = true;
m_FloorContactTasks[node].setPointPosition
= iDynTree::toEigen(m_kinDyn->getWorldTransform(m_FloorContactTasks[node].frameName).getPosition());
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
m_qpIK.setTaskWeight(m_FloorContactTasks[node].taskName, Eigen::Vector3d::Zero());
m_FloorContactTasks[node].footInContact = false;
}
// Compute the rotation matrix from the world to the link frame as:
// W_R_link = W_R_WIMU * WIMU_R_IMU * IMU_R_link
manif::SE3d I_H_IMU(I_position, I_R_IMU);

// if the foot is in contact, set the set point of the task
if (m_FloorContactTasks[node].footInContact)
{
ok = m_FloorContactTasks[node].task->setSetPoint(m_FloorContactTasks[node].setPointPosition);
// Create the transformation matrix from the world to the link frame
manif::SE3d I_H_link = I_H_IMU * m_FloorContactTasks[node].IMU_H_link;

// Compute the linear velocity in the link frame (left trivialized)
// I_linearVelocity_link = link_R_W * W_R_WIMU * WIMU_linearVelocity
Eigen::Vector3d I_linearVelocity_link = I_H_link.rotation().transpose() * I_linearVelocity;

// Compute the angular velocity of the link frame (right trivialized)
Eigen::Vector3d I_omega_link = I_omega_IMU.coeffs();

Eigen::VectorXd mixedVelocityVector(6);
mixedVelocityVector << I_linearVelocity_link, I_omega_link;

// Create mixed velocity vector
manif::SE3d::Tangent mixedVelocity(mixedVelocityVector);

if (verticalForce > m_FloorContactTasks[node].verticalForceThreshold)
{ // if the vertical force is greater than the threshold, set the foot height to 0
I_H_link.translation(Eigen::Vector3d(I_H_link.translation().x(), I_H_link.translation().y(), linkHeight));
}

return ok;
// then set the set point for the task regardless of whether the foot is in contact
return m_FloorContactTasks[node].task->setSetPoint(I_H_link, mixedVelocity);
}

bool HumanIK::updateJointRegularizationTask()
Expand Down Expand Up @@ -412,11 +415,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, const double linkHeight)
bool HumanIK::updateFloorContactTasks(const std::unordered_map<int, Eigen::Matrix<double, 6, 1>>& wrenchMap, const Eigen::Vector3d& I_position, const manif::SO3d& I_R_IMU, const Eigen::Vector3d& I_linearVelocity, const manif::SO3Tangentd& I_omega_IMU, const double linkHeight)
{
for (const auto& [node, data] : wrenchMap)
{
if (!updateFloorContactTask(node, data(WRENCH_FORCE_Z), linkHeight))
if (!updateFloorContactTask(node, data(WRENCH_FORCE_Z), I_position, I_R_IMU, I_linearVelocity, I_omega_IMU, linkHeight))
{
BiomechanicalAnalysis::log()->error("[HumanIK::updateFloorContactTasks] Error in updating "
"the floor contact task of node {}",
Expand All @@ -443,6 +446,10 @@ bool HumanIK::clearCalibrationMatrices()
data.calibrationMatrix = manif::SO3d::Identity();
data.IMU_R_link = m_GravityTasks[node].IMU_R_link_init;
}
for (auto& [frameName, data] : m_FloorContactTasks)
{
data.IMU_H_link = m_FloorContactTasks[frameName].IMU_H_link_init;
}
return true;
}

Expand Down Expand Up @@ -994,24 +1001,73 @@ bool HumanIK::initializeFloorContactTask(const std::string& taskName,
return false;
}

// Retrieve weight parameter from the task handler
// Set node number and task name for the FloorContactTask
m_FloorContactTasks[nodeNumber].nodeNumber = nodeNumber;
m_FloorContactTasks[nodeNumber].taskName = taskName;

// Retrieve the mask parameter from config file, using the task handler
std::vector<bool> mask;
size_t numTrue;
if (!taskHandler->getParameter("mask", mask))
{
numTrue = 6;
}
else
{
numTrue = std::count(mask.begin(), mask.end(), true) + 3;
}

// Retrieve weight parameter from config file, using the task handler
std::vector<double> weight;
if (!taskHandler->getParameter("weight", weight))
{
BiomechanicalAnalysis::log()->error("{} Parameter weight of the {} task is missing", logPrefix, taskName);
return false;

if (weight.size() != numTrue)
{
BiomechanicalAnalysis::log()->error("{} The size of the parameter weight of the {} task is "
"{}, it should be {}",
logPrefix,
taskName,
weight.size(),
numTrue);
return false;
}

}

// Check that the weight is a 3D vector
if (weight.size() != 3)
m_FloorContactTasks[nodeNumber].weight.resize(weight.size());
m_FloorContactTasks[nodeNumber].weight = Eigen::Map<Eigen::VectorXd>(weight.data(), weight.size());

// Create an SE3Task object for the pose task
m_FloorContactTasks[nodeNumber].task = std::make_shared<BipedalLocomotion::IK::SE3Task>();

//*****************************************************************************************************
// Retrieve Transformation matrix IMU-to-link from configuration file exampleIK.ini
//*****************************************************************************************************

std::vector<double> transformation_matrix;
if (taskHandler->getParameter("transformation_matrix", transformation_matrix))
{
BiomechanicalAnalysis::log()->error("{} The size of the parameter weight of the {} task is "
"{}, it should be 3",
logPrefix,
taskName,
weight.size());
return false;
// Convert transformation matrix to ManifRot and assign it to IMU_R_link
Eigen::Matrix4d transformation_matrix_eigen = Eigen::Map<Eigen::Matrix<double, 4, 4, Eigen::RowMajor>>(transformation_matrix.data());
m_FloorContactTasks[nodeNumber].IMU_H_link_init = manif::SE3d(transformation_matrix_eigen.block<3, 1>(0, 3),
BipedalLocomotion::Conversions::toManifRot(transformation_matrix_eigen.block<3, 3>(0, 0)));
} else
{
// If transformation_matrix parameter is missing, set IMU_H_link to identity
std::string frame_name;
taskHandler->getParameter("frame_name", frame_name);
BiomechanicalAnalysis::log()->warn("{} Parameter transformation_matrix of the {} task is "
"missing, setting the transformation "
"matrix from the IMU to the frame {} to identity",
logPrefix,
taskName,
frame_name);
m_FloorContactTasks[nodeNumber].IMU_H_link_init.setIdentity();
}
m_FloorContactTasks[nodeNumber].IMU_H_link = m_FloorContactTasks[nodeNumber].IMU_H_link_init;

// Retrieve vertical force threshold parameter from the task handler and assign it to the
// corresponding FloorContactTask
Expand All @@ -1024,24 +1080,20 @@ bool HumanIK::initializeFloorContactTask(const std::string& taskName,
return false;
}

// Map weight vector to Eigen::Vector3d and assign it to the corresponding FloorContactTask
m_FloorContactTasks[nodeNumber].weight = Eigen::Map<Eigen::Vector3d>(weight.data());

// Set node number and task name for the FloorContactTask
m_FloorContactTasks[nodeNumber].nodeNumber = nodeNumber;
m_FloorContactTasks[nodeNumber].taskName = taskName;

// Create an R3Task object for the floor contact task
m_FloorContactTasks[nodeNumber].task = std::make_shared<BipedalLocomotion::IK::R3Task>();

// Initialize the R3Task object
// Initialize the SE3Task object
ok = ok && m_FloorContactTasks[nodeNumber].task->setKinDyn(m_kinDyn);
ok = ok && m_FloorContactTasks[nodeNumber].task->initialize(taskHandler);

// Add the floor contact task to the QP solver
ok = ok && m_qpIK.addTask(m_FloorContactTasks[nodeNumber].task, taskName, 1, m_FloorContactTasks[nodeNumber].weight);

// Check if initialization was successful
if (!ok)
{
BiomechanicalAnalysis::log()->error("{} Error in the initialization of the {} task", logPrefix, taskName);
return false;
}

return ok;
}

Expand Down

0 comments on commit e851676

Please sign in to comment.