def control():
""" Control if brakes slip"""
kneepitch_position_actuator = subdevice.JointPositionActuator(
dcm, mem, "KneePitch")
kneepitch_position_sensor = subdevice.JointPositionSensor(
dcm, mem, "KneePitch")
kneepitch_hardness_actuator = subdevice.JointHardnessActuator(
dcm, mem, "KneePitch")
hippitch_position_actuator = subdevice.JointPositionActuator(
dcm, mem, "HipPitch")
hippitch_position_sensor = subdevice.JointPositionSensor(
dcm, mem, "HipPitch")
hippitch_hardness_actuator = subdevice.JointHardnessActuator(
dcm, mem, "HipPitch")
hippitch_hardness_actuator.qqvalue = 0.
kneepitch_hardness_actuator.qqvalue = 0.
while not thread_flag.is_set():
if abs(hippitch_position_sensor.value) > 0.1 or\
abs(kneepitch_position_sensor.value) > 0.1:
hippitch_hardness_actuator.qqvalue = 1.
kneepitch_hardness_actuator.qqvalue = 1.
hippitch_position_actuator.qvalue = (0., 1000)
kneepitch_position_actuator.qvalue = (0., 1000)
tools.wait(dcm, 2100)
hippitch_hardness_actuator.qqvalue = 0.
kneepitch_hardness_actuator.qqvalue = 0.
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 hippitch_cycling(dcm, mem, max_joint_temperature):
""" HipPitch cycling"""
# Objects creation
kneepitch_position_actuator = subdevice.JointPositionActuator(
dcm, mem, "KneePitch")
kneepitch_position_sensor = subdevice.JointPositionSensor(
dcm, mem, "KneePitch")
kneepitch_hardness_actuator = subdevice.JointHardnessActuator(
dcm, mem, "KneePitch")
hippitch_position_actuator = subdevice.JointPositionActuator(
dcm, mem, "HipPitch")
hippitch_temperature = subdevice.JointTemperature(
dcm, mem, "HipPitch")
hiproll_position_actuator = subdevice.JointPositionActuator(
dcm, mem, "HipRoll")
parameters = qha_tools.read_section("test_pod.cfg", "DynamicCycling")
# Initial position
kneepitch_position_actuator.qvalue = (0.,
int(parameters["time_go_initial_position"][0]) * 1000)
hiproll_position_actuator.qvalue = (0.,
int(parameters["time_go_initial_position"][0]) * 1000)
qha_tools.wait(dcm, int(parameters["time_go_initial_position"][0]) * 1000)
kneepitch_hardness_actuator.qqvalue = 0.
while hippitch_temperature.value < max_joint_temperature:
hippitch_position_actuator.qvalue = (
float(parameters["amplitude_hippitch"][0]),
int(parameters["time_movement_hippitch"][0]) * 1000
)
qha_tools.wait(dcm, int(parameters["time_movement_hippitch"][0]) * 1000)
hippitch_position_actuator.qvalue = (
-float(parameters["amplitude_hippitch"][0]),
int(parameters["time_movement_hippitch"][0]) * 1000
)
qha_tools.wait(dcm, int(parameters["time_movement_hippitch"][0]) * 1000)
print(str(hippitch_temperature.value))
if abs(kneepitch_position_sensor.value) > \
float(parameters["angle_slipping"][0]):
print "KneePitch slip"
kneepitch_hardness_actuator.qqvalue = 1.
kneepitch_position_actuator.qvalue = (0.,
int(parameters["time_after_slipping"][0]) * 1000)
hippitch_position_actuator.qvalue = (0.,
int(parameters["time_after_slipping"][0]) * 1000)
qha_tools.wait(dcm, int(parameters["time_after_slipping"][0]) * 1000)
kneepitch_hardness_actuator.qqvalue = 0.
hippitch_position_actuator.qvalue = (0.,
int(parameters["time_go_initial_position"][0]) * 1000)
qha_tools.wait(dcm, int(parameters["time_go_initial_position"][0]) * 1000)
kneepitch_hardness_actuator.qqvalue = 1.
def JointDico(dcm, mem, joint):
"""
Return dictionnary of object (position, speed, hardness, temperature)
for specified joint.
@dcm : proxy to DCM (object)
@mem : proxy to ALMemory (object)
@joint : joint name (string)
"""
joint_pos_act = subdevice.JointPositionActuator(dcm, mem, joint)
joint_pos_sen = subdevice.JointPositionSensor(dcm, mem, joint)
joint_speed_act = MotionActuatorValue(mem, joint)
joint_speed_sen = MotionSensorValue(mem, joint)
joint_hardness = subdevice.JointHardnessActuator(dcm, mem, joint)
joint_temp = subdevice.JointTemperature(dcm, mem, joint)
dico_joint = {
"PosAct": joint_pos_act,
"PosSen": joint_pos_sen,
"SpeedAct": joint_speed_act,
"SpeedSen": joint_speed_sen,
"Hardness": joint_hardness,
"Temp": joint_temp
}
return dico_joint
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_objects_dico(request, dcm, mem):
"""
Create the appropriate objects for each joint.
"""
joint_position_actuator = subdevice.JointPositionActuator(
dcm, mem, request.param)
joint_position_sensor = subdevice.JointPositionSensor(
dcm, mem, request.param)
joint_speed_actuator = MotionActuatorValue(mem, request.param)
joint_speed_sensor = MotionSensorValue(mem, request.param)
joint_temperature = subdevice.JointTemperature(dcm, mem, request.param)
dico_objects = {
"jointName": str(request.param),
"jointPositionActuator": joint_position_actuator,
"jointPositionSensor": joint_position_sensor,
"jointSpeedActuator": joint_speed_actuator,
"jointSpeedSensor": joint_speed_sensor,
"jointTemperature": joint_temperature
}
return dico_objects
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 run(self):
""" Log """
robot_on_charging_station = subdevice.ChargingStationSensor(
self.dcm, self.mem)
battery_current = subdevice.BatteryCurrentSensor(self.dcm, self.mem)
kneepitch_position_actuator = subdevice.JointPositionActuator(
self.dcm, self.mem, "KneePitch")
hippitch_position_actuator = subdevice.JointPositionActuator(
self.dcm, self.mem, "HipPitch")
hiproll_position_actuator = subdevice.JointPositionActuator(
self.dcm, self.mem, "HipRoll")
kneepitch_position_sensor = subdevice.JointPositionSensor(
self.dcm, self.mem, "KneePitch")
hippitch_position_sensor = subdevice.JointPositionSensor(
self.dcm, self.mem, "HipPitch")
hiproll_position_sensor = subdevice.JointPositionSensor(
self.dcm, self.mem, "HipRoll")
kneepitch_temperature = subdevice.JointTemperature(
self.dcm, self.mem, "KneePitch")
hippitch_temperature = subdevice.JointTemperature(
self.dcm, self.mem, "HipPitch")
hiproll_temperature = subdevice.JointTemperature(
self.dcm, self.mem, "HipRoll")
log_file = open(self.file_name, 'w')
log_file.write(
"Time,Detection,Current,KneePitchPositionActuator," +
"KneePitchPositionSensor,KneePitchTemperature," +
"HipPitchPositionActuator,HipPitchPositionSensor," +
"HipPitchTemperature,HipRollPositionActuator," +
"HipRollPositionSensor,HipRollTemperature,WheelBStiffness," +
"WheelFRStiffness,WheelFLStiffness\n"
)
plot_server = easy_plot_connection.Server()
time_init = time.time()
while not self._end_plot:
elapsed_time = time.time() - time_init
plot_server.add_point(
"Detection", elapsed_time, robot_on_charging_station.value)
plot_server.add_point(
"Current", elapsed_time, battery_current.value)
plot_server.add_point("KneePitchPositionActuator", elapsed_time,
kneepitch_position_actuator)
plot_server.add_point("KneePitchPositionSensor", elapsed_time,
kneepitch_position_sensor)
plot_server.add_point("KneePitchTemperature", elapsed_time,
kneepitch_temperature)
plot_server.add_point("HipPitchPositionActuator", elapsed_time,
hippitch_position_actuator)
plot_server.add_point("HipPitchPositionSensor", elapsed_time,
hippitch_position_sensor)
plot_server.add_point("HipPitchTemperature", elapsed_time,
hippitch_temperature)
plot_server.add_point("HipRollPositionActuator", elapsed_time,
hiproll_position_actuator)
plot_server.add_point("HipRollPositionSensor", elapsed_time,
hiproll_position_sensor)
plot_server.add_point("HipRollTemperature", elapsed_time,
hiproll_temperature)
line_to_write = ",".join([
str(elapsed_time),
str(robot_on_charging_station.value),
str(battery_current.value),
str(kneepitch_position_actuator.value),
str(kneepitch_position_sensor.value),
str(kneepitch_temperature.value),
str(hippitch_position_actuator.value),
str(hippitch_position_sensor.value),
str(hippitch_temperature.value),
str(hiproll_position_actuator.value),
str(hiproll_position_sensor.value),
str(hiproll_temperature.value)
])
line_to_write += "\n"
log_file.write(line_to_write)
log_file.flush()
time.sleep(0.1)