Implement the joint position tracking application

CHANGELOG.md

@@ -17,5 +17,6 @@ All notable changes to this project are documented in this file.
 - Renamed from ``bipedal-locomotion-controllers`` to ``bipedal-locomotion-framework`` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/40).
 - Implement `Contact` library. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/43 and https://github.com/dic-iit/bipedal-locomotion-framework/pull/45)
 - Added `CommonConversions` and `ManifConversions` libraries to handle type conversions.
+- Implement the `JointPositionTracking` application. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/136)
 
 [Unreleased]: https://github.com/dic-iit/bipedal-locomotion-framework/

CMakeLists.txt

@@ -71,6 +71,9 @@ include(AddBipedalLocomotionUnitTest)
 #Function to automatize the process of creating a new library
 include(AddBipedalLocomotionLibrary)
 
+#Function to automatize the process of creating a new application
+include(AddBipedalLocomotionApplication)
+
 #Utility to install ini files
 include(InstallIniFiles)
 
@@ -114,3 +117,5 @@ include(AddUninstallTarget)
 
 include(GenerateAndAddUrdfModel)
 add_subdirectory(Python)
+
+add_subdirectory(utilities)

cmake/AddBipedalLocomotionApplication.cmake

@@ -0,0 +1,56 @@
+# 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.
+
+function(add_bipedal_locomotion_application)
+
+ set(options)
+ set(oneValueArgs NAME)
+ set(multiValueArgs
+ SOURCES
+ HEADERS
+ LINK_LIBRARIES
+ SUBDIRECTORIES)
+
+ set(prefix "bipedal_component")
+
+ cmake_parse_arguments(${prefix}
+ "${options}"
+ "${oneValueArgs}"
+ "${multiValueArgs}"
+ ${ARGN})
+
+ set(name ${${prefix}_NAME})
+ set(sources ${${prefix}_SOURCES})
+ set(headers ${${prefix}_HEADERS})
+ set(link_libraries ${${prefix}_LINK_LIBRARIES})
+ set(subdirectories ${${prefix}_SUBDIRECTORIES})
+
+ # add an executable to the project using the specified source files.
+ add_executable(${name} ${sources} ${headers})
+
+ # Add C++17 features
+ target_compile_features(${name} PRIVATE cxx_std_17)
+ target_compile_definitions(${name} PRIVATE -D_USE_MATH_DEFINES)
+
+ set_target_properties(${name} PROPERTIES
+ OUTPUT_NAME "blf-${name}"
+ VERSION ${BipedalLocomotionFramework_VERSION})
+
+ target_link_libraries(${name} PRIVATE ${link_libraries})
+
+
+ target_include_directories(${name} PRIVATE "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>")
+
+ # Specify installation targets, typology and destination folders.
+ install(TARGETS ${name}
+ DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin)
+
+ # Add all subdirectories
+ foreach(subdir ${subdirectories})
+ add_subdirectory(${subdir})
+ endforeach()
+
+ message(STATUS "Created target application ${name}.")
+
+endfunction()

cmake/BipedalLocomotionFrameworkFindDependencies.cmake

@@ -189,3 +189,7 @@ framework_dependent_option(FRAMEWORK_COMPILE_FloatingBaseEstimators
 framework_dependent_option(FRAMEWORK_COMPILE_ManifConversions
  "Compile manif Conversions libraries?" ON
  "FRAMEWORK_USE_manif" OFF)
+
+framework_dependent_option(FRAMEWORK_COMPILE_JointPositionTrackingApplication
+ "Compile joint-position-tracking application?" ON
+ "FRAMEWORK_COMPILE_YarpImplementation;FRAMEWORK_COMPILE_Planners;FRAMEWORK_COMPILE_RobotInterface" OFF)

src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpRobotControl.h

@@ -104,6 +104,12 @@ class YarpRobotControl : public IRobotControl
  bool setReferences(Eigen::Ref<const Eigen::VectorXd> desiredJointValues,
  const IRobotControl::ControlMode& mode) final;
 
+ /**
+ * Get the list of the controlled joints
+ * @return A vector containing the name of the controlled joints.
+ */
+ std::vector<std::string> getJointList() const final;
+
  /**
  * Destructor
  */

src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp

@@ -635,3 +635,8 @@ bool YarpRobotControl::checkMotionDone(bool& motionDone,
 
  return true;
 }
+
+std::vector<std::string> YarpRobotControl::getJointList() const
+{
+ return m_pimpl->axesName;
+}

src/RobotInterface/include/BipedalLocomotion/RobotInterface/IRobotControl.h

@@ -90,6 +90,12 @@ class IRobotControl
  virtual bool setReferences(Eigen::Ref<const Eigen::VectorXd> desiredJointValues,
  const IRobotControl::ControlMode& controlMode) = 0;
 
+ /**
+ * Get the list of the controlled joints
+ * @return A vector containing the name of the controlled joints.
+ */
+ virtual std::vector<std::string> getJointList() const = 0;
+
  /**
  * Destructor.
  */

utilities/CMakeLists.txt

@@ -0,0 +1,6 @@
+# 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.
+
+
+add_subdirectory(joint-position-tracking)

utilities/joint-position-tracking/CMakeLists.txt

@@ -0,0 +1,23 @@
+# 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
+
+if(FRAMEWORK_COMPILE_JointPositionTrackingApplication)
+
+ add_bipedal_locomotion_application(
+ NAME joint-position-tracking
+ SOURCES src/Main.cpp src/Module.cpp
+ HEADERS include/BipedalLocomotion/JointPositionTracking/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_position_tracking_plot_dataset.py
+ DESTINATION "${CMAKE_INSTALL_PREFIX}/share/BipedalLocomotionFramework/python")
+
+ install(FILES script/blf-joint-position-tracking-script.sh
+ PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ
+ DESTINATION "${CMAKE_INSTALL_BINDIR}")
+
+endif()

utilities/joint-position-tracking/README.md

@@ -0,0 +1,26 @@
+# joint-position-tracking
+
+The **joint-position-tracking** is a simple tool for testing the joint position tracking of a robot in a `YARP` environment.
+
+## :running: How to use the application
+The fastest way to use the application is to run the `sh` script [`blf-joint-position-tracking-script.sh`](./script/blf-joint-position-tracking-script.sh). If you correctly installed the the framework you can run the script from any folder.
+
+The script will:
+- move the robot
+- store the desired and actual joint position in a `txt` file
+- run a python script to plot the data in a file called `figure.png`
+
+The [`blf-joint-position-tracking-options.ini`](./config/robots/iCubGenova09/blf-joint-position-tracking-options.ini) file contains some parameters that you may modify to control a given joint:
+- `max_angle_deg`: is the max angle reached by the joint (the value is in degrees)
+- `min_angle_def`: is the min angle reached by the joint (the value is in degrees)
+- `trajectory_duration`: is the duration of the generated trajectory (the value is in seconds)
+- `robot_name`: name of the robot
+- `joints_list`: list of the controlled joint (for the time being multiple joints control is not supported. Please specify only one joint)
+- `remote_control_boards`: list of associated control board (for the time being multiple joints control is not supported. Please specify only one control board)
+
+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.
+
+
+This is an example of usage
+
+![](https://user-images.githubusercontent.com/16744101/97494667-e5576280-1966-11eb-840b-56e5120f6b29.gif)

utilities/joint-position-tracking/config/robots/iCubGenova09/blf-joint-position-tracking-options.ini

@@ -0,0 +1,19 @@
+name joint-position-tracking
+sampling_time 0.01
+max_angle_deg 70.0
+min_angle_deg 0.0
+trajectory_duration 5.0
+
+[ROBOT_INTERFACE]
+robot_name icub
+joints_list ("r_hip_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-position-tracking/include/BipedalLocomotion/JointPositionTracking/Module.h

@@ -0,0 +1,93 @@
+/**
+ * @file Module.h
+ * @authors Giulio Romualdi
+ * @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_POSITION_TRACKING_MODULE_H
+#define BIPEDAL_LOCOMOTION_UTILITIES_JOINT_POSITION_TRACKING_MODULE_H
+
+// std
+#include <memory>
+#include <string>
+#include <vector>
+
+// YARP
+#include <yarp/os/RFModule.h>
+
+#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
+#include <BipedalLocomotion/Planners/QuinticSpline.h>
+#include <BipedalLocomotion/RobotInterface/YarpRobotControl.h>
+#include <BipedalLocomotion/RobotInterface/YarpSensorBridge.h>
+
+namespace BipedalLocomotion
+{
+namespace JointPositionTracking
+{
+
+class Module : public yarp::os::RFModule
+{
+ double m_dT; /**< RFModule period. */
+ std::string m_robot; /**< Robot name. */
+
+ Eigen::VectorXd m_currentJointPos;
+
+ std::shared_ptr<yarp::dev::PolyDriver> m_robotDevice;
+
+ BipedalLocomotion::RobotInterface::YarpRobotControl m_robotControl;
+ BipedalLocomotion::RobotInterface::YarpSensorBridge m_sensorBridge;
+
+ std::vector<double> m_setPoints;
+ std::vector<double>::const_iterator m_currentSetPoint;
+
+ BipedalLocomotion::Planners::QuinticSpline m_spline;
+ std::vector<double> m_timeKnots;
+ std::vector<Eigen::VectorXd> m_trajectoryKnots;
+
+ double m_initTrajectoryTime;
+
+ bool generateNewTrajectory();
+
+ bool createPolydriver(
+ std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler);
+
+ bool initializeRobotControl(
+ std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler);
+
+ bool instantiateSensorBridge(
+ std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler);
+
+ std::vector<double> m_logJointPos;
+ std::vector<double> 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 JointPositionTracking
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_UTILITIES_JOINT_POSITION_TRACKING_MODULE_H

utilities/joint-position-tracking/script/blf-joint-position-tracking-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."