コード例 #1
0
    def _init_nodes(self, limb, light):
        try:
            self._limb = intera_interface.Limb(limb)

            self._head = intera_interface.Head()

            self._light = SawyerLight(light)

            #self._head_display=intera_interface.HeadDisplay()
            self._display = SawyerDisplay()
            self._cuff = intera_interface.Cuff()

            self._limits = intera_interface.JointLimits()
            self._navigator = intera_interface.Navigator()

            self._joint_names = self._limb.joint_names()
            self._velocity_limits = self._limits.joint_velocity_limits()

            self._stop_cmd = {}
            for i, name in enumerate(self._joint_names):
                self._stop_cmd[name] = 0.0
        except:
            print("Warning caught exception...")
            traceback.print_exc()
            pass
    def __init__(self):
        
        self.waypoints = Waypoints()

        self.rs = intera_interface.RobotEnable()
        self._init_state = self.rs.state().enabled
        self.rs.enable()

        #  Set up arm, cuff and gripper
        self.sawyer_arm = intera_interface.Limb('right')
        self.sawyer_gripper = intera_interface.Gripper()
        self.sawyer_cuff = intera_interface.Cuff(limb='right')
        self.sawyer_lights = intera_interface.Lights()
        
        #  Move to default position when the ik solver is initially launched
        self.sawyer_arm.move_to_neutral(timeout=10, speed=0.28) 
        self.sawyer_gripper.open()

        self.gripper_dist = self.sawyer_gripper.get_position()

        self.sawyer_cuff.register_callback(self.cuff_open_gripper_callback, '{0}_button_upper'.format('right'))
        self.sawyer_cuff.register_callback(self.cuff_close_gripper_callback, '{0}_button_lower'.format('right'))

        self.set_light_status('red', False)
        self.set_light_status('green', True)
        self.set_light_status('blue', False)
コード例 #3
0
def init():
    global g_limb, g_orientation_hand_down, g_position_neutral
    rospy.init_node('motion')
    g_limb = intera_interface.Limb('right')
    print("in init")
    #publisher/subscriber calls
    global subscriber_voicerec, subscriber_foundobj, subscriber_objectLocation
    #subscribe to speech_recognition
    subscriber_voicerec = rospy.Subscriber('/speech_recognition', String,
                                           callback_search_target)
    #subscribe to foundObject
    subscriber_foundobj = rospy.Subscriber('/foundObject', String,
                                           callback_found_obj)
    #subscribe to objectLocation
    subscriber_objectLocation = rospy.Subscriber('/objectLocation', Pose2D,
                                                 callback_object_location)

    # This quaternion will have the hand face straight down (ideal for picking tasks)
    g_orientation_hand_down = Quaternion()
    g_orientation_hand_down.x = 0.704238785359
    g_orientation_hand_down.y = 0.709956638597
    g_orientation_hand_down.z = -0.00229009932359
    g_orientation_hand_down.w = 0.00201493272073

    # This is the default neutral position for the robot's hand (no guarantee this will move the joints to neutral though)
    g_position_neutral = Point()
    g_position_neutral.x = 0.449559195663
    g_position_neutral.y = 0.16070379419
    g_position_neutral.z = 0.212938808947
コード例 #4
0
    def __init__(self, params):

        self._dyn = params.reconfig_server
        self._freq, self._missed_cmds = 20.0, 5.0

        # Control parameters
        self.bounds = params.bound * np.ones([7])

        # Create our limb instance
        self._limb = intera_interface.Limb(params.get('limb', 'right'))

        # Create cuff disable publisher
        cuff_ns = "robot/limb/%s/supress_cuff_interaction" % params.get('limb', 'right')
        self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)

        # Verify robot is enabled
        print("Getting robot state... ")
        self._rs = intera_interface.RobotEnable(CHECK_VERSION)
        self._init_state = self._rs.state().enabled
        print("Enabling robot... ")
        self._rs.enable()
        print("Running. Ctrl-c to quit")

        # PD controller gains for resets
        self.P = np.array([5, 5, 10, 15, 30, 20, 25])             # P controller gains
        self.K = np.array([0.7, 0.2, 1.63, 0.5, 3.5, 4.7, 6.0])   # D controller gains

        self._smoother_params = params.smoother_params
        self._smoother_history = np.zeros(shape=[self._smoother_params.get("history_length", 1), 7])

        self.sent_torques = []
コード例 #5
0
def move_position(xs, ys, zs):
    limb = intera_interface.Limb('right')
    kinematics = sawyer_kinematics('right')
    xyz_pos = [xs, ys, zs]
    x = dict(zip(limb._joint_names, kinematics.inverse_kinematics(xyz_pos)))
    limb.move_to_joint_positions(x)
    rospy.sleep(1.0)
コード例 #6
0
def main():
	angle=[0,0,0,0,0,0,0]
	limb=intera_interface.Limb("right")
	angles=limb.joint_angles()
	for i in range(7):
		angle[i]=angles['right_j'+`i`]
	return angle
コード例 #7
0
def reset():
    limb = intera_interface.Limb('right')

    default_joint_angle = {'right_j0': 0.0}
    limb.move_to_joint_positions(default_joint_angle)

    limb.move_to_neutral()
    def __init__(self):
        global TF, DIST_THRE, LINE_LENGTH, HOLD_TIME, CHECKER_CENTER_COORD, FROM_CENTER_D

        self._limb = intera_interface.Limb("right")
        self._head = intera_interface.Head()
        self.center = np.array([])
        self.target_list = []
        self.draw_status_list = []
        self.hold_init_time = None
        self.line_init_time = None

        self.current_target = np.array([])
        self.current_draw_status = 0    #-1 is going up, 0 is not draw, 1 is apply force control
        self.if_hold = 0 #0 is not to hold, -1 is not to hold but just get back from hold. 1 is to hold
        self.line_init_pose = None
        self.s = 0

        self.object_2_draw = "idle"
        #go to camera_pose
        self.go_to_camera_or_standoff('camera')

        # michael camera object to pass to bennett
        self.camera = gs.Camera()
        self.camera.updateBoard()
        self.gameRunning = True
コード例 #9
0
def run():
    rospy.init_node('set_joint_torques', anonymous=True)

    limb = intera_interface.Limb('right')

    limb.move_to_joint_positions({
        'right_j0': -0.140923828125,
        'right_j1': -1.2789248046875,
        'right_j2': -3.043166015625,
        'right_j3': -2.139623046875,
        'right_j4': -0.047607421875,
        'right_j5': -0.7052822265625,
        'right_j6': -1.4102060546875,
    })
    rospy.sleep(6)
    limb.set_command_timeout(0.03)
    rate = rospy.Rate(33)
    while True:
        limb.set_joint_torques({
            'right_j0': np.random.uniform(-3, 3),
            'right_j1': np.random.uniform(-10, 10),
            'right_j2': np.random.uniform(-5, 5),
            'right_j3': np.random.uniform(-5, 5),
            'right_j4': np.random.uniform(-2.25, 2.25),
            'right_j5': np.random.uniform(-2.25, 2.25),
            'right_j6': np.random.uniform(-2.25, 2.25)
        })
        rate.sleep()
コード例 #10
0
    def move(self, jointpos):
        self.head_display.display_image("/home/microshak/Pictures/Moving.png",
                                        False, 1.0)

        print "MOVING!!!!!!!!!!!!!!!!!"

        position = ast.literal_eval(json.dumps(jointpos['Cartisian']))
        print(position)
        p = position["position"]
        o = position["orientation"]
        pose_target = geometry_msgs.msg.Pose()
        pose_target.orientation.w = o["w"]
        pose_target.orientation.x = o["x"]
        pose_target.orientation.y = o["y"]
        pose_target.orientation.z = o["z"]
        pose_target.position.x = p["x"]
        pose_target.position.y = p["y"]
        pose_target.position.z = p["z"]
        print pose_target
        group = moveit_commander.MoveGroupCommander("right_arm")
        limb = intera_interface.Limb("right")
        limb.set_joint_position_speed(.1)

        #   limb.set_joint_position_speed(.1)
        group.set_pose_target(pose_target)
        # group.set_joint_value_target(pose_target)
        plan2 = group.plan()
        group.go(wait=True)
        '''  
コード例 #11
0
def angle_action_server():
    rospy.init_node('angle_action_server', anonymous=True)
    global arm
    arm = ii.Limb('right')
    arm.set_joint_position_speed(ros_config.JOINT_POSITION_SPEED)
    s = rospy.Service('angle_action', angle_action, execute_action)
    rospy.spin()
コード例 #12
0
ファイル: controller.py プロジェクト: aubrune/cs_sawyer
 def start_robot(self):
     self.rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
     self.rs.enable()
     self.limb = intera_interface.Limb('right')
     self._sticks.set_limb(self.limb)
     rospy.Subscriber("/robot/limb/right/endpoint_state", EndpointState,
                      self._cb_endpoint_received)
コード例 #13
0
    def __init__(self, reconfig_server, limb="right"):
        self._dyn = reconfig_server

        # create our limb instance
        self._limb = intera_interface.Limb(limb)

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

        self._performedMotion = False
        self.marker = Marker()

        self.tf_buffer = tf2_ros.Buffer(
            rospy.Duration(1200.0))  #tf buffer length
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)

        print("Running. Ctrl-c to quit")

        self.polishTouchPointSub = rospy.Subscriber("/polishTouchPose",
                                                    PoseStamped,
                                                    self.pose_callback)
        self.markerPub = rospy.Publisher("/polishTouchGotoPose", Marker)
        self.flagPub = rospy.Publisher("/flag_topic", String)
        rospy.on_shutdown(self.clean_shutdown)

        rate = rospy.Rate(100)
        while not rospy.is_shutdown():
            self.flagPub.publish(str(self._performedMotion))
            if self._performedMotion:
                self.markerPub.publish(self.marker)
            rate.sleep()
コード例 #14
0
ファイル: assembly.py プロジェクト: Marcin-Regulski/Sawyer4.0
    def __init__(self,
                 limb="right",
                 hover_distance=0.16,
                 tip_name="right_hand"):
        self._limb = intera_interface.Limb(limb)
        self._gripper = intera_interface.Gripper()
        self._limb_name = limb
        self._process = bool
        self._hover_distance = hover_distance
        self._tip_name = tip_name
        self._starting_pose = Pose()
        self._gripper.set_cmd_velocity(0.16)

        self.speed_ratio = 0.3
        self.accel_ratio = 0.4
        self.linear_speed = 0.3
        self.linear_accel = 0.4
        self._Sequence = server.Sequence()
        self._camera_check = self._Sequence.poses['Camera_check']

        self._zero_pose = self._Sequence._zero_pose

        print("Getting robot state... ")
        self._rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
        self._init_state = self._rs.state().enabled
        print("Enabling robot... ")
        self._rs.enable()
    def __init__(self):
        super(MoveGroupPythonInteface, self).__init__()

        ## First initialize `moveit_commander`_ and a `rospy`_ node:
        # joint_state_topic = ['joint_states:=/robot/joint_states']
        moveit_commander.roscpp_initialize(sys.argv)
        # moveit_commander.roscpp_initialize(joint_state_topic)
        rospy.init_node('move_group_python_interface', anonymous=True)
        self.limb = intera_interface.Limb('right')
        # roscpp_initialize(sys.argv)

        # self.limb.move_to_neutral()
        ## Instantiate a `RobotCommander`_ object. This object is the outer-level interface to
        ## the robot:
        robot = moveit_commander.RobotCommander()

        ## Instantiate a `PlanningSceneInterface`_ object.  This object is an interface
        ## to the world surrounding the robot:
        scene = moveit_commander.PlanningSceneInterface()

        ## Instantiate a `MoveGroupCommander`_ object.  This object is an interface
        ## to one group of joints.
        group_name = "right_arm"
        group = moveit_commander.MoveGroupCommander(group_name)
        # group.set_planner_id("RRTkConfigDefault")
        group.set_planner_id("RRTConnectkConfigDefault")
        ## We create a `DisplayTrajectory`_ publisher which is used later to publish
        ## trajectories for RViz to visualize:
        display_trajectory_publisher = rospy.Publisher(
            '/move_group/display_planned_path',
            moveit_msgs.msg.DisplayTrajectory,
            queue_size=20)

        # We can get the name of the reference frame for this robot:
        planning_frame = group.get_planning_frame()
        print "============ Reference frame: %s" % planning_frame

        # We can also print the name of the end-effector link for this group:
        eef_link = group.get_end_effector_link()
        print "============ End effector: %s" % eef_link

        # We can get a list of all the groups in the robot:
        group_names = robot.get_group_names()
        print "============ Robot Groups:", robot.get_group_names()

        # Sometimes for debugging it is useful to print the entire state of the
        # robot:
        print "============ Printing robot state"
        print robot.get_current_state()
        print ""

        # Misc variables
        self.box_name = 'roller'
        self.robot = robot
        self.scene = scene
        self.group = group
        self.display_trajectory_publisher = display_trajectory_publisher
        self.planning_frame = planning_frame
        self.eef_link = eef_link
        self.group_names = group_names
コード例 #16
0
    def __init__(self):
        rospy.loginfo("Creating IK Controller class")
        
        # setup flags and useful vars
        self.running_flag = False
        self.step_scale = rospy.get_param("~scale", STEP_SCALE)
        self.freq = rospy.get_param("~freq", FREQ)
        self.arm = rospy.get_param("~arm", ARM)
        self.q = np.zeros(7)
        with cl.suppress_stdout_stderr():
            self.urdf = URDF.from_parameter_server()
        self.kin = KDLKinematics(self.urdf, "base", "%s_hand"%self.arm)
        self.goal = np.array(self.kin.forward(self.q))
        self.mutex = threading.Lock()
        self.joint_vel_limit = rospy.get_param("~joint_vel_limit", JOINT_VEL_LIMIT)
        self.q_sim = np.zeros(7)
        self.damping = rospy.get_param("~damping", DAMPING)
        self.joint_names = self.kin.get_joint_names() 
        self.qdot_dict = {}
        self.limb_interface = intera_interface.Limb()
        self.ep = EndpointState()

        # create all subscribers, publishers, and timers
        self.int_timer = rospy.Timer(rospy.Duration(1/float(self.freq)), self.ik_step_timercb)
        self.actual_js_sub = rospy.Subscriber("robot/joint_states", JointState, self.actual_js_cb)
        self.endpoint_update_cb = rospy.Subscriber("robot/limb/right/endpoint_state", EndpointState, self.endpoint_upd_cb)
コード例 #17
0
    def __init__(self):
        rospy.init_node("pick_and_place_node", log_level=rospy.DEBUG)

        self._hover_distance = rospy.get_param("~hover_distance", 0.2)
        self._limb_name = rospy.get_param("~limb", 'right')
        self._limb = intera_interface.Limb(self._limb_name)
        self._limb.set_joint_position_speed(0.3)

        self._gripper = intera_interface.Gripper(self._limb_name)
        if self._gripper is None:
            rospy.logerr("Gripper error")
        else:
            rospy.logdebug("Gripper OK")

        self._head = intera_interface.Head()

        self._iksvc_name = "ExternalTools/" + self._limb_name + "/PositionKinematicsNode/IKService"
        rospy.wait_for_service(self._iksvc_name, 5.0)
        self._iksvc = rospy.ServiceProxy(self._iksvc_name, SolvePositionIK)

        # Enable the robot
        _rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
        _init_state = _rs.state().enabled
        rospy.logdebug("Enabling robot... ")
        _rs.enable()

        rospy.Service("pick_and_place", PickAndPlace, self.pnp)
        rospy.Service("pick_and_throw", PickAndPlace, self.pnt)
        rospy.Service("move_to_position", PositionMovement,
                      self.move_to_position)
        rospy.Service("move_to_joint", JointMovement, self.move_to_joint)
        rospy.Service("move_head", HeadMovement, self.move_head)
        #rospy.Service("throw", Throw, self.throw)
        rospy.logdebug("PNP Ready")
def callback_func(data):
    rospy.loginfo("I got the message")

    try:
        grip = intera_interface.Gripper('right_gripper')
    except ValueError:
        rospy.logerr("ERROR: Unable to detect Gripper")
        return

    rospy.loginfo("Gripper position is {}".format(grip.get_position()))

    tmp_limb = intera_interface.Limb('right')
    end_pt = tmp_limb.endpoint_pose()['position']

    print("Endpoint is x: {x}\n y: {y}\n z: {z}".format(x=end_pt[0],
                                                        y=end_pt[1],
                                                        z=end_pt[2]))

    # new_pt = Point(
    #     x = end_pt[0],
    #     y = end_pt[1],
    #     z = end_pt[2]-0.09
    # )

    # tmp_limb.set_joint_trajectory(["right_j6", "right_j4"], new_pt, [0.3, 0.3], [0.01, 0.01])

    grip.set_position(0.01)

    rospy.loginfo("Gripper position is {}".format(grip.get_position()))
    rospy.signal_shutdown("Finished grip action")
コード例 #19
0
    def __init__(self, limb="right"):
        # Constants
        self._key_command_torques  = "cs225a::sawyer::actuators::fgc"
        self._key_joint_positions  = "cs225a::sawyer::sensors::q"
        self._key_joint_velocities = "cs225a::sawyer::sensors::dq"
        self._key_timestamp        = "cs225a::sawyer::timestamp"
        self._dof                  = 7
        self._control_rate         = 2500.0 # Hz
        self._timeout_missed_cmds  = 20.0 # Missed cycles before triggering timeout
        self._joint_names          = ["{0}_j{1}".format(limb, i) for i in range(self._dof)]

        # Initialize limb interface
        self._limb = intera_interface.Limb(limb)

        # Initialize redis instance
        self._redis = redis.StrictRedis(host="127.0.0.1", port=6379, db=0)

        # Reset redis q values so they aren't random
        self._redis.set(self._key_command_torques, "0 0 0 0 0 0 0")

        # Verify robot is enabled
        print("Getting robot state... ")
        self._rs = intera_interface.RobotEnable(CHECK_VERSION)
        self._init_state = self._rs.state().enabled
        print("Enabling robot... ")
        self._rs.enable()
        print("Running. Ctrl-c to quit")

        # Get initial time
        self._start_time = rospy.Time.from_sec(time.time())
コード例 #20
0
    def execute(self):
        """
        Initializes the connection to the robot.
        """
        robot_params = intera_interface.RobotParams()
        valid_limbs = robot_params.get_limb_names()
        if not valid_limbs:
            logger.error("Cannot detect any limb parameters on this robot!")
            return
        rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
        init_state = rs.state().enabled

        def _shutdown_hook():
            logger.info("Exiting the application ...")
            if not init_state:
                logger.info("Disabling robot ...")
                rs.disable()

        rospy.on_shutdown(_shutdown_hook)
        logger.info("Enabling robot ...")
        rs.enable()

        if self.limb_name not in valid_limbs:
            logger.error("{} is not a valid limb on this robot!".format(
                self.limb_name))

        # Initialize the limb and send it to the resting position.
        self.limb = intera_interface.Limb(self.limb_name)
        self.limb.set_joint_position_speed(0.35)
        self.spin()
コード例 #21
0
def doStuff():
    #rospy.init_node('Pls_move')  # Initializes a node with name Pls_move
    limb = intera_interface.Limb(
        'right')  # Gets the right arm (not like theres another one)

    angles = limb.joint_angles()  # Gets the joint angles

    print(angles['right_j0'])

    new_right_j0 = angles['right_j0'] + 0.55

    joint_dict = {
        'right_j0': new_right_j0
    }  # Create dictionary with joint:value mapping
    limb.move_to_joint_positions(
        joint_dict)  # Move the joints to the dictionary specific positions

    new_angles = limb.joint_angles()

    print(new_angles['right_j0'])

    time.sleep(3.0)

    joint_dict['right_j0'] -= 0.55
    limb.move_to_joint_positions(joint_dict)

    print(limb.joint_angles()['right_j0'])
コード例 #22
0
    def __init__(self):
        rospy.loginfo("Creating VelocityController class")

        # Create KDL model
        with cl.suppress_stdout_stderr():    # Eliminates urdf tag warnings
            self.robot = URDF.from_parameter_server()
        self.kin = KDLKinematics(self.robot, "base", "right_hand")
        self.names = self.kin.get_joint_names()

        # Limb interface
        self.arm = intera_interface.Limb("right")
        self.hand = intera_interface.gripper.Gripper('right')

        # Grab M0 and Blist from saywer_MR_description.py
        self.M0 = s.M #Zero config of right_hand
        self.Blist = s.Blist #6x7 screw axes mx of right arm

        # Shared variables
        self.mutex = threading.Lock()
        self.time_limit = rospy.get_param("~time_limit", TIME_LIMIT)
        self.damping = rospy.get_param("~damping", DAMPING)
        self.joint_vel_limit = rospy.get_param("~joint_vel_limit", JOINT_VEL_LIMIT)
        self.q = np.zeros(7)        # Joint angles
        self.qdot = np.zeros(7)     # Joint velocities
        self.T_goal = np.array(self.kin.forward(self.q))    # Ref se3

        # Subscriber
        self.ref_pose_sub = rospy.Subscriber('ref_pose', Pose, self.ref_pose_cb)

        self.hand.calibrate()

        self.r = rospy.Rate(100)
        while not rospy.is_shutdown():
            self.calc_joint_vel()
            self.r.sleep()
コード例 #23
0
    def __init__(self, reconfig_server, limb="right"):
        self._dyn = reconfig_server

        # control parameters
        self._rate = 1000.0  # Hz
        self._missed_cmds = 20.0  # Missed cycles before triggering timeout

        # create our limb instance
        self._limb = intera_interface.Limb(limb)

        # initialize parameters
        self._springs = dict()
        self._damping = dict()
        self._start_angles = dict()

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

        #
        self.joint_torques = dict()

        # verify robot is enabled
        print("Getting robot state... ")
        self._rs = intera_interface.RobotEnable(CHECK_VERSION)
        self._init_state = self._rs.state().enabled
        print("Enabling robot... ")
        self._rs.enable()
        print("Running. Ctrl-c to quit")
コード例 #24
0
    def __init__(self, trajectory_planner, limb="right", hover_distance=0.08, tip_name="right_gripper_tip"):
        """
        :param limb:
        :param hover_distance:
        :param tip_name:
        """
        self.trajectory_planner = trajectory_planner
        self._limb_name = limb  # string
        self._tip_name = tip_name  # string
        self._hover_distance = hover_distance  # in meters
        self._limb = intera_interface.Limb(limb)

        if demo_constants.is_real_robot():
            self._gripper =PSGGripper()
        else:
            self._gripper = intera_interface.Gripper()

        self._robot_enable = intera_interface.RobotEnable()

        # verify robot is enabled
        print("Getting robot state... ")
        self._rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
        self._init_state = self._rs.state().enabled
        print("Enabling robot... ")
        self._rs.enable()
    def __init__(self, speed=0.28, accuracy=0.003726646, timeout=5):
        self.sawyer_arm = intera_interface.Limb('right')
        self.joint_speed = speed
        self.accuracy = accuracy
        self.timeout = timeout

        self.waypoints = list()
        self._is_recording_waypoints = False

        self.rs = intera_interface.RobotEnable()
        self._init_state = self.rs.state().enabled
        self.rs.enable()

        # Create Navigator
        self.navigator_io = intera_interface.Navigator()
        self.sawyer_lights = intera_interface.Lights()

        # Create Gripper & Cuff
        self.sawyer_gripper = intera_interface.Gripper()
        self.sawyer_cuff = intera_interface.Cuff(limb='right')

        self.sawyer_cuff.register_callback(self.cuff_open_gripper_callback,
                                           '{0}_button_upper'.format('right'))
        self.sawyer_cuff.register_callback(self.cuff_close_gripper_callback,
                                           '{0}_button_lower'.format('right'))

        # print(self.navigator_io.list_all_items())
        self.sawyer_arm.move_to_neutral(timeout=5, speed=self.joint_speed)
        self.sawyer_gripper.open()

        self.set_light_status('red', False)
        self.set_light_status('green', False)
        self.set_light_status('blue', True)
コード例 #26
0
def ik_service_client(limb = "right", tip_name = "right_gripper_tip"):
    limb_mv = intera_interface.Limb(limb)
    #gripper = intera_interface.Gripper()
    ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
    iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
    ikreq = SolvePositionIKRequest()
    hdr = Header(stamp=rospy.Time.now(), frame_id='base')

    # Add desired pose for inverse kinematics
    current_pose = limb_mv.endpoint_pose()
    #print current_pose 
    movement = [0.460, -0.563,0.35]
    orientation = [0.0,1,0.0,0.0]
    #gripper.close()
    rospy.sleep(1.0)
    [dx,dy,dz] = movement
    [ox,oy,oz,ow] = orientation
    dy = constrain(dy,-0.7596394482267009,0.7596394482267009)
    dz = constrain(dz, 0.02, 1)


    # up position [0.45,-0.453,0.6] 0.11 for pick up location

     #poses = {'right': PoseStamped(header=hdr,pose=Pose(position=Point(x=0.450628752997,y=0.161615832271,z=0.217447307078,),
     #orientation=Quaternion(x=0.704020578925,y=0.710172716916,z=0.00244101361829,w=0.00194372088834,),),),} neutral pose
    
    #x= 0.5,y = 0.5, z= 0.5,w= 0.5 for enpoint facing forward (orientation)
    #table side parametres x=0.6529605227057904, y= +-0.7596394482267009, z=0.10958623747123714)
    #table straight parametres x=0.99, y=0.0, z=0.1)

    poses = {
        'right': PoseStamped(
            header=hdr,
            pose=Pose(
                position=Point(
                    x= dx,
                    y= dy,
                    z= dz,
                ),
                orientation=Quaternion(
                    x= ox,
                    y= oy,
                    z= oz,
                    w= ow,
                ),
            ),
        ),
    }

    ikreq.pose_stamp.append(poses[limb])
    
    # Request inverse kinematics from base to "right_hand" link
    ikreq.tip_names.append('right_gripper_tip') # right_hand, right_wrist, right_hand_camera 

    try:
        rospy.wait_for_service(ns, 5.0)
        resp = iksvc(ikreq)
    except (rospy.ServiceException, rospy.ROSException), e:
        rospy.logerr("Service call failed: %s" % (e,))
        return False
コード例 #27
0
    def __init__(self,
                 limb,
                 reconfig_server,
                 rate=100.0,
                 mode='position_w_id',
                 interpolation='minjerk'):
        self._dyn = reconfig_server
        self._ns = 'robot/limb/' + limb
        self._fjt_ns = self._ns + '/follow_joint_trajectory'
        self._server = actionlib.SimpleActionServer(
            self._fjt_ns,
            FollowJointTrajectoryAction,
            execute_cb=self._on_trajectory_action,
            auto_start=False)
        self._action_name = rospy.get_name()
        self._limb = intera_interface.Limb(limb, synchronous_pub=True)
        self._enable = intera_interface.RobotEnable()
        self._name = limb
        self._interpolation = interpolation
        self._cuff = intera_interface.Cuff(limb=limb)
        # Verify joint control mode
        self._mode = mode
        if (self._mode != 'position' and self._mode != 'position_w_id'
                and self._mode != 'velocity'):
            rospy.logerr("%s: Action Server Creation Failed - "
                         "Provided Invalid Joint Control Mode '%s' (Options: "
                         "'position_w_id', 'position', 'velocity')" % (
                             self._action_name,
                             self._mode,
                         ))
            return
        self._server.start()
        self._alive = True
        # Action Feedback/Result
        self._fdbk = FollowJointTrajectoryFeedback()
        self._result = FollowJointTrajectoryResult()

        # Controller parameters from arguments, messages, and dynamic
        # reconfigure
        self._control_rate = rate  # Hz
        self._control_joints = []
        self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()}
        self._goal_time = 0.0
        self._stopped_velocity = 0.0
        self._goal_error = dict()
        self._path_thresh = dict()

        # Create our PID controllers
        self._pid = dict()
        for joint in self._limb.joint_names():
            self._pid[joint] = intera_control.PID()

        # Create our spline coefficients
        self._coeff = [None] * len(self._limb.joint_names())

        # Set joint state publishing to specified control rate
        self._pub_rate = rospy.Publisher('/robot/joint_state_publish_rate',
                                         UInt16,
                                         queue_size=10)
        self._pub_rate.publish(self._control_rate)
コード例 #28
0
    def __init__(self, filename, rate, side="right"):
        """
        Records joint data to a file at a specified rate.
        """
        self.gripper_name = '_'.join([side, 'gripper'])
        self._filename = filename
        self._raw_rate = rate
        self._rate = rospy.Rate(rate)
        self._start_time = rospy.get_time()
        self._done = False

        self._limb_right = intera_interface.Limb(side)
        try:
            self._gripper = intera_interface.Gripper(side)
            rospy.loginfo("Electric gripper detected.")
        except Exception as e:
            self._gripper = None
            rospy.loginfo("No electric gripper detected.")

        # Verify Gripper Have No Errors and are Calibrated
        if self._gripper:
            if self._gripper.has_error():
                self._gripper.reboot()
            if not self._gripper.is_calibrated():
                self._gripper.calibrate()

        self._cuff = intera_interface.Cuff(side)
コード例 #29
0
def init():
    global g_limb, g_orientation, g_orientation2, g_position_neutral
    rospy.init_node('cairo_sawyer_ik_example')
    g_limb = intera_interface.Limb('right')

    # This quaternion will have the hand face straight down (ideal for picking tasks)
    """ g_orientation_hand_down = Quaternion()
    g_orientation_hand_down.x = 0.704238785359
    g_orientation_hand_down.y =0.709956638597
    g_orientation_hand_down.z = -0.00229009932359
    g_orientation_hand_down.w = 0.00201493272073 """
   
   # Ros quaternion info - http://wiki.ros.org/tf2/Tutorials/Quaternions
   #FIRST ROTATION
    q_orig = quaternion_from_euler(0, 0, 0) # Set origin
    q_rot1 = quaternion_from_euler(0, pi/2, 0) # Rotate 90 degrees around y axis
    q_rot2 = quaternion_from_euler(0, 0, pi/4)   # Rotate 45 degrees around z axis
    quat_tf = quaternion_multiply(q_rot2, quaternion_multiply(q_rot1, q_orig)) # multiply rotations in order
    g_orientation = Quaternion(quat_tf[0], quat_tf[1], quat_tf[2], quat_tf[3]) # Create Quaternion object ROS can read

    #SECOND ROTATION
    q_rot3 = quaternion_from_euler(0, 0,0.3) # rotate 0.3 around z axis
    quat_tf2 = quaternion_multiply(q_rot3, quat_tf) # Multiply rotation times position quat_tf to get quat_tf2
    g_orientation2 = Quaternion(quat_tf2[0], quat_tf2[1], quat_tf2[2], quat_tf2[3]) # Create Quaternion object ROS ccan read

    # This is the default neutral position for the robot's hand (no guarantee this will move the joints to neutral though)
    g_position_neutral = Point()
    g_position_neutral.x = 0.449559195663
    g_position_neutral.y = 0.16070379419
    g_position_neutral.z = 0.212938808947
コード例 #30
0
ファイル: move_end.py プロジェクト: mushenghe/ee106a-project
def move_joints(side):
    limb = intera_interface.Limb(side)

    joints = limb.joint_names()

    # Publish end-effector torques to a topic
    pub_torque = rospy.Publisher('endpoint_torque', Wrench, queue_size = 10)

    action_count = 0; i = 0;
    while not rospy.is_shutdown():
        force = limb.endpoint_effort()['force']
        torque = limb.endpoint_effort()['torque']
        w = Wrench(Vector3(force.x, force.y, force.z), Vector3(torque.x, torque.y, torque.z));
        pub_torque.publish(w);

        action_count += 1;
        if action_count % 10000 != 0:
            continue
        else:
            action_count = 0
        # Move joints according to specified locations
        print(force)
        i += 1;
        if (i >= len(default_positions)):            
            i = 0;        
        raw_input("Press Enter to continue..." + str(i))
        limb.set_joint_positions(default_positions[i]);
        diff = sum([(limb.joint_angle(key) - default_positions[i][key])** 2 for key in default_positions[i]])
        while (diff > 0.0001):
            limb.set_joint_positions(default_positions[i])
            diff = sum([(limb.joint_angle(key) - default_positions[i][key])** 2 for key in default_positions[i]])
            time.sleep(0.005)