def get_slope_segments(request, result_base_folder, dcm, mem):
"""
Return a dictionary with several objects for
each X coordinate of all horizontal segments
"""
dico = {}
dico["Left_Inclinaison"] = Laser(
dcm, mem, "Front/Vertical/Left/Slope/Inclination/Sensor")
dico["Left_Distance"] = Laser(
dcm, mem, "Front/Vertical/Left/Slope/X/Sensor")
dico["Right_Inclinaison"] = Laser(
dcm, mem, "Front/Vertical/Right/Slope/Inclination/Sensor")
dico["Right_Distance"] = Laser(
dcm, mem, "Front/Vertical/Right/Slope/X/Sensor")
logger = qha_tools.Logger()
dico["logger"] = logger
def fin():
"""Method executed after a joint test."""
result_file_path = "/".join(
[
result_base_folder,
"Vertical_Test"
]) + ".csv"
logger.log_file_write(result_file_path)
request.addfinalizer(fin)
return dico
def test_objects_dico(request, result_base_folder, dcm, mem):
"""
Create the appropriate objects for each joint.
It logs the informations into a dictionnary and save the data into a file
after each joint test.
"""
joint_position_actuator = subdevice.JointPositionActuator(
dcm, mem, request.param)
joint_position_sensor = subdevice.JointPositionSensor(
dcm, mem, request.param)
joint_test_limit = qha_tools.read_parameter("joint_enslavement.cfg",
"JointsLimits", request.param)
logger = qha_tools.Logger()
def fin():
"""Method executed after a joint test."""
result_file_path = "/".join([
result_base_folder, joint_position_actuator.subdevice_type,
"_".join([joint_position_actuator.short_name,
str(logger.flag)])
]) + ".csv"
logger.log_file_write(result_file_path)
request.addfinalizer(fin)
# creating a dictionnary with all the objects
dico_object = {
"jointActuator": joint_position_actuator,
"jointSensor": joint_position_sensor,
"logger": logger,
"joint_test_limit": float(joint_test_limit)
}
return dico_object
def test_wheels_dico(request, result_base_folder, dcm, mem):
"""
Create the appropriate objects for each joint.
It logs the informations into a dictionnary and save the data into a file
after each wheel test.
"""
wheel_speed_actuator = subdevice.WheelSpeedActuator(
dcm, mem, request.param)
wheel_speed_sensor = subdevice.WheelSpeedSensor(dcm, mem, request.param)
logger = qha_tools.Logger()
def fin():
"""Method executed after a joint test."""
result_file_path = "/".join([
result_base_folder, wheel_speed_actuator.subdevice_type, "_".join(
[wheel_speed_actuator.short_name,
str(logger.flag)])
]) + ".csv"
logger.log_file_write(result_file_path)
request.addfinalizer(fin)
# creating a dictionnary with all the objects
dico_object = {
"wheelActuator": wheel_speed_actuator,
"wheelSensor": wheel_speed_sensor,
"logger": logger
}
return dico_object
def test_leg_dico(request, result_base_folder, dcm, mem):
"""
Create the appropriate objects for each joint.
It logs the informations into a dictionnary and save the data into a file
after each joint test.
"""
hippitch_position_actuator = subdevice.JointPositionActuator(
dcm, mem, "HipPitch")
hippitch_position_sensor = subdevice.JointPositionSensor(
dcm, mem, "HipPitch")
kneepitch_position_actuator = subdevice.JointPositionActuator(
dcm, mem, "KneePitch")
kneepitch_position_sensor = subdevice.JointPositionSensor(
dcm, mem, "KneePitch")
logger = qha_tools.Logger()
def fin():
"""Method executed after a joint test."""
result_file_path = "/".join([
result_base_folder, hippitch_position_actuator.subdevice_type,
"_".join(["Leg", str(logger.flag)])
]) + ".csv"
logger.log_file_write(result_file_path)
request.addfinalizer(fin)
# creating a dictionnary with all the objects
dico_object = {
"hipActuator": hippitch_position_actuator,
"hipSensor": hippitch_position_sensor,
"kneeActuator": kneepitch_position_actuator,
"kneeSensor": kneepitch_position_sensor,
"logger": logger
}
return dico_object
def get_sensor_objects(request, result_base_folder, dcm, mem):
"""
Return a dictionary with several objects for
each X coordinate of all lasers segments
"""
h_sides = ["Front", "Left", "Right"]
v_sides = ["Left", "Right"]
s_sides = ["Front", "Back"]
bag = qha_tools.Bag(mem)
dico = {}
for each in h_sides:
for i in range(1, 10):
bag.add_object(
"Horizontal_X_seg" + str(i) + "_" + each,
Laser(dcm, mem,
each + "/Horizontal/Seg0" + str(i) + "/X/Sensor"))
for i in range(10, 16):
bag.add_object(
"Horizontal_X_seg" + str(i) + "_" + each,
Laser(dcm, mem,
each + "/Horizontal/Seg" + str(i) + "/X/Sensor"))
for each in v_sides:
bag.add_object(
"Vertical_X_seg01_" + each,
Laser(dcm, mem, "Front/Vertical/" + each + "/Seg01/X/Sensor"))
for each in s_sides:
bag.add_object("Sonar_" + each, Sonar(dcm, mem, each))
for i in range(1, 4):
bag.add_object(
"Shovel_X_seg" + str(i),
Laser(dcm, mem, "Front/Shovel/Seg0" + str(i) + "/X/Sensor"))
logger_dist = qha_tools.Logger()
logger_error = qha_tools.Logger()
dico["logger_dist"] = logger_dist
dico["logger_error"] = logger_error
dico["bag"] = bag
def fin():
"""Method executed after a joint test."""
result_file_path1 = "/".join(
[result_base_folder, "Dance_test_distances"]) + ".csv"
logger_dist.log_file_write(result_file_path1)
request.addfinalizer(fin)
return dico
def sensor_logger(request, result_base_folder, dcm, mem):
"""
Return a dictionary with several objects for
each X coordinate of all lasers segments
"""
thread_flag = threading.Event()
h_sides = ["Front", "Left", "Right"]
v_sides = ["Left", "Right"]
s_sides = ["Front", "Back"]
bag = qha_tools.Bag(mem)
dico = {}
for each in h_sides:
for i in range(1, 10):
bag.add_object("Horizontal_X_seg" + str(i) + "_" + each, Laser(
dcm, mem, each + "/Horizontal/Seg0" + str(i) + "/X/Sensor"))
for i in range(10, 16):
bag.add_object("Horizontal_X_seg" + str(i) + "_" + each, Laser(
dcm, mem, each + "/Horizontal/Seg" + str(i) + "/X/Sensor"))
for each in v_sides:
bag.add_object("Vertical_X_seg01_" + each, Laser(
dcm, mem, "Front/Vertical/" + each + "/Seg01/X/Sensor"))
for each in s_sides:
bag.add_object("Sonar_" + each, Sonar(
dcm, mem, each))
for i in range(1, 4):
bag.add_object("Shovel_X_seg" + str(i), Laser(
dcm, mem, "Front/Shovel/Seg0" + str(i) + "/X/Sensor"))
logger = qha_tools.Logger()
def log(logger, bag):
"""
Docstring
"""
t_0 = time.time()
while not thread_flag.is_set():
sensor_value = bag.value
for each in sensor_value.keys():
logger.log((each, sensor_value[each]))
logger.log(("Time", time.time() - t_0))
time.sleep(0.01)
log_thread = threading.Thread(target=log, args=(logger, bag))
log_thread.start()
def fin():
"""Method executed after a joint test."""
thread_flag.set()
result_file_path1 = "/".join(
[
result_base_folder,
"Front_sensors_distances"
]) + ".csv"
logger.log_file_write(result_file_path1)
request.addfinalizer(fin)
return dico
def log_joints(request, result_base_folder, dcm, mem, motion):
"""
Return a dictionary with several objects for
POD test
"""
thread_flag = threading.Event()
bag = qha_tools.Bag(mem)
for each in motion.getBodyNames("Body"):
if "Wheel" in each:
bag.add_object(each + "_Actuator",
WheelSpeedActuator(dcm, mem, each))
bag.add_object(each + "_Sensor",
WheelSpeedSensor(dcm, mem, each))
else:
bag.add_object(each + "_Actuator",
JointPositionActuator(dcm, mem, each))
bag.add_object(each + "_Sensor",
JointPositionSensor(dcm, mem, each))
bag.add_object("BackBumper", Bumper(dcm, mem, "Back"))
bag.add_object(
"robot_on_charging_station", ChargingStationSensor(dcm, mem))
bag.add_object("battery_current", BatteryCurrentSensor(dcm, mem))
logger = qha_tools.Logger()
def log(logger, bag):
"""
Docstring
"""
t_0 = time.time()
while not thread_flag.is_set():
joints_value = bag.value
for each in joints_value.keys():
if "Wheel" in each or "Bumper" in each or \
"charging" in each or "battery" in each:
logger.log((each, joints_value[each]))
else:
logger.log((each, joints_value[each] * (180 / pi)))
logger.log(("Time", time.time() - t_0))
time.sleep(0.01)
log_thread = threading.Thread(target=log, args=(logger, bag))
log_thread.start()
def fin():
"""
Docstring
"""
thread_flag.set()
result_file_path = "/".join(
[
result_base_folder,
"Joint_Log"
]) + ".csv"
logger.log_file_write(result_file_path)
request.addfinalizer(fin)
def get_pod_objects(dcm, mem):
"""
Return a dictionary with several objects for
POD test
"""
dico = {}
dico["robot_on_charging_station"] = ChargingStationSensor(dcm, mem)
dico["backbumper"] = Bumper(dcm, mem, "Back")
dico["battery_current"] = BatteryCurrentSensor(dcm, mem)
logger = qha_tools.Logger()
dico["logger"] = logger
return dico
def logger(request, result_base_folder, period):
'''
Docstring
'''
logger_joints = qha_tools.Logger()
frequence = 1 / period[0]
def fin():
"""Method executed after a joint test."""
result_file_path = "/".join(
[result_base_folder,
str(frequence) + "_log_joints"]) + ".csv"
logger_joints.log_file_write(result_file_path)
request.addfinalizer(fin)
return logger_joints
def log_wheel(request, result_base_folder, dcm, mem):
"""
Return a dictionary with several objects for
POD test
"""
thread_flag = threading.Event()
bag = qha_tools.Bag(mem)
wheels = ["WheelFR", "WheelFL", "WheelB"]
for wheel in wheels:
bag.add_object(
wheel + "SpeedActuator", WheelSpeedActuator(dcm, mem, wheel))
bag.add_object(
wheel + "SpeedSensor", WheelSpeedSensor(dcm, mem, wheel))
bag.add_object(
wheel + "CurrentSensor", WheelCurrentSensor(dcm, mem, wheel))
logger = qha_tools.Logger()
def log(logger, bag):
"""
Docstring
"""
t_0 = time.time()
while not thread_flag.is_set():
wheel_value = bag.value
for each in wheel_value.keys():
logger.log((each, wheel_value[each]))
logger.log(("Time", time.time() - t_0))
time.sleep(0.01)
log_thread = threading.Thread(target=log, args=(logger, bag))
log_thread.start()
def fin():
"""
Docstring
"""
thread_flag.set()
result_file_path = "/".join(
[
result_base_folder,
"wheel_Log"
]) + ".csv"
logger.log_file_write(result_file_path)
request.addfinalizer(fin)
def get_horizontal_x_segments(request, result_base_folder, dcm, mem, side):
"""
Return a dictionary with several objects for
each X coordinate of all horizontal segments
"""
dico = {}
for i in range(1, 10):
dico["seg" + str(i)] = Laser(
dcm, mem, side + "/Horizontal/Seg0" + str(i) + "/X/Sensor")
for i in range(10, 16):
dico["seg" + str(i)] = Laser(
dcm, mem, side + "/Horizontal/Seg" + str(i) + "/X/Sensor")
logger = qha_tools.Logger()
dico["logger"] = logger
def fin():
"""Method executed after a joint test."""
result_file_path = "/".join([result_base_folder, side]) + ".csv"
logger.log_file_write(result_file_path)
request.addfinalizer(fin)
return dico
def test_fan_01(self, dcm, mem, plot, test_time, cycle_number,
result_base_folder, threshold):
"""
FanBoard basic specification check.
When fan actuator = 1 and fan speed is lower than the hald of its
nominal speed, fan status must be set to 1.
"""
# flags initialization
fan_state = True
# plot_server initialisation
if plot:
plot_server = socket_connection.Server()
# objects initialization
fan_hardness_actuator = subdevice.FanHardnessActuator(dcm, mem)
right_fan_frequency = subdevice.FanFrequencySensor(dcm, mem, "Right")
middle_fan_frequency = subdevice.FanFrequencySensor(dcm, mem, "Mid")
left_fan_frequency = subdevice.FanFrequencySensor(dcm, mem, "Left")
fan = subdevice.FanStatus(dcm, mem)
# creating fan cycling behavior thread
fan_behavior = Thread(target=fan_utils.fan_cycle,
args=(dcm, mem, cycle_number))
# starting behavior
fan_behavior.start()
# logger and timer objects creation
logger = qha_tools.Logger()
timer = qha_tools.Timer(dcm, 1000)
# test loop
while fan_behavior.isAlive():
fan_hardness_value = fan_hardness_actuator.value
right_fan_frequency_value = right_fan_frequency.value
middle_fan_frequency_value = middle_fan_frequency.value
left_fan_frequency_value = left_fan_frequency.value
fan_status = fan.status
dcm_time = timer.dcm_time() / 1000.
# logging informations
logger.log(
("Time", dcm_time),
(fan_hardness_actuator.header_name, fan_hardness_value),
(right_fan_frequency.header_name, right_fan_frequency_value),
(middle_fan_frequency.header_name, middle_fan_frequency_value),
(left_fan_frequency.header_name, left_fan_frequency_value),
(fan.header_name, fan_status))
# for real time graphs
if plot:
plot_server.add_point(fan_hardness_actuator.header_name,
dcm_time, fan_hardness_value)
plot_server.add_point(right_fan_frequency.header_name,
dcm_time, right_fan_frequency_value)
plot_server.add_point(middle_fan_frequency.header_name,
dcm_time, middle_fan_frequency_value)
plot_server.add_point(left_fan_frequency.header_name, dcm_time,
left_fan_frequency_value)
plot_server.add_point(fan.header_name, dcm_time, fan_status)
# checking that fan status works well
if fan_hardness_value == 1:
if right_fan_frequency_value < threshold["RightFan"] and \
fan_status == 0:
fan_state = False
if left_fan_frequency_value < threshold["LeftFan"] and \
fan_status == 0:
fan_state = False
if middle_fan_frequency_value < threshold["MiddleFan"] and \
fan_status == 0:
fan_state = False
file_name = "_".join(["Fan", str(fan_state)])
result_file_path = "/".join([result_base_folder, file_name]) + ".csv"
logger.log_file_write(result_file_path)
assert fan_state
def test_wheel_temperature_protection(self, dcm, parameters, wheel,
result_base_folder):
# logger initialization
log = logging.getLogger('MOTOR_LIMITATION_PERF_HW_002_Wheels')
# flags initialization
flag_wheel = True
flag_loop = True
flag_max_current_exceeded = False
flag_max_temperature_exceeded = False
flag_info = False
flag_info2 = False
flag_info3 = False
flag_info4 = False
wheel.stiffness.qvalue = (1.0, 5000)
wheel.speed.actuator.qvalue = (wheel.speed.actuator.maximum, 10000)
test_params = parameters
timer = qha_tools.Timer(dcm, test_params["test_time"])
slav = qha_tools.SlidingAverage(test_params["sa_nb_points"])
logger = qha_tools.Logger()
log.info("")
log.info("*************************************")
log.info("Testing : " + str(wheel.short_name))
log.info("*************************************")
log.info("")
wheel_temperature_max = wheel.temperature.maximum
wheel_temperature_min = wheel.temperature.minimum
wheel_temperature_mid = \
(wheel_temperature_max + wheel_temperature_min) * 0.5
wheel_current_max = wheel.current.maximum
current_limit_high = wheel_current_max * \
(1.0 + test_params["limit_factor_sup"])
current_limit_low = wheel_current_max * \
(1.0 - test_params["limit_factor_inf"])
while timer.is_time_not_out and flag_loop is True:
try:
wheel_temperature = wheel.temperature.value
wheel_current = wheel.current.value
slav.point_add(wheel_current)
wheel_current_sa = slav.calc()
wheel_speed_command = wheel.speed.actuator.value
wheel_speed_sensor = wheel.speed.sensor.value
wheel_stiffness = wheel.stiffness.value
dcm_time = timer.dcm_time() / 1000.
logger.log(
("Time", dcm_time),
("Stiffness", wheel_stiffness),
("Current", wheel_current),
("CurrentSA", wheel_current_sa),
("MaxAllowedCurrent", wheel_current_max),
("CurrentLimitHigh", current_limit_high),
("CurrentLimitLow", current_limit_low),
("Temperature", wheel_temperature),
("TemperatureMin", wheel_temperature_min),
("TemperatureMax", wheel_temperature_max),
("SpeedActuator", wheel_speed_command),
("SpeedSensor", wheel_speed_sensor),
)
# out of test loop if temperature is higher than MAX + 1 degree
if wheel_temperature >= wheel_temperature_max + 5:
flag_loop = False
# if joint temperature higher than a limit value,
# joint current must be null after 100ms.
if flag_max_temperature_exceeded is False and \
wheel_temperature >= wheel_temperature_max:
flag_max_temperature_exceeded = True
timer_max = qha_tools.Timer(dcm, 1000)
log.info("max temperature exceeded a first time")
if flag_max_temperature_exceeded and \
wheel_temperature <= wheel_temperature_mid:
flag_loop = False
log.info("Motor windings are cold enough")
# setting flag to True if 90 percent of max current is exceeded
if wheel_current_sa > 0.9 * wheel_current_max and not \
flag_max_current_exceeded and dcm_time > 0.4:
flag_max_current_exceeded = True
log.info("90 percent of max allowed currend reached")
# averaged current has not to exceed limit high
if wheel_current_sa > current_limit_high and not flag_info:
flag_wheel = False
flag_info = True
log.warning("current high limit exceeded")
# once max current is exceeded, current hasn't to be lower than
# limit low
if flag_max_current_exceeded and \
wheel_current_sa < current_limit_low and not \
flag_max_temperature_exceeded and not flag_info2:
flag_wheel = False
flag_info2 = True
log.info("current has been lower than low limit")
if flag_max_temperature_exceeded and\
timer_max.is_time_out() and wheel_current != 0 and not\
flag_info3:
flag_wheel = False
flag_info3 = True
log.critical("max temperature exceeded and current "+\
"is not null")
if flag_max_temperature_exceeded and wheel_current == 0.0 and\
not flag_info4:
flag_info4 = True
log.info("current null reached")
except KeyboardInterrupt:
flag_loop = False
log.info("KeyboardInterrupt from user")
wheel.speed.actuator.qvalue = (0.0, 1000)
wheel.stiffness.qqvalue = 0.0
# stop and unstiff wheel
wheel.speed.actuator.qvalue = (0.0, 1000)
wheel.stiffness.qqvalue = 0.0
# writing logger results into a csv file
result_file_path = "/".join([
result_base_folder, wheel.temperature.subdevice_type,
wheel.short_name + "_" + str(flag_wheel)
]) + ".csv"
logger.log_file_write(result_file_path)
assert flag_wheel
def test_fuseboard_temperature(self, dcm, mem, fuse, wheel_objects,
multi_fuseboard_ambiant_tmp,
multi_fuseboard_total_current, test_time,
joint_limit_extension, result_base_folder,
plot, plot_server):
"""
If on a fuse max temperature is reached, HAL is supposed to cut the
stiffness on all the joints behind the considered fuse.
To add in /media/internal/DeviveInternalHeadGeode.xml before the test:
<Preference name="Key15"
memoryName="RobotConfig/Head/MinMaxChangeAllowed" description=""
value="1" type="string" />
"""
# logger initialization
log = logging.getLogger('MULTIFUSEBOARD_PERF_HW_01')
# erasing real time curves
if plot:
plot_server.curves_erase()
# flag initialization
flag_loop = True
flag_key = True
fuse_state = True
flag_ambiant_temperature = True
flag_fuse_status = True
flag_first_overshoot = False
flag_protection_on = False
# flags
(flag1, flag2, flag3) = (False, False, False)
# objects creation
fuse_temperature = fuse["FuseTemperature"]
fuse_current = fuse["FuseCurrent"]
fuse_voltage = fuse["FuseVoltage"]
fuse_resistor = fuse["FuseResistor"]
battery = subdevice.BatteryChargeSensor(dcm, mem)
log.info("")
log.info("***********************************************")
log.info("Testing fuse : " + str(fuse_temperature.part))
log.info("***********************************************")
log.info("")
# checking that ALMemory key MinMaxChange Allowed = 1
if int(mem.getData("RobotConfig/Head/MinMaxChangeAllowed")) != 1:
flag_loop = False
flag_key = False
log.error("MinMaxChangeAllowed ALMemory key missing")
# setting fuse temperature min and max
fuse_temperature_max = fuse_temperature.maximum
fuse_temperature_min = fuse_temperature.minimum
fuse_temperature_mid = (fuse_temperature_max +
fuse_temperature_min) / 2.
# setting fuse temperature min and max for hysteresis
fuse_temperature_max_hyst = fuse_temperature_max - 10
log.debug("fuse_temperature_max_hyst = " +
str(fuse_temperature_max_hyst))
fuse_temperature_min_hyst = fuse_temperature_min - 10
log.debug("fuse_temperature_min_hyst = " +
str(fuse_temperature_min_hyst))
fuse_temperature_mid_hyst = fuse_temperature_mid - 10
log.debug("fuse_temperature_mid_hyst = " +
str(fuse_temperature_mid_hyst))
# position dictionnary creation with normal min or max
state = \
qha_tools.read_section(
"multifuse_scenario4.cfg", fuse_temperature.part)
# creating joint list
joint_list = state.keys()
log.info("Joints to use are : " + str(joint_list))
# max stiffness is set on all joints except for LegFuse
if fuse_temperature.part == "LegFuse":
# stiff joints
qha_tools.stiff_joints_proportion(dcm, mem, joint_list, 1.0)
# stiff wheels
for wheel in wheel_objects:
wheel.stiffness.qqvalue = 1.0
wheel.speed.actuator.qvalue = \
(wheel.speed.actuator.maximum, 5000)
else:
qha_tools.stiff_joints_proportion(dcm, mem, joint_list, 1.0)
# defining increment in radians
increment = math.radians(joint_limit_extension)
# modifying max or min position and put the joint in that position
# it makes current rise a lot
for joint, value in state.items():
joint_object = subdevice.JointPositionActuator(dcm, mem, joint)
if value[0] == 'max':
new_maximum_angle = joint_object.maximum + increment
else:
new_maximum_angle = joint_object.minimum - increment
joint_object.maximum = [[[new_maximum_angle,
dcm.getTime(0)]], "Merge"]
joint_object.qvalue = (new_maximum_angle, 10000)
# logger creation
logger = qha_tools.Logger()
# list object creation
joint_hardness_list = \
[subdevice.JointHardnessActuator(dcm, mem, x) for x in joint_list]
joint_current_list = \
[subdevice.JointCurrentSensor(dcm, mem, joint)
for joint in joint_list]
# loop timer creation
timer = qha_tools.Timer(dcm, test_time)
# test loop
while flag_loop and timer.is_time_not_out():
try:
loop_time = timer.dcm_time() / 1000.
fuse_temperature_status = fuse_temperature.status
fuse_temperature_value = fuse_temperature.value
fuse_current_value = fuse_current.value
fuse_voltage_value = fuse_voltage.value
fuse_resistor_value = fuse_resistor.value
fuse_resistor_calculated = fuse_voltage_value / \
fuse_current_value
battery_total_voltage = battery.total_voltage
multifuseboard_ambiant_tmp = \
multi_fuseboard_ambiant_tmp.value
multifuseboard_total_current = \
multi_fuseboard_total_current.value
stiffness_decrease = mem.getData(
"Device/SubDeviceList/FuseProtection/"+\
"StiffnessDecrease/Value")
stiffness_decrease_immediate = mem.getData(
"Device/SubDeviceList/FuseProtection/"+\
"StiffnessDecreaseImmediate/Value")
listeofparams = [("Time", loop_time),
("MultifuseBoardAmbiantTemperature",
multifuseboard_ambiant_tmp),
("MultifuseBoardTotalCurrent",
multifuseboard_total_current),
("FuseTemperature", fuse_temperature_value),
("FuseCurrent", fuse_current_value),
("FuseVoltage", fuse_voltage_value),
("FuseResistor", fuse_resistor_value),
("FuseResistorCalculated",
fuse_resistor_calculated),
("BatteryVoltage", battery_total_voltage),
("Status", fuse_temperature_status),
("StiffnessDecrease", stiffness_decrease),
("StiffnessDecreaseImmediate",
stiffness_decrease_immediate),
("FuseTemperatureMin", fuse_temperature_min),
("FuseTemperatureMid", fuse_temperature_mid),
("FuseTemperatureMax", fuse_temperature_max)]
for joint_hardness in joint_hardness_list:
new_tuple = \
(joint_hardness.header_name, joint_hardness.value)
listeofparams.append(new_tuple)
for joint_current in joint_current_list:
new_tuple = (joint_current.header_name,
joint_current.value)
listeofparams.append(new_tuple)
for wheel in wheel_objects:
new_tuple = (wheel.short_name + "_Current",
wheel.current.value)
listeofparams.append(new_tuple)
# Logging informations
logger.log_from_list(listeofparams)
# for real time plot
if plot:
plot_server.add_point("MultifuseBoardAmbiantTemperature",
loop_time,
multifuseboard_ambiant_tmp)
plot_server.add_point("MultifuseBoardTotalCurrent",
loop_time,
multifuseboard_total_current)
plot_server.add_point("FuseTemperature", loop_time,
fuse_temperature_value)
plot_server.add_point("FuseCurrent", loop_time,
fuse_current_value)
plot_server.add_point("FuseVoltage", loop_time,
fuse_voltage_value)
plot_server.add_point("FuseResistor", loop_time,
fuse_resistor_value)
plot_server.add_point("FuseResistorCalculated", loop_time,
fuse_resistor_calculated)
plot_server.add_point("BatteryVoltage", loop_time,
battery_total_voltage)
plot_server.add_point("Status", loop_time,
fuse_temperature_status)
plot_server.add_point("StiffnessDecrease", loop_time,
stiffness_decrease)
plot_server.add_point("StiffnessDecreaseImmediate",
loop_time,
stiffness_decrease_immediate)
# Checking REQ_FUSE_TEMPERATURE_002
if fuse_temperature_value < multifuseboard_ambiant_tmp:
flag_ambiant_temperature = False
log.info("Fuse temperature is lower than MultiFuseBoard" +\
"ambiant temperature")
# Checking REQ_FUSE_PERF_003
# Fuse status evolves correctly with its estimated temperature
# Hysteresis works correctly too
if fuse_temperature_value >= fuse_temperature_min:
flag1 = True
if fuse_temperature_mid < fuse_temperature_value <=\
fuse_temperature_max:
flag2 = True
if fuse_temperature_value >= fuse_temperature_max:
flag3 = True
if (flag1, flag2, flag3) == (False, False, False):
theorical_status = 0
elif (flag1, flag2, flag3) == (True, False, False):
theorical_status = 1
elif (flag1, flag2, flag3) == (True, True, False):
theorical_status = 2
elif (flag1, flag2, flag3) == (True, True, True):
theorical_status = 3
if theorical_status == 3 and fuse_temperature_value <=\
fuse_temperature_max_hyst:
theorical_status = 2
flag3 = False
if theorical_status == 2 and fuse_temperature_value <=\
fuse_temperature_mid_hyst:
theorical_status = 1
flag2 = False
if theorical_status == 1 and fuse_temperature_value <=\
fuse_temperature_min_hyst:
theorical_status = 0
flag1 = False
if fuse_temperature_status != theorical_status:
log.warning("!!! Fuse status problem !!!")
log.info("fuse temperature = " +\
str(fuse_temperature_value))
log.info("fuse status = " + str(fuse_temperature_status))
log.info("fuse theorical status = " +
str(theorical_status))
flag_fuse_status = False
# Indicating fuse first max temperature overshoot
if fuse_temperature_value >= fuse_temperature_max and not\
flag_first_overshoot:
flag_first_overshoot = True
log.info("First temperature overshoot")
timer_overshoot = qha_tools.Timer(dcm, 200)
# Indicating that protection worked
# Set stiffness actuator to 0 to let fuse cool down
if flag_first_overshoot and timer_overshoot.is_time_out()\
and not flag_protection_on and\
(stiffness_decrease_immediate == 0 or\
stiffness_decrease == 0):
log.info("Flag protection ON")
flag_protection_on = True
log.info("Concerned joints pluggin activated")
qha_tools.unstiff_joints(dcm, mem, joint_list)
# Checking REQ_FUSE_PERF_004
if flag_first_overshoot and\
timer_overshoot.is_time_out() and not\
flag_protection_on:
if fuse_temperature.part == "LegFuse":
if stiffness_decrease_immediate != 0:
fuse_state = False
log.warning("LegFuse protection NOK")
else:
if stiffness_decrease != 0:
fuse_state = False
log.warning(fuse_temperature.part +\
" protection NOK")
if flag_protection_on and fuse_temperature_value < 80.0:
flag_loop = False
log.info("End of test, fuse is cold enough")
except KeyboardInterrupt:
flag_loop = False # out of test loop
log.info("KeyboardInterrupt from user")
log.info("!!!! OUT OF TEST LOOP !!!!")
file_name = "_".join([str(fuse_temperature.part), str(fuse_state)])
result_file_path = "/".join([result_base_folder, file_name]) + ".csv"
logger.log_file_write(result_file_path)
# setting original min and max
log.info("Setting orininal min and max position actuator values...")
for joint, value in state.items():
joint_object = subdevice.JointPositionActuator(dcm, mem, joint)
if value[0] > 0:
joint_object.maximum = value[0]
joint_object.qvalue = (joint_object.maximum, 500)
else:
joint_object.minimum = value[0]
joint_object.qvalue = (joint_object.minimum, 500)
qha_tools.wait(dcm, 200)
log.info("Unstiff concerned joints")
qha_tools.unstiff_joints(dcm, mem, joint_list)
if fuse_temperature.part == "LegFuse":
for wheel in wheel_objects:
wheel.speed.actuator.qvalue = (0.0, 3000)
time.sleep(3.0)
wheel.stiffness.qqvalue = 0.0
assert flag_key
assert fuse_state
assert flag_fuse_status
assert flag_ambiant_temperature
def temperature_logger(request, dcm, joint_temperature_object,
result_base_folder, sa_objects, plot_server, plot,
max_allowed_temperatures):
"""
@param dcm : proxy to DCM
@type dcm : object
@param joint_temperature_object : dictionary of jointTemperature objects
@type joint_temperature_object : dictionary
@param result_base_folder : path where logged data are saved
@type result_base_folder : string
@param sa_objects : dictionary of sliding average objects with the adapted
coefficient pour each joint
@type sa_objects : dictionary
@param plot_server : plot server object
@type plot_server : object
@param plot : if True, allow real time plot
@type plot : Booleen
@returns : Nothing. Automatically launch logger at the beggining of the
test class and stops it at the end.
"""
print "activating temperature logger..."
logger = qha_tools.Logger()
thread_flag = threading.Event()
def logging(thread_flag, plot, plot_server, max_allowed_temperatures):
"""Logging temperatures while the test class is not finished."""
timer = qha_tools.Timer(dcm, 1000)
while not thread_flag.is_set():
loop_time = timer.dcm_time() / 1000.
list_of_param = [("Time", loop_time)]
for joint_temperature in joint_temperature_object.values():
measured_temperature = joint_temperature.value
sa_object = sa_objects[joint_temperature.short_name]
temperature_header = joint_temperature.header_name
temperature_sa_header = temperature_header + "_sa"
temperature_max_header = temperature_header + "_max"
temperature_max = max_allowed_temperatures[
joint_temperature.short_name]
sa_object.point_add(measured_temperature)
sa_temperature = sa_object.calc()
new_tuple = (temperature_header, measured_temperature)
sa_tuple = (temperature_sa_header, sa_temperature)
max_tmp_tuple = (temperature_max_header, temperature_max)
list_of_param.append(new_tuple)
list_of_param.append(sa_tuple)
list_of_param.append(max_tmp_tuple)
# real time plot
if plot:
plot_server.add_point(temperature_sa_header, loop_time,
sa_temperature)
plot_server.add_point(temperature_max_header, loop_time,
temperature_max)
logger.log_from_list(list_of_param)
time.sleep(5.0)
my_logger = threading.Thread(target=logging,
args=(thread_flag, plot, plot_server,
max_allowed_temperatures))
my_logger.start()
def fin():
"""
Class teardown. Executed at the end of test class.
The logger is stopped and data is saved into a file.
"""
# stop logger thread
thread_flag.set()
print "logger stoped, saving file..."
result_file_path = "/".join([result_base_folder, "Temperatures"
]) + ".csv"
logger.log_file_write(result_file_path)
request.addfinalizer(fin)
def test_joint_current_limitation(self, dcm, mem, parameters, joint,
result_base_folder, rest_pos,
stiffness_off, plot, plot_server):
# logger initialization
log = logging.getLogger('test_joint_current_limitation')
# erasing real time plot
plot_server.curves_erase()
# flags initialization
flag_loop = True # test loop stops when this flag is False
flag_joint = True # giving test result
flag_current_limit_low_exceeded = False
# checking that ALMemory key MinMaxChange Allowed = 1
if int(mem.getData("RobotConfig/Head/MinMaxChangeAllowed")) != 1:
flag_loop = False
log.error("MinMaxChangeAllowed ALMemory key missing")
# test parameters
test_params = parameters
joint_position_actuator = joint.position.actuator
joint_position_sensor = joint.position.sensor
joint_temperature_sensor = joint.temperature
joint_current_sensor = joint.current
slav = qha_tools.SlidingAverage(test_params["sa_nb_points"])
logger = qha_tools.Logger()
# Going to initial position
subdevice.multiple_set(dcm, mem, rest_pos, wait=True)
# unstiffing all the other joints to avoid leg motors overheat
subdevice.multiple_set(dcm, mem, stiffness_off, wait=False)
time.sleep(0.1)
# stiffing the joint we want to test
joint.hardness.qqvalue = 1.0
# keeping initial joint min and max
joint_initial_maximum = joint_position_actuator.maximum
joint_initial_minimum = joint_position_actuator.minimum
# put joint to its initial maximum in 3 seconds
if joint_position_actuator.short_name in\
("HipPitch", "RShoulderRoll", "RElbowRoll"):
joint_position_actuator.qvalue = (joint_initial_minimum, 3000)
else:
joint_position_actuator.qvalue = (joint_initial_maximum, 3000)
qha_tools.wait(dcm, 3000)
# setting current limitations
joint_max_current = joint_current_sensor.maximum
joint_min_current = joint_current_sensor.minimum
delta_current = joint_max_current - joint_min_current
k_sup = test_params["limit_factor_sup"]
k_inf = test_params["limit_factor_inf"]
current_limit_high = joint_max_current + k_sup * delta_current
current_limit_low = joint_max_current - k_inf * delta_current
# setting temperature limitatons
joint_temperature_min = joint_temperature_sensor.minimum
# setting new min and max out of the mechanical stop
joint_new_maximum = \
joint_initial_maximum + \
math.radians(test_params["limit_extension"])
joint_new_minimum = \
joint_initial_minimum - \
math.radians(test_params["limit_extension"])
joint_position_actuator.maximum = [[[
joint_new_maximum, dcm.getTime(0)
]], "Merge"]
joint_position_actuator.minimum = [[[
joint_new_minimum, dcm.getTime(0)
]], "Merge"]
if joint_position_actuator.short_name in\
("HipPitch", "RShoulderRoll", "RElbowRoll"):
joint_position_actuator.qvalue = (joint_new_minimum, 1000)
else:
joint_position_actuator.qvalue = (joint_new_maximum, 1000)
timer = qha_tools.Timer(dcm, test_params["test_time"])
timer_limit = qha_tools.Timer(dcm, test_params["test_time_limit"])
# test loop
while flag_loop and timer.is_time_not_out():
try:
loop_time = timer.dcm_time() / 1000.
joint_temperature = joint_temperature_sensor.value
joint_current = joint_current_sensor.value
slav.point_add(joint_current)
joint_current_sa = slav.calc()
if not flag_current_limit_low_exceeded and\
joint_current_sa > current_limit_low:
flag_current_limit_low_exceeded = True
log.info("Current limit low exceeded")
if joint_current_sa > current_limit_high:
flag_joint = False
log.warning("Current limit high overshoot")
if flag_current_limit_low_exceeded and\
joint_current_sa < current_limit_low:
flag_joint = False
log.warning("Current lower than current limit low")
if timer_limit.is_time_out() and not \
flag_current_limit_low_exceeded:
flag_joint = False
log.info("Timer limit finished and "+\
"current limit low not exceeded")
# out of test loop if temperature is higher than min temperature
if joint_temperature >= joint_temperature_min:
flag_loop = False
log.info("Temperature higher than Min temperature")
log.info("Test finished")
logger.log(("Time", timer.dcm_time() / 1000.),
("Current", joint_current),
("CurrentSA", joint_current_sa),
("CurrentLimitHigh", current_limit_high),
("CurrentLimitLow", current_limit_low),
("MaxAllowedCurrent", joint_max_current),
("Temperature", joint_temperature),
("TemperatureMin", joint_temperature_min),
("Command", joint_position_actuator.value),
("Position", joint_position_sensor.value))
if plot:
plot_server.add_point("Current", loop_time, joint_current)
plot_server.add_point("CurrentSA", loop_time,
joint_current_sa)
plot_server.add_point("CurrentLimitHigh", loop_time,
current_limit_high)
plot_server.add_point("CurrentLimitLow", loop_time,
current_limit_low)
plot_server.add_point("MaxAllowedCurrent", loop_time,
joint_max_current)
plot_server.add_point("Temperature", loop_time,
joint_temperature)
plot_server.add_point("TemperatureMin", loop_time,
joint_temperature_min)
except KeyboardInterrupt:
flag_loop = False # out of test loop
log.info("KeyboardInterrupt from user")
log.info("OUT OF TEST LOOP")
result_file_path = "/".join([
result_base_folder, joint_position_actuator.subdevice_type,
joint_position_actuator.short_name + "_" + str(flag_joint)
]) + ".csv"
logger.log_file_write(result_file_path)
joint_position_actuator.maximum = [[[
joint_initial_maximum, dcm.getTime(0)
]], "Merge"]
joint_position_actuator.minimum = [[[
joint_initial_minimum, dcm.getTime(0)
]], "Merge"]
plot_server.curves_erase()
assert flag_joint
assert flag_current_limit_low_exceeded
def test_joint_temperature_protection(self, dcm, mem, parameters, joint,
result_base_folder, rest_pos,
stiffness_off, plot_server, plot):
# logger initialization
log = logging.getLogger('MOTOR_LIMITATION_PERF_HW_002')
# flags initialization
flag_joint = True
flag_loop = True
flag_max_current_exceeded = False
flag_max_temperature_exceeded = False
flag_low_limit = False
flag_info = False
flag_info2 = False
flag_info3 = False
flag_info4 = False
# erasing real time curves
if plot:
plot_server.curves_erase()
# Objects creation
test_params = parameters
joint_position_actuator = joint.position.actuator
joint_position_sensor = joint.position.sensor
joint_temperature_sensor = joint.temperature
joint_hardness_actuator = joint.hardness
joint_current_sensor = joint.current
slav = qha_tools.SlidingAverage(test_params["sa_nb_points"])
logger = qha_tools.Logger()
log.info("")
log.info("*************************************")
log.info("Testing : " + str(joint.short_name))
log.info("*************************************")
log.info("")
# Knowing the board, we can know if the motor is a MCC or DC Brushless
joint_board = joint_position_actuator.device
# Creating device object to acceed to its error
joint_board_object = device.Device(dcm, mem, joint_board)
# Going to initial position
subdevice.multiple_set(dcm, mem, rest_pos, wait=True)
# unstiffing all the other joints to avoid leg motors overheat
subdevice.multiple_set(dcm, mem, stiffness_off, wait=False)
time.sleep(0.1)
# stiffing the joint we want to test
joint.hardness.qqvalue = 1.0
# keeping initial joint min and max
joint_initial_maximum = joint_position_actuator.maximum
joint_initial_minimum = joint_position_actuator.minimum
# setting current limitations
joint_current_max = joint_current_sensor.maximum
# setting temperature limitatons
joint_temperature_min = joint_temperature_sensor.minimum
joint_temperature_max = joint_temperature_sensor.maximum
delta_temperature = joint_temperature_max - joint_temperature_min
# setting new min and max out of the mechanical stop
if joint_initial_maximum >= 0.0:
joint_new_maximum = \
joint_initial_maximum + \
math.radians(test_params["limit_extension"])
else:
joint_new_maximum = \
joint_initial_maximum - \
math.radians(test_params["limit_extension"])
if joint_initial_minimum <= 0.0:
joint_new_minimum = \
joint_initial_minimum - \
math.radians(test_params["limit_extension"])
else:
joint_new_minimum = \
joint_initial_minimum + \
math.radians(test_params["limit_extension"])
joint_position_actuator.maximum = [[[
joint_new_maximum, dcm.getTime(0)
]], "Merge"]
joint_position_actuator.minimum = [[[
joint_new_minimum, dcm.getTime(0)
]], "Merge"]
# set timer limit
timer_limit = qha_tools.Timer(dcm, test_params["test_time_limit"])
# for Brushless motors, max test time is 60 seconds
brushless_motors = ("KneePitch", "HipPitch", "HipRoll")
if joint_position_actuator.short_name in brushless_motors:
timer = qha_tools.Timer(dcm, 60000)
else:
timer = qha_tools.Timer(dcm, test_params["test_time"])
# set position actuator out of the joint mechanical stop
# going out of physical mechanical stop in 5 seconds
if joint_position_actuator.short_name in \
("HipPitch", "RShoulderRoll", "HipRoll"):
joint_position_actuator.qvalue = (joint_new_minimum, 5000)
else:
joint_position_actuator.qvalue = (joint_new_maximum, 5000)
flag_first_iteration = True
timer_current_decrease = qha_tools.Timer(dcm, 100)
while flag_loop is True and timer.is_time_not_out():
try:
joint_temperature = joint_temperature_sensor.value
joint_current = joint_current_sensor.value
slav.point_add(joint_current)
joint_position_command = joint_position_actuator.value
joint_position = joint_position_sensor.value
joint_hardness_value = joint_hardness_actuator.value
joint_current_sa = slav.calc()
firmware_error = joint_board_object.error
dcm_time = timer.dcm_time() / 1000.
# Max current adaptation if joint temperature higher than Min
# No lower current for DC Brushless motors
if joint_board not in \
("HipBoard", "ThighBoard", "BackPlatformBoard"):
if joint_temperature > joint_temperature_min:
delta_max = joint_temperature_max - joint_temperature
max_allowed_current = (
(delta_max) / (delta_temperature)) *\
joint_current_max
else:
max_allowed_current = joint_current_max
else:
max_allowed_current = joint_current_max
# max allowed current can not be lower than 0.
if max_allowed_current < 0.0:
max_allowed_current = 0.0
# defining old max current as first calculated max current if
# it is the first loop iteration
if flag_first_iteration is True:
old_mac = max_allowed_current
flag_first_iteration = False
# defining current regulation limits
current_limit_high = max_allowed_current * \
(1.0 + test_params["limit_factor_sup"])
current_limit_low = max_allowed_current *\
(1.0 - test_params["limit_factor_inf"])
# setting flag to True if 90 percent of max current is exceeded
if joint_current_sa > 0.9 * max_allowed_current and not\
flag_max_current_exceeded:
flag_max_current_exceeded = True
log.info("90 percent of max allowed currend reached")
if max_allowed_current != old_mac:
timer_current_decrease = qha_tools.Timer(dcm, 100)
# averaged current has not to exceed limit high
if joint_current_sa > current_limit_high and \
timer_current_decrease.is_time_out() and not flag_info4:
flag_joint = False
flag_info4 = True
log.warning("current high limit exceeded")
# once max current is exceeded, current hasn't to be lower than
# limit low
if flag_max_current_exceeded and \
joint_current_sa < current_limit_low and not \
flag_max_temperature_exceeded and not flag_low_limit:
flag_joint = False
flag_low_limit = True
log.info("current has been lower than low limit")
# after time limit, current has to have exceeded max current
# if it has not, it is written on time in log file
if timer_limit.is_time_out() and not\
flag_max_current_exceeded and flag_info is False:
flag_joint = False
flag_info = True
log.info("current has not exceeded 90 percent of max "+\
"allowed current")
# hardware protection
if joint_temperature >= joint_temperature_max + 1:
flag_loop = False
log.warning("temperature too high (max+1 degree)")
# if joint temperature higher than a limit value,
# joint current must be null after 100ms.
if flag_max_temperature_exceeded is False and \
joint_temperature >= joint_temperature_max:
flag_max_temperature_exceeded = True
timer_max = qha_tools.Timer(dcm, 100)
log.info("max temperature exceeded a first time")
if flag_max_temperature_exceeded and\
timer_max.is_time_out() and joint_current != 0 and not\
flag_info2:
flag_joint = False
flag_info2 = True
log.critical("max temperature exceeded and current "+\
"is not null")
if flag_max_temperature_exceeded and joint_current == 0.0 and\
not flag_info3:
flag_info3 = True
log.info("current null reached")
old_mac = max_allowed_current
logger.log(
("Time", dcm_time), ("Hardness", joint_hardness_value),
("Current", joint_current),
("CurrentSA", joint_current_sa),
("MaxAllowedCurrent", max_allowed_current),
("CurrentLimitHigh", current_limit_high),
("CurrentLimitLow", current_limit_low),
("Temperature", joint_temperature),
("TemperatureMin", joint_temperature_min),
("TemperatureMax", joint_temperature_max),
("Command", joint_position_command),
("Position", joint_position), ("FWError", firmware_error))
# for real time plot
if plot:
plot_server.add_point("Hardness", dcm_time,
joint_hardness_value)
plot_server.add_point("CurrentSA", dcm_time,
joint_current_sa)
plot_server.add_point("MaxAllowedCurrent", dcm_time,
max_allowed_current)
plot_server.add_point("CurrentLimitHigh", dcm_time,
current_limit_high)
plot_server.add_point("CurrentLimitLow", dcm_time,
current_limit_low)
plot_server.add_point("Temperature", dcm_time,
joint_temperature)
plot_server.add_point("TemperatureMin", dcm_time,
joint_temperature_min)
plot_server.add_point("TemperatureMax", dcm_time,
joint_temperature_max)
plot_server.add_point("Command", dcm_time,
joint_position_command)
plot_server.add_point("Position", dcm_time, joint_position)
except KeyboardInterrupt:
flag_loop = False
log.info("KeyboardInterrupt from user")
# writing logger results into a csv file
result_file_path = "/".join([
result_base_folder, joint_position_actuator.subdevice_type,
joint_position_actuator.short_name + "_" + str(flag_joint)
]) + ".csv"
logger.log_file_write(result_file_path)
# seting min and max joint software limits to their original values
joint_position_actuator.maximum = [[[
joint_initial_maximum, dcm.getTime(0)
]], "Merge"]
joint_position_actuator.minimum = [[[
joint_initial_minimum, dcm.getTime(0)
]], "Merge"]
assert flag_joint
