Пример #1
0
 def grasp_goal_cb(userdata, goal):
     ############################################################
     # Creating grasp goal
     grasp_goal = OverheadGraspGoal()
     grasp_goal.is_grasp = True
     grasp_goal.disable_head = False
     grasp_goal.disable_coll = False
     grasp_goal.grasp_type = OverheadGraspGoal.VISION_GRASP
     grasp_goal.x = GRASP_LOCATION[0]
     grasp_goal.y = arm_mult * GRASP_LOCATION[1]
     grasp_goal.behavior_name = "overhead_grasp"
     grasp_goal.sig_level = 0.999
     ############################################################
     return grasp_goal
Пример #2
0
 def place_goal_cb(userdata, goal):
     print "object Number", userdata.object_number
     ############################################################
     # Creating place place_goal
     place_goal = OverheadGraspGoal()
     place_goal.is_grasp = False
     place_goal.disable_head = False
     place_goal.disable_coll = False
     place_goal.grasp_type = OverheadGraspGoal.MANUAL_GRASP
     place_goal.x = PLACE_LOCATIONS[userdata.object_number - 1][0]
     place_goal.y = arm_mult * PLACE_LOCATIONS[userdata.object_number -
                                               1][1]
     place_goal.roll = PLACE_LOCATIONS[userdata.object_number - 1][2]
     place_goal.behavior_name = "overhead_grasp"
     place_goal.sig_level = 0.999
     ############################################################
     return place_goal
Пример #3
0
    def do_action(self, req):
        rospy.loginfo("[grasp_obj_button] Table clicked, grasping object...")
        # put 3d pt in grasping frame
        req.pixel3d.header.stamp = rospy.Time(0)
        grasp_pt = self.tl.transformPoint("/torso_lift_link", req.pixel3d)

        # move to grasp setup position
        setup_goal = OverheadGraspSetupGoal()
        setup_goal.disable_head = False
        setup_goal.open_gripper = True
        self.grasp_setup_client.send_goal(setup_goal)
        self.grasp_setup_client.wait_for_result()

        ############################################################
        # Creating grasp goal
        grasp_goal = OverheadGraspGoal()
        grasp_goal.is_grasp = True
        grasp_goal.disable_head = False
        grasp_goal.disable_coll = False
        grasp_goal.grasp_type=OverheadGraspGoal.VISION_GRASP
        grasp_goal.x = grasp_pt.point.x
        grasp_goal.y = grasp_pt.point.y
        grasp_goal.behavior_name = "overhead_grasp"
        grasp_goal.sig_level = 0.999
        ############################################################

        # grasp object
        self.grasp_client.send_goal(grasp_goal)
        self.grasp_client.wait_for_result()
        result = self.grasp_client.get_result()
        rospy.loginfo("[grasp_obj_button] Grasp result: %s" % result)
        obj_in_hand = result.grasp_result == "Object grasped"

        # go back to grasp setup
        setup_goal.open_gripper = False
        self.grasp_setup_client.send_goal(setup_goal)
        self.grasp_setup_client.wait_for_result()

        return ButtonActionResponse()