def __init__(self, usegripper = True):
rospy.init_node("Baxter")
threading.Thread(target=self.__jointtraj_server).start()
self.trajrgt = Trajectory("right")
self.trajlft = Trajectory("left")
if usegripper:
self.__right_hand = Gripper("right")
self.__left_hand = Gripper("left")
self.__right_hand.calibrate()
self.__left_hand.calibrate()
self.__right_limb = Limb("right")
self.__left_limb = Limb("left")
self.baxter = RobotEnable()
self.__enabled = self.baxter.state().enabled
self.enable_baxter()
self.img = None
for camname in ['head_camera', 'left_hand_camera', 'right_hand_camera']:
try:
cam = CameraController(camname)
# cam.resolution = (1280, 800)
cam.resolution = (320, 200)
except:
pass
print("baxter opened")
def __init__(self):
self._rs = RobotEnable()
self._cvbr = CvBridge()
self._sm = RobotStateMachine(self)
self.ik = IKHelper()
self.ik.set_right(0.5, 0.0, 0.0, wait=True)
self.left_img = None
self.right_img = None
self._left_camera_sub = rospy.Subscriber(
'/cameras/left_hand_camera/image_rect_avg', Image,
self.on_left_imagemsg_received)
self._right_camera_sub = rospy.Subscriber(
'/cameras/right_hand_camera/image_rect_avg', Image,
self.on_right_imagemsg_received)
self._display_pub = rospy.Publisher('/robot/xdisplay',
Image,
tcp_nodelay=True,
latch=True)
self.range = None
self._range_sub = rospy.Subscriber(
'/robot/range/right_hand_range/state', Range,
self.on_rangemsg_received)
self.itb = None
self._itb_sub = rospy.Subscriber('/robot/itb/right_itb/state',
ITBState, self.on_itbmsg_received)
self.gripper = Gripper('right')
self.gripper.calibrate()
self.gripper.close(block=True)
def main():
rospy.init_node('pid_calibration')
enabler = RobotEnable()
enabler.enable()
init_params = {'left_s0': {'kp': 80.0, 'ki': 0.29, 'kd': 1.4},
'left_s1': {'kp': 100.0, 'ki': 1.0, 'kd': 1.0},
'left_e0': {'kp': 10.0, 'ki': 2.1, 'kd': 0.12},
'left_e1': {'kp': 16.0, 'ki': 2.1, 'kd': 0.26},
'left_w0': {'kp': 2.0, 'ki': 0.1, 'kd': 0.5},
'left_w1': {'kp': 30.0, 'ki': 1.71, 'kd': 0.9},
'left_w2': {'kp': 1.8, 'ki': 2.31, 'kd': 0.11}}
new_params = deepcopy(init_params)
new_errors = calculate_error(init_params)
change = {name: {constant: value/10 for constant, value in constants.items()}
for name, constants in init_params.items()}
csv_save(new_params, new_errors, change)
new_params, new_errors, change = csv_load()
init_errors = {joint: value + 1 for joint, value in new_errors.items()}
while any(new_errors[joint] < init_errors[joint] for joint in new_errors.keys()):
init_errors = new_errors.copy()
init_params = new_params.copy()
for joint, params in init_params.items():
new_params[joint], new_errors[joint], change[joint] = \
coordinate_descent(params, init_errors[joint], coord_desc_error, stop_condition, change[joint], csv_save,
{'joint': joint, 'params': new_params}, joint)
csv_save(new_params, new_errors, change)
print(init_params, init_errors)
class Baxter_Interactive(object):
def __init__(self, arm):
#Vector to record poses
self.recorded = []
self.arm = arm
#enable Baxter
self._en = RobotEnable()
self._en.enable()
#use DigitalIO to connect to gripper buttons
self._close_button = DigitalIO(self.arm + '_upper_button')
self._open_button = DigitalIO(self.arm + '_lower_button')
#set up gripper
self._gripper = Gripper(self.arm)
self._gripper.calibrate()
#set up navigator
self._navigator = Navigator(self.arm)
#set up limb
self._limb = Limb(self.arm)
self._limb.set_joint_position_speed(0.5)
self._limb.move_to_neutral()
print 'Ready to record...'
def recorder(self):
doneRecording = False
while not doneRecording:
if self._navigator.button0:
self.recorded.append(self._limb.joint_angles())
print 'Waypoint Added'
rospy.sleep(1)
if self._close_button.state:
self.recorded.append('CLOSE')
self._gripper.close()
print 'Gripper Closed'
rospy.sleep(1)
if self._open_button.state:
self.recorded.append('OPEN')
self._gripper.open()
print 'Gripper Opened'
rospy.sleep(1)
if self._navigator.button1:
print 'END RECORDING'
doneRecording = True
rospy.sleep(3)
while doneRecording:
for waypoint in self.recorded:
if waypoint == 'OPEN':
self._gripper.open()
rospy.sleep(1)
elif waypoint == 'CLOSE':
self._gripper.close()
rospy.sleep(1)
else:
self._limb.move_to_joint_positions(waypoint)
def __init__(self):
self._tf = tf.TransformListener()
self._rs = RobotEnable()
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.5)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.5)
self._left_arm_neutral = None
self._right_arm_neutral = None
self._left_iksvc = rospy.ServiceProxy(Mimic.IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(Mimic.IKSVC_RIGHT_URI,
SolvePositionIK)
self._left_camera = CameraController('left_hand_camera')
self._right_camera = CameraController('right_hand_camera')
self._head_camera = CameraController('head_camera')
self._left_camera.close()
self._right_camera.close()
self._head_camera.close()
self._head_camera.resolution = CameraController.MODES[0]
self._head_camera.open()
self._head_camera_sub = rospy.Subscriber(
'/cameras/head_camera/image', Image,
self._head_camera_sub_callback)
self._xdisplay_pub = rospy.Publisher('/robot/xdisplay',
Image,
latch=True)
self._out_of_range = False
self._ik_smooth = 4
self._ik_rate = 200
self._avg_window = 1000
self._r_trans_prev = []
self._l_trans_prev = []
self._r_ik_prev = []
self._l_ik_prev = []
self._joint_update_pub = rospy.Publisher(
'/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(self._ik_rate)
self._mimic_timer = None
def __init__(self, calibrate_grippers=True):
self._baxter_state = RobotEnable()
self._left = Limb(LEFT)
self._right = Limb(RIGHT)
self._limbs = {LEFT: self._left, RIGHT: self._right}
self._head = Head()
self._left_gripper, self._right_gripper = Gripper(LEFT), Gripper(RIGHT)
if calibrate_grippers:
self.calibrate()
self._left_ikservice = IKService(LEFT)
self._right_ikservice = IKService(RIGHT)
def __init__(self):
self._rs = RobotEnable()
self._cvbr = CvBridge()
self._sm = RobotStateMachine(self)
self.ik = IKHelper()
self.ik.set_right(0.5, 0.0, 0.0, wait=True)
self.gripper = Gripper('right')
self.gripper.calibrate()
self.gripper.close(block=True)
self.right_img = None
self._right_camera_sub = rospy.Subscriber(
'/cameras/right_hand_camera/image_rect_avg',
Image,
self.on_right_imagemsg_received)
self.itb = None
self._itb_sub = rospy.Subscriber(
'/robot/itb/right_itb/state',
ITBState,
self.on_itbmsg_received)
self._display_pub = rospy.Publisher(
'/robot/xdisplay',
Image,
tcp_nodelay=True,
latch=True)
def enable_arms(self):
"""
Enables the robot which enables the arms
:return: bool
"""
rospy.loginfo("Attempting to enabling robot.")
rs = RobotEnable(CHECK_VERSION)
try:
rs.enable()
except Exception, e:
rospy.logerr(e.strerror)
rospy.logerr("Failed to enable arms.")
return False
def __init__(self, camera_averaging=False):
self._cvbr = CvBridge()
self.rs = RobotEnable()
self.ik = IKHelper()
camera_topic = '/cameras/{0}_camera/image_rect'
if camera_averaging:
camera_topic += '_avg'
self.left_img = None
self._left_camera_sub = rospy.Subscriber(
camera_topic.format('left_hand'), Image,
self._on_left_imagemsg_received)
self.right_img = None
self._right_camera_sub = rospy.Subscriber(
camera_topic.format('right_hand'), Image,
self._on_right_imagemsg_received)
self.head_img = None
self._head_camera_sub = rospy.Subscriber(
camera_topic.format('head'), Image,
self._on_head_imagemsg_received)
self.left_itb = None
self._left_itb_sub = rospy.Subscriber('/robot/itb/left_itb/state',
ITBState,
self._on_left_itbmsg_received)
self.right_itb = None
self._right_itb_sub = rospy.Subscriber('/robot/itb/right_itb/state',
ITBState,
self._on_right_itbmsg_received)
self.left_gripper = Gripper('left')
self.right_gripper = Gripper('right')
self.head = Head()
self._display_pub = rospy.Publisher('/robot/xdisplay',
Image,
tcp_nodelay=True,
latch=True)
def main():
"""
Calculate best PID constans using simulated annealing
:return:
"""
rospy.init_node('pid_calibration')
enabler = RobotEnable()
enabler.enable()
limb = Limb('left')
init_params = {'left_s0': {'kp': 80.0, 'ki': 0.29, 'kd': 1.4},
'left_s1': {'kp': 100.0, 'ki': 1.0, 'kd': 1.0},
'left_e0': {'kp': 10.0, 'ki': 2.1, 'kd': 0.12},
'left_e1': {'kp': 16.0, 'ki': 2.1, 'kd': 0.26},
'left_w0': {'kp': 2.0, 'ki': 0.1, 'kd': 0.5},
'left_w1': {'kp': 1.9, 'ki': 1.71, 'kd': 0.15},
'left_w2': {'kp': 1.8, 'ki': 2.31, 'kd': 0.11}}
init_error = calculate_error(init_params)
best = simulated_annealing(init_params, init_error, calculate_error, neighbour, acceptance,
stop_condition, 100.0, temperature_update=lambda x: 0.95*x, callback=callback)
print best
def __init__(self, arm):
#Vector to record poses
self.recorded = []
self.arm = arm
#enable Baxter
self._en = RobotEnable()
self._en.enable()
#use DigitalIO to connect to gripper buttons
self._close_button = DigitalIO(self.arm + '_upper_button')
self._open_button = DigitalIO(self.arm + '_lower_button')
#set up gripper
self._gripper = Gripper(self.arm)
self._gripper.calibrate()
#set up navigator
self._navigator = Navigator(self.arm)
#set up limb
self._limb = Limb(self.arm)
self._limb.set_joint_position_speed(0.5)
self._limb.move_to_neutral()
print 'Ready to record...'
def __init__(
self,
camera_averaging=False):
self._cvbr = CvBridge()
self.rs = RobotEnable()
self.ik = IKHelper()
camera_topic = '/cameras/{0}_camera/image_rect'
if camera_averaging:
camera_topic += '_avg'
self.left_img = None
self._left_camera_sub = rospy.Subscriber(
camera_topic.format('left_hand'),
Image,
self._on_left_imagemsg_received)
self.right_img = None
self._right_camera_sub = rospy.Subscriber(
camera_topic.format('right_hand'),
Image,
self._on_right_imagemsg_received)
self.head_img = None
self._head_camera_sub = rospy.Subscriber(
camera_topic.format('head'),
Image,
self._on_head_imagemsg_received)
self.left_itb = None
self._left_itb_sub = rospy.Subscriber(
'/robot/itb/left_itb/state',
ITBState,
self._on_left_itbmsg_received)
self.right_itb = None
self._right_itb_sub = rospy.Subscriber(
'/robot/itb/right_itb/state',
ITBState,
self._on_right_itbmsg_received)
self.left_gripper = Gripper('left')
self.right_gripper = Gripper('right')
self.head = Head()
self._display_pub = rospy.Publisher(
'/robot/xdisplay',
Image,
tcp_nodelay=True,
latch=True)
def __init__(self):
self._tf = tf.TransformListener()
self._rs = RobotEnable()
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.5)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.5)
self._left_arm_neutral = None
self._right_arm_neutral = None
self._left_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_RIGHT_URI,
SolvePositionIK)
self._left_camera = CameraController('left_hand_camera')
self._right_camera = CameraController('right_hand_camera')
self._head_camera = CameraController('head_camera')
self._left_camera.close()
self._right_camera.close()
self._head_camera.close()
self._head_camera.resolution = CameraController.MODES[0]
self._head_camera.open()
self._head_camera_sub = rospy.Subscriber('/cameras/head_camera/image', Image,
self._head_camera_sub_callback)
self._xdisplay_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self._out_of_range = False
self._ik_smooth = 4
self._ik_rate = 200
self._avg_window = 1000
self._r_trans_prev = []
self._l_trans_prev = []
self._r_ik_prev = []
self._l_ik_prev = []
self._joint_update_pub = rospy.Publisher('/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(self._ik_rate)
self._mimic_timer = None
class BaxterNode(object):
"""Helper class for creating Baxter nodes quickly, intended to be
sublclassed.
Attributes:
rs -- An instance of RobotEnable provided by baxter_interface.
ik -- An instance of IKHelper.
left_img -- Last recieved image from left camera, rectified and as an
OpenCV numpy array.
right_img -- Last recieved image from right camera, rectified and as an
OpenCV numpy array.
left_itb -- Last received state from left ITB (shoulder buttons).
left_itb -- Last received state from right ITB (shoulder buttons).
left_gripper -- Instance of Gripper from baxter_interface, for left
hand.
right_gripper -- Instance of Gripper from baxter_interface, for right
hand.
"""
############################################################################
def __init__(
self,
camera_averaging=False):
self._cvbr = CvBridge()
self.rs = RobotEnable()
self.ik = IKHelper()
camera_topic = '/cameras/{0}_camera/image_rect'
if camera_averaging:
camera_topic += '_avg'
self.left_img = None
self._left_camera_sub = rospy.Subscriber(
camera_topic.format('left_hand'),
Image,
self._on_left_imagemsg_received)
self.right_img = None
self._right_camera_sub = rospy.Subscriber(
camera_topic.format('right_hand'),
Image,
self._on_right_imagemsg_received)
self.head_img = None
self._head_camera_sub = rospy.Subscriber(
camera_topic.format('head'),
Image,
self._on_head_imagemsg_received)
self.left_itb = None
self._left_itb_sub = rospy.Subscriber(
'/robot/itb/left_itb/state',
ITBState,
self._on_left_itbmsg_received)
self.right_itb = None
self._right_itb_sub = rospy.Subscriber(
'/robot/itb/right_itb/state',
ITBState,
self._on_right_itbmsg_received)
self.left_gripper = Gripper('left')
self.right_gripper = Gripper('right')
self.head = Head()
self._display_pub = rospy.Publisher(
'/robot/xdisplay',
Image,
tcp_nodelay=True,
latch=True)
############################################################################
def start(self, spin=False, calibrate=False):
"""Start up the node and initalise Baxter.
Keyword arguments:
spin -- Enter a spin loop once initialised (default False).
calibrate -- Calibrate the grippers (default False).
"""
self.rs.enable()
# Wait for initial topic messages to come in.
while self.left_itb is None or \
self.right_itb is None:
rospy.sleep(100)
# Calibrate both grippers.
if calibrate:
self.left_gripper.calibrate()
self.right_gripper.calibrate()
if spin:
rospy.spin()
def display_image(self, img):
"""Displays an image on the screen.
Arguments:
img -- A OpenCV numpy array to be displayed.
"""
img = cv2.resize(img, (1024, 600))
if len(img.shape) == 2:
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
img_msg = self._cv2_to_display_imgmsg(img)
self._display_pub.publish(img_msg)
############################################################################
def on_left_image_received(self, img):
"""Called when a image is received from the left camera. Intended to be
overridden.
Arguments:
img -- The rectified OpenCV numpy array from the camera.
"""
pass
def on_right_image_received(self, img):
"""Called when a image is received from the right camera. Intended to be
overridden.
Arguments:
img -- The rectified OpenCV numpy array from the camera.
"""
pass
def on_head_image_received(self, img):
"""Called when a image is received from the head camera. Intended to be
overridden.
Arguments:
img -- The rectified OpenCV numpy array from the camera.
"""
pass
def on_left_itb_received(self, itb):
"""Called when a left ITB state update is received. Intended to be
overridden.
Arguments:
itb -- The new ITB state.
"""
pass
def on_right_itb_received(self, itb):
"""Called when a right ITB state update is received. Intended to be
overridden.
Arguments:
itb -- The new ITB state.
"""
pass
############################################################################
def _camera_imgmsg_to_cv2(self, img_msg):
return self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
def _cv2_to_display_imgmsg(self, img_msg):
return self._cvbr.cv2_to_imgmsg(img_msg, 'bgr8')
############################################################################
def _on_left_imagemsg_received(self, img_msg):
self.left_img = self._camera_imgmsg_to_cv2(img_msg)
self.on_left_image_received(self.left_img)
def _on_right_imagemsg_received(self, img_msg):
self.right_img = self._camera_imgmsg_to_cv2(img_msg)
self.on_right_image_received(self.right_img)
def _on_head_imagemsg_received(self, img_msg):
img = self._camera_imgmsg_to_cv2(img_msg)
img = cv2.flip(img, 0)
self.head_img = img
self.on_head_image_received(self.head_img)
def _on_left_itbmsg_received(self, itb_msg):
self.left_itb = itb_msg
self.on_left_itb_received(self.left_itb)
def _on_right_itbmsg_received(self, itb_msg):
self.right_itb = itb_msg
self.on_right_itb_received(self.right_itb)
class BlockStackerNode(object):
RATE = 250
############################################################################
def __init__(self):
self._rs = RobotEnable()
self._cvbr = CvBridge()
self._sm = RobotStateMachine(self)
self.ik = IKHelper()
self.ik.set_right(0.5, 0.0, 0.0, wait=True)
self.left_img = None
self.right_img = None
self._left_camera_sub = rospy.Subscriber(
'/cameras/left_hand_camera/image_rect_avg',
Image,
self.on_left_imagemsg_received)
self._right_camera_sub = rospy.Subscriber(
'/cameras/right_hand_camera/image_rect_avg',
Image,
self.on_right_imagemsg_received)
self._display_pub = rospy.Publisher(
'/robot/xdisplay',
Image,
tcp_nodelay=True,
latch=True)
self.range = None
self._range_sub = rospy.Subscriber(
'/robot/range/right_hand_range/state',
Range,
self.on_rangemsg_received)
self.itb = None
self._itb_sub = rospy.Subscriber(
'/robot/itb/right_itb/state',
ITBState,
self.on_itbmsg_received)
self.gripper = Gripper('right')
self.gripper.calibrate()
self.gripper.close(block=True)
############################################################################
def start(self):
self._rs.enable()
self._sm.start()
rate = rospy.Rate(BlockStackerNode.RATE)
while not rospy.is_shutdown():
self._sm.run_step()
rate.sleep()
############################################################################
def on_left_imagemsg_received(self, img_msg):
img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
img = cv2.resize(img, (640, 400))
self.left_img = img.copy()
self._sm.on_left_image_received(img)
def on_right_imagemsg_received(self, img_msg):
img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
img = cv2.resize(img, (640, 400))
self.right_img = img.copy()
self._sm.on_right_image_received(img)
def on_rangemsg_received(self, range_msg):
self.range = range_msg.range
def on_itbmsg_received(self, itb_msg):
self.itb = itb_msg
def display_image(self, img):
img = cv2.resize(img, (1024, 600))
print img.shape
if img.shape[2] == 1:
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
img_msg = self._cvbr.cv2_to_imgmsg(img, 'bgr8')
self._display_pub.publish(img_msg)
class BaxterNode(object):
"""Helper class for creating Baxter nodes quickly, intended to be
sublclassed.
Attributes:
rs -- An instance of RobotEnable provided by baxter_interface.
ik -- An instance of IKHelper.
left_img -- Last recieved image from left camera, rectified and as an
OpenCV numpy array.
right_img -- Last recieved image from right camera, rectified and as an
OpenCV numpy array.
left_itb -- Last received state from left ITB (shoulder buttons).
left_itb -- Last received state from right ITB (shoulder buttons).
left_gripper -- Instance of Gripper from baxter_interface, for left
hand.
right_gripper -- Instance of Gripper from baxter_interface, for right
hand.
"""
############################################################################
def __init__(self, camera_averaging=False):
self._cvbr = CvBridge()
self.rs = RobotEnable()
self.ik = IKHelper()
camera_topic = '/cameras/{0}_camera/image_rect'
if camera_averaging:
camera_topic += '_avg'
self.left_img = None
self._left_camera_sub = rospy.Subscriber(
camera_topic.format('left_hand'), Image,
self._on_left_imagemsg_received)
self.right_img = None
self._right_camera_sub = rospy.Subscriber(
camera_topic.format('right_hand'), Image,
self._on_right_imagemsg_received)
self.head_img = None
self._head_camera_sub = rospy.Subscriber(
camera_topic.format('head'), Image,
self._on_head_imagemsg_received)
self.left_itb = None
self._left_itb_sub = rospy.Subscriber('/robot/itb/left_itb/state',
ITBState,
self._on_left_itbmsg_received)
self.right_itb = None
self._right_itb_sub = rospy.Subscriber('/robot/itb/right_itb/state',
ITBState,
self._on_right_itbmsg_received)
self.left_gripper = Gripper('left')
self.right_gripper = Gripper('right')
self.head = Head()
self._display_pub = rospy.Publisher('/robot/xdisplay',
Image,
tcp_nodelay=True,
latch=True)
############################################################################
def start(self, spin=False, calibrate=False):
"""Start up the node and initalise Baxter.
Keyword arguments:
spin -- Enter a spin loop once initialised (default False).
calibrate -- Calibrate the grippers (default False).
"""
self.rs.enable()
# Wait for initial topic messages to come in.
while self.left_itb is None or \
self.right_itb is None:
rospy.sleep(100)
# Calibrate both grippers.
if calibrate:
self.left_gripper.calibrate()
self.right_gripper.calibrate()
if spin:
rospy.spin()
def display_image(self, img):
"""Displays an image on the screen.
Arguments:
img -- A OpenCV numpy array to be displayed.
"""
img = cv2.resize(img, (1024, 600))
if len(img.shape) == 2:
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
img_msg = self._cv2_to_display_imgmsg(img)
self._display_pub.publish(img_msg)
############################################################################
def on_left_image_received(self, img):
"""Called when a image is received from the left camera. Intended to be
overridden.
Arguments:
img -- The rectified OpenCV numpy array from the camera.
"""
pass
def on_right_image_received(self, img):
"""Called when a image is received from the right camera. Intended to be
overridden.
Arguments:
img -- The rectified OpenCV numpy array from the camera.
"""
pass
def on_head_image_received(self, img):
"""Called when a image is received from the head camera. Intended to be
overridden.
Arguments:
img -- The rectified OpenCV numpy array from the camera.
"""
pass
def on_left_itb_received(self, itb):
"""Called when a left ITB state update is received. Intended to be
overridden.
Arguments:
itb -- The new ITB state.
"""
pass
def on_right_itb_received(self, itb):
"""Called when a right ITB state update is received. Intended to be
overridden.
Arguments:
itb -- The new ITB state.
"""
pass
############################################################################
def _camera_imgmsg_to_cv2(self, img_msg):
return self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
def _cv2_to_display_imgmsg(self, img_msg):
return self._cvbr.cv2_to_imgmsg(img_msg, 'bgr8')
############################################################################
def _on_left_imagemsg_received(self, img_msg):
self.left_img = self._camera_imgmsg_to_cv2(img_msg)
self.on_left_image_received(self.left_img)
def _on_right_imagemsg_received(self, img_msg):
self.right_img = self._camera_imgmsg_to_cv2(img_msg)
self.on_right_image_received(self.right_img)
def _on_head_imagemsg_received(self, img_msg):
img = self._camera_imgmsg_to_cv2(img_msg)
img = cv2.flip(img, 0)
self.head_img = img
self.on_head_image_received(self.head_img)
def _on_left_itbmsg_received(self, itb_msg):
self.left_itb = itb_msg
self.on_left_itb_received(self.left_itb)
def _on_right_itbmsg_received(self, itb_msg):
self.right_itb = itb_msg
self.on_right_itb_received(self.right_itb)
class Mimic(object):
IKSVC_LEFT_URI = 'ExternalTools/left/PositionKinematicsNode/IKService'
IKSVC_RIGHT_URI = 'ExternalTools/right/PositionKinematicsNode/IKService'
def __init__(self):
self._tf = tf.TransformListener()
self._rs = RobotEnable()
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.5)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.5)
self._left_arm_neutral = None
self._right_arm_neutral = None
self._left_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_RIGHT_URI,
SolvePositionIK)
self._left_camera = CameraController('left_hand_camera')
self._right_camera = CameraController('right_hand_camera')
self._head_camera = CameraController('head_camera')
self._left_camera.close()
self._right_camera.close()
self._head_camera.close()
self._head_camera.resolution = CameraController.MODES[0]
self._head_camera.open()
self._head_camera_sub = rospy.Subscriber('/cameras/head_camera/image', Image,
self._head_camera_sub_callback)
self._xdisplay_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self._out_of_range = False
self._ik_smooth = 4
self._ik_rate = 200
self._avg_window = 1000
self._r_trans_prev = []
self._l_trans_prev = []
self._r_ik_prev = []
self._l_ik_prev = []
self._joint_update_pub = rospy.Publisher('/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(self._ik_rate)
self._mimic_timer = None
############################################################################
def start(self):
self._rs.enable()
self._reset()
self._mimic_timer = rospy.Timer(rospy.Duration(1.0 / self._ik_rate), self._mimic_callback)
rate = rospy.Rate(self._avg_window)
while not rospy.is_shutdown():
try:
(r_trans,r_rot) = self._tf.lookupTransform(
'/skeleton/user_1/right_hand',
'/skeleton/user_1/torso',
rospy.Time(0))
(l_trans,l_rot) = self._tf.lookupTransform(
'/skeleton/user_1/left_hand',
'/skeleton/user_1/torso',
rospy.Time(0))
self._l_trans_prev.append(l_trans)
if (len(self._l_trans_prev) > self._avg_window):
self._l_trans_prev.pop(0)
self._r_trans_prev.append(r_trans)
if (len(self._r_trans_prev) > self._avg_window):
self._r_trans_prev.pop(0)
except:
self._out_of_range = True
rate.sleep()
continue
rate.sleep()
def _reset(self):
if self._mimic_timer:
self._mimic_timer.shutdown()
self._left_arm.move_to_neutral()
self._left_arm_neutral = self._left_arm.joint_angles()
self._right_arm.move_to_neutral()
self._right_arm_neutral = self._right_arm.joint_angles()
print(self._left_arm_neutral)
print(self._right_arm_neutral)
def shutdown(self):
self._reset()
############################################################################
def _mimic_callback(self, event):
try:
l_trans = (
sum(map(lambda x: x[0], self._l_trans_prev)) / self._avg_window,
sum(map(lambda x: x[1], self._l_trans_prev)) / self._avg_window,
sum(map(lambda x: x[2], self._l_trans_prev)) / self._avg_window)
r_trans = (
sum(map(lambda x: x[0], self._r_trans_prev)) / self._avg_window,
sum(map(lambda x: x[1], self._r_trans_prev)) / self._avg_window,
sum(map(lambda x: x[2], self._r_trans_prev)) / self._avg_window)
rh_x = clamp(0.65 + (l_trans[2] / 1.5), 0.5, 0.9)
rh_y = clamp(-l_trans[0], -0.8, 0)
rh_z = clamp(-l_trans[1], -0.1, 0.8)
lh_x = clamp(0.65 + (r_trans[2] / 1.5), 0.5, 0.9)
lh_y = clamp(-r_trans[0], 0, 0.8)
lh_z = clamp(-r_trans[1], -0.1, 0.8)
self.set_left_coords(lh_x, lh_y, lh_z)
self.set_right_coords(rh_x, rh_y, rh_z)
except:
return
def _head_camera_sub_callback(self, data):
orig_img = cvbr.imgmsg_to_cv2(data, 'rgb8')
img = cv2.cvtColor(orig_img, cv2.COLOR_RGB2GRAY)
img = cv2.equalizeHist(img)
img = cv2.GaussianBlur(img, (5, 5), 0)
img = cv2.Canny(img, 100, 200)
img = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB)
img = cv2.addWeighted(img, 0.75, orig_img, 0.25, 0)
img = cv2.resize(img, (1024, 768))
if self._out_of_range:
cv2.putText(img, 'Out of range!', (50, 768-50), cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 0, 0), 4)
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
img = cvbr.cv2_to_imgmsg(img)
self._xdisplay_pub.publish(img)
############################################################################
def set_left_coords(self,
x, y, z,
er=math.pi * -1, ep=math.pi * 0.5, ey=math.pi * -1):
self._set_arm_coords(self._left_iksvc, self._left_arm,
x, y, z,
er, ep, ey,
self._get_neutral_joint_state(self._left_arm_neutral),
self._l_ik_prev)
def set_right_coords(self,
x, y, z,
er=math.pi * -1, ep=math.pi * 0.5, ey=math.pi * -1):
self._set_arm_coords(self._right_iksvc, self._right_arm,
x, y, z,
er, ep, ey,
self._get_neutral_joint_state(self._right_arm_neutral),
self._r_ik_prev)
############################################################################
def _set_arm_coords(self, iksvc, limb, x, y, z, er, ep, ey, njs, prev):
resp = self._get_ik(iksvc, x, y, z, er, ep, ey, njs)
positions = resp[0]
isValid = resp[1]
self._out_of_range = not isValid
prev.append(positions)
if (len(prev) > self._ik_smooth):
prev.pop(0)
smooth_positions = {}
for joint_name in positions:
smooth_positions[joint_name] = \
sum(map(lambda x: x[joint_name], prev)) / self._ik_smooth
limb.set_joint_positions(smooth_positions)
def _get_ik(self, iksvc, x, y, z, er, ep, ey, njs):
q = quaternion_from_euler(er, ep, ey)
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(
position=Point(
x=x,
y=y,
z=z,
),
orientation=Quaternion(q[0], q[1], q[2], q[3])
),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
ikreq.seed_angles.append(njs)
iksvc.wait_for_service(1.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
def _get_neutral_joint_state(self, njs):
js = JointState()
js.header = Header(stamp=rospy.Time.now(), frame_id='base')
for j in njs:
js.name.append(j)
js.position.append(njs[j])
return js
class BlockStackerNode(object):
RATE = 250
############################################################################
def __init__(self):
self._rs = RobotEnable()
self._cvbr = CvBridge()
self._sm = RobotStateMachine(self)
self.ik = IKHelper()
self.ik.set_right(0.5, 0.0, 0.0, wait=True)
self.left_img = None
self.right_img = None
self._left_camera_sub = rospy.Subscriber(
'/cameras/left_hand_camera/image_rect_avg', Image,
self.on_left_imagemsg_received)
self._right_camera_sub = rospy.Subscriber(
'/cameras/right_hand_camera/image_rect_avg', Image,
self.on_right_imagemsg_received)
self._display_pub = rospy.Publisher('/robot/xdisplay',
Image,
tcp_nodelay=True,
latch=True)
self.range = None
self._range_sub = rospy.Subscriber(
'/robot/range/right_hand_range/state', Range,
self.on_rangemsg_received)
self.itb = None
self._itb_sub = rospy.Subscriber('/robot/itb/right_itb/state',
ITBState, self.on_itbmsg_received)
self.gripper = Gripper('right')
self.gripper.calibrate()
self.gripper.close(block=True)
############################################################################
def start(self):
self._rs.enable()
self._sm.start()
rate = rospy.Rate(BlockStackerNode.RATE)
while not rospy.is_shutdown():
self._sm.run_step()
rate.sleep()
############################################################################
def on_left_imagemsg_received(self, img_msg):
img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
img = cv2.resize(img, (640, 400))
self.left_img = img.copy()
self._sm.on_left_image_received(img)
def on_right_imagemsg_received(self, img_msg):
img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
img = cv2.resize(img, (640, 400))
self.right_img = img.copy()
self._sm.on_right_image_received(img)
def on_rangemsg_received(self, range_msg):
self.range = range_msg.range
def on_itbmsg_received(self, itb_msg):
self.itb = itb_msg
def display_image(self, img):
img = cv2.resize(img, (1024, 600))
print img.shape
if img.shape[2] == 1:
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
img_msg = self._cvbr.cv2_to_imgmsg(img, 'bgr8')
self._display_pub.publish(img_msg)
class Baxter:
def __init__(self, usegripper = True):
rospy.init_node("Baxter")
threading.Thread(target=self.__jointtraj_server).start()
self.trajrgt = Trajectory("right")
self.trajlft = Trajectory("left")
if usegripper:
self.__right_hand = Gripper("right")
self.__left_hand = Gripper("left")
self.__right_hand.calibrate()
self.__left_hand.calibrate()
self.__right_limb = Limb("right")
self.__left_limb = Limb("left")
self.baxter = RobotEnable()
self.__enabled = self.baxter.state().enabled
self.enable_baxter()
self.img = None
for camname in ['head_camera', 'left_hand_camera', 'right_hand_camera']:
try:
cam = CameraController(camname)
# cam.resolution = (1280, 800)
cam.resolution = (320, 200)
except:
pass
print("baxter opened")
@property
def enabled(self):
return self.__enabled
def __jointtraj_server(self):
limb = "both"
rate = 100.0
mode = "position"
if mode == 'velocity':
dyn_cfg_srv = Server(VelocityJointTrajectoryActionServerConfig,
lambda config, level: config)
elif mode == 'position':
dyn_cfg_srv = Server(PositionJointTrajectoryActionServerConfig,
lambda config, level: config)
else:
dyn_cfg_srv = Server(PositionFFJointTrajectoryActionServerConfig,
lambda config, level: config)
jtas = []
if limb == 'both':
jtas.append(JointTrajectoryActionServer('right', dyn_cfg_srv,
rate, mode))
jtas.append(JointTrajectoryActionServer('left', dyn_cfg_srv,
rate, mode))
else:
jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode))
def cleanup():
for j in jtas:
j.clean_shutdown()
rospy.on_shutdown(cleanup)
print("Running. Ctrl-c to quit")
rospy.spin()
def enable_baxter(self):
print("Opening the baxter")
if not self.__enabled: self.baxter.enable()
self.__enabled = self.baxter.state().enabled
def disable_baxter(self):
if self.__enabled: self.baxter.disable()
self.__enabled = self.baxter.state().enabled
def opengripper(self, armname="rgt"):
if not self.__enabled: return
arm = self.__right_hand if armname == "rgt" else self.__left_hand
if self._is_grippable(arm):
arm.open()
else:
rospy.logwarn(armname+ " have not been calibrated")
def closegripper(self, armname="rgt"):
if not self.__enabled: return
arm = self.__right_hand if armname == "rgt" else self.__left_hand
if self._is_grippable(arm):
arm.close()
else:
rospy.logwarn(armname + " have not been calibrated")
def commandgripper(self,pos , armname="rgt"):
if not self.__enabled: return
arm = self.__right_hand if armname == "rgt" else self.__left_hand
if self._is_grippable(arm):
arm.command_position(position = pos)
else:
rospy.logwarn(armname + " have not been calibrated")
def _is_grippable(self,gripper):
return (gripper.calibrated() and gripper.ready())
def currentposgripper(self,armname ="rgt"):
arm = self.__right_hand if armname == "rgt" else self.__left_hand
return arm.position()
def getjnts(self,armname="rgt"):
if not self.__enabled: return
limb = self.__right_limb if armname == "rgt" else self.__left_limb
angles = limb.joint_angles()
return angles
def movejnts(self,jnts_dict, speed =.6 , armname="rgt"):
if not self.__enabled: return
limb = self.__right_limb if armname == "rgt" else self.__left_limb
traj = self.trajrgt if armname is "rgt" else self.trajlft
# limb.set_joint_position_speed(speed)
currentjnts = [limb.joint_angle(joint) for joint in limb.joint_names()]
traj.add_point(currentjnts, 0)
if isinstance(jnts_dict, dict):
path_angle = [jnts_dict[joint_name] for joint_name in limb.joint_names()]
else:
path_angle = jnts_dict
# print(path_angle)
traj.add_point(path_angle, speed)
# limb.move_to_joint_positions(jnts_dict,threshold= baxter_interface.settings.JOINT_ANGLE_TOLERANCE)
limb.set_joint_position_speed(.3)
def movejnts_cont(self, jnts_dict_list, speed=.1, armname="rgt"):
traj = self.trajrgt if armname is "rgt" else self.trajlft
if not self.__enabled: return
limb = self.__right_limb if armname == "rgt" else self.__left_limb
# limb.set_joint_position_speed(.6)
currentjnts = [limb.joint_angle(joint) for joint in limb.joint_names()]
traj.add_point(currentjnts, 0)
rospy.on_shutdown(traj.stop)
t = speed
for jnts_dict in jnts_dict_list:
# limb.move_to_joint_positions(jnts_dict, threshold= baxter_interface.settings.JOINT_ANGLE_TOLERANCE)
if isinstance(jnts_dict,dict):
path_angle = [jnts_dict[joint_name] for joint_name in limb.joint_names()]
else:
path_angle = jnts_dict
# print(path_angle)
traj.add_point(path_angle,t)
t += speed
traj.start()
traj.wait(-1)
limb.set_joint_position_speed(.3)
traj.clear()
def getforce(self,armname="rgt"):
if not self.__enabled: return
limb = self.__right_limb if armname == "rgt" else self.__left_limb
ft = limb.endpoint_effort()
force = ft['force']
torque = ft['torque']
print ft
return list(force+torque)
def setscreenimage(self,path):
# it seems not working
"""
Send the image located at the specified path to the head
display on Baxter.
@param path: path to the image file to load and send
"""
# rospy.init_node('rsdk_xdisplay_image', anonymous=True)
print("Changing the photo")
img = cv.imread(path)
msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
pub = rospy.Publisher('/robot_s/xdisplay', Image, latch=True, queue_size=1)
pub.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(1)
def getimage(self,cameraname="left_hand_camera"):
# ['head_camera', 'left_hand_camera', 'right_hand_camera']
# if cameraname != self.camera_on:
# cam = CameraController(self.camera_on)
# cam.close()
# cam = CameraController(cameraname)
# cam.resolution = (1280, 800)
# cam.open()
# self.camera_on = cameraname
print("Getting imag...")
"rosrun image_view image_view image:=/cameras/left_hand_camera/image"
"rosrun image_view image_view image:=/cameras/head_camera/image"
# Instantiate CvBridge
bridge = cv_bridge.CvBridge()
def image_callback(msg):
print("Received an image!")
try:
# Convert your ROS Image message to OpenCV2
cv_img = bridge.imgmsg_to_cv2(msg, "bgr8")
except cv_bridge.CvBridgeError, e:
print(e)
else:
class Baxter(object):
def __init__(self, calibrate_grippers=True):
self._baxter_state = RobotEnable()
self._left = Limb(LEFT)
self._right = Limb(RIGHT)
self._limbs = {LEFT: self._left, RIGHT: self._right}
self._head = Head()
self._left_gripper, self._right_gripper = Gripper(LEFT), Gripper(RIGHT)
if calibrate_grippers:
self.calibrate()
self._left_ikservice = IKService(LEFT)
self._right_ikservice = IKService(RIGHT)
def calibrate(self):
self._left_gripper.calibrate()
self._left_gripper_max = self._left_gripper.position()
self._right_gripper.calibrate()
self._right_gripper_max = self._right_gripper.position()
@property
def left_gripper_max(self):
return self._left_gripper_max
@property
def right_gripper_max(self):
return self._right_gripper_max
@property
def left_gripper(self):
return self._left_gripper.position()
@left_gripper.setter
def left_gripper(self, position):
self._left_gripper.command_position(position)
@property
def right_gripper(self):
return self._right_gripper.position()
@right_gripper.setter
def right_gripper(self, position):
self._right_gripper.command_position(position)
def set_left_joints(self, angles):
joints = self._left.joint_angles()
for joint, angle in angles.iteritems():
if angle:
joints[joint] = angle
self.enable_check()
self._left.set_joint_positions(joints)
def set_right_joints(self, angles):
joints = self._right.joint_angles()
for joint, angle in angles.iteritems():
if angle:
joints[joint] = angle
self.enable_check()
self._right.set_joint_positions(joints)
def reset_limb(self, side):
angles = {joint: 0.0 for joint in self._limbs[side].joint_angles()}
self.enable_check()
self._limbs[side].move_to_joint_positions(angles)
def enable_check(self):
# Sometimes robot is disabled due to another program resetting state
if not self._baxter_state.state().enabled:
self._baxter_state.enable()
@property
def joints(self):
joints = {
limb: joint.joint_angles()
for limb, joint in self._limbs.iteritems()
}
return joints
@property
def enabled(self):
return self._baxter_state.state().enabled
@property
def left_s0(self):
return self._left.joint_angle('left_s0')
@left_s0.setter
def left_s0(self, angle):
self.set_left_joints({'left_s0': angle})
@property
def left_s1(self):
return self._left.joint_angle('left_s1')
@left_s1.setter
def left_s1(self, angle):
self.set_left_joints({'left_s1': angle})
@property
def left_e0(self):
return self._left.joint_angle('left_e0')
@left_e0.setter
def left_e0(self, angle):
self.set_left_joints({'left_e0': angle})
@property
def left_e1(self):
return self._left.joint_angle('left_e1')
@left_e1.setter
def left_e1(self, angle):
self.set_left_joints({'left_e1': angle})
@property
def left_w0(self):
return self._left.joint_angle('left_w0')
@left_w0.setter
def left_w0(self, angle):
self.set_left_joints({'left_w0': angle})
@property
def left_w1(self):
return self._left.joint_angle('left_w1')
@left_w1.setter
def left_w1(self, angle):
self.set_left_joints({'left_w1': angle})
@property
def left_w2(self):
return self._left.joint_angle('left_w2')
@left_w2.setter
def left_w2(self, angle):
self.set_left_joints({'left_w2': angle})
@property
def right_s0(self):
return self._right.joint_angle('right_s0')
@right_s0.setter
def right_s0(self, angle):
self.set_right_joints({'right_s0': angle})
@property
def right_s1(self):
return self._right.joint_angle('right_s1')
@right_s1.setter
def right_s1(self, angle):
self.set_right_joints({'right_s1': angle})
@property
def right_e0(self):
return self._right.joint_angle('right_e0')
@right_e0.setter
def right_e0(self, angle):
self.set_right_joints({'right_e0': angle})
@property
def right_e1(self):
return self._right.joint_angle('right_e1')
@right_e1.setter
def right_e1(self, angle):
self.set_right_joints({'right_e1': angle})
@property
def right_w0(self):
return self._right.joint_angle('right_w0')
@right_w0.setter
def right_w0(self, angle):
self.set_right_joints({'right_w0': angle})
@property
def right_w1(self):
return self._right.joint_angle('right_w1')
@right_w1.setter
def right_w1(self, angle):
self.set_right_joints({'right_w1': angle})
@property
def right_w2(self):
return self._right.joint_angle('right_w2')
@right_w2.setter
def right_w2(self, angle):
self.set_right_joints({'right_w2': angle})
@property
def left_position(self):
return self._left.endpoint_pose()['position']
@property
def left_position_x(self):
return self.left_position.x
@left_position_x.setter
def left_position_x(self, point):
self.set_left_pose(position={'x': point})
@property
def left_position_y(self):
return self.left_position.y
@left_position_y.setter
def left_position_y(self, point):
self.set_left_pose(position={'y': point})
@property
def left_position_z(self):
return self.left_position.z
@left_position_z.setter
def left_position_z(self, point):
self.set_left_pose(position={'z': point})
@property
def left_orientation(self):
return self._left.endpoint_pose()['orientation']
@property
def left_orientation_x(self):
return self.left_orientation.x
@left_orientation_x.setter
def left_orientation_x(self, point):
self.set_left_pose(orientation={'x': point})
@property
def left_orientation_y(self):
return self.left_orientation.y
@left_orientation_y.setter
def left_orientation_y(self, point):
self.set_left_pose(orientation={'y': point})
@property
def left_orientation_z(self):
return self.left_orientation.z
@left_orientation_z.setter
def left_orientation_z(self, point):
self.set_left_pose(orientation={'z': point})
@property
def left_orientation_w(self):
return self.left_orientation.w
@left_orientation_w.setter
def left_orientation_w(self, point):
self.set_left_pose(orientation={'w': point})
@property
def right_position(self):
return self._right.endpoint_pose()['position']
@property
def right_orientation(self):
return self._right.endpoint_pose()['orientation']
def set_left_pose(self, position={}, orientation={}):
pos = {
'x': self.left_position_x,
'y': self.left_position_y,
'z': self.left_position_z,
}
for key, value in position.iteritems():
pos[key] = value
orient = {
'x': self.left_orientation_x,
'y': self.left_orientation_y,
'z': self.left_orientation_z,
'w': self.left_orientation_w,
}
for key, value in orientation.iteritems():
orient[key] = value
pos = self._left_ikservice.solve_position(
Pose(position=Point(**pos), orientation=Quaternion(**orient)))
if pos:
self.set_left_joints(pos)
else:
print 'nothing'
#print self.joints
@property
def right_position(self):
return self._right.endpoint_pose()['position']
@property
def right_position_x(self):
return self.right_position.x
@right_position_x.setter
def right_position_x(self, point):
self.set_right_pose(position={'x': point})
@property
def right_position_y(self):
return self.right_position.y
@right_position_y.setter
def right_position_y(self, point):
self.set_right_pose(position={'y': point})
@property
def right_position_z(self):
return self.right_position.z
@right_position_z.setter
def right_position_z(self, point):
self.set_right_pose(position={'z': point})
@property
def right_orientation(self):
return self._right.endpoint_pose()['orientation']
@property
def right_orientation_x(self):
return self.right_orientation.x
@right_orientation_x.setter
def right_orientation_x(self, point):
self.set_right_pose(orientation={'x': point})
@property
def right_orientation_y(self):
return self.right_orientation.y
@right_orientation_y.setter
def right_orientation_y(self, point):
self.set_right_pose(orientation={'y': point})
@property
def right_orientation_z(self):
return self.right_orientation.z
@right_orientation_z.setter
def right_orientation_z(self, point):
self.set_right_pose(orientation={'z': point})
@property
def right_orientation_w(self):
return self.right_orientation.w
@right_orientation_w.setter
def right_orientation_w(self, point):
self.set_right_pose(orientation={'w': point})
@property
def right_position(self):
return self._right.endpoint_pose()['position']
@property
def right_orientation(self):
return self._right.endpoint_pose()['orientation']
def set_right_pose(self, position={}, orientation={}):
pos = {
'x': self.right_position_x,
'y': self.right_position_y,
'z': self.right_position_z,
}
for key, value in position.iteritems():
pos[key] = value
orient = {
'x': self.right_orientation_x,
'y': self.right_orientation_y,
'z': self.right_orientation_z,
'w': self.right_orientation_w,
}
for key, value in orientation.iteritems():
orient[key] = value
pos = self._right_ikservice.solve_position(
Pose(position=Point(**pos), orientation=Quaternion(**orient)))
if pos:
self.set_right_joints(pos)
@property
def head_position(self):
return self._head.pan()
@head_position.setter
def head_position(self, position):
self._head.set_pan(position)
Quaternion,
)
from std_msgs.msg import Header
from baxter_core_msgs.srv import (
SolvePositionIK,
SolvePositionIKRequest,
)
from sensor_msgs.msg import (
Image, )
if __name__ == '__main__':
rospy.init_node("Record_Baxter_Joint_Angles")
RobotEnable().enable()
navigator_arm_r = baxter_interface.Navigator('right')
navigator_arm_l = baxter_interface.Navigator('left')
robot_arm_r = baxter_interface.Limb('right')
robot_arm_l = baxter_interface.Limb('left')
while True:
if navigator_arm_l.button0:
print robot_arm_l.joint_angles()
elif navigator_arm_r.button0:
print robot_arm_r.joint_angles()
elif navigator_arm_l.button1 or navigator_arm_r.button1:
class BrrUi(object):
def __init__(self, share_path, conf_path, commands):
self.share_path = share_path
self.conf_path = conf_path
self.windows = dict()
self._btn_context = dict()
self._functions = demo_functions
self._load_config()
self.img = Image.new('RGB', (1024, 600), 'white')
self.active_window = self.windows['demo_1']
self.xdisp = rospy.Publisher('/robot/xdisplay',
ImageMsg,
latch=True,
queue_size=1)
self._status = RobotEnable()
self._commands = commands
self._font = ImageFont.truetype('%s/HelveticaLight.ttf' % share_path,
30)
self._textHeight = self._font.getsize('W')[1]
self._active_example = False
self._navigators = {
'left': Navigator('left'),
'torso_left': Navigator('torso_left'),
'right': Navigator('right'),
'torso_right': Navigator('torso_right')
}
self._listeners_connected = False
self._connect_listeners()
self._estop_state = False
self._estop_sub = rospy.Subscriber('robot/state', AssemblyState,
self._estop_callback)
self._wheel_ok = True
self.cameras = dict()
camera_list = ['left_hand', 'right_hand', 'head']
for idx, cam in enumerate(camera_list):
try:
self.cameras[cam] = CameraController('%s_camera' % cam)
except AttributeError:
try:
# This camera might not be powered
# Turn off the power to the last camera
# this will turn power on to the current camera
CameraController('%s_camera' %
camera_list[idx - 1]).close()
# And try again to locate the camera service
self.cameras[cam] = CameraController('%s_camera' % cam)
except AttributeError:
# This camera is unavailable (might be broken)
# Disable camera button in the UI
self.windows['cam_submenu'].set_btn_selectable(
'cam_%s' % cam, False)
sel = self.windows['cam_submenu'].selected_btn()
bad_cam = self.windows['cam_submenu'].get_btn('cam_%s' %
cam)
if (sel == bad_cam
and not self.windows['cam_submenu'].scroll(1)):
self.windows['cam_submenu'].selected_btn_index = 0
self.cam_sub = None
self._l_grip = {'interface': Gripper('left'), 'type': 'custom'}
self._r_grip = {'interface': Gripper('right'), 'type': 'custom'}
rospy.Timer(rospy.Duration(.5), self._check_enable)
self.error_state = False
self._enable()
self.calib_stage = 0
self.draw()
mk_process('rosrun baxter_tools tuck_arms.py -u')
def _estop_callback(self, msg):
if self._estop_state != msg.stopped:
self._estop_state = msg.stopped
if msg.stopped and self._listeners_connected:
self._disconnect_listeners()
elif not msg.stopped and not self._listeners_connected:
self._connect_listeners()
def _connect_listeners(self):
# Navigator OK Button
self._navigators['left'].button0_changed.connect(self._left_ok_pressed)
self._navigators['torso_left'].button0_changed.connect(
self._left_ok_pressed)
self._navigators['right'].button0_changed.connect(
self._right_ok_pressed)
self._navigators['torso_right'].button0_changed.connect(
self._right_ok_pressed)
# Navigator Wheel
self._navigators['left'].wheel_changed.connect(self._left_wheel_moved)
self._navigators['torso_left'].wheel_changed.connect(
self._left_wheel_moved)
self._navigators['right'].wheel_changed.connect(
self._right_wheel_moved)
self._navigators['torso_right'].wheel_changed.connect(
self._right_wheel_moved)
# Navigator Baxter Button
self._navigators['left'].button2_changed.connect(self._enable)
self._navigators['torso_left'].button2_changed.connect(self._enable)
self._navigators['right'].button2_changed.connect(self._enable)
self._navigators['torso_right'].button2_changed.connect(self._enable)
# Navigator Back Button
self._navigators['left'].button1_changed.connect(self.back)
self._navigators['torso_left'].button1_changed.connect(self.back)
self._navigators['right'].button1_changed.connect(self.back)
self._navigators['torso_right'].button1_changed.connect(self.back)
self._listeners_connected = True
def _disconnect_listeners(self):
# Navigator OK Button
self._navigators['left'].button0_changed.disconnect(
self._left_ok_pressed)
self._navigators['torso_left'].button0_changed.disconnect(
self._left_ok_pressed)
self._navigators['right'].button0_changed.disconnect(
self._right_ok_pressed)
self._navigators['torso_right'].button0_changed.disconnect(
self._right_ok_pressed)
# Navigator Wheel
self._navigators['left'].wheel_changed.disconnect(
self._left_wheel_moved)
self._navigators['torso_left'].wheel_changed.disconnect(
self._left_wheel_moved)
self._navigators['right'].wheel_changed.disconnect(
self._right_wheel_moved)
self._navigators['torso_right'].wheel_changed.disconnect(
self._right_wheel_moved)
# Navigator Baxter Button
self._navigators['left'].button2_changed.disconnect(self._enable)
self._navigators['torso_left'].button2_changed.disconnect(self._enable)
self._navigators['right'].button2_changed.disconnect(self._enable)
self._navigators['torso_right'].button2_changed.disconnect(
self._enable)
# Navigator Back Button
self._navigators['left'].button1_changed.disconnect(self.back)
self._navigators['torso_left'].button1_changed.disconnect(self.back)
self._navigators['right'].button1_changed.disconnect(self.back)
self._navigators['torso_right'].button1_changed.disconnect(self.back)
self._listeners_connected = False
def _load_config(self):
f = open(self.conf_path).read()
conf_data = json.loads(f)
for window in conf_data['Windows']:
buttons = dict()
if window['back']:
name = '%s_back' % window['name']
size = window['back']['size']
offset = window['back']['offset']
icon_prefix = 'Inner_Back'
icon_offset = window['back']['icon_offset']
buttons[name] = BrrButton(name, size, offset, 0, icon_prefix,
'TopSmall', icon_offset, '', 0, True,
self.share_path)
self._btn_context[name] = {
'nextWindow': window['parent'],
'function': 'Back'
}
if 'Buttons' in window.keys():
for btn in window['Buttons']:
buttons[btn['name']] = BrrButton(
btn['name'], btn['size'], btn['offset'], btn['index'],
btn['icon_prefix'], btn['button'], btn['icon_offset'],
btn['label'], btn['label_y'], btn['selectable'],
self.share_path)
self._btn_context[btn['name']] = {
'nextWindow': btn['nextWindow'],
'function': btn['function']
}
self.windows[window['name']] = BrrWindow(window, buttons,
self.share_path)
errors = conf_data['Error']
for error in errors['errors']:
name = error['name']
buttons = dict()
buttons['OK'] = BrrButton(
'%s_OK' % name, # name
[200, 60], # size
errors['OK']['offset'], # button offset
0, # index
None, # icon prefix
"Wide", # button type
[0, 0], # icon offset
"OK", # label
16, # label y-offset
True, # selectable?
self.share_path)
self._btn_context["%s_OK" % name] = {
'nextWindow': None,
'function': 'Back'
}
window = {
'name': '%s_error' % name,
'bg': errors['bg'],
'back': False,
'offset': errors['offset'],
'parent': False,
'default': '%s_OK' % name,
'no_scroll': False,
'text': [{
'text': error['text'],
'text_y': error['text_y']
}],
}
self.windows['%s_error' % name] = BrrWindow(
window, buttons, self.share_path)
for win in conf_data['Confirmation']['Windows']:
conf = conf_data['Confirmation']
name = win['name']
labels = list()
for text in win['messages']:
labels.append({'text': text['text'], 'text_y': text['text_y']})
buttons = dict()
buttons['OK'] = BrrButton(
'%s_OK' % name, # name
[200, 60], # size
conf['OK']['offset'], # button offset
1, # index
None, # icon prefix
"Wide", # button type
[0, 0], # icon offset
win['conf_text'], # label
16, # label y-offset
True, # selectable?
self.share_path)
self._btn_context['%s_OK' % name] = {
'nextWindow': win['nextWindow'],
'function': win['function']
}
buttons['Cancel'] = BrrButton('%s_Back' % name, [200, 60],
conf['Cancel']['offset'], 0, None,
"Wide", [0, 0], "Cancel", 16, True,
self.share_path)
self._btn_context['%s_Back' % name] = {
'nextWindow': win['parent'],
'function': 'Back'
}
window = {
'name': '%s_conf' % win['name'],
'bg': conf['bg'],
'back': False,
'offset': conf['offset'],
'parent': win['parent'],
'default': '%s_OK' % name,
'no_scroll': False,
'text': labels
}
self.windows['%s_conf' % name] = BrrWindow(window, buttons,
self.share_path)
def selected(self):
return self.active_window.selected_btn()
'''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Main Draw function.
# Converts the appropriate frame to a ros message and sends
# it to the screen.
# Also sets the current_frame parameter, in expectation of
# future hooks to merge images into the current view
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~'''
def draw(self):
img = Image.new('RGB', (1024, 600), 'white')
img = gen_cv(self._draw_window(img, self.active_window.name))
self.img = img
msg = cv_to_msg(img)
self.xdisp.publish(msg)
rospy.sleep(.1)
'''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Simple method that sets the active window based on the window's name
# and re-draws the UI.
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~'''
def set_active_window(self, name):
self.active_window = self.windows[name]
self.draw()
def _draw_window(self, img, window, selected=True):
if self.windows[window].parent:
img = self._draw_window(img,
window=self.windows[window].parent,
selected=False)
return self.windows[window].draw(img, selected)
'''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Functions linking wheel turns with scrolling in the UI
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~'''
def _left_wheel_moved(self, v):
self._wheel_moved(v, 'left')
def _right_wheel_moved(self, v):
self._wheel_moved(v, 'right')
def _wheel_moved(self, v, side):
if not self._active_example and self._wheel_ok:
if v > 0:
self.scroll(1)
else:
self.scroll(-1)
self._wheel_ok = False
rospy.Timer(rospy.Duration(.01), self._set_wheel_ok, oneshot=True)
def scroll(self, direction):
self.active_window.scroll(direction)
self.draw()
def _set_wheel_ok(self, event):
self._wheel_ok = True
'''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Functions linking pressing the OK button on either arm with
# the currently selected example
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~'''
def _left_ok_pressed(self, v):
self.ok_pressed(v, 'left')
def _right_ok_pressed(self, v):
self.ok_pressed(v, 'right')
def ok_pressed(self, v, side):
if v == True:
context = self._btn_context[self.selected().name]
func = self._btn_context[self.selected().name]['function']
if func == 'Back':
self.error_state = False
self.kill_examples()
mk_process('rm -rf /var/tmp/hlr/demo_calib.txt')
self.set_active_window(context['nextWindow'])
self.draw()
if func and func != 'Back':
getattr(self._functions, func)(self, side)
def back(self, v):
if v == True:
self.error_state = False
if self.active_window.parent:
self.kill_examples()
self.set_active_window(self.active_window.parent)
self.draw()
'''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Commands to enable the robot (if it is disabled when the demo
# starts) and to kill all currently running examples.
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~'''
def kill_examples(self, v=1):
rospy.loginfo('--@kill_examples')
self._active_example = False
self.selected()._status = 'selected'
for cmd in self._commands:
kill_python_procs(cmd)
if self.cam_sub != None:
self.cam_sub.unregister()
self.draw()
if not self.error_state:
self._enable()
def _check_enable(self, event):
self._enable()
def _enable(self, v=1):
if v == 1 and not self._status.state().enabled:
try:
self._status.enable()
except:
self.error_screen('stopped')
return False
if not self._status.state().enabled:
self.error_screen('no_enable')
return False
self._enable_cuff()
return True
def error_screen(self, error):
if self.error_state == False:
self.error_state = error
self.kill_examples()
error_screen = '%s_error' % error
if self.active_window.name.startswith('run'):
new_parent = self.active_window.parent
elif not self.active_window.name.endswith('_error'):
new_parent = self.active_window.name
self._btn_context['%s_OK' % error]['nextWindow'] = new_parent
self.windows[error_screen].parent = new_parent
self.set_active_window(error_screen)
self.draw()
def _enable_cuff(self):
if len(python_proc_ids('gripper_cuff_control')) == 0:
RosProcess('rosrun baxter_examples gripper_cuff_control.py')
class BrrUi(object):
def __init__(self, share_path, conf_path, commands):
self.share_path = share_path
self.conf_path = conf_path
self.windows = dict()
self._btn_context = dict()
self._functions = demo_functions
self._load_config()
self.img = Image.new('RGB', (1024, 600), 'white')
self.active_window = self.windows['demo_1']
self.xdisp = rospy.Publisher('/robot/xdisplay', ImageMsg,
latch=True, queue_size=1)
self._status = RobotEnable()
self._commands = commands
self._font = ImageFont.truetype(
'%s/HelveticaLight.ttf' % share_path, 30
)
self._textHeight = self._font.getsize('W')[1]
self._active_example = False
self._navigators = {'left': Navigator('left'),
'torso_left': Navigator('torso_left'),
'right': Navigator('right'),
'torso_right': Navigator('torso_right')}
self._listeners_connected = False
self._connect_listeners()
self._estop_state = False
self._estop_sub = rospy.Subscriber('robot/state', AssemblyState,
self._estop_callback)
self._wheel_ok = True
self.cameras = dict()
camera_list = ['left_hand', 'right_hand', 'head']
for idx, cam in enumerate(camera_list):
try:
self.cameras[cam] = CameraController('%s_camera' % cam)
except AttributeError:
try:
# This camera might not be powered
# Turn off the power to the last camera
# this will turn power on to the current camera
CameraController('%s_camera' % camera_list[idx-1]).close()
# And try again to locate the camera service
self.cameras[cam] = CameraController('%s_camera' % cam)
except AttributeError:
# This camera is unavailable (might be broken)
# Disable camera button in the UI
self.windows['cam_submenu'].set_btn_selectable('cam_%s' % cam,
False)
sel = self.windows['cam_submenu'].selected_btn()
bad_cam = self.windows['cam_submenu'].get_btn('cam_%s' % cam)
if (sel == bad_cam and
not self.windows['cam_submenu'].scroll(1)):
self.windows['cam_submenu'].selected_btn_index = 0
self.cam_sub = None
self._l_grip = {'interface': Gripper('left'), 'type': 'custom'}
self._r_grip = {'interface': Gripper('right'), 'type': 'custom'}
rospy.Timer(rospy.Duration(.5), self._check_enable)
self.error_state = False
self._enable()
self.calib_stage = 0
self.draw()
mk_process('rosrun baxter_tools tuck_arms.py -u')
def _estop_callback(self, msg):
if self._estop_state != msg.stopped:
self._estop_state = msg.stopped
if msg.stopped and self._listeners_connected:
self._disconnect_listeners()
elif not msg.stopped and not self._listeners_connected:
self._connect_listeners()
def _connect_listeners(self):
# Navigator OK Button
self._navigators['left'].button0_changed.connect(self._left_ok_pressed)
self._navigators['torso_left'].button0_changed.connect(
self._left_ok_pressed)
self._navigators['right'].button0_changed.connect(
self._right_ok_pressed)
self._navigators['torso_right'].button0_changed.connect(
self._right_ok_pressed)
# Navigator Wheel
self._navigators['left'].wheel_changed.connect(self._left_wheel_moved)
self._navigators['torso_left'].wheel_changed.connect(
self._left_wheel_moved)
self._navigators['right'].wheel_changed.connect(
self._right_wheel_moved)
self._navigators['torso_right'].wheel_changed.connect(
self._right_wheel_moved)
# Navigator Baxter Button
self._navigators['left'].button2_changed.connect(self._enable)
self._navigators['torso_left'].button2_changed.connect(self._enable)
self._navigators['right'].button2_changed.connect(self._enable)
self._navigators['torso_right'].button2_changed.connect(self._enable)
# Navigator Back Button
self._navigators['left'].button1_changed.connect(self.back)
self._navigators['torso_left'].button1_changed.connect(self.back)
self._navigators['right'].button1_changed.connect(self.back)
self._navigators['torso_right'].button1_changed.connect(self.back)
self._listeners_connected = True
def _disconnect_listeners(self):
# Navigator OK Button
self._navigators['left'].button0_changed.disconnect(
self._left_ok_pressed)
self._navigators['torso_left'].button0_changed.disconnect(
self._left_ok_pressed)
self._navigators['right'].button0_changed.disconnect(
self._right_ok_pressed)
self._navigators['torso_right'].button0_changed.disconnect(
self._right_ok_pressed)
# Navigator Wheel
self._navigators['left'].wheel_changed.disconnect(
self._left_wheel_moved)
self._navigators['torso_left'].wheel_changed.disconnect(
self._left_wheel_moved)
self._navigators['right'].wheel_changed.disconnect(
self._right_wheel_moved)
self._navigators['torso_right'].wheel_changed.disconnect(
self._right_wheel_moved)
# Navigator Baxter Button
self._navigators['left'].button2_changed.disconnect(self._enable)
self._navigators['torso_left'].button2_changed.disconnect(self._enable)
self._navigators['right'].button2_changed.disconnect(self._enable)
self._navigators['torso_right'].button2_changed.disconnect(self._enable)
# Navigator Back Button
self._navigators['left'].button1_changed.disconnect(self.back)
self._navigators['torso_left'].button1_changed.disconnect(self.back)
self._navigators['right'].button1_changed.disconnect(self.back)
self._navigators['torso_right'].button1_changed.disconnect(self.back)
self._listeners_connected = False
def _load_config(self):
f = open(self.conf_path).read()
conf_data = json.loads(f)
for window in conf_data['Windows']:
buttons = dict()
if window['back']:
name = '%s_back' % window['name']
size = window['back']['size']
offset = window['back']['offset']
icon_prefix = 'Inner_Back'
icon_offset = window['back']['icon_offset']
buttons[name] = BrrButton(name, size, offset, 0,
icon_prefix, 'TopSmall', icon_offset,
'', 0, True, self.share_path)
self._btn_context[name] = {'nextWindow': window['parent'],
'function': 'Back'}
if 'Buttons' in window.keys():
for btn in window['Buttons']:
buttons[btn['name']] = BrrButton(
btn['name'],
btn['size'],
btn['offset'],
btn['index'],
btn['icon_prefix'],
btn['button'],
btn['icon_offset'],
btn['label'],
btn['label_y'],
btn['selectable'],
self.share_path
)
self._btn_context[btn['name']] = {
'nextWindow': btn['nextWindow'],
'function': btn['function']
}
self.windows[window['name']] = BrrWindow(window,
buttons,
self.share_path)
errors = conf_data['Error']
for error in errors['errors']:
name = error['name']
buttons = dict()
buttons['OK'] = BrrButton(
'%s_OK' % name, # name
[200, 60], # size
errors['OK']['offset'], # button offset
0, # index
None, # icon prefix
"Wide", # button type
[0, 0], # icon offset
"OK", # label
16, # label y-offset
True, # selectable?
self.share_path
)
self._btn_context["%s_OK" % name] = {
'nextWindow': None,
'function': 'Back'
}
window = {
'name': '%s_error' % name,
'bg': errors['bg'],
'back': False,
'offset': errors['offset'],
'parent': False,
'default': '%s_OK' % name,
'no_scroll': False,
'text': [{'text': error['text'],
'text_y': error['text_y']}],
}
self.windows['%s_error' % name] = BrrWindow(window, buttons,
self.share_path)
for win in conf_data['Confirmation']['Windows']:
conf = conf_data['Confirmation']
name = win['name']
labels = list()
for text in win['messages']:
labels.append({'text': text['text'],
'text_y': text['text_y']})
buttons = dict()
buttons['OK'] = BrrButton(
'%s_OK' % name, # name
[200, 60], # size
conf['OK']['offset'], # button offset
1, # index
None, # icon prefix
"Wide", # button type
[0, 0], # icon offset
win['conf_text'], # label
16, # label y-offset
True, # selectable?
self.share_path)
self._btn_context['%s_OK' % name] = {
'nextWindow': win['nextWindow'],
'function': win['function']}
buttons['Cancel'] = BrrButton(
'%s_Back' % name,
[200, 60],
conf['Cancel']['offset'],
0,
None,
"Wide",
[0, 0],
"Cancel",
16,
True,
self.share_path
)
self._btn_context['%s_Back' % name] = {
'nextWindow': win['parent'],
'function': 'Back'}
window = {
'name': '%s_conf' % win['name'],
'bg': conf['bg'],
'back': False,
'offset': conf['offset'],
'parent': win['parent'],
'default': '%s_OK' % name,
'no_scroll': False,
'text': labels
}
self.windows['%s_conf' % name] = BrrWindow(window, buttons,
self.share_path)
def selected(self):
return self.active_window.selected_btn()
'''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Main Draw function.
# Converts the appropriate frame to a ros message and sends
# it to the screen.
# Also sets the current_frame parameter, in expectation of
# future hooks to merge images into the current view
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~'''
def draw(self):
img = Image.new('RGB', (1024, 600), 'white')
img = gen_cv(self._draw_window(img, self.active_window.name))
self.img = img
msg = cv_to_msg(img)
self.xdisp.publish(msg)
rospy.sleep(.1)
'''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Simple method that sets the active window based on the window's name
# and re-draws the UI.
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~'''
def set_active_window(self, name):
self.active_window = self.windows[name]
self.draw()
def _draw_window(self, img, window, selected=True):
if self.windows[window].parent:
img = self._draw_window(img,
window=self.windows[window].parent,
selected=False)
return self.windows[window].draw(img, selected)
'''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Functions linking wheel turns with scrolling in the UI
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~'''
def _left_wheel_moved(self, v):
self._wheel_moved(v, 'left')
def _right_wheel_moved(self, v):
self._wheel_moved(v, 'right')
def _wheel_moved(self, v, side):
if not self._active_example and self._wheel_ok:
if v > 0:
self.scroll(1)
else:
self.scroll(-1)
self._wheel_ok = False
rospy.Timer(rospy.Duration(.01), self._set_wheel_ok, oneshot=True)
def scroll(self, direction):
self.active_window.scroll(direction)
self.draw()
def _set_wheel_ok(self, event):
self._wheel_ok = True
'''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Functions linking pressing the OK button on either arm with
# the currently selected example
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~'''
def _left_ok_pressed(self, v):
self.ok_pressed(v, 'left')
def _right_ok_pressed(self, v):
self.ok_pressed(v, 'right')
def ok_pressed(self, v, side):
if v == True:
context = self._btn_context[self.selected().name]
func = self._btn_context[self.selected().name]['function']
if func == 'Back':
self.error_state = False
self.kill_examples()
mk_process('rm -rf /var/tmp/hlr/demo_calib.txt')
self.set_active_window(context['nextWindow'])
self.draw()
if func and func != 'Back':
getattr(self._functions, func)(self, side)
def back(self, v):
if v == True:
self.error_state = False
if self.active_window.parent:
self.kill_examples()
self.set_active_window(self.active_window.parent)
self.draw()
'''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Commands to enable the robot (if it is disabled when the demo
# starts) and to kill all currently running examples.
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~'''
def kill_examples(self, v=1):
rospy.loginfo('--@kill_examples')
self._active_example = False
self.selected()._status = 'selected'
for cmd in self._commands:
kill_python_procs(cmd)
if self.cam_sub != None:
self.cam_sub.unregister()
self.draw()
if not self.error_state:
self._enable()
def _check_enable(self, event):
self._enable()
def _enable(self, v=1):
if v == 1 and not self._status.state().enabled:
try:
self._status.enable()
except:
self.error_screen('stopped')
return False
if not self._status.state().enabled:
self.error_screen('no_enable')
return False
self._enable_cuff()
return True
def error_screen(self, error):
if self.error_state == False:
self.error_state = error
self.kill_examples()
error_screen = '%s_error' % error
if self.active_window.name.startswith('run'):
new_parent = self.active_window.parent
elif not self.active_window.name.endswith('_error'):
new_parent = self.active_window.name
self._btn_context['%s_OK' % error]['nextWindow'] = new_parent
self.windows[error_screen].parent = new_parent
self.set_active_window(error_screen)
self.draw()
def _enable_cuff(self):
if len(python_proc_ids('gripper_cuff_control')) == 0:
RosProcess('rosrun baxter_examples gripper_cuff_control.py')
class Mimic(object):
IKSVC_LEFT_URI = 'ExternalTools/left/PositionKinematicsNode/IKService'
IKSVC_RIGHT_URI = 'ExternalTools/right/PositionKinematicsNode/IKService'
def __init__(self):
self._tf = tf.TransformListener()
self._rs = RobotEnable()
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.5)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.5)
self._left_arm_neutral = None
self._right_arm_neutral = None
self._left_iksvc = rospy.ServiceProxy(Mimic.IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(Mimic.IKSVC_RIGHT_URI,
SolvePositionIK)
self._left_camera = CameraController('left_hand_camera')
self._right_camera = CameraController('right_hand_camera')
self._head_camera = CameraController('head_camera')
self._left_camera.close()
self._right_camera.close()
self._head_camera.close()
self._head_camera.resolution = CameraController.MODES[0]
self._head_camera.open()
self._head_camera_sub = rospy.Subscriber(
'/cameras/head_camera/image', Image,
self._head_camera_sub_callback)
self._xdisplay_pub = rospy.Publisher('/robot/xdisplay',
Image,
latch=True)
self._out_of_range = False
self._ik_smooth = 4
self._ik_rate = 200
self._avg_window = 1000
self._r_trans_prev = []
self._l_trans_prev = []
self._r_ik_prev = []
self._l_ik_prev = []
self._joint_update_pub = rospy.Publisher(
'/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(self._ik_rate)
self._mimic_timer = None
############################################################################
def start(self):
self._rs.enable()
self._reset()
self._mimic_timer = rospy.Timer(rospy.Duration(1.0 / self._ik_rate),
self._mimic_callback)
rate = rospy.Rate(self._avg_window)
while not rospy.is_shutdown():
try:
(r_trans, r_rot) = self._tf.lookupTransform(
'/skeleton/user_1/right_hand', '/skeleton/user_1/torso',
rospy.Time(0))
(l_trans, l_rot) = self._tf.lookupTransform(
'/skeleton/user_1/left_hand', '/skeleton/user_1/torso',
rospy.Time(0))
self._l_trans_prev.append(l_trans)
if (len(self._l_trans_prev) > self._avg_window):
self._l_trans_prev.pop(0)
self._r_trans_prev.append(r_trans)
if (len(self._r_trans_prev) > self._avg_window):
self._r_trans_prev.pop(0)
except:
self._out_of_range = True
rate.sleep()
continue
rate.sleep()
def _reset(self):
if self._mimic_timer:
self._mimic_timer.shutdown()
self._left_arm.move_to_neutral()
self._left_arm_neutral = self._left_arm.joint_angles()
self._right_arm.move_to_neutral()
self._right_arm_neutral = self._right_arm.joint_angles()
print(self._left_arm_neutral)
print(self._right_arm_neutral)
def shutdown(self):
self._reset()
############################################################################
def _mimic_callback(self, event):
try:
l_trans = (sum(map(lambda x: x[0], self._l_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[1], self._l_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[2], self._l_trans_prev)) /
self._avg_window)
r_trans = (sum(map(lambda x: x[0], self._r_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[1], self._r_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[2], self._r_trans_prev)) /
self._avg_window)
rh_x = clamp(0.65 + (l_trans[2] / 1.5), 0.5, 0.9)
rh_y = clamp(-l_trans[0], -0.8, 0)
rh_z = clamp(-l_trans[1], -0.1, 0.8)
lh_x = clamp(0.65 + (r_trans[2] / 1.5), 0.5, 0.9)
lh_y = clamp(-r_trans[0], 0, 0.8)
lh_z = clamp(-r_trans[1], -0.1, 0.8)
self.set_left_coords(lh_x, lh_y, lh_z)
self.set_right_coords(rh_x, rh_y, rh_z)
except:
return
def _head_camera_sub_callback(self, data):
orig_img = cvbr.imgmsg_to_cv2(data, 'rgb8')
img = cv2.cvtColor(orig_img, cv2.COLOR_RGB2GRAY)
img = cv2.equalizeHist(img)
img = cv2.GaussianBlur(img, (5, 5), 0)
img = cv2.Canny(img, 100, 200)
img = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB)
img = cv2.addWeighted(img, 0.75, orig_img, 0.25, 0)
img = cv2.resize(img, (1024, 768))
if self._out_of_range:
cv2.putText(img, 'Out of range!', (50, 768 - 50),
cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 0, 0), 4)
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
img = cvbr.cv2_to_imgmsg(img)
self._xdisplay_pub.publish(img)
############################################################################
def set_left_coords(self,
x,
y,
z,
er=math.pi * -1,
ep=math.pi * 0.5,
ey=math.pi * -1):
self._set_arm_coords(
self._left_iksvc, self._left_arm, x, y, z, er, ep, ey,
self._get_neutral_joint_state(self._left_arm_neutral),
self._l_ik_prev)
def set_right_coords(self,
x,
y,
z,
er=math.pi * -1,
ep=math.pi * 0.5,
ey=math.pi * -1):
self._set_arm_coords(
self._right_iksvc, self._right_arm, x, y, z, er, ep, ey,
self._get_neutral_joint_state(self._right_arm_neutral),
self._r_ik_prev)
############################################################################
def _set_arm_coords(self, iksvc, limb, x, y, z, er, ep, ey, njs, prev):
resp = self._get_ik(iksvc, x, y, z, er, ep, ey, njs)
positions = resp[0]
isValid = resp[1]
self._out_of_range = not isValid
prev.append(positions)
if (len(prev) > self._ik_smooth):
prev.pop(0)
smooth_positions = {}
for joint_name in positions:
smooth_positions[joint_name] = \
sum(map(lambda x: x[joint_name], prev)) / self._ik_smooth
limb.set_joint_positions(smooth_positions)
def _get_ik(self, iksvc, x, y, z, er, ep, ey, njs):
q = quaternion_from_euler(er, ep, ey)
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(position=Point(
x=x,
y=y,
z=z,
),
orientation=Quaternion(q[0], q[1], q[2], q[3])),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
ikreq.seed_angles.append(njs)
iksvc.wait_for_service(1.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
def _get_neutral_joint_state(self, njs):
js = JointState()
js.header = Header(stamp=rospy.Time.now(), frame_id='base')
for j in njs:
js.name.append(j)
js.position.append(njs[j])
return js
def __init__(self, share_path, conf_path, commands):
self.share_path = share_path
self.conf_path = conf_path
self.windows = dict()
self._btn_context = dict()
self._functions = demo_functions
self._load_config()
self.img = Image.new('RGB', (1024, 600), 'white')
self.active_window = self.windows['demo_1']
self.xdisp = rospy.Publisher('/robot/xdisplay', ImageMsg,
latch=True, queue_size=1)
self._status = RobotEnable()
self._commands = commands
self._font = ImageFont.truetype(
'%s/HelveticaLight.ttf' % share_path, 30
)
self._textHeight = self._font.getsize('W')[1]
self._active_example = False
self._navigators = {'left': Navigator('left'),
'torso_left': Navigator('torso_left'),
'right': Navigator('right'),
'torso_right': Navigator('torso_right')}
self._listeners_connected = False
self._connect_listeners()
self._estop_state = False
self._estop_sub = rospy.Subscriber('robot/state', AssemblyState,
self._estop_callback)
self._wheel_ok = True
self.cameras = dict()
camera_list = ['left_hand', 'right_hand', 'head']
for idx, cam in enumerate(camera_list):
try:
self.cameras[cam] = CameraController('%s_camera' % cam)
except AttributeError:
try:
# This camera might not be powered
# Turn off the power to the last camera
# this will turn power on to the current camera
CameraController('%s_camera' % camera_list[idx-1]).close()
# And try again to locate the camera service
self.cameras[cam] = CameraController('%s_camera' % cam)
except AttributeError:
# This camera is unavailable (might be broken)
# Disable camera button in the UI
self.windows['cam_submenu'].set_btn_selectable('cam_%s' % cam,
False)
sel = self.windows['cam_submenu'].selected_btn()
bad_cam = self.windows['cam_submenu'].get_btn('cam_%s' % cam)
if (sel == bad_cam and
not self.windows['cam_submenu'].scroll(1)):
self.windows['cam_submenu'].selected_btn_index = 0
self.cam_sub = None
self._l_grip = {'interface': Gripper('left'), 'type': 'custom'}
self._r_grip = {'interface': Gripper('right'), 'type': 'custom'}
rospy.Timer(rospy.Duration(.5), self._check_enable)
self.error_state = False
self._enable()
self.calib_stage = 0
self.draw()
mk_process('rosrun baxter_tools tuck_arms.py -u')
#!/usr/bin/env python
from baxter_interface import RobotEnable, Gripper
import rospy
import os
if __name__ == '__main__':
rospy.init_node('Prepper')
enableObj = RobotEnable()
enableObj.enable()
lGrip = Gripper('left')
rGrip = Gripper('right')
lGrip.calibrate()
rGrip.calibrate()
lGrip.open()
rGrip.open()
os.system(
"~/catkin_ws/baxter.sh; rosrun baxter_examples gripper_cuff_control.py -g both"
)
def __init__(self, share_path, conf_path, commands):
self.share_path = share_path
self.conf_path = conf_path
self.windows = dict()
self._btn_context = dict()
self._functions = demo_functions
self._load_config()
self.img = Image.new('RGB', (1024, 600), 'white')
self.active_window = self.windows['demo_1']
self.xdisp = rospy.Publisher('/robot/xdisplay',
ImageMsg,
latch=True,
queue_size=1)
self._status = RobotEnable()
self._commands = commands
self._font = ImageFont.truetype('%s/HelveticaLight.ttf' % share_path,
30)
self._textHeight = self._font.getsize('W')[1]
self._active_example = False
self._navigators = {
'left': Navigator('left'),
'torso_left': Navigator('torso_left'),
'right': Navigator('right'),
'torso_right': Navigator('torso_right')
}
self._listeners_connected = False
self._connect_listeners()
self._estop_state = False
self._estop_sub = rospy.Subscriber('robot/state', AssemblyState,
self._estop_callback)
self._wheel_ok = True
self.cameras = dict()
camera_list = ['left_hand', 'right_hand', 'head']
for idx, cam in enumerate(camera_list):
try:
self.cameras[cam] = CameraController('%s_camera' % cam)
except AttributeError:
try:
# This camera might not be powered
# Turn off the power to the last camera
# this will turn power on to the current camera
CameraController('%s_camera' %
camera_list[idx - 1]).close()
# And try again to locate the camera service
self.cameras[cam] = CameraController('%s_camera' % cam)
except AttributeError:
# This camera is unavailable (might be broken)
# Disable camera button in the UI
self.windows['cam_submenu'].set_btn_selectable(
'cam_%s' % cam, False)
sel = self.windows['cam_submenu'].selected_btn()
bad_cam = self.windows['cam_submenu'].get_btn('cam_%s' %
cam)
if (sel == bad_cam
and not self.windows['cam_submenu'].scroll(1)):
self.windows['cam_submenu'].selected_btn_index = 0
self.cam_sub = None
self._l_grip = {'interface': Gripper('left'), 'type': 'custom'}
self._r_grip = {'interface': Gripper('right'), 'type': 'custom'}
rospy.Timer(rospy.Duration(.5), self._check_enable)
self.error_state = False
self._enable()
self.calib_stage = 0
self.draw()
mk_process('rosrun baxter_tools tuck_arms.py -u')
