From dd8e3bc5d2bfeeedab9fd79820f8d59be3b18d72 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 7 Jul 2021 19:26:26 +0200 Subject: [PATCH 01/12] Implement set_from_file_path and set_from_filename methods for IParameterHandler YARP bindings --- .../ParametersHandler/src/YarpParametersHandler.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/bindings/python/ParametersHandler/src/YarpParametersHandler.cpp b/bindings/python/ParametersHandler/src/YarpParametersHandler.cpp index 45bd11f26b..451ed1f54e 100644 --- a/bindings/python/ParametersHandler/src/YarpParametersHandler.cpp +++ b/bindings/python/ParametersHandler/src/YarpParametersHandler.cpp @@ -13,6 +13,8 @@ #include #include +#include + namespace BipedalLocomotion { namespace bindings @@ -28,7 +30,14 @@ void CreateYarpParameterHandler(pybind11::module& module) std::shared_ptr, IParametersHandler>(module, "YarpParametersHandler") .def(py::init()) - .def("set_from_file", &YarpImplementation::setFromFile, py::arg("Filename")); + .def("set_from_filename", + [](YarpImplementation& impl, const std::string& filename) -> bool { + const auto filePath + = yarp::os::ResourceFinder::getResourceFinderSingleton().findFileByName( + filename); + return impl.setFromFile(filePath); + }) + .def("set_from_file_path", &YarpImplementation::setFromFile, py::arg("file_path")); } } // namespace ParametersHandler From 429b531889d66106a5c8e63c10ba1dbff582ed67 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 7 Jul 2021 19:27:28 +0200 Subject: [PATCH 02/12] Add the possibility to set an expected value in blf-calibration-delta-updater.py script --- .../script/blf-calibration-delta-updater.py | 71 +++++++++++++------ 1 file changed, 50 insertions(+), 21 deletions(-) diff --git a/utilities/calibration-delta-updater/script/blf-calibration-delta-updater.py b/utilities/calibration-delta-updater/script/blf-calibration-delta-updater.py index d47f7e6bf7..7bc89781ad 100755 --- a/utilities/calibration-delta-updater/script/blf-calibration-delta-updater.py +++ b/utilities/calibration-delta-updater/script/blf-calibration-delta-updater.py @@ -9,21 +9,16 @@ import re import math import numpy as np +import os import bipedal_locomotion_framework.bindings as blf + def print_info(msg): print('\33[104m' + '[offset_updater]' + '\033[0m ' + msg) -def open_polydriver(robot_name, control_board, joints_list): - param_handler = blf.parameters_handler.StdParametersHandler() - param_handler.set_parameter_vector_string('joints_list', joints_list) - param_handler.set_parameter_vector_string('remote_control_boards', [control_board]) - param_handler.set_parameter_string('robot_name', robot_name) - param_handler.set_parameter_string('local_prefix', 'joint_offset_updater') - param_handler.set_parameter_bool("check_for_nan", False) - param_handler.set_parameter_bool("stream_joint_states", True) +def open_polydriver(param_handler): board = blf.robot_interface.construct_remote_control_board_remapper(param_handler) bridge = blf.robot_interface.YarpSensorBridge() @@ -43,12 +38,14 @@ def get_offsets(xml_root): return deltas + def prettify(elem): return etree.tostring(elem, encoding="UTF-8", xml_declaration=True, pretty_print=True, doctype='') + def set_offsets(xml_root, offsets): for node in root.findall("group"): @@ -59,34 +56,66 @@ def set_offsets(xml_root, offsets): return xml_root + +def compute_joint_offset(joint_index, expected_values): + # read the joint state + assert sensor_bridge.advance() is True + _, joints_values, _ = sensor_bridge.get_joint_positions() + delta = joints_values[joint_index] * 180 / math.pi - expected_values[joint_index] + return delta + + if __name__ == '__main__': parser = argparse.ArgumentParser(description='Automatically update the calibration delta of the robot configuration file.') parser.add_argument('-i', '--input', type=str, required=True, help='Input xml file') parser.add_argument('-o', '--output', type=str, required=True, help='Output xml file') - parser.add_argument('-b', '--board', type=str, required=True, help='Name of the control board') - parser.add_argument('-j', '--joints', type=str, nargs='+', required=True, help='Name of the control board. The order must be the same of the one considered in the input xml') - parser.add_argument('-r', '--robot', type=str, required=True, help='Name of the robot') + parser.add_argument('--from', type=str, required=False, help='Path or name of the configuration file', default="blf-calibration-delta-update-options.ini") + parser.add_argument('-p', '--part', type=str, required=True, help='name of the part.') args = parser.parse_args() + # get the original offset tree = etree.parse(args.input) root = tree.getroot() offsets = get_offsets(root) - control_board, sensor_bridge = open_polydriver(args.robot, args.board, args.joints) + # load the parameter hander + parameterHandler = blf.parameters_handler.YarpParametersHandler() - print_info("Please put the joints in zero configuration.") - print_info("Press enter and I will show you the offsets.") - input() + if (os.path.isfile(args.from)): + assert parameterHandler.set_from_file_path(args.from) is True + else: + assert parameterHandler.set_from_filename(args.from) is True - assert sensor_bridge.advance() is True + group = parameterHandler.get_group(parser.part) + assert group is not None + joints_list = group.get_parameter_vector_string('joints_list') + control_board = group.get_parameter_string('control_board') + expected_values = param_handler.get_parameter_vector_double('expected_value') + param_handler.set_parameter_vector_string('joints_list', joints_list) + param_handler.set_parameter_vector_string('remote_control_boards', [control_board]) - _, joints_values, _ = sensor_bridge.get_joint_positions() - new_offsets = np.array(offsets) + joints_values * 180 / math.pi + # Open the sensorbridge + control_board, sensor_bridge = open_polydriver(parameter_handler) + + new_offsets = [] - print_info("Joint values = " + str(['%.4f' % joint for joint in joints_values * 180 / math.pi]) + " deg") + for i, joint in enumerate(joints_list): + key = "" + while key != 'y' and key != 'n': + print_info("Do you want to calibrate the joint named: " + joint + " [y|n]") + key = input() + if key = 'n': + new_offsets.append(0); + elif key == 'y' : + print_info("Please move the joint in the expected configuration. Press enter when you are ready.") + input() + new_offsets.append(compute_joint_offset(i, expected_values)) + + updated_offset = np.array(offsets) + np.array(new_offsets) print_info("previous offset = " + str(['%.4f' % offset for offset in offsets]) + " deg") - print_info("new offset = " + str(['%.4f' % offset for offset in new_offsets]) + " deg") + print_info("new offset = " + str(['%.4f' % offset for offset in updated_offset]) + " deg") + key = "" while key != 'y' and key != 'n': @@ -95,7 +124,7 @@ def set_offsets(xml_root, offsets): if key == 'n' : print_info("Offset not changed") elif key == 'y' : - root = set_offsets(root, new_offsets) + root = set_offsets(root, updated_offset) with open(args.output, 'bw') as f: f.write(prettify(root)) From 32699deba7e8bef5ac7ad6caf4d34c04273f31d6 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 8 Jul 2021 13:46:52 +0200 Subject: [PATCH 03/12] General improvements in blf-calibration-delta-updater.py --- .../script/blf-calibration-delta-updater.py | 115 +++++++++++------- 1 file changed, 73 insertions(+), 42 deletions(-) diff --git a/utilities/calibration-delta-updater/script/blf-calibration-delta-updater.py b/utilities/calibration-delta-updater/script/blf-calibration-delta-updater.py index 7bc89781ad..1423eae379 100755 --- a/utilities/calibration-delta-updater/script/blf-calibration-delta-updater.py +++ b/utilities/calibration-delta-updater/script/blf-calibration-delta-updater.py @@ -15,12 +15,15 @@ def print_info(msg): - print('\33[104m' + '[offset_updater]' + '\033[0m ' + msg) + print('\33[104m' + '[blf-calibration-delta-updater]' + '\033[0m ' + msg) def open_polydriver(param_handler): board = blf.robot_interface.construct_remote_control_board_remapper(param_handler) + if not board.is_valid(): + raise RuntimeError('Unable to build the control board') + bridge = blf.robot_interface.YarpSensorBridge() bridge.initialize(param_handler) bridge.set_drivers_list([board]) @@ -30,7 +33,7 @@ def open_polydriver(param_handler): def get_offsets(xml_root): deltas = [] - for node in root.findall("group"): + for node in xml_root.findall("group"): if node.get('name') == "CALIBRATION": for param in node: if param.get('name') == "calibrationDelta": @@ -40,15 +43,16 @@ def get_offsets(xml_root): def prettify(elem): - return etree.tostring(elem, encoding="UTF-8", - xml_declaration=True, - pretty_print=True, - doctype='') + return etree.tostring(elem, + encoding="UTF-8", + xml_declaration=True, + pretty_print=True, + doctype='') def set_offsets(xml_root, offsets): - - for node in root.findall("group"): + for node in xml_root.findall("group"): if node.get('name') == "CALIBRATION": for param in node: if param.get('name') == "calibrationDelta": @@ -57,21 +61,57 @@ def set_offsets(xml_root, offsets): return xml_root -def compute_joint_offset(joint_index, expected_values): +def compute_joint_offset(sensor_bridge, joint_index, expected_values): # read the joint state - assert sensor_bridge.advance() is True + if not sensor_bridge.advance(): + raise RuntimeError('Unable to reda the sensors') + _, joints_values, _ = sensor_bridge.get_joint_positions() + + # compute the offset delta = joints_values[joint_index] * 180 / math.pi - expected_values[joint_index] return delta -if __name__ == '__main__': - parser = argparse.ArgumentParser(description='Automatically update the calibration delta of the robot configuration file.') +def parse_handler(file, robot_part): + # load the parameter handler + parameters = blf.parameters_handler.YarpParametersHandler() + + print("---------------------------------->" + file) + + if os.path.isfile(file): + if not parameters.set_from_file_path(file): + raise RuntimeError('Unable to load the configuration file') + else: + if not parameters.set_from_filename(file): + raise RuntimeError('Unable to load the configuration file') + + group = parameters.get_group(robot_part) + if group is None: + raise RuntimeError('Unable to find the group named ' + robot_part) + + joints = group.get_parameter_vector_string('joints_list') + board = group.get_parameter_string('control_board') + parameters.set_parameter_vector_string('joints_list', joints) + parameters.set_parameter_vector_string('remote_control_boards', [board]) + values = group.get_parameter_vector_float('expected_values') + + # populate the parameters handler with additional parameters required by the application. There is no need to + # expose these parameter in the configuration file + parameters.set_parameter_string('local_prefix', 'joint-offset-updater') + parameters.set_parameter_bool('check_for_nan', False) + parameters.set_parameter_bool('stream_joint_states', True) + return parameters, values, joints + + +def main(): + parser = argparse.ArgumentParser(description='Automatically update the calibration delta of the robot ' + 'configuration file.') parser.add_argument('-i', '--input', type=str, required=True, help='Input xml file') parser.add_argument('-o', '--output', type=str, required=True, help='Output xml file') - parser.add_argument('--from', type=str, required=False, help='Path or name of the configuration file', default="blf-calibration-delta-update-options.ini") - parser.add_argument('-p', '--part', type=str, required=True, help='name of the part.') - + parser.add_argument('-p', '--part', type=str, required=True, help='Name of the part.') + parser.add_argument('--config', type=str, required=False, help='Path or name of the configuration file', + default="blf-calibration-delta-updater-options.ini") args = parser.parse_args() # get the original offset @@ -79,54 +119,45 @@ def compute_joint_offset(joint_index, expected_values): root = tree.getroot() offsets = get_offsets(root) - # load the parameter hander - parameterHandler = blf.parameters_handler.YarpParametersHandler() - - if (os.path.isfile(args.from)): - assert parameterHandler.set_from_file_path(args.from) is True - else: - assert parameterHandler.set_from_filename(args.from) is True - - group = parameterHandler.get_group(parser.part) - assert group is not None - joints_list = group.get_parameter_vector_string('joints_list') - control_board = group.get_parameter_string('control_board') - expected_values = param_handler.get_parameter_vector_double('expected_value') - param_handler.set_parameter_vector_string('joints_list', joints_list) - param_handler.set_parameter_vector_string('remote_control_boards', [control_board]) + # load the parameter handler + handler, expected_values, joints_list = parse_handler(args.config, args.part) # Open the sensorbridge - control_board, sensor_bridge = open_polydriver(parameter_handler) + control_board, sensor_bridge = open_polydriver(handler) new_offsets = [] - for i, joint in enumerate(joints_list): key = "" while key != 'y' and key != 'n': print_info("Do you want to calibrate the joint named: " + joint + " [y|n]") key = input() - if key = 'n': - new_offsets.append(0); - elif key == 'y' : + if key == 'n': + new_offsets.append(0) + elif key == 'y': print_info("Please move the joint in the expected configuration. Press enter when you are ready.") input() - new_offsets.append(compute_joint_offset(i, expected_values)) + print(expected_values) + new_offsets.append(compute_joint_offset(sensor_bridge, i, expected_values)) updated_offset = np.array(offsets) + np.array(new_offsets) - print_info("previous offset = " + str(['%.4f' % offset for offset in offsets]) + " deg") - print_info("new offset = " + str(['%.4f' % offset for offset in updated_offset]) + " deg") - + print_info("Previous offsets = " + str(['%.4f' % offset for offset in offsets]) + " deg") + print_info("New offsets = " + str(['%.4f' % offset for offset in updated_offset]) + " deg") + # Update the configuration file key = "" while key != 'y' and key != 'n': - print_info("Do you want to add this offset to the already existing one? [y|n]") + print_info("Do you want to replace the new offset to the existing one? [y|n]") key = input() - if key == 'n' : + if key == 'n': print_info("Offset not changed") - elif key == 'y' : + elif key == 'y': root = set_offsets(root, updated_offset) with open(args.output, 'bw') as f: f.write(prettify(root)) print_info("Offset changed") + + +if __name__ == '__main__': + main() From 5a2d01f06eebbe85f7d36c01eeb2e1b28985f41a Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 8 Jul 2021 13:47:41 +0200 Subject: [PATCH 04/12] Add the configuration file for iCubGenova09 --- utilities/calibration-delta-updater/CMakeLists.txt | 2 ++ .../blf-calibration-delta-updater-options.ini | 11 +++++++++++ 2 files changed, 13 insertions(+) create mode 100644 utilities/calibration-delta-updater/config/robots/iCubGenova09/blf-calibration-delta-updater-options.ini diff --git a/utilities/calibration-delta-updater/CMakeLists.txt b/utilities/calibration-delta-updater/CMakeLists.txt index 85bb3a7c45..71ff7c0255 100644 --- a/utilities/calibration-delta-updater/CMakeLists.txt +++ b/utilities/calibration-delta-updater/CMakeLists.txt @@ -8,4 +8,6 @@ if(FRAMEWORK_COMPILE_CalibrationDeltaUpdaterApplication) PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ DESTINATION "${CMAKE_INSTALL_BINDIR}") + install_ini_files(${CMAKE_CURRENT_SOURCE_DIR}/config) + endif() diff --git a/utilities/calibration-delta-updater/config/robots/iCubGenova09/blf-calibration-delta-updater-options.ini b/utilities/calibration-delta-updater/config/robots/iCubGenova09/blf-calibration-delta-updater-options.ini new file mode 100644 index 0000000000..fdf4fe69bb --- /dev/null +++ b/utilities/calibration-delta-updater/config/robots/iCubGenova09/blf-calibration-delta-updater-options.ini @@ -0,0 +1,11 @@ +robot_name icub + +[left_leg] +expected_values (-45.9, 0.0, 0.0, 5.153, -46.9, -26.62) +control_board "left_leg" +joints_list ( "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll") + +[right_leg] +expected_values (-46.99, 0.0, 0.0, 6.68, -45.256, -26.72) +control_board "right_leg" +joints_list ( "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") From ca590dbc84dd6d017230d95fb6e575cdee44aa25 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 8 Jul 2021 13:49:02 +0200 Subject: [PATCH 05/12] Fix test_parameters_handler_yarp.py --- .../ParametersHandler/tests/test_parameters_handler_yarp.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/bindings/python/ParametersHandler/tests/test_parameters_handler_yarp.py b/bindings/python/ParametersHandler/tests/test_parameters_handler_yarp.py index 392cb5a351..22adaa34c0 100644 --- a/bindings/python/ParametersHandler/tests/test_parameters_handler_yarp.py +++ b/bindings/python/ParametersHandler/tests/test_parameters_handler_yarp.py @@ -201,7 +201,7 @@ def test_clear(): def test_load_from_file(): handler = blf.YarpParametersHandler() - assert handler.set_from_file('config.ini') == True + assert handler.set_from_file_path('config.ini') == True assert handler.get_parameter_int("answer_to_the_ultimate_question_of_life") == 42 assert handler.get_group("CARTOONS").get_parameter_string("John") == "Doe" From 4ad174d7837b136849d1a11f79dee9ec69048332 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 8 Jul 2021 13:50:31 +0200 Subject: [PATCH 06/12] Update the CHANGELOG --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index eae4b6eb7d..b5e4a8e11e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,7 @@ All notable changes to this project are documented in this file. ### Changed - Add common Python files to gitignore (https://github.com/dic-iit/bipedal-locomotion-framework/pull/338) +- General improvements of `blf-calibration-delta-updater` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/361) ### Fix From a4b30eb6ebe4bf3e0ac6865d2e6efedf4bc0f013 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 8 Jul 2021 14:33:08 +0200 Subject: [PATCH 07/12] Update the calibration-delta-updater README --- utilities/calibration-delta-updater/README.md | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/utilities/calibration-delta-updater/README.md b/utilities/calibration-delta-updater/README.md index bd58826d6e..0591f5f456 100644 --- a/utilities/calibration-delta-updater/README.md +++ b/utilities/calibration-delta-updater/README.md @@ -1,10 +1,10 @@ -# calibration-delta-updater +# blf-calibration-delta-updater -**calibration-delta-updater** is a simple tool for updating the calibration delta of the robot. +**blf-calibration-delta-updater** is a simple tool to semi-automatic update of the calibration delta of a YARP-based robot. ## :computer: Dependencies -**calibration-delta-updater** depends of the [`YarpImplementation` of the `ISensorBridge`](https://github.com/dic-iit/bipedal-locomotion-framework/tree/master/src/RobotInterface/YarpImplementation) and on the [python bindings](https://github.com/dic-iit/bipedal-locomotion-framework/tree/master/bindings/python/RobotInterface). To run the script you also need to install some additional python dependencies +**blf-calibration-delta-updater** depends of the [`YarpImplementation` of the `ISensorBridge`](https://github.com/dic-iit/bipedal-locomotion-framework/tree/master/src/RobotInterface/YarpImplementation) and on the [python bindings](https://github.com/dic-iit/bipedal-locomotion-framework/tree/master/bindings/python/RobotInterface). To run the script you also need to install some additional python dependencies ``` sudo apt-get install python3-numpy python3-lxml @@ -12,13 +12,16 @@ sudo apt-get install python3-numpy python3-lxml ## :running: How to use the application -Please move the joint associated to the `input` configuration file in _zero_. Please refer to the posture shown [here](http://wiki.icub.org/wiki/LegsFineCalibration). +The syntax of the application follows: -You can run the script with following command ```shell -blf-calibration-delta-updater.py -i /iCubGenova09/calibrators/left_leg-calib.xml \ - -o /iCubGenova09/calibrators/left_leg-calib.xml \ - -r icub \ - -b left_leg \ - -j l_hip_pitch l_hip_roll l_hip_yaw l_knee l_ankle_pitch l_ankle_roll +blf-calibration-delta-updater.py [-h] -i INPUT -o OUTPUT -p PART [--config CONFIG] ``` + +where: +- **`INPUT`** is the path to the input `xml` file containing the calibration deltas +- **`OUTPUT`** is the path to the output `xml` file containing the calibration deltas +- **`PART`** is the name of the group will be loaded in the configuration file. For instance [here](./config/robots/iCubGenova09/blf-calibration-delta-updater-options.ini) only `left_leg` or `right_leg` are admissible. +- **`CONFIG`** (optional) is the path to the configuration file loaded by the application. By default the **blf-calibration-delta-updater** uses [`YARP ResourceFinder`](http://www.yarp.it/git-master/resource_finder_spec.html) to locate a file named `blf-calibration-delta-updater-options.ini` + +The application will ask you which joints you want to calibrate and will automatically create a configuration file. From 57fc966de2532cb152f04251f9619258a712ff72 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 9 Jul 2021 11:34:24 +0200 Subject: [PATCH 08/12] Fix typos in calibration-delta-updater README Co-authored-by: Stefano Dafarra --- utilities/calibration-delta-updater/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/utilities/calibration-delta-updater/README.md b/utilities/calibration-delta-updater/README.md index 0591f5f456..540c6f0b25 100644 --- a/utilities/calibration-delta-updater/README.md +++ b/utilities/calibration-delta-updater/README.md @@ -1,10 +1,10 @@ # blf-calibration-delta-updater -**blf-calibration-delta-updater** is a simple tool to semi-automatic update of the calibration delta of a YARP-based robot. +**blf-calibration-delta-updater** is a simple tool to semi-automatically update the calibration delta of a YARP-based robot. ## :computer: Dependencies -**blf-calibration-delta-updater** depends of the [`YarpImplementation` of the `ISensorBridge`](https://github.com/dic-iit/bipedal-locomotion-framework/tree/master/src/RobotInterface/YarpImplementation) and on the [python bindings](https://github.com/dic-iit/bipedal-locomotion-framework/tree/master/bindings/python/RobotInterface). To run the script you also need to install some additional python dependencies +**blf-calibration-delta-updater** depends on the [`YarpImplementation` of the `ISensorBridge`](https://github.com/dic-iit/bipedal-locomotion-framework/tree/master/src/RobotInterface/YarpImplementation) and on the [python bindings](https://github.com/dic-iit/bipedal-locomotion-framework/tree/master/bindings/python/RobotInterface). To run the script you also need to install some additional python dependencies ``` sudo apt-get install python3-numpy python3-lxml From e82851299724fa2d9384efce5657194d89058a86 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 9 Jul 2021 12:56:52 +0200 Subject: [PATCH 09/12] Take into account the tolerance blf-calibration-delta-updater --- .../script/blf-calibration-delta-updater.py | 54 +++++++++++++------ 1 file changed, 38 insertions(+), 16 deletions(-) diff --git a/utilities/calibration-delta-updater/script/blf-calibration-delta-updater.py b/utilities/calibration-delta-updater/script/blf-calibration-delta-updater.py index 1423eae379..1578462103 100755 --- a/utilities/calibration-delta-updater/script/blf-calibration-delta-updater.py +++ b/utilities/calibration-delta-updater/script/blf-calibration-delta-updater.py @@ -61,24 +61,31 @@ def set_offsets(xml_root, offsets): return xml_root -def compute_joint_offset(sensor_bridge, joint_index, expected_values): +def compute_joint_offset(sensor_bridge, joint_index, expected_values, tolerances): # read the joint state if not sensor_bridge.advance(): - raise RuntimeError('Unable to reda the sensors') + raise RuntimeError('Unable to read the sensors') _, joints_values, _ = sensor_bridge.get_joint_positions() # compute the offset delta = joints_values[joint_index] * 180 / math.pi - expected_values[joint_index] - return delta + + tolerance = tolerances[joint_index] + if abs(delta) < tolerances[joint_index]: + print_info("The offset is equal to " + str(delta) + + " deg. The absolute value is lower than the desired tolerance " + + str(tolerance) + " deg.") + print_info("The offset is set to 0 deg") + return 0.0 + else: + return delta def parse_handler(file, robot_part): # load the parameter handler parameters = blf.parameters_handler.YarpParametersHandler() - print("---------------------------------->" + file) - if os.path.isfile(file): if not parameters.set_from_file_path(file): raise RuntimeError('Unable to load the configuration file') @@ -92,35 +99,51 @@ def parse_handler(file, robot_part): joints = group.get_parameter_vector_string('joints_list') board = group.get_parameter_string('control_board') + values = group.get_parameter_vector_float('expected_values_in_degrees') + tolerance = group.get_parameter_vector_float('tolerance_in_degrees') + parameters.set_parameter_vector_string('joints_list', joints) parameters.set_parameter_vector_string('remote_control_boards', [board]) - values = group.get_parameter_vector_float('expected_values') # populate the parameters handler with additional parameters required by the application. There is no need to # expose these parameter in the configuration file parameters.set_parameter_string('local_prefix', 'joint-offset-updater') parameters.set_parameter_bool('check_for_nan', False) parameters.set_parameter_bool('stream_joint_states', True) - return parameters, values, joints + return parameters, values, joints, tolerance def main(): - parser = argparse.ArgumentParser(description='Automatically update the calibration delta of the robot ' - 'configuration file.') - parser.add_argument('-i', '--input', type=str, required=True, help='Input xml file') - parser.add_argument('-o', '--output', type=str, required=True, help='Output xml file') - parser.add_argument('-p', '--part', type=str, required=True, help='Name of the part.') - parser.add_argument('--config', type=str, required=False, help='Path or name of the configuration file', + parser = argparse.ArgumentParser(description='Simple tool to semi-automatically update the calibration delta of a YARP-based robot.') + parser.add_argument('-i', '--input', type=str, required=True, help='Path to the input xml file containing the calibration deltas') + parser.add_argument('-o', '--output', type=str, + required=True, help='Path to the output xml file containing the calibration deltas') + parser.add_argument('-p', '--part', type=str, required=True, + help='Name of the group will be loaded in the configuration file. For instance left_leg or right_leg.') + parser.add_argument('--config', type=str, required=False, + help='Path to the configuration file loaded by the application. ' + 'By default the blf-calibration-delta-updater uses YARP ResourceFinder ' + 'to locate a file named blf-calibration-delta-updater-options.ini', default="blf-calibration-delta-updater-options.ini") args = parser.parse_args() + # check if the input file exists + if not os.path.isfile(args.input): + raise RuntimeError('The file path ' + args.input + ' does not exist') + + output_dir = os.path.dirname(args.output) + + if output_dir and not os.path.isdir(output_dir): + raise RuntimeError('The directory named ' + os.path.dirname(args.output) + ' does not exist') + + # get the original offset tree = etree.parse(args.input) root = tree.getroot() offsets = get_offsets(root) # load the parameter handler - handler, expected_values, joints_list = parse_handler(args.config, args.part) + handler, expected_values, joints_list, tolerances = parse_handler(args.config, args.part) # Open the sensorbridge control_board, sensor_bridge = open_polydriver(handler) @@ -136,8 +159,7 @@ def main(): elif key == 'y': print_info("Please move the joint in the expected configuration. Press enter when you are ready.") input() - print(expected_values) - new_offsets.append(compute_joint_offset(sensor_bridge, i, expected_values)) + new_offsets.append(compute_joint_offset(sensor_bridge, i, expected_values, tolerances)) updated_offset = np.array(offsets) + np.array(new_offsets) print_info("Previous offsets = " + str(['%.4f' % offset for offset in offsets]) + " deg") From 736b63b9f3986abcda45cf98df20487ae44c7c0a Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 9 Jul 2021 12:57:27 +0200 Subject: [PATCH 10/12] Update the blf-calibration-delta-updater-options.ini configuration file --- .../blf-calibration-delta-updater-options.ini | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/utilities/calibration-delta-updater/config/robots/iCubGenova09/blf-calibration-delta-updater-options.ini b/utilities/calibration-delta-updater/config/robots/iCubGenova09/blf-calibration-delta-updater-options.ini index fdf4fe69bb..c1c96d195e 100644 --- a/utilities/calibration-delta-updater/config/robots/iCubGenova09/blf-calibration-delta-updater-options.ini +++ b/utilities/calibration-delta-updater/config/robots/iCubGenova09/blf-calibration-delta-updater-options.ini @@ -1,11 +1,13 @@ -robot_name icub +robot_name icubSim [left_leg] -expected_values (-45.9, 0.0, 0.0, 5.153, -46.9, -26.62) +expected_values_in_degrees (-45.9, 0.0, 0.0, 5.153, -46.9, -26.62) control_board "left_leg" joints_list ( "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll") +tolerance_in_degrees (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) [right_leg] -expected_values (-46.99, 0.0, 0.0, 6.68, -45.256, -26.72) +expected_values_in_degrees (-46.99, 0.0, 0.0, 6.68, -45.256, -26.72) control_board "right_leg" joints_list ( "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") +tolerance_in_degrees (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) \ No newline at end of file From cedabc9c7f7d8fbb6a1adfe6a512e1699f8f7c4f Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 9 Jul 2021 14:51:58 +0200 Subject: [PATCH 11/12] Update blf-calibration-delta-updater-options.ini --- .../iCubGenova09/blf-calibration-delta-updater-options.ini | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utilities/calibration-delta-updater/config/robots/iCubGenova09/blf-calibration-delta-updater-options.ini b/utilities/calibration-delta-updater/config/robots/iCubGenova09/blf-calibration-delta-updater-options.ini index c1c96d195e..63070ec748 100644 --- a/utilities/calibration-delta-updater/config/robots/iCubGenova09/blf-calibration-delta-updater-options.ini +++ b/utilities/calibration-delta-updater/config/robots/iCubGenova09/blf-calibration-delta-updater-options.ini @@ -10,4 +10,4 @@ tolerance_in_degrees (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) expected_values_in_degrees (-46.99, 0.0, 0.0, 6.68, -45.256, -26.72) control_board "right_leg" joints_list ( "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll") -tolerance_in_degrees (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) \ No newline at end of file +tolerance_in_degrees (0.01, 0.01, 0.01, 0.01, 0.01, 0.01) From 7c965952fd86fbd8b919bdc9a7e853e185ce6d3b Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Fri, 9 Jul 2021 14:54:03 +0200 Subject: [PATCH 12/12] Update README.md --- utilities/calibration-delta-updater/README.md | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/utilities/calibration-delta-updater/README.md b/utilities/calibration-delta-updater/README.md index 540c6f0b25..10c0fb0d7b 100644 --- a/utilities/calibration-delta-updater/README.md +++ b/utilities/calibration-delta-updater/README.md @@ -15,7 +15,18 @@ sudo apt-get install python3-numpy python3-lxml The syntax of the application follows: ```shell -blf-calibration-delta-updater.py [-h] -i INPUT -o OUTPUT -p PART [--config CONFIG] +usage: blf-calibration-delta-updater.py [-h] -i INPUT -o OUTPUT -p PART [--config CONFIG] + +Simple tool to semi-automatically update the calibration delta of a YARP-based robot. + +optional arguments: + -h, --help show the help message and exit + -i INPUT, --input INPUT + Path to the input xml file containing the calibration deltas + -o OUTPUT, --output OUTPUT + Path to the output xml file containing the calibration deltas + -p PART, --part PART Name of the group will be loaded in the configuration file. For instance left_leg or right_leg. + --config CONFIG Path to the configuration file loaded by the application. By default the blf-calibration-delta-updater uses YARP ResourceFinder to locate a file named blf-calibration-delta-updater-options.ini ``` where: