def verify_joint_temperature(dcm, mem, motion, joint, joint_temperature,
temp_max_to_start, time_wait):
# Verify joint not too hot
# If too hot, remove stiffness and wait
while joint_temperature.value > temp_max_to_start:
motion._setStiffnesses("Body", 0.0)
print("Joint too hot : " + str(joint_temperature.value))
print("-> Wait " + str(time_wait) + "s")
time.sleep(time_wait)
motion._setStiffnesses("Body", 1.0)
# Move ElbowYaw needs to raise ShoulderRoll
# So verify ShoulderRoll temperature
if joint == "RElbowYaw":
rshoulderroll_temp = subdevice.JointTemperature(
dcm, mem, "RShoulderRoll")
while rshoulderroll_temp.value > temp_max_to_start:
print "RShoulderRoll too hot -> Wait"
time.sleep(time_wait)
if joint == "LElbowYaw":
lshoulderroll_temp = subdevice.JointTemperature(
dcm, mem, "LShoulderRoll")
while lshoulderroll_temp.value > temp_max_to_start:
print "LShoulderRoll too hot -> Wait"
time.sleep(time_wait)
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 body_initial_temperatures(dcm, mem, joint_list):
"""Return a dictionnary corresponding to initial body temperatures."""
print "reading body initial temperatures..."
dico_initial_temperatures = {}
for joint in joint_list:
joint_temperature = subdevice.JointTemperature(dcm, mem, joint)
dico_initial_temperatures[joint] = joint_temperature.value
return dico_initial_temperatures
def joint_temperature_object(motion, dcm, mem, joint_list):
"""
Return a dictionnary of objects related to JointTemperature class from
subdevice.py
"""
dico_object = dict()
for joint in joint_list:
joint_object = subdevice.JointTemperature(dcm, mem, joint)
dico_object[joint] = joint_object
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 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_touchdetection(dcm, mem, motion, session, motion_wake_up,
remove_safety, parameters, speed_value,
test_objects_dico):
"""
Test touch detection : no false positive test.
Move joints at different speeds (cf associated config file).
Assert True if no TouchChanged event is detected.
@dcm : proxy to DCM (object)
@mem : proxy to ALMemory (object)
@motion : proxy to ALMotion (object)
@session : Session in qi (object)
@motion_wake_up : robot does is wakeUp
@remove_safety : remove safety
@parameters : dictionnary {"parameter":value} from config file
(dictionnary)
@speed_value : dictionnary {"speed":value}
(dictionnary)
@test_objects_dico : dictionnary {"Name":object} (dictionnary)
"""
expected = {"TouchChanged": 1}
module_name = "EventChecker_{}_".format(uuid.uuid4())
touchdetection = EventModule(mem)
module_id = session.registerService(module_name, touchdetection)
touchdetection.subscribe(module_name, expected)
# Objects creation
joint = test_objects_dico["jointName"]
joint_position_actuator = test_objects_dico["jointPositionActuator"]
joint_position_sensor = test_objects_dico["jointPositionSensor"]
joint_speed_actuator = test_objects_dico["jointSpeedActuator"]
joint_speed_sensor = test_objects_dico["jointSpeedSensor"]
joint_temperature = test_objects_dico["jointTemperature"]
speed = speed_value["Speed"]
# Go to reference position
set_position(dcm, mem, motion, "ReferencePosition")
# Send datas
plot = plot_touchdetection.Plot(
joint,
mem,
touchdetection,
joint_position_actuator,
joint_position_sensor,
joint_speed_actuator,
joint_speed_sensor,
joint_temperature,
float(parameters["LimitErrorPosition"][0]),
float(parameters["LimitErrorSpeed"][0]),
int(parameters["TemperatureMaxToStart"][0]),
int(parameters["TemperatureMax"][0])
)
plot.start()
print("\n\nJoint : " + joint + " - Speed : " + str(speed) + "\n")
# Verify joint not too hot
# If too hot, remove stiffness and wait
while joint_temperature.value > \
int(parameters["TemperatureMaxToStart"][0]):
motion._setStiffnesses("Body", 0.0)
print("Joint too hot : " + str(joint_temperature.value))
print("-> Wait " + str(parameters["TimeWait"][0]) + "s")
time.sleep(int(parameters["TimeWait"][0]))
motion._setStiffnesses("Body", 1.0)
# Move ElbowYaw needs to raise ShoulderRoll
# So verify ShoulderRoll temperature
if joint == "RElbowYaw":
rshoulderroll_temp = subdevice.JointTemperature(
dcm, mem, "RShoulderRoll")
while rshoulderroll_temp.value > \
int(parameters["TemperatureMaxToStart"][0]):
print "RShoulderRoll too hot -> Wait"
time.sleep(int(parameters["TimeWait"][0]))
if joint == "LElbowYaw":
lshoulderroll_temp = subdevice.JointTemperature(
dcm, mem, "LShoulderRoll")
while lshoulderroll_temp.value > \
int(parameters["TemperatureMaxToStart"][0]):
print "LShoulderRoll too hot -> Wait"
time.sleep(int(parameters["TimeWait"][0]))
# Initial position
set_position(dcm, mem, motion, joint)
time.sleep(2)
# Remove stiffness for unused parts
# Head stiffness never remove
# Can't remove stiffness on the Leg and move upper body
# Can't remove stiffness on arm if Leg move -> event detection
if parameters["StiffAllRobot"][0] == "False":
if joint in ["RShoulderPitch", "RShoulderRoll", "RElbowYaw",
"RElbowRoll", "RWristYaw"]:
stiffness_part(motion, ["LArm"], 0.0)
elif joint in ["LShoulderPitch", "LshoulderRoll", "LElbowYaw",
"LElbowRoll", "LWristYaw"]:
stiffness_part(motion, ["RArm"], 0.0)
elif joint in ["HeadPitch", "HeadYaw"]:
stiffness_part(motion, ["LArm", "RArm"], 0.0)
# Movement
movement(joint,
joint_position_actuator.minimum,
joint_position_actuator.maximum,
joint_temperature,
speed,
bool(parameters["MechanicalStop"][0]),
int(parameters["MovementNumberByJoint"][0]),
int(parameters["TemperatureMax"][0]),
motion,
plot
)
stiffness_part(motion, ["Body"], 1.0)
# Stop send datas
plot.stop()
time.sleep(0.25)
session.unregisterService(module_id)
if touchdetection.flag:
assert False
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 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)