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 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 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" 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/README.md b/utilities/calibration-delta-updater/README.md index bd58826d6e..10c0fb0d7b 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-automatically update 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 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 @@ -12,13 +12,27 @@ 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 +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: +- **`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. 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..63070ec748 --- /dev/null +++ b/utilities/calibration-delta-updater/config/robots/iCubGenova09/blf-calibration-delta-updater-options.ini @@ -0,0 +1,13 @@ +robot_name icubSim + +[left_leg] +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_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) 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..1578462103 100755 --- a/utilities/calibration-delta-updater/script/blf-calibration-delta-updater.py +++ b/utilities/calibration-delta-updater/script/blf-calibration-delta-updater.py @@ -9,23 +9,21 @@ 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) + print('\33[104m' + '[blf-calibration-delta-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) + 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]) @@ -35,7 +33,7 @@ def open_polydriver(robot_name, control_board, joints_list): 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": @@ -43,15 +41,18 @@ def get_offsets(xml_root): return deltas + 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"): +def set_offsets(xml_root, offsets): + for node in xml_root.findall("group"): if node.get('name') == "CALIBRATION": for param in node: if param.get('name') == "calibrationDelta": @@ -59,45 +60,126 @@ def set_offsets(xml_root, offsets): return xml_root -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') - args = parser.parse_args() +def compute_joint_offset(sensor_bridge, joint_index, expected_values, tolerances): + # read the joint state + if not sensor_bridge.advance(): + raise RuntimeError('Unable to read the sensors') - tree = etree.parse(args.input) - root = tree.getroot() - offsets = get_offsets(root) + _, joints_values, _ = sensor_bridge.get_joint_positions() - control_board, sensor_bridge = open_polydriver(args.robot, args.board, args.joints) + # compute the offset + delta = joints_values[joint_index] * 180 / math.pi - expected_values[joint_index] + + 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() + + 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') + 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]) + + # 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, tolerance + + +def main(): + 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() - print_info("Please put the joints in zero configuration.") - print_info("Press enter and I will show you the offsets.") - input() + # check if the input file exists + if not os.path.isfile(args.input): + raise RuntimeError('The file path ' + args.input + ' does not exist') - assert sensor_bridge.advance() is True + output_dir = os.path.dirname(args.output) - _, joints_values, _ = sensor_bridge.get_joint_positions() - new_offsets = np.array(offsets) + joints_values * 180 / math.pi + if output_dir and not os.path.isdir(output_dir): + raise RuntimeError('The directory named ' + os.path.dirname(args.output) + ' does not exist') - print_info("Joint values = " + str(['%.4f' % joint for joint in joints_values * 180 / math.pi]) + " deg") - 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") + # 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, tolerances = parse_handler(args.config, args.part) + + # Open the sensorbridge + 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': + print_info("Please move the joint in the expected configuration. Press enter when you are ready.") + input() + 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") + 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' : - root = set_offsets(root, new_offsets) + 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()