Skip to content

Commit

Permalink
Merge branch 'main' into add-comment-IDcpp
Browse files Browse the repository at this point in the history
  • Loading branch information
davidegorbani authored Jul 2, 2024
2 parents ced9560 + 4d292ae commit d029d50
Show file tree
Hide file tree
Showing 5 changed files with 122 additions and 7 deletions.
72 changes: 71 additions & 1 deletion README.md
Original file line number Diff line number Diff line change
@@ -1 +1,71 @@
# biomechanical-analysis-framework
# biomechanical-analysis-framework

The biomechanical-analysis-framework projects contains a set of C++ libraries that can be used to estimate a human kinematics and dynamics quantities.

## Dependencies

The dependencies can be installed in two ways; the first one is via `conda` creating a conda environment running the following steps.

* Clone the repo:

```
git clone https://github.com/ami-iit/biomechanical-analysis-framework
```
* Create the conda environment from file
```
cd biomechanical-analysis-framework
conda env create -n <conda-environment-name> --file ci_env.yml
```
The second option is to install the dependencies via [robotology-superbuild](https://github.com/robotology/robotology-superbuild/blob/master/README.md), making sure to enable the following CMake options:
- `ROBOTOLOGY_ENABLE_DYNAMICS`;
- `ROBOTOLOGY_ENABLE_DYNAMICS_FULL_DEPS`;
- `ROBOTOLOGY_ENABLE_ICUB_HEAD`;

## Install

Clone the repo (if not already done to install the dependencies) and create a `build` directory

```
git clone https://github.com/ami-iit/biomechanical-analysis-framework
cd biomechanical-analysis-framework
mkdir build
cd build
```

### Linux/macOS

From the build directory, configure the CMake project and install it in the `build/install` directory:

```bash

cmake -DCMAKE_BUILD_TYPE="Release" -DCMAKE_INSTALL_PREFIX="./install" ..
make
make install
```

Once the installation is completed, append the following lines to your `.bashrc`:
```bash
BAF_INSTALL_DIR=<directory-where-you-downloaded-component_ifeel>element_wearable_sw/build/install
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:${BAF_INSTALL_DIR}/lib
export CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:${BAF_INSTALL_DIR}
```

>:warning: If on macOS, replace LD_LIBRARY_PATH with DYLD_LIBRARY_PATH in your `.bash_profile`

### Windows

From the build directory, configure the CMake project and install it in the `build/install` directory as it follows:

```bash
cmake -G"Visual Studio 17 2022" ..
cmake -DCMAKE_BUILD_TYPE="Release" -DCMAKE_INSTALL_PREFIX="./install" .
cmake --build . --config Release --target INSTALL
```

>:warning: Change Visual Studio 17 2022 according to your installed version.
Once the installation is completed, update the following environment variables:

- `PATH`
Add `<directory-where-you-downloaded-BAF>/build/install/bin`
10 changes: 9 additions & 1 deletion src/ID/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,8 +1,16 @@
include(FetchContent)

FetchContent_Declare(
ResolveRoboticsURICpp
GIT_REPOSITORY https://github.com/ami-iit/resolve-robotics-uri-cpp
GIT_TAG v0.0.2
)
FetchContent_MakeAvailable(ResolveRoboticsURICpp)

add_biomechanical_analysis_library(
NAME ID
PUBLIC_HEADERS include/BiomechanicalAnalysis/ID/InverseDynamics.h
SOURCES src/InverseDynamics.cpp
PUBLIC_LINK_LIBRARIES iDynTree::idyntree-estimation iDynTree::idyntree-high-level BipedalLocomotion::ParametersHandler
PRIVATE_LINK_LIBRARIES BiomechanicalAnalysis::Logging
PRIVATE_LINK_LIBRARIES BiomechanicalAnalysis::Logging ResolveRoboticsURICpp::ResolveRoboticsURICpp
SUBDIRECTORIES tests)
2 changes: 2 additions & 0 deletions src/ID/include/BiomechanicalAnalysis/ID/InverseDynamics.h
Original file line number Diff line number Diff line change
Expand Up @@ -163,6 +163,8 @@ class HumanID
*/
iDynTree::VectorDynSize getJointTorques();

void getJointTorques(Eigen::Ref<Eigen::VectorXd> jointTorques);

/**
* @brief Function to get the list of the joints
* @return vector of joint names
Expand Down
34 changes: 31 additions & 3 deletions src/ID/src/InverseDynamics.cpp
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
#include <BiomechanicalAnalysis/ID/InverseDynamics.h>
#include <BiomechanicalAnalysis/Logging/Logger.h>
#include <ResolveRoboticsURICpp.h>
#include <iDynTree/EigenHelpers.h>
#include <iDynTree/ModelLoader.h>

using namespace BiomechanicalAnalysis::ID;
Expand Down Expand Up @@ -33,8 +35,16 @@ bool HumanID::initialize(std::weak_ptr<const BipedalLocomotion::ParametersHandle

// Check if the model path is provided and load the model if present, otherwise use the kinDyn object
iDynTree::ModelLoader loader;
if (ptr->getParameter("modelPath", m_modelPath))
std::string urdfModel;
if (ptr->getParameter("urdfModel", urdfModel))
{
std::optional<std::string> urdfOpt = ResolveRoboticsURICpp::resolveRoboticsURI(urdfModel);
if (!urdfOpt.has_value())
{
BiomechanicalAnalysis::log()->error("Cannot resolve the URDF file");
return false;
}
m_modelPath = urdfOpt.value();
std::vector<std::string> jointsList;

// Retrieve the joints list if available
Expand Down Expand Up @@ -72,8 +82,8 @@ bool HumanID::initialize(std::weak_ptr<const BipedalLocomotion::ParametersHandle
m_useFullModel = true;
} else
{
// Warning if 'modelPath' parameter couldn't be retrieved, use the default model
BiomechanicalAnalysis::log()->warn("{} Error getting the modelPath parameter, using the default model.", logPrefix);
// Warning if 'urdfModel' parameter couldn't be retrieved, use the default model
BiomechanicalAnalysis::log()->warn("{} Error getting the 'urdfModel' parameter, using the default model.", logPrefix);

// Set m_useFullModel to false and m_kinDynFullModel to m_kinDyn
m_useFullModel = false;
Expand Down Expand Up @@ -185,6 +195,13 @@ bool HumanID::updateExtWrenchesMeasurements(const std::unordered_map<std::string

// Compute the wrench in the output frame's transformed coordinate system
m_wrenchSources[i].wrench = m_wrenchSources[i].outputFrameTransform * wrenches.at(m_wrenchSources[i].outputFrame);
iDynTree::Wrench wrenchMeas = m_wrenchSources[i].outputFrameTransform * wrenches.at(m_wrenchSources[i].outputFrame);
iDynTree::LinkIndex linkIndex = m_extWrenchesEstimator.berdyHelper.model().getLinkIndex(m_wrenchSources[i].outputFrame);
if (linkIndex == iDynTree::LINK_INVALID_INDEX)
{
BiomechanicalAnalysis::log()->error("{} Link {} not found.", logPrefix, m_wrenchSources[i].outputFrame);
return false;
}

// Determine the range of indices in the measurement vector for this sensor
iDynTree::IndexRange sensorRange
Expand Down Expand Up @@ -315,6 +332,17 @@ iDynTree::VectorDynSize HumanID::getJointTorques()
return m_jointTorquesHelper.estimatedJointTorques;
}

void HumanID::getJointTorques(Eigen::Ref<Eigen::VectorXd> jointTorques)
{
if (jointTorques.size() != m_kinDynFullModel->model().getNrOfDOFs())
{
BiomechanicalAnalysis::log()->error("[HumanID::getJointTorques] The size of the input vector "
"is different from the number of DOFs of the model.");
return;
}
jointTorques = iDynTree::toEigen(m_jointTorquesHelper.estimatedJointTorques);
}

std::vector<std::string> HumanID::getJointsList()
{
std::vector<std::string> jointNames;
Expand Down
11 changes: 9 additions & 2 deletions src/IK/src/InverseKinematics.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,13 @@ using namespace BipedalLocomotion::ContinuousDynamicalSystem;
using namespace BipedalLocomotion::Conversions;
using namespace std::chrono_literals;

constexpr size_t WRENCH_FORCE_X = 0;
constexpr size_t WRENCH_FORCE_Y = 1;
constexpr size_t WRENCH_FORCE_Z = 2;
constexpr size_t WRENCH_TORQUE_X = 3;
constexpr size_t WRENCH_TORQUE_Y = 4;
constexpr size_t WRENCH_TORQUE_Z = 5;

bool HumanIK::initialize(std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler,
std::shared_ptr<iDynTree::KinDynComputations> kinDyn)
{
Expand Down Expand Up @@ -203,7 +210,7 @@ bool HumanIK::updateFloorContactTask(const int node, const double verticalForce)
// check if the node number is valid
if (m_FloorContactTasks.find(node) == m_FloorContactTasks.end())
{
BiomechanicalAnalysis::log()->error("[HumanIK::setNodeSetPoint] Invalid node number.");
BiomechanicalAnalysis::log()->error("[HumanIK::updateFloorContactTask] Invalid node number.");
return false;
}

Expand Down Expand Up @@ -284,7 +291,7 @@ bool HumanIK::updateFloorContactTasks(const std::unordered_map<int, Eigen::Matri
{
for (const auto& [node, data] : wrenchMap)
{
if (!updateFloorContactTask(node, data(5)))
if (!updateFloorContactTask(node, data(WRENCH_FORCE_Z)))
{
BiomechanicalAnalysis::log()->error("[HumanIK::updateFloorContactTasks] Error in updating "
"the floor contact task of node {}",
Expand Down

0 comments on commit d029d50

Please sign in to comment.