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 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 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 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)