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: