def run(self): move_thread = None if not is_moveit_running(): rospy.loginfo("starting moveit") move_thread = MoveItThread() rospy.loginfo("Waiting for MoveIt...") self.client = MoveGroupInterface("arm_with_torso", "base_link") rospy.loginfo("...connected") # Padding does not work (especially for self collisions) # So we are adding a box above the base of the robot scene = PlanningSceneInterface("base_link") scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375) joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.client.moveToJointPosition(joints, pose, 0.0, max_velocity_scaling_factor=0.5) if result and result.error_code.val == MoveItErrorCodes.SUCCESS: scene.removeCollisionObject("keepout") if move_thread: move_thread.stop() # On success quit # Stopping the MoveIt thread works, however, the action client # does not get shut down, and future tucks will not work. # As a work-around, we die and roslaunch will respawn us. rospy.signal_shutdown("done") sys.exit(0) return
def tuck(): if not is_moveit_running(): rospy.loginfo("starting moveit") move_thread = MoveItThread() rospy.loginfo("Waiting for MoveIt...") client = MoveGroupInterface("arm_with_torso", "base_link") rospy.loginfo("...connected") # Padding does not work (especially for self collisions) # So we are adding a box above the base of the robot scene = PlanningSceneInterface("base_link") scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375) joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = client.moveToJointPosition(joints, pose, 0.0, max_velocity_scaling_factor=0.5) if result.error_code.val == MoveItErrorCodes.SUCCESS: scene.removeCollisionObject("keepout") move_thread.stop() return
def main(): rospy.init_node('a5_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('floor') planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2) planning_scene.addBox('table', 0.5, 1, 0.72, 1, 0, 0.72 / 2) rospy.sleep(2)
def main(): rospy.init_node('a5_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('top_shelf') planning_scene.removeCollisionObject('bottom_shelf') planning_scene.addBox('top_shelf', 0.32, 1, 0.4, 0.99, 0, 1.64) planning_scene.addBox('bottom_shelf', 0.5, 1, 1.09, 0.9, 0, 0.545) rospy.sleep(2)
def main(): rospy.init_node('part1_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('floor') planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2) table_height = 0.767 table_width = 0.7 table_x = 0.95 planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height / 2) planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37 / 2) microwave_height = 0.28 microwave_width = 0.48 # microwave_depth = 0.33 microwave_depth = 0.27 microwave_x = 0.97 microwave_z = 0.06 microwave_y = 0.18 planning_scene.addBox('microwave', microwave_depth, microwave_width, microwave_height, microwave_x, microwave_y, table_height + microwave_z + microwave_height / 2) rospy.sleep(2) rospy.spin()
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def updateScene(self): # find objects rospy.loginfo("Add collision objects") self.scene.removeCollisionObject("my_front_ground") self.scene.removeCollisionObject("my_back_ground") self.scene.removeCollisionObject("my_right_ground") self.scene.removeCollisionObject("my_left_ground") self.scene.removeCollisionObject("box_1") self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) self.scene.addBox("box_1", 0.44, 0.43, 0.68, 0.7, -0.4, .34) self.scene.waitForSync() def tuck(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def zero(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
def run(self): move_thread = None if not is_moveit_running(): rospy.loginfo("starting moveit") move_thread = MoveItThread() else: rospy.loginfo("moveit already started") rospy.loginfo("Waiting for MoveIt...") self.client = MoveGroupInterface("arm_with_torso", "base_link") rospy.loginfo("...connected") # Padding does not work (especially for self collisions) # So we are adding a box above the base of the robot scene = PlanningSceneInterface("base_link") scene.addBox("keepout", 0.3, 0.5, 0.05, 0.12, 0.0, 0.375) scene.addBox("ground", 1.5, 1.5, 0.05, 0.5, 0.0, 0.0) joints = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.client.moveToJointPosition( joints, pose, 0.0, max_velocity_scaling_factor=0.5) if result and result.error_code.val == MoveItErrorCodes.SUCCESS: scene.removeCollisionObject("keepout") scene.removeCollisionObject("ground") if move_thread: move_thread.stop() # On success quit # Stopping the MoveIt thread works, however, the action client # does not get shut down, and future tucks will not work. # As a work-around, we die and roslaunch will respawn us. rospy.signal_shutdown("done") sys.exit(0) return
def main(): rospy.init_node('part2_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('floor') planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01/2) table_height = 0.767 table_width = 0.7 table_x = 0.95 planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height/2) planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37/2) rospy.sleep(2) rospy.spin()
class UR_Moveit_API: def __init__(self, boundaries=False): self.scene = PlanningSceneInterface("/base_link") self.robot = RobotCommander() self.group_commander = MoveGroupCommander("manipulator") #self.gripper = MoveGroupCommander("gripper") self.group_commander.set_end_effector_link('ee_link') self.get_basic_infomation() if boundaries is True: self.add_bounds() self.joint_state_subscriber = rospy.Subscriber("/joint_states", JointState, self.joint_callback) self.joint_pubs = [ rospy.Publisher('/%s/command' % name, Float64, queue_size=1) for name in CONTROLLER_NAMES ] self.gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1) # create a DisplayTrajectory publisher which is used later to publish trajectories for RViz to visualize self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.reset_subscriber = rospy.Subscriber("/ur/reset", String, self.reset) rospy.sleep(2) def get_basic_infomation(self): # We can get the name of the reference frame for this robot: planning_frame = self.group_commander.get_planning_frame() print("============ Reference frame: %s" % planning_frame, "============") # We can also print the name of the end-effector link for this group: eef_link = self.group_commander.get_end_effector_link() print("============ End effector: %s" % eef_link, "============") # We can get a list of all the groups in the robot: group_names = self.robot.get_group_names() print("============ Robot Groups:", group_names, "============") # Sometimes for debugging it is useful to print the entire state of the # robot: print("============ Printing robot state") print(self.robot.get_current_state(), "============") print("================================================") def joint_callback(self, joint_state): self.joint_state = joint_state def plan_cartesian_path(self, scale=1): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. group = self.group_commander waypoints = [] wpose = group.get_current_pose().pose wpose.position.z -= scale * 0.1 # First move up (z) wpose.position.y += scale * 0.2 # and sideways (y) waypoints.append(copy.deepcopy(wpose)) wpose.position.x += scale * 0.1 # Second move forward/backwards in (x) waypoints.append(copy.deepcopy(wpose)) wpose.position.y -= scale * 0.1 # Third move sideways (y) waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold # Note: We are just planning, not asking move_group to actually move the robot yet: return plan, fraction def display_trajectory(self, plan): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. robot = self.robot display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan) # Publish display_trajectory_publisher.publish(display_trajectory) ''' def open_gripper(self, drop=False): plan = self.gripper.plan(GRIPPER_DROP if drop else GRIPPER_OPEN) return self.gripper.execute(plan, wait=True) ''' def add_bounds(self): print("Complete to add_bounds") # size_x, size_y, size_z, x, y, z self.scene.addBox('table', 1.0, 1.0, 1.0, .0, .0, -0.5) # add a cube of 0.1m size, at [1, 0, 0.5] in the base_link frame #self.scene.addCube("my_cube", 0.1, 1, 0, 0.5) # Remove the cube #self.scene.removeCollisionObject("my_cube") def remove_bounds(self): for obj in self.scene.get_objects().keys(): self.scene.remove_world_object(obj) ''' def close_gripper(self): plan = self.gripper.plan(GRIPPER_CLOSED) return self.gripper.execute(plan, wait=True) ''' def get_ik_client(self, request): rospy.wait_for_service('/compute_ik') inverse_ik = rospy.ServiceProxy('/compute_ik', GetPositionIK) ret = inverse_ik(request) if ret.error_code.val != 1: return None return ret.solution.joint_state def get_fk_client(self, header, link_names, robot_state): rospy.wait_for_service('/compute_fk') fk = rospy.ServiceProxy('/compute_fk', GetPositionFK) ret = fk(header, link_names, robot_state) if ret.error_code.val != 1: return None return ret.pose_stamped def orient_to_target(self, x=None, y=None, angle=None): current = self.get_joint_values() if x or y: angle = np.arctan2(y, x) if angle >= 3.1: angle = 3.1 elif angle <= -3.1: angle = -3.1 current[0] = angle plan = self.group_commander.plan(current) return self.group_commander.execute(plan, wait=True) def wrist_rotate(self, angle): rotated_values = self.group_commander.get_current_joint_values() rotated_values[4] = angle - rotated_values[0] if rotated_values[4] > np.pi / 2: rotated_values[4] -= np.pi elif rotated_values[4] < -(np.pi / 2): rotated_values[4] += np.pi plan = self.group_commander.plan(rotated_values) return self.group_commander.execute(plan, wait=True) def get_joint_values(self): return self.group_commander.get_current_joint_values() def get_current_pose(self): return self.group_commander.get_current_pose() def move_to_neutral(self): print('Moving to neutral...') plan = self.group_commander.plan(NEUTRAL_VALUES) return self.group_commander.execute(plan, wait=True) def move_to_home(self): print('Moving to home...') plan = self.group_commander.plan(HOME) return self.group_commander.execute(plan, wait=True) def move_to_up(self): print('Moving to up...') plan = self.group_commander.plan(UP) return self.group_commander.execute(plan, wait=True) def reset(self, data): print("data: ", data) self.move_to_neutral() rospy.sleep(0.5) def orient_to_pregrasp(self, x, y): angle = np.arctan2(y, x) return self.move_to_drop(angle) def move_to_grasp(self, x, y, z, angle, compensate_control_noise=True): if compensate_control_noise: x = (x - CONTROL_NOISE_COEFFICIENT_BETA ) / CONTROL_NOISE_COEFFICIENT_ALPHA y = (y - CONTROL_NOISE_COEFFICIENT_BETA ) / CONTROL_NOISE_COEFFICIENT_ALPHA current_p = self.group_commander.get_current_pose().pose p1 = Pose(position=Point(x=x, y=y, z=z), orientation=DOWN_ORIENTATION) plan, f = self.group_commander.compute_cartesian_path([current_p, p1], 0.001, 0.0) joint_goal = list(plan.joint_trajectory.points[-1].positions) first_servo = joint_goal[0] joint_goal[4] = (angle - first_servo) % np.pi if joint_goal[4] > np.pi / 2: joint_goal[4] -= np.pi elif joint_goal[4] < -(np.pi / 2): joint_goal[4] += np.pi try: plan = self.group_commander.plan(joint_goal) except MoveItCommanderException as e: print('Exception while planning') traceback.print_exc(e) return False return self.group_commander.execute(plan, wait=True) def move_to_vertical(self, z, force_orientation=True, shift_factor=1.0): current_p = self.group_commander.get_current_pose().pose current_angle = self.get_joint_values()[4] orientation = current_p.orientation if force_orientation else None p1 = Pose(position=Point(x=current_p.position.x * shift_factor, y=current_p.position.y * shift_factor, z=z), orientation=orientation) waypoints = [current_p, p1] plan, f = self.group_commander.compute_cartesian_path( waypoints, 0.001, 0.0) if not force_orientation: return self.group_commander.execute(plan, wait=True) else: if len(plan.joint_trajectory.points) > 0: joint_goal = list(plan.joint_trajectory.points[-1].positions) else: return False joint_goal[4] = current_angle plan = self.group_commander.plan(joint_goal) return self.group_commander.execute(plan, wait=True) def move_to_target(self, target): assert len(target) >= 6, 'Invalid target command' for i, pos in enumerate(target): self.joint_pubs[i].publish(pos) def move_to_joint_position(self, joints): """ Adds the given joint values to the current joint values, moves to position """ joint_state = self.joint_state joint_dict = dict(zip(joint_state.name, joint_state.position)) for i in range(len(JOINT_NAMES)): joint_dict[JOINT_NAMES[i]] += joints[i] joint_state = JointState() joint_state.name = JOINT_NAMES joint_goal = [joint_dict[joint] for joint in JOINT_NAMES] joint_goal = np.clip(np.array(joint_goal), JOINT_MIN, JOINT_MAX) joint_state.position = joint_goal header = Header() robot_state = RobotState() robot_state.joint_state = joint_state link_names = ['ee_link'] position = self.get_fk_client(header, link_names, robot_state) target_p = position[0].pose.position x, y, z = target_p.x, target_p.y, target_p.z conditions = [ x <= BOUNDS_LEFTWALL, x >= BOUNDS_RIGHTWALL, y <= BOUNDS_BACKWALL, y >= BOUNDS_FRONTWALL, z <= BOUNDS_FLOOR, z >= 0.15 ] print("Target Position: %0.4f, %0.4f, %0.4f" % (x, y, z)) for condition in conditions: if not condition: return self.move_to_target(joint_goal) rospy.sleep(0.15) def scenario_plan(self): plan = self.group_commander.plan(PREDISCARD_VALUES) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(DISCARD_VALUES) self.group_commander.execute(plan, wait=True) #self.open_gripper(drop=True) plan = self.group_commander.plan(PREDISCARD_VALUES) self.group_commander.execute(plan, wait=True) self.move_to_neutral() def execute_plan(self, plan): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. group = self.group_commander ## Use execute if you would like the robot to follow ## the plan that has already been computed: group.execute(plan, wait=True)
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. import argparse import rospy from moveit_python import PlanningSceneInterface if __name__ == "__main__": parser = argparse.ArgumentParser( description="Add a box to the MoveIt planning scene.") parser.add_argument("name", help="Name of the box to add") parser.add_argument("x", type=float, help="X coordinate of center of box") parser.add_argument("y", type=float, help="Y coordinate of center of box") parser.add_argument("z", type=float, help="Z coordinate of center of box") parser.add_argument("size_x", type=float, help="Size of box in x dimension") parser.add_argument("size_y", type=float, help="Size of box in y dimension") parser.add_argument("size_z", type=float, help="Size of box in z dimension") args = parser.parse_args() if args.name: rospy.init_node("add_box") scene = PlanningSceneInterface("/base_link") print("Adding Box with name: %s" % args.name) scene.addBox(args.name, args.size_x, args.size_y, args.size_z, args.x, args.y, args.z, use_service=True) else: parser.print_help()
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") grasp_topic = "fetch_grasp_planner_node/plan" rospy.loginfo("Waiting for %s..." % grasp_topic) self.grasp_planner_client = actionlib.SimpleActionClient( grasp_topic, GraspPlanningAction) wait = self.grasp_planner_client.wait_for_server(rospy.Duration(5)) if (wait): print("successfully connected to grasp server") else: print("failed to connect to grasp server") rospy.Subscriber("fetch_fruit_harvest/apple_pose", Pose, self.apple_pose_callback) self.pub = rospy.Publisher("fetch_fruit_harvest/grasp_msg", String, queue_size=10) self.object = Object() self.grasps = Grasp() self.ready_for_grasp = False self.pick_place_finished = False self.first_time_grasp = True self.scene.addBox("ground", 300, 300, 0.02, 0, 0, 0) self.tuck() def apple_pose_callback(self, message): if self.pick_place_finished or self.first_time_grasp: self.first_time_grasp = False self.pick_place_finished = False print("apple_pose_callback") apple = SolidPrimitive() apple.type = SolidPrimitive.SPHERE apple.dimensions = [0.03, 0.03, 0.03] self.object.name = "apple" self.object.primitives = [apple] self.object.primitive_poses = [message] # add stamp and frame self.object.header.stamp = rospy.Time.now() self.object.header.frame_id = "base_link" goal = GraspPlanningGoal() goal.object = self.object self.grasp_planner_client.send_goal(goal) self.grasp_planner_client.wait_for_result(rospy.Duration(5.0)) grasp_planner_result = self.grasp_planner_client.get_result() self.grasps = grasp_planner_result.grasps self.ready_for_grasp = True def updateScene(self): # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene self.scene.addBox("ground", 300, 300, 0.02, 0, 0, 0) self.scene.addSolidPrimitive(self.object.name, self.object.primitives[0], self.object.primitive_poses[0]) self.scene.waitForSync() def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, scene=self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry( block.name, places, scene=self.scene) return success def tuck(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def stow(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def intermediate_stow(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
target, origin, wrist, group_name, group_joint_name, base_link = pd.show_states() final_trans = pd.calculate_transform(target, origin) final_grasp = pd.grasp_to_body(final_trans, grasp) final_msg_trans = pd.convert_to_message(final_grasp) get_ik = pd.IK(final_msg_trans) tablemsg = pd.show_states_table() tablest = pd.calculate_transform(tablemsg, origin) table = pd.convert_to_message(tablest) planning_scene.removeCollisionObject('table') table_size_x = 0.913 table_size_y = 0.913 table_size_z = 0.040 table_x = table.position.x table_y = table.position.y table_z = table.position.z + 0.74 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) target = pd.convert_to_message(final_trans) planning_scene.removeCollisionObject('cube') cube_size_x = 0.044 cube_size_y = 0.044 cube_size_z = 0.180 cube_x = target.position.x cube_y = target.position.y cube_z = target.position.z planning_scene.addBox('cube', cube_size_x, cube_size_y, cube_size_z, cube_x, cube_y, cube_z) final_grasp = pd.grasp_to_body(final_trans,grasp) fmt = pd.convert_to_message(final_grasp) pose_goal = geometry_msgs.msg.Pose() pose_goal.position.x = fmt.position.x pose_goal.position.y = fmt.position.y
class UR_Moveit_API: def __init__(self, boundaries=False): self.scene = PlanningSceneInterface("/base_link") self.robot = RobotCommander() self.group_commander = MoveGroupCommander("manipulator") self.gripper = MoveGroupCommander("gripper") self.group_commander.set_end_effector_link('eef_link') self.get_basic_infomation() if boundaries is True: self.add_bounds() self.joint_state_subscriber = rospy.Subscriber("/joint_states", JointState, self.joint_callback) self.joint_pubs = [ rospy.Publisher('/%s/command' % name, Float64, queue_size=1) for name in CONTROLLER_NAMES ] self.gripper_pub = rospy.Publisher('/gripper_prismatic_joint/command', Float64, queue_size=1) # create a DisplayTrajectory publisher which is used later to publish trajectories for RViz to visualize self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.reset_subscriber = rospy.Subscriber("/ur/reset", String, self.reset) rospy.sleep(2) def get_basic_infomation(self): # We can get the name of the reference frame for this robot: planning_frame = self.group_commander.get_planning_frame() print("============ Reference frame: %s" % planning_frame, "============") # We can also print the name of the end-effector link for this group: eef_link = self.group_commander.get_end_effector_link() print("============ End effector: %s" % eef_link, "============") # We can get a list of all the groups in the robot: group_names = self.robot.get_group_names() print("============ Robot Groups:", group_names, "============") # Sometimes for debugging it is useful to print the entire state of the # robot: print("============ Printing robot state") print(self.robot.get_current_state(), "============") print("================================================") def joint_callback(self, joint_state): self.joint_state = joint_state def open_gripper(self, drop=False): plan = self.gripper.plan(GRIPPER_DROP if drop else GRIPPER_OPEN) return self.gripper.execute(plan, wait=True) def add_bounds(self): print("Complete to add_bounds") # size_x, size_y, size_z, x, y, z self.scene.addBox('table', 1.0, 1.0, 1.0, .0, .0, -0.5) # add a cube of 0.1m size, at [1, 0, 0.5] in the base_link frame #self.scene.addCube("my_cube", 0.1, 1, 0, 0.5) # Remove the cube #self.scene.removeCollisionObject("my_cube") def remove_bounds(self): for obj in self.scene.get_objects().keys(): self.scene.remove_world_object(obj) def close_gripper(self): plan = self.gripper.plan(GRIPPER_CLOSED) return self.gripper.execute(plan, wait=True) def get_ik_client(self, request): rospy.wait_for_service('/compute_ik') inverse_ik = rospy.ServiceProxy('/compute_ik', GetPositionIK) ret = inverse_ik(request) if ret.error_code.val != 1: return None return ret.solution.joint_state def get_fk_client(self, header, link_names, robot_state): rospy.wait_for_service('/compute_fk') fk = rospy.ServiceProxy('/compute_fk', GetPositionFK) ret = fk(header, link_names, robot_state) if ret.error_code.val != 1: return None return ret.pose_stamped def joint_limits(self, header, link_names, robot_state): rospy.wait_for_service('/compute_fk') fk = rospy.ServiceProxy('/compute_fk', GetPositionFK) ret = fk(header, link_names, robot_state) if ret.error_code.val != 1: return None return ret.pose_stamped def eval_grasp(self, threshold=.0001, manual=False): if manual: user_input = None while user_input not in ('y', 'n', 'r'): user_input = raw_input( 'Successful grasp? [(y)es/(n)o/(r)edo]: ') if user_input == 'y': return 1, None elif user_input == 'n': return 0, None else: return -1, None else: current = np.array(self.gripper.get_current_joint_values()) target = np.array(GRIPPER_CLOSED) error = current[0] - target[0] return error > threshold, error def orient_to_target(self, x=None, y=None, angle=None): current = self.get_joint_values() if x or y: angle = np.arctan2(y, x) if angle >= 3.1: angle = 3.1 elif angle <= -3.1: angle = -3.1 current[0] = angle plan = self.group_commander.plan(current) return self.group_commander.execute(plan, wait=True) def wrist_rotate(self, angle): rotated_values = self.group_commander.get_current_joint_values() rotated_values[4] = angle - rotated_values[0] if rotated_values[4] > np.pi / 2: rotated_values[4] -= np.pi elif rotated_values[4] < -(np.pi / 2): rotated_values[4] += np.pi plan = self.group_commander.plan(rotated_values) return self.group_commander.execute(plan, wait=True) def get_joint_values(self): return self.group_commander.get_current_joint_values() def get_current_pose(self): return self.group_commander.get_current_pose() def move_to_neutral(self): print('Moving to neutral...') plan = self.group_commander.plan(NEUTRAL_VALUES) return self.group_commander.execute(plan, wait=True) def move_to_drop(self, angle=None): drop_positions = DROPPING_VALUES[:] if angle: drop_positions[0] = angle plan = self.group_commander.plan(drop_positions) return self.group_commander.execute(plan, wait=True) def move_to_empty(self): plan = self.group_commander.plan(EMPTY_VALUES) return self.group_commander.execute(plan, wait=True) def move_to_reset(self): print('Moving to reset...') plan = self.group_commander.plan(RESET_VALUES) return self.group_commander.execute(plan, wait=True) def reset(self, data): print("data: ", data) self.move_to_reset() rospy.sleep(0.5) def orient_to_pregrasp(self, x, y): angle = np.arctan2(y, x) return self.move_to_drop(angle) def move_to_grasp(self, x, y, z, angle, compensate_control_noise=True): if compensate_control_noise: x = (x - CONTROL_NOISE_COEFFICIENT_BETA ) / CONTROL_NOISE_COEFFICIENT_ALPHA y = (y - CONTROL_NOISE_COEFFICIENT_BETA ) / CONTROL_NOISE_COEFFICIENT_ALPHA current_p = self.group_commander.get_current_pose().pose p1 = Pose(position=Point(x=x, y=y, z=z), orientation=DOWN_ORIENTATION) plan, f = self.group_commander.compute_cartesian_path([current_p, p1], 0.001, 0.0) joint_goal = list(plan.joint_trajectory.points[-1].positions) first_servo = joint_goal[0] joint_goal[4] = (angle - first_servo) % np.pi if joint_goal[4] > np.pi / 2: joint_goal[4] -= np.pi elif joint_goal[4] < -(np.pi / 2): joint_goal[4] += np.pi try: plan = self.group_commander.plan(joint_goal) except MoveItCommanderException as e: print('Exception while planning') traceback.print_exc(e) return False return self.group_commander.execute(plan, wait=True) def move_to_vertical(self, z, force_orientation=True, shift_factor=1.0): current_p = self.group_commander.get_current_pose().pose current_angle = self.get_joint_values()[4] orientation = current_p.orientation if force_orientation else None p1 = Pose(position=Point(x=current_p.position.x * shift_factor, y=current_p.position.y * shift_factor, z=z), orientation=orientation) waypoints = [current_p, p1] plan, f = self.group_commander.compute_cartesian_path( waypoints, 0.001, 0.0) if not force_orientation: return self.group_commander.execute(plan, wait=True) else: if len(plan.joint_trajectory.points) > 0: joint_goal = list(plan.joint_trajectory.points[-1].positions) else: return False joint_goal[4] = current_angle plan = self.group_commander.plan(joint_goal) return self.group_commander.execute(plan, wait=True) def move_to_target(self, target): assert len(target) >= 6, 'Invalid target command' for i, pos in enumerate(target): self.joint_pubs[i].publish(pos) def move_to_joint_position(self, joints): """ Adds the given joint values to the current joint values, moves to position """ joint_state = self.joint_state joint_dict = dict(zip(joint_state.name, joint_state.position)) for i in range(len(JOINT_NAMES)): joint_dict[JOINT_NAMES[i]] += joints[i] joint_state = JointState() joint_state.name = JOINT_NAMES joint_goal = [joint_dict[joint] for joint in JOINT_NAMES] joint_goal = np.clip(np.array(joint_goal), JOINT_MIN, JOINT_MAX) joint_state.position = joint_goal header = Header() robot_state = RobotState() robot_state.joint_state = joint_state link_names = ['eef_link'] position = self.get_fk_client(header, link_names, robot_state) target_p = position[0].pose.position x, y, z = target_p.x, target_p.y, target_p.z conditions = [ x <= BOUNDS_LEFTWALL, x >= BOUNDS_RIGHTWALL, y <= BOUNDS_BACKWALL, y >= BOUNDS_FRONTWALL, z <= BOUNDS_FLOOR, z >= 0.15 ] print("Target Position: %0.4f, %0.4f, %0.4f" % (x, y, z)) for condition in conditions: if not condition: return self.move_to_target(joint_goal) rospy.sleep(0.15) def sweep_arena(self): self.remove_bounds() self.move_to_drop(.8) plan = self.group_commander.plan(TL_CORNER[0]) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(TL_CORNER[1]) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(L_SWEEP[0]) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(L_SWEEP[1]) self.group_commander.execute(plan, wait=True) self.move_to_drop(-.8) plan = self.group_commander.plan(BL_CORNER[0]) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(BL_CORNER[1]) self.group_commander.execute(plan, wait=True) self.move_to_drop(-2.45) plan = self.group_commander.plan(BR_CORNER[0]) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(BR_CORNER[1]) self.group_commander.execute(plan, wait=True) self.move_to_drop(2.3) plan = self.group_commander.plan(TR_CORNER[0]) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(TR_CORNER[1]) self.group_commander.execute(plan, wait=True) self.add_bounds() def discard_object(self): plan = self.group_commander.plan(PREDISCARD_VALUES) self.group_commander.execute(plan, wait=True) plan = self.group_commander.plan(DISCARD_VALUES) self.group_commander.execute(plan, wait=True) self.open_gripper(drop=True) plan = self.group_commander.plan(PREDISCARD_VALUES) self.group_commander.execute(plan, wait=True) self.move_to_neutral()
def main(): rospy.init_node('part1_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('floor') planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2) table_height = 0.767 table_width = 0.7 table_x = 0.95 planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height / 2) planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37 / 2) microwave_height = 0.28 microwave_width = 0.48 microwave_depth = 0.27 microwave_x = 0.97 microwave_y = 0.18 microwave_z = 0.06 microwave_side_height = 0.2 microwave_r_width = 0.135 microwave_r_y = microwave_y - 0.175 microwave_l_width = 0.035 microwave_l_y = microwave_y + 0.222 microwave_bottom_height = 0.05 microwave_top_height = 0.04 microwave_back_depth = 0.03 microwave_back_x = table_x + (microwave_depth / 2) + (microwave_back_depth / 2) microwave_door_width = 0.09 microwave_door_x = microwave_x - 0.33 microwave_door_y = microwave_l_y + 0.027 planning_scene.addBox( 'microwave_top', microwave_depth, microwave_width, microwave_top_height, microwave_x, microwave_y, table_height + microwave_z + microwave_bottom_height + microwave_side_height + (microwave_top_height / 2)) planning_scene.addBox( 'microwave_bottom', microwave_depth, microwave_width, microwave_bottom_height, microwave_x, microwave_y, table_height + microwave_z + (microwave_bottom_height / 2)) planning_scene.addBox( 'microwave_side_r', microwave_depth, microwave_r_width, microwave_side_height, microwave_x, microwave_r_y, table_height + microwave_z + microwave_bottom_height + (microwave_side_height / 2)) planning_scene.addBox( 'microwave_side_l', microwave_depth, microwave_l_width, microwave_side_height, microwave_x, microwave_l_y, table_height + microwave_z + microwave_bottom_height + microwave_side_height / 2) planning_scene.addBox('microwave_back', microwave_back_depth, microwave_width, microwave_height, microwave_back_x, microwave_y, table_height + microwave_z + microwave_height / 2) planning_scene.addBox( 'microwave_door', 0.39, microwave_door_width, microwave_height + 0.01, microwave_door_x, microwave_door_y, table_height + microwave_z + microwave_height / 2 + 0.005) rospy.sleep(2) rospy.spin()
def main(): rospy.init_node('pour_scene') wait_for_time() target_name = 'cup1' x_pose = 0.329202820349 y_pose = -0.01 z_pose = 0.060 x_ori, y_ori, z_ori, w_ori = cvt_e2q(0, 0, 0) x, y, z, _ = getState('cup2') head = fetch_api.Head() arm = fetch_api.Arm() torso = fetch_api.Torso() torso.set_height(fetch_api.Torso.MAX_HEIGHT) head.look_at('base_link', x, y, z) sess = tensorflow.Session() model = load_model(sess) x, y, z, _ = getState(target_name) # observation test get_observation() move_arm_to(arm, x_pose, y + y_pose, z + z_pose, x_ori, y_ori, z_ori, w_ori) x, y, z, _ = getState('table1') base = fetch_api.Base() if x > 1: base.go_forward(0.6, speed=0.2) # collision in motion planning planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() length_table = 1 width_table = 1 height_table = 0.04 x_table, y_table, z_table, _ = getState('table1') z_table = z + 0.7 planning_scene.addBox('table', length_table, width_table, height_table, x_table, y_table, z_table) length_box = 0.05 width_box = 0.05 height_box = 0.2 x, y, z, _ = getState('cup1') x_box = x y_box = y z_box = z planning_scene.addBox('mug_1', length_box, width_box, height_box, x_box, y_box, z_box) length_box = 0.03 width_box = 0.05 height_box = 0.2 x, y, z, _ = getState('cup2') x_box = x y_box = y z_box = z planning_scene.addBox('mug_2', length_box, width_box, height_box, x_box, y_box, z_box) # the initial position of gripper is (-0.5, 0, 0), and # the final position of gripper is (-0.5+x_pose, y_pose, z_pose). x, y, z, _ = getState(target_name) move_arm_to(arm, x - 0.5 + x_pose, y + y_pose, z + z_pose, x_ori, y_ori, z_ori, w_ori) planning_scene.removeCollisionObject('mug_1') # planning_scene.removeCollisionObject('mug_2') # planning_scene.removeCollisionObject('mug') # planning_scene.removeCollisionObject('table') gripper = fetch_api.Gripper() effort = gripper.MAX_EFFORT gripper.close(effort) x, y, z, _ = getState(target_name) move_arm_to(arm, x - 0.5 + x_pose, y + y_pose, z + z_pose + 0.06, x_ori, y_ori, z_ori, w_ori) x, y, z, _ = getState('cup2') head.look_at('base_link', x, y, z) for _ in xrange(50): obs = get_observation() act = model.choose_action(obs) print('obs: {}, action: {}'.format(obs, act)) execute_action(act, arm) time.sleep(0.5)
class FakePerceptionNode: def __init__(self): self.planning_scene = PlanningSceneInterface("map") self.tf_broacaster = tf.TransformBroadcaster() self.tf_listener = tf.TransformListener() self.pub = rospy.Subscriber('/gazebo/link_states', LinkStates, self.simulated_link_state_callback, queue_size=1) self.update_planning_scene = True self.last_update = rospy.Time.now() self.update_period = rospy.Duration(10) self.table_collision = True self.cube_collision = False def update(self): pass def simulated_link_state_callback(self, linksmsg): """ string[] name geometry_msgs/Pose[] pose geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w geometry_msgs/Twist[] twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z """ cube_transforms = self.propagate_link_states_to_tf( linksmsg, "cube", "cube_", "map") table_transforms = self.propagate_link_states_to_tf( linksmsg, "table", "table_", "map") ellapsed = rospy.Time.now() - self.last_update if ellapsed > self.update_period: # if self.update_planning_scene: rospy.logdebug("updating planning scene") attached_objects = self.planning_scene.getKnownAttachedObjects() rospy.loginfo(attached_objects) #self.update_planning_scene = False if self.table_collision and not "cube_0" in attached_objects: for i, table_transf in enumerate(table_transforms): #self.planning_scene.removeCollisionObject("table_" + str(i)) thickness = 0.12 pos = table_transf[0] self.planning_scene.addBox("table_" + str(i), 1.2, 1.3, 0.001 + thickness, pos[0], pos[1], 0.7 - thickness * 0.5) # 0.68 if self.cube_collision and not "cube_0" in attached_objects: for i, cube_transf in enumerate(cube_transforms): #self.planning_scene.removeCollisionObject("cube_" + str(i)) pos = cube_transf[0] self.planning_scene.addCube("cube_" + str(i), 0.06, pos[0], pos[1], pos[2]) #self.cube_collision = False def propagate_link_states_to_tf(self, linksmsg, link_name_filter, object_prefix, global_frame): filteredLinkPoses = [ b for i, b in enumerate(linksmsg.pose) if link_name_filter in linksmsg.name[i] ] transforms = [] for i, cubepose in enumerate(filteredLinkPoses): quat = [ cubepose.orientation.x, cubepose.orientation.y, cubepose.orientation.z, cubepose.orientation.w ] trans = [ cubepose.position.x, cubepose.position.y, cubepose.position.z ] self.tf_broacaster.sendTransform(trans, quat, rospy.Time.now(), object_prefix + str(i), global_frame) transforms.append([trans, quat]) return transforms
# rightgripper.open() scene = moveit_commander.PlanningSceneInterface() robot = moveit_commander.RobotCommander() rospy.sleep(2) # box_pose = geometry_msgs.msg.PoseStamped() # box_pose.header.frame_id = robot.get_planning_frame() # box_pose.pose.position.x = 1. # box_pose.pose.position.y = 0.5 # box_pose.pose.position.z = 0.5 # scene.add_box("table", box_pose, (0.5, 1.5, 0.6)) # name size and position p.addBox("first part", 0.8, 0.43, 0.04, 1.23, 0, 0.36) p.addBox("under part", 0.8, 0.43, 0.04, 1.23, 0, 0.0) p.addBox("second part", 0.8, 0.01, 1.6, 1.23, -0.22, 0) p.addBox("third part", 0.8, 0.01, 1.6, 1.23, 0.22, 0) p.waitForSync() def load_gazebo_models(table_pose=Pose(position=Point(x=0.7, y=0.0, z=0.0)), table_reference_frame="world", block_pose1=Pose(position=Point(x=0.6725, y=0.0765, z=-0.135)), block_pose2=Pose(position=Point(x=0.55, y=-0.2, z=-0.135)), block_pose3=Pose(position=Point(x=0.7, y=-0.1, z=-0.135)), block_pose4=Pose(position=Point(x=0.58, y=-0.03, z=-0.135)), block_reference_frame="base"): pass
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. import argparse import rospy from moveit_python import PlanningSceneInterface if __name__ == "__main__": parser = argparse.ArgumentParser( description="Remove objects from the MoveIt planning scene.") parser.add_argument("--name", nargs="?", help="Name of object to remove") args = parser.parse_args() if args.name: rospy.init_node("add_boxes") scene = PlanningSceneInterface("/base_link") print("Adding Box with name: %s" % args.name) scene.addBox(args.name, 0.1, 0.1, 0.1, 3, 3, 3, True) scene.waitForSync() else: parser.print_help()
def main(): print('We are running the main of arm_obstable_demo') rospy.init_node('arm_obstacle_demo') wait_for_time() print( "If planning scene fails to init, make sure you have launched move_group.launch" ) planning_scene = PlanningSceneInterface("base_link") # Create table object planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider object planning_scene.removeCollisionObject('divider') size_x = 0.3 size_y = 0.01 size_z = 0.2 x = table_x - (table_size_x / 2) + (size_x / 2) y = 0 z = table_z + (table_size_z / 2) + (size_z / 2) planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) # Before moving to the first pose planning_scene.removeAttachedObject('tray') pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 15, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': False } error = arm.move_to_pose(pose1, **kwargs) # If the robot reaches the first pose successfully, then "attach" an object there # Of course, you would have to close the gripper first and ensure that you grasped the object properly if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() rospy.sleep(1) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') # At the end of your program planning_scene.removeAttachedObject('tray')
class Reaching: def __init__(self): rospy.init_node('reaching', anonymous=True) #ノードの初期化 reaching ていう名前 self.robot = moveit_commander.RobotCommander() # インスタンスの作成 self.model_pose = [0.0, 0.0, 0.0] #モデルの姿勢 self.obstacle_pose = [0.0, 0.0, 0.0] #障害物の位置 self.subsc_pose = [0.0, 0.0, 0.0] #購読した座標 self.subsc_orientation = [0.0, 0.0, 0.0, 1.0] #購読した四元数 self.set_forbidden() #禁止エリアの初期化 self.set_init_pose() #位置姿勢の初期化 self.before_pose = [0, 0, 0] #前回の場所 self.pcarray = 0 #pointcloud を np array にしたものを入れる self.executeFlag = False #unity コントローラ B ボタンと対応 self.before_executeFlag = False #前フレームの executeFlag self.waypoints = [] self.way_flag = False #unity コントローラの 中指のボタンに対応 self.before_wayflag = False #前フレームのway_flag self.state = { 'default': 0, 'hold': 1, 'way_standby': 2, 'plan_standby': 3 } # default : 何もしていない target のセットか hold button を待つ # hold : waypoint のセットを受け付ける hold button が離れたら終了 #way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合 #plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合 self.now_state = self.state['default'] #現在フレームの状態 def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface( "base_link") #PlanningSceneInterface のインスタンスの作成 self.planning_scene.removeCollisionObject("my_front_ground") self.planning_scene.removeCollisionObject("my_back_ground") self.planning_scene.removeCollisionObject("my_right_ground") self.planning_scene.removeCollisionObject("my_left_ground") #オブジェクトの削除 self.planning_scene.addCube("my_front_ground", 2, 1.1, 0, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.0, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.0, -1.0) #座標に立方体を挿入 self.planning_scene.addCube("targetOBJ", 0.5, 0.0, 1.0, 2.0) self.planning_scene.removeCollisionObject("demo_cube") self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除 print dir(self.planning_scene) #インスタンス内のあらゆるオブジェクトの属性とメソッドのリストを表示 import inspect print "addBox's variable is ", inspect.getargspec( self.planning_scene.addBox) print "attachBox's variable is ", inspect.getargspec( self.planning_scene.attachBox) print "addCube's variable is ", inspect.getargspec( self.planning_scene.addCube) print "setColor's variable is ", inspect.getargspec( self.planning_scene.setColor) #python関数のパラメータの名前とデフォルト値を取得 def set_init_pose(self): #set init pose move_group = MoveGroupInterface( "manipulator", "base_link") #MoveGroupInterface のインスタンスの作成 planning_scene = PlanningSceneInterface( "base_link") #PlanningSceneInterface のインスタンスの作成 joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", #ジョイントの名前を定義 "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ] pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0] move_group.moveToJointPosition(joint_names, pose, wait=False) #joint_names を pose に動かす move_group.get_move_action().wait_for_result() #wait result result = move_group.get_move_action().get_result() #result を代入 move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル def callback(self, data): #トピックにデータが追加されるたびに呼ばれる x = data.pose[1].position.x y = data.pose[1].position.y z = data.pose[1].position.z self.model_pose = [x, y, z] #modelの姿勢を代入 x = data.pose[2].position.x y = data.pose[2].position.y z = data.pose[2].position.z self.obstacle_pose = [x, y, z] #障害物の姿勢を代入 def callbacksub(self, data): x = data.x y = data.y z = data.z self.subsc_pose = [x, y, z] #購読した座標を代入 def callbackunity(self, data): x = data.pose.position.x y = data.pose.position.y z = data.pose.position.z self.subsc_pose = [y, -x, z] #unity からの座標 座標変換も行っている x = data.pose.orientation.x y = data.pose.orientation.y z = data.pose.orientation.z w = data.pose.orientation.w self.subsc_orientation = [y, -x, z, w] #unity からの四元数 def rtabcallback(self, data): dtype_list = [(f.name, np.float32) for f in data.fields] cloud_arr = np.fromstring(data.data, dtype_list) self.pcarray = np.reshape(cloud_arr, (data.height, data.width)) def excallback(self, data): self.executeFlag = data.data def waycallback(self, data): self.way_flag = data.data def start_subscriber(self): #購読をスタート rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback) # /gazebo/model_states から gazebo_msgs/ModelStates 型のトピックを購読 トピックが更新されるたびに self.callback を呼ぶ #rospy.Subscriber("/UR/reaching/pose",Vector3,self.callbacksub) rospy.Subscriber("/unity/target", PoseStamped, self.callbackunity) #unityから配信しているトピックを購読 rospy.Subscriber('/rtabmap/cloud_map', PointCloud2, self.rtabcallback) #zed で slam したpointcloud を購読 rospy.Subscriber('/unity/execute', Bool, self.excallback) rospy.Subscriber('/unity/wayflag', Bool, self.waycallback) def change_state(self, target): #self.state={'default':0,'hold':1,'way_standby':2,'plan_standby':3} # default : 何もしていない target のセットか hold button を待つ # hold : waypoint のセットを受け付ける hold button が離れたら終了 #way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合 #plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合 set_flag = (self.before_pose != target.position ) # target の位置が変化している = トリガーが押された hold_flag = self.way_flag # hold button が押されている exe_flag = (self.before_executeFlag != self.executeFlag ) # executeFlag が前フレームと違う = execute button が押された print('set ' + str(set_flag)) print('hold ' + str(hold_flag)) print('exe ' + str(exe_flag)) if self.now_state == self.state['default']: if set_flag: self.now_state = self.state['plan_standby'] elif hold_flag: self.now_state = self.state['hold'] elif self.now_state == self.state['hold']: if not hold_flag: self.now_state = self.state['way_standby'] elif self.now_state == self.state['way_standby']: if exe_flag: self.now_state = self.state['default'] elif self.now_state == self.state['plan_standby']: if exe_flag: self.now_state = self.state['default'] def target(self): rate = rospy.Rate(1) #Rateクラスのインスタンス rateを1で rospy.sleep(1) #1秒スリープ useway = False while not rospy.is_shutdown(): #シャットダウンフラグが立っていなければ、 self.planning_scene.removeCollisionObject("demo_cube") #オブジェクトの削除 self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除 self.planning_scene.addCube( "demo_cube", 0.06, self.model_pose[0], self.model_pose[1], self.model_pose[2]) #一辺が0.06の正方形をmodel_pose(座標)に追加 self.planning_scene.addBox( "obstacle", 0.540614, 0.083603, 0.54373, self.obstacle_pose[0], self.obstacle_pose[1], self.obstacle_pose[2]) #---の大きさの箱をobstacle_pose(座標)に追加 print self.model_pose #model_poseを表示 group = moveit_commander.MoveGroupCommander( "manipulator") #MoveGroupCommander クラスのインスタンス #print group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose = group.get_current_pose().pose #エンドエフェクタの位置姿勢 pose_target = geometry_msgs.msg.Pose( ) #geometry_msgs.pose のインスタンス ここにターゲット設定 pose_target.orientation.x = self.subsc_orientation[0] pose_target.orientation.y = self.subsc_orientation[1] pose_target.orientation.z = self.subsc_orientation[2] pose_target.orientation.w = self.subsc_orientation[ 3] #トピックから 四元数を代入 pose_target.position.x = self.subsc_pose[0] # pose_target.position.y = self.subsc_pose[1] # pose_target.position.z = self.subsc_pose[2] #トピックから 座標を代入 #self.planning_scene.removeCollisionObject("targetOBJ") #self.planning_scene.addCube("targetOBJ", 0.005,self.subsc_pose[0] ,self.subsc_pose[1] ,self.subsc_pose[2] ) self.way_flag = False #self.state={'default':0,'hold':1,'way_standby':2,'plan_standby':3} # default : 何もしていない target のセットか hold button を待つ # hold : waypoint のセットを受け付ける hold button が離れたら終了 #way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合 #plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合 self.change_state(pose_target) exe_flag = (self.before_executeFlag != self.executeFlag ) # executeFlag が前フレームと違う = execute button が押された if self.now_state == self.state['default']: if self.before_pose != pose_target.position: group.set_pose_target(pose_target) group.plan() if self.now_state == self.state['hold']: if self.before_pose != pose_target.position: self.waypoints.append(copy.deepcopy(pose_target)) if self.now_state == self.state['way_standby']: plan, fraction = group.compute_cartesian_path( self.waypoints, 0.01, 0.0) if exe_flag: group.execute(plan) self.waypoints = [] if self.now_state == self.state['plan_standby']: if exe_flag: group.go() self.before_executeFlag = self.executeFlag if self.before_pose != pose_target.position: #前フレームと座標が変化した if self.way_flag: #waypoint を使用したティーチング(グリップボタン押してる) self.waypoints.append(copy.deepcopy(pose_target)) useway = True else: group.set_pose_target(pose_target) #ターゲットをセット useway = False # group.set_pose_target(pose_target)#毎フレーム必要っぽい!!!!!!!!!!!!! if not useway and self.before_pose != pose_target.position and not self.before_wayflag: group.plan() if self.before_wayflag != self.way_flag and not self.way_flag: #前のフレームが true で false に変わったフレーム print(self.waypoints) plan, fraction = group.compute_cartesian_path( self.waypoints, 0.01, 0.0) self.before_pose = pose_target.position self.before_wayflag = self.way_flag if self.executeFlag != self.before_executeFlag: if useway: group.execute(plan) self.before_executeFlag = self.executeFlag self.waypoints = [] else: group.go() self.before_executeFlag = self.executeFlag rospy.sleep(1) #1秒スリープ
class Reaching: def __init__(self): rospy.init_node('reaching', anonymous=True)#ノードの初期化 reaching ていう名前 self.robot = moveit_commander.RobotCommander()# インスタンスの作成 self.model_pose = [0.0,0.0,0.0] #モデルの姿勢 self.obstacle_pose = [0.0,0.0,0.0] #障害物の位置 self.set_forbidden() #禁止エリアの初期化 self.set_init_pose() #位置姿勢の初期化 def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface("base_link") #PlanningSceneInterface のインスタンスの作成 self.planning_scene.removeCollisionObject("my_front_ground") self.planning_scene.removeCollisionObject("my_back_ground") self.planning_scene.removeCollisionObject("my_right_ground") self.planning_scene.removeCollisionObject("my_left_ground") #オブジェクトの削除 self.planning_scene.addCube("my_front_ground", 2, 1.1, 0, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.0, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.0, -1.0) #座標に立方体を挿入 self.planning_scene.removeCollisionObject("demo_cube") self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除 print dir(self.planning_scene) #インスタンス内のあらゆるオブジェクトの属性とメソッドのリストを表示 import inspect print "addBox's variable is ",inspect.getargspec(self.planning_scene.addBox) print "attachBox's variable is ",inspect.getargspec(self.planning_scene.attachBox) print "addCube's variable is ",inspect.getargspec(self.planning_scene.addCube) print "setColor's variable is ",inspect.getargspec(self.planning_scene.setColor) #python関数のパラメータの名前とデフォルト値を取得 def set_init_pose(self): #set init pose move_group = MoveGroupInterface("manipulator","base_link") #MoveGroupInterface のインスタンスの作成 planning_scene = PlanningSceneInterface("base_link") #PlanningSceneInterface のインスタンスの作成 joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", #ジョイントの名前を定義 "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0] move_group.moveToJointPosition(joint_names, pose, wait=False)#joint_names を pose に動かす move_group.get_move_action().wait_for_result() #wait result result = move_group.get_move_action().get_result() #result を代入 move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル def callback(self,data): #トピックにデータが追加されるたびに呼ばれる x = data.pose[1].position.x y = data.pose[1].position.y z = data.pose[1].position.z self.model_pose = [x,y,z] #modelの姿勢を代入 x = data.pose[2].position.x y = data.pose[2].position.y z = data.pose[2].position.z self.obstacle_pose = [x,y,z] #障害物の姿勢を代入 def start_subscriber(self): #購読をスタート rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback) # /gazebo/model_states から gazebo_msgs/ModelStates 型のトピックを購読 トピックが更新されるたびに self.callback を呼ぶ def target(self): rate = rospy.Rate(1) #Rateクラスのインスタンス rateを1で rospy.sleep(1) #1秒スリープ while not rospy.is_shutdown(): #シャットダウンフラグが立っていなければ、 self.planning_scene.removeCollisionObject("demo_cube") #オブジェクトの削除 self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除 self.planning_scene.addCube("demo_cube", 0.06,self.model_pose[0],self.model_pose[1],self.model_pose[2]) #一辺が0.06の正方形をmodel_pose(座標)に追加 self.planning_scene.addBox("obstacle", 0.540614, 0.083603, 0.54373, self.obstacle_pose[0],self.obstacle_pose[1],self.obstacle_pose[2])#---の大きさの箱をobstacle_pose(座標)に追加 print self.model_pose #model_poseを表示 group = moveit_commander.MoveGroupCommander("manipulator") #MoveGroupCommander クラスのインスタンス #print group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose = group.get_current_pose().pose #エンドエフェクタの位置姿勢 pose_target = geometry_msgs.msg.Pose() #geometry_msgs.pose のインスタンス これに入れるとトピックに反映される? pose_target.orientation.x = pose.orientation.x # pose_target.orientation.y = pose.orientation.y # pose_target.orientation.z = pose.orientation.z # pose_target.orientation.w = pose.orientation.w #トピックに 四元数を代入 #pose_target.position.x = pose.position.x #pose_target.position.y = pose.position.y #pose_target.position.z = pose.position.z pose_target.position.x = self.model_pose[0] # pose_target.position.y = self.model_pose[1] # pose_target.position.z = self.model_pose[2]+0.05 #トピックに 座標を代入 group.set_pose_target(pose_target) #ターゲットをセット group.go() #移動 rospy.sleep(1) #1秒スリープ
def main(): rospy.init_node("arm_obstacle_demo") wait_for_time() argv = rospy.myargv() planning_scene = PlanningSceneInterface("base_link") # Create table obstacle planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle planning_scene.removeCollisionObject('divider') size_x = 0.3 size_y = 0.01 size_z = 0.2 x = table_x - (table_size_x / 2) + (size_x / 2) y = 0 z = table_z + (table_size_z / 2) + (size_z / 2) planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 gripper = robot_api.Gripper() arm = robot_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 15, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': True, 'orientation_constraint': oc } planning_scene.removeAttachedObject('tray') gripper.open() error = arm.move_to_pose(pose1, **kwargs) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') rospy.sleep(2) frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() gripper.close() rospy.sleep(2) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') gripper.open() planning_scene.removeAttachedObject('tray') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') return
def main(): rospy.init_node('arm_obstacle_demo') wait_for_time() planning_scene = PlanningSceneInterface('base_link') # Create table obstacle planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle planning_scene.removeCollisionObject('divider') size_x = 0.3 size_y = 0.01 size_z = 0.4 x = table_x - (table_size_x / 2) + (size_x / 2) y = 0 z = table_z + (table_size_z / 2) + (size_z / 2) #planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 torso = fetch_api.Torso() torso.set_height(fetch_api.Torso.MAX_HEIGHT) arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) # and add an orientation constraint to pose 2: oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 kwargs = { 'allowed_planning_time': 100, 'execution_timeout': 100, 'num_planning_attempts': 15, 'replan': True, 'replan_attempts': 10 } # Before moving to the first pose planning_scene.removeAttachedObject('tray') error = arm.move_to_pose(pose1, **kwargs) # If the robot reaches the first pose successfully, then "attach" an object there # Of course, you would have to close the gripper first and ensure that you grasped the object properly if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() rospy.sleep(1) kwargs['orientation'] = oc error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') # At the end of your program planning_scene.removeAttachedObject('tray')
def main(): # Create table obstacle rospy.init_node('arm_obstacle_demo') planning_scene = PlanningSceneInterface('base_link') planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle # planning_scene.removeCollisionObject('divider') # size_x = 0.3 # size_y = 0.01 # size_z = 0.4 # x = table_x - (table_size_x / 2) + (size_x / 2) # y = 0 # z = table_z + (table_size_z / 2) + (size_z / 2) # planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) #Orientation constraint oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 # move to pose args kwargs = { 'allowed_planning_time': 20, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': True, 'orientation_constraint': oc } # Before moving to the first pose planning_scene.removeAttachedObject('tray') error = arm.move_to_pose(pose1, **kwargs) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() rospy.sleep(1) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') # At the end of your program planning_scene.removeAttachedObject('tray') # Was working without this but now its needed planning_scene.clear()
class Reaching: def __init__(self): rospy.init_node('reaching', anonymous=True) self.robot = moveit_commander.RobotCommander() self.model_pose = [0.0, 0.0, 0.0] self.obstacle_pose = [0.0, 0.0, 0.0] self.set_forbidden() self.set_init_pose() def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.removeCollisionObject("my_front_ground") self.planning_scene.removeCollisionObject("my_back_ground") self.planning_scene.removeCollisionObject("my_right_ground") self.planning_scene.removeCollisionObject("my_left_ground") self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) self.planning_scene.removeCollisionObject("demo_cube") self.planning_scene.removeCollisionObject("obstacle") print dir(self.planning_scene) import inspect print "addBox's variable is ", inspect.getargspec( self.planning_scene.addBox) print "attachBox's variable is ", inspect.getargspec( self.planning_scene.attachBox) print "addCube's variable is ", inspect.getargspec( self.planning_scene.addCube) print "setColor's variable is ", inspect.getargspec( self.planning_scene.setColor) def set_init_pose(sefl): #set init pose move_group = MoveGroupInterface("manipulator", "base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ] pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0] move_group.moveToJointPosition(joint_names, pose, wait=False) move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() move_group.get_move_action().cancel_all_goals() def callback(self, data): x = data.pose[1].position.x y = data.pose[1].position.y z = data.pose[1].position.z self.model_pose = [x, y, z] x = data.pose[2].position.x y = data.pose[2].position.y z = data.pose[2].position.z self.obstacle_pose = [x, y, z] def start_subscriber(self): rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback) def target(self): rate = rospy.Rate(1) rospy.sleep(1) while not rospy.is_shutdown(): self.planning_scene.removeCollisionObject("demo_cube") self.planning_scene.removeCollisionObject("obstacle") self.planning_scene.addCube("demo_cube", 0.06, self.model_pose[0], self.model_pose[1], self.model_pose[2]) self.planning_scene.addBox("obstacle", 0.540614, 0.083603, 0.54373, self.obstacle_pose[0], self.obstacle_pose[1], self.obstacle_pose[2]) print self.model_pose group = moveit_commander.MoveGroupCommander("manipulator") #print group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose = group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = pose.orientation.x pose_target.orientation.y = pose.orientation.y pose_target.orientation.z = pose.orientation.z pose_target.orientation.w = pose.orientation.w #pose_target.position.x = pose.position.x #pose_target.position.y = pose.position.y #pose_target.position.z = pose.position.z pose_target.position.x = self.model_pose[0] pose_target.position.y = self.model_pose[1] pose_target.position.z = self.model_pose[2] + 0.05 group.set_pose_target(pose_target) group.go() rospy.sleep(1)
class World: def __init__(self, tower_array, disk_array, config, debug=0): print "[INFO] Initialise World" self.DEBUG = debug # initialise arrays self.tower_array = tower_array self.disk_array = disk_array # configs self.max_gripper = config.get_max_gripper() self.disk_height = config.disk_height self.tower_positions = config.tower_positions self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) """Calls a method to display the collision objects. """ def create_planning_scene(self): print "[INFO] Create_planning_scene" self.display_towers_and_disks() """This method collects all needed information and publish them to other methods. """ def display_towers_and_disks(self): print "[INFO] Display towers and disks" for tower in self.tower_array: # call method to publish new tower self.publish_scene(tower.get_position(), tower) # set color by name self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0, 1.0) # publish color self.planning_scene_interface.sendColors() for disk in self.disk_array: self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors() # wait for sync after publishing collision objects self.planning_scene_interface.waitForSync(5.0) rospy.sleep(5) """This method creates a box or a cylinder with methods of the planning scene interface. :param position: the position of the new collision object :param tower: the tower object :param disk: the disk object """ def publish_scene(self, position, tower=None, disk=None): if tower is not None: self.planning_scene_interface.addBox(tower.get_name(), self.max_gripper / 100.0, self.max_gripper / 100.0, (self.tower_positions[tower.get_id() - 1][2] * 2), position[0], position[1], position[2]) else: self.planning_scene_interface.addCylinder(disk.get_id(), self.disk_height, disk.get_diameter() / 2, position[0], position[1], position[2]) """This method cleans the planning scene. :param close: with this flag a new planning scene objects will be removed in sync otherwise the object will be deleted without syncing the scene """ def clean_up(self, close=False): if close: # get the actual list after game self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME) for name in self.planning_scene_interface.getKnownCollisionObjects(): if self.DEBUG is 1: print("[DEBUG] Removing object %s" % name) self.planning_scene_interface.removeCollisionObject(name, False) for name in self.planning_scene_interface.getKnownAttachedObjects(): if self.DEBUG is 1: print("[DEBUG] Removing attached object %s" % name) self.planning_scene_interface.removeAttachedObject(name, False) if close: self.planning_scene_interface.waitForSync(5.0) pass """This method corrects the position of the moved disk. :param tower: parent tower of the disk """ def refresh_disk_pose(self, tower): print "[INFO] Refresh disk pose" disk = tower.get_last() if self.DEBUG is 1: print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\ "tower pose", tower.get_position() # remove the disk from planning scene self.planning_scene_interface.removeCollisionObject(disk.get_id(), False) # publish collision object and set old color self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors()
def main(): rospy.init_node('arm_obstacle_demo') wait_for_time() # argv = rospy.myargv() planning_scene = PlanningSceneInterface(frame='base_link') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') planning_scene.removeAttachedObject('tray') # Create table obstacle table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle # size_x = 0.3 # size_y = 0.01 # size_z = 0.4 # x = table_x - (table_size_x / 2) + (size_x / 2) # y = 0 # z = table_z + (table_size_z / 2) + (size_z / 2) # planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) # add orientation constraint oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 # # set torso to max height torso = robot_api.Torso() torso.set_height(robot_api.Torso.MAX_HEIGHT) arm = robot_api.Arm() # register shutdown method def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 20, 'execution_timeout': 20, 'num_planning_attempts': 10, 'replan': True } error = arm.move_to_pose(pose1, **kwargs) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') # attach an object if pose1 is successfully reached frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() # close the gripper # gripper = robot_api.Gripper() # gripper.close() rospy.sleep(1) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') # At the end of the program planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') planning_scene.removeAttachedObject('tray')
class ArmController(object): def __init__(self): # set arm and gripper, and fiducial id self.arm = fetch_api.Arm() self.gripper = fetch_api.Gripper() # Will need to change this to something real at some point #self.target_id = None# target_id self.planning_scene = PlanningSceneInterface('base_link') # First, load the poses for the fiducial insert movement self.sequence = pickle.load(open(INSERT_GRASP_POSES, "rb")) self.gripper_open = True # # Init the reader for the tags self.reader = ArTagReader() self.sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.reader.callback) def open_gripper(self): self.gripper.open() self.gripper_open = True def add_bounding_box(self): # Init the planning scene for collisions print "waiting for service...." rospy.wait_for_service('get_spines') print "found service!" get_spine_poses = rospy.ServiceProxy('get_spines', GetSpineLocations) response = get_spine_poses() # 0.17 self.planning_scene.addBox('surface', (response.surface_x_size - 0.19), response.surface_y_size, response.surface_z_size, response.surface_pose.position.x, response.surface_pose.position.y, response.surface_pose.position.z) # Adding a bounding box for the delivery table def add_delivery_bounding_box(self): print "waiting for service...." rospy.wait_for_service('get_spines') print "found service!" get_spine_poses = rospy.ServiceProxy('get_spines', GetSpineLocations) response = get_spine_poses() self.planning_scene.addBox('surface', (response.surface_x_size + 0.05), response.surface_y_size, response.surface_z_size, response.surface_pose.position.x, response.surface_pose.position.y, response.surface_pose.position.z) def grab_tray(self, target_id, target_marker_pose): # This is the same as the pbd action stuff, not making any changes at the moment self.gripper.open() self.gripper_open = True # target_marker_pose = None # check = 0 # print "Searching for fiducial...." # while target_marker_pose == None and check < 100: # # If the fiducial was not seen, try again # rospy.sleep(0.1) # check += 1 # for marker in self.reader.markers: # if target_id == marker.id: # target_marker_pose = marker.pose.pose # if target_marker_pose == None: # print "Fiducial not found :(" # return False # else: # print "Fiducial Found :)" # target_marker_pose.orientation.x = 0.0 # target_marker_pose.orientation.y = 0.0 # target_marker_pose.orientation.z = 0.0 # target_marker_pose.orientation.w = 1.0 everError = None # This is the same as the pbd action stuff, not making any changes at the moment for pbd_pose in self.sequence: move_pose = PoseStamped() move_pose.header.frame_id = 'base_link' if pbd_pose.frame == 'base_link': move_pose.pose = pbd_pose.pose print "Default pose" print pbd_pose.pose else: # for marker in reader.markers: # if target_id == marker.id: print "Calculating pose relative to marker...." # Transform the pose to be in the base_link frame pose_in_tag_frame = pose_to_transform(pbd_pose.pose) #tag_in_base_frame = pose_to_transform(marker.pose.pose) tag_in_base_frame = pose_to_transform(target_marker_pose) target_matrix = np.dot(tag_in_base_frame, pose_in_tag_frame) target_pose = transform_to_pose(target_matrix) move_pose.pose = target_pose #rospy.sleep(1) err = self.arm.move_to_pose(move_pose, num_planning_attempts=2) # 3 print "Error in move to pose: ", err temp = 0 while temp < 3 and err != None: #if err != None: #return False print "Trying This pose again" err = self.arm.move_to_pose(move_pose, num_planning_attempts=3, tolerance=0.03) print "Error in move to pose: ", err temp += 1 if err != None: print "Arm failed to move to pose" print "Returning to Default Pose" default_pose = PoseStamped() default_pose.header.frame_id = 'base_link' default_pose.pose.position.x = 0.0 default_pose.pose.position.y = 0.467635764991 default_pose.pose.position.z = 0.743876436337 default_pose.pose.orientation.x = 0.0 default_pose.pose.orientation.y = 0.0 default_pose.pose.orientation.z = 0.0 default_pose.pose.orientation.w = 1.0 err = self.arm.move_to_pose(default_pose) print "returned to default pose" return False # Check the gripper to open/close if pbd_pose.gripper_open != self.gripper_open: if self.gripper_open == True: self.gripper.close() self.gripper_open = False else: self.gripper.open() self.gripper_open = True return True def find_grasp_pose(self, target_id, target_marker_pose): # target_fiducial = None # check = 0 # while target_fiducial == None and check < 100: # # If the fiducial was not seen, try again # rospy.sleep(0.1) # check += 1 # for marker in self.reader.markers: # if marker.id == target_id: # target_fiducial = marker # if target_fiducial == None: # print "Failed to find fiducial :(" # else: # print "Found fidcuail!" check = 0 found_good_pose = False closest_pose = None print "waiting for service...." rospy.wait_for_service('get_spines') print "found service!" try: while check < 20 and found_good_pose == False: get_spine_poses = rospy.ServiceProxy('get_spines', GetSpineLocations) response = get_spine_poses() spine_poses = response.spine_poses min_dist = float('inf') for pose in spine_poses: #distance = calculate_euclidean_distance(pose, target_fiducial.pose.pose) distance = calculate_euclidean_distance( pose, target_marker_pose) if distance < min_dist: min_dist = distance closest_pose = pose check += 1 if closest_pose != None: #if closest_pose.position.x > target_fiducial.pose.pose.position.x and closest_pose.position.y < (target_fiducial.pose.pose.position.y + 0.025) and closest_pose.position.y > (target_fiducial.pose.pose.position.y - 0.025): if closest_pose.position.x > target_marker_pose.position.x and closest_pose.position.y < ( target_marker_pose.position.y + 0.025) and closest_pose.position.y > ( target_marker_pose.position.y - 0.025): found_good_pose = True except rospy.ServiceException, e: print "Service call failed: %s" % e # print target_fiducial.id # closest_pose = None # min_dist = float('inf') # for pose in spine_poses: # distance = calculate_euclidean_distance(pose, target_fiducial.pose.pose) # if distance < min_dist: # min_dist = distance # closest_pose = pose if found_good_pose == False: print "Failed to find good pose" return None else: print "Found good pose" #print "Pose closest to target fiducial" #print closest_pose return closest_pose
class Reaching: def __init__(self): rospy.init_node('reaching', anonymous=True) self.robot = moveit_commander.RobotCommander() # self.model_pose = [0.0,0.2,0.2 , 0,0,0,1] self.model_pose = [0.0, 0.3, 0.1, 0, 0, 0, 1] self.obstacle_pose = [0.0, 0.0, 0.0] self.set_forbidden() self.set_init_pose() def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.removeCollisionObject("my_ground") self.planning_scene.addCube("my_ground", 2, 0, 0, -1.04) self.planning_scene.addBox("obstacle", 0.1, 0.5, 1, -0.3, 0, 0.5) self.planning_scene.addBox("obstacle2", 0.5, 0.1, 1, 0, -0.3, 0.5) #self.planning_scene.addCube("demo_cube", 0.06,0,0.3,0) print dir(self.planning_scene) import inspect print "addBox's variable is ", inspect.getargspec( self.planning_scene.addBox) print "attachBox's variable is ", inspect.getargspec( self.planning_scene.attachBox) print "addCube's variable is ", inspect.getargspec( self.planning_scene.addCube) print "setColor's variable is ", inspect.getargspec( self.planning_scene.setColor) def set_init_pose(sefl): #set init pose move_group = MoveGroupInterface("manipulator", "base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ] pose = [-1.26, -0.64, -2.44, -0.66, 1.56, 0.007] move_group.moveToJointPosition(joint_names, pose, wait=False) move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() move_group.get_move_action().cancel_all_goals() def callback(self, data): x = data.pose[1].position.x y = data.pose[1].position.y z = data.pose[1].position.z def start_subscriber(self): rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback) def reaching_pose(self): print self.model_pose group = moveit_commander.MoveGroupCommander("manipulator") #print group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose = group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = self.model_pose[3] pose_target.orientation.y = self.model_pose[4] pose_target.orientation.z = self.model_pose[5] pose_target.orientation.w = self.model_pose[6] pose_target.position.x = self.model_pose[0] pose_target.position.y = self.model_pose[1] pose_target.position.z = self.model_pose[2] group.set_pose_target(pose_target) group.go() #raw_input("Enter -->") def gripper_close(self): print("Gripper_Close") gripper_pose = Char() gripper_pose = 255 pub.publish(gripper_pose) #raw_input("Enter -->") def gripper_open(self): print("Gripper_Open") gripper_pose = Char() gripper_pose = 0 pub.publish(gripper_pose) #raw_input("Enter -->") def move_45(self): rate = rospy.Rate(1) rospy.sleep(1) a = 0.065 b = 0.040 c = 0.012 pattern_45 = [ [-0.10, 0.31, 0.015, 0, 0, 0, 1], #B 'c', [-0.10, 0.31, 0.1, 0, 0, 0, 1], #B #[0.0 ,0.31 ,0.15 , 0,0,0,1], #A90 [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795], [0, 0.5, 0.0365, 0.3826834, 0, 0, 0.9238795], 'o', [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795], #[0.0 ,0.31 ,0.1 , 0,0,0,1], #A90 [-0.10, 0.401, 0.041, 0, 0, 0, 1], 'c', [-0.10, 0.401, 0.100, 0, 0, 0, 1], [0, 0.5 - a, 0.037 + a, 0.3826834, 0, 0, 0.9238795], [0, 0.5 - b, 0.037 + b, 0.3826834, 0, 0, 0.9238795], [0, 0.5 - c, 0.037 + c, 0.3826834, 0, 0, 0.9238795], 'o', 'c', [0, 0.5 - a, 0.037 + a, 0.3826834, 0, 0, 0.9238795], [-0.10, 0.401, 0.100, 0, 0, 0, 1], [-0.10, 0.401, 0.050, 0, 0, 0, 1], 'o', [-0.10, 0.401, 0.100, 0, 0, 0, 1], [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795], [0, 0.5, 0.0365, 0.3826834, 0, 0, 0.9238795], 'c', [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795], [-0.10, 0.31, 0.1, 0, 0, 0, 1], #B [-0.10, 0.31, 0.015, 0, 0, 0, 1], #B 'o', [-0.10, 0.31, 0.1, 0, 0, 0, 1], #B ] while not rospy.is_shutdown(): for pose in pattern_45: if pose == 'c': self.gripper_close() rospy.sleep(1) elif pose == 'o': self.gripper_open() rospy.sleep(1) else: self.model_pose = pose self.reaching_pose() if rospy.is_shutdown(): return 0 def move_90(self): rate = rospy.Rate(1) rospy.sleep(1) a = 0.065 b = 0.040 c = 0.012 pattern_45 = [ [-0.10, 0.31, 0.015, 0, 0, 0, 1], #B 'c', [-0.10, 0.31, 0.1, 0, 0, 0, 1], #B [0.0, 0.300, 0.1, 0, 0, 0, 1], #A90 [0.0, 0.315, 0.0358, 0, 0, 0, 1], #A90 'o', [0.0, 0.315, 0.1, 0, 0, 0, 1], #A90 [-0.10, 0.40, 0.041, 0, 0, 0, 1], #negi 'c', [-0.10, 0.40, 0.1, 0, 0, 0, 1], [-0.001, 0.316, 0.1, 0, 0, 0, 1], #A90 [-0.001, 0.316, 0.053, 0, 0, 0, 1], #A90 'o', 'c', [0.0, 0.314, 0.1, 0, 0, 0, 1], #A90 [-0.10, 0.40, 0.1, 0, 0, 0, 1], [-0.10, 0.40, 0.05, 0, 0, 0, 1], #negi 'o', #[-0.10,0.40 ,0.1 , 0,0,0,1], [0.0, 0.300, 0.1, 0, 0, 0, 1], #A90 [0.0, 0.315, 0.036, 0, 0, 0, 1], #A90 'c', [0.0, 0.300, 0.1, 0, 0, 0, 1], #A90 [-0.10, 0.320, 0.1, 0, 0, 0, 1], #B [-0.10, 0.31, 0.0153, 0, 0, 0, 1], #B 'o', [-0.10, 0.31, 0.1, 0, 0, 0, 1], #B ] while not rospy.is_shutdown(): for pose in pattern_45: if pose == 'c': self.gripper_close() rospy.sleep(1) elif pose == 'o': self.gripper_open() rospy.sleep(1) else: self.model_pose = pose self.reaching_pose() if rospy.is_shutdown(): return 0
def main(): rospy.init_node("book_grasp_procedure") wait_for_time() # First, load the poses for the fiducial insert movement sequence = pickle.load( open(INSERT_GRASP_POSES, "rb") ) gripper = fetch_api.Gripper() arm = fetch_api.Arm() gripper_open = True # # Init the reader for the tags reader = ArTagReader() sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback) rospy.sleep(0.5) # Init the planning scene for collisions planning_scene = PlanningSceneInterface('base_link') print "waiting for service...." rospy.wait_for_service('get_spines') print "found service!" get_spine_poses = rospy.ServiceProxy('get_spines', GetSpineLocations) response = get_spine_poses() planning_scene.addBox('surface', (response.surface_x_size - 0.17), (response.surface_y_size), response.surface_z_size, response.surface_pose.position.x, response.surface_pose.position.y, response.surface_pose.position.z) gripper.open() gripper_open = True target_marker_pose = None check = 0 print "Searching for fiducial...." while target_marker_pose == None and check < 100: # If the fiducial was not seen, try again rospy.sleep(0.1) check += 1 for marker in reader.markers: if TARGET_ID == marker.id: target_marker_pose = marker.pose.pose if target_marker_pose == None: print "Failed to find fiducial" print "Surface position z" print response.surface_pose.position.z print "target marker position z" print target_marker_pose.position.z # This is the same as the pbd action stuff, not making any changes at the moment for pbd_pose in sequence: move_pose = PoseStamped() move_pose.header.frame_id = 'base_link' if pbd_pose.frame == 'base_link': move_pose.pose = pbd_pose.pose else: # for marker in reader.markers: # if TARGET_ID == marker.id: print "Calculating pose relative to marker...." # Transform the pose to be in the base_link frame pose_in_tag_frame = pose_to_transform(pbd_pose.pose) #tag_in_base_frame = pose_to_transform(marker.pose.pose) tag_in_base_frame = pose_to_transform(target_marker_pose) target_matrix = np.dot(tag_in_base_frame, pose_in_tag_frame) target_pose = transform_to_pose(target_matrix) move_pose.pose = target_pose rospy.sleep(1) err = arm.move_to_pose(move_pose, replan=True) print "Error in move to pose: ", err # Check the gripper to open/close if pbd_pose.gripper_open != gripper_open: if gripper_open == True: gripper.close() gripper_open = False else: gripper.open() gripper_open = True print "Take this opportunity to change to a different mock point cloud, if necessary" user_input = raw_input("Press enter to continue") # After this we calculate the spine_pose closest to the fiducial. I will test that if I can get the service call working target_fiducial = None check = 0 print "Searching for fiducial" while target_fiducial == None and check < 100: # If the fiducial was not seen, try again rospy.sleep(0.1) check += 1 for marker in reader.markers: if marker.id == TARGET_ID: target_fiducial = marker if target_fiducial == None: print "Failed to find fiducial" print target_fiducial.id # Now, we make a request to the perception side of things spine_poses = [] # code for checking check = 0 found_good_pose = False closest_pose = None print "waiting for service...." rospy.wait_for_service('get_spines') print "found service!" print "Searching for a good grasp pose...." try: while check < 20 and found_good_pose == False: get_spine_poses = rospy.ServiceProxy('get_spines', GetSpineLocations) response = get_spine_poses() spine_poses = response.spine_poses min_dist = float('inf') for pose in spine_poses: distance = calculate_euclidean_distance(pose, target_fiducial.pose.pose) if distance < min_dist: min_dist = distance closest_pose = pose check += 1 if closest_pose.position.x > target_fiducial.pose.pose.position.x and closest_pose.position.y < (target_fiducial.pose.pose.position.y + 0.025) and closest_pose.position.y > (target_fiducial.pose.pose.position.y - 0.025): found_good_pose = True # debugging line # for pose in spine_poses: # print pose except rospy.ServiceException, e: print "Service call failed: %s"%e
class World: def __init__(self, tower_array, disk_array, config, debug=0): print "[INFO] Initialise World" self.DEBUG = debug # initialise arrays self.tower_array = tower_array self.disk_array = disk_array # configs self.max_gripper = config.get_max_gripper() self.disk_height = config.disk_height self.tower_positions = config.tower_positions self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) """Calls a method to display the collision objects. """ def create_planning_scene(self): print "[INFO] Create_planning_scene" self.display_towers_and_disks() """This method collects all needed information and publish them to other methods. """ def display_towers_and_disks(self): print "[INFO] Display towers and disks" for tower in self.tower_array: # call method to publish new tower self.publish_scene(tower.get_position(), tower) # set color by name self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0, 1.0) # publish color self.planning_scene_interface.sendColors() for disk in self.disk_array: self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors() # wait for sync after publishing collision objects self.planning_scene_interface.waitForSync(5.0) rospy.sleep(5) """This method creates a box or a cylinder with methods of the planning scene interface. :param position: the position of the new collision object :param tower: the tower object :param disk: the disk object """ def publish_scene(self, position, tower=None, disk=None): if tower is not None: self.planning_scene_interface.addBox( tower.get_name(), self.max_gripper / 100.0, self.max_gripper / 100.0, (self.tower_positions[tower.get_id() - 1][2] * 2), position[0], position[1], position[2]) else: self.planning_scene_interface.addCylinder(disk.get_id(), self.disk_height, disk.get_diameter() / 2, position[0], position[1], position[2]) """This method cleans the planning scene. :param close: with this flag a new planning scene objects will be removed in sync otherwise the object will be deleted without syncing the scene """ def clean_up(self, close=False): if close: # get the actual list after game self.planning_scene_interface = PlanningSceneInterface( REFERENCE_FRAME) for name in self.planning_scene_interface.getKnownCollisionObjects(): if self.DEBUG is 1: print("[DEBUG] Removing object %s" % name) self.planning_scene_interface.removeCollisionObject(name, False) for name in self.planning_scene_interface.getKnownAttachedObjects(): if self.DEBUG is 1: print("[DEBUG] Removing attached object %s" % name) self.planning_scene_interface.removeAttachedObject(name, False) if close: self.planning_scene_interface.waitForSync(5.0) pass """This method corrects the position of the moved disk. :param tower: parent tower of the disk """ def refresh_disk_pose(self, tower): print "[INFO] Refresh disk pose" disk = tower.get_last() if self.DEBUG is 1: print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\ "tower pose", tower.get_position() # remove the disk from planning scene self.planning_scene_interface.removeCollisionObject( disk.get_id(), False) # publish collision object and set old color self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors()
class Reaching: def __init__(self): rospy.init_node('reaching', anonymous=True) #ノードの初期化 reaching ていう名前 self.robot = moveit_commander.RobotCommander() # インスタンスの作成 self.model_pose = [0.0, 0.0, 0.0] #モデルの姿勢 self.obstacle_pose = [0.0, 0.0, 0.0] #障害物の位置 self.subsc_pose = [0.0, 0.0, 0.0] #購読した座標 self.subsc_orientation = [0.0, 0.0, 0.0, 1.0] #購読した四元数 self.set_forbidden() #禁止エリアの初期化 self.set_init_pose() #位置姿勢の初期化 self.before_pose = [0, 0, 0] #前回の場所 self.pcarray = 0 #pointcloud を np array にしたものを入れる self.executeFlag = False #unity コントローラ B ボタンと対応 self.before_executeFlag = False #前フレームの executeFlag self.waypoints = [] self.way_flag = False #unity コントローラの 中指のボタンに対応 self.before_wayflag = False #前フレームのway_flag self.infirst_frame = True #最初のフレーム self.calc_way_flag = False self.frame = 0 # def set_ZEDbase(self): # listener = tf.TransformListener() # listener.waitForTransform('world','ee_link',rospy.Time(0),rospy.Duration(2.0)) # (self.trans,rot)=listener.lookupTransform('/world','/ee_link',rospy.Time(0)) # trans(x,y,z) rot (x,y,z,w) # print('trans = '+str(self.trans)) # br = tf.TransformBroadcaster() # br.sendTransform((self.trans[0],self.trans[1],self.trans[2]),(0.0,0.0,0.0,1.0),rospy.Time.now(),'base_forZED','world') def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface( "base_link") #PlanningSceneInterface のインスタンスの作成 self.planning_scene.removeCollisionObject("my_ground") self.planning_scene.addCube("my_ground", 2, 0, 0, -1.0) self.planning_scene.addBox("obstacle", 0.1, 0.5, 1, -0.3, 0, 0.5) self.planning_scene.addBox("obstacle2", 0.5, 0.1, 1, 0, -0.3, 0.5) self.planning_scene.addCube("demo_cube", 0.06, 0, 0.3, 0) print dir(self.planning_scene) #インスタンス内のあらゆるオブジェクトの属性とメソッドのリストを表示 import inspect print "addBox's variable is ", inspect.getargspec( self.planning_scene.addBox) print "attachBox's variable is ", inspect.getargspec( self.planning_scene.attachBox) print "addCube's variable is ", inspect.getargspec( self.planning_scene.addCube) print "setColor's variable is ", inspect.getargspec( self.planning_scene.setColor) #python関数のパラメータの名前とデフォルト値を取得 def set_init_pose(self): #set init pose move_group = MoveGroupInterface( "manipulator", "base_link") #MoveGroupInterface のインスタンスの作成 planning_scene = PlanningSceneInterface( "base_link") #PlanningSceneInterface のインスタンスの作成 joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", #ジョイントの名前を定義 "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ] # pose = [-1.26 , -0.64 , -2.44 , -0.66 , 1.56 , 0.007] # move_group.moveToJointPosition(joint_names, pose, wait=False)#joint_names を pose に動かす # move_group.get_move_action().wait_for_result() #wait result # result = move_group.get_move_action().get_result() #result を代入 move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル self.state = { 'default': 0, 'hold': 1, 'way_standby': 2, 'plan_standby': 3 } # default : 何もしていない target のセットか hold button を待つ # hold : waypoint のセットを受け付ける hold button が離れたら終了 #way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合 #plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合 self.now_state = self.state['default'] #現在フレームの状態 # self.set_ZEDbase() def callback(self, data): #トピックにデータが追加されるたびに呼ばれる x = data.pose[1].position.x y = data.pose[1].position.y z = data.pose[1].position.z self.model_pose = [x, y, z] #modelの姿勢を代入 x = data.pose[2].position.x y = data.pose[2].position.y z = data.pose[2].position.z self.obstacle_pose = [x, y, z] #障害物の姿勢を代入 def callbacksub(self, data): x = data.x y = data.y z = data.z self.subsc_pose = [x, y, z] #購読した座標を代入 def callbackunity(self, data): x = data.pose.position.x y = data.pose.position.y z = data.pose.position.z self.subsc_pose = [y, -x, z] #unity からの座標 座標変換も行っている x = data.pose.orientation.x y = data.pose.orientation.y z = data.pose.orientation.z w = data.pose.orientation.w self.subsc_orientation = [y, -x, z, w] #unity からの四元数 def rtabcallback(self, data): # dtype_list = [(f.name, np.float32) for f in data.fields] # cloud_arr = np.fromstring(data.data, dtype_list) # self.pcarray = np.reshape(cloud_arr, (data.height, data.width)) # print(self.pcarray) print('rtab subscribed ') def excallback(self, data): self.executeFlag = data.data #print(data.data) def waycallback(self, data): self.way_flag = data.data self.wayflagcalc() def wayflagcalc(self): if self.before_wayflag != self.way_flag: if self.calc_way_flag: self.calc_way_flag = False else: self.calc_way_flag = True self.before_wayflag = self.way_flag def start_subscriber(self): #購読をスタート rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback) # /gazebo/model_states から gazebo_msgs/ModelStates 型のトピックを購読 トピックが更新されるたびに self.callback を呼ぶ #rospy.Subscriber("/UR/reaching/pose",Vector3,self.callbacksub) rospy.Subscriber("/unity/target", PoseStamped, self.callbackunity) #unityから配信しているトピックを購読 #rospy.Subscriber('/rtabmap/cloud_map', PointCloud2, self.rtabcallback) #zed で slam したpointcloud を購読 rospy.Subscriber('/unity/execute', Bool, self.excallback) rospy.Subscriber('/unity/wayflag', Bool, self.waycallback) rospy.Subscriber('/pcl/near_points', PointCloud2, self.create_object) def change_state(self, target): #self.state = {'default':0,'hold':1,'way_standby':2,'plan_standby':3} # default : 何もしていない target のセットか hold button を待つ # hold : waypoint のセットを受け付ける hold button が離れたら終了 #way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合 target が指定されたら plan_standby に以降 #plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合 set_flag = (self.before_pose != target.position ) # target の位置が変化している = トリガーが押された #self.calc_way_flag # hold button が押されている exe_flag = (self.before_executeFlag != self.executeFlag ) # executeFlag が前フレームと違う = execute button が押された print('set ' + str(set_flag)) print('hold ' + str(self.calc_way_flag)) print('exe ' + str(exe_flag)) print('state ' + str(self.now_state)) if self.now_state == self.state['default']: if set_flag: self.now_state = self.state['plan_standby'] elif self.calc_way_flag: self.now_state = self.state['hold'] elif self.now_state == self.state['hold']: if not self.calc_way_flag: self.now_state = self.state['way_standby'] elif self.now_state == self.state['way_standby']: if set_flag: self.now_state = self.state['plan_standby'] elif exe_flag: self.now_state = self.state['default'] elif self.now_state == self.state['plan_standby']: if exe_flag: self.now_state = self.state['default'] elif self.calc_way_flag: self.now_state = self.state['hold'] def create_object(self, data): self.Points = [data[0:3] for data in pc2.read_points(data)] def target(self): rate = rospy.Rate(1) #Rateクラスのインスタンス rateを1で rospy.sleep(1) #1秒スリープ useway = False while not rospy.is_shutdown(): #シャットダウンフラグが立っていなければ、 print self.model_pose #model_poseを表示 group = moveit_commander.MoveGroupCommander( "manipulator") #MoveGroupCommander クラスのインスタンス #print group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose = group.get_current_pose().pose #エンドエフェクタの位置姿勢 pose_target = geometry_msgs.msg.Pose( ) #geometry_msgs.pose のインスタンス ここにターゲット設定 pose_target.orientation.x = self.subsc_orientation[0] pose_target.orientation.y = self.subsc_orientation[1] pose_target.orientation.z = self.subsc_orientation[2] pose_target.orientation.w = self.subsc_orientation[ 3] #トピックから 四元数を代入 pose_target.position.x = self.subsc_pose[0] # pose_target.position.y = self.subsc_pose[1] # pose_target.position.z = self.subsc_pose[2] #トピックから 座標を代入 # br = tf.TransformBroadcaster() # br.sendTransform((self.trans[0],self.trans[1],self.trans[2]),(0.0,0.0,0.8509035,0.525322),rospy.Time.now(),'base_forZED','world') # #逐一ブロードキャストする if self.infirst_frame: self.before_pose = pose_target.position self.infirst_frame = False #self.planning_scene.removeCollisionObject("targetOBJ") #self.planning_scene.addCube("targetOBJ", 0.005,self.subsc_pose[0] ,self.subsc_pose[1] ,self.subsc_pose[2] ) self.way_flag = False #self.state={'default':0,'hold':1,'way_standby':2,'plan_standby':3} # default : 何もしていない target のセットか hold button を待つ # hold : waypoint のセットを受け付ける hold button が離れたら終了 #way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合 #plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合 exe_flag = (self.before_executeFlag != self.executeFlag ) # executeFlag が前フレームと違う = execute button が押された if self.now_state == self.state['default']: if self.before_pose != pose_target.position: group.set_pose_target(pose_target) print(' Set Target !!!\n') print(pose_target) if self.now_state == self.state['hold']: if self.before_pose != pose_target.position: self.waypoints.append(copy.deepcopy(pose_target)) print(' Append Target !!!') if self.now_state == self.state['way_standby']: plan, fraction = group.compute_cartesian_path( self.waypoints, 0.01, 0.0) if self.before_pose != pose_target.position: group.set_pose_target(pose_target) print(' Set Target !!!\n') print(pose_target) elif exe_flag: group.execute(plan) print(' Planning Execute !!!') print(self.waypoints) self.waypoints = [] if self.now_state == self.state['plan_standby']: group.set_pose_target(pose_target) group.plan() if self.before_pose != pose_target.position: group.set_pose_target(pose_target) print(' Set Target !!!\n') print(pose_target) elif exe_flag: group.go() print(' Planning Go !!!') self.change_state(pose_target) self.before_executeFlag = self.executeFlag self.before_pose = pose_target.position self.frame += 1 rospy.sleep(2) #2秒スリープ