def head(x):
head = baxter_interface.Head()
y = head.pan()
if x == 'Nod':
head.command_nod()
print ('\x1b[1;32m' + 'Nod action successfully completed.' + '\x1b[0m')
if x == 'Forward':
head.set_pan(0.0, speed = 0.05, timeout=0)
print ('\x1b[1;32m'
+ 'Look action successfully completed: Theta = %s.\x1b[0m' %y)
if x == 'Left':
if y <= 1.2:
head.set_pan(y + 0.25, speed=0.05, timeout=0)
print ('\x1b[1;32m'
+ 'Look action started successfully: Theta = %s.\x1b[0m' %y)
else:
print ('\x1b[0;31m'
+ 'Can not move further left; extreme reached.'
+ '\x1b[0m')
if x == 'Right':
if y >= -1.2:
head.set_pan(y - 0.25, speed=0.05, timeout=0)
print ('\x1b[1;32m'
+ 'Look action started successfully: Theta = %s.\x1b[0m' %y)
else:
print ('\x1b[0;31m'
+ 'Can not move further right; extreme reached.'
+ '\x1b[0m')
def start_test(self):
"""
Runs Head Smoke Test.
"""
try:
print "Enabling robot..."
self._rs.enable()
print "Test: Moving Head to Neutral Location"
head = baxter_interface.Head()
head.set_pan(0.0, 0.05)
print "Test: Pan Head"
head.set_pan(1.0, 0.05)
head.set_pan(-1.0, 0.05)
head.set_pan(0.0, 0.05)
print "Test: Nod Head"
for _ in xrange(3):
head.command_nod()
print "Test: Display Image on Screen - 5 seconds"
image_path = (self._rp.get_path('baxter_tools') + '/share/images')
img = cv2.imread(image_path + '/baxterworking.png')
msg = cv_bridge.CvBridge().cv2_to_imgmsg(img)
pub = rospy.Publisher('/robot/xdisplay',
Image,
latch=True,
queue_size=10)
pub.publish(msg)
rospy.sleep(5.0)
img = cv2.imread(image_path + '/researchsdk.png')
msg = cv_bridge.CvBridge().cv2_to_imgmsg(img)
pub.publish(msg)
print "Disabling robot..."
self._rs.disable()
self.result[0] = True
except Exception:
self.return_failure(traceback.format_exc())
def __init__(self, limbs, tracking_limb):
# Initializing connection to Baxter Head Control
rospy.loginfo('Starting Head-Arm Movement Tracker')
rospy.loginfo('Initializing connection to Baxter head')
self._done = False
self._head = baxter_interface.Head()
rospy.loginfo('Getting Robot State')
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
rospy.loginfo('Enabling Robot')
self._rs.enable()
# Initializing arm movement variables
self._track_limb = tracking_limb
self._endpoint_topics = dict()
self._endpoint_pos = dict()
for limb in limbs:
self._endpoint_topics[
limb] = '/robot/limb/' + limb + '/endpoint_state'
self._endpoint_listeners = dict()
self._goal = dict()
self.head_neutral()
self.timer = rospy.Timer(rospy.Duration(self.TIMER_PERIOD),
self.update_tracking_target(self._track_limb))
rospy.loginfo('Running Head-Arm Movement Tracker. Ctrl-C to quit')
def __init__(self, directory):
self.fnames = [fname for fname in glob.glob("%s/*" % directory)]
self.fnames.sort()
self.images = [cv2.imread(path) for path in self.fnames]
self.animation_timer = None
self.current_value = 0
self.current_idx = 0
self.set_velocity(1 / 20.0)
self.current_target = None
self._head = baxter_interface.Head()
self._head.set_pan(0.0)
self.listener = tf.TransformListener()
self.timer = rospy.Timer(rospy.Duration(self.velocity), self.timer_cb)
self.image_publisher = rospy.Publisher("/robot/xdisplay",
Image,
queue_size=10)
self.valuey_publisher = rospy.Publisher("/eyeGaze/valuey/state",
Int32,
queue_size=10)
self.targety_publisher = rospy.Publisher("/eyeGaze/targety/state",
Int32,
queue_size=10)
self.valuex_subscriber = rospy.Subscriber("/eyeGaze/Point/command2",
Pose,
self.set_value,
queue_size=1)
self.set_valuey(49)
def __init__(self, arm):
self.limb = arm
self.limb_interface = baxter_interface.Limb(
self.limb) #Declares the limb
self.head = baxter_interface.Head() #Declares the head
self.gripper = baxter_interface.Gripper(
self.limb) #Declares the gripper
self.camera = baxter_interface.CameraController('right_hand_camera')
self.camera.open()
self.camera.resolution = self.camera.MODES[0]
self.pub = rospy.Publisher(
'/robot/xdisplay', Image, latch=True,
queue_size=10) #Publisher for changing the screen display
#Declares the other arm (The opposite arm to the one passed as argument. If you pass left, declares right as the other arm and vice versa)
if arm == "left":
self.other_limb = "right"
else:
self.other_limb = "left"
self.other_limb_interface = baxter_interface.Limb(self.other_limb)
self.limb_interface.set_joint_position_speed(
0.5) #Sets the speed for the args arm. {0.0, 1.0}
self.other_limb_interface.set_joint_position_speed(
0.5) #Sets the speed for the other arm {0.0, 1.0}
self.angles = self.limb_interface.joint_angles(
) #Stores all the joint angles
def __init__(self, limb):
self._rp = rospkg.RosPack()
self._config_path = self._rp.get_path('baxter_mill') + '/config/'
self._config_file_path = self._config_path + limb + '_positions.config'
self._images_path = self._rp.get_path('learn_play') + '/share/images/'
self._good_face_path = self._images_path + "good_face.jpg"
self._angry_face_path = self._images_path + "angry_face.jpg"
self._cheeky_face_path = self._images_path + "cheeky_face.jpg"
self._limb = limb
self._baxter_limb = baxter_interface.Limb(self._limb)
self._baxter_gripper = baxter_interface.Gripper(self._limb)
self._baxter_head = baxter_interface.Head()
self._baxter_head.set_pan(0.0)
print "Calibrating gripper..."
self._baxter_gripper.calibrate()
self._baxter_gripper.set_holding_force(50.0)
self.acceptable = {'left': ['a1', 'a4', 'a7', 'b2', 'b4', 'b6', 'c3',
'c4', 'c5', 'd1', 'd2', 'd3', 'd5', 'd6',
'd7', 'e3', 'e4', 'e5', 'f2', 'f4', 'f6',
'g1', 'g4', 'g7'],
'positions' : ['p1','p2', 'p3', 'p4', 'p5', 'p6',
'p7', 'p8', 'p9']}
self._mill_pos = {}
self._picking_pos = {}
self.picked = False
self._is_pickable = False
self._nodded = False
if (not self._check_config()):
exit(0)
self._read_config(self._config_file_path)
def start_test(self):
"""Runs MoveArms Smoke Test
"""
try:
print("Enabling robot...")
self._rs.enable()
print("Test: Moving Head to Neutral Location.")
head = baxter_interface.Head()
head.set_pan(0.0, 5.0)
print("Test: Pan Head")
head.set_pan(1.0, 5.0)
head.set_pan(-1.0, 5.0)
head.set_pan(0.0, 5.0)
print("Test: Nod Head")
for x in range(3):
head.command_nod()
print("Test: Display Image on Screen - 5 seconds.")
image_path = os.path.dirname(os.path.abspath(__file__))
img = cv.LoadImage(image_path + '/baxterworking.png')
msg = cv_bridge.CvBridge().cv_to_imgmsg(img)
pub = rospy.Publisher('/sdk/xdisplay', Image, latch=True)
pub.publish(msg)
rospy.sleep(5.0)
img = cv.LoadImage(image_path + '/researchsdk.png')
msg = cv_bridge.CvBridge().cv_to_imgmsg(img)
pub.publish(msg)
print("Disabling robot...")
self._rs.disable()
self.result[0] = True
except:
self.return_failure(traceback.format_exc())
def __init__(self):
self._head = baxter_interface.Head()
self._PoseStamped = PoseStamped()
self.head_pose = Quaternion()
self.Oculus_head_sub = rospy.Subscriber('/Oculus_head_move',
PoseStamped,
self.callback_head_pose)
def Init(self):
self.limbs= [None,None]
self.limbs[RIGHT]= baxter_interface.Limb(LRTostr(RIGHT))
self.limbs[LEFT]= baxter_interface.Limb(LRTostr(LEFT))
self.joint_names= [[],[]]
#self.joint_names[RIGHT]= ['right_'+joint for joint in ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']]
#self.joint_names[LEFT]= ['left_' +joint for joint in ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']]
self.joint_names[RIGHT]= self.limbs[RIGHT].joint_names()
self.joint_names[LEFT]= self.limbs[LEFT].joint_names()
self.head= baxter_interface.Head()
self.client= [None,None]
self.client[RIGHT]= actionlib.SimpleActionClient('/robot/limb/%s/follow_joint_trajectory'%LRTostr(RIGHT), control_msgs.msg.FollowJointTrajectoryAction)
self.client[LEFT]= actionlib.SimpleActionClient('/robot/limb/%s/follow_joint_trajectory'%LRTostr(LEFT), control_msgs.msg.FollowJointTrajectoryAction)
def WaitClient(c):
if not c.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Exiting - Joint Trajectory Action Server Not Found')
rospy.logerr('Run: rosrun baxter_interface joint_trajectory_action_server.py')
rospy.signal_shutdown('Action Server not found')
sys.exit(1)
WaitClient(self.client[RIGHT])
WaitClient(self.client[LEFT])
self.default_face= 'baymax.jpg'
self.ChangeFace(self.default_face)
self.bxjs= BxJointSprings() #virtual joint spring controller
def callback(msg):
rospy.loginfo(msg)
head = baxter_interface.Head()
head.set_pan(0.0)
while True:
baxter_sleep()
rospy.sleep(2)
def _prepare_to_tuck(self):
# If arms are in "tucked" state, disable collision avoidance
# before enabling robot, to avoid arm jerking from "force-field".
head = baxter_interface.Head()
start_disabled = not self._rs.state().enabled
at_goal = lambda: (abs(head.pan()) <= baxter_interface.settings.
HEAD_PAN_ANGLE_TOLERANCE)
rospy.loginfo("Moving head to neutral position")
while not at_goal() and not rospy.is_shutdown():
if start_disabled:
[
pub.publish(Empty())
for pub in list(self._disable_pub.values())
]
if not self._rs.state().enabled:
self._enable_pub.publish(True)
head.set_pan(0.0, 50.0, timeout=0)
self._tuck_rate.sleep()
if start_disabled:
while self._rs.state().enabled == True and not rospy.is_shutdown():
[
pub.publish(Empty())
for pub in list(self._disable_pub.values())
]
self._enable_pub.publish(False)
self._tuck_rate.sleep()
def angles_callback(data):
set_angles=data.data;
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
lj = left.joint_names()
rj = right.joint_names()
print(lj)
#['left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
# s0 is shoulder twist (-1.7016, 1.7016), s1 is shoulder bend(-2.147, +1.047)
# e0 is elbow twist(-3.0541, 3.0541), e1 is elbow bend(-0.05,+2.618)
# w0 is writst twist(-3.059, 3.059), w1 is wrist bend(-1.5707,+2.094), w2 is writst twist 2(-3.059,3.059)
get_angles=left.joint_angles()
print("angles to be moved")
print(set_angles)
print("current angles")
print(get_angles['left_s0'])
ang0=set_angles[1]*(3.4/2.4)-1.7016
set_j(left,lj[1],ang0,0.3);
ang1=set_angles[0]*(3.4/2.4)-1.7016
set_j(left,lj[0],ang0,0.3);
head=baxter_interface.Head();
head.set_pan(0, speed=0.5)
print("Done.")
def __init__(self):
rospy.init_node('head_controller')
self.head = baxter_interface.Head()
self.led = haloLED()
self.sonar = None
self.state = 1
rospy.Subscriber('/robot/sonar/head_sonar/state',PointCloud,self.cb_sonar)
rospy.Subscriber('ai_state',Char,self.cb_state)
def __init__(self):
"""
'Wobbles' the head
"""
self._done = False
signal.signal(signal.SIGINT, self._handle_ctrl_c)
self._head = baxter_interface.Head()
def main(tuck):
print("Initializing node... ")
rospy.init_node("tuck_arms")
print("Moving head to neutral position")
head = baxter_interface.Head()
head.set_pan(0.0, 5.0)
print("%sucking arms" % ("T" if tuck else "Unt",))
Tuck(tuck)
def set_neutral(self):
"""
Sets both arms and the head back into a neutral pose.
"""
print("Moving to neutral pose...")
self._left_arm.move_to_neutral()
self._right_arm.move_to_neutral()
baxter_interface.Head().set_pan(0.0, speed=0.5, timeout=10)
def main():
# initialize our ROS node, registering it with the Master
rospy.init_node("Baxter_Move")
# create an instance of baxter_interface's Limb class
right_limb = baxter_interface.Limb("right")
left_limb = baxter_interface.Limb("left")
# get the right limb's current joint angles
right_angles = right_limb.joint_angles()
left_angles = left_limb.joint_angles()
head = baxter_interface.Head()
#head.set_pan(0)
#print("Here are the current humanoidish pose angles", right_angles)
right_angles['right_s0'] = -0.7854
right_angles['right_s1'] = 0 * 0.7854
right_angles['right_e0'] = 4 * 0.7854
right_angles['right_e1'] = 0 * 0.7854
right_angles['right_w0'] = 0 * 0.7854
right_angles['right_w1'] = 2 * 0.7854
right_angles['right_w2'] = 0 * 0.7854
left_angles['left_s0'] = 0.7854
left_angles['left_s1'] = 0.0
left_angles['left_e0'] = 0.0
left_angles['left_e1'] = 0.0
left_angles['left_w0'] = 0.0
left_angles['left_w1'] = 0.0
left_angles['left_w2'] = 0.0
# angles = {'right_s0': -0.7854, 'right_s1': 0, 'right_e0': 0, 'right_e1': 2*0.7854,
# 'right_w0': -2*0.7854, 'right_w1': -2*0.7854, 'right_w2': 0}
# move the right arm to those joint angles
right_limb.set_joint_position_speed(0.4)
#right_limb.move_to_joint_positions(right_angles)
#right_limb.move_to_joint_positions(ik_solve('right'))
quaternion = right_limb.endpoint_pose()["orientation"]
euler = tf.transformations.euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
print("Quaternion to euler:", roll * 57.2958, pitch * 57.2958,
yaw * 57.2958)
#print(left_limb.endpoint_pose())
# print("Right limb pose:")
# print(right_limb.endpoint_pose())
# quit
quit()
def startup():
rospy.loginfo("Begin Startup")
baxter_interface.RobotEnable().enable()
head = baxter_interface.Head()
rightlimb = baxter_interface.Limb('right')
leftlimb = baxter_interface.Limb('left')
leftlimb.move_to_neutral()
rightlimb.move_to_neutral()
head.set_pan(0.0)
def __init__(self):
"""
'Wobbles' the head
"""
self._done = False
self._head = baxter_interface.Head()
self.tolerance = baxter_interface.HEAD_PAN_ANGLE_TOLERANCE
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
print "Wobbler is initilized"
def __init__(self):
self.right_interface = baxter_interface.Limb("right")
self.left_interface = baxter_interface.Limb("left")
self.right_interface.set_joint_position_speed(0.1)
self.left_interface.set_joint_position_speed(0.1)
# START ARMS IN INITIAL UNTUCKED POSITION
self.init_left_x = 0.5793 # x = front back
self.init_left_y = 0.1883 # y = left right
self.init_left_z = 0.15 # z = up down
self.left_roll = 3.1024 # roll = horizontal
self.left_pitch = 0.0446 # pitch = vertical
self.left_yaw = 2.8645 # yaw = rotation
self.left_pose = [self.init_left_x, self.init_left_y, self.init_left_z, \
self.left_roll, self.left_pitch, self.left_yaw]
# START ARMS IN INITIAL UNTUCKED POSITION
self.init_right_x = 0.5797 # x = front back
self.init_right_y = -0.1910 # y = left right
self.init_right_z = 0.1013 # z = up down
self.right_roll = -3.0999 # roll = horizontal
self.right_pitch = 0.0441 # pitch = vertical
self.right_yaw = -2.8676 # yaw = rotation
self.right_pose = [self.init_right_x, self.init_right_y, self.init_right_z, \
self.right_roll, self.right_pitch, self.right_yaw]
self._left_grip = baxter_interface.Gripper('left', CHECK_VERSION)
self._right_grip = baxter_interface.Gripper('right', CHECK_VERSION)
# calibrate
self._left_grip.calibrate()
self._right_grip.calibrate()
# control parameters
self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate',
UInt16,
queue_size=10)
self._rate = 500.0 # Hz
self._max_speed = .05
self._arm = 'none'
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
# set joint state publishing to 500Hz
self._pub_rate.publish(self._rate)
self.twist = np.array([[.0], [.0], [.0], [.0], [.0], [.0]])
self.MINZ = -0.215
self.head = baxter_interface.Head()
def set_neutral():
"""
Sets both arms back into a neutral pose.
"""
print("Moving to neutral pose...")
baxter_interface.limb.Limb("left").move_to_neutral()
baxter_interface.limb.Limb("right").move_to_neutral()
# Sets head into neutral pose
baxter_interface.Head().set_pan(0.0)
def main():
rospy.init_node("me495_baxter_reset")
rate = rospy.Rate(10)
baxter_interface.RobotEnable().enable()
head = baxter_interface.Head()
rightlimb = baxter_interface.Limb('right')
leftlimb = baxter_interface.Limb('left')
leftlimb.move_to_neutral()
rightlimb.move_to_neutral()
head.set_pan(0.0)
def __init__(self):
self._done = False
self._head = baxter_interface.Head()
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
def handle_reset():
rospy.init_node("baxter_reset")
baxter_interface.RobotEnable().enable()
head = baxter_interface.Head()
rightlimb = baxter_interface.Limb('right')
leftlimb = baxter_interface.Limb('left')
leftlimb.move_to_neutral()
rightlimb.move_to_neutral()
head.set_pan(0.0)
return 0
def shutdown():
rospy.loginfo("Begin Shutdown")
head = baxter_interface.Head()
rightlimb = baxter_interface.Limb('right')
leftlimb = baxter_interface.Limb('left')
leftlimb.move_to_neutral()
rightlimb.move_to_neutral()
head.set_pan(0.0)
rospy.sleep(1)
baxter_interface.RobotEnable().disable()
def __init__(self, limb, experiment, number, threed):
""" Joint position or joint velocity data acquisition for handshake
scenario.
:param limb: The 'robotic' limb to record data from. The other limb
will be the 'human' limb.
:param experiment: Two robotic arms (r-r) or one robotic and one
human arm (r-h) in the experiment.
:param number: The number of samples to record.
:param threed: Whether 3d point clouds are to be recorded.
:return: A baxter robot instance.
"""
self._arm_robot = limb
if self._arm_robot == 'left':
self._arm_human = 'right'
else:
self._arm_human = 'left'
self._experiment = experiment
self._number = number
self._threed = threed
self._limb_robot = baxter_interface.Limb(self._arm_robot)
self._rec_joint_robot = JointClient(limb=self._arm_robot,
rate=settings.recording_rate)
self._limb_human = baxter_interface.Limb(self._arm_human)
self._rec_joint_human = JointClient(limb=self._arm_human,
rate=settings.recording_rate)
self._head = baxter_interface.Head()
if self._threed:
self._rec_kinect = RecorderClient('kinect_recorder')
self._rec_senz3d = RecorderClient('senz3d_recorder')
self._rec_flash = RecorderClient('flash_recorder')
self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate',
UInt16,
queue_size=10)
s = 'data/limb/' + self._arm_robot + '/cfg/des'
self._pub_cfg_des_robot = rospy.Publisher(s,
JointCommand,
queue_size=10)
s = 'data/limb/' + self._arm_human + '/cfg/des'
self._pub_cfg_des_human = rospy.Publisher(s,
JointCommand,
queue_size=10)
print "\nGetting robot state ... "
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print "Enabling robot... "
self._rs.enable()
self._limb_robot.set_joint_position_speed(0.3)
self._limb_human.set_joint_position_speed(0.3)
self._pub_rate.publish(settings.recording_rate)
def __init__(self):
"""
'Wobbles' the head
"""
self._done = False
self._head = baxter_interface.Head()
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable()
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
def __init__(self):
#Wobbles head
self._done = False
self._head = baxter_interface.Head()
#Make sure robot is enabled
print("Getting robot state...")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot...")
self._rs.enable()
print("Program start!")
def Init(self):
self._is_initialized = False
res = []
ra = lambda r: res.append(r)
self.default_face = 'config/baymax.jpg'
self.ChangeFace(self.default_face)
self.limbs = [None, None]
self.limbs[RIGHT] = baxter_interface.Limb(LRTostr(RIGHT))
self.limbs[LEFT] = baxter_interface.Limb(LRTostr(LEFT))
#self.joint_names= [[],[]]
#self.joint_names[RIGHT]= self.limbs[RIGHT].joint_names()
#self.joint_names[LEFT]= self.limbs[LEFT].joint_names()
#end_link=*_gripper(default),*_wrist,*_hand
self.kin = [None, None]
self.kin[RIGHT] = TKinematics(base_link=None, end_link='right_gripper')
self.kin[LEFT] = TKinematics(base_link=None, end_link='left_gripper')
self.head = baxter_interface.Head()
ra(
self.AddActC('r_traj',
'/robot/limb/right/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction,
time_out=3.0))
ra(
self.AddActC('l_traj',
'/robot/limb/left/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction,
time_out=3.0))
self.robotiq = TRobotiq() #Robotiq controller
self.epgripper = TBaxterEPG('right')
self.grippers = [self.epgripper, self.robotiq]
print 'Enabling the robot...'
baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION).enable()
print 'Calibrating electric parallel gripper...'
ra(self.epgripper.Init())
print 'Initializing and activating Robotiq gripper...'
ra(self.robotiq.Init())
if False not in res: self._is_initialized = True
return self._is_initialized
def main():
rospy.init_node('sanity_check_of_head_action_server')
head = baxter_interface.Head()
pos_init = head.pan()
hc = HeadClient()
if pos_init > (-1.39 + 0.3):
hc.command(position=pos_init - 0.3, velocity=1.0)
hc.wait()
if pos_init < (1.39 - 0.3):
hc.command(position=pos_init + 0.3, velocity=1.0)
hc.wait()
hc.command(position=pos_init, velocity=1.0)
hc.wait()
