Commit 71580595 authored by Mathieu Reymond's avatar Mathieu Reymond
Browse files

Baxter interface with moveit

parent 1a453bdb
#!/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]
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment