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 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 stiff_joints_proportion(dcm, mem, joint_list, proportion): """Stiff joints in joint list.""" for joint in joint_list: joint_hardness_act = subdevice.JointHardnessActuator(dcm, mem, joint) if type(proportion) is not float: raise TypeError("proportion must be a float") joint_hardness_act.qqvalue = proportion
def unstiff_joints(dcm, mem, wait_time): """ Unstiff all joints except HipPitch, KneePitch and Wheels """ joints = tools.use_section("config.cfg", "JulietteJoints") for joint in joints: joint_hardness = subdevice.JointHardnessActuator(dcm, mem, joint) joint_hardness.qqvalue = 0.0 tools.wait(dcm, wait_time)
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 is_stiffness_null(dcm, mem, joint_list): """Returns True if all joint hardness are null.""" cpt = 0 for joint in joint_list: joint_hardness_actuator = \ subdevice.JointHardnessActuator(dcm, mem, str(joint)) if float(joint_hardness_actuator.value) >= 0.0: cpt += 1 if cpt == 0: return True else: return False
def are_there_null_stiffnesses(dcm, mem, joint_list): """ Return a boolean which indicates if one or more joint stiffnesses are null. If some joints stiffnesses are null, it indicates which ones: [True, ['HipPitch', 'KneePitch']] If no stiffnesses are null, it returns [False, []] """ list_of_joint = list() state = is_stiffness_null(dcm, mem, joint_list) if state is True: for joint in joint_list: joint_hardness_actuator = \ subdevice.JointHardnessActuator(dcm, mem, str(joint)) if float(joint_hardness_actuator.value) == 0.0: list_of_joint.append(joint) return [state, list_of_joint]
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) joint_hardness = subdevice.JointHardnessActuator(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, "jointHardness": joint_hardness } return dico_objects
def unstiff_joints(dcm, mem, joint_list): """Unstiff joints in joint list.""" for joint in joint_list: joint_hardness_act = subdevice.JointHardnessActuator(dcm, mem, joint) joint_hardness_act.qqvalue = 0.0
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 unstiff_parts(dcm, mem): joints = qha_tools.use_section("test_pod.cfg", "Parts") for name in joints: joint_hardness = subdevice.JointHardnessActuator(dcm, mem, name) joint_hardness.qqvalue = 0.