Skip to content

Commit

Permalink
add motor parameters to config file
Browse files Browse the repository at this point in the history
  • Loading branch information
@LoreMoretti
LoreMoretti committed Sep 27, 2024
1 parent becc9dc commit 29255ac
Show file tree
Hide file tree
Showing 4 changed files with 100 additions and 36 deletions.
15 changes: 15 additions & 0 deletions utilities/motor-current-tracking/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@ In this file, the following parameters are defined:
4. the number of starting positions `number_of_starting_points` from which the same reference trajectory is repeated.
5. the `SINUSOID` parameters group, which defines the sinusoid trajectory as detailed later
6. the `RAMP` parameters group, which defines the ramp trajectory as detailed later
7. 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.

Expand Down Expand Up @@ -118,6 +119,20 @@ 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")
Expand Down
93 changes: 57 additions & 36 deletions utilities/motor-current-tracking/blf-motor-current-tracking.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,35 +25,35 @@

class MotorParameters(ABC):
# k_tau[A/Nm] includes the gear ratio
k_tau = {
"l_hip_roll": 94 * 1e-3,
"l_hip_pitch": 64 * 1e-3,
"l_hip_yaw": 150 * 1e-3,
"l_knee": 64 * 1e-3,
"l_ankle_pitch": 64 * 1e-3,
"l_ankle_roll": 177 * 1e-3,
"r_hip_roll": 94 * 1e-3,
"r_hip_pitch": 64 * 1e-3,
"r_hip_yaw": 150 * 1e-3,
"r_knee": 64 * 1e-3,
"r_ankle_pitch": 64 * 1e-3,
"r_ankle_roll": 177 * 1e-3,
}
k_tau = dict()
# max_safety_current[A] limits the motor current to avoid damages
max_safety_current = {
"l_hip_roll": 8.0,
"l_hip_pitch": 8.0,
"l_hip_yaw": 4.0,
"l_knee": 6.0,
"l_ankle_pitch": 3.5,
"l_ankle_roll": 3.0,
"r_hip_roll": 8.0,
"r_hip_pitch": 8.0,
"r_hip_yaw": 4.0,
"r_knee": 6.0,
"r_ankle_pitch": 3.5,
"r_ankle_roll": 3.0,
}
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):
Expand Down Expand Up @@ -122,12 +122,18 @@ def generate(

# Check if joint list is set
if not self.joint_list:
raise ValueError("Joint list must be set before generating the trajectory")
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"
"{} Joint index must be specified when more than one joint is controlled".format(
logPrefix
)
)

# Set default joint index, if not specified
Expand Down Expand Up @@ -168,7 +174,9 @@ def create_starting_points(
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"
"{} 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))
Expand Down Expand Up @@ -203,7 +211,9 @@ def from_parameter_handler(
param_handler: ParamHandler,
) -> "RampTrajectoryGenerator":

max_current_increment = param_handler.get_parameter_vector_float("max_current_increment")
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")
Expand Down Expand Up @@ -246,12 +256,18 @@ def generate(

# Check if joint list is set
if not self.joint_list:
raise ValueError("Joint list must be set before generating the trajectory")
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"
"{} Joint index must be specified when more than one joint is controlled".format(
logPrefix
)
)

# Set default joint index, if not specified
Expand Down Expand Up @@ -295,7 +311,9 @@ def create_starting_points(
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"
"{} 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))
Expand Down Expand Up @@ -407,11 +425,14 @@ def main():
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 supported joints
# 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(
Expand Down
14 changes: 14 additions & 0 deletions ...r-current-tracking/config/robots/ergoCubGazeboV1_1/blf-motor-current-tracking-options.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,20 @@ 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"]
14 changes: 14 additions & 0 deletions .../motor-current-tracking/config/robots/ergoCubSN001/blf-motor-current-tracking-options.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,20 @@ 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"]

0 comments on commit 29255ac

Please sign in to comment.