def fin():
"""Head capacitive sensor test teardown"""
# going to rest position
subdevice.multiple_set(dcm, mem, rest_pos, wait=True)
# unstiff robot
subdevice.multiple_set(dcm, mem, stiffness_off, wait=True)
def test_leg_enslavement(self, dcm, mem, zero_pos, test_leg_dico,
test_time, test_limit):
"""
Test joint enslavement for HipPitch and KneePitch.
Error must be lower than a fixed limit.
"""
# Objects creation
hippitch_position_actuator = test_leg_dico["hipActuator"]
hippitch_position_sensor = test_leg_dico["hipSensor"]
kneepitch_position_actuator = test_leg_dico["kneeActuator"]
kneepitch_position_sensor = test_leg_dico["kneeSensor"]
logger = test_leg_dico["logger"]
# Flag initialization
logger.flag = True
# Going to initial position
subdevice.multiple_set(dcm, mem, zero_pos, wait=True)
# Behavior of the tested part
hippitch_position_actuator.explore(2 * test_time)
kneepitch_position_actuator.explore(2 * test_time, max_to_min=False)
# Timer creation just before test loop
timer = qha_tools.Timer(dcm, 4 * test_time)
# Test loop
while timer.is_time_not_out():
hip_act_rad = hippitch_position_actuator.value
hip_act_deg = math.degrees(hip_act_rad)
hip_sensor_rad = hippitch_position_sensor.value
hip_sensor_deg = math.degrees(hip_sensor_rad)
knee_act_rad = kneepitch_position_actuator.value
knee_act_deg = math.degrees(knee_act_rad)
knee_sensor_rad = kneepitch_position_sensor.value
knee_sensor_deg = math.degrees(knee_sensor_rad)
error_hip = hip_act_deg - hip_sensor_deg
error_knee = knee_act_deg - knee_sensor_deg
logger.log(("Time", timer.dcm_time() / 1000.),
("HipPitchActuator", hip_act_deg),
("HipPitchSensor", hip_sensor_deg),
("ErrorHip", error_hip),
("KneePitchActuator", knee_act_deg),
("KneePitchSensor", knee_sensor_deg),
("ErrorKnee", error_knee), ("Eps", test_limit),
("-Eps", test_limit * -1.),
("HipPitchActuator+Eps", hip_act_deg + test_limit),
("HipPitchActuator-Eps", hip_act_deg - test_limit),
("KneePitchActuator+Eps", knee_act_deg + test_limit),
("KneePitchActuator-Eps", knee_act_deg - test_limit))
if abs(error_hip) > test_limit or abs(error_knee) > test_limit:
logger.flag = False
assert logger.flag
def test_joint_enslavement(self, dcm, mem, zero_pos, test_objects_dico,
test_time):
"""
Test joint enslavement.
Error must be lower than a fixed limit.
@type dcm : object
@param dcm : proxy to DCM
@type mem : object
@param mem : proxy to ALMemory
@type zero_pos : dictionnary
@param zero_pos : dictionnary {"ALMemory key":value} from config file.
@type joint : string
@param joint : string describing the current joint.
@type test_time : integer
@param test_time : time to wait to test a joint enslavement [ms].
"""
# Objects creation
joint_position_actuator = test_objects_dico["jointActuator"]
joint_position_sensor = test_objects_dico["jointSensor"]
logger = test_objects_dico["logger"]
test_limit = test_objects_dico["joint_test_limit"]
# Flag initialization
logger.flag = True
# Going to initial position
subdevice.multiple_set(dcm, mem, zero_pos, wait=True)
# Going to min position to have a complete exploration
joint_position_actuator.go_to(dcm, "min")
# Behavior of the tested part
joint_position_actuator.explore(2 * test_time)
# Timer creation just before test loop
timer = qha_tools.Timer(dcm, 4 * test_time)
# Test loop
while timer.is_time_not_out():
error = joint_position_actuator.value - joint_position_sensor.value
actuator_degrees = math.degrees(joint_position_actuator.value)
logger.log(("Time", timer.dcm_time() / 1000.),
("Actuator", actuator_degrees),
("Sensor", math.degrees(joint_position_sensor.value)),
("Error", math.degrees(error)), ("Eps", test_limit),
("-Eps", test_limit * -1.),
("Actuator+Eps", actuator_degrees + test_limit),
("Actuator-Eps", actuator_degrees - test_limit))
if abs(error) > math.radians(test_limit):
logger.flag = False
assert logger.flag
def fin():
"""Hands capacitive sensor test teardown"""
# going to rest position
subdevice.multiple_set(dcm, mem, rest_pos, wait=True)
# unstiff robot
subdevice.multiple_set(dcm, mem, stiffness_off, wait=True)
# setting max to their initial values
lwristyaw.position.actuator.maximum = \
[[[lwristyaw_initial_max, dcm.getTime(0)]], "Merge"]
rwristyaw.position.actuator.maximum = \
[[[rwristyaw_initial_max, dcm.getTime(0)]], "Merge"]
def wake_up_pos_brakes_closed(request, dcm, mem, wake_up_pos, rest_pos,
kill_motion, stiff_robot):
"""
Fixture which make the robot wakeUp, close brakes.
Control if brakes slip.
"""
subdevice.multiple_set(dcm, mem, wake_up_pos, wait=True)
thread_flag = threading.Event()
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.
my_thread = threading.Thread(target=control)
my_thread.start()
def fin():
"""
Stop control and put the robot in rest position at the end of session"
"""
thread_flag.set()
subdevice.multiple_set(dcm, mem, rest_pos, wait=True)
request.addfinalizer(fin)
def st_head(request, kill_motion, dcm, mem, stiffness_on, wake_up_pos,
rest_pos, stiffness_off):
"""Test Setup and TearDown for head capacitive sensor false positive"""
# stiffing body
subdevice.multiple_set(dcm, mem, stiffness_on, wait=True)
# going to zero position
subdevice.multiple_set(dcm, mem, wake_up_pos, wait=True)
def fin():
"""Head capacitive sensor test teardown"""
# going to rest position
subdevice.multiple_set(dcm, mem, rest_pos, wait=True)
# unstiff robot
subdevice.multiple_set(dcm, mem, stiffness_off, wait=True)
request.addfinalizer(fin)
def st_hands(request, kill_motion, dcm, mem, stiffness_on, zero_pos, rest_pos,
stiffness_off, rwristyaw, lwristyaw):
"""Test Setup and TearDown for hands capacitive sensor false positive"""
# stiffing body
subdevice.multiple_set(dcm, mem, stiffness_on, wait=True)
# going to zero position
subdevice.multiple_set(dcm, mem, zero_pos, wait=True)
# defining initial max
lwristyaw_initial_max = lwristyaw.position.actuator.maximum
rwristyaw_initial_max = rwristyaw.position.actuator.maximum
# new max position actuator out of real mechanical stop
lwristyaw_new_max = lwristyaw_initial_max + radians(10.0)
rwristyaw_new_max = rwristyaw_initial_max + radians(10.0)
# setting new max
lwristyaw.position.actuator.maximum = \
[[[lwristyaw_new_max, dcm.getTime(0)]], "Merge"]
rwristyaw.position.actuator.maximum = \
[[[rwristyaw_new_max, dcm.getTime(0)]], "Merge"]
# going to new max position
lwristyaw.position.actuator.qvalue = (lwristyaw_new_max, 4000)
rwristyaw.position.actuator.qvalue = (rwristyaw_new_max, 4000)
def fin():
"""Hands capacitive sensor test teardown"""
# going to rest position
subdevice.multiple_set(dcm, mem, rest_pos, wait=True)
# unstiff robot
subdevice.multiple_set(dcm, mem, stiffness_off, wait=True)
# setting max to their initial values
lwristyaw.position.actuator.maximum = \
[[[lwristyaw_initial_max, dcm.getTime(0)]], "Merge"]
rwristyaw.position.actuator.maximum = \
[[[rwristyaw_initial_max, dcm.getTime(0)]], "Merge"]
request.addfinalizer(fin)
def fin():
"""Wait between two tests."""
subdevice.multiple_set(dcm, mem, stiffness_on, wait=True)
subdevice.multiple_set(dcm, mem, rest_pos, wait=True)
subdevice.multiple_set(dcm, mem, stiffness_off, wait=True)
logging.info("Waiting " + str(time_to_wait) + " s")
time.sleep(float(time_to_wait))
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 main():
"""Main function"""
parser = argparse.ArgumentParser(description="Log datas from ALMemory")
parser.add_argument(dest="robot_ip", help="Robot IP or name")
args = parser.parse_args()
general = qha_tools.read_section("slip.cfg", "general")
robot_ip = args.robot_ip
result_folder = general["resultsFolder"][0]
# Create result folder if not existing
try:
os.mkdir(result_folder)
except OSError:
pass
dcm = ALProxy("DCM", robot_ip, 9559)
mem = ALProxy("ALMemory", robot_ip, 9559)
# Get Body ID
body_id = mem.getData("Device/DeviceList/ChestBoard/BodyId")
# Kill motion (if not already done)
try:
mot = ALProxy("ALMotion", robot_ip, 9559)
mot.exit()
except RuntimeError:
pass
print "[I] ALMotion is now killed"
# Ask the operator to write his name
name = raw_input("Please enter your name and press Enter : ")
while name == "":
name = raw_input("Please enter your name and press Enter : ")
nb_of_trials = int(general["numberOfTrials"][0])
stiffness_on = qha_tools.read_section("slip.cfg",
"stiffnessOnWithoutWheels")
stiffness_off = qha_tools.read_section("slip.cfg", "stiffnessOff")
rest_pos = qha_tools.read_section("slip.cfg", "restPosition")
force_list = []
for i in range(nb_of_trials):
# Set Stiffness on
subdevice.multiple_set(dcm, mem, stiffness_on)
# Go to rest position
subdevice.multiple_set(dcm, mem, rest_pos, wait=True)
time.sleep(1)
# Set Stiffness off
subdevice.multiple_set(dcm, mem, stiffness_off)
if (nb_of_trials - i - 1) <= 1:
trial_string = "trial"
else:
trial_string = "trials"
force = raw_input("Please push the robot, write the force (in Newton) "
"and press Enter. (" + str(nb_of_trials - i - 1) +
" " + trial_string + " left) : ")
while not is_number(force):
force = raw_input("Please push the robot, write the force"
"(in Newton) and press Enter. (" +
str(nb_of_trials - i - 1) + " " + trial_string +
" left) : ")
force_list.append(force)
print("Thank you !")
# At the end of the test, put the robot in initial position
# Set Stiffness on
subdevice.multiple_set(dcm, mem, stiffness_on)
# Go to rest position
subdevice.multiple_set(dcm, mem, rest_pos, wait=True)
time.sleep(1)
# Set Stiffness off
subdevice.multiple_set(dcm, mem, stiffness_off)
# Write results file
file_path = result_folder + "/" + body_id + ".dat"
my_file = open(file_path, 'a')
string_to_print = name + " " + " ".join(force_list) + "\n"
my_file.write(string_to_print)
def fin():
"""
Stop control and put the robot in rest position at the end of session"
"""
thread_flag.set()
subdevice.multiple_set(dcm, mem, rest_pos, wait=True)
def fin():
"""Method automatically executed at the end of the test."""
dcm.set(["Hardness", "Merge", [[1.0, dcm.getTime(0)]]])
subdevice.multiple_set(dcm, mem, rest_pos, wait=True)
dcm.set(["Hardness", "Merge", [[0.1, dcm.getTime(0)]]])
dcm.set(["Hardness", "Merge", [[0., dcm.getTime(1000)]]])
def test_pod_damage(dcm, mem, wake_up_pos, kill_motion, stiff_robot,
unstiff_parts):
"""
Test robot moving in the pod to check damages
"""
# Objects creation
robot_on_charging_station = subdevice.ChargingStationSensor(dcm, mem)
kneepitch_temperature = subdevice.JointTemperature(
dcm, mem, "KneePitch")
hippitch_temperature = subdevice.JointTemperature(
dcm, mem, "HipPitch")
hiproll_temperature = subdevice.JointTemperature(
dcm, mem, "HipRoll")
# Test parameters
parameters = qha_tools.read_section("test_pod.cfg", "DynamicTestParameters")
motion = subdevice.WheelsMotion(dcm, mem, 0.15)
motion.stiff_wheels(
["WheelFR", "WheelFL", "WheelB"],
int(parameters["stiffness_wheels_value"][0])
)
cycling_flag = 0
cycling_step = 0
# Going to initial position
subdevice.multiple_set(dcm, mem, wake_up_pos, wait=True)
# Flag initialization
flag = True
# Use plot class
plot_log = Plot(dcm, mem, parameters["easy_plot_csv_name"][0])
plot_log.start()
timer = qha_tools.Timer(dcm, int(parameters["test_time"][0]) * 1000)
# Use detection during movement class
log_during_movement = DetectionDuringMovement(
dcm, mem, timer,
parameters["log_during_movement_csv_name"][0],
parameters["lost_detection_csv_name"][0]
)
log_during_movement.start()
if robot_on_charging_station.value == 0:
print "Put the robot on the robot.\nVerify detection.\n"
flag = False
while timer.is_time_not_out() and flag == True:
# Verify if robot moves
if cycling_step == 3:
log_during_movement.set_cycling_stop_flag_true()
if cycling_flag == 0:
if kneepitch_temperature.value < \
int(parameters["max_joint_temperature"][0]):
print "KneePitch cycling"
cycling_step = 0
log_during_movement.set_cycling_stop_flag_false()
kneepitch_cycling(dcm, mem,
int(parameters["max_joint_temperature"][0]))
else:
cycling_step += 1
cycling_flag = 1
elif cycling_flag == 1:
if hippitch_temperature.value < \
int(parameters["max_joint_temperature"][0]):
print "HipPitch cycling"
cycling_step = 0
log_during_movement.set_cycling_stop_flag_false()
hippitch_cycling(
dcm, mem,
int(parameters["max_joint_temperature"][0])
)
else:
cycling_step += 1
cycling_flag = 2
else:
if hiproll_temperature.value < \
int(parameters["max_joint_temperature"][0]):
print "HipRoll cycling"
cycling_step = 0
log_during_movement.set_cycling_stop_flag_false()
hiproll_cycling(
dcm, mem,
int(parameters["max_joint_temperature"][0])
)
else:
cycling_step += 1
cycling_flag = 0
plot_log.stop()
log_during_movement.stop()
assert flag
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
