Test iCub Arm manipulability

!icub-arm-manipulability.md

This gist contains code snippets to support the analysis done in robotology/community#559.

Build the project

cmake -S . -B build
cmake --build build

Execute the project

manipulability

Display results in MATLAB

man = importdata('manipulability');
histogram(man, 'Normalization', 'probability');
xlabel('manipulability');
ylabel('probability');

CMakeLists.txt

################################################################################
#                                                                              #
# Copyright (C) 2021 Fondazione Istitito Italiano di Tecnologia (IIT)          #
# All Rights Reserved.                                                         #
#                                                                              #
################################################################################

cmake_minimum_required(VERSION 3.16)
project(manipulability)

find_package(YARP REQUIRED)
find_package(ICUB REQUIRED)

add_executable(${PROJECT_NAME})
target_sources(${PROJECT_NAME} PRIVATE manipulability.cpp)
target_link_libraries(${PROJECT_NAME} PRIVATE YARP::YARP_sig YARP::YARP_math ICUB::iKin)
target_compile_definitions(${PROJECT_NAME} PRIVATE _USE_MATH_DEFINES)

manipulability.cpp

/******************************************************************************
 *                                                                            *
 * Copyright (C) 2021 Fondazione Istituto Italiano di Tecnologia (IIT)        *
 * All Rights Reserved.                                                       *
 *                                                                            *
 ******************************************************************************/

#include <cstdlib>
#include <iostream>
#include <fstream>
#include <cmath>
#include <yarp/sig/Matrix.h>
#include <yarp/math/Math.h>
#include <iCub/iKin/iKinFwd.h>

/*********************************************************************************/
void manipulability(std::ofstream& fout, iCub::iKin::iKinChain& chain,
                    const unsigned int j0, const unsigned int N) {
  const auto min = chain(j0).getMin();
  const auto max = chain(j0).getMax();
  for (auto j = min; j <= max; j += (max - min) / N) {
    chain(j0).setAng(j);
    const auto j1 = j0 + 1;
    if (j1 < chain.getDOF()) {
      manipulability(fout, chain, j1, N);
    }
    const auto J = chain.GeoJacobian();
    std::cout << ((180. / M_PI) * chain.getAng()).toString(3, 3) << std::endl;
    fout << std::sqrt(yarp::math::det(J * J.transposed())) << std::endl;
  }
}


/*********************************************************************************/
int main() {
  iCub::iKin::iCubArm arm("left_v2");
  arm.releaseLink(0);
  arm.releaseLink(1);
  arm.releaseLink(2);

  std::ofstream fout("manipulability");
  manipulability(fout, *arm.asChain(), 3, 6);
  fout.close();
  
  return EXIT_SUCCESS;
}