class CollisionSceneExample(object): def __init__(self): self._scene = PlanningSceneInterface() # clear the scene self._scene.remove_world_object() self.robot = RobotCommander() # pause to wait for rviz to load print "============ Waiting while RVIZ displays the scene with obstacles..." # TODO: need to replace this sleep by explicitly waiting for the scene to be updated. rospy.sleep(2) def add_pap1(self): floor_pose = [1.0, -1.0, -0.17, 0, 0, 0, 1] floor_dimensions = [5.0, 4.0, 0.02] wall1_pose = [1.0, 1.0, 0.92, 0, 0, 0, 1] wall1_dimensions = [5.0, 0.02, 2.18] wall2_pose = [3.5, -1.0, 0.92, 0, 0, 0, 1] wall2_dimensions = [0.02, 4.0, 2.18] wall3_pose = [-1.5, -1.0, 1.0, 0, 0, 0, 1] wall3_dimensions = [0.02, 4.0, 2.18] obj1_pose = [0.3, 0.4, -0.08, 0, 0, 0, 1] obj1_dimensions = [0.27, 0.4, 0.12] obj2_pose = [0.3, -0.01, -0.08, 0, 0, 0, 1] obj2_dimensions = [0.27, 0.4, 0.12] obj3_pose = [0.6, 0.4, -0.08, 0, 0, 0, 1] obj3_dimensions = [0.27, 0.4, 0.12] obj4_pose = [0.6, -0.01, -0.08, 0, 0, 0, 1] obj4_dimensions = [0.27, 0.4, 0.12] obj5_pose = [0.45, 0.10, 0.05, 0, 0, 0, 1] obj5_dimensions = [0.27, 0.4, 0.12] con_bottom_pose = [1.9, -0.5, 0.012, 0, 0, 0, 1] con_bottom_dimensions = [0.704, 0.80, 0.02] con_left_pose = [1.9, -0.9, 0.705, 0, 0, 0, 1] con_left_dimensions = [0.744, 0.044, 1.386] con_right_pose = [2.252, -0.5, 0.705, 0, 0, 0, 1] con_right_dimensions = [0.044, 0.8, 1.386] con_back_pose = [1.548, -0.5, 0.705, 0, 0, 0, 1] con_back_dimensions = [0.044, 0.8, 1.386] self.add_box_object("wall1", wall1_dimensions, wall1_pose) self.add_box_object("floor", floor_dimensions, floor_pose) self.add_box_object("wall3", wall3_dimensions, wall3_pose) self.add_box_object("wall2", wall2_dimensions, wall2_pose) self.add_box_object("obj3", obj3_dimensions, obj3_pose) self.add_box_object("obj2", obj2_dimensions, obj2_pose) self.add_box_object("obj1", obj1_dimensions, obj1_pose) self.add_box_object("obj4", obj4_dimensions, obj4_pose) self.add_box_object("obj5", obj5_dimensions, obj5_pose) self.add_box_object("con_bottom", con_bottom_dimensions, con_bottom_pose) self.add_box_object("con_left", con_left_dimensions, con_left_pose) self.add_box_object("con_right", con_right_dimensions, con_right_pose) self.add_box_object("con_back", con_back_dimensions, con_back_pose) print "========== Added test 1 scene!!" def add_pap2(self): floor_pose = [1.0, -1.0, -0.17, 0, 0, 0, 1] floor_dimensions = [5.0, 4.0, 0.02] wall1_pose = [1.0, 1.0, 0.92, 0, 0, 0, 1] wall1_dimensions = [5.0, 0.02, 2.18] wall2_pose = [3.5, -1.0, 0.92, 0, 0, 0, 1] wall2_dimensions = [0.02, 4.0, 2.18] wall3_pose = [-1.5, -1.0, 1.0, 0, 0, 0, 1] wall3_dimensions = [0.02, 4.0, 2.18] obj1_pose = [0.3, 0.4, -0.08, 0, 0, 0, 1] obj1_dimensions = [0.27, 0.4, 0.12] obj2_pose = [0.3, -0.01, -0.08, 0, 0, 0, 1] obj2_dimensions = [0.27, 0.4, 0.12] obj3_pose = [0.6, 0.4, -0.08, 0, 0, 0, 1] obj3_dimensions = [0.27, 0.4, 0.12] obj4_pose = [0.6, -0.01, -0.08, 0, 0, 0, 1] obj4_dimensions = [0.27, 0.4, 0.12] obj5_pose = [0.45, 0.10, 0.05, 0, 0, 0, 1] obj5_dimensions = [0.27, 0.4, 0.12] obs_pose = [1.7, 0.20, 0.35, 0, 0, 0, 1] obs_dimensions = [0.1, 0.1, 0.9] con_bottom_pose = [1.9, -0.5, 0.012, 0, 0, 0, 1] con_bottom_dimensions = [0.704, 0.80, 0.02] con_left_pose = [1.9, -0.9, 0.705, 0, 0, 0, 1] con_left_dimensions = [0.744, 0.044, 1.386] con_right_pose = [2.252, -0.5, 0.705, 0, 0, 0, 1] con_right_dimensions = [0.044, 0.8, 1.386] con_back_pose = [1.548, -0.5, 0.705, 0, 0, 0, 1] con_back_dimensions = [0.044, 0.8, 1.386] self.add_box_object("wall1", wall1_dimensions, wall1_pose) self.add_box_object("floor", floor_dimensions, floor_pose) self.add_box_object("wall3", wall3_dimensions, wall3_pose) self.add_box_object("wall2", wall2_dimensions, wall2_pose) self.add_box_object("obj3", obj3_dimensions, obj3_pose) self.add_box_object("obj2", obj2_dimensions, obj2_pose) self.add_box_object("obj1", obj1_dimensions, obj1_pose) self.add_box_object("obj4", obj4_dimensions, obj4_pose) self.add_box_object("obj5", obj5_dimensions, obj5_pose) self.add_box_object("con_bottom", con_bottom_dimensions, con_bottom_pose) self.add_box_object("con_left", con_left_dimensions, con_left_pose) self.add_box_object("con_right", con_right_dimensions, con_right_pose) self.add_box_object("con_back", con_back_dimensions, con_back_pose) self.add_box_object("obs", obs_dimensions, obs_pose) print "========== Added test 2 scene!!" def add_pap3(self): floor_pose = [1.0, -1.0, -0.17, 0, 0, 0, 1] floor_dimensions = [5.0, 4.0, 0.02] wall1_pose = [1.0, 1.0, 0.92, 0, 0, 0, 1] wall1_dimensions = [5.0, 0.02, 2.18] wall2_pose = [3.5, -1.0, 0.92, 0, 0, 0, 1] wall2_dimensions = [0.02, 4.0, 2.18] wall3_pose = [-1.5, -1.0, 0.92, 0, 0, 0, 1] wall3_dimensions = [0.02, 4.0, 2.18] obj1_pose = [0.3, 0.6, -0.08, 0, 0, 0, 1] obj1_dimensions = [0.27, 0.4, 0.12] obj2_pose = [0.3, 0.21, -0.08, 0, 0, 0, 1] obj2_dimensions = [0.27, 0.4, 0.12] obj3_pose = [0.6, 0.6, -0.08, 0, 0, 0, 1] obj3_dimensions = [0.27, 0.4, 0.12] obj4_pose = [0.6, 0.21, -0.08, 0, 0, 0, 1] obj4_dimensions = [0.27, 0.4, 0.12] obj5_pose = [0.45, 0.40, 0.05, 0, 0, 0, 1] obj5_dimensions = [0.27, 0.4, 0.12] con_bottom_pose = [1.9, -0.5, 0.012, 0, 0, 0, 1] con_bottom_dimensions = [0.704, 0.80, 0.02] con_left_pose = [1.9, -0.9, 0.705, 0, 0, 0, 1] con_left_dimensions = [0.744, 0.044, 1.386] con_right_pose = [2.252, -0.5, 0.705, 0, 0, 0, 1] con_right_dimensions = [0.044, 0.8, 1.386] con_back_pose = [1.548, -0.5, 0.705, 0, 0, 0, 1] con_back_dimensions = [0.044, 0.8, 1.386] con2_bottom_pose = [0.3, -0.5, 0.012, 0, 0, 0, 1] con2_bottom_dimensions = [0.704, 0.80, 0.02] con2_left_pose = [0.3, -0.9, 0.705, 0, 0, 0, 1] con2_left_dimensions = [0.744, 0.044, 1.386] con2_right_pose = [0.652, -0.5, 0.705, 0, 0, 0, 1] con2_right_dimensions = [0.044, 0.8, 1.386] con2_back_pose = [-0.052, -0.5, 0.705, 0, 0, 0, 1] con2_back_dimensions = [0.044, 0.8, 1.386] con3_bottom_pose = [1.1, -0.5, 0.012, 0, 0, 0, 1] con3_bottom_dimensions = [0.704, 0.80, 0.02] con3_left_pose = [1.1, -0.9, 0.705, 0, 0, 0, 1] con3_left_dimensions = [0.744, 0.044, 1.386] con3_right_pose = [1.452, -0.5, 0.705, 0, 0, 0, 1] con3_right_dimensions = [0.044, 0.8, 1.386] con3_back_pose = [0.748, -0.5, 0.705, 0, 0, 0, 1] con3_back_dimensions = [0.044, 0.8, 1.386] self.add_box_object("wall1", wall1_dimensions, wall1_pose) self.add_box_object("floor", floor_dimensions, floor_pose) self.add_box_object("wall3", wall3_dimensions, wall3_pose) self.add_box_object("wall2", wall2_dimensions, wall2_pose) self.add_box_object("obj3", obj3_dimensions, obj3_pose) self.add_box_object("obj2", obj2_dimensions, obj2_pose) self.add_box_object("obj1", obj1_dimensions, obj1_pose) self.add_box_object("obj4", obj4_dimensions, obj4_pose) self.add_box_object("obj5", obj5_dimensions, obj5_pose) self.add_box_object("con_bottom", con_bottom_dimensions, con_bottom_pose) self.add_box_object("con_left", con_left_dimensions, con_left_pose) self.add_box_object("con_right", con_right_dimensions, con_right_pose) self.add_box_object("con_back", con_back_dimensions, con_back_pose) self.add_box_object("con2_bottom", con2_bottom_dimensions, con2_bottom_pose) self.add_box_object("con2_left", con2_left_dimensions, con2_left_pose) self.add_box_object("con2_right", con2_right_dimensions, con2_right_pose) self.add_box_object("con2_back", con2_back_dimensions, con2_back_pose) self.add_box_object("con3_bottom", con3_bottom_dimensions, con3_bottom_pose) self.add_box_object("con3_left", con3_left_dimensions, con3_left_pose) self.add_box_object("con3_right", con3_right_dimensions, con3_right_pose) self.add_box_object("con3_back", con3_back_dimensions, con3_back_pose) print "========== Added test 3 scene!!" def add_pap4(self): floor_pose = [1.0, -1.0, -0.17, 0, 0, 0, 1] floor_dimensions = [5.0, 4.0, 0.02] wall1_pose = [1.0, 1.0, 0.92, 0, 0, 0, 1] wall1_dimensions = [5.0, 0.02, 2.18] wall2_pose = [3.5, -1.0, 0.92, 0, 0, 0, 1] wall2_dimensions = [0.02, 4.0, 2.18] wall3_pose = [-1.5, -1.0, 0.92, 0, 0, 0, 1] wall3_dimensions = [0.02, 4.0, 2.18] obj1_pose = [0.3, 0.6, -0.08, 0, 0, 0, 1] obj1_dimensions = [0.27, 0.4, 0.12] obj2_pose = [0.3, 0.21, -0.08, 0, 0, 0, 1] obj2_dimensions = [0.27, 0.4, 0.12] obj3_pose = [0.6, 0.6, -0.08, 0, 0, 0, 1] obj3_dimensions = [0.27, 0.4, 0.12] obj4_pose = [0.6, 0.21, -0.08, 0, 0, 0, 1] obj4_dimensions = [0.27, 0.4, 0.12] obj5_pose = [0.45, 0.40, 0.05, 0, 0, 0, 1] obj5_dimensions = [0.27, 0.4, 0.12] obj10_pose = [2.08, -0.65, 0.1, 0, 0, 0, 1] obj10_dimensions = [0.27, 0.4, 0.12] obj11_pose = [2.08, -0.65, 0.23, 0, 0, 0, 1] obj11_dimensions = [0.27, 0.4, 0.12] obj12_pose = [2.08, -0.65, 0.36, 0, 0, 0, 1] obj12_dimensions = [0.27, 0.4, 0.12] con_bottom_pose = [1.9, -0.5, 0.012, 0, 0, 0, 1] con_bottom_dimensions = [0.704, 0.80, 0.02] con_left_pose = [1.9, -0.9, 0.705, 0, 0, 0, 1] con_left_dimensions = [0.744, 0.044, 1.386] con_right_pose = [2.252, -0.5, 0.705, 0, 0, 0, 1] con_right_dimensions = [0.044, 0.8, 1.386] con_back_pose = [1.548, -0.5, 0.705, 0, 0, 0, 1] con_back_dimensions = [0.044, 0.8, 1.386] con2_bottom_pose = [0.3, -0.5, 0.012, 0, 0, 0, 1] con2_bottom_dimensions = [0.704, 0.80, 0.02] con2_left_pose = [0.3, -0.9, 0.705, 0, 0, 0, 1] con2_left_dimensions = [0.744, 0.044, 1.386] con2_right_pose = [0.652, -0.5, 0.705, 0, 0, 0, 1] con2_right_dimensions = [0.044, 0.8, 1.386] con2_back_pose = [-0.052, -0.5, 0.705, 0, 0, 0, 1] con2_back_dimensions = [0.044, 0.8, 1.386] con3_bottom_pose = [1.1, -0.5, 0.012, 0, 0, 0, 1] con3_bottom_dimensions = [0.704, 0.80, 0.02] con3_left_pose = [1.1, -0.9, 0.705, 0, 0, 0, 1] con3_left_dimensions = [0.744, 0.044, 1.386] con3_right_pose = [1.452, -0.5, 0.705, 0, 0, 0, 1] con3_right_dimensions = [0.044, 0.8, 1.386] con3_back_pose = [0.748, -0.5, 0.705, 0, 0, 0, 1] con3_back_dimensions = [0.044, 0.8, 1.386] self.add_box_object("wall1", wall1_dimensions, wall1_pose) self.add_box_object("floor", floor_dimensions, floor_pose) self.add_box_object("wall3", wall3_dimensions, wall3_pose) self.add_box_object("wall2", wall2_dimensions, wall2_pose) self.add_box_object("obj3", obj3_dimensions, obj3_pose) self.add_box_object("obj2", obj2_dimensions, obj2_pose) self.add_box_object("obj1", obj1_dimensions, obj1_pose) self.add_box_object("obj4", obj4_dimensions, obj4_pose) self.add_box_object("obj5", obj5_dimensions, obj5_pose) self.add_box_object("obj10", obj10_dimensions, obj10_pose) self.add_box_object("obj11", obj11_dimensions, obj11_pose) self.add_box_object("obj12", obj12_dimensions, obj12_pose) self.add_box_object("con_bottom", con_bottom_dimensions, con_bottom_pose) self.add_box_object("con_left", con_left_dimensions, con_left_pose) self.add_box_object("con_right", con_right_dimensions, con_right_pose) self.add_box_object("con_back", con_back_dimensions, con_back_pose) self.add_box_object("con2_bottom", con2_bottom_dimensions, con2_bottom_pose) self.add_box_object("con2_left", con2_left_dimensions, con2_left_pose) self.add_box_object("con2_right", con2_right_dimensions, con2_right_pose) self.add_box_object("con2_back", con2_back_dimensions, con2_back_pose) self.add_box_object("con3_bottom", con3_bottom_dimensions, con3_bottom_pose) self.add_box_object("con3_left", con3_left_dimensions, con3_left_pose) self.add_box_object("con3_right", con3_right_dimensions, con3_right_pose) self.add_box_object("con3_back", con3_back_dimensions, con3_back_pose) print "========== Added test 4 scene!!" def add_pap5(self): floor_pose = [1.0, -1.0, -0.17, 0, 0, 0, 1] floor_dimensions = [5.0, 4.0, 0.02] wall1_pose = [1.0, 1.0, 0.92, 0, 0, 0, 1] wall1_dimensions = [5.0, 0.02, 2.18] wall2_pose = [3.5, -1.0, 0.92, 0, 0, 0, 1] wall2_dimensions = [0.02, 4.0, 2.18] wall3_pose = [-1.5, -1.0, 0.92, 0, 0, 0, 1] wall3_dimensions = [0.02, 4.0, 2.18] obj1_pose = [0.3, 0.6, -0.08, 0, 0, 0, 1] obj1_dimensions = [0.27, 0.4, 0.12] obj2_pose = [0.3, 0.21, -0.08, 0, 0, 0, 1] obj2_dimensions = [0.27, 0.4, 0.12] obj3_pose = [0.6, 0.6, -0.08, 0, 0, 0, 1] obj3_dimensions = [0.27, 0.4, 0.12] obj4_pose = [0.6, 0.21, -0.08, 0, 0, 0, 1] obj4_dimensions = [0.27, 0.4, 0.12] obj5_pose = [0.45, 0.40, 0.05, 0, 0, 0, 1] obj5_dimensions = [0.27, 0.4, 0.12] obj10_pose = [2.08, -0.65, 0.1, 0, 0, 0, 1] obj10_dimensions = [0.27, 0.4, 0.12] obj11_pose = [2.08, -0.65, 0.23, 0, 0, 0, 1] obj11_dimensions = [0.27, 0.4, 0.12] obj12_pose = [2.08, -0.65, 0.36, 0, 0, 0, 1] obj12_dimensions = [0.27, 0.4, 0.12] obs_pose = [2.1, -0.1, 0.35, 0, 0, 0, 1] obs_dimensions = [0.1, 0.1, 0.9] con_bottom_pose = [1.9, -0.5, 0.012, 0, 0, 0, 1] con_bottom_dimensions = [0.704, 0.80, 0.02] con_left_pose = [1.9, -0.9, 0.705, 0, 0, 0, 1] con_left_dimensions = [0.744, 0.044, 1.386] con_right_pose = [2.252, -0.5, 0.705, 0, 0, 0, 1] con_right_dimensions = [0.044, 0.8, 1.386] con_back_pose = [1.548, -0.5, 0.705, 0, 0, 0, 1] con_back_dimensions = [0.044, 0.8, 1.386] con2_bottom_pose = [0.3, -0.5, 0.012, 0, 0, 0, 1] con2_bottom_dimensions = [0.704, 0.80, 0.02] con2_left_pose = [0.3, -0.9, 0.705, 0, 0, 0, 1] con2_left_dimensions = [0.744, 0.044, 1.386] con2_right_pose = [0.652, -0.5, 0.705, 0, 0, 0, 1] con2_right_dimensions = [0.044, 0.8, 1.386] con2_back_pose = [-0.052, -0.5, 0.705, 0, 0, 0, 1] con2_back_dimensions = [0.044, 0.8, 1.386] con3_bottom_pose = [1.1, -0.5, 0.012, 0, 0, 0, 1] con3_bottom_dimensions = [0.704, 0.80, 0.02] con3_left_pose = [1.1, -0.9, 0.705, 0, 0, 0, 1] con3_left_dimensions = [0.744, 0.044, 1.386] con3_right_pose = [1.452, -0.5, 0.705, 0, 0, 0, 1] con3_right_dimensions = [0.044, 0.8, 1.386] con3_back_pose = [0.748, -0.5, 0.705, 0, 0, 0, 1] con3_back_dimensions = [0.044, 0.8, 1.386] self.add_box_object("wall1", wall1_dimensions, wall1_pose) self.add_box_object("floor", floor_dimensions, floor_pose) self.add_box_object("wall3", wall3_dimensions, wall3_pose) self.add_box_object("wall2", wall2_dimensions, wall2_pose) self.add_box_object("obj3", obj3_dimensions, obj3_pose) self.add_box_object("obj2", obj2_dimensions, obj2_pose) self.add_box_object("obj1", obj1_dimensions, obj1_pose) self.add_box_object("obj4", obj4_dimensions, obj4_pose) self.add_box_object("obj5", obj5_dimensions, obj5_pose) self.add_box_object("obj10", obj10_dimensions, obj10_pose) self.add_box_object("obj11", obj11_dimensions, obj11_pose) self.add_box_object("obj12", obj12_dimensions, obj12_pose) self.add_box_object("obs", obs_dimensions, obs_pose) self.add_box_object("con_bottom", con_bottom_dimensions, con_bottom_pose) self.add_box_object("con_left", con_left_dimensions, con_left_pose) self.add_box_object("con_right", con_right_dimensions, con_right_pose) self.add_box_object("con_back", con_back_dimensions, con_back_pose) self.add_box_object("con2_bottom", con2_bottom_dimensions, con2_bottom_pose) self.add_box_object("con2_left", con2_left_dimensions, con2_left_pose) self.add_box_object("con2_right", con2_right_dimensions, con2_right_pose) self.add_box_object("con2_back", con2_back_dimensions, con2_back_pose) self.add_box_object("con3_bottom", con3_bottom_dimensions, con3_bottom_pose) self.add_box_object("con3_left", con3_left_dimensions, con3_left_pose) self.add_box_object("con3_right", con3_right_dimensions, con3_right_pose) self.add_box_object("con3_back", con3_back_dimensions, con3_back_pose) print "========== Added test 5 scene!!" def add_box_object(self, name, dimensions, pose): p = geometry_msgs.msg.PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = pose[0] p.pose.position.y = pose[1] p.pose.position.z = pose[2] p.pose.orientation.x = pose[3] p.pose.orientation.y = pose[4] p.pose.orientation.z = pose[5] p.pose.orientation.w = pose[6] self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) if name == "obj5": print True self._scene.attach_box("pressure_box", name)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') scene = PlanningSceneInterface() self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) self.colors = dict() rospy.sleep(1) arm = MoveGroupCommander('arm') end_effector_link = arm.get_end_effector_link() arm.set_goal_position_tolerance(0.005) arm.set_goal_orientation_tolerance(0.025) arm.allow_replanning(True) reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) arm.set_planning_time(5) #scene planning table_id = 'table' #cylinder_id='cylinder' #box1_id='box1' box2_id = 'box2' target_id = 'target_object' #scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(table_id) scene.remove_world_object(target_id) rospy.sleep(2) table_ground = 0.68 table_size = [0.5, 1, 0.01] #box1_size=[0.1,0.05,0.03] box2_size = [0.05, 0.05, 0.1] r_tool_size = [0.03, 0.01, 0.06] l_tool_size = [0.03, 0.01, 0.06] target_size = [0.05, 0.05, 0.1] table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.75 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.7 box1_pose.pose.position.y=-0.2 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.6 box2_pose.pose.position.y = -0.05 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) target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.6 target_pose.pose.position.y = 0.05 target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.x = 0 target_pose.pose.orientation.y = 0 target_pose.pose.orientation.z = 0 target_pose.pose.orientation.w = 1 scene.add_box(target_id, target_pose, target_size) #left gripper l_p = PoseStamped() l_p.header.frame_id = end_effector_link l_p.pose.position.x = 0.00 l_p.pose.position.y = 0.04 l_p.pose.position.z = 0.04 l_p.pose.orientation.w = 1 scene.attach_box(end_effector_link, 'l_tool', l_p, l_tool_size) #right gripper r_p = PoseStamped() r_p.header.frame_id = end_effector_link r_p.pose.position.x = 0.00 r_p.pose.position.y = -0.04 r_p.pose.position.z = 0.04 r_p.pose.orientation.w = 1 scene.attach_box(end_effector_link, 'r_tool', r_p, r_tool_size) #grasp g_p = PoseStamped() g_p.header.frame_id = end_effector_link g_p.pose.position.x = 0.00 g_p.pose.position.y = -0.00 g_p.pose.position.z = 0.025 g_p.pose.orientation.w = 0.707 g_p.pose.orientation.x = 0 g_p.pose.orientation.y = -0.707 g_p.pose.orientation.z = 0 #set color 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) self.setColor('r_tool', 0.8, 0, 0) self.setColor('l_tool', 0.8, 0, 0) self.setColor('target_object', 0, 1, 0) self.sendColors() #motion planning arm.set_named_target("initial_arm1") arm.go() rospy.sleep(2) grasp_pose = target_pose grasp_pose.pose.position.x -= 0.15 #grasp_pose.pose.position.z= grasp_pose.pose.orientation.x = 0 grasp_pose.pose.orientation.y = 0.707 grasp_pose.pose.orientation.z = 0 grasp_pose.pose.orientation.w = 0.707 arm.set_start_state_to_current_state() arm.set_pose_target(grasp_pose, end_effector_link) traj = arm.plan() arm.execute(traj) rospy.sleep(2) print arm.get_current_joint_values() #arm.shift_pose_target(4,1.57,end_effector_link) #arm.go() #rospy.sleep(2) arm.shift_pose_target(0, 0.11, end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() saved_target_pose = arm.get_current_pose(end_effector_link) #arm.set_named_target("initial_arm2") #grasp scene.attach_box(end_effector_link, target_id, g_p, target_size) rospy.sleep(2) #grasping is over , from now is placing arm.shift_pose_target(2, 0.15, end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() arm.shift_pose_target(1, -0.2, end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() arm.shift_pose_target(2, -0.15, end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() scene.remove_attached_object(end_effector_link, target_id) rospy.sleep(2) #arm.set_pose_target(saved_target_pose,end_effector_link) #arm.go() #rospy.sleep(2) arm.set_named_target("initial_arm1") arm.go() rospy.sleep(2) #remove and shut down scene.remove_attached_object(end_effector_link, 'l_tool') scene.remove_attached_object(end_effector_link, 'r_tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class EdPick: def __init__(self): allowed_planning_time = 0.0 ARM_AND_LINEAR_GROUP_NAME = "arm_and_linear" ARM_GROUP_NAME = "arm_torso" GRIPPER_GROUP = "gripper" REFERENCE_FRAME = "base_link" PLANNER = "RRTConnectkConfigDefault" #PLANNER = "RRTstarkConfigDefault" MOVE_GROUP_SERVICE = "/move_group/plan_execution/set_parameters" result = False upright_constraints = None robot = None arm = None arm_and_linear = None database = None scene = None grasp_generator = None place_generator = None marker_publisher = None self.robot = RobotCommander() self.arm = self.robot.get_group("arm_torso") self.gripper = self.robot.get_group(GRIPPER_GROUP) self.scene = PlanningSceneInterface() self.pickup_object = RecognizedObject("part") self.table = EntityInfo() def get_object(self): rospy.wait_for_service('/ed/kinect/update') self.update_client = rospy.ServiceProxy('/ed/kinect/update', Update) self.request_update = UpdateRequest() self.request_update.area_description = "on_top_of dinner-table" self.result_update = self.update_client(self.request_update) print "Found IDs: " % self.result_update.new_ids rospy.wait_for_service('/ed/simple_query') self.query_client = rospy.ServiceProxy('/ed/simple_query', SimpleQuery) self.query_request = SimpleQueryRequest() self.query_request.id = self.result_update.new_ids[0] self.query_response = self.query_client(self.query_request) rospy.wait_for_service('/ed/simple_query') self.ed_simple = rospy.ServiceProxy('/ed/simple_query', SimpleQuery) simpleRequest = SimpleQueryRequest() simpleRequest.id = 'dinner-table' self.table = self.ed_simple(simpleRequest).entities[0] print self.table.pose def pick_object(self): # self.object = self.query_response.entities self.pickup_object = RecognizedObject("part") tmpPoseStamped = PoseStamped() tmpPoseStamped.header.frame_id = '/map' tmpPoseStamped.header.stamp = rospy.Time(0) tmpPoseStamped.pose = self.query_response.entities[0].pose self.pickup_obj.pose = tmpPoseStamped self.value_x = [] self.value_y = [] self.value_z = [] for j in range(0, len(self.query_response.entities[0].convex_hull)): self.value_x.append( self.query_response.entities[0].convex_hull[j].x) self.value_y.append( self.query_response.entities[0].convex_hull[j].y) self.value_z.append( self.query_response.entities[0].convex_hull[j].z) self.dim_x = max(self.value_x) - min(self.value_x) self.dim_y = max(self.value_y) - min(self.value_y) self.dim_z = max(self.value_z) - min(self.value_z) self.pickup_object.dimensions = (self.dim_x, self.dim_y, self.dim_z) self.add_to_planning_scene(self.pickup_object) self.grasps = self.generate_grasps_for_object(self.pickup_object) if not self.pickup(self.pickup_object, self.grasps) == -1: print "Executed pick action" self.attach_object_to_gripper(self.pickup_object) self.place_location.header = tmpPoseStamped.header self.place_location.pose.position.x = self.result.entityInfos[ i].pose.position.x self.place_location.pose.position.y = self.result.entityInfos[ i].pose.position.y + 0.1 self.place_location.pose.position.z = self.result.entityInfos[ i].pose.position.z self.place_location.pose.orientation.x = self.result.entityInfos[ i].pose.orientation.x self.place_location.pose.orientation.y = self.result.entityInfos[ i].pose.orientation.y self.place_location.pose.orientation.z = self.result.entityInfos[ i].pose.orientation.z self.place_location.pose.orientation.w = self.result.entityInfos[ i].pose.orientation.w myTable = MyTable(tmpPoseStamped.header, self.table.pose, self.table.convex_hull) self.place(self.recog_obj, myTable) print "Executed place action" def add_to_planning_scene(self, recognized_object): pose_stamped = copy.deepcopy(recognized_object.pose) self.scene.add_box( recognized_object.name, pose_stamped, (recognized_object.dimensions[0], recognized_object.dimensions[1], recognized_object.dimensions[2])) rospy.loginfo("Object {} added to planning scene".format( recognized_object.name)) rospy.logdebug( "Object {} added with pose \n{} \nand dimensions {}".format( recognized_object.name, pose_stamped, recognized_object.dimensions)) def attach_object_to_gripper(self, recognized_object): rospy.loginfo("Attach object to gripper") pose_stamped = copy.deepcopy(recognized_object.pose) # self.remove_attached_object(recognized_object.name) self.scene.remove_world_object(recognized_object.name) self.scene.attach_box( END_EFFECTOR_LINK_NAME, recognized_object.name, pose_stamped, (recognized_object.dimensions[0], recognized_object.dimensions[1], recognized_object.dimensions[2]), GRIPPER_LINKS_TO_IGNORE_COLLISION) def pickup(self, pickup_object, grasps): supp_surface = 'table', rospy.loginfo("Recognized object: " + str(pick_obj)) self.arm.set_support_surface_name(supp_surface) result = self.arm.pick(pick_object.name, grasps) self.print_moveit_result(result) return result def generate_grasps_for_object(self, recognized_object): self.recognized_object = recognized_object self.base_grasp_pose_stamped = copy.deepcopy( self.recognized_object.pose) self.horizontal_center_grasp_pose_stamped = self.base_grasp_pose_stamped self.temp_grasp_pose_stamped = self.base_grasp_pose_stamped self.grasp_quality = 1.0 if recognized_object.dimensions[2] < self.high_res_threshold: rospy.loginfo( "Z axis of object ({} m) is shorter than threshold {}. \ create denser grasp poses.".format( recognized_object.dimensions[2], self.high_res_threshold)) self.distance_vertical = self.distance_vertical_default / 2.0 self.padding_from_top_and_bottom = self.padding_from_top_and_bottom_default / 2.0 else: rospy.loginfo( "Z axis of object ({} m) is longer than threshold {}. \ create normal distance grasp poses.".format( recognized_object.dimensions[2], self.high_res_threshold)) self.distance_vertical = self.distance_vertical_default self.padding_from_top_and_bottom = self.padding_from_top_and_bottom_default self.number_of_poses_vertical = int( (recognized_object.dimensions[2] - float(self.padding_from_top_and_bottom)) / float(self.distance_vertical) / 2.0) rospy.loginfo("Number of vertical poses " + str(self.number_of_poses_vertical)) # Generate the central grasp pose. Based on this pose the other poses are generated. # check if this is a top grasp (x axis faces in negative z direction of base_link) or a side grasp # pose with x downward, z forward pose_stamped_compare = PoseStamped() pose_stamped_compare.header.frame_id = "base_link" pose_stamped_compare.pose.position.x = 0.0 pose_stamped_compare.pose.position.y = 0.0 pose_stamped_compare.pose.position.z = 0.0 pose_stamped_compare.pose.orientation.w = 0.7071 pose_stamped_compare.pose.orientation.x = 0.0 pose_stamped_compare.pose.orientation.y = 0.7071 pose_stamped_compare.pose.orientation.z = 0.0 angle = self.angle_between_poses(self.temp_grasp_pose_stamped, pose_stamped_compare) if angle < pi / 4: # do a pinch grasp rospy.loginfo("This grasp is a top grasp. Do a pinch grasp") self.x_offset = -recognized_object.dimensions[ 0] / 2 + self.palm_to_finger_tips_distance self.recognized_object.top_grasp = True else: rospy.loginfo("This grasp is a side grasp. Do a power grasp") self.x_offset = recognized_object.dimensions[ 0] / 2 + self.additional_offset_in_grasp_direction self.recognized_object.top_grasp = False self.shift_pose_along_the_grippers_axes(self.temp_grasp_pose_stamped, [-self.x_offset, 0, 0]) self.create_grasp(self.temp_grasp_pose_stamped) # vertical for vertical_counter in range(0, self.number_of_poses_vertical + 1): if self.recognized_object.rotational_symmetric: self.generate_horizontal_grasps(True) self.generate_vertical_grasp_upper_part(vertical_counter) self.generate_horizontal_grasps(True) self.generate_vertical_grasp_lower_part(vertical_counter) else: self.generate_horizontal_grasps(False) self.generate_vertical_grasp_upper_part(vertical_counter) self.generate_horizontal_grasps(False) self.generate_vertical_grasp_lower_part(vertical_counter) rospy.loginfo("Generated vertical pose " + str(vertical_counter)) rospy.loginfo("Number of generated grasps: " + str(len(self.grasps))) for grasp in self.grasps: # rospy.logdebug("Publishing grasp z position:" + str(grasp.grasp_pose.pose.position)) # rospy.loginfo("Grasp quality: %s" % (grasp.grasp_quality)) return self.grasps def generate_vertical_grasp_upper_part(self, vertical_counter): self.generate_vertical_grasp("upper", vertical_counter) def generate_vertical_grasp_lower_part(self, vertical_counter): self.generate_vertical_grasp("lower", vertical_counter) def generate_vertical_grasp(self, part, vertical_counter): if part == "upper": distance_vertical = self.distance_vertical elif part == "lower": distance_vertical = -self.distance_vertical else: rospy.logerror('Part %s unkown. Only use "upper" or "lower"' % (part)) self.horizontal_center_grasp_pose_stamped = copy.deepcopy( self.base_grasp_pose_stamped) self.temp_grasp_pose_stamped = self.horizontal_center_grasp_pose_stamped marvin_manipulation.transformations_common.shift_pose_along_the_grippers_axes( self.temp_grasp_pose_stamped, [0, 0, float(distance_vertical) * float(vertical_counter)]) self.grasp_quality = 1.0 / (float(vertical_counter) + 0.001 ) #avoid devided by zero self.create_grasp(self.temp_grasp_pose_stamped) def generate_horizontal_grasps(self, rotational_symmetric): if rotational_symmetric: rospy.loginfo('Rotational symmetric') for horizontal_counter in range( -self.number_of_poses_horizontal, self.number_of_poses_horizontal + 1): self.temp_grasp_pose_stamped = copy.deepcopy( self.horizontal_center_grasp_pose_stamped) self.temp_grasp_pose_stamped = self.rotate_pose_around_object_center( self.temp_grasp_pose_stamped, self.angle_horizontal * float(horizontal_counter), self.x_offset) self.create_grasp(self.temp_grasp_pose_stamped) else: rospy.loginfo('non symmetric') #create grasp facing in negative x direction self.temp_grasp_pose_stamped = copy.deepcopy( self.horizontal_center_grasp_pose_stamped) self.temp_grasp_pose_stamped = self.rotate_pose_around_object_center( self.temp_grasp_pose_stamped, pi, self.x_offset) self.create_grasp(self.temp_grasp_pose_stamped) def rotate_pose_around_object_center(self, pose_stamped, angle, distance_to_object_center): self.shift_pose_along_the_grippers_axes( pose_stamped, (distance_to_object_center, 0, 0)) self.rotate_pose_around_axis_by_angle(pose_stamped, angle, (0, 0, 1)) self.shift_pose_along_the_grippers_axes( pose_stamped, (-distance_to_object_center, 0, 0)) return pose_stamped def create_grasp(self, pose_stamped): grasp = Grasp() grasp.grasp_pose = copy.deepcopy(pose_stamped) # pre grasp gripper configuration grasp.pre_grasp_posture.header.frame_id = self.gripper_frame grasp.pre_grasp_posture.header.stamp = rospy.Time.now() grasp.pre_grasp_posture.joint_names = self.finger_joint_names grasp.pre_grasp_posture.points.append( JointTrajectoryPoint(positions=[ self.gripper_open_angle_radian, self.gripper_open_angle_radian ], effort=[self.max_effort, self.max_effort])) # approach grasp.pre_grasp_approach.direction.header.frame_id = self.gripper_frame grasp.pre_grasp_approach.direction.vector.x = 1.0 grasp.pre_grasp_approach.min_distance = 0.04 grasp.pre_grasp_approach.desired_distance = 0.15 # grasp gripper configuration grasp.grasp_posture.header.frame_id = self.gripper_frame grasp.grasp_posture.header.stamp = rospy.Time.now() grasp.grasp_posture.joint_names = self.finger_joint_names grasp.grasp_posture.points.append( JointTrajectoryPoint(positions=[ self.gripper_closed_angle_radian, self.gripper_closed_angle_radian ], effort=[self.max_effort, self.max_effort])) # TODO: add time_from_start here and for gripper open # retreat grasp.post_grasp_retreat.direction.header.frame_id = "base_link" grasp.post_grasp_retreat.direction.vector.z = 1.0 grasp.post_grasp_retreat.min_distance = 0.03 grasp.post_grasp_retreat.desired_distance = 0.05 # objects allowed to touch while approaching grasp.allowed_touch_objects = [self.recognized_object.name] grasp.grasp_quality = self.grasp_quality self.grasps.append(grasp) def angle_between_poses(pose_stamped, pose_stamped_compare=None): if not pose_stamped_compare: #pose with x forward, z upward pose_stamped_compare = PoseStamped() pose_stamped_compare.header.frame_id = "base_link" pose_stamped_compare.pose.position.x = 0.0 pose_stamped_compare.pose.position.y = 0.0 pose_stamped_compare.pose.position.z = 0.0 pose_stamped_compare.pose.orientation.w = 1.0 pose_stamped_compare.pose.orientation.x = 0.0 pose_stamped_compare.pose.orientation.y = 0.0 pose_stamped_compare.pose.orientation.z = 0.0 quaternion = [ pose_stamped.pose.orientation.x, pose_stamped.pose.orientation.y, pose_stamped.pose.orientation.z, pose_stamped.pose.orientation.w ] compare_quaternion = [ pose_stamped_compare.pose.orientation.x, pose_stamped_compare.pose.orientation.y, pose_stamped_compare.pose.orientation.z, pose_stamped_compare.pose.orientation.w ] inner_product = numpy.inner(quaternion, compare_quaternion) angle_between_poses = math.degrees(math.acos(2 * inner_product**2 - 1)) print("Angle between poses: %s" % angle_between_poses) return angle_between_poses def shift_pose_along_the_grippers_axes(pose_stamped, shift_per_axis_array): ''' Shifts given pose_stamped by the amounts given in shift_per_axis_array :param PoseStamped pose_stamped: :param () shift_per_axis_array: tupel of translations e.g. (0.5, 0, 0) to shift the pose 0.5m in x direction ''' frame = PyKDL.Frame( PyKDL.Rotation.Quaternion(pose_stamped.pose.orientation.x, pose_stamped.pose.orientation.y, pose_stamped.pose.orientation.z, pose_stamped.pose.orientation.w), PyKDL.Vector(pose_stamped.pose.position.x, pose_stamped.pose.position.y, pose_stamped.pose.position.z)) point = PyKDL.Vector(shift_per_axis_array[0], shift_per_axis_array[1], shift_per_axis_array[2]) point = frame * point pose_stamped.pose.position.x = point.x() pose_stamped.pose.position.y = point.y() pose_stamped.pose.position.z = point.z() def place(self, recognized_object, pose_stamped): result = self.arm.place(recognized_object.name, pose_stamped) rospy.loginfo("Place result: " + str(result)) if result == True: return result return result
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) # Construct the initial scene object scene = PlanningSceneInterface() rospy.sleep(1) # Connect to the right_arm move group right_arm = moveit_commander.MoveGroupCommander('arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Display the name of the end_effector link rospy.loginfo("The end effector link is: " + str(end_effector_link)) # Set a small tolerance on joint angles right_arm.set_goal_joint_tolerance(0.001) right_arm.set_goal_position_tolerance(0.0001) for i in range(5): scene.remove_attached_object('block' + str(i+1)) scene.remove_world_object('block' + str(i+1)) rospy.sleep(0.5) # Start the arm target in "right" pose stored in the SRDF file right_arm.set_named_target('right') # Plan a trajectory to the goal configuration traj = right_arm.plan() # Execute the planned trajectory right_arm.execute(traj) # Pause for a moment rospy.sleep(1) target_pose = right_arm.get_current_pose() target_pose.pose.position.x -= 0.2 target_pose.pose.position.y += 0.5 target_pose.pose.position.z -= 0.5 target_pose.pose.orientation.w = 1 count = 0 while count < 2: # Set the size of block block_size = [0.01, 0.05, 0.05] count += 1 scene.add_box('block'+str(count), target_pose, block_size) self.move_arm(right_arm, 1) rospy.sleep(2) # Create a pose for the block relative to the end-effector p = PoseStamped() p.header.frame_id = end_effector_link # Place the end of the object within the grasp of the gripper p.pose.position.x = -0.015 p.pose.position.y = 0.0 p.pose.position.z = 0.0 # Align the object with the gripper (straight out) p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 # Attach the tool to the end-effector scene.attach_box(end_effector_link, 'block'+str(count), p, block_size) rospy.sleep(1) self.move_arm(right_arm, 2) rospy.sleep(1) right_arm.shift_pose_target(1, 0.025*(count-1), end_effector_link) right_arm.go() rospy.sleep(1) scene.remove_attached_object(end_effector_link, 'block'+str(count)) rospy.sleep(2) #right_arm.shift_pose_target(0, -0.05, end_effector_link) #right_arm.go() #self.move_arm(right_arm, 3) #rospy.sleep(2) # Return the arm to the named "resting" pose stored in the SRDF file right_arm.set_named_target('right') right_arm.go() rospy.sleep(1) # Cleanly shut down MoveIt moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') scene=PlanningSceneInterface() self.scene_pub=rospy.Publisher('planning_scene',PlanningScene) self.colors=dict() rospy.sleep(1) self.arm=MoveGroupCommander('arm') cartesian=rospy.get_param('~cartesian',True) self.end_effector_link=self.arm.get_end_effector_link() self.arm.set_goal_position_tolerance(0.005) self.arm.set_goal_orientation_tolerance(0.025) self.arm.allow_replanning(True) self.reference_frame='base_link' self.arm.set_pose_reference_frame(self.reference_frame) self.arm.set_planning_time(5) #scene planning table_id='table' #cylinder_id='cylinder' #box1_id='box1' box2_id='box2' target_id='target_object' #scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(table_id) scene.remove_world_object(target_id) rospy.sleep(2) table_ground=0.20 table_size=[1,0.5,0.01] #box1_size=[0.1,0.05,0.03] #box2_size=[0.05,0.05,0.1] r_tool_size=[0.03,0.01,0.06] l_tool_size=[0.03,0.01,0.06] target_size=[0.03,0.03,0.1] table_pose=PoseStamped() table_pose.header.frame_id=self.reference_frame table_pose.pose.position.x=0 table_pose.pose.position.y=0.75 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.7 box1_pose.pose.position.y=-0.2 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=self.reference_frame box2_pose.pose.position.x=0.6 box2_pose.pose.position.y=-0.05 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) ''' #left gripper l_p=PoseStamped() l_p.header.frame_id=self.end_effector_link l_p.pose.position.x=0.00 l_p.pose.position.y=0.04 l_p.pose.position.z=0.04 l_p.pose.orientation.w=1 scene.attach_box(self.end_effector_link,'l_tool',l_p,l_tool_size) #right gripper r_p=PoseStamped() r_p.header.frame_id=self.end_effector_link r_p.pose.position.x=0.00 r_p.pose.position.y=-0.04 r_p.pose.position.z=0.04 r_p.pose.orientation.w=1 scene.attach_box(self.end_effector_link,'r_tool',r_p,r_tool_size) #target target_pose=PoseStamped() target_pose.header.frame_id=self.reference_frame target_pose.pose.position.x=-0.4 target_pose.pose.position.y=0.65 target_pose.pose.position.z=table_ground+table_size[2]+target_size[2]/2.0 target_pose.pose.orientation.x=0 target_pose.pose.orientation.y=0 target_pose.pose.orientation.z=0 target_pose.pose.orientation.w=1 scene.add_box(target_id,target_pose,target_size) #grasp #grasp_pose(self,x,y,z,r,theta) #set color 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) self.setColor('r_tool',0.8,0,0) self.setColor('l_tool',0.8,0,0) self.setColor('target_object',0,1,0) self.sendColors() #motion planning j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156] self.arm.set_joint_value_target(j_ori_state) self.arm.go() rospy.sleep(3) r=0.06 maxtries=300 for theta in range(0,157,16): theta/=100.0 start_pose=self.arm.get_current_pose(self.end_effector_link) rospy.sleep(2) grasp_pose=self.grasp_pose(target_pose.pose.position.x,target_pose.pose.position.y,target_pose.pose.position.z,r,theta) if cartesian: (plan,e_j_state,s)=self.cts(start_pose.pose,grasp_pose.pose,maxtries) if s==0: continue #print(plan.joint_trajectory.points) #self.arm.set_joint_value_target(e_j_state) #self.arm.go() self.arm.execute(plan) rospy.sleep(5) #self.arm.set_named_target("initial_arm") #self.arm.go() #rospy.sleep(2) #remove and shut down scene.remove_attached_object(self.end_effector_link,'l_tool') scene.remove_attached_object(self.end_effector_link,'r_tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # Initialize API for movo_group moveit_commander.roscpp_initialize(sys.argv) # Initialize Node of ROS rospy.init_node('zhiyang_attached_camera_demo') # Define Scene Object scene = PlanningSceneInterface() rospy.sleep(1) # Initialize arm group from moveit group to control manipulator rarm = MoveGroupCommander('right_arm') larm = MoveGroupCommander('left_arm') # Get end effector link name r_end_effector_link = rarm.get_end_effector_link() l_end_effector_link = larm.get_end_effector_link() # Set tolerance of position(m) and orentation(rad) rarm.set_goal_position_tolerance(0.01) rarm.set_goal_orientation_tolerance(0.05) larm.set_goal_position_tolerance(0.01) larm.set_goal_orientation_tolerance(0.05) # Allow replanning rarm.allow_replanning(True) rarm.set_planning_time(10) larm.allow_replanning(True) larm.set_planning_time(10) # Let arm go back to home position # arm.set_named_target('home') # arm.go() # Remove other attached object in before Scene run # scene.remove_attached_object(end_effector_link, 'tool') # scene.remove_world_object('table') # scene.remove_world_object('target') # Set height of table table_ground = 0.6 # Set 3d size of table and tool table_size = [0.3, 0.7, 0.01] tool_size = [0.15, 0.15, 0.06] # Set orentation and position of tool p1 = PoseStamped() p1.header.frame_id = r_end_effector_link # p.pose.position.x = tool_size[0] / 2.0 - 0.025 # p.pose.position.y = -0.015 # p.pose.position.z = 0.0 p1.pose.position.x = -0.07 p1.pose.position.y = 0.0 p1.pose.position.z = 0.08 p1.pose.orientation.x = 0 p1.pose.orientation.y = 0 p1.pose.orientation.z = 0 p1.pose.orientation.w = 1 p2 = PoseStamped() p2.header.frame_id = l_end_effector_link p2.pose.position.x = -0.07 p2.pose.position.y = 0.0 p2.pose.position.z = 0.08 p2.pose.orientation.x = 0 p2.pose.orientation.y = 0 p2.pose.orientation.z = 0 p2.pose.orientation.w = 1 # Make tool attact to end effector scene.attach_box(r_end_effector_link, 'Camera1', p1, tool_size) scene.attach_box(l_end_effector_link, 'Camera2', p2, tool_size) # Add table to Scene table_pose = PoseStamped() table_pose.header.frame_id = 'base_link' table_pose.pose.position.x = 0.75 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', table_pose, table_size) rospy.sleep(2) # Update current state (position and orentation) rarm.set_start_state_to_current_state() larm.set_start_state_to_current_state() # Set target (rad) joint_positions = [0.827228546495185, 0.29496592875743577, 0.29496592875743577, 1.1185644936946095, -0.7987583317769674, -0.18950024740190782, 0.11752152218233858] rarm.set_joint_value_target(joint_positions) larm.set_joint_value_target(joint_positions) # Let arm go rarm.go() larm.go() rospy.sleep(1) # Let arm go back to initial state # arm.set_named_target('home') # arm.go() moveit_commander.roscpp_shutdown() moveit_commander.os._exit(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, queue_size=10) # 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 left_arm = MoveGroupCommander('left_arm') left_gripper = MoveGroupCommander('left_gripper') # Get the name of the end-effector link left_eef = left_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) left_arm.set_goal_position_tolerance(0.01) left_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution left_arm.allow_replanning(True) left_reference_frame = left_arm.get_planning_frame() # Allow 5 seconds per planning attempt left_arm.set_planning_time(5) object1_id = 'object1' obstacle1_id = 'obstacle1' # Remove leftover objects from a previous run scene.remove_world_object(object1_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_arm_zero') left_arm.go() rospy.sleep(1) left_gripper.set_joint_value_target(GRIPPER_OPEN) left_gripper.go() rospy.sleep(1) object1_size = [0.088, 0.04, 0.02] # Add a table top and two boxes to the scene obstacle1_size = [0.3, 0.05, 0.45] # Add a table top and two boxes to the scene obstacle1_pose = PoseStamped() obstacle1_pose.header.frame_id = left_reference_frame obstacle1_pose.pose.position.x = 0.96 obstacle1_pose.pose.position.y = 0.5 obstacle1_pose.pose.position.z = 0.04 obstacle1_pose.pose.orientation.w = 1.0 scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) self.setColor(obstacle1_id, 0.8, 0.4, 0, 1.0) object1_pose = PoseStamped() object1_pose.header.frame_id = left_reference_frame object1_pose.pose.position.x = 0.80 object1_pose.pose.position.y = 0.3 object1_pose.pose.position.z = 0 object1_pose.pose.orientation.w = 1.0 scene.add_box(object1_id, object1_pose, object1_size) self.setColor(object1_id, 0.8, 0, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the start state to the current state left_arm.set_start_state_to_current_state() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = left_reference_frame target_pose.pose.position.x = 0.58 target_pose.pose.position.y = 0.1878 target_pose.pose.position.z = .1116 target_pose.pose.orientation.x = 0.1325 target_pose.pose.orientation.y = 0.9908 target_pose.pose.orientation.z = 0.0095 target_pose.pose.orientation.w = 0.0254 # Set the target pose for the arm left_arm.set_pose_target(target_pose, left_eef) # Move the arm to the target pose (if possible) left_arm.go() # Pause for a moment... rospy.sleep(2) left_gripper.set_joint_value_target(GRIPPER_CLOSE) left_gripper.go() rospy.sleep(1) scene.attach_box(left_eef, object1_id, object1_pose, object1_size) # Cartesian Paths waypoints1 = [] start_pose = left_arm.get_current_pose(left_eef).pose wpose = deepcopy(start_pose) waypoints1.append(deepcopy(wpose)) wpose.position.x -= 0.05 wpose.position.y += 0.105 wpose.position.z += 0.07 waypoints1.append(deepcopy(wpose)) wpose.position.x -= 0.05 wpose.position.y += 0.105 wpose.position.z -= 0.07 waypoints1.append(deepcopy(wpose)) (cartesian_plan, fraction) = left_arm.compute_cartesian_path( waypoints1, # waypoint poses 0.01, # eef_step 1cm 0.0, # jump_threshold True) # avoid_collisions left_arm.execute(cartesian_plan) rospy.sleep(2) left_gripper.set_joint_value_target(GRIPPER_OPEN) left_gripper.go() rospy.sleep(1) scene.remove_attached_object(left_eef, object1_id) # Exit MoveIt cleanly waypoints2 = [] start_pose = left_arm.get_current_pose(left_eef).pose wpose = deepcopy(start_pose) waypoints2.append(deepcopy(wpose)) wpose.position.z += 0.07 waypoints2.append(deepcopy(wpose)) wpose.position.x += 0.1 wpose.position.y -= 0.21 waypoints2.append(deepcopy(wpose)) wpose.position.z -= 0.07 waypoints2.append(deepcopy(wpose)) (cartesian_plan, fraction) = left_arm.compute_cartesian_path( waypoints2, # waypoint poses 0.01, # eef_step 1cm 0.0, # jump_threshold True) # avoid_collisions left_arm.execute(cartesian_plan) rospy.sleep(2) moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): rospy.init_node('moveit_demo') #self.sub = rospy.Subscriber('/aruco_single/pose', Pose, self.update) #self.sub = rospy.Subscriber('/aruco_single/pose', PoseArray, self.update, queue_size=1) #msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped) #grasp msg publisher m_pub_joint_g_1 = rospy.Publisher("/ik_solution_g_1", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_g_2 = rospy.Publisher("/ik_solution_g_2", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_g_3 = rospy.Publisher("/ik_solution_g_3", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_g_4 = rospy.Publisher("/ik_solution_g_4", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_g_5 = rospy.Publisher("/ik_solution_g_5", JointTrajectoryPoint, queue_size=1, latch=True) #place msg publisher m_pub_joint_p_1 = rospy.Publisher("/ik_solution_p_1", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_p_2 = rospy.Publisher("/ik_solution_p_2", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_p_3 = rospy.Publisher("/ik_solution_p_3", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_p_4 = rospy.Publisher("/ik_solution_p_4", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_p_5 = rospy.Publisher("/ik_solution_p_5", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_p_6 = rospy.Publisher("/ik_solution_p_6", JointTrajectoryPoint, queue_size=1, latch=True) moveit_commander.roscpp_initialize(sys.argv) cartesian = rospy.get_param('~cartesian', True) scene = PlanningSceneInterface() self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) self.colors = dict() ''' """set up ros publishers""" self.m_pub_m2j = rospy.Publisher("/two_finger/motoman_control/move_to_joint", JointAnglesDuration, queue_size=1, latch=True); self.m_pub_m2p = rospy.Publisher("/two_finger/motoman_control/move_to_pos", PoseDuration, queue_size=1, latch=True) self.m_pub_m2p_star = rospy.Publisher("/two_finger/motoman_control/move_to_pos_straight", PoseDuration, queue_size=1, latch=True) """ set up ros service """ print("wait gripper service") rospy.wait_for_service("/two_finger/gripper/gotoPosition") rospy.wait_for_service("/two_finger/gripper/gotoPositionUntilTouch") self.m_srv_grpCtrl = rospy.ServiceProxy("/two_finger/gripper/gotoPosition", SetPosition) self.m_srv_grpCtrlUntilTouch = rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch", SetPosition) self.m_srv_grpSpeed = rospy.ServiceProxy("/two_finger/gripper/setSpeed", SetParameter) self.m_srv_grpTorque = rospy.ServiceProxy("/two_finger/gripper/setTorque", SetParameter) self.m_srv_grpTorque(20) self.m_srv_grpSpeed(128) rospy.wait_for_service("/two_finger/motoman_control/trajectory_status") self.m_srv_trajectoryStatus = rospy.ServiceProxy("/two_finger/motoman_control/trajectory_status", Trigger) ''' rospy.sleep(1) arm = MoveGroupCommander('arm') arm.allow_replanning(True) end_effector_link = arm.get_end_effector_link() arm.set_goal_position_tolerance(0.03) arm.set_goal_orientation_tolerance(0.025) arm.allow_replanning(True) reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) arm.set_planning_time(5) #scene planning table_id = 'table' #cylinder_id='cylinder' box_id = 'box' #box2_id='box2' target_id = 'target_object' scene.remove_world_object(box_id) #scene.remove_world_object(box2_id) scene.remove_world_object(table_id) #scene.remove_world_object(target_id) scene.remove_world_object(target_id) rospy.sleep(2) #table_ground=0.62 table_size = [0.9, 0.6, 0.018] box_size = [0.15, 0.15, 0.015] #box2_size=[0.05,0.05,0.1] r_tool_size = [0.03, 0.02, 0.18] l_tool_size = [0.03, 0.02, 0.18] target_size = [0.059, 0.059, 0.12] table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = -0.05 table_pose.pose.position.y = 0.61 table_pose.pose.position.z = 0.194 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.7 box1_pose.pose.position.y=-0.2 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.6 box2_pose.pose.position.y=-0.05 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) ''' #left gripper l_p = PoseStamped() l_p.header.frame_id = end_effector_link l_p.pose.position.x = 0.00 l_p.pose.position.y = 0.057 l_p.pose.position.z = 0.09 l_p.pose.orientation.w = 1 scene.attach_box(end_effector_link, 'l_tool', l_p, l_tool_size) #right gripper r_p = PoseStamped() r_p.header.frame_id = end_effector_link r_p.pose.position.x = 0.00 r_p.pose.position.y = -0.057 r_p.pose.position.z = 0.09 r_p.pose.orientation.w = 1 scene.attach_box(end_effector_link, 'r_tool', r_p, r_tool_size) #box #place box_pose = PoseStamped() box_pose.header.frame_id = reference_frame box_pose.pose.position.x = -0.30 box_pose.pose.position.y = 0.69 box_pose.pose.position.z = 0.21 box_pose.pose.orientation.w = 1 scene.add_box(box_id, box_pose, box_size) #place p_pose = box_pose p_pose.pose.position.x += 0.01 p_pose.pose.position.y += 0.01 p_pose.pose.position.z += 0.11 p_pose.pose.orientation.w = -0.5 p_pose.pose.orientation.x = -0.5 p_pose.pose.orientation.y = -0.5 p_pose.pose.orientation.z = 0.5 #scene.add_box(box1_id,p_pose,box2_size) #grasp g_p = PoseStamped() g_p.header.frame_id = end_effector_link g_p.pose.position.x = 0 g_p.pose.position.y = 0 g_p.pose.position.z = 0.15 g_p.pose.orientation.w = 0.707 g_p.pose.orientation.x = -0 g_p.pose.orientation.y = -0.707 g_p.pose.orientation.z = 0.0 #set color 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) self.setColor('r_tool', 0.8, 0, 0) self.setColor('l_tool', 0.8, 0, 0) self.sendColors() #motion planning dev = InputDevice('/dev/input/event4') print "xxx" while not rospy.is_shutdown(): duration = rospy.Duration(4) duration_s = rospy.Duration(2.5) select([dev], [], []) for event in dev.read(): if (event.value == 0) and (event.code == 16): print " Path planning" scene.remove_world_object(target_id) rospy.loginfo("Plz move the target") msg = rospy.wait_for_message('/aruco_single/pose', PoseStamped) target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = -(msg.pose.position.y) - 0.28 target_pose.pose.position.y = -msg.pose.position.x + 0.81 - 0.068 target_pose.pose.position.z = 1.36 - msg.pose.position.z + 0.06 target_pose.pose.orientation.x = 0 target_pose.pose.orientation.y = 0 target_pose.pose.orientation.z = -0 target_pose.pose.orientation.w = 1 scene.add_box(target_id, target_pose, target_size) self.setColor('target_object', 0, 1, 0) #pre-grasp pose pre_grasp_pose = PoseStamped() pre_grasp_pose.header.frame_id = reference_frame pre_grasp_pose.pose.position = target_pose.pose.position pre_grasp_pose.pose.position.y -= 0.1 pre_grasp_pose.pose.position.z += 0.015 pre_grasp_pose.pose.orientation.x = -0.5 pre_grasp_pose.pose.orientation.y = -0.5 pre_grasp_pose.pose.orientation.z = -0.5 pre_grasp_pose.pose.orientation.w = 0.5 #grasp pose grasp_pose = PoseStamped() grasp_pose.header.frame_id = reference_frame grasp_pose.pose.position = target_pose.pose.position grasp_pose.pose.position.y -= 0.05 grasp_pose.pose.position.z += 0.01 ''' #orientation from left grasp_pose.pose.orientation.x=-0.000149 grasp_pose.pose.orientation.y=-0.707 grasp_pose.pose.orientation.z=0 grasp_pose.pose.orientation.w=0.707 ''' #orientation from forward grasp_pose.pose.orientation.x = -0.5 grasp_pose.pose.orientation.y = -0.5 grasp_pose.pose.orientation.z = -0.5 grasp_pose.pose.orientation.w = 0.5 arm.set_named_target("initial_arm") arm.go() rospy.sleep(2) start_pose = arm.get_current_pose(end_effector_link).pose #initial waypoints list waypoints_1 = [] waypoints_2 = [] waypoints_3 = [] waypoints_4 = [] if cartesian: waypoints_1.append(start_pose) waypoints_1.append(deepcopy(pre_grasp_pose.pose)) fraction = 0.0 maxtries = 300 attempts = 0 #arm.set_start_state__1to_current_state() #plan the cartesian path connecting waypoints while fraction < 1.0 and attempts < maxtries: (plan_1, fraction) = arm.compute_cartesian_path( waypoints_1, 0.01, 0.0, True) attempts += 1 if (attempts % 300 == 0 and fraction != 1.0): rospy.loginfo( "path planning failed with only " + str(fraction * 100) + "% success after " + str(maxtries) + " attempts") continue if fraction == 1.0: rospy.loginfo( "path compute successfully. Move the arm.") #place = JointAnglesDuration(JointAngles(plan.joint_trajectory.points[len(plan.joint_trajectory)-1].positions), duration) #self.m_pub_m2j.publish(place) print arm.execute(plan_1) m_pub_joint_g_1.publish( plan_1.joint_trajectory.points[-1]) m_pub_joint_g_2.publish( plan_1.joint_trajectory.points[-2]) m_pub_joint_g_3.publish( plan_1.joint_trajectory.points[-3]) m_pub_joint_g_4.publish( plan_1.joint_trajectory.points[-5]) m_pub_joint_g_5.publish( plan_1.joint_trajectory.points[-8]) rospy.loginfo("path execution complete. ") scene.attach_box(end_effector_link, target_id, g_p, target_size) rospy.sleep(5) g_pose = arm.get_current_pose(end_effector_link) if cartesian: waypoints_2.append(g_pose.pose) waypoints_2.append(deepcopy(p_pose.pose)) fraction = 0.0 maxtries = 300 attempts = 0 #arm.set_start_state_to_current_state() #plan the cartesian path connecting waypoints while fraction < 1.0 and attempts < maxtries: (plan_2, fraction) = arm.compute_cartesian_path( waypoints_2, 0.01, 0.0, True) attempts += 1 if (attempts % 300 == 0 and fraction != 1.0): rospy.loginfo( "path planning failed with only " + str(fraction * 100) + "% success after " + str(maxtries) + " attempts") continue if fraction == 1.0: rospy.loginfo( "path compute successfully. Move the arm.") #place = JointAnglesDuration(JointAngles(plan.joint_trajectory.points[len(plan.joint_trajectory)-1].positions), duration) #self.m_pub_m2j.publish(place) arm.execute(plan_2) m_pub_joint_p_1.publish( plan_2.joint_trajectory.points[5]) m_pub_joint_p_1.publish( plan_2.joint_trajectory.points[len( plan_2.joint_trajectory.points) / 5]) m_pub_joint_p_1.publish( plan_2.joint_trajectory.points[len( plan_2.joint_trajectory.points) / 2]) m_pub_joint_p_1.publish( plan_2.joint_trajectory.points[ -len(plan_2.joint_trajectory.points) / 5]) m_pub_joint_p_1.publish( plan_2.joint_trajectory.points[5]) m_pub_joint_p_1.publish( plan_2.joint_trajectory.points[1]) rospy.loginfo("path execution complete. ") rospy.sleep(5) #m_pub_joint_g.publish(arm.get_current_joint_value()) ''' arm.set_start_state_to_current_state() arm.set_pose_target(grasp_pose,end_effector_link) traj=arm.plan() arm.execute(traj) rospy.sleep(2) print arm.get_current_joint_values() #arm.shift_pose_target(4,1.57,end_effector_link) #arm.go() #rospy.sleep(2) arm.shift_pose_target(0,0.11,end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() saved_target_pose=arm.get_current_pose(end_effector_link) #arm.set_named_target("initial_arm2") #grasp scene.attach_box(end_effector_link,target_id,g_p,target_size) rospy.sleep(2) #grasping is over , from now is placing arm.shift_pose_target(2,0.15,end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() arm.shift_pose_target(1,-0.2,end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() arm.shift_pose_target(2,-0.15,end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() scene.remove_attached_object(end_effector_link,target_id) rospy.sleep(2) #arm.set_pose_target(saved_target_pose,end_effector_link) #arm.go() #rospy.sleep(2) arm.set_named_target("initial_arm1") arm.go() rospy.sleep(2) ''' #remove and shut down scene.remove_world_object(target_id) scene.remove_world_object(table_id) scene.remove_attached_object(end_effector_link, 'l_tool') scene.remove_attached_object(end_effector_link, 'r_tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_object', anonymous=True) # 初始化场景对象,用来监听外部环境的变化 scene = PlanningSceneInterface() rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('dianaS1_arm') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) arm.set_planning_time(10) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() # 移除场景中之前运行残留的物体 scene.remove_attached_object(end_effector_link, 'tool') scene.remove_world_object('table') scene.remove_world_object('target') # 设置桌面的高度 table_ground = 0.4 # 设置table和tool的三维尺寸 table_size = [0.05, 0.4, 0.7] # 设置长宽高 tool_size = [0.2, 0.02, 0.02] # 设置tool的位姿 p = PoseStamped() #tool放置位置的参考坐标系 p.header.frame_id = end_effector_link #tool的具体位置姿态 p.pose.position.x = tool_size[0] / 2.0 - 0.025 p.pose.position.y = -0.015 p.pose.position.z = 0.0 p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 # 将tool附着到机器人的终端,该函数为附着某物体到机器人上 scene.attach_box(end_effector_link, 'tool', p, tool_size,touch_links=[end_effector_link]) # 将table加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = 'base_link' table_pose.pose.position.x = 0.25 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', table_pose, table_size) # 添加障碍物 rospy.sleep(1) # 姿态使用四元数描述,基于base_link坐标系 target_pose = PoseStamped() # 参考坐标系,前面设置了 target_pose.header.frame_id = 'base_link' target_pose.header.stamp = rospy.Time.now() # 时间戳? # 末端位置 target_pose.pose.position.x =0.424167 target_pose.pose.position.y = 0.0622007 target_pose.pose.position.z = 0.712836 # 末端姿态,四元数 target_pose.pose.orientation.x = 0.999735 target_pose.pose.orientation.y = 0.0230112 target_pose.pose.orientation.z = 0.0000697211 target_pose.pose.orientation.w = -0.000124006 # 更新当前的位姿 arm.set_start_state_to_current_state() # 设置机械臂终端运动的目标位姿 arm.set_pose_target(target_pose, end_effector_link) # 规划运动路径,返回虚影的效果 traj = arm.plan() # 按照规划的运动路径控制机械臂运动 arm.execute(traj) rospy.sleep(2) # 执行完成后休息1s # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() moveit_commander.roscpp_shutdown() moveit_commander.os._exit(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) # eel = len(self.right_arm.get_end_effector_link()) # print eel # 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) # self.m_error = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) ### OPEN THE GRIPPER ### self.open_gripper() ### Attach / Remove Object ### self.aro = None # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) # 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() ### GIVE SCENE TIME TO CATCH UP ### rospy.sleep(5) print "==================== Executing ===========================" blist = ['target','custom_2','custom_3', 'custom_table'] self.idx_list = [] for name in obj_id: print name if name not in blist: self.idx_list.append(obj_id.index(name)) ################################## TESTING AREA ##################################### # ga = self.grasp_attempt() # print ga # print obj_id success = False i=0 while success == False: idx = self.idx_list[i] print idx new_pose, ds = self.declutter_scene(idx) if ds == True: self.post_grasp(new_pose, obj_id[idx]) i+=1 ################# GRASPING ATTEMPTS ################### # ga = self.grasp_attempt() # print ga rospy.sleep(5) # # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # # Exit the script moveit_commander.os._exit(0) ################################################################# FUNCTIONS ################################################################################# def grasp_attempt(self): init_poses = [] grasp_poses = [] for axis in range(0,5): pg = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'pg') gp = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'gp') init_poses.append(pg) grasp_poses.append(gp) gid, grasps = self.grasp_generator(grasp_poses) for i in range(0, len(gid)): self.gripper_pose_pub.publish(grasps[i]) rospy.sleep(0.1) success = False for pg in range(0,5): print pg plp = self.right_arm.plan(init_poses[pg].pose) print len(plp.joint_trajectory.points) if len(plp.joint_trajectory.points) == 0: print "No valid pregrasp Position, continuing on next one" continue self.right_arm.plan(init_poses[pg].pose) self.right_arm.go() if pg == 0: idx = gid.index('front') elif pg == 1: idx = gid.index('right') elif pg == 2: idx = gid.index('left') elif pg == 3: idx = gid.index('topx') else: idx = gid.index('topy') print ("idx = ",idx) for g in range(idx, len(gid)): ### TODO: fix the loop here, we dont want to check every single grasp after the index print g pl2 = self.right_arm.plan(grasps[g].pose) print len(pl2.joint_trajectory.points) if len(pl2.joint_trajectory.points) == 0: print "No Valid Grasp, continuing on next one" continue self.right_arm.plan(grasps[g].pose) self.right_arm.go() success = True break if success == True: break return success def declutter_scene(self,index): print obj_id[index] init_poses = [] grasp_poses = [] for axis in range(0,5): pg = self.grasp_pose(obj_pose[index], axis, 'pg') gp = self.grasp_pose(obj_pose[index], axis, 'gp') init_poses.append(pg) grasp_poses.append(gp) gid, grasps = self.grasp_generator(grasp_poses) for i in range(0, len(gid)): self.gripper_pose_pub.publish(grasps[i]) rospy.sleep(0.1) success = False for pg in range(0,5): print pg plp = self.right_arm.plan(init_poses[pg].pose) print len(plp.joint_trajectory.points) if len(plp.joint_trajectory.points) == 0: print "No valid pregrasp Position, continuing on next one" continue self.right_arm.plan(init_poses[pg].pose) self.right_arm.go() if pg == 0: idx = gid.index('front') elif pg == 1: idx = gid.index('right') elif pg == 2: idx = gid.index('left') elif pg == 3: idx = gid.index('topx') else: idx = gid.index('topy') print ("idx = ",idx) for g in range(idx, len(gid)): ### TODO: fix the loop here, we dont want to check every single grasp after the index print g pl2 = self.right_arm.plan(grasps[g].pose) print len(pl2.joint_trajectory.points) if len(pl2.joint_trajectory.points) == 0: print "No Valid Grasp, continuing on next one" continue self.right_arm.plan(grasps[g].pose) self.right_arm.go() new_pose = grasps[g] success = True break if success == True: break return (new_pose,success) def post_grasp(self,new_pose, obj_to_grasp): ######### GRASP OBJECT/ REMOVE FROM SCENE ######. self.close_gripper() self.aro = obj_to_grasp print self.aro # ### POST GRASP RETREAT ### # M1 = transformations.quaternion_matrix([new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z, new_pose.pose.orientation.w]) # M1[0,3] = new_pose.pose.position.x # M1[1,3] = new_pose.pose.position.y # M1[2,3] = new_pose.pose.position.z # M2 = transformations.euler_matrix(0, 0, 0) # M2[0,3] = 0.0 # offset about x # M2[1,3] = 0.0 # about y # M2[2,3] = 0.3 # about z # T1 = np.dot(M1, M2) # npo = deepcopy(new_pose) # npo.pose.position.x = T1[0,3] # npo.pose.position.y = T1[1,3] # npo.pose.position.z = T1[2,3] # quat = transformations.quaternion_from_matrix(T1) # npo.pose.orientation.x = quat[0] # npo.pose.orientation.y = quat[1] # npo.pose.orientation.z = quat[2] # npo.pose.orientation.w = quat[3] # npo.header.frame_id = REFERENCE_FRAME # self.right_arm.plan(npo.pose) # self.right_arm.go() ### PLACE THE OBJECT place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.80 place_pose.pose.position.y = -0.33 place_pose.pose.position.z = 0.53 place_pose.pose.orientation.w = 1.0 def grasp_pose(self, target_pose, axis, stage): ############ TODO : GENERATE AUTOMATED PRE-GRASPING POSITIONS BASED ON THE PRIMITIVE ######### if axis == 0: 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, 0, 0) if stage == 'pg': M2[0,3] = -0.25 # offset about x elif stage == 'gp': M2[0,3] = -0.18 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.0 # about z elif axis == 1: 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, 0, 1.57) M2[0,3] = 0.0 # offset about x if stage == 'pg': M2[1,3] = -0.25 # about y elif stage == 'gp': M2[1,3] = -0.18 # about y M2[2,3] = 0.0 # about z elif axis ==2: 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, 0, -1.57) M2[0,3] = 0.0 # offset about x if stage == 'pg': M2[1,3] = 0.25 # about y elif stage == 'gp': M2[1,3] = 0.18 # about y M2[2,3] = 0.0 # about z elif axis ==3: 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 if stage == 'pg': M2[2,3] = 0.30 # about z elif stage == 'gp': M2[2,3] = 0.23 # about z else: 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(1.57, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y if stage == 'pg': M2[2,3] = 0.30 # about z elif stage == 'gp': M2[2,3] = 0.23 # about z T1 = np.dot(M1, M2) grasp_pose = deepcopy(target_pose) grasp_pose.pose.position.x = T1[0,3] grasp_pose.pose.position.y = T1[1,3] grasp_pose.pose.position.z = T1[2,3] quat = transformations.quaternion_from_matrix(T1) grasp_pose.pose.orientation.x = quat[0] grasp_pose.pose.orientation.y = quat[1] grasp_pose.pose.orientation.z = quat[2] grasp_pose.pose.orientation.w = quat[3] grasp_pose.header.frame_id = REFERENCE_FRAME return grasp_pose def grasp_generator(self, initial_poses): # A list to hold the grasps grasps = [] o = [] # Original Pose of the object (o) O=[] gid = [] i= 0 while i < len(initial_poses): o.append(initial_poses[i]) i+=1 G = transformations.euler_matrix(0, 0, 0) # Generate a grasps for along z axis (x and y directions) k = 0 while k <= 4: O.append(transformations.quaternion_matrix([o[k].pose.orientation.x, o[k].pose.orientation.y, o[k].pose.orientation.z, o[k].pose.orientation.w])) O[k][0,3] = o[k].pose.position.x O[k][1,3] = o[k].pose.position.y O[k][2,3] = o[k].pose.position.z if k in range(0,3): for z in self.drange(-obj_size[obj_id.index('target')][2]/2, obj_size[obj_id.index('target')][2]/2, 0.02): # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] grasp.pose.position.y = T[1,3] grasp.pose.position.z = T[2,3] +z quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) if k ==0: gid.append('front') elif k ==1: gid.append('right') elif k ==2: gid.append('left') elif k == 3: for x in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.01): # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] +x grasp.pose.position.y = T[1,3] grasp.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) gid.append('topx') else: for y in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.01): # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] grasp.pose.position.y = T[1,3] +y grasp.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) gid.append('topy') k+=1 # Return the list return (gid,grasps) def scene_generator(self): obj_pose =[] obj_id = [] obj_size = [] bl = ['ground_plane','pr2'] global obj_pose, obj_id , obj_size ops = PoseStamped() ops.header.frame_id = REFERENCE_FRAME for model_name in self.pwh.name: if model_name not in bl: obj_id.append(model_name) ops.pose = self.pwh.pose[self.pwh.name.index(model_name)] obj_pose.append(deepcopy(ops)) obj_size.append([0.05, 0.05, 0.15]) obj_id[obj_id.index('custom_1')] = 'target' obj_size[obj_id.index('custom_2')] = [0.05, 0.05, 0.10] obj_size[obj_id.index('custom_3')] = [0.05, 0.05, 0.05] obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03] ### REMOVE OBJECT FROM PREVIOUS RUNS ### for i in range(0, len(obj_id)): self.scene.remove_world_object(obj_id[i]) # Remove any attached objects from a previous session self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index('target')]) next_call = time.time() while True: next_call = next_call+1 if self.aro is None: for i in range(0, len(obj_id)): ### CREATE THE SCENE ### self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i]) ### Make the target purple and obstacles orange ### self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0) self.setColor(obj_id[obj_id.index('custom_2')], 1, 0.623, 0, 1.0) self.setColor(obj_id[obj_id.index('custom_3')], 1, 0.623, 0, 1.0) self.setColor(obj_id[obj_id.index('custom_1_0')], 1, 0.623, 0, 1.0) # Send the colors to the planning scene self.sendColors() else: ###### ATTACH ITEM TO GRIPPER ###### 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, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links) ### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ### self.scene.remove_world_object(obj_id[self.aro]) time.sleep(next_call - time.time()) def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg def drange(self, start, stop, step): r = start while r < stop: yield r r += step 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 # 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)
class PickAndPlace(object): def __init__(self, limb, start_pose, hover_distance=0.15): self.start_pose = start_pose self.hover_distance = hover_distance self.limb = robot_limb.Limb(limb) self.gripper = robot_gripper.Gripper(limb) self.base = "base_marker" self.pick_obj = "obj_marker" self.dest = "dest_marker" self.destinations = [] self.queue = Queue.Queue() self.tf = TransformListener() self.set_scene() self.set_limb_planner() self.enable_robot() self.set_destinations() print("Moving Sawyer Arm to Start Position") self.move_to_start() def set_limb_planner(): self.right_arm = MoveGroupCommander("right_arm") self.right_arm.set_planner_id('RRTConnectkConfigDefault') self.right_arm.allow_replanning(True) self.right_arm.set_planning_time(7) def enable_robot(): print("Getting robot state... ") self._rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() def set_scene(): self.scene_pub = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10) self.scene = PlanningSceneInterface() rospy.sleep(2) self.add_collision_object('turtlebot', 0.8, 0., -0.65, .5, 1.5, .33) self.add_collision_object('right_wall', 0., 0.65, 0., 4., .1, 4.) self.add_collision_object('left_wall', 0., -0.8, 0., 4., .1, 4.) self.add_collision_object('back_wall', -0.6, 0., 0., .1, 4., 4.) self.add_collision_object('destination_table', .5, -.4, -.35, 1, .5, .5) self.plan_scene = PlanningScene() self.plan_scene.is_diff = True self.scene_pub.publish(self.plan_scene) def add_collision_object(self, name, x, y, z, xs, ys, zs): self.scene.remove_world_object(name) o = PoseStamped() o.pose.position.x = x o.pose.position.y = y o.pose.position.z = z self.scene.attach_box('base', name, o, [xs, ys, zs]) def set_destinations(self): while len(self.destinations) == 0: if self.tf.frameExists(self.base) and self.tf.frameExists( self.dest): point, quaternion = self.tf.lookupTransform( self.base, self.dest, self.tf.getLatestCommonTime(self.base, self.dest)) position = list_to_point(point) self.destinations.append( Pose(position=position, orientation=self.start_pose.orientation)) def add_new_objects_to_queue(self, sleep=True): print("Checking if " + self.base + " and " + self.pick_obj + " both exist.") if self.tf.frameExists(self.base) and self.tf.frameExists( self.pick_obj): if sleep: rospy.sleep(10.0) self.add_new_objects_to_queue(sleep=False) else: print(self.base + " and " + self.pick_obj + " both exist.") point, quaternion = self.tf.lookupTransform( self.base, self.pick_obj, self.tf.getLatestCommonTime(self.base, self.pick_obj)) position = list_to_point(point) print("Adding " + self.pick_obj + " to queue") obj_location = Pose(position=position, orientation=self.start_pose.orientation) print("Picking Object from:", obj_location) self.queue.put(obj_location) def complete_pick_place(self): if not self.queue.empty(): start_pose = self.queue.get() end_pose = self.destinations[0] print("\nPicking...") self.pick(start_pose) print("\nPlacing...") self.place(end_pose) print("\Resetting...") self.move_to_start() def guarded_move_to_joint_position(self, pose): self.right_arm.set_pose_target(pose) self.right_arm.go() def servo_to_pose(self, pose): # servo down to release self.guarded_move_to_joint_position(pose) def approach(self, pose): # approach with a pose the hover-distance above the requested pose approach = copy.deepcopy(pose) approach.position.z += self.hover_distance print("approaching:", approach) self.guarded_move_to_joint_position(approach) def retract(self): # retrieve current pose from endpoint current_pose = self.limb.endpoint_pose() ik_pose = Pose() ik_pose.position.x = current_pose['position'].x ik_pose.position.y = current_pose['position'].y ik_pose.position.z = current_pose['position'].z + self.hover_distance ik_pose.orientation.x = current_pose['orientation'].x ik_pose.orientation.y = current_pose['orientation'].y ik_pose.orientation.z = current_pose['orientation'].z ik_pose.orientation.w = current_pose['orientation'].w # servo up from current pose self.guarded_move_to_joint_position(ik_pose) def move_to_start(self): print("Moving the right arm to start pose...") self.guarded_move_to_joint_position(self.start_pose) self.gripper_open() rospy.sleep(1.0) print("Running. Ctrl-c to quit") def gripper_open(self): self.gripper.open() rospy.sleep(1.0) def gripper_close(self): self.gripper.close() rospy.sleep(1.0) def pick(self, pose): # open the gripper self.gripper_open() # servo above pose self.approach(pose) # servo to pose self.servo_to_pose(pose) # close gripper self.gripper_close() # retract to clear object self.retract() def place(self, pose): # servo above pose self.approach(pose) # servo to pose self.servo_to_pose(pose) # open the gripper self.gripper_open() # retract to clear object self.retract()
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() # Pause for the scene to get ready rospy.sleep(1) # Initialize the MoveIt! commander for the right arm right_arm = MoveGroupCommander('right_arm') # Initialize the MoveIt! commander for the gripper right_gripper = MoveGroupCommander('right_gripper') # 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) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Remove leftover objects from a previous run scene.remove_attached_object(end_effector_link, 'tool') scene.remove_world_object('table') scene.remove_world_object('box1') scene.remove_world_object('box2') scene.remove_world_object('target') # Set the height of the table off the ground table_ground = 0.75 # Set the length, width and height of the table table_size = [0.2, 0.7, 0.01] # Set the length, width and height of the object to attach tool_size = [0.3, 0.02, 0.02] # Create a pose for the tool relative to the end-effector p = PoseStamped() p.header.frame_id = end_effector_link scene.attach_mesh # Place the end of the object within the grasp of the gripper p.pose.position.x = tool_size[0] / 2.0 - 0.025 p.pose.position.y = 0.0 p.pose.position.z = 0.0 # Align the object with the gripper (straight out) p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 # Attach the tool to the end-effector scene.attach_box(end_effector_link, 'tool', p, tool_size) # Add a floating table top table_pose = PoseStamped() table_pose.header.frame_id = 'base_footprint' 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', table_pose, table_size) # Update the current state right_arm.set_start_state_to_current_state() # Move the arm with the attached object to the 'straight_forward' position right_arm.set_named_target('straight_forward') right_arm.go() rospy.sleep(2) # Return the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() rospy.sleep(2) scene.remove_attached_object(end_effector_link, 'tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def simple_function(): rc = RobotCommander() mgc = MoveGroupCommander("manipulator") # print(rc.get_group_names()) # print(rc.get_group('manipulator')) # exit() eef_step = 0.01 jump_threshold = 2 ### Create a handle for the Planning Scene Interface psi = PlanningSceneInterface() rc.get_current_state() rospy.logwarn(rc.get_current_state()) sss.wait_for_input() #rate = rospy.Rate(0.1) # 10hz rate = rospy.Rate(1) # 10hz rospy.sleep(1) theSub = rospy.Subscriber('/attached_collision_object', AttachedCollisionObject, attCollObjCb, queue_size = 1) while not rospy.is_shutdown(): approach_pose = PoseStamped() approach_pose.header.frame_id = "table" approach_pose.header.stamp = rospy.Time(0) approach_pose.pose.position.x = 0.146 approach_pose.pose.position.y = 0.622 approach_pose.pose.position.z = 0.01 quat = tf.transformations.quaternion_from_euler(0, 0, 1.57/2) approach_pose.pose.orientation.x = quat[0] approach_pose.pose.orientation.y = quat[1] approach_pose.pose.orientation.z = quat[2] approach_pose.pose.orientation.w = quat[3] # mgc.set_start_state_to_current_state() # (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True) # if(frac_approach != 1): # rospy.logwarn("Planning did not succeed before adding collision objects") # else: # rospy.logwarn("Planning succeeded before adding collision objects") # # rospy.logwarn("waiting for input to add dummy box") # sss.wait_for_input() # box_dummy_pose = PoseStamped() box_dummy_pose.header.frame_id = "table" box_dummy_pose.pose.position.x = 0.147 box_dummy_pose.pose.position.y = 0.636 box_dummy_pose.pose.position.z = 0 psi.add_box("dummy_box", box_dummy_pose, (0.18,0.09,0.26))# #size x,y,z x is always to the left viewing the robot from the PC # rospy.logwarn("I have added the box") # rospy.sleep(1) # rospy.logwarn("waiting for input to try planning with dummy box") # sss.wait_for_input() # # (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True) # if(frac_approach != 1): # rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)") # else: # rospy.logwarn("Planning succeeded after adding collision objects (dummy box)") # rospy.logwarn("waiting for input to add end effector box box") sss.wait_for_input() # end effector collision object link_attached_to_ef = "ee_link" mb_ef_collisonobj = "metal_bracket" mb_ef_pose = PoseStamped() mb_ef_pose.header.frame_id = link_attached_to_ef mb_ef_pose.pose.position.x = 0.065/2.0 mb_ef_pose.pose.position.y = 0.0 mb_ef_pose.pose.position.z = 0.0 mb_ef_size = (0.065,0.06,0.06) psi.attach_box(link_attached_to_ef, mb_ef_collisonobj, mb_ef_pose, mb_ef_size, [link_attached_to_ef, "wrist_3_link"]) raw_input("0 hi") mgc.attach_object("dummy_box", link_attached_to_ef, [link_attached_to_ef, "wrist_3_link"]) rospy.logwarn("I have added the attached box to the end effector") rospy.sleep(1) raw_input("1 hi") rospy.logwarn(rc.get_current_state()) # mgc.attach_object(object_name, link_name, touch_links) mgc.set_start_state_to_current_state() rospy.logwarn(rc.get_current_state()) raw_input("2 hi") rospy.logwarn("waiting for input to try planning with end effector box") sss.wait_for_input() (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True) if(frac_approach != 1): rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)") else: rospy.logwarn("Planning succeeded after adding collision objects (dummy box)") rospy.logwarn("waiting for input to try planning next loop") sss.wait_for_input() rate.sleep()
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') scene = PlanningSceneInterface() self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) self.colors = dict() rospy.sleep(1) arm = MoveGroupCommander('arm') gripper = MoveGroupCommander('gripper') end_effector_link = arm.get_end_effector_link() arm.set_goal_position_tolerance(0.005) arm.set_goal_orientation_tolerance(0.025) arm.allow_replanning(True) gripper.set_goal_position_tolerance(0.005) gripper.set_goal_orientation_tolerance(0.025) gripper.allow_replanning(True) reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) arm.set_planning_time(5) r_tool_size = [0.03, 0.01, 0.06] l_tool_size = [0.03, 0.01, 0.06] #left gripper l_p = PoseStamped() l_p.header.frame_id = end_effector_link l_p.pose.position.x = 0.00 l_p.pose.position.y = 0.04 l_p.pose.position.z = 0.04 l_p.pose.orientation.w = 1 scene.attach_box(end_effector_link, 'l_tool', l_p, l_tool_size) #right gripper r_p = PoseStamped() r_p.header.frame_id = end_effector_link r_p.pose.position.x = 0.00 r_p.pose.position.y = -0.04 r_p.pose.position.z = 0.04 r_p.pose.orientation.w = 1 scene.attach_box(end_effector_link, 'r_tool', r_p, r_tool_size) grasp_pose = target_pose grasp_pose.pose.position.x -= 0.15 #grasp_pose.pose.position.z= grasp_pose.pose.orientation.x = 0 grasp_pose.pose.orientation.y = 0.707 grasp_pose.pose.orientation.z = 0 grasp_pose.pose.orientation.w = 0.707 arm.set_start_state_to_current_state() arm.set_pose_target(grasp_pose, end_effector_link) traj = arm.plan() arm.execute(traj) rospy.sleep(2) #arm.shift_pose_target(4,1.57,end_effector_link) #arm.go() #rospy.sleep(2) arm.shift_pose_target(0, 0.11, end_effector_link) arm.go() rospy.sleep(2) saved_target_pose = arm.get_current_pose(end_effector_link) #arm.set_named_target("initial_arm2") #grasp scene.attach_box(end_effector_link, target_id, g_p, target_size) rospy.sleep(2) #grasping is over , from now is placing arm.shift_pose_target(2, 0.15, end_effector_link) arm.go() rospy.sleep(2) arm.shift_pose_target(1, -0.2, end_effector_link) arm.go() rospy.sleep(2) #pouring test arm.shift_pose_target(3, 1.57, end_effector_link) rospy.sleep(2) arm.shift_pose_target(3, -1.57, end_effector_link) rospy.sleep(2) #gripper.shift_pose_target(3,1.57,end_effector_link) arm.set_pose_target(saved_target_pose) arm.go() rospy.sleep(2) scene.remove_attached_object(end_effector_link, 'l_tool') scene.remove_attached_object(end_effector_link, 'r_tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_attached_object_demo') # 初始化场景对象 scene = PlanningSceneInterface() rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('manipulator') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() # 设置每次运动规划的时间限制:10s arm.set_planning_time(10) # 移除场景中之前运行残留的物体 scene.remove_attached_object(end_effector_link, 'tool') scene.remove_world_object('table') # 设置桌面的高度 table_ground = 0.7 # 设置table和tool的三维尺寸 table_size = [0.1, 0.3, 0.02] tool_size = [0.2, 0.02, 0.02] # 设置tool的位姿 p = PoseStamped() p.header.frame_id = end_effector_link p.pose.position.x = tool_size[0] / 2.0 p.pose.position.z = -0.015 p.pose.orientation.w = 1 # 将tool附着到机器人的终端 scene.attach_box(end_effector_link, 'tool', p, tool_size) # 将table加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = 'base_link' table_pose.pose.position.x = 0.4 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] table_pose.pose.orientation.w = 1.0 scene.add_box('table', table_pose, table_size) rospy.sleep(2) # 更新当前的位姿 arm.set_start_state_to_current_state() # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) joint_positions = [ -0.5110620856285095, 0.32814791798591614, 0.5454912781715393, 1.0146701335906982, 0.8498637080192566, -0.45695754885673523 ] arm.set_joint_value_target(joint_positions) # 控制机械臂完成运动 arm.go() rospy.sleep(1) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def main(argv): #moveit_commander.roscpp_initialize(sys.argv) rospy.init_node("PickPlaceDemo") rospy.on_shutdown(onshutdownhook) transform = tf.TransformListener() global scene scene = PlanningSceneInterface() robot = RobotCommander() group = MoveGroupCommander("manipulator") rospy.sleep(2) #scene.is_diff = True #t = transform.getLatestCommonTime("/tool0", "/world") #tool0_pose, tool0_quaternion = transform.lookupTransform("world", "tool0", t) quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0) p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = -0.700 p.pose.position.y = 0.000 p.pose.position.z = 0.060 p.pose.orientation.x = quaternion[0] p.pose.orientation.y = quaternion[1] p.pose.orientation.z = quaternion[2] p.pose.orientation.w = quaternion[3] scene.add_box("box1", p, (0.15, 0.15, 0.15)) p.pose.position.x = -0.700 p.pose.position.y = -0.250 p.pose.position.z = 0.060 scene.add_box("box2", p, (0.15, 0.15, 0.15)) p.pose.position.x = -0.700 p.pose.position.y = -0.500 p.pose.position.z = 0.060 scene.add_box("box3", p, (0.15, 0.15, 0.15)) rospy.sleep(5) quaternion = tf.transformations.quaternion_from_euler( 0.0, (math.pi / 2.0), 0.0) pose_target = Pose() pose_target.position.x = -0.700 pose_target.position.y = 0.000 pose_target.position.z = 0.155 pose_target.orientation.x = quaternion[0] pose_target.orientation.y = quaternion[1] pose_target.orientation.z = quaternion[2] pose_target.orientation.w = quaternion[3] group.set_pose_target(pose_target) group.go(wait=True) rospy.sleep(1) scene.attach_box("tool0", "box1") rospy.sleep(1) pose_target.position.x = 0.000 pose_target.position.y = -0.700 pose_target.position.z = 0.155 group.set_pose_target(pose_target) group.go(wait=True) rospy.sleep(1) scene.remove_attached_object("tool0") rospy.sleep(1) pose_target.position.x = -0.700 pose_target.position.y = -0.250 pose_target.position.z = 0.155 group.set_pose_target(pose_target) group.go(wait=True) rospy.sleep(1) scene.attach_box("tool0", "box2") rospy.sleep(1) pose_target.position.x = 0.000 pose_target.position.y = -0.700 pose_target.position.z = 0.306 group.set_pose_target(pose_target) group.go(wait=True) rospy.sleep(1) scene.remove_attached_object("tool0") rospy.sleep(1) pose_target.position.x = -0.700 pose_target.position.y = -0.500 pose_target.position.z = 0.155 group.set_pose_target(pose_target) group.go(wait=True) rospy.sleep(1) scene.attach_box("tool0", "box3") rospy.sleep(1) pose_target.position.x = 0.000 pose_target.position.y = -0.700 pose_target.position.z = 0.457 group.set_pose_target(pose_target) group.go(wait=True) rospy.sleep(1) scene.remove_attached_object("tool0") while not rospy.is_shutdown(): pass
class PlanningSceneHelper: def __init__(self, package=None, mesh_folder_path=None): # interface to the planning and collision scenes self.psi = PlanningSceneInterface() # self.scene_pub = Publisher("/collision_object", CollisionObject, # queue_size=1, latch=True) self.vis_pub = Publisher("/collision_scene", MarkerArray, queue_size=10) self.marker_array = MarkerArray() sleep(1.0) if package is not None and mesh_folder_path is not None: # Build folder path for use in load rospack = RosPack() self.package = package self.mesh_folder_path = mesh_folder_path self.package_path = rospack.get_path(package) self.folder_path = os.path.join(self.package_path, mesh_folder_path)\ + os.sep else: loginfo( "Missing package or mesh folder path supplied to planning_scene_helper; no meshes can be added" ) # Create dict of objects, for removing markers later self.objects = {} self.next_id = 1 # Loads the selected mesh file into collision scene at the desired location. def add_mesh(self, object_id, frame, filename, visual=None, pose=None, position=None, orientation=None, rpy=None, color=None): if self.package is None: logwarn("No package was provided; no meshes can be loaded.") if visual is None: visual = filename filename = self.folder_path + filename stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy) try: self.psi.add_mesh(object_id, stamped_pose, filename) loginfo("Loaded " + object_id + " from" + filename + " as collision object.") marker = self.make_marker_stub(stamped_pose, color=color) marker.type = Marker.MESH_RESOURCE # marker.mesh_resource = "file:/" + self.folder_path + visual marker.mesh_resource = "package://" + self.package + os.sep + self.mesh_folder_path + os.sep + visual self.marker_array = self.make_new_marker_array_msg() self.marker_array.markers.append(marker) self.vis_pub.publish(self.marker_array) self.objects[object_id] = marker loginfo("Loaded " + object_id + " from " + marker.mesh_resource + " as marker.") except ROSException as e: loginfo("Problem loading " + object_id + " collision object: " + str(e)) except AssimpError as ae: loginfo("Problem loading " + object_id + " collision object: " + str(ae)) def add_cylinder(self, object_id, frame, size, pose=None, position=None, orientation=None, rpy=None, color=None): try: stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy) # Cylinders are special - they are not supported directly by # moveit_commander. It must be published manually to collision scene cyl = CollisionObject() cyl.operation = CollisionObject.ADD cyl.id = object_id cyl.header = stamped_pose.header prim = SolidPrimitive() prim.type = SolidPrimitive.CYLINDER prim.dimensions = [size[0], size[1]] cyl.primitives = [prim] cyl.primitive_poses = [stamped_pose.pose] # self.scene_pub.publish(cyl) marker = self.make_marker_stub(stamped_pose, [size[1] * 2, size[2] * 2, size[0]], color=color) marker.type = Marker.CYLINDER self.publish_marker(object_id, marker) sleep(0.5) logdebug("Loaded " + object_id + " as cylindrical collision object.") except ROSException as e: loginfo("Problem loading " + object_id + " collision object: " + str(e)) def add_sphere(self, object_id, frame, size, pose=None, position=None, orientation=None, rpy=None, color=None): try: stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy) self.psi.add_sphere(object_id, stamped_pose, size[0]) loginfo("got past adding collision scene object") marker = self.make_marker_stub( stamped_pose, [size[0] * 2, size[1] * 2, size[2] * 2], color=color) marker.type = Marker.SPHERE self.publish_marker(object_id, marker) sleep(0.5) loginfo("Loaded " + object_id + " as collision object.") except ROSException as e: loginfo("Problem loading " + object_id + " collision object: " + str(e)) def add_box(self, object_id, frame, size, pose=None, position=None, orientation=None, rpy=None, color=None): try: stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy) self.psi.add_box(object_id, stamped_pose, size) marker = self.make_marker_stub(stamped_pose, size, color=color) marker.type = Marker.CUBE self.publish_marker(object_id, marker) sleep(0.5) loginfo("Loaded " + object_id + " as collision object.") except ROSException as e: loginfo("Problem loading " + object_id + " collision object: " + str(e)) def attach_box(self, link, object_id, frame, size, attach_to_link, pose=None, position=None, orientation=None, rpy=None): """TODO: color is not yet supported, since it's not internally supported by psi.attach_box. Basically duplicate this method but with color support.""" try: stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy), self.psi.attach_box(link, object_id, stamped_pose, size, attach_to_link) sleep(0.5) loginfo("Attached " + object_id + " as collision object.") except ROSException as e: loginfo("Problem attaching " + object_id + " collision object: " + str(e)) @staticmethod def make_stamped_pose(frame, pose=None, position=None, orientation=None, rpy=None): if orientation is not None and rpy is not None: logwarn("Collision object has both orientation (quaternion) and " "Rotation (rpy) defined! Defaulting to quaternion " "representation") stamped_pose = PoseStamped() stamped_pose.header.frame_id = frame stamped_pose.header.stamp = Time.now() # use a pose if one is provided, otherwise, make your own from the # position and orientation. if pose is not None: stamped_pose.pose = pose else: stamped_pose.pose.position.x = position[0] stamped_pose.pose.position.y = position[1] stamped_pose.pose.position.z = position[2] # for orientation, allow either quaternion or RPY specification if orientation is not None: stamped_pose.pose.orientation.x = orientation[0] stamped_pose.pose.orientation.y = orientation[1] stamped_pose.pose.orientation.z = orientation[2] stamped_pose.pose.orientation.w = orientation[3] elif rpy is not None: quaternion = transformations.quaternion_from_euler( rpy[0], rpy[1], rpy[2]) stamped_pose.pose.orientation.x = quaternion[0] stamped_pose.pose.orientation.y = quaternion[1] stamped_pose.pose.orientation.z = quaternion[2] stamped_pose.pose.orientation.w = quaternion[3] else: stamped_pose.pose.orientation.w = 1 # make basic quaternion return stamped_pose # Fill a ROS Marker message with the provided data def make_marker_stub(self, stamped_pose, size=None, color=None): if color is None: color = (0.5, 0.5, 0.5, 1.0) if size is None: size = (1, 1, 1) marker = Marker() marker.header = stamped_pose.header # marker.ns = "collision_scene" marker.id = self.next_id marker.lifetime = Time(0) # forever marker.action = Marker.ADD marker.pose = stamped_pose.pose marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] if len(color) == 4: alpha = color[3] else: alpha = 1.0 marker.color.a = alpha marker.scale.x = size[0] marker.scale.y = size[1] marker.scale.z = size[2] self.next_id += 1 return marker def make_new_marker_array_msg(self): ma = MarkerArray() return ma # publish a single marker def publish_marker(self, object_id, marker): loginfo("Publishing marker for object {}".format(object_id)) self.marker_array = self.make_new_marker_array_msg() self.marker_array.markers.append(marker) self.vis_pub.publish(self.marker_array) self.objects[object_id] = marker # Remove the provided object from the world. def remove(self, object_id): # if object_id not in self.objects: # logwarn("PlanningSceneHelper was not used to create object with id " # + object_id + ". Attempting to remove it anyway.") try: # first remove the actual collision object self.psi.remove_world_object(object_id) if object_id in self.objects: # then remove the marker associated with it marker = self.objects[object_id] marker.action = Marker.DELETE self.publish_marker(object_id, marker) self.objects.pop(object_id) logdebug("Marker for collision object " + object_id + " removed.") except ROSException as e: loginfo("Problem removing " + object_id + " from collision scene:" + str(e)) return except KeyError as e: loginfo("Problem removing " + object_id + " from collision scene:" + str(e)) return loginfo("Model " + object_id + " removed from collision scene.") # Remove the provided attached collision object. def remove_attached(self, link, object_id): try: self.psi.remove_attached_object(link=link, name=object_id) loginfo("Attached object '" + object_id + "' removed from collision scene.") except: loginfo("Problem attached object '" + object_id + "' removing from collision scene.")
class MoveitObjectHandler(object): ''' Use this class to create objects that reside in moveit environments ''' def __init__(self): ''' Constructor of the MoveitObjectHandler class. ''' self.planning_scene_interface = PlanningSceneInterface() rospy.loginfo("Connecting to /get_planning_scene service") self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.scene_srv.wait_for_service() rospy.loginfo("Connected.") rospy.loginfo("Connecting to clear octomap service...") self.clear_octomap_srv = rospy.ServiceProxy( '/clear_octomap', Empty) self.clear_octomap_srv.wait_for_service() rospy.loginfo("Connected!") self.scene_objects = [] self.attached_objects = [] def clear_octomap(self): rospy.loginfo("Clearing octomap") self.clear_octomap_srv.call(EmptyRequest()) rospy.sleep(2.0) def add_mesh_object(self, id_name, pose, size = (1, 1, 1), rotation = (0, 0, 0), frame='/world'): pass def add_box_object(self, id_name, pose, size = (1, 1, 1), rotation = (0, 0, 0), frame='/world'): ''' Adds the particular BOX TYPE objects to the moveit planning scene :param id_name: unique id that object should be labeled with :param pose: pose of the object :param size: size of the object :param rotation: rotation offset in r, p, y :param frame: frame in which the object pose is passed :type id_name: string :type pose: list of double of length 7 (x, y, z, q_x, q_y, q_z, q_w) :type size: tuple of length 3 :type frame: string ''' assert type(size) is tuple, 'size should be tuple' assert len(size)==3, 'size should be of length 3' assert not id_name in self.scene_objects, \ 'Object with the same name already exists!' self.scene_objects.append(id_name) object_pose = PoseStamped() object_pose.header.frame_id = frame if type(pose) is list: pose = list_to_pose(pose) (r, p, y) = rotation q_object_rotation = quaternion_from_euler(math.radians(r), math.radians(p), math.radians(y)) q_object = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] q_object_final = quaternion_multiply(q_object, q_object_rotation) x = pose.position.x y = pose.position.y z = pose.position.z object_pose.pose = Pose(Point(x, y, z), Quaternion(*q_object_final)) #Add object description in scene self.clear_octomap() #need to sleep before adding part!! https://answers.ros.org/question/209030/moveit-planningsceneinterface-addbox-not-showing-in-rviz/ self.planning_scene_interface.add_box(id_name, object_pose, size) self.wait_for_planning_scene_object(id_name) def remove_world_object(self, object_name): ''' Removes a specified object from the MoveIt! planning scene :param object_name: unique name for the object :type object_name: string ''' assert object_name is not None, 'Please pass in an object_name for the object!' self.planning_scene_interface.remove_world_object(object_name) self.clear_octomap() def remove_all_objects(self): ''' Removes all the objects in the current MoveIt! planning scene ''' self.clear_octomap() #need to sleep before or else it gets skipped over... self.planning_scene_interface.remove_world_object(None) def attach_gripper_object(self, object_name, arm, gripper_planning_group): ''' Attaches an object to the robot gripper :param name: name of the object in the planning scene :param arm: the Arm object :param gripper_planning_group: the gripper planning group in MoveIt! :type name: string :type arm: Arm :type gripper_planning_group: string ''' object_name = str(object_name) eef_link = arm.get_end_effector_link() touch_links = arm.robot.get_link_names(group=gripper_planning_group) self.clear_octomap() self.planning_scene_interface.attach_box(eef_link, object_name, touch_links=touch_links) def detach_gripper_object(self, object_name, arm, remove_from_world=False): ''' Detaches an object earlier attached to the robot gripper :param name: name of the object in the planning scene :param arm: the Arm object :param remove_from_world: if true, object also deleted from world :type name: string :type arm: Arm :type remove_from_world: bool ''' eef_link = arm.get_end_effector_link() self.planning_scene_interface.remove_attached_object(eef_link, object_name) if remove_from_world is True: self.remove_world_object(object_name) self.clear_octomap() def wait_for_planning_scene_object(self, object_name): ''' Waits for object to appear in planning scene :param object_name: name of the object in the planning scene :type object_name: string ''' rospy.loginfo("Waiting for object '" + object_name + "'' to appear in planning scene...") gps_req = GetPlanningSceneRequest() gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES part_in_scene = False while not rospy.is_shutdown() and not part_in_scene: # This call takes a while when rgbd sensor is set gps_resp = self.scene_srv.call(gps_req) # check if object_name is in the answer for collision_obj in gps_resp.scene.world.collision_objects: if collision_obj.id == object_name: part_in_scene = True break else: rospy.sleep(1.0) rospy.loginfo("'" + object_name + "'' is in scene!")
# Close gripper #------------------------------ robot.get_current_state() print(robot.pincher_arm.get_current_pose()) print(robot.pincher_arm.get_current_rpy()) exit() robot.pincher_arm.set_start_state(RobotState()) fechado = closeGripper() robot.pincher_gripper.set_joint_value_target(fechado) gplan = robot.pincher_gripper.plan() robot.pincher_gripper.execute(gplan) #with closed gripper, attach the box to it rospy.sleep(1) scene.attach_box("gripper_link", "box", p, [0.03, 0.03, 0.03]) rospy.sleep(1) # ----------------------------- # Go to some pose #------------------------------ robot.get_current_state() robot.pincher_arm.set_start_state(RobotState()) target = [0.17, 0.10, 0.028] plan = ef_pose(target, robot.pincher_arm) if plan is not None: robot.pincher_arm.execute(plan) print(plan) else: print('No trajectory found')
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)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_attached_object_demo') # 初始化场景对象 scene = PlanningSceneInterface() rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('manipulator') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() # 设置每次运动规划的时间限制:10s arm.set_planning_time(10) # 移除场景中之前运行残留的物体 scene.remove_attached_object(end_effector_link, 'tool') scene.remove_world_object('table') # 设置桌面的高度 table_ground = 0.6 # 设置table和tool的三维尺寸 table_size = [0.1, 0.3, 0.02] tool_size = [0.2, 0.02, 0.02] # 设置tool的位姿 p = PoseStamped() p.header.frame_id = end_effector_link p.pose.position.x = tool_size[0] / 2.0 p.pose.position.y = 0 p.pose.position.z = -0.015 p.pose.orientation.w = 1 # 将tool附着到机器人的终端 scene.attach_box(end_effector_link, 'tool', p, tool_size) # 将table加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = 'base_link' table_pose.pose.position.x = 0.3 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', table_pose, table_size) rospy.sleep(2) # 更新当前的位姿 arm.set_start_state_to_current_state() # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) joint_positions = [0.827228546495185, 0.29496592875743577, 1.1185644936946095, -0.7987583317769674, -0.18950024740190782, 0.11752152218233858] arm.set_joint_value_target(joint_positions) # 控制机械臂完成运动 arm.go() rospy.sleep(1) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() moveit_commander.roscpp_shutdown() 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) rospy.on_shutdown(self.shutdown) # Construct the initial scene object scene = PlanningSceneInterface() rospy.sleep(1) # Connect to the right_arm move group right_arm = moveit_commander.MoveGroupCommander('arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Display the name of the end_effector link rospy.loginfo("The end effector link is: " + str(end_effector_link)) right_arm.allow_replanning(True) # Set the solve method. #right_arm.set_planner_id('RRTConnectkConfigDefault') # Set a small tolerance on joint angles right_arm.set_goal_joint_tolerance(0.0001) right_arm.set_goal_position_tolerance(0.00001) right_arm.set_goal_orientation_tolerance(0.0001) # Set the position of the mesh file. file = '/home/jyk/catkin_ws/src/kuca/meshes/block.STL' # Set the planning time. right_arm.set_planning_time(5) total = 25 for i in range(total): #scene.remove_attached_object('block' + str(i)) scene.remove_world_object('block' + str(i)) rospy.sleep(1) # Start the arm target in "right" pose stored in the SRDF file right_arm.set_named_target('right') # Plan a trajectory to the goal configuration traj = right_arm.plan() # Execute the planned trajectory right_arm.execute(traj) # Pause for a moment rospy.sleep(1) ########### print right_arm.get_current_joint_values() print '#' * 10 ########### # Make a block in fixed pose q = quaternion_from_euler(0, 0, 1.5707) target_pose = right_arm.get_current_pose() target_pose.pose.position.x -= 0.8 target_pose.pose.position.y += 0.9 target_pose.pose.position.z -= 0.5 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] object_pose = deepcopy(target_pose) refer_place_pose = PoseStamped() refer_place_pose.header.frame_id = 'base_footprint' refer_place_pose.pose.position.x = 0.8 refer_place_pose.pose.position.y = 0.1 refer_place_pose.pose.position.z = 0.6 refer_place_pose.pose.orientation.x = 0 refer_place_pose.pose.orientation.y = 0 refer_place_pose.pose.orientation.z = 0 refer_place_pose.pose.orientation.w = 1 count = 0 while (count < total): # Set the size of block block_size = [0.01, 0.05, 0.05] place_pose = deepcopy(refer_place_pose) place_pose.pose.position.y += 0.051 * (count % 5) place_pose.pose.position.z -= 0.051 * (count // 5) scene.add_box('block' + str(count), object_pose, block_size) rospy.sleep(1) #right_arm.set_joint_value_target(target_pose, end_effector_link, True) target_pose.header.stamp = rospy.Time() right_arm.set_pose_target(target_pose) print target_pose print '&' * 10 right_arm.go() #rospy.sleep(0.5) print right_arm.get_current_pose() print '-' * 30 print right_arm.get_goal_tolerance() # Create a pose for the block relative to the end-effector p = PoseStamped() p.header.frame_id = end_effector_link # Place the end of the object within the grasp of the gripper p.pose.position.x = 0 p.pose.position.y = 0.0 p.pose.position.z = 0.0 # Align the object with the gripper (straight out) p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 # Attach the tool to the end-effector scene.attach_box(end_effector_link, 'block' + str(count), p, block_size) rospy.sleep(1) #right_arm.set_start_state_to_current_state() place_pose.header.stamp = rospy.Time() right_arm.set_pose_target(place_pose) print place_pose right_arm.go() rospy.sleep(0.5) print right_arm.get_current_pose().pose print '*' * 30 right_arm.detach_object(end_effector_link) #scene.remove_attached_object(end_effector_link, 'block'+str(count)) #scene.remove_world_object('block'+str(count)) rospy.sleep(1) count += 1 # Return the arm to the named "resting" pose stored in the SRDF file right_arm.set_named_target('right') right_arm.go() rospy.sleep(1) # Cleanly shut down MoveIt moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class MoveGroupCommandInterpreter(object): """ Interpreter for simple commands """ DEFAULT_FILENAME = "move_group.cfg" GO_DIRS = {"up" : (2,1), "down" : (2, -1), "z" : (2, 1), "left" : (1, 1), "right" : (1, -1), "y" : (1, 1), "forward" : (0, 1), "backward" : (0, -1), "back" : (0, -1), "x" : (0, 1) } def __init__(self): self._gdict = {} self._group_name = "" self._prev_group_name = "" self._planning_scene_interface = PlanningSceneInterface() self._robot = RobotCommander() self._last_plan = None self._db_host = None self._db_port = 33829 self._trace = False def get_active_group(self): if len(self._group_name) > 0: return self._gdict[self._group_name] else: return None def get_loaded_groups(self): return self._gdict.keys() def execute(self, cmd): cmd = self.resolve_command_alias(cmd) result = self.execute_generic_command(cmd) if result != None: return result else: if len(self._group_name) > 0: return self.execute_group_command(self._gdict[self._group_name], cmd) else: return (MoveGroupInfoLevel.INFO, self.get_help() + "\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n") def execute_generic_command(self, cmd): if os.path.isfile("cmd/" + cmd): cmd = "load cmd/" + cmd cmdlow = cmd.lower() if cmdlow.startswith("use"): if cmdlow == "use": return (MoveGroupInfoLevel.INFO, "\n".join(self.get_loaded_groups())) clist = cmd.split() clist[0] = clist[0].lower() if len(clist) == 2: if clist[0] == "use": if clist[1] == "previous": clist[1] = self._prev_group_name if len(clist[1]) == 0: return (MoveGroupInfoLevel.DEBUG, "OK") if self._gdict.has_key(clist[1]): self._prev_group_name = self._group_name self._group_name = clist[1] return (MoveGroupInfoLevel.DEBUG, "OK") else: try: mg = MoveGroupCommander(clist[1]) self._gdict[clist[1]] = mg self._group_name = clist[1] return (MoveGroupInfoLevel.DEBUG, "OK") except MoveItCommanderException as e: return (MoveGroupInfoLevel.FAIL, "Initializing " + clist[1] + ": " + str(e)) except: return (MoveGroupInfoLevel.FAIL, "Unable to initialize " + clist[1]) elif cmdlow.startswith("trace"): if cmdlow == "trace": return (MoveGroupInfoLevel.INFO, "trace is on" if self._trace else "trace is off") clist = cmdlow.split() if clist[0] == "trace" and clist[1] == "on": self._trace = True return (MoveGroupInfoLevel.DEBUG, "OK") if clist[0] == "trace" and clist[1] == "off": self._trace = False return (MoveGroupInfoLevel.DEBUG, "OK") elif cmdlow.startswith("load"): filename = self.DEFAULT_FILENAME clist = cmd.split() if len(clist) > 2: return (MoveGroupInfoLevel.WARN, "Unable to parse load command") if len(clist) == 2: filename = clist[1] if not os.path.isfile(filename): filename = "cmd/" + filename line_num = 0 line_content = "" try: f = open(filename, 'r') for line in f: line_num += 1 line = line.rstrip() line_content = line if self._trace: print ("%s:%d: %s" % (filename, line_num, line_content)) comment = line.find("#") if comment != -1: line = line[0:comment].rstrip() if line != "": self.execute(line.rstrip()) line_content = "" return (MoveGroupInfoLevel.DEBUG, "OK") except MoveItCommanderException as e: if line_num > 0: return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s\n%s" % (filename, line_num, line_content, str(e))) else: return (MoveGroupInfoLevel.WARN, "Processing " + filename + ": " + str(e)) except: if line_num > 0: return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s" % (filename, line_num, line_content)) else: return (MoveGroupInfoLevel.WARN, "Unable to load " + filename) elif cmdlow.startswith("save"): filename = self.DEFAULT_FILENAME clist = cmd.split() if len(clist) > 2: return (MoveGroupInfoLevel.WARN, "Unable to parse save command") if len(clist) == 2: filename = clist[1] try: f = open(filename, 'w') for gr in self._gdict.keys(): f.write("use " + gr + "\n") known = self._gdict[gr].get_remembered_joint_values() for v in known.keys(): f.write(v + " = [" + " ".join([str(x) for x in known[v]]) + "]\n") if self._db_host != None: f.write("database " + self._db_host + " " + str(self._db_port) + "\n") return (MoveGroupInfoLevel.DEBUG, "OK") except: return (MoveGroupInfoLevel.WARN, "Unable to save " + filename) else: return None def execute_group_command(self, g, cmdorig): cmd = cmdorig.lower() if cmd == "stop": g.stop() return (MoveGroupInfoLevel.DEBUG, "OK") if cmd == "id": return (MoveGroupInfoLevel.INFO, g.get_name()) if cmd == "help": return (MoveGroupInfoLevel.INFO, self.get_help()) if cmd == "vars": known = g.get_remembered_joint_values() return (MoveGroupInfoLevel.INFO, "[" + " ".join(known.keys()) + "]") if cmd == "joints": joints = g.get_joints() return (MoveGroupInfoLevel.INFO, "\n" + "\n".join([str(i) + " = " + joints[i] for i in range(len(joints))]) + "\n") if cmd == "show": return self.command_show(g) if cmd == "current": return self.command_current(g) if cmd == "ground": pose = PoseStamped() pose.pose.position.x = 0 pose.pose.position.y = 0 # offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself) pose.pose.position.z = -0.0501 pose.pose.orientation.x = 0 pose.pose.orientation.y = 0 pose.pose.orientation.z = 0 pose.pose.orientation.w = 1 pose.header.stamp = rospy.get_rostime() pose.header.frame_id = self._robot.get_root_link() self._planning_scene_interface.attach_box(self._robot.get_root_link(), "ground", pose, (3, 3, 0.1)) return (MoveGroupInfoLevel.INFO, "Added ground") if cmd == "eef": if len(g.get_end_effector_link()) > 0: return (MoveGroupInfoLevel.INFO, g.get_end_effector_link()) else: return (MoveGroupInfoLevel.INFO, "There is no end effector defined") if cmd == "database": if self._db_host == None: return (MoveGroupInfoLevel.INFO, "Not connected to a database") else: return (MoveGroupInfoLevel.INFO, "Connected to " + self._db_host + ":" + str(self._db_port)) if cmd == "plan": if self._last_plan != None: return (MoveGroupInfoLevel.INFO, str(self._last_plan)) else: return (MoveGroupInfoLevel.INFO, "No previous plan") if cmd == "constrain": g.clear_path_constraints() return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints") if cmd == "tol" or cmd == "tolerance": return (MoveGroupInfoLevel.INFO, "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" % g.get_goal_tolerance()) if cmd == "time": return (MoveGroupInfoLevel.INFO, str(g.get_planning_time())) if cmd == "execute": if self._last_plan != None: if g.execute(self._last_plan): return (MoveGroupInfoLevel.SUCCESS, "Plan submitted for execution") else: return (MoveGroupInfoLevel.WARN, "Failed to submit plan for execution") else: return (MoveGroupInfoLevel.WARN, "No motion plan computed") # see if we have assignment between variables assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd) if assign_match: known = g.get_remembered_joint_values() if known.has_key(assign_match.group(2)): g.remember_joint_values(assign_match.group(1), known[assign_match.group(2)]) return (MoveGroupInfoLevel.SUCCESS, assign_match.group(1) + " is now the same as " + assign_match.group(2)) else: return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") # see if we have assignment of matlab-like vector syntax set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd) if set_match: try: g.remember_joint_values(set_match.group(1), [float(x) for x in set_match.group(2).split()]) return (MoveGroupInfoLevel.SUCCESS, "Remembered joint values [" + set_match.group(2) + "] under the name " + set_match.group(1)) except: return (MoveGroupInfoLevel.WARN, "Unable to parse joint value [" + set_match.group(2) + "]") # see if we have assignment of matlab-like element update component_match = re.match(r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd) if component_match: known = g.get_remembered_joint_values() if known.has_key(component_match.group(1)): try: val = known[component_match.group(1)] val[int(component_match.group(2))] = float(component_match.group(3)) g.remember_joint_values(component_match.group(1), val) return (MoveGroupInfoLevel.SUCCESS, "Updated " + component_match.group(1) + "[" + component_match.group(2) + "]") except: return (MoveGroupInfoLevel.WARN, "Unable to parse index or value in '" + cmd +"'") else: return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") clist = cmdorig.split() clist[0] = clist[0].lower() # if this is an unknown one-word command, it is probably a variable if len(clist) == 1: known = g.get_remembered_joint_values() if known.has_key(cmd): return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[cmd]]) + "]") else: return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") # command with one argument if len(clist) == 2: if clist[0] == "go": self._last_plan = None if clist[1] == "rand" or clist[1] == "random": vals = g.get_random_joint_values() g.set_joint_value_target(vals) if g.go(): return (MoveGroupInfoLevel.SUCCESS, "Moved to random target [" + " ".join([str(x) for x in vals]) + "]") else: return (MoveGroupInfoLevel.FAIL, "Failed while moving to random target [" + " ".join([str(x) for x in vals]) + "]") else: try: g.set_named_target(clist[1]) if g.go(): return (MoveGroupInfoLevel.SUCCESS, "Moved to " + clist[1]) else: return (MoveGroupInfoLevel.FAIL, "Failed while moving to " + clist[1]) except MoveItCommanderException as e: return (MoveGroupInfoLevel.WARN, "Going to " + clist[1] + ": " + str(e)) except: return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown") if clist[0] == "plan": self._last_plan = None vals = None if clist[1] == "rand" or clist[1] == "random": vals = g.get_random_joint_values() g.set_joint_value_target(vals) self._last_plan = g.plan() else: try: g.set_named_target(clist[1]) self._last_plan = g.plan() except MoveItCommanderException as e: return (MoveGroupInfoLevel.WARN, "Planning to " + clist[1] + ": " + str(e)) except: return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown") if self._last_plan != None: if len(self._last_plan.joint_trajectory.points) == 0 and len(self._last_plan.multi_dof_joint_trajectory.points) == 0: self._last_plan = None dest_str = clist[1] if vals != None: dest_str = "[" + " ".join([str(x) for x in vals]) + "]" if self._last_plan != None: return (MoveGroupInfoLevel.SUCCESS, "Planned to " + dest_str) else: return (MoveGroupInfoLevel.FAIL, "Failed while planning to " + dest_str) elif clist[0] == "pick": self._last_plan = None if g.pick(clist[1]): return (MoveGroupInfoLevel.SUCCESS, "Picked object " + clist[1]) else: return (MoveGroupInfoLevel.FAIL, "Failed while trying to pick object " + clist[1]) elif clist[0] == "place": self._last_plan = None if g.place(clist[1]): return (MoveGroupInfoLevel.SUCCESS, "Placed object " + clist[1]) else: return (MoveGroupInfoLevel.FAIL, "Failed while trying to place object " + clist[1]) elif clist[0] == "planner": g.set_planner_id(clist[1]) return (MoveGroupInfoLevel.SUCCESS, "Planner is now " + clist[1]) elif clist[0] == "record" or clist[0] == "rec": g.remember_joint_values(clist[1]) return (MoveGroupInfoLevel.SUCCESS, "Remembered current joint values under the name " + clist[1]) elif clist[0] == "rand" or clist[0] == "random": g.remember_joint_values(clist[1], g.get_random_joint_values()) return (MoveGroupInfoLevel.SUCCESS, "Remembered random joint values under the name " + clist[1]) elif clist[0] == "del" or clist[0] == "delete": g.forget_joint_values(clist[1]) return (MoveGroupInfoLevel.SUCCESS, "Forgot joint values under the name " + clist[1]) elif clist[0] == "show": known = g.get_remembered_joint_values() if known.has_key(clist[1]): return (MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[clist[1]]]) + "]") else: return (MoveGroupInfoLevel.WARN, "Joint values for " + clist[1] + " are not known") elif clist[0] == "tol" or clist[0] == "tolerance": try: g.set_goal_tolerance(float(clist[1])) return (MoveGroupInfoLevel.SUCCESS, "OK") except: return (MoveGroupInfoLevel.WARN, "Unable to parse tolerance value '" + clist[1] + "'") elif clist[0] == "time": try: g.set_planning_time(float(clist[1])) return (MoveGroupInfoLevel.SUCCESS, "OK") except: return (MoveGroupInfoLevel.WARN, "Unable to parse planning duration value '" + clist[1] + "'") elif clist[0] == "constrain": try: g.set_path_constraints(clist[1]) return (MoveGroupInfoLevel.SUCCESS, "OK") except: if self._db_host != None: return (MoveGroupInfoLevel.WARN, "Constraint " + clist[1] + " is not known.") else: return (MoveGroupInfoLevel.WARN, "Not connected to a database.") elif clist[0] == "wait": try: time.sleep(float(clist[1])) return (MoveGroupInfoLevel.SUCCESS, clist[1] + " seconds passed") except: return (MoveGroupInfoLevel.WARN, "Unable to wait '" + clist[1] + "' seconds") elif clist[0] == "database": try: g.set_constraints_database(clist[1], self._db_port) self._db_host = clist[1] return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port)) except: return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + str(self._db_port) + "'") else: return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") if len(clist) == 3: if clist[0] == "go" and self.GO_DIRS.has_key(clist[1]): self._last_plan = None try: offset = float(clist[2]) (axis, factor) = self.GO_DIRS[clist[1]] return self.command_go_offset(g, offset, factor, axis, clist[1]) except MoveItCommanderException as e: return (MoveGroupInfoLevel.WARN, "Going " + clist[1] + ": " + str(e)) except: return (MoveGroupInfoLevel.WARN, "Unable to parse distance '" + clist[2] + "'") elif clist[0] == "allow" and (clist[1] == "look" or clist[1] == "looking"): if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True": g.allow_looking(True) else: g.allow_looking(False) return (MoveGroupInfoLevel.DEBUG, "OK") elif clist[0] == "allow" and (clist[1] == "replan" or clist[1] == "replanning"): if clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True": g.allow_replanning(True) else: g.allow_replanning(False) return (MoveGroupInfoLevel.DEBUG, "OK") elif clist[0] == "database": try: g.set_constraints_database(clist[1], int(clist[2])) self._db_host = clist[1] self._db_port = int(clist[2]) return (MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port)) except: self._db_host = None return (MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + clist[2] + "'") if len(clist) == 4: if clist[0] == "rotate": try: g.set_rpy_target([float(x) for x in clist[1:]]) if g.go(): return (MoveGroupInfoLevel.SUCCESS, "Rotation complete") else: return (MoveGroupInfoLevel.FAIL, "Failed while rotating to " + " ".join(clist[1:])) except MoveItCommanderException as e: return (MoveGroupInfoLevel.WARN, str(e)) except: return (MoveGroupInfoLevel.WARN, "Unable to parse X-Y-Z rotation values '" + " ".join(clist[1:]) + "'") if len(clist) >= 7: if clist[0] == "go": self._last_plan = None approx = False if (len(clist) > 7): if (clist[7] == "approx" or clist[7] == "approximate"): approx = True try: p = Pose() p.position.x = float(clist[1]) p.position.y = float(clist[2]) p.position.z = float(clist[3]) q = tf.transformations.quaternion_from_euler(float(clist[4]), float(clist[5]), float(clist[6])) p.orientation.x = q[0] p.orientation.y = q[1] p.orientation.z = q[2] p.orientation.w = q[3] if approx: g.set_joint_value_target(p, True) else: g.set_pose_target(p) if g.go(): return (MoveGroupInfoLevel.SUCCESS, "Moved to pose target\n%s\n" % (str(p))) else: return (MoveGroupInfoLevel.FAIL, "Failed while moving to pose \n%s\n" % (str(p))) except MoveItCommanderException as e: return (MoveGroupInfoLevel.WARN, "Going to pose target: %s" % (str(e))) except: return (MoveGroupInfoLevel.WARN, "Unable to parse pose '" + " ".join(clist[1:]) + "'") return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") def command_show(self, g): known = g.get_remembered_joint_values() res = [] for k in known.keys(): res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]") return (MoveGroupInfoLevel.INFO, "\n".join(res)) def command_current(self, g): res = "joints = [" + " ".join([str(x) for x in g.get_current_joint_values()]) + "]" if len(g.get_end_effector_link()) > 0: res = res + "\n" + g.get_end_effector_link() + " pose = [\n" + str(g.get_current_pose()) + " ]" res = res + "\n" + g.get_end_effector_link() + " RPY = " + str(g.get_current_rpy()) return (MoveGroupInfoLevel.INFO, res) def command_go_offset(self, g, offset, factor, dimension_index, direction_name): if g.has_end_effector_link(): try: g.shift_pose_target(dimension_index, factor * offset) if g.go(): return (MoveGroupInfoLevel.SUCCESS, "Moved " + direction_name + " by " + str(offset) + " m") else: return (MoveGroupInfoLevel.FAIL, "Failed while moving " + direction_name) except MoveItCommanderException as e: return (MoveGroupInfoLevel.WARN, str(e)) except: return (MoveGroupInfoLevel.WARN, "Unable to process pose update") else: return (MoveGroupInfoLevel.WARN, "No known end effector. Cannot move " + direction_name) def resolve_command_alias(self, cmdorig): cmd = cmdorig.lower() if cmd == "which": return "id" if cmd == "groups": return "use" return cmdorig def get_help(self): res = [] res.append("Known commands:") res.append(" help show this screen") res.append(" id|which display the name of the group that is operated on") res.append(" load [<file>] load a set of interpreted commands from a file") res.append(" save [<file>] save the currently known variables as a set of commands") res.append(" use <name> switch to using the group named <name> (and load it if necessary)") res.append(" use|groups show the group names that are already loaded") res.append(" vars display the names of the known states") res.append(" show display the names and values of the known states") res.append(" show <name> display the value of a state") res.append(" record <name> record the current joint values under the name <name>") res.append(" delete <name> forget the joint values under the name <name>") res.append(" current show the current state of the active group") res.append(" constrain <name> use the constraint <name> as a path constraint") res.append(" constrain clear path constraints") res.append(" eef print the name of the end effector attached to the current group") res.append(" go <name> plan and execute a motion to the state <name>") res.append(" go <dir> <dx>| plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>") res.append(" go rand plan and execute a motion to a random state") res.append(" plan <name> plan a motion to the state <name>") res.append(" execute execute a previously computed motion plan") res.append(" rotate <x> <y> <z> plan and execute a motion to a specified orientation (about the X,Y,Z axes)") res.append(" tolerance show the tolerance for reaching the goal region") res.append(" tolerance <val> set the tolerance for reaching the goal region") res.append(" wait <dt> sleep for <dt> seconds") res.append(" x = y assign the value of y to x") res.append(" x[idx] = val assign a value to dimension idx of x") res.append(" x = [v1 v2...] assign a vector of values to x") res.append(" trace <on|off> enable/disable replanning or looking around") res.append(" ground add a ground plane to the planning scene") res.append(" allow replanning <true|false> enable/disable replanning") res.append(" allow looking <true|false> enable/disable looking around") return "\n".join(res) def get_keywords(self): known_vars = [] known_constr = [] if self.get_active_group() != None: known_vars = self.get_active_group().get_remembered_joint_values().keys() known_constr = self.get_active_group().get_known_constraints() groups = self._robot.get_group_names() return {'go':['up', 'down', 'left', 'right', 'backward', 'forward', 'random'] + known_vars, 'help':[], 'record':known_vars, 'show':known_vars, 'wait':[], 'delete':known_vars, 'database': [], 'current':[], 'use':groups, 'load':[], 'save':[], 'pick':[], 'place':[], 'plan':known_vars, 'allow':['replanning', 'looking'], 'constrain':known_constr, 'vars':[], 'joints':[], 'tolerance':[], 'time':[], 'eef':[], 'execute':[], 'ground':[], 'id':[]}
tool_size = [0.2, 0.02, 0.02] ############################################# # 设置tool的位姿 p = PoseStamped() p.header.frame_id = end_effector_link p.pose.position.x = tool_size[0] / 2.0 - 0.025 p.pose.position.y = -0.015 p.pose.position.z = 0.0 p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 scene.attach_box(end_effector_link, 'tool', p, tool_size) # ############################################ # 将obstacle加入场景当中 obstacle_pose = PoseStamped() obstacle_pose.header.frame_id = 'base_link' obstacle_pose.pose.position.x = 0.35 obstacle_pose.pose.position.y = 0.0 obstacle_pose.pose.position.z = 0.33 obstacle_pose.pose.orientation.w = 1.0 scene.add_box('obstacle', obstacle_pose, obstacle_size) rospy.sleep(2) ############################################ target_pose = PoseStamped()
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) # Construct the initial scene object scene = PlanningSceneInterface() rospy.sleep(1) # Connect to the right_arm move group right_arm = moveit_commander.MoveGroupCommander('arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Display the name of the end_effector link rospy.loginfo("The end effector link is: " + str(end_effector_link)) # Set a small tolerance on joint angles right_arm.set_goal_joint_tolerance(0.001) right_arm.set_goal_position_tolerance(0.0001) scene.remove_world_object('block') # Start the arm target in "contract" pose stored in the SRDF file # right_arm.set_named_target('contract') # right_arm.go() # rospy.sleep(2) # Start the arm target in "right" pose stored in the SRDF file right_arm.set_named_target('right') # Plan a trajectory to the goal configuration traj = right_arm.plan() # Execute the planned trajectory right_arm.execute(traj) # Pause for a moment rospy.sleep(1) start_pose = right_arm.get_current_pose().pose waypoints = [] wpose = deepcopy(start_pose) waypoints.append(deepcopy(wpose)) wpose.position.x -= 0.2 waypoints.append(deepcopy(wpose)) wpose.position.y += 0.3 waypoints.append(deepcopy(wpose)) wpose.position.z -= 0.5 waypoints.append(deepcopy(wpose)) 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.") # Set the size of block block_size = [0.01, 0.05, 0.05] # Create a pose for the block relative to the end-effector p = PoseStamped() p.header.frame_id = end_effector_link # Place the end of the object within the grasp of the gripper p.pose.position.x = -0.015 p.pose.position.y = 0.0 p.pose.position.z = 0.0 # Align the object with the gripper (straight out) p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 # Attach the tool to the end-effector scene.attach_box(end_effector_link, 'block', p, block_size) waypoints = [] start_pose = right_arm.get_current_pose().pose wpose = deepcopy(start_pose) waypoints.append(deepcopy(wpose)) wpose.position.z += 0.25 waypoints.append(deepcopy(wpose)) #wpose.position.y -= 0.30 #waypoints.append(deepcopy(wpose)) 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.") joint_positions = right_arm.get_current_joint_values() joint_positions[0] = -1.5707 right_arm.set_joint_value_target(joint_positions) right_arm.go() rospy.sleep(1) waypoints = [] start_pose = right_arm.get_current_pose().pose wpose = deepcopy(start_pose) waypoints.append(deepcopy(wpose)) wpose.position.z -= 0.25 waypoints.append(deepcopy(wpose)) 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.") scene.remove_attached_object(end_effector_link, 'block') rospy.sleep(1) waypoints = [] start_pose = right_arm.get_current_pose().pose wpose = deepcopy(start_pose) waypoints.append(deepcopy(wpose)) wpose.position.z += 0.15 waypoints.append(deepcopy(wpose)) #wpose.position.y -= 0.30 #waypoints.append(deepcopy(wpose)) 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.") rospy.sleep(1) joint_positions = right_arm.get_current_joint_values() joint_positions[0] = 0 joint_positions[4] = 0 joint_positions[5] = 0 right_arm.set_joint_value_target(joint_positions) right_arm.go() rospy.sleep(1) # Return the arm to the named "resting" pose stored in the SRDF file right_arm.set_named_target('right') right_arm.go() rospy.sleep(1) # Cleanly shut down MoveIt moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class Pick(object): _feedback = moveit_msgs.msg.PickupActionFeedback().feedback _result = moveit_msgs.msg.PickupActionResult().result def __init__(self, name): self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, moveit_msgs.msg.PickupAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() rospy.loginfo('Action Service Loaded') self.robot = Phantomx_Pincher() rospy.loginfo('Moveit Robot Commander Loaded') self.scene = PlanningSceneInterface() rospy.loginfo('Moveit Planning Scene Loaded') rospy.loginfo('Pick action is ok. Awaiting for connections') def get_target(self, target_name): obj = self.scene.get_objects([target_name]) obj = obj[target_name] pose = obj.primitive_poses[0].position target = [pose.x, pose.y, pose.z] size = obj.primitives[0].dimensions rospy.loginfo(target) return size, target def execute_cb(self, goal): r = rospy.Rate(1) sucess = True #self._result.trajectory_start = self.robot.robot.get_current_state() self.robot.set_start_state_to_current_state() self._feedback.state = "Open Gripper" self._as.publish_feedback(self._feedback) self.robot.robot.get_current_state() rospy.loginfo('Open Gripper Plan') plan = self.robot.openGripper() if plan is None: rospy.loginfo("Open Gripper plan failed") self._result.error_code.val = -1 self._as.set_aborted(self._result) sucess = False return None self._result.trajectory_descriptions.append('OpenGripper') self._result.trajectory_stages.append(plan) self._feedback.state = "Opening gripper" rospy.loginfo('Opening gripper') ex_status = self.robot.gripper_execute(plan) if not ex_status: rospy.loginfo("Execution to open gripper failed: [%s]", ex_status) self._result.error_code.val = -4 self._as.set_aborted(self._result) sucess = False return None self._as.publish_feedback(self._feedback) self.scene.remove_attached_object('gripper_link') self._feedback.state = "Planning to reach object" rospy.loginfo('Planning to reach obj') self._as.publish_feedback(self._feedback) dimension, target = self.get_target(goal.target_name) quat = [] if len(goal.possible_grasps) > 0: quat = [ goal.possible_grasps[0].grasp_pose.pose.orientation.x, goal.possible_grasps[0].grasp_pose.pose.orientation.y, goal.possible_grasps[0].grasp_pose.pose.orientation.z, goal.possible_grasps[0].grasp_pose.pose.orientation.w ] rospy.loginfo('Pick Quaternion [%s]', quat) plan = self.robot.ef_pose(target, dimension, orientation=quat) if plan is None: rospy.loginfo("Plan to grasp failed") self._result.error_code.val = -1 self._as.set_aborted(self._result) sucess = False return None self._feedback.state = "Going to the object" rospy.loginfo('Going to Obj') self._as.publish_feedback(self._feedback) self._result.trajectory_descriptions.append( "Going to grasp the object") self._result.trajectory_stages.append(plan) ex_status = self.robot.arm_execute(plan) if not ex_status: rospy.loginfo("Execution to grasp failed: [%s]", ex_status) self._result.error_code.val = -4 self._as.set_aborted(self._result) sucess = False return None # self._feedback.state = "Removing obtect to be grasp from the planning scene" # self._as.publish_feedback(self._feedback) obj = self.scene.get_objects([goal.target_name]) obj = obj[goal.target_name] # self.scene.remove_world_object(goal.target_name)# self._feedback.state = "Attaching object" self._as.publish_feedback(self._feedback) pose = PoseStamped() pose.pose = obj.primitive_poses[0] pose.header = obj.header self.scene.attach_box( "gripper_link", obj.id, pose, obj.primitives[0].dimensions, ['gripper_link', 'gripper_active_link', 'gripper_active2_link']) rospy.sleep(1) rospy.sleep(1) self._feedback.state = "Planning to close the gripper" self._as.publish_feedback(self._feedback) plan = self.robot.closeGripper() if plan is None: rospy.loginfo("Close Gripper plan failed") self._result.error_code.val = -1 self._as.set_aborted(self._result) sucess = False return None self._result.trajectory_descriptions.append('CloseGripper') self._result.trajectory_stages.append(plan) self._feedback.state = "Closing gripper" ex_status = self.robot.gripper_execute(plan) if not ex_status: rospy.loginfo("Execution to grasp failed: [%s]", ex_status) self._result.error_code.val = -4 self._as.set_aborted(self._result) sucess = False return None self._as.publish_feedback(self._feedback) if sucess: self._result.error_code.val = 1 self._as.set_succeeded(self._result)
class PickAndPlace: def setColor(self, name, r, g, b, a=0.9): color = ObjectColor() color.id = name color.color.r = r color.color.g = g color.color.b = b color.color.a = a self.colors[name] = color def sendColors(self): p = PlanningScene() p.is_diff = True for color in self.colors.values(): p.object_colors.append(color) self.scene_pub.publish(p) def add_point(self, traj, time, positions, velocities=None): point = trajectory_msgs.msg.JointTrajectoryPoint() point.positions = copy.deepcopy(positions) if velocities is not None: point.velocities = copy.deepcopy(velocities) point.time_from_start = rospy.Duration(time) traj.points.append(point) def FollowQTraj(self, q_traj, t_traj): assert (len(q_traj) == len(t_traj)) #Insert current position to beginning. if t_traj[0] > 1.0e-2: t_traj.insert(0, 0.0) q_traj.insert(0, self.Q(arm=arm)) self.dq_traj = self.QTrajToDQTraj(q_traj, t_traj) #self.traj= self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, dq_traj) #, dq_traj #print traj #self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, dq_traj)) def QTrajToDQTraj(self, q_traj, t_traj): dof = len(q_traj[0]) #Modeling the trajectory with spline. splines = [TCubicHermiteSpline() for d in range(dof)] for d in range(len(splines)): data_d = [[t, q[d]] for q, t in zip(q_traj, t_traj)] splines[d].Initialize(data_d, tan_method=splines[d].CARDINAL, c=0.0, m=0.0) #NOTE: We don't have to make spline models as we just want velocities at key points. # They can be obtained by computing tan_method, which will be more efficient. with_tan=True dq_traj = [] for t in t_traj: dq = [splines[d].Evaluate(t, with_tan=True)[1] for d in range(dof)] dq_traj.append(dq) #print dq_traj return dq_traj def JointNames(self): #0arm= 0 return self.joint_names[0] def ROSGetJTP(self, q, t, dq=None): jp = trajectory_msgs.msg.JointTrajectoryPoint() jp.positions = q jp.time_from_start = rospy.Duration(t) if dq is not None: jp.velocities = dq return jp def ToROSTrajectory(self, joint_names, q_traj, t_traj, dq_traj=None): assert (len(q_traj) == len(t_traj)) if dq_traj is not None: (len(dq_traj) == len(t_traj)) #traj= trajectory_msgs.msg.JointTrajectory() self.traj.joint_names = joint_names if dq_traj is not None: self.traj.points = [ self.ROSGetJTP(q, t, dq) for q, t, dq in zip(q_traj, t_traj, dq_traj) ] else: self.traj.points = [ self.ROSGetJTP(q, t) for q, t in zip(q_traj, t_traj) ] self.traj.header.stamp = rospy.Time.now() #print self.traj return self.traj def SmoothQTraj(self, q_traj): if len(q_traj) == 0: return q_prev = np.array(q_traj[0]) q_offset = np.array([0] * len(q_prev)) for q in q_traj: q_diff = np.array(q) - q_prev for d in range(len(q_prev)): if q_diff[d] < -math.pi: q_offset[d] += 1 elif q_diff[d] > math.pi: q_offset[d] -= 1 q_prev = copy.deepcopy(q) q[:] = q + q_offset * 2.0 * math.pi def add_target(self, f_target_pose, frame, x, y, o1, o2, o3, o4): f_target_pose.header.frame_id = frame f_target_pose.pose.position.x = x f_target_pose.pose.position.y = y f_target_pose.pose.position.z = self.table_ground + self.table_size[ 2] + self.f_target_size[2] / 2.0 f_target_pose.pose.orientation.x = o1 f_target_pose.pose.orientation.y = o2 f_target_pose.pose.orientation.z = o3 f_target_pose.pose.orientation.w = o4 #self.scene.add_box(f_target_id,f_target_pose,f_target_size) def cts(self, start_pose, end_pose, maxtries, exe_signal=False): waypoints = [] fraction = 0.0 attempts = 0 #maxtries_z=300 waypoints.append(start_pose) waypoints.append(end_pose) while fraction != 1 and attempts < maxtries: (plan, fraction) = self.arm.compute_cartesian_path( waypoints, 0.005, 0.0, True) attempts += 1 if (attempts % maxtries == 0 and fraction != 1): rospy.loginfo("path planning failed with " + str(fraction * 100) + "% success.") signal = False elif fraction == 1: rospy.loginfo("path compute successfully with " + str(attempts) + " attempts.") if exe_signal: self.arm.execute(plan) end_joint_state = plan.joint_trajectory.points[-1].positions signal = True return plan, end_joint_state, signal # shaking function: # freq : shaking freqence # times : shaking time per action def shaking(self, initial_state, start_joint_state, end_joint_state, freq, times): q_traj = [initial_state] t_traj = [0.0] for i in range(times): q_traj.append(end_joint_state) t_traj.append(t_traj[-1] + 0.5 / freq) q_traj.append(start_joint_state) t_traj.append(t_traj[-1] + 0.5 / freq) q_traj.append(initial_state) t_traj.append(t_traj[-1] + 0.5 / freq) self.FollowQTraj(q_traj, t_traj) self.sub_jpc.publish( self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj)) rospy.sleep(6) def setupSence(self): r_tool_size = [0.03, 0.02, 0.18] l_tool_size = [0.03, 0.02, 0.18] #real scene table table_pose = PoseStamped() table_pose.header.frame_id = self.reference_frame table_pose.pose.position.x = -0.184 table_pose.pose.position.y = 0.62 table_pose.pose.position.z = self.table_ground + self.table_size[ 2] / 2.0 table_pose.pose.orientation.w = 1.0 self.scene.add_box(self.table_id, table_pose, self.table_size) #left gripper l_p = PoseStamped() l_p.header.frame_id = self.arm_end_effector_link l_p.pose.position.x = 0.00 l_p.pose.position.y = 0.057 l_p.pose.position.z = 0.09 l_p.pose.orientation.w = 1 self.scene.attach_box(self.arm_end_effector_link, self.l_id, l_p, l_tool_size) #right gripper r_p = PoseStamped() r_p.header.frame_id = self.arm_end_effector_link r_p.pose.position.x = 0.00 r_p.pose.position.y = -0.057 r_p.pose.position.z = 0.09 r_p.pose.orientation.w = 1 self.scene.attach_box(self.arm_end_effector_link, self.r_id, r_p, r_tool_size) def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') self.scene = PlanningSceneInterface() pub_traj = rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory, queue_size=10) self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) self.gripperCtrl = rospy.ServiceProxy( "/two_finger/gripper/gotoPositionUntilTouch", SetPosition) #self.m2j=rospy.Publisher("/two_finger/motoman_control/move_to_joint",JointAnglesDuration,queue_size=1,latch=True) self.colors = dict() rospy.sleep(1) self.arm = MoveGroupCommander('arm') cartesian = rospy.get_param('~cartesian', True) self.arm_end_effector_link = self.arm.get_end_effector_link() self.arm.set_goal_position_tolerance(0.005) self.arm.set_goal_orientation_tolerance(0.025) self.arm.allow_replanning(True) self.reference_frame = 'base_link' self.arm.set_pose_reference_frame(self.reference_frame) self.arm.set_planning_time(5) #shaking self.joint_names = [[]] self.joint_names[0] = rospy.get_param('controller_joint_names') self.traj = trajectory_msgs.msg.JointTrajectory() self.sub_jpc = rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory, queue_size=10) #scene planning self.l_id = 'l_tool' self.r_id = 'r_tool' self.table_id = 'table' self.target1_id = 'target1_object' self.target2_id = 'target2_object' self.target3_id = 'target3_object' self.target4_id = 'target4_object' self.f_target_id = 'receive_container' self.scene.remove_world_object(self.l_id) self.scene.remove_world_object(self.r_id) self.scene.remove_world_object(self.table_id) self.scene.remove_world_object(self.target1_id) self.scene.remove_world_object(self.target2_id) self.scene.remove_world_object(self.target3_id) self.scene.remove_world_object(self.target4_id) #self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id) self.scene.remove_world_object(self.f_target_id) self.table_ground = 0.13 self.table_size = [0.9, 0.6, 0.018] self.setupSence() joint_names = [ 'joint_' + jkey for jkey in ('s', 'l', 'e', 'u', 'r', 'b', 't') ] joint_names = rospy.get_param('controller_joint_names') traj = trajectory_msgs.msg.JointTrajectory() traj.joint_names = joint_names target1_size = [0.058, 0.058, 0.19] target2_size = [0.058, 0.058, 0.19] target3_size = [0.058, 0.058, 0.19] target4_size = [0.058, 0.058, 0.19] self.f_target_size = [0.2, 0.2, 0.04] f_target_pose = PoseStamped() pre_pour_pose = PoseStamped() target1_pose = PoseStamped() target2_pose = PoseStamped() target3_pose = PoseStamped() target4_pose = PoseStamped() #self.target_pose=PoseStamped() #final target #self.add_target(f_target_pose,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1) #self.scene.add_box(self.f_target_id,f_target_pose,f_target_size) #self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1) #pouring pose pour_pose = f_target_pose pour_pose.pose.position.x -= 0.06 pour_pose.pose.position.y -= 0.12 pour_pose.pose.position.z += 0.15 pour_pose.pose.orientation.x = -0.5 pour_pose.pose.orientation.y = -0.5 pour_pose.pose.orientation.z = -0.5 pour_pose.pose.orientation.w = 0.5 #grasp_pose g_p = PoseStamped() g_p.header.frame_id = self.arm_end_effector_link g_p.pose.position.x = 0.00 g_p.pose.position.y = -0.00 g_p.pose.position.z = 0.025 g_p.pose.orientation.w = 0.707 g_p.pose.orientation.x = 0 g_p.pose.orientation.y = -0.707 g_p.pose.orientation.z = 0 #set color self.setColor(self.target1_id, 0.8, 0, 0, 1.0) self.setColor(self.target2_id, 0.8, 0, 0, 1.0) self.setColor(self.target3_id, 0.8, 0, 0, 1.0) self.setColor(self.target4_id, 0.8, 0, 0, 1.0) self.setColor(self.f_target_id, 0.8, 0.4, 0, 1.0) self.setColor('r_tool', 0.8, 0, 0) self.setColor('l_tool', 0.8, 0, 0) self.setColor(self.table_id, 0, 1, 0) self.sendColors() self.gripperCtrl(255) rospy.sleep(3) self.arm.set_named_target("initial_arm") self.arm.go() rospy.sleep(5) j_ori_state = [ -1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156 ] signal = True self.arm.set_joint_value_target(j_ori_state) self.arm.go() rospy.sleep(3) #target localization tar_num = 2 maxtries = 300 #msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped) self.add_target(target1_pose, self.reference_frame, -0.05, 0.6, 0, 0, 0, 1) self.scene.add_box(self.target1_id, target1_pose, target1_size) self.add_target(target2_pose, self.reference_frame, -0.15, 0.55, 0, 0, 0, 1) self.scene.add_box(self.target2_id, target2_pose, target2_size) #self.add_target(target3_pose,self.reference_frame,-0.25y,0,0,0,1) #self.scene.add_box(target3_id,target3_pose,target3_size) #self.add_target(target4_pose,self.reference_frame,x,y,0,0,0,1) #self.scene.add_box(target4_id,target4_pose,target4_size) #path planning #input : target pose #output : policy and execute #cts : output : plan, end_joint_state, signal #global target_pose,target_id,target_size for i in range(tar_num): start_pose = self.arm.get_current_pose( self.arm_end_effector_link).pose if i == 0: target_pose = target1_pose target_id = self.target1_id target_size = target1_size elif i == 1: target_pose = target2_pose target_id = self.target2_id target_size = target2_size elif i == 2: self.target_pose = target3_pose self.target_id = target3_id self.target_size = target3_size elif i == 3: self.target_pose = target4_pose self.target_id = target4_id self.target_size = target4_size #pick up if cartesian: plan_1, end_joint_state_1, signal_1 = self.cts( start_pose, target_pose.pose, maxtries) if signal_1: self.arm.set_joint_value_target(end_joint_state_1) self.arm.go() rospy.sleep(5) self.gripperCtrl(0) rospy.sleep(2) self.scene.attach_box(self.arm_end_effector_link, target_id, g_p, target_size) self.arm.set_joint_value_target(start_pose) self.arm.go() rospy.sleep(5) self.arm.set_joint_value_target(end_joint_state_1) self.arm.go() rospy.sleep(5) self.gripperCtrl(255) self.scene.remove_attached_object( self.arm_end_effector_link, target_id) rospy.sleep(3) self.arm.set_joint_value_target(start_pose) self.arm.go() rospy.sleep(5) """ #move to pouring pose if cartesian: plan_2, end_joint_state_2,signal_2=self.cts(start_pose,target_pose,maxtries) if signal_2: self.arm.set_joint_value_target(end_joint_state_2) self.arm.go() rospy.sleep(5) #scene.remove_attached_object(end_effector_link,target_id) #shaking amp= freq= r_angle= start_pick_pose=self.arm.get_current_pose(self.arm_end_effector_link).pose start_joint_state=self.arm.get_current_joint_values() shift_x_pose=deepcopy(start_pick_pose) shift_z_pose=deepcopy(start_pick_pose) shift_x_pose.position.x+=0.04 shift_z_pose.position.z-=0.04 #exe_sig=False if cartesian: plan_z,end_joint_state_z,signal_z=self.cts(start_pick_pose,shift_z_pose,300) plan_x,end_joint_state_x,signal_x=self.cts(start_pick_pose,shift_x_pose,300) #shaking process #signal_x=False shake_times=0 #self.freq=3.0 signal=True while signal: if (shake_times%3!=0 and signal_x) or (shake_times%3==0 and not signal_z): self.shaking(start_joint_state,end_joint_state_x,3,3) elif shake_times%3==0 and signal_z: self.shaking(start_joint_state,end_joint_state_z,3,1) shake_times+=1 rospy.loginfo("shaking times : "+str(shake_times)+ " .") if shake_times==10: signal=False """ #remove and shut down #self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool') #self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_ik_demo') # 初始化场景对象 scene = PlanningSceneInterface() rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = moveit_commander.MoveGroupCommander('manipulator') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置目标位置所使用的参考坐标系 reference_frame = 'wall' arm.set_pose_reference_frame(reference_frame) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) arm.set_planning_time(10) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.002) arm.set_goal_orientation_tolerance(0.01) # ===========移除场景中之前运行残留的物体================ scene.remove_attached_object(end_effector_link, 'tool') scene.remove_world_object('wall') scene.remove_world_object('floor') scene.remove_world_object('target') #===================在末端添加物体,防止碰撞============= # 设置tool的三维尺寸 tool_size = [0.01, 0.12, 0.07] # 设置tool的位姿 p = PoseStamped() p.header.frame_id = end_effector_link p.pose.position.x = -0.05 p.pose.position.y = 0.079 p.pose.position.z = 0.0 p.pose.orientation.x = -0.5 p.pose.orientation.y = -0.5 p.pose.orientation.z = 0.5 p.pose.orientation.w = 0.5 # 将tool附着到机器人的终端 scene.attach_box(end_effector_link, 'tool', p, tool_size) #======================================================= #===================在场景中添加物体,防止碰撞==================== # 设置三维尺寸 wall_size = [0.001, 2, 2] floor_size = [2, 2, 0.001] # 将wall加入场景当中 wall_pose = PoseStamped() wall_pose.header.frame_id = 'wall' wall_pose.pose.position.x = 0.0 wall_pose.pose.position.y = 0.0 wall_pose.pose.position.z = 0.0 wall_pose.pose.orientation.w = 1.0 scene.add_box('wall', wall_pose, wall_size) rospy.sleep(2) #========================================================= # 设置机械臂工作空间中的目标位姿,位置使用x、y、z坐标描述, # 姿态使用四元数描述,基于base_link坐标系 target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.header.stamp = rospy.Time.now() target_pose.pose = ccd_to_ur5_ik(-0.1,0,0,0) print target_pose.pose # 设置机器臂当前的状态作为运动初始状态 arm.set_start_state_to_current_state() # 设置机械臂终端运动的目标位姿 arm.set_pose_target(target_pose, end_effector_link) # 规划运动路径 traj = arm.plan() # 按照规划的运动路径控制机械臂运动 arm.execute(traj) rospy.sleep(1) #===================================================== #===================================================== # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class Pick(object): _feedback = moveit_msgs.msg.PickupActionFeedback().feedback _result = moveit_msgs.msg.PickupActionResult().result def __init__(self, name): self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, moveit_msgs.msg.PickupAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() rospy.loginfo('Action Service Loaded') self.robot = Phantomx_Pincher() rospy.loginfo('Moveit Robot Commander Loaded') self.scene = PlanningSceneInterface() rospy.loginfo('Moveit Planning Scene Loaded') rospy.loginfo('Pick action is ok. Awaiting for connections') def get_target(self, target_name): obj = self.scene.get_objects([target_name]) obj = obj[target_name] pose = obj.primitive_poses[0].position target = [0.17, 0.005, 0.019] rospy.loginfo(target) target = [pose.x, pose.y, pose.z] rospy.loginfo(target) return target def execute_cb(self, goal): r = rospy.Rate(1) sucess = True #self._result.trajectory_start = self.robot.robot.get_current_state() self.robot.set_start_state_to_current_state() self._feedback.state = "Open Gripper" self._as.publish_feedback(self._feedback) self.robot.robot.get_current_state() plan = self.robot.openGripper() if plan is None: rospy.loginfo("Open Gripper plan failed") self._as.set_preempted() self._result.error_code.val = -1 sucess = False return None self._result.trajectory_descriptions.append('OpenGripper') self._result.trajectory_stages.append(plan) self._feedback.state = "Opening gripper" print self._feedback self.robot.gripper_execute(plan) self._as.publish_feedback(self._feedback) rospy.sleep(1) self._feedback.state = "Planning to reach object" self._as.publish_feedback(self._feedback) target = self.get_target(goal.target_name) plan = self.robot.ef_pose(target) if plan is None: rospy.loginfo("Plan to grasp failed") self._as.set_preempted() self._result.error_code.val = -1 sucess = False return None self._feedback.state = "Going to the object" self._as.publish_feedback(self._feedback) self._result.trajectory_descriptions.append( "Going to grasp the object") self._result.trajectory_stages.append(plan) self.robot.arm_execute(plan) rospy.sleep(7) self._feedback.state = "Removing obtect to be grasp from the planning scene" self._as.publish_feedback(self._feedback) obj = self.scene.get_objects([goal.target_name]) obj = obj[goal.target_name] self.scene.remove_world_object(goal.target_name) rospy.sleep(1) self._feedback.state = "Planning to close the gripper" self._as.publish_feedback(self._feedback) plan = self.robot.closeGripper() if plan is None: rospy.loginfo("Close Gripper plan failed") self._as.set_preempted() self._result.error_code.val = -1 sucess = False return None self._result.trajectory_descriptions.append('CloseGripper') self._result.trajectory_stages.append(plan) self._feedback.state = "Closing gripper" print self._feedback self.robot.gripper_execute(plan) rospy.sleep(1) self._as.publish_feedback(self._feedback) self._feedback.state = "Attaching object" pose = PoseStamped() pose.pose = obj.primitive_poses[0] pose.header = obj.header self.scene.attach_box( "gripper_link", obj.id, pose, obj.primitives[0].dimensions, ['gripper_link', 'gripper_active_link', 'gripper_active2_link']) self._as.publish_feedback(self._feedback) if sucess: self._result.error_code.val = 1 rospy.loginfo(self._result) self._as.set_succeeded(self._result)
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() # Pause for the scene to get ready rospy.sleep(1) # Initialize the MoveIt! commander for the left arm left_arm = MoveGroupCommander('left_arm') # Initialize the MoveIt! commander for the gripper left_gripper = MoveGroupCommander('left_gripper') # Get the name of the end-effector link end_eef = left_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) left_arm.set_goal_position_tolerance(0.01) left_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution left_arm.allow_replanning(True) # Allow 5 seconds per planning attempt left_arm.set_planning_time(5) # Remove leftover objects from a previous run scene.remove_attached_object(end_eef, 'tool') scene.remove_world_object('table') scene.remove_world_object('box1') scene.remove_world_object('box2') scene.remove_world_object('target') # Set the height of the table off the ground table_ground = 0.3 # Set the length, width and height of the table table_size = [0.2, 0.7, 0.01] # Set the length, width and height of the object to attach tool_size = [0.3, 0.02, 0.02] # Create a pose for the tool relative to the end-effector p = PoseStamped() p.header.frame_id = end_eef scene.attach_mesh # Place the end of the object within the grasp of the gripper p.pose.position.x = tool_size[0] / 2.0 - 0.025 p.pose.position.y = 0.0 p.pose.position.z = 0.0 # Align the object with the gripper (straight out) p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 # Attach the tool to the end-effector scene.attach_box(end_eef, 'tool', p, tool_size) # Add a floating table top table_pose = PoseStamped() table_pose.header.frame_id = 'base' 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', table_pose, table_size) # Update the current state left_arm.set_start_state_to_current_state() # Move the arm with the attached object to the 'straight_forward' position left_arm.set_named_target('left_ready') left_arm.go() rospy.sleep(2) # Return the arm in the "left_ready" pose stored in the SRDF file left_arm.set_named_target('left_arm_zero') left_arm.go() rospy.sleep(2) scene.remove_attached_object(end_eef, 'tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') rospy.Subscriber('/aruco_single/pose', PoseStamped, self.pose_cb,queue_size=1) scene=PlanningSceneInterface() self.scene_pub=rospy.Publisher('planning_scene',PlanningScene) self.colors=dict() rospy.sleep(1) arm=MoveGroupCommander('arm') #gripper=MoveGroupCommander('gripper') end_effector_link=arm.get_end_effector_link() arm.set_goal_position_tolerance(0.005) arm.set_goal_orientation_tolerance(0.025) arm.allow_replanning(True) #gripper.set_goal_position_tolerance(0.005) #gripper.set_goal_orientation_tolerance(0.025) #gripper.allow_replanning(True) reference_frame='base_link' arm.set_pose_reference_frame(reference_frame) arm.set_planning_time(5) #scene planning table_id='table' #cylinder_id='cylinder' box2_id='box2' target_id='target_object' #scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(table_id) scene.remove_world_object(target_id) rospy.sleep(2) table_ground=0.59 table_size=[0.5,1,0.01] #box1_size=[0.1,0.05,0.03] box2_size=[0.15,0.15,0.02] r_tool_size=[0.05,0.04,0.22] l_tool_size=[0.05,0.04,0.22] target_size=[0.05,0.05,0.1] table_pose=PoseStamped() table_pose.header.frame_id=reference_frame table_pose.pose.position.x=0.7 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.7 box1_pose.pose.position.y=-0.2 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.55 box2_pose.pose.position.y=-0.12 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) target_pose=PoseStamped() target_pose.header.frame_id=reference_frame target_pose.pose.position.x=0.58 target_pose.pose.position.y=0.05 target_pose.pose.position.z=table_ground+table_size[2]+target_size[2]/2.0 target_pose.pose.orientation.x=0 target_pose.pose.orientation.y=0 target_pose.pose.orientation.z=0 target_pose.pose.orientation.w=1 scene.add_box(target_id,target_pose,target_size) #left gripper l_p=PoseStamped() l_p.header.frame_id=end_effector_link l_p.pose.position.x=0.00 l_p.pose.position.y=0.06 l_p.pose.position.z=0.11 l_p.pose.orientation.w=1 scene.attach_box(end_effector_link,'l_tool',l_p,l_tool_size) #right gripper r_p=PoseStamped() r_p.header.frame_id=end_effector_link r_p.pose.position.x=0.00 r_p.pose.position.y= -0.06 r_p.pose.position.z=0.11 r_p.pose.orientation.w=1 scene.attach_box(end_effector_link,'r_tool',r_p,r_tool_size) #grasp g_p=PoseStamped() g_p.header.frame_id=end_effector_link g_p.pose.position.x=0.00 g_p.pose.position.y= -0.00 g_p.pose.position.z=0.025 g_p.pose.orientation.w=0.707 g_p.pose.orientation.x=0 g_p.pose.orientation.y=-0.707 g_p.pose.orientation.z=0 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) self.setColor('r_tool',0.8,0,0) self.setColor('l_tool',0.8,0,0) self.setColor('target_object',0,1,0) self.sendColors() #motion planning arm.set_named_target("initial_arm") arm.go() rospy.sleep(2) grasp_pose=target_pose grasp_pose.pose.position.x-=0.15 #grasp_pose.pose.position.z= grasp_pose.pose.orientation.x=0 grasp_pose.pose.orientation.y=0.707 grasp_pose.pose.orientation.z=0 grasp_pose.pose.orientation.w=0.707 #arm.set_start_state_to_current_state() ''' arm.set_pose_target(grasp_pose,end_effector_link) traj=arm.plan() arm.execute(traj) print arm.get_current_joint_values() ''' pre_joint_state=[0.16588150906995922, 1.7060146047438647, -0.00961761728757362, 1.8614674591892713, -2.9556667436476847, 1.7432451233907822, 3.1415] arm.set_joint_value_target(pre_joint_state) traj=arm.plan() arm.execute(traj) rospy.sleep(2) arm.shift_pose_target(0,0.09,end_effector_link) arm.go() rospy.sleep(2) scene.attach_box(end_effector_link,target_id,g_p,target_size) rospy.sleep(2) #grasping is over , from now is pouring arm.shift_pose_target(2,0.15,end_effector_link) arm.go() rospy.sleep(2) joint_state_1=arm.get_current_joint_values() joint_state_1[0]-=0.17 arm.set_joint_value_target(joint_state_1) arm.go() rospy.sleep(1) joint_state_2=arm.get_current_joint_values() joint_state_2[6]-=1.8 arm.set_joint_value_target(joint_state_2) arm.go() rospy.sleep(1) #print arm.get_current_joint_values() #pouring test for i in range(1,5): joint_state_2[6]+=0.087 arm.set_joint_value_target(joint_state_2) arm.go() time.sleep(0.05) joint_state_2[6]-=0.087 arm.set_joint_value_target(joint_state_2) arm.go() time.sleep(0.05) print i joint_state_2[6]+=1.8 arm.set_joint_value_target(joint_state_2) arm.go() rospy.sleep(2) joint_state_1[0]+=0.17 arm.set_joint_value_target(joint_state_1) arm.go() rospy.sleep(1) arm.shift_pose_target(2,-0.15,end_effector_link) arm.go() rospy.sleep(2) scene.remove_attached_object(end_effector_link,target_id) rospy.sleep(2) arm.set_named_target("initial_arm") arm.go() rospy.sleep(2) #remove and shut down scene.remove_attached_object(end_effector_link,'l_tool') rospy.sleep(1) scene.remove_attached_object(end_effector_link,'r_tool') rospy.sleep(1) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def init(): global marker_array_pub, marker_pub, tf_broadcaster, tf_listener global move_group, turntable global camera_mesh, camera_pos, camera_size, min_distance, max_distance, num_positions, num_spins, object_size, photobox_pos, photobox_size, reach, simulation, test, turntable_pos, working_dir rospy.init_node('acquisition') camera_mesh = rospy.get_param('~camera_mesh', None) camera_orientation = rospy.get_param('~camera_orientation', None) camera_pos = rospy.get_param('~camera_pos', [0.0, 0.0, 0.0]) camera_size = rospy.get_param('~camera_size', [0.1, 0.1, 0.1]) min_distance = rospy.get_param('~min_distance', 0.2) max_distance = rospy.get_param('~max_distance', min_distance) max_velocity = rospy.get_param('~max_velocity', 0.1) num_positions = rospy.get_param('~num_positions', 15) num_spins = rospy.get_param('~num_spins', 8) object_size = numpy.array(rospy.get_param('~object_size', [0.2, 0.2, 0.2])) photobox_pos = rospy.get_param('~photobox_pos', [0.0, -0.6, 0.0]) photobox_size = rospy.get_param('~photobox_size', [0.7, 0.7, 1.0]) ptpip = rospy.get_param('~ptpip', None) reach = rospy.get_param('~reach', 0.85) simulation = '/gazebo' in get_node_names() test = rospy.get_param('~test', True) turntable_pos = rospy.get_param( '~turntable_pos', photobox_pos[:2] + [photobox_pos[2] + 0.05]) turntable_radius = rospy.get_param('~turntable_radius', 0.2) wall_thickness = rospy.get_param('~wall_thickness', 0.04) marker_array_pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=1, latch=True) marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=1, latch=True) tf_broadcaster = tf.TransformBroadcaster() tf_listener = tf.TransformListener() move_group = MoveGroupCommander('manipulator') move_group.set_max_acceleration_scaling_factor( 1.0 if simulation else max_velocity) move_group.set_max_velocity_scaling_factor( 1.0 if simulation else max_velocity) robot = RobotCommander() scene = PlanningSceneInterface(synchronous=True) try: st_control = load_source( 'st_control', os.path.join(RosPack().get_path('iai_scanning_table'), 'scripts', 'iai_scanning_table', 'st_control.py')) turntable = st_control.ElmoUdp() if turntable.check_device(): turntable.configure() turntable.reset_encoder() turntable.start_controller() turntable.set_speed_deg(30) except Exception as e: print(e) sys.exit('Could not connect to turntable.') if simulation: move_home() elif not test: working_dir = rospy.get_param('~working_dir', None) if working_dir is None: sys.exit('Working directory not specified.') elif not camera.init( os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'out', working_dir, 'images'), ptpip): sys.exit('Could not initialize camera.') # add ground plane ps = PoseStamped() ps.header.frame_id = robot.get_planning_frame() scene.add_plane('ground', ps) # add photobox ps.pose.position.x = photobox_pos[ 0] + photobox_size[0] / 2 + wall_thickness / 2 ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_left', ps, (wall_thickness, photobox_size[1], photobox_size[2])) ps.pose.position.x = photobox_pos[ 0] - photobox_size[0] / 2 - wall_thickness / 2 ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_right', ps, (wall_thickness, photobox_size[1], photobox_size[2])) ps.pose.position.x = photobox_pos[0] ps.pose.position.y = photobox_pos[ 1] - photobox_size[1] / 2 - wall_thickness / 2 ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_back', ps, (photobox_size[0], wall_thickness, photobox_size[2])) ps.pose.position.x = photobox_pos[0] ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] - wall_thickness / 2 scene.add_box('box_ground', ps, (photobox_size[0], photobox_size[1], wall_thickness)) # add turntable turntable_height = turntable_pos[2] - photobox_pos[2] ps.pose.position.x = turntable_pos[0] ps.pose.position.y = turntable_pos[1] ps.pose.position.z = photobox_pos[2] + turntable_height / 2 scene.add_cylinder('turntable', ps, turntable_height, turntable_radius) # add object on turntable ps.pose.position.x = turntable_pos[0] ps.pose.position.y = turntable_pos[1] ps.pose.position.z = turntable_pos[2] + object_size[2] / 2 scene.add_cylinder('object', ps, object_size[2], max(object_size[:2]) / 2) # add cable mounts scene.remove_attached_object('upper_arm_link', 'upper_arm_cable_mount') scene.remove_attached_object('forearm_link', 'forearm_cable_mount') scene.remove_world_object('upper_arm_cable_mount') scene.remove_world_object('forearm_cable_mount') if ptpip is None and not simulation: size = [0.08, 0.08, 0.08] ps.header.frame_id = 'upper_arm_link' ps.pose.position.x = -0.13 ps.pose.position.y = -0.095 ps.pose.position.z = 0.135 scene.attach_box(ps.header.frame_id, 'upper_arm_cable_mount', ps, size) ps.header.frame_id = 'forearm_link' ps.pose.position.x = -0.275 ps.pose.position.y = -0.08 ps.pose.position.z = 0.02 scene.attach_box(ps.header.frame_id, 'forearm_cable_mount', ps, size) # add camera eef_link = move_group.get_end_effector_link() ps = PoseStamped() ps.header.frame_id = eef_link scene.remove_attached_object(eef_link, 'camera_0') scene.remove_attached_object(eef_link, 'camera_1') scene.remove_world_object('camera_0') scene.remove_world_object('camera_1') ps.pose.position.y = camera_pos[1] ps.pose.position.z = camera_pos[2] if camera_mesh: ps.pose.position.x = camera_pos[0] quaternion = tf.transformations.quaternion_from_euler( math.radians(camera_orientation[0]), math.radians(camera_orientation[1]), math.radians(camera_orientation[2])) ps.pose.orientation.x = quaternion[0] ps.pose.orientation.y = quaternion[1] ps.pose.orientation.z = quaternion[2] ps.pose.orientation.w = quaternion[3] scene.attach_mesh(eef_link, 'camera_0', ps, os.path.expanduser(camera_mesh), camera_size) if not simulation: scene.attach_mesh(eef_link, 'camera_1', ps, os.path.expanduser(camera_mesh), numpy.array(camera_size) * 1.5) vertices = scene.get_attached_objects( ['camera_0'])['camera_0'].object.meshes[0].vertices camera_size[0] = max(vertice.x for vertice in vertices) - min( vertice.x for vertice in vertices) camera_size[1] = max(vertice.y for vertice in vertices) - min( vertice.y for vertice in vertices) camera_size[2] = max(vertice.z for vertice in vertices) - min( vertice.z for vertice in vertices) else: ps.pose.position.x = camera_pos[0] + camera_size[0] / 2 scene.attach_box(eef_link, 'camera_0', ps, camera_size)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # 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=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # 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.05) right_arm.set_goal_orientation_tolerance(0.1) # 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 5 seconds per planning attempt right_arm.set_planning_time(60) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # 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) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "grasp" pose stored in the SRDF file right_arm.set_named_target('right_arm_up') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_OPEN) right_gripper.go() rospy.sleep(5) # Set the height of the table off the ground table_ground = 0.04 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] target_x = 0.135 #target_y = -0.32 target_y = -0.285290879999 # 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.25 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) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = target_x target_pose.pose.position.y = target_y target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table blue and the boxes orange self.setColor(table_id, 0, 0, 0.8, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object right_arm.set_support_surface_name(table_id) # 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.18 place_pose.pose.position.y = 0 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 p = PoseStamped() p.header.frame_id = "up1_footprint" p.pose.position.x = 0.12792118579 p.pose.position.y = -0.285290879999 p.pose.position.z = 0.120301181892 p.pose.orientation.x = 0.0 p.pose.orientation.y = 0.0 p.pose.orientation.z = -0.706825181105 p.pose.orientation.w = 0.707388269167 right_gripper.set_pose_target(p.pose) # pick an object right_arm.allow_replanning(True) right_arm.allow_looking(True) right_arm.set_goal_tolerance(0.05) right_arm.set_planning_time(60) print "arm grasp" success = 0 attempt = 0 while not success: p_plan = right_arm.plan() attempt = attempt + 1 print "Planning attempt: " + str(attempt) if p_plan.joint_trajectory.points != []: success = 1 print "arm grasp" right_arm.execute(p_plan) rospy.sleep(5) right_gripper.set_joint_value_target(GRIPPER_GRASP) right_gripper.go() print "gripper closed" rospy.sleep(5) scene.attach_box(GRIPPER_FRAME, target_id) print "object attached" right_arm.set_named_target('right_arm_up') right_arm.go() print "arm up" rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(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) self.left_arm = MoveGroupCommander('left_arm') # Initialize the MoveIt! commander for the gripper self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.left_gripper = MoveGroupCommander('left_gripper') # eel = len(self.right_arm.get_end_effector_link()) # print eel # 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) ### Attach / Remove Object Flag ### self.aro = None # Run and keep in the BG the scene generator with ctrl^c kill ### timerThread = threading.Thread(target=self.scene_generator) timerThread.daemon = True timerThread.start() ## Give some time to ensure the thread starts!! ## rospy.sleep(5) ### GENERATE THE BLACKLIST AND REMOVE ATTACHED OBJECTS FROM PREVIOUS RUNS ### self.idx_list = self.bl() ### GIVE SCENE TIME TO CATCH UP ### rospy.sleep(5) ################################## GRASP EXECUTION ##################################### print "==================== Executing ===========================" start_time = time.time() ### PERSONAL REMINDER!!! WHAT IS WHAT!!! ### # print obj_id[obj_id.index('target')] # print obj_id.index('target') ### MOVE LEFT ARM OUT OF THE WAY ### self.lasp() success = False while success == False and len(self.idx_list)>0: success, pgr_target = self.grasp_attempt() print ("GA Returns:", success) if success is not False: self.flag = 0 # To let the planning scene know when to remove the object self.post_grasp(pgr_target, obj_id.index('target'),'true') self.place_object(obj_id.index('target')) break else: idx = self.idx_list[0] ds, pgr_col_obj = self.declutter_scene(idx) print ("DS Returns:", ds) if ds == True: self.flag = 0 # To let the planning scene know when to remove the object self.post_grasp(pgr_col_obj, obj_id.index(obj_id[idx]),'true') self.place_object(obj_id.index(obj_id[idx])) self.idx_list.pop(0) print "==================== THE END! ======================" print("--- %s seconds ---" % (time.time() - start_time)) rospy.sleep(5) # # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # # Exit the script moveit_commander.os._exit(0) ################################################################# FUNCTIONS ################################################################################# def grasp_attempt(self): # start_time = time.time() retreat = None init_poses = [] grasp_poses = [] for axis in range(0,6): # while obj_id[obj_id.index('target')] is not 'target': # print '!!!!!' # rospy.sleep(0.05) pg = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'pg') gp = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'gp') init_poses.append(pg) grasp_poses.append(gp) pre_grasps = self.grasp_generator(init_poses) grasps = self.grasp_generator(grasp_poses) for grasp in grasps: self.gripper_pose_pub.publish(grasp) rospy.sleep(0.05) success = False i = 1 for pg, gr in izip(pre_grasps, grasps): self.gripper_pose_pub.publish(gr) print ("G Attempt: ", i) plp = self.right_arm.plan(pg.pose) if len(plp.joint_trajectory.points) == 0: print "No valid pregrasp Position, continuing on next one" i+=1 continue i+=1 self.right_arm.plan(pg.pose) self.right_arm.go(wait=True) rospy.sleep(5) plg = self.right_arm.plan(gr.pose) if len(plg.joint_trajectory.points) >= 10: self.right_arm.go() success = True retreat = gr print "Grasping" break # print("--- %s seconds ---" % (time.time() - start_time)) return success , retreat def declutter_scene(self,index): retreat = None init_poses = [] grasp_poses = [] for axis in range(0,6): pg = self.grasp_pose(obj_pose[index], axis, 'pg') gp = self.grasp_pose(obj_pose[index], axis, 'gp') init_poses.append(pg) grasp_poses.append(gp) pre_grasps = self.grasp_generator(init_poses) grasps = self.grasp_generator(grasp_poses) for grasp in grasps: self.gripper_pose_pub.publish(grasp) rospy.sleep(0.05) success = False i= 1 for pg, gr in izip(pre_grasps, grasps): plp = self.right_arm.plan(pg.pose) print (" DS Attempt: ", i) self.gripper_pose_pub.publish(gr) self.right_arm.plan(pg.pose) if len(plp.joint_trajectory.points) == 0: print "No valid pregrasp Position, continuing on next one" i+=1 continue i+=1 self.right_arm.plan(pg.pose) self.right_arm.go() rospy.sleep(5) plg = self.right_arm.plan(gr.pose) if len(plg.joint_trajectory.points) >= 10: self.right_arm.go() print "Grasping" success = True retreat = gr break return success, retreat def place_object(self, obj_idx): self.aro = obj_idx ### GENERATE PLACE POSES ### places = self.place_generator() ### TRY THESE POSES ### i = 1 for place in places: print (" Place Attempt: ", i) plpl = self.right_arm.plan(place.pose) print len(plpl.joint_trajectory.points) if len(plpl.joint_trajectory.points) == 0: i+=1 continue self.right_arm.plan(plpl) self.right_arm.go(wait=True) ### INFORM SCENE ### # self.open_gripper() # self.aro = None ### RETURN HAND TO STARTING POSITION ### self.post_grasp(place,obj_idx, 'false') self.rasp() break def post_grasp(self,new_pose, obj_idx, fl): ######### GRASP OBJECT/ REMOVE FROM SCENE ######. if fl == 'true': self.close_gripper() self.aro = obj_idx else: self.open_gripper() self.aro = None rospy.sleep(2) ### POST GRASP RETREAT ### M1 = transformations.quaternion_matrix([new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z, new_pose.pose.orientation.w]) M1[0,3] = new_pose.pose.position.x M1[1,3] = new_pose.pose.position.y M1[2,3] = new_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.25 # about z T1 = np.dot(M2, M1) npo = deepcopy(new_pose) npo.pose.position.x = T1[0,3] npo.pose.position.y = T1[1,3] npo.pose.position.z = T1[2,3] quat = transformations.quaternion_from_matrix(T1) npo.pose.orientation.x = quat[0] npo.pose.orientation.y = quat[1] npo.pose.orientation.z = quat[2] npo.pose.orientation.w = quat[3] npo.header.frame_id = REFERENCE_FRAME self.right_arm.plan(npo.pose) self.right_arm.go(wait=True) def place_generator(self): place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.57 place_pose.pose.position.y = 0.16 place_pose.pose.position.z = 0.56 place_pose.pose.orientation.w = 1.0 P = transformations.quaternion_matrix([place_pose.pose.orientation.x, place_pose.pose.orientation.y, place_pose.pose.orientation.z, place_pose.pose.orientation.w]) P[0,3] = place_pose.pose.position.x P[1,3] = place_pose.pose.position.y P[2,3] = place_pose.pose.position.z places =[] yaw_angles = [0, 1,57, -1,57 , 3,14] x_vals = [0, 0.05 ,0.1 , 0.15] z_vals = [0.05 ,0.1 , 0.15] for y in yaw_angles: G = transformations.euler_matrix(0, 0, y) G[0,3] = 0.0 # offset about x G[1,3] = 0.0 # about y G[2,3] = 0.0 # about z for z in z_vals: for x in x_vals: TM = np.dot(P, G) pl = deepcopy(place_pose) pl.pose.position.x = TM[0,3] +x pl.pose.position.y = TM[1,3] pl.pose.position.z = TM[2,3] +z quat = transformations.quaternion_from_matrix(TM) pl.pose.orientation.x = quat[0] pl.pose.orientation.y = quat[1] pl.pose.orientation.z = quat[2] pl.pose.orientation.w = quat[3] pl.header.frame_id = REFERENCE_FRAME places.append(deepcopy(pl)) return places def grasp_pose(self, target_pose, axis, stage): ############ TODO : GENERATE AUTOMATED PRE-GRASPING POSITIONS BASED ON THE PRIMITIVE ######### if axis == 0: 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, 0, 0) if stage == 'pg': M2[0,3] = -0.25 # offset about x elif stage == 'gp': M2[0,3] = -0.18 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.0 # about z elif axis == 1: 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, 0, 1.57) M2[0,3] = 0.0 # offset about x if stage == 'pg': M2[1,3] = -0.25 # about y elif stage == 'gp': M2[1,3] = -0.18 # about y M2[2,3] = 0.0 # about z elif axis == 2: 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, 0, -1.57) M2[0,3] = 0.0 # offset about x if stage == 'pg': M2[1,3] = 0.25 # about y elif stage == 'gp': M2[1,3] = 0.18 # about y M2[2,3] = 0.0 # about z elif axis == 3: 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, 0, 3.14) if stage == 'pg': M2[0,3] = 0.25 # offset about x elif stage == 'gp': M2[0,3] = 0.18 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.0 # about z elif axis == 4: 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 if stage == 'pg': M2[2,3] = 0.30 # about z elif stage == 'gp': M2[2,3] = 0.23 # about z else: 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(1.57, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y if stage == 'pg': M2[2,3] = 0.30 # about z elif stage == 'gp': M2[2,3] = 0.23 # about z T1 = np.dot(M1, M2) grasp_pose = deepcopy(target_pose) grasp_pose.pose.position.x = T1[0,3] grasp_pose.pose.position.y = T1[1,3] grasp_pose.pose.position.z = T1[2,3] quat = transformations.quaternion_from_matrix(T1) grasp_pose.pose.orientation.x = quat[0] grasp_pose.pose.orientation.y = quat[1] grasp_pose.pose.orientation.z = quat[2] grasp_pose.pose.orientation.w = quat[3] grasp_pose.header.frame_id = REFERENCE_FRAME return grasp_pose def grasp_generator(self, initial_poses): # A list to hold the grasps grasps = [] o = [] # Original Pose of the object (o) O=[] i= 0 while i < len(initial_poses): o.append(initial_poses[i]) i+=1 G = transformations.euler_matrix(0, 0, 0) # Generate a grasps for along z axis (x and y directions) k = 0 while k <= 5: O.append(transformations.quaternion_matrix([o[k].pose.orientation.x, o[k].pose.orientation.y, o[k].pose.orientation.z, o[k].pose.orientation.w])) O[k][0,3] = o[k].pose.position.x O[k][1,3] = o[k].pose.position.y O[k][2,3] = o[k].pose.position.z if k in range(0,4): for z in self.drange(0.005-obj_size[obj_id.index('target')][2]/2,-0.005 + obj_size[obj_id.index('target')][2]/2, 0.02): ### TODO: USE EACH OBJECTS SIZE NOT ONLY THE TARGETS ### # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] grasp.pose.position.y = T[1,3] grasp.pose.position.z = T[2,3] +z quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) elif k == 4: for x in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02): # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] +x grasp.pose.position.y = T[1,3] grasp.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) else: for y in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02): # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] grasp.pose.position.y = T[1,3] +y grasp.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) k+=1 print len(grasps) # Return the list return grasps def scene_generator(self): while True: # print "happening" obj_pose =[] obj_id = [] obj_size = [] bl = ['ground_plane','pr2'] global obj_pose, obj_id , obj_size ops = PoseStamped() ops.header.frame_id = REFERENCE_FRAME for model_name in self.pwh.name: if model_name not in bl: obj_id.append(model_name) ops.pose = self.pwh.pose[self.pwh.name.index(model_name)] obj_pose.append(deepcopy(ops)) obj_size.append([0.05, 0.05, 0.15]) # obj_id[obj_id.index('custom_1')] = 'target' obj_size[obj_id.index('custom_2')] = [0.05, 0.05, 0.10] obj_size[obj_id.index('custom_3')] = [0.05, 0.05, 0.05] obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03] if self.aro is None: for i in range(0, len(obj_id)): ### CREATE THE SCENE ### self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i]) self.setColor(obj_id[i], 1, 0.623, 0, 1.0) ### Make the target purple and table green ### self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0) self.setColor(obj_id[obj_id.index('custom_table')], 0.3, 1, 0.3, 1.0) self.scene.remove_attached_object(GRIPPER_FRAME) # Send the colors to the planning scene self.sendColors() else: if self.flag == 0: 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', 'r_wrist_roll_link', 'r_upper_arm_link'] #print touch_links self.scene.attach_box(GRIPPER_FRAME, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links) ### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ### self.scene.remove_world_object(obj_id[self.aro]) self.flag +=1 time.sleep(0.5) def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg def bl(self): blist = ['target','custom_2','custom_3', 'custom_table'] self.blist = [] for name in obj_id: if name not in blist: self.blist.append(obj_id.index(name)) # Remove any attached objects from a previous session self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index(name)]) self.scene.remove_attached_object(GRIPPER_FRAME, 'target') return self.blist def drange(self, start, stop, step): r = start while r < stop: yield r r += step def close_gripper(self): g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.044, 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.0899, 100)) self.ac.send_goal(g_open) self.ac.wait_for_result() rospy.sleep(5) # And up to 20 to detach it # 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) def lasp(self): sp = PoseStamped() sp.header.frame_id = REFERENCE_FRAME sp.pose.position.x = 0.3665 sp.pose.position.y = 0.74094 sp.pose.position.z = 1.1449 sp.pose.orientation.x = 0.80503 sp.pose.orientation.y = -0.18319 sp.pose.orientation.z = 0.31988 sp.pose.orientation.w = 0.46481 self.left_arm.plan(sp) self.left_arm.go(wait=True) def rasp(self): sp = PoseStamped() sp.header.frame_id = REFERENCE_FRAME sp.pose.position.x = 0.39571 sp.pose.position.y = -0.40201 sp.pose.position.z = 1.1128 sp.pose.orientation.x =0.00044829 sp.pose.orientation.y = 0.57956 sp.pose.orientation.z = 9.4878e-05 sp.pose.orientation.w = 0.81493 self.right_arm.plan(sp) self.right_arm.go(wait=True)
class MoveGroupCommandInterpreter(object): """ Interpreter for simple commands """ DEFAULT_FILENAME = "move_group.cfg" GO_DIRS = { "up": (2, 1), "down": (2, -1), "z": (2, 1), "left": (1, 1), "right": (1, -1), "y": (1, 1), "forward": (0, 1), "backward": (0, -1), "back": (0, -1), "x": (0, 1), } def __init__(self): self._gdict = {} self._group_name = "" self._prev_group_name = "" self._planning_scene_interface = PlanningSceneInterface() self._robot = RobotCommander() self._last_plan = None self._db_host = None self._db_port = 33829 self._trace = False def get_active_group(self): if len(self._group_name) > 0: return self._gdict[self._group_name] else: return None def get_loaded_groups(self): return self._gdict.keys() def execute(self, cmd): cmd = self.resolve_command_alias(cmd) result = self.execute_generic_command(cmd) if result != None: return result else: if len(self._group_name) > 0: return self.execute_group_command( self._gdict[self._group_name], cmd) else: return ( MoveGroupInfoLevel.INFO, self.get_help() + "\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n", ) def execute_generic_command(self, cmd): if os.path.isfile("cmd/" + cmd): cmd = "load cmd/" + cmd cmdlow = cmd.lower() if cmdlow.startswith("use"): if cmdlow == "use": return (MoveGroupInfoLevel.INFO, "\n".join(self.get_loaded_groups())) clist = cmd.split() clist[0] = clist[0].lower() if len(clist) == 2: if clist[0] == "use": if clist[1] == "previous": clist[1] = self._prev_group_name if len(clist[1]) == 0: return (MoveGroupInfoLevel.DEBUG, "OK") if clist[1] in self._gdict: self._prev_group_name = self._group_name self._group_name = clist[1] return (MoveGroupInfoLevel.DEBUG, "OK") else: try: mg = MoveGroupCommander(clist[1]) self._gdict[clist[1]] = mg self._group_name = clist[1] return (MoveGroupInfoLevel.DEBUG, "OK") except MoveItCommanderException as e: return ( MoveGroupInfoLevel.FAIL, "Initializing " + clist[1] + ": " + str(e), ) except: return ( MoveGroupInfoLevel.FAIL, "Unable to initialize " + clist[1], ) elif cmdlow.startswith("trace"): if cmdlow == "trace": return ( MoveGroupInfoLevel.INFO, "trace is on" if self._trace else "trace is off", ) clist = cmdlow.split() if clist[0] == "trace" and clist[1] == "on": self._trace = True return (MoveGroupInfoLevel.DEBUG, "OK") if clist[0] == "trace" and clist[1] == "off": self._trace = False return (MoveGroupInfoLevel.DEBUG, "OK") elif cmdlow.startswith("load"): filename = self.DEFAULT_FILENAME clist = cmd.split() if len(clist) > 2: return (MoveGroupInfoLevel.WARN, "Unable to parse load command") if len(clist) == 2: filename = clist[1] if not os.path.isfile(filename): filename = "cmd/" + filename line_num = 0 line_content = "" try: f = open(filename, "r") for line in f: line_num += 1 line = line.rstrip() line_content = line if self._trace: print("%s:%d: %s" % (filename, line_num, line_content)) comment = line.find("#") if comment != -1: line = line[0:comment].rstrip() if line != "": self.execute(line.rstrip()) line_content = "" return (MoveGroupInfoLevel.DEBUG, "OK") except MoveItCommanderException as e: if line_num > 0: return ( MoveGroupInfoLevel.WARN, "Error at %s:%d: %s\n%s" % (filename, line_num, line_content, str(e)), ) else: return ( MoveGroupInfoLevel.WARN, "Processing " + filename + ": " + str(e), ) except: if line_num > 0: return ( MoveGroupInfoLevel.WARN, "Error at %s:%d: %s" % (filename, line_num, line_content), ) else: return (MoveGroupInfoLevel.WARN, "Unable to load " + filename) elif cmdlow.startswith("save"): filename = self.DEFAULT_FILENAME clist = cmd.split() if len(clist) > 2: return (MoveGroupInfoLevel.WARN, "Unable to parse save command") if len(clist) == 2: filename = clist[1] try: f = open(filename, "w") for gr in self._gdict.keys(): f.write("use " + gr + "\n") known = self._gdict[gr].get_remembered_joint_values() for v in known.keys(): f.write(v + " = [" + " ".join([str(x) for x in known[v]]) + "]\n") if self._db_host != None: f.write("database " + self._db_host + " " + str(self._db_port) + "\n") return (MoveGroupInfoLevel.DEBUG, "OK") except: return (MoveGroupInfoLevel.WARN, "Unable to save " + filename) else: return None def execute_group_command(self, g, cmdorig): cmd = cmdorig.lower() if cmd == "stop": g.stop() return (MoveGroupInfoLevel.DEBUG, "OK") if cmd == "id": return (MoveGroupInfoLevel.INFO, g.get_name()) if cmd == "help": return (MoveGroupInfoLevel.INFO, self.get_help()) if cmd == "vars": known = g.get_remembered_joint_values() return (MoveGroupInfoLevel.INFO, "[" + " ".join(known.keys()) + "]") if cmd == "joints": joints = g.get_joints() return ( MoveGroupInfoLevel.INFO, "\n" + "\n".join( [str(i) + " = " + joints[i] for i in range(len(joints))]) + "\n", ) if cmd == "show": return self.command_show(g) if cmd == "current": return self.command_current(g) if cmd == "ground": pose = PoseStamped() pose.pose.position.x = 0 pose.pose.position.y = 0 # offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself) pose.pose.position.z = -0.0501 pose.pose.orientation.x = 0 pose.pose.orientation.y = 0 pose.pose.orientation.z = 0 pose.pose.orientation.w = 1 pose.header.stamp = rospy.get_rostime() pose.header.frame_id = self._robot.get_root_link() self._planning_scene_interface.attach_box( self._robot.get_root_link(), "ground", pose, (3, 3, 0.1)) return (MoveGroupInfoLevel.INFO, "Added ground") if cmd == "eef": if len(g.get_end_effector_link()) > 0: return (MoveGroupInfoLevel.INFO, g.get_end_effector_link()) else: return (MoveGroupInfoLevel.INFO, "There is no end effector defined") if cmd == "database": if self._db_host == None: return (MoveGroupInfoLevel.INFO, "Not connected to a database") else: return ( MoveGroupInfoLevel.INFO, "Connected to " + self._db_host + ":" + str(self._db_port), ) if cmd == "plan": if self._last_plan != None: return (MoveGroupInfoLevel.INFO, str(self._last_plan)) else: return (MoveGroupInfoLevel.INFO, "No previous plan") if cmd == "constrain": g.clear_path_constraints() return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints") if cmd == "tol" or cmd == "tolerance": return ( MoveGroupInfoLevel.INFO, "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" % g.get_goal_tolerance(), ) if cmd == "time": return (MoveGroupInfoLevel.INFO, str(g.get_planning_time())) if cmd == "execute": if self._last_plan != None: if g.execute(self._last_plan): return (MoveGroupInfoLevel.SUCCESS, "Plan submitted for execution") else: return ( MoveGroupInfoLevel.WARN, "Failed to submit plan for execution", ) else: return (MoveGroupInfoLevel.WARN, "No motion plan computed") # see if we have assignment between variables assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd) if assign_match: known = g.get_remembered_joint_values() if assign_match.group(2) in known: g.remember_joint_values(assign_match.group(1), known[assign_match.group(2)]) return ( MoveGroupInfoLevel.SUCCESS, assign_match.group(1) + " is now the same as " + assign_match.group(2), ) else: return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") # see if we have assignment of matlab-like vector syntax set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd) if set_match: try: g.remember_joint_values( set_match.group(1), [float(x) for x in set_match.group(2).split()]) return ( MoveGroupInfoLevel.SUCCESS, "Remembered joint values [" + set_match.group(2) + "] under the name " + set_match.group(1), ) except: return ( MoveGroupInfoLevel.WARN, "Unable to parse joint value [" + set_match.group(2) + "]", ) # see if we have assignment of matlab-like element update component_match = re.match( r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd) if component_match: known = g.get_remembered_joint_values() if component_match.group(1) in known: try: val = known[component_match.group(1)] val[int(component_match.group(2))] = float( component_match.group(3)) g.remember_joint_values(component_match.group(1), val) return ( MoveGroupInfoLevel.SUCCESS, "Updated " + component_match.group(1) + "[" + component_match.group(2) + "]", ) except: return ( MoveGroupInfoLevel.WARN, "Unable to parse index or value in '" + cmd + "'", ) else: return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") clist = cmdorig.split() clist[0] = clist[0].lower() # if this is an unknown one-word command, it is probably a variable if len(clist) == 1: known = g.get_remembered_joint_values() if cmd in known: return ( MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[cmd]]) + "]", ) else: return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") # command with one argument if len(clist) == 2: if clist[0] == "go": self._last_plan = None if clist[1] == "rand" or clist[1] == "random": vals = g.get_random_joint_values() g.set_joint_value_target(vals) if g.go(): return ( MoveGroupInfoLevel.SUCCESS, "Moved to random target [" + " ".join([str(x) for x in vals]) + "]", ) else: return ( MoveGroupInfoLevel.FAIL, "Failed while moving to random target [" + " ".join([str(x) for x in vals]) + "]", ) else: try: g.set_named_target(clist[1]) if g.go(): return (MoveGroupInfoLevel.SUCCESS, "Moved to " + clist[1]) else: return ( MoveGroupInfoLevel.FAIL, "Failed while moving to " + clist[1], ) except MoveItCommanderException as e: return ( MoveGroupInfoLevel.WARN, "Going to " + clist[1] + ": " + str(e), ) except: return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown") if clist[0] == "plan": self._last_plan = None vals = None if clist[1] == "rand" or clist[1] == "random": vals = g.get_random_joint_values() g.set_joint_value_target(vals) self._last_plan = g.plan()[1] # The trajectory msg else: try: g.set_named_target(clist[1]) self._last_plan = g.plan()[1] # The trajectory msg except MoveItCommanderException as e: return ( MoveGroupInfoLevel.WARN, "Planning to " + clist[1] + ": " + str(e), ) except: return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown") if self._last_plan != None: if (len(self._last_plan.joint_trajectory.points) == 0 and len(self._last_plan.multi_dof_joint_trajectory. points) == 0): self._last_plan = None dest_str = clist[1] if vals != None: dest_str = "[" + " ".join([str(x) for x in vals]) + "]" if self._last_plan != None: return (MoveGroupInfoLevel.SUCCESS, "Planned to " + dest_str) else: return ( MoveGroupInfoLevel.FAIL, "Failed while planning to " + dest_str, ) elif clist[0] == "pick": self._last_plan = None if g.pick(clist[1]): return (MoveGroupInfoLevel.SUCCESS, "Picked object " + clist[1]) else: return ( MoveGroupInfoLevel.FAIL, "Failed while trying to pick object " + clist[1], ) elif clist[0] == "place": self._last_plan = None if g.place(clist[1]): return (MoveGroupInfoLevel.SUCCESS, "Placed object " + clist[1]) else: return ( MoveGroupInfoLevel.FAIL, "Failed while trying to place object " + clist[1], ) elif clist[0] == "planner": g.set_planner_id(clist[1]) return (MoveGroupInfoLevel.SUCCESS, "Planner is now " + clist[1]) elif clist[0] == "record" or clist[0] == "rec": g.remember_joint_values(clist[1]) return ( MoveGroupInfoLevel.SUCCESS, "Remembered current joint values under the name " + clist[1], ) elif clist[0] == "rand" or clist[0] == "random": g.remember_joint_values(clist[1], g.get_random_joint_values()) return ( MoveGroupInfoLevel.SUCCESS, "Remembered random joint values under the name " + clist[1], ) elif clist[0] == "del" or clist[0] == "delete": g.forget_joint_values(clist[1]) return ( MoveGroupInfoLevel.SUCCESS, "Forgot joint values under the name " + clist[1], ) elif clist[0] == "show": known = g.get_remembered_joint_values() if clist[1] in known: return ( MoveGroupInfoLevel.INFO, "[" + " ".join([str(x) for x in known[clist[1]]]) + "]", ) else: return ( MoveGroupInfoLevel.WARN, "Joint values for " + clist[1] + " are not known", ) elif clist[0] == "tol" or clist[0] == "tolerance": try: g.set_goal_tolerance(float(clist[1])) return (MoveGroupInfoLevel.SUCCESS, "OK") except: return ( MoveGroupInfoLevel.WARN, "Unable to parse tolerance value '" + clist[1] + "'", ) elif clist[0] == "time": try: g.set_planning_time(float(clist[1])) return (MoveGroupInfoLevel.SUCCESS, "OK") except: return ( MoveGroupInfoLevel.WARN, "Unable to parse planning duration value '" + clist[1] + "'", ) elif clist[0] == "constrain": try: g.set_path_constraints(clist[1]) return (MoveGroupInfoLevel.SUCCESS, "OK") except: if self._db_host != None: return ( MoveGroupInfoLevel.WARN, "Constraint " + clist[1] + " is not known.", ) else: return (MoveGroupInfoLevel.WARN, "Not connected to a database.") elif clist[0] == "wait": try: time.sleep(float(clist[1])) return (MoveGroupInfoLevel.SUCCESS, clist[1] + " seconds passed") except: return ( MoveGroupInfoLevel.WARN, "Unable to wait '" + clist[1] + "' seconds", ) elif clist[0] == "database": try: g.set_constraints_database(clist[1], self._db_port) self._db_host = clist[1] return ( MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port), ) except: return ( MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + str(self._db_port) + "'", ) else: return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") if len(clist) == 3: if clist[0] == "go" and clist[1] in self.GO_DIRS: self._last_plan = None try: offset = float(clist[2]) (axis, factor) = self.GO_DIRS[clist[1]] return self.command_go_offset(g, offset, factor, axis, clist[1]) except MoveItCommanderException as e: return ( MoveGroupInfoLevel.WARN, "Going " + clist[1] + ": " + str(e), ) except: return ( MoveGroupInfoLevel.WARN, "Unable to parse distance '" + clist[2] + "'", ) elif clist[0] == "allow" and (clist[1] == "look" or clist[1] == "looking"): if (clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True"): g.allow_looking(True) else: g.allow_looking(False) return (MoveGroupInfoLevel.DEBUG, "OK") elif clist[0] == "allow" and (clist[1] == "replan" or clist[1] == "replanning"): if (clist[2] == "1" or clist[2] == "true" or clist[2] == "T" or clist[2] == "True"): g.allow_replanning(True) else: g.allow_replanning(False) return (MoveGroupInfoLevel.DEBUG, "OK") elif clist[0] == "database": try: g.set_constraints_database(clist[1], int(clist[2])) self._db_host = clist[1] self._db_port = int(clist[2]) return ( MoveGroupInfoLevel.SUCCESS, "Connected to " + self._db_host + ":" + str(self._db_port), ) except: self._db_host = None return ( MoveGroupInfoLevel.WARN, "Unable to connect to '" + clist[1] + ":" + clist[2] + "'", ) if len(clist) == 4: if clist[0] == "rotate": try: g.set_rpy_target([float(x) for x in clist[1:]]) if g.go(): return (MoveGroupInfoLevel.SUCCESS, "Rotation complete") else: return ( MoveGroupInfoLevel.FAIL, "Failed while rotating to " + " ".join(clist[1:]), ) except MoveItCommanderException as e: return (MoveGroupInfoLevel.WARN, str(e)) except: return ( MoveGroupInfoLevel.WARN, "Unable to parse X-Y-Z rotation values '" + " ".join(clist[1:]) + "'", ) if len(clist) >= 7: if clist[0] == "go": self._last_plan = None approx = False if len(clist) > 7: if clist[7] == "approx" or clist[7] == "approximate": approx = True try: p = Pose() p.position.x = float(clist[1]) p.position.y = float(clist[2]) p.position.z = float(clist[3]) q = tf.transformations.quaternion_from_euler( float(clist[4]), float(clist[5]), float(clist[6])) p.orientation.x = q[0] p.orientation.y = q[1] p.orientation.z = q[2] p.orientation.w = q[3] if approx: g.set_joint_value_target(p, True) else: g.set_pose_target(p) if g.go(): return ( MoveGroupInfoLevel.SUCCESS, "Moved to pose target\n%s\n" % (str(p)), ) else: return ( MoveGroupInfoLevel.FAIL, "Failed while moving to pose \n%s\n" % (str(p)), ) except MoveItCommanderException as e: return ( MoveGroupInfoLevel.WARN, "Going to pose target: %s" % (str(e)), ) except: return ( MoveGroupInfoLevel.WARN, "Unable to parse pose '" + " ".join(clist[1:]) + "'", ) return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") def command_show(self, g): known = g.get_remembered_joint_values() res = [] for k in known.keys(): res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]") return (MoveGroupInfoLevel.INFO, "\n".join(res)) def command_current(self, g): res = ("joints = [" + ", ".join([str(x) for x in g.get_current_joint_values()]) + "]") if len(g.get_end_effector_link()) > 0: res = (res + "\n\n" + g.get_end_effector_link() + " pose = [\n" + str(g.get_current_pose()) + " ]") res = (res + "\n" + g.get_end_effector_link() + " RPY = " + str(g.get_current_rpy())) return (MoveGroupInfoLevel.INFO, res) def command_go_offset(self, g, offset, factor, dimension_index, direction_name): if g.has_end_effector_link(): try: g.shift_pose_target(dimension_index, factor * offset) if g.go(): return ( MoveGroupInfoLevel.SUCCESS, "Moved " + direction_name + " by " + str(offset) + " m", ) else: return ( MoveGroupInfoLevel.FAIL, "Failed while moving " + direction_name, ) except MoveItCommanderException as e: return (MoveGroupInfoLevel.WARN, str(e)) except: return (MoveGroupInfoLevel.WARN, "Unable to process pose update") else: return ( MoveGroupInfoLevel.WARN, "No known end effector. Cannot move " + direction_name, ) def resolve_command_alias(self, cmdorig): cmd = cmdorig.lower() if cmd == "which": return "id" if cmd == "groups": return "use" return cmdorig def get_help(self): res = [] res.append("Known commands:") res.append(" help show this screen") res.append( " allow looking <true|false> enable/disable looking around") res.append( " allow replanning <true|false> enable/disable replanning") res.append(" constrain clear path constraints") res.append( " constrain <name> use the constraint <name> as a path constraint" ) res.append( " current show the current state of the active group") res.append( " database display the current database connection (if any)" ) res.append( " delete <name> forget the joint values under the name <name>" ) res.append( " eef print the name of the end effector attached to the current group" ) res.append( " execute execute a previously computed motion plan") res.append( " go <name> plan and execute a motion to the state <name>" ) res.append( " go rand plan and execute a motion to a random state" ) res.append( " go <dir> <dx>| plan and execute a motion in direction up|down|left|right|forward|backward for distance <dx>" ) res.append( " ground add a ground plane to the planning scene") res.append( " id|which display the name of the group that is operated on" ) res.append( " joints display names of the joints in the active group" ) res.append( " load [<file>] load a set of interpreted commands from a file" ) res.append(" pick <name> pick up object <name>") res.append(" place <name> place object <name>") res.append(" plan <name> plan a motion to the state <name>") res.append(" plan rand plan a motion to a random state") res.append( " planner <name> use planner <name> to plan next motion") res.append( " record <name> record the current joint values under the name <name>" ) res.append( " rotate <x> <y> <z> plan and execute a motion to a specified orientation (about the X,Y,Z axes)" ) res.append( " save [<file>] save the currently known variables as a set of commands" ) res.append( " show display the names and values of the known states" ) res.append(" show <name> display the value of a state") res.append(" stop stop the active group") res.append( " time show the configured allowed planning time") res.append(" time <val> set the allowed planning time") res.append( " tolerance show the tolerance for reaching the goal region" ) res.append( " tolerance <val> set the tolerance for reaching the goal region" ) res.append( " trace <on|off> enable/disable replanning or looking around" ) res.append( " use <name> switch to using the group named <name> (and load it if necessary)" ) res.append( " use|groups show the group names that are already loaded" ) res.append( " vars display the names of the known states") res.append(" wait <dt> sleep for <dt> seconds") res.append(" x = y assign the value of y to x") res.append(" x = [v1 v2...] assign a vector of values to x") res.append( " x[idx] = val assign a value to dimension idx of x") return "\n".join(res) def get_keywords(self): known_vars = [] known_constr = [] if self.get_active_group() != None: known_vars = self.get_active_group().get_remembered_joint_values( ).keys() known_constr = self.get_active_group().get_known_constraints() groups = self._robot.get_group_names() return { "go": ["up", "down", "left", "right", "backward", "forward", "random"] + list(known_vars), "help": [], "record": known_vars, "show": known_vars, "wait": [], "delete": known_vars, "database": [], "current": [], "use": groups, "load": [], "save": [], "pick": [], "place": [], "plan": known_vars, "allow": ["replanning", "looking"], "constrain": known_constr, "vars": [], "joints": [], "tolerance": [], "time": [], "eef": [], "execute": [], "ground": [], "id": [], }
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() 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 both_arms = moveit_commander.MoveGroupCommander('both_arms') right_arm = moveit_commander.MoveGroupCommander('right_arm') left_arm = moveit_commander.MoveGroupCommander('left_arm') left_gripper = MoveGroupCommander('left_gripper') # Get the name of the end-effector link right_eef = right_arm.get_end_effector_link() left_eef = left_arm.get_end_effector_link() print "============ Printing end-effector link" print right_eef print left_eef # Set the reference frame for pose targets right_reference_frame = right_arm.get_planning_frame() left_reference_frame = left_arm.get_planning_frame() # Set the right arm reference frame accordingly #right_arm.set_pose_reference_frame(reference_frame) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) right_arm.set_goal_orientation_tolerance(0.01) left_arm.allow_replanning(True) # Allow some leeway in position (meters) and orientation (radians) left_arm.set_goal_position_tolerance(0.01) left_arm.set_goal_orientation_tolerance(0.01) # Allow 5 seconds per planning attempt left_arm.set_planning_time(5) right_arm.set_planning_time(5) object1_id = 'object1' # Remove leftover objects from a previous run scene.remove_world_object(object1_id) rospy.sleep(2) right_arm.set_named_target('right_ready') right_arm.go() left_arm.set_named_target('left_ready') left_arm.go() left_gripper.set_joint_value_target(GRIPPER_OPEN) left_gripper.go() rospy.sleep(1) # attach_box # Set the length, width and height of the object to attach object1_size = [0.09, 0.057, 0.001] # Create a pose for the tool relative to the end-effector object1_p = PoseStamped() object1_p.header.frame_id = right_reference_frame object1_p.pose.position.x = 0.77 object1_p.pose.position.y = -0.27 object1_p.pose.position.z = -0.21 object1_p.pose.orientation.w = 1 scene.add_box(object1_id, object1_p, object1_size) self.setColor(object1_id, 0.8, 0.4, 0, 1.0) self.sendColors() right_arm.set_start_state_to_current_state() left_arm.set_start_state_to_current_state() # target pose target_pose1 = PoseStamped() target_pose1.header.frame_id = right_reference_frame target_pose1.header.stamp = rospy.Time.now() target_pose1.pose.position.x = 0.77 target_pose1.pose.position.y = -0.27 target_pose1.pose.position.z = -0.14 target_pose1.pose.orientation.x = 0.500166303975 target_pose1.pose.orientation.y = 0.865326245156 target_pose1.pose.orientation.z = -0.0155702691449 target_pose1.pose.orientation.w = 0.0283147405248 right_arm.set_pose_target(target_pose1, right_eef) right_arm.go() rospy.sleep(1) # Attach the tool to the end-effector scene.attach_box(right_eef, object1_id, object1_p, object1_size) target_pose2 = PoseStamped() target_pose2.header.frame_id = right_reference_frame target_pose2.header.stamp = rospy.Time.now() target_pose2.pose.position.x = 0.632 target_pose2.pose.position.y = -0.125 target_pose2.pose.position.z = 0.08 target_pose2.pose.orientation.x = 0.2392 target_pose2.pose.orientation.y = -0.9708 target_pose2.pose.orientation.z = -0.0137 target_pose2.pose.orientation.w = 0.0103 target_pose3 = PoseStamped() target_pose3.header.frame_id = left_reference_frame target_pose3.header.stamp = rospy.Time.now() target_pose3.pose.position.x = 0.632 target_pose3.pose.position.y = 0.026 target_pose3.pose.position.z = 0.013 target_pose3.pose.orientation.x = -0.021 target_pose3.pose.orientation.y = 0.017 target_pose3.pose.orientation.z = -0.999 target_pose3.pose.orientation.w = 0.031 both_arms.set_pose_target(target_pose2, right_eef) both_arms.set_pose_target(target_pose3, left_eef) both_arms.go() rospy.sleep(3) #scene.remove_attached_object(right_eef, 'tool') object1_p = left_arm.get_current_pose(left_eef) object1_p.pose.position.y -= 0.12 object1_p.pose.orientation.x = 0 object1_p.pose.orientation.y = 0 object1_p.pose.orientation.z = 0.707 object1_p.pose.orientation.w = 0.707 scene.remove_attached_object(right_eef, object1_id) scene.attach_box(left_eef, object1_id, object1_p, object1_size) rospy.sleep(1) left_gripper.set_joint_value_target(GRIPPER_CLOSE) left_gripper.go() target_pose4 = PoseStamped() target_pose4.header.frame_id = right_reference_frame target_pose4.header.stamp = rospy.Time.now() target_pose4.pose.position.x = 0.58241 target_pose4.pose.position.y = -0.30286 target_pose4.pose.position.z = 0.05471 target_pose4.pose.orientation.x = 0.23931 target_pose4.pose.orientation.y = -0.97080 target_pose4.pose.orientation.z = -0.01346 target_pose4.pose.orientation.w = 0.01003 target_pose5 = PoseStamped() target_pose5.header.frame_id = left_reference_frame target_pose5.header.stamp = rospy.Time.now() target_pose5.pose.position.x = 0.843 target_pose5.pose.position.y = 0.438 target_pose5.pose.position.z = 0.105 target_pose5.pose.orientation.x = -0.01386 target_pose5.pose.orientation.y = -0.02950 target_pose5.pose.orientation.z = -0.70489 target_pose5.pose.orientation.w = 0.70856 #right_arm.set_pose_target(target_pose4, right_eef) both_arms.set_pose_target(target_pose4, right_eef) both_arms.set_pose_target(target_pose5, left_eef) both_arms.go() rospy.sleep(2) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)