Esempio n. 1
0
	def __init__(self):
		rospy.init_node("baxter_test")
		self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
		self._init_state = self._rs.state().enabled
		self._rs.enable()
		self._rate = 5.0 #Hz

		#IK
		self.ns = "ExternalTools/left/PositionKinematicsNode/IKService"
		self.hdr = Header(stamp = rospy.Time(0), frame_id = "base")
		self.iksvc = rospy.ServiceProxy(self.ns, SolvePositionIK)
		self.ikreq = SolvePositionIKRequest()
		self._left_arm = baxter_interface.limb.Limb("left")
		self._right_arm = baxter_interface.limb.Limb("right")
		self._left_joint_names = self._left_arm.joint_names()
		self._right_joint_names = self._right_arm.joint_names()
		self._left_angles = [self._left_arm.joint_angle(joint) for joint in self._left_joint_names]
		self._right_angles = [self._right_arm.joint_angle(joint) for joint in self._right_joint_names]
Esempio n. 2
0
    def __init__(self, limb, verbose=True):
        self._limb_name = limb  # string
        self._verbose = verbose  # bool
        self._limb = baxter_interface.Limb(limb)
        self._gripper = baxter_interface.Gripper(limb)
        self._limb.set_joint_position_speed(0.1)
        self._gripper.calibrate()
        ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
        self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
        rospy.wait_for_service(ns, 5.0)

        # verify robot is enabled
        print("Getting robot state... ")
        self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
        self._init_state = self._rs.state().enabled

        print("Enabling robot... ")
        self._rs.enable()
Esempio n. 3
0
    def __init__(self):
        self._limb = baxter_interface.limb.Limb("right")
        self._angles = self._limb.joint_angles()
        # open left gripper
        #self.leftGripper =baxter_interface.Gripper('left')
        #self.leftGripper.calibrate()
        #self.leftGripper.open()
        # open Right gripper
        self.rightGripper = baxter_interface.Gripper('right')
        self.rightGripper.calibrate()
        self.rightGripper.open()
        # set control params

        print("Getting robot state...")
        self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
        self._init_state = self._rs.state().enabled
        print("Enabling robot..")
        self._rs.enable()
Esempio n. 4
0
 def __init__(self, limb, hover_distance=0.15, verbose=True):
     self._limb_name = limb  # string
     self._hover_distance = hover_distance  # in meters
     self._verbose = verbose  # bool
     self._limb = baxter_interface.Limb(limb)
     self.joint_space = len(self._limb.joint_angles())
     self._gripper = baxter_interface.Gripper(limb)
     ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
     self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
     rospy.wait_for_service(ns, 5.0)
     # verify robot is enabled
     print("Getting robot state... ")
     self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
     self._init_state = self._rs.state().enabled
     print("Enabling robot... ")
     self._rs.enable()
     self.ctr = 0
     self.canSubscriber = True
Esempio n. 5
0
    def __init__(self):
        # initialize interfaces
        rs = baxter_interface.RobotEnable(CHECK_VERSION)
        init_state = rs.state().enabled

        self.leftGripper = baxter_interface.Gripper('left', CHECK_VERSION)
        self.rightGripper = baxter_interface.Gripper('right', CHECK_VERSION)

        # MANUAL JOINT CONTROL
        self.leftLimb = baxter_interface.Limb('left')
        self.rightLimb = baxter_interface.Limb('right')
        self.manualJointAccuracy = baxter_interface.settings.JOINT_ANGLE_TOLERANCE
        self.manualJointTimeout = 20.0

        # JOINT SPEEDS
        self.manualJointSpeed = 0.3
        self.leftLimb.set_joint_position_speed(self.manualJointSpeed)
        self.rightLimb.set_joint_position_speed(self.manualJointSpeed)
Esempio n. 6
0
    def __init__(self):
        """
        'Waves' right arm by driving the joint velocities to sinusoid functions

        """
        self._pub_rate = rospy.Publisher('/robot/joint_state_publish_rate',
                                         UInt16)
        self._left_arm = baxter_interface.limb.Limb("left")
        self._right_arm = baxter_interface.limb.Limb("right")
        self._right_joint_names = self._right_arm.joint_names()
        self._left_joint_names = self._left_arm.joint_names()
        self._head = baxter_interface.Head()

        # set joint state publishing to 500Hz
        self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
        self._init_state = self._rs.state().enabled
        self._rs.enable()
        self._pub_rate.publish(500)
Esempio n. 7
0
 def __init__(self, limb, hover_distance=0.15, verbose=True):
     self._limb_name = limb  # string
     self._hover_distance = hover_distance  # in meters
     self._verbose = verbose  # bool
     self._limb = baxter_interface.Limb(limb)
     self._gripper = baxter_interface.Gripper(limb)
     ns = "ExternalTools/" + self._limb_name + "/PositionKinematicsNode/IKService"
     self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
     rospy.wait_for_service(ns, 5.0)
     # verify robot is enabled
     print("Getting robot state... ")
     self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
     self._init_state = self._rs.state().enabled
     print("Enabling robot... ")
     self._rs.enable()
     self.block_detected = False
     self.block_poses = []
     self.pose = Pose()
Esempio n. 8
0
    def __init__(self):

        self._speak_ctr = PollySpeech()
        self._mbot_relay = MultiBotInterface()
        self._face_ctr = FaceController()

        #enable robot
        enable_robot = baxter_interface.RobotEnable()
        enable_robot.enable()

        rospack = rospkg.RosPack()
        self._package_path = rospack.get_path(
            'hri19_multirobot_transition_study')

        #to deal with SST
        rospy.Subscriber('stt', Speech, self._stt_callback, queue_size=1)
        self._stt_toggle = rospy.ServiceProxy('toggle_stt', SetBool)
        self._last_sentence = ""
        self._stt_condition = threading.Condition()
        self._stt_event = threading.Event()
        self._pause_flag = True
        self._nlu_engine = SnipsNLU()

        self._head = baxter_interface.Head()

        self._running = True

        #memory for each session
        self._temp_memory = dict()
        self._memory_stack = list()

        self._index_stack = list()
        self._index = 0
        """ The following are the list of actions/behaviors
        that exist for the robot, each action's name directly correspond
        to the same name of the function
        """
        self._operation_list = [
            'speak', 'move_to_posture', 'move_head', 'change_face',
            'natural_language_understanding', 'wait_response', 'wait',
            'yes_no_question', 'signal_robot', 'wait_for_signal', 'run_file',
            'choice_question', 'load_param'
        ]
        rospy.loginfo("RUN Complete Initiailization")
Esempio n. 9
0
def main():
    print("Initializing node... ")
    rospy.init_node("set_joint_positions")
#################Resposible for setting joint positions for Baxter ############################################    
    print("Getting robot state... ")
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state().enabled

    def clean_shutdown():
        print("\nExiting example...")
        if not init_state:
            print("Disabling robot...")
            rs.disable()

    print("Enabling robot... ")
    rs.enable()
    #right_e1 is special.   joint limit [-0.05] is the reason. keeping it zero for now 

    #left_e1 is special. joint limit [-0.05] is the reason. keeping it zero for now 
    # joint_name_left=["left_s0", "left_s1", "left_e0", "left_e1", "left_w0", "left_w1", "left_w2"]
    # joint_name_right=["right_e0", "right_e1", "right_s0", "right_s1", "right_w0", "right_w1", "right_w2"]

    ################################################################################################################### 
    # joint_name_left=["left_e0", "left_e1", "left_s0", "left_s1", "left_w0", "left_w1", "left_w2"]

    # joint_desired_state_right=[0.7410703227630888, 1.5362719506658014, 0.01001108907777759, -0.7006024655803085, 0.9735489859081357, 1.447864944629445, -0.4520477771263609]
    # joint_desired_state_left=[-0.02477285952504804, 1.5454976873355646, -0.34985937977411385, -0.862626895167157, -1.3115418478918244, 1.8487306917056667, -0.892602931497076]    
    ###################################################################################################################
    joint_name_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
    joint_desired_state_left= [0.041363752507206364, 1.9348352917089535, -0.2726798000286923, -0.5658538514940457, -1.1000522708348006, 1.7430249026235884, -0.1936378991920895]
    joint_name_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
    joint_desired_state_right = [0.6068619915741271, 1.911764079997937, -0.13185736132604742, -0.4816721820621366, 0.790763174233601, 1.304727973182544, -1.0247054998116605]

    
    joint_name_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
    joint_desired_state_left= [0.4345000581685434, 1.4078108680818384, -1.117505003974524, -0.8383205005793786, -0.35128160042575973, 1.0530778108833365, 0.20401944478876002]
    joint_name_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
    joint_desired_state_right = [-0.33824276372873374, 1.5082866096883332, 1.035053536625683, -0.9276748814737039, 0.25080585881926515, 1.0895098545956152, -0.07209709703061444]
    setJointPositions(joint_name_right, joint_name_left, joint_desired_state_right, joint_desired_state_left)
######################################################################################################################
#################Resposible for getting joint positions for Baxter from /robot/joint_states topic ############################################  

    joint_states_current_right, joint_states_current_left=getCurrentJointState(joint_name_right, joint_name_left)    
    printSetPositionResult(joint_desired_state_right, joint_states_current_right, joint_desired_state_left, joint_states_current_left,joint_name_right, joint_name_left)
    def __init__(self, limb, reconfig_server):
        self._dyn = reconfig_server

        # control parameters
        self._rate = 1000.0  # Hz
        self._missed_cmds = 20.0  # Missed cycles before triggering timeout
        self.joint_names = ['left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
        # define start and end time to calcualte velocity
        self._end = time()
        self._start = time()
        # create our limb instance
        self._limb = baxter_interface.Limb(limb)
        self._start_angles = dict()
        # self._dof_names = {'x': 1, 'y': 2, 'z': 3}
        self._dof_names = ['x', 'y', 'z']
        # initialize parameters
        self._springs = dict()
        self._damping = dict()
        self._start_pos = [0, 0, 0]
        self._last_pos = self._start_pos
        self._kin = baxter_kinematics('left')
        # gravity compensation
        self._gravity_comp_effort = dict()
        self._gravity_comp_effort_ok = False
        gravity_comp_topic = '/robot/limb/left/gravity_compensation_torques'
        self._gravity_comp_sub = rospy.Subscriber(
            gravity_comp_topic,
            SEAJointState,
            self._on_gravity_comp,
            queue_size=1,
            tcp_nodelay=True)

        # create cuff disable publisher
        cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
        self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)

        # verify robot is enabled
        print("Getting robot state... ")
        self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
        self._init_state = self._rs.state().enabled
        print("Enabling robot... ")
        self._rs.enable()
        print("Running. Ctrl-c to quit")
        self.safe_file = open('mani.csv', 'w')
Esempio n. 11
0
    def __init__(self, arm, distance):
        global image_directory

        baxter_interface.RobotEnable().enable()

        # arm ("left" or "right")
        self.limb = arm
        self.distance = distance
        self.limb_interface = baxter_interface.Limb(self.limb)

        self.gripper = baxter_interface.Gripper(arm)
        self.gripper.calibrate()

        self.other_limb_interface = baxter_interface.Limb("left")
        self.baxter_ik_move("left", (0.25, 0.50, 0.2, math.pi, 0.0, 0.0))
        self.limb_interface.set_joint_position_speed(0.1)
        self.other_limb_interface.set_joint_position_speed(0.1)

        #self.open_camera(arm)
        self.bridge = CvBridge()

        # image directory
        self.image_dir = image_directory

        # flag to control saving of analysis images
        self.save_images = True

        # required position accuracy in metres
        # self.ball_tolerance = 0.005
        # self.tray_tolerance = 0.05

        # number of balls found
        # self.balls_found = 0

        # start positions
        self.origin_x = 0.6  # x     = front back   0.50    0.70(good)
        self.origin_y = -0.1  # y     = left right   0.30    -0.1(good)
        self.origin_z = 0  # z     = up down    0.15
        self.roll = -1.0 * math.pi  # roll  = horizontal
        self.pitch = 0.0 * math.pi  # pitch = vertical
        self.yaw = 0.0 * math.pi  # yaw   = rotation

        self.pose = [self.origin_x, self.origin_y, self.origin_z,     \
                     self.roll, self.pitch, self.yaw]
Esempio n. 12
0
 def __init__(self):
     # save the output messages into .txt file in order to draw plot
     #self.saveFile = open('record.txt', 'w')
     #self.saveFile1 = open('record1.txt', 'w')
     self.limb_interface = baxter_interface.Limb("left")
     # start positions
     self.x = 0.62  # x     = front back
     self.y = 0.14  # y     = left right
     self.z = 0.14  # z     = up down
     self.roll = -1.0 * math.pi  # roll  = horizontal
     self.pitch = 0.0 * math.pi  # pitch = vertical
     self.yaw = 0.0 * math.pi  # yaw   = rotation
     self.pose = [self.x, self.y, self.z, self.roll, self.pitch, self.yaw]
     print(self.pose)
     # Enable the actuators
     baxter_interface.RobotEnable().enable()
     # set speed as a ratio of maximum speed
     self.limb_interface.set_joint_position_speed(0.5)
     # height of table
     self.table = -0.077
     # camera parameter
     # This is a very interesting argument. Actually this is a simplification of the camera projection transformation.
     # This parameter works only when the camera is vertical to the detection surface!
     self.cam_calib = 0.0025  # meters per pixel at 1 meter.
     self.width = 960  # Camera resolution
     self.height = 600
     # bridge to use opencv to process images
     self.bridge = CvBridge()
     # subscribe image message from webcam
     self.image_sub = rospy.Subscriber("/cameras/left_hand_camera/image",
                                       Image, self.callback)
     # setup topic for images with detection results
     self.image_pub = rospy.Publisher('/image_bridge',
                                      Image,
                                      latch=True,
                                      queue_size=10)
     self.XL_coo = rospy.Publisher('/Xcoordinate_left',
                                   Float32,
                                   latch=True,
                                   queue_size=10)
     self.YL_coo = rospy.Publisher('/Ycoordinate_left',
                                   Float32,
                                   latch=True,
                                   queue_size=10)
Esempio n. 13
0
    def __init__(self,
                 shape,
                 desired_position,
                 limb,
                 hover_distance=0.15,
                 verbose=False):
        self._limb_name = limb  # string
        self._shape_name = shape  # string
        self._desired_position = desired_position  # list[x,y] (integer)
        self._hover_distance = hover_distance  # in meters
        self._verbose = verbose  # bool
        self._limb = baxter_interface.limb.Limb(limb)
        self._limb_joint_names = self._limb.joint_names()
        self.prev_time = 0.0
        self.control_effort_x = 0.0
        self.control_effort_y = 0.0
        self.object_position = Object()
        self.hover_position = Pose()
        self.diff_old = 0.0
        self.zposi = 0.0

        self.step = 0
        self._rate = 10  #10Hz
        self.display_pub = rospy.Publisher('/robot/xdisplay',
                                           Image,
                                           queue_size=60)
        self.pub_grasp_now = rospy.Publisher("pump_on", Empty, queue_size=1)
        self.pub_release_now = rospy.Publisher("pump_off", Empty, queue_size=1)
        self.sub = rospy.Subscriber('/detected_image', Image, self.republish,
                                    None, 1)
        #msg_grasp_now = Bool(False)
        #self.pub_grasp_now.publish(msg_grasp_now)

        ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
        self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
        rospy.wait_for_service(ns, 5.0)
        # verify robot is enabled
        print("Getting robot state... ")
        self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
        self._init_state = self._rs.state().enabled
        print("Enabling robot... ")
        self._rs.enable()
        print("Moving to neutral pose...")
        self._limb.move_to_neutral()
Esempio n. 14
0
def main():

    # arg_fmt = argparse.RawDescriptionHelpFormatter
    # parser = argparse.ArgumentParser(formatter_class=arg_fmt,
    #                                  description=main.__doc__)
    # parser.add_argument(
    #     '-l', '--limb', choices=['left', 'right'], required=True,
    #     help="the limb to test"
    # )
    # args = parser.parse_args(rospy.myargv()[1:])

    print("Getting robot state... ")
    rospy.init_node("IKSolver")
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state().enabled
    limb = baxter_interface.Limb("right")
    right = baxter_interface.Gripper('right', CHECK_VERSION)

    # return ik_test(args.limb)

    goToOrigin(limb)
    current_pose = limb.endpoint_pose()
    print current_pose

    print limb.joint_angles()

    running = True
    while (running):
        x = raw_input("X Value?\n")
        y = raw_input("Y Value?\n")
        z = raw_input("Z Value?\n")

        joints = solveIK('right', float(x), float(y), float(z),
                         current_pose['orientation'].x,
                         current_pose['orientation'].y,
                         current_pose['orientation'].z,
                         current_pose['orientation'].w)

        if (x == " " or y == " " or z == " "):
            running = False
        else:
            if (joints):
                print "moving"
                limb.move_to_joint_positions(joints)
    def __init__(self, verbose=False):
        if not ProxyTrajectoryClient._is_initialized:
            ProxyTrajectoryClient._is_initialized = True
            Logger.loginfo(
                "Initializing proxy FollowJointTrajectory client...")

            ProxyTrajectoryClient._client = ProxyActionClient({
                ProxyTrajectoryClient._action_topic:
                FollowJointTrajectoryAction
            })

            ProxyTrajectoryClient._verbose = verbose

            # enable the IK Service
            ik_ns = "ExternalTools/" + ProxyTrajectoryClient._limb + "/PositionKinematicsNode/IKService"
            ProxyTrajectoryClient._iksvc = rospy.ServiceProxy(
                ik_ns, SolvePositionIK)
            rospy.wait_for_service(ik_ns, 5.0)

            # enable the Baxter
            ProxyTrajectoryClient._rs = baxter_interface.RobotEnable(
                baxter_interface.CHECK_VERSION)
            ProxyTrajectoryClient._init_state = ProxyTrajectoryClient._rs.state(
            ).enabled
            print("Enabling robot... ")
            ProxyTrajectoryClient._rs.enable()

            #  enable baxter limb interface
            ProxyTrajectoryClient._baxter_limb_interface = baxter_interface.limb.Limb(
                ProxyTrajectoryClient._limb)

            #  Get the names from joints
            ProxyTrajectoryClient._joint_name = [
                joint for joint in
                ProxyTrajectoryClient._baxter_limb_interface.joint_names()
            ]

            ProxyTrajectoryClient._gripper = baxter_interface.Gripper(
                ProxyTrajectoryClient._limb)

        self._goal = FollowJointTrajectoryGoal()
        self._result = None
        self._goal_time_tolerance = rospy.Time(0.1)
        self._goal.goal_time_tolerance = self._goal_time_tolerance
Esempio n. 16
0
    def __init__(self, arm):
        self.limb = arm
        self.limb_interface = baxter_interface.Limb(arm)
        self.gripper = baxter_interface.Gripper(arm)

        moveit_commander.roscpp_initialize(sys.argv)
        self.robot = moveit_commander.RobotCommander()
        self.scene = moveit_commander.PlanningSceneInterface()

        self.tolerance = 0.005

        self.pos_x = 0.50
        self.pos_y = 0.00
        self.pos_z = 0.05
        self.or_x = 0.0
        self.or_y = -1.0
        self.or_z = 0.0
        self.or_w = 0.0

        self.pose = [self.pos_x, self.pos_y, self.pos_z, self.or_x, self.or_y, self.or_z, self.or_w]

        self.hough_accumulator = 30
        self.hough_min_radius = 35
        self.hough_max_radius = 70

        self.cam_calib    = 0.0004
        self.cam_x_offset = 0.045
        self.cam_y_offset = -0.01
        self.width        = 960
        self.height       = 600

        self.cv_image = cv.CreateImage((self.width, self.height), 8, 3)

        baxter_interface.RobotEnable().enable()

        self.limb_interface.set_joint_position_speed(0.5)

        self.gripper.calibrate()

        self.reset_camera(self.limb, self.width, self.height)

        self.subscribe_to_camera(self.limb)

        self.distance = 0
Esempio n. 17
0
    def __init__(self, limb, verbose=True):
        # Initialize parameters
        self._limb_name = limb
        self._verbose = False
        self._limb = baxter_interface.Limb(limb)
        self._gripper = baxter_interface.Gripper(limb)
        self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
        self._init_state = self._rs.state().enabled
        left = baxter_interface.Gripper('left', baxter_interface.CHECK_VERSION)
        if self._verbose:
            print("Getting robot state... ")
            print("Enabling robot... ")
        self._rs.enable()
        if self._verbose:
            print("Calibrating gripper...")
        left.calibrate()

        # Node cycle rate (in Hz).
        self.loop_rate = rospy.Rate(100)
Esempio n. 18
0
def main(limb):
    print("Initializing node... ")
    rospy.init_node("rethink_rsdk_joint_trajectory_controller_test")
    print("Getting robot state... ")
    rs = baxter_interface.RobotEnable()
    print("Enabling robot... ")
    rs.enable()
    print("Running. Ctrl-c to quit")
    positions = {
        'left': [-0.11, -0.62, -1.15, 1.32, 0.80, 1.27, 2.39],
        'right': [0.11, -0.62, 1.15, 1.32, -0.80, 1.27, -2.39],
    }
    p1 = positions[limb]
    traj = Trajectory(limb)
    traj.add_point(p1, 7.0)
    traj.add_point([x * 0.75 for x in p1], 9.0)
    traj.add_point([x * 1.25 for x in p1], 12.0)
    traj.start()
    traj.wait()
Esempio n. 19
0
    def __init__(self):

        print(' initializing Tf2AnglesController node ')

        rospy.init_node('Tf2AnglesController', anonymous=True)

        # Enable the Robot
        self.__robot = baxter_interface.RobotEnable(CHECK_VERSION)

        # Robot Limbs
        self.__left_limb = baxter_interface.limb.Limb('left')
        self.__right_limb = baxter_interface.limb.Limb('right')

        self.__rate = rospy.Rate(10)
        self.__command_joint_angles = dict()

        self.__enable_robot_and_move_to_neutral_pose()
        self.__setup_subscribers()
        self.__setup_command_joint_angles()
Esempio n. 20
0
def main():

    rospy.init_node("rsdk_test")
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state().enabled

    def clean_shutdown():
        if not init_state:
            rs.disable()
    rospy.on_shutdown(clean_shutdown)

    rs.enable()

    print('1. velocity \n 2. path')
    c = raw_input()
    if c == 1:
        run_velocity_loop()
    else if c == 2:
        run_path_loop()
    def __init__(self):
        """
        'Wobbles' both arms by commanding joint velocities sinusoidally.
        """
        self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate',
                                         UInt16)
        self._left_arm = baxter_interface.limb.Limb("left")
        self._right_arm = baxter_interface.limb.Limb("right")
        self._left_joint_names = self._left_arm.joint_names()
        self._right_joint_names = self._right_arm.joint_names()

        print("Getting robot state... ")
        self._rs = baxter_interface.RobotEnable()
        self._init_state = self._rs.state().enabled
        print("Enabling robot... ")
        self._rs.enable()

        # set joint state publishing to 100Hz
        self._pub_rate.publish(100)
def main():
    rospy.init_node("pick_and_place_service")

    parser = argparse.ArgumentParser(
        description='Process Pick and Place Command Line Arguments')
    parser.add_argument('--save', '-s', type=str)
    parser.add_argument('--read', '-r', type=str)
    args = parser.parse_args()

    rs = baxter_interface.RobotEnable()
    print("Enabling Robot")
    rs.enable()
    pp = PickPlace('right')

    # Calibrate Pickup Locations
    if args.read:
        print "Reading Position File: " + args.read
        pp.ReadCalibration(args.read)
    else:
        pp.CalibrateObjects()

    # Post Params
    pp.PostParameters()
    # Save Calibration to File
    if args.save:
        print "Saving Calibration File: " + args.save
        pp.SaveCalibration(args.save)

    # Advertise Service
    s = rospy.Service('pick_and_place_object', pick_and_place,
                      pp.PickAndPlaceObject)

    s_2 = rospy.Service('pick_and_place_check', pick_and_place,
                        pp.PickAndPlaceCheck)

    s_3 = rospy.Service('pick_and_place_state', pick_and_place_state,
                        pp.PickAndPlaceState)

    s_4 = rospy.Service('pick_and_place_stop', pick_and_place_stop,
                        pp.PickAndPlaceStop)

    print "READY to PICK and Place"
    rospy.spin()
def main():

    global data_s, mutex

    t = threading.Thread(target=receive)
    t.setDaemon(True)
    t.start()

    print("Initializing node... ")
    rospy.init_node('moveit_demo')
    rospy.sleep(1)

    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    limb = baxter_interface.Limb("right")
    # limb.move_to_neutral()

    moveit = MoveItDemo()

    print "Waiting to receive messages..."
    while True:

        if data_s in [
                "fight", "fight0", "handshake", "fetch", "hello", "salute"
        ]:

            moveit.execute(data_s)  #execute action

            print "reset..."  #reset pose
            rs.reset()
            print "move to natual..."
            limb.move_to_neutral()

            if mutex.acquire():  #clear command
                data_s = ""
                mutex.release()

        # if data_s == "exit":
        # 	moveit_commander.roscpp_shutdown()
        # 	moveit_commander.os._exit(0)
        # 	break

    UDPSock.close()
    os._exit(0)
Esempio n. 24
0
def image_callback(ros_img):
	
	
	pattern = cv2.imread("/home/crl7/ros_ws/marker.jpg")
	rs = baxter_interface.RobotEnable(CHECK_VERSION)
        #get robot state
        init_state = rs.state().enabled
        #enable robot
        rs.enable()
	
	
	#left arm initial state
	left = baxter_interface.Limb('left')
    	initial_left = left.endpoint_pose()['position'][:] + left.endpoint_pose()['orientation'][:]
    	Pose_state_left = [x for x in initial_left]
    	limb = 'left'
	
	
	ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
def listener():
    rospy.init_node('end_effector_trajectory_client', anonymous=True)
    fine = False
    rate = rospy.Rate(1)
    while not fine | rospy.is_shutdown():
        try:
            rs = baxter_interface.RobotEnable()
            rs.enable()
            fine = True
        except (OSError):
            rospy.logwarn("Can not enable robot.Will keep trying...")
            rate.sleep()
    if rospy.is_shutdown():
        return

    rospy.Subscriber("end_effector_command_solution", JointCommand, callback)

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()
Esempio n. 26
0
 def __init__(self, limb, hover_distance=0.15, verbose=True):
     self._limb_name = limb  # string
     self._hover_distance = hover_distance  # in meters
     self._verbose = verbose  # bool
     self.cap_status = '1.0'  #string
     self.status = 'working'  #string
     self._limb = baxter_interface.Limb(limb)
     self._gripper = baxter_interface.Gripper(limb)
     ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
     self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
     rospy.wait_for_service(ns, 5.0)
     # verify robot is enabled
     print("Getting robot state... ")
     self._rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
     self._init_state = self._rs.state().enabled
     print("Enabling robot... ")
     self._rs.enable()
     self.cap_sub = rospy.Subscriber('capflag', Float64,
                                     self.callback)  #initialize listener
    def __init__(self, limb, amplification=1.0):
        """
        Puppets one arm with the other.

        @param limb: the control arm used to puppet the other
        @param amplification: factor by which to amplify the arm movement
        """
        puppet_arm = {"left": "right", "right": "left"}
        self._control_limb = limb
        self._puppet_limb = puppet_arm[limb]
        self._control_arm = baxter_interface.limb.Limb(self._control_limb)
        self._puppet_arm = baxter_interface.limb.Limb(self._puppet_limb)
        self._amp = amplification

        print("Getting robot state... ")
        self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
        self._init_state = self._rs.state().enabled
        print("Enabling robot... ")
        self._rs.enable()
Esempio n. 28
0
def main():

    # arg_fmt = argparse.RawDescriptionHelpFormatter
    # parser = argparse.ArgumentParser(formatter_class=arg_fmt,
    #                                  description=main.__doc__)
    # parser.add_argument(
    #     '-l', '--limb', choices=['left', 'right'], required=True,
    #     help="the limb to test"
    # )
    # args = parser.parse_args(rospy.myargv()[1:])

    print("Getting robot state... ")
    rospy.init_node("IKSolver")
    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state().enabled
    limb = baxter_interface.Limb("right")
    right = baxter_interface.Gripper('right', CHECK_VERSION)

    # return ik_test(args.limb)

    goToOrigin(limb)
    current_pose = limb.endpoint_pose()
    print current_pose

    print limb.joint_angles()

    x = input("X Value?\n")
    y = input("Y Value?\n")
    z = input("Z Value?\n")
    # q1 = input("Q1 Value?\n")
    # q2 = input("Q2 Value?\n")
    # q3 = input("Q3 Value?\n")
    # q4 = input("Q4 Value?\n")

    # limb.move_to_neutral()

    # joints = solveIK('right', float(x), float(y), float(z), float(q1), float(q2), float(q3), float(q4));
    joints = solveIK('right', x, y, z, current_pose['orientation'].x,
                     current_pose['orientation'].y,
                     current_pose['orientation'].z,
                     current_pose['orientation'].w)
    if (joints):
        limb.move_to_joint_positions(joints)
Esempio n. 29
0
def main():
    parser = argparse.ArgumentParser()
    required = parser.add_argument_group('required arguments')
    required.add_argument('-l',
                          '--limb',
                          required=True,
                          choices=['left', 'right'],
                          help='Tare the specified limb')
    args = parser.parse_args(rospy.myargv()[1:])
    limb = args.limb

    print("Initializing node...")
    rospy.init_node('rsdk_tare_%s' % (limb, ))

    print("Preparing to tare...")
    gripper_warn = ("\nIMPORTANT: Make sure to remove grippers and other"
                    " attachments before running tare.\n")
    print(gripper_warn)
    if not gripper_removed(args.limb):
        return 1

    rs = baxter_interface.RobotEnable(CHECK_VERSION)
    rs.enable()
    tt = Tare(limb)
    rospy.loginfo("Running tare on %s limb" % (limb, ))

    error = None
    try:
        tt.run()
    except Exception as e:
        error = e.strerror
    finally:
        try:
            rs.disable()
        except Exception:
            pass

    if error == None:
        rospy.loginfo("Tare finished")
    else:
        rospy.logerr("Tare failed: %s" % (error, ))

    return 0 if error == None else 1
Esempio n. 30
0
    def __init__(self):

        self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate',
                                         UInt16, queue_size=10)
        self._left_arm = baxter_interface.limb.Limb("left")
        self._right_arm = baxter_interface.limb.Limb("right")
        self._left_joint_names = self._left_arm.joint_names()
        self._right_joint_names = self._right_arm.joint_names()

        # control parameters
        self._rate = 500.0  # Hz

        print("Getting robot state... ")
        self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
        self._init_state = self._rs.state().enabled
        print("Enabling robot... ")
        self._rs.enable()
        # set joint state publishing to 500Hz
        self._pub_rate.publish(self._rate)