Generalize the blf-calibration-delta-updater script

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
 

bindings/python/ParametersHandler/src/YarpParametersHandler.cpp

@@ -13,6 +13,8 @@
 #include <BipedalLocomotion/ParametersHandler/YarpImplementation.h>
 #include <BipedalLocomotion/bindings/ParametersHandler/YarpParametersHandler.h>
 
+#include <yarp/os/ResourceFinder.h>
+
 namespace BipedalLocomotion
 {
 namespace bindings
@@ -28,7 +30,14 @@ void CreateYarpParameterHandler(pybind11::module& module)
                std::shared_ptr<YarpImplementation>,
                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

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"

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

utilities/calibration-delta-updater/README.md

@@ -1,24 +1,38 @@
-# 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
 ```
 
 ## :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 <robots-configuration_dir>/iCubGenova09/calibrators/left_leg-calib.xml \
-                                 -o <robots-configuration_dir>/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.

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)

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,69 +33,153 @@ 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":
                     deltas = [float(x.group()) for x in re.finditer(r'[-+]?[0-9]+(?:.[0-9]+)?', param.text)]
 
     return deltas
 
+
 def prettify(elem):
-    return etree.tostring(elem, encoding="UTF-8",
-                   xml_declaration=True,
-                   pretty_print=True,
-                   doctype='<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">')
+    return etree.tostring(elem,
+                          encoding="UTF-8",
+                          xml_declaration=True,
+                          pretty_print=True,
+                          doctype='<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" '
+                                  '"http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">')
 
-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":
                     param.text = ' '.join(format(x, "10.3f") for x in 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()