class Battery:
""" The Flash class provides the Python API for the FLASH robot. """
def __init__(self):
""" Creates an Flash API object and connects to the robot. """
self.uw = UrbiWrapper()
# check if the result from the dummy request fits; otherwise abort
if not self.uw.isConnected:
raise RuntimeError('Connection to Flash failed.')
@property
def voltage(self):
return float(self.uw.send("robot.body.battery.voltage")[0])
@property
def reading(self):
value, timestamp = self.uw.send("robot.body.battery.voltage")
return float(value), timestamp
class Laser:
""" The Flash class provides the Python API for the FLASH robot. """
def __init__(self):
""" Creates an Flash API object and connects to the robot. """
self.uw = UrbiWrapper()
# check if the result from the dummy request fits; otherwise abort
# if not self.uw.isConnected:
# raise RuntimeError('Connection to Flash failed.')
@property
def scan(self):
try:
reading, ts = self.uw.send("robot.body.laser.val")
return eval(reading), ts
except:
pass
class FlashState:
def __init__(self):
self.uw = UrbiWrapper()
self.fc = FC()
self.joints = []
self.joint_names = {}
self.joints_descr = []
self.zero_positions = []
self.init()
self.init_remote_scripts()
def init(self):
# get zero positions
self.zero_positions.extend(self.fc._Head_ZeroPosition)
self.zero_positions.extend(self.fc._Arms_ZeroPositions)
self.zero_positions.extend(self.fc._Hands_LZeroPosition)
self.zero_positions.extend(self.fc._Hands_RZeroPosition)
# get joints description
self.joints_descr.extend(self.fc.JOINTS_HEAD)
self.joints_descr.extend(self.fc.JOINTS_LARM)
self.joints_descr.extend(self.fc.JOINTS_RARM)
self.joints_descr.extend(self.fc.JOINTS_LHAND)
self.joints_descr.extend(self.fc.JOINTS_RHAND)
# create joint objects
for idx, j_descr in enumerate(self.joints_descr):
joint = Joint(self.uw, j_descr[1], j_descr[2], j_descr[3],
self.zero_positions[idx])
setattr(self, j_descr[0], joint)
self.joints.append(joint)
def init_remote_scripts(self):
# creates the joints_val function which returns the raw values for all joints in an tuple
self.uw.send(FS_USCRIPT1 %
','.join(['%s.val' % x.name for x in self.joints]))
# creates the joints_pos function which returns the positions for all joints in an tuple
statements = [
'(%s.val - %.4f) / %.4f' % (j.name, j.raw_zero, j.ratio)
for j in self.joints
]
self.uw.send(FS_USCRIPT2 % ','.join(statements))
def printAll(self):
start = time.time()
pos = [joint.pos for joint in self.joints]
print(time.time() - start)
print(pos)
def center(self):
self.uw.send('robot.body.neck.head.BehaveNormal(2)')
for joint in self.joints:
print('center', joint.name)
joint.center()
print(joint.pos, joint.raw)
def joints_val(self):
return eval(self.uw.send('joints_val')[0])
def joints_pos(self):
return eval(self.uw.send('joints_pos')[0])
class Flash:
""" The Flash class provides the Python API for the FLASH robot. """
DEF_SPEED_ROTATION = 10
DEF_SPEED_TRANSLATION = 100
MAX_SPEED_ROTATION = 20
MAX_SPEED_TRANSLATION = 200
def __init__(self, uscript_filename=None):
""" Creates an Flash API object and connects to the robot.
@param uscript_filename - absolute file path; if a filename is given the content will be
uploaded during the start of the robot.
"""
self.uw = UrbiWrapper()
# check if the result from the dummy request fits; otherwise abort
if not self.uw.isConnected:
raise RuntimeError('Connection to Flash failed.')
# general URBI scripts
self.uw.send(U_FLASH_INIT)
# load auxiliary URBI script
if uscript_filename is not None:
self.uploadUrbiScript(uscript_filename)
# battery and laser bring their own connections
self.battery = Battery()
self.laser = Laser()
def uploadUrbiScript(self, filename):
""" Uploads a UrbiScript given by filename.
@param filename - absolute file path
"""
with open(filename, 'r') as f:
self.uw.send(f.read())
def say(self, text, intensity=2):
""" Speaks the given text for the given duration. The duration is not yet calculated
based on the text.
"""
self.uw.send('robot.body.neck.head.Say("%s", %i, 0)' %
(text, intensity))
def translate(self, duration, speed=DEF_SPEED_TRANSLATION):
""" Moves the robot forward if the speed is positive and backwards for negative speed.
The current speed limit is 200mm/s and the robot will stop the movement afterwards.
@param duration - amount of seconds the robot will keep going before stopping
@param speed - given in mm/s (default: 100)
"""
if speed != 0:
speed = max(min(speed, Flash.MAX_SPEED_TRANSLATION),
-Flash.MAX_SPEED_TRANSLATION)
self.uw.send("robot.body.x.speed = %i" % speed)
time.sleep(duration)
self.uw.send("robot.body.x.speed = 0")
def rotate(self, duration, speed=DEF_SPEED_ROTATION):
""" Rotates the robot left if the speed is positive and right for negative speed.
The current speed limit is 20deg/s and the robot will stop the movement afterwards.
@param duration - amount of seconds the robot will keep going before stopping
@param speed - given in deg/s (default: 10)
"""
if speed != 0:
speed = max(min(speed, Flash.MAX_SPEED_ROTATION),
-Flash.MAX_SPEED_ROTATION)
self.uw.send("robot.body.yaw.speed = %i" % speed)
time.sleep(duration)
self.uw.send("robot.body.yaw.speed = 0")
def translateAndRotate(self,
duration,
x_speed=DEF_SPEED_TRANSLATION,
yaw_speed=DEF_SPEED_ROTATION):
""" Moves and rotates the robot at the same time as one action.
The current speed limit is 200mm/s for translation and 20deg/s for rotation. The robot will
stop the movement afterwards.
@param duration - amount of seconds the robot will keep going before stopping
@param x_speed - given in mm/s (default: 100)
@param yaw_speed - given in deg/s (default: 10)
"""
x_speed = max(min(x_speed, Flash.MAX_SPEED_TRANSLATION),
-Flash.MAX_SPEED_TRANSLATION)
yaw_speed = max(min(yaw_speed, Flash.MAX_SPEED_ROTATION),
-Flash.MAX_SPEED_ROTATION)
self.uw.send("robot.body.x.speed = %i & robot.body.yaw.speed = %i" %
(x_speed, yaw_speed))
time.sleep(duration)
self.uw.send("robot.body.x.speed = 0 & robot.body.yaw.speed = 0")
def stop(self):
""" Stops the robot from moving around and brings it in the default position. """
self.uw.send("stop;")
def forward(self, duration, speed=DEF_SPEED_TRANSLATION):
""" Convenience method for moving forward (@see translate). """
self.translate(duration, speed)
def backward(self, duration, speed=DEF_SPEED_TRANSLATION):
""" Convenience method for moving backwards (@see translate). """
self.translate(duration, -speed)
def turnLeft(self, duration, speed=DEF_SPEED_ROTATION):
""" Convenience method for rotating left (@see rotate). """
self.rotate(duration, speed)
def turnRight(self, duration, speed=DEF_SPEED_ROTATION):
""" Convenience method for rotating right (@see rotate). """
self.rotate(duration, -speed)
def triggerBehavior(self, behavior):
self.uw.send('robot.competency.%s()' % behavior)
def exp(self, behavior, duration=2, intensity=8):
if behavior == 'Yawn':
cmd = 'robot.body.neck.head.BehaveYawn(%s)' % duration
else:
cmd = 'robot.body.neck.head.Behave%s(%s, %s)' % (
behavior, intensity, duration)
self.uw.send(cmd)
def backgroundBehavior(self, behavior, duration=3, intensity=8):
self.uw.send("robot.body.neck.head.Act%s(%s, %s)" %
(behavior, intensity, duration))
def __del__(self):
""" Reseting the robot before closing the object. """
if self.uw is not None:
self.stop()
class FC:
""" The FC class provides the configuration for the FLASH robot as provided by the _CONFIG_.u
file.
"""
def __init__(self):
self.uw = UrbiWrapper()
self.refresh()
def refresh(self):
""" Reads the current configuration from the robot and sets the variables of this
class accordingly.
"""
for var in FC.CONFIG_VARS:
value = self.uw.send(var)[0]
value = value.replace(b'true', b'True')
value = value.replace(b'false', b'False')
setattr(self, var, eval(value))
# List of joints with their respective settings
# (http://flashrobotics.pl/hardware/emys/dimensions-and-joints)
JOINTS_HEAD = [
['neck_pitch', 'robot.body.neck.pitch', -20.0, 30.0],
['head_yaw', 'robot.body.neck.head.yaw', -90.0, 90.0],
['head_pitch', 'robot.body.neck.head.pitch', -6.0, 29.0],
['disc_low', 'robot.body.neck.head.disc[down]', -16.0, 5.0],
['disc_high', 'robot.body.neck.head.disc[up]', -7.0, 15.0],
['left_eye_lid', 'robot.body.neck.head.eye[left].lid', 0.0, 120.0],
['left_eye_rot', 'robot.body.neck.head.eye[left].brow', -45.0, 45.0],
['left_eye_trans', 'robot.body.neck.head.eye[left].trans', 0.0, 100.0],
['right_eye_lid', 'robot.body.neck.head.eye[right].lid', 0.0, 120.0],
['right_eye_rot', 'robot.body.neck.head.eye[right].brow', -45.0, 45.0],
[
'right_eye_trans', 'robot.body.neck.head.eye[right].trans', 0.0,
100.0
],
]
# List of joints with their respective settings
# (http://flashrobotics.pl/hardware/flash/dimensions-and-joints)
JOINTS_LARM = [
['l_arm_q1', 'robot.body.arm[left][1]', -170.0, 170.0],
['l_arm_q2', 'robot.body.arm[left][2]', -90.0, 90.0],
['l_arm_q3', 'robot.body.arm[left][3]', -170.0, 170.0],
['l_arm_q4', 'robot.body.arm[left][4]', -50.0, 130.0],
['l_arm_q5', 'robot.body.arm[left][5]', -170.0, 170.0],
]
# List of joints with their respective settings
# (http://flashrobotics.pl/hardware/flash/dimensions-and-joints)
JOINTS_RARM = [
['r_arm_q1', 'robot.body.arm[right][1]', -170.0, 170.0],
['r_arm_q2', 'robot.body.arm[right][2]', -90.0, 90.0],
['r_arm_q3', 'robot.body.arm[right][3]', -170.0, 170.0],
['r_arm_q4', 'robot.body.arm[right][4]', -50.0, 130.0],
['r_arm_q5', 'robot.body.arm[right][5]', -170.0, 170.0],
]
# List of joints with their respective settings
# (http://flashrobotics.pl/hardware/flash/dimensions-and-joints)
JOINTS_LHAND = [
['l_hand_q1', 'robot.body.arm[left].hand.finger[1][1]', 0.0, 90.0],
['l_hand_q2', 'robot.body.arm[left].hand.finger[1][2]', 0.0, 90.0],
['l_hand_q3', 'robot.body.arm[left].hand.finger[1][3]', -20.0, 20.0],
['l_hand_q4', 'robot.body.arm[left].hand.finger[2][1]', 0.0, 90.0],
['l_hand_q5', 'robot.body.arm[left].hand.finger[2][2]', 0.0, 90.0],
['l_hand_q6', 'robot.body.arm[left].hand.finger[2][3]', -20.0, 20.0],
['l_hand_q7', 'robot.body.arm[left].hand.finger[3][1]', 0.0, 90.0],
['l_hand_q8', 'robot.body.arm[left].hand.finger[3][2]', 0.0, 90.0],
['l_hand_q9', 'robot.body.arm[left].hand.finger[3][3]', -20.0, 90.0],
['l_hand_q10', 'robot.body.arm[left].hand.finger[4][1]', 0.0, 90.0],
['l_hand_q11', 'robot.body.arm[left].hand.finger[4][2]', -20.0, 20.0],
['l_hand_q12', 'robot.body.arm[left].hand.finger[4][3]', 0.0, 90.0],
['l_arm_q6', 'robot.body.arm[left].hand.yaw', -30.0, 30.0],
['l_arm_q7', 'robot.body.arm[left].hand.pitch', -30.0, 30.0],
]
# List of joints with their respective settings
# (http://flashrobotics.pl/hardware/flash/dimensions-and-joints)
JOINTS_RHAND = [
['r_hand_q1', 'robot.body.arm[right].hand.finger[1][1]', 0.0, 90.0],
['r_hand_q2', 'robot.body.arm[right].hand.finger[1][2]', 0.0, 90.0],
['r_hand_q3', 'robot.body.arm[right].hand.finger[1][3]', -20.0, 20.0],
['r_hand_q4', 'robot.body.arm[right].hand.finger[2][1]', 0.0, 90.0],
['r_hand_q5', 'robot.body.arm[right].hand.finger[2][2]', 0.0, 90.0],
['r_hand_q6', 'robot.body.arm[right].hand.finger[2][3]', -20.0, 20.0],
['r_hand_q7', 'robot.body.arm[right].hand.finger[3][1]', 0.0, 90.0],
['r_hand_q8', 'robot.body.arm[right].hand.finger[3][2]', 0.0, 90.0],
['r_hand_q9', 'robot.body.arm[right].hand.finger[3][3]', -20.0, 90.0],
['r_hand_q10', 'robot.body.arm[right].hand.finger[4][1]', 0.0, 90.0],
['r_hand_q11', 'robot.body.arm[right].hand.finger[4][2]', -20.0, 20.0],
['r_hand_q12', 'robot.body.arm[right].hand.finger[4][3]', 0.0, 90.0],
['r_arm_q6', 'robot.body.arm[right].hand.yaw', -30.0, 30.0],
['r_arm_q7', 'robot.body.arm[right].hand.pitch', -30.0, 30.0],
]
JOINTS = [
JOINTS_HEAD, JOINTS_LARM, JOINTS_LHAND, JOINTS_RARM, JOINTS_RHAND
]
# List of variables from the _CONFIG_.u file.
# (_CONFIG_.u)
CONFIG_VARS = [
'_RobotStructure',
'_En_UPlayer',
'_En_UPlayerNext',
'_En_URecog',
'_En_UKinectAudio',
'_En_UTextToolAudio',
'_En_UMP3',
'_Speech_module',
'_Speech_volume',
'_Speech_param',
'_Recog_module',
'_Recog_param',
'_Recog_input',
'_Record_module',
'_Player_module',
'_PlayerNext_MasterVolume',
'_PlayerNext_PlayerVolume',
'_PlayerNext_MusicPlayerVolume',
'_PlayerNext_MicRecordLevel',
'_PlayerNext_KinectRecordLevel',
'_En_UCamera',
'_En_UKinectOpenNI',
'_En_UKinectOpenNI2',
'_En_UKinectVideo',
'_En_UObjectDetector',
'_En_UColorDetector',
'_En_UMoveDetector',
'_En_UFacet',
'_En_UImageTool',
'_En_UImageDisplay',
'_Camera_flip',
'_Camera_index',
'_Kinect_Offset',
'_Kinect_FaceTrackingEnable',
'_Kinect_InteractionEnable',
'_Kinect_DepthNearMode',
'_Kinect_SkeletonTrackingMode',
'_Kinect_SkeletonChooserMode',
'_ObjectDetector_source',
'_ObjectDetector_multi',
'_Color1Detector_color',
'_Color1Detector_source',
'_Color2Detector_color',
'_Color2Detector_source',
'_Color3Detector_color',
'_Color3Detector_source',
'_Color4Detector_color',
'_Color4Detector_source',
'_MoveDetector_source',
'_MoveDetector_frameBuffer',
'_MoveDetector_duration',
'_MoveDetector_smooth',
'_FacetDetector_source',
'_ImageDisplayWindows',
# PLATFORM CONFIGURATION
'_En_UPlayerNavigation',
'_En_UAria',
'_navigationPort',
'_En_ARIA_Sonars',
'_En_ARIA_Laser',
'_ARIA_LaserType',
'_ARIA_LaserStart',
'_ARIA_LaserEnd',
'_ARIA_Flipped',
'_com_robot',
'_com_laser',
'_ARIA_LaserStartingBaud',
# EMYS CONFIGURATION
'_En_DynamixelHead',
'_Dyn_FindServosHead',
'_com_head',
'_Head_IDs',
'_Head_ZeroPosition',
'_Head_DynamixelRatio',
'_Head_HitecRatio',
'_Head_TransRatio',
# ARMS CONFIGURATION
'_En_DynamixelArms',
'_Dyn_FindServosArms',
'_com_arms',
'_Arms_IDs',
'_Arms_ZeroPositions',
'_Arms_Ratio',
'_Arms_MaxSpeed',
# HAND CONFIGURATION
'_En_DynamixelHands',
'_Dyn_FindServosHands',
'_com_Lhands',
'_com_Rhands',
'_Hands_LIDs',
'_Hands_RIDs',
'_Hands_LZeroPosition',
'_Hands_RZeroPosition',
'_Hands_Finger_Ratio',
'_Hands_Wrist_Ratio',
# ML
'_En_UKNearestML',
'_En_UEigenfaces',
# Network
'_En_UBrowser',
'_En_UMail',
'_En_UFacebook',
'_En_UGCalendar',
'_En_UTextToolNetwork',
# ... skipping some since they are not relevant right now ...
# APPRAISAL
'_En_UWordNet',
'_En_USentiWordNet',
'_En_UAnew',
# EMOTIONS
'_En_UPAD',
'_En_UKNearestEmot',
'_En_UWASABI',
'_WASABI_RobotPersonality',
]
class Head:
""" The Flash class provides the Python API for the FLASH robot. """
DEF_SPEED_ROTATION = 10
DEF_SPEED_TRANSLATION = 100
MAX_SPEED_ROTATION = 20
MAX_SPEED_TRANSLATION = 200
# [<attribute>, <name>, <pos_min>, <pos_max> ]
JOINTS = [
['neck_pitch', 'robot.body.neck.pitch', -20.0, 30.0 ],
['head_yaw', 'robot.body.neck.head.yaw', -90.0, 90.0 ],
['head_pitch', 'robot.body.neck.head.pitch', -6.0, 29.0 ],
['disc_low', 'robot.body.neck.head.disc[down]', -20.0, 30.0 ],
['disc_high', 'robot.body.neck.head.disc[up]', -7.0, 15.0 ],
['left_eye_lid', 'robot.body.neck.head.eye[left].lid', 0.0, 120.0 ],
['left_eye_rot', 'robot.body.neck.head.eye[left].brow', -45.0, 45.0 ],
['left_eye_trans', 'robot.body.neck.head.eye[left].trans', 0.0, 100.0 ],
['right_eye_lid', 'robot.body.neck.head.eye[right].lid', 0.0, 120.0 ],
['right_eye_rot', 'robot.body.neck.head.eye[right].brow', -45.0, 45.0 ],
['right_eye_trans', 'robot.body.neck.head.eye[right].trans', 0.0, 100.0 ],
]
def __init__(self):
""" Creates an Flash API object and connects to the robot. """
self.uw = UrbiWrapper()
# check if the result from the dummy request fits; otherwise abort
if not self.uw.isConnected:
raise RuntimeError('Connection to Flash failed.')
joint_zero_positions = eval(self.uw.send('_Head_ZeroPosition')[0])
# create joints
for idx, info in enumerate(Head.JOINTS):
joint = Joint(self.uw, info[1], info[2], info[3], joint_zero_positions[idx])
setattr(self, info[0], joint)
# center joints
for joint in Head.JOINTS:
getattr(self, joint[0]).center()
# create sensors
self.touch_up = Sensor(self.uw, 'robot.body.neck.head.sensor[up]', 0, 2000)
self.touch_left = Sensor(self.uw, 'robot.body.neck.head.sensor[left]', 0, 2000)
self.touch_front = Sensor(self.uw, 'robot.body.neck.head.sensor[front]', 0, 2000)
self.touch_right = Sensor(self.uw, 'robot.body.neck.head.sensor[right]', 0, 2000)
self.touch_down = Sensor(self.uw, 'robot.body.neck.head.sensor[down]', 0, 2000)
def getStatusStr(self):
status = ["Head Status"]
status.append("===========")
status.append('- Joints')
status.append(str(self.neck_pitch))
status.append(str(self.head_yaw))
status.append(str(self.head_pitch))
status.append(str(self.disc_low))
status.append(str(self.disc_high))
status.append(str(self.left_eye_lid))
status.append(str(self.left_eye_rot))
status.append(str(self.left_eye_trans))
status.append(str(self.right_eye_lid))
status.append(str(self.right_eye_rot))
status.append(str(self.right_eye_trans))
status.append('- Sensors')
status.append(str(self.touch_up))
status.append(str(self.touch_left))
status.append(str(self.touch_front))
status.append(str(self.touch_right))
status.append(str(self.touch_down))
return '\n'.join(status)