def __init__(self, config, debug=0): print "[INFO] Initialise Robot" self.DEBUG = debug # configuration self.config = config # initialise move groups self.arm = MoveGroupCommander(ARM_GROUP) self.gripper = MoveGroupCommander(GRIPPER_GROUP) # initialise pick and place manager if self.DEBUG is 1: # verbose mode self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, True) else: # non verbose mode self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, False) # tolerance: position (in m), orientation (in rad) self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.1) self.place_manager = None self.pick_manager = None self.initialise_move_actions() self.start_position()
def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") grasp_topic = "fetch_grasp_planner_node/plan" rospy.loginfo("Waiting for %s..." % grasp_topic) self.grasp_planner_client = actionlib.SimpleActionClient( grasp_topic, GraspPlanningAction) wait = self.grasp_planner_client.wait_for_server(rospy.Duration(5)) if (wait): print("successfully connected to grasp server") else: print("failed to connect to grasp server") rospy.Subscriber("fetch_fruit_harvest/apple_pose", Pose, self.apple_pose_callback) self.pub = rospy.Publisher("fetch_fruit_harvest/grasp_msg", String, queue_size=10) self.object = Object() self.grasps = Grasp() self.ready_for_grasp = False self.pick_place_finished = False self.first_time_grasp = True self.scene.addBox("ground", 300, 300, 0.02, 0, 0, 0) self.tuck()
def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") # 初始化需要使用move group控制的机械臂中的arm group self.arm = MoveGroupCommander('arm') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 self.arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' self.arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s self.arm.set_planning_time(5) find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server()
def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server()
def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") ##self.objectname_sub= rospy.Subscriber("/objects",Float32MultiArray,self.callback) ##self.interestedObjectName = "" #self.objects = [] find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server()
def __init__(self): self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"] self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) rospy.loginfo("Waiting for gripper_controller...") self.client.wait_for_server() self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True) find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server()
def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("left_arm", "left_hand", verbose = True) self.move_group = MoveGroupInterface("left_arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server(rospy.Duration(15.0)) self.cubes = [] self.tf_listener = TransformListener() # self.gripper_client = actionlib.SimpleActionClient("/robot/end_effector/left_gripper/gripper_action", GripperCommandAction) # self.gripper_client.wait_for_server() # rospy.loginfo("...connected.") rospy.sleep(2.0)
def setup_scene(): scene = PlanningSceneInterface('base_link') scene.removeCollisionObject('box') scene.addCube('box', 0.25, -0.45, 0.1, 0.125) pick_place = PickPlaceInterface('xarm5', 'gripper', verbose=True) grasp = Grasp() # fill in g # setup object named object_name using PlanningSceneInterface pick_place.pickup('box', [grasp], support_name='supporting_surface') place_loc = PlaceLocation() # fill in l pick_place.place('box'[place_loc], goal_is_eef=True, support_name='supporting_surface')
def __init__(self): self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"] self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) rospy.loginfo("Waiting for gripper_controller...") self.client.wait_for_server() rospy.loginfo("...connected.") self.scene = PlanningSceneInterface("base_link") self.attached_object_publisher = rospy.Publisher('attached_collision_object', AttachedCollisionObject, queue_size = 10) self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True) find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() self.debug_index = -1 self.graspables = []
def __init__(self, sim=False): # Define planning groups self.scene = PlanningSceneInterface("base_link") self.dof = rospy.get_param('~jaco_dof') self.move_group = MoveGroupInterface("upper_body", "base_link") self.rmove_group = MoveGroupInterface("right_arm", "base_link") planner_id = "RRTConnectkConfigDefault" self.move_group.setPlannerId(planner_id) self.rmove_group.setPlannerId(planner_id) self.objects_heights = [0.122, 0.240] self.objects_heights_tolerances = [0.02, 0.03] # Define joints and positions for 6 and 7 DOF if "6dof" == self.dof: self._upper_body_joints = [ "right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", "right_wrist_1_joint", "right_wrist_2_joint", "right_wrist_3_joint", "left_elbow_joint", "left_shoulder_lift_joint", "left_shoulder_pan_joint", "left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", "right_wrist_1_joint", "right_wrist_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_elbow_joint", "left_shoulder_lift_joint", "left_shoulder_pan_joint", "left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint" ] self.tucked = [ -2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571, 0.0371, 0.0, 0.0 ] self.constrained_stow = [ 2.28, 2.00, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56, 0.09, -0.15, 2.06, 0.42, 0.0, 0.0 ] self.rarm_const_stow = [2.28, 2.00, -2.56, -0.09, 0.15, 1.06] #self.rarm_const_stow = [-1.51, -0.22, -2.00, -2.04, 1.42, 1.32] elif "7dof" == self.dof: self._upper_body_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint", "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint" ] self.tucked = [ -1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7, 0.0, -0.5, 1.7, 0.04, 0, 0 ] self.constrained_stow = [ -2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0, 0.0, -1.0, 0.42, 0, 0 ] self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0] else: rospy.logerr("DoF needs to be set 6 or 7, aborting demo") return # Define the MoveIt pickplace pipeline self.pickplace = None self.pickplace = PickPlaceInterface("left_side", "left_gripper", verbose=True) self.pickplace.planner_id = planner_id self.pick_result = None self.place_result = None self._listener = tf.TransformListener() self._lgripper = GripperActionClient('left') find_grasp_planning_topic = "grasp_planner/plan" rospy.loginfo("Waiting for %s..." % find_grasp_planning_topic) self.find_grasp_planning_client = actionlib.SimpleActionClient( find_grasp_planning_topic, GraspPlanningAction) self.find_grasp_planning_client.wait_for_server() rospy.loginfo("...connected") self.scene.clear() # This is a simulation so need to adjust gripper parameters if sim: self._gripper_closed = 0.96 self._gripper_open = 0.00 else: self._gripper_closed = 0.01 self._gripper_open = 0.165
def __init__(self): self._scene = PlanningSceneInterface('base_link') self._pickplace = PickPlaceInterface('xarm5', 'gripper', verbose=True) self._move_group = MoveGroupInterface('xarm5', 'base_link')
def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link")
def __init__(self, sim=False): self.scene = PlanningSceneInterface("base_link") self.dof = rospy.get_param('~jaco_dof') self.move_group = MoveGroupInterface("upper_body", "base_link") self.lmove_group = MoveGroupInterface("left_arm", "base_link") self.rmove_group = MoveGroupInterface("right_arm", "base_link") self.move_group.setPlannerId("RRTConnectkConfigDefault") self.lmove_group.setPlannerId("RRTConnectkConfigDefault") self.rmove_group.setPlannerId("RRTConnectkConfigDefault") if "6dof" == self.dof: self._upper_body_joints = [ "right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", "right_wrist_1_joint", "right_wrist_2_joint", "right_wrist_3_joint", "left_elbow_joint", "left_shoulder_lift_joint", "left_shoulder_pan_joint", "left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", "right_wrist_1_joint", "right_wrist_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_elbow_joint", "left_shoulder_lift_joint", "left_shoulder_pan_joint", "left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint" ] self.tucked = [ -2.8, -1.48, -1.48, 0, 0, 1.571, 2.8, 1.48, 1.48, 0, 0, -1.571, 0.0371, 0.0, 0.0 ] self.constrained_stow = [ 2.28, 2.17, -2.56, -0.09, 0.15, 1.082, -2.28, -2.17, 2.56, 0.09, -0.15, 2.06, 0.42, 0.0, 0.0 ] self.larm_const_stow = [-2.28, -2.17, 2.56, 0.09, -0.15, 2.08] self.rarm_const_stow = [2.28, 2.17, -2.56, -0.09, 0.15, 1.06] self.tableDist = 0.7 elif "7dof" == self.dof: self._upper_body_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint", "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint" ] self.tucked = [ -1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7, 0.0, -0.5, 1.7, 0.04, 0, 0 ] self.constrained_stow = [ -2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0, 0.0, -1.0, 0.42, 0, 0 ] self.larm_const_stow = [2.6, -2.0, 0.0, -2.0, 0.0, 0.0, 1.0] self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0] self.tableDist = 0.8 else: rospy.logerr("DoF needs to be set 6 or 7, aborting demo") return self.pickplace = [None] * 2 self.pickplace[0] = PickPlaceInterface("left_side", "left_gripper", verbose=True) self.pickplace[0].planner_id = "RRTConnectkConfigDefault" self.pickplace[1] = PickPlaceInterface("right_side", "right_gripper", verbose=True) self.pickplace[1].planner_id = "RRTConnectkConfigDefault" self.pick_result = [None] * 2 self._last_gripper_picked = 0 self.place_result = [None] * 2 self._last_gripper_placed = 0 self._objs_to_keep = [] self._listener = tf.TransformListener() self._lgripper = GripperActionClient('left') self._rgripper = GripperActionClient('right') find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient( find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() self.scene.clear() # This is a simulation so need to adjust gripper parameters if sim: self._gripper_closed = 0.96 self._gripper_open = 0.00 else: self._gripper_closed = 0.01 self._gripper_open = 0.165
def __init__(self, sim=False): self.scene = PlanningSceneInterface("base_link") self.dof = rospy.get_param('~jaco_dof') self.robot = moveit_commander.RobotCommander() self._listener = tf.TransformListener() # self.frame = self._listener.lookupTransform('/left_ee_link', '/base_link', rospy.Time(0)) # rospy.loginfo("========== Left_ee to base_link is: ") # rospy.loginfo(self.frame) # curr_pos = self._listener.lookupTransform('/base_link', '/left_ee_link', rospy.Time(0)) # rospy.loginfo(curr_pos) # self.move_group = moveit_commander.MoveGroupCommander("upper_body") # rospy.loginfo("======upper_body_group connected =========") # rospy.loginfo(self.move_group.get_joints()) # self.lmove_group = moveit_commander.MoveGroupCommander("left_arm") # rospy.loginfo("======left_arm_group connected =========") # self.rmove_group = moveit_commander.MoveGroupCommander("right_arm") # rospy.loginfo("======right_arm_group connected =========") self.move_group = MoveGroupInterface("upper_body", "base_link") self.lmove_group = MoveGroupInterface("left_arm", "base_link") self.rmove_group = MoveGroupInterface("right_arm", "base_link") self.move_group.setPlannerId("RRTConnectkConfigDefault") self.lmove_group.setPlannerId("RRTConnectkConfigDefault") self.rmove_group.setPlannerId("RRTConnectkConfigDefault") if "6dof" == self.dof: rospy.logwarn( "======= Please change launch param to 7DoF =========") elif "7dof" == self.dof: self._upper_body_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint", "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_shoulder_pan_joint", "right_shoulder_lift_joint", "right_arm_half_joint", "right_elbow_joint", "right_wrist_spherical_1_joint", "right_wrist_spherical_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_shoulder_pan_joint", "left_shoulder_lift_joint", "left_arm_half_joint", "left_elbow_joint", "left_wrist_spherical_1_joint", "left_wrist_spherical_2_joint", "left_wrist_3_joint" ] self.tucked = [ -1.6, -1.5, 0.4, -2.7, 0.0, 0.5, -1.7, 1.6, 1.5, -0.4, 2.7, 0.0, -0.5, 1.7, 0.04, 0, 0 ] # self.constrained_stow =[-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, 1.0, 2.6, -2.0, 0.0, -2.0, 0.0, 0.0, -1.0, 0.42, 0, 0] # flipping pose self.constrained_stow = [ -0.23, -0.71, -1.02, -1.0, 0.9, 1.89, -2.41, 0.44, 0.71, 1.12, 1.21, -1.02, -1.84, 2.61, 0.20, 0, 0 ] # self.constrained_stow_2 = [-0.34, -0.66, -0.92, -1.18, 0.94, 1.75, -2.49, 0.54, 0.65, 1.0, 1.35, -1.07, -1.75, 2.67, 0.20, 0, 0] self.larm_const_stow = [2.6, -2.0, 0.0, -2.0, 0.0, 0.0, 1.0] self.rarm_const_stow = [-2.6, 2.0, 0.0, 2.0, 0.0, 0.0, -1.0] self.tableDist = 0.8 else: rospy.logerr("DoF needs to be set 6 or 7, aborting demo") return ''' pickplace = [lg_itf, rg_itf] pick_result = [None, None] ''' self.pickplace = [None] * 2 self.pickplace[0] = PickPlaceInterface("left_side", "left_gripper", verbose=True) self.pickplace[0].planner_id = "RRTConnectkConfigDefault" self.pickplace[1] = PickPlaceInterface("right_side", "right_gripper", verbose=True) self.pickplace[1].planner_id = "RRTConnectkConfigDefault" self.pick_result = [None] * 2 self._last_gripper_picked = 0 self.place_result = [None] * 2 self._last_gripper_placed = 0 self._objs_to_keep = [] self._lgripper = GripperActionClient('left') self._rgripper = GripperActionClient('right') find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient( find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() rospy.loginfo( "============= FindGraspableObjectsAction connected! ============") self.scene.clear() # This is a simulation so need to adjust gripper parameters if sim: self._gripper_closed = 0.96 self._gripper_open = 0.00 else: self._gripper_closed = 0.01 self._gripper_open = 0.165
def __init__(self, sim=False): self.scene = PlanningSceneInterface("base_link") self.move_group = MoveGroupInterface("upper_body", "base_link") self.lmove_group = MoveGroupInterface("left_arm", "base_link") self.rmove_group = MoveGroupInterface("right_arm", "base_link") self.move_group.setPlannerId("RRTConnectkConfigDefault") self.lmove_group.setPlannerId("RRTConnectkConfigDefault") self.rmove_group.setPlannerId("RRTConnectkConfigDefault") self._upper_body_joints = [ "right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", "right_wrist_1_joint", "right_wrist_2_joint", "right_wrist_3_joint", "left_elbow_joint", "left_shoulder_lift_joint", "left_shoulder_pan_joint", "left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint", "linear_joint", "pan_joint", "tilt_joint" ] self._right_arm_joints = [ "right_elbow_joint", "right_shoulder_lift_joint", "right_shoulder_pan_joint", "right_wrist_1_joint", "right_wrist_2_joint", "right_wrist_3_joint" ] self._left_arm_joints = [ "left_elbow_joint", "left_shoulder_lift_joint", "left_shoulder_pan_joint", "left_wrist_1_joint", "left_wrist_2_joint", "left_wrist_3_joint" ] self.pickplace = [None] * 2 self.pickplace[0] = PickPlaceInterface("left_side", "left_gripper", verbose=True) self.pickplace[0].planner_id = "RRTConnectkConfigDefault" self.pickplace[1] = PickPlaceInterface("right_side", "right_gripper", verbose=True) self.pickplace[1].planner_id = "RRTConnectkConfigDefault" self.pick_result = [None] * 2 self._last_gripper_picked = 0 self.place_result = [None] * 2 self._last_gripper_placed = 0 self._objs_to_keep = [] self._listener = tf.TransformListener() self._lgripper = GripperActionClient('left') self._rgripper = GripperActionClient('right') find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient( find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() self.scene.clear() # This is a simulation so need to adjust gripper parameters if sim: self._gripper_closed = 0.96 self._gripper_open = 0.0 else: self._gripper_closed = 0.0 self._gripper_open = 0.165