From e8fc8aa41e82815af365e30934d9c65becdb28a8 Mon Sep 17 00:00:00 2001 From: Ines Date: Wed, 13 Jan 2021 18:24:31 +0100 Subject: [PATCH 01/51] Joint trajectory player added --- utilities/CMakeLists.txt | 2 + .../joint-trajectory-player/CMakeLists.txt | 21 ++ .../jointTrajectoryPlayerOptions.ini | 19 + .../JointTrajectoryPlayer/Module.h | 95 +++++ .../blf-joint-trajectory-player-script.sh | 20 ++ ...lf_joint-trajectory-player_plot_dataset.py | 85 +++++ .../joint-trajectory-player/src/Main.cpp | 36 ++ .../joint-trajectory-player/src/Module.cpp | 336 ++++++++++++++++++ 8 files changed, 614 insertions(+) create mode 100644 utilities/joint-trajectory-player/CMakeLists.txt create mode 100644 utilities/joint-trajectory-player/config/robots/iCubGazeboV3/jointTrajectoryPlayerOptions.ini create mode 100644 utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h create mode 100755 utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh create mode 100644 utilities/joint-trajectory-player/script/blf_joint-trajectory-player_plot_dataset.py create mode 100644 utilities/joint-trajectory-player/src/Main.cpp create mode 100644 utilities/joint-trajectory-player/src/Module.cpp diff --git a/utilities/CMakeLists.txt b/utilities/CMakeLists.txt index 4153f71bab..f616c8fcdd 100644 --- a/utilities/CMakeLists.txt +++ b/utilities/CMakeLists.txt @@ -4,3 +4,5 @@ add_subdirectory(joint-position-tracking) + +add_subdirectory(joint-trajectory-player) diff --git a/utilities/joint-trajectory-player/CMakeLists.txt b/utilities/joint-trajectory-player/CMakeLists.txt new file mode 100644 index 0000000000..76e7778e69 --- /dev/null +++ b/utilities/joint-trajectory-player/CMakeLists.txt @@ -0,0 +1,21 @@ +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. +# Authors: Ines Sorrentino + +# TODO +#if(FRAMEWORK_COMPILE_JointTrajectoryPlayerApplication) + + add_bipedal_locomotion_application( + NAME joint-trajectory-player + SOURCES src/Main.cpp src/Module.cpp + HEADERS include/BipedalLocomotion/JointTrajectoryPlayer/Module.h + LINK_LIBRARIES YARP::YARP_dev BipedalLocomotion::Planners BipedalLocomotion::ParametersHandlerYarpImplementation BipedalLocomotion::RobotInterfaceYarpImplementation + ) + + install_ini_files(${CMAKE_CURRENT_SOURCE_DIR}/config) + + install(FILES script/blf-joint-trajectory-player-script.sh + PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ + DESTINATION "${CMAKE_INSTALL_BINDIR}") + +#endif() diff --git a/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/jointTrajectoryPlayerOptions.ini b/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/jointTrajectoryPlayerOptions.ini new file mode 100644 index 0000000000..a33e26ce3d --- /dev/null +++ b/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/jointTrajectoryPlayerOptions.ini @@ -0,0 +1,19 @@ +[GENERAL] +name joint-trajectory-player +sampling_time 0.01 +trajectory_file generatedtrajectory.txt + +[ROBOT_INTERFACE] +robot_name icubSim +joints_list ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +remote_control_boards ("right_leg") + +[ROBOT_CONTROL] +positioning_duration 3.0 +positioning_tolerance 0.05 +position_direct_max_admissible_error 0.1 + +[SENSOR_BRIDGE] +check_for_nan false +stream_joint_states true + diff --git a/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h b/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h new file mode 100644 index 0000000000..fdc7b67dee --- /dev/null +++ b/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h @@ -0,0 +1,95 @@ +/** + * @file Module.h + * @authors Giulio Romualdi, Ines Sorrentino + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BIPEDAL_LOCOMOTION_UTILITIES_JOINT_TRAJECTORY_PLAYER_MODULE_H +#define BIPEDAL_LOCOMOTION_UTILITIES_JOINT_TRAJECTORY_PLAYER_MODULE_H + +// std +#include +#include +#include + +// YARP +#include + +#include +#include +#include +#include + +namespace BipedalLocomotion +{ +namespace JointTrajectoryPlayer +{ + +class Module : public yarp::os::RFModule +{ + double m_dT; /**< RFModule period. */ + std::string m_robot; /**< Robot name. */ + + Eigen::VectorXd m_currentJointPos; + + std::shared_ptr m_robotDevice; + + BipedalLocomotion::RobotInterface::YarpRobotControl m_robotControl; + BipedalLocomotion::RobotInterface::YarpSensorBridge m_sensorBridge; + + int m_numOfJoints; /**< Number of joints to control. */ + + std::vector m_setPoints; + std::vector::const_iterator m_currentSetPoint; + + BipedalLocomotion::Planners::QuinticSpline m_spline; + std::vector m_timeKnots; + std::vector m_trajectoryKnots; + + double m_initTrajectoryTime; + + bool generateNewTrajectory(); + + bool createPolydriver( + std::shared_ptr handler); + + bool initializeRobotControl( + std::shared_ptr handler); + + bool instantiateSensorBridge( + std::shared_ptr handler); + + std::vector m_logJointPos; + std::vector m_logDesiredJointPos; + +public: + /** + * Get the period of the RFModule. + * @return the period of the module. + */ + double getPeriod() override; + + /** + * Main function of the RFModule. + * @return true in case of success and false otherwise. + */ + bool updateModule() override; + + /** + * Configure the RFModule. + * @param rf is the reference to a resource finder object + * @return true in case of success and false otherwise. + */ + bool configure(yarp::os::ResourceFinder& rf) override; + + /** + * Close the RFModule. + * @return true in case of success and false otherwise. + */ + bool close() override; +}; +} // namespace JointTrajectoryPlayer +} // namespace BipedalLocomotion + +#endif // BIPEDAL_LOCOMOTION_UTILITIES_JOINT_TRAJECTORY_PLAYER_MODULE_H diff --git a/utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh b/utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh new file mode 100755 index 0000000000..5291a6e141 --- /dev/null +++ b/utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh @@ -0,0 +1,20 @@ +#!/usr/bin/env bash + +scriptDirectory="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" +pythonScriptRootPath=`realpath $scriptDirectory/../share/BipedalLocomotionFramework/python` + +echo $pythonScriptRootPath + +echo "Welcome to the JointPositionTrackingTest. I will first move the robot and then I will store the plot the data in a file called image.png" + +echo "I'm going to move the robot. Watch out!" + +blf-joint-position-tracking --from blf-joint-position-tracking-options.ini + +echo "The trajectory is terminated." + +echo "Plot data" + +python3 "$pythonScriptRootPath"/blf_joint_position_tracking_plot_dataset.py --dataset `ls -t Dataset* | head -1` + +echo "Done. Thanks for using the script." diff --git a/utilities/joint-trajectory-player/script/blf_joint-trajectory-player_plot_dataset.py b/utilities/joint-trajectory-player/script/blf_joint-trajectory-player_plot_dataset.py new file mode 100644 index 0000000000..cc86846dec --- /dev/null +++ b/utilities/joint-trajectory-player/script/blf_joint-trajectory-player_plot_dataset.py @@ -0,0 +1,85 @@ +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. +# Authors: Giulio Romualdi and Nicola Piga + +#!/usr/bin/env python3 + +import numpy as np +import argparse +import csv +import matplotlib +import matplotlib.pyplot as plt + +from matplotlib import rc +rc('font',**{'family':'sans-serif','sans-serif':['Helvetica']}) +rc('text', usetex=True) + +def read_data(file_name): + data = [] + + with open(file_name, newline='') as csv_data: + for row in csv_data: + try: + data.append([float(num_string.rstrip()) for num_string in row.split(sep = ", ") if num_string != '']) + except: + pass + + return np.array(data) + +def times(signal, i0 = 0): + times = signal.copy() + counter = i0 * (1.0 / 100.0) + dt = 1.0 / 100.0 + for i in range(len(times)): + times[i] = counter + counter = counter + dt + + return times + +def plot_data(axes, x_data, y_data, color, linewidth = 1.0): + + col = [] + if color == "blue": + col = plt.cm.Blues(0.8) + elif color == "red": + col = plt.cm.Reds(0.8) + + return axes.plot(x_data, y_data, color = col, linewidth = linewidth) + +def plot_and_save(data): + fig, ax = plt.subplots(1) + + channel_0 = data[:, 0] + channel_1 = data[:, 1] + + plot_0, = plot_data(ax, times(channel_0), channel_0, "red") + plot_1, = plot_data(ax, times(channel_1), channel_1, "blue") + + legend = [plot_0, plot_1] + legend = fig.legend(legend, labels = ["$\mathrm{Reference}$", "$\mathrm{Actual}$"], ncol = 2, loc = "upper center", frameon=False) + + for line in legend.get_lines(): + line.set_linewidth(2.0) + + label_font_size = 10 + ax.grid() + ax.set_xlabel('time [s]', fontsize = label_font_size) + ax.set_ylabel('position [rad]', fontsize = label_font_size) + ax.set_title('joint position', fontsize = label_font_size) + + figure = plt.gcf() + plt.savefig("./figure.png", bbox_inches='tight', dpi = 150) + + # plt.show() + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='plot_dataset.py is a python script useful to plot a dataset generated by JointControlModule and save it.') + parser.add_argument('--dataset', type = str, required = True, help='Name of the dataset') + args = parser.parse_args() + + data = read_data(args.dataset) + + plot_and_save(data) + + diff --git a/utilities/joint-trajectory-player/src/Main.cpp b/utilities/joint-trajectory-player/src/Main.cpp new file mode 100644 index 0000000000..ac6402fba4 --- /dev/null +++ b/utilities/joint-trajectory-player/src/Main.cpp @@ -0,0 +1,36 @@ +/** + * @file Main.cpp + * @authors Giulio Romualdi, Ines Sorrentino + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +// YARP +#include +#include +#include + +#include + +int main(int argc, char* argv[]) +{ + // initialize yarp network + yarp::os::Network yarp; + if (!yarp.checkNetwork()) + { + yError() << "[main] Unable to find YARP network"; + return EXIT_FAILURE; + } + + // prepare and configure the resource finder + yarp::os::ResourceFinder& rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + + rf.setDefaultConfigFile("jointTrajectoryPlayerOptions.ini"); + + rf.configure(argc, argv); + + // create the module + BipedalLocomotion::JointTrajectoryPlayer::Module module; + + return module.runModule(rf); +} diff --git a/utilities/joint-trajectory-player/src/Module.cpp b/utilities/joint-trajectory-player/src/Module.cpp new file mode 100644 index 0000000000..b005412b90 --- /dev/null +++ b/utilities/joint-trajectory-player/src/Module.cpp @@ -0,0 +1,336 @@ +/** + * @file Module.cpp + * @authors Giulio Romualdi, Ines Sorrentino + * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include +#include + +#include +#include +#include +#include + +#include + +#include + +#include + +using Vector1d = Eigen::Matrix; + +using namespace BipedalLocomotion; +using namespace BipedalLocomotion::JointTrajectoryPlayer; + +double Module::getPeriod() +{ + return m_dT; +} + +bool Module::createPolydriver(std::shared_ptr handler) +{ + auto ptr = handler->getGroup("ROBOT_INTERFACE").lock(); + if (ptr == nullptr) + { + std::cerr << "[Module::createPolydriver] Robot interface options is empty." << std::endl; + return false; + } + ptr->setParameter("local_name", this->getName()); + m_robotDevice = RobotInterface::constructYarpRobotDevice(ptr); + if (m_robotDevice == nullptr) + { + std::cerr << "[Module::createPolydriver] the robot polydriver has not been constructed." + << std::endl; + return false; + } + + + // check the number of controlled joints + m_numOfJoints = 0; + yarp::dev::IEncoders* axis; + m_robotDevice->view(axis); + if (axis != nullptr) + axis->getAxes(&m_numOfJoints); + + /* + if (controlBoardDOFs != 1) + { + std::cerr << "[Module::createPolydriver] The current implementation can be used to control " + "only one joint. Please be sure that the size of the joint_list and " + "remote_control_boards is equal to one." + << std::endl; + return false; + } + */ + + return true; +} + +bool Module::initializeRobotControl(std::shared_ptr handler) +{ + if (!m_robotControl.initialize(handler->getGroup("ROBOT_CONTROL"))) + { + std::cerr << "[Module::initializeRobotControl] Unable to initialize the " + "control board" + << std::endl; + return false; + } + if (!m_robotControl.setDriver(m_robotDevice)) + { + std::cerr << "[Module::initializeRobotControl] Unable to initialize the " + "control board" + << std::endl; + return false; + } + if (!m_robotControl.initialize(handler->getGroup("PID"))) + { + std::cerr << "[Module::initializeRobotControl] Unable to initialize the " + "control board" + << std::endl; + return false; + } + + return true; +} + +bool Module::instantiateSensorBridge(std::shared_ptr handler) +{ + if (!m_sensorBridge.initialize(handler->getGroup("SENSOR_BRIDGE"))) + { + std::cerr << "[Module::initializeSensorBridge] Unable to initialize the sensor bridge" + << std::endl; + return false; + } + + yarp::dev::PolyDriverList list; + list.push(m_robotDevice.get(), "Remote control board"); + if (!m_sensorBridge.setDriversList(list)) + { + std::cerr << "[Module::initializeSensorBridge] Unable to set the driver list" << std::endl; + return false; + } + + return true; +} + +std::pair> readStateFromFile(const std::string& filename, const std::size_t num_fields) +{ + std::deque data; + + std::ifstream istrm(filename); + + if (!istrm.is_open()) + { + std::cout << "Failed to open " << filename << '\n'; + return std::make_pair(false, data); + } + else + { + std::vector istrm_strings; + std::string line; + while (std::getline(istrm, line)) + { + istrm_strings.push_back(line); + } + + iDynTree::VectorDynSize vector; + vector.resize(num_fields); + std::size_t found_lines = 0; + for (auto line : istrm_strings) + { + std::size_t found_fields = 0; + std::string number_str; + std::istringstream iss(line); + + while (iss >> number_str) + { + vector(found_fields) = std::stod(number_str); + found_fields++; + } + if (num_fields != found_fields) + { + std::cout << "Malformed input file " << filename << '\n'; + + return std::make_pair(false, data); + } + data.push_back(vector); + found_lines++; + } + + return std::make_pair(true, data); + } +} + +bool Module::configure(yarp::os::ResourceFinder& rf) +{ + m_currentJointPos.resize(1); + + auto parametersHandler = std::make_shared(rf); + + std::string name; + if(!parametersHandler->getParameter("name", name)) + return false; + this->setName(name.c_str()); + + if(!parametersHandler->getParameter("sampling_time", m_dT)) + return false; + + /* + double maxValue = 0; + if(!parametersHandler->getParameter("max_angle_deg", maxValue)) + return false; + + maxValue *= M_PI / 180; + + double minValue = 0; + if(!parametersHandler->getParameter("min_angle_deg", minValue)) + return false; + + minValue *= M_PI / 180; + + double trajectoryDuration = 5; + if(!parametersHandler->getParameter("trajectory_duration", trajectoryDuration)) + return false; + */ + + std::string trajectoryFile; + if(!parametersHandler->getParameter("trajectory_file", trajectoryFile)) + return false; + + this->createPolydriver(parametersHandler); + this->initializeRobotControl(parametersHandler); + this->instantiateSensorBridge(parametersHandler); + + auto data = readStateFromFile(trajectoryFile, m_numOfJoints); + if(!data.first) + { + return false; + } + + /* + m_setPoints.push_back((maxValue + minValue) / 2); + m_setPoints.push_back(maxValue); + m_setPoints.push_back(minValue); + m_setPoints.push_back((maxValue + minValue) / 2); + */ + + m_spline.setAdvanceTimeStep(m_dT); + m_spline.setInitialConditions(Vector1d::Zero(), Vector1d::Zero()); + m_spline.setFinalConditions(Vector1d::Zero(), Vector1d::Zero()); + + m_timeKnots.clear(); + m_timeKnots.push_back(0); + //m_timeKnots.push_back(trajectoryDuration); + + if (!m_sensorBridge.advance()) + { + std::cerr << "[Module::updateModule] Unable to read the sensor." << std::endl; + return false; + } + m_sensorBridge.getJointPositions(m_currentJointPos); + + // the trajectory will bring the robot in the initial configuration + m_setPoints.push_back(m_currentJointPos[0]); + m_currentSetPoint = m_setPoints.begin(); + + m_trajectoryKnots.push_back(m_currentJointPos); + m_trajectoryKnots.push_back(Vector1d::Constant(*m_currentSetPoint)); + + m_spline.setKnots(m_trajectoryKnots, m_timeKnots); + + m_initTrajectoryTime = yarp::os::Time::now(); + + std::cout << "[Module::configure] Starting the experiment." << std::endl; + + return true; +} + +bool Module::generateNewTrajectory() +{ + double initTrajectory = *m_currentSetPoint; + + // the trajectory is ended + if (std::next(m_currentSetPoint, 1) == m_setPoints.end()) + return false; + + std::advance(m_currentSetPoint, 1); + double endTrajectory = *m_currentSetPoint; + + m_trajectoryKnots[0] = Vector1d::Constant(initTrajectory); + m_trajectoryKnots[1] = Vector1d::Constant(endTrajectory); + + m_spline.setKnots(m_trajectoryKnots, m_timeKnots); + + m_initTrajectoryTime = yarp::os::Time::now(); + + return true; +} + +bool Module::updateModule() +{ + if (!m_sensorBridge.advance()) + { + std::cerr << "[Module::updateModule] Unable to read the sensor." << std::endl; + return false; + } + + m_sensorBridge.getJointPositions(m_currentJointPos); + + // set the reference + m_robotControl.setReferences(m_spline.get().position, + RobotInterface::IRobotControl::ControlMode::PositionDirect); + + m_logJointPos.push_back(m_currentJointPos[0]); + m_logDesiredJointPos.push_back(m_spline.get().position[0]); + + // advance the spline + m_spline.advance(); + + const double now = yarp::os::Time::now(); + if (now - m_initTrajectoryTime > m_timeKnots.back() + 2) + { + std::cout << "[Module::updateModule] Generate a new trajectory." << std::endl; + + if (!generateNewTrajectory()) + { + std::cerr << "[Module::updateModule] Experiment finished." << std::endl; + return false; + } + } + + return true; +} + +bool Module::close() +{ + std::cout << "[Module::close] I'm storing the dataset." << std::endl; + + // set the file name + std::time_t t = std::time(nullptr); + std::tm tm = *std::localtime(&t); + + std::ofstream stream; /**< std stream. */ + std::stringstream fileName; + + fileName << "Dataset_" << m_robotControl.getJointList().front() << "_" + << std::put_time(&tm, "%Y_%m_%d_%H_%M_%S") << ".txt"; + stream.open(fileName.str().c_str()); + + const auto minSize = std::min(m_logJointPos.size(), m_logDesiredJointPos.size()); + + stream << "desired_joint, measured_joint" << std::endl; + for (int i = 0; i < minSize; i++) + stream << m_logDesiredJointPos[i] << ", " << m_logJointPos[i] << std::endl; + + stream.close(); + + std::cout << "[Module::close] Dataset stored. Closing." << std::endl; + + // switch back in position control + m_robotControl.setReferences(m_spline.get().position, + RobotInterface::IRobotControl::ControlMode::Position); + + return true; +} From 263c0562bc29355126405a1f3c11182302bcf891 Mon Sep 17 00:00:00 2001 From: Ines Date: Thu, 14 Jan 2021 09:30:52 +0100 Subject: [PATCH 02/51] first version of joint-trajectory-player --- .../blf-joint-trajectory-player-options.ini | 20 +++ .../jointTrajectoryPlayerOptions.ini | 2 +- .../JointTrajectoryPlayer/Module.h | 13 ++ .../blf-joint-trajectory-player-script.sh | 7 +- .../joint-trajectory-player/src/Module.cpp | 116 ++++++++---------- 5 files changed, 86 insertions(+), 72 deletions(-) create mode 100644 utilities/joint-trajectory-player/config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini diff --git a/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini b/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini new file mode 100644 index 0000000000..c83749620a --- /dev/null +++ b/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini @@ -0,0 +1,20 @@ +[GENERAL] +name joint-trajectory-player +sampling_time 0.01 +trajectory_file generatedtrajectory.txt +initial_trajectory_duration 5 + +[ROBOT_INTERFACE] +robot_name icubSim +joints_list ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +remote_control_boards ("right_leg") + +[ROBOT_CONTROL] +positioning_duration 3.0 +positioning_tolerance 0.05 +position_direct_max_admissible_error 0.1 + +[SENSOR_BRIDGE] +check_for_nan false +stream_joint_states true + diff --git a/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/jointTrajectoryPlayerOptions.ini b/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/jointTrajectoryPlayerOptions.ini index a33e26ce3d..b2b4acdcdc 100644 --- a/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/jointTrajectoryPlayerOptions.ini +++ b/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/jointTrajectoryPlayerOptions.ini @@ -1,7 +1,7 @@ -[GENERAL] name joint-trajectory-player sampling_time 0.01 trajectory_file generatedtrajectory.txt +initial_trajectory_duration 5 [ROBOT_INTERFACE] robot_name icubSim diff --git a/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h b/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h index fdc7b67dee..c3c1c718ee 100644 --- a/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h +++ b/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h @@ -12,6 +12,12 @@ #include #include #include +#include + + +// iDynTree +#include + // YARP #include @@ -48,6 +54,7 @@ class Module : public yarp::os::RFModule std::vector m_trajectoryKnots; double m_initTrajectoryTime; + std::deque m_qDesired; /**< Vector containing the results of the IK alg */ bool generateNewTrajectory(); @@ -63,6 +70,12 @@ class Module : public yarp::os::RFModule std::vector m_logJointPos; std::vector m_logDesiredJointPos; + /** + * Advance the reference signal. + * @return true in case of success and false otherwise. + */ + bool advanceReferenceSignals(); + public: /** * Get the period of the RFModule. diff --git a/utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh b/utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh index 5291a6e141..b69b1ec6c6 100755 --- a/utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh +++ b/utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh @@ -9,12 +9,7 @@ echo "Welcome to the JointPositionTrackingTest. I will first move the robot and echo "I'm going to move the robot. Watch out!" -blf-joint-position-tracking --from blf-joint-position-tracking-options.ini +blf-joint-trajectory-player --from blf-joint-trajectory-player-options.ini echo "The trajectory is terminated." -echo "Plot data" - -python3 "$pythonScriptRootPath"/blf_joint_position_tracking_plot_dataset.py --dataset `ls -t Dataset* | head -1` - -echo "Done. Thanks for using the script." diff --git a/utilities/joint-trajectory-player/src/Module.cpp b/utilities/joint-trajectory-player/src/Module.cpp index b005412b90..85b98d1c36 100644 --- a/utilities/joint-trajectory-player/src/Module.cpp +++ b/utilities/joint-trajectory-player/src/Module.cpp @@ -19,6 +19,8 @@ #include +#include + using Vector1d = Eigen::Matrix; using namespace BipedalLocomotion; @@ -189,11 +191,12 @@ bool Module::configure(yarp::os::ResourceFinder& rf) return false; minValue *= M_PI / 180; + */ - double trajectoryDuration = 5; - if(!parametersHandler->getParameter("trajectory_duration", trajectoryDuration)) + double initialTrajDuration = 5; + if(!parametersHandler->getParameter("initial_trajectory_duration", initialTrajDuration)) return false; - */ + std::string trajectoryFile; if(!parametersHandler->getParameter("trajectory_file", trajectoryFile)) @@ -203,11 +206,14 @@ bool Module::configure(yarp::os::ResourceFinder& rf) this->initializeRobotControl(parametersHandler); this->instantiateSensorBridge(parametersHandler); + m_qDesired.clear(); auto data = readStateFromFile(trajectoryFile, m_numOfJoints); if(!data.first) { return false; } + m_qDesired = data.second; + /* m_setPoints.push_back((maxValue + minValue) / 2); @@ -216,31 +222,31 @@ bool Module::configure(yarp::os::ResourceFinder& rf) m_setPoints.push_back((maxValue + minValue) / 2); */ - m_spline.setAdvanceTimeStep(m_dT); - m_spline.setInitialConditions(Vector1d::Zero(), Vector1d::Zero()); - m_spline.setFinalConditions(Vector1d::Zero(), Vector1d::Zero()); + //m_spline.setAdvanceTimeStep(m_dT); + //m_spline.setInitialConditions(Vector1d::Zero(), Vector1d::Zero()); + //m_spline.setFinalConditions(Vector1d::Zero(), Vector1d::Zero()); - m_timeKnots.clear(); - m_timeKnots.push_back(0); - //m_timeKnots.push_back(trajectoryDuration); + //m_timeKnots.clear(); + //m_timeKnots.push_back(0); + //m_timeKnots.push_back(initialTrajDuration); - if (!m_sensorBridge.advance()) - { - std::cerr << "[Module::updateModule] Unable to read the sensor." << std::endl; - return false; - } - m_sensorBridge.getJointPositions(m_currentJointPos); + //if (!m_sensorBridge.advance()) + //{ + // std::cerr << "[Module::updateModule] Unable to read the sensor." << std::endl; + // return false; + //} + //m_sensorBridge.getJointPositions(m_currentJointPos); // the trajectory will bring the robot in the initial configuration - m_setPoints.push_back(m_currentJointPos[0]); - m_currentSetPoint = m_setPoints.begin(); + //m_setPoints.push_back(m_currentJointPos[0]); + //m_currentSetPoint = m_setPoints.begin(); - m_trajectoryKnots.push_back(m_currentJointPos); - m_trajectoryKnots.push_back(Vector1d::Constant(*m_currentSetPoint)); + //m_trajectoryKnots.push_back(m_currentJointPos); + //m_trajectoryKnots.push_back(Vector1d::Constant(*m_currentSetPoint)); - m_spline.setKnots(m_trajectoryKnots, m_timeKnots); + //m_spline.setKnots(m_trajectoryKnots, m_timeKnots); - m_initTrajectoryTime = yarp::os::Time::now(); + //m_initTrajectoryTime = yarp::os::Time::now(); std::cout << "[Module::configure] Starting the experiment." << std::endl; @@ -268,6 +274,21 @@ bool Module::generateNewTrajectory() return true; } +bool Module::advanceReferenceSignals() +{ + // check if vector is not initialized + if(m_qDesired.empty()) + { + std::cerr << "[jointControlModule::advanceReferenceSignals] Cannot advance empty reference signals." << std::endl; + return false; + } + + m_qDesired.pop_front(); + m_qDesired.push_back(m_qDesired.back()); + + return true; +} + bool Module::updateModule() { if (!m_sensorBridge.advance()) @@ -276,60 +297,25 @@ bool Module::updateModule() return false; } - m_sensorBridge.getJointPositions(m_currentJointPos); - // set the reference - m_robotControl.setReferences(m_spline.get().position, - RobotInterface::IRobotControl::ControlMode::PositionDirect); - - m_logJointPos.push_back(m_currentJointPos[0]); - m_logDesiredJointPos.push_back(m_spline.get().position[0]); - - // advance the spline - m_spline.advance(); - - const double now = yarp::os::Time::now(); - if (now - m_initTrajectoryTime > m_timeKnots.back() + 2) + if(!m_robotControl.setReferences(iDynTree::toEigen(m_qDesired.front()), + RobotInterface::IRobotControl::ControlMode::PositionDirect)) { - std::cout << "[Module::updateModule] Generate a new trajectory." << std::endl; - - if (!generateNewTrajectory()) - { - std::cerr << "[Module::updateModule] Experiment finished." << std::endl; - return false; - } + std::cerr << "[Module::updateModule] Error while setting the reference position to iCub." << std::endl; + return false; } + advanceReferenceSignals(); + + m_sensorBridge.getJointPositions(m_currentJointPos); + return true; } bool Module::close() { - std::cout << "[Module::close] I'm storing the dataset." << std::endl; - - // set the file name - std::time_t t = std::time(nullptr); - std::tm tm = *std::localtime(&t); - - std::ofstream stream; /**< std stream. */ - std::stringstream fileName; - - fileName << "Dataset_" << m_robotControl.getJointList().front() << "_" - << std::put_time(&tm, "%Y_%m_%d_%H_%M_%S") << ".txt"; - stream.open(fileName.str().c_str()); - - const auto minSize = std::min(m_logJointPos.size(), m_logDesiredJointPos.size()); - - stream << "desired_joint, measured_joint" << std::endl; - for (int i = 0; i < minSize; i++) - stream << m_logDesiredJointPos[i] << ", " << m_logJointPos[i] << std::endl; - - stream.close(); - - std::cout << "[Module::close] Dataset stored. Closing." << std::endl; - // switch back in position control - m_robotControl.setReferences(m_spline.get().position, + m_robotControl.setReferences(m_currentJointPos, RobotInterface::IRobotControl::ControlMode::Position); return true; From 0e1e0f7ed66e03fd832ef4b6dec4131f0a2c48f0 Mon Sep 17 00:00:00 2001 From: Ines Date: Thu, 14 Jan 2021 10:23:22 +0100 Subject: [PATCH 03/51] ini file modified for joint-trajectory-player --- .../blf-joint-trajectory-player-options.ini | 3 +-- .../jointTrajectoryPlayerOptions.ini | 19 ------------------- .../blf-joint-trajectory-player-script.sh | 5 +---- .../joint-trajectory-player/src/Main.cpp | 2 +- 4 files changed, 3 insertions(+), 26 deletions(-) delete mode 100644 utilities/joint-trajectory-player/config/robots/iCubGazeboV3/jointTrajectoryPlayerOptions.ini diff --git a/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini b/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini index c83749620a..2189e97bec 100644 --- a/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini +++ b/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini @@ -1,8 +1,7 @@ -[GENERAL] name joint-trajectory-player sampling_time 0.01 trajectory_file generatedtrajectory.txt -initial_trajectory_duration 5 +initial_trajectory_duration 5.0 [ROBOT_INTERFACE] robot_name icubSim diff --git a/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/jointTrajectoryPlayerOptions.ini b/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/jointTrajectoryPlayerOptions.ini deleted file mode 100644 index b2b4acdcdc..0000000000 --- a/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/jointTrajectoryPlayerOptions.ini +++ /dev/null @@ -1,19 +0,0 @@ -name joint-trajectory-player -sampling_time 0.01 -trajectory_file generatedtrajectory.txt -initial_trajectory_duration 5 - -[ROBOT_INTERFACE] -robot_name icubSim -joints_list ("r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") -remote_control_boards ("right_leg") - -[ROBOT_CONTROL] -positioning_duration 3.0 -positioning_tolerance 0.05 -position_direct_max_admissible_error 0.1 - -[SENSOR_BRIDGE] -check_for_nan false -stream_joint_states true - diff --git a/utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh b/utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh index b69b1ec6c6..6555431156 100755 --- a/utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh +++ b/utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh @@ -1,11 +1,8 @@ #!/usr/bin/env bash scriptDirectory="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" -pythonScriptRootPath=`realpath $scriptDirectory/../share/BipedalLocomotionFramework/python` -echo $pythonScriptRootPath - -echo "Welcome to the JointPositionTrackingTest. I will first move the robot and then I will store the plot the data in a file called image.png" +echo "Welcome to the JointPositionTrackingTest." echo "I'm going to move the robot. Watch out!" diff --git a/utilities/joint-trajectory-player/src/Main.cpp b/utilities/joint-trajectory-player/src/Main.cpp index ac6402fba4..47ee56bb9b 100644 --- a/utilities/joint-trajectory-player/src/Main.cpp +++ b/utilities/joint-trajectory-player/src/Main.cpp @@ -25,7 +25,7 @@ int main(int argc, char* argv[]) // prepare and configure the resource finder yarp::os::ResourceFinder& rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); - rf.setDefaultConfigFile("jointTrajectoryPlayerOptions.ini"); + rf.setDefaultConfigFile("blf-joint-trajectory-player-options.ini"); rf.configure(argc, argv); From d87e791edca21b39a06cba67a99e70a315881137 Mon Sep 17 00:00:00 2001 From: Ines Date: Fri, 15 Jan 2021 12:45:09 +0100 Subject: [PATCH 04/51] Saving file with measured positions added. Check on the number of joints of the trajectory file added. Initial trajectory to go to the initial desired position added. Removed iDynTree vectors. Some problems fixed. --- .../joint-trajectory-player/CMakeLists.txt | 24 +-- .../JointTrajectoryPlayer/Module.h | 34 +-- .../joint-trajectory-player/src/Module.cpp | 203 ++++++++---------- 3 files changed, 107 insertions(+), 154 deletions(-) diff --git a/utilities/joint-trajectory-player/CMakeLists.txt b/utilities/joint-trajectory-player/CMakeLists.txt index 76e7778e69..a7d1fdc0b8 100644 --- a/utilities/joint-trajectory-player/CMakeLists.txt +++ b/utilities/joint-trajectory-player/CMakeLists.txt @@ -2,20 +2,16 @@ # GNU Lesser General Public License v2.1 or any later version. # Authors: Ines Sorrentino -# TODO -#if(FRAMEWORK_COMPILE_JointTrajectoryPlayerApplication) +add_bipedal_locomotion_application( + NAME joint-trajectory-player + SOURCES src/Main.cpp src/Module.cpp + HEADERS include/BipedalLocomotion/JointTrajectoryPlayer/Module.h + LINK_LIBRARIES YARP::YARP_dev BipedalLocomotion::Planners BipedalLocomotion::ParametersHandlerYarpImplementation BipedalLocomotion::RobotInterfaceYarpImplementation + ) - add_bipedal_locomotion_application( - NAME joint-trajectory-player - SOURCES src/Main.cpp src/Module.cpp - HEADERS include/BipedalLocomotion/JointTrajectoryPlayer/Module.h - LINK_LIBRARIES YARP::YARP_dev BipedalLocomotion::Planners BipedalLocomotion::ParametersHandlerYarpImplementation BipedalLocomotion::RobotInterfaceYarpImplementation - ) +install_ini_files(${CMAKE_CURRENT_SOURCE_DIR}/config) - install_ini_files(${CMAKE_CURRENT_SOURCE_DIR}/config) +install(FILES script/blf-joint-trajectory-player-script.sh + PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ + DESTINATION "${CMAKE_INSTALL_BINDIR}") - install(FILES script/blf-joint-trajectory-player-script.sh - PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ - DESTINATION "${CMAKE_INSTALL_BINDIR}") - -#endif() diff --git a/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h b/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h index c3c1c718ee..f87d09687d 100644 --- a/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h +++ b/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h @@ -1,6 +1,6 @@ /** * @file Module.h - * @authors Giulio Romualdi, Ines Sorrentino + * @authors Ines Sorrentino * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. */ @@ -9,15 +9,10 @@ #define BIPEDAL_LOCOMOTION_UTILITIES_JOINT_TRAJECTORY_PLAYER_MODULE_H // std +#include #include #include #include -#include - - -// iDynTree -#include - // YARP #include @@ -41,34 +36,23 @@ class Module : public yarp::os::RFModule std::shared_ptr m_robotDevice; - BipedalLocomotion::RobotInterface::YarpRobotControl m_robotControl; - BipedalLocomotion::RobotInterface::YarpSensorBridge m_sensorBridge; + RobotInterface::YarpRobotControl m_robotControl; + RobotInterface::YarpSensorBridge m_sensorBridge; int m_numOfJoints; /**< Number of joints to control. */ std::vector m_setPoints; std::vector::const_iterator m_currentSetPoint; - BipedalLocomotion::Planners::QuinticSpline m_spline; - std::vector m_timeKnots; - std::vector m_trajectoryKnots; - - double m_initTrajectoryTime; - std::deque m_qDesired; /**< Vector containing the results of the IK alg */ - - bool generateNewTrajectory(); + std::deque m_qDesired; /**< Vector containing the results of the IK alg */ - bool createPolydriver( - std::shared_ptr handler); + bool createPolydriver(std::shared_ptr handler); - bool initializeRobotControl( - std::shared_ptr handler); + bool initializeRobotControl(std::shared_ptr handler); - bool instantiateSensorBridge( - std::shared_ptr handler); + bool instantiateSensorBridge(std::shared_ptr handler); - std::vector m_logJointPos; - std::vector m_logDesiredJointPos; + std::vector m_logJointPos; /** * Advance the reference signal. diff --git a/utilities/joint-trajectory-player/src/Module.cpp b/utilities/joint-trajectory-player/src/Module.cpp index 85b98d1c36..8bde4aa2f8 100644 --- a/utilities/joint-trajectory-player/src/Module.cpp +++ b/utilities/joint-trajectory-player/src/Module.cpp @@ -9,9 +9,9 @@ #include #include +#include #include #include -#include #include @@ -19,8 +19,6 @@ #include -#include - using Vector1d = Eigen::Matrix; using namespace BipedalLocomotion; @@ -48,7 +46,6 @@ bool Module::createPolydriver(std::shared_ptrgetAxes(&m_numOfJoints); - /* - if (controlBoardDOFs != 1) - { - std::cerr << "[Module::createPolydriver] The current implementation can be used to control " - "only one joint. Please be sure that the size of the joint_list and " - "remote_control_boards is equal to one." - << std::endl; - return false; - } - */ - return true; } @@ -117,9 +103,10 @@ bool Module::instantiateSensorBridge(std::shared_ptr> readStateFromFile(const std::string& filename, const std::size_t num_fields) +std::pair> +readStateFromFile(const std::string& filename, const std::size_t num_fields) { - std::deque data; + std::deque data; std::ifstream istrm(filename); @@ -127,8 +114,7 @@ std::pair> readStateFromFile(const std { std::cout << "Failed to open " << filename << '\n'; return std::make_pair(false, data); - } - else + } else { std::vector istrm_strings; std::string line; @@ -137,7 +123,7 @@ std::pair> readStateFromFile(const std istrm_strings.push_back(line); } - iDynTree::VectorDynSize vector; + Eigen::VectorXd vector; vector.resize(num_fields); std::size_t found_lines = 0; for (auto line : istrm_strings) @@ -153,7 +139,10 @@ std::pair> readStateFromFile(const std } if (num_fields != found_fields) { - std::cout << "Malformed input file " << filename << '\n'; + std::cout << "[Module::readStateFromFile] Malformed input file " << filename + << std::endl; + std::cout << "[Module::readStateFromFile] Expected " << num_fields + << " columns, found " << found_fields << std::endl; return std::make_pair(false, data); } @@ -167,153 +156,137 @@ std::pair> readStateFromFile(const std bool Module::configure(yarp::os::ResourceFinder& rf) { - m_currentJointPos.resize(1); - auto parametersHandler = std::make_shared(rf); std::string name; - if(!parametersHandler->getParameter("name", name)) + if (!parametersHandler->getParameter("name", name)) return false; this->setName(name.c_str()); - if(!parametersHandler->getParameter("sampling_time", m_dT)) - return false; - - /* - double maxValue = 0; - if(!parametersHandler->getParameter("max_angle_deg", maxValue)) + if (!parametersHandler->getParameter("sampling_time", m_dT)) return false; - maxValue *= M_PI / 180; - - double minValue = 0; - if(!parametersHandler->getParameter("min_angle_deg", minValue)) - return false; - - minValue *= M_PI / 180; - */ - double initialTrajDuration = 5; - if(!parametersHandler->getParameter("initial_trajectory_duration", initialTrajDuration)) + if (!parametersHandler->getParameter("initial_trajectory_duration", initialTrajDuration)) return false; - std::string trajectoryFile; - if(!parametersHandler->getParameter("trajectory_file", trajectoryFile)) + if (!parametersHandler->getParameter("trajectory_file", trajectoryFile)) return false; this->createPolydriver(parametersHandler); this->initializeRobotControl(parametersHandler); this->instantiateSensorBridge(parametersHandler); + m_currentJointPos.resize(m_robotControl.getJointList().size()); + m_qDesired.clear(); auto data = readStateFromFile(trajectoryFile, m_numOfJoints); - if(!data.first) + if (!data.first) { return false; } m_qDesired = data.second; - - - /* - m_setPoints.push_back((maxValue + minValue) / 2); - m_setPoints.push_back(maxValue); - m_setPoints.push_back(minValue); - m_setPoints.push_back((maxValue + minValue) / 2); - */ - //m_spline.setAdvanceTimeStep(m_dT); - //m_spline.setInitialConditions(Vector1d::Zero(), Vector1d::Zero()); - //m_spline.setFinalConditions(Vector1d::Zero(), Vector1d::Zero()); - - //m_timeKnots.clear(); - //m_timeKnots.push_back(0); - //m_timeKnots.push_back(initialTrajDuration); - - //if (!m_sensorBridge.advance()) - //{ - // std::cerr << "[Module::updateModule] Unable to read the sensor." << std::endl; - // return false; - //} - //m_sensorBridge.getJointPositions(m_currentJointPos); - - // the trajectory will bring the robot in the initial configuration - //m_setPoints.push_back(m_currentJointPos[0]); - //m_currentSetPoint = m_setPoints.begin(); - - //m_trajectoryKnots.push_back(m_currentJointPos); - //m_trajectoryKnots.push_back(Vector1d::Constant(*m_currentSetPoint)); - - //m_spline.setKnots(m_trajectoryKnots, m_timeKnots); - - //m_initTrajectoryTime = yarp::os::Time::now(); - - std::cout << "[Module::configure] Starting the experiment." << std::endl; - - return true; -} - -bool Module::generateNewTrajectory() -{ - double initTrajectory = *m_currentSetPoint; - - // the trajectory is ended - if (std::next(m_currentSetPoint, 1) == m_setPoints.end()) + // check if vector is not initialized + if (m_qDesired.empty()) + { + std::cerr << "[Module::configure] Cannot advance empty reference signals." << std::endl; return false; + } - std::advance(m_currentSetPoint, 1); - double endTrajectory = *m_currentSetPoint; - - m_trajectoryKnots[0] = Vector1d::Constant(initTrajectory); - m_trajectoryKnots[1] = Vector1d::Constant(endTrajectory); - - m_spline.setKnots(m_trajectoryKnots, m_timeKnots); + std::cout << "[Module::configure] Starting the experiment." << std::endl; - m_initTrajectoryTime = yarp::os::Time::now(); + // Reach the first position of the desired trajectory in position control + m_robotControl.setReferences(m_qDesired.front(), + RobotInterface::IRobotControl::ControlMode::Position); return true; } bool Module::advanceReferenceSignals() { - // check if vector is not initialized - if(m_qDesired.empty()) + m_qDesired.pop_front(); + + // check if the vector is empty. If true the trajectory is ended + if (m_qDesired.empty()) { - std::cerr << "[jointControlModule::advanceReferenceSignals] Cannot advance empty reference signals." << std::endl; return false; } - m_qDesired.pop_front(); - m_qDesired.push_back(m_qDesired.back()); - return true; } bool Module::updateModule() { - if (!m_sensorBridge.advance()) - { - std::cerr << "[Module::updateModule] Unable to read the sensor." << std::endl; - return false; - } + bool isMotionDone; + bool isMimeExpired; + std::vector> jointlist; - // set the reference - if(!m_robotControl.setReferences(iDynTree::toEigen(m_qDesired.front()), - RobotInterface::IRobotControl::ControlMode::PositionDirect)) + m_robotControl.checkMotionDone(isMotionDone, isMimeExpired, jointlist); + + if (isMotionDone) { - std::cerr << "[Module::updateModule] Error while setting the reference position to iCub." << std::endl; - return false; - } + if (!m_sensorBridge.advance()) + { + std::cerr << "[Module::updateModule] Unable to read the sensor." << std::endl; + return false; + } - advanceReferenceSignals(); + if (!m_sensorBridge.getJointPositions(m_currentJointPos)) + { + std::cerr << "[Module::updateModule] Error in reading current position." << std::endl; + return false; + } + + // set the reference + if (!m_robotControl + .setReferences(m_qDesired.front(), + RobotInterface::IRobotControl::ControlMode::PositionDirect)) + { + std::cerr << "[Module::updateModule] Error while setting the reference position to " + "iCub." + << std::endl; + return false; + } + + m_logJointPos.push_back(m_currentJointPos); - m_sensorBridge.getJointPositions(m_currentJointPos); + if (!advanceReferenceSignals()) + { + std::cout << "[Module::updateModule] Experiment finished." << std::endl; + + return false; + } + } return true; } bool Module::close() { + std::cout << "[Module::close] I'm storing the dataset." << std::endl; + + // set the file name + std::time_t t = std::time(nullptr); + std::tm tm = *std::localtime(&t); + + std::ofstream stream; /**< std stream. */ + std::stringstream fileName; + + fileName << "Dataset_Measured_" << m_robotControl.getJointList().front() << "_" + << std::put_time(&tm, "%Y_%m_%d_%H_%M_%S") << ".txt"; + stream.open(fileName.str().c_str()); + + const auto sizeTraj = m_logJointPos.size(); + + for (int i = 0; i < sizeTraj; i++) + stream << m_logJointPos[i].transpose() << std::endl; + + stream.close(); + + std::cout << "[Module::close] Dataset stored. Closing." << std::endl; + // switch back in position control m_robotControl.setReferences(m_currentJointPos, RobotInterface::IRobotControl::ControlMode::Position); From 10950f3e3876c798c0bdf19bc4f7bdcd234498c8 Mon Sep 17 00:00:00 2001 From: Ines Date: Fri, 15 Jan 2021 14:41:31 +0100 Subject: [PATCH 05/51] script deleted from joint-trajectory-player/script. Unused variables deleted from joint-trajectory-player code. --- .../joint-trajectory-player/CMakeLists.txt | 5 -- .../blf-joint-trajectory-player-options.ini | 2 - .../JointTrajectoryPlayer/Module.h | 8 +- .../blf-joint-trajectory-player-script.sh | 12 --- ...lf_joint-trajectory-player_plot_dataset.py | 85 ------------------- .../joint-trajectory-player/src/Main.cpp | 2 +- .../joint-trajectory-player/src/Module.cpp | 8 +- 7 files changed, 7 insertions(+), 115 deletions(-) delete mode 100755 utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh delete mode 100644 utilities/joint-trajectory-player/script/blf_joint-trajectory-player_plot_dataset.py diff --git a/utilities/joint-trajectory-player/CMakeLists.txt b/utilities/joint-trajectory-player/CMakeLists.txt index a7d1fdc0b8..f9ce768995 100644 --- a/utilities/joint-trajectory-player/CMakeLists.txt +++ b/utilities/joint-trajectory-player/CMakeLists.txt @@ -10,8 +10,3 @@ add_bipedal_locomotion_application( ) install_ini_files(${CMAKE_CURRENT_SOURCE_DIR}/config) - -install(FILES script/blf-joint-trajectory-player-script.sh - PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ - DESTINATION "${CMAKE_INSTALL_BINDIR}") - diff --git a/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini b/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini index 2189e97bec..e4f0df3aaa 100644 --- a/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini +++ b/utilities/joint-trajectory-player/config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini @@ -1,7 +1,5 @@ name joint-trajectory-player sampling_time 0.01 -trajectory_file generatedtrajectory.txt -initial_trajectory_duration 5.0 [ROBOT_INTERFACE] robot_name icubSim diff --git a/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h b/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h index f87d09687d..4c493b94d3 100644 --- a/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h +++ b/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h @@ -41,18 +41,18 @@ class Module : public yarp::os::RFModule int m_numOfJoints; /**< Number of joints to control. */ - std::vector m_setPoints; - std::vector::const_iterator m_currentSetPoint; - std::deque m_qDesired; /**< Vector containing the results of the IK alg */ + std::vector m_logJointPos; + bool createPolydriver(std::shared_ptr handler); bool initializeRobotControl(std::shared_ptr handler); bool instantiateSensorBridge(std::shared_ptr handler); - std::vector m_logJointPos; + std::pair> + readStateFromFile(const std::string& filename, const std::size_t num_fields); /** * Advance the reference signal. diff --git a/utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh b/utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh deleted file mode 100755 index 6555431156..0000000000 --- a/utilities/joint-trajectory-player/script/blf-joint-trajectory-player-script.sh +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env bash - -scriptDirectory="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" - -echo "Welcome to the JointPositionTrackingTest." - -echo "I'm going to move the robot. Watch out!" - -blf-joint-trajectory-player --from blf-joint-trajectory-player-options.ini - -echo "The trajectory is terminated." - diff --git a/utilities/joint-trajectory-player/script/blf_joint-trajectory-player_plot_dataset.py b/utilities/joint-trajectory-player/script/blf_joint-trajectory-player_plot_dataset.py deleted file mode 100644 index cc86846dec..0000000000 --- a/utilities/joint-trajectory-player/script/blf_joint-trajectory-player_plot_dataset.py +++ /dev/null @@ -1,85 +0,0 @@ -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. -# Authors: Giulio Romualdi and Nicola Piga - -#!/usr/bin/env python3 - -import numpy as np -import argparse -import csv -import matplotlib -import matplotlib.pyplot as plt - -from matplotlib import rc -rc('font',**{'family':'sans-serif','sans-serif':['Helvetica']}) -rc('text', usetex=True) - -def read_data(file_name): - data = [] - - with open(file_name, newline='') as csv_data: - for row in csv_data: - try: - data.append([float(num_string.rstrip()) for num_string in row.split(sep = ", ") if num_string != '']) - except: - pass - - return np.array(data) - -def times(signal, i0 = 0): - times = signal.copy() - counter = i0 * (1.0 / 100.0) - dt = 1.0 / 100.0 - for i in range(len(times)): - times[i] = counter - counter = counter + dt - - return times - -def plot_data(axes, x_data, y_data, color, linewidth = 1.0): - - col = [] - if color == "blue": - col = plt.cm.Blues(0.8) - elif color == "red": - col = plt.cm.Reds(0.8) - - return axes.plot(x_data, y_data, color = col, linewidth = linewidth) - -def plot_and_save(data): - fig, ax = plt.subplots(1) - - channel_0 = data[:, 0] - channel_1 = data[:, 1] - - plot_0, = plot_data(ax, times(channel_0), channel_0, "red") - plot_1, = plot_data(ax, times(channel_1), channel_1, "blue") - - legend = [plot_0, plot_1] - legend = fig.legend(legend, labels = ["$\mathrm{Reference}$", "$\mathrm{Actual}$"], ncol = 2, loc = "upper center", frameon=False) - - for line in legend.get_lines(): - line.set_linewidth(2.0) - - label_font_size = 10 - ax.grid() - ax.set_xlabel('time [s]', fontsize = label_font_size) - ax.set_ylabel('position [rad]', fontsize = label_font_size) - ax.set_title('joint position', fontsize = label_font_size) - - figure = plt.gcf() - plt.savefig("./figure.png", bbox_inches='tight', dpi = 150) - - # plt.show() - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description='plot_dataset.py is a python script useful to plot a dataset generated by JointControlModule and save it.') - parser.add_argument('--dataset', type = str, required = True, help='Name of the dataset') - args = parser.parse_args() - - data = read_data(args.dataset) - - plot_and_save(data) - - diff --git a/utilities/joint-trajectory-player/src/Main.cpp b/utilities/joint-trajectory-player/src/Main.cpp index 47ee56bb9b..e830aa3495 100644 --- a/utilities/joint-trajectory-player/src/Main.cpp +++ b/utilities/joint-trajectory-player/src/Main.cpp @@ -1,6 +1,6 @@ /** * @file Main.cpp - * @authors Giulio Romualdi, Ines Sorrentino + * @authors Ines Sorrentino * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. */ diff --git a/utilities/joint-trajectory-player/src/Module.cpp b/utilities/joint-trajectory-player/src/Module.cpp index 8bde4aa2f8..099ef338de 100644 --- a/utilities/joint-trajectory-player/src/Module.cpp +++ b/utilities/joint-trajectory-player/src/Module.cpp @@ -1,6 +1,6 @@ /** * @file Module.cpp - * @authors Giulio Romualdi, Ines Sorrentino + * @authors Ines Sorrentino * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. */ @@ -104,7 +104,7 @@ bool Module::instantiateSensorBridge(std::shared_ptr> -readStateFromFile(const std::string& filename, const std::size_t num_fields) +Module::readStateFromFile(const std::string& filename, const std::size_t num_fields) { std::deque data; @@ -166,10 +166,6 @@ bool Module::configure(yarp::os::ResourceFinder& rf) if (!parametersHandler->getParameter("sampling_time", m_dT)) return false; - double initialTrajDuration = 5; - if (!parametersHandler->getParameter("initial_trajectory_duration", initialTrajDuration)) - return false; - std::string trajectoryFile; if (!parametersHandler->getParameter("trajectory_file", trajectoryFile)) return false; From c327dae72d76bd0479c5e2bf696b19637ce3049c Mon Sep 17 00:00:00 2001 From: Ines Date: Fri, 15 Jan 2021 15:01:58 +0100 Subject: [PATCH 06/51] CHANGELOG.md modified --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index b4bb1a198c..95353291f5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -28,5 +28,6 @@ All notable changes to this project are documented in this file. - Implement `FloatingBaseEstimatorDevice` YARP device for wrapping floating base estimation algorithms. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/130) - Implement Continuous algebraic Riccati equation function (https://github.com/dic-iit/bipedal-locomotion-framework/pull/157) - Implement YARP based `ROSPublisher` in the `YarpUtilities` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/156) +- Implement the `JointTrajectoryPlayer` application. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/169) [Unreleased]: https://github.com/dic-iit/bipedal-locomotion-framework/ From e259b229f4c91a554f6eaacc7d9b9fe01bae8259 Mon Sep 17 00:00:00 2001 From: Ines Date: Mon, 18 Jan 2021 11:56:21 +0100 Subject: [PATCH 07/51] joint-trajectory-player changes after review --- .../JointTrajectoryPlayer/Module.h | 11 ++-- .../joint-trajectory-player/src/Module.cpp | 62 +++++++++++++------ 2 files changed, 51 insertions(+), 22 deletions(-) diff --git a/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h b/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h index 4c493b94d3..b3fceb0e08 100644 --- a/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h +++ b/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h @@ -32,18 +32,18 @@ class Module : public yarp::os::RFModule double m_dT; /**< RFModule period. */ std::string m_robot; /**< Robot name. */ - Eigen::VectorXd m_currentJointPos; + Eigen::VectorXd m_currentJointPos; /**< Current joint positions. */ std::shared_ptr m_robotDevice; - RobotInterface::YarpRobotControl m_robotControl; - RobotInterface::YarpSensorBridge m_sensorBridge; + RobotInterface::YarpRobotControl m_robotControl; /**< Robot control object. */ + RobotInterface::YarpSensorBridge m_sensorBridge; /**< Sensor bridge object. */ int m_numOfJoints; /**< Number of joints to control. */ std::deque m_qDesired; /**< Vector containing the results of the IK alg */ - std::vector m_logJointPos; + std::vector m_logJointPos; /**< Measured joint positions. */ bool createPolydriver(std::shared_ptr handler); @@ -54,6 +54,9 @@ class Module : public yarp::os::RFModule std::pair> readStateFromFile(const std::string& filename, const std::size_t num_fields); + enum class State {idle, positioning, running}; + State m_state{State::idle}; + /** * Advance the reference signal. * @return true in case of success and false otherwise. diff --git a/utilities/joint-trajectory-player/src/Module.cpp b/utilities/joint-trajectory-player/src/Module.cpp index 099ef338de..976409a615 100644 --- a/utilities/joint-trajectory-player/src/Module.cpp +++ b/utilities/joint-trajectory-player/src/Module.cpp @@ -19,8 +19,6 @@ #include -using Vector1d = Eigen::Matrix; - using namespace BipedalLocomotion; using namespace BipedalLocomotion::JointTrajectoryPlayer; @@ -46,13 +44,6 @@ bool Module::createPolydriver(std::shared_ptrview(axis); - if (axis != nullptr) - axis->getAxes(&m_numOfJoints); - return true; } @@ -104,7 +95,7 @@ bool Module::instantiateSensorBridge(std::shared_ptr> -Module::readStateFromFile(const std::string& filename, const std::size_t num_fields) +Module::readStateFromFile(const std::string& filename, const std::size_t numFields) { std::deque data; @@ -124,7 +115,7 @@ Module::readStateFromFile(const std::string& filename, const std::size_t num_fie } Eigen::VectorXd vector; - vector.resize(num_fields); + vector.resize(numFields); std::size_t found_lines = 0; for (auto line : istrm_strings) { @@ -137,11 +128,11 @@ Module::readStateFromFile(const std::string& filename, const std::size_t num_fie vector(found_fields) = std::stod(number_str); found_fields++; } - if (num_fields != found_fields) + if (numFields != found_fields) { std::cout << "[Module::readStateFromFile] Malformed input file " << filename << std::endl; - std::cout << "[Module::readStateFromFile] Expected " << num_fields + std::cout << "[Module::readStateFromFile] Expected " << numFields << " columns, found " << found_fields << std::endl; return std::make_pair(false, data); @@ -168,13 +159,23 @@ bool Module::configure(yarp::os::ResourceFinder& rf) std::string trajectoryFile; if (!parametersHandler->getParameter("trajectory_file", trajectoryFile)) + { + std::cerr << "[Module::configure] trajectory_file parameter not specified." << std::endl; return false; + } this->createPolydriver(parametersHandler); this->initializeRobotControl(parametersHandler); this->instantiateSensorBridge(parametersHandler); - m_currentJointPos.resize(m_robotControl.getJointList().size()); + m_numOfJoints = m_robotControl.getJointList().size(); + if (m_numOfJoints == 0) + { + std::cerr << "[Module::configure] No joints to control." << std::endl; + return false; + } + + m_currentJointPos.resize(m_numOfJoints); m_qDesired.clear(); auto data = readStateFromFile(trajectoryFile, m_numOfJoints); @@ -197,6 +198,8 @@ bool Module::configure(yarp::os::ResourceFinder& rf) m_robotControl.setReferences(m_qDesired.front(), RobotInterface::IRobotControl::ControlMode::Position); + m_state = State::positioning; + return true; } @@ -216,13 +219,30 @@ bool Module::advanceReferenceSignals() bool Module::updateModule() { bool isMotionDone; - bool isMimeExpired; + bool isTimeExpired; std::vector> jointlist; - m_robotControl.checkMotionDone(isMotionDone, isMimeExpired, jointlist); - - if (isMotionDone) + switch (m_state) { + case State::positioning: + if (!m_robotControl.checkMotionDone(isMotionDone, isTimeExpired, jointlist)) + { + std::cerr << ""; + return false; + } + if (isTimeExpired) + { + std::cerr << "Printa lista giunti che non hanno finito (primo nome del giunto secondo " + "errore in radianti)"; + return false; + } + if (isMotionDone) + { + m_state = State::running; + } + break; + + case State::running: if (!m_sensorBridge.advance()) { std::cerr << "[Module::updateModule] Unable to read the sensor." << std::endl; @@ -254,6 +274,12 @@ bool Module::updateModule() return false; } + break; + + default: + std::cerr << ""; + return false; + break; } return true; From 46fae0cb96c9be6d03808064dff602f7b4d4cf86 Mon Sep 17 00:00:00 2001 From: Ines Date: Tue, 19 Jan 2021 14:42:51 +0100 Subject: [PATCH 08/51] joint-trajectory-player changes after review --- .../joint-trajectory-player/CMakeLists.txt | 7 +- .../JointTrajectoryPlayer/Module.h | 27 ++-- .../joint-trajectory-player/src/Module.cpp | 123 ++++++------------ 3 files changed, 64 insertions(+), 93 deletions(-) diff --git a/utilities/joint-trajectory-player/CMakeLists.txt b/utilities/joint-trajectory-player/CMakeLists.txt index f9ce768995..7367c32fa3 100644 --- a/utilities/joint-trajectory-player/CMakeLists.txt +++ b/utilities/joint-trajectory-player/CMakeLists.txt @@ -6,7 +6,12 @@ add_bipedal_locomotion_application( NAME joint-trajectory-player SOURCES src/Main.cpp src/Module.cpp HEADERS include/BipedalLocomotion/JointTrajectoryPlayer/Module.h - LINK_LIBRARIES YARP::YARP_dev BipedalLocomotion::Planners BipedalLocomotion::ParametersHandlerYarpImplementation BipedalLocomotion::RobotInterfaceYarpImplementation + LINK_LIBRARIES YARP::YARP_dev + BipedalLocomotion::Planners + BipedalLocomotion::ParametersHandlerYarpImplementation + BipedalLocomotion::RobotInterfaceYarpImplementation + matioCpp::matioCpp + BipedalLocomotion::matioCppConversions ) install_ini_files(${CMAKE_CURRENT_SOURCE_DIR}/config) diff --git a/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h b/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h index b3fceb0e08..c3f4fff952 100644 --- a/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h +++ b/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h @@ -17,6 +17,8 @@ // YARP #include +#include + #include #include #include @@ -43,7 +45,13 @@ class Module : public yarp::os::RFModule std::deque m_qDesired; /**< Vector containing the results of the IK alg */ - std::vector m_logJointPos; /**< Measured joint positions. */ + std::unordered_map> m_logJointPos; /**< Measured joint + positions. */ + + std::vector m_axisList; /**< Axis name list. */ + + matioCpp::MultiDimensionalArray m_traj; + unsigned int m_idxTraj{0}; bool createPolydriver(std::shared_ptr handler); @@ -51,18 +59,17 @@ class Module : public yarp::os::RFModule bool instantiateSensorBridge(std::shared_ptr handler); - std::pair> - readStateFromFile(const std::string& filename, const std::size_t num_fields); + bool readStateFromFile(const std::string& filename, const std::size_t numFields); + - enum class State {idle, positioning, running}; + enum class State + { + idle, + positioning, + running + }; State m_state{State::idle}; - /** - * Advance the reference signal. - * @return true in case of success and false otherwise. - */ - bool advanceReferenceSignals(); - public: /** * Get the period of the RFModule. diff --git a/utilities/joint-trajectory-player/src/Module.cpp b/utilities/joint-trajectory-player/src/Module.cpp index 976409a615..3331d0443f 100644 --- a/utilities/joint-trajectory-player/src/Module.cpp +++ b/utilities/joint-trajectory-player/src/Module.cpp @@ -5,7 +5,6 @@ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. */ -#include #include #include @@ -19,6 +18,8 @@ #include +#include + using namespace BipedalLocomotion; using namespace BipedalLocomotion::JointTrajectoryPlayer; @@ -94,54 +95,28 @@ bool Module::instantiateSensorBridge(std::shared_ptr> -Module::readStateFromFile(const std::string& filename, const std::size_t numFields) +bool Module::readStateFromFile(const std::string& filename, const std::size_t numFields) { std::deque data; - std::ifstream istrm(filename); + matioCpp::File input(filename); - if (!istrm.is_open()) + if (!input.isOpen()) { - std::cout << "Failed to open " << filename << '\n'; - return std::make_pair(false, data); + std::cout << "[Module::readStateFromFile] Failed to open " << filename << "." << std::endl; + return false; } else { - std::vector istrm_strings; - std::string line; - while (std::getline(istrm, line)) - { - istrm_strings.push_back(line); - } - - Eigen::VectorXd vector; - vector.resize(numFields); - std::size_t found_lines = 0; - for (auto line : istrm_strings) + m_traj = input.read("traj").asMultiDimensionalArray(); // Read a multi dimensional + // array named "traj" + if (!m_traj.isValid()) { - std::size_t found_fields = 0; - std::string number_str; - std::istringstream iss(line); - - while (iss >> number_str) - { - vector(found_fields) = std::stod(number_str); - found_fields++; - } - if (numFields != found_fields) - { - std::cout << "[Module::readStateFromFile] Malformed input file " << filename - << std::endl; - std::cout << "[Module::readStateFromFile] Expected " << numFields - << " columns, found " << found_fields << std::endl; - - return std::make_pair(false, data); - } - data.push_back(vector); - found_lines++; + std::cerr << "[Module::readStateFromFile] Error reading input file: " << filename << "." + << std::endl; + return false; } - return std::make_pair(true, data); + return true; } } @@ -160,7 +135,7 @@ bool Module::configure(yarp::os::ResourceFinder& rf) std::string trajectoryFile; if (!parametersHandler->getParameter("trajectory_file", trajectoryFile)) { - std::cerr << "[Module::configure] trajectory_file parameter not specified." << std::endl; + std::cerr << "[Module::configure] Trajectory_file parameter not specified." << std::endl; return false; } @@ -175,27 +150,19 @@ bool Module::configure(yarp::os::ResourceFinder& rf) return false; } - m_currentJointPos.resize(m_numOfJoints); + m_axisList = m_robotControl.getJointList(); - m_qDesired.clear(); - auto data = readStateFromFile(trajectoryFile, m_numOfJoints); - if (!data.first) - { - return false; - } - m_qDesired = data.second; + m_currentJointPos.resize(m_numOfJoints); - // check if vector is not initialized - if (m_qDesired.empty()) + if (!readStateFromFile(trajectoryFile, m_numOfJoints)) { - std::cerr << "[Module::configure] Cannot advance empty reference signals." << std::endl; return false; } std::cout << "[Module::configure] Starting the experiment." << std::endl; // Reach the first position of the desired trajectory in position control - m_robotControl.setReferences(m_qDesired.front(), + m_robotControl.setReferences(Conversions::toEigen(m_traj).row(m_idxTraj), RobotInterface::IRobotControl::ControlMode::Position); m_state = State::positioning; @@ -203,19 +170,6 @@ bool Module::configure(yarp::os::ResourceFinder& rf) return true; } -bool Module::advanceReferenceSignals() -{ - m_qDesired.pop_front(); - - // check if the vector is empty. If true the trajectory is ended - if (m_qDesired.empty()) - { - return false; - } - - return true; -} - bool Module::updateModule() { bool isMotionDone; @@ -255,9 +209,22 @@ bool Module::updateModule() return false; } + for (int i = 0; i < m_numOfJoints; i++) + { + m_logJointPos[m_axisList[i]].push_back(m_currentJointPos[i]); + } + + m_idxTraj++; + if (m_idxTraj == Conversions::toEigen(m_traj).rows()) + { + std::cout << "[Module::updateModule] Experiment finished." << std::endl; + + return false; + } + // set the reference if (!m_robotControl - .setReferences(m_qDesired.front(), + .setReferences(Conversions::toEigen(m_traj).row(m_idxTraj), RobotInterface::IRobotControl::ControlMode::PositionDirect)) { std::cerr << "[Module::updateModule] Error while setting the reference position to " @@ -266,14 +233,6 @@ bool Module::updateModule() return false; } - m_logJointPos.push_back(m_currentJointPos); - - if (!advanceReferenceSignals()) - { - std::cout << "[Module::updateModule] Experiment finished." << std::endl; - - return false; - } break; default: @@ -293,19 +252,19 @@ bool Module::close() std::time_t t = std::time(nullptr); std::tm tm = *std::localtime(&t); - std::ofstream stream; /**< std stream. */ std::stringstream fileName; fileName << "Dataset_Measured_" << m_robotControl.getJointList().front() << "_" - << std::put_time(&tm, "%Y_%m_%d_%H_%M_%S") << ".txt"; - stream.open(fileName.str().c_str()); - - const auto sizeTraj = m_logJointPos.size(); + << std::put_time(&tm, "%Y_%m_%d_%H_%M_%S") << ".mat"; - for (int i = 0; i < sizeTraj; i++) - stream << m_logJointPos[i].transpose() << std::endl; + matioCpp::File file = matioCpp::File::Create(fileName.str().c_str()); - stream.close(); + for (auto& [key, value] : m_logJointPos) + { + matioCpp::Vector out(key); + out = value; + file.write(out); + } std::cout << "[Module::close] Dataset stored. Closing." << std::endl; From a9a2c599ad1ea5db8ca2419042432049fe85258b Mon Sep 17 00:00:00 2001 From: Ines Date: Tue, 19 Jan 2021 18:19:37 +0100 Subject: [PATCH 09/51] added README.md for joint-trajectory-player application --- utilities/joint-trajectory-player/README.md | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 utilities/joint-trajectory-player/README.md diff --git a/utilities/joint-trajectory-player/README.md b/utilities/joint-trajectory-player/README.md new file mode 100644 index 0000000000..2944ea7a1c --- /dev/null +++ b/utilities/joint-trajectory-player/README.md @@ -0,0 +1,18 @@ +# joint-trajectory-player* + +The **joint-trajectory-player** is a simple tool for playing a joint trajectory on a robot in a `YARP` environment. The tool allows you to save the measured joint positions and measured currents. + +## :running: How to use the application +The application can be launched with the following command: +``` +blf-joint-trajectory-player --from [`blf-joint-position-tracking-options.ini`](./config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini) --trajectory_file mat_file_where_you_saved_the_joint_trajectory.mat +``` +The trajectory file must have a field called `traj` containing the trajectory stored as a matrix in row major order. +If you correctly installed the framework you can run the application from any folder. + +The [`blf-joint-trajectory-player-options.ini`](./config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini) file contains some parameters that you may modify to control a given set of joints: +- `robot_name`: name of the robot +- `joints_list`: list of the controlled joints +- `remote_control_boards`: list of associated control boards + +Please if you want to run the application for a different robot remember to create a new folder in `./config/robots/`. The name of the folder should match the name of the robot. From cc1025f0fde07d44b3f293e77db1a30cff008f9a Mon Sep 17 00:00:00 2001 From: Ines Date: Tue, 19 Jan 2021 18:23:47 +0100 Subject: [PATCH 10/51] README.md for joint-trajectory-player application modified --- utilities/joint-trajectory-player/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/utilities/joint-trajectory-player/README.md b/utilities/joint-trajectory-player/README.md index 2944ea7a1c..af8f84489f 100644 --- a/utilities/joint-trajectory-player/README.md +++ b/utilities/joint-trajectory-player/README.md @@ -5,9 +5,9 @@ The **joint-trajectory-player** is a simple tool for playing a joint trajectory ## :running: How to use the application The application can be launched with the following command: ``` -blf-joint-trajectory-player --from [`blf-joint-position-tracking-options.ini`](./config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini) --trajectory_file mat_file_where_you_saved_the_joint_trajectory.mat +blf-joint-trajectory-player --from blf-joint-trajectory-player-options.ini --trajectory_file file_of_the_joint_trajectory.mat ``` -The trajectory file must have a field called `traj` containing the trajectory stored as a matrix in row major order. +The `.mat` file must have a field called `traj` containing the trajectory stored as a matrix in row major order. If you correctly installed the framework you can run the application from any folder. The [`blf-joint-trajectory-player-options.ini`](./config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini) file contains some parameters that you may modify to control a given set of joints: From 27c5b7224ac8295a45643f15dc6ed9c8cd76519f Mon Sep 17 00:00:00 2001 From: Ines Date: Tue, 19 Jan 2021 18:28:08 +0100 Subject: [PATCH 11/51] Clang formatting and documentation added --- .../JointTrajectoryPlayer/Module.h | 26 +++++++++---------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h b/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h index c3f4fff952..27abcd3b3c 100644 --- a/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h +++ b/utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h @@ -36,31 +36,21 @@ class Module : public yarp::os::RFModule Eigen::VectorXd m_currentJointPos; /**< Current joint positions. */ - std::shared_ptr m_robotDevice; + std::shared_ptr m_robotDevice; /**< PolyDriver. */ RobotInterface::YarpRobotControl m_robotControl; /**< Robot control object. */ RobotInterface::YarpSensorBridge m_sensorBridge; /**< Sensor bridge object. */ int m_numOfJoints; /**< Number of joints to control. */ - std::deque m_qDesired; /**< Vector containing the results of the IK alg */ - std::unordered_map> m_logJointPos; /**< Measured joint positions. */ std::vector m_axisList; /**< Axis name list. */ - matioCpp::MultiDimensionalArray m_traj; - unsigned int m_idxTraj{0}; - - bool createPolydriver(std::shared_ptr handler); - - bool initializeRobotControl(std::shared_ptr handler); - - bool instantiateSensorBridge(std::shared_ptr handler); + matioCpp::MultiDimensionalArray m_traj; /**< Joint trajectory. */ - bool readStateFromFile(const std::string& filename, const std::size_t numFields); - + unsigned int m_idxTraj{0}; /**< Index to iterate the trajectory. */ enum class State { @@ -68,7 +58,15 @@ class Module : public yarp::os::RFModule positioning, running }; - State m_state{State::idle}; + State m_state{State::idle}; /** State machine */ + + bool createPolydriver(std::shared_ptr handler); + + bool initializeRobotControl(std::shared_ptr handler); + + bool instantiateSensorBridge(std::shared_ptr handler); + + bool readStateFromFile(const std::string& filename, const std::size_t numFields); public: /** From f452d633d33422f104779fc8df6bcbe7d87dcbc9 Mon Sep 17 00:00:00 2001 From: Ines Date: Tue, 19 Jan 2021 19:14:30 +0100 Subject: [PATCH 12/51] Added cerr messages to joint-trajectory-player application (Module.cpp) --- utilities/joint-trajectory-player/src/Module.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/utilities/joint-trajectory-player/src/Module.cpp b/utilities/joint-trajectory-player/src/Module.cpp index 3331d0443f..b0cadd8d3c 100644 --- a/utilities/joint-trajectory-player/src/Module.cpp +++ b/utilities/joint-trajectory-player/src/Module.cpp @@ -181,13 +181,19 @@ bool Module::updateModule() case State::positioning: if (!m_robotControl.checkMotionDone(isMotionDone, isTimeExpired, jointlist)) { - std::cerr << ""; + std::cerr << "[Module::updateModule] Impossible to check if the motion is done." + << std::endl; return false; } if (isTimeExpired) { - std::cerr << "Printa lista giunti che non hanno finito (primo nome del giunto secondo " - "errore in radianti)"; + std::cerr << "[Module::updateModule] List of joints not finishing in time: " + << std::endl; + for (int i = 0; i < jointlist.size(); i++) + { + std::cerr << "Joint " << jointlist[i].first << "--> Error " << jointlist[i].second << " rad" + << std::endl; + } return false; } if (isMotionDone) @@ -236,7 +242,7 @@ bool Module::updateModule() break; default: - std::cerr << ""; + std::cerr << "[Module::updateModule] Cannot pro"; return false; break; } From 7bcb29615dd59f11b1517ddd534ca5ac427ae456 Mon Sep 17 00:00:00 2001 From: Ines Date: Wed, 20 Jan 2021 08:24:59 +0100 Subject: [PATCH 13/51] Added cerr messages to joint-trajectory-player application (Module.cpp) --- utilities/joint-trajectory-player/src/Module.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/utilities/joint-trajectory-player/src/Module.cpp b/utilities/joint-trajectory-player/src/Module.cpp index b0cadd8d3c..5a69e891e1 100644 --- a/utilities/joint-trajectory-player/src/Module.cpp +++ b/utilities/joint-trajectory-player/src/Module.cpp @@ -191,8 +191,8 @@ bool Module::updateModule() << std::endl; for (int i = 0; i < jointlist.size(); i++) { - std::cerr << "Joint " << jointlist[i].first << "--> Error " << jointlist[i].second << " rad" - << std::endl; + std::cerr << "Joint " << jointlist[i].first << "--> Error " << jointlist[i].second + << " rad" << std::endl; } return false; } @@ -242,7 +242,7 @@ bool Module::updateModule() break; default: - std::cerr << "[Module::updateModule] Cannot pro"; + std::cerr << "[Module::updateModule] The program is in an unknown state. Cannot proceed."; return false; break; } From 6a3377ff4a9993744db78675482d29095db7c1d4 Mon Sep 17 00:00:00 2001 From: InesSorrentino <43743081+isorrentino@users.noreply.github.com> Date: Wed, 20 Jan 2021 09:07:57 +0100 Subject: [PATCH 14/51] Update README.md --- utilities/joint-trajectory-player/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utilities/joint-trajectory-player/README.md b/utilities/joint-trajectory-player/README.md index af8f84489f..cc6573a5c1 100644 --- a/utilities/joint-trajectory-player/README.md +++ b/utilities/joint-trajectory-player/README.md @@ -1,4 +1,4 @@ -# joint-trajectory-player* +# joint-trajectory-player The **joint-trajectory-player** is a simple tool for playing a joint trajectory on a robot in a `YARP` environment. The tool allows you to save the measured joint positions and measured currents. From 0a0a5149637bca8b5a2917bf659c3df7654735a3 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 20 Feb 2020 14:30:03 +0100 Subject: [PATCH 15/51] Added CMake skeleton for MAS IMU test. --- ...lLocomotionFrameworkFindDependencies.cmake | 5 +++ utilities/CMakeLists.txt | 4 +- utilities/MasImuTest/CMakeLists.txt | 43 +++++++++++++++++++ utilities/MasImuTest/src/main.cpp | 12 ++++++ 4 files changed, 62 insertions(+), 2 deletions(-) create mode 100644 utilities/MasImuTest/CMakeLists.txt create mode 100644 utilities/MasImuTest/src/main.cpp diff --git a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake index 3d357af1dc..026c9fee91 100644 --- a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake @@ -228,3 +228,8 @@ framework_dependent_option(FRAMEWORK_COMPILE_PYTHON_BINDINGS framework_dependent_option(FRAMEWORK_TEST_PYTHON_BINDINGS "Do you want to test the Python bindings?" ON "FRAMEWORK_COMPILE_tests;FRAMEWORK_COMPILE_PYTHON_BINDINGS;FRAMEWORK_USE_pytest" OFF) + +framework_dependent_option(BIPEDAL_LOCOMOTION_CONTROLLERS_COMPILE_MasImuTest + "Compile test on the MAS IMU?" ON + "FRAMEWORK_COMPILE_YarpUtilities" OFF) + diff --git a/utilities/CMakeLists.txt b/utilities/CMakeLists.txt index f616c8fcdd..9d7b8875d0 100644 --- a/utilities/CMakeLists.txt +++ b/utilities/CMakeLists.txt @@ -1,8 +1,8 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. add_subdirectory(joint-position-tracking) - add_subdirectory(joint-trajectory-player) +add_subdirectory(MasImuTest) diff --git a/utilities/MasImuTest/CMakeLists.txt b/utilities/MasImuTest/CMakeLists.txt new file mode 100644 index 0000000000..4fe48df34d --- /dev/null +++ b/utilities/MasImuTest/CMakeLists.txt @@ -0,0 +1,43 @@ +# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +# set target name +if(BIPEDAL_LOCOMOTION_CONTROLLERS_COMPILE_MasImuTest) + + set(BINARY_TARGET_NAME MasImuTest) + + # set cpp files + set(MasImuTest_SRC + src/main.cpp + ) + + # set hpp files + set(MasImuTest_HDR + + + ) + + add_executable(${BINARY_TARGET_NAME} ${MasImuTest_SRC} ${MasImuTest_HDR}) + + target_link_libraries(${BINARY_TARGET_NAME} PUBLIC ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} + BipedalLocomotionControllers::YarpUtilities BipedalLocomotionControllers::ParametersHandlerYarpImplementation) + target_compile_features(${BINARY_TARGET_NAME} PUBLIC cxx_std_17) + + # Specify include directories for both compilation and installation process. + # The $ generator expression is useful to ensure to create + # relocatable configuration files, see https://cmake.org/cmake/help/latest/manual/cmake-packages.7.html#creating-relocatable-packages + target_include_directories(${BINARY_TARGET_NAME} PUBLIC "$" + "$/${CMAKE_INSTALL_INCLUDEDIR}>") + + # Specify installation targets, typology and destination folders. + install(TARGETS ${BINARY_TARGET_NAME} + EXPORT ${PROJECT_NAME} + COMPONENT runtime + LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib + ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib + RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin) + + message(STATUS "Created target ${BINARY_TARGET_NAME} for export ${PROJECT_NAME}.") + +endif() diff --git a/utilities/MasImuTest/src/main.cpp b/utilities/MasImuTest/src/main.cpp new file mode 100644 index 0000000000..79b9dadcb4 --- /dev/null +++ b/utilities/MasImuTest/src/main.cpp @@ -0,0 +1,12 @@ +/* + * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia + * Authors: Stefano Dafarra + * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * + */ + + +int main(int argc, char * argv[]) +{ + +} From e7e54636778c84b2d6d77e4d311d09667ccef876 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 20 Feb 2020 18:59:18 +0100 Subject: [PATCH 16/51] Added basic structure of the MAS IMU test. --- utilities/MasImuTest/CMakeLists.txt | 26 ++++--- .../BipedalLocomotionControllers/MasImuTest.h | 70 +++++++++++++++++++ utilities/MasImuTest/src/MasImuTest.cpp | 48 +++++++++++++ utilities/MasImuTest/src/main.cpp | 12 ++-- .../thrifts/MasImuTestCommands.thrift | 20 ++++++ 5 files changed, 161 insertions(+), 15 deletions(-) create mode 100644 utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h create mode 100644 utilities/MasImuTest/src/MasImuTest.cpp create mode 100644 utilities/MasImuTest/thrifts/MasImuTestCommands.thrift diff --git a/utilities/MasImuTest/CMakeLists.txt b/utilities/MasImuTest/CMakeLists.txt index 4fe48df34d..f3ace248e2 100644 --- a/utilities/MasImuTest/CMakeLists.txt +++ b/utilities/MasImuTest/CMakeLists.txt @@ -5,39 +5,45 @@ # set target name if(BIPEDAL_LOCOMOTION_CONTROLLERS_COMPILE_MasImuTest) - set(BINARY_TARGET_NAME MasImuTest) + set(EXE_TARGET_NAME MasImuTest) # set cpp files - set(MasImuTest_SRC + set(${EXE_TARGET_NAME}_SRC src/main.cpp + src/MasImuTest.cpp ) # set hpp files - set(MasImuTest_HDR + set(${EXE_TARGET_NAME}_HDR + include/BipedalLocomotionControllers/MasImuTest.h + ) + set(${EXE_TARGET_NAME}_THRIFT_HDR + thrifts/MasImuTestCommands.thrift + ) - ) + yarp_add_idl(${EXE_TARGET_NAME}_THRIFT_GEN_FILES ${${EXE_TARGET_NAME}_THRIFT_HDR}) - add_executable(${BINARY_TARGET_NAME} ${MasImuTest_SRC} ${MasImuTest_HDR}) + add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_SRC} ${${EXE_TARGET_NAME}_HDR} ${${EXE_TARGET_NAME}_THRIFT_GEN_FILES}) - target_link_libraries(${BINARY_TARGET_NAME} PUBLIC ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} + target_link_libraries(${EXE_TARGET_NAME} PUBLIC ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} BipedalLocomotionControllers::YarpUtilities BipedalLocomotionControllers::ParametersHandlerYarpImplementation) - target_compile_features(${BINARY_TARGET_NAME} PUBLIC cxx_std_17) + target_compile_features(${EXE_TARGET_NAME} PUBLIC cxx_std_17) # Specify include directories for both compilation and installation process. # The $ generator expression is useful to ensure to create # relocatable configuration files, see https://cmake.org/cmake/help/latest/manual/cmake-packages.7.html#creating-relocatable-packages - target_include_directories(${BINARY_TARGET_NAME} PUBLIC "$" + target_include_directories(${EXE_TARGET_NAME} PUBLIC "$" "$/${CMAKE_INSTALL_INCLUDEDIR}>") # Specify installation targets, typology and destination folders. - install(TARGETS ${BINARY_TARGET_NAME} + install(TARGETS ${EXE_TARGET_NAME} EXPORT ${PROJECT_NAME} COMPONENT runtime LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin) - message(STATUS "Created target ${BINARY_TARGET_NAME} for export ${PROJECT_NAME}.") + message(STATUS "Created target ${EXE_TARGET_NAME} for export ${PROJECT_NAME}.") endif() diff --git a/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h b/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h new file mode 100644 index 0000000000..4db82187a6 --- /dev/null +++ b/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h @@ -0,0 +1,70 @@ +/** + * @file MasImuTest.h + * @authors Stefano Dafarra + * @copyright 2019 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef BIPEDAL_LOCOMOTION_CONTROLLERS_MASIMUTEST_H +#define BIPEDAL_LOCOMOTION_CONTROLLERS_MASIMUTEST_H + +// YARP +#include + +//Thrifts +#include + +#include + +namespace BipedalLocomotionControllers +{ + class MasImuTest; +} + + +class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, public MasImuTestCommands { + + BipedalLocomotionControllers::ParametersHandler::YarpImplementation::unique_ptr m_parametersPtr; + + bool configureRobot(); + +public: + + /** + * Get the period of the RFModule. + * @return the period of the module. + */ + double getPeriod() override; + + /** + * Main function of the RFModule. + * @return true in case of success and false otherwise. + */ + bool updateModule() override; + + /** + * Configure the RFModule. + * @param rf is the reference to a resource finder object + * @return true in case of success and false otherwise. + */ + bool configure(yarp::os::ResourceFinder& rf) override; + + /** + * Close the RFModule. + * @return true in case of success and false otherwise. + */ + bool close() override; + + /** + * Call this method to start the test. + * @return true/false in case of success/failure (for example if the preparation phase was not successfull); + */ + bool startTest() override; + + /** + * Stop the test + */ + void stopTest() override; +}; + +#endif // BIPEDAL_LOCOMOTION_CONTROLLERS_MASIMUTEST_H diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp new file mode 100644 index 0000000000..8b853d933c --- /dev/null +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -0,0 +1,48 @@ +/** + * @file MasImuTest.cpp + * @authors Stefano Dafarra + * @copyright 2019 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +#include + +using namespace BipedalLocomotionControllers; + + + +bool MasImuTest::configureRobot() +{ + +} + +double MasImuTest::getPeriod() +{ + +} + +bool MasImuTest::updateModule() +{ + +} + +bool MasImuTest::configure(yarp::os::ResourceFinder &rf) +{ + m_parametersPtr = std::make_unique(rf); + +} + +bool MasImuTest::close() +{ + +} + +bool MasImuTest::startTest() +{ + +} + +void MasImuTest::stopTest() +{ + +} diff --git a/utilities/MasImuTest/src/main.cpp b/utilities/MasImuTest/src/main.cpp index 79b9dadcb4..a5f92fc4f0 100644 --- a/utilities/MasImuTest/src/main.cpp +++ b/utilities/MasImuTest/src/main.cpp @@ -1,12 +1,14 @@ -/* - * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia - * Authors: Stefano Dafarra - * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT - * +/** + * @file main.cpp + * @authors Stefano Dafarra + * @copyright 2019 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. */ +#include int main(int argc, char * argv[]) { + BipedalLocomotionControllers::MasImuTest test; } diff --git a/utilities/MasImuTest/thrifts/MasImuTestCommands.thrift b/utilities/MasImuTest/thrifts/MasImuTestCommands.thrift new file mode 100644 index 0000000000..01dc36dd58 --- /dev/null +++ b/utilities/MasImuTest/thrifts/MasImuTestCommands.thrift @@ -0,0 +1,20 @@ +/** + * @file MasImuTestCommands.thrift + * @authors Stefano Dafarra + * @copyright 2019 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. + */ + +service MasImuTestCommands +{ + /** + * Call this method to start the test. + * @return true/false in case of success/failure (for example if the preparation phase was not successfull); + */ + bool startTest(); + + /** + * Stop the test + */ + void stopTest(); +} From f04046db5a11099484726868a7fd8bd1e9f754d1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Sat, 7 Mar 2020 11:45:52 +0100 Subject: [PATCH 17/51] Added configuration of masImuTest. --- .../robots/iCubGenova04/MasImuTestConfig.ini | 30 ++ .../BipedalLocomotionControllers/MasImuTest.h | 76 +++- utilities/MasImuTest/src/MasImuTest.cpp | 370 +++++++++++++++++- 3 files changed, 469 insertions(+), 7 deletions(-) create mode 100644 utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini diff --git a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini new file mode 100644 index 0000000000..1e97be7e3a --- /dev/null +++ b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini @@ -0,0 +1,30 @@ +name masImuTest + +robot icub + +model model.urdf + +period 0.01 + +base_link root_link + +#The base_rotation is defined (by rows) in the Inertia frame (x pointing forward, z pointing upward) +base_rotation ((-1.0 0.0 0.0),(0.0 -1.0 0.0),(0.0 0.0 1.0)) + +filter_yaw 1 + +[LEFT_LEG] + +remote left_leg/inertials + +imu_frame l_foot_ft_acc_3b13 + +remote_control_boards ("left_leg") + +[RIGHT_LEG] + +remote right_leg/inertials + +imu_frame r_foot_ft_acc_3b14 + +remote_control_boards ("right_leg") diff --git a/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h b/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h index 4db82187a6..8a788b5c33 100644 --- a/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h +++ b/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h @@ -8,8 +8,23 @@ #ifndef BIPEDAL_LOCOMOTION_CONTROLLERS_MASIMUTEST_H #define BIPEDAL_LOCOMOTION_CONTROLLERS_MASIMUTEST_H +// STD +#include +#include +#include + // YARP #include +#include +#include +#include +#include + +// iDynTree +#include +#include +#include +#include //Thrifts #include @@ -21,18 +36,73 @@ namespace BipedalLocomotionControllers class MasImuTest; } - class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, public MasImuTestCommands { + struct CommonData + { + std::string robotName; + std::string prefix; + iDynTree::Model fullModel; + iDynTree::Traversal traversal; + iDynTree::Rotation baseRotation; + bool filterYaw; + }; + + class MasImuData + { + std::shared_ptr m_commonDataPtr; + BipedalLocomotionControllers::ParametersHandler::YarpImplementation::shared_ptr m_group; + iDynTree::FrameIndex m_frame; + std::string m_frameName; + iDynTree::LinkIndex m_link; + std::vector m_consideredJointIndexes; + std::vector m_consideredJointNames; + iDynTree::KinDynComputations m_kinDyn; + yarp::dev::PolyDriver m_orientationDriver, m_robotDriver; + yarp::dev::IOrientationSensors* m_orientationInterface; + yarp::dev::IEncodersTimed* m_encodersInterface; + size_t m_sensorIndex; + + bool setupModel(); + + bool setupOrientationSensors(); + + bool setupEncoders(); + + public: + + bool setup(BipedalLocomotionControllers::ParametersHandler::YarpImplementation::shared_ptr group, + std::shared_ptr commonDataPtr); + + void reset(); + + bool close(); + }; + + enum class State + { + STARTED, + PREPARED, + FIRST_RUN, + RUNNING + }; + BipedalLocomotionControllers::ParametersHandler::YarpImplementation::unique_ptr m_parametersPtr; + std::shared_ptr m_commonDataPtr; + double m_period; + MasImuData m_leftIMU, m_rightIMU; + State m_state{State::STARTED}; + std::mutex m_mutex; + yarp::os::Port m_rpcPort; /**< Remote Procedure Call port. */ + - bool configureRobot(); + void reset(); public: /** * Get the period of the RFModule. - * @return the period of the module. + * @return the period of the module in seconds. */ double getPeriod() override; diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index 8b853d933c..54d0ffc53c 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -6,43 +6,405 @@ */ #include +#include +#include +#include +#include +#include +#include +#include using namespace BipedalLocomotionControllers; +bool MasImuTest::MasImuData::setupModel() +{ + bool ok = m_group->getParameter("imu_frame", m_frameName); + if (!ok) + { + yError() << "[MasImuTest::MasImuData::setupModel] Setup failed."; + return false; + } + + m_frame = m_commonDataPtr->fullModel.getFrameIndex(m_frameName); + + if (m_frame == iDynTree::FRAME_INVALID_INDEX) + { + yError() << "[MasImuTest::MasImuData::setupModel] The frame " << m_frameName << " does not exists in the robot model." + << ". Configuration failed."; + return false; + } + + m_link = m_commonDataPtr->fullModel.getFrameLink(m_frame); + assert(m_link != iDynTree::LINK_INVALID_INDEX); + + m_consideredJointIndexes.clear(); + m_consideredJointNames.clear(); + iDynTree::LinkIndex baseLinkIndex = m_commonDataPtr->traversal.getBaseLink()->getIndex(); + iDynTree::LinkIndex currentLink = m_link; + while (currentLink != baseLinkIndex) { + const iDynTree::IJoint* joint = m_commonDataPtr->traversal.getParentJointFromLinkIndex(currentLink); + assert(joint); + m_consideredJointIndexes.push_back(joint->getIndex()); + m_consideredJointNames.push_back(m_commonDataPtr->fullModel.getJointName(m_consideredJointIndexes.back())); + currentLink = m_commonDataPtr->traversal.getParentLinkFromLinkIndex(currentLink)->getIndex(); + } -bool MasImuTest::configureRobot() + iDynTree::ModelLoader reducedModelLoader; + ok = reducedModelLoader.loadReducedModelFromFullModel(m_commonDataPtr->fullModel, m_consideredJointNames); + + if (!ok) + { + yError() << "[MasImuTest::MasImuData::setupModel] Failed to build the reduced model. Configuration failed."; + return false; + } + + ok = m_kinDyn.loadRobotModel(reducedModelLoader.model()); + + if (!ok) + { + yError() << "[MasImuTest::MasImuData::setupModel] Failed to load the reduced model. Configuration failed."; + return false; + } + + return true; +} + +bool MasImuTest::MasImuData::setupOrientationSensors() { + std::string remote; + bool ok = m_group->getParameter("remote",remote); + if (!ok) + { + yError() << "[MasImuTest::MasImuData::setupOrientationSensors] Setup failed."; + return false; + } + + yarp::os::Property inertialClientProperty; + inertialClientProperty.put("remote", "/" + m_commonDataPtr->robotName + "/" + remote); + inertialClientProperty.put("local", "/" + m_commonDataPtr->prefix + "/" + remote); + inertialClientProperty.put("device","multipleanalogsensorsclient"); + + if (!m_orientationDriver.open(inertialClientProperty)) + { + yError() << "[MasImuTest::MasImuData::setupOrientationSensors] Failed to open multipleanalogsensorsclient on remote " + << remote << ". Setup failed."; + return false; + } + + if (!m_orientationDriver.view(m_orientationInterface) || !m_orientationInterface) + { + yError() << "[MasImuTest::MasImuData::setupOrientationSensors] Failed to open multipleanalogsensorsclient on remote " + << remote << ". Setup failed."; + return false; + } + + m_sensorIndex = 0; + std::string name; + bool found = false; + do + { + bool ok = m_orientationInterface->getOrientationSensorFrameName(m_sensorIndex, name); + if (ok) + { + found = name == m_frameName; + if (!found) + { + m_sensorIndex++; + } + } + } + while (ok && (m_sensorIndex < m_orientationInterface->getNrOfOrientationSensors()) && !found); + + if (!found) + { + yError() << "[MasImuTest::MasImuData::setupOrientationSensors] The interface contains no orientation sensors on frame " + << m_frameName << ". Setup failed."; + return false; + } + + return true; } -double MasImuTest::getPeriod() +bool MasImuTest::MasImuData::setupEncoders() +{ + std::vector inputControlBoards; + bool ok = m_group->getParameter("remote_control_boards", inputControlBoards); + if (!ok) + { + yError() << "[MasImuTest::MasImuData::setupEncoders] Setup failed."; + return false; + } + + // open the remotecontrolboardremepper YARP device + yarp::os::Property remapperOptions; + remapperOptions.put("device", "remotecontrolboardremapper"); + + YarpUtilities::addVectorOfStringToProperty(remapperOptions, "axesNames", m_consideredJointNames); + + // prepare the remotecontrolboards + yarp::os::Bottle remoteControlBoardsYarp; + yarp::os::Bottle& remoteControlBoardsYarpList = remoteControlBoardsYarp.addList(); + for (auto& rcb : inputControlBoards) + remoteControlBoardsYarpList.addString("/" + m_commonDataPtr->robotName + "/" + rcb); + + remapperOptions.put("remoteControlBoards", remoteControlBoardsYarp.get(0)); + remapperOptions.put("localPortPrefix", "/" + m_commonDataPtr->prefix + "/remoteControlBoard"); + + // open the device + if (!m_robotDriver.open(remapperOptions)) + { + yError() << "[MasImuTest::MasImuData::setupEncoders] Could not open remotecontrolboardremapper object. Setup failed."; + return false; + } + + if(!m_robotDriver.view(m_encodersInterface) || !m_encodersInterface) + { + yError() << "[MasImuTest::MasImuData::setupEncoders] Cannot obtain IEncoders interface. Setup failed."; + return false; + } + + return true; +} + +bool MasImuTest::MasImuData::setup(ParametersHandler::YarpImplementation::shared_ptr group, + std::shared_ptr commonDataPtr) +{ + + m_commonDataPtr = commonDataPtr; + m_group = group; + + bool ok = setupModel(); + if (!ok) + { + yError() << "[MasImuTest::MasImuData::setup] setupModel failed."; + return false; + } + + ok = setupOrientationSensors(); + if (!ok) + { + yError() << "[MasImuTest::MasImuData::setup] setupOrientationSensors failed."; + return false; + } + + ok = setupEncoders(); + if (!ok) + { + yError() << "[MasImuTest::MasImuData::setup] setupEncoders failed."; + return false; + } + + return true; +} + +void MasImuTest::MasImuData::reset() +{ + +} + +bool MasImuTest::MasImuData::close() { + if(!m_orientationDriver.close()) + { + yError() << "[MasImuTest::MasImuData::close] Unable to close the orientation driver."; + return false; + } + + if(!m_robotDriver.close()) + { + yError() << "[MasImuTest::MasImuData::close] Unable to close the robot driver."; + return false; + } +} + +void MasImuTest::reset() +{ + m_state = State::PREPARED; + m_leftIMU.reset(); + m_rightIMU.reset(); +} + +double MasImuTest::getPeriod() +{ + std::lock_guard lock(m_mutex); + return m_period; } bool MasImuTest::updateModule() { + std::lock_guard lock(m_mutex); + + if (m_state == State::RUNNING) + { + + } + + if (m_state == State::FIRST_RUN) + { + + m_state = State::RUNNING; + } + return true; } bool MasImuTest::configure(yarp::os::ResourceFinder &rf) { - m_parametersPtr = std::make_unique(rf); + std::lock_guard lock(m_mutex); + + m_parametersPtr = BipedalLocomotionControllers::ParametersHandler::YarpImplementation::make_unique(rf); + m_commonDataPtr = std::make_shared(); + + bool ok = m_parametersPtr->getParameter("name", m_commonDataPtr->prefix); + if (!ok) + { + yError() << "[MasImuTest::configure] Configuration failed."; + return false; + } + + ok = m_parametersPtr->getParameter("period", m_period); + if (!ok) + { + yError() << "[MasImuTest::configure] Configuration failed."; + return false; + } + + if (m_period < 0) + { + yError() << "[MasImuTest::configure] The period cannot be negative. Configuration failed."; + return false; + } + ok = m_parametersPtr->getParameter("robot", m_commonDataPtr->robotName); + if (!ok) + { + yError() << "[MasImuTest::configure] Configuration failed."; + return false; + } + std::string robotModelName; + ok = m_parametersPtr->getParameter("model", robotModelName); + if (!ok) + { + yError() << "[MasImuTest::configure] Configuration failed."; + return false; + } + std::string pathToModel = yarp::os::ResourceFinder::getResourceFinderSingleton().findFileByName(robotModelName); + iDynTree::ModelLoader modelLoader; + if (!modelLoader.loadModelFromFile(pathToModel)) + { + yError() << "[MasImuTest::configure] Configuration failed."; + return false; + } + + m_commonDataPtr->fullModel = modelLoader.model(); + + std::string baseLink; + ok = m_parametersPtr->getParameter("base_link", baseLink); + if (!ok) + { + yError() << "[MasImuTest::configure] Configuration failed."; + return false; + } + + iDynTree::LinkIndex baseLinkIndex = m_commonDataPtr->fullModel.getLinkIndex(baseLink); + if (baseLinkIndex == iDynTree::LINK_INVALID_INDEX) + { + yError() << "[MasImuTest::configure] The link " << baseLink << " does not exists in " << robotModelName + << ". Configuration failed."; + return false; + } + + ok = m_commonDataPtr->fullModel.computeFullTreeTraversal(m_commonDataPtr->traversal, baseLinkIndex); + + if (!ok) + { + yError() << "[MasImuTest::configure] Failed to build the traversal. Configuration failed."; + return false; + } + + if(!iDynTree::parseRotationMatrix(rf, "base_rotation", m_commonDataPtr->baseRotation)) + { + m_commonDataPtr->baseRotation = iDynTree::Rotation::Identity(); + yInfo() << "Using the identity as desired rotation for the additional frame"; + } + + ok = m_parametersPtr->getParameter("filter_yaw", m_commonDataPtr->filterYaw); + if (!ok) + { + yError() << "[MasImuTest::configure] Configuration failed."; + return false; + } + + auto leftLegGroup = m_parametersPtr->getGroup("LEFT_LEG").lock(); + if (!leftLegGroup) + { + yError() << "[MasImuTest::configure] LEFT_LEG group not available. Configuration failed."; + return false; + } + + m_leftIMU.setup(leftLegGroup, m_commonDataPtr); + + auto rightLegGroup = m_parametersPtr->getGroup("RIGHT_LEG").lock(); + if (!leftLegGroup) + { + yError() << "[MasImuTest::configure] RIGHT_LEG group not available. Configuration failed."; + return false; + } + + m_rightIMU.setup(rightLegGroup, m_commonDataPtr); + + // open RPC port for external command + std::string rpcPortName = "/" + m_commonDataPtr->prefix + "/rpc"; + this->yarp().attachAsServer(this->m_rpcPort); + if(!m_rpcPort.open(rpcPortName)) + { + yError() << "[MasImuTest::configure] Could not open" << rpcPortName << " RPC port."; + return false; + } + + m_state = State::PREPARED; + + return true; } bool MasImuTest::close() { + std::lock_guard lock(m_mutex); + bool okL = m_leftIMU.close(); + if (!okL) + { + yError() << "Failed to close left leg part."; + } + bool okR = m_rightIMU.close(); + if (!okR) + { + yError() << "Failed to close right leg part."; + } + + return okL && okR; } bool MasImuTest::startTest() { + std::lock_guard lock(m_mutex); + if (m_state == State::PREPARED) + { + reset(); + m_state = State::FIRST_RUN; + return true; + } + + return false; } void MasImuTest::stopTest() { - + std::lock_guard lock(m_mutex); + m_state = State::PREPARED; } + From f7d69ce185d057d152170f451f26609980578f6c Mon Sep 17 00:00:00 2001 From: Stefano Date: Sat, 7 Mar 2020 14:52:50 +0100 Subject: [PATCH 18/51] Updated draft of mas imu test. Added methods to compute IMU world. --- .../robots/iCubGenova04/MasImuTestConfig.ini | 2 + .../BipedalLocomotionControllers/MasImuTest.h | 20 ++- utilities/MasImuTest/src/MasImuTest.cpp | 139 +++++++++++++++++- 3 files changed, 157 insertions(+), 4 deletions(-) diff --git a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini index 1e97be7e3a..9a4cd94318 100644 --- a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini +++ b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini @@ -13,6 +13,8 @@ base_rotation ((-1.0 0.0 0.0),(0.0 -1.0 0.0),(0.0 0.0 1.0)) filter_yaw 1 +max_samples 100 + [LEFT_LEG] remote left_leg/inertials diff --git a/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h b/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h index 8a788b5c33..620b3341c5 100644 --- a/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h +++ b/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h @@ -25,6 +25,8 @@ #include #include #include +#include +#include //Thrifts #include @@ -44,8 +46,9 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ std::string prefix; iDynTree::Model fullModel; iDynTree::Traversal traversal; - iDynTree::Rotation baseRotation; + iDynTree::Transform baseTransform; bool filterYaw; + int maxSamples; }; class MasImuData @@ -62,6 +65,15 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ yarp::dev::IOrientationSensors* m_orientationInterface; yarp::dev::IEncodersTimed* m_encodersInterface; size_t m_sensorIndex; + std::vector m_data; + yarp::sig::Vector m_positionFeedbackDeg; /**< Current joint position [deg]. */ + yarp::sig::Vector m_rpyInDeg; + iDynTree::VectorDynSize m_positionFeedbackInRad; + iDynTree::VectorDynSize m_dummyVelocity; + iDynTree::Rotation m_rotationFeedback; + iDynTree::Rotation m_rotationFromEncoders; + iDynTree::Rotation m_imuWorld; //i_R_imuworld + bool setupModel(); @@ -69,11 +81,17 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ bool setupEncoders(); + bool getFeedback(); + + bool updateRotationFromEncoders(); + public: bool setup(BipedalLocomotionControllers::ParametersHandler::YarpImplementation::shared_ptr group, std::shared_ptr commonDataPtr); + bool setImuWorld(); + void reset(); bool close(); diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index 54d0ffc53c..2dbd1b9661 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -124,6 +125,8 @@ bool MasImuTest::MasImuData::setupOrientationSensors() return false; } + m_rpyInDeg.resize(3); + return true; } @@ -165,6 +168,88 @@ bool MasImuTest::MasImuData::setupEncoders() return false; } + m_positionFeedbackDeg.resize(m_consideredJointNames.size()); + m_positionFeedbackInRad.resize(m_consideredJointNames.size()); + m_dummyVelocity.resize(m_consideredJointNames.size()); + m_dummyVelocity.zero(); + + return true; +} + +bool MasImuTest::MasImuData::getFeedback() +{ + size_t maxAttempts = 100; + + size_t attempt = 0; + bool okEncoders = false; + bool okIMU = false; + + do + { + if (!okEncoders) + okEncoders = m_encodersInterface->getEncoders(m_positionFeedbackDeg.data()); + + if (!okIMU) + { + yarp::dev::MAS_status status = m_orientationInterface->getOrientationSensorStatus(m_sensorIndex); + if (status == yarp::dev::MAS_status::MAS_OK) + { + double timestamp; + okIMU = m_orientationInterface->getOrientationSensorMeasureAsRollPitchYaw(m_sensorIndex, m_rpyInDeg, timestamp); + } + } + + if (okEncoders && okIMU) + { + for(unsigned j = 0 ; j < m_positionFeedbackDeg.size(); j++) + { + m_positionFeedbackInRad(j) = iDynTree::deg2rad(m_positionFeedbackDeg(j)); + } + + m_rotationFeedback = iDynTree::Rotation::RPY(iDynTree::deg2rad(m_rpyInDeg[0]), + iDynTree::deg2rad(m_rpyInDeg[1]), + iDynTree::deg2rad(m_rpyInDeg[2])); + + return true; + } + + yarp::os::Time::delay(0.001); + attempt++; + } + while(attempt < maxAttempts); + + yError() << "[MasImuTest::MasImuData::getFeedback] The following readings failed:"; + if(!okEncoders) + yError() << "\t - Position encoders"; + + if (!okIMU) + yError() << "\t - IMU"; + + return false; +} + +bool MasImuTest::MasImuData::updateRotationFromEncoders() +{ + + iDynTree::Twist dummy; + dummy.zero(); + + iDynTree::Vector3 gravity; + gravity(0) = 0.0; + gravity(1) = 0.0; + gravity(2) = -9.81; + + bool ok = m_kinDyn.setRobotState(m_commonDataPtr->baseTransform, m_positionFeedbackInRad, dummy, m_dummyVelocity, gravity); + + if (!ok) + { + yError() << "[MasImuTest::MasImuData::updateRotationFromEncoders] Failed to set the state in kinDyn object."; + return false; + } + + iDynTree::Transform frameTransform = m_kinDyn.getWorldTransform(m_frame); + m_rotationFromEncoders = frameTransform.getRotation(); + return true; } @@ -175,6 +260,8 @@ bool MasImuTest::MasImuData::setup(ParametersHandler::YarpImplementation::shared m_commonDataPtr = commonDataPtr; m_group = group; + m_data.reserve(m_commonDataPtr->maxSamples); + bool ok = setupModel(); if (!ok) { @@ -199,9 +286,24 @@ bool MasImuTest::MasImuData::setup(ParametersHandler::YarpImplementation::shared return true; } -void MasImuTest::MasImuData::reset() +bool MasImuTest::MasImuData::setImuWorld() { + bool ok = getFeedback(); + if (!ok) + return false; + + ok = updateRotationFromEncoders(); + if (!ok) + return false; + + m_imuWorld = m_rotationFromEncoders * m_rotationFeedback.inverse(); + + return true; +} +void MasImuTest::MasImuData::reset() +{ + m_data.clear(); } bool MasImuTest::MasImuData::close() @@ -244,7 +346,20 @@ bool MasImuTest::updateModule() if (m_state == State::FIRST_RUN) { + bool ok = m_leftIMU.setImuWorld(); + + if (!ok) + { + yError() << "Failed to set left IMU world."; + return false; + } + ok = m_rightIMU.setImuWorld(); + if (!ok) + { + yError() << "Failed to set right IMU world."; + return false; + } m_state = State::RUNNING; } @@ -325,12 +440,23 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) return false; } - if(!iDynTree::parseRotationMatrix(rf, "base_rotation", m_commonDataPtr->baseRotation)) + iDynTree::Rotation baseRotation; + if(!iDynTree::parseRotationMatrix(rf, "base_rotation", baseRotation)) { - m_commonDataPtr->baseRotation = iDynTree::Rotation::Identity(); + baseRotation = iDynTree::Rotation::Identity(); yInfo() << "Using the identity as desired rotation for the additional frame"; } + ok = iDynTree::isValidRotationMatrix(baseRotation); + if (!ok) + { + yError() << "[MasImuTest::configure] The specified base rotation is not a rotation matrix."; + return false; + } + + m_commonDataPtr->baseTransform = iDynTree::Transform::Identity(); + m_commonDataPtr->baseTransform.setRotation(baseRotation); + ok = m_parametersPtr->getParameter("filter_yaw", m_commonDataPtr->filterYaw); if (!ok) { @@ -338,6 +464,13 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) return false; } + ok = m_parametersPtr->getParameter("max_samples", m_commonDataPtr->maxSamples); + if (!ok || m_commonDataPtr->maxSamples < 0) + { + yError() << "[MasImuTest::configure] Configuration failed while reading \"max_samples\"."; + return false; + } + auto leftLegGroup = m_parametersPtr->getGroup("LEFT_LEG").lock(); if (!leftLegGroup) { From b8a7de71e8b3cbc328d0f945ba78e8ae9cd5dc0d Mon Sep 17 00:00:00 2001 From: Stefano Date: Sun, 8 Mar 2020 19:27:27 +0100 Subject: [PATCH 19/51] Added code to add data in the mas imu test. --- .gitignore | 1 + .../robots/iCubGenova04/MasImuTestConfig.ini | 2 + .../BipedalLocomotionControllers/MasImuTest.h | 10 +- utilities/MasImuTest/src/MasImuTest.cpp | 117 +++++++++++++++++- 4 files changed, 126 insertions(+), 4 deletions(-) diff --git a/.gitignore b/.gitignore index b26f0578a4..e42da90a78 100644 --- a/.gitignore +++ b/.gitignore @@ -7,3 +7,4 @@ build/* # Qtcreator *.user* +*.autosave diff --git a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini index 9a4cd94318..9a765d6072 100644 --- a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini +++ b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini @@ -13,6 +13,8 @@ base_rotation ((-1.0 0.0 0.0),(0.0 -1.0 0.0),(0.0 0.0 1.0)) filter_yaw 1 +min_joint_variation_deg 2.0 + max_samples 100 [LEFT_LEG] diff --git a/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h b/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h index 620b3341c5..859b9521f8 100644 --- a/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h +++ b/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h @@ -49,6 +49,7 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ iDynTree::Transform baseTransform; bool filterYaw; int maxSamples; + double minJointVariationRad; }; class MasImuData @@ -69,6 +70,7 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ yarp::sig::Vector m_positionFeedbackDeg; /**< Current joint position [deg]. */ yarp::sig::Vector m_rpyInDeg; iDynTree::VectorDynSize m_positionFeedbackInRad; + iDynTree::VectorDynSize m_previousPositionFeedbackInRad; iDynTree::VectorDynSize m_dummyVelocity; iDynTree::Rotation m_rotationFeedback; iDynTree::Rotation m_rotationFromEncoders; @@ -85,12 +87,18 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ bool updateRotationFromEncoders(); + double maxVariation(); + public: bool setup(BipedalLocomotionControllers::ParametersHandler::YarpImplementation::shared_ptr group, std::shared_ptr commonDataPtr); - bool setImuWorld(); + bool firstRun(); + + bool addSample(bool &maxSamplesReached); + + size_t addedSamples() const; void reset(); diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index 2dbd1b9661..670ae60c33 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -14,6 +14,7 @@ #include #include #include +#include using namespace BipedalLocomotionControllers; @@ -170,6 +171,7 @@ bool MasImuTest::MasImuData::setupEncoders() m_positionFeedbackDeg.resize(m_consideredJointNames.size()); m_positionFeedbackInRad.resize(m_consideredJointNames.size()); + m_previousPositionFeedbackInRad.resize(m_consideredJointNames.size()); m_dummyVelocity.resize(m_consideredJointNames.size()); m_dummyVelocity.zero(); @@ -253,6 +255,24 @@ bool MasImuTest::MasImuData::updateRotationFromEncoders() return true; } +double MasImuTest::MasImuData::maxVariation() +{ + // clear the std::pair + double maxVariation = 0; + double jointVariation; + + for (unsigned int i = 0; i < m_positionFeedbackInRad.size(); i++) + { + jointVariation = std::fabs(m_positionFeedbackInRad(i) - m_previousPositionFeedbackInRad(i)); + if (jointVariation > maxVariation) + { + maxVariation = jointVariation; + } + } + + return maxVariation; +} + bool MasImuTest::MasImuData::setup(ParametersHandler::YarpImplementation::shared_ptr group, std::shared_ptr commonDataPtr) { @@ -286,7 +306,7 @@ bool MasImuTest::MasImuData::setup(ParametersHandler::YarpImplementation::shared return true; } -bool MasImuTest::MasImuData::setImuWorld() +bool MasImuTest::MasImuData::firstRun() { bool ok = getFeedback(); if (!ok) @@ -298,9 +318,60 @@ bool MasImuTest::MasImuData::setImuWorld() m_imuWorld = m_rotationFromEncoders * m_rotationFeedback.inverse(); + m_previousPositionFeedbackInRad = m_positionFeedbackInRad; + + return true; +} + +bool MasImuTest::MasImuData::addSample(bool& maxSamplesReached) +{ + bool ok = getFeedback(); + if (!ok) + return false; + + if (maxVariation() < m_commonDataPtr->minJointVariationRad) + { + return true; + } + + if (static_cast(addedSamples()) >= m_commonDataPtr->maxSamples) + { + maxSamplesReached = true; + return true; + } + maxSamplesReached = false; + + ok = updateRotationFromEncoders(); + if (!ok) + return false; + + iDynTree::Rotation measuredImu = m_imuWorld * m_rotationFeedback; + + if (m_commonDataPtr->filterYaw) + { + double measuredRoll, measuredPitch, measuredYaw; + measuredImu.getRPY(measuredRoll, measuredPitch, measuredYaw); + + double estimatedRoll, estimatedPitch, estimatedYaw; + m_rotationFromEncoders.getRPY(estimatedRoll, estimatedPitch, estimatedYaw); + + measuredImu = iDynTree::Rotation::RPY(measuredRoll, measuredPitch, estimatedYaw); + } + + iDynTree::Rotation error = m_rotationFromEncoders.inverse() * measuredImu; + + m_data.push_back(error); + + m_previousPositionFeedbackInRad = m_positionFeedbackInRad; + return true; } +size_t MasImuTest::MasImuData::addedSamples() const +{ + return m_data.size(); +} + void MasImuTest::MasImuData::reset() { m_data.clear(); @@ -341,12 +412,42 @@ bool MasImuTest::updateModule() if (m_state == State::RUNNING) { + bool maxSampleReachedLeft, maxSampleReachedRight; + + bool ok = m_leftIMU.addSample(maxSampleReachedLeft); + if (!ok) + { + yError() << "[MasImuTest::updateModule] Failed to add data for left IMU."; + return false; + } + + if (maxSampleReachedLeft) + { + yWarning() << "[MasImuTest::updateModule] Data limit reached for the left leg."; + } + + ok = m_rightIMU.addSample(maxSampleReachedRight); + if (!ok) + { + yError() << "[MasImuTest::updateModule] Failed to add data for right IMU."; + return false; + } + + if (maxSampleReachedRight) + { + yWarning() << "[MasImuTest::updateModule] Data limit reached for the right leg."; + } + + if (maxSampleReachedLeft && maxSampleReachedRight) + { + //DO something to print the results. + } } if (m_state == State::FIRST_RUN) { - bool ok = m_leftIMU.setImuWorld(); + bool ok = m_leftIMU.firstRun(); if (!ok) { @@ -354,12 +455,13 @@ bool MasImuTest::updateModule() return false; } - ok = m_rightIMU.setImuWorld(); + ok = m_rightIMU.firstRun(); if (!ok) { yError() << "Failed to set right IMU world."; return false; } + m_state = State::RUNNING; } @@ -464,6 +566,15 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) return false; } + double minJointVariationInDeg; + ok = m_parametersPtr->getParameter("min_joint_variation_deg", minJointVariationInDeg); + if (!ok) + { + yError() << "[MasImuTest::configure] Configuration failed."; + return false; + } + m_commonDataPtr->minJointVariationRad = iDynTree::deg2rad(minJointVariationInDeg); + ok = m_parametersPtr->getParameter("max_samples", m_commonDataPtr->maxSamples); if (!ok || m_commonDataPtr->maxSamples < 0) { From ea90a1276f49822a8b5c0fb075d8fb87515cb6c0 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 9 Mar 2020 15:56:00 +0100 Subject: [PATCH 20/51] Completed MasImuTest draft. --- .../BipedalLocomotionControllers/MasImuTest.h | 22 +- utilities/MasImuTest/src/MasImuTest.cpp | 224 +++++++++++++----- .../thrifts/MasImuTestCommands.thrift | 7 +- 3 files changed, 184 insertions(+), 69 deletions(-) diff --git a/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h b/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h index 859b9521f8..59c6f64fa9 100644 --- a/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h +++ b/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h @@ -54,6 +54,7 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ class MasImuData { + std::string m_testName; std::shared_ptr m_commonDataPtr; BipedalLocomotionControllers::ParametersHandler::YarpImplementation::shared_ptr m_group; iDynTree::FrameIndex m_frame; @@ -75,6 +76,7 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ iDynTree::Rotation m_rotationFeedback; iDynTree::Rotation m_rotationFromEncoders; iDynTree::Rotation m_imuWorld; //i_R_imuworld + bool m_completed{false}; bool setupModel(); @@ -91,15 +93,22 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ public: - bool setup(BipedalLocomotionControllers::ParametersHandler::YarpImplementation::shared_ptr group, + bool setup(const std::string& testName, + BipedalLocomotionControllers::ParametersHandler::YarpImplementation::shared_ptr group, std::shared_ptr commonDataPtr); bool firstRun(); - bool addSample(bool &maxSamplesReached); + bool addSample(); size_t addedSamples() const; + bool isCompleted() const; + + void setCompleted(); + + void printResults() const; + void reset(); bool close(); @@ -124,6 +133,8 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ void reset(); + void printResultsPrivate() const; + public: /** @@ -160,7 +171,12 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ /** * Stop the test */ - void stopTest() override; + bool stopTest() override; + + /** + * Print the results. This works only if the test has already been stopped. + */ + bool printResults() override; }; #endif // BIPEDAL_LOCOMOTION_CONTROLLERS_MASIMUTEST_H diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index 670ae60c33..9890064263 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -20,10 +20,12 @@ using namespace BipedalLocomotionControllers; bool MasImuTest::MasImuData::setupModel() { + std::string errorPrefix = "[MasImuTest::MasImuData::setupModel](" + m_testName +") "; + bool ok = m_group->getParameter("imu_frame", m_frameName); if (!ok) { - yError() << "[MasImuTest::MasImuData::setupModel] Setup failed."; + yError() << errorPrefix << "Setup failed."; return false; } @@ -31,7 +33,7 @@ bool MasImuTest::MasImuData::setupModel() if (m_frame == iDynTree::FRAME_INVALID_INDEX) { - yError() << "[MasImuTest::MasImuData::setupModel] The frame " << m_frameName << " does not exists in the robot model." + yError() << errorPrefix << "The frame " << m_frameName << " does not exists in the robot model." << ". Configuration failed."; return false; } @@ -57,7 +59,7 @@ bool MasImuTest::MasImuData::setupModel() if (!ok) { - yError() << "[MasImuTest::MasImuData::setupModel] Failed to build the reduced model. Configuration failed."; + yError() << errorPrefix << "Failed to build the reduced model. Configuration failed."; return false; } @@ -65,7 +67,7 @@ bool MasImuTest::MasImuData::setupModel() if (!ok) { - yError() << "[MasImuTest::MasImuData::setupModel] Failed to load the reduced model. Configuration failed."; + yError() << errorPrefix << "Failed to load the reduced model. Configuration failed."; return false; } @@ -74,11 +76,12 @@ bool MasImuTest::MasImuData::setupModel() bool MasImuTest::MasImuData::setupOrientationSensors() { + std::string errorPrefix = "[MasImuTest::MasImuData::setupOrientationSensors](" + m_testName +") "; std::string remote; bool ok = m_group->getParameter("remote",remote); if (!ok) { - yError() << "[MasImuTest::MasImuData::setupOrientationSensors] Setup failed."; + yError() << errorPrefix << "Setup failed."; return false; } @@ -89,14 +92,14 @@ bool MasImuTest::MasImuData::setupOrientationSensors() if (!m_orientationDriver.open(inertialClientProperty)) { - yError() << "[MasImuTest::MasImuData::setupOrientationSensors] Failed to open multipleanalogsensorsclient on remote " + yError() << errorPrefix << "Failed to open multipleanalogsensorsclient on remote " << remote << ". Setup failed."; return false; } if (!m_orientationDriver.view(m_orientationInterface) || !m_orientationInterface) { - yError() << "[MasImuTest::MasImuData::setupOrientationSensors] Failed to open multipleanalogsensorsclient on remote " + yError() << errorPrefix << "Failed to open multipleanalogsensorsclient on remote " << remote << ". Setup failed."; return false; } @@ -121,7 +124,7 @@ bool MasImuTest::MasImuData::setupOrientationSensors() if (!found) { - yError() << "[MasImuTest::MasImuData::setupOrientationSensors] The interface contains no orientation sensors on frame " + yError() << errorPrefix << "The interface contains no orientation sensors on frame " << m_frameName << ". Setup failed."; return false; } @@ -133,11 +136,13 @@ bool MasImuTest::MasImuData::setupOrientationSensors() bool MasImuTest::MasImuData::setupEncoders() { + std::string errorPrefix = "[MasImuTest::MasImuData::setupEncoders](" + m_testName +") "; + std::vector inputControlBoards; bool ok = m_group->getParameter("remote_control_boards", inputControlBoards); if (!ok) { - yError() << "[MasImuTest::MasImuData::setupEncoders] Setup failed."; + yError() << errorPrefix << "Setup failed."; return false; } @@ -159,13 +164,13 @@ bool MasImuTest::MasImuData::setupEncoders() // open the device if (!m_robotDriver.open(remapperOptions)) { - yError() << "[MasImuTest::MasImuData::setupEncoders] Could not open remotecontrolboardremapper object. Setup failed."; + yError() << errorPrefix << "Could not open remotecontrolboardremapper object. Setup failed."; return false; } if(!m_robotDriver.view(m_encodersInterface) || !m_encodersInterface) { - yError() << "[MasImuTest::MasImuData::setupEncoders] Cannot obtain IEncoders interface. Setup failed."; + yError() << errorPrefix << "Cannot obtain IEncoders interface. Setup failed."; return false; } @@ -180,6 +185,8 @@ bool MasImuTest::MasImuData::setupEncoders() bool MasImuTest::MasImuData::getFeedback() { + std::string errorPrefix = "[MasImuTest::MasImuData::getFeedback](" + m_testName +") "; + size_t maxAttempts = 100; size_t attempt = 0; @@ -220,7 +227,7 @@ bool MasImuTest::MasImuData::getFeedback() } while(attempt < maxAttempts); - yError() << "[MasImuTest::MasImuData::getFeedback] The following readings failed:"; + yError() << errorPrefix << "The following readings failed:"; if(!okEncoders) yError() << "\t - Position encoders"; @@ -232,6 +239,7 @@ bool MasImuTest::MasImuData::getFeedback() bool MasImuTest::MasImuData::updateRotationFromEncoders() { + std::string errorPrefix = "[MasImuTest::MasImuData::updateRotationFromEncoders](" + m_testName +") "; iDynTree::Twist dummy; dummy.zero(); @@ -245,7 +253,7 @@ bool MasImuTest::MasImuData::updateRotationFromEncoders() if (!ok) { - yError() << "[MasImuTest::MasImuData::updateRotationFromEncoders] Failed to set the state in kinDyn object."; + yError() << errorPrefix << "Failed to set the state in kinDyn object."; return false; } @@ -273,33 +281,35 @@ double MasImuTest::MasImuData::maxVariation() return maxVariation; } -bool MasImuTest::MasImuData::setup(ParametersHandler::YarpImplementation::shared_ptr group, +bool MasImuTest::MasImuData::setup(const std::string &testName, ParametersHandler::YarpImplementation::shared_ptr group, std::shared_ptr commonDataPtr) { - + m_testName = testName; m_commonDataPtr = commonDataPtr; m_group = group; m_data.reserve(m_commonDataPtr->maxSamples); + std::string errorPrefix = "[MasImuTest::MasImuData::setup](" + m_testName +") "; + bool ok = setupModel(); if (!ok) { - yError() << "[MasImuTest::MasImuData::setup] setupModel failed."; + yError() << errorPrefix << "setupModel failed."; return false; } ok = setupOrientationSensors(); if (!ok) { - yError() << "[MasImuTest::MasImuData::setup] setupOrientationSensors failed."; + yError() << errorPrefix << "setupOrientationSensors failed."; return false; } ok = setupEncoders(); if (!ok) { - yError() << "[MasImuTest::MasImuData::setup] setupEncoders failed."; + yError() << errorPrefix << "setupEncoders failed."; return false; } @@ -308,13 +318,21 @@ bool MasImuTest::MasImuData::setup(ParametersHandler::YarpImplementation::shared bool MasImuTest::MasImuData::firstRun() { + std::string errorPrefix = "[MasImuTest::MasImuData::firstRun](" + m_testName +") "; + bool ok = getFeedback(); if (!ok) + { + yError() << errorPrefix << "Failed to get feedbacks."; return false; + } ok = updateRotationFromEncoders(); if (!ok) + { + yError() << errorPrefix << "updateRotationFromEncoders() failed."; return false; + } m_imuWorld = m_rotationFromEncoders * m_rotationFeedback.inverse(); @@ -323,27 +341,33 @@ bool MasImuTest::MasImuData::firstRun() return true; } -bool MasImuTest::MasImuData::addSample(bool& maxSamplesReached) +bool MasImuTest::MasImuData::addSample() { + if (isCompleted()) + { + return true; + } + + std::string errorPrefix = "[MasImuTest::MasImuData::addSample](" + m_testName +") "; + bool ok = getFeedback(); if (!ok) - return false; - - if (maxVariation() < m_commonDataPtr->minJointVariationRad) { - return true; + yError() << errorPrefix << "Failed to get feedbacks."; + return false; } - if (static_cast(addedSamples()) >= m_commonDataPtr->maxSamples) + if (maxVariation() < m_commonDataPtr->minJointVariationRad) { - maxSamplesReached = true; return true; } - maxSamplesReached = false; ok = updateRotationFromEncoders(); if (!ok) + { + yError() << errorPrefix << "updateRotationFromEncoders() failed."; return false; + } iDynTree::Rotation measuredImu = m_imuWorld * m_rotationFeedback; @@ -364,6 +388,13 @@ bool MasImuTest::MasImuData::addSample(bool& maxSamplesReached) m_previousPositionFeedbackInRad = m_positionFeedbackInRad; + yInfo() << errorPrefix << "Sample " << addedSamples() << "/" << m_commonDataPtr->maxSamples; + + if (static_cast(addedSamples()) >= m_commonDataPtr->maxSamples) + { + setCompleted(); + } + return true; } @@ -372,22 +403,57 @@ size_t MasImuTest::MasImuData::addedSamples() const return m_data.size(); } +bool MasImuTest::MasImuData::isCompleted() const +{ + return m_completed; +} + +void MasImuTest::MasImuData::setCompleted() +{ + m_completed = true; +} + +void MasImuTest::MasImuData::printResults() const +{ + std::string errorPrefix = "[MasImuTest::MasImuData::printResults](" + m_testName +") "; + + iDynTree::Rotation meanError; + + iDynTree::GeodesicL2MeanOptions options; + options.maxIterations = 1000; //If it takes so many steps to converge, it is probably in a loop. Hence, this parameter can be safely hardcoded. + + bool ok = iDynTree::geodesicL2MeanRotation(m_data, meanError, options); + if (!ok) + { + yError() << errorPrefix << "Failed to compute the mean rotation."; + return; + } + + yInfo() << errorPrefix << " Mean rotation error ("< lock(m_mutex); @@ -412,54 +484,38 @@ bool MasImuTest::updateModule() if (m_state == State::RUNNING) { - bool maxSampleReachedLeft, maxSampleReachedRight; - bool ok = m_leftIMU.addSample(maxSampleReachedLeft); + bool ok = m_leftIMU.addSample(); if (!ok) { - yError() << "[MasImuTest::updateModule] Failed to add data for left IMU."; - return false; - } - - if (maxSampleReachedLeft) - { - yWarning() << "[MasImuTest::updateModule] Data limit reached for the left leg."; + yError() << "[MasImuTest::updateModule] Failed to add data. Marking it as completed."; + m_leftIMU.setCompleted(); } - ok = m_rightIMU.addSample(maxSampleReachedRight); + ok = m_rightIMU.addSample(); if (!ok) { - yError() << "[MasImuTest::updateModule] Failed to add data for right IMU."; - return false; + yError() << "[MasImuTest::updateModule] Failed to add data. Marking it as completed."; + m_rightIMU.setCompleted(); } - if (maxSampleReachedRight) + if (m_leftIMU.isCompleted() && m_rightIMU.isCompleted()) { - yWarning() << "[MasImuTest::updateModule] Data limit reached for the right leg."; - } - - if (maxSampleReachedLeft && maxSampleReachedRight) - { - //DO something to print the results. + printResultsPrivate(); + m_state = State::PREPARED; } } if (m_state == State::FIRST_RUN) { - bool ok = m_leftIMU.firstRun(); + bool okL = m_leftIMU.firstRun(); + bool okR = m_rightIMU.firstRun(); - if (!ok) + if (!okL || !okR) { - yError() << "Failed to set left IMU world."; - return false; - } - - ok = m_rightIMU.firstRun(); - if (!ok) - { - yError() << "Failed to set right IMU world."; - return false; + yError() << "[MasImuTest::updateModule] Failed to perform first run."; + m_state = State::PREPARED; } m_state = State::RUNNING; @@ -546,7 +602,7 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) if(!iDynTree::parseRotationMatrix(rf, "base_rotation", baseRotation)) { baseRotation = iDynTree::Rotation::Identity(); - yInfo() << "Using the identity as desired rotation for the additional frame"; + yInfo() << "[MasImuTest::configure] Using the identity as desired rotation for the additional frame"; } ok = iDynTree::isValidRotationMatrix(baseRotation); @@ -589,7 +645,12 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) return false; } - m_leftIMU.setup(leftLegGroup, m_commonDataPtr); + ok = m_leftIMU.setup("Left IMU Test", leftLegGroup, m_commonDataPtr); + if (!ok) + { + yError() << "[MasImuTest::configure] Configuration failed."; + return false; + } auto rightLegGroup = m_parametersPtr->getGroup("RIGHT_LEG").lock(); if (!leftLegGroup) @@ -598,7 +659,12 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) return false; } - m_rightIMU.setup(rightLegGroup, m_commonDataPtr); + ok = m_rightIMU.setup("Right IMU Test", rightLegGroup, m_commonDataPtr); + if (!ok) + { + yError() << "[MasImuTest::configure] Configuration failed."; + return false; + } // open RPC port for external command std::string rpcPortName = "/" + m_commonDataPtr->prefix + "/rpc"; @@ -617,15 +683,18 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) bool MasImuTest::close() { std::lock_guard lock(m_mutex); + + m_rpcPort.close(); + bool okL = m_leftIMU.close(); if (!okL) { - yError() << "Failed to close left leg part."; + yError() << "[MasImuTest::close] Failed to close left leg part."; } bool okR = m_rightIMU.close(); if (!okR) { - yError() << "Failed to close right leg part."; + yError() << "[MasImuTest::close] Failed to close right leg part."; } return okL && okR; @@ -642,13 +711,38 @@ bool MasImuTest::startTest() return true; } + yError() << "[MasImuTest::startTest] The module is not ready yet."; + return false; } -void MasImuTest::stopTest() +bool MasImuTest::stopTest() { std::lock_guard lock(m_mutex); - m_state = State::PREPARED; + if (m_state == State::RUNNING) + { + printResultsPrivate(); + m_state = State::PREPARED; + return true; + } + + yError() << "[MasImuTest::startTest] The test is not ready yet."; + + return false; +} + +bool MasImuTest::printResults() +{ + std::lock_guard lock(m_mutex); + if (m_state == State::PREPARED) + { + printResultsPrivate(); + return true; + } + + yError() << "[MasImuTest::startTest] The results can be printed only after stopping the test."; + + return false; } diff --git a/utilities/MasImuTest/thrifts/MasImuTestCommands.thrift b/utilities/MasImuTest/thrifts/MasImuTestCommands.thrift index 01dc36dd58..a2a3fd2bfe 100644 --- a/utilities/MasImuTest/thrifts/MasImuTestCommands.thrift +++ b/utilities/MasImuTest/thrifts/MasImuTestCommands.thrift @@ -16,5 +16,10 @@ service MasImuTestCommands /** * Stop the test */ - void stopTest(); + bool stopTest(); + + /** + * Print the results. This works only if the test has already been stopped. + */ + bool printResults(); } From f0f8d2a3822e1a7f05df82bea98ebde8a7c833fd Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 10 Mar 2020 17:29:09 +0100 Subject: [PATCH 21/51] Updated main of MasImuTest. --- utilities/MasImuTest/src/main.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/utilities/MasImuTest/src/main.cpp b/utilities/MasImuTest/src/main.cpp index a5f92fc4f0..ce80c518e9 100644 --- a/utilities/MasImuTest/src/main.cpp +++ b/utilities/MasImuTest/src/main.cpp @@ -5,10 +5,31 @@ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. */ +// YARP +#include +#include +#include + #include int main(int argc, char * argv[]) { + // initialise yarp network + yarp::os::Network yarp; + if (!yarp.checkNetwork()) + { + yError()<<"[main] Unable to find YARP network"; + return EXIT_FAILURE; + } + + // prepare and configure the resource finder + yarp::os::ResourceFinder& rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); + + rf.setDefaultConfigFile("dcm_walking_with_joypad.ini"); + + rf.configure(argc, argv); + BipedalLocomotionControllers::MasImuTest test; + return test.runModule(rf); } From 1412dfd96fd36ba876b4a70a93ac94f76d43bd3d Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 12 Mar 2020 18:57:28 +0100 Subject: [PATCH 22/51] Test on simulation and bugfix. --- src/YarpUtilities/src/Helper.cpp | 4 ++- .../iCubGazeboV2_5/MasImuTestConfig.ini | 34 +++++++++++++++++++ .../robots/iCubGenova04/MasImuTestConfig.ini | 2 +- .../BipedalLocomotionControllers/MasImuTest.h | 5 +++ utilities/MasImuTest/src/MasImuTest.cpp | 20 ++++++++--- utilities/MasImuTest/src/main.cpp | 4 ++- .../thrifts/MasImuTestCommands.thrift | 5 +++ 7 files changed, 67 insertions(+), 7 deletions(-) create mode 100644 utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini diff --git a/src/YarpUtilities/src/Helper.cpp b/src/YarpUtilities/src/Helper.cpp index 6684fa7c50..f5d772cd9b 100644 --- a/src/YarpUtilities/src/Helper.cpp +++ b/src/YarpUtilities/src/Helper.cpp @@ -21,7 +21,9 @@ bool YarpUtilities::addVectorOfStringToProperty(yarp::os::Property& prop, return false; } - yarp::os::Bottle& bot = prop.addGroup(key).findGroup(key).addList(); + prop.addGroup(key); + + yarp::os::Bottle& bot = prop.findGroup(key).addList(); for (size_t i = 0; i < list.size(); i++) bot.addString(list[i].c_str()); diff --git a/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini b/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini new file mode 100644 index 0000000000..36e688a23e --- /dev/null +++ b/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini @@ -0,0 +1,34 @@ +name masImuTest + +robot icubSim + +model model.urdf + +period 0.01 + +base_link root_link + +#The base_rotation is defined (by rows) in the Inertia frame (x pointing forward, z pointing upward) +base_rotation ((-1.0 0.0 0.0),(0.0 -1.0 0.0),(0.0 0.0 1.0)) + +filter_yaw true + +min_joint_variation_deg 2.0 + +max_samples 100 + +[LEFT_LEG] + +remote left_foot/inertials + +imu_frame l_foot_ft_acc_3b13 + +remote_control_boards ("left_leg") + +[RIGHT_LEG] + +remote right_foot/inertials + +imu_frame r_foot_ft_acc_3b14 + +remote_control_boards ("right_leg") diff --git a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini index 9a765d6072..275909cc62 100644 --- a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini +++ b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini @@ -11,7 +11,7 @@ base_link root_link #The base_rotation is defined (by rows) in the Inertia frame (x pointing forward, z pointing upward) base_rotation ((-1.0 0.0 0.0),(0.0 -1.0 0.0),(0.0 0.0 1.0)) -filter_yaw 1 +filter_yaw true min_joint_variation_deg 2.0 diff --git a/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h b/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h index 59c6f64fa9..77ce3fc194 100644 --- a/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h +++ b/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h @@ -177,6 +177,11 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ * Print the results. This works only if the test has already been stopped. */ bool printResults() override; + + /** + * Quits the module + */ + void quit() override; }; #endif // BIPEDAL_LOCOMOTION_CONTROLLERS_MASIMUTEST_H diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index 9890064263..b1b057a78a 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -49,8 +49,11 @@ bool MasImuTest::MasImuData::setupModel() while (currentLink != baseLinkIndex) { const iDynTree::IJoint* joint = m_commonDataPtr->traversal.getParentJointFromLinkIndex(currentLink); assert(joint); - m_consideredJointIndexes.push_back(joint->getIndex()); - m_consideredJointNames.push_back(m_commonDataPtr->fullModel.getJointName(m_consideredJointIndexes.back())); + if (joint->getNrOfDOFs() > 0) + { + m_consideredJointIndexes.push_back(joint->getIndex()); + m_consideredJointNames.push_back(m_commonDataPtr->fullModel.getJointName(m_consideredJointIndexes.back())); + } currentLink = m_commonDataPtr->traversal.getParentLinkFromLinkIndex(currentLink)->getIndex(); } @@ -456,6 +459,8 @@ bool MasImuTest::MasImuData::close() yError() << errorPrefix << "Unable to close the robot driver."; return false; } + + return true; } @@ -469,7 +474,7 @@ void MasImuTest::reset() void MasImuTest::printResultsPrivate() const { m_leftIMU.printResults(); - m_leftIMU.printResults(); + m_rightIMU.printResults(); } double MasImuTest::getPeriod() @@ -530,7 +535,7 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) m_parametersPtr = BipedalLocomotionControllers::ParametersHandler::YarpImplementation::make_unique(rf); m_commonDataPtr = std::make_shared(); - + bool ok = m_parametersPtr->getParameter("name", m_commonDataPtr->prefix); if (!ok) { @@ -565,6 +570,7 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) return false; } std::string pathToModel = yarp::os::ResourceFinder::getResourceFinderSingleton().findFileByName(robotModelName); + yInfo() << "[MasImuTest::configure] Robot model path: " << pathToModel; iDynTree::ModelLoader modelLoader; if (!modelLoader.loadModelFromFile(pathToModel)) { @@ -746,3 +752,9 @@ bool MasImuTest::printResults() return false; } +void MasImuTest::quit() +{ + std::lock_guard guard(m_mutex); + stopModule(); +} + diff --git a/utilities/MasImuTest/src/main.cpp b/utilities/MasImuTest/src/main.cpp index ce80c518e9..be812dda80 100644 --- a/utilities/MasImuTest/src/main.cpp +++ b/utilities/MasImuTest/src/main.cpp @@ -25,10 +25,12 @@ int main(int argc, char * argv[]) // prepare and configure the resource finder yarp::os::ResourceFinder& rf = yarp::os::ResourceFinder::getResourceFinderSingleton(); - rf.setDefaultConfigFile("dcm_walking_with_joypad.ini"); + rf.setDefaultConfigFile("MasImuTestConfig.ini"); rf.configure(argc, argv); + yInfo() << "[MasImuTest] Configuration file: " << rf.findFileByName("MasImuTestConfig.ini"); + BipedalLocomotionControllers::MasImuTest test; return test.runModule(rf); diff --git a/utilities/MasImuTest/thrifts/MasImuTestCommands.thrift b/utilities/MasImuTest/thrifts/MasImuTestCommands.thrift index a2a3fd2bfe..005ba5c296 100644 --- a/utilities/MasImuTest/thrifts/MasImuTestCommands.thrift +++ b/utilities/MasImuTest/thrifts/MasImuTestCommands.thrift @@ -7,6 +7,11 @@ service MasImuTestCommands { + /** + * Quits the module. + */ + oneway void quit(); + /** * Call this method to start the test. * @return true/false in case of success/failure (for example if the preparation phase was not successfull); From 8ccb6c539bb1ad3e4aeab99b2c52069e7c29b96f Mon Sep 17 00:00:00 2001 From: Stefano Date: Sun, 15 Mar 2020 11:29:46 +0100 Subject: [PATCH 23/51] Increased MAS timeout and added more detailed printout. --- .../iCubGazeboV2_5/MasImuTestConfig.ini | 2 + .../robots/iCubGenova04/MasImuTestConfig.ini | 2 + .../BipedalLocomotionControllers/MasImuTest.h | 1 + utilities/MasImuTest/src/MasImuTest.cpp | 62 ++++++++++++++++++- 4 files changed, 66 insertions(+), 1 deletion(-) diff --git a/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini b/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini index 36e688a23e..030b0ef146 100644 --- a/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini +++ b/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini @@ -17,6 +17,8 @@ min_joint_variation_deg 2.0 max_samples 100 +mas_timeout 0.02 + [LEFT_LEG] remote left_foot/inertials diff --git a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini index 275909cc62..29c757b907 100644 --- a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini +++ b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini @@ -17,6 +17,8 @@ min_joint_variation_deg 2.0 max_samples 100 +mas_timeout 0.02 + [LEFT_LEG] remote left_leg/inertials diff --git a/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h b/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h index 77ce3fc194..843c435e70 100644 --- a/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h +++ b/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h @@ -50,6 +50,7 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ bool filterYaw; int maxSamples; double minJointVariationRad; + double masTimeout; }; class MasImuData diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index b1b057a78a..defc70756a 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -91,6 +91,7 @@ bool MasImuTest::MasImuData::setupOrientationSensors() yarp::os::Property inertialClientProperty; inertialClientProperty.put("remote", "/" + m_commonDataPtr->robotName + "/" + remote); inertialClientProperty.put("local", "/" + m_commonDataPtr->prefix + "/" + remote); + inertialClientProperty.put("timeout", m_commonDataPtr->masTimeout); inertialClientProperty.put("device","multipleanalogsensorsclient"); if (!m_orientationDriver.open(inertialClientProperty)) @@ -420,6 +421,14 @@ void MasImuTest::MasImuData::printResults() const { std::string errorPrefix = "[MasImuTest::MasImuData::printResults](" + m_testName +") "; + if (!m_data.size()) + { + yInfo() << errorPrefix << "Results ("< 0) && ((minError < 0) || (error < minError))) //Avoiding to pick the very first data point as it may be too close to the initialization + { + minError = error; + minIndex = i; + } + + if ((maxError < 0) || (error > maxError)) + { + maxError = error; + maxIndex = i; + } + } + + auto rpyPrinter = [](const iDynTree::Rotation& rot)->std::string + { + std::string output; + iDynTree::Vector3 rpy = rot.asRPY(); + + output = "RPY: (" + std::to_string(rpy[0]) + ", " + std::to_string(rpy[1]) + ", " + std::to_string(rpy[2]) + ")\n"; + return output; + }; + + yInfo() << errorPrefix << "Results ("<getParameter("mas_timeout", m_commonDataPtr->masTimeout); + if (!ok || m_commonDataPtr->masTimeout < 0) + { + yError() << "[MasImuTest::configure] Configuration failed while reading \"mas_timeout\"."; + return false; + } + auto leftLegGroup = m_parametersPtr->getGroup("LEFT_LEG").lock(); if (!leftLegGroup) { @@ -683,6 +738,8 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) m_state = State::PREPARED; + yInfo() << "[MasImuTest::configure] Ready!"; + return true; } @@ -714,6 +771,9 @@ bool MasImuTest::startTest() { reset(); m_state = State::FIRST_RUN; + + yInfo() << "[MasImuTest::startTest] Started!"; + return true; } From 8229778332ac44d363ac4a509d702569abf05222 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 10 Jun 2020 16:51:18 +0200 Subject: [PATCH 24/51] Modifications after rebasing on top of master. --- .../BipedalLocomotionFrameworkFindDependencies.cmake | 2 +- src/YarpUtilities/src/Helper.cpp | 4 +--- utilities/MasImuTest/CMakeLists.txt | 6 +++--- .../MasImuTest.h | 12 ++++++------ utilities/MasImuTest/src/MasImuTest.cpp | 10 +++++----- utilities/MasImuTest/src/main.cpp | 4 ++-- 6 files changed, 18 insertions(+), 20 deletions(-) rename utilities/MasImuTest/include/{BipedalLocomotionControllers => BipedalLocomotion}/MasImuTest.h (89%) diff --git a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake index 026c9fee91..036b881ba4 100644 --- a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake @@ -229,7 +229,7 @@ framework_dependent_option(FRAMEWORK_TEST_PYTHON_BINDINGS "Do you want to test the Python bindings?" ON "FRAMEWORK_COMPILE_tests;FRAMEWORK_COMPILE_PYTHON_BINDINGS;FRAMEWORK_USE_pytest" OFF) -framework_dependent_option(BIPEDAL_LOCOMOTION_CONTROLLERS_COMPILE_MasImuTest +framework_dependent_option(FRAMEWORK_COMPILE_MasImuTest "Compile test on the MAS IMU?" ON "FRAMEWORK_COMPILE_YarpUtilities" OFF) diff --git a/src/YarpUtilities/src/Helper.cpp b/src/YarpUtilities/src/Helper.cpp index f5d772cd9b..6684fa7c50 100644 --- a/src/YarpUtilities/src/Helper.cpp +++ b/src/YarpUtilities/src/Helper.cpp @@ -21,9 +21,7 @@ bool YarpUtilities::addVectorOfStringToProperty(yarp::os::Property& prop, return false; } - prop.addGroup(key); - - yarp::os::Bottle& bot = prop.findGroup(key).addList(); + yarp::os::Bottle& bot = prop.addGroup(key).findGroup(key).addList(); for (size_t i = 0; i < list.size(); i++) bot.addString(list[i].c_str()); diff --git a/utilities/MasImuTest/CMakeLists.txt b/utilities/MasImuTest/CMakeLists.txt index f3ace248e2..077062c343 100644 --- a/utilities/MasImuTest/CMakeLists.txt +++ b/utilities/MasImuTest/CMakeLists.txt @@ -3,7 +3,7 @@ # GNU Lesser General Public License v2.1 or any later version. # set target name -if(BIPEDAL_LOCOMOTION_CONTROLLERS_COMPILE_MasImuTest) +if(FRAMEWORK_COMPILE_MasImuTest) set(EXE_TARGET_NAME MasImuTest) @@ -15,7 +15,7 @@ if(BIPEDAL_LOCOMOTION_CONTROLLERS_COMPILE_MasImuTest) # set hpp files set(${EXE_TARGET_NAME}_HDR - include/BipedalLocomotionControllers/MasImuTest.h + include/BipedalLocomotion/MasImuTest.h ) set(${EXE_TARGET_NAME}_THRIFT_HDR @@ -27,7 +27,7 @@ if(BIPEDAL_LOCOMOTION_CONTROLLERS_COMPILE_MasImuTest) add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_SRC} ${${EXE_TARGET_NAME}_HDR} ${${EXE_TARGET_NAME}_THRIFT_GEN_FILES}) target_link_libraries(${EXE_TARGET_NAME} PUBLIC ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} - BipedalLocomotionControllers::YarpUtilities BipedalLocomotionControllers::ParametersHandlerYarpImplementation) + BipedalLocomotion::YarpUtilities BipedalLocomotion::ParametersHandlerYarpImplementation) target_compile_features(${EXE_TARGET_NAME} PUBLIC cxx_std_17) # Specify include directories for both compilation and installation process. diff --git a/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h b/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h similarity index 89% rename from utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h rename to utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h index 843c435e70..0a155d1202 100644 --- a/utilities/MasImuTest/include/BipedalLocomotionControllers/MasImuTest.h +++ b/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h @@ -31,14 +31,14 @@ //Thrifts #include -#include +#include -namespace BipedalLocomotionControllers +namespace BipedalLocomotion { class MasImuTest; } -class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, public MasImuTestCommands { +class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTestCommands { struct CommonData { @@ -57,7 +57,7 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ { std::string m_testName; std::shared_ptr m_commonDataPtr; - BipedalLocomotionControllers::ParametersHandler::YarpImplementation::shared_ptr m_group; + BipedalLocomotion::ParametersHandler::YarpImplementation::shared_ptr m_group; iDynTree::FrameIndex m_frame; std::string m_frameName; iDynTree::LinkIndex m_link; @@ -95,7 +95,7 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ public: bool setup(const std::string& testName, - BipedalLocomotionControllers::ParametersHandler::YarpImplementation::shared_ptr group, + BipedalLocomotion::ParametersHandler::YarpImplementation::shared_ptr group, std::shared_ptr commonDataPtr); bool firstRun(); @@ -123,7 +123,7 @@ class BipedalLocomotionControllers::MasImuTest : public yarp::os::RFModule, publ RUNNING }; - BipedalLocomotionControllers::ParametersHandler::YarpImplementation::unique_ptr m_parametersPtr; + BipedalLocomotion::ParametersHandler::YarpImplementation::shared_ptr m_parametersPtr; std::shared_ptr m_commonDataPtr; double m_period; MasImuData m_leftIMU, m_rightIMU; diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index defc70756a..c0bb2c708a 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -5,8 +5,8 @@ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version. */ -#include -#include +#include +#include #include #include #include @@ -16,7 +16,7 @@ #include #include -using namespace BipedalLocomotionControllers; +using namespace BipedalLocomotion; bool MasImuTest::MasImuData::setupModel() { @@ -581,9 +581,9 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) { std::lock_guard lock(m_mutex); - m_parametersPtr = BipedalLocomotionControllers::ParametersHandler::YarpImplementation::make_unique(rf); + m_parametersPtr = std::make_shared(rf); m_commonDataPtr = std::make_shared(); - + bool ok = m_parametersPtr->getParameter("name", m_commonDataPtr->prefix); if (!ok) { diff --git a/utilities/MasImuTest/src/main.cpp b/utilities/MasImuTest/src/main.cpp index be812dda80..becfe0e0b3 100644 --- a/utilities/MasImuTest/src/main.cpp +++ b/utilities/MasImuTest/src/main.cpp @@ -10,7 +10,7 @@ #include #include -#include +#include int main(int argc, char * argv[]) { @@ -31,7 +31,7 @@ int main(int argc, char * argv[]) yInfo() << "[MasImuTest] Configuration file: " << rf.findFileByName("MasImuTestConfig.ini"); - BipedalLocomotionControllers::MasImuTest test; + BipedalLocomotion::MasImuTest test; return test.runModule(rf); } From f2354cf8d2a0fe880e44f8fbb4f747a9fb73ceb8 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 6 Jul 2020 15:02:11 +0200 Subject: [PATCH 25/51] More detailed message when the orientation sensor is not found. --- utilities/MasImuTest/src/MasImuTest.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index c0bb2c708a..80d40b7201 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -129,7 +129,18 @@ bool MasImuTest::MasImuData::setupOrientationSensors() if (!found) { yError() << errorPrefix << "The interface contains no orientation sensors on frame " - << m_frameName << ". Setup failed."; + << m_frameName << ". Available orientation sensors frame names (" << m_orientationInterface->getNrOfOrientationSensors() << "):"; + ok = true; + do + { + bool ok = m_orientationInterface->getOrientationSensorFrameName(m_sensorIndex, name); + if (ok) + { + yError() << " - " << name; + } + } + while (ok && (m_sensorIndex < m_orientationInterface->getNrOfOrientationSensors())); + return false; } From f5c2dcea49ac5c9cc778bb9559a62ce9d00120f6 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 6 Jul 2020 15:25:44 +0200 Subject: [PATCH 26/51] Specifying also the sensor name. --- .../robots/iCubGazeboV2_5/MasImuTestConfig.ini | 4 ++++ .../robots/iCubGenova04/MasImuTestConfig.ini | 4 ++++ .../include/BipedalLocomotion/MasImuTest.h | 2 +- utilities/MasImuTest/src/MasImuTest.cpp | 18 +++++++++++++----- 4 files changed, 22 insertions(+), 6 deletions(-) diff --git a/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini b/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini index 030b0ef146..cf58e47ad7 100644 --- a/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini +++ b/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini @@ -25,6 +25,8 @@ remote left_foot/inertials imu_frame l_foot_ft_acc_3b13 +sensor_name l_foot_ft_acc_3b13 + remote_control_boards ("left_leg") [RIGHT_LEG] @@ -33,4 +35,6 @@ remote right_foot/inertials imu_frame r_foot_ft_acc_3b14 +sensor_name r_foot_ft_acc_3b14 + remote_control_boards ("right_leg") diff --git a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini index 29c757b907..abcd74226c 100644 --- a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini +++ b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini @@ -25,6 +25,8 @@ remote left_leg/inertials imu_frame l_foot_ft_acc_3b13 +sensor_name l_foot_ft_eul_3b13 + remote_control_boards ("left_leg") [RIGHT_LEG] @@ -33,4 +35,6 @@ remote right_leg/inertials imu_frame r_foot_ft_acc_3b14 +sensor_name r_foot_ft_eul_3b14 + remote_control_boards ("right_leg") diff --git a/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h b/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h index 0a155d1202..51422f527c 100644 --- a/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h +++ b/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h @@ -59,7 +59,7 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe std::shared_ptr m_commonDataPtr; BipedalLocomotion::ParametersHandler::YarpImplementation::shared_ptr m_group; iDynTree::FrameIndex m_frame; - std::string m_frameName; + std::string m_frameName, m_sensorName; iDynTree::LinkIndex m_link; std::vector m_consideredJointIndexes; std::vector m_consideredJointNames; diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index 80d40b7201..13ff13bd19 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -88,6 +88,13 @@ bool MasImuTest::MasImuData::setupOrientationSensors() return false; } + ok = m_group->getParameter("sensor_name", m_sensorName); + if (!ok) + { + yError() << errorPrefix << "Setup failed."; + return false; + } + yarp::os::Property inertialClientProperty; inertialClientProperty.put("remote", "/" + m_commonDataPtr->robotName + "/" + remote); inertialClientProperty.put("local", "/" + m_commonDataPtr->prefix + "/" + remote); @@ -116,7 +123,7 @@ bool MasImuTest::MasImuData::setupOrientationSensors() bool ok = m_orientationInterface->getOrientationSensorFrameName(m_sensorIndex, name); if (ok) { - found = name == m_frameName; + found = name == m_sensorName; if (!found) { @@ -129,17 +136,18 @@ bool MasImuTest::MasImuData::setupOrientationSensors() if (!found) { yError() << errorPrefix << "The interface contains no orientation sensors on frame " - << m_frameName << ". Available orientation sensors frame names (" << m_orientationInterface->getNrOfOrientationSensors() << "):"; + << m_sensorName << ". Available orientation sensors frame names (" << m_orientationInterface->getNrOfOrientationSensors() << "):"; ok = true; - do + size_t index = 0; + while (ok && (index < m_orientationInterface->getNrOfOrientationSensors())) { - bool ok = m_orientationInterface->getOrientationSensorFrameName(m_sensorIndex, name); + bool ok = m_orientationInterface->getOrientationSensorFrameName(index, name); if (ok) { yError() << " - " << name; + index++; } } - while (ok && (m_sensorIndex < m_orientationInterface->getNrOfOrientationSensors())); return false; } From ad3aa01b78314e71e622b19adfee83b89b5663d2 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 6 Jul 2020 15:32:27 +0200 Subject: [PATCH 27/51] The vector to read remote_control_boards is resizable. --- utilities/MasImuTest/src/MasImuTest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index 13ff13bd19..94bb8eae02 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -162,7 +162,7 @@ bool MasImuTest::MasImuData::setupEncoders() std::string errorPrefix = "[MasImuTest::MasImuData::setupEncoders](" + m_testName +") "; std::vector inputControlBoards; - bool ok = m_group->getParameter("remote_control_boards", inputControlBoards); + bool ok = m_group->getParameter("remote_control_boards", inputControlBoards, GenericContainer::VectorResizeMode::Resizable); if (!ok) { yError() << errorPrefix << "Setup failed."; From 496e6147269c2b2e2adcb649b1d1605e939ff871 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 6 Jul 2020 15:43:58 +0200 Subject: [PATCH 28/51] Fixed bug in addVectorOfStringToProperty. --- src/YarpUtilities/src/Helper.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/YarpUtilities/src/Helper.cpp b/src/YarpUtilities/src/Helper.cpp index 6684fa7c50..456c34e5a2 100644 --- a/src/YarpUtilities/src/Helper.cpp +++ b/src/YarpUtilities/src/Helper.cpp @@ -21,7 +21,8 @@ bool YarpUtilities::addVectorOfStringToProperty(yarp::os::Property& prop, return false; } - yarp::os::Bottle& bot = prop.addGroup(key).findGroup(key).addList(); + prop.addGroup(key); + yarp::os::Bottle& bot = prop.findGroup(key).addList(); for (size_t i = 0; i < list.size(); i++) bot.addString(list[i].c_str()); From 21a40ea0877530056d7325bb8712bee941eb1e80 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 6 Jul 2020 16:05:01 +0200 Subject: [PATCH 29/51] Added auto-start option. --- .../robots/iCubGazeboV2_5/MasImuTestConfig.ini | 2 ++ .../app/robots/iCubGenova04/MasImuTestConfig.ini | 4 +++- utilities/MasImuTest/src/MasImuTest.cpp | 16 ++++++++++++++++ 3 files changed, 21 insertions(+), 1 deletion(-) diff --git a/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini b/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini index cf58e47ad7..01e9b149ca 100644 --- a/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini +++ b/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini @@ -19,6 +19,8 @@ max_samples 100 mas_timeout 0.02 +auto_start true + [LEFT_LEG] remote left_foot/inertials diff --git a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini index abcd74226c..6023791768 100644 --- a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini +++ b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini @@ -15,10 +15,12 @@ filter_yaw true min_joint_variation_deg 2.0 -max_samples 100 +max_samples 500 mas_timeout 0.02 +auto_start true + [LEFT_LEG] remote left_leg/inertials diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index 94bb8eae02..30c5e593dc 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -755,10 +755,26 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) return false; } + bool autoStart; + ok = m_parametersPtr->getParameter("auto_start", autoStart); + if (!ok) + { + yError() << "[MasImuTest::configure] Configuration failed."; + return false; + } + m_state = State::PREPARED; yInfo() << "[MasImuTest::configure] Ready!"; + + if (autoStart) + { + m_state = State::FIRST_RUN; + + yInfo() << "[MasImuTest::startTest] Started!"; + } + return true; } From 7de42f0ef26e61ee8847901a0abd37c0f13fcb04 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 6 Jul 2020 18:16:36 +0200 Subject: [PATCH 30/51] Fixed missing return. --- utilities/MasImuTest/src/MasImuTest.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index 30c5e593dc..95447d18d4 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -588,6 +588,7 @@ bool MasImuTest::updateModule() { yError() << "[MasImuTest::updateModule] Failed to perform first run."; m_state = State::PREPARED; + return true; } m_state = State::RUNNING; From 96c4e7bc58b7083f1c0365af7b6c89dc698b8c2b Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 8 Jul 2020 09:49:15 +0200 Subject: [PATCH 31/51] Using macro to install ini files. Fixed some indentation problem and a flag in the cmake file for creating tests. --- cmake/AddBipedalLocomotionUnitTest.cmake | 2 +- utilities/MasImuTest/CMakeLists.txt | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/cmake/AddBipedalLocomotionUnitTest.cmake b/cmake/AddBipedalLocomotionUnitTest.cmake index 4449f25bbb..43e328826e 100644 --- a/cmake/AddBipedalLocomotionUnitTest.cmake +++ b/cmake/AddBipedalLocomotionUnitTest.cmake @@ -14,7 +14,7 @@ checkandset_dependency(VALGRIND) framework_dependent_option(FRAMEWORK_COMPILE_tests "Compile tests?" ON - "FRAMEWORK_HAS_Catch2;BUILD_TESTING" OFF) + "FRAMEWORK_USE_Catch2;BUILD_TESTING" OFF) framework_dependent_option(FRAMEWORK_RUN_Valgrind_tests "Run Valgrind tests?" OFF diff --git a/utilities/MasImuTest/CMakeLists.txt b/utilities/MasImuTest/CMakeLists.txt index 077062c343..825df7f118 100644 --- a/utilities/MasImuTest/CMakeLists.txt +++ b/utilities/MasImuTest/CMakeLists.txt @@ -44,6 +44,8 @@ if(FRAMEWORK_COMPILE_MasImuTest) ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin) + install_ini_files(${CMAKE_CURRENT_SOURCE_DIR}/app) + message(STATUS "Created target ${EXE_TARGET_NAME} for export ${PROJECT_NAME}.") endif() From edfccf2d9d9ddb7d02414afe89c19f88a56c76c3 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 8 Jul 2020 11:35:18 +0200 Subject: [PATCH 32/51] Added printout of the initial calibration matrix. --- ...lLocomotionFrameworkFindDependencies.cmake | 2 +- utilities/MasImuTest/src/MasImuTest.cpp | 32 ++++++++++++------- 2 files changed, 22 insertions(+), 12 deletions(-) diff --git a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake index 036b881ba4..9b61686f21 100644 --- a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake @@ -130,7 +130,7 @@ endmacro() ################################################################################ # Find all packages -find_package(iDynTree 0.11.105 REQUIRED) #Right now, all the packages built in the framework +find_package(iDynTree 1.1.0 REQUIRED) #Right now, all the packages built in the framework #depend directly or indirectly from iDynTree and Eigen #(which is an iDynTree dependency by the way) find_package(Eigen3 3.2.92 REQUIRED) diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index 95447d18d4..ee1d26a34f 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -440,9 +440,23 @@ void MasImuTest::MasImuData::printResults() const { std::string errorPrefix = "[MasImuTest::MasImuData::printResults](" + m_testName +") "; + auto rpyPrinter = [](const iDynTree::Rotation& rot)->std::string + { + std::string output; + iDynTree::Vector3 rpy = rot.asRPY(); + + output = "RPY: (" + std::to_string(rpy[0]) + ", " + std::to_string(rpy[1]) + ", " + std::to_string(rpy[2]) + ")\n"; + return output; + }; + if (!m_data.size()) { - yInfo() << errorPrefix << "Results ("<std::string - { - std::string output; - iDynTree::Vector3 rpy = rot.asRPY(); - - output = "RPY: (" + std::to_string(rpy[0]) + ", " + std::to_string(rpy[1]) + ", " + std::to_string(rpy[2]) + ")\n"; - return output; - }; - - yInfo() << errorPrefix << "Results ("< Date: Fri, 10 Jul 2020 10:13:30 +0200 Subject: [PATCH 33/51] Printing rpy in deg. --- utilities/MasImuTest/src/MasImuTest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index ee1d26a34f..1e689394d5 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -445,7 +445,7 @@ void MasImuTest::MasImuData::printResults() const std::string output; iDynTree::Vector3 rpy = rot.asRPY(); - output = "RPY: (" + std::to_string(rpy[0]) + ", " + std::to_string(rpy[1]) + ", " + std::to_string(rpy[2]) + ")\n"; + output = "RPY [deg]: (" + std::to_string(iDynTree::rad2deg(rpy[0])) + ", " + std::to_string(iDynTree::rad2deg(rpy[1])) + ", " + std::to_string(iDynTree::rad2deg(rpy[2])) + ")\n"; return output; }; From a846a146ba8ab42b9ecca46cf27736b743404c06 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 7 Sep 2020 09:42:32 +0200 Subject: [PATCH 34/51] Specifying iDynTree 1.1.0 as dependency in the config file. --- CMakeLists.txt | 2 +- ...lLocomotionFrameworkFindDependencies.cmake | 4 +- utilities/MasImuTest/README.md | 59 +++++++++++++++++++ 3 files changed, 62 insertions(+), 3 deletions(-) create mode 100644 utilities/MasImuTest/README.md diff --git a/CMakeLists.txt b/CMakeLists.txt index 3e8f55c88d..0dd794d519 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,7 +89,7 @@ add_subdirectory(devices) #By including the file "BipedalLocomotion/Framework.h" it is possible to include all the headers automatically. include(AddBipedalLocomotionFrameworkTarget) -set(FRAMEWORK_PUBLIC_DEPENDENCIES iDynTree "Eigen3 3.2.92") +set(FRAMEWORK_PUBLIC_DEPENDENCIES "iDynTree 1.1.0" "Eigen3 3.2.92") if (FRAMEWORK_USE_YARP) list(APPEND FRAMEWORK_PUBLIC_DEPENDENCIES YARP) diff --git a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake index 9b61686f21..9ca4991718 100644 --- a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake @@ -131,8 +131,8 @@ endmacro() # Find all packages find_package(iDynTree 1.1.0 REQUIRED) #Right now, all the packages built in the framework - #depend directly or indirectly from iDynTree and Eigen - #(which is an iDynTree dependency by the way) + #depend directly or indirectly from iDynTree and Eigen + #(which is an iDynTree dependency by the way) find_package(Eigen3 3.2.92 REQUIRED) find_package(YARP QUIET) diff --git a/utilities/MasImuTest/README.md b/utilities/MasImuTest/README.md new file mode 100644 index 0000000000..559a01164c --- /dev/null +++ b/utilities/MasImuTest/README.md @@ -0,0 +1,59 @@ +# MAS IMU Test + +The ``MasImuTest`` was designed with the intent of testing whether the orientation provided by the IMU sensors is coherent with the encoder measurements. +The test requires the operator to move manually the limbs where the sensors are located. In this way, the test acquires data to detect if the IMU measurements and the encoder measurements coincide. + +## Installation +The ``MasImuTest`` is part of the ``BipedalLocomotionFramework``. It is compiled if the ``CMake`` option ``FRAMEWORK_COMPILE_MasImuTest`` is set to ``ON``. To install, follow the instructions for ``BipedalLocomotionFramework``. In order for the executable to be easily launched, add the folder +``` +/path/to/install/bin +``` +to the path (substitute ``/path/to/install`` with the installation directory of bipedal locomotion framework). +The test also requires some configuration files. In order to find the correct configuration file, set the environmental variable ``YARP_ROBOT_NAME`` with the name of the robot on which you want to perform the test. + +:warning: In the ``BipedalLocomotionFramework`` repo only the files for ``iCubGenova04`` and ``iCubGazeboV2_5`` are present. + +In order for the configuration files to be found, it is necessary to add the following directory +``` +/path/to/install/share/BipedalLocomotionFramework +``` +to the ``YARP_DATA_DIRS`` environmental variable. + +## Running the test +Simply run +``` +MasImuTest +``` +in a terminal. The test should run automatically. + +#### Insert here some real output + +The test has also an RPC interface. It is possible to launch it by running +``` +yarp rpc /masImuTest/rpc +``` +in a terminal. Then the following commands are available: +- ``quit`` Quits the module. +- ``startTest`` Manually start the test. +- ``stopTest`` Manually stop the test. +- ``printResults`` If the test is stopped, print the results. + +## Configuration file explanation +The configuration file presents the following data: +- ``name masImuTest`` The name of the test, used to define the name of the opened ports. +- ``robot icub`` The prefix of the ports opened by the robot to which the test has to connect. +- ``model model.urdf`` The name of the ``urdf`` model. +- ``period 0.01`` The period at which the module runs. +- ``base_link root_link`` The link of the robot to be considered fixed. +- ``base_rotation ((-1.0 0.0 0.0),(0.0 -1.0 0.0),(0.0 0.0 1.0))``. The rotation of the base link with respect to an inertial frame having the z-axis pointing against gravity and the x axis defining the forward direction. The rotation is defined by rows +- ``filter_yaw true`` Boolean value used to trigger the filtering of the yaw measurement coming from the IMU. This measurement is substituted by the one obtained through forward kinematics. +- ``min_joint_variation_deg 2.0`` Minimum joint variation after which a new sample is taken into consideration. +- ``max_samples 500`` The max number of samples considered in the test. +- ``mas_timeout 0.02`` Timeout for reading the MAS IMU sensor. A warning is thrown if this timeout is not respected. +- ``auto_start true`` The test start automatically without having to use the RPC interface. + +The following part of the configuration file contains two part which are equivalent, one for the left leg, having the tag ``[LEFT_LEG]`` and one for the right leg. We will detail here only the part for the left leg since the other is equivalent. +- ``remote left_leg/inertials`` The remote port from where the output of the IMU is streamed. +- ``imu_frame l_foot_ft_acc_3b13`` The name of the frame in the URDF corresponding to the IMU to check. +- ``sensor_name l_foot_ft_eul_3b13`` The name of the sensor to check. +- ``remote_control_boards ("left_leg")`` The comma-separated list of control boards including all the joints connecting the sensor under testing to the base link. From 2ce7ed3f18b22f6b8b192e1508548c8a29b5a08d Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 9 Nov 2020 17:25:52 +0100 Subject: [PATCH 35/51] Yet another rebase fix. --- utilities/MasImuTest/src/MasImuTest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index 1e689394d5..fefe8ce344 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -162,7 +162,7 @@ bool MasImuTest::MasImuData::setupEncoders() std::string errorPrefix = "[MasImuTest::MasImuData::setupEncoders](" + m_testName +") "; std::vector inputControlBoards; - bool ok = m_group->getParameter("remote_control_boards", inputControlBoards, GenericContainer::VectorResizeMode::Resizable); + bool ok = m_group->getParameter("remote_control_boards", inputControlBoards); if (!ok) { yError() << errorPrefix << "Setup failed."; From 7c43e924cbb3187eaf9836fc00213fedcc8c201b Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 9 Nov 2020 20:10:25 +0100 Subject: [PATCH 36/51] Added matioCpp dependency. --- cmake/BipedalLocomotionFrameworkFindDependencies.cmake | 2 +- utilities/MasImuTest/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake index 9ca4991718..1f5c960bf5 100644 --- a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake @@ -231,5 +231,5 @@ framework_dependent_option(FRAMEWORK_TEST_PYTHON_BINDINGS framework_dependent_option(FRAMEWORK_COMPILE_MasImuTest "Compile test on the MAS IMU?" ON - "FRAMEWORK_COMPILE_YarpUtilities" OFF) + "FRAMEWORK_COMPILE_YarpUtilities;FRAMEWORK_USE_matioCpp" OFF) diff --git a/utilities/MasImuTest/CMakeLists.txt b/utilities/MasImuTest/CMakeLists.txt index 825df7f118..bf91fcc17c 100644 --- a/utilities/MasImuTest/CMakeLists.txt +++ b/utilities/MasImuTest/CMakeLists.txt @@ -27,7 +27,7 @@ if(FRAMEWORK_COMPILE_MasImuTest) add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_SRC} ${${EXE_TARGET_NAME}_HDR} ${${EXE_TARGET_NAME}_THRIFT_GEN_FILES}) target_link_libraries(${EXE_TARGET_NAME} PUBLIC ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} - BipedalLocomotion::YarpUtilities BipedalLocomotion::ParametersHandlerYarpImplementation) + BipedalLocomotion::YarpUtilities BipedalLocomotion::ParametersHandlerYarpImplementation matioCpp::matioCpp) target_compile_features(${EXE_TARGET_NAME} PUBLIC cxx_std_17) # Specify include directories for both compilation and installation process. From 4b3178fefcf9aa8f97ca6a4c627edfbdcc6576af Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 10 Nov 2020 19:12:18 +0100 Subject: [PATCH 37/51] Saving all the internal data to file. --- utilities/MasImuTest/CMakeLists.txt | 2 +- .../iCubGazeboV2_5/MasImuTestConfig.ini | 2 + .../robots/iCubGenova04/MasImuTestConfig.ini | 2 + .../include/BipedalLocomotion/MasImuTest.h | 19 +- utilities/MasImuTest/src/MasImuTest.cpp | 237 ++++++++++++++++-- 5 files changed, 242 insertions(+), 20 deletions(-) diff --git a/utilities/MasImuTest/CMakeLists.txt b/utilities/MasImuTest/CMakeLists.txt index bf91fcc17c..564a90e37d 100644 --- a/utilities/MasImuTest/CMakeLists.txt +++ b/utilities/MasImuTest/CMakeLists.txt @@ -27,7 +27,7 @@ if(FRAMEWORK_COMPILE_MasImuTest) add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_SRC} ${${EXE_TARGET_NAME}_HDR} ${${EXE_TARGET_NAME}_THRIFT_GEN_FILES}) target_link_libraries(${EXE_TARGET_NAME} PUBLIC ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} - BipedalLocomotion::YarpUtilities BipedalLocomotion::ParametersHandlerYarpImplementation matioCpp::matioCpp) + BipedalLocomotion::GenericContainer BipedalLocomotion::YarpUtilities BipedalLocomotion::ParametersHandlerYarpImplementation matioCpp::matioCpp) target_compile_features(${EXE_TARGET_NAME} PUBLIC cxx_std_17) # Specify include directories for both compilation and installation process. diff --git a/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini b/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini index 01e9b149ca..8293ada4c5 100644 --- a/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini +++ b/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini @@ -21,6 +21,8 @@ mas_timeout 0.02 auto_start true +file_name masImuTestOutput.mat + [LEFT_LEG] remote left_foot/inertials diff --git a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini index 6023791768..ac64a45729 100644 --- a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini +++ b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini @@ -21,6 +21,8 @@ mas_timeout 0.02 auto_start true +file_name masImuTestOutput.mat + [LEFT_LEG] remote left_leg/inertials diff --git a/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h b/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h index 51422f527c..cd29ab81df 100644 --- a/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h +++ b/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h @@ -28,6 +28,9 @@ #include #include +// matioCpp +#include + //Thrifts #include @@ -51,6 +54,7 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe int maxSamples; double minJointVariationRad; double masTimeout; + matioCpp::File outputFile; }; class MasImuData @@ -68,17 +72,27 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe yarp::dev::IOrientationSensors* m_orientationInterface; yarp::dev::IEncodersTimed* m_encodersInterface; size_t m_sensorIndex; - std::vector m_data; + std::vector m_errorData; yarp::sig::Vector m_positionFeedbackDeg; /**< Current joint position [deg]. */ yarp::sig::Vector m_rpyInDeg; + iDynTree::Vector3 m_rpyInRad; iDynTree::VectorDynSize m_positionFeedbackInRad; iDynTree::VectorDynSize m_previousPositionFeedbackInRad; iDynTree::VectorDynSize m_dummyVelocity; iDynTree::Rotation m_rotationFeedback; + std::vector m_jointsPositionData; + std::vector m_rotationFeedbackData; + std::vector m_rotationFeedbackInInertialData; + std::vector m_rotationFeedbackInInertialYawFilteredData; + std::vector m_rpyImuData; iDynTree::Rotation m_rotationFromEncoders; + std::vector m_rotationFromEncodersData; iDynTree::Rotation m_imuWorld; //i_R_imuworld bool m_completed{false}; + void reserveData(); + + void clearData(); bool setupModel(); @@ -110,6 +124,8 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe void printResults() const; + bool saveResults(); + void reset(); bool close(); @@ -131,7 +147,6 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe std::mutex m_mutex; yarp::os::Port m_rpcPort; /**< Remote Procedure Call port. */ - void reset(); void printResultsPrivate() const; diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index fefe8ce344..56ba1a6f30 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -11,12 +11,66 @@ #include #include #include +#include #include #include #include #include using namespace BipedalLocomotion; +using namespace BipedalLocomotion::GenericContainer; + +template +Eigen::Map> to_eigen(matioCpp::MultiDimensionalArray& input) +{ + assert(input.isValid()); + assert(input.dimensions().size() == 2); + return Eigen::Map>(input.data(), input.dimensions()(1), input.dimensions()(2)); +} + +template +const Eigen::Map> to_eigen(const matioCpp::MultiDimensionalArray& input) +{ + assert(input.isValid()); + assert(input.dimensions().size() == 2); + return Eigen::Map>(input.data(), input.dimensions()(1), input.dimensions()(2)); +} + +template +matioCpp::MultiDimensionalArray to_matio(const Eigen::MatrixBase& input, const std::string& name) +{ + matioCpp::MultiDimensionalArray matio(name, {static_cast(input.rows()), static_cast(input.cols())}); + to_eigen(matio) = input; + return matio; +} + +template +matioCpp::Vector to_matio(typename GenericContainer::Vector::Ref input, const std::string& name) +{ + return matioCpp::Vector(name, input); +} + +void MasImuTest::MasImuData::reserveData() +{ + m_errorData.reserve(m_commonDataPtr->maxSamples); + m_rotationFeedbackData.reserve(m_commonDataPtr->maxSamples); + m_rotationFeedbackInInertialData.reserve(m_commonDataPtr->maxSamples); + m_rotationFeedbackInInertialYawFilteredData.reserve(m_commonDataPtr->maxSamples); + m_rotationFromEncodersData.reserve(m_commonDataPtr->maxSamples); + m_jointsPositionData.reserve(m_commonDataPtr->maxSamples); + m_rpyImuData.reserve(m_commonDataPtr->maxSamples); +} + +void MasImuTest::MasImuData::clearData() +{ + m_errorData.clear(); + m_rotationFeedbackData.clear(); + m_rotationFeedbackInInertialData.clear(); + m_rotationFeedbackInInertialYawFilteredData.clear(); + m_rotationFromEncodersData.clear(); + m_jointsPositionData.clear(); + m_rpyImuData.clear(); +} bool MasImuTest::MasImuData::setupModel() { @@ -238,9 +292,11 @@ bool MasImuTest::MasImuData::getFeedback() m_positionFeedbackInRad(j) = iDynTree::deg2rad(m_positionFeedbackDeg(j)); } - m_rotationFeedback = iDynTree::Rotation::RPY(iDynTree::deg2rad(m_rpyInDeg[0]), - iDynTree::deg2rad(m_rpyInDeg[1]), - iDynTree::deg2rad(m_rpyInDeg[2])); + m_rpyInRad(0) = iDynTree::deg2rad(m_rpyInDeg[0]); + m_rpyInRad(1) = iDynTree::deg2rad(m_rpyInDeg[1]); + m_rpyInRad(2) = iDynTree::deg2rad(m_rpyInDeg[2]); + + m_rotationFeedback = iDynTree::Rotation::RPY(m_rpyInRad(0), m_rpyInRad(1), m_rpyInRad(2)); return true; } @@ -311,7 +367,7 @@ bool MasImuTest::MasImuData::setup(const std::string &testName, ParametersHandle m_commonDataPtr = commonDataPtr; m_group = group; - m_data.reserve(m_commonDataPtr->maxSamples); + reserveData(); std::string errorPrefix = "[MasImuTest::MasImuData::setup](" + m_testName +") "; @@ -392,8 +448,15 @@ bool MasImuTest::MasImuData::addSample() return false; } + m_rpyImuData.push_back(m_rpyInRad); + m_jointsPositionData.push_back(m_positionFeedbackInRad); + m_rotationFromEncodersData.push_back(m_rotationFromEncoders); + m_rotationFeedbackData.push_back(m_rotationFeedback); + iDynTree::Rotation measuredImu = m_imuWorld * m_rotationFeedback; + m_rotationFeedbackInInertialData.push_back(measuredImu); + if (m_commonDataPtr->filterYaw) { double measuredRoll, measuredPitch, measuredYaw; @@ -405,9 +468,11 @@ bool MasImuTest::MasImuData::addSample() measuredImu = iDynTree::Rotation::RPY(measuredRoll, measuredPitch, estimatedYaw); } + m_rotationFeedbackInInertialYawFilteredData.push_back(measuredImu); + iDynTree::Rotation error = m_rotationFromEncoders.inverse() * measuredImu; - m_data.push_back(error); + m_errorData.push_back(error); m_previousPositionFeedbackInRad = m_positionFeedbackInRad; @@ -423,7 +488,7 @@ bool MasImuTest::MasImuData::addSample() size_t MasImuTest::MasImuData::addedSamples() const { - return m_data.size(); + return m_errorData.size(); } bool MasImuTest::MasImuData::isCompleted() const @@ -449,14 +514,14 @@ void MasImuTest::MasImuData::printResults() const return output; }; - if (!m_data.size()) + if (!m_errorData.size()) { yInfo() << errorPrefix << "Inertial calibration matrix:\n" << "--------------------------------------\n" << m_imuWorld.toString() << rpyPrinter(m_imuWorld) << "--------------------------------------\n" - << "Results ("< 0) && ((minError < 0) || (error < minError))) //Avoiding to pick the very first data point as it may be too close to the initialization { minError = error; @@ -499,25 +564,115 @@ void MasImuTest::MasImuData::printResults() const << m_imuWorld.toString() << rpyPrinter(m_imuWorld) << "--------------------------------------\n" - << "Results ("< fields = {"RotationError", "RotationFromIMU", "RotationFromIMUInInertial", "RotationFromIMUInInertialYawFiltered", "RotationFromEncoders", "JointPositions_rad", "RPYfromIMU"}; + + for (std::string& field : fields) + { + if (!dataArray.addField(field)) + { + yError() << errorPrefix << "Unable to add the field " << field << " to the data struct."; + return false; + } + } + + for (size_t i = 0; i < m_errorData.size(); ++i) + { + matioCpp::StructArrayElement el = dataArray[{i, 1}]; + if (!el.setField(to_matio(iDynTree::toEigen(m_errorData[i]), "RotationError"))) + { + yError() << errorPrefix << "Failed to set the field RotationError."; + return false; + } + if(!el.setField(to_matio(iDynTree::toEigen(m_rotationFeedbackData[i]), "RotationFromIMU"))) + { + yError() << errorPrefix << "Failed to set the field RotationError."; + return false; + } + if(!el.setField(to_matio(iDynTree::toEigen(m_rotationFeedbackInInertialData[i]), "RotationFromIMUInInertial"))) + { + yError() << errorPrefix << "Failed to set the field RotationError."; + return false; + } + if(!el.setField(to_matio(iDynTree::toEigen(m_rotationFeedbackInInertialYawFilteredData[i]), "RotationFromIMUInInertialYawFiltered"))) + { + yError() << errorPrefix << "Failed to set the field RotationError."; + return false; + } + if(!el.setField(to_matio(iDynTree::toEigen(m_rotationFromEncodersData[i]), "RotationFromEncoders"))) + { + yError() << errorPrefix << "Failed to set the field RotationError."; + return false; + } + if(!el.setField(to_matio(m_jointsPositionData[i], "JointPositions_rad"))) + { + yError() << errorPrefix << "Failed to set the field RotationError."; + return false; + } + if(!el.setField(to_matio(m_rpyImuData[i], "RPYfromIMU"))) + { + yError() << errorPrefix << "Failed to set the field RotationError."; + return false; + } + } + + std::vector options; + options.push_back(matioCpp::String("SensorName", m_sensorName)); + options.push_back(matioCpp::String("FrameName", m_frameName)); + matioCpp::CellArray consideredJoints("ConsideredJoints", {m_consideredJointNames.size(), 1}); + for (size_t i = 0; i < m_consideredJointNames.size(); ++i) + { + consideredJoints.setElement({i,1}, matioCpp::String(m_consideredJointNames[i])); + } + options.push_back(consideredJoints); + options.push_back(to_matio(iDynTree::toEigen(m_imuWorld), "I_R_world")); + + matioCpp::Struct optionsStruct(m_testName + "_options", options); + + if (!optionsStruct.isValid()) + { + yError() << errorPrefix << "Failed to create the options struct."; + return false; + } + + if (!m_commonDataPtr->outputFile.write(dataArray)) + { + yError() << errorPrefix << "Failed to write the data array to file."; + return false; + } + + if (!m_commonDataPtr->outputFile.write(optionsStruct)) + { + yError() << errorPrefix << "Failed to write the options struct to file."; + return false; + } + + return true; +} + void MasImuTest::MasImuData::reset() { - m_data.clear(); + clearData(); m_completed = false; } @@ -525,6 +680,12 @@ bool MasImuTest::MasImuData::close() { std::string errorPrefix = "[MasImuTest::MasImuData::close](" + m_testName +") "; + if (!saveResults()) + { + yError() << errorPrefix << "Unable to save the results."; + return false; + } + if(!m_orientationDriver.close()) { yError() << errorPrefix << "Unable to close the orientation driver."; @@ -729,6 +890,14 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) return false; } + std::string outputFileName; + ok = m_parametersPtr->getParameter("file_name", outputFileName); + if (!ok || m_commonDataPtr->masTimeout < 0) + { + yError() << "[MasImuTest::configure] Configuration failed while reading \"file_name\"."; + return false; + } + auto leftLegGroup = m_parametersPtr->getGroup("LEFT_LEG").lock(); if (!leftLegGroup) { @@ -757,6 +926,40 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) return false; } + matioCpp::File testFile(outputFileName, matioCpp::FileMode::ReadOnly); + + if (testFile.isOpen()) + { + testFile.close(); + matioCpp::File::Delete(outputFileName); + } + + matioCpp::File::Create(outputFileName); + m_commonDataPtr->outputFile.open(outputFileName); + + if (!(m_commonDataPtr->outputFile.isOpen())) + { + yError() << "[MasImuTest::configure] Failed to create new file with the name " << outputFileName <<"."; + return false; + } + + std::vector settings; + settings.push_back(matioCpp::String("robot_name", m_commonDataPtr->robotName)); + settings.push_back(matioCpp::Element("period", m_period)); + settings.push_back(matioCpp::String("model", pathToModel)); + settings.push_back(matioCpp::String("base_link", baseLink)); + settings.push_back(to_matio(iDynTree::toEigen(baseRotation), "base_rotation")); + settings.push_back(matioCpp::Element("filter_yaw", m_commonDataPtr->filterYaw)); + settings.push_back(matioCpp::Element("min_joint_variation_deg", minJointVariationInDeg)); + settings.push_back(matioCpp::Element("max_samples", m_commonDataPtr->maxSamples)); + settings.push_back(matioCpp::Element("mas_timeout", m_commonDataPtr->masTimeout)); + + if (!m_commonDataPtr->outputFile.write(matioCpp::Struct("settings", settings))) + { + yError() << "[MasImuTest::configure] Error while writing the settings to file"; + return false; + } + // open RPC port for external command std::string rpcPortName = "/" + m_commonDataPtr->prefix + "/rpc"; this->yarp().attachAsServer(this->m_rpcPort); From 8b1a6132c4d2cd354e4cb0efe22b8efbc345dd10 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 10 Nov 2020 23:13:02 +0100 Subject: [PATCH 38/51] Dumping also angular velocity and acceleration. --- .../iCubGazeboV2_5/MasImuTestConfig.ini | 12 +- .../robots/iCubGenova04/MasImuTestConfig.ini | 12 +- .../include/BipedalLocomotion/MasImuTest.h | 29 +- utilities/MasImuTest/src/MasImuTest.cpp | 259 ++++++++++++++++-- 4 files changed, 274 insertions(+), 38 deletions(-) diff --git a/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini b/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini index 8293ada4c5..337b3af920 100644 --- a/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini +++ b/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini @@ -29,7 +29,11 @@ remote left_foot/inertials imu_frame l_foot_ft_acc_3b13 -sensor_name l_foot_ft_acc_3b13 +imu_sensor_name l_foot_ft_acc_3b13 + +gyro_sensor_name l_foot_ft_acc_3b13 + +acc_sensor_name l_foot_ft_acc_3b13 remote_control_boards ("left_leg") @@ -39,6 +43,10 @@ remote right_foot/inertials imu_frame r_foot_ft_acc_3b14 -sensor_name r_foot_ft_acc_3b14 +imu_sensor_name r_foot_ft_acc_3b14 + +gyro_sensor_name r_foot_ft_acc_3b14 + +acc_sensor_name r_foot_ft_acc_3b14 remote_control_boards ("right_leg") diff --git a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini index ac64a45729..20f43f293b 100644 --- a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini +++ b/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini @@ -29,7 +29,11 @@ remote left_leg/inertials imu_frame l_foot_ft_acc_3b13 -sensor_name l_foot_ft_eul_3b13 +imu_sensor_name l_foot_ft_eul_3b13 + +gyro_sensor_name l_foot_ft_gyro_3b13 + +acc_sensor_name l_foot_ft_acc_3b13 remote_control_boards ("left_leg") @@ -39,6 +43,10 @@ remote right_leg/inertials imu_frame r_foot_ft_acc_3b14 -sensor_name r_foot_ft_eul_3b14 +imu_sensor_name r_foot_ft_eul_3b14 + +gyro_sensor_name r_foot_ft_gyro_3b14 + +acc_sensor_name r_foot_ft_acc_3b14 remote_control_boards ("right_leg") diff --git a/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h b/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h index cd29ab81df..14fbdb30fd 100644 --- a/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h +++ b/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h @@ -63,31 +63,38 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe std::shared_ptr m_commonDataPtr; BipedalLocomotion::ParametersHandler::YarpImplementation::shared_ptr m_group; iDynTree::FrameIndex m_frame; - std::string m_frameName, m_sensorName; + std::string m_frameName, m_imuName, m_gyroName, m_accName; iDynTree::LinkIndex m_link; std::vector m_consideredJointIndexes; std::vector m_consideredJointNames; iDynTree::KinDynComputations m_kinDyn; yarp::dev::PolyDriver m_orientationDriver, m_robotDriver; yarp::dev::IOrientationSensors* m_orientationInterface; + yarp::dev::IThreeAxisGyroscopes* m_gyroInterface; + yarp::dev::IThreeAxisLinearAccelerometers* m_accInterface; yarp::dev::IEncodersTimed* m_encodersInterface; - size_t m_sensorIndex; - std::vector m_errorData; + size_t m_imuSensorIndex, m_gyroSensorIndex, m_accSensorIndex; yarp::sig::Vector m_positionFeedbackDeg; /**< Current joint position [deg]. */ - yarp::sig::Vector m_rpyInDeg; + yarp::sig::Vector m_rpyInDeg, m_gyroInDeg_s, m_acc; iDynTree::Vector3 m_rpyInRad; iDynTree::VectorDynSize m_positionFeedbackInRad; iDynTree::VectorDynSize m_previousPositionFeedbackInRad; iDynTree::VectorDynSize m_dummyVelocity; iDynTree::Rotation m_rotationFeedback; + + iDynTree::Rotation m_rotationFromEncoders; + iDynTree::Rotation m_imuWorld; //i_R_imuworld + + std::vector m_errorData; std::vector m_jointsPositionData; std::vector m_rotationFeedbackData; std::vector m_rotationFeedbackInInertialData; std::vector m_rotationFeedbackInInertialYawFilteredData; - std::vector m_rpyImuData; - iDynTree::Rotation m_rotationFromEncoders; + std::vector m_rpyImuData; std::vector m_rotationFromEncodersData; - iDynTree::Rotation m_imuWorld; //i_R_imuworld + std::vector m_gyroData; + std::vector m_accData; + bool m_completed{false}; void reserveData(); @@ -96,7 +103,13 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe bool setupModel(); - bool setupOrientationSensors(); + bool setupIMUDriver(); + + bool setupOrientationSensor(); + + bool setupGyro(); + + bool setupAccelerometer(); bool setupEncoders(); diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index 56ba1a6f30..7f4af9fc55 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -59,6 +59,8 @@ void MasImuTest::MasImuData::reserveData() m_rotationFromEncodersData.reserve(m_commonDataPtr->maxSamples); m_jointsPositionData.reserve(m_commonDataPtr->maxSamples); m_rpyImuData.reserve(m_commonDataPtr->maxSamples); + m_gyroData.reserve(m_commonDataPtr->maxSamples); + m_accData.reserve(m_commonDataPtr->maxSamples); } void MasImuTest::MasImuData::clearData() @@ -70,6 +72,8 @@ void MasImuTest::MasImuData::clearData() m_rotationFromEncodersData.clear(); m_jointsPositionData.clear(); m_rpyImuData.clear(); + m_gyroData.clear(); + m_accData.clear(); } bool MasImuTest::MasImuData::setupModel() @@ -131,18 +135,11 @@ bool MasImuTest::MasImuData::setupModel() return true; } -bool MasImuTest::MasImuData::setupOrientationSensors() +bool MasImuTest::MasImuData::setupIMUDriver() { - std::string errorPrefix = "[MasImuTest::MasImuData::setupOrientationSensors](" + m_testName +") "; + std::string errorPrefix = "[MasImuTest::MasImuData::setupIMUDriver](" + m_testName +") "; std::string remote; - bool ok = m_group->getParameter("remote",remote); - if (!ok) - { - yError() << errorPrefix << "Setup failed."; - return false; - } - - ok = m_group->getParameter("sensor_name", m_sensorName); + bool ok = m_group->getParameter("remote", remote); if (!ok) { yError() << errorPrefix << "Setup failed."; @@ -162,35 +159,48 @@ bool MasImuTest::MasImuData::setupOrientationSensors() return false; } + return true; +} + +bool MasImuTest::MasImuData::setupOrientationSensor() +{ + std::string errorPrefix = "[MasImuTest::MasImuData::setupOrientationSensors](" + m_testName +") "; + + bool ok = m_group->getParameter("imu_sensor_name", m_imuName); + if (!ok) + { + yError() << errorPrefix << "Setup failed."; + return false; + } + if (!m_orientationDriver.view(m_orientationInterface) || !m_orientationInterface) { - yError() << errorPrefix << "Failed to open multipleanalogsensorsclient on remote " - << remote << ". Setup failed."; + yError() << errorPrefix << "Failed to view orientation interface. Setup failed."; return false; } - m_sensorIndex = 0; + m_imuSensorIndex = 0; std::string name; bool found = false; do { - bool ok = m_orientationInterface->getOrientationSensorFrameName(m_sensorIndex, name); + bool ok = m_orientationInterface->getOrientationSensorFrameName(m_imuSensorIndex, name); if (ok) { - found = name == m_sensorName; + found = name == m_imuName; if (!found) { - m_sensorIndex++; + m_imuSensorIndex++; } } } - while (ok && (m_sensorIndex < m_orientationInterface->getNrOfOrientationSensors()) && !found); + while (ok && (m_imuSensorIndex < m_orientationInterface->getNrOfOrientationSensors()) && !found); if (!found) { yError() << errorPrefix << "The interface contains no orientation sensors on frame " - << m_sensorName << ". Available orientation sensors frame names (" << m_orientationInterface->getNrOfOrientationSensors() << "):"; + << m_imuName << ". Available orientation sensor frame names (" << m_orientationInterface->getNrOfOrientationSensors() << "):"; ok = true; size_t index = 0; while (ok && (index < m_orientationInterface->getNrOfOrientationSensors())) @@ -211,6 +221,124 @@ bool MasImuTest::MasImuData::setupOrientationSensors() return true; } +bool MasImuTest::MasImuData::setupGyro() +{ + std::string errorPrefix = "[MasImuTest::MasImuData::setupGyro](" + m_testName +") "; + + bool ok = m_group->getParameter("gyro_sensor_name", m_imuName); + if (!ok) + { + yError() << errorPrefix << "Setup failed."; + return false; + } + + if (!m_orientationDriver.view(m_gyroInterface) || !m_gyroInterface) + { + yError() << errorPrefix << "Failed to view gyro interface. Setup failed."; + return false; + } + + m_gyroSensorIndex = 0; + std::string name; + bool found = false; + do + { + bool ok = m_gyroInterface->getThreeAxisGyroscopeFrameName(m_gyroSensorIndex, name); + if (ok) + { + found = name == m_gyroName; + + if (!found) + { + m_gyroSensorIndex++; + } + } + } + while (ok && (m_gyroSensorIndex < m_gyroInterface->getNrOfThreeAxisGyroscopes()) && !found); + + if (!found) + { + yError() << errorPrefix << "The interface contains no gyro sensors on frame " + << m_gyroName << ". Available gyro sensor frame names (" << m_gyroInterface->getNrOfThreeAxisGyroscopes() << "):"; + ok = true; + size_t index = 0; + while (ok && (index < m_gyroInterface->getNrOfThreeAxisGyroscopes())) + { + bool ok = m_gyroInterface->getThreeAxisGyroscopeFrameName(index, name); + if (ok) + { + yError() << " - " << name; + index++; + } + } + + return false; + } + + m_gyroInDeg_s.resize(3); + + return true; +} + +bool MasImuTest::MasImuData::setupAccelerometer() +{ + std::string errorPrefix = "[MasImuTest::MasImuData::setupAccelerometer](" + m_testName +") "; + + bool ok = m_group->getParameter("acc_sensor_name", m_imuName); + if (!ok) + { + yError() << errorPrefix << "Setup failed."; + return false; + } + + if (!m_orientationDriver.view(m_accInterface) || !m_accInterface) + { + yError() << errorPrefix << "Failed to view accelerometers interface. Setup failed."; + return false; + } + + m_accSensorIndex = 0; + std::string name; + bool found = false; + do + { + bool ok = m_accInterface->getThreeAxisLinearAccelerometerFrameName(m_accSensorIndex, name); + if (ok) + { + found = name == m_accName; + + if (!found) + { + m_accSensorIndex++; + } + } + } + while (ok && (m_accSensorIndex < m_accInterface->getNrOfThreeAxisLinearAccelerometers()) && !found); + + if (!found) + { + yError() << errorPrefix << "The interface contains no accelerometers on frame " + << m_accName << ". Available accelerometer frame names (" << m_accInterface->getNrOfThreeAxisLinearAccelerometers() << "):"; + ok = true; + size_t index = 0; + while (ok && (index < m_accInterface->getNrOfThreeAxisLinearAccelerometers())) + { + bool ok = m_accInterface->getThreeAxisLinearAccelerometerFrameName(index, name); + if (ok) + { + yError() << " - " << name; + index++; + } + } + + return false; + } + + m_acc.resize(3); + + return true; +} + bool MasImuTest::MasImuData::setupEncoders() { std::string errorPrefix = "[MasImuTest::MasImuData::setupEncoders](" + m_testName +") "; @@ -269,6 +397,8 @@ bool MasImuTest::MasImuData::getFeedback() size_t attempt = 0; bool okEncoders = false; bool okIMU = false; + bool okGyro = false; + bool okAcc = false; do { @@ -277,15 +407,35 @@ bool MasImuTest::MasImuData::getFeedback() if (!okIMU) { - yarp::dev::MAS_status status = m_orientationInterface->getOrientationSensorStatus(m_sensorIndex); + yarp::dev::MAS_status status = m_orientationInterface->getOrientationSensorStatus(m_imuSensorIndex); + if (status == yarp::dev::MAS_status::MAS_OK) + { + double timestamp; + okIMU = m_orientationInterface->getOrientationSensorMeasureAsRollPitchYaw(m_imuSensorIndex, m_rpyInDeg, timestamp); + } + } + + if (!okGyro) + { + yarp::dev::MAS_status status = m_gyroInterface->getThreeAxisGyroscopeStatus(m_gyroSensorIndex); if (status == yarp::dev::MAS_status::MAS_OK) { double timestamp; - okIMU = m_orientationInterface->getOrientationSensorMeasureAsRollPitchYaw(m_sensorIndex, m_rpyInDeg, timestamp); + okGyro = m_gyroInterface->getThreeAxisGyroscopeMeasure(m_gyroSensorIndex, m_gyroInDeg_s, timestamp); } } - if (okEncoders && okIMU) + if (!okAcc) + { + yarp::dev::MAS_status status = m_accInterface->getThreeAxisLinearAccelerometerStatus(m_accSensorIndex); + if (status == yarp::dev::MAS_status::MAS_OK) + { + double timestamp; + okAcc = m_accInterface->getThreeAxisLinearAccelerometerMeasure(m_accSensorIndex, m_acc, timestamp); + } + } + + if (okEncoders && okIMU && okGyro && okAcc) { for(unsigned j = 0 ; j < m_positionFeedbackDeg.size(); j++) { @@ -313,6 +463,12 @@ bool MasImuTest::MasImuData::getFeedback() if (!okIMU) yError() << "\t - IMU"; + if (!okGyro) + yError() << "\t - Gyro"; + + if (!okAcc) + yError() << "\t - Accelerometer"; + return false; } @@ -378,13 +534,34 @@ bool MasImuTest::MasImuData::setup(const std::string &testName, ParametersHandle return false; } - ok = setupOrientationSensors(); + ok = setupIMUDriver(); + if (!ok) + { + yError() << errorPrefix << "setupDriver failed."; + return false; + } + + ok = setupOrientationSensor(); if (!ok) { yError() << errorPrefix << "setupOrientationSensors failed."; return false; } + ok = setupGyro(); + if (!ok) + { + yError() << errorPrefix << "setupGyro failed."; + return false; + } + + ok = setupAccelerometer(); + if (!ok) + { + yError() << errorPrefix << "setupAccelerometer failed."; + return false; + } + ok = setupEncoders(); if (!ok) { @@ -448,10 +625,12 @@ bool MasImuTest::MasImuData::addSample() return false; } - m_rpyImuData.push_back(m_rpyInRad); + m_rpyImuData.push_back(m_rpyInDeg); m_jointsPositionData.push_back(m_positionFeedbackInRad); m_rotationFromEncodersData.push_back(m_rotationFromEncoders); m_rotationFeedbackData.push_back(m_rotationFeedback); + m_gyroData.push_back(m_gyroInDeg_s); + m_accData.push_back(m_acc); iDynTree::Rotation measuredImu = m_imuWorld * m_rotationFeedback; @@ -585,7 +764,15 @@ bool MasImuTest::MasImuData::saveResults() std::string errorPrefix = "[MasImuTest::MasImuData::saveResults](" + m_testName +") "; matioCpp::StructArray dataArray(m_testName, {m_errorData.size(), 1}); - std::vector fields = {"RotationError", "RotationFromIMU", "RotationFromIMUInInertial", "RotationFromIMUInInertialYawFiltered", "RotationFromEncoders", "JointPositions_rad", "RPYfromIMU"}; + std::vector fields = {"RotationError", + "RotationFromIMU", + "RotationFromIMUInInertial", + "RotationFromIMUInInertialYawFiltered", + "RotationFromEncoders", + "JointPositions_rad", + "RPYfromIMUinDeg", + "AngularVelocity_deg_s", + "Accelerometer"}; for (std::string& field : fields) { @@ -604,40 +791,60 @@ bool MasImuTest::MasImuData::saveResults() yError() << errorPrefix << "Failed to set the field RotationError."; return false; } + if(!el.setField(to_matio(iDynTree::toEigen(m_rotationFeedbackData[i]), "RotationFromIMU"))) { yError() << errorPrefix << "Failed to set the field RotationError."; return false; } + if(!el.setField(to_matio(iDynTree::toEigen(m_rotationFeedbackInInertialData[i]), "RotationFromIMUInInertial"))) { yError() << errorPrefix << "Failed to set the field RotationError."; return false; } + if(!el.setField(to_matio(iDynTree::toEigen(m_rotationFeedbackInInertialYawFilteredData[i]), "RotationFromIMUInInertialYawFiltered"))) { yError() << errorPrefix << "Failed to set the field RotationError."; return false; } + if(!el.setField(to_matio(iDynTree::toEigen(m_rotationFromEncodersData[i]), "RotationFromEncoders"))) { yError() << errorPrefix << "Failed to set the field RotationError."; return false; } + if(!el.setField(to_matio(m_jointsPositionData[i], "JointPositions_rad"))) { yError() << errorPrefix << "Failed to set the field RotationError."; return false; } - if(!el.setField(to_matio(m_rpyImuData[i], "RPYfromIMU"))) + + if(!el.setField(to_matio(m_rpyImuData[i], "RPYfromIMUinDeg"))) { yError() << errorPrefix << "Failed to set the field RotationError."; return false; } + + if(!el.setField(to_matio(m_gyroData[i], "AngularVelocity_deg_s"))) + { + yError() << errorPrefix << "Failed to set the field AngularVelocity_deg_s."; + return false; + } + + if(!el.setField(to_matio(m_accData[i], "Accelerometer"))) + { + yError() << errorPrefix << "Failed to set the field Accelerometer."; + return false; + } } std::vector options; - options.push_back(matioCpp::String("SensorName", m_sensorName)); + options.push_back(matioCpp::String("IMUSensorName", m_imuName)); + options.push_back(matioCpp::String("GyroName", m_gyroName)); + options.push_back(matioCpp::String("AccelerometerName", m_accName)); options.push_back(matioCpp::String("FrameName", m_frameName)); matioCpp::CellArray consideredJoints("ConsideredJoints", {m_consideredJointNames.size(), 1}); for (size_t i = 0; i < m_consideredJointNames.size(); ++i) From 02700ae2a98e955d7d1cf065dfc860cb1b857529 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 11 Nov 2020 20:38:11 +0100 Subject: [PATCH 39/51] Bug fix and tests on the robot. --- .../include/BipedalLocomotion/MasImuTest.h | 6 +- utilities/MasImuTest/src/MasImuTest.cpp | 83 +++++++++---------- 2 files changed, 44 insertions(+), 45 deletions(-) diff --git a/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h b/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h index 14fbdb30fd..68761ef789 100644 --- a/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h +++ b/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h @@ -54,12 +54,12 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe int maxSamples; double minJointVariationRad; double masTimeout; - matioCpp::File outputFile; + std::string outputFile; }; class MasImuData { - std::string m_testName; + std::string m_testName, m_logPrefix; std::shared_ptr m_commonDataPtr; BipedalLocomotion::ParametersHandler::YarpImplementation::shared_ptr m_group; iDynTree::FrameIndex m_frame; @@ -123,7 +123,7 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe bool setup(const std::string& testName, BipedalLocomotion::ParametersHandler::YarpImplementation::shared_ptr group, - std::shared_ptr commonDataPtr); + std::shared_ptr commonDataPtr, const std::string &logPrefix); bool firstRun(); diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index 7f4af9fc55..988cf606c3 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -25,7 +25,7 @@ Eigen::Map> to_eigen(matioCp { assert(input.isValid()); assert(input.dimensions().size() == 2); - return Eigen::Map>(input.data(), input.dimensions()(1), input.dimensions()(2)); + return Eigen::Map>(input.data(), input.dimensions()(0), input.dimensions()(1)); } template @@ -33,7 +33,7 @@ const Eigen::Map> to_eigen(c { assert(input.isValid()); assert(input.dimensions().size() == 2); - return Eigen::Map>(input.data(), input.dimensions()(1), input.dimensions()(2)); + return Eigen::Map>(input.data(), input.dimensions()(0), input.dimensions()(1)); } template @@ -225,7 +225,7 @@ bool MasImuTest::MasImuData::setupGyro() { std::string errorPrefix = "[MasImuTest::MasImuData::setupGyro](" + m_testName +") "; - bool ok = m_group->getParameter("gyro_sensor_name", m_imuName); + bool ok = m_group->getParameter("gyro_sensor_name", m_gyroName); if (!ok) { yError() << errorPrefix << "Setup failed."; @@ -284,7 +284,7 @@ bool MasImuTest::MasImuData::setupAccelerometer() { std::string errorPrefix = "[MasImuTest::MasImuData::setupAccelerometer](" + m_testName +") "; - bool ok = m_group->getParameter("acc_sensor_name", m_imuName); + bool ok = m_group->getParameter("acc_sensor_name", m_accName); if (!ok) { yError() << errorPrefix << "Setup failed."; @@ -517,9 +517,10 @@ double MasImuTest::MasImuData::maxVariation() } bool MasImuTest::MasImuData::setup(const std::string &testName, ParametersHandler::YarpImplementation::shared_ptr group, - std::shared_ptr commonDataPtr) + std::shared_ptr commonDataPtr, const std::string& logPrefix) { m_testName = testName; + m_logPrefix = logPrefix; m_commonDataPtr = commonDataPtr; m_group = group; @@ -763,29 +764,24 @@ bool MasImuTest::MasImuData::saveResults() { std::string errorPrefix = "[MasImuTest::MasImuData::saveResults](" + m_testName +") "; - matioCpp::StructArray dataArray(m_testName, {m_errorData.size(), 1}); - std::vector fields = {"RotationError", - "RotationFromIMU", - "RotationFromIMUInInertial", - "RotationFromIMUInInertialYawFiltered", - "RotationFromEncoders", - "JointPositions_rad", - "RPYfromIMUinDeg", - "AngularVelocity_deg_s", - "Accelerometer"}; - - for (std::string& field : fields) - { - if (!dataArray.addField(field)) - { - yError() << errorPrefix << "Unable to add the field " << field << " to the data struct."; - return false; - } - } + std::vector structFields; + structFields.push_back(matioCpp::MultiDimensionalArray("RotationError", {3,3})); + structFields.push_back(matioCpp::MultiDimensionalArray("RotationFromIMU", {3,3})); + structFields.push_back(matioCpp::MultiDimensionalArray("RotationFromIMUInInertial", {3,3})); + structFields.push_back(matioCpp::MultiDimensionalArray("RotationFromIMUInInertialYawFiltered", {3,3})); + structFields.push_back(matioCpp::MultiDimensionalArray("RotationFromEncoders", {3,3})); + structFields.push_back(matioCpp::Vector("JointPositions_rad", m_consideredJointNames.size())); + structFields.push_back(matioCpp::Vector("RPYfromIMUinDeg", 3)); + structFields.push_back(matioCpp::Vector("AngularVelocity_deg_s", 3)); + structFields.push_back(matioCpp::Vector("Accelerometer", 3)); + matioCpp::Struct structElement("structElement", structFields); + std::vector structElements(m_errorData.size(), structElement); + + matioCpp::StructArray dataArray(m_logPrefix + "_data", {m_errorData.size(), 1}, structElements); for (size_t i = 0; i < m_errorData.size(); ++i) { - matioCpp::StructArrayElement el = dataArray[{i, 1}]; + matioCpp::StructArrayElement el = dataArray[{i, 0}]; if (!el.setField(to_matio(iDynTree::toEigen(m_errorData[i]), "RotationError"))) { yError() << errorPrefix << "Failed to set the field RotationError."; @@ -849,12 +845,12 @@ bool MasImuTest::MasImuData::saveResults() matioCpp::CellArray consideredJoints("ConsideredJoints", {m_consideredJointNames.size(), 1}); for (size_t i = 0; i < m_consideredJointNames.size(); ++i) { - consideredJoints.setElement({i,1}, matioCpp::String(m_consideredJointNames[i])); + consideredJoints.setElement({i,0}, matioCpp::String(m_consideredJointNames[i])); } options.push_back(consideredJoints); options.push_back(to_matio(iDynTree::toEigen(m_imuWorld), "I_R_world")); - matioCpp::Struct optionsStruct(m_testName + "_options", options); + matioCpp::Struct optionsStruct(m_logPrefix + "_options", options); if (!optionsStruct.isValid()) { @@ -862,13 +858,22 @@ bool MasImuTest::MasImuData::saveResults() return false; } - if (!m_commonDataPtr->outputFile.write(dataArray)) + std::cerr << "Saving data" <outputFile); + if (!(outputFile.isOpen())) + { + yError() << errorPrefix << "Failed to open file."; + return false; + } + + if (!outputFile.write(dataArray)) { yError() << errorPrefix << "Failed to write the data array to file."; return false; } - if (!m_commonDataPtr->outputFile.write(optionsStruct)) + if (!outputFile.write(optionsStruct)) { yError() << errorPrefix << "Failed to write the options struct to file."; return false; @@ -1112,7 +1117,7 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) return false; } - ok = m_leftIMU.setup("Left IMU Test", leftLegGroup, m_commonDataPtr); + ok = m_leftIMU.setup("Left IMU Test", leftLegGroup, m_commonDataPtr, "left"); if (!ok) { yError() << "[MasImuTest::configure] Configuration failed."; @@ -1126,25 +1131,19 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) return false; } - ok = m_rightIMU.setup("Right IMU Test", rightLegGroup, m_commonDataPtr); + ok = m_rightIMU.setup("Right IMU Test", rightLegGroup, m_commonDataPtr, "right"); if (!ok) { yError() << "[MasImuTest::configure] Configuration failed."; return false; } - matioCpp::File testFile(outputFileName, matioCpp::FileMode::ReadOnly); - - if (testFile.isOpen()) - { - testFile.close(); - matioCpp::File::Delete(outputFileName); - } + matioCpp::File::Delete(outputFileName); - matioCpp::File::Create(outputFileName); - m_commonDataPtr->outputFile.open(outputFileName); + matioCpp::File outputFile = matioCpp::File::Create(outputFileName); + m_commonDataPtr->outputFile = outputFileName; - if (!(m_commonDataPtr->outputFile.isOpen())) + if (!(outputFile.isOpen())) { yError() << "[MasImuTest::configure] Failed to create new file with the name " << outputFileName <<"."; return false; @@ -1161,7 +1160,7 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) settings.push_back(matioCpp::Element("max_samples", m_commonDataPtr->maxSamples)); settings.push_back(matioCpp::Element("mas_timeout", m_commonDataPtr->masTimeout)); - if (!m_commonDataPtr->outputFile.write(matioCpp::Struct("settings", settings))) + if (!outputFile.write(matioCpp::Struct("settings", settings))) { yError() << "[MasImuTest::configure] Error while writing the settings to file"; return false; From c6c8ed9a723384f1c37638e4b5ec119c0a574997 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 18 Nov 2020 12:08:26 +0100 Subject: [PATCH 40/51] Using matioCpp conversions and logging the output string. --- ...lLocomotionFrameworkFindDependencies.cmake | 2 +- utilities/MasImuTest/CMakeLists.txt | 2 +- .../include/BipedalLocomotion/MasImuTest.h | 6 +- utilities/MasImuTest/src/MasImuTest.cpp | 193 ++++++++---------- 4 files changed, 92 insertions(+), 111 deletions(-) diff --git a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake index 1f5c960bf5..13878c7801 100644 --- a/cmake/BipedalLocomotionFrameworkFindDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkFindDependencies.cmake @@ -231,5 +231,5 @@ framework_dependent_option(FRAMEWORK_TEST_PYTHON_BINDINGS framework_dependent_option(FRAMEWORK_COMPILE_MasImuTest "Compile test on the MAS IMU?" ON - "FRAMEWORK_COMPILE_YarpUtilities;FRAMEWORK_USE_matioCpp" OFF) + "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_matioCppConversions" OFF) diff --git a/utilities/MasImuTest/CMakeLists.txt b/utilities/MasImuTest/CMakeLists.txt index 564a90e37d..c20c8775f3 100644 --- a/utilities/MasImuTest/CMakeLists.txt +++ b/utilities/MasImuTest/CMakeLists.txt @@ -27,7 +27,7 @@ if(FRAMEWORK_COMPILE_MasImuTest) add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_SRC} ${${EXE_TARGET_NAME}_HDR} ${${EXE_TARGET_NAME}_THRIFT_GEN_FILES}) target_link_libraries(${EXE_TARGET_NAME} PUBLIC ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} - BipedalLocomotion::GenericContainer BipedalLocomotion::YarpUtilities BipedalLocomotion::ParametersHandlerYarpImplementation matioCpp::matioCpp) + BipedalLocomotion::GenericContainer BipedalLocomotion::YarpUtilities BipedalLocomotion::ParametersHandlerYarpImplementation BipedalLocomotion::matioCppConversions) target_compile_features(${EXE_TARGET_NAME} PUBLIC cxx_std_17) # Specify include directories for both compilation and installation process. diff --git a/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h b/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h index 68761ef789..579db3f666 100644 --- a/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h +++ b/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h @@ -95,6 +95,8 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe std::vector m_gyroData; std::vector m_accData; + std::string m_output; + bool m_completed{false}; void reserveData(); @@ -135,7 +137,7 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe void setCompleted(); - void printResults() const; + std::string printResults(); bool saveResults(); @@ -162,7 +164,7 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe void reset(); - void printResultsPrivate() const; + void printResultsPrivate(); public: diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index 988cf606c3..535e841b3f 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include #include @@ -16,39 +17,12 @@ #include #include #include +#include using namespace BipedalLocomotion; using namespace BipedalLocomotion::GenericContainer; +using namespace BipedalLocomotion::Conversions; -template -Eigen::Map> to_eigen(matioCpp::MultiDimensionalArray& input) -{ - assert(input.isValid()); - assert(input.dimensions().size() == 2); - return Eigen::Map>(input.data(), input.dimensions()(0), input.dimensions()(1)); -} - -template -const Eigen::Map> to_eigen(const matioCpp::MultiDimensionalArray& input) -{ - assert(input.isValid()); - assert(input.dimensions().size() == 2); - return Eigen::Map>(input.data(), input.dimensions()(0), input.dimensions()(1)); -} - -template -matioCpp::MultiDimensionalArray to_matio(const Eigen::MatrixBase& input, const std::string& name) -{ - matioCpp::MultiDimensionalArray matio(name, {static_cast(input.rows()), static_cast(input.cols())}); - to_eigen(matio) = input; - return matio; -} - -template -matioCpp::Vector to_matio(typename GenericContainer::Vector::Ref input, const std::string& name) -{ - return matioCpp::Vector(name, input); -} void MasImuTest::MasImuData::reserveData() { @@ -681,30 +655,33 @@ void MasImuTest::MasImuData::setCompleted() m_completed = true; } -void MasImuTest::MasImuData::printResults() const +std::string MasImuTest::MasImuData::printResults() { std::string errorPrefix = "[MasImuTest::MasImuData::printResults](" + m_testName +") "; + std::stringstream outputStream; + auto rpyPrinter = [](const iDynTree::Rotation& rot)->std::string { std::string output; iDynTree::Vector3 rpy = rot.asRPY(); - output = "RPY [deg]: (" + std::to_string(iDynTree::rad2deg(rpy[0])) + ", " + std::to_string(iDynTree::rad2deg(rpy[1])) + ", " + std::to_string(iDynTree::rad2deg(rpy[2])) + ")\n"; + output = "RPY [deg]: (" + std::to_string(iDynTree::rad2deg(rpy[0])) + ", " + std::to_string(iDynTree::rad2deg(rpy[1])) + ", " + std::to_string(iDynTree::rad2deg(rpy[2])) + ")"; return output; }; if (!m_errorData.size()) { - yInfo() << errorPrefix << "Inertial calibration matrix:\n" - << "--------------------------------------\n" + outputStream << errorPrefix << "Inertial calibration matrix:" << std::endl + << "--------------------------------------" << std::endl << m_imuWorld.toString() - << rpyPrinter(m_imuWorld) - << "--------------------------------------\n" - << "Results ("< structFields; - structFields.push_back(matioCpp::MultiDimensionalArray("RotationError", {3,3})); - structFields.push_back(matioCpp::MultiDimensionalArray("RotationFromIMU", {3,3})); - structFields.push_back(matioCpp::MultiDimensionalArray("RotationFromIMUInInertial", {3,3})); - structFields.push_back(matioCpp::MultiDimensionalArray("RotationFromIMUInInertialYawFiltered", {3,3})); - structFields.push_back(matioCpp::MultiDimensionalArray("RotationFromEncoders", {3,3})); - structFields.push_back(matioCpp::Vector("JointPositions_rad", m_consideredJointNames.size())); - structFields.push_back(matioCpp::Vector("RPYfromIMUinDeg", 3)); - structFields.push_back(matioCpp::Vector("AngularVelocity_deg_s", 3)); - structFields.push_back(matioCpp::Vector("Accelerometer", 3)); - matioCpp::Struct structElement("structElement", structFields); - std::vector structElements(m_errorData.size(), structElement); - - matioCpp::StructArray dataArray(m_logPrefix + "_data", {m_errorData.size(), 1}, structElements); + matioCpp::StructArray dataArray(m_logPrefix + "_data", {m_errorData.size(), 1}, {"RotationError", + "RotationFromIMU", + "RotationFromIMUInInertial", + "RotationFromIMUInInertialYawFiltered", + "RotationFromEncoders", + "JointPositions_rad", + "RPYfromIMUinDeg", + "AngularVelocity_deg_s", + "Accelerometer"}); for (size_t i = 0; i < m_errorData.size(); ++i) { matioCpp::StructArrayElement el = dataArray[{i, 0}]; - if (!el.setField(to_matio(iDynTree::toEigen(m_errorData[i]), "RotationError"))) + if (!el.setField(tomatioCpp(m_errorData[i], "RotationError"))) { yError() << errorPrefix << "Failed to set the field RotationError."; return false; } - if(!el.setField(to_matio(iDynTree::toEigen(m_rotationFeedbackData[i]), "RotationFromIMU"))) + if(!el.setField(tomatioCpp(m_rotationFeedbackData[i], "RotationFromIMU"))) { yError() << errorPrefix << "Failed to set the field RotationError."; return false; } - if(!el.setField(to_matio(iDynTree::toEigen(m_rotationFeedbackInInertialData[i]), "RotationFromIMUInInertial"))) + if(!el.setField(tomatioCpp(m_rotationFeedbackInInertialData[i], "RotationFromIMUInInertial"))) { yError() << errorPrefix << "Failed to set the field RotationError."; return false; } - if(!el.setField(to_matio(iDynTree::toEigen(m_rotationFeedbackInInertialYawFilteredData[i]), "RotationFromIMUInInertialYawFiltered"))) + if(!el.setField(tomatioCpp(m_rotationFeedbackInInertialYawFilteredData[i], "RotationFromIMUInInertialYawFiltered"))) { yError() << errorPrefix << "Failed to set the field RotationError."; return false; } - if(!el.setField(to_matio(iDynTree::toEigen(m_rotationFromEncodersData[i]), "RotationFromEncoders"))) + if(!el.setField(tomatioCpp(m_rotationFromEncodersData[i], "RotationFromEncoders"))) { yError() << errorPrefix << "Failed to set the field RotationError."; return false; } - if(!el.setField(to_matio(m_jointsPositionData[i], "JointPositions_rad"))) + if(!el.setField(tomatioCpp(m_jointsPositionData[i], "JointPositions_rad"))) { yError() << errorPrefix << "Failed to set the field RotationError."; return false; } - if(!el.setField(to_matio(m_rpyImuData[i], "RPYfromIMUinDeg"))) + if(!el.setField(tomatioCpp(m_rpyImuData[i], "RPYfromIMUinDeg"))) { yError() << errorPrefix << "Failed to set the field RotationError."; return false; } - if(!el.setField(to_matio(m_gyroData[i], "AngularVelocity_deg_s"))) + if(!el.setField(tomatioCpp(m_gyroData[i], "AngularVelocity_deg_s"))) { yError() << errorPrefix << "Failed to set the field AngularVelocity_deg_s."; return false; } - if(!el.setField(to_matio(m_accData[i], "Accelerometer"))) + if(!el.setField(tomatioCpp(m_accData[i], "Accelerometer"))) { yError() << errorPrefix << "Failed to set the field Accelerometer."; return false; @@ -838,17 +814,12 @@ bool MasImuTest::MasImuData::saveResults() } std::vector options; - options.push_back(matioCpp::String("IMUSensorName", m_imuName)); - options.push_back(matioCpp::String("GyroName", m_gyroName)); - options.push_back(matioCpp::String("AccelerometerName", m_accName)); - options.push_back(matioCpp::String("FrameName", m_frameName)); - matioCpp::CellArray consideredJoints("ConsideredJoints", {m_consideredJointNames.size(), 1}); - for (size_t i = 0; i < m_consideredJointNames.size(); ++i) - { - consideredJoints.setElement({i,0}, matioCpp::String(m_consideredJointNames[i])); - } - options.push_back(consideredJoints); - options.push_back(to_matio(iDynTree::toEigen(m_imuWorld), "I_R_world")); + options.push_back(tomatioCpp(m_imuName, "IMUSensorName")); + options.push_back(tomatioCpp(m_gyroName, "GyroName")); + options.push_back(tomatioCpp(m_accName, "AccelerometerName")); + options.push_back(tomatioCpp(m_frameName, "FrameName")); + options.push_back(tomatioCpp(m_consideredJointNames, "ConsideredJoints")); + options.push_back(tomatioCpp(m_imuWorld, "I_R_world")); matioCpp::Struct optionsStruct(m_logPrefix + "_options", options); @@ -858,7 +829,9 @@ bool MasImuTest::MasImuData::saveResults() return false; } - std::cerr << "Saving data" <outputFile); if (!(outputFile.isOpen())) @@ -879,6 +852,12 @@ bool MasImuTest::MasImuData::saveResults() return false; } + if (!outputFile.write(outputString)) + { + yError() << errorPrefix << "Failed to write the output string to file."; + return false; + } + return true; } @@ -921,10 +900,10 @@ void MasImuTest::reset() m_rightIMU.reset(); } -void MasImuTest::printResultsPrivate() const +void MasImuTest::printResultsPrivate() { - m_leftIMU.printResults(); - m_rightIMU.printResults(); + yInfo() << m_leftIMU.printResults(); + yInfo() << m_rightIMU.printResults(); } double MasImuTest::getPeriod() @@ -1150,15 +1129,15 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) } std::vector settings; - settings.push_back(matioCpp::String("robot_name", m_commonDataPtr->robotName)); - settings.push_back(matioCpp::Element("period", m_period)); - settings.push_back(matioCpp::String("model", pathToModel)); - settings.push_back(matioCpp::String("base_link", baseLink)); - settings.push_back(to_matio(iDynTree::toEigen(baseRotation), "base_rotation")); - settings.push_back(matioCpp::Element("filter_yaw", m_commonDataPtr->filterYaw)); - settings.push_back(matioCpp::Element("min_joint_variation_deg", minJointVariationInDeg)); - settings.push_back(matioCpp::Element("max_samples", m_commonDataPtr->maxSamples)); - settings.push_back(matioCpp::Element("mas_timeout", m_commonDataPtr->masTimeout)); + settings.push_back(tomatioCpp(m_commonDataPtr->robotName, "robot_name")); + settings.push_back(tomatioCpp(m_period, "period")); + settings.push_back(tomatioCpp(pathToModel, "model")); + settings.push_back(tomatioCpp(baseLink, "base_link")); + settings.push_back(tomatioCpp(baseRotation, "base_rotation")); + settings.push_back(tomatioCpp(m_commonDataPtr->filterYaw, "filter_yaw")); + settings.push_back(tomatioCpp(minJointVariationInDeg, "min_joint_variation_deg")); + settings.push_back(tomatioCpp(m_commonDataPtr->maxSamples, "max_samples")); + settings.push_back(tomatioCpp(m_commonDataPtr->masTimeout, "mas_timeout")); if (!outputFile.write(matioCpp::Struct("settings", settings))) { From 2960d94236b1d41c46685fca1b4a76ec1fd5d47d Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 24 Nov 2020 18:31:15 +0100 Subject: [PATCH 41/51] The frame index was taken from the full model, resulting in wrong calibration. --- utilities/MasImuTest/src/MasImuTest.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/MasImuTest/src/MasImuTest.cpp index 535e841b3f..65e0fd414c 100644 --- a/utilities/MasImuTest/src/MasImuTest.cpp +++ b/utilities/MasImuTest/src/MasImuTest.cpp @@ -100,6 +100,15 @@ bool MasImuTest::MasImuData::setupModel() ok = m_kinDyn.loadRobotModel(reducedModelLoader.model()); + m_frame = m_kinDyn.getFrameIndex(m_frameName); + + if (m_frame == iDynTree::FRAME_INVALID_INDEX) + { + yError() << errorPrefix << "The frame " << m_frameName << " does not exists in the reduced robot model." + << ". Configuration failed."; + return false; + } + if (!ok) { yError() << errorPrefix << "Failed to load the reduced model. Configuration failed."; From 98f0d00e26c8870694224aa0760dc8289f1ef462 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 24 Nov 2020 18:32:12 +0100 Subject: [PATCH 42/51] Added plotting script. --- utilities/MasImuTest/scripts/plotResults.m | 83 ++++++++++++++++++++++ 1 file changed, 83 insertions(+) create mode 100644 utilities/MasImuTest/scripts/plotResults.m diff --git a/utilities/MasImuTest/scripts/plotResults.m b/utilities/MasImuTest/scripts/plotResults.m new file mode 100644 index 0000000000..ba364be917 --- /dev/null +++ b/utilities/MasImuTest/scripts/plotResults.m @@ -0,0 +1,83 @@ +icubModelsInstallPrefix = getenv('ROBOTOLOGY_SUPERBUILD_INSTALL_PREFIX'); + +meshFilePrefix = [icubModelsInstallPrefix '/share']; + +robotName='iCubGenova04'; + +modelPath = [icubModelsInstallPrefix '/share/iCub/robots/' robotName '/']; +fileName='model.urdf'; + +datasets = {left_data, right_data}; +options = {left_options, right_options}; + + + +for dataIndex = 1 : length(datasets) + data = datasets{dataIndex}; + opt = options{dataIndex}; + rpyImu = zeros(3, length(datasets)); + rpyFK = zeros(3, length(datasets),1); + + jointOrder = opt.ConsideredJoints; + world_H_base = eye(4); + world_H_base(1:3, 1:3) = settings.base_rotation; + + KinDynModel = iDynTreeWrappers.loadReducedModel(jointOrder,settings.base_link,modelPath,fileName,false); + joints_positions = data(1).JointPositions_rad; + iDynTreeWrappers.setRobotState(KinDynModel,world_H_base,joints_positions,zeros(6,1),zeros(size(joints_positions)),[0,0,-9.81]); + figure('WindowState', 'maximized') + [visualizer,objects]=iDynTreeWrappers.prepareVisualization(KinDynModel,meshFilePrefix, 'transparency',1, 'reuseFigure', 'gcf'); + xlim([-0.8, 0.8]) + ylim([-0.8, 0.8]) + zlim([-1, 0.8]) + frameTransform = iDynTreeWrappers.getWorldTransform(KinDynModel, opt.FrameName); + wRimu = frameTransform(1:3, 1:3); + rpyFK(:, 1) = wbc.rollPitchYawFromRotation(frameTransform(1:3, 1:3)); + forward_kin_frame = iDynTreeWrappers.plotFrame(frameTransform, 0.2, 10); + rpy(1) = data(1).RPYfromIMUinDeg(3) * pi/180; + rpy(2) = -data(1).RPYfromIMUinDeg(2) * pi/180; + rpy(3) = -data(1).RPYfromIMUinDeg(1) * pi/180; + rpyImu(:, 1) = rpy; + frameTransform(1:3, 1:3) = wbc.rotz(rpy(3))*wbc.roty(rpy(2))*wbc.rotx(rpy(1)); + wRimu = wRimu * frameTransform(1:3, 1:3)'; + frameTransform(1:3, 1:3) = wRimu * frameTransform(1:3, 1:3); + imu_frame = iDynTreeWrappers.plotFrame(frameTransform, 0.4, 5); + or = frameTransform(1:3, 4); + acc = frameTransform * [data(1).Accelerometer'/9.81/2; 1]; + gravity = plot3([or(1) acc(1)], [or(2) acc(2)], [or(3) acc(3)], 'm', 'linewidth', 7); + pause + + for i = 2 : length(data) + joints_positions = data(i).JointPositions_rad; + iDynTreeWrappers.setRobotState(KinDynModel,world_H_base,joints_positions,zeros(6,1),zeros(size(joints_positions)),[0,0,-9.81]); + frameTransform = iDynTreeWrappers.getWorldTransform(KinDynModel, opt.FrameName); + rpyFK(:, i) = wbc.rollPitchYawFromRotation(frameTransform(1:3, 1:3)); + iDynTreeWrappers.updateFrame(forward_kin_frame, frameTransform); + rpy(1) = data(i).RPYfromIMUinDeg(3) * pi/180; + rpy(2) = -data(i).RPYfromIMUinDeg(2) * pi/180; + rpy(3) = -data(i).RPYfromIMUinDeg(1) * pi/180; + rpyImu(:, i) = rpy; + frameTransform(1:3, 1:3) = wRimu * wbc.rotz(rpy(3))*wbc.roty(rpy(2))*wbc.rotx(rpy(1)); + iDynTreeWrappers.updateFrame(imu_frame, frameTransform); + iDynTreeWrappers.updateVisualization(KinDynModel,visualizer); + or = frameTransform(1:3, 4); + acc = frameTransform * [data(i).Accelerometer'/9.81/2; 1]; + set(gravity, 'XData', [or(1) acc(1)], 'YData', [or(2) acc(2)], 'ZData', [or(3) acc(3)]); + + drawnow() + pause(0.01) + end + figure + plot(rpyFK') + title('FK') + legend('Roll', 'Pitch', 'Yaw') + figure + plot(rpyImu') + title('IMU') + legend('Roll', 'Pitch', 'Yaw') + + if (dataIndex ~= length(datasets)) + pause + end + +end \ No newline at end of file From 15a35907070eaff323d09d92196c884cd9c5f1e2 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 25 Nov 2020 10:20:03 +0100 Subject: [PATCH 43/51] Renamed MasImuTest to follow utilities convention. Updated the CMakeLists to use the CMake macro. Updated the README. --- utilities/CMakeLists.txt | 2 +- utilities/MasImuTest/CMakeLists.txt | 51 ------------------- utilities/mas-imu-test/CMakeLists.txt | 36 +++++++++++++ .../{MasImuTest => mas-imu-test}/README.md | 11 ++-- .../iCubGazeboV2_5/MasImuTestConfig.ini | 0 .../robots/iCubGenova04/MasImuTestConfig.ini | 0 .../include/BipedalLocomotion/MasImuTest.h | 0 .../scripts/plotResults.m | 0 .../src/MasImuTest.cpp | 0 .../{MasImuTest => mas-imu-test}/src/main.cpp | 0 .../thrifts/MasImuTestCommands.thrift | 0 11 files changed, 44 insertions(+), 56 deletions(-) delete mode 100644 utilities/MasImuTest/CMakeLists.txt create mode 100644 utilities/mas-imu-test/CMakeLists.txt rename utilities/{MasImuTest => mas-imu-test}/README.md (79%) rename utilities/{MasImuTest => mas-imu-test}/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini (100%) rename utilities/{MasImuTest => mas-imu-test}/app/robots/iCubGenova04/MasImuTestConfig.ini (100%) rename utilities/{MasImuTest => mas-imu-test}/include/BipedalLocomotion/MasImuTest.h (100%) rename utilities/{MasImuTest => mas-imu-test}/scripts/plotResults.m (100%) rename utilities/{MasImuTest => mas-imu-test}/src/MasImuTest.cpp (100%) rename utilities/{MasImuTest => mas-imu-test}/src/main.cpp (100%) rename utilities/{MasImuTest => mas-imu-test}/thrifts/MasImuTestCommands.thrift (100%) diff --git a/utilities/CMakeLists.txt b/utilities/CMakeLists.txt index 9d7b8875d0..1419035622 100644 --- a/utilities/CMakeLists.txt +++ b/utilities/CMakeLists.txt @@ -5,4 +5,4 @@ add_subdirectory(joint-position-tracking) add_subdirectory(joint-trajectory-player) -add_subdirectory(MasImuTest) +add_subdirectory(mas-imu-test) diff --git a/utilities/MasImuTest/CMakeLists.txt b/utilities/MasImuTest/CMakeLists.txt deleted file mode 100644 index c20c8775f3..0000000000 --- a/utilities/MasImuTest/CMakeLists.txt +++ /dev/null @@ -1,51 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -# set target name -if(FRAMEWORK_COMPILE_MasImuTest) - - set(EXE_TARGET_NAME MasImuTest) - - # set cpp files - set(${EXE_TARGET_NAME}_SRC - src/main.cpp - src/MasImuTest.cpp - ) - - # set hpp files - set(${EXE_TARGET_NAME}_HDR - include/BipedalLocomotion/MasImuTest.h - ) - - set(${EXE_TARGET_NAME}_THRIFT_HDR - thrifts/MasImuTestCommands.thrift - ) - - yarp_add_idl(${EXE_TARGET_NAME}_THRIFT_GEN_FILES ${${EXE_TARGET_NAME}_THRIFT_HDR}) - - add_executable(${EXE_TARGET_NAME} ${${EXE_TARGET_NAME}_SRC} ${${EXE_TARGET_NAME}_HDR} ${${EXE_TARGET_NAME}_THRIFT_GEN_FILES}) - - target_link_libraries(${EXE_TARGET_NAME} PUBLIC ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} - BipedalLocomotion::GenericContainer BipedalLocomotion::YarpUtilities BipedalLocomotion::ParametersHandlerYarpImplementation BipedalLocomotion::matioCppConversions) - target_compile_features(${EXE_TARGET_NAME} PUBLIC cxx_std_17) - - # Specify include directories for both compilation and installation process. - # The $ generator expression is useful to ensure to create - # relocatable configuration files, see https://cmake.org/cmake/help/latest/manual/cmake-packages.7.html#creating-relocatable-packages - target_include_directories(${EXE_TARGET_NAME} PUBLIC "$" - "$/${CMAKE_INSTALL_INCLUDEDIR}>") - - # Specify installation targets, typology and destination folders. - install(TARGETS ${EXE_TARGET_NAME} - EXPORT ${PROJECT_NAME} - COMPONENT runtime - LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib - ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib - RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin) - - install_ini_files(${CMAKE_CURRENT_SOURCE_DIR}/app) - - message(STATUS "Created target ${EXE_TARGET_NAME} for export ${PROJECT_NAME}.") - -endif() diff --git a/utilities/mas-imu-test/CMakeLists.txt b/utilities/mas-imu-test/CMakeLists.txt new file mode 100644 index 0000000000..c52373502c --- /dev/null +++ b/utilities/mas-imu-test/CMakeLists.txt @@ -0,0 +1,36 @@ +# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +# set target name +if(FRAMEWORK_COMPILE_MasImuTest) + + set(EXE_TARGET_NAME mas-imu-test) + + # set cpp files + set(${EXE_TARGET_NAME}_SRC + src/main.cpp + src/MasImuTest.cpp + ) + + # set hpp files + set(${EXE_TARGET_NAME}_HDR + include/BipedalLocomotion/MasImuTest.h + ) + + set(${EXE_TARGET_NAME}_THRIFT_HDR + thrifts/MasImuTestCommands.thrift + ) + + yarp_add_idl(${EXE_TARGET_NAME}_THRIFT_GEN_FILES ${${EXE_TARGET_NAME}_THRIFT_HDR}) + + add_bipedal_locomotion_application( + NAME ${EXE_TARGET_NAME} + SOURCES ${${EXE_TARGET_NAME}_SRC} + HEADERS ${${EXE_TARGET_NAME}_HDR} ${${EXE_TARGET_NAME}_THRIFT_GEN_FILES} + LINK_LIBRARIES ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} BipedalLocomotion::GenericContainer BipedalLocomotion::YarpUtilities BipedalLocomotion::ParametersHandlerYarpImplementation BipedalLocomotion::matioCppConversions + ) + + install_ini_files(${CMAKE_CURRENT_SOURCE_DIR}/app) + +endif() diff --git a/utilities/MasImuTest/README.md b/utilities/mas-imu-test/README.md similarity index 79% rename from utilities/MasImuTest/README.md rename to utilities/mas-imu-test/README.md index 559a01164c..7330f97aca 100644 --- a/utilities/MasImuTest/README.md +++ b/utilities/mas-imu-test/README.md @@ -1,10 +1,10 @@ # MAS IMU Test -The ``MasImuTest`` was designed with the intent of testing whether the orientation provided by the IMU sensors is coherent with the encoder measurements. +The ``mas-imu-test`` was designed with the intent of testing whether the orientation provided by the IMU sensors is coherent with the encoder measurements. The test requires the operator to move manually the limbs where the sensors are located. In this way, the test acquires data to detect if the IMU measurements and the encoder measurements coincide. ## Installation -The ``MasImuTest`` is part of the ``BipedalLocomotionFramework``. It is compiled if the ``CMake`` option ``FRAMEWORK_COMPILE_MasImuTest`` is set to ``ON``. To install, follow the instructions for ``BipedalLocomotionFramework``. In order for the executable to be easily launched, add the folder +The ``mas-imu-test`` is part of the ``BipedalLocomotionFramework``. It is compiled if the ``CMake`` option ``FRAMEWORK_COMPILE_MasImuTest`` is set to ``ON``. To install, follow the instructions for ``BipedalLocomotionFramework``. In order for the executable to be easily launched, add the folder ``` /path/to/install/bin ``` @@ -22,7 +22,7 @@ to the ``YARP_DATA_DIRS`` environmental variable. ## Running the test Simply run ``` -MasImuTest +blf-mas-imu-test ``` in a terminal. The test should run automatically. @@ -51,9 +51,12 @@ The configuration file presents the following data: - ``max_samples 500`` The max number of samples considered in the test. - ``mas_timeout 0.02`` Timeout for reading the MAS IMU sensor. A warning is thrown if this timeout is not respected. - ``auto_start true`` The test start automatically without having to use the RPC interface. +- ``file_name masImuTestOutput.mat`` The name of the mat file where data is logged. The following part of the configuration file contains two part which are equivalent, one for the left leg, having the tag ``[LEFT_LEG]`` and one for the right leg. We will detail here only the part for the left leg since the other is equivalent. - ``remote left_leg/inertials`` The remote port from where the output of the IMU is streamed. - ``imu_frame l_foot_ft_acc_3b13`` The name of the frame in the URDF corresponding to the IMU to check. -- ``sensor_name l_foot_ft_eul_3b13`` The name of the sensor to check. +- ``imu_sensor_name l_foot_ft_eul_3b13`` The name of the IMU sensor to check. +- ``gyro_sensor_name l_foot_ft_gyro_3b13`` The name of the gyro sensor attached to the IMU sensor. +- ``acc_sensor_name l_foot_ft_acc_3b13`` The name of the accelerometer attached to the IMU sensor. - ``remote_control_boards ("left_leg")`` The comma-separated list of control boards including all the joints connecting the sensor under testing to the base link. diff --git a/utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini b/utilities/mas-imu-test/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini similarity index 100% rename from utilities/MasImuTest/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini rename to utilities/mas-imu-test/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini diff --git a/utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini b/utilities/mas-imu-test/app/robots/iCubGenova04/MasImuTestConfig.ini similarity index 100% rename from utilities/MasImuTest/app/robots/iCubGenova04/MasImuTestConfig.ini rename to utilities/mas-imu-test/app/robots/iCubGenova04/MasImuTestConfig.ini diff --git a/utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h b/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h similarity index 100% rename from utilities/MasImuTest/include/BipedalLocomotion/MasImuTest.h rename to utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h diff --git a/utilities/MasImuTest/scripts/plotResults.m b/utilities/mas-imu-test/scripts/plotResults.m similarity index 100% rename from utilities/MasImuTest/scripts/plotResults.m rename to utilities/mas-imu-test/scripts/plotResults.m diff --git a/utilities/MasImuTest/src/MasImuTest.cpp b/utilities/mas-imu-test/src/MasImuTest.cpp similarity index 100% rename from utilities/MasImuTest/src/MasImuTest.cpp rename to utilities/mas-imu-test/src/MasImuTest.cpp diff --git a/utilities/MasImuTest/src/main.cpp b/utilities/mas-imu-test/src/main.cpp similarity index 100% rename from utilities/MasImuTest/src/main.cpp rename to utilities/mas-imu-test/src/main.cpp diff --git a/utilities/MasImuTest/thrifts/MasImuTestCommands.thrift b/utilities/mas-imu-test/thrifts/MasImuTestCommands.thrift similarity index 100% rename from utilities/MasImuTest/thrifts/MasImuTestCommands.thrift rename to utilities/mas-imu-test/thrifts/MasImuTestCommands.thrift From 001e8ad64c49622abe6c45daa6a2a04ab368ff56 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 25 Nov 2020 14:39:36 +0100 Subject: [PATCH 44/51] Added possibility to remap the RPY angles from conf file. --- utilities/mas-imu-test/README.md | 1 + .../iCubGazeboV2_5/MasImuTestConfig.ini | 4 + .../robots/iCubGenova04/MasImuTestConfig.ini | 5 ++ .../include/BipedalLocomotion/MasImuTest.h | 9 +- utilities/mas-imu-test/src/MasImuTest.cpp | 88 ++++++++++++++++--- 5 files changed, 95 insertions(+), 12 deletions(-) diff --git a/utilities/mas-imu-test/README.md b/utilities/mas-imu-test/README.md index 7330f97aca..258418b099 100644 --- a/utilities/mas-imu-test/README.md +++ b/utilities/mas-imu-test/README.md @@ -60,3 +60,4 @@ The following part of the configuration file contains two part which are equival - ``gyro_sensor_name l_foot_ft_gyro_3b13`` The name of the gyro sensor attached to the IMU sensor. - ``acc_sensor_name l_foot_ft_acc_3b13`` The name of the accelerometer attached to the IMU sensor. - ``remote_control_boards ("left_leg")`` The comma-separated list of control boards including all the joints connecting the sensor under testing to the base link. +- ``rpy_shuffling ("roll", "pitch", "yaw")`` It allows shuffling the rpy order. Use only the "roll", "pitch", "yaw" keywords. It is possible to prepend "-", e.g. "-pitch" to change the sign. diff --git a/utilities/mas-imu-test/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini b/utilities/mas-imu-test/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini index 337b3af920..d4d9db9304 100644 --- a/utilities/mas-imu-test/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini +++ b/utilities/mas-imu-test/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini @@ -37,6 +37,8 @@ acc_sensor_name l_foot_ft_acc_3b13 remote_control_boards ("left_leg") +rpy_shuffling ("roll", "pitch", "yaw") + [RIGHT_LEG] remote right_foot/inertials @@ -50,3 +52,5 @@ gyro_sensor_name r_foot_ft_acc_3b14 acc_sensor_name r_foot_ft_acc_3b14 remote_control_boards ("right_leg") + +rpy_shuffling ("roll", "pitch", "yaw") diff --git a/utilities/mas-imu-test/app/robots/iCubGenova04/MasImuTestConfig.ini b/utilities/mas-imu-test/app/robots/iCubGenova04/MasImuTestConfig.ini index 20f43f293b..c44cc2b41a 100644 --- a/utilities/mas-imu-test/app/robots/iCubGenova04/MasImuTestConfig.ini +++ b/utilities/mas-imu-test/app/robots/iCubGenova04/MasImuTestConfig.ini @@ -37,6 +37,8 @@ acc_sensor_name l_foot_ft_acc_3b13 remote_control_boards ("left_leg") +rpy_shuffling ("yaw", "-pitch", "roll") + [RIGHT_LEG] remote right_leg/inertials @@ -50,3 +52,6 @@ gyro_sensor_name r_foot_ft_gyro_3b14 acc_sensor_name r_foot_ft_acc_3b14 remote_control_boards ("right_leg") + +rpy_shuffling ("yaw", "-pitch", "roll") + diff --git a/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h b/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h index 579db3f666..ab41bd4a45 100644 --- a/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h +++ b/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h @@ -28,6 +28,9 @@ #include #include +// Eigen +#include + // matioCpp #include @@ -74,13 +77,14 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe yarp::dev::IThreeAxisLinearAccelerometers* m_accInterface; yarp::dev::IEncodersTimed* m_encodersInterface; size_t m_imuSensorIndex, m_gyroSensorIndex, m_accSensorIndex; - yarp::sig::Vector m_positionFeedbackDeg; /**< Current joint position [deg]. */ + yarp::sig::Vector m_positionFeedbackDeg; yarp::sig::Vector m_rpyInDeg, m_gyroInDeg_s, m_acc; - iDynTree::Vector3 m_rpyInRad; + Eigen::Vector3d m_rpyInDegRemapped; iDynTree::VectorDynSize m_positionFeedbackInRad; iDynTree::VectorDynSize m_previousPositionFeedbackInRad; iDynTree::VectorDynSize m_dummyVelocity; iDynTree::Rotation m_rotationFeedback; + Eigen::Matrix3Xd m_rpyMapping; iDynTree::Rotation m_rotationFromEncoders; iDynTree::Rotation m_imuWorld; //i_R_imuworld @@ -91,6 +95,7 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe std::vector m_rotationFeedbackInInertialData; std::vector m_rotationFeedbackInInertialYawFilteredData; std::vector m_rpyImuData; + std::vector m_rpyRemappedData; std::vector m_rotationFromEncodersData; std::vector m_gyroData; std::vector m_accData; diff --git a/utilities/mas-imu-test/src/MasImuTest.cpp b/utilities/mas-imu-test/src/MasImuTest.cpp index 65e0fd414c..5dace0b197 100644 --- a/utilities/mas-imu-test/src/MasImuTest.cpp +++ b/utilities/mas-imu-test/src/MasImuTest.cpp @@ -18,6 +18,8 @@ #include #include #include +#include +#include using namespace BipedalLocomotion; using namespace BipedalLocomotion::GenericContainer; @@ -33,6 +35,7 @@ void MasImuTest::MasImuData::reserveData() m_rotationFromEncodersData.reserve(m_commonDataPtr->maxSamples); m_jointsPositionData.reserve(m_commonDataPtr->maxSamples); m_rpyImuData.reserve(m_commonDataPtr->maxSamples); + m_rpyRemappedData.reserve(m_commonDataPtr->maxSamples); m_gyroData.reserve(m_commonDataPtr->maxSamples); m_accData.reserve(m_commonDataPtr->maxSamples); } @@ -48,6 +51,7 @@ void MasImuTest::MasImuData::clearData() m_rpyImuData.clear(); m_gyroData.clear(); m_accData.clear(); + m_rpyRemappedData.clear(); } bool MasImuTest::MasImuData::setupModel() @@ -201,6 +205,62 @@ bool MasImuTest::MasImuData::setupOrientationSensor() m_rpyInDeg.resize(3); + std::vector rpy_shuffling; + ok = m_group->getParameter("rpy_shuffling", rpy_shuffling); + + if (!ok) + { + yError() << errorPrefix << "Setup failed."; + return false; + } + + if (rpy_shuffling.size() != 3) + { + yError() << errorPrefix << "The rpy_shuffling parameter is supposed to a list of three strings."; + return false; + } + + m_rpyMapping.setZero(); + + for (size_t i = 0; i < 3; ++i) + { + std::string angle = rpy_shuffling[i]; + double sign = +1; + if (angle.size() == 0) + { + yError() << errorPrefix << "The rpy_shuffling parameter contains a null string at position " << i << " (0-based)."; + return false; + } + + if (angle[0] == '-') + { + sign = -1; + angle.erase(angle.begin()); + } + + std::transform(angle.begin(), angle.end(), angle.begin(), + [](unsigned char c){ return std::tolower(c); }); + + if (angle == "roll") + { + m_rpyMapping(i,0) = sign; + } + else if (angle == "pitch") + { + m_rpyMapping(i,1) = sign; + } + else if (angle == "yaw") + { + m_rpyMapping(i,2) = sign; + } + else + { + yError() << errorPrefix << "\"" << rpy_shuffling[i] + << "\" is not a recognized keyword in rpy_shuffling. Use only \"roll\", \"pitch\" or \"yaw\", eventually with a \"-\" in front." ; + return false; + } + } + return true; } @@ -425,11 +485,11 @@ bool MasImuTest::MasImuData::getFeedback() m_positionFeedbackInRad(j) = iDynTree::deg2rad(m_positionFeedbackDeg(j)); } - m_rpyInRad(0) = iDynTree::deg2rad(m_rpyInDeg[0]); - m_rpyInRad(1) = iDynTree::deg2rad(m_rpyInDeg[1]); - m_rpyInRad(2) = iDynTree::deg2rad(m_rpyInDeg[2]); + m_rpyInDegRemapped = m_rpyMapping * to_eigen(m_rpyInDeg); - m_rotationFeedback = iDynTree::Rotation::RPY(m_rpyInRad(0), m_rpyInRad(1), m_rpyInRad(2)); + m_rotationFeedback = iDynTree::Rotation::RPY(iDynTree::deg2rad(m_rpyInDegRemapped(0)), + iDynTree::deg2rad(m_rpyInDegRemapped(1)), + iDynTree::deg2rad(m_rpyInDegRemapped(2))); return true; } @@ -610,6 +670,7 @@ bool MasImuTest::MasImuData::addSample() } m_rpyImuData.push_back(m_rpyInDeg); + m_rpyRemappedData.push_back(m_rpyInDegRemapped); m_jointsPositionData.push_back(m_positionFeedbackInRad); m_rotationFromEncodersData.push_back(m_rotationFromEncoders); m_rotationFeedbackData.push_back(m_rotationFeedback); @@ -761,6 +822,7 @@ bool MasImuTest::MasImuData::saveResults() "RotationFromEncoders", "JointPositions_rad", "RPYfromIMUinDeg", + "RPYfromIMUinDegRemapped", "AngularVelocity_deg_s", "Accelerometer"}); @@ -775,37 +837,43 @@ bool MasImuTest::MasImuData::saveResults() if(!el.setField(tomatioCpp(m_rotationFeedbackData[i], "RotationFromIMU"))) { - yError() << errorPrefix << "Failed to set the field RotationError."; + yError() << errorPrefix << "Failed to set the field RotationFromIMU."; return false; } if(!el.setField(tomatioCpp(m_rotationFeedbackInInertialData[i], "RotationFromIMUInInertial"))) { - yError() << errorPrefix << "Failed to set the field RotationError."; + yError() << errorPrefix << "Failed to set the field RotationFromIMUInInertial."; return false; } if(!el.setField(tomatioCpp(m_rotationFeedbackInInertialYawFilteredData[i], "RotationFromIMUInInertialYawFiltered"))) { - yError() << errorPrefix << "Failed to set the field RotationError."; + yError() << errorPrefix << "Failed to set the field RotationFromIMUInInertialYawFiltered."; return false; } if(!el.setField(tomatioCpp(m_rotationFromEncodersData[i], "RotationFromEncoders"))) { - yError() << errorPrefix << "Failed to set the field RotationError."; + yError() << errorPrefix << "Failed to set the field RotationFromEncoders."; return false; } if(!el.setField(tomatioCpp(m_jointsPositionData[i], "JointPositions_rad"))) { - yError() << errorPrefix << "Failed to set the field RotationError."; + yError() << errorPrefix << "Failed to set the field JointPositions_rad."; return false; } if(!el.setField(tomatioCpp(m_rpyImuData[i], "RPYfromIMUinDeg"))) { - yError() << errorPrefix << "Failed to set the field RotationError."; + yError() << errorPrefix << "Failed to set the field RPYfromIMUinDeg."; + return false; + } + + if(!el.setField(tomatioCpp(m_rpyRemappedData[i], "RPYfromIMUinDegRemapped"))) + { + yError() << errorPrefix << "Failed to set the field RPYfromIMUinDegRemapped."; return false; } From c2db6ba46159d58b264f3fc70ada9f4cdc918cae Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 25 Nov 2020 16:02:18 +0100 Subject: [PATCH 45/51] Made imu tests more independent. Prepare the ground to have an arbitrary number of MAS IMU tests. --- utilities/mas-imu-test/README.md | 4 + .../iCubGazeboV2_5/MasImuTestConfig.ini | 10 ++ .../robots/iCubGenova04/MasImuTestConfig.ini | 10 ++ .../include/BipedalLocomotion/MasImuTest.h | 15 ++- utilities/mas-imu-test/src/MasImuTest.cpp | 119 ++++++++++-------- 5 files changed, 100 insertions(+), 58 deletions(-) diff --git a/utilities/mas-imu-test/README.md b/utilities/mas-imu-test/README.md index 258418b099..e2bda4eec9 100644 --- a/utilities/mas-imu-test/README.md +++ b/utilities/mas-imu-test/README.md @@ -52,8 +52,12 @@ The configuration file presents the following data: - ``mas_timeout 0.02`` Timeout for reading the MAS IMU sensor. A warning is thrown if this timeout is not respected. - ``auto_start true`` The test start automatically without having to use the RPC interface. - ``file_name masImuTestOutput.mat`` The name of the mat file where data is logged. +- ``tests ("LEFT_LEG", "RIGHT_LEG")`` The set of tests to be performed. The name should correspond to a group with the structure described below. + The following part of the configuration file contains two part which are equivalent, one for the left leg, having the tag ``[LEFT_LEG]`` and one for the right leg. We will detail here only the part for the left leg since the other is equivalent. +- ``pretty_name "Left IMU Test"`` A pretty name for the test +- ``log_prefix left`` The name used for the test in the logged data (it should start with a letter, and contain only alphanumeric charachters or "_"). - ``remote left_leg/inertials`` The remote port from where the output of the IMU is streamed. - ``imu_frame l_foot_ft_acc_3b13`` The name of the frame in the URDF corresponding to the IMU to check. - ``imu_sensor_name l_foot_ft_eul_3b13`` The name of the IMU sensor to check. diff --git a/utilities/mas-imu-test/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini b/utilities/mas-imu-test/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini index d4d9db9304..e6baa8d6d4 100644 --- a/utilities/mas-imu-test/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini +++ b/utilities/mas-imu-test/app/robots/iCubGazeboV2_5/MasImuTestConfig.ini @@ -23,8 +23,14 @@ auto_start true file_name masImuTestOutput.mat +tests ("LEFT_LEG", "RIGHT_LEG") + [LEFT_LEG] +pretty_name "Left IMU Test" + +log_prefix left + remote left_foot/inertials imu_frame l_foot_ft_acc_3b13 @@ -41,6 +47,10 @@ rpy_shuffling ("roll", "pitch", "yaw") [RIGHT_LEG] +pretty_name "Right IMU Test" + +log_prefix right + remote right_foot/inertials imu_frame r_foot_ft_acc_3b14 diff --git a/utilities/mas-imu-test/app/robots/iCubGenova04/MasImuTestConfig.ini b/utilities/mas-imu-test/app/robots/iCubGenova04/MasImuTestConfig.ini index c44cc2b41a..f201e5b693 100644 --- a/utilities/mas-imu-test/app/robots/iCubGenova04/MasImuTestConfig.ini +++ b/utilities/mas-imu-test/app/robots/iCubGenova04/MasImuTestConfig.ini @@ -23,8 +23,14 @@ auto_start true file_name masImuTestOutput.mat +tests ("LEFT_LEG", "RIGHT_LEG") + [LEFT_LEG] +pretty_name "Left IMU Test" + +log_prefix left + remote left_leg/inertials imu_frame l_foot_ft_acc_3b13 @@ -41,6 +47,10 @@ rpy_shuffling ("yaw", "-pitch", "roll") [RIGHT_LEG] +pretty_name "Right IMU Test" + +log_prefix right + remote right_leg/inertials imu_frame r_foot_ft_acc_3b14 diff --git a/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h b/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h index ab41bd4a45..b0a049eb9a 100644 --- a/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h +++ b/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h @@ -57,7 +57,6 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe int maxSamples; double minJointVariationRad; double masTimeout; - std::string outputFile; }; class MasImuData @@ -128,9 +127,8 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe public: - bool setup(const std::string& testName, - BipedalLocomotion::ParametersHandler::YarpImplementation::shared_ptr group, - std::shared_ptr commonDataPtr, const std::string &logPrefix); + bool setup(BipedalLocomotion::ParametersHandler::YarpImplementation::shared_ptr group, + std::shared_ptr commonDataPtr); bool firstRun(); @@ -144,11 +142,13 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe std::string printResults(); - bool saveResults(); + bool saveResults(matioCpp::Struct &logStruct); void reset(); bool close(); + + const std::string& name(); }; enum class State @@ -165,12 +165,15 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe MasImuData m_leftIMU, m_rightIMU; State m_state{State::STARTED}; std::mutex m_mutex; - yarp::os::Port m_rpcPort; /**< Remote Procedure Call port. */ + yarp::os::Port m_rpcPort; + matioCpp::File m_outputFile; void reset(); void printResultsPrivate(); + void saveResultsPrivate(); + public: /** diff --git a/utilities/mas-imu-test/src/MasImuTest.cpp b/utilities/mas-imu-test/src/MasImuTest.cpp index 5dace0b197..e11bfaa873 100644 --- a/utilities/mas-imu-test/src/MasImuTest.cpp +++ b/utilities/mas-imu-test/src/MasImuTest.cpp @@ -559,19 +559,31 @@ double MasImuTest::MasImuData::maxVariation() return maxVariation; } -bool MasImuTest::MasImuData::setup(const std::string &testName, ParametersHandler::YarpImplementation::shared_ptr group, - std::shared_ptr commonDataPtr, const std::string& logPrefix) +bool MasImuTest::MasImuData::setup(ParametersHandler::YarpImplementation::shared_ptr group, + std::shared_ptr commonDataPtr) { - m_testName = testName; - m_logPrefix = logPrefix; m_commonDataPtr = commonDataPtr; m_group = group; + bool ok = group->getParameter("pretty_name", m_testName); + if (!ok) + { + yError() << "[MasImuTest::MasImuData::setup] Failed to fetch \"pretty_name\" from configuration files."; + return false; + } + reserveData(); std::string errorPrefix = "[MasImuTest::MasImuData::setup](" + m_testName +") "; - bool ok = setupModel(); + ok = group->getParameter("log_prefix", m_logPrefix); + if (!ok) + { + yError() << "[MasImuTest::MasImuData::setup] Failed to fetch \"log_prefix\" from configuration files."; + return false; + } + + ok = setupModel(); if (!ok) { yError() << errorPrefix << "setupModel failed."; @@ -811,20 +823,20 @@ std::string MasImuTest::MasImuData::printResults() return m_output; } -bool MasImuTest::MasImuData::saveResults() +bool MasImuTest::MasImuData::saveResults(matioCpp::Struct& logStruct) { std::string errorPrefix = "[MasImuTest::MasImuData::saveResults](" + m_testName +") "; - matioCpp::StructArray dataArray(m_logPrefix + "_data", {m_errorData.size(), 1}, {"RotationError", - "RotationFromIMU", - "RotationFromIMUInInertial", - "RotationFromIMUInInertialYawFiltered", - "RotationFromEncoders", - "JointPositions_rad", - "RPYfromIMUinDeg", - "RPYfromIMUinDegRemapped", - "AngularVelocity_deg_s", - "Accelerometer"}); + matioCpp::StructArray dataArray("data", {m_errorData.size(), 1}, {"RotationError", + "RotationFromIMU", + "RotationFromIMUInInertial", + "RotationFromIMUInInertialYawFiltered", + "RotationFromEncoders", + "JointPositions_rad", + "RPYfromIMUinDeg", + "RPYfromIMUinDegRemapped", + "AngularVelocity_deg_s", + "Accelerometer"}); for (size_t i = 0; i < m_errorData.size(); ++i) { @@ -897,8 +909,9 @@ bool MasImuTest::MasImuData::saveResults() options.push_back(tomatioCpp(m_frameName, "FrameName")); options.push_back(tomatioCpp(m_consideredJointNames, "ConsideredJoints")); options.push_back(tomatioCpp(m_imuWorld, "I_R_world")); + options.push_back(tomatioCpp(m_rpyMapping, "RPYMapping")); - matioCpp::Struct optionsStruct(m_logPrefix + "_options", options); + matioCpp::Struct optionsStruct("options", options); if (!optionsStruct.isValid()) { @@ -906,32 +919,13 @@ bool MasImuTest::MasImuData::saveResults() return false; } - matioCpp::String outputString(m_logPrefix + "_outputString", m_output); - - yInfo() << errorPrefix << "Saving data"; - - matioCpp::File outputFile(m_commonDataPtr->outputFile); - if (!(outputFile.isOpen())) - { - yError() << errorPrefix << "Failed to open file."; - return false; - } - - if (!outputFile.write(dataArray)) - { - yError() << errorPrefix << "Failed to write the data array to file."; - return false; - } + matioCpp::String outputString("outputString", m_output); - if (!outputFile.write(optionsStruct)) - { - yError() << errorPrefix << "Failed to write the options struct to file."; - return false; - } + matioCpp::Struct testStruct(m_logPrefix, {dataArray, optionsStruct, outputString}); - if (!outputFile.write(outputString)) + if (!logStruct.setField(testStruct)) { - yError() << errorPrefix << "Failed to write the output string to file."; + yError() << errorPrefix << "Failed to add the \"" << m_logPrefix << "\" field to logging struct."; return false; } @@ -948,12 +942,6 @@ bool MasImuTest::MasImuData::close() { std::string errorPrefix = "[MasImuTest::MasImuData::close](" + m_testName +") "; - if (!saveResults()) - { - yError() << errorPrefix << "Unable to save the results."; - return false; - } - if(!m_orientationDriver.close()) { yError() << errorPrefix << "Unable to close the orientation driver."; @@ -969,6 +957,11 @@ bool MasImuTest::MasImuData::close() return true; } +const std::string &MasImuTest::MasImuData::name() +{ + return m_testName; +} + void MasImuTest::reset() { @@ -983,6 +976,27 @@ void MasImuTest::printResultsPrivate() yInfo() << m_rightIMU.printResults(); } +void MasImuTest::saveResultsPrivate() +{ + matioCpp::Struct testData("tests"); + + bool okL = m_leftIMU.saveResults(testData); + if (!okL) + { + yError() << "[MasImuTest::saveResultsPrivate] Failed to save the " << m_leftIMU.name() << " data."; + } + bool okR = m_rightIMU.saveResults(testData); + if (!okR) + { + yError() << "[MasImuTest::saveResultsPrivate] Failed to save the " << m_rightIMU.name() << " data."; + } + + if (!m_outputFile.write(testData)) + { + yError() << "[MasImuTest::saveResultsPrivate] Failed to save data struct to file."; + } +} + double MasImuTest::getPeriod() { std::lock_guard lock(m_mutex); @@ -1173,7 +1187,7 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) return false; } - ok = m_leftIMU.setup("Left IMU Test", leftLegGroup, m_commonDataPtr, "left"); + ok = m_leftIMU.setup(leftLegGroup, m_commonDataPtr); if (!ok) { yError() << "[MasImuTest::configure] Configuration failed."; @@ -1187,7 +1201,7 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) return false; } - ok = m_rightIMU.setup("Right IMU Test", rightLegGroup, m_commonDataPtr, "right"); + ok = m_rightIMU.setup(rightLegGroup, m_commonDataPtr); if (!ok) { yError() << "[MasImuTest::configure] Configuration failed."; @@ -1196,10 +1210,9 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) matioCpp::File::Delete(outputFileName); - matioCpp::File outputFile = matioCpp::File::Create(outputFileName); - m_commonDataPtr->outputFile = outputFileName; + m_outputFile = matioCpp::File::Create(outputFileName); - if (!(outputFile.isOpen())) + if (!(m_outputFile.isOpen())) { yError() << "[MasImuTest::configure] Failed to create new file with the name " << outputFileName <<"."; return false; @@ -1216,7 +1229,7 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) settings.push_back(tomatioCpp(m_commonDataPtr->maxSamples, "max_samples")); settings.push_back(tomatioCpp(m_commonDataPtr->masTimeout, "mas_timeout")); - if (!outputFile.write(matioCpp::Struct("settings", settings))) + if (!m_outputFile.write(matioCpp::Struct("settings", settings))) { yError() << "[MasImuTest::configure] Error while writing the settings to file"; return false; @@ -1258,6 +1271,8 @@ bool MasImuTest::close() { std::lock_guard lock(m_mutex); + saveResultsPrivate(); + m_rpcPort.close(); bool okL = m_leftIMU.close(); From 8916201ff80f0668d2b86b5873e1dc0467fee5ed Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 25 Nov 2020 17:31:56 +0100 Subject: [PATCH 46/51] Added the possibility to run an arbitrary number of imu tests. --- .../include/BipedalLocomotion/MasImuTest.h | 4 +- utilities/mas-imu-test/src/MasImuTest.cpp | 116 +++++++++--------- 2 files changed, 61 insertions(+), 59 deletions(-) diff --git a/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h b/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h index b0a049eb9a..7784a55ba4 100644 --- a/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h +++ b/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h @@ -148,7 +148,7 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe bool close(); - const std::string& name(); + const std::string& name() const; }; enum class State @@ -162,7 +162,7 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe BipedalLocomotion::ParametersHandler::YarpImplementation::shared_ptr m_parametersPtr; std::shared_ptr m_commonDataPtr; double m_period; - MasImuData m_leftIMU, m_rightIMU; + std::vector> m_tests; State m_state{State::STARTED}; std::mutex m_mutex; yarp::os::Port m_rpcPort; diff --git a/utilities/mas-imu-test/src/MasImuTest.cpp b/utilities/mas-imu-test/src/MasImuTest.cpp index e11bfaa873..1382991db6 100644 --- a/utilities/mas-imu-test/src/MasImuTest.cpp +++ b/utilities/mas-imu-test/src/MasImuTest.cpp @@ -957,7 +957,7 @@ bool MasImuTest::MasImuData::close() return true; } -const std::string &MasImuTest::MasImuData::name() +const std::string &MasImuTest::MasImuData::name() const { return m_testName; } @@ -966,29 +966,33 @@ const std::string &MasImuTest::MasImuData::name() void MasImuTest::reset() { m_state = State::PREPARED; - m_leftIMU.reset(); - m_rightIMU.reset(); + for (std::unique_ptr& test : m_tests) + { + test->reset(); + } } void MasImuTest::printResultsPrivate() { - yInfo() << m_leftIMU.printResults(); - yInfo() << m_rightIMU.printResults(); + for (std::unique_ptr& test : m_tests) + { + yInfo() << test->printResults(); + } } void MasImuTest::saveResultsPrivate() { matioCpp::Struct testData("tests"); - bool okL = m_leftIMU.saveResults(testData); - if (!okL) - { - yError() << "[MasImuTest::saveResultsPrivate] Failed to save the " << m_leftIMU.name() << " data."; - } - bool okR = m_rightIMU.saveResults(testData); - if (!okR) + bool ok = false; + + for (std::unique_ptr& test : m_tests) { - yError() << "[MasImuTest::saveResultsPrivate] Failed to save the " << m_rightIMU.name() << " data."; + ok = test->saveResults(testData); + if (!ok) + { + yError() << "[MasImuTest::saveResultsPrivate] Failed to save the " << test->name() << " data."; + } } if (!m_outputFile.write(testData)) @@ -1009,22 +1013,21 @@ bool MasImuTest::updateModule() if (m_state == State::RUNNING) { - - bool ok = m_leftIMU.addSample(); - if (!ok) + bool ok = false; + bool allCompleted = true; + for (std::unique_ptr& test : m_tests) { - yError() << "[MasImuTest::updateModule] Failed to add data. Marking it as completed."; - m_leftIMU.setCompleted(); - } + ok = test->addSample(); + if (!ok) + { + yError() << "[MasImuTest::updateModule] Failed to add data to "<< test->name() <<". Marking it as completed."; + test->setCompleted(); + } - ok = m_rightIMU.addSample(); - if (!ok) - { - yError() << "[MasImuTest::updateModule] Failed to add data. Marking it as completed."; - m_rightIMU.setCompleted(); + allCompleted = allCompleted && test->isCompleted(); } - if (m_leftIMU.isCompleted() && m_rightIMU.isCompleted()) + if (allCompleted) { printResultsPrivate(); m_state = State::PREPARED; @@ -1034,10 +1037,14 @@ bool MasImuTest::updateModule() if (m_state == State::FIRST_RUN) { - bool okL = m_leftIMU.firstRun(); - bool okR = m_rightIMU.firstRun(); + bool ok = false; - if (!okL || !okR) + for (std::unique_ptr& test : m_tests) + { + ok = ok && test->firstRun(); + } + + if (!ok) { yError() << "[MasImuTest::updateModule] Failed to perform first run."; m_state = State::PREPARED; @@ -1180,32 +1187,25 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) return false; } - auto leftLegGroup = m_parametersPtr->getGroup("LEFT_LEG").lock(); - if (!leftLegGroup) + std::vector tests; + ok = m_parametersPtr->getParameter("tests", tests); + if (!ok || tests.size() == 0) { - yError() << "[MasImuTest::configure] LEFT_LEG group not available. Configuration failed."; + yError() << "[MasImuTest::configure] Configuration failed while reading \"tests\"."; return false; } - ok = m_leftIMU.setup(leftLegGroup, m_commonDataPtr); - if (!ok) + for (size_t t = 0; t < tests.size(); ++t) { - yError() << "[MasImuTest::configure] Configuration failed."; - return false; - } + auto testGroup = m_parametersPtr->getGroup(tests[t]).lock(); + if (!testGroup) + { + yError() << "[MasImuTest::configure] " << tests[t] <<" group not available. Configuration failed."; + return false; + } - auto rightLegGroup = m_parametersPtr->getGroup("RIGHT_LEG").lock(); - if (!leftLegGroup) - { - yError() << "[MasImuTest::configure] RIGHT_LEG group not available. Configuration failed."; - return false; - } - - ok = m_rightIMU.setup(rightLegGroup, m_commonDataPtr); - if (!ok) - { - yError() << "[MasImuTest::configure] Configuration failed."; - return false; + m_tests.push_back(std::make_unique()); + ok = m_tests.back()->setup(testGroup, m_commonDataPtr); } matioCpp::File::Delete(outputFileName); @@ -1275,18 +1275,20 @@ bool MasImuTest::close() m_rpcPort.close(); - bool okL = m_leftIMU.close(); - if (!okL) - { - yError() << "[MasImuTest::close] Failed to close left leg part."; - } - bool okR = m_rightIMU.close(); - if (!okR) + bool ok = false; + bool allOk = true; + + for (std::unique_ptr& test : m_tests) { - yError() << "[MasImuTest::close] Failed to close right leg part."; + ok = test->close(); + if (!ok) + { + yError() << "[MasImuTest::close] Failed to close " << test->name() << "."; + } + allOk = allOk & ok; } - return okL && okR; + return allOk; } From 40825ff6b37997c8b02963b486384bca7ad137bf Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 27 Nov 2020 17:55:27 +0100 Subject: [PATCH 47/51] Bugfixes after tests on the robot. --- .../robots/iCubGenova04/MasImuTestConfig.ini | 32 +++- .../include/BipedalLocomotion/MasImuTest.h | 2 +- utilities/mas-imu-test/scripts/plotResults.m | 20 +-- utilities/mas-imu-test/src/MasImuTest.cpp | 137 ++++++++++-------- 4 files changed, 110 insertions(+), 81 deletions(-) diff --git a/utilities/mas-imu-test/app/robots/iCubGenova04/MasImuTestConfig.ini b/utilities/mas-imu-test/app/robots/iCubGenova04/MasImuTestConfig.ini index f201e5b693..0be5fcee15 100644 --- a/utilities/mas-imu-test/app/robots/iCubGenova04/MasImuTestConfig.ini +++ b/utilities/mas-imu-test/app/robots/iCubGenova04/MasImuTestConfig.ini @@ -15,7 +15,7 @@ filter_yaw true min_joint_variation_deg 2.0 -max_samples 500 +max_samples 300 mas_timeout 0.02 @@ -23,13 +23,13 @@ auto_start true file_name masImuTestOutput.mat -tests ("LEFT_LEG", "RIGHT_LEG") +tests ("LEFT_LEG", "RIGHT_LEG", "HEAD") [LEFT_LEG] -pretty_name "Left IMU Test" +pretty_name "Left Foot IMU Test" -log_prefix left +log_prefix left_foot remote left_leg/inertials @@ -47,9 +47,9 @@ rpy_shuffling ("yaw", "-pitch", "roll") [RIGHT_LEG] -pretty_name "Right IMU Test" +pretty_name "Right Foot IMU Test" -log_prefix right +log_prefix right_foot remote right_leg/inertials @@ -65,3 +65,23 @@ remote_control_boards ("right_leg") rpy_shuffling ("yaw", "-pitch", "roll") +[HEAD] + +pretty_name "Head IMU Test" + +log_prefix head + +remote head/inertials + +imu_frame imu_frame + +imu_sensor_name sensor_imu_bosch_bno055 + +gyro_sensor_name sensor_imu_bosch_bno055 + +acc_sensor_name sensor_imu_bosch_bno055 + +remote_control_boards ("head", "torso") + +rpy_shuffling ("roll", "pitch", "yaw") + diff --git a/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h b/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h index 7784a55ba4..983b57e00b 100644 --- a/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h +++ b/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h @@ -83,7 +83,7 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe iDynTree::VectorDynSize m_previousPositionFeedbackInRad; iDynTree::VectorDynSize m_dummyVelocity; iDynTree::Rotation m_rotationFeedback; - Eigen::Matrix3Xd m_rpyMapping; + Eigen::Matrix3d m_rpyMapping; iDynTree::Rotation m_rotationFromEncoders; iDynTree::Rotation m_imuWorld; //i_R_imuworld diff --git a/utilities/mas-imu-test/scripts/plotResults.m b/utilities/mas-imu-test/scripts/plotResults.m index ba364be917..7916cf36b2 100644 --- a/utilities/mas-imu-test/scripts/plotResults.m +++ b/utilities/mas-imu-test/scripts/plotResults.m @@ -7,16 +7,13 @@ modelPath = [icubModelsInstallPrefix '/share/iCub/robots/' robotName '/']; fileName='model.urdf'; -datasets = {left_data, right_data}; -options = {left_options, right_options}; +tests_fields = fieldnames(tests); - - -for dataIndex = 1 : length(datasets) - data = datasets{dataIndex}; - opt = options{dataIndex}; - rpyImu = zeros(3, length(datasets)); - rpyFK = zeros(3, length(datasets),1); +for testIndex = 1 : numel(tests_fields) + data = tests.(tests_fields{testIndex}).data; + opt = tests.(tests_fields{testIndex}).options; + rpyImu = zeros(3, size(data, 1)); + rpyFK = zeros(3, size(data, 1)); jointOrder = opt.ConsideredJoints; world_H_base = eye(4); @@ -45,8 +42,7 @@ or = frameTransform(1:3, 4); acc = frameTransform * [data(1).Accelerometer'/9.81/2; 1]; gravity = plot3([or(1) acc(1)], [or(2) acc(2)], [or(3) acc(3)], 'm', 'linewidth', 7); - pause - + for i = 2 : length(data) joints_positions = data(i).JointPositions_rad; iDynTreeWrappers.setRobotState(KinDynModel,world_H_base,joints_positions,zeros(6,1),zeros(size(joints_positions)),[0,0,-9.81]); @@ -76,7 +72,7 @@ title('IMU') legend('Roll', 'Pitch', 'Yaw') - if (dataIndex ~= length(datasets)) + if (testIndex ~= numel(tests_fields)) pause end diff --git a/utilities/mas-imu-test/src/MasImuTest.cpp b/utilities/mas-imu-test/src/MasImuTest.cpp index 1382991db6..2773114d65 100644 --- a/utilities/mas-imu-test/src/MasImuTest.cpp +++ b/utilities/mas-imu-test/src/MasImuTest.cpp @@ -827,82 +827,93 @@ bool MasImuTest::MasImuData::saveResults(matioCpp::Struct& logStruct) { std::string errorPrefix = "[MasImuTest::MasImuData::saveResults](" + m_testName +") "; - matioCpp::StructArray dataArray("data", {m_errorData.size(), 1}, {"RotationError", - "RotationFromIMU", - "RotationFromIMUInInertial", - "RotationFromIMUInInertialYawFiltered", - "RotationFromEncoders", - "JointPositions_rad", - "RPYfromIMUinDeg", - "RPYfromIMUinDegRemapped", - "AngularVelocity_deg_s", - "Accelerometer"}); + std::vector testStructVariables; - for (size_t i = 0; i < m_errorData.size(); ++i) + if (addedSamples() == 0) { - matioCpp::StructArrayElement el = dataArray[{i, 0}]; - if (!el.setField(tomatioCpp(m_errorData[i], "RotationError"))) - { - yError() << errorPrefix << "Failed to set the field RotationError."; - return false; - } + yWarning() << errorPrefix << "No samples added. No data will be saved"; + } + else + { + matioCpp::StructArray dataArray("data", {m_errorData.size(), 1}, {"RotationError", + "RotationFromIMU", + "RotationFromIMUInInertial", + "RotationFromIMUInInertialYawFiltered", + "RotationFromEncoders", + "JointPositions_rad", + "RPYfromIMUinDeg", + "RPYfromIMUinDegRemapped", + "AngularVelocity_deg_s", + "Accelerometer"}); - if(!el.setField(tomatioCpp(m_rotationFeedbackData[i], "RotationFromIMU"))) + for (size_t i = 0; i < m_errorData.size(); ++i) { - yError() << errorPrefix << "Failed to set the field RotationFromIMU."; - return false; - } + matioCpp::StructArrayElement el = dataArray[{i, 0}]; + if (!el.setField(tomatioCpp(m_errorData[i], "RotationError"))) + { + yError() << errorPrefix << "Failed to set the field RotationError."; + return false; + } - if(!el.setField(tomatioCpp(m_rotationFeedbackInInertialData[i], "RotationFromIMUInInertial"))) - { - yError() << errorPrefix << "Failed to set the field RotationFromIMUInInertial."; - return false; - } + if(!el.setField(tomatioCpp(m_rotationFeedbackData[i], "RotationFromIMU"))) + { + yError() << errorPrefix << "Failed to set the field RotationFromIMU."; + return false; + } - if(!el.setField(tomatioCpp(m_rotationFeedbackInInertialYawFilteredData[i], "RotationFromIMUInInertialYawFiltered"))) - { - yError() << errorPrefix << "Failed to set the field RotationFromIMUInInertialYawFiltered."; - return false; - } + if(!el.setField(tomatioCpp(m_rotationFeedbackInInertialData[i], "RotationFromIMUInInertial"))) + { + yError() << errorPrefix << "Failed to set the field RotationFromIMUInInertial."; + return false; + } - if(!el.setField(tomatioCpp(m_rotationFromEncodersData[i], "RotationFromEncoders"))) - { - yError() << errorPrefix << "Failed to set the field RotationFromEncoders."; - return false; - } + if(!el.setField(tomatioCpp(m_rotationFeedbackInInertialYawFilteredData[i], "RotationFromIMUInInertialYawFiltered"))) + { + yError() << errorPrefix << "Failed to set the field RotationFromIMUInInertialYawFiltered."; + return false; + } - if(!el.setField(tomatioCpp(m_jointsPositionData[i], "JointPositions_rad"))) - { - yError() << errorPrefix << "Failed to set the field JointPositions_rad."; - return false; - } + if(!el.setField(tomatioCpp(m_rotationFromEncodersData[i], "RotationFromEncoders"))) + { + yError() << errorPrefix << "Failed to set the field RotationFromEncoders."; + return false; + } - if(!el.setField(tomatioCpp(m_rpyImuData[i], "RPYfromIMUinDeg"))) - { - yError() << errorPrefix << "Failed to set the field RPYfromIMUinDeg."; - return false; - } + if(!el.setField(tomatioCpp(m_jointsPositionData[i], "JointPositions_rad"))) + { + yError() << errorPrefix << "Failed to set the field JointPositions_rad."; + return false; + } - if(!el.setField(tomatioCpp(m_rpyRemappedData[i], "RPYfromIMUinDegRemapped"))) - { - yError() << errorPrefix << "Failed to set the field RPYfromIMUinDegRemapped."; - return false; - } + if(!el.setField(tomatioCpp(m_rpyImuData[i], "RPYfromIMUinDeg"))) + { + yError() << errorPrefix << "Failed to set the field RPYfromIMUinDeg."; + return false; + } - if(!el.setField(tomatioCpp(m_gyroData[i], "AngularVelocity_deg_s"))) - { - yError() << errorPrefix << "Failed to set the field AngularVelocity_deg_s."; - return false; - } + if(!el.setField(tomatioCpp(m_rpyRemappedData[i], "RPYfromIMUinDegRemapped"))) + { + yError() << errorPrefix << "Failed to set the field RPYfromIMUinDegRemapped."; + return false; + } - if(!el.setField(tomatioCpp(m_accData[i], "Accelerometer"))) - { - yError() << errorPrefix << "Failed to set the field Accelerometer."; - return false; + if(!el.setField(tomatioCpp(m_gyroData[i], "AngularVelocity_deg_s"))) + { + yError() << errorPrefix << "Failed to set the field AngularVelocity_deg_s."; + return false; + } + + if(!el.setField(tomatioCpp(m_accData[i], "Accelerometer"))) + { + yError() << errorPrefix << "Failed to set the field Accelerometer."; + return false; + } } + testStructVariables.push_back(dataArray); } std::vector options; + options.push_back(tomatioCpp(m_testName, "TestName")); options.push_back(tomatioCpp(m_imuName, "IMUSensorName")); options.push_back(tomatioCpp(m_gyroName, "GyroName")); options.push_back(tomatioCpp(m_accName, "AccelerometerName")); @@ -918,10 +929,12 @@ bool MasImuTest::MasImuData::saveResults(matioCpp::Struct& logStruct) yError() << errorPrefix << "Failed to create the options struct."; return false; } + testStructVariables.push_back(optionsStruct); matioCpp::String outputString("outputString", m_output); + testStructVariables.push_back(outputString); - matioCpp::Struct testStruct(m_logPrefix, {dataArray, optionsStruct, outputString}); + matioCpp::Struct testStruct(m_logPrefix, testStructVariables); if (!logStruct.setField(testStruct)) { @@ -1037,7 +1050,7 @@ bool MasImuTest::updateModule() if (m_state == State::FIRST_RUN) { - bool ok = false; + bool ok = true; for (std::unique_ptr& test : m_tests) { From 25581dfd6244c1fe92064e03c1ae99bce4413b9b Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 27 Nov 2020 18:33:41 +0100 Subject: [PATCH 48/51] Polished matlab script. --- utilities/mas-imu-test/scripts/plotResults.m | 57 ++++++++++++++------ 1 file changed, 40 insertions(+), 17 deletions(-) diff --git a/utilities/mas-imu-test/scripts/plotResults.m b/utilities/mas-imu-test/scripts/plotResults.m index 7916cf36b2..52ebd3a149 100644 --- a/utilities/mas-imu-test/scripts/plotResults.m +++ b/utilities/mas-imu-test/scripts/plotResults.m @@ -1,3 +1,5 @@ +%% Settings + icubModelsInstallPrefix = getenv('ROBOTOLOGY_SUPERBUILD_INSTALL_PREFIX'); meshFilePrefix = [icubModelsInstallPrefix '/share']; @@ -7,69 +9,90 @@ modelPath = [icubModelsInstallPrefix '/share/iCub/robots/' robotName '/']; fileName='model.urdf'; +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% No need to edit from here on +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%% Iterate over the tests tests_fields = fieldnames(tests); for testIndex = 1 : numel(tests_fields) + %% Buffers initialization data = tests.(tests_fields{testIndex}).data; opt = tests.(tests_fields{testIndex}).options; rpyImu = zeros(3, size(data, 1)); rpyFK = zeros(3, size(data, 1)); + %% Base initialization jointOrder = opt.ConsideredJoints; world_H_base = eye(4); world_H_base(1:3, 1:3) = settings.base_rotation; + %% Model loading and set robot state KinDynModel = iDynTreeWrappers.loadReducedModel(jointOrder,settings.base_link,modelPath,fileName,false); joints_positions = data(1).JointPositions_rad; iDynTreeWrappers.setRobotState(KinDynModel,world_H_base,joints_positions,zeros(6,1),zeros(size(joints_positions)),[0,0,-9.81]); - figure('WindowState', 'maximized') - [visualizer,objects]=iDynTreeWrappers.prepareVisualization(KinDynModel,meshFilePrefix, 'transparency',1, 'reuseFigure', 'gcf'); + + %% Visualization setup + [visualizer,objects]=iDynTreeWrappers.prepareVisualization(KinDynModel,meshFilePrefix, 'transparency',1, 'name', 'Imu Test', 'reuseFigure', 'name'); + set(gcf, 'WindowState', 'maximized'); xlim([-0.8, 0.8]) ylim([-0.8, 0.8]) zlim([-1, 0.8]) + title(opt.TestName); + + %% Frame from forward kinematics and plotting frameTransform = iDynTreeWrappers.getWorldTransform(KinDynModel, opt.FrameName); - wRimu = frameTransform(1:3, 1:3); + frameTransform(1:3, 1:3) = data(1).RotationFromEncoders; rpyFK(:, 1) = wbc.rollPitchYawFromRotation(frameTransform(1:3, 1:3)); forward_kin_frame = iDynTreeWrappers.plotFrame(frameTransform, 0.2, 10); - rpy(1) = data(1).RPYfromIMUinDeg(3) * pi/180; - rpy(2) = -data(1).RPYfromIMUinDeg(2) * pi/180; - rpy(3) = -data(1).RPYfromIMUinDeg(1) * pi/180; - rpyImu(:, 1) = rpy; - frameTransform(1:3, 1:3) = wbc.rotz(rpy(3))*wbc.roty(rpy(2))*wbc.rotx(rpy(1)); - wRimu = wRimu * frameTransform(1:3, 1:3)'; - frameTransform(1:3, 1:3) = wRimu * frameTransform(1:3, 1:3); + + %% Frame from IMU + rpyImu(:, 1) = data(1).RPYfromIMUinDegRemapped; + frameTransform(1:3, 1:3) = data(1).RotationFromIMUInInertialYawFiltered; imu_frame = iDynTreeWrappers.plotFrame(frameTransform, 0.4, 5); + + %% Plot of accelerometer or = frameTransform(1:3, 4); acc = frameTransform * [data(1).Accelerometer'/9.81/2; 1]; gravity = plot3([or(1) acc(1)], [or(2) acc(2)], [or(3) acc(3)], 'm', 'linewidth', 7); + %% Data loop for i = 2 : length(data) + %% Update robot state joints_positions = data(i).JointPositions_rad; iDynTreeWrappers.setRobotState(KinDynModel,world_H_base,joints_positions,zeros(6,1),zeros(size(joints_positions)),[0,0,-9.81]); + + %% Update frame from forward kinematics frameTransform = iDynTreeWrappers.getWorldTransform(KinDynModel, opt.FrameName); + frameTransform(1:3, 1:3) = data(i).RotationFromEncoders; rpyFK(:, i) = wbc.rollPitchYawFromRotation(frameTransform(1:3, 1:3)); iDynTreeWrappers.updateFrame(forward_kin_frame, frameTransform); - rpy(1) = data(i).RPYfromIMUinDeg(3) * pi/180; - rpy(2) = -data(i).RPYfromIMUinDeg(2) * pi/180; - rpy(3) = -data(i).RPYfromIMUinDeg(1) * pi/180; - rpyImu(:, i) = rpy; - frameTransform(1:3, 1:3) = wRimu * wbc.rotz(rpy(3))*wbc.roty(rpy(2))*wbc.rotx(rpy(1)); + + %% Update frame from IMU + rpyImu(:, i) = data(i).RPYfromIMUinDegRemapped; + frameTransform(1:3, 1:3) = data(i).RotationFromIMUInInertialYawFiltered; iDynTreeWrappers.updateFrame(imu_frame, frameTransform); + + %% Update robot visualization iDynTreeWrappers.updateVisualization(KinDynModel,visualizer); + + %% Update gravity visualization or = frameTransform(1:3, 4); acc = frameTransform * [data(i).Accelerometer'/9.81/2; 1]; set(gravity, 'XData', [or(1) acc(1)], 'YData', [or(2) acc(2)], 'ZData', [or(3) acc(3)]); + %% Force rendering drawnow() pause(0.01) end figure plot(rpyFK') - title('FK') + title(strcat(opt.TestName, " RPY from Forward Kinematics")) legend('Roll', 'Pitch', 'Yaw') figure plot(rpyImu') - title('IMU') + title(strcat(opt.TestName, " RPY from IMU")) legend('Roll', 'Pitch', 'Yaw') if (testIndex ~= numel(tests_fields)) From 704e327cc92eb5a53248f95f3fe9dc22fd33bb0e Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 27 Nov 2020 19:35:14 +0100 Subject: [PATCH 49/51] Updated README and changelog. --- CHANGELOG.md | 1 + utilities/mas-imu-test/README.md | 71 +++++++++++++++++++- utilities/mas-imu-test/scripts/plotResults.m | 19 ++++-- 3 files changed, 82 insertions(+), 9 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index afc4e6acc8..6ed12fa01d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -32,5 +32,6 @@ All notable changes to this project are documented in this file. - Implement `TSID` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/167) - Implement the `JointTrajectoryPlayer` application. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/169)29ed234a1c - Implement `ContactDetectors` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/142) +- Added `mas-imu-test` application to check the output of MAS IMUs (https://github.com/dic-iit/bipedal-locomotion-framework/pull/62) [Unreleased]: https://github.com/dic-iit/bipedal-locomotion-framework/ diff --git a/utilities/mas-imu-test/README.md b/utilities/mas-imu-test/README.md index e2bda4eec9..b4e24626af 100644 --- a/utilities/mas-imu-test/README.md +++ b/utilities/mas-imu-test/README.md @@ -24,9 +24,44 @@ Simply run ``` blf-mas-imu-test ``` -in a terminal. The test should run automatically. +in a terminal. The test should run automatically. Then, move the limbs (in the way you prefer) where each IMU you want to test is located. +For each IMU to test, the module prints a status message like +``` +[INFO][MasImuTest::MasImuData::addSample](Left IMU Test) Sample 3 / 500 +``` +every time a joint in the kinematic chain from the root link to the IMU is moved. +When all the tests are completed, a summary similar to the following is printed. +``` +[INFO][MasImuTest::MasImuData::printResults](Left IMU Test) Inertial calibration matrix: + -------------------------------------- + 0.99305 -0.0163568 0.116548 +0.00289557 -0.986599 -0.163135 +0.117654 0.162339 -0.979696 + RPY [deg]: (170.591379, -6.756744, 0.167065) + -------------------------------------- + Results ( 500 samples) : + -------------------------------------- + --------------Mean Rotation----------- + 0.961574 0.274212 0.0134994 +-0.258299 0.88692 0.382954 +0.0930379 -0.371726 0.923669 + RPY [deg]: (-21.922084, -5.338397, -15.035917) + ----------------Min Error------------- + Index: 358 + 0.999966 -0.000548794 0.0082332 +0.00045783 0.999939 0.0110462 +-0.00823876 -0.0110421 0.999905 + RPY [deg]: (-0.632700, 0.472051, 0.026233) + ----------------Max Error------------- + Index: 153 + 0.31468 0.103543 0.943533 +-0.00670651 -0.993765 0.111292 +0.949174 -0.0413493 -0.312024 + RPY [deg]: (-172.451167, -71.654180, -1.220911) + -------------------------------------- + ``` -#### Insert here some real output +When the test closes normally, it saves a ``.mat`` file with all the data and the settings saved in. The test has also an RPC interface. It is possible to launch it by running ``` @@ -38,6 +73,38 @@ in a terminal. Then the following commands are available: - ``stopTest`` Manually stop the test. - ``printResults`` If the test is stopped, print the results. +## Plotting +In the folder ``scripts``, a Matlab script is provided. Load in the workspace the saved ``.mat`` file and simply run the script ``plotResults`` from Matlab. +You may need to edit these lines +```matlab +%% Settings + +robotName='iCubGenova04'; %% Name of the robot + +meshFilePrefix = [getenv('ROBOTOLOGY_SUPERBUILD_INSTALL_PREFIX') '/share']; %% Path to the model meshes + +modelPath = [getenv('ROBOTOLOGY_SUPERBUILD_INSTALL_PREFIX') '/share/iCub/robots/' robotName '/']; %% Path to the robot model + +fileName='model.urdf'; %% Name of the urdf file +``` +according to your environment. If you installed ``BipedalLocomotionFramework`` via the ``robotology-superbuild``, you may need to change only the robot name. + +The script should start playing the data on a 3D version of the robot. You should be able to see two frames. One with long and thin axis is the expected frame orientation from forward kinematics. +The other has short and thick axis. This is the estimated one via the IMU. These two should match. +In addition, you should be able to see a magenta line indicating the accelerometer measurement (scaled). + +It also prints the expected RPY values vs the measured ones. + +### Example of not aligned frames + +(The magenta vector is disabled for this image.) + +![ezgif-2-38372e5d4323](https://user-images.githubusercontent.com/18591940/100088488-7a5b5780-2e50-11eb-8b85-603f806f8105.gif) + +### Example of aligned frames + +![left](https://user-images.githubusercontent.com/18591940/100474051-0cc55a80-30e0-11eb-9e45-1b4f95820bf3.gif) + ## Configuration file explanation The configuration file presents the following data: - ``name masImuTest`` The name of the test, used to define the name of the opened ports. diff --git a/utilities/mas-imu-test/scripts/plotResults.m b/utilities/mas-imu-test/scripts/plotResults.m index 52ebd3a149..9ceb7e3998 100644 --- a/utilities/mas-imu-test/scripts/plotResults.m +++ b/utilities/mas-imu-test/scripts/plotResults.m @@ -1,18 +1,21 @@ %% Settings -icubModelsInstallPrefix = getenv('ROBOTOLOGY_SUPERBUILD_INSTALL_PREFIX'); +robotName='iCubGenova04'; %% Name of the robot -meshFilePrefix = [icubModelsInstallPrefix '/share']; +meshFilePrefix = [getenv('ROBOTOLOGY_SUPERBUILD_INSTALL_PREFIX') '/share']; %% Path to the model meshes -robotName='iCubGenova04'; +modelPath = [getenv('ROBOTOLOGY_SUPERBUILD_INSTALL_PREFIX') '/share/iCub/robots/' robotName '/']; %% Path to the robot model -modelPath = [icubModelsInstallPrefix '/share/iCub/robots/' robotName '/']; -fileName='model.urdf'; +fileName='model.urdf'; %% Name of the urdf file %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% No need to edit from here on %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +if (~exist('tests', 'var')) + error("tests variable not found. Please, load the saved dataset in the workspace (double click on the file generated by the blf-mas-imu-test executable"); +end + %% Iterate over the tests tests_fields = fieldnames(tests); @@ -41,13 +44,13 @@ zlim([-1, 0.8]) title(opt.TestName); - %% Frame from forward kinematics and plotting + %% Frame from forward kinematics and plotting (long and thin frame) frameTransform = iDynTreeWrappers.getWorldTransform(KinDynModel, opt.FrameName); frameTransform(1:3, 1:3) = data(1).RotationFromEncoders; rpyFK(:, 1) = wbc.rollPitchYawFromRotation(frameTransform(1:3, 1:3)); forward_kin_frame = iDynTreeWrappers.plotFrame(frameTransform, 0.2, 10); - %% Frame from IMU + %% Frame from IMU (short and thick frame) rpyImu(:, 1) = data(1).RPYfromIMUinDegRemapped; frameTransform(1:3, 1:3) = data(1).RotationFromIMUInInertialYawFiltered; imu_frame = iDynTreeWrappers.plotFrame(frameTransform, 0.4, 5); @@ -86,6 +89,8 @@ drawnow() pause(0.01) end + + %% Plotting of the RPY from forward kinematics and from IMU figure plot(rpyFK') title(strcat(opt.TestName, " RPY from Forward Kinematics")) From 1e2e7611b1ec7b0d88989d6443a0ff4dad2be274 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 22 Jan 2021 12:12:47 +0100 Subject: [PATCH 50/51] Removed redundant check on the existence of a frame. --- .../include/BipedalLocomotion/MasImuTest.h | 1 - utilities/mas-imu-test/src/MasImuTest.cpp | 13 +++---------- 2 files changed, 3 insertions(+), 11 deletions(-) diff --git a/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h b/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h index 983b57e00b..09e7adbc38 100644 --- a/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h +++ b/utilities/mas-imu-test/include/BipedalLocomotion/MasImuTest.h @@ -66,7 +66,6 @@ class BipedalLocomotion::MasImuTest : public yarp::os::RFModule, public MasImuTe BipedalLocomotion::ParametersHandler::YarpImplementation::shared_ptr m_group; iDynTree::FrameIndex m_frame; std::string m_frameName, m_imuName, m_gyroName, m_accName; - iDynTree::LinkIndex m_link; std::vector m_consideredJointIndexes; std::vector m_consideredJointNames; iDynTree::KinDynComputations m_kinDyn; diff --git a/utilities/mas-imu-test/src/MasImuTest.cpp b/utilities/mas-imu-test/src/MasImuTest.cpp index 2773114d65..9bc771c6a8 100644 --- a/utilities/mas-imu-test/src/MasImuTest.cpp +++ b/utilities/mas-imu-test/src/MasImuTest.cpp @@ -74,14 +74,14 @@ bool MasImuTest::MasImuData::setupModel() return false; } - m_link = m_commonDataPtr->fullModel.getFrameLink(m_frame); - assert(m_link != iDynTree::LINK_INVALID_INDEX); + iDynTree::LinkIndex link = m_commonDataPtr->fullModel.getFrameLink(m_frame); + assert(link != iDynTree::LINK_INVALID_INDEX); m_consideredJointIndexes.clear(); m_consideredJointNames.clear(); iDynTree::LinkIndex baseLinkIndex = m_commonDataPtr->traversal.getBaseLink()->getIndex(); - iDynTree::LinkIndex currentLink = m_link; + iDynTree::LinkIndex currentLink = link; while (currentLink != baseLinkIndex) { const iDynTree::IJoint* joint = m_commonDataPtr->traversal.getParentJointFromLinkIndex(currentLink); assert(joint); @@ -106,13 +106,6 @@ bool MasImuTest::MasImuData::setupModel() m_frame = m_kinDyn.getFrameIndex(m_frameName); - if (m_frame == iDynTree::FRAME_INVALID_INDEX) - { - yError() << errorPrefix << "The frame " << m_frameName << " does not exists in the reduced robot model." - << ". Configuration failed."; - return false; - } - if (!ok) { yError() << errorPrefix << "Failed to load the reduced model. Configuration failed."; From f053b43fec94159eaf02696cd3e69a659cf9feb9 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 22 Jan 2021 12:17:04 +0100 Subject: [PATCH 51/51] Using more descriptive messages when the configuration fails. --- utilities/mas-imu-test/src/MasImuTest.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/utilities/mas-imu-test/src/MasImuTest.cpp b/utilities/mas-imu-test/src/MasImuTest.cpp index 9bc771c6a8..c23514fe43 100644 --- a/utilities/mas-imu-test/src/MasImuTest.cpp +++ b/utilities/mas-imu-test/src/MasImuTest.cpp @@ -1073,14 +1073,14 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) bool ok = m_parametersPtr->getParameter("name", m_commonDataPtr->prefix); if (!ok) { - yError() << "[MasImuTest::configure] Configuration failed."; + yError() << "[MasImuTest::configure] Configuration failed while reading \"name\"."; return false; } ok = m_parametersPtr->getParameter("period", m_period); if (!ok) { - yError() << "[MasImuTest::configure] Configuration failed."; + yError() << "[MasImuTest::configure] Configuration failed while reading \"period\"."; return false; } @@ -1093,14 +1093,14 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) ok = m_parametersPtr->getParameter("robot", m_commonDataPtr->robotName); if (!ok) { - yError() << "[MasImuTest::configure] Configuration failed."; + yError() << "[MasImuTest::configure] Configuration failed while reading \"robot\"."; return false; } std::string robotModelName; ok = m_parametersPtr->getParameter("model", robotModelName); if (!ok) { - yError() << "[MasImuTest::configure] Configuration failed."; + yError() << "[MasImuTest::configure] Configuration failed while reading \"model\"."; return false; } std::string pathToModel = yarp::os::ResourceFinder::getResourceFinderSingleton().findFileByName(robotModelName); @@ -1108,7 +1108,7 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) iDynTree::ModelLoader modelLoader; if (!modelLoader.loadModelFromFile(pathToModel)) { - yError() << "[MasImuTest::configure] Configuration failed."; + yError() << "[MasImuTest::configure] Configuration failed. Failed to load the specified model."; return false; } @@ -1118,7 +1118,7 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) ok = m_parametersPtr->getParameter("base_link", baseLink); if (!ok) { - yError() << "[MasImuTest::configure] Configuration failed."; + yError() << "[MasImuTest::configure] Configuration failed while reading \"base_link\"."; return false; } @@ -1158,7 +1158,7 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) ok = m_parametersPtr->getParameter("filter_yaw", m_commonDataPtr->filterYaw); if (!ok) { - yError() << "[MasImuTest::configure] Configuration failed."; + yError() << "[MasImuTest::configure] Configuration failed while reading \"filter_yaw\"."; return false; } @@ -1166,7 +1166,7 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) ok = m_parametersPtr->getParameter("min_joint_variation_deg", minJointVariationInDeg); if (!ok) { - yError() << "[MasImuTest::configure] Configuration failed."; + yError() << "[MasImuTest::configure] Configuration failed while reading \"min_joint_variation_deg\"."; return false; } m_commonDataPtr->minJointVariationRad = iDynTree::deg2rad(minJointVariationInDeg); @@ -1254,7 +1254,7 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) ok = m_parametersPtr->getParameter("auto_start", autoStart); if (!ok) { - yError() << "[MasImuTest::configure] Configuration failed."; + yError() << "[MasImuTest::configure] Configuration failed while reading \"auto_start\"."; return false; } @@ -1267,7 +1267,7 @@ bool MasImuTest::configure(yarp::os::ResourceFinder &rf) { m_state = State::FIRST_RUN; - yInfo() << "[MasImuTest::startTest] Started!"; + yInfo() << "[MasImuTest::configure] Started!"; } return true;