From c377cb0ec8b777f8c026b6e762629bacafbd1901 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 6 Jun 2023 18:51:49 +0200 Subject: [PATCH] Implement the test for the gyro --- run_tests.py | 16 +++- xcub_sensor_check/IMU_test.py | 133 ++++++++++++++++++++++++++++-- xcub_sensor_check/__init__.py | 2 +- xcub_sensor_check/generic_test.py | 4 +- 4 files changed, 145 insertions(+), 10 deletions(-) diff --git a/run_tests.py b/run_tests.py index 1cc5324..93b2e61 100644 --- a/run_tests.py +++ b/run_tests.py @@ -5,7 +5,7 @@ from typing import Dict -def generate_test_list(config_file: Path): +def generate_test_list(config_file: Path, output_folder: Path): param_handler = blf.parameters_handler.TomlParametersHandler() param_handler.set_from_file(str(config_file)) @@ -19,7 +19,7 @@ def generate_test_list(config_file: Path): for test_name in tests_name_list: test_group = param_handler.get_group(test_name) test_type = getattr(xcub_sensor_check, test_group.get_parameter_string("type")) - test = test_type(test_name) + test = test_type(test_name, output_folder) test.configure(test_group) test_list.append(test) @@ -47,9 +47,19 @@ def main(): required=True, help="Path to the configuration file containing all the name of the tests.", ) + parser.add_argument( + "--output", + "-o", + type=lambda p: Path(p).absolute(), + default=Path.cwd(), + required=False, + help="Path where the file generated by the tests will be saved.", + ) args = parser.parse_args() - test_list = generate_test_list(config_file=args.config_file) + test_list = generate_test_list( + config_file=args.config_file, output_folder=args.output + ) for test in test_list: test.outcome = test.run() diff --git a/xcub_sensor_check/IMU_test.py b/xcub_sensor_check/IMU_test.py index 5bdca71..c0a1c86 100644 --- a/xcub_sensor_check/IMU_test.py +++ b/xcub_sensor_check/IMU_test.py @@ -1,13 +1,136 @@ from xcub_sensor_check.generic_test import GenericTest import bipedal_locomotion_framework as blf +import numpy as np +import h5py +from enum import Enum +import idyntree.bindings as idyn +from pathlib import Path +import matplotlib.pyplot as plt -class IMUTest(GenericTest): - def __init__(self, name): - super().__init__(name=name) +class SignalType(Enum): + def __str__(self): + return str(self.name) + "s" + + unknown = 0 + accelerometer = 1 + gyro = 2 + + +class GenericImuSignalTest(GenericTest): + def __init__( + self, name: str, additional_output_folder: Path, signal_type: SignalType + ): + super().__init__(name=name, additional_output_folder=additional_output_folder) + + self.model_path = "" + self.sensor_name = "" + self.frame_name = "" + self.joints_name = [] + + class JointState: + self.positions = np.array([]) + self.velocities = np.array([]) + self.accelerations = np.array([]) + + self.signal_type = signal_type + self.joint_state = JointState() + self.imu_signal = np.array([]) + self.expected_imu_signal = np.array([]) + + self.accepted_mean_error = np.array([]) + self.accepted_std_error = np.array([]) def configure(self, param_handler: blf.parameters_handler.IParametersHandler): - pass + self.sensor_name = param_handler.get_parameter_string("sensor_name") + self.frame_name = param_handler.get_parameter_string("frame_name") + self.model_path = param_handler.get_parameter_string("model_path") + self.accepted_mean_error = param_handler.get_parameter_vector_float( + "error_mean_tolerance" + ) + self.accepted_std_error = param_handler.get_parameter_vector_float( + "error_std_tolerance" + ) + + file_name = param_handler.get_parameter_string("dataset_file_name") + with h5py.File(file_name, "r") as file: + root_variable = file.get("robot_logger_device") + joint_ref = root_variable["description_list"] + self.joints_name = [ + "".join(chr(c[0]) for c in file[ref]) for ref in joint_ref[0] + ] + self.joint_state.positions = np.squeeze( + np.array(root_variable["joints_state"]["positions"]["data"]) + ) + + self.joint_state.velocities = np.squeeze( + np.array(root_variable["joints_state"]["velocities"]["data"]) + ) + self.joint_state.accelerations = np.squeeze( + np.array(root_variable["joints_state"]["accelerations"]["data"]) + ) + self.imu_signal = np.squeeze( + np.array(root_variable[str(self.signal_type)][self.sensor_name]["data"]) + ) + + self.expected_imu_signal.resize((self.joint_state.positions.shape[0], 3)) + + def get_kindyn(self) -> idyn.KinDynComputations: + ml = idyn.ModelLoader() + ml.loadReducedModelFromFile(self.model_path, self.joints_name) + kindyn = idyn.KinDynComputations() + kindyn.loadRobotModel(ml.model()) + kindyn.setFrameVelocityRepresentation(idyn.BODY_FIXED_REPRESENTATION) + return kindyn + + +class GyroTest(GenericImuSignalTest): + def __init__(self, name: str, additional_output_folder: Path): + super().__init__( + name=name, + additional_output_folder=additional_output_folder, + signal_type=SignalType.gyro, + ) def run(self): - return True + kindyn = self.get_kindyn() + gravity = np.array([0, 0, -blf.math.StandardAccelerationOfGravitation]) + + for i in range(self.joint_state.positions.shape[0]): + kindyn.setRobotState( + self.joint_state.positions[i, :], + self.joint_state.velocities[i, :], + gravity, + ) + + tmp = kindyn.getFrameVel(self.frame_name).toNumPy() + self.expected_imu_signal[i, :] = tmp[3:6] + + error = self.expected_imu_signal - self.imu_signal + + fig, axs = plt.subplots(3, 2) + axis_name = ["x", "y", "z"] + fig.suptitle(self.name.replace("_", " "), fontsize=16) + fig.set_size_inches(10.5, 6.5) + + for i in range(3): + axs[i][0].set(ylabel="ω" + axis_name[i] + " (rad/s)") + axs[i][0].plot(self.expected_imu_signal[:, i], label="Kinematics") + axs[i][0].plot(self.imu_signal[:, i], label="Sensor") + axs[i][1].plot(error[:, i], label="Error") + axs[i][0].legend() + axs[i][1].legend() + + (self.additional_output_folder / self.name).mkdir(parents=True, exist_ok=True) + + plt.savefig( + str(self.additional_output_folder / self.name / (self.name + ".png")) + ) + plt.close() + + std = np.std(error, axis=0) + mean = np.mean(error, axis=0) + + return (np.abs(mean) < self.accepted_mean_error).all() and ( + std < self.accepted_std_error + ).all() diff --git a/xcub_sensor_check/__init__.py b/xcub_sensor_check/__init__.py index b9fe91a..9a203a3 100644 --- a/xcub_sensor_check/__init__.py +++ b/xcub_sensor_check/__init__.py @@ -1 +1 @@ -from xcub_sensor_check.IMU_test import IMUTest +from xcub_sensor_check.IMU_test import GyroTest diff --git a/xcub_sensor_check/generic_test.py b/xcub_sensor_check/generic_test.py index 8698405..d1f2a9f 100644 --- a/xcub_sensor_check/generic_test.py +++ b/xcub_sensor_check/generic_test.py @@ -1,11 +1,13 @@ import bipedal_locomotion_framework.bindings as blf from abc import ABC, abstractmethod +from pathlib import Path class GenericTest(ABC): - def __init__(self, name): + def __init__(self, name: str, additional_output_folder: Path): self.outcome = False self.name = name + self.additional_output_folder = additional_output_folder @abstractmethod def configure(