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)
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 __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 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...'
#!/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')
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:
