def larm_joints(in_degrees = True):
if in_degrees:
gain = 180.0/math.pi
else:
gain = 1.0
lajd = PyPR2.getArmJointPositions(True)
return(gain*gen_larm_joint_vector_from_dic(lajd))
def twist_forearm(angle = 180.0, is_left_arm = True):
if is_left_arm:
joint_name = 'l_forearm_roll_joint'
else:
joint_name = 'r_forearm_roll_joint'
jd = PyPR2.getArmJointPositions(is_left_arm)
jd[joint_name] = jd[joint_name] + angle*math.pi/180.0
PyPR2.moveArmWithJointPos(**jd)
def rarm_elbow_flex_joint(in_degrees = True):
'''
returns the current angle of the shoulder pan joint of the right arm
'''
if in_degrees:
gain = 180.0/math.pi
else:
gain = 1.0
rajd = PyPR2.getArmJointPositions(False)
return(gain*rajd['r_elbow_flex_joint'])
def larm_elbow_flex_joint(in_degrees = True):
'''
returns the current angle of the shoulder pan joint of the left arm
'''
if in_degrees:
gain = 180.0/math.pi
else:
gain = 1.0
lajd = PyPR2.getArmJointPositions(True)
return(gain*lajd['l_elbow_flex_joint'])
def larm_shoulder_lift_joint(in_degrees = True):
'''
returns the current angle of the shoulder lift joint of the left arm
'''
if in_degrees:
gain = 180.0/math.pi
else:
gain = 1.0
rajd = PyPR2.getArmJointPositions(True)
return(gain*rajd['l_shoulder_lift_joint'])
def play(self):
if not self.action_finishes:
print "still playing action '%s'." % self.name
return
myaction = PyPR2.getArmJointPositions(self.isleft)
myaction['time_to_reach'] = 1.0
rot = 0.0
if self.clockwise:
rot = math.pi / 2.0
else:
rot = -math.pi / 2.0
if self.isleft == True:
myaction[
'l_wrist_roll_joint'] = myaction['l_wrist_roll_joint'] + rot
else:
myaction[
'r_wrist_roll_joint'] = myaction['r_wrist_roll_joint'] + rot
PyPR2.moveArmWithJointPos(**myaction)
self.action_finishes = False
def finishsArmAction(self, is_left):
self.count = self.count + 1
if self.count == self.revolution * 4:
self.count = 0
self.action_finishes = True
return
myaction = PyPR2.getArmJointPositions(self.isleft)
myaction['time_to_reach'] = 1.0
rot = 0.0
if self.clockwise:
rot = math.pi / 2.0
else:
rot = -math.pi / 2.0
if self.isleft == True:
myaction[
'l_wrist_roll_joint'] = myaction['l_wrist_roll_joint'] + rot
else:
myaction[
'r_wrist_roll_joint'] = myaction['r_wrist_roll_joint'] + rot
PyPR2.moveArmWithJointPos(**myaction)
def twist_rg(angle = 180.0):
jd = PyPR2.getArmJointPositions(False)
jd['r_wrist_roll_joint'] = jd['r_wrist_roll_joint'] + angle*math.pi/180.0
PyPR2.moveArmWithJointPos(**jd)