Baxter interface with moveit

src/test/moveit_we.py

+#!/usr/bin/python
+import sys
+import time
+import rospy
+import baxter_interface
+from baxter_pykdl import baxter_kinematics
+import moveit_commander
+import moveit_msgs.msg
+from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
+from sensor_msgs.msg import Image
+import cv_bridge as cvb
+import cv2
+
+
+class Baxter(object):
+    def __init__(self):
+        self.bridge = cvb.CvBridge()
+        moveit_commander.roscpp_initialize(sys.argv)
+        rospy.init_node('baxter_kinematics')
+
+        self.arm_names = ['left', 'right']
+        self.arms = [moveit_commander.MoveGroupCommander(a + '_arm') for a in self.arm_names]
+
+        self.right_poses = {
+            'top': [0.74667248, 0.02808032, 0.50554368, -0.00295978, 0.97005472, -0.02409497, 0.24167026],
+            'neutral': [0.649446, -0.83735957, 0.06168316, 0.38299498, 0.92211736, -0.02071508, 0.050846]
+        }
+
+    def _arm_by_name(self, name):
+        return self.arms[self.arm_names.index(name)]
+
+    def _msg_to_jpg(self, msg, name):
+        try:
+            # Convert your ROS Image message to OpenCV2
+            cv2_img = bridge.imgmsg_to_cv2(msg, "bgr8")
+        except cvb.CvBridgeError, e:
+            print(e)
+        else:
+            # Save your OpenCV2 image as a jpeg
+            cv2.imwrite(name + '.jpg', cv2_img)
+
+    def _regular_snapshot(self, msg, name='snap', timeout=1):
+        self._msg_to_jpg(msg, name)
+        time.sleep(timeout)
+
+
+    def ready_camera(self, arm):
+        callback = lambda m: self._regular_snapshot(n)
+        subscriber = rospy.Subscriber('cameras/'+arm+'_hand_camera/image', Image, callback)
+        baxter_interface.CameraController(arm+'_hand_camera').open()
+
+    def to_pose(self, *args):
+        pos = ['x', 'y', 'z']
+        ori = ['x', 'y', 'z', 'w']
+        if len(args) is 1:
+            args = args[0]
+        p = Point(**dict(zip(pos, args[:3])))
+        q = Quaternion(**dict(zip(ori, args[-4:])))
+        return Pose(position=p, orientation=q)
+
+    def to_list(self, pose):
+        pos = ['x', 'y', 'z']
+        ori = ['x', 'y', 'z', 'w']
+        return [getattr(pose.position, n) for n in pos], [getattr(pose.orientation, n) for n in ori]
+
+    def current_pose(self, arm):
+        kin = baxter_kinematics(arm)
+        return kin.forward_position_kinematics()
+
+    def to_neutral(self):
+        p = self.right_poses['neutral']
+        self.move(p, 'right')
+
+    def move(self, pose, arm):
+        arm = self._arm_by_name(arm)
+        arm.set_pose_target(pose)
+        arm.plan()
+        arm.go(wait=True)
+
+b = Baxter()
+b.to_neutral()
+rospy.sleep(3)
+b.move(b.right_poses['top'], 'right')
+
+# [x, y, z, rot_i, rot_j, rot_k, rot_w]
+# left [ 0.12840291  0.4378986  -0.71167261 -0.61170038  0.79106672  0.00464676
+#  -0.00380794]