def main():

    init_node()

    # Preparing Left Arm
    rospy.loginfo("Preparing Left Arm...")
    larmg = MoveGroupCommander("left_arm")
    larmg.set_pose_target([0.325, 0.182, 0.067, 0.0, -math.pi / 2, 0.0])
    larmg.go()

    # Right Arm
    group = MoveGroupCommander("right_arm")

    # Frame ID Definitoins
    planning_frame_id = group.get_planning_frame()
    tgt_frame_id = '/LARM_JOINT5_Link'

    # Get a target pose
    pose_target = get_current_target_pose(tgt_frame_id, planning_frame_id)

    # Move to a point above target
    if pose_target:
        pose_target.pose.position.z += 0.4
        rospy.loginfo("Set Target To: \n{}".format(pose_target))
        group.set_pose_target(pose_target)
        ret = group.go()
        rospy.loginfo("Executed ... {}".format(ret))
    else:
        rospy.logwarn("Pose Error: {}".format(pose_target))
def move_action(x):

    #Create the function used to call the service
    compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
    step = 0
    pose = [x]

    if step == 0:

        s = pose[step]
        request = GetPositionIKRequest()
        request.ik_request.group_name = "left_arm"
        request.ik_request.ik_link_name = "left_gripper"
        request.ik_request.attempts = 20
        request.ik_request.pose_stamped.header.frame_id = "ar_marker_0"

        request.ik_request.pose_stamped.pose.position.x = s[0]
        request.ik_request.pose_stamped.pose.position.y = s[1]
        request.ik_request.pose_stamped.pose.position.z = s[2]
        request.ik_request.pose_stamped.pose.orientation.x = s[3]
        request.ik_request.pose_stamped.pose.orientation.y = s[4]
        request.ik_request.pose_stamped.pose.orientation.z = s[5]
        request.ik_request.pose_stamped.pose.orientation.w = s[6]

        response = compute_ik(request)
        group = MoveGroupCommander("left_arm")
        group.set_pose_target(request.ik_request.pose_stamped)
        group.go()
        step = step + 1
def requestIK(link_name, group_name, p, o, compute_ik):
    px, py, pz = p
    ox, oy, oz, ow = o
    request = GetPositionIKRequest()
    request.ik_request.group_name = group_name
    request.ik_request.ik_link_name = link_name
    request.ik_request.attempts = 10000
    request.ik_request.pose_stamped.header.frame_id = "head_camera"
    request.ik_request.pose_stamped.pose.position.x = px
    request.ik_request.pose_stamped.pose.position.y = py
    request.ik_request.pose_stamped.pose.position.z = pz
    request.ik_request.pose_stamped.pose.orientation.x = ox
    request.ik_request.pose_stamped.pose.orientation.y = oy
    request.ik_request.pose_stamped.pose.orientation.z = oz
    request.ik_request.pose_stamped.pose.orientation.w = ow
    try:
        # print request
        print 'finding ik....'
        response = compute_ik(request)
        print 'moveIt succeeded'
        # print(response)
        group = MoveGroupCommander(group_name)
        group.set_pose_target(request.ik_request.pose_stamped)
        group.go()
    except rospy.ServiceException, e:
        print "Service call failed: %s" % e
Esempio n. 4
0
    def __init__(self):
        group = MoveGroupCommander("arm")

        #group.set_orientation_tolerance([0.3,0.3,0,3])

        p = PoseStamped()
        p.header.frame_id = "/katana_base_link"
        p.pose.position.x = 0.4287
        #p.pose.position.y = -0.0847
        p.pose.position.y = 0.0
        p.pose.position.z = 0.4492

        q = quaternion_from_euler(2, 0, 0)

        p.pose.orientation.x = q[0]
        p.pose.orientation.y = q[1]
        p.pose.orientation.z = q[2]
        p.pose.orientation.w = q[3]

        print "Planning frame: ", group.get_planning_frame()
        print "Pose reference frame: ", group.get_pose_reference_frame()

        #group.set_pose_reference_frame("katana_base_link")

        print "RPy target: 0,0,0"
        #group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame")
        #group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame")

        group.set_pose_target(p, "katana_gripper_tool_frame")

        group.go()
        print "Current rpy: ", group.get_current_rpy(
            "katana_gripper_tool_frame")
Esempio n. 5
0
def callback(states):
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    arm = MoveGroupCommander("manipulator")
    arm.set_planner_id("RRTConnectkConfigDefault")
    arm.set_num_planning_attempts(20)
    arm.allow_replanning(True)
    pose = copy.deepcopy(states.pose[-1])
    temp = tf.transformations.euler_from_quaternion(
        (pose.orientation.x, pose.orientation.y, pose.orientation.z,
         pose.orientation.w))
    quaternion = tf.transformations.quaternion_from_euler(
        math.pi, temp[1], temp[2])
    pose.position.z += 0.22
    pose.orientation.x = quaternion[0]
    pose.orientation.y = quaternion[1]
    pose.orientation.z = quaternion[2]
    pose.orientation.w = quaternion[3]
    print pose
    arm.set_pose_target(pose)
    move_plan = arm.plan()
    i = 0
    while (not move_plan.joint_trajectory.joint_names):
        move_plan = arm.plan()
        i += 1
        if (i == 5): break
    state = arm.execute(move_plan)
Esempio n. 6
0
class GenericDualArmClient():
    _safe_kinematicsolver = "RRTConnectkConfigDefault"

    def __init__(self, *args, **kwargs):
        larm_name = args[0]
        rarm_name = args[1]  # "arm_right" for Motoman SDA10F
        self.r_arm = MoveGroupCommander(rarm_name)
        self.r_arm.set_planner_id(self._safe_kinematicsolver)

    def move_rarm_shift_forward(self, axis, value):
        '''
        Ref. http://docs.ros.org/indigo/api/moveit_commander/html/classmoveit__commander_1_1move__group_1_1MoveGroupCommander.html#a22f9ec1477c3d61e728660850d50ce1f
        '''
        self.r_arm.shift_pose_target(axis, value)
        self.r_arm.plan()
        self.r_arm.go()

    def move_rarm_fixedpose_forward(self):
        tpose = Pose()
        #TODO Currently the following position may work with SDA10F but not with NXO
        tpose.position.x = 0.599
        tpose.position.y = -0.379
        tpose.position.z = 1.11
        self.r_arm.set_pose_target(tpose)
        self.r_arm.plan()
        self.r_arm.go()
Esempio n. 7
0
def make_single_move(pose):
    	print "ok"
    	s = pose
    	print s
    	#Construct the request
    	request = GetPositionIKRequest()
    	request.ik_request.group_name = "left_arm"
    	request.ik_request.ik_link_name = "left_gripper"
    	request.ik_request.attempts = 20
    	request.ik_request.pose_stamped.header.frame_id = "ar_marker_3"  
    	#Set the desired orientation for the end effector HERE
    	request.ik_request.pose_stamped.pose.position.x = s[0]
    	request.ik_request.pose_stamped.pose.position.y = s[1]
    	request.ik_request.pose_stamped.pose.position.z = s[2]        
    	request.ik_request.pose_stamped.pose.orientation.x = s[3]
    	request.ik_request.pose_stamped.pose.orientation.y = s[4]
    	request.ik_request.pose_stamped.pose.orientation.z = s[5]
    	request.ik_request.pose_stamped.pose.orientation.w = s[6]
    	try:
   	    print "ok2"
   	    response = compute_ik(request)
   	    group = MoveGroupCommander("left_arm")
   	    print "ok3"
   	    group.set_pose_target(request.ik_request.pose_stamped)
  	    print "ok4"
  	    group.go()
   	    print "in"
   	    print  x
      		#print "out"
  	except rospy.ServiceException, e:
     		print "Service call failed: %s"%e
Esempio n. 8
0
class TestMove():
    def __init__(self):
        roscpp_initialize(sys.argv)
        rospy.init_node('ur3_move', anonymous=True)

        self.scene = PlanningSceneInterface()
        self.robot_cmd = RobotCommander()

        self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
        #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
        self.robot_arm.set_goal_orientation_tolerance(0.005)
        self.robot_arm.set_planning_time(5)
        self.robot_arm.set_num_planning_attempts(5)
        self.pose_goal = Pose()

        rospy.sleep(2)
        # Allow replanning to increase the odds of a solution
        self.robot_arm.allow_replanning(True)

    def move_code(self):

        self.robot_arm.set_named_target("home")  #go to goal state.
        self.robot_arm.go(wait=True)
        print("====== move plan go to home 1 ======")
        rospy.sleep(0.5)

        #        print("====== move plan go to up ======")
        self.robot_arm.set_named_target("up")  #go to goal state.
        self.robot_arm.go(wait=True)
        print("====== move plan go to up ======")
        rospy.sleep(0.5)

        self.robot_arm.set_named_target("home")  #go to goal state.
        self.robot_arm.go(wait=True)
        print("====== move plan go to home 1 ======")
        rospy.sleep(0.5)

        #        robot_arm.set_named_target("up")
        #        robot_arm.go(wait=True)

        robot_state = self.robot_arm.get_current_pose()
        robot_angle = self.robot_arm.get_current_joint_values()

        print(robot_state)

    def move_TF(self):

        self.ee_TF_list = [-2.046, 1.121, 0.575, 0.453, 0.561, -0.435, 0.538]
        self.pose_goal.position.x = self.ee_TF_list[0]
        self.pose_goal.position.y = self.ee_TF_list[1]
        self.pose_goal.position.z = self.ee_TF_list[2]

        self.pose_goal.orientation.x = self.ee_TF_list[3]
        self.pose_goal.orientation.y = self.ee_TF_list[4]
        self.pose_goal.orientation.z = self.ee_TF_list[5]
        self.pose_goal.orientation.w = self.ee_TF_list[6]

        self.robot_arm.set_pose_target(self.pose_goal)
        self.robot_arm.go(True)
Esempio n. 9
0
def main(robo):
    #Wait for the IK service to become available
    rospy.wait_for_service('compute_ik')
    rospy.init_node('service_query')
    arm = 'left'
    #Create the function used to call the service
    compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
    if robo == 'sawyer':
        arm = 'right'
    while not rospy.is_shutdown():
        raw_input('Press [ Enter ]: ')

        #Construct the request
        request = GetPositionIKRequest()
        request.ik_request.group_name = arm + "_arm"

        #Alan does not have a gripper so replace link with 'right_wrist' instead
        link = arm + "_gripper"
        if robo == 'sawyer':
            link += '_tip'

        request.ik_request.ik_link_name = link
        request.ik_request.attempts = 20
        request.ik_request.pose_stamped.header.frame_id = "base"

        #Set the desired orientation for the end effector HERE
        request.ik_request.pose_stamped.pose.position.x = 0.5
        request.ik_request.pose_stamped.pose.position.y = 0.5
        request.ik_request.pose_stamped.pose.position.z = 0.0
        request.ik_request.pose_stamped.pose.orientation.x = 0.0
        request.ik_request.pose_stamped.pose.orientation.y = 1.0
        request.ik_request.pose_stamped.pose.orientation.z = 0.0
        request.ik_request.pose_stamped.pose.orientation.w = 0.0

        try:
            #Send the request to the service
            response = compute_ik(request)

            #Print the response HERE
            print(response)
            group = MoveGroupCommander(arm + "_arm")

            # Setting position and orientation target
            group.set_pose_target(request.ik_request.pose_stamped)

            # TRY THIS
            # Setting just the position without specifying the orientation
            ###group.set_position_target([0.5, 0.5, 0.0])

            # Plan IK and execute
            group.go()

        except rospy.ServiceException, e:
            print "Service call failed: %s" % e
Esempio n. 10
0
class TestHironxMoveit(unittest.TestCase):

    @classmethod
    def setUpClass(self):

        rospy.init_node("test_hironx_moveit")

        self.rarm = MoveGroupCommander("right_arm")
        self.larm = MoveGroupCommander("left_arm")

        self.rarm_current_pose = self.rarm.get_current_pose().pose
        self.larm_current_pose = self.larm.get_current_pose().pose

    def _set_target_random(self):
        '''
        @type self: moveit_commander.MoveGroupCommander
        @param self: In this particular test script, the argument "self" is either
                     'rarm' or 'larm'.
        '''
        global current, current2, target
        current = self.get_current_pose()
        print "*current*", current
        target = self.get_random_pose()
        print "*target*", target
        self.set_pose_target(target)
        self.go()
        current2 = self.get_current_pose()
        print "*current2*", current2

    # Associate a method to MoveGroupCommander class. This enables users to use
    # the method on interpreter.
    # Although not sure if this is the smartest Python code, this works fine from
    # Python interpreter.
    ##MoveGroupCommander._set_target_random = _set_target_random
    
    # ****** Usage ******
    #
    # See wiki tutorial
    #  https://code.google.com/p/rtm-ros-robotics/wiki/hironx_ros_bridge_en#Manipulate_Hiro_with_Moveit_via_Python
    #

    def test_set_pose_target_rpy(self):    
        # #rpy ver
        target=[0.2035, -0.5399, 0.0709, 0,-1.6,0]
        self.rarm.set_pose_target(target)
        self.rarm.go()
        time.sleep(_SEC_WAIT_BETWEEN_TESTCASES)
    
    def test_set_pose_target_quarternion(self):    
        # #Quaternion ver
        target2=[0.2035, -0.5399, 0.0709, 0.000427, 0.000317, -0.000384, 0.999999]
        self.rarm.set_pose_target(target2)
        self.rarm.go()
        time.sleep(_SEC_WAIT_BETWEEN_TESTCASES)
Esempio n. 11
0
    def run(self, cycle, verbose=2):

        #enable robot if not already done
        enableProcess = subprocess.Popen(
            ["rosrun", "baxter_tools", "enable_robot.py", "-e"])
        enableProcess.wait()

        #start moveit servers. Since we do not know how long it will take them
        #to start, pause for 15 seconds
        jointServer = subprocess.Popen([
            "rosrun", "baxter_interface", "joint_trajectory_action_server.py"
        ])
        planServer = subprocess.Popen(
            ["roslaunch", "baxter_moveit_config", "move_group.launch"])
        time.sleep(15)

        raw_input("press enter")

        try:
            #left = MoveGroupCommander("left_arm")
            right = MoveGroupCommander("right_arm")

            upRight = geometry_msgs.msg.Pose(
                position=geometry_msgs.msg.Point(x=0.9667369015076861,
                                                 y=-0.1190874231664102,
                                                 z=-0.07489146157788866))

            #upLeft = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
            #x = 0.7, y = 0.35, z = 0.8))

            #downRight = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
            #x = 0.7, y = -0.45, z = 0.3))

            #downLeft = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
            #x = 0.7, y = 0.45, z = 0.3))

            right.set_pose_target(upRight)
            #left.set_pose_target(upLeft)
            right.go()
            #left.go()

            #right.set_pose_target(downRight)
            #left.set_pose_target(downLeft)

            right.go()
            #left.go()

        finally:
            #clean up - kill servers
            jointServer.kill()
            planServer.kill()

            sys.exit()
Esempio n. 12
0
def main():
    #Wait for the IK service to become available
    rospy.wait_for_service('compute_ik')
    rospy.init_node('service_query')

    #Create the function used to call the service
    compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)

    while not rospy.is_shutdown():
        raw_input('Press [ Enter ]: ')

        #Construct the request
        request = GetPositionIKRequest()
        request.ik_request.group_name = "left_arm"
        request.ik_request.ik_link_name = "left_gripper"
        request.ik_request.attempts = 20
        #request.ik_request.pose_stamped.header.frame_id = "base"
        request.ik_request.pose_stamped.header.frame_id = "base"

        #Set the desired orientation for the end effector HERE
        euler_angle = [-np.pi, 0, 0]
        traslation = [0.591, 0.261, 0.327 - 0.1]
        quaternion = tf.transformations.quaternion_from_euler(
            euler_angle[0], euler_angle[1], euler_angle[2])
        request.ik_request.pose_stamped.pose.position.x = traslation[0]
        request.ik_request.pose_stamped.pose.position.y = traslation[1]
        request.ik_request.pose_stamped.pose.position.z = traslation[2]
        request.ik_request.pose_stamped.pose.orientation.x = quaternion[0]
        request.ik_request.pose_stamped.pose.orientation.y = quaternion[1]
        request.ik_request.pose_stamped.pose.orientation.z = quaternion[2]
        request.ik_request.pose_stamped.pose.orientation.w = quaternion[3]

        try:
            #Send the request to the service
            response = compute_ik(request)

            #Print the response HERE
            #print(response)
            group = MoveGroupCommander("left_arm")

            # Setting position and orientation target
            group.set_pose_target(request.ik_request.pose_stamped)

            # TRY THIS
            # Setting just the position without specifying the orientation
            ###group.set_position_target([0.5, 0.5, 0.0])

            # Plan IK and execute
            group.go()

        except rospy.ServiceException, e:
            print "Service call failed: %s" % e
Esempio n. 13
0
class TestMove():

    def __init__(self):
        roscpp_initialize(sys.argv)
        rospy.init_node('ur3_move', anonymous=True)

        self.scene = PlanningSceneInterface()
        self.robot_cmd = RobotCommander()

        self.Pose_goal = Pose()

        self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
        # robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
        self.robot_arm.set_goal_orientation_tolerance(0.005)
        self.robot_arm.set_planning_time(5)
        self.robot_arm.set_num_planning_attempts(5)

        rospy.sleep(2)
        # Allow replanning to increase the odds of a solution
        self.robot_arm.allow_replanning(True)

    def move_code(self):
        # self.robot_arm.set_named_target("up")  #go to goal state.
        # self.robot_arm.go()
        # print("====== move plan go to home 1 ======")
        # rospy.sleep(2)
        #        print("====== move plan go to up ======")
        #        self.robot_arm.set_named_target("zeros")  #go to goal state.
        #        self.robot_arm.go(wait=True)
        #        print("====== move plan go to zeros ======")
        #        rospy.sleep(1)

        #        robot_arm.set_named_target("up")
        #        robot_arm.go(wait=True)

        self.robot_arm.set_pose_target(self.Pose_goal)  # go to goal state.
        self.robot_arm.go(True)
        print("====== move plan go to Pose_goal ======")
        #rospy.sleep(2)

        robot_state = self.robot_arm.get_current_pose();
        robot_angle = self.robot_arm.get_current_joint_values();

        print(robot_state)

    def callback(self, data):
        rospy.loginfo(rospy.get_caller_id() + "I heard %s", data)
        self.Pose_goal = data
        self.move_code()

    def listener(self):
        rospy.Subscriber("chatter", Pose, self.callback)
Esempio n. 14
0
 def execute(self, userdata):
     group = resolve_planning_group(userdata.ik_link)
     if not group:
         return 'aborted'
     
     (goal, tip_frame) = transform_to_tip(group, userdata.ik_link, userdata.goal)
     mc = MoveGroupCommander(group)
     mc.set_pose_target(goal, tip_frame)
     plan = mc.plan()
     userdata.trajectory = plan.joint_trajectory
     if not plan.joint_trajectory.points:
         return 'aborted'
     return 'succeeded'
Esempio n. 15
0
class TestMove():
    def __init__(self):
        roscpp_initialize(sys.argv)
        rospy.init_node('ur3_move', anonymous=True)

        self.scene = PlanningSceneInterface()
        self.robot_cmd = RobotCommander()

        self.Pose_goal = Pose()

        self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
        # robot_gripper = MoveGroupComm # self.robot_arm.set_named_target("up")  #go to goal state.
        # self.robot_arm.go()
        # print("====== move plan go to home 1 ======")
        # rospy.sleep(2)
        # print("====== move plan go to home 1 ======")
        # rospy.sleep(2)
        #        print("====== move plan go to up ======")
        #        self.robot_arm.set_named_target("zeros")  #go to goal state.
        #        self.robot_arm.go(wait=True)
        #        print("====== move plan go to zeros ======")
        #        rospy.sleep(1)

        #        robot_arm.set_named_target("up")
        #        robot_arm.go(wait=True)
        Pose_goal.header.frame_id = 'rightbase_link'
        Pose_goal.pose.position.x = -0.1955793462195291  # red line      0.2   0.2
        Pose_goal.pose.position.y = 0.3456909607161672  # green line  0.15   0.15
        Pose_goal.pose.position.z = 0.16049011785234568  # blue line   # 0.35   0.6
        # Pose_goal.pose.orientation = start_pose.orientation
        Pose_goal.pose.orientation.x = 0.28520761755123414
        Pose_goal.pose.orientation.y = 0.24468120052517786
        Pose_goal.pose.orientation.z = 0.6034841927127607
        Pose_goal.pose.orientation.w = 0.7032741671255489
        self.robot_arm.set_pose_target(self.Pose_goal)  # go to goal state.
        self.robot_arm.go(True)
        print("====== move plan go to Pose_goal ======")
        #rospy.sleep(2)

        robot_state = self.robot_arm.get_current_pose()
        robot_angle = self.robot_arm.get_current_joint_values()

        print(robot_state)

    def callback(self, data):
        rospy.loginfo(rospy.get_caller_id() + "I heard %s", data)
        self.Pose_goal = data
        self.move_code()

    def listener(self):
        rospy.Subscriber("chatter", Pose, self.callback)
Esempio n. 16
0
    def move_to(self,
                pos_x,
                pos_y,
                pos_z,
                orien_x,
                orien_y,
                orien_z,
                orien_w,
                ik,
                orien_const=None):
        #Construct the request
        request = GetPositionIKRequest()
        request.ik_request.group_name = "right_arm"
        request.ik_request.ik_link_name = "right_gripper"
        request.ik_request.attempts = 20
        request.ik_request.pose_stamped.header.frame_id = "base"

        #Set the desired orientation for the end effector HERE
        request.ik_request.pose_stamped.pose.position.x = pos_x
        request.ik_request.pose_stamped.pose.position.y = pos_y
        request.ik_request.pose_stamped.pose.position.z = pos_z
        request.ik_request.pose_stamped.pose.orientation.x = orien_x
        request.ik_request.pose_stamped.pose.orientation.y = orien_y
        request.ik_request.pose_stamped.pose.orientation.z = orien_z
        request.ik_request.pose_stamped.pose.orientation.w = orien_w

        try:
            #Send the request to the service
            response = ik(request)

            #Print the response HERE
            print(response)
            group = MoveGroupCommander("right_arm")

            if orien_const is not None:
                constraints = Constraints()
                constraints.orientation_constraints = orien_const
                group.set_path_constraints(constraints)

            # Setting position and orientation target
            group.set_pose_target(request.ik_request.pose_stamped)

            # TRY THIS
            # Setting just the position without specifying the orientation
            # group.set_position_target([0.5, 0.5, 0.0])

            # Plan IK and execute
            group.go()

        except rospy.ServiceException, e:
            print("Service call failed: %s")
Esempio n. 17
0
 def execute(self, userdata):
     group = 'arms'            
     mc = MoveGroupCommander(group)
     group_1 = resolve_planning_group(userdata.ik_link_1)
     (goal_1, tip_frame_1) = transform_to_tip(group_1, userdata.ik_link_1, userdata.goal_1)
     group_2 = resolve_planning_group(userdata.ik_link_2)
     (goal_2, tip_frame_2) = transform_to_tip(group_2, userdata.ik_link_2, userdata.goal_2)
     mc.set_pose_target(goal_1, tip_frame_1)
     mc.set_pose_target(goal_2, tip_frame_2)
     plan = mc.plan()
     userdata.trajectory = plan.joint_trajectory
     if not plan.joint_trajectory.points:
         return 'aborted'
     return 'succeeded'
Esempio n. 18
0
	def run(self, cycle, verbose = 2):

		#enable robot if not already done
		enableProcess = subprocess.Popen(["rosrun", "baxter_tools", 
		"enable_robot.py", "-e"])
		enableProcess.wait()
		
		#start moveit servers. Since we do not know how long it will take them 
		#to start, pause for 15 seconds
		jointServer = subprocess.Popen(["rosrun", "baxter_interface", 
		"joint_trajectory_action_server.py"])
		planServer = subprocess.Popen(["roslaunch", "baxter_moveit_config", 
		"move_group.launch"])
		time.sleep(15)
		
		raw_input("press enter")
		
		try:
			#left = MoveGroupCommander("left_arm")
			right = MoveGroupCommander("right_arm")
			
			upRight = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			x = 0.9667369015076861, y = -0.1190874231664102, z = -0.07489146157788866 ))
			
			#upLeft = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			#x = 0.7, y = 0.35, z = 0.8))
			
			#downRight = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			#x = 0.7, y = -0.45, z = 0.3))
			
			#downLeft = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
			#x = 0.7, y = 0.45, z = 0.3))
			
			right.set_pose_target(upRight)
			#left.set_pose_target(upLeft)
			right.go()
			#left.go()
			
			#right.set_pose_target(downRight)
			#left.set_pose_target(downLeft)
			
			right.go()
			#left.go()
			
		finally:
			#clean up - kill servers
			jointServer.kill()
			planServer.kill()
			
			sys.exit()
Esempio n. 19
0
class TestDensoMoveit(unittest.TestCase):
    arm = None

    def setUp(self):
        self.arm = MoveGroupCommander("manipulator")

    def test_moveit(self):
        p = [0.35, -0.35, 0.4]
        pose = PoseStamped(
            header=rospy.Header(stamp=rospy.Time.now(), frame_id='/BASE'),
            pose=Pose(position=Point(*p),
                      orientation=Quaternion(
                          *quaternion_from_euler(1.57, 0, 1.57, 'sxyz'))))
        self.arm.set_pose_target(pose)
        self.assertTrue(self.arm.go() or self.arm.go())
Esempio n. 20
0
def movement(des_coor, i):
    #Wait for the IK service to become available
    rospy.wait_for_service('compute_ik')
    received_info = True
    #Create the function used to call the service
    compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
    while received_info == True:

        #Construct the requests
        request = GetPositionIKRequest()
        request.ik_request.group_name = "left_arm"
        request.ik_request.ik_link_name = "left_gripper"
        request.ik_request.attempts = 30
        request.ik_request.pose_stamped.header.frame_id = "base"

        #Set the desired orientation for the end effector HERE
        print(des_coor)
        request.ik_request.pose_stamped.pose.position.x = des_coor[0]
        request.ik_request.pose_stamped.pose.position.y = des_coor[1]
        request.ik_request.pose_stamped.pose.position.z = 0.25 + initial_xyz[
            2]  # this should be a found constant
        # Should be constant Determined by Experiment
        request.ik_request.pose_stamped.pose.orientation.x = 0.0
        request.ik_request.pose_stamped.pose.orientation.y = 1.0
        request.ik_request.pose_stamped.pose.orientation.z = 0.0
        request.ik_request.pose_stamped.pose.orientation.w = 0.0

        try:
            #Send the request to the service
            response = compute_ik(request)

            #Print the response HERE
            print(response)
            if response.error_code.val == 1:
                received_info = False
            else:
                received_info = False
            # received_info = False
            group = MoveGroupCommander("left_arm")

            # Setting position and orientation target
            group.set_pose_target(request.ik_request.pose_stamped)

            # Plan IK and execute
            group.go()

        except rospy.ServiceException, e:
            print "Service call failed: %s" % e
Esempio n. 21
0
def talker_by13():
    #init
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('moveit_fk_demo')
    #cartesian = rospy.get_param('~cartesian', True)
    arm = MoveGroupCommander('manipulator')

    arm.set_pose_reference_frame('base_link')

    arm.allow_replanning(True)
    arm.set_goal_position_tolerance(0.001)
    arm.set_goal_orientation_tolerance(0.001)
    # arm.set_max_acceleration_scaling_factor(0.5)
    #arm.set_max_velocity_scaling_factor(0.5)

    end_effector_link = arm.get_end_effector_link()

    #arm.set_named_target('home')
    arm.set_named_target('up')
    arm.go()
    rospy.sleep(2)

    target_pose = PoseStamped()
    target_pose.header.frame_id = 'base_link'
    target_pose.header.stamp = rospy.Time.now()
    target_pose.pose.position.x = 0.86
    target_pose.pose.position.y = 0.25
    target_pose.pose.position.z = 0.02832
    target_pose.pose.orientation.x = 0
    target_pose.pose.orientation.y = 0
    target_pose.pose.orientation.z = 0
    target_pose.pose.orientation.w = 1
    #next: find workspace

    arm.set_start_state_to_current_state()
    arm.set_pose_target(target_pose, end_effector_link)

    traj = arm.plan()

    arm.execute(traj)

    #arm.shift_pose_target(2,-0.05,end_effector_link)
    #arm.go()
    rospy.sleep(2)

    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)
def move_action(x):

    #Create the function used to call the service

    compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
    step = 0
    pose = [x]
    if step == 0:

        s = pose[step]

        request = GetPositionIKRequest()
        request.ik_request.group_name = "left_arm"
        request.ik_request.ik_link_name = "left_gripper"
        request.ik_request.attempts = 20
        request.ik_request.pose_stamped.header.frame_id = "ar_marker_0"

        request.ik_request.pose_stamped.pose.position.x = s[0]
        request.ik_request.pose_stamped.pose.position.y = s[1]
        request.ik_request.pose_stamped.pose.position.z = s[2]
        request.ik_request.pose_stamped.pose.orientation.x = s[3]
        request.ik_request.pose_stamped.pose.orientation.y = s[4]
        request.ik_request.pose_stamped.pose.orientation.z = s[5]
        request.ik_request.pose_stamped.pose.orientation.w = s[6]

        response = compute_ik(request)
        group = MoveGroupCommander("left_arm")
        group.set_pose_target(request.ik_request.pose_stamped)
        # orien_const = OrientationConstraint()
        # orien_const.link_name = "left_arm"
        # orien_const.header.frame_id = "base"
        # orien_const.orientation.x = 0.0

        # orien_const.orientation.y = 1.0

        # orien_const.orientation.z = 0.0

        # orien_const.absolute_x_axis_tolerance = 0.1
        # orien_const.absolute_y_axis_tolerance = 0.1
        # orien_const.absolute_z_axis_tolerance = 0.1
        # orien_const.weight = 1.0
        # consts = Constraints()
        # consts.orientation_constraints = [orien_const]
        # group.set_path_constraints(consts)
        group.go()
        step = step + 1
Esempio n. 23
0
    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('moveit_test')

        arm = MoveGroupCommander('arm')
        end_effector_link = arm.get_end_effector_link()
        arm.allow_replanning(True)
        arm.set_planning_time(5)

        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_init_orientation = Quaternion()
        target_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0)
        self.setPose(target_pose, [0.5, 0.2, 0.5],list(target_init_orientation))

        current_pose = arm.get_current_pose(end_effector_link)
        self.setPose(current_pose, [current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z], list(target_init_orientation))
        arm.set_pose_target(current_pose)
        arm.go()
        rospy.sleep(2)


        constraints = Constraints()
        orientation_constraint = OrientationConstraint()
        constraints.name = 'gripper constraint'
        orientation_constraint.header = target_pose.header
        orientation_constraint.link_name = end_effector_link
        orientation_constraint.orientation.x = target_init_orientation[0]
        orientation_constraint.orientation.y = target_init_orientation[1]
        orientation_constraint.orientation.z = target_init_orientation[2]
        orientation_constraint.orientation.w = target_init_orientation[3]

        orientation_constraint.absolute_x_axis_tolerance = 0.03
        orientation_constraint.absolute_y_axis_tolerance = 0.03
        orientation_constraint.absolute_z_axis_tolerance = 3.14
        orientation_constraint.weight = 1.0

        constraints.orientation_constraints.append(orientation_constraint)
        arm.set_path_constraints(constraints)

        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()
        rospy.sleep(1)

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
Esempio n. 24
0
def main():
    #Wait for the IK service to become available
    rospy.wait_for_service('compute_ik')
    rospy.init_node('service_query')
    
    #Create the function used to call the service
    compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
    
    while not rospy.is_shutdown():
        raw_input('Press [ Enter ]: ')
        
        #Construct the request
        request = GetPositionIKRequest()
        request.ik_request.group_name = "left_arm"
        request.ik_request.ik_link_name = "left_hand"
        request.ik_request.attempts = 20
        request.ik_request.pose_stamped.header.frame_id = "base"
        
        #Set the desired orientation for the end effector HERE
        request.ik_request.pose_stamped.pose.position.x = 0.5
        request.ik_request.pose_stamped.pose.position.y = 0.5
        request.ik_request.pose_stamped.pose.position.z = 0.3        
        request.ik_request.pose_stamped.pose.orientation.x = 0.0
        request.ik_request.pose_stamped.pose.orientation.y = 1.0
        request.ik_request.pose_stamped.pose.orientation.z = 0.0
        request.ik_request.pose_stamped.pose.orientation.w = 0.0
        
        try:
            #Send the request to the service
            response = compute_ik(request)
            
            #Print the response HERE
            print(response)
            group = MoveGroupCommander("left_arm")
            
            #this command tries to achieve a pose: which is position, orientation
            group.set_pose_target(request.ik_request.pose_stamped)

            #then, this command tries to achieve a position only.  orientation is airbitrary.
            group.set_position_target([0.5,0.5,0.3])
            group.go()
            
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e
Esempio n. 25
0
def main():
#Wait for the IK service to become available
	rospy.wait_for_service('compute_ik')
	rospy.init_node('service_query')

#Create the function used to call the service
	compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)

	while not rospy.is_shutdown():
		raw_input('Press [ Enter ]: ')

		#Construct the request
		request = moveit_msgs.srv.GetPositionIKRequest() #asdf
		request.ik_request.group_name = "left_arm"
		request.ik_request.ik_link_name = "left_gripper"
		request.ik_request.attempts = 20
		request.ik_request.pose_stamped.header.frame_id = "base"

		#Set the desired orientation for the end effector HERE
		request.ik_request.pose_stamped.pose.position.x = 0.5
		request.ik_request.pose_stamped.pose.position.y = 0.5
		request.ik_request.pose_stamped.pose.position.z = 0.0
		request.ik_request.pose_stamped.pose.orientation.x = 0.0
		request.ik_request.pose_stamped.pose.orientation.y = 1.0
		request.ik_request.pose_stamped.pose.orientation.z = 0.0
		request.ik_request.pose_stamped.pose.orientation.w = 0.0

		try:
		#Send the request to the service
			response = compute_ik(request)

			#Print the response HERE
			print(response)
			group = MoveGroupCommander("left_arm")
			# Setting position and orientation target
			group.set_pose_target(request.ik_request.pose_stamped)
			# TRY THIS
			# Setting just the position without specifying the orientation
			###group.set_position_target([0.5, 0.5, 0.0])
			# Plan IK and execute
			group.go()

		except rospy.ServiceException, e:
			print "Service call failed: %s"%e
class Tiago():
    def __init__(self, group_name='arm_torso'):

        self.rate = rospy.Rate(10)

        self.robot = RobotCommander()
        rospy.sleep(1)

        self.move_group = MoveGroupCommander(group_name)

        self.velocity = Twist()
        self.velocity_publisher = rospy.Publisher(
            '/mobile_base_controller/cmd_vel', Twist, queue_size=10)

    def move(self, linear=(0, 0, 0), angular=(0, 0, 0)):
        self.velocity.linear.x = linear[
            0]  # Forward or Backward with in m/sec.
        self.velocity.linear.y = linear[1]
        self.velocity.linear.z = linear[2]

        self.velocity.angular.x = angular[0]
        self.velocity.angular.y = angular[1]
        self.velocity.angular.z = angular[
            2]  # Anti-clockwise/clockwise in radians per sec

        self.velocity_publisher.publish(self.velocity)

    def send_arm_goal(self, frame_id='base_footprint'):
        # SET EEF GOAL
        goal_pose = PoseStamped()
        goal_pose.header.frame_id = 'base_footprint'
        goal_pose.pose.position.x = 0.55
        goal_pose.pose.position.y = -0.30
        goal_pose.pose.position.z = 0.76
        goal_pose.pose.orientation = Quaternion(
            *quaternion_from_euler(PI / 2, 0.0, 0.0))

        # SEND GOAL
        self.move_group.set_pose_reference_frame(frame_id)
        self.move_group.set_pose_target(goal_pose)

        # EXECUTE EEF GOAL
        # print(dir(self.move_group))
        self.move_group.go()
Esempio n. 27
0
def talker_by13():
    	#init
    	moveit_commander.roscpp_initialize(sys.argv)
    	rospy.init_node('moveit_fk_demo')
        cartesian = rospy.get_param('~cartesian', True)
        arm = MoveGroupCommander('manipulator')

        #arm.set_pose_reference_frame('base_link')

        #arm.set_goal_position_tolerance(0.001)
       # arm.set_goal_orientation_tolerance(0.001)
       # arm.set_max_acceleration_scaling_factor(0.5)
        #arm.set_max_velocity_scaling_factor(0.5)

        end_effector_link = arm.get_end_effector_link()

        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)

        start_pose = arm.get_current_pose(end_effector_link).pose

        waypoints = []

        wpose = deepcopy(start_pose)
        wpose.position.x -= 0.2
        wpose.position.y -= 0.2
	#wpose.position.z += 0.6 
        #wpose.position.x = 0.8
        #wpose.position.y = 0.9
	#wpose.position.z = 0.9

        arm.set_pose_target(wpose)  
        arm.go()
        rospy.sleep(1)

        




   	moveit_commander.roscpp_shutdown()
    	moveit_commander.os._exit(0)
Esempio n. 28
0
def movement(des_coor):
    #Wait for the IK service to become available
    rospy.wait_for_service('compute_ik')
    rospy.init_node('service_query')

    #Create the function used to call the service
    compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)

    while not rospy.is_shutdown():

        #Construct the request
        request = GetPositionIKRequest()
        request.ik_request.group_name = "right_arm"
        request.ik_request.ik_link_name = "right_gripper"
        request.ik_request.attempts = 20
        request.ik_request.pose_stamped.header.frame_id = "base"

        #Set the desired orientation for the end effector HERE
        request.ik_request.pose_stamped.pose.position.x = des_coor[0]
        request.ik_request.pose_stamped.pose.position.y = des_coor[1]
        request.ik_request.pose_stamped.pose.position.z = 0.0  # this should be a found constant
        # Should be constant Determined by Experiment
        request.ik_request.pose_stamped.pose.orientation.x = 0.0
        request.ik_request.pose_stamped.pose.orientation.y = 1.0
        request.ik_request.pose_stamped.pose.orientation.z = 0.0
        request.ik_request.pose_stamped.pose.orientation.w = 0.0

        try:
            #Send the request to the service
            response = compute_ik(request)

            #Print the response HERE
            print(response)
            group = MoveGroupCommander("right_arm")

            # Setting position and orientation target
            group.set_pose_target(request.ik_request.pose_stamped)

            # Plan IK and execute
            group.go()

        except [rospy.ServiceException, e]:
            print("Service call failed: %s" % e)
Esempio n. 29
0
def Baxter_Go(target_translation, reference_frame, compute_ik):
    #Construct the request
    request = GetPositionIKRequest()
    request.ik_request.group_name = "left_arm"
    request.ik_request.ik_link_name = "left_gripper"
    request.ik_request.attempts = 20
    #request.ik_request.pose_stamped.header.frame_id = "base"
    request.ik_request.pose_stamped.header.frame_id = reference_frame

    #Set the desired orientation for the end effector HERE
    euler_angle = [-np.pi, 0, 0]
    quaternion = tf.transformations.quaternion_from_euler(
        euler_angle[0], euler_angle[1], euler_angle[2])
    request.ik_request.pose_stamped.pose.position.x = target_translation[0]
    request.ik_request.pose_stamped.pose.position.y = target_translation[1]
    request.ik_request.pose_stamped.pose.position.z = target_translation[2]
    request.ik_request.pose_stamped.pose.orientation.x = quaternion[0]
    request.ik_request.pose_stamped.pose.orientation.y = quaternion[1]
    request.ik_request.pose_stamped.pose.orientation.z = quaternion[2]
    request.ik_request.pose_stamped.pose.orientation.w = quaternion[3]

    try:
        #Send the request to the service
        response = compute_ik(request)

        #Print the response HERE
        #print(response)
        group = MoveGroupCommander("left_arm")

        # Setting position and orientation target
        group.set_pose_target(request.ik_request.pose_stamped)

        # TRY THIS
        # Setting just the position without specifying the orientation
        ###group.set_position_target([0.5, 0.5, 0.0])

        # Plan IK and execute
        group.go()

    except rospy.ServiceException, e:
        print "Service call failed: %s" % e
Esempio n. 30
0
def move_hand(robot_hand, human_hand):  
    #Initiaize other variables
    right_gripper = robot_gripper.Gripper('right')
    left_gripper = robot_gripper.Gripper('left')

    #right_gripper.calibrate()
    right_gripper.close()

    #Move to outside position
    #Move right hand to outside position
    raw_input('Press [Enter]:')

    #Construct the request for right hand
    request = GetPositionIKRequest()
    request.ik_request.group_name = "right_arm"
    request.ik_request.ik_link_name = "right_gripper"
    request.ik_request.attempts = 500
    request.ik_request.pose_stamped.header.frame_id = "base"
        
    #Set the desired orientation for the end effector HERE
    request.ik_request.pose_stamped.pose.position.x = 0.5
    request.ik_request.pose_stamped.pose.position.y = -0.35
    request.ik_request.pose_stamped.pose.position.z = 0.4
    request.ik_request.pose_stamped.pose.orientation.x = 0.0
    request.ik_request.pose_stamped.pose.orientation.y = 1.0
    request.ik_request.pose_stamped.pose.orientation.z = 0.0
    request.ik_request.pose_stamped.pose.orientation.w = 0.0

    try:

        group = MoveGroupCommander("right_arm")

        # Setting position and orientation target
        group.set_pose_target(request.ik_request.pose_stamped)

        print("Trying to move the right arm out of the way!")
        # Plan IK and execute
        group.go()

    except rospy.ServiceException, e:
        print "Service call failed: %s"%e
Esempio n. 31
0
def move_to(x, y, z):
    #Create the function used to call the service
    compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)

    #Construct the request
    request = GetPositionIKRequest()
    request.ik_request.group_name = "left_arm"
    request.ik_request.ik_link_name = "left_gripper"
    request.ik_request.attempts = 20
    request.ik_request.pose_stamped.header.frame_id = "base"
    #Set the desired orientation for the end effector HERE
    request.ik_request.pose_stamped.pose.position.x = x
    request.ik_request.pose_stamped.pose.position.y = y
    request.ik_request.pose_stamped.pose.position.z = z
    request.ik_request.pose_stamped.pose.orientation.x = 0.0
    request.ik_request.pose_stamped.pose.orientation.y = 1.0
    request.ik_request.pose_stamped.pose.orientation.z = 0.0
    request.ik_request.pose_stamped.pose.orientation.w = 0.0

    try:
        #Send the request to the service
        response = compute_ik(request)

        #Print the response HERE
        print(response)
        group = MoveGroupCommander("left_arm")

        # Setting position and orientation target
        group.set_pose_target(request.ik_request.pose_stamped)

        # TRY THIS
        # Setting just the position without specifying the orientation
        ###group.set_position_target([0.5, 0.5, 0.0])

        # Plan IK and execute
        group.go()

    except rospy.ServiceException, e:
        print "Service call failed: %s" % e
Esempio n. 32
0
    def __init__(self):
        group = MoveGroupCommander("arm")
        
        #group.set_orientation_tolerance([0.3,0.3,0,3])
        
        p = PoseStamped()
        p.header.frame_id = "/katana_base_link"
        p.pose.position.x = 0.4287
        #p.pose.position.y = -0.0847
        p.pose.position.y = 0.0
        p.pose.position.z = 0.4492
        
        
        q = quaternion_from_euler(2, 0, 0)
        
        p.pose.orientation.x = q[0]
        p.pose.orientation.y = q[1]
        p.pose.orientation.z = q[2]
        p.pose.orientation.w = q[3]
        


        
        print "Planning frame: " ,group.get_planning_frame()
        print "Pose reference frame: ",group.get_pose_reference_frame()
        
        #group.set_pose_reference_frame("katana_base_link")

        print "RPy target: 0,0,0"
        #group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame")
        #group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame")
        
        group.set_pose_target(p, "katana_gripper_tool_frame")
        
        group.go()
        print "Current rpy: " , group.get_current_rpy("katana_gripper_tool_frame")
def main():

    init_node()

    # Preparing Head
    rospy.loginfo("Preparing Head...")
    headg = MoveGroupCommander("head")
    headg.set_joint_value_target([0.0, 60.0 / 180.0 * math.pi])
    headg.go()

    # Preparing Both Arms
    rospy.loginfo("Preparing Left Arm...")
    barmg = MoveGroupCommander("botharms")
    barmg.set_pose_target([0.325, 0.482, 0.167, 0.0, -math.pi / 2, 0.0],
                          'LARM_JOINT5_Link')
    barmg.set_pose_target([0.325, -0.482, 0.167, 0.0, -math.pi / 2, 0.0],
                          'RARM_JOINT5_Link')
    barmg.go()
    rospy.sleep(2.0)

    # Right Arm
    group = MoveGroupCommander("right_arm")

    # Frame ID Definitoins
    planning_frame_id = group.get_planning_frame()
    tgt_frame_id = '/ar_marker_4'

    # Get a target pose
    pose_target = get_current_target_pose(tgt_frame_id, planning_frame_id, 5.0)

    # Move to a point above target
    if pose_target:

        # Rotate Pose for Right Hand
        quat = []
        quat.append(pose_target.pose.orientation.x)
        quat.append(pose_target.pose.orientation.y)
        quat.append(pose_target.pose.orientation.z)
        quat.append(pose_target.pose.orientation.w)
        quat = quaternion_multiply(
            quat, quaternion_about_axis(math.pi / 2, (1, 0, 0)))
        quat = quaternion_multiply(
            quat, quaternion_about_axis(math.pi / 2, (0, 0, 1)))
        pose_target.pose.orientation.x = quat[0]
        pose_target.pose.orientation.y = quat[1]
        pose_target.pose.orientation.z = quat[2]
        pose_target.pose.orientation.w = quat[3]

        pose_target.pose.position.z += 0.4
        rospy.loginfo("Set Target To: \n{}".format(pose_target))
        group.set_pose_target(pose_target)
        ret = group.go()
        rospy.loginfo("Executed ... {}".format(ret))
    else:
        rospy.logwarn("Pose Error: {}".format(pose_target))
Esempio n. 34
0
class ArmTracker:
    def __init__(self):
        rospy.init_node('arm_tracker')
        
        rospy.on_shutdown(self.shutdown)
        
        # Maximum distance of the target before the arm will lower
        self.max_target_dist = 1.2
        
        # Arm length to center of gripper frame
        self.arm_length = 0.4
        
        # Distance between the last target and the new target before we move the arm
        self.last_target_threshold = 0.01
        
        # Distance between target and end-effector before we move the arm
        self.target_ee_threshold = 0.025
        
        # Initialize the move group for the right arm
        self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
        
        # Set the reference frame for pose targets
        self.reference_frame = REFERENCE_FRAME
        
        # Keep track of the last target pose
        self.last_target_pose = PoseStamped()
        
        # Set the right arm reference frame accordingly
        self.right_arm.set_pose_reference_frame(self.reference_frame)
                        
        # Allow replanning to increase the chances of a solution
        self.right_arm.allow_replanning(False)
                
        # Set a position tolerance in meters
        self.right_arm.set_goal_position_tolerance(0.05)
        
        # Set an orientation tolerance in radians
        self.right_arm.set_goal_orientation_tolerance(0.2)
        
        # What is the end effector link?
        self.ee_link = self.right_arm.get_end_effector_link()
        
        # Create the transform listener
        self.listener = tf.TransformListener()
        
        # Queue up some tf data...
        rospy.sleep(3)
        
        # Set the gripper target to closed position using a joint value target
        right_gripper.set_joint_value_target(GRIPPER_CLOSED)
         
        # Plan and execute the gripper motion
        right_gripper.go()
        rospy.sleep(1)
                
        # Subscribe to the target topic
        rospy.wait_for_message('/target_pose', PoseStamped)
        
        # Use queue_size=1 so we don't pile up outdated target messages
        self.target_subscriber = rospy.Subscriber('/target_pose', PoseStamped, self.update_target_pose, queue_size=1)
        
        rospy.loginfo("Ready for action!")
        
        while not rospy.is_shutdown():
            try:
                target = self.target
            except:
                rospy.sleep(0.5)
                continue
                        
            # Timestamp the target with the current time
            target.header.stamp = rospy.Time()
            
            # Get the target pose in the right_arm shoulder lift frame
            #target_arm = self.listener.transformPose('right_arm_shoulder_pan_link', target)
            target_arm = self.listener.transformPose('right_arm_base_link', target)
            
            # Convert the position values to a Python list
            p0 = [target_arm.pose.position.x, target_arm.pose.position.y, target_arm.pose.position.z]
             
            # Compute the distance between the target and the shoulder link
            dist_target_shoulder = euclidean(p0, [0, 0, 0])
                                         
            # If the target is too far away, then lower the arm
            if dist_target_shoulder > self.max_target_dist:
                rospy.loginfo("Target is too far away")
                self.right_arm.set_named_target('resting')
                self.right_arm.go()
                rospy.sleep(1)
                continue
            
            # Transform the pose to the base reference frame
            target_base = self.listener.transformPose(self.reference_frame, target)
            
            # Compute the distance between the current target and the last target
            p1 = [target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z]
            p2 = [self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z]
                    
            dist_last_target = euclidean(p1, p2)
            
            # Move the arm only if we are far enough away from the previous target
            if dist_last_target < self.last_target_threshold:
                rospy.loginfo("Still close to last target")
                rospy.sleep(0.5)
                continue
            
            # Get the pose of the end effector in the base reference frame
            ee_pose = self.right_arm.get_current_pose(self.ee_link)
            
            # Convert the position values to a Python list
            p3 = [ee_pose.pose.position.x, ee_pose.pose.position.y, ee_pose.pose.position.z]
            
            # Compute the distance between the target and the end-effector
            dist_target = euclidean(p1, p3)
            
            # Only move the arm if we are far enough away from the target
            if dist_target < self.target_ee_threshold:
                rospy.loginfo("Already close enough to target")
                rospy.sleep(1)
                continue
            
            # We want the gripper somewhere on the line connecting the shoulder and the target.
            # Using a parametric form of the line, the parameter ranges from 0 to the
            # minimum of the arm length and the distance to the target.
            t_max = min(self.arm_length, dist_target_shoulder)
            
            # Bring it back 10% so we don't collide with the target
            t = 0.9 * t_max
            
            # Now compute the target positions from the parameter
            try:
                target_arm.pose.position.x *= (t / dist_target_shoulder)
                target_arm.pose.position.y *= (t / dist_target_shoulder)
                target_arm.pose.position.z *= (t / dist_target_shoulder)
            except:
                rospy.sleep(1)
                rospy.loginfo("Exception!")
                continue
            
            # Transform to the base_footprint frame
            target_ee = self.listener.transformPose(self.reference_frame, target_arm)
            
            # Set the target gripper orientation to be horizontal
            target_ee.pose.orientation.x = 0
            target_ee.pose.orientation.y = 0
            target_ee.pose.orientation.z = 0
            target_ee.pose.orientation.w = 1
            
            # Update the current start state
            self.right_arm.set_start_state_to_current_state()
            
            # Set the target pose for the end-effector
            self.right_arm.set_pose_target(target_ee, self.ee_link)
            
            # Plan and execute the trajectory
            success = self.right_arm.go()
            
            if success:
                # Store the current target as the last target
                self.last_target_pose = target
            
            # Pause a bit between motions to keep from locking up
            rospy.sleep(0.5)
            
                    
    def update_target_pose(self, target):
        self.target = target

    def relax_all_servos(self):
        command = 'rosrun donaxi_dynamixels arbotix_relax_all_servos.py'
        args = shlex.split(command)
        subprocess.Popen(args)
           
    def shutdown(self):
        # Stop any further target messages from being processed
        self.target_subscriber.unregister()
        
        # Stop any current arm movement
        self.right_arm.stop()
        
        # Move to the resting position
        self.right_arm.set_named_target('resting')
        self.right_arm.go()
        
        # Relax the servos
        self.relax_all_servos()
        
        os._exit(0) 
Esempio n. 35
0
class MoveItDemo:
    def __init__(self):



        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')
        
        #Initialize robot
        robot = moveit_commander.RobotCommander()

        # Use the planning scene object to add or remove objects
        self.scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
        
        # Create a publisher for displaying object frames
        self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)

        ### Create a publisher for visualizing direction ###
        self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)


        # Create a dictionary to hold object colors
        self.colors = dict()

        # Initialize the MoveIt! commander for the arm
        self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the MoveIt! commander for the gripper
        self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Allow 5 seconds per planning attempt
        self.right_arm.set_planning_time(5)

        # Prepare Action Controller for gripper
        self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
        self.ac.wait_for_server()

        # Give the scene a chance to catch up
        rospy.sleep(2)
        
        # Prepare Gazebo Subscriber
        self.pwh = None
        self.pwh_copy = None
        self.idx_targ = None
        self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)


        ### OPEN THE GRIPPER ###
        self.open_gripper()

        self.obj_att = None


        # PREPARE THE SCENE
        while self.pwh is None:
            rospy.sleep(0.05)

        ############## CLEAR THE SCENE ################


#        planning_scene.world.collision_objects.remove('target')

        # Remove leftover objects from a previous run
        self.scene.remove_world_object('target')
        self.scene.remove_world_object('table')
#        self.scene.remove_world_object(obstacle1_id)

        # Remove any attached objects from a previous session
        self.scene.remove_attached_object(GRIPPER_FRAME, 'target')

        # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
        timerThread = threading.Thread(target=self.scene_generator)
        timerThread.daemon = True
        timerThread.start()


        initial_pose = target_pose
        initial_pose.header.frame_id = 'gazebo_world'


        print "==================== Generating Transformations ==========================="

        #################### PRE GRASPING POSE #########################
#        M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
#        M1[0,3] = target_pose.pose.position.x
#        M1[1,3] = target_pose.pose.position.y 
#        M1[2,3] = target_pose.pose.position.z

#        M2 = transformations.euler_matrix(0, 1.57, 0)
#        M2[0,3] = 0.0  # offset about x
#        M2[1,3] = 0.0   # about y
#        M2[2,3] = 0.25  # about z

#        T = np.dot(M1,  M2)
#        pre_grasping = deepcopy(target_pose)
#        pre_grasping.pose.position.x = T[0,3] 
#        pre_grasping.pose.position.y = T[1,3]
#        pre_grasping.pose.position.z = T[2,3]

#        quat = transformations.quaternion_from_matrix(T)
#        pre_grasping.pose.orientation.x = quat[0]
#        pre_grasping.pose.orientation.y = quat[1]
#        pre_grasping.pose.orientation.z = quat[2]
#        pre_grasping.pose.orientation.w = quat[3]
#        pre_grasping.header.frame_id = 'gazebo_world'
#        self.plan_exec(pre_grasping)


        ################# GENERATE GRASPS ###################
        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 5

        # Track success/failure and number of attempts for pick operation
        success = False
        n_attempts = 0

        grasps = self.grasp_generator(initial_pose)
        possible_grasps = []

        for grasp in grasps:
            self.gripper_pose_pub.publish(grasp)
            possible_grasps.append(grasp.pose)
            rospy.sleep(0.2)
        #print possible_grasps

        self.right_arm.pick(target_id, grasps)

#        target_name = target_id
#        group_name = GROUP_NAME_ARM
#        end_effector = GROUP_NAME_GRIPPER
#        attached_object_touch_links = ['r_forearm_link']
#        #print (target_name, group_name, end_effector)
#        PickupGoal(target_name, group_name ,end_effector, possible_grasps )



#        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

#        # Exit the script
        moveit_commander.os._exit(0)



################################################################# FUNCTIONS #################################################################################



    def model_state_callback(self,msg):

        self.pwh = ModelStates()
        self.pwh = msg


    def grasp_generator(self, initial_pose):

        # Pitch angles to try
        pitch_vals = [1, 1.57,0, 1,2 ]

        # Yaw angles to try
        yaw_vals = [0]#, 1.57, -1.57]
        

        # A list to hold the grasps
        grasps = []
        g = PoseStamped()
        g.header.frame_id = REFERENCE_FRAME
        g.pose = initial_pose.pose
        #g.pose.position.z += 0.18

        # Generate a grasp for each pitch and yaw angle
        for y in yaw_vals:
            for p in pitch_vals:
                # Create a quaternion from the Euler angles
                q = transformations.quaternion_from_euler(0, p, y)

                # Set the grasp pose orientation accordingly
                g.pose.orientation.x = q[0]
                g.pose.orientation.y = q[1]
                g.pose.orientation.z = q[2]
                g.pose.orientation.w = q[3]


                # Append the grasp to the list
                grasps.append(deepcopy(g))
     
        # Return the list
        return grasps





    def plan_exec(self, pose):

        self.right_arm.clear_pose_targets()
        self.right_arm.set_pose_target(pose, GRIPPER_FRAME)
        self.right_arm.plan()
        rospy.sleep(5)
        self.right_arm.go(wait=True)


    def close_gripper(self):

        g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
        self.ac.send_goal(g_close)
        self.ac.wait_for_result()
        rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object


    def open_gripper(self):

        g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
        self.ac.send_goal(g_open)
        self.ac.wait_for_result()
        rospy.sleep(5) # And up to 20 to detach it


    def scene_generator(self):
#        print obj_att
        global target_pose
        global target_id

        next_call = time.time()
        while True:
            next_call = next_call+1
            target_id = 'target'
            self.taid = self.pwh.name.index('wood_cube_5cm')
            table_id = 'table'
            self.tid = self.pwh.name.index('table') 
#            obstacle1_id = 'obstacle1'
#            self.o1id = self.pwh.name.index('wood_block_10_2_1cm')


            # Set the target size [l, w, h]
            target_size = [0.05, 0.05, 0.05]
            table_size = [1.5, 0.8, 0.03]
#            obstacle1_size = [0.1, 0.025, 0.01]

            ## Set the target pose on the table
            target_pose = PoseStamped()
            target_pose.header.frame_id = REFERENCE_FRAME
            target_pose.pose = self.pwh.pose[self.taid]
            target_pose.pose.position.z += 0.025
            if self.obj_att is None:
                # Add the target object to the scene
                self.scene.add_box(target_id, target_pose, target_size)

                table_pose = PoseStamped()
                table_pose.header.frame_id = REFERENCE_FRAME
                table_pose.pose = self.pwh.pose[self.tid]
                table_pose.pose.position.z += 1
                self.scene.add_box(table_id, table_pose, table_size)
                
#                obstacle1_pose = PoseStamped()
#                obstacle1_pose.header.frame_id = REFERENCE_FRAME
#                obstacle1_pose.pose = self.pwh.pose[self.o1id]
#                # Add the target object to the scene
#                scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)

                ### Make the target purple ###
                self.setColor(target_id, 0.6, 0, 1, 1.0)

                # Send the colors to the planning scene
                self.sendColors()
            else: 
                self.scene.remove_world_object('target')

            time.sleep(next_call - time.time())
        #threading.Timer(0.5, self.scene_generator).start()

    # Set the color of an object
    def setColor(self, name, r, g, b, a = 0.9):
        # Initialize a MoveIt color object
        color = ObjectColor()

        # Set the id to the name given as an argument
        color.id = name

        # Set the rgb and alpha values given as input
        color.color.r = r
        color.color.g = g
        color.color.b = b
        color.color.a = a

        # Update the global color dictionary
        self.colors[name] = color

    # Actually send the colors to MoveIt!
    def sendColors(self):
        # Initialize a planning scene object
        p = PlanningScene()

        # Need to publish a planning scene diff        
        p.is_diff = True

        # Append the colors from the global color dictionary 
        for color in self.colors.values():
            p.object_colors.append(color)

        # Publish the scene diff
        self.scene_pub.publish(p)
Esempio n. 36
0
class Planner(object):
    move_group = None
    goals = None
    jspub = None
    namespace = None

    # These will eventually go to model objects
    robot_data = {
        'group_name': 'right_arm_and_torso',
        'eef_link': 'r_wrist_joint_link'
    }

    # Current state of the 'session' (right now, only one)
    current_scene = None
    status = None
    link_poses = None

    def __init__(self):
        rospy.init_node('moveit_web',disable_signals=True)
        self.jspub = rospy.Publisher('/update_joint_states',JointState)
        self.psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)

        # Give time for subscribers to connect to the publisher
        rospy.sleep(1)
        self.goals = []

        # HACK: Synthesize a valid initial joint configuration for PR2
        initial_joint_state = JointState()
        initial_joint_state.name = ['r_elbow_flex_joint']
        initial_joint_state.position = [-0.1]
        self.jspub.publish(initial_joint_state)

        # Create group we'll use all along this demo
        # self.move_group = MoveGroupCommander('right_arm_and_torso')
        self.move_group = MoveGroupCommander(self.robot_data['group_name'])
        self._move_group = self.move_group._g
        self.ps = PlanningSceneInterface()

        self.status = {'text':'ready to plan','ready':True}

    def get_scene(self):
        return self.current_scene

    def set_scene(self, scene):
        self.current_scene = scene
        psw = PlanningSceneWorld()
        for co_json in scene['objects']:
            # TODO: Fix orientation by using proper quaternions on the client
            pose = self._make_pose(co_json['pose'])
            # TODO: Decide what to do with STL vs. Collada. The client has a Collada
            # loader but the PlanningSceneInterface can only deal with STL.
            # TODO: Proper mapping between filenames and URLs
            # filename = '/home/julian/aaad/moveit/src/moveit_web/django%s' % co_json['meshUrl']
            filename = '/home/julian/aaad/moveit/src/moveit_web/django/static/meshes/table_4legs.stl'
            co = self.ps.make_mesh(co_json['name'], pose, filename)
            psw.collision_objects.append(co)
        self.psw_pub.publish(psw)


    def get_link_poses(self):
        if self.link_poses is None:
            self.link_poses = self._move_group.get_link_poses_compressed()
        return self.link_poses

    # Create link back to socket.io namespace to allow emitting information
    def set_socket(self, namespace):
        self.namespace = namespace

    def emit(self, event, data=None):
        if self.namespace:
            self.namespace.emit(event, data)

    def emit_new_goal(self, pose):
        self.emit('target_pose', message_converter.convert_ros_message_to_dictionary(pose)['pose'])

    def set_random_goal(self):
        goal_pose = self.move_group.get_random_pose()
        # goal_pose = self.move_group.get_random_pose('base_footprint')
        self.emit_new_goal(goal_pose)

    def _make_pose(self, json_pose):
        pose = PoseStamped()
        pose.header.frame_id = "odom_combined"
        pp = json_pose['position']
        pose.pose.position.x = pp['x']
        pose.pose.position.y = pp['y']
        pose.pose.position.z = pp['z']
        # TODO: Orientation is not working. See about
        # properly using Quaternions everywhere
        pp = json_pose['orientation']
        pose.pose.orientation.x = pp['x']
        pose.pose.orientation.y = pp['y']
        pose.pose.orientation.z = pp['z']
        pose.pose.orientation.w = pp['w']
        return pose

    def plan_to_poses(self, poses):
        goal_pose = self._make_pose(poses[0])
        self.move_group.set_pose_target(goal_pose)
        # self.move_group.set_pose_target(goal_pose,'base_footprint')
        self.emit('status',{'text':'Starting to plan'})
        trajectory = self.move_group.plan()
        if trajectory is None or len(trajectory.joint_trajectory.joint_names) == 0:
            self.status = {'reachable':False,'text':'Ready to plan','ready':True}
            self.emit('status', self.status)
        else:
            self.status = {'reachable':True,'text':'Rendering trajectory'}
            self.emit('status', self.status)
            self.publish_trajectory(trajectory)

    def publish_goal_position(self, trajectory):
        self.publish_position(self, trajectory, -1)

    def publish_position(self, trajectory, step):
        jsmsg = JointState()
        jsmsg.name = trajectory.joint_trajectory.joint_names
        jsmsg.position = trajectory.joint_trajectory.points[step].positions
        self.jspub.publish(jsmsg)

    def publish_trajectory(self, trajectory):
        cur_time = 0.0
        acceleration = 4.0
        for i in range(len(trajectory.joint_trajectory.points)):
            point = trajectory.joint_trajectory.points[i]
            gevent.sleep((point.time_from_start - cur_time)/acceleration)
            cur_time = point.time_from_start
            # self.publish_position(trajectory, i)

            # TODO: Only say "True" to update state on the last step of the trajectory
            new_poses = self._move_group.update_robot_state(trajectory.joint_trajectory.joint_names,
                    trajectory.joint_trajectory.points[i].positions, True)
            self.link_poses = new_poses
            self.emit('link_poses', new_poses)

        self.status = {'text':'Ready to plan','ready':True}
        self.emit('status', self.status)
Esempio n. 37
0
    def __init__(self):

        roscpp_initialize(sys.argv)
        rospy.init_node('moveit_py_demo', anonymous=True)
      
        scene = PlanningSceneInterface()
        robot = RobotCommander()
       
        right_arm = MoveGroupCommander("right_arm")
        right_gripper = MoveGroupCommander("right_gripper")
       
        eef = right_arm.get_end_effector_link()
       
        rospy.sleep(2)
       
        scene.remove_attached_object("right_gripper_link", "part")

   
        # clean the scene
        scene.remove_world_object("table")
        scene.remove_world_object("part")
   
        right_arm.set_named_target("start1")
        right_arm.go()
      
        right_gripper.set_named_target("open")
        right_gripper.go()
      
        rospy.sleep(1)
   
        # publish a demo scene
        p = PoseStamped()
        p.header.frame_id = robot.get_planning_frame()
   
        # add a table
        #p.pose.position.x = 0.42
        #p.pose.position.y = -0.2
        #p.pose.position.z = 0.3
        #scene.add_box("table", p, (0.5, 1.5, 0.6))

   
        # add an object to be grasped
        p.pose.position.x = 0.15

        p.pose.position.y = -0.12
        p.pose.position.z = 0.7
        scene.add_box("part", p, (0.07, 0.01, 0.2))
      
        rospy.sleep(1)
             
        g = Grasp()
        g.id = "test"
        start_pose = PoseStamped()
        start_pose.header.frame_id = FIXED_FRAME
   
        # start the gripper in a neutral pose part way to the target
        start_pose.pose.position.x = 0.0130178
        start_pose.pose.position.y = -0.125155
        start_pose.pose.position.z = 0.597653
        start_pose.pose.orientation.x = 0.0
        start_pose.pose.orientation.y = 0.388109
        start_pose.pose.orientation.z = 0.0
        start_pose.pose.orientation.w = 0.921613
          
        right_arm.set_pose_target(start_pose)
        right_arm.go()
       
        rospy.sleep(2)

        # generate a list of grasps
        #grasps = self.make_grasps(start_pose)
   
        #result = False
        #n_attempts = 0
       
        # repeat until will succeed
        #while result == False:
            #result = robot.right_arm.pick("part", grasps)      
            #n_attempts += 1
            #print "Attempts: ", n_attempts
            #rospy.sleep(0.2)
          
        rospy.spin()
        roscpp_shutdown()
Esempio n. 38
0
class MoveItDemo:
    def __init__(self):

        global obj_att

        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')
        
        #Initialize robot
        robot = moveit_commander.RobotCommander()

        # Use the planning scene object to add or remove objects
        self.scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
        
        # Create a publisher for displaying object frames
        self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)

        ### Create a publisher for visualizing direction ###
        self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)


        # Create a dictionary to hold object colors
        self.colors = dict()

        # Initialize the MoveIt! commander for the arm
        self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the MoveIt! commander for the gripper
        self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Allow 5 seconds per planning attempt
        self.right_arm.set_planning_time(5)

        # Prepare Action Controller for gripper
        self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
        self.ac.wait_for_server()

        # Give the scene a chance to catch up
        rospy.sleep(2)
        
        # Prepare Gazebo Subscriber
        self.pwh = None
        self.pwh_copy = None
        self.idx_targ = None
        self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)


        ### OPEN THE GRIPPER ###
        self.open_gripper()


        # PREPARE THE SCENE
        while self.pwh is None:
            rospy.sleep(0.05)

        ############## CLEAR THE SCENE ################


#        planning_scene.world.collision_objects.remove('target')

        # Remove leftover objects from a previous run
        self.scene.remove_world_object('target')
        self.scene.remove_world_object('table')
#        self.scene.remove_world_object(obstacle1_id)

        # Remove any attached objects from a previous session
        self.scene.remove_attached_object(GRIPPER_FRAME, 'target')

        # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
        timerThread = threading.Thread(target=self.scene_generator)
        timerThread.daemon = True
        timerThread.start()

        initial_pose = PoseStamped()
        initial_pose.header.frame_id = 'gazebo_world'
        initial_pose.pose = target_pose.pose
        

        print "==================== Generating Transformations ==========================="

        #################### PRE GRASPING POSE #########################
        M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
        M1[0,3] = target_pose.pose.position.x
        M1[1,3] = target_pose.pose.position.y 
        M1[2,3] = target_pose.pose.position.z

        M2 = transformations.euler_matrix(0, 1.57, 0)
        M2[0,3] = 0.0  # offset about x
        M2[1,3] = 0.0   # about y
        M2[2,3] = 0.25  # about z

        T = np.dot(M1,  M2)
        pre_grasping = deepcopy(target_pose)
        pre_grasping.pose.position.x = T[0,3] 
        pre_grasping.pose.position.y = T[1,3]
        pre_grasping.pose.position.z = T[2,3]

        quat = transformations.quaternion_from_matrix(T)
        pre_grasping.pose.orientation.x = quat[0]
        pre_grasping.pose.orientation.y = quat[1]
        pre_grasping.pose.orientation.z = quat[2]
        pre_grasping.pose.orientation.w = quat[3]
        pre_grasping.header.frame_id = 'gazebo_world'
        self.plan_exec(pre_grasping)


        #################### GRASPING POSE #########################

        M3 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
        M3[0,3] = target_pose.pose.position.x
        M3[1,3] = target_pose.pose.position.y 
        M3[2,3] = target_pose.pose.position.z

        M4 = transformations.euler_matrix(0, 1.57, 0)
        M4[0,3] = 0.0  # offset about x
        M4[1,3] = 0.0   # about y
        M4[2,3] = 0.18  # about z

        T2 = np.dot(M3,  M4)
        grasping = deepcopy(target_pose)
        grasping.pose.position.x = T2[0,3] 
        grasping.pose.position.y = T2[1,3]
        grasping.pose.position.z = T2[2,3]

        quat2 = transformations.quaternion_from_matrix(T2)
        grasping.pose.orientation.x = quat2[0]
        grasping.pose.orientation.y = quat2[1]
        grasping.pose.orientation.z = quat2[2]
        grasping.pose.orientation.w = quat2[3]
        grasping.header.frame_id = 'gazebo_world'

        self.plan_exec(grasping)


        #Close the gripper
        print "========== Waiting for gazebo to catch up =========="
        self.close_gripper()



        #################### ATTACH OBJECT ######################

        touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link']
        #print touch_links
        self.scene.attach_box(GRIPPER_FRAME, target_id, target_pose, target_size, touch_links)

        # counter to let the planning scene know when to remove the object
        obj_att = 1

        #self.scene.remove_world_object(target_id)

        #################### POST-GRASP RETREAT #########################

        M5 = transformations.quaternion_matrix([initial_pose.pose.orientation.x, initial_pose.pose.orientation.y, initial_pose.pose.orientation.z, initial_pose.pose.orientation.w])
        M5[0,3] = initial_pose.pose.position.x
        M5[1,3] = initial_pose.pose.position.y 
        M5[2,3] = initial_pose.pose.position.z

        M6 = transformations.euler_matrix(0, 1.57, 0)
        M6[0,3] = 0.0  # offset about x
        M6[1,3] = 0.0  # about y
        M6[2,3] = 0.3  # about z

        T3 = np.dot(M5,  M6)
        post_grasping = deepcopy(initial_pose)
        post_grasping.pose.position.x = T3[0,3] 
        post_grasping.pose.position.y = T3[1,3]
        post_grasping.pose.position.z = T3[2,3]

        quat3 = transformations.quaternion_from_matrix(T3)
        post_grasping.pose.orientation.x = quat3[0]
        post_grasping.pose.orientation.y = quat3[1]
        post_grasping.pose.orientation.z = quat3[2]
        post_grasping.pose.orientation.w = quat3[3]
        post_grasping.header.frame_id = 'gazebo_world'

        self.plan_exec(post_grasping)




        # Specify a pose to place the target after being picked up
        place_pose = PoseStamped()
        place_pose.header.frame_id = REFERENCE_FRAME
        place_pose.pose.position.x = 0.52
        place_pose.pose.position.y = -0.48
        place_pose.pose.position.z = 0.48
        place_pose.pose.orientation.w = 1.0


        n_attempts = 0
        max_place_attempts = 2
        # Generate valid place poses
        places = self.make_places(place_pose)

        success = False
        # Repeat until we succeed or run out of attempts
        while success == False and n_attempts < max_place_attempts:
            for place in places:
                success = self.right_arm.place(target_id, place)
                if success:
                    break
            n_attempts += 1
            rospy.loginfo("Place attempt: " +  str(n_attempts))
            rospy.sleep(0.2)


        self.open_gripper()
        obj_att = None
        rospy.sleep(3)



##        # Initialize the grasp object
##        g = Grasp()
##        grasps = []
##        # Set the first grasp pose to the input pose
##        g.grasp_pose = pre_grasping
##        g.allowed_touch_objects = [target_id]
##        grasps.append(deepcopy(g))

##        right_arm.pick(target_id, grasps)


#        #Change the frame_id for the planning to take place!
#        #target_pose.header.frame_id = 'gazebo_world'


#        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

#        # Exit the script
        moveit_commander.os._exit(0)

##################################################################################################################

    #Get pose from Gazebo
    def model_state_callback(self,msg):

        self.pwh = ModelStates()
        self.pwh = msg

    # Generate a list of possible place poses
    def make_places(self, init_pose):
        # Initialize the place location as a PoseStamped message
        place = PoseStamped()

        # Start with the input place pose
        place = init_pose

        # A list of x shifts (meters) to try
        x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]

        # A list of y shifts (meters) to try
        y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]       

        # A list of pitch angles to try
        #pitch_vals = [0, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02]

        pitch_vals = [0]

        # A list of yaw angles to try
        yaw_vals = [0]

        # A list to hold the places
        places = []

        # Generate a place pose for each angle and translation
        for y in yaw_vals:
            for p in pitch_vals:
                for y in y_vals:
                    for x in x_vals:
                        place.pose.position.x = init_pose.pose.position.x + x
                        place.pose.position.y = init_pose.pose.position.y + y

                        # Create a quaternion from the Euler angles
                        q = quaternion_from_euler(0, p, y)

                        # Set the place pose orientation accordingly
                        place.pose.orientation.x = q[0]
                        place.pose.orientation.y = q[1]
                        place.pose.orientation.z = q[2]
                        place.pose.orientation.w = q[3]

                        # Append this place pose to the list
                        places.append(deepcopy(place))

        # Return the list
        return places


    def plan_exec(self, pose):

        self.right_arm.clear_pose_targets()
        self.right_arm.set_pose_target(pose, GRIPPER_FRAME)
        self.right_arm.plan()
        rospy.sleep(5)
        self.right_arm.go(wait=True)


    def close_gripper(self):

        g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
        self.ac.send_goal(g_close)
        self.ac.wait_for_result()
        rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object


    def open_gripper(self):

        g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
        self.ac.send_goal(g_open)
        self.ac.wait_for_result()
        rospy.sleep(5) # And up to 20 to detach it


    def scene_generator(self):
#        print obj_att
        global target_pose
        global target_id
        global target_size
        target_id = 'target'
        self.taid = self.pwh.name.index('wood_cube_5cm')
        table_id = 'table'
        self.tid = self.pwh.name.index('table') 
        #obstacle1_id = 'obstacle1'
        #self.o1id = self.pwh.name.index('wood_block_10_2_1cm')


        # Set the target size [l, w, h]
        target_size = [0.05, 0.05, 0.05]
        table_size = [1.5, 0.8, 0.03]
        #obstacle1_size = [0.1, 0.025, 0.01]

        ## Set the target pose on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose = self.pwh.pose[self.taid]
        target_pose.pose.position.z += 0.025
        # Add the target object to the scene
        if obj_att is None:
            self.scene.add_box(target_id, target_pose, target_size)

            table_pose = PoseStamped()
            table_pose.header.frame_id = REFERENCE_FRAME
            table_pose.pose = self.pwh.pose[self.tid]
            table_pose.pose.position.z += 1
            self.scene.add_box(table_id, table_pose, table_size)
            
            #obstacle1_pose = PoseStamped()
            #obstacle1_pose.header.frame_id = REFERENCE_FRAME
            #obstacle1_pose.pose = self.pwh.pose[self.o1id]
            ## Add the target object to the scene
            #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)

            # Specify a pose to place the target after being picked up
            place_pose = PoseStamped()
            place_pose.header.frame_id = REFERENCE_FRAME
            place_pose.pose.position.x = 0.50
            place_pose.pose.position.y = -0.30
            place_pose.pose.orientation.w = 1.0

            # Add the target object to the scene
            self.scene.add_box(target_id, target_pose, target_size)
                    
            ### Make the target purple ###
            self.setColor(target_id, 0.6, 0, 1, 1.0)

            # Send the colors to the planning scene
            self.sendColors()
        else: 
            self.scene.remove_world_object('target')
        # Publish targe's frame
        #self.object_frames_pub.publish(target_pose)


        threading.Timer(0.5, self.scene_generator).start()

    # Set the color of an object
    def setColor(self, name, r, g, b, a = 0.9):
        # Initialize a MoveIt color object
        color = ObjectColor()

        # Set the id to the name given as an argument
        color.id = name

        # Set the rgb and alpha values given as input
        color.color.r = r
        color.color.g = g
        color.color.b = b
        color.color.a = a

        # Update the global color dictionary
        self.colors[name] = color

    # Actually send the colors to MoveIt!
    def sendColors(self):
        # Initialize a planning scene object
        p = PlanningScene()

        # Need to publish a planning scene diff        
        p.is_diff = True

        # Append the colors from the global color dictionary 
        for color in self.colors.values():
            p.object_colors.append(color)

        # Publish the scene diff
        self.scene_pub.publish(p)
Esempio n. 39
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_constraints_demo', anonymous=True)

        robot = RobotCommander()

        # Connect to the arm move group
        arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the move group for the right gripper
        gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Increase the planning time since constraint planning can take a while
        arm.set_planning_time(5)

        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)

        # Set the right arm reference frame
        arm.set_pose_reference_frame(REFERENCE_FRAME)

        # Allow some leeway in position(meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.05)
        arm.set_goal_orientation_tolerance(0.1)

        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()

        # Start in the "resting" configuration stored in the SRDF file
        arm.set_named_target('l_arm_init')

        # Plan and execute a trajectory to the goal configuration
        arm.go()
        rospy.sleep(1)

        # Open the gripper
        gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        gripper.go()
        rospy.sleep(1)

        # Set an initial target pose with the arm up and to the right
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.263803774718
        target_pose.pose.position.y = 0.295405791959
        target_pose.pose.position.z = 0.690438884208
        q = quaternion_from_euler(0, 0, -1.57079633)
        target_pose.pose.orientation.x = q[0]
        target_pose.pose.orientation.y = q[1]
        target_pose.pose.orientation.z = q[2]
        target_pose.pose.orientation.w = q[3]

        # Set the start state and target pose, then plan and execute
        arm.set_start_state(robot.get_current_state())
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()
        rospy.sleep(2)

        # Close the gripper
        gripper.set_joint_value_target(GRIPPER_CLOSED)
        gripper.go()
        rospy.sleep(1)

        # Store the current pose
        start_pose = arm.get_current_pose(end_effector_link)

        # Create a contraints list and give it a name
        constraints = Constraints()
        constraints.name = "Keep gripper horizontal"

        # Create an orientation constraint for the right gripper
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header = start_pose.header
        orientation_constraint.link_name = arm.get_end_effector_link()
        orientation_constraint.orientation.w = 1.0
        orientation_constraint.absolute_x_axis_tolerance = 0.1
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 0.1
        orientation_constraint.weight = 1.0
        # q = quaternion_from_euler(0, 0, -1.57079633)
        # orientation_constraint.orientation.x = q[0]
        # orientation_constraint.orientation.y = q[1]
        # orientation_constraint.orientation.z = q[2]
        # orientation_constraint.orientation.w = q[3]

        # Append the constraint to the list of contraints
        constraints.orientation_constraints.append(orientation_constraint)

        # Set the path constraints on the arm
        arm.set_path_constraints(constraints)

        # Set a target pose for the arm
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.39000848183
        target_pose.pose.position.y = 0.185900663329
        target_pose.pose.position.z = 0.732752341378
        target_pose.pose.orientation.w = 1

        # Set the start state and target pose, then plan and execute
        arm.set_start_state_to_current_state()
        arm.set_pose_target(target_pose, end_effector_link)
        arm.go()
        rospy.sleep(1)

        # Clear all path constraints
        arm.clear_path_constraints()

        # Open the gripper
        gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        gripper.go()
        rospy.sleep(1)

        # Return to the "resting" configuration stored in the SRDF file
        arm.set_named_target('l_arm_init')

        # Plan and execute a trajectory to the goal configuration
        arm.go()
        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit MoveIt
        moveit_commander.os._exit(0)
Esempio n. 40
0
def move_arm():
    #Wait for the IK service to become available
    rospy.wait_for_service('compute_ik')
    rospy.init_node('service_query')
    
    #Create the function used to call the service
    compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
    global step

    grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
    
    while not rospy.is_shutdown():
        print("I am on step " + str(step))
        raw_input('Press [ Enter ]: ')
        
        #Construct the request
        request = GetPositionIKRequest()
        request.ik_request.group_name = "left_arm"
        request.ik_request.ik_link_name = "left_hand"
        request.ik_request.attempts = 20
        request.ik_request.pose_stamped.header.frame_id = "base"

        cmd = positions[step]
        
        #Set the desired orientation for the end effector HERE
        request.ik_request.pose_stamped.pose.position.x = cmd[0]
        request.ik_request.pose_stamped.pose.position.y = cmd[1]
        request.ik_request.pose_stamped.pose.position.z = cmd[2]     
        request.ik_request.pose_stamped.pose.orientation.x = cmd[3]
        request.ik_request.pose_stamped.pose.orientation.y = cmd[4]
        request.ik_request.pose_stamped.pose.orientation.z = cmd[5]
        request.ik_request.pose_stamped.pose.orientation.w = cmd[6]
        
        try:
            #Send the request to the service
            response = compute_ik(request)
            
            #Print the response HERE
            print(response)
            group = MoveGroupCommander("left_arm")


            #this command tries to achieve a pose: which is position, orientation
            group.set_pose_target(request.ik_request.pose_stamped)

            #then, this command tries to achieve a position only.  orientation is airbitrary.
            group.go()


            if (step == 1) or (step == 6):
                print("closing")
                grip_left.close()
            elif(step == 3) or (step == 8):
                print("OPENING")
                grip_left.open()
            
        




        except rospy.ServiceException, e:
            print "Service call failed: %s"%e
        step = (step+1) % len(positions)
Esempio n. 41
0
    start_pose = geometry_msgs.msg.Pose()
    start_pose.position.x = -0.0988490064784
    start_pose.position.y = 0.272349904278
    start_pose.position.z = 1.18864499931
    start_pose.orientation.x = 0.393751611087
    start_pose.orientation.y = 0.918424640162
    start_pose.orientation.z = -0.0150455838492
    start_pose.orientation.w = 0.0350639347048
#     start_pose.orientation.w = 0
#     start_pose.orientation.x = 0
#     start_pose.orientation.y = 1
#     start_pose.orientation.z = 0
#     start_pose.position.y = 0.0256415233819
#     start_pose.position.z = 1.25871460368
#     start_pose.position.x = 0.243500142238
    right_arm.set_pose_target(start_pose)
    plan_start = right_arm.plan()
    print "============ Waiting while RVIZ displays plan_start..."
    rospy.sleep(5)
    right_arm.execute(plan_start)
    print "============ Waiting while RVIZ executes plan_start..."
    rospy.sleep(5)
     
    waypoints = []
    waypoints.append(right_arm.get_current_pose().pose)
 
    gain = 0.2
    points = 5
    for i in xrange(points):
        wpose = geometry_msgs.msg.Pose()
        wpose.orientation.w = waypoints[i-1].orientation.w
Esempio n. 42
0
    ####################### INIT POSITION RIGHT ARM ###############################
    pose.pose.position.x = 0.5410
    pose.pose.position.y = -0.6719
    pose.pose.position.z = 1.3042
    pose.pose.orientation.w = 0.68025
    pose.pose.orientation.x = -0.2169
    pose.pose.orientation.y = 0.65613
    pose.pose.orientation.z = 0.24435
    pose.header.frame_id = robot.get_planning_frame()
    pose.header.stamp = rospy.Time.now()

    
     
     
   
    group_right.set_pose_target(pose, "r_eef")
     
 
    result = group_right.go(None,1)
    
    if result == 0:
        print "ERROR init pos right"
        sys.exit()
        
    ####################### INIT POSITION LEFT ARM ###############################    
    pose.pose.position.x = 0.5636
    pose.pose.position.y = 0.6398
    pose.pose.position.z = 1.1455
    pose.pose.orientation.w = 0.15506
    pose.pose.orientation.x = 0.8216
    pose.pose.orientation.y = -0.32951
Esempio n. 43
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_obstacles_demo')

        # Construct the initial scene object
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Pause for the scene to get ready
        rospy.sleep(1)

        # Initialize the move group for the right arm
        arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)

        # Allow replanning to increase the odds of a solution
        arm.allow_replanning(True)

        # Set the right arm reference frame accordingly
        arm.set_pose_reference_frame(REFERENCE_FRAME)

        # Allow 5 seconds per planning attempt
        arm.set_planning_time(5)

        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'

        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)

        # Give the scene a chance to catch up
        rospy.sleep(1)

        # Start the arm in the "resting" pose stored in the SRDF file
        arm.set_named_target('l_arm_init')
        arm.go()

        rospy.sleep(2)

        # Set the height of the table off the ground
        table_ground = 0.65

        # Set the length, width and height of the table and boxes
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]

        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose.position.x = 0.35
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)

        box1_pose = PoseStamped()
        box1_pose.header.frame_id = REFERENCE_FRAME
        box1_pose.pose.position.x = 0.3
        box1_pose.pose.position.y = 0
        box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0
        scene.add_box(box1_id, box1_pose, box1_size)

        box2_pose = PoseStamped()
        box2_pose.header.frame_id = REFERENCE_FRAME
        box2_pose.pose.position.x = 0.3
        box2_pose.pose.position.y = 0.25
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0
        scene.add_box(box2_id, box2_pose, box2_size)

        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)

        # Send the colors to the planning scene
        self.sendColors()

        # Set the target pose in between the boxes and above the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.22
        target_pose.pose.position.y = 0.14
        target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05

        q = quaternion_from_euler(0, 0, -1.57079633)
        target_pose.pose.orientation.x = q[0]
        target_pose.pose.orientation.y = q[1]
        target_pose.pose.orientation.z = q[2]
        target_pose.pose.orientation.w = q[3]

        # Set the target pose for the arm
        arm.set_pose_target(target_pose, end_effector_link)

        # Move the arm to the target pose (if possible)
        arm.go()

        # Pause for a moment...
        rospy.sleep(2)

        # Return the arm to the "resting" pose stored in the SRDF file
        arm.set_named_target('l_arm_init')
        arm.go()

        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
Esempio n. 44
0
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
            rospy.logwarn(e)

        if pose_st_target:
            pub.publish(pose_st_target)
            rospy.loginfo(trans)

        rate.sleep()

    if raw_input() == 'q':
        sys.exit(1)

    # move to a way point
    pose_st_target.pose.position.z += 0.1
    rospy.loginfo("set target to {}".format(pose_st_target.pose))
    group.set_pose_target(pose_st_target.pose)
    plan = group.plan()
    rospy.loginfo("plan is {}".format(plan))
    ret = group.go()
    rospy.loginfo("executed ... {}".format(ret))

    pose_st_target.pose.position.z -= 0.1
    rospy.loginfo("set target to {}".format(pose_st_target.pose))
    group.set_pose_target(pose_st_target.pose)
    plan = group.plan()
    rospy.loginfo("plan is {}".format(plan))
    ret = group.go()
    rospy.loginfo("executed ... {}".format(ret))
    gripper_close()

    pose_st_target.pose.position.y -= 0.2
Esempio n. 45
0
                # 各コントローラ毎に,ゲインを指示するための service proxy (ROS の service を送るためのクライアント) を作成
                set_compliance_slope = rospy.ServiceProxy(controller + '/set_compliance_slope', SetComplianceSlope)
                temp = set_compliance_slope(sl)
        except rospy.ServiceException, e:
            print "Service call failed: %s"%e

if __name__ == '__main__':

    group = MoveGroupCommander("whole_arm")
    rospy.init_node("temp_arm_demo")
      	 
    target_position = [0.05, 0.0, 0.24]  # x, y, z それぞれの位置
    target_orientation = Quaternion(*quaternion_from_euler(0.0, 1.571, 0.0, 'sxyz'))
    # 位置と傾きを用いて姿勢を生成
    target_pose = Pose(Point(*target_position), target_orientation)
    group.set_pose_target(target_pose)
    # 姿勢への移動指示を実行
    group.go()
    
    time.sleep(1)

    # 新たな姿勢を指定
    target_position = [0.12, 0.12, 0.02]
    target_pose = Pose(Point(*target_position), target_orientation)
    group.set_pose_target(target_pose)
    group.go()

    time.sleep(1)

    target_position = [0.05, 0.0, 0.24]
    target_pose = Pose(Point(*target_position), target_orientation)
Esempio n. 46
0
    def __init__(self):

        roscpp_initialize(sys.argv)
        rospy.init_node('moveit_py_demo', anonymous=True)
      
        scene = PlanningSceneInterface()
        robot = RobotCommander()
       
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        #right_arm.set_planner_id("KPIECEkConfigDefault");
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
       
        eef = right_arm.get_end_effector_link()
       
        rospy.sleep(2)
       
        scene.remove_attached_object(GRIPPER_FRAME, "part")

   
        # clean the scene
        scene.remove_world_object("table")
        scene.remove_world_object("part")
   
        #right_arm.set_named_target("r_start")
        #right_arm.go()
      
        #right_gripper.set_named_target("right_gripper_open")
        #right_gripper.go()
      
        rospy.sleep(1)
   
        # publish a demo scene
        p = PoseStamped()
        p.header.frame_id = robot.get_planning_frame()
   
        # add a table
        #p.pose.position.x = 0.42
        #p.pose.position.y = -0.2
        #p.pose.position.z = 0.3
        #scene.add_box("table", p, (0.5, 1.5, 0.6))

   
        # add an object to be grasped
        p.pose.position.x = 0.7
        p.pose.position.y = -0.2
        p.pose.position.z = 0.85
        scene.add_box("part", p, (0.07, 0.01, 0.2))
      
        rospy.sleep(1)
             
        g = Grasp()
        g.id = "test"
        start_pose = PoseStamped()
        start_pose.header.frame_id = FIXED_FRAME
   
        # start the gripper in a neutral pose part way to the target
        start_pose.pose.position.x = 0.47636
        start_pose.pose.position.y = -0.21886
        start_pose.pose.position.z = 0.9
        start_pose.pose.orientation.x = 0.00080331
        start_pose.pose.orientation.y = 0.001589
        start_pose.pose.orientation.z = -2.4165e-06
        start_pose.pose.orientation.w = 1
          
        right_arm.set_pose_target(start_pose)
        right_arm.go()
       
        rospy.sleep(2)

        # generate a list of grasps
        grasps = self.make_grasps(start_pose)
   
        result = False
        n_attempts = 0
       
        # repeat until will succeed
        while result == False:
            result = robot.right_arm.pick("part", grasps)      
            n_attempts += 1
            print "Attempts: ", n_attempts
            rospy.sleep(0.3)
          
        rospy.spin()
        roscpp_shutdown()
Esempio n. 47
0
mg = MoveGroupCommander('right_arm_and_torso')

p = mg.get_current_pose()
print "Start pose:"
print p

p.pose.position.x += 0.3

#ps = PlanningSceneInterface()
#psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)
#rospy.sleep(1)

#co = ps.make_sphere("test_co", p, 0.02)
#psw = PlanningSceneWorld()
#psw.collision_objects.append(co)

#psw_pub.publish(psw)

# ps.remove_world_object("test_sphere")

# ps.add_sphere("test_sphere", p, 0.1)
# rospy.sleep(1)
# ps.add_sphere("test_sphere", p, 0.1)

#p.pose.position.x += 0.3
print "End pose:"
print p
mg.set_pose_target(mg.get_random_pose())
traj = mg.plan()
print traj
Esempio n. 48
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')
        
        #Initialize robot
        robot = moveit_commander.RobotCommander()

        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10)
        
        # Create a publisher for displaying object frames
        self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)

        ## Create a publisher for visualizing direction ###
        self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Initialize the MoveIt! commander for the arm
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the MoveIt! commander for the gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)

        # Prepare Gripper and open it
        self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
        self.ac.wait_for_server()

        g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
        g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1))
        self.ac.send_goal(g_open)

        # Give the scene a chance to catch up
        rospy.sleep(2)
        
        # Prepare Gazebo Subscriber
        self.pwh = None
        self.pwh_copy = None
        self.idx_targ = None
        self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)


        rospy.sleep(2)
        
        # PREPARE THE SCENE
       	
       	while self.pwh is None:
            rospy.sleep(0.05)

        target_id = 'target'
        self.taid = self.pwh.name.index('wood_cube_5cm')
        table_id = 'table'
        self.tid = self.pwh.name.index('table') 
        #obstacle1_id = 'obstacle1'
        #self.o1id = self.pwh.name.index('wood_block_10_2_1cm')

        # Remove leftover objects from a previous run
        scene.remove_world_object(target_id)
        scene.remove_world_object(table_id)
        #scene.remove_world_object(obstacle1_id)

        # Remove any attached objects from a previous session
        scene.remove_attached_object(GRIPPER_FRAME, target_id)

        # Set the target size [l, w, h]
        target_size = [0.05, 0.05, 0.05]
        table_size = [1.5, 0.8, 0.03]
        #obstacle1_size = [0.1, 0.025, 0.01]

        ## Set the target pose on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose = self.pwh.pose[self.taid]
        target_pose.pose.position.z += 0.025
        # Add the target object to the scene
        scene.add_box(target_id, target_pose, target_size)

        table_pose = PoseStamped()
        table_pose.header.frame_id = REFERENCE_FRAME
        table_pose.pose = self.pwh.pose[self.tid]
        table_pose.pose.position.z += 1
        scene.add_box(table_id, table_pose, table_size)
        
        #obstacle1_pose = PoseStamped()
        #obstacle1_pose.header.frame_id = REFERENCE_FRAME
        #obstacle1_pose.pose = self.pwh.pose[self.o1id]
        ## Add the target object to the scene
        #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)

        # Specify a pose to place the target after being picked up
        place_pose = PoseStamped()
        place_pose.header.frame_id = REFERENCE_FRAME
        place_pose.pose.position.x = 0.50
        place_pose.pose.position.y = -0.30
        place_pose.pose.orientation.w = 1.0

        # Add the target object to the scene
        scene.add_box(target_id, target_pose, target_size)
                
        ### Make the target purple ###
        self.setColor(target_id, 0.6, 0, 1, 1.0)

        # Send the colors to the planning scene
        self.sendColors()
        
        #print target_pose
        self.object_frames_pub.publish(target_pose)
        rospy.sleep(2)

        print "==================== Generating Transformations ==========================="

        M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
        M1[0,3] = target_pose.pose.position.x
        M1[1,3] = target_pose.pose.position.y 
        M1[2,3] = target_pose.pose.position.z

        M2 = transformations.euler_matrix(0, 1.57, 0)
        M2[0,3] = 0.0  # offset about x
        M2[1,3] = 0.0   # about y
        M2[2,3] = 0.25  # about z

        T = np.dot(M1,  M2)
        pre_grasping = deepcopy(target_pose)
        pre_grasping.pose.position.x = T[0,3] 
        pre_grasping.pose.position.y = T[1,3]
        pre_grasping.pose.position.z = T[2,3]

        quat = transformations.quaternion_from_matrix(T)
        pre_grasping.pose.orientation.x = quat[0]
        pre_grasping.pose.orientation.y = quat[1]
        pre_grasping.pose.orientation.z = quat[2]
        pre_grasping.pose.orientation.w = quat[3]
        pre_grasping.header.frame_id = 'gazebo_world'


#        # Initialize the grasp object
#        g = Grasp()
#        grasps = []
#        # Set the first grasp pose to the input pose
#        g.grasp_pose = pre_grasping
#        g.allowed_touch_objects = [target_id]
#        grasps.append(deepcopy(g))

#        right_arm.pick(target_id, grasps)


        #Change the frame_id for the planning to take place!
        #target_pose.header.frame_id = 'gazebo_world'
        
        self.p_pub.publish(pre_grasping)
        right_arm.set_pose_target(pre_grasping, GRIPPER_FRAME)
        plan = right_arm.plan()
        rospy.sleep(2)
        right_arm.go(wait=True)



        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
class RazerControl():

    def __init__(self):
        self.pub_right_hand_pose = rospy.Publisher(RIGHT_HAND_POSESTAMPED_TOPIC, PoseStamped, latch=True)
        self.pub_right_hand_pose_reference = rospy.Publisher(RIGHT_HAND_REFERENCE_POSESTAMPED_TOPIC, PoseStamped, latch=True)
        self.pub_left_hand_pose = rospy.Publisher(LEFT_HAND_POSESTAMPED_TOPIC, PoseStamped, latch=True)
        self.pub_left_hand_pose_reference = rospy.Publisher(LEFT_HAND_REFERENCE_POSESTAMPED_TOPIC, PoseStamped, latch=True)
        self.hydra_data_subs = rospy.Subscriber(HYDRA_DATA_TOPIC, Hydra, self.hydraDataCallback)
        self.pub_move_base = rospy.Publisher(MOVE_BASE_TOPIC, Twist)
        self.subs = rospy.Subscriber('/joint_states', JointState, self.getJointStates)
        self.current_joint_states = None
        rospy.loginfo("Getting first joint_states")
        while self.current_joint_states == None:
            rospy.sleep(0.1)
        rospy.loginfo("Gotten!")
        rospy.loginfo("Connecting with right hand AS")
        self.right_hand_as = actionlib.SimpleActionClient(HAND_RIGHT_AS, FollowJointTrajectoryAction)
        self.right_hand_as.wait_for_server()
        rospy.loginfo("Connecting with left hand AS")
        self.left_hand_as = actionlib.SimpleActionClient(HAND_LEFT_AS, FollowJointTrajectoryAction)
        self.left_hand_as.wait_for_server()
        rospy.loginfo("Starting up move group commander for right, left, torso and head... (slow)")
        self.right_arm_mgc = MoveGroupCommander("right_arm")
        self.right_arm_mgc.set_pose_reference_frame('base_link')
        self.left_arm_mgc = MoveGroupCommander("left_arm")
        self.left_arm_mgc.set_pose_reference_frame('base_link')
        self.torso_mgc = MoveGroupCommander("right_arm_torso")
        self.torso_mgc.set_pose_reference_frame('base_link')
        self.head_mgc = MoveGroupCommander("head")
        self.head_mgc.set_pose_reference_frame('base_link')
        self.last_hydra_message = None
        self.tmp_pose_right = PoseStamped()
        self.tmp_pose_left = PoseStamped()
        self.read_message = False

    def getJointStates(self, data):
        self.current_joint_states = data
        
    def create_hand_goal(self, hand_side="right", hand_pose="closed", values=0.0):
        """Returns the hand goal to send
        possible poses: closed, open, intermediate"""
        hand_goal = FollowJointTrajectoryGoal()
        hand_goal.trajectory.joint_names.append('hand_'+ hand_side +'_thumb_joint')
        hand_goal.trajectory.joint_names.append('hand_'+ hand_side +'_middle_joint')
        hand_goal.trajectory.joint_names.append('hand_'+ hand_side +'_index_joint')
        jtp = JointTrajectoryPoint()
        
        joint_list = ['hand_'+ hand_side +'_thumb_joint',
                      'hand_'+ hand_side +'_middle_joint',
                      'hand_'+ hand_side +'_index_joint']
        ids_list = []
        values_list = []
        rospy.loginfo("current_joint_state is:\n" + str(self.current_joint_states))
        for joint in joint_list:
            idx_in_message = self.current_joint_states.name.index(joint)
            ids_list.append(idx_in_message)
            values_list.append(self.current_joint_states.position[idx_in_message])
        
        if hand_pose == "closed":
            jtp.positions.append(2.0)
            jtp.positions.append(values_list[1]) # TODO: read values and keep them
            jtp.positions.append(values_list[2]) # TODO: read values and keep them
        elif hand_pose == "open":
            jtp.positions.append(0.0)
            jtp.positions.append(values_list[1]) # TODO: read values and keep them
            jtp.positions.append(values_list[2]) # TODO: read values and keep them
        elif hand_pose == "intermediate":
            jtp.positions.append(values_list[0]) # TODO: read values and keep them
            jtp.positions.append(values) 
            jtp.positions.append(values)
        jtp.velocities.append(0.0)
        jtp.velocities.append(0.0)
        jtp.velocities.append(0.0)
        jtp.time_from_start.secs = 2
        hand_goal.trajectory.points.append(jtp)
        return hand_goal

    def hydraDataCallback(self, data):
        #rospy.loginfo("Received data from " + HYDRA_DATA_TOPIC)
        self.last_hydra_message = data
        self.tmp_pose_right = PoseStamped()
        self.tmp_pose_right.header.frame_id = 'base_link'
        self.tmp_pose_right.header.stamp = rospy.Time.now()
        self.tmp_pose_right.pose.position.x = self.last_hydra_message.paddles[1].transform.translation.x
        self.tmp_pose_right.pose.position.y = self.last_hydra_message.paddles[1].transform.translation.y
        self.tmp_pose_right.pose.position.z = self.last_hydra_message.paddles[1].transform.translation.z
        self.tmp_pose_right.pose.position.x += RIGHT_HAND_INITIAL_POINT.x
        self.tmp_pose_right.pose.position.y += RIGHT_HAND_INITIAL_POINT.y
        self.tmp_pose_right.pose.position.z += RIGHT_HAND_INITIAL_POINT.z
        self.tmp_pose_right.pose.orientation = self.last_hydra_message.paddles[1].transform.rotation
        
        self.tmp_pose_left = PoseStamped()
        self.tmp_pose_left.header.frame_id = 'base_link'
        self.tmp_pose_left.header.stamp = rospy.Time.now()
        self.tmp_pose_left.pose.position.x = self.last_hydra_message.paddles[0].transform.translation.x
        self.tmp_pose_left.pose.position.y = self.last_hydra_message.paddles[0].transform.translation.y
        self.tmp_pose_left.pose.position.z = self.last_hydra_message.paddles[0].transform.translation.z
        self.tmp_pose_left.pose.position.x += LEFT_HAND_INITIAL_POINT.x
        self.tmp_pose_left.pose.position.y += LEFT_HAND_INITIAL_POINT.y
        self.tmp_pose_left.pose.position.z += LEFT_HAND_INITIAL_POINT.z
        
        self.tmp_pose_left.pose.orientation = self.last_hydra_message.paddles[0].transform.rotation
        if self.last_hydra_message.paddles[1].buttons[0] == True:
            self.pub_right_hand_pose.publish(self.tmp_pose_right)
        if self.last_hydra_message.paddles[0].buttons[0] == True:
            self.pub_left_hand_pose.publish(self.tmp_pose_left)
            
        self.pub_right_hand_pose_reference.publish(self.tmp_pose_right)
        self.pub_left_hand_pose_reference.publish(self.tmp_pose_left)
        self.read_message = False

    def run(self):
        rospy.loginfo("Press LB / RB to send the current pose")
        
        while self.last_hydra_message == None:
            rospy.sleep(0.1)
            
        rospy.loginfo("Got the first data of the razer... Now we can do stuff")
        
        sleep_rate=0.05 # check at 20Hz
        counter = 0
        while True:
            counter += 1
            rospy.loginfo("Loop #" + str(counter))
            if not self.read_message:
                self.read_message = True
                if self.last_hydra_message.paddles[1].buttons[0] == True:
                    # send curr left paddle pos to move_group right
                    rospy.loginfo("sending curr right hand")
                    self.right_arm_mgc.set_pose_target(self.tmp_pose_right)
                    self.right_arm_mgc.go(wait=False)
                    
                if self.last_hydra_message.paddles[0].buttons[0] == True:
                    # send curr right paddle pos to move_group left
                    rospy.loginfo("sending curr left hand")
                    self.left_arm_mgc.set_pose_target(self.tmp_pose_left)
                    self.left_arm_mgc.go(wait=False)
                    
                if self.last_hydra_message.paddles[1].trigger > 0.0:
                    # send goal right hand close proportional to trigger value (2.0 max?)
                    rospy.loginfo("Closing right hand to value: " + str(self.last_hydra_message.paddles[1].trigger * 2.0))
                    right_hand_goal = self.create_hand_goal(hand_side="right", hand_pose="intermediate", values=self.last_hydra_message.paddles[1].trigger * 2.0)
                    self.right_hand_as.send_goal(right_hand_goal)
                    
                if self.last_hydra_message.paddles[0].trigger > 0.0:
                    # send goal left hand close proportional to trigger value (2.0 max?)
                    rospy.loginfo("Closing left hand to value: " + str(self.last_hydra_message.paddles[0].trigger * 2.0))
                    left_hand_goal = self.create_hand_goal(hand_side="left", hand_pose="intermediate", values=self.last_hydra_message.paddles[0].trigger * 2.0)
                    self.left_hand_as.send_goal(left_hand_goal)
                    
                if self.last_hydra_message.paddles[1].joy[0] != 0.0:
                    # send torso rotation left(neg)/right (pos)
                    rospy.loginfo("Rotation torso")
                    curr_joint_val = self.torso_mgc.get_current_joint_values()
                    self.torso_mgc.set_joint_value_target("torso_1_joint", curr_joint_val[0] + (self.last_hydra_message.paddles[1].joy[0] * 0.1 * -1))
                    self.torso_mgc.go(wait=True)
                    rospy.loginfo("Rotation torso sent!")
                    
                if self.last_hydra_message.paddles[1].joy[1] != 0.0:
                    # send torso inclination front(pos)/back(neg)
                    rospy.loginfo("Inclination torso")
                    curr_joint_val = self.torso_mgc.get_current_joint_values()
                    self.torso_mgc.set_joint_value_target("torso_2_joint", curr_joint_val[1] + (self.last_hydra_message.paddles[1].joy[1] * 0.1))
                    self.torso_mgc.go(wait=True)
                    rospy.loginfo("Inclination torso sent!")
                    
                if self.last_hydra_message.paddles[0].joy[0] != 0.0 or self.last_hydra_message.paddles[0].joy[1] != 0.0:
                    twist_goal = Twist()
                    twist_goal.linear.x = 1.0 * self.last_hydra_message.paddles[0].joy[1]
                    twist_goal.angular.z = 1.0 * self.last_hydra_message.paddles[0].joy[0] * -1.0
                    self.pub_move_base.publish(twist_goal)
                    # move base rotate left (neg)/ right(pos)
                    rospy.loginfo("Move base")
                            
                    
                if self.last_hydra_message.paddles[1].buttons[3] == True:
                    # thumb up
                    rospy.loginfo("Right thumb up")
                    right_thumb_up = self.create_hand_goal(hand_side="right", hand_pose="open")
                    self.right_hand_as.send_goal(right_thumb_up)
                  
                if self.last_hydra_message.paddles[0].buttons[3] == True:
                    # thumb up
                    rospy.loginfo("Left thumb up") 
                    left_thumb_up = self.create_hand_goal(hand_side="left", hand_pose="open")
                    self.left_hand_as.send_goal(left_thumb_up)
                
                if self.last_hydra_message.paddles[1].buttons[1] == True:
                    # thumb down
                    rospy.loginfo("Right thumb down")
                    right_thumb_up = self.create_hand_goal(hand_side="right", hand_pose="closed")
                    self.right_hand_as.send_goal(right_thumb_up)
     
                if self.last_hydra_message.paddles[0].buttons[1] == True:
                    # thumb down
                    rospy.loginfo("Left thumb down")
                    left_thumb_up = self.create_hand_goal(hand_side="left", hand_pose="closed")
                    self.left_hand_as.send_goal(left_thumb_up)            
                
            rospy.sleep(sleep_rate)
Esempio n. 50
0
		marker2.pose.position.x = eef_pose.pose.position.x
		marker2.pose.position.y = eef_pose.pose.position.y
		marker2.pose.position.z = eef_pose.pose.position.z

		# We add the new marker to the MarkerArray, removing the oldest
		# marker from it when necessary
		if(count > MARKERS_MAX):
			markerArray.markers.pop(0)

		markerArray.markers.append(marker)
		markerArray.markers.append(marker2)

		print 'how far away: ', (goal_pose.pose.position.x-eef_pose.pose.position.x), ' ',(goal_pose.pose.position.y-eef_pose.pose.position.y), ' ',(goal_pose.pose.position.z-eef_pose.pose.position.z)

		# Renumber the marker IDs
		id = 0
		for m in markerArray.markers:
			m.id = id
			id += 1

#	while not rospy.is_shutdown():
		# Publish the MarkerArray
		publisher.publish(markerArray)

		robot.set_pose_target(eef_pose)
		robot.go()

	rospy.sleep(1)

	rospy.spin()
Esempio n. 51
0
    msg.trajectory.joint_names = ['ur5_arm_shoulder_pan_joint',
                                  'ur5_arm_shoulder_lift_joint',
                                  'ur5_arm_elbow_joint',
                                  'ur5_arm_wrist_1_joint',
                                  'ur5_arm_wrist_2_joint',
                                  'ur5_arm_wrist_3_joint']
    msg.trajectory.points.append(JointTrajectoryPoint(positions=[-1.57, -0.1745, -2.79, -1.57, 0, 0],
                                                      time_from_start=rospy.Duration(2)))
    client.send_goal(msg)
    client.wait_for_result()

    # open
    open_hand()
    # reach
    rospy.loginfo("reach")
    arm.set_pose_target([0.90, 0.16, 0.255, 0, 0, 0])
    arm.plan() and arm.go()
    arm.plan() and arm.go()
    # approach
    rospy.loginfo("approach")
    arm.set_pose_target([1.13, 0.16, 0.255, 0, 0, 0])
    arm.plan() and arm.go()
    # rotate
    for i in range(4):
        # close
        rospy.loginfo("close")
        close_hand()
        # rotate
        angles = arm.get_current_joint_values()
        import numpy
        start_angle = angles[5]
    roscpp_initialize(sys.argv)
    rospy.init_node('pumpkin_planning', anonymous=True)

    right_arm = MoveGroupCommander("right_arm")
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
    print right_arm.get_current_pose().pose

    wpose = geometry_msgs.msg.Pose()
    wpose.orientation.w = 0.0176318609849
    wpose.orientation.x = 0.392651866792
    wpose.orientation.y = 0.918465607415
    wpose.orientation.z = 0.0439835989663
    wpose.position.y = 0.227115629827
    wpose.position.z = 1.32344046934
    wpose.position.x = -0.358178766726
    right_arm.set_pose_target(wpose)
    plan1 = right_arm.plan()

    right_arm.execute(plan1)
    print "============ Waiting while RVIZ executes plan1..."
    rospy.sleep(5)
    waypoints = []
    waypoints.append(right_arm.get_current_pose().pose)
    print right_arm.get_current_pose().pose

    points = 20
    for i in xrange(points):
        wpose = geometry_msgs.msg.Pose()
        wpose.orientation.w = waypoints[i-1].orientation.w + 0.5285
        wpose.orientation.x = waypoints[i-1].orientation.x - 0.6736
        wpose.orientation.y = waypoints[i-1].orientation.y - 1.638
Esempio n. 53
0
    ## We can get the name of the reference frame for this robot
    print ">>>>> Reference frame: %s" % group.get_planning_frame()
    print ">>>>> Printing robot state"
    print robot.get_current_state()

    print ">>>>> Printing robot pose"
    print group.get_current_pose()

    ## Planning to a Pose goal
    print ">>>>> Generating plan"
    pose_target = geometry_msgs.msg.Pose()
    #pose_target.orientation.w = 1.0
    pose_target.position.x = 0.5  #0.4
    pose_target.position.y = 0.2  #0.2
    pose_target.position.z = 0.2  #0.2
    group.set_pose_target(pose_target)

    plan = group.plan()

    #print "============ Waiting while RVIZ displays plan..."
    rospy.sleep(1)

    print ">>>>> Go for plan"
    group.go(wait=True)

    ## Adding/Removing Objects and Attaching/Detaching Objects

    collision_object = moveit_msgs.msg.CollisionObject()

    moveit_commander.roscpp_shutdown()
Esempio n. 54
0
class Pick_Place:

	def __init__(self, planArm="left_arm"):
		# Initialize the move_group API
		#moveit_commander.roscpp_initialize(sys.argv)

		# Initialize the ROS node
		rospy.init_node('pick_place', anonymous=True)

		# Connect to the arm move group
		if planArm == "right_arm":
			self.arm = MoveGroupCommander('right_arm')
			self.hand = Gripper(1)
		else:
			self.arm = MoveGroupCommander('left_arm')
			self.hand = Gripper(0)

		rospy.sleep(1)
		self.hand.release()
		
		# Allow replanning to increase the odds of a solution
		self.arm.allow_replanning(True)

		# Allow some leeway in position(meters) and orientation (radians)
		#self.arm.set_goal_position_tolerance(0.001)
		#self.arm.set_goal_orientation_tolerance(0.01)

	def pick(self):
		self.graspFromSide()
	
	def graspFromSide(self):

		targetPose=Pose()
		targetPose.position.x = 0.96
		targetPose.position.y = 0.10
		targetPose.position.z = -0.0

		targetPose.orientation.x = 0.044
		targetPose.orientation.y = 0.702
		targetPose.orientation.z = 0.045
		targetPose.orientation.w = 0.708

		# right side grasp
		# Position
		#x = 0.636
		#y = 0.832
		#z = 0.192

		# Orientation
		#x = -0.382
		#y = 0.922
		#z = 0.021
		#w= 0.049
		
		#xyz 0.871 0.428 0.038 
		#orientation 0.119 0.698 -0.083 0.700

		self.moveToTargetPose(targetPose)

		while self.hand.range.state() == 65535.0:
			targetPose.position.x += 0.05
			self.moveToTargetPose(targetPose)
		
		while self.hand.range.state() > 70:
			targetPose.position.x += 0.02
			self.moveToTargetPose(targetPose)
		
		self.hand.grasp()
		rospy.sleep(1)
		
		targetPose.position.x -= 0.05
		targetPose.position.y -= 0.05
		targetPose.position.z += 0.1
		self.moveToTargetPose(targetPose)

		self.hand.release()
		rospy.sleep(1)
		
	def moveToTargetPose(self, targetPose):
		# move to the defined pose
		self.arm.set_start_state_to_current_state()
		self.arm.set_pose_target(targetPose)
		self.arm.go()
		rospy.sleep(0.5)	
Esempio n. 55
0
    # right_arm.set_position_target([.75,-0.3, 1])
    # right_arm.go()
    # rospy.sleep(1)

    grasps = []

    g = Grasp()
    g.id = "test"
    grasp_pose = PoseStamped()
    grasp_pose.header.frame_id = "base_link"
    grasp_pose.pose.position.x = 0.47636
    grasp_pose.pose.position.y = -0.21886
    grasp_pose.pose.position.z = 0.7164
    grasp_pose.pose.orientation.x = 0.00080331
    grasp_pose.pose.orientation.y = 0.001589
    grasp_pose.pose.orientation.z = -2.4165e-06
    grasp_pose.pose.orientation.w = 1

    rospy.logwarn("moving to arm")
    right_arm.set_pose_target(grasp_pose)
    right_arm.go()
    rospy.sleep(3)

    rospy.sleep(2)
    rospy.logwarn("pick part")
    # pick the object
    robot.right_arm.pick("part")

    rospy.spin()
    roscpp_shutdown()
for name in MoveItErrorCodes.__dict__.keys():
    if not name[:1] == '_':
        code = MoveItErrorCodes.__dict__[name]
        moveit_error_dict[code] = name



if __name__=='__main__':
    rospy.init_node("pose_goal_test")
    
    rospy.loginfo("Starting up move group commander for right arm")
    right_arm_mgc = MoveGroupCommander("right_arm_torso")
    rospy.loginfo("Creating first goal")
    goal_point = Point(0.4, -0.2, 1.1)
    goal_ori = Quaternion(0.0,0.0,0.0,1.0)
    goal_pose = Pose(goal_point, goal_ori)
    right_arm_mgc.set_pose_target(goal_pose)
    right_arm_mgc.set_pose_reference_frame('base_link')

    while True:
        rospy.loginfo("go()")
        right_arm_mgc.go()
#         goal_point.x += (random() - 0.5) / 10
#         goal_point.y += (random() - 0.5) / 10
#         goal_point.z += (random() - 0.5) / 10
        goal_point.z += 0.05
        rospy.loginfo("Setting new goal:\n " + str(goal_point))
        goal_pose = Pose(goal_point, goal_ori)
        right_arm_mgc.set_pose_target(goal_pose)

    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        
        robot = RobotCommander()

        # Connect to the right_arm move group
        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        
        # Initialize the move group for the right gripper
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
                                
        # Increase the planning time since contraint planning can take a while
        right_arm.set_planning_time(15)
                        
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the right arm reference frame
        right_arm.set_pose_reference_frame(REFERENCE_FRAME)
                
        # Allow some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.05)
        right_arm.set_goal_orientation_tolerance(0.1)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
        
        # Start in the "resting" configuration stored in the SRDF file
        right_arm.set_named_target('resting')
         
        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        rospy.sleep(1)
        
        # Open the gripper
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
        rospy.sleep(1)
        
        # Set an initial target pose with the arm up and to the right
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.237012590198
        target_pose.pose.position.y = -0.0747191267505
        target_pose.pose.position.z = 0.901578401949
        target_pose.pose.orientation.w = 1.0
         
        # Set the start state and target pose, then plan and execute
        right_arm.set_start_state(robot.get_current_state())
        right_arm.set_pose_target(target_pose, end_effector_link)
        right_arm.go()
        rospy.sleep(2)
        
        # Close the gripper
        right_gripper.set_joint_value_target(GRIPPER_CLOSED)
        right_gripper.go()
        rospy.sleep(1)
        
        # Store the current pose
        start_pose = right_arm.get_current_pose(end_effector_link)
        
        # Create a contraints list and give it a name
        constraints = Constraints()
        constraints.name = "Keep gripper horizontal"
        
        # Create an orientation constraint for the right gripper 
        orientation_constraint = OrientationConstraint()
        orientation_constraint.header = start_pose.header
        orientation_constraint.link_name = right_arm.get_end_effector_link()
        orientation_constraint.orientation.w = 1.0
        orientation_constraint.absolute_x_axis_tolerance = 0.1
        orientation_constraint.absolute_y_axis_tolerance = 0.1
        orientation_constraint.absolute_z_axis_tolerance = 3.14
        orientation_constraint.weight = 1.0
        
        # Append the constraint to the list of contraints
        constraints.orientation_constraints.append(orientation_constraint)
          
        # Set the path constraints on the right_arm
        right_arm.set_path_constraints(constraints)
        
        # Set a target pose for the arm        
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.173187824708
        target_pose.pose.position.y = -0.0159929871606
        target_pose.pose.position.z = 0.692596608605
        target_pose.pose.orientation.w = 1.0

        # Set the start state and target pose, then plan and execute
        right_arm.set_start_state_to_current_state()
        right_arm.set_pose_target(target_pose, end_effector_link)
        right_arm.go()
        rospy.sleep(1)
          
        # Clear all path constraints
        right_arm.clear_path_constraints()
        
        # Open the gripper
        right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        right_gripper.go()
        rospy.sleep(1)
        
        # Return to the "resting" configuration stored in the SRDF file
        right_arm.set_named_target('resting')

        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit MoveIt
        moveit_commander.os._exit(0)
Esempio n. 58
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
        
        # Construct the initial scene object
        scene = PlanningSceneInterface()
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
        
        # Pause for the scene to get ready
        rospy.sleep(1)
                        
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander('right_arm')
        # Initialize the move group for the left arm
        left_arm = MoveGroupCommander('left_arm')

        right_arm.set_planner_id("KPIECEkConfigDefault");
        left_arm.set_planner_id("KPIECEkConfigDefault");
        rospy.sleep(1)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
        
        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.01)
        right_arm.set_goal_orientation_tolerance(0.05)
       
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the reference frame for pose targets
        reference_frame = 'base_footprint'
        
        # Set the right arm reference frame accordingly
        right_arm.set_pose_reference_frame(reference_frame)
        
        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)
        
        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        
        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)
        
        # Give the scene a chance to catch up
        rospy.sleep(1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        #left_arm.set_named_target('left_start')
        #left_arm.go()

        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()
        
        rospy.sleep(2)
        
        # Set the height of the table off the ground
        table_ground = 0.75
        
        # Set the length, width and height of the table and boxes
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]
        
        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = 0.56
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)
        
        box1_pose = PoseStamped()
        box1_pose.header.frame_id = reference_frame
        box1_pose.pose.position.x = 0.51
        box1_pose.pose.position.y = -0.1
        box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0   
        scene.add_box(box1_id, box1_pose, box1_size)
        
        box2_pose = PoseStamped()
        box2_pose.header.frame_id = reference_frame
        box2_pose.pose.position.x = 0.49
        box2_pose.pose.position.y = 0.15
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0   
        scene.add_box(box2_id, box2_pose, box2_size)
        
        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
        
        # Send the colors to the planning scene
        self.sendColors()    
        
        # Set the target pose in between the boxes and above the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.pose.position.x = 0.4
        target_pose.pose.position.y = 0.0
        target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
        target_pose.pose.orientation.w = 1.0
        
        # Set the target pose for the arm
        right_arm.set_pose_target(target_pose, end_effector_link)
        
        # Move the arm to the target pose (if possible)
        right_arm.go()
        
        # Pause for a moment...
        rospy.sleep(2)
        

        # Return the arm to the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()
        
        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit the script        
        moveit_commander.os._exit(0)
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)
        
        cartesian = rospy.get_param('~cartesian', True)
                        
        # Connect to the right_arm move group
        right_arm = MoveGroupCommander('right_arm')
        
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the right arm reference frame
        right_arm.set_pose_reference_frame('base_footprint')
                
        # Allow some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.01)
        right_arm.set_goal_orientation_tolerance(0.1)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
                                        
        # Start in the "straight_forward" configuration stored in the SRDF file
        right_arm.set_named_target('straight_forward')
        
        # Plan and execute a trajectory to the goal configuration
        right_arm.go()
        
        # Get the current pose so we can add it as a waypoint
        start_pose = right_arm.get_current_pose(end_effector_link).pose
                
        # Initialize the waypoints list
        waypoints = []
                
        # Set the first waypoint to be the starting pose
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(start_pose)
            
        wpose = deepcopy(start_pose)
                
        # Set the next waypoint back 0.2 meters and right 0.2 meters
        wpose.position.x -= 0.2
        wpose.position.y -= 0.2

        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            right_arm.set_pose_target(wpose)
            right_arm.go()
            rospy.sleep(1)
         
        # Set the next waypoint to the right 0.15 meters
        wpose.position.x += 0.05
        wpose.position.y += 0.15
        wpose.position.z -= 0.15
          
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(wpose))
        else:
            right_arm.set_pose_target(wpose)
            right_arm.go()
            rospy.sleep(1)
            
        if cartesian:
            # Append the pose to the waypoints list
            waypoints.append(deepcopy(start_pose))
        else:
            right_arm.set_pose_target(start_pose)
            right_arm.go()
            rospy.sleep(1)
            
        if cartesian:
            fraction = 0.0
            maxtries = 100
            attempts = 0
            
            # Set the internal state to the current state
            right_arm.set_start_state_to_current_state()
     
            # Plan the Cartesian path connecting the waypoints
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = right_arm.compute_cartesian_path (
                                        waypoints,   # waypoint poses
                                        0.01,        # eef_step
                                        0.0,         # jump_threshold
                                        True)        # avoid_collisions
                
                # Increment the number of attempts 
                attempts += 1
                
                # Print out a progress message
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
                         
            # If we have a complete plan, execute the trajectory
            if fraction == 1.0:
                rospy.loginfo("Path computed successfully. Moving the arm.")
    
                right_arm.execute(plan)
                            
                rospy.loginfo("Path execution complete.")
            else:
                rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  

        # Move normally back to the 'resting' position
        right_arm.set_named_target('resting')
        right_arm.go()
        rospy.sleep(1)
        
        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit MoveIt
        moveit_commander.os._exit(0)