initLeft = raw_input("Initialize left arm joint angles? [y/n]")
if initLeft == 'y':
print "Initializing left arm joint angles: "
self.setPostureGoal(self.initialJointAnglesSideFacingFowardLEFT, 7)
raw_input("Press Enter to continue")
initRight = raw_input("Initialize right arm joint angles? [y/n]")
if initRight == 'y':
print "Initializing right arm joint angles: "
self.setPostureGoalRight(self.initialJointAnglesSideOfBodyRIGHT, 7)
raw_input("Press Enter to continue")
if __name__ == '__main__':
import optparse
p = optparse.OptionParser()
haptic_mpc_util.initialiseOptParser(p)
opt = haptic_mpc_util.getValidInput(p)
# Initial variables
d_robot = 'pr2'
controller = 'static'
#controller = 'actionlib'
#arm = 'l'
#try:
#arm = opt.arm1 #added/changed due to new launch file controlling both arms (arm1, arm2)
#except:
#arm = opt.arm
arm = 'l'
def setPostureGoalRight(self, msg):
try:
self.setPostureGoal(msg.angles, msg.timeout)
outputString = "Set right arm joint angles: " + msg.angles + "\n"
print outputString
return "Worked"
except:
print "Could not set right arm joint angles"
return "Didn't work"
if __name__ == '__main__':
import optparse
p = optparse.OptionParser()
haptic_mpc_util.initialiseOptParser(p)
opt = haptic_mpc_util.getValidInput(p)
# Initial variables
d_robot = 'pr2'
controller = 'static'
#controller = 'actionlib'
#arm = 'r'
try:
arm = opt.arm2
except:
arm = opt.arm
arm = 'r'
rospy.init_node('arm_reacher_helper')