def arm_give_condimentBC():
print "<give customer a condimentBC>"
#shifts gaze back right
Head.set_pan(head, -0.55, speed = 0.3, timeout = 0.0)
#move to ketchup
right.move_to_joint_positions({'right_s0': 0.8287331206550947, 'right_s1': -0.5115825927597855, 'right_w0': 0.35971849475912954, 'right_w1': 0.5073641455931006, 'right_w2': -3.0591411862404865, 'right_e0': -0.4237621926533455, 'right_e1': 1.3905535842181276}
)
#move to mustard
right.move_to_joint_positions({'right_s0': 0.7474321388971679, 'right_s1': -0.28532042654668693, 'right_w0': 0.39346607209260864, 'right_w1': 0.8302671014429802, 'right_w2': -3.058757691043515, 'right_e0': -0.4249126782442596, 'right_e1': 0.8732185635037718}
)
#move to ketchup
right.move_to_joint_positions({'right_s0': 0.8287331206550947, 'right_s1': -0.5115825927597855, 'right_w0': 0.35971849475912954, 'right_w1': 0.5073641455931006, 'right_w2': -3.0591411862404865, 'right_e0': -0.4237621926533455, 'right_e1': 1.3905535842181276}
)
#lowers down on ketchup
right.move_to_joint_positions({'right_s0': 0.768140879533621, 'right_s1': -0.2665291618950906, 'right_w0': 0.43526704856248616, 'right_w1': 0.49547579448698864, 'right_w2': -3.058757691043515, 'right_e0': -0.4019029664259784, 'right_e1': 1.15892248524743}
,timeout = 15.0)
#grippers close
rightg.close()
#lifts
right.move_to_joint_positions({'right_s0': 0.9821311994436361, 'right_s1': -0.6178107623208504, 'right_w0': 0.33555829734993425, 'right_w1': 0.5303738574113818, 'right_w2': -3.0591411862404865, 'right_e0': -0.4778350154263064, 'right_e1': 1.504068162521648}
)
#shifts gaze back center
Head.set_pan(head, 0.0, speed = 0.3, timeout = 0.0)
#lifts more
right.move_to_joint_positions({'right_s0': 1.1362962686261202, 'right_s1': -0.7340098070031704, 'right_w0': 0.27074760906177553, 'right_w1': 1.0833739314440736, 'right_w2': -3.0591411862404865, 'right_e0': -0.4759175394414496, 'right_e1': 1.1255584031109223}
)
#moves towards plate
right.move_to_joint_positions({'right_s0': 1.2996652225359169, 'right_s1': -0.16298545871282522, 'right_w0': 0.2987427584406843, 'right_w1': 1.0051409112619174, 'right_w2': -3.0591411862404865, 'right_e0': -0.514267059138585, 'right_e1': 0.3324903357741634}
)
#lowers on plate
#right.move_to_joint_positions({'right_s0': 1.303116679308659, 'right_s1': 0.0674951546669582, 'right_w0': 0.30871363356193954, 'right_w1': 0.6028544496389676, 'right_w2': -3.0572237102556294, 'right_e0': -0.4341165629715721, 'right_e1': 0.31408256631953846}
#)
#sets down on plate
right.move_to_joint_positions({'right_s0': 1.331495323884539, 'right_s1': 0.16336895390979655, 'right_w0': 0.2972087776527989, 'right_w1': 0.18062623777350748, 'right_w2': -3.058757691043515, 'right_e0': -0.5146505543355563, 'right_e1': 0.41225733674420495}
)
#opens gripper
rightg.open()
#lifts
right.move_to_joint_positions({'right_s0': 1.2720535683539793, 'right_s1': 0.009587379924283835, 'right_w0': 0.2964417872588562, 'right_w1': 0.6703496043059258, 'right_w2': -3.0572237102556294, 'right_e0': -0.5725583290782307, 'right_e1': 0.146495165243057}
)
#move back to neutral
right.move_to_joint_positions(neutral_right)
#close gripper
rightg.close()
def arm_give_condimentC():
print "<give customer a condimentBC>"
#shifts gaze back right
Head.set_pan(head, -0.55, speed = 0.3, timeout = 0.0)
#move to ketchup
right.move_to_joint_positions({'right_s0': 0.11389807350049197, 'right_s1': -0.8977622561099384, 'right_w0': -0.4061214135926633, 'right_w1': 0.17487380981893716, 'right_w2': -0.2500388684253224, 'right_e0': 0.03106311095467963, 'right_e1': 2.1790197091912304})
#move to mustard
right.move_to_joint_positions({'right_s0': 0.3861796633501529, 'right_s1': -0.9422476989586154, 'right_w0': -0.03029612056073692, 'right_w1': 0.35128160042575973, 'right_w2': -0.6017039640480536, 'right_e0': -0.10737865515197896, 'right_e1': 1.9803691971600692})
#new_movement_of_condiment
right.move_to_joint_positions({'right_s0': 0.038733014894106695, 'right_s1': -0.8624806979885739, 'right_w0': -0.2339320701525256, 'right_w1': 0.1514806028036846, 'right_w2': -0.30756314797102546, 'right_e0': 0.0782330201821561, 'right_e1': 2.157160482963863})
#move lower
#right.move_to_joint_positions({'right_s0': 0.2998932440315984, 'right_s1': -0.7075486384121471, 'right_w0': 0.0337475773334791, 'right_w1': 0.095490304045867, 'right_w2': -0.6013204688510821, 'right_e0': -0.09165535207615347, 'right_e1': 2.05706823655434}
#)
#lowers down on ketchup
#lowers down on ketchup #changed
right.move_to_joint_positions({'right_s0': 0.046402918833533764, 'right_s1': -0.6810874698211237, 'right_w0': -0.3064126623801114, 'right_w1': -0.06787864986392955, 'right_w2': -0.4789855010172204, 'right_e0': 0.04410194765170564, 'right_e1': 2.1372187327213528})
#grippers close
rightg.close()
#lifts
right.move_to_joint_positions({'right_s0': 0.3271214030165645, 'right_s1': -1.071869075534933, 'right_w0': -0.03911651009107805, 'right_w1': 0.14304370847031483, 'right_w2': -0.5997864880631968, 'right_e0': -0.08820389530341129, 'right_e1': 2.2844808883583525})
#shifts gaze back center
Head.set_pan(head, 0.0, speed = 0.3, timeout = 0.0)
#lifts more
right.move_to_joint_positions({'right_s0': 0.4437039428958559, 'right_s1': -1.2647671596115235, 'right_w0': -0.037582529303192634, 'right_w1': 0.7715923363063631, 'right_w2': -0.4049709280017492, 'right_e0': 0.06404369789421602, 'right_e1': 1.9308983167507645})
#moves over
right.move_to_joint_positions({'right_s0': 0.9384127469889019, 'right_s1': -0.9955535313376335, 'right_w0': -0.05675728915176031, 'right_w1': 1.021247709534714, 'right_w2': 0.2822524649709161, 'right_e0': 0.2231942046373277, 'right_e1': 1.5132720472489607})
#sets down on plate
#sets down on plate
right.move_to_joint_positions({'right_s0': 1.2183642407779898, 'right_s1': -0.47975249141116316, 'right_w0': 0.04141748127290617, 'right_w1': 0.6718835850938112, 'right_w2': 0.34131072530450457, 'right_e0': -0.046402918833533764, 'right_e1': 1.318456487187513}
)
#opens gripper
rightg.open()
#lifts
right.move_to_joint_positions({'right_s0': 1.1984224905354794, 'right_s1': -0.7957525337155584, 'right_w0': 0.05138835639416136, 'right_w1': 0.7182865039273449, 'right_w2': 0.10239321759135137, 'right_e0': -0.0839854481367264, 'right_e1': 1.5953400194008303})
#move back to neutral
right.move_to_joint_positions(neutral_right)
#close gripper
rightg.close()
def arm_give_sandwichA():
print "<give customer a sandwich A>"
Head.set_pan(head, -0.45, speed = 1.0, timeout = 0.0)
#lifts arm above sandwiches
right.move_to_joint_positions({'right_s0': 0.7719758315033345, 'right_s1': -0.6446554261088451, 'right_w0': 0.27074760906177553, 'right_w1': 0.984048675428493, 'right_w2': -0.2922233400921713, 'right_e0': -0.26307770512234846, 'right_e1': 1.2616991980357528}
)
#shifts gaze right
Head.set_pan(head, -0.55, speed = 0.3, timeout = 0.0)
#shifts arm right
right.move_to_joint_positions({'right_s0': 0.8080243800186417, 'right_s1': -0.6273981422451342, 'right_w0': 0.324436936637765, 'right_w1': 0.7792622402457902, 'right_w2': -0.24275245968286674, 'right_e0': -0.3037281960013119, 'right_e1': 1.367160377202875}
)
#lowers to grab it
right.move_to_joint_positions({'right_s0': 0.8041894280489281, 'right_s1': -0.30871363356193954, 'right_w0': 0.47246608266870743, 'right_w1': 0.7156020375485456, 'right_w2': -0.3485971340469603, 'right_e0': -0.33325732616810616, 'right_e1': 1.1293933550806359}
)
#grippers close
rightg.close()
#lifts
right.move_to_joint_positions({'right_s0': 0.8041894280489281, 'right_s1': -0.48051948180510584, 'right_w0': 0.4628787027444236, 'right_w1': 0.7911505913519021, 'right_w2': -0.3508981052287884, 'right_e0': -0.3432282012893613, 'right_e1': 1.2317865726719872}
)
#set gaze back to customer
head.set_pan(0.0, speed = 0.8, timeout = 0.0)
#lifts higher
right.move_to_joint_positions({'right_s0': 0.9978545025194616, 'right_s1': -0.6435049405179311, 'right_w0': 0.4222282118654601, 'right_w1': 1.1501020957170889, 'right_w2': -0.34131072530450457, 'right_e0': -0.5986360024722828, 'right_e1': 1.1953545289597087}
)
#moves to middle
right.move_to_joint_positions({'right_s0': 1.315005030414771, 'right_s1': -0.24236896448589537, 'right_w0': 0.705247667230319, 'right_w1': 1.1117525760199536, 'right_w2': -0.0030679615757708274, 'right_e0': -0.6768690226544388, 'right_e1': 0.7827136970185323}
)
#lowers sandwich
right.move_to_joint_positions({'right_s0': 1.3165390112026563, 'right_s1': 0.11543205428837738, 'right_w0': 0.7025632008515195, 'right_w1': 0.34092723010753323, 'right_w2': -0.1499466220157992, 'right_e0': -0.68568941218478, 'right_e1': 0.4437039428958559}
)
#sets sandwich on palte
right.move_to_joint_positions({'right_s0': 1.2958302705662033, 'right_s1': 0.20325245439481732, 'right_w0': 0.6952767921090638, 'right_w1': 0.22626216621309853, 'right_w2': -0.02339320701525256, 'right_e0': -0.6929758209272356, 'right_e1': 0.40151947122900705}
)
#opens gripper
rightg.open()
#lifts
right.move_to_joint_positions({'right_s0': 1.2693691019751798, 'right_s1': -0.13192234775814557, 'right_w0': 0.7911505913519021, 'right_w1': 0.9008302176857093, 'right_w2': -0.0030679615757708274, 'right_e0': -0.6929758209272356, 'right_e1': 0.66306319556347}
)
#lifts higher
right.move_to_joint_positions({'right_s0': 1.2693691019751798, 'right_s1': -0.13038836697026016, 'right_w0': 0.7926845721397876, 'right_w1': 0.9012137128826806, 'right_w2': -0.002684466378799474, 'right_e0': -0.693359316124207, 'right_e1': 0.66306319556347}
)
def arm_give_sandwichAB():
print "<give customer a sandwich A>"
#shifts gaze right
Head.set_pan(head, -0.55, speed = 0.3, timeout = 0.0)
#shifts right
right.move_to_joint_positions({'right_s0': 0.7033301912454623, 'right_s1': -1.1424321917776619, 'right_w0': 0.006902913545484362, 'right_w1': 0.2880048929254864, 'right_w2': -0.4111068511532909, 'right_e0': -0.2703641138648042, 'right_e1': 2.317461475297889}
)
#lowers
right.move_to_joint_positions({'right_s0': 0.34476218207724674, 'right_s1': -0.8279661302611521, 'right_w0': -0.16260196351585385, 'right_w1': -0.05829126993964572, 'right_w2': -0.2941408160770281, 'right_e0': -0.04716990922747647, 'right_e1': 2.2837138979644096})
#lowers to grab it
right.move_to_joint_positions({'right_s0': 0.34361169648633266, 'right_s1': -0.7359272829880272, 'right_w0': -0.16336895390979655, 'right_w1': -0.09702428483375242, 'right_w2': -0.2933738256830854, 'right_e0': -0.049854375606275945, 'right_e1': 2.2365439887369334})
#grippers close
rightg.close()
#lifts
right.move_to_joint_positions({'right_s0': 0.558752501987262, 'right_s1': -1.2156797743991905, 'right_w0': 0.1250194342126612, 'right_w1': 0.768140879533621, 'right_w2': -0.4183932598957466, 'right_e0': -0.23508255574343967, 'right_e1': 2.074709015615022})
#set gaze back to customer
head.set_pan(0.0, speed = 0.8, timeout = 0.0)
#moves to middle
right.move_to_joint_positions({'right_s0': 0.960271973216269, 'right_s1': -0.9759952762920945, 'right_w0': 0.08858739050038264, 'right_w1': 1.228335115899245, 'right_w2': 0.15953400194008302, 'right_e0': -0.049854375606275945, 'right_e1': 1.4829759266882236})
#lowers sandwich 0
right.move_to_joint_positions({'right_s0': 1.0618982004136777, 'right_s1': -0.6515583396543295, 'right_w0': 0.10584467436409355, 'right_w1': 0.6239466854723921, 'right_w2': 0.14879613642488512, 'right_e0': -0.05330583237901813, 'right_e1': 1.5194079704005024})
#lowers sandwich
right.move_to_joint_positions({'right_s0': 1.0515438300954512, 'right_s1': -0.5399612373356656, 'right_w0': 0.10277671278832272, 'right_w1': 0.6668981475331837, 'right_w2': 0.1499466220157992, 'right_e0': -0.05483981316690354, 'right_e1': 1.417014752809151})
#opens gripper
rightg.open()
#lifts
right.move_to_joint_positions({'right_s0': 1.023165185519571, 'right_s1': -0.7098496095939753, 'right_w0': 0.11159710231866385, 'right_w1': 0.8920098281553681, 'right_w2': 0.15186409800065595, 'right_e0': -0.05829126993964572, 'right_e1': 1.4262186375364634})
def arm_give_cookie():
print "<give customer a granola bar>"
leftg.open()
Head.set_pan(head, 0.45, speed = 1.0, timeout = 0.0) #looks at cookie section
left.move_to_joint_positions(neutral_left)
left.move_to_joint_positions({'left_w0': 0.029529130166794215, 'left_w1': 0.6051554208207958, 'left_w2': 3.049937301513174, 'left_e0': 0.08053399136398422, 'left_e1': 2.185155632342772, 'left_s0': -0.7328593214122564, 'left_s1': -1.113286556807839})
left.move_to_joint_positions({'left_w0': 0.11811652066717686, 'left_w1': 0.06289321230330196, 'left_w2': 3.0487868159222598, 'left_e0': 0.06442719309118737, 'left_e1': 2.106155621766673, 'left_s0': -0.6151262959420509, 'left_s1': -0.6450389213058165})
leftg.close()
left.move_to_joint_positions({'left_w0': -0.0030679615757708274, 'left_w1': 0.09433981845495294, 'left_w2': 3.049170311119231, 'left_e0': 0.10852914074289302, 'left_e1': 2.456286736601519, 'left_s0': -0.7083156288060898, 'left_s1': -1.1270923838988078})
#head turns to customer
head.set_pan(0.0, speed = 0.8, timeout = 0.0)
left.move_to_joint_positions({'left_w0': -0.2166747862888147, 'left_w1': 0.6715000898968398, 'left_w2': 2.7139955089662684, 'left_e0': 0.18484468494019235, 'left_e1': 1.3506700837331067, 'left_s0': -1.3146215352177997, 'left_s1': -0.4747670538505356})
left.move_to_joint_positions({'left_w0': -0.2718980946526896, 'left_w1': 0.5572185211993765, 'left_w2': 2.7231993936935806, 'left_e0': 0.20363594959178868, 'left_e1': 1.4296700943092056, 'left_s0': -1.2946797849752891, 'left_s1': -0.45942724597168144})
leftg.open()
#lifts
#left.move_to_joint_positions({'left_w0': -0.3804272353955826, 'left_w1': -0.6055389160177671, 'left_w2': 1.6241021591736817, 'left_e0': 0.10239321759135137, 'left_e1': 1.973849778811556, 'left_s0': -1.4273691231273775, 'left_s1': -0.5311408478053246})
#lifts more
left.move_to_joint_positions({'left_w0': -0.17679128580379394, 'left_w1': 0.890859342564454, 'left_w2': 2.9325877712399397, 'left_e0': 0.2151408055009293, 'left_e1': 1.4902623354306794, 'left_s0': -1.14473316295949, 'left_s1': -0.8003544760792146})
#move to neutral
left.move_to_joint_positions(neutral_left)
#close grippers
leftg.close()
class HeadPanNoDisplay:
def __init__(self):
# This checks if the head camera is open. If it isn't, this will
# close the left hand camera.
try:
CameraController('head_camera')
except AttributeError:
left_cam = CameraController('left_hand_camera')
left_cam.close()
head_cam = CameraController('head_camera')
head_cam.resolution = (1280, 800)
head_cam.open()
# Creating the head variable to mess with later
self.head = Head()
self.head.set_pan(angle=0.)
# Field of view
self.CENTER_X = int(1280 / 2)
self.FOV = pi / 3
# Range away from center you want the arm to stop within
self.FACE_RANGE = 50
self.FACE_CASCADE = cv2.CascadeClassifier(
'src/baxter_face_tracking_demos/src' +
'/haarcascade_frontalface_default.xml')
self.EYE_CASCADE = cv2.CascadeClassifier(
'src/baxter_face_tracking_demos/src/haarcascade_eye.xml')
self.subscription = rospy.Subscriber('/cameras/head_camera/image',
Image, self.send)
rospy.on_shutdown(self.leave_subs_n_pubs)
def leave_subs_n_pubs(self):
self.head.set_pan(angle=0.)
self.subscription.unregister()
def send(self, data):
# Getting the image
img = CvBridge().imgmsg_to_cv2(data, desired_encoding='bgr8')
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Finding the faces in the image
faces = self.FACE_CASCADE.detectMultiScale(
gray,
scaleFactor=1.25,
minNeighbors=4,
minSize=(10, 10),
flags=cv2.cv.CV_HAAR_SCALE_IMAGE)
known_face_x = False
# Make it an ridiculous value so it won't affect anything
dif_x = self.CENTER_X * 2
# Iterating through all faces
for (x, y, w, h) in faces:
roi_gray = gray[y:y + h, x:x + w]
# Finding eyes inside the face box
eyes = self.EYE_CASCADE.detectMultiScale(roi_gray,
scaleFactor=1.1,
minNeighbors=2)
for (ex, ey, ew, eh) in eyes:
temp_dif_x = (x + (w / 2)) - self.CENTER_X
# If the face is closer than the last one found, then set it
# as the face to go to
if temp_dif_x < dif_x:
known_face_x = True
dif_x = temp_dif_x
cur_pan = self.head.pan()
# If you found a face, the head is not moving, and that face is in a
# legal position, then turn the head to center that face.
if known_face_x and not self.head.panning() and abs(
(-1 * (dif_x * (self.FOV / 2) / self.CENTER_X) + cur_pan)) < \
(pi / 2):
if dif_x > self.FACE_RANGE:
self.head.set_pan(angle=cur_pan + -1 *
(dif_x * (self.FOV / 2)) / self.CENTER_X)
elif dif_x < (-1 * self.FACE_RANGE):
self.head.set_pan(angle=cur_pan + -1 *
(dif_x * (self.FOV / 2)) / self.CENTER_X)
class Baxter(object):
def __init__(self, calibrate_grippers=True):
self._baxter_state = RobotEnable()
self._left = Limb(LEFT)
self._right = Limb(RIGHT)
self._limbs = {LEFT: self._left, RIGHT: self._right}
self._head = Head()
self._left_gripper, self._right_gripper = Gripper(LEFT), Gripper(RIGHT)
if calibrate_grippers:
self.calibrate()
self._left_ikservice = IKService(LEFT)
self._right_ikservice = IKService(RIGHT)
def calibrate(self):
self._left_gripper.calibrate()
self._left_gripper_max = self._left_gripper.position()
self._right_gripper.calibrate()
self._right_gripper_max = self._right_gripper.position()
@property
def left_gripper_max(self):
return self._left_gripper_max
@property
def right_gripper_max(self):
return self._right_gripper_max
@property
def left_gripper(self):
return self._left_gripper.position()
@left_gripper.setter
def left_gripper(self, position):
self._left_gripper.command_position(position)
@property
def right_gripper(self):
return self._right_gripper.position()
@right_gripper.setter
def right_gripper(self, position):
self._right_gripper.command_position(position)
def set_left_joints(self, angles):
joints = self._left.joint_angles()
for joint, angle in angles.iteritems():
if angle:
joints[joint] = angle
self.enable_check()
self._left.set_joint_positions(joints)
def set_right_joints(self, angles):
joints = self._right.joint_angles()
for joint, angle in angles.iteritems():
if angle:
joints[joint] = angle
self.enable_check()
self._right.set_joint_positions(joints)
def reset_limb(self, side):
angles = {joint: 0.0 for joint in self._limbs[side].joint_angles()}
self.enable_check()
self._limbs[side].move_to_joint_positions(angles)
def enable_check(self):
# Sometimes robot is disabled due to another program resetting state
if not self._baxter_state.state().enabled:
self._baxter_state.enable()
@property
def joints(self):
joints = {
limb: joint.joint_angles()
for limb, joint in self._limbs.iteritems()
}
return joints
@property
def enabled(self):
return self._baxter_state.state().enabled
@property
def left_s0(self):
return self._left.joint_angle('left_s0')
@left_s0.setter
def left_s0(self, angle):
self.set_left_joints({'left_s0': angle})
@property
def left_s1(self):
return self._left.joint_angle('left_s1')
@left_s1.setter
def left_s1(self, angle):
self.set_left_joints({'left_s1': angle})
@property
def left_e0(self):
return self._left.joint_angle('left_e0')
@left_e0.setter
def left_e0(self, angle):
self.set_left_joints({'left_e0': angle})
@property
def left_e1(self):
return self._left.joint_angle('left_e1')
@left_e1.setter
def left_e1(self, angle):
self.set_left_joints({'left_e1': angle})
@property
def left_w0(self):
return self._left.joint_angle('left_w0')
@left_w0.setter
def left_w0(self, angle):
self.set_left_joints({'left_w0': angle})
@property
def left_w1(self):
return self._left.joint_angle('left_w1')
@left_w1.setter
def left_w1(self, angle):
self.set_left_joints({'left_w1': angle})
@property
def left_w2(self):
return self._left.joint_angle('left_w2')
@left_w2.setter
def left_w2(self, angle):
self.set_left_joints({'left_w2': angle})
@property
def right_s0(self):
return self._right.joint_angle('right_s0')
@right_s0.setter
def right_s0(self, angle):
self.set_right_joints({'right_s0': angle})
@property
def right_s1(self):
return self._right.joint_angle('right_s1')
@right_s1.setter
def right_s1(self, angle):
self.set_right_joints({'right_s1': angle})
@property
def right_e0(self):
return self._right.joint_angle('right_e0')
@right_e0.setter
def right_e0(self, angle):
self.set_right_joints({'right_e0': angle})
@property
def right_e1(self):
return self._right.joint_angle('right_e1')
@right_e1.setter
def right_e1(self, angle):
self.set_right_joints({'right_e1': angle})
@property
def right_w0(self):
return self._right.joint_angle('right_w0')
@right_w0.setter
def right_w0(self, angle):
self.set_right_joints({'right_w0': angle})
@property
def right_w1(self):
return self._right.joint_angle('right_w1')
@right_w1.setter
def right_w1(self, angle):
self.set_right_joints({'right_w1': angle})
@property
def right_w2(self):
return self._right.joint_angle('right_w2')
@right_w2.setter
def right_w2(self, angle):
self.set_right_joints({'right_w2': angle})
@property
def left_position(self):
return self._left.endpoint_pose()['position']
@property
def left_position_x(self):
return self.left_position.x
@left_position_x.setter
def left_position_x(self, point):
self.set_left_pose(position={'x': point})
@property
def left_position_y(self):
return self.left_position.y
@left_position_y.setter
def left_position_y(self, point):
self.set_left_pose(position={'y': point})
@property
def left_position_z(self):
return self.left_position.z
@left_position_z.setter
def left_position_z(self, point):
self.set_left_pose(position={'z': point})
@property
def left_orientation(self):
return self._left.endpoint_pose()['orientation']
@property
def left_orientation_x(self):
return self.left_orientation.x
@left_orientation_x.setter
def left_orientation_x(self, point):
self.set_left_pose(orientation={'x': point})
@property
def left_orientation_y(self):
return self.left_orientation.y
@left_orientation_y.setter
def left_orientation_y(self, point):
self.set_left_pose(orientation={'y': point})
@property
def left_orientation_z(self):
return self.left_orientation.z
@left_orientation_z.setter
def left_orientation_z(self, point):
self.set_left_pose(orientation={'z': point})
@property
def left_orientation_w(self):
return self.left_orientation.w
@left_orientation_w.setter
def left_orientation_w(self, point):
self.set_left_pose(orientation={'w': point})
@property
def right_position(self):
return self._right.endpoint_pose()['position']
@property
def right_orientation(self):
return self._right.endpoint_pose()['orientation']
def set_left_pose(self, position={}, orientation={}):
pos = {
'x': self.left_position_x,
'y': self.left_position_y,
'z': self.left_position_z,
}
for key, value in position.iteritems():
pos[key] = value
orient = {
'x': self.left_orientation_x,
'y': self.left_orientation_y,
'z': self.left_orientation_z,
'w': self.left_orientation_w,
}
for key, value in orientation.iteritems():
orient[key] = value
pos = self._left_ikservice.solve_position(
Pose(position=Point(**pos), orientation=Quaternion(**orient)))
if pos:
self.set_left_joints(pos)
else:
print 'nothing'
#print self.joints
@property
def right_position(self):
return self._right.endpoint_pose()['position']
@property
def right_position_x(self):
return self.right_position.x
@right_position_x.setter
def right_position_x(self, point):
self.set_right_pose(position={'x': point})
@property
def right_position_y(self):
return self.right_position.y
@right_position_y.setter
def right_position_y(self, point):
self.set_right_pose(position={'y': point})
@property
def right_position_z(self):
return self.right_position.z
@right_position_z.setter
def right_position_z(self, point):
self.set_right_pose(position={'z': point})
@property
def right_orientation(self):
return self._right.endpoint_pose()['orientation']
@property
def right_orientation_x(self):
return self.right_orientation.x
@right_orientation_x.setter
def right_orientation_x(self, point):
self.set_right_pose(orientation={'x': point})
@property
def right_orientation_y(self):
return self.right_orientation.y
@right_orientation_y.setter
def right_orientation_y(self, point):
self.set_right_pose(orientation={'y': point})
@property
def right_orientation_z(self):
return self.right_orientation.z
@right_orientation_z.setter
def right_orientation_z(self, point):
self.set_right_pose(orientation={'z': point})
@property
def right_orientation_w(self):
return self.right_orientation.w
@right_orientation_w.setter
def right_orientation_w(self, point):
self.set_right_pose(orientation={'w': point})
@property
def right_position(self):
return self._right.endpoint_pose()['position']
@property
def right_orientation(self):
return self._right.endpoint_pose()['orientation']
def set_right_pose(self, position={}, orientation={}):
pos = {
'x': self.right_position_x,
'y': self.right_position_y,
'z': self.right_position_z,
}
for key, value in position.iteritems():
pos[key] = value
orient = {
'x': self.right_orientation_x,
'y': self.right_orientation_y,
'z': self.right_orientation_z,
'w': self.right_orientation_w,
}
for key, value in orientation.iteritems():
orient[key] = value
pos = self._right_ikservice.solve_position(
Pose(position=Point(**pos), orientation=Quaternion(**orient)))
if pos:
self.set_right_joints(pos)
@property
def head_position(self):
return self._head.pan()
@head_position.setter
def head_position(self, position):
self._head.set_pan(position)
time.sleep(1)
rightg = Gripper('right')
rightg.set_holding_force(50)
leftg = Gripper('left')
right = Limb('right')
left = Limb('left')
head = Head()
neutral_right = {'right_s0': 0.476301034638421, 'right_s1': -0.5606699779721187, 'right_w0': 0.07094661143970038, 'right_w1': 0.7037136864424336, 'right_w2': -0.28033498898605935, 'right_e0': -0.16566992509162468, 'right_e1': 1.3077186216723151}
neutral_left = {'left_w0': -0.15339807878854136, 'left_w1': 0.34552917247118947, 'left_w2': 3.0158062289827234, 'left_e0': 0.18676216092504913, 'left_e1': 1.5715633171886063, 'left_s0': -0.5836796897904, 'left_s1': -0.6845389265938658}
right.move_to_joint_positions(neutral_right)
left.move_to_joint_positions(neutral_left)
head.set_pan(0.0, speed = 0.8, timeout = 0.0)
Gripper.calibrate(rightg)
Gripper.calibrate(leftg)
# neutral_right = {'right_s0': 0.476301034638421, 'right_s1': -0.5606699779721187, 'right_w0': 0.07094661143970038, 'right_w1': 0.7037136864424336, 'right_w2': -0.28033498898605935, 'right_e0': -0.16566992509162468, 'right_e1': 1.3077186216723151}
# neutral_left = {'left_w0': -0.15339807878854136, 'left_w1': 0.34552917247118947, 'left_w2': 3.0158062289827234, 'left_e0': 0.18676216092504913, 'left_e1': 1.5715633171886063, 'left_s0': -0.5836796897904, 'left_s1': -0.6845389265938658}
# right.move_to_joint_positions(neutral_right)
# left.move_to_joint_positions(neutral_left)
def wait():
raw_input("Press Enter to continue...")
def send_voice(msg):
print "Baxter: %s" % index_to_string[msg]
def arm_give_sandwichBC():
print "<give customer a sandwich B,C>"
Head.set_pan(head, -0.45, speed = 1.0, timeout = 0.0)
def arm_give_condimentC():
print "<give customer a condimentBC>"
#shifts gaze back right
Head.set_pan(head, -0.55, speed=0.3, timeout=0.0)
#move to ketchup
right.move_to_joint_positions({
'right_s0': 0.08168447695489828,
'right_s1': -0.9131020639887926,
'right_w0': -0.38924762492592374,
'right_w1': 0.19213109368264808,
'right_w2': -0.38656315854712425,
'right_e0': 0.1284708909854034,
'right_e1': 2.1422041702819805
})
#move to mustard
right.move_to_joint_positions({
'right_s0': 0.3861796633501529,
'right_s1': -0.9422476989586154,
'right_w0': -0.03029612056073692,
'right_w1': 0.35128160042575973,
'right_w2': -0.6017039640480536,
'right_e0': -0.10737865515197896,
'right_e1': 1.9803691971600692
})
#START HERE#
#move towards ketchup
right.move_to_joint_positions({
'right_s0': 0.09357282806101024,
'right_s1': -0.9641069251859825,
'right_w0': -0.2741990658345177,
'right_w1': 0.05790777474267437,
'right_w2': -0.34476218207724674,
'right_e0': 0.04601942363656241,
'right_e1': 2.2607041861461283
})
#move lower
right.move_to_joint_positions({
'right_s0': 0.1457281748491143,
'right_s1': -0.7673738891396782,
'right_w0': -0.13077186216723152,
'right_w1': 0.13077186216723152,
'right_w2': -0.4506068564413403,
'right_e0': 0.04908738521233324,
'right_e1': 2.052082798993712
})
#lowers down on ketchup #changed
right.move_to_joint_positions({
'right_s0': 0.13614079492483047,
'right_s1': -0.6818544602150665,
'right_w0': -0.07861651537912745,
'right_w1': 0.07439806821244256,
'right_w2': -0.4463884092746554,
'right_e0': 0.04563592843959106,
'right_e1': 2.021786678432975
})
#grippers close
rightg.close()
#lifts
right.move_to_joint_positions({
'right_s0': 0.3271214030165645,
'right_s1': -1.071869075534933,
'right_w0': -0.03911651009107805,
'right_w1': 0.14304370847031483,
'right_w2': -0.5997864880631968,
'right_e0': -0.08820389530341129,
'right_e1': 2.2844808883583525
})
#shifts gaze back center
Head.set_pan(head, 0.0, speed=0.3, timeout=0.0)
#lifts more
right.move_to_joint_positions({
'right_s0': 0.4437039428958559,
'right_s1': -1.2647671596115235,
'right_w0': -0.037582529303192634,
'right_w1': 0.7715923363063631,
'right_w2': -0.4049709280017492,
'right_e0': 0.06404369789421602,
'right_e1': 1.9308983167507645
})
#moves over
right.move_to_joint_positions({
'right_s0': 0.9384127469889019,
'right_s1': -0.9955535313376335,
'right_w0': -0.05675728915176031,
'right_w1': 1.021247709534714,
'right_w2': 0.2822524649709161,
'right_e0': 0.2231942046373277,
'right_e1': 1.5132720472489607
})
#sets down on plate
right.move_to_joint_positions({
'right_s0': 1.2183642407779898,
'right_s1': -0.47975249141116316,
'right_w0': 0.04141748127290617,
'right_w1': 0.6718835850938112,
'right_w2': 0.34131072530450457,
'right_e0': -0.046402918833533764,
'right_e1': 1.318456487187513
})
#opens gripper
rightg.open()
#lifts
right.move_to_joint_positions({
'right_s0': 1.1984224905354794,
'right_s1': -0.7957525337155584,
'right_w0': 0.05138835639416136,
'right_w1': 0.7182865039273449,
'right_w2': 0.10239321759135137,
'right_e0': -0.0839854481367264,
'right_e1': 1.5953400194008303
})
#move back to neutral
right.move_to_joint_positions(neutral_right)
#close gripper
rightg.close()
def arm_give_sandwichC():
print "<give customer a sandwich B,C>"
Head.set_pan(head, -0.45, speed=1.0, timeout=0.0)
#move right
right.move_to_joint_positions({
'right_s0': 0.40535442319872056,
'right_s1': -1.0277671278832272,
'right_w0': 0.1277039005914607,
'right_w1': 0.42299520225940285,
'right_w2': -0.7907670961549308,
'right_e0': -0.14802914603094242,
'right_e1': 2.1705828148578603
})
#shifts gaze left
Head.set_pan(head, -0.25, speed=0.3, timeout=0.0)
#shifts left
right.move_to_joint_positions({
'right_s0': 0.7915340865488735,
'right_s1': -1.090276844989558,
'right_w0': 0.18637866572807776,
'right_w1': 0.23853401251618184,
'right_w2': -0.40727189918357737,
'right_e0': -0.16336895390979655,
'right_e1': 2.3887915819345604
})
#shifts gaze back right
Head.set_pan(head, -0.55, speed=0.3, timeout=0.0)
#shifts back right
right.move_to_joint_positions({
'right_s0': 0.40535442319872056,
'right_s1': -1.0277671278832272,
'right_w0': 0.1277039005914607,
'right_w1': 0.42299520225940285,
'right_w2': -0.7907670961549308,
'right_e0': -0.14802914603094242,
'right_e1': 2.1705828148578603
})
#shifts gaze left
Head.set_pan(head, -0.25, speed=0.3, timeout=0.0)
#shifts left
right.move_to_joint_positions({
'right_s0': 0.7915340865488735,
'right_s1': -1.090276844989558,
'right_w0': 0.18637866572807776,
'right_w1': 0.23853401251618184,
'right_w2': -0.40727189918357737,
'right_e0': -0.16336895390979655,
'right_e1': 2.3887915819345604
})
#shifts gaze back right
Head.set_pan(head, -0.55, speed=0.3, timeout=0.0)
#shifts back right
right.move_to_joint_positions({
'right_s0': 0.4935583185021319,
'right_s1': -0.9790632378678653,
'right_w0': 0.0030679615757708274,
'right_w1': 0.28877188331942916,
'right_w2': -0.49816026086578813,
'right_e0': -0.15033011721277054,
'right_e1': 2.216602238494423
})
send_voice(V_WARN)
wait()
#lowers
right.move_to_joint_positions({
'right_s0': 0.43526704856248616,
'right_s1': -0.8417719573521208,
'right_w0': -0.2703641138648042,
'right_w1': 0.10162622719740866,
'right_w2': -0.28685440733457235,
'right_e0': -0.07439806821244256,
'right_e1': 2.1870731083276285
})
#lowers to grab it
right.move_to_joint_positions({
'right_s0': 0.42529617344123094,
'right_s1': -0.6699661091089545,
'right_w0': -0.2607767339405203,
'right_w1': 0.03566505331833587,
'right_w2': -0.28033498898605935,
'right_e0': -0.06864564025787226,
'right_e1': 2.085063385933249
})
#grippers close
rightg.close()
#lifts
right.move_to_joint_positions({
'right_s0': 0.558752501987262,
'right_s1': -1.2156797743991905,
'right_w0': 0.1250194342126612,
'right_w1': 0.768140879533621,
'right_w2': -0.4183932598957466,
'right_e0': -0.23508255574343967,
'right_e1': 2.074709015615022
})
#set gaze back to customer
head.set_pan(0.0, speed=0.8, timeout=0.0)
#moves to middle
right.move_to_joint_positions({
'right_s0': 0.960271973216269,
'right_s1': -0.9759952762920945,
'right_w0': 0.08858739050038264,
'right_w1': 1.228335115899245,
'right_w2': 0.15953400194008302,
'right_e0': -0.049854375606275945,
'right_e1': 1.4829759266882236
})
#lowers sandwich 0
right.move_to_joint_positions({
'right_s0': 1.0618982004136777,
'right_s1': -0.6515583396543295,
'right_w0': 0.10584467436409355,
'right_w1': 0.6239466854723921,
'right_w2': 0.14879613642488512,
'right_e0': -0.05330583237901813,
'right_e1': 1.5194079704005024
})
#lowers sandwich
right.move_to_joint_positions({
'right_s0': 1.0937283017623,
'right_s1': -0.49279132810818915,
'right_w0': 0.09165535207615347,
'right_w1': 0.5472476460781214,
'right_w2': 0.17487380981893716,
'right_e0': -0.04295146206079158,
'right_e1': 1.4135632960364088
})
#opens gripper
rightg.open()
#lifts
right.move_to_joint_positions({
'right_s0': 1.023165185519571,
'right_s1': -0.7098496095939753,
'right_w0': 0.11159710231866385,
'right_w1': 0.8920098281553681,
'right_w2': 0.15186409800065595,
'right_e0': -0.05829126993964572,
'right_e1': 1.4262186375364634
})
def arm_give_cookie():
print "<give customer a granola bar>"
leftg.open()
Head.set_pan(head, 0.45, speed=1.0, timeout=0.0) #looks at cookie section
left.move_to_joint_positions(neutral_left)
left.move_to_joint_positions({
'left_w0': 0.029529130166794215,
'left_w1': 0.6051554208207958,
'left_w2': 3.049937301513174,
'left_e0': 0.08053399136398422,
'left_e1': 2.185155632342772,
'left_s0': -0.7328593214122564,
'left_s1': -1.113286556807839
})
left.move_to_joint_positions({
'left_w0': 0.21552430069790063,
'left_w1': 0.10661166475803625,
'left_w2': 3.050320796710145,
'left_e0': 0.006135923151541655,
'left_e1': 2.0455633806451994,
'left_s0': -0.5518495884417776,
'left_s1': -0.6258641614572488
})
leftg.close()
left.move_to_joint_positions({
'left_w0': -0.0030679615757708274,
'left_w1': 0.09433981845495294,
'left_w2': 3.049170311119231,
'left_e0': 0.10852914074289302,
'left_e1': 2.456286736601519,
'left_s0': -0.7083156288060898,
'left_s1': -1.1270923838988078
})
#head turns to customer
head.set_pan(0.0, speed=0.8, timeout=0.0)
left.move_to_joint_positions({
'left_w0': -0.2166747862888147,
'left_w1': 0.6715000898968398,
'left_w2': 2.7139955089662684,
'left_e0': 0.18484468494019235,
'left_e1': 1.3506700837331067,
'left_s0': -1.3146215352177997,
'left_s1': -0.4747670538505356
})
left.move_to_joint_positions({
'left_w0': -0.22204371904641365,
'left_w1': 0.6458059116997592,
'left_w2': 2.715912984951125,
'left_e0': 0.19251458887961942,
'left_e1': 1.3219079439602552,
'left_s0': -1.3192234775814558,
'left_s1': -0.4099563655623768
})
leftg.open()
#lifts
#left.move_to_joint_positions({'left_w0': -0.3804272353955826, 'left_w1': -0.6055389160177671, 'left_w2': 1.6241021591736817, 'left_e0': 0.10239321759135137, 'left_e1': 1.973849778811556, 'left_s0': -1.4273691231273775, 'left_s1': -0.5311408478053246})
#lifts more
left.move_to_joint_positions({
'left_w0': -0.17679128580379394,
'left_w1': 0.890859342564454,
'left_w2': 2.9325877712399397,
'left_e0': 0.2151408055009293,
'left_e1': 1.4902623354306794,
'left_s0': -1.14473316295949,
'left_s1': -0.8003544760792146
})
#move to neutral
left.move_to_joint_positions(neutral_left)
#close grippers
leftg.close()
'right_e1': 1.3077186216723151
}
neutral_left = {
'left_w0': -0.15339807878854136,
'left_w1': 0.34552917247118947,
'left_w2': 3.0158062289827234,
'left_e0': 0.18676216092504913,
'left_e1': 1.5715633171886063,
'left_s0': -0.5836796897904,
'left_s1': -0.6845389265938658
}
right.move_to_joint_positions(neutral_right)
left.move_to_joint_positions(neutral_left)
head.set_pan(0.0, speed=0.8, timeout=0.0)
Gripper.calibrate(rightg)
Gripper.calibrate(leftg)
def wait():
raw_input("Press Enter to continue...")
def send_voice(msg):
print "Baxter: %s" % index_to_string[msg]
voice_pub.publish(data=msg)
def arm_give_cookie():
def arm_give_cookie():
print "<give customer a cookie>"
Head.set_pan(head, 0.35, speed = 1.0, timeout = 0.0) #looks at cookie section
#individually wrapped triangle sandwich will not do
#!/usr/bin/env python
import rospy, baxter_interface
from baxter_interface import Gripper, Head, Limb
rospy.init_node('deli_Baxter')
rightg = Gripper('right')
rightg.set_holding_force(100)
leftg = Gripper('left')
right = Limb('right')
left = Limb('left')
head = Head()
head.set_pan(0.0, speed=0.8, timeout=0.0)
Gripper.calibrate(rightg)
Gripper.calibrate(leftg)
neutral_right = {
'right_s0': 0.5971020216843973,
'right_s1': -0.4237621926533455,
'right_w0': 0.4226117070624315,
'right_w1': 0.8471408901097197,
'right_w2': -0.9725438195193523,
'right_e0': -0.40727189918357737,
'right_e1': 1.161990446823201
}
neutral_left = {
#individually wrapped triangle sandwich will not do
#!/usr/bin/env python
import rospy, baxter_interface
from baxter_interface import Gripper, Head, Limb
rospy.init_node('deli_Baxter')
rightg = Gripper('right')
rightg.set_holding_force(100)
leftg = Gripper('left')
right = Limb('right')
left = Limb('left')
head = Head()
head.set_pan(0.0, speed = 0.8, timeout = 0.0)
Gripper.calibrate(rightg)
Gripper.calibrate(leftg)
neutral_right ={'right_s0': 0.5971020216843973, 'right_s1': -0.4237621926533455, 'right_w0': 0.4226117070624315, 'right_w1': 0.8471408901097197, 'right_w2': -0.9725438195193523, 'right_e0': -0.40727189918357737, 'right_e1': 1.161990446823201}
neutral_left = {'left_w0': -0.3006602344255411, 'left_w1': 0.5549175500175484, 'left_w2': 3.050704291907117, 'left_e0': 0.5161845351234418, 'left_e1': 1.1984224905354794, 'left_s0': -0.8904758473674826, 'left_s1': -0.38387869216832476}
right.move_to_joint_positions(neutral_right)
left.move_to_joint_positions(neutral_left)
####PART 1 ("what would you like today")
'left_e1': 0.7850146682003605,
'left_s0': -0.73,
'left_s1': -0.6293156182299909
}
down3point2 = {
'left_w0': 0.13077186216723152,
'left_w1': 1.1,
'left_w2': 0.0015339807878854137,
'left_e0': -0.16605342028859604,
'left_e1': 0.7,
'left_s0': -0.62,
'left_s1': -0.28
}
Gripper.calibrate(leftg)
leftg.open()
time.sleep(1)
left.move_to_joint_positions(above3point2, timeout=4)
left.move_to_joint_positions(down3point2, timeout=4)
leftg.open()
leftg.close()
time.sleep(1)
left.move_to_joint_positions(above3point2)
leftg.open()
head.set_pan(0, speed=100)
pickUp3Point2()
dab()
left.move_to_joint_positions({'left_w0': -1.4741555371578825, 'left_w1': 0.4049709280017492, 'left_w2': 3.0158062289827234, 'left_e0': 0.3324903357741634, 'left_e1': 1.2421409429902137, 'left_s0': -0.6270146470481629, 'left_s1': -0.27228158984966094}
)
#grippers close
leftg.close()
#lifts a little bit
left.move_to_joint_positions({'left_w0': -1.474539032354854, 'left_w1': 0.3278883934105072, 'left_w2': 3.0165732193766663, 'left_e0': 0.33325732616810616, 'left_e1': 1.2950632801722606, 'left_s0': -0.6645971763513555, 'left_s1': -0.362402961137929}
)
#lifts more
left.move_to_joint_positions({'left_w0': -1.476840003536682, 'left_w1': 0.26307770512234846, 'left_w2': 3.0108207914220957, 'left_e0': 0.35856800916821546, 'left_e1': 1.4848934026730805, 'left_s0': -0.8279661302611521, 'left_s1': -0.7827136970185323}
)
#head turns to customer
head.set_pan(0.0, speed = 0.8, timeout = 0.0)
#lowers
left.move_to_joint_positions({'left_w0': -1.5079031144913617, 'left_w1': 0.2067039111675595, 'left_w2': 3.008519820240268, 'left_e0': 0.3466796580621035, 'left_e1': 0.8847234194129123, 'left_s0': -0.9955535313376335, 'left_s1': -0.11543205428837738}
)
#sets on table
left.move_to_joint_positions({'left_w0': -1.5082866096883332, 'left_w1': 0.2059369207736168, 'left_w2': 3.0158062289827234, 'left_e0': 0.33977674451661916, 'left_e1': 0.9265243958827899, 'left_s0': -0.9963205217315763, 'left_s1': -0.07017962104575767}
)
#opens gripper
leftg.open()
#lifts
left.move_to_joint_positions({'left_w0': -1.5293788455217574, 'left_w1': 0.23431556534949696, 'left_w2': 3.017340209770609, 'left_e0': 0.381961216183468, 'left_e1': 1.3245924103390547, 'left_s0': -1.0672671331712766, 'left_s1': -0.3508981052287884}
)