Merge branch 'master' into TSID/joint_regularization

.gitignore

@@ -7,3 +7,4 @@ build/*
 
 # Qtcreator
 *.user*
+*.autosave

CHANGELOG.md

@@ -30,6 +30,8 @@ All notable changes to this project are documented in this file.
 - Implement YARP based `ROSPublisher` in the `YarpUtilities` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/156)
 - Implement example YARP device `ROSPublisherTestDevice` for understanding the usage of `ROSPublisher`. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/160)
 - Implement `TSID` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/167, https://github.com/dic-iit/bipedal-locomotion-framework/pull/170)
+- 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/

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)

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

cmake/BipedalLocomotionFrameworkFindDependencies.cmake

@@ -130,9 +130,9 @@ endmacro()
 ################################################################################
 # Find all packages
 
-find_package(iDynTree 0.11.105 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(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)
 
 find_package(YARP QUIET)
@@ -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(FRAMEWORK_COMPILE_MasImuTest
+ "Compile test on the MAS IMU?" ON
+ "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_matioCppConversions" OFF)
+

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());
 

utilities/CMakeLists.txt

@@ -1,6 +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(mas-imu-test)

utilities/joint-trajectory-player/CMakeLists.txt

@@ -0,0 +1,17 @@
+# 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
+
+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 
+ matioCpp::matioCpp 
+ BipedalLocomotion::matioCppConversions
+ )
+
+install_ini_files(${CMAKE_CURRENT_SOURCE_DIR}/config)

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-trajectory-player-options.ini --trajectory_file file_of_the_joint_trajectory.mat
+```
+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:
+- `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.

utilities/joint-trajectory-player/config/robots/iCubGazeboV3/blf-joint-trajectory-player-options.ini

@@ -0,0 +1,17 @@
+name joint-trajectory-player
+sampling_time 0.01
+
+[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
+

utilities/joint-trajectory-player/include/BipedalLocomotion/JointTrajectoryPlayer/Module.h

@@ -0,0 +1,100 @@
+/**
+ * @file Module.h
+ * @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.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_UTILITIES_JOINT_TRAJECTORY_PLAYER_MODULE_H
+#define BIPEDAL_LOCOMOTION_UTILITIES_JOINT_TRAJECTORY_PLAYER_MODULE_H
+
+// std
+#include <deque>
+#include <memory>
+#include <string>
+#include <vector>
+
+// YARP
+#include <yarp/os/RFModule.h>
+
+#include <matioCpp/matioCpp.h>
+
+#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
+#include <BipedalLocomotion/Planners/QuinticSpline.h>
+#include <BipedalLocomotion/RobotInterface/YarpRobotControl.h>
+#include <BipedalLocomotion/RobotInterface/YarpSensorBridge.h>
+
+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; /**< Current joint positions. */
+
+ std::shared_ptr<yarp::dev::PolyDriver> 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::unordered_map<std::string, std::vector<double>> m_logJointPos; /**< Measured joint
+ positions. */
+
+ std::vector<std::string> m_axisList; /**< Axis name list. */
+
+ matioCpp::MultiDimensionalArray<double> m_traj; /**< Joint trajectory. */
+
+ unsigned int m_idxTraj{0}; /**< Index to iterate the trajectory. */
+
+ enum class State
+ {
+ idle,
+ positioning,
+ running
+ };
+ State m_state{State::idle}; /** State machine */
+
+ bool createPolydriver(std::shared_ptr<ParametersHandler::IParametersHandler> handler);
+
+ bool initializeRobotControl(std::shared_ptr<ParametersHandler::IParametersHandler> handler);
+
+ bool instantiateSensorBridge(std::shared_ptr<ParametersHandler::IParametersHandler> handler);
+
+ bool readStateFromFile(const std::string& filename, const std::size_t numFields);
+
+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

utilities/joint-trajectory-player/src/Main.cpp

@@ -0,0 +1,36 @@
+/**
+ * @file Main.cpp
+ * @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.
+ */
+
+// YARP
+#include <yarp/os/LogStream.h>
+#include <yarp/os/Network.h>
+#include <yarp/os/RFModule.h>
+
+#include <BipedalLocomotion/JointTrajectoryPlayer/Module.h>
+
+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("blf-joint-trajectory-player-options.ini");
+
+ rf.configure(argc, argv);
+
+ // create the module
+ BipedalLocomotion::JointTrajectoryPlayer::Module module;
+
+ return module.runModule(rf);
+}