diff --git a/CHANGELOG.md b/CHANGELOG.md
index 9629b4bdee..a67fbc27e1 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -6,6 +6,7 @@ All notable changes to this project are documented in this file.
- Add `USE_SYSTEM_tiny-process-library` CMake option to use `tiny-process-library` found in system (https://github.com/ami-iit/bipedal-locomotion-framework/pull/891)
- Add the test for the `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/862)
- Implement low-pass filter for estimated friction torques in `JointTorqueControlDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/892)
+- Add `blf-motor-current-tracking.py` application (https://github.com/ami-iit/bipedal-locomotion-framework/pull/894)
### Changed
diff --git a/bindings/python/RobotInterface/src/RobotControl.cpp b/bindings/python/RobotInterface/src/RobotControl.cpp
index 0fa66b31ac..5890429b69 100644
--- a/bindings/python/RobotInterface/src/RobotControl.cpp
+++ b/bindings/python/RobotInterface/src/RobotControl.cpp
@@ -36,6 +36,7 @@ void CreateIRobotControl(pybind11::module& module)
.value("Velocity", IRobotControl::ControlMode::Velocity)
.value("Torque", IRobotControl::ControlMode::Torque)
.value("PWM", IRobotControl::ControlMode::PWM)
+ .value("Current", IRobotControl::ControlMode::Current)
.value("Idle", IRobotControl::ControlMode::Idle)
.value("Unknown", IRobotControl::ControlMode::Unknown)
.export_values();
diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml
index 638c786b96..6caefa65ff 100644
--- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml
+++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml
@@ -17,7 +17,7 @@ BSD-3-Clause license. -->
- ("Walking")
+ ("Walking", "CurrentControl")
()
@@ -26,6 +26,13 @@ BSD-3-Clause license. -->
"walking"
"udp"
+
+
+ "/yarp-robot-logger/exogenous_signals/current_control_trajectory"
+ "/motor_current_tracking/logger"
+ "current_control"
+ "udp"
+
@@ -38,6 +45,7 @@ BSD-3-Clause license. -->
false
true
+ false
true
false
true
diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml
index ad4185012d..43ea7684ff 100644
--- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml
+++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml
@@ -30,7 +30,7 @@ BSD-3-Clause license. -->
- ("Walking", "GridTracking","cmw", "Balancing")
+ ("Walking", "GridTracking","cmw", "Balancing", "CurrentControl")
()
@@ -54,6 +54,13 @@ BSD-3-Clause license. -->
"udp"
+
+ "/yarp-robot-logger/exogenous_signals/current_control_trajectory"
+ "/motor_current_tracking/logger"
+ "current_control"
+ "udp"
+
+
"/yarp-robot-logger/exogenous_signals/rde"
"/robot-dynamics-estimator/logger"
diff --git a/utilities/CMakeLists.txt b/utilities/CMakeLists.txt
index e653e43ecc..3a7cc273fa 100644
--- a/utilities/CMakeLists.txt
+++ b/utilities/CMakeLists.txt
@@ -11,3 +11,4 @@ add_subdirectory(calibration-delta-updater)
add_subdirectory(balancing-position-control)
add_subdirectory(balancing-torque-control)
add_subdirectory(joints-grid-position-tracking)
+add_subdirectory(motor-current-tracking)
diff --git a/utilities/motor-current-tracking/CMakeLists.txt b/utilities/motor-current-tracking/CMakeLists.txt
new file mode 100644
index 0000000000..10701319b6
--- /dev/null
+++ b/utilities/motor-current-tracking/CMakeLists.txt
@@ -0,0 +1,13 @@
+# This software may be modified and distributed under the terms of the
+# BSD-3-Clause license.
+# Authors: Ines Sorrentino, Lorenzo Moretti
+
+if(FRAMEWORK_COMPILE_MotorCurrentTrackingApplication)
+
+ install(FILES blf-motor-current-tracking.py
+ PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ
+ DESTINATION "${CMAKE_INSTALL_BINDIR}")
+
+ install_ini_files(${CMAKE_CURRENT_SOURCE_DIR}/config)
+
+endif()
diff --git a/utilities/motor-current-tracking/README.md b/utilities/motor-current-tracking/README.md
new file mode 100644
index 0000000000..14ab5eebb9
--- /dev/null
+++ b/utilities/motor-current-tracking/README.md
@@ -0,0 +1,145 @@
+# motor-current-tracking
+The **motor-current-tracking** is an application that allows to send current commands
+to the motors. The user can leverage predefined signals to define the reference current.
+
+## 🏃 How to use the application
+It is a command line application, which can be run like:
+
+```sh
+blf-motor-current-tracking.py
+```
+
+The application accepts an additional argument to specify the type of trajectory to deploy.
+From terminal, run:
+
+```sh
+blf-motor-current-tracking.py -h
+```
+
+to display the help message which details how to pass the additional input argument.
+
+If bipedal locmotion framework is installed correctly, the application can be run from anywhere. In order to install the application, you should set the option `FRAMEWORK_COMPILE_MotorCurrentTrackingApplication` to `ON`.
+
+Once run, the application will:
+
+1. Generate the current trajectory used to drive the motors.
+
+2. Drive the motors in current as long as they are within the safety limits, which are defined in the configuration files.
+
+In fact, the application will check if the joints come close to the position limits. In this case, the related motors are switch to position control
+and kept still in the last read position as long as all the other motors have completed the current trajectory.
+
+Moreover, the application streams data, which is the motors reference current and the control mode, through the port
+`/motor_current_tracking/logger/data:o`. The user may collect the data via [`YarpRobotLoggerDevice`](../../devices/YarpRobotLoggerDevice).
+
+## Configuration files
+Configuration files allow to specify the motors to drive and the current trajectory to use.
+[`blf-motor-current-tracking-options.ini`](./config/robots/ergoCubSN001/blf-motor-current-tracking-options.ini) is an example of the main configuration file.
+
+In this file, the following parameters are defined:
+
+1. the sample time `dt` of the reference trajectory
+2. the joints position safety threshold `safety_threshold`, which restricts the motion range of joints to `[joint_lower_limit + safety_threshold, joint_upper_limit - safety_threshold]`
+3. the `use_safety_threshold_for_starting_position` boolean, which determines wheter to use (or not) the `safety_threshold` when computing the starting positions.
+4. the number of starting positions `number_of_starting_points` from which the same reference trajectory is repeated.
+5. the `bypass_motor_current_measure` boolean vector (one element per motor), which determines wether to use the measured motor current when generating the trajectory as detailed later.
+6. the `SINUSOID` parameters group, which defines the sinusoid trajectory as detailed later.
+7. the `RAMP` parameters group, which defines the ramp trajectory as detailed later.
+8. the `MOTOR` parameters group, which defines the list of available joints `joints_list`, the respective `k_tau` coefficient (in [A/Nm]) and the respective `max_safety_current` (in [A]).
+
+Instead, the configuration file [`robot_control.ini`](./config/robots/ergoCubSN001/blf_motor_current_tracking/robot_control.ini) defines the list of motors to command.
+
+If you want to run the application for a different robot remember to create a new folder in
+[`./config/robots/`](./config/robots). The name of the folder should match the name of the robot.
+
+## Predefined Trajectories
+Two trajectory types are currently supported: sinusoids and ramps.
+
+### Sinusoids
+
+They are defined as sinusoidal signals with varying frequency `f` and amplitude `A`:
+
+```math
+
+current = current_0 + A \sin (2 \pi f t)
+
+```
+
+The frequency is gradually decreased, every 2 cycles, from `max_frequency` to `min_frequency` by `frequency_decrement`.
+Once the frequency range is covered the amplitude is increased by `delta_current_increment`.
+The starting amplitude is set by `min_delta_current`, while the final one by `max_delta_current`.
+
+For each motor, if the respecitive boolean in the `bypass_motor_current_measure` vector is set to `true`, then the initial current `current_0` is not the measured one at the starting position, but is set to `0`.
+
+Each current sinusoidal signal is repeated for each starting position.
+
+These starting positions are computed by dividing the range of motion of the joint
+(`[joint_lower_limit, joint_upper_limit]`) in `number_of_starting_points + 1` segments, and
+then taking the inner nodes.
+
+Finally, every parameter of the `SINUSOID` group has to be defined by a vector whose length corresponds to the number
+of motors to drive.
+
+
+### Ramps
+
+They are ramp signals which increases of from an initial current to a maximum current, with a slope that ranges from `initial_speed` to `final_speed`.
+
+Depending on the `bypass_motor_current_measure` parameter, the initial current is set to `0` (if the respective parameter is set to `true`) or to the measured one at the starting position (if the respective parameter is set to `false`).
+While the maximum current is given by the initial current increased of `max_current_increment`.
+
+Note that, each ramp is repeated twice for each starting position, in order to cover both direction of motion.
+Morever from one ramp to the following one the slope is incremented by `speed_increment`.
+
+The sarting positions are computed like for the sinusoidal trajectories.
+
+Finally, every parameter of the `RAMP` group has to be defined by a vector whose length corresponds to the number
+of motors to drive.
+
+### Examples
+
+The following are some set of configurations which were tested on `ergoCubSN001`.
+
+```
+dt 0.002 #[s]
+safety_threshold 2.0 #[deg]
+use_safety_threshold_for_starting_position false
+number_of_starting_points 5
+bypass_motor_current_measure (true)
+
+[SINUSOID]
+min_delta_current ( 0.7 ) #[A]
+max_delta_current ( 1.8 ) #[A]
+delta_current_increment ( 0.2 ) #[A]
+min_frequency ( 0.05 ) #[Hz]
+max_frequency ( 0.45 ) #[Hz]
+frequency_decrement ( 0.1 ) #[Hz]
+
+[RAMP]
+initial_speed (0.15)
+final_speed (0.40)
+speed_increment (0.08)
+max_current_increment (2.00)
+
+[MOTOR]
+joints_list ("l_hip_roll", "l_hip_pitch", "l_hip_yaw",
+ "l_knee", "l_ankle_pitch", "l_ankle_roll",
+ "r_hip_roll", "r_hip_pitch", "r_hip_yaw",
+ "r_knee", "r_ankle_pitch", "r_ankle_roll")
+k_tau (0.094, 0.064, 0.150,
+ 0.064, 0.064, 0.177,
+ 0.094, 0.064, 0.150,
+ 0.064, 0.064, 0.177)
+max_safety_current (8.0, 8.0, 4.0,
+ 6.0, 3.5, 3.0,
+ 8.0, 8.0, 4.0,
+ 6.0, 3.5, 3.0)
+
+[ROBOT_CONTROL]
+robot_name ergocub
+joints_list ("r_ankle_pitch")
+remote_control_boards ("right_leg")
+positioning_duration 3.0 #[s]
+positioning_tolerance 0.05 #[rad]
+position_direct_max_admissible_error 0.1 #[rad]
+```
diff --git a/utilities/motor-current-tracking/blf-motor-current-tracking.py b/utilities/motor-current-tracking/blf-motor-current-tracking.py
new file mode 100644
index 0000000000..518ce6c7b9
--- /dev/null
+++ b/utilities/motor-current-tracking/blf-motor-current-tracking.py
@@ -0,0 +1,863 @@
+#!/usr/bin/env python3
+
+# This software may be modified and distributed under the terms of the BSD-3-Clause license.
+
+import argparse
+import datetime
+import os
+import signal
+import sys
+from abc import ABC, abstractmethod
+from typing import Callable, List, Optional, Type
+
+import bipedal_locomotion_framework as blf
+import numpy as np
+import numpy.typing as npt
+import yarp
+
+logPrefix = "[MotorCurrentTrackingApplication]"
+
+ParamHandler = Type[blf.parameters_handler.YarpParametersHandler]
+RobotControl = Type[blf.robot_interface.YarpRobotControl]
+SensorBridge = Type[blf.robot_interface.YarpSensorBridge]
+PolyDriver = Type[blf.robot_interface.PolyDriver]
+
+
+class MotorParameters(ABC):
+ # k_tau[A/Nm] includes the gear ratio
+ k_tau = dict()
+ # max_safety_current[A] limits the motor current to avoid damages
+ max_safety_current = dict()
+
+ @staticmethod
+ def from_parameter_handler(motor_param_handler: ParamHandler) -> None:
+
+ joints_list = motor_param_handler.get_parameter_vector_string("joints_list")
+ k_tau = motor_param_handler.get_parameter_vector_float("k_tau")
+ max_safety_current = motor_param_handler.get_parameter_vector_float(
+ "max_safety_current"
+ )
+
+ if len(joints_list) != len(k_tau):
+ raise ValueError(
+ "{} The number of joints must be equal to the size of the k_tau".format(
+ logPrefix
+ )
+ )
+
+ if len(joints_list) != len(max_safety_current):
+ raise ValueError(
+ "{} The number of joints must be equal to the size of the max_current".format(
+ logPrefix
+ )
+ )
+
+ MotorParameters.k_tau = dict(zip(joints_list, k_tau))
+ MotorParameters.max_safety_current = dict(zip(joints_list, max_safety_current))
+
+
+class Trajectory(ABC):
+ @abstractmethod
+ def generate(self, *args, **kwargs):
+ pass
+
+
+class SinusoidTrajectoryGenerator(Trajectory):
+ def __init__(
+ self,
+ min_delta_current: npt.NDArray[np.float_],
+ max_delta_current: npt.NDArray[np.float_],
+ delta_current_increment: npt.NDArray[np.float_],
+ min_frequency: npt.NDArray[np.float_],
+ max_frequency: npt.NDArray[np.float_],
+ frequency_decrement: npt.NDArray[np.float_],
+ ):
+ self.min_delta_current = min_delta_current
+ self.max_delta_current = max_delta_current
+ self.delta_current_increment = delta_current_increment
+ self.min_frequency = min_frequency
+ self.max_frequency = max_frequency
+ self.frequency_decrement = frequency_decrement
+ self.joint_list = []
+ self.trajectory = np.array([])
+
+ def from_parameter_handler(
+ param_handler: ParamHandler,
+ ) -> "SinusoidTrajectoryGenerator":
+
+ min_delta_current = param_handler.get_parameter_vector_float(
+ "min_delta_current"
+ )
+ max_delta_current = param_handler.get_parameter_vector_float(
+ "max_delta_current"
+ )
+ delta_current_increment = param_handler.get_parameter_vector_float(
+ "delta_current_increment"
+ )
+ min_frequency = param_handler.get_parameter_vector_float("min_frequency")
+ max_frequency = param_handler.get_parameter_vector_float("max_frequency")
+ frequency_decrement = param_handler.get_parameter_vector_float(
+ "frequency_decrement"
+ )
+
+ return SinusoidTrajectoryGenerator(
+ min_delta_current=min_delta_current,
+ max_delta_current=max_delta_current,
+ delta_current_increment=delta_current_increment,
+ min_frequency=min_frequency,
+ max_frequency=max_frequency,
+ frequency_decrement=frequency_decrement,
+ )
+
+ def set_joint_list(self, joint_list: List[str]):
+ self.joint_list = joint_list
+
+ def generate(
+ self,
+ dt: float,
+ initial_current: float,
+ joint_index: Optional[int] = None,
+ opposite_direction: Optional[bool] = False,
+ ) -> List[float]:
+
+ # Check if joint list is set
+ if not self.joint_list:
+ raise ValueError(
+ "{} Joint list must be set before generating the trajectory".format(
+ logPrefix
+ )
+ )
+
+ # Check if joint index has to be specified
+ if len(self.joint_list) > 1 and (joint_index is None):
+ raise ValueError(
+ "{} Joint index must be specified when more than one joint is controlled".format(
+ logPrefix
+ )
+ )
+
+ # Set default joint index, if not specified
+ if joint_index is None:
+ joint_index = 0
+
+ A = self.min_delta_current[joint_index]
+ f_in = self.max_frequency[joint_index]
+ f_end = self.min_frequency[joint_index]
+ delta_f = -self.frequency_decrement[joint_index]
+ delta_current_increment = self.delta_current_increment[joint_index]
+
+ # Generate the trajectory
+ trajectory = []
+
+ while np.abs(A) <= np.abs(self.max_delta_current[joint_index]):
+
+ for f in np.arange(f_in, f_end, delta_f):
+ t_max = 2.0 / f
+ t = np.arange(0, t_max, dt)
+ trajectory.extend(initial_current + A * np.sin(2 * np.pi * f * t))
+
+ A += delta_current_increment
+
+ return trajectory
+
+ def create_starting_points(
+ self,
+ number_of_starting_points: int,
+ number_of_joints: int,
+ lower_limits: npt.NDArray[np.float_],
+ upper_limits: npt.NDArray[np.float_],
+ safety_threshold: Optional[float] = 0.0,
+ ) -> npt.NDArray[np.float_]:
+
+ if (
+ number_of_joints != lower_limits.size
+ or number_of_joints != upper_limits.size
+ ):
+ raise ValueError(
+ "{} The number of joints must be equal to the size of the lower and upper limits".format(
+ logPrefix
+ )
+ )
+
+ starting_points = np.zeros((number_of_starting_points, number_of_joints))
+ for joint_index in range(number_of_joints):
+ tmp = np.linspace(
+ lower_limits[joint_index] + safety_threshold,
+ upper_limits[joint_index] - safety_threshold,
+ number_of_starting_points + 2,
+ )
+ starting_points[:, joint_index] = tmp[1:-1]
+
+ return starting_points
+
+
+class RampTrajectoryGenerator(Trajectory):
+ def __init__(
+ self,
+ max_current_increment: npt.NDArray[np.float_],
+ speed_in: npt.NDArray[np.float_],
+ speed_end: npt.NDArray[np.float_],
+ speed_increment: npt.NDArray[np.float_],
+ ):
+ self.max_current_increment = max_current_increment
+ self.speed_in = speed_in
+ self.speed = speed_in.copy()
+ self.speed_end = speed_end
+ self.speed_increment = speed_increment
+ self.joint_list = []
+ self.trajectory = np.array([])
+
+ def from_parameter_handler(
+ param_handler: ParamHandler,
+ ) -> "RampTrajectoryGenerator":
+
+ max_current_increment = param_handler.get_parameter_vector_float(
+ "max_current_increment"
+ )
+ speed_in = param_handler.get_parameter_vector_float("initial_speed")
+ speed_end = param_handler.get_parameter_vector_float("final_speed")
+ speed_increment = param_handler.get_parameter_vector_float("speed_increment")
+
+ return RampTrajectoryGenerator(
+ max_current_increment=max_current_increment,
+ speed_in=speed_in,
+ speed_end=speed_end,
+ speed_increment=speed_increment,
+ )
+
+ def set_joint_list(self, joint_list: List[str]):
+ self.joint_list = joint_list
+
+ def reset_velocity(self, joint_index: Optional[int] = None):
+
+ # Set default joint index, if not specified
+ if joint_index is None:
+ joint_index = 0
+
+ self.speed[joint_index] = self.speed_in[joint_index]
+
+ def increment_velocity(self, joint_index: Optional[int] = None):
+
+ # Set default joint index, if not specified
+ if joint_index is None:
+ joint_index = 0
+
+ self.speed[joint_index] = (
+ self.speed[joint_index] + self.speed_increment[joint_index]
+ )
+
+ def generate(
+ self,
+ dt: float,
+ initial_current: float,
+ joint_index: Optional[int] = None,
+ opposite_direction: Optional[bool] = False,
+ ) -> List[float]:
+
+ # Check if joint list is set
+ if not self.joint_list:
+ raise ValueError(
+ "{} Joint list must be set before generating the trajectory".format(
+ logPrefix
+ )
+ )
+
+ # Check if joint index has to be specified
+ if len(self.joint_list) > 1 and (joint_index is None):
+ raise ValueError(
+ "{} Joint index must be specified when more than one joint is controlled".format(
+ logPrefix
+ )
+ )
+
+ # Set default joint index, if not specified
+ if joint_index is None:
+ joint_index = 0
+
+ # Generate the trajectory
+ trajectory = []
+
+ max_current_increment = (
+ -self.max_current_increment[joint_index]
+ if opposite_direction
+ else self.max_current_increment[joint_index]
+ )
+ speed = (
+ -self.speed[joint_index] if opposite_direction else self.speed[joint_index]
+ )
+ t_start = 0.0
+ t_end = max_current_increment / speed
+ t = t_start
+ motor_current = initial_current
+
+ while t <= t_end:
+ trajectory.append(motor_current)
+ t = t + dt
+ motor_current = motor_current + dt * speed
+
+ return trajectory
+
+ def create_starting_points(
+ self,
+ number_of_starting_points: int,
+ number_of_joints: int,
+ lower_limits: npt.NDArray[np.float_],
+ upper_limits: npt.NDArray[np.float_],
+ safety_threshold: Optional[float] = 0.0,
+ ) -> npt.NDArray[np.float_]:
+
+ if (
+ number_of_joints != lower_limits.size
+ or number_of_joints != upper_limits.size
+ ):
+ raise ValueError(
+ "{} The number of joints must be equal to the size of the lower and upper limits".format(
+ logPrefix
+ )
+ )
+
+ starting_points = np.zeros((number_of_starting_points, number_of_joints))
+ for joint_index in range(number_of_joints):
+ tmp = np.linspace(
+ lower_limits[joint_index] + safety_threshold,
+ upper_limits[joint_index] - safety_threshold,
+ number_of_starting_points + 2,
+ )
+ starting_points[:, joint_index] = tmp[1:-1]
+
+ # repeat the starting points
+ # each ramp has to be performed in both direction and for each speed
+ speed_in = np.array(self.speed_in)
+ speed_end = np.array(self.speed_end)
+ speed_increment = np.array(self.speed_increment)
+ repeats = 2 * int(
+ np.floor(np.min((speed_end - speed_in) / speed_increment)) + 1
+ )
+
+ return np.repeat(starting_points, repeats=repeats, axis=0)
+
+
+def build_remote_control_board_driver(
+ param_handler: ParamHandler, local_prefix: str
+) -> PolyDriver:
+ param_handler.set_parameter_string("local_prefix", local_prefix)
+ return blf.robot_interface.construct_remote_control_board_remapper(param_handler)
+
+
+def create_ctrl_c_handler(
+ sensor_bridge: SensorBridge, robot_control: RobotControl
+) -> Callable[[int, object], None]:
+ def ctrl_c_handler(sig, frame):
+ blf.log().info("{} Ctrl+C pressed. Exiting gracefully.".format(logPrefix))
+ # get the feedback
+ if not sensor_bridge.advance():
+ raise RuntimeError(
+ "{} Unable to advance the sensor bridge".format(logPrefix)
+ )
+
+ are_joints_ok, joint_positions, _ = sensor_bridge.get_joint_positions()
+
+ if not are_joints_ok:
+ raise RuntimeError("{} Unable to get the joint positions".format(logPrefix))
+
+ # set the control mode to position
+ if not robot_control.set_control_mode(
+ blf.robot_interface.YarpRobotControl.Position
+ ):
+ raise RuntimeError("{} Unable to set the control mode".format(logPrefix))
+ if not robot_control.set_references(
+ joint_positions, blf.robot_interface.YarpRobotControl.Position
+ ):
+ raise RuntimeError("{} Unable to set the references".format(logPrefix))
+
+ blf.log().info(
+ "{} Sleep for two seconds. Just to be sure the interfaces are on.".format(
+ logPrefix
+ )
+ )
+ blf.clock().sleep_for(datetime.timedelta(seconds=2))
+ sys.exit(0)
+
+ return ctrl_c_handler
+
+
+def main():
+
+ # Create the argument parser
+ arg_parser = argparse.ArgumentParser(
+ description="This application drives the motors in current based on the trajectory type specified from the user."
+ )
+
+ # Add an argument for the trajectory type with default value "sinusoid"
+ arg_parser.add_argument(
+ "-t",
+ "--trajectory",
+ type=str,
+ default="sinusoid",
+ choices=["ramp", "sinusoid"],
+ help="which trajectory (default: sinusoid)",
+ )
+
+ # Assign the content of the input argument to a variable
+ trajectory_type = arg_parser.parse_args().trajectory
+ blf.log().info("{} Trajectory type: {}".format(logPrefix, trajectory_type))
+
+ # Check if running with Gazebo
+ isGazebo = False
+ if "gazebo" in (os.environ.get("YARP_ROBOT_NAME")).lower():
+ isGazebo = True
+
+ # Load parameters file
+ param_handler = blf.parameters_handler.YarpParametersHandler()
+
+ param_file = "blf-motor-current-tracking-options.ini"
+
+ if not param_handler.set_from_filename(param_file):
+ raise RuntimeError("{} Unable to load the parameters".format(logPrefix))
+
+ # Load the trajectory parameters and create the trajectory generator
+ if trajectory_type == "sinusoid":
+ trajectory_generator = SinusoidTrajectoryGenerator.from_parameter_handler(
+ param_handler.get_group("SINUSOID")
+ )
+ elif trajectory_type == "ramp":
+ trajectory_generator = RampTrajectoryGenerator.from_parameter_handler(
+ param_handler.get_group("RAMP")
+ )
+
+ # Load the motor parameters
+ MotorParameters.from_parameter_handler(param_handler.get_group("MOTOR"))
+
+ # Load joints to control and build the control board driver
+ robot_control_handler = param_handler.get_group("ROBOT_CONTROL")
+ joints_to_control = robot_control_handler.get_parameter_vector_string("joints_list")
+ blf.log().info("{} Joints to control: {}".format(logPrefix, joints_to_control))
+ # check if the joints are in the list of available joints
+ for joint in joints_to_control:
+ if joint not in MotorParameters.max_safety_current.keys():
+ raise RuntimeError(
+ "{} The joint {} is not supported by the application".format(
+ logPrefix, joint
+ )
+ )
+ # check for which joints the motor current measure should be bypassed, when generating the reference trajectory
+ bypass_motor_current_measure = param_handler.get_parameter_vector_bool(
+ "bypass_motor_current_measure"
+ )
+ if len(bypass_motor_current_measure) != len(joints_to_control):
+ raise ValueError(
+ "{} The number of joints must be equal to the size of the bypass_motor_current_measure parameter".format(
+ logPrefix
+ )
+ )
+
+ trajectory_generator.set_joint_list(joints_to_control)
+
+ poly_drivers = dict()
+
+ poly_drivers["REMOTE_CONTROL_BOARD"] = build_remote_control_board_driver(
+ param_handler=robot_control_handler,
+ local_prefix="motor_current_tracking",
+ )
+ if not poly_drivers["REMOTE_CONTROL_BOARD"].is_valid():
+ raise RuntimeError(
+ "{} Unable to create the remote control board driver".format(logPrefix)
+ )
+
+ blf.log().info(
+ "{} Sleep for two seconds. Just to be sure the interfaces are on.".format(
+ logPrefix
+ )
+ )
+ blf.clock().sleep_for(datetime.timedelta(seconds=2))
+
+ robot_control = blf.robot_interface.YarpRobotControl()
+ if not robot_control.initialize(robot_control_handler):
+ raise RuntimeError(
+ "{} Unable to initialize the robot control".format(logPrefix)
+ )
+ if not robot_control.set_driver(poly_drivers["REMOTE_CONTROL_BOARD"].poly):
+ raise RuntimeError(
+ "{} Unable to set the driver for the robot control".format(logPrefix)
+ )
+
+ # Get joint limits
+ safety_threshold = param_handler.get_parameter_float("safety_threshold")
+ safety_threshold = np.deg2rad(safety_threshold)
+ use_safety_threshold_for_starting_position = param_handler.get_parameter_bool(
+ "use_safety_threshold_for_starting_position"
+ )
+ _, lower_limits, upper_limits = robot_control.get_joint_limits()
+ lower_limits = np.deg2rad(lower_limits)
+ upper_limits = np.deg2rad(upper_limits)
+
+ # Modify joint limits to reduce range of motion
+ for idx, joint in enumerate(joints_to_control):
+ if joint == "l_hip_roll" or joint == "r_hip_roll":
+ upper_limits[idx] = np.deg2rad(80)
+
+ # Create the sensor bridge
+ sensor_bridge = blf.robot_interface.YarpSensorBridge()
+ sensor_bridge_handler = param_handler.get_group("SENSOR_BRIDGE")
+ if not sensor_bridge.initialize(sensor_bridge_handler):
+ raise RuntimeError(
+ "{} Unable to initialize the sensor bridge".format(logPrefix)
+ )
+ if not sensor_bridge.set_drivers_list(list(poly_drivers.values())):
+ raise RuntimeError(
+ "{} Unable to set the drivers for the sensor bridge".format(logPrefix)
+ )
+ if not sensor_bridge.advance():
+ raise RuntimeError("{} Unable to advance the sensor bridge".format(logPrefix))
+
+ are_joints_ok, joint_positions, _ = sensor_bridge.get_joint_positions()
+ if not are_joints_ok:
+ raise RuntimeError("{} Unable to get the joint positions".format(logPrefix))
+
+ # Create the vectors collection server for logging
+ vectors_collection_server = blf.yarp_utilities.VectorsCollectionServer()
+ if not vectors_collection_server.initialize(
+ param_handler.get_group("DATA_LOGGING")
+ ):
+ raise RuntimeError(
+ "{} Unable to initialize the vectors collection server".format(logPrefix)
+ )
+
+ vectors_collection_server.populate_metadata(
+ "motors::desired::current",
+ joints_to_control,
+ )
+ vectors_collection_server.populate_metadata(
+ "motors::control_mode::current",
+ joints_to_control,
+ )
+ vectors_collection_server.finalize_metadata()
+ blf.clock().sleep_for(datetime.timedelta(milliseconds=200))
+
+ # Create the ctrl+c handler
+ ctrl_c_handler = create_ctrl_c_handler(
+ sensor_bridge=sensor_bridge, robot_control=robot_control
+ )
+ signal.signal(signal.SIGINT, ctrl_c_handler)
+
+ # Check if tqdm is installed
+ try:
+ from tqdm import tqdm
+
+ is_tqdm_installed = True
+ except ImportError:
+ blf.log().warn(
+ "{} tqdm is not installed, the progress bar will not be shown".format(
+ logPrefix
+ )
+ )
+ is_tqdm_installed = False
+ last_printed_progress = -1
+ # hijack tqdm with a dummy function
+ tqdm = lambda x: x
+
+ # Set the time step
+ dt = param_handler.get_parameter_datetime("dt")
+
+ # Get the starting position: the trajectory will be generated for each starting position
+ number_of_starting_points = param_handler.get_parameter_int(
+ "number_of_starting_points"
+ )
+ starting_positions = trajectory_generator.create_starting_points(
+ number_of_starting_points=number_of_starting_points,
+ number_of_joints=len(joints_to_control),
+ safety_threshold=safety_threshold
+ if use_safety_threshold_for_starting_position
+ else 0.0,
+ lower_limits=lower_limits,
+ upper_limits=upper_limits,
+ )
+ blf.log().info(
+ "{} Starting positions: \n {}".format(logPrefix, np.rad2deg(starting_positions))
+ )
+
+ # Start the data collection
+ blf.log().info(
+ "{} Waiting for your input, press ENTER to start the data collection".format(
+ logPrefix
+ )
+ )
+ input()
+ blf.log().info("{} Start".format(logPrefix))
+
+ opposite_direction = False
+
+ for counter, starting_position in enumerate(starting_positions):
+
+ # drive the joints to the starting position
+ if not robot_control.set_control_mode(
+ blf.robot_interface.YarpRobotControl.Position
+ ):
+ raise RuntimeError("{} Unable to set the control mode".format(logPrefix))
+ if not robot_control.set_references(
+ starting_position, blf.robot_interface.YarpRobotControl.Position
+ ):
+ raise RuntimeError("{} Unable to set the references".format(logPrefix))
+
+ # wait for the joints to reach the starting position
+ is_motion_done = False
+ while not is_motion_done:
+ is_ok, is_motion_done, _, _ = robot_control.check_motion_done()
+ if not is_ok:
+ raise RuntimeError(
+ "{} Unable to check if the motion is done".format(logPrefix)
+ )
+ blf.clock().sleep_for(datetime.timedelta(milliseconds=200))
+ blf.clock().sleep_for(datetime.timedelta(seconds=1))
+ blf.log().info(
+ "{} Joints moved to the starting position #{}, starting the trajectory".format(
+ logPrefix, counter + 1
+ )
+ )
+
+ # Generate the trajectory
+ if not sensor_bridge.advance():
+ raise RuntimeError(
+ "{} Unable to advance the sensor bridge".format(logPrefix)
+ )
+
+ if isGazebo:
+ are_joints_ok, joint_torques, _ = sensor_bridge.get_joint_torques()
+ motor_currents = joint_torques * [
+ MotorParameters.k_tau[joint] for joint in joints_to_control
+ ]
+ if not are_joints_ok:
+ raise RuntimeError(
+ "{} Unable to get the joint torques".format(logPrefix)
+ )
+ else:
+ are_joints_ok, motor_currents, _ = sensor_bridge.get_motor_currents()
+ if not are_joints_ok:
+ raise RuntimeError(
+ "{} Unable to get the motor current".format(logPrefix)
+ )
+
+ trajectories = []
+ opposite_direction = not opposite_direction
+
+ for joint_index in range(len(joints_to_control)):
+
+ if trajectory_type == "ramp":
+
+ if counter % (2 * number_of_starting_points) == 0:
+ trajectory_generator.reset_velocity(joint_index=joint_index)
+
+ trajectories.append(
+ trajectory_generator.generate(
+ dt=dt.total_seconds(),
+ initial_current=motor_currents[joint_index]
+ if not (bypass_motor_current_measure[joint_index])
+ else 0,
+ joint_index=joint_index,
+ opposite_direction=opposite_direction,
+ )
+ )
+
+ if not (counter % 2 == 0):
+ trajectory_generator.increment_velocity(joint_index=joint_index)
+
+ else:
+ trajectories.append(
+ trajectory_generator.generate(
+ dt=dt.total_seconds(),
+ initial_current=motor_currents[joint_index]
+ if not (bypass_motor_current_measure[joint_index])
+ else 0,
+ joint_index=joint_index,
+ opposite_direction=False,
+ )
+ )
+
+ # reset control modes for current/torque
+ control_modes = [
+ blf.robot_interface.YarpRobotControl.Torque
+ if isGazebo
+ else blf.robot_interface.YarpRobotControl.Current
+ for _ in joints_to_control
+ ]
+
+ # update motor control modes
+ if not robot_control.set_control_mode(control_modes):
+ raise RuntimeError("{} Unable to set the control mode".format(logPrefix))
+
+ # reset variables
+ is_out_of_safety_limits = [False for _ in joints_to_control]
+ position_reference = starting_position
+ traj_index = 0
+ delta_traj_index = 1
+
+ # loop over the trajectory
+ trajectories_length = np.array([len(trajectory) for trajectory in trajectories])
+ for _ in tqdm(range(trajectories_length.max())):
+ tic = blf.clock().now()
+
+ # get the feedback
+ if not sensor_bridge.advance():
+ raise RuntimeError(
+ "{} Unable to advance the sensor bridge".format(logPrefix)
+ )
+
+ are_joints_ok, joint_positions, _ = sensor_bridge.get_joint_positions()
+ if not are_joints_ok:
+ raise RuntimeError(
+ "{} Unable to get the joint positions".format(logPrefix)
+ )
+
+ # check if the joints are within the safety limits
+ # if not, stop the trajectory and set the related control mode to position
+ for joint_index, joint in enumerate(joints_to_control):
+ if is_out_of_safety_limits[joint_index]:
+ # nothing to do, it was already notified before
+ continue
+ if (
+ np.abs(joint_positions[joint_index] - lower_limits[joint_index])
+ < safety_threshold
+ ) or (
+ np.abs(joint_positions[joint_index] - upper_limits[joint_index])
+ < safety_threshold
+ ):
+ # set the control mode to position
+ control_modes[
+ joint_index
+ ] = blf.robot_interface.YarpRobotControl.Position
+
+ # set the reference to the current position
+ position_reference[joint_index] = joint_positions[joint_index]
+
+ # update flag
+ is_out_of_safety_limits[joint_index] = True
+
+ # set control modes
+ if not robot_control.set_control_mode(control_modes):
+ raise RuntimeError(
+ "{} Unable to set the control mode".format(logPrefix)
+ )
+
+ blf.log().warn(
+ "{} Joint {} is out of the safety limits, stopping its trajectory and switching to Position control with reference position {}".format(
+ logPrefix, joint, position_reference[joint_index]
+ )
+ )
+
+ # get the current/torque references
+ current_reference = []
+ for joint_idx, trajectory in enumerate(trajectories):
+ # check if the trajectory is over
+ if traj_index < len(trajectory):
+ if is_out_of_safety_limits[joint_idx]:
+ current_reference.append(0)
+ else:
+ if isGazebo:
+ current_reference.append(
+ trajectory[traj_index]
+ / MotorParameters.k_tau[joints_to_control[joint_idx]]
+ )
+ else:
+ # check if the current is within the safety limits
+ if (
+ np.abs(trajectory[traj_index])
+ > MotorParameters.max_safety_current[
+ joints_to_control[joint_idx]
+ ]
+ ):
+ RuntimeError(
+ "{} The current reference for joint {} is exceeding the safety limits, exiting the application".format(
+ logPrefix, joints_to_control[joint_idx]
+ )
+ )
+ current_reference.append(trajectory[traj_index])
+ else:
+ # if the trajectory is over, switch to position control with
+ # the the posiition reference as the last measured position
+ current_reference.append(0)
+ position_reference[joint_idx] = joint_positions[joint_idx]
+ is_out_of_safety_limits[joint_idx] = True
+
+ # merge the position and current/torque references depending on the control mode
+ reference = np.where(
+ is_out_of_safety_limits, position_reference, np.array(current_reference)
+ )
+
+ # send the references motor current (or joint torque for Gazebo)
+ if not robot_control.set_references(
+ reference, control_modes, joint_positions
+ ):
+ raise RuntimeError("{} Unable to set the references".format(logPrefix))
+
+ # check if it is time to move to next starting position
+ if all(is_out_of_safety_limits):
+ blf.log().info(
+ "{} The trajectory is stopped due to all joints exceeding safety limits. Moving to next starting position, if available.".format(
+ logPrefix
+ )
+ )
+ break
+
+ # log the data
+ vectors_collection_server.prepare_data()
+ if not vectors_collection_server.clear_data():
+ raise RuntimeError("{} Unable to clear the data".format(logPrefix))
+ vectors_collection_server.populate_data(
+ "motors::desired::current", current_reference
+ )
+ vectors_collection_server.populate_data(
+ "motors::control_mode::current", np.where(is_out_of_safety_limits, 0, 1)
+ )
+ vectors_collection_server.send_data()
+
+ # sleep
+ toc = blf.clock().now()
+ delta_time = toc - tic
+ if delta_time < dt:
+ blf.clock().sleep_for(dt - delta_time)
+ else:
+ blf.log().debug(
+ "{} The control loop is too slow, real time constraints not satisfied".format(
+ logPrefix
+ )
+ )
+
+ # Print progress percentage, only if tqdm is not installed
+ if not is_tqdm_installed:
+
+ # Calculate progress
+ progress = (traj_index + 1) / trajectories_length.max() * 100
+
+ # Check if progress is a multiple of 10 and different from last printed progress
+ if int(progress) % 10 == 0 and int(progress) != last_printed_progress:
+ print(f"Progress: {int(progress)}%", end="\r")
+ last_printed_progress = int(progress)
+
+ # update the trajectory index
+ traj_index = traj_index + delta_traj_index
+
+ blf.log().info("{} Data colection completed".format(logPrefix))
+
+ # get the feedback
+ if not sensor_bridge.advance():
+ raise RuntimeError("{} Unable to advance the sensor bridge".format(logPrefix))
+
+ are_joints_ok, joint_positions, _ = sensor_bridge.get_joint_positions()
+
+ if not are_joints_ok:
+ raise RuntimeError("{} Unable to get the joint positions".format(logPrefix))
+
+ # set the control mode to position
+ if not robot_control.set_control_mode(
+ blf.robot_interface.YarpRobotControl.Position
+ ):
+ raise RuntimeError("{} Unable to set the control mode".format(logPrefix))
+ if not robot_control.set_references(
+ joint_positions, blf.robot_interface.YarpRobotControl.Position
+ ):
+ raise RuntimeError("{} Unable to set the references".format(logPrefix))
+
+
+if __name__ == "__main__":
+ network = yarp.Network()
+ main()
diff --git a/utilities/motor-current-tracking/config/robots/ergoCubGazeboV1_1/blf-motor-current-tracking-options.ini b/utilities/motor-current-tracking/config/robots/ergoCubGazeboV1_1/blf-motor-current-tracking-options.ini
new file mode 100644
index 0000000000..6ca9cf22bf
--- /dev/null
+++ b/utilities/motor-current-tracking/config/robots/ergoCubGazeboV1_1/blf-motor-current-tracking-options.ini
@@ -0,0 +1,37 @@
+dt 0.005
+safety_threshold 5.0
+use_safety_threshold_for_starting_position false
+number_of_starting_points 3
+bypass_motor_current_measure (true, true)
+
+[SINUSOID]
+min_delta_current (0.05, 0.05)
+max_delta_current (0.3, 0.3)
+delta_current_increment (0.05, 0.05)
+min_frequency (0.3, 0.3)
+max_frequency (0.5, 0.5)
+frequency_decrement (0.1, 0.1)
+
+[RAMP]
+initial_speed (0.5, 0.5)
+final_speed (2.0, 5.0)
+speed_increment (0.5, 2.0)
+max_current_increment (5.0, 5.0)
+
+[MOTOR]
+joints_list ("l_hip_roll", "l_hip_pitch", "l_hip_yaw",
+ "l_knee", "l_ankle_pitch", "l_ankle_roll",
+ "r_hip_roll", "r_hip_pitch", "r_hip_yaw",
+ "r_knee", "r_ankle_pitch", "r_ankle_roll")
+k_tau (0.094, 0.064, 0.150,
+ 0.064, 0.064, 0.177,
+ 0.094, 0.064, 0.150,
+ 0.064, 0.064, 0.177)
+max_safety_current (8.0, 8.0, 4.0,
+ 6.0, 3.5, 3.0,
+ 8.0, 8.0, 4.0,
+ 6.0, 3.5, 3.0)
+
+[include ROBOT_CONTROL "./blf_motor_current_tracking/robot_control.ini"]
+[include SENSOR_BRIDGE "./blf_motor_current_tracking/sensor_bridge.ini"]
+[include DATA_LOGGING "./blf_motor_current_tracking/data_logging.ini"]
diff --git a/utilities/motor-current-tracking/config/robots/ergoCubGazeboV1_1/blf_motor_current_tracking/data_logging.ini b/utilities/motor-current-tracking/config/robots/ergoCubGazeboV1_1/blf_motor_current_tracking/data_logging.ini
new file mode 100644
index 0000000000..46c33471d6
--- /dev/null
+++ b/utilities/motor-current-tracking/config/robots/ergoCubGazeboV1_1/blf_motor_current_tracking/data_logging.ini
@@ -0,0 +1 @@
+remote "/motor_current_tracking/logger"
diff --git a/utilities/motor-current-tracking/config/robots/ergoCubGazeboV1_1/blf_motor_current_tracking/robot_control.ini b/utilities/motor-current-tracking/config/robots/ergoCubGazeboV1_1/blf_motor_current_tracking/robot_control.ini
new file mode 100644
index 0000000000..a0939b623f
--- /dev/null
+++ b/utilities/motor-current-tracking/config/robots/ergoCubGazeboV1_1/blf_motor_current_tracking/robot_control.ini
@@ -0,0 +1,8 @@
+robot_name ergocubSim
+
+joints_list ("r_ankle_roll", "l_ankle_roll")
+
+remote_control_boards ("left_leg","right_leg")
+positioning_duration 3.0 #in seconds
+positioning_tolerance 0.05 #in radians
+position_direct_max_admissible_error 0.1 #in radians
diff --git a/utilities/motor-current-tracking/config/robots/ergoCubGazeboV1_1/blf_motor_current_tracking/sensor_bridge.ini b/utilities/motor-current-tracking/config/robots/ergoCubGazeboV1_1/blf_motor_current_tracking/sensor_bridge.ini
new file mode 100644
index 0000000000..de834500ed
--- /dev/null
+++ b/utilities/motor-current-tracking/config/robots/ergoCubGazeboV1_1/blf_motor_current_tracking/sensor_bridge.ini
@@ -0,0 +1,4 @@
+check_for_nan false
+stream_joint_states true
+stream_joint_accelerations false
+stream_motor_states false
diff --git a/utilities/motor-current-tracking/config/robots/ergoCubSN001/blf-motor-current-tracking-options.ini b/utilities/motor-current-tracking/config/robots/ergoCubSN001/blf-motor-current-tracking-options.ini
new file mode 100644
index 0000000000..7783bb9d40
--- /dev/null
+++ b/utilities/motor-current-tracking/config/robots/ergoCubSN001/blf-motor-current-tracking-options.ini
@@ -0,0 +1,37 @@
+dt 0.002
+safety_threshold 8.0
+use_safety_threshold_for_starting_position false
+number_of_starting_points 5
+bypass_motor_current_measure (true)
+
+[SINUSOID]
+min_delta_current ( 0.7 )
+max_delta_current ( 1.8 )
+delta_current_increment ( 0.2 )
+min_frequency ( 0.05)
+max_frequency ( 0.45)
+frequency_decrement ( 0.1 )
+
+[RAMP]
+initial_speed (0.15)
+final_speed (0.40)
+speed_increment (0.08)
+max_current_increment (2.00)
+
+[MOTOR]
+joints_list ("l_hip_roll", "l_hip_pitch", "l_hip_yaw",
+ "l_knee", "l_ankle_pitch", "l_ankle_roll",
+ "r_hip_roll", "r_hip_pitch", "r_hip_yaw",
+ "r_knee", "r_ankle_pitch", "r_ankle_roll")
+k_tau (0.094, 0.064, 0.150,
+ 0.064, 0.064, 0.177,
+ 0.094, 0.064, 0.150,
+ 0.064, 0.064, 0.177)
+max_safety_current (8.0, 8.0, 4.0,
+ 6.0, 3.5, 3.0,
+ 8.0, 8.0, 4.0,
+ 6.0, 3.5, 3.0)
+
+[include ROBOT_CONTROL "./blf_motor_current_tracking/robot_control.ini"]
+[include SENSOR_BRIDGE "./blf_motor_current_tracking/sensor_bridge.ini"]
+[include DATA_LOGGING "./blf_motor_current_tracking/data_logging.ini"]
diff --git a/utilities/motor-current-tracking/config/robots/ergoCubSN001/blf_motor_current_tracking/data_logging.ini b/utilities/motor-current-tracking/config/robots/ergoCubSN001/blf_motor_current_tracking/data_logging.ini
new file mode 100644
index 0000000000..46c33471d6
--- /dev/null
+++ b/utilities/motor-current-tracking/config/robots/ergoCubSN001/blf_motor_current_tracking/data_logging.ini
@@ -0,0 +1 @@
+remote "/motor_current_tracking/logger"
diff --git a/utilities/motor-current-tracking/config/robots/ergoCubSN001/blf_motor_current_tracking/robot_control.ini b/utilities/motor-current-tracking/config/robots/ergoCubSN001/blf_motor_current_tracking/robot_control.ini
new file mode 100644
index 0000000000..a3970ab59c
--- /dev/null
+++ b/utilities/motor-current-tracking/config/robots/ergoCubSN001/blf_motor_current_tracking/robot_control.ini
@@ -0,0 +1,8 @@
+robot_name ergocub
+
+joints_list ("r_ankle_pitch")
+
+remote_control_boards ("right_leg")
+positioning_duration 3.0 #in seconds
+positioning_tolerance 0.05 #in radians
+position_direct_max_admissible_error 0.1 #in radians
diff --git a/utilities/motor-current-tracking/config/robots/ergoCubSN001/blf_motor_current_tracking/sensor_bridge.ini b/utilities/motor-current-tracking/config/robots/ergoCubSN001/blf_motor_current_tracking/sensor_bridge.ini
new file mode 100644
index 0000000000..a5a9d9dcb3
--- /dev/null
+++ b/utilities/motor-current-tracking/config/robots/ergoCubSN001/blf_motor_current_tracking/sensor_bridge.ini
@@ -0,0 +1,3 @@
+check_for_nan false
+stream_joint_states true
+stream_motor_states true