def main(): rospy.init_node('moveit_py_place', anonymous=True) #right_arm.set_planner_id("KPIECEkConfigDefault"); scene = PlanningSceneInterface() robot = RobotCommander() #group = MoveGroupCommander("head") right_arm = MoveGroupCommander("right_arm") #right_arm.set_planner_id("KPIECEkConfigDefault"); rospy.logwarn("cleaning world") GRIPPER_FRAME = 'gripper_bracket_f2' #scene.remove_world_object("table") scene.remove_world_object("part") scene.remove_attached_object(GRIPPER_FRAME, "part") p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0.67 p.pose.position.y = -0. p.pose.position.z = 0.75 scene.add_box("part", p, (0.07, 0.01, 0.2)) # move to a random target #group.set_named_target("ahead") #group.go() #rospy.sleep(1) result = False n_attempts = 0 # repeat until will succeed while result == False: result = robot.right_arm.pick("part") n_attempts += 1 print "Attempts pickup: ", n_attempts rospy.sleep(0.2) #robot.right_arm.pick("part") #right_arm.go() rospy.sleep(5)
class CokeCanPickAndPlace: def __init__(self): # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~arm', 'arm') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.6) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander: self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) # Clean the scene: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: self._pose_table = self._add_table(self._table_object_name) self._pose_coke_can = self._add_coke_can(self._grasp_object_name) rospy.sleep(1.0) # Define target place pose: self._pose_place = Pose() self._pose_place.position.x = self._pose_coke_can.position.x self._pose_place.position.y = self._pose_coke_can.position.y + 0.02 self._pose_place.position.z = self._pose_coke_can.position.z self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown('Grasp generator action client not available!') return # Create move group 'pickup' action client: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # Create move group 'place' action client: self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return # Pick Coke can object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') # Place Coke can object on another place on the support surface (table): while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Place successfully') def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._table_object_name) def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose def _add_coke_can(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.25 p.pose.position.y = 0 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.01, 0.01, 0.009)) return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp """ # Generate places: places = [] now = rospy.Time.now() for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle) place.place_pose.pose.orientation = Quaternion(*q) # Generate pre place approach: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame() place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = -1 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame() place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 1 # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) return places def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) goal.support_surface_name = self._table_object_name # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _pickup(self, group, target, width): """ Pick up a target using the planning group """ # Obtain possible grasps from the grasp generator server: grasps = self._generate_grasps(self._pose_coke_can, width) # Create and send Pickup goal: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _place(self, group, target, place): """ Place a target using the planning group """ # Obtain possible places: places = self._generate_places(place) # Create and send Place goal: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg) def _publish_places(self, places): """ Publish places as poses, using a PoseArray message """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg)
eef = MoveGroupCommander("endeffector") rospy.sleep(1) #Start State Gripper group_variable_values = eef.get_current_joint_values() group_variable_values[0] = 0.0 eef.set_joint_value_target(group_variable_values) plan2 = eef.plan() arm.execute(plan2) # clean the scene scene.remove_world_object("pole") scene.remove_world_object("table") scene.remove_world_object("part") # publish a demo scene k = PoseStamped() k.header.frame_id = robot.get_planning_frame() k.pose.position.x = 0.0 k.pose.position.y = 0.0 k.pose.position.z = -0.05 scene.add_box("table", k, (2, 2, 0.0001)) p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0.35 p.pose.position.y = 0.35 p.pose.position.z = 0.05 scene.add_box("part", p, (0.09, 0.09, 0.09)) rospy.sleep(1) #Planning Part 1: Move to correct xy coorinate temPose = p print temPose
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander(GROUP_NAME_ARM) right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) #right_arm.set_goal_position_tolerance(0.005) right_arm.set_goal_orientation_tolerance(0.005) right_arm.set_planning_time(5) right_arm.set_num_planning_attempts(5) eef = right_arm.get_end_effector_link() rospy.sleep(2) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) #scene.remove_attached_object(GRIPPER_FRAME, "part") # clean the scene #scene.remove_world_object("table") #scene.remove_world_object("part") right_arm.set_named_target("default") right_arm.go(wait=True) right_gripper.set_named_target("open") right_gripper.go(wait=True) rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add an object to be grasped p.pose.position.x = 0.170 p.pose.position.y = 0.04 p.pose.position.z = 0.3 #scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) start_pose = PoseStamped() start_pose.header.frame_id = FIXED_FRAME # start the gripper in a neutral pose part way to the target start_pose.pose.position.x = -0.00756585784256 start_pose.pose.position.y = -0.225419849157 start_pose.pose.position.z = 0.117192693055 start_pose.pose.orientation.x = 0.95493721962 start_pose.pose.orientation.y = -0.0160209629685 start_pose.pose.orientation.z = -0.00497157918289 start_pose.pose.orientation.w = 0.296333402395 print("going to pick up pose") right_arm.set_pose_target(start_pose) right_gripper.set_named_target("close") right_arm.go(wait=True) right_gripper.go(wait=True) rospy.sleep(1) right_arm.set_named_target("default") right_arm.go(wait=True) next_pose = PoseStamped() next_pose.header.frame_id = FIXED_FRAME next_pose.pose.position.x = -0.100732862949 next_pose.pose.position.y = -0.210876911879 next_pose.pose.position.z = 0.244678631425 next_pose.pose.orientation.x = 0.784905433655 next_pose.pose.orientation.y = -0.177844554186 next_pose.pose.orientation.z = -0.131161093712 next_pose.pose.orientation.w = 0.578870952129 right_arm.set_pose_target(next_pose) right_gripper.set_named_target("open") right_arm.go(wait=True) right_gripper.go(wait=True) rospy.sleep(3) right_arm.set_named_target("default") right_arm.go(wait=True) rospy.spin() roscpp_shutdown()
class TestPlanners(object): def __init__(self, group_id, planner): rospy.init_node('moveit_test_planners', anonymous=True) self.pass_list = [] self.fail_list = [] self.planner = planner self.scene = PlanningSceneInterface() self.robot = RobotCommander() self.group = MoveGroupCommander(group_id) rospy.sleep(1) self.group.set_planner_id(self.planner) self.test_trajectories_empty_environment() self.test_waypoints() self.test_trajectories_with_walls_and_ground() for pass_test in self.pass_list: print(pass_test) for fail_test in self.fail_list: print(fail_test) def _add_walls_and_ground(self): # publish a scene p = geometry_msgs.msg.PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.pose.position.x = 0 p.pose.position.y = 0 # offset such that the box is below ground (to prevent collision with # the robot itself) p.pose.position.z = -0.11 p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 self.scene.add_box("ground", p, (3, 3, 0.1)) p.pose.position.x = 0.4 p.pose.position.y = 0.85 p.pose.position.z = 0.4 p.pose.orientation.x = 0.5 p.pose.orientation.y = -0.5 p.pose.orientation.z = 0.5 p.pose.orientation.w = 0.5 self.scene.add_box("wall_front", p, (0.8, 2, 0.01)) p.pose.position.x = 1.33 p.pose.position.y = 0.4 p.pose.position.z = 0.4 p.pose.orientation.x = 0.0 p.pose.orientation.y = -0.707388 p.pose.orientation.z = 0.0 p.pose.orientation.w = 0.706825 self.scene.add_box("wall_right", p, (0.8, 2, 0.01)) p.pose.position.x = -0.5 p.pose.position.y = 0.4 p.pose.position.z = 0.4 p.pose.orientation.x = 0.0 p.pose.orientation.y = -0.707107 p.pose.orientation.z = 0.0 p.pose.orientation.w = 0.707107 self.scene.add_box("wall_left", p, (0.8, 2, 0.01)) # rospy.sleep(1) def _check_plan(self, plan): if len(plan.joint_trajectory.points) > 0: return True else: return False def _plan_joints(self, joints): self.group.clear_pose_targets() group_variable_values = self.group.get_current_joint_values() group_variable_values[0:6] = joints[0:6] self.group.set_joint_value_target(group_variable_values) plan = self.group.plan() return self._check_plan(plan) def test_trajectories_rotating_each_joint(self): # test_joint_values = [numpy.pi/2.0, numpy.pi-0.33, -numpy.pi/2] test_joint_values = [numpy.pi / 2.0] joints = [0.0, 0.0, 0.0, -numpy.pi / 2.0, 0.0, 0.0] # Joint 4th is colliding with the hand # for joint in range(6): for joint in [0, 1, 2, 3, 5]: for value in test_joint_values: joints[joint] = value if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_rotating_each_joint, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_rotating_each_joint, " + self.planner + ", joints:" + str(joints)) def test_trajectories_empty_environment(self): # Up - Does not work with sbpl but it does with ompl joints = [0.0, -1.99, 2.19, 0.58, 0.0, -0.79, 0.0, 0.0] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) # All joints up joints = [ -1.67232, -2.39104, 0.264862, 0.43346, 2.44148, 2.48026, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) # Down joints = [ -0.000348431194526, 0.397651011661, 0.0766181197394, -0.600353691727, -0.000441966540076, 0.12612019707, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) # left joints = [ 0.146182953165, -2.6791929848, -0.602721109682, -3.00575848765, 0.146075718452, 0.00420656698366, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) # Front joints = [ 1.425279839, -0.110370375874, -1.52548746261, -1.50659865247, -1.42700242769, 3.1415450794, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) # Behind joints = [ 1.57542451065, 3.01734161219, 2.01043257686, -1.14647092839, 0.694689321451, -0.390769365032, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) # Should fail because it is in self-collision joints = [ -0.289797803762, 2.37263860495, 2.69118483159, 1.65486712181, 1.04235601797, -1.69730925867, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_empty_environment, " + self.planner + ", joints:" + str(joints)) def test_waypoints(self): # Start planning in a given joint position joints = [ -0.324590029242, -1.42602359749, -1.02523472017, -0.754761892979, 0.344227622185, -3.03250264451, 0.0, 0.0 ] current = RobotState() current.joint_state.name = self.robot.get_current_state( ).joint_state.name current_joints = list( self.robot.get_current_state().joint_state.position) current_joints[0:8] = joints current.joint_state.position = current_joints self.group.set_start_state(current) waypoints = [] initial_pose = self.group.get_current_pose().pose initial_pose.position.x = -0.301185959729 initial_pose.position.y = 0.517069787724 initial_pose.position.z = 1.20681710541 initial_pose.orientation.x = 0.0207499700474 initial_pose.orientation.y = -0.723943002716 initial_pose.orientation.z = -0.689528413407 initial_pose.orientation.w = 0.00515118111221 # start with a specific position waypoints.append(initial_pose) # first move it down wpose = geometry_msgs.msg.Pose() wpose.orientation = waypoints[0].orientation wpose.position.x = waypoints[0].position.x wpose.position.y = waypoints[0].position.y wpose.position.z = waypoints[0].position.z - 0.20 waypoints.append(copy.deepcopy(wpose)) # second front wpose.position.y += 0.20 waypoints.append(copy.deepcopy(wpose)) # third side wpose.position.x -= 0.20 waypoints.append(copy.deepcopy(wpose)) # fourth return to initial pose wpose = waypoints[0] waypoints.append(copy.deepcopy(wpose)) (plan3, fraction) = self.group.compute_cartesian_path(waypoints, 0.01, 0.0) if not self._check_plan(plan3) and fraction > 0.8: self.fail_list.append("Failed: test_waypoints, " + self.planner) else: self.pass_list.append("Passed: test_waypoints, " + self.planner) def test_trajectories_with_walls_and_ground(self): self._add_walls_and_ground() # Should fail to plan: Goal is in collision with the wall_front joints = [ 0.302173213174, 0.192487443763, -1.94298265002, 1.74920382275, 0.302143499777, 0.00130280337897, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_with_walls_and_ground, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_with_walls_and_ground, " + self.planner + ", joints:" + str(joints)) # Should fail to plan: Goal is in collision with the ground joints = [ 3.84825722288e-05, 0.643694953509, -1.14391175311, 1.09463824437, 0.000133883149666, -0.594498939239, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_with_walls_and_ground, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed: test_trajectories_with_walls_and_ground, " + self.planner + ", joints:" + str(joints)) # Goal close to right corner joints = [ 0.354696232081, -0.982224980654, 0.908055961723, -1.92328051116, -1.3516255551, 2.8225061435, 0.0, 0.0 ] if not self._plan_joints(joints): self.fail_list.append( "Failed: test_trajectories_with_walls_and_ground, " + self.planner + ", joints:" + str(joints)) else: self.pass_list.append( "Passed, test_trajectories_with_walls_and_ground, " + self.planner + ", joints:" + str(joints)) self.scene.remove_world_object("ground") self.scene.remove_world_object("wall_left") self.scene.remove_world_object("wall_right") self.scene.remove_world_object("wall_front")
def init(): global marker_array_pub, marker_pub, tf_broadcaster, tf_listener global move_group, turntable global camera_mesh, camera_pos, camera_size, min_distance, max_distance, num_positions, num_spins, object_size, photobox_pos, photobox_size, reach, simulation, test, turntable_pos, working_dir rospy.init_node('acquisition') camera_mesh = rospy.get_param('~camera_mesh', None) camera_orientation = rospy.get_param('~camera_orientation', None) camera_pos = rospy.get_param('~camera_pos', [0.0, 0.0, 0.0]) camera_size = rospy.get_param('~camera_size', [0.1, 0.1, 0.1]) min_distance = rospy.get_param('~min_distance', 0.2) max_distance = rospy.get_param('~max_distance', min_distance) max_velocity = rospy.get_param('~max_velocity', 0.1) num_positions = rospy.get_param('~num_positions', 15) num_spins = rospy.get_param('~num_spins', 8) object_size = numpy.array(rospy.get_param('~object_size', [0.2, 0.2, 0.2])) photobox_pos = rospy.get_param('~photobox_pos', [0.0, -0.6, 0.0]) photobox_size = rospy.get_param('~photobox_size', [0.7, 0.7, 1.0]) ptpip = rospy.get_param('~ptpip', None) reach = rospy.get_param('~reach', 0.85) simulation = '/gazebo' in get_node_names() test = rospy.get_param('~test', True) turntable_pos = rospy.get_param( '~turntable_pos', photobox_pos[:2] + [photobox_pos[2] + 0.05]) turntable_radius = rospy.get_param('~turntable_radius', 0.2) wall_thickness = rospy.get_param('~wall_thickness', 0.04) marker_array_pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=1, latch=True) marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=1, latch=True) tf_broadcaster = tf.TransformBroadcaster() tf_listener = tf.TransformListener() move_group = MoveGroupCommander('manipulator') move_group.set_max_acceleration_scaling_factor( 1.0 if simulation else max_velocity) move_group.set_max_velocity_scaling_factor( 1.0 if simulation else max_velocity) robot = RobotCommander() scene = PlanningSceneInterface(synchronous=True) try: st_control = load_source( 'st_control', os.path.join(RosPack().get_path('iai_scanning_table'), 'scripts', 'iai_scanning_table', 'st_control.py')) turntable = st_control.ElmoUdp() if turntable.check_device(): turntable.configure() turntable.reset_encoder() turntable.start_controller() turntable.set_speed_deg(30) except Exception as e: print(e) sys.exit('Could not connect to turntable.') if simulation: move_home() elif not test: working_dir = rospy.get_param('~working_dir', None) if working_dir is None: sys.exit('Working directory not specified.') elif not camera.init( os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'out', working_dir, 'images'), ptpip): sys.exit('Could not initialize camera.') # add ground plane ps = PoseStamped() ps.header.frame_id = robot.get_planning_frame() scene.add_plane('ground', ps) # add photobox ps.pose.position.x = photobox_pos[ 0] + photobox_size[0] / 2 + wall_thickness / 2 ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_left', ps, (wall_thickness, photobox_size[1], photobox_size[2])) ps.pose.position.x = photobox_pos[ 0] - photobox_size[0] / 2 - wall_thickness / 2 ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_right', ps, (wall_thickness, photobox_size[1], photobox_size[2])) ps.pose.position.x = photobox_pos[0] ps.pose.position.y = photobox_pos[ 1] - photobox_size[1] / 2 - wall_thickness / 2 ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_back', ps, (photobox_size[0], wall_thickness, photobox_size[2])) ps.pose.position.x = photobox_pos[0] ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] - wall_thickness / 2 scene.add_box('box_ground', ps, (photobox_size[0], photobox_size[1], wall_thickness)) # add turntable turntable_height = turntable_pos[2] - photobox_pos[2] ps.pose.position.x = turntable_pos[0] ps.pose.position.y = turntable_pos[1] ps.pose.position.z = photobox_pos[2] + turntable_height / 2 scene.add_cylinder('turntable', ps, turntable_height, turntable_radius) # add object on turntable ps.pose.position.x = turntable_pos[0] ps.pose.position.y = turntable_pos[1] ps.pose.position.z = turntable_pos[2] + object_size[2] / 2 scene.add_cylinder('object', ps, object_size[2], max(object_size[:2]) / 2) # add cable mounts scene.remove_attached_object('upper_arm_link', 'upper_arm_cable_mount') scene.remove_attached_object('forearm_link', 'forearm_cable_mount') scene.remove_world_object('upper_arm_cable_mount') scene.remove_world_object('forearm_cable_mount') if ptpip is None and not simulation: size = [0.08, 0.08, 0.08] ps.header.frame_id = 'upper_arm_link' ps.pose.position.x = -0.13 ps.pose.position.y = -0.095 ps.pose.position.z = 0.135 scene.attach_box(ps.header.frame_id, 'upper_arm_cable_mount', ps, size) ps.header.frame_id = 'forearm_link' ps.pose.position.x = -0.275 ps.pose.position.y = -0.08 ps.pose.position.z = 0.02 scene.attach_box(ps.header.frame_id, 'forearm_cable_mount', ps, size) # add camera eef_link = move_group.get_end_effector_link() ps = PoseStamped() ps.header.frame_id = eef_link scene.remove_attached_object(eef_link, 'camera_0') scene.remove_attached_object(eef_link, 'camera_1') scene.remove_world_object('camera_0') scene.remove_world_object('camera_1') ps.pose.position.y = camera_pos[1] ps.pose.position.z = camera_pos[2] if camera_mesh: ps.pose.position.x = camera_pos[0] quaternion = tf.transformations.quaternion_from_euler( math.radians(camera_orientation[0]), math.radians(camera_orientation[1]), math.radians(camera_orientation[2])) ps.pose.orientation.x = quaternion[0] ps.pose.orientation.y = quaternion[1] ps.pose.orientation.z = quaternion[2] ps.pose.orientation.w = quaternion[3] scene.attach_mesh(eef_link, 'camera_0', ps, os.path.expanduser(camera_mesh), camera_size) if not simulation: scene.attach_mesh(eef_link, 'camera_1', ps, os.path.expanduser(camera_mesh), numpy.array(camera_size) * 1.5) vertices = scene.get_attached_objects( ['camera_0'])['camera_0'].object.meshes[0].vertices camera_size[0] = max(vertice.x for vertice in vertices) - min( vertice.x for vertice in vertices) camera_size[1] = max(vertice.y for vertice in vertices) - min( vertice.y for vertice in vertices) camera_size[2] = max(vertice.z for vertice in vertices) - min( vertice.z for vertice in vertices) else: ps.pose.position.x = camera_pos[0] + camera_size[0] / 2 scene.attach_box(eef_link, 'camera_0', ps, camera_size)
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander("right_arm") right_gripper = MoveGroupCommander("right_gripper") eef = right_arm.get_end_effector_link() rospy.sleep(2) scene.remove_attached_object("right_gripper_link", "part") # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") right_arm.set_named_target("start1") right_arm.go() right_gripper.set_named_target("open") right_gripper.go() rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add a table #p.pose.position.x = 0.42 #p.pose.position.y = -0.2 #p.pose.position.z = 0.3 #scene.add_box("table", p, (0.5, 1.5, 0.6)) # add an object to be grasped p.pose.position.x = 0.15 p.pose.position.y = -0.12 p.pose.position.z = 0.7 scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) g = Grasp() g.id = "test" start_pose = PoseStamped() start_pose.header.frame_id = FIXED_FRAME # start the gripper in a neutral pose part way to the target start_pose.pose.position.x = 0.0130178 start_pose.pose.position.y = -0.125155 start_pose.pose.position.z = 0.597653 start_pose.pose.orientation.x = 0.0 start_pose.pose.orientation.y = 0.388109 start_pose.pose.orientation.z = 0.0 start_pose.pose.orientation.w = 0.921613 right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(2) # generate a list of grasps #grasps = self.make_grasps(start_pose) #result = False #n_attempts = 0 # repeat until will succeed #while result == False: #result = robot.right_arm.pick("part", grasps) #n_attempts += 1 #print "Attempts: ", n_attempts #rospy.sleep(0.2) rospy.spin() roscpp_shutdown()
class add_box: def __init__(self): # Retrieve params: self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object2_name = rospy.get_param('~grasp_object_name', 'Grasp_Object-2') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.1) # Create (debugging) publishers:创建(调试)发布者: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander:创建规划场景和机器人命令: self._scene = PlanningSceneInterface()#未知 self._robot = RobotCommander()#未知 rospy.sleep(0.5) # Clean the scene:清理现场: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._grasp_object2_name) # Add table and Coke can objects to the planning scene:将桌子和可乐罐对象添加到计划场景: self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)#增加障碍物块 self._pose_coke_can = self._add_grasp_fanfkuai_(self._grasp_object2_name)#增加障碍物块 rospy.sleep(0.5) def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._grasp_object2_name) #----------------------------------1hang--------------------------------- def _add_grasp_block_(self, name): """ Create and add block to the scene创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.6 p.pose.position.y = -0.0 p.pose.position.z = 0.75 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.045, 0.045, 0.045)) return p.pose def _add_grasp_fanfkuai_(self, name): """ Create and add block to the scene创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.6 p.pose.position.y = 0.4 p.pose.position.z = 0.75 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.045, 0.045, 0.045)) return p.pose
class WarehousePlanner(object): def __init__(self): rospy.init_node('moveit_warehouse_planner', anonymous=True) self.scene = PlanningSceneInterface() self.robot = RobotCommander() rospy.sleep(2) group_id = rospy.get_param("~arm_group_name") self.eef_step = rospy.get_param("~eef_step", 0.01) self.jump_threshold = rospy.get_param("~jump_threshold", 1000) self.group = MoveGroupCommander(group_id) # self._add_ground() self._robot_name = self.robot._r.get_robot_name() self._robot_link = self.group.get_end_effector_link() self._robot_frame = self.group.get_pose_reference_frame() self._min_wp_fraction = rospy.get_param("~min_waypoint_fraction", 0.9) rospy.sleep(4) rospy.loginfo("Waiting for warehouse services...") rospy.wait_for_service('/list_robot_states') rospy.wait_for_service('/get_robot_state') rospy.wait_for_service('/has_robot_state') rospy.wait_for_service('/compute_fk') self._list_states = rospy.ServiceProxy('/list_robot_states', ListStates) self._get_state = rospy.ServiceProxy('/get_robot_state', GetState) self._has_state = rospy.ServiceProxy('/has_robot_state', HasState) self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK) rospy.loginfo("Service proxies connected") self._tr_frm_list_srv = rospy.Service('plan_trajectory_from_list', PlanTrajectoryFromList, self._plan_from_list_cb) self._tr_frm_prfx_srv = rospy.Service('plan_trajectory_from_prefix', PlanTrajectoryFromPrefix, self._plan_from_prefix_cb) self._execute_tr_srv = rospy.Service('execute_planned_trajectory', ExecutePlannedTrajectory, self._execute_plan_cb) self.__plan = None def get_waypoint_names_by_prefix(self, prefix): regex = "^" + str(prefix) + ".*" waypoint_names = self._list_states(regex, self._robot_name).states return waypoint_names def get_pose_from_state(self, state): header = Header() fk_link_names = [self._robot_link] header.frame_id = self._robot_frame response = self._forward_k(header, fk_link_names, state) return response.pose_stamped[0].pose def get_cartesian_waypoints(self, waypoint_names): waypoints = [] waypoints.append(self.group.get_current_pose().pose) for name in waypoint_names: if self._has_state(name, self._robot_name).exists: robot_state = self._get_state(name, "").state waypoints.append(self.get_pose_from_state(robot_state)) else: rospy.logerr("Waypoint %s doesn't exist for robot %s.", name, self._robot_name) return waypoints def _add_ground(self): p = geometry_msgs.msg.PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.pose.position.x = 0 p.pose.position.y = 0 # offset such that the box is below the dome p.pose.position.z = -0.11 p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 self.scene.add_box("ground", p, (3, 3, 0.01)) rospy.sleep(1) def plan_from_filter(self, prefix): waypoint_names = self.get_waypoint_names_by_prefix(prefix) waypoint_names.sort() if 0 == len(waypoint_names): rospy.logerr( "No waypoints found for robot %s with prefix %s. " + "Can't make trajectory :(", self._robot_name, str(prefix)) return False rospy.loginfo( "Creating trajectory with %d waypoints selected by " + "prefix %s.", len(waypoint_names), str(prefix)) return self.plan_from_list(waypoint_names) def plan_from_list(self, waypoint_names): self.group.clear_pose_targets() waypoints = self.get_cartesian_waypoints(waypoint_names) if len(waypoints) != len(waypoint_names) + 1: # +1 because current position is used as first waypiont. rospy.logerr("Not all waypoints existed, not executing.") return False (plan, fraction) = self.group.compute_cartesian_path(waypoints, self.eef_step, self.jump_threshold) if fraction < self._min_wp_fraction: rospy.logerr( "Only managed to generate trajectory through" + "%f of waypoints. Not executing", fraction) return False self.__plan = plan return True def _plan_from_list_cb(self, request): # check all exist self.__plan = None rospy.loginfo("Creating trajectory from points given: %s", ",".join(request.waypoint_names)) return self.plan_from_list(request.waypoint_names) def _plan_from_prefix_cb(self, request): self.__plan = None rospy.loginfo("Creating trajectory from points filtered by prefix %s", request.prefix) return self.plan_from_filter(request.prefix) def _execute_plan_cb(self, request): if self.__plan is None: rospy.logerr("No plan stored!") return False rospy.loginfo("Executing stored plan") response = self.group.execute(self.__plan) self.__plan = None return response
def main(): rospy.init_node('Baxter_Env') robot = RobotCommander() scene = PlanningSceneInterface() scene._scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=0) rospy.sleep(2) # centre table p1 = PoseStamped() p1.header.frame_id = robot.get_planning_frame() p1.pose.position.x = 1. # position of the table in the world frame p1.pose.position.y = 0. p1.pose.position.z = 0. # left table (from baxter's perspective) p1_l = PoseStamped() p1_l.header.frame_id = robot.get_planning_frame() p1_l.pose.position.x = 0.5 # position of the table in the world frame p1_l.pose.position.y = 1. p1_l.pose.position.z = 0. p1_l.pose.orientation.x = 0.707 p1_l.pose.orientation.y = 0.707 # right table (from baxter's perspective) p1_r = PoseStamped() p1_r.header.frame_id = robot.get_planning_frame() p1_r.pose.position.x = 0.5 # position of the table in the world frame p1_r.pose.position.y = -1. p1_r.pose.position.z = 0. p1_r.pose.orientation.x = 0.707 p1_r.pose.orientation.y = 0.707 # open back shelf p2 = PoseStamped() p2.header.frame_id = robot.get_planning_frame() p2.pose.position.x = 1.2 # position of the table in the world frame p2.pose.position.y = 0.0 p2.pose.position.z = 0.75 p2.pose.orientation.x = 0.5 p2.pose.orientation.y = -0.5 p2.pose.orientation.z = -0.5 p2.pose.orientation.w = 0.5 pw = PoseStamped() pw.header.frame_id = robot.get_planning_frame() # add an object to be grasped p_ob1 = PoseStamped() p_ob1.header.frame_id = robot.get_planning_frame() p_ob1.pose.position.x = .92 p_ob1.pose.position.y = 0.3 p_ob1.pose.position.z = 0.89 # the ole duck p_ob2 = PoseStamped() p_ob2.header.frame_id = robot.get_planning_frame() p_ob2.pose.position.x = 0.87 p_ob2.pose.position.y = -0.4 p_ob2.pose.position.z = 0.24 # clean environment scene.remove_world_object("table_center") scene.remove_world_object("table_side_left") scene.remove_world_object("table_side_right") scene.remove_world_object("shelf") scene.remove_world_object("wall") scene.remove_world_object("part") scene.remove_world_object("duck") rospy.sleep(1) scene.add_box("table_center", p1, size=(.5, 1.5, 0.4)) # dimensions of the table scene.add_box("table_side_left", p1_l, size=(.5, 1.5, 0.4)) scene.add_box("table_side_right", p1_r, size=(.5, 1.5, 0.4)) scene.add_mesh("shelf", p2, "./bookshelf_light.stl", size=(.025, .01, .01)) scene.add_plane("wall", pw, normal=(0, 1, 0)) part_size = (0.07, 0.05, 0.12) scene.add_box("part", p_ob1, size=part_size) scene.add_mesh("duck", p_ob2, "./duck.stl", size=(.001, .001, .001)) rospy.sleep(1) print(scene.get_known_object_names()) ## ---> SET COLOURS table_color = color_norm([105, 105, 105]) # normalize colors to range [0, 1] shelf_color = color_norm([139, 69, 19]) duck_color = color_norm([255, 255, 0]) _colors = {} setColor('table_center', _colors, table_color, 1) setColor('table_side_left', _colors, table_color, 1) setColor('table_side_right', _colors, table_color, 1) setColor('shelf', _colors, shelf_color, 1) setColor('duck', _colors, duck_color, 1) sendColors(_colors, scene)
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander(GROUP_NAME_ARM) #right_arm.set_planner_id("KPIECEkConfigDefault"); right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) eef = right_arm.get_end_effector_link() rospy.sleep(2) scene.remove_attached_object(GRIPPER_FRAME, "part") # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") #right_arm.set_named_target("r_start") #right_arm.go() #right_gripper.set_named_target("right_gripper_open") #right_gripper.go() rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add a table #p.pose.position.x = 0.42 #p.pose.position.y = -0.2 #p.pose.position.z = 0.3 #scene.add_box("table", p, (0.5, 1.5, 0.6)) # add an object to be grasped p.pose.position.x = 0.7 p.pose.position.y = -0.2 p.pose.position.z = 0.85 scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) g = Grasp() g.id = "test" start_pose = PoseStamped() start_pose.header.frame_id = FIXED_FRAME # start the gripper in a neutral pose part way to the target start_pose.pose.position.x = 0.47636 start_pose.pose.position.y = -0.21886 start_pose.pose.position.z = 0.9 start_pose.pose.orientation.x = 0.00080331 start_pose.pose.orientation.y = 0.001589 start_pose.pose.orientation.z = -2.4165e-06 start_pose.pose.orientation.w = 1 right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(2) # generate a list of grasps grasps = self.make_grasps(start_pose) result = False n_attempts = 0 # repeat until will succeed while result == False: result = robot.right_arm.pick("part", grasps) n_attempts += 1 print "Attempts: ", n_attempts rospy.sleep(0.3) rospy.spin() roscpp_shutdown()
# scene.add_box("table", p, (0.5, 1.5, 0.7)) rospy.sleep(1) #bring the arm near the object (aim_x, aim_y, aim_z, aim_yaw) = onine_arm.get_valid_pose(item_translation[0], item_translation[1], item_translation[2], -0.20) # onine_arm.ready() # onine_arm.open_gripper() # onine_arm.go(aim_x, aim_y, aim_z, aim_yaw) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = item_translation[0] p.pose.position.y = item_translation[1] p.pose.position.z = item_translation[2] scene.add_box("target", p, (0.01, 0.01, 0.08)) # rospy.sleep(20) # (aim_x, aim_y, aim_z, aim_yaw) = onine_arm.get_valid_pose(item_translation[0], item_translation[1], item_translation[2], - 0.080) grasp_pose = PoseStamped() grasp_pose.header.frame_id = "left_gripper_link" grasp_pose.header.stamp = rospy.Time.now() grasp_pose.pose.position.x = aim_x grasp_pose.pose.position.y = aim_y grasp_pose.pose.position.z = aim_z grasp_pose.pose.orientation = Quaternion(
class planning: def __init__(self): # Retrieve params: # if there is no param named 'table_object_name' then use the default # set the scene in rzvi # original initilization is for 2 self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') # table self._grasp_object_name_1 = rospy.get_param('~grasp_object_name', 'Grasp_Object1') # box1 self._grasp_object_name_2 = rospy.get_param( '~grasp_object_name2', 'Wall_behind') # wall_behind as a obstacle self._grasp_object_name_3 = rospy.get_param( '~grasp_object_name3', 'Wall_above') # wall_above as a obstacle self._grasp_object_name_screw = rospy.get_param( '~grasp_object_name5', 'screw') # screw #print self._grasp_object_name_screw self._grasp_object_name_capscrew4 = rospy.get_param( '~grasp_object_name4', 'capscrew4') # capscrew #print self._grasp_object_name_capscrew4 #### Not needed in the model spawn Section #### # Create (debugging) publishers(display in rviz): #self._grasps_pub = rospy.Publisher( # 'grasps', PoseArray, queue_size=1, latch=True) #self._places_pub = rospy.Publisher( # 'places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander: self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) # Clean the scene: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name_1) self._scene.remove_world_object( self._grasp_object_name_2) #wall_behind #self._scene.remove_world_object(self._grasp_object_name_3) #wall_above self._scene.remove_world_object( self._grasp_object_name_capscrew4) #capscrew4 self._scene.remove_world_object(self._grasp_object_name_screw) #screw ##############3# Add table and Coke can objects to the planning scene:##################################### self._pose_table = self._add_table(self._table_object_name) self._add_grasp_block_1(self._grasp_object_name_1) # ##########################################3Adding obstacle_behind############################# p_wall = PoseStamped() robot = moveit_commander.RobotCommander() p_wall.header.frame_id = robot.get_planning_frame() p_wall.header.stamp = rospy.Time.now() p_wall.pose.position.x = -0.25 p_wall.pose.position.y = 0.0 p_wall.pose.position.z = 1.5 q_wall_temp = quaternion_from_euler(0.0, 80.0, 0.0) #Q number p_wall.pose.orientation = Quaternion(*q_wall_temp) self._scene.add_box(self._grasp_object_name_2, p_wall, (3, 3, 0.05)) ###################################### Adding obstacle wall_above###################################### p_wall2 = PoseStamped() p_wall2.header.frame_id = robot.get_planning_frame() p_wall2.header.stamp = rospy.Time.now() p_wall2.pose.position.x = table_x p_wall2.pose.position.y = table_y p_wall2.pose.position.z = 1.25 q_wall2_temp = quaternion_from_euler(0.0, 0.0, 0.0) #Q number p_wall2.pose.orientation = Quaternion(*q_wall2_temp) #self._scene.add_box(self._grasp_object_name_3, p_wall2, (3, 3, 0.05)) ###################################### Adding obstacle side-1##################################### p_wall3 = PoseStamped() p_wall3.header.frame_id = robot.get_planning_frame() p_wall3.header.stamp = rospy.Time.now() p_wall3.pose.position.x = table_x p_wall3.pose.position.y = table_y - 0.8 / 2 - 0.2 p_wall3.pose.position.z = table_z q_wall3_temp = quaternion_from_euler(80.0, 0.0, 0.0) #Q number p_wall3.pose.orientation = Quaternion(*q_wall3_temp) self._scene.add_box("wall_side-1", p_wall3, (2, 2, 0.05)) ###################################### Adding obstacle side -2###################################### p_wall4 = PoseStamped() p_wall4.header.frame_id = robot.get_planning_frame() p_wall4.header.stamp = rospy.Time.now() p_wall4.pose.position.x = table_x p_wall4.pose.position.y = table_y + 0.8 / 2 + 0.05 p_wall4.pose.position.z = table_z q_wall4_temp = quaternion_from_euler(80.0, 0.0, 0.0) #Q number p_wall4.pose.orientation = Quaternion(*q_wall4_temp) self._scene.add_box("wall_side-2", p_wall4, (2, 2, 0.05)) ##############################################adding screw########################### screw = PoseStamped() screw.header.frame_id = robot.get_planning_frame() screw.header.stamp = rospy.Time.now() screw.pose.position.x = table_x screw.pose.position.y = table_y - 0.2 screw.pose.position.z = box1_z + 0.02 screw_q = quaternion_from_euler(0.0, 0.0, 0.0) #Q number screw.pose.orientation = Quaternion(*screw_q) print self._scene.add_box(self._grasp_object_name_screw, screw, (0.036, 0.046, 0.032)) ##############################################adding capscrew 0.045########################### capscrew4 = PoseStamped() capscrew4.header.frame_id = robot.get_planning_frame() capscrew4.header.stamp = rospy.Time.now() capscrew4.pose.position.x = table_x capscrew4.pose.position.y = table_y capscrew4.pose.position.z = box1_z + 0.02 q4 = quaternion_from_euler(0.0, 0.0, 0.0) #Q number capscrew4.pose.orientation = Quaternion(*q4) print self._scene.add_box(self._grasp_object_name_capscrew4, capscrew4, (0.05, 0.05, 0.040)) rospy.sleep(1.0) # gripper graspping point setting. # Define target place pose: #########################################Section: Neccessay function for (displaying)Rviz################################### ######this section for adding stuffs in the planning scene.(table, box1, box2, etc...) in RViz############################3 def _add_table(self, name): """ Create and add table to the scene """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = table_x p.pose.position.y = table_y p.pose.position.z = table_z q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. # display in RZvi print self._scene.add_box(name, p, (1, 1, 0.05)) #arg(name, pose, size) return p.pose def _add_grasp_block_1(self, name): """ Create and add block to the scene """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = box1_x + 0.3 p.pose.position.y = box1_y + 0.3 p.pose.position.z = box1_z q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. #self._scene.add_box(name, p, (0.045, 0.045, 0.045)) return p.pose
class Pick_Place: def __init__(self): # rospy.init_node('pick_and_place') self.scene = PlanningSceneInterface() self.robot = RobotCommander() filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_mobile_manipulator', 'joints_setup.yaml') with open(filename) as file: joints_setup = yaml.load(file) jointslimit = joints_setup["joints_limit"] home_value = joints_setup["home_value"] j1 = home_value["joint_1"] j2 = home_value["joint_2"] j3 = home_value["joint_3"] j4 = home_value["joint_4"] j5 = home_value["joint_5"] j6 = home_value["joint_6"] g = home_value["gripper"] self.set_home_value([j1, j2, j3, j4, j5, j6, g]) home_value = joints_setup["pick_place_home_value"] j1 = home_value["joint_1"] j2 = home_value["joint_2"] j3 = home_value["joint_3"] j4 = home_value["joint_4"] j5 = home_value["joint_5"] j6 = home_value["joint_6"] g = home_value["gripper"] self.set_pick_place_home_value([j1, j2, j3, j4, j5, j6, g]) self.object_list = {} self.obstacle_list = {} filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_mobile_manipulator', 'interfaces', 'models_info.yaml') with open(filename) as file: objects_info = yaml.load(file) rospy.loginfo("Spawning Objects in Gazebo and planning scene") objects = objects_info["objects"] objects_name = objects.keys() for object_name in objects_name: name = object_name shape = objects[name]["shape"] color = objects[name]["color"] x = objects[name]["pose"]["x"] y = objects[name]["pose"]["y"] z = objects[name]["pose"]["z"] roll = objects[name]["pose"]["roll"] pitch = objects[name]["pose"]["pitch"] yaw = objects[name]["pose"]["yaw"] object_pose = self.pose2msg(x, y, z, roll, pitch, yaw) p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = x p.pose.position.y = y p.pose.position.z = z q = quaternion_from_euler(roll, pitch, yaw) p.pose.orientation = Quaternion(*q) if shape == "box": x = objects[name]["size"]["x"] y = objects[name]["size"]["y"] z = objects[name]["size"]["z"] p.pose.position.z += z / 2 height = z width = y length = x self.object_list[name] = Object(p.pose, p.pose, height, width, length, shape, color) elif shape == "cylinder": height = objects[name]["size"]["height"] radius = objects[name]["size"]["radius"] p.pose.position.z += height / 2 self.object_list[name] = Object(p.pose, p.pose, height, radius * 2, radius * 2, shape, color) elif shape == "sphere": radius = objects[name]["size"] p.pose.position.z += radius self.object_list[name] = Object(p.pose, p.pose, radius * 2, radius * 2, radius * 2, shape, color) objects = objects_info["objects"] obstacles = objects_info["obstacles"] obstacles_name = obstacles.keys() for object_name in obstacles_name: name = object_name x = obstacles[name]["pose"]["x"] y = obstacles[name]["pose"]["y"] z = obstacles[name]["pose"]["z"] roll = obstacles[name]["pose"]["roll"] pitch = obstacles[name]["pose"]["pitch"] yaw = obstacles[name]["pose"]["yaw"] object_pose = self.pose2msg(x, y, z, roll, pitch, yaw) p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = x p.pose.position.y = y p.pose.position.z = z x = obstacles[name]["size"]["x"] y = obstacles[name]["size"]["y"] z = obstacles[name]["size"]["z"] p.pose.position.z += z / 2 height = z width = y length = x self.obstacle_list[name] = Obstacle(p.pose, p.pose, height, width, length) # self.object_list = object_list self.goal_list = {} self.set_target_info() self.gripper_width = {} self.set_gripper_width_relationship() self.arm = moveit_commander.MoveGroupCommander("ur10_manipulator") self.arm.set_goal_tolerance(0.01) self.arm.set_pose_reference_frame("ur10_base_link") self.gripperpub = rospy.Publisher("gripper_controller/command", JointTrajectory, queue_size=0) self.transform_arm_to_baselink = Point() self.get_arm_to_baselink() self.gripper_length = 0.33 self.get_workspace() self.message_pub = rospy.Publisher("/gui_message", String, queue_size=0) self.updatepose_pub = rospy.Publisher("/updatepose", Bool, queue_size=0) self.robot_pose = Pose() self.odom_sub = rospy.Subscriber("/odom", Odometry, self.robot_pose_callback) def send_message(self, message): msg = String() msg.data = message self.message_pub.publish(msg) def updatepose_trigger(self, value): msg = Bool() msg.data = value self.updatepose_pub.publish(msg) def clean_scene(self, object_name): self.scene.remove_world_object(object_name) def clean_all_objects_in_scene(self): objects_name = self.object_list.keys() for object_name in objects_name: self.clean_scene(object_name) def spawn_all_objects(self): objects_name = self.object_list.keys() for object_name in objects_name: self.spawn_object_rviz(object_name) def spawn_object_rviz(self, object_name): self.clean_scene(object_name) this_object = self.object_list[object_name] p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose = copy.deepcopy(this_object.abs_pose) robot_pose = self.get_robot_pose() p.pose.position.x -= robot_pose.position.x p.pose.position.y -= robot_pose.position.y quaternion = (robot_pose.orientation.x, robot_pose.orientation.y, robot_pose.orientation.z, robot_pose.orientation.w) euler = euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1] yaw = -euler[2] quat = quaternion_from_euler(roll, pitch, yaw) p.pose.orientation.x = quat[0] p.pose.orientation.y = quat[1] p.pose.orientation.z = quat[2] p.pose.orientation.w = quat[3] self.object_list[object_name].relative_pose = self.baselink2arm(p.pose) shape = this_object.shape if shape == "box": x = this_object.length y = this_object.width z = this_object.height size = (x, y, z) self.scene.add_box(object_name, p, size) elif shape == "cylinder": height = this_object.height radius = this_object.width / 2 self.scene.add_cylinder(object_name, p, height, radius) elif shape == "sphere": radius = this_object.width / 2 self.scene.add_sphere(object_name, p, radius) rospy.sleep(0.5) def spawn_obstacle_rviz(self, obstacle_name): self.clean_scene(obstacle_name) this_object = self.obstacle_list[obstacle_name] p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose = copy.deepcopy(this_object.abs_pose) robot_pose = self.get_robot_pose() # p.pose.position.x -= robot_pose.position.x # p.pose.position.y -= robot_pose.position.y quaternion = (robot_pose.orientation.x, robot_pose.orientation.y, robot_pose.orientation.z, robot_pose.orientation.w) euler = euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1] yaw = -euler[2] quat = quaternion_from_euler(roll, pitch, yaw) p.pose.orientation.x = quat[0] p.pose.orientation.y = quat[1] p.pose.orientation.z = quat[2] p.pose.orientation.w = quat[3] x = p.pose.position.x - robot_pose.position.x y = p.pose.position.y - robot_pose.position.y p.pose.position.x = math.cos(yaw) * x - math.sin(yaw) * y p.pose.position.y = math.sin(yaw) * x + math.cos(yaw) * y x = this_object.length y = this_object.width z = this_object.height size = (x, y, z) self.scene.add_box(obstacle_name, p, size) rospy.sleep(0.5) def set_target_info(self): filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_mobile_manipulator', 'interfaces', 'models_info.yaml') with open(filename) as file: objects_info = yaml.load(file) robot_x = objects_info["robot"]["pose"]["x"] robot_y = objects_info["robot"]["pose"]["y"] robot_z = objects_info["robot"]["pose"]["z"] targets = objects_info["targets"] target_name = targets.keys() for name in target_name: position = Point() position.x = targets[name]["x"] - robot_x position.y = targets[name]["y"] - robot_y position.z = targets[name]["z"] - robot_z self.goal_list[name] = position def set_gripper_width_relationship(self): filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_mobile_manipulator', 'interfaces', 'models_info.yaml') with open(filename) as file: objects_info = yaml.load(file) gripper_joint_value = objects_info["gripper_joint_value"] objects_width = gripper_joint_value.keys() for object_width in objects_width: self.gripper_width[object_width] = gripper_joint_value[ object_width] def get_object_list(self): return self.object_list.keys() def get_target_list(self): return self.goal_list.keys() def get_object_pose(self, object_name): return copy.deepcopy(self.object_list[object_name].relative_pose) def get_object_info(self, object_name): this_object = copy.deepcopy(self.object_list[object_name]) pose = this_object.relative_pose height = this_object.height width = this_object.width length = this_object.length shape = this_object.shape color = this_object.color return pose, height, width, length, shape, color def get_target_position(self, target_name): position = copy.deepcopy(self.goal_list[target_name]) # print("before transformation",position) p = Pose() p.position = position robot_pose = self.get_robot_pose() quaternion = (robot_pose.orientation.x, robot_pose.orientation.y, robot_pose.orientation.z, robot_pose.orientation.w) euler = euler_from_quaternion(quaternion) yaw = -euler[2] x = position.x - robot_pose.position.x y = position.y - robot_pose.position.y position.x = math.cos(yaw) * x - math.sin(yaw) * y position.y = math.sin(yaw) * x + math.cos(yaw) * y # print("after transformation",position, yaw) position = self.baselink2arm(p).position return position def robot_pose_callback(self, msg): self.robot_pose.position = msg.pose.pose.position self.robot_pose.orientation = msg.pose.pose.orientation def get_robot_pose(self): # try: # listener = tf.TransformListener() # (trans,rot) = listener.lookupTransform('/map', "/base_link", rospy.Time(0)) # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): # rospy.loginfo("no transformation") # return # pose = Pose() # pose.position.x = trans[0] # pose.position.y = trans[1] # pose.position.z = trans[2] # pose.orientation.x = rot[0] # pose.orientation.y = rot[1] # pose.orientation.z = rot[2] # pose.orientation.w = rot[3] return self.robot_pose def get_arm_to_baselink(self): # try: # listener = tf.TransformListener() # (trans,rot) = listener.lookupTransform('/base_link', "/ur10_base_link", rospy.Time(0)) # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): # rospy.loginfo("no transformation") # return # self.transform_arm_to_baselink.x = trans[0] # self.transform_arm_to_baselink.y = trans[1] # self.transform_arm_to_baselink.z = trans[2] # print(self.transform_arm_to_baselink) self.transform_arm_to_baselink.x = 0.205 self.transform_arm_to_baselink.y = 0 self.transform_arm_to_baselink.z = 0.802 def get_workspace(self): filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_mobile_manipulator', 'joints_setup.yaml') with open(filename) as file: joints_setup = yaml.load(file) workspace = joints_setup["workspace"] x = workspace["center"]["x"] y = workspace["center"]["y"] z = workspace["center"]["z"] min_r = workspace["r"]["min"] max_r = workspace["r"]["max"] min_z = workspace["min_z"] self.workspace = WorkSpace(x, y, z, min_r, max_r, min_z) # check if the position is inside workspace def is_inside_workspace(self, x, y, z): if z > self.workspace.min_z: dx = x - self.workspace.x dy = y - self.workspace.y dz = z - self.workspace.z r = math.sqrt(dx**2 + dy**2 + dz**2) if self.workspace.min_r < r < self.workspace.max_r: return True return False # move one joint of the arm to value def set_arm_joint(self, joint_id, value): joint_goal = self.arm.get_current_joint_values() joint_goal[joint_id - 1] = value self.arm.go(joint_goal, wait=True) self.arm.stop() # Forward Kinematics (FK): move the arm by axis values def move_joint_arm(self, joint_0, joint_1, joint_2, joint_3, joint_4, joint_5): joint_goal = self.arm.get_current_joint_values() joint_goal[0] = joint_0 joint_goal[1] = joint_1 joint_goal[2] = joint_2 joint_goal[3] = joint_3 joint_goal[4] = joint_4 joint_goal[5] = joint_5 self.arm.go(joint_goal, wait=True) self.arm.stop() # To guarantee no residual movement def get_joint_value(self, joint_id): joints = self.arm.get_current_joint_values() return joints[joint_id - 1] def get_joints_value(self): joints = self.arm.get_current_joint_values() return joints def get_arm_pose(self): pose = self.arm.get_current_pose().pose pose = self.baselink2arm(pose) pose = self.TCP2gripper(pose, self.gripper_length) # print(pose) return self.msg2pose(pose) def set_random_pose(self): self.arm.set_random_target() def set_gripper_length(self, length): self.gripper_length = length def set_home_value(self, home_value): self.home_value = home_value def back_to_home(self, move_gripper=True): j1, j2, j3, j4, j5, j6, g = self.home_value self.move_joint_arm(j1, j2, j3, j4, j5, j6) if move_gripper: self.move_joint_hand(g) def set_pick_place_home_value(self, home_value): self.pick_place_home_value = home_value def move_to_pick_place_home(self, move_gripper=True): j1, j2, j3, j4, j5, j6, g = self.pick_place_home_value self.move_joint_arm(j1, j2, j3, j4, j5, j6) if move_gripper: self.move_joint_hand(g) def fold_robot_arm(self, ): self.move_joint_arm(0, 0, 0, 0, 0, 0) self.move_joint_hand(0) def gripper2TCP(self, pose, length=0): roll, pitch, yaw, x, y, z = self.msg2pose(pose) T = euler_matrix(roll, pitch, yaw) T[0:3, 3] = numpy.array([x, y, z]) pos_gripper_tcp = numpy.array([0, -length, 0, 1]) pos_tcp = T.dot(pos_gripper_tcp) pose.position.x = pos_tcp[0] pose.position.y = pos_tcp[1] pose.position.z = pos_tcp[2] return pose def TCP2gripper(self, pose, length=0): roll, pitch, yaw, x, y, z = self.msg2pose(pose) T = euler_matrix(roll, pitch, yaw) T[0:3, 3] = numpy.array([x, y, z]) pos_gripper_tcp = numpy.array([0, length, 0, 1]) pos_tcp = T.dot(pos_gripper_tcp) pose.position.x = pos_tcp[0] pose.position.y = pos_tcp[1] pose.position.z = pos_tcp[2] return pose def arm2baselink(self, in_pose): pose = copy.deepcopy(in_pose) pose.position.x += self.transform_arm_to_baselink.x pose.position.y += self.transform_arm_to_baselink.y pose.position.z += self.transform_arm_to_baselink.z return pose def baselink2arm(self, in_pose): pose = copy.deepcopy(in_pose) pose.position.x -= self.transform_arm_to_baselink.x pose.position.y -= self.transform_arm_to_baselink.y pose.position.z -= self.transform_arm_to_baselink.z return pose # Inverse Kinematics (IK): move TCP to given position and orientation def move_pose_arm(self, pose_goal): # pose_goal = self.pose2msg(roll, pitch, yaw, x, y, z) # pose_goal = self.gripper2TCP(pose_goal, self.gripper_length) x = pose_goal.position.x y = pose_goal.position.y z = pose_goal.position.z if not self.is_inside_workspace(x, y, z): rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****') return False self.arm.set_pose_target(pose_goal) self.arm.go(wait=True) self.arm.stop() # To guarantee no residual movement self.arm.clear_pose_targets() return True # Move gripper def move_joint_hand(self, finger1_goal, finger2_goal=10, finger3_goal=10): if finger2_goal == 10 and finger3_goal == 10: finger2_goal, finger3_goal = finger1_goal, finger1_goal jointtrajectory = JointTrajectory() jointtrajectory.header.stamp = rospy.Time.now() jointtrajectory.joint_names.extend(["H1_F1J3", "H1_F2J3", "H1_F3J3"]) joint = JointTrajectoryPoint() joint.positions.extend([finger1_goal, finger2_goal, finger3_goal]) joint.time_from_start = rospy.Duration(1) jointtrajectory.points.append(joint) self.gripperpub.publish(jointtrajectory) def pose2msg(self, roll, pitch, yaw, x, y, z): pose = geometry_msgs.msg.Pose() quat = quaternion_from_euler(roll, pitch, yaw) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] pose.position.x = x pose.position.y = y pose.position.z = z return pose def msg2pose(self, pose): x = pose.position.x y = pose.position.y z = pose.position.z quaternion = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) euler = euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1] yaw = euler[2] return roll, pitch, yaw, x, y, z def set_grasp_distance(self, min_distance, desired_distance): self.approach_retreat_min_dist = min_distance self.approach_retreat_desired_dist = desired_distance def count_gripper_width(self, object_name): object_width = self.object_list[object_name].width if object_width in self.gripper_width: return self.gripper_width[object_width] else: rospy.loginfo( "Cannot find suitable gripper joint value for this object width" ) return 0 # pick object to goal position def pickup(self, object_name, position, width, distance=0.12): if not self.is_inside_workspace(position.x, position.y, position.z): rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****') rospy.loginfo('Stop placing') return pose = Pose() pose.position = position q = quaternion_from_euler(numpy.deg2rad(-90), 0, numpy.deg2rad(180)) pose.orientation = Quaternion(*q) # transform from gripper to TCP pose = self.gripper2TCP(pose, self.gripper_length) pose.position.z += distance rospy.loginfo('Start picking') self.move_pose_arm(pose) rospy.sleep(1) # move down waypoints = [] wpose = self.arm.get_current_pose().pose wpose = self.baselink2arm(wpose) print(wpose) wpose.position.z -= distance waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold self.arm.execute(plan, wait=True) self.updatepose_trigger(True) # pick rospy.sleep(0.5) self.move_joint_hand(width) rospy.sleep(3) # if object_name != "yellow_ball": # self.arm.attach_object(object_name) # move up waypoints = [] wpose = self.arm.get_current_pose().pose wpose = self.baselink2arm(wpose) wpose.position.z += distance waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold self.arm.execute(plan, wait=True) self.updatepose_trigger(True) rospy.loginfo('Pick finished') # place object to goal position def place(self, object_name, position, width=-0.2, distance=0.12): if not self.is_inside_workspace(position.x, position.y, position.z): rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****') rospy.loginfo('Stop placing') return pose = Pose() pose.position = position q = quaternion_from_euler(numpy.deg2rad(-90), 0, numpy.deg2rad(180)) pose.orientation = Quaternion(*q) # transform from gripper to TCP pose = self.gripper2TCP(pose, self.gripper_length) pose.position.z += distance rospy.loginfo('Start placing') self.move_pose_arm(pose) rospy.sleep(1) # move down waypoints = [] wpose = self.arm.get_current_pose().pose wpose = self.baselink2arm(wpose) print(wpose) wpose.position.z -= distance waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold self.arm.execute(plan, wait=True) self.updatepose_trigger(True) # place self.move_joint_hand(width) rospy.sleep(3) # if object_name != "yellow_ball": # self.arm.detach_object(object_name) # self.clean_scene(object_name) self.object_list.pop(object_name) # move up waypoints = [] wpose = self.arm.get_current_pose().pose wpose = self.baselink2arm(wpose) wpose.position.z += distance waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold self.arm.execute(plan, wait=True) self.updatepose_trigger(True) rospy.loginfo('Place finished')
class Grasp_Place: def __init__(self): # Retrieve params: #if there is no param named 'table_object_name' then use the default self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object1') self._grasp_object_name_capscrew4 = rospy.get_param( '~grasp_object_name4', 'capscrew4') #capscrew self._grasp_object_name_screw = rospy.get_param( '~grasp_object_name5', 'screw') # screw #print self._grasp_object_name_capscrew4 self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander: #self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) ############################################################################################################################## ############################################################################################################################## #Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) #get the pose of endfection print( self._arm.get_current_pose(self._arm.get_end_effector_link()).pose) #self._Init_robot_state() #robot stata initilizations #self._Go_to_default_pose() #set the robot pose to the 'home' pose set in moveit_setup_assistant (before a grasp) #self._move_to_obj_for_grasp(Grasp_pose_use, gripper_depth) #move to the taget object for grasping #self._grasp_action(GRIPPER_CLOSED) #Close the gripper to grasp the object #self._move_up(displacementUp) #move up after the gripper grasp the target object #self._move_to_placeGoal(place_Pose_use, gripper_depth) #move to the above area of the placeGoal pose(a little bit higher) #self._move_down(displacementDown) #move down to the placeGoal pose #self._place_action() #place -> release or open the gripper #self._Go_to_default_pose() #set the robot pose to the 'home' pose set in moveit_setup_assistant (after a place) #Costom_Command (Int) # 0: _Init_robot_state() # 1: _Go_to_default_pose() # 2: _move_to_obj_for_grasp(Grasp_pose_use, gripper_depth) # 3: _grasp_action(GRIPPER_CLOSED) # 4: _move_up(displacementUp) # 5: _move_to_placeGoal(place_Pose_use, gripper_depth) # 6: _move_down(displacementDown) # 7: _place_action() ##########Receive Command and respongding data from the server########### # _Go_to_default_pose() #让机械臂到达默认位姿 # _move_to_obj_for_grasp(Grasp_pose_use, gripper_depth) # 移->到达螺母预抓取位姿态 # _grasp_action(GRIPPER_CLOSED) # 抓->控制爪子关闭 # _move_up(displacementUp) # 移->向上提螺母 # _move_to_placeGoal(place_Pose_use, gripper_depth) #移->趋向螺栓 # _move_down(displacementDown) # 移-> 插入螺栓 # _place_action() # 放->控制爪子放开 self._Init_robot_state() #Initialzing the state of the robot def _Init_robot_state(self): rospy.loginfo('Initializing the robot state') self._arm.set_goal_position_tolerance(0.001) self._arm.set_goal_orientation_tolerance(0.001) self._arm.allow_replanning(True) self._arm.set_pose_reference_frame(self._robot.get_planning_frame()) self._arm.set_planning_time(7) #Set the 'home' pose of the ur5 before graspsing or after placing sth def _Go_to_default_pose(self): rospy.set_param('Excute_Lock', False) rospy.loginfo('Set the defaulit pose of the ur5 ') self._arm.set_named_target( 'home' ) #go to the 'home' place that are set in moveit_setup_assistant. self._arm.go() self._gripper.set_joint_value_target( 'gripper_finger1_joint', [0.0]) #[0.0] is the wide open pose of the gripper group. self._gripper.go() #move to the object for grasp #gripper_depth : default->0.15 def _move_to_obj_for_grasp(self, pose, gripper_depth): rospy.set_param('Excute_Lock', False) #Step-1##move-to-the-object-for-grasp### rospy.loginfo('moving-to-the-object-for-grasp') pose.position.z = pose.position.z + gripper_depth self._arm.set_pose_target(pose, self._arm.get_end_effector_link()) traj = self._arm.plan() self._arm.execute(traj) #rospy.sleep(1) #grasp action #gripper_depth : default->0.15 #GRIPPER_CLOSED is a list type and can be modified accodring to the object width def _grasp_action(self, GRIPPER_CLOSED): rospy.set_param('Excute_Lock', False) G_C = [GRIPPER_CLOSED] #Step-2##grasp### #grasp_clsoe posture() rospy.loginfo('grasping the object') self._gripper.set_joint_value_target('gripper_finger1_joint', G_C) self._gripper.go() #rospy.sleep(1) #move up 0.1 def _move_up(self, displacementUp): rospy.set_param('Excute_Lock', False) #Step-3##move-up after the grsap### rospy.loginfo('moving up') current_pose = self._arm.get_current_pose( self._arm.get_end_effector_link( )).pose #get the current pose of the end_effector current_pose.position.z = current_pose.position.z + displacementUp self._arm.set_pose_target(current_pose, self._arm.get_end_effector_link()) traj = self._arm.plan() self._arm.execute(traj) #rospy.sleep(1) #move to the placeGoal def _move_to_placeGoal(self, pose, gripper_depth): rospy.set_param('Excute_Lock', False) #Step-4##move-to-the-placeGoal(a little bit higer in case of collision)### rospy.loginfo( 'move-to-the-placeGoal: a little bit higer in case of collision') pose.position.z = pose.position.z + gripper_depth self._arm.set_pose_target(pose, self._arm.get_end_effector_link()) traj = self._arm.plan() self._arm.execute(traj) #rospy.sleep(1) #maybe->Step-5##move-down-to the placeGoal### def _move_down(self, displacementDown): rospy.set_param('Excute_Lock', False) #Step-5##move-down-to the placeGoal### rospy.loginfo('moving down') current_pose = self._arm.get_current_pose( self._arm.get_end_effector_link( )).pose #get the current pose of the end_effector current_pose.position.z = current_pose.position.z - displacementDown self._arm.set_pose_target(current_pose, self._arm.get_end_effector_link()) traj = self._arm.plan() self._arm.execute(traj) #rospy.sleep(1) #maybe->Step-5##move-down-to the placeGoal### def _place_action(self): rospy.set_param('Excute_Lock', False) #Step-6##place### #grasp_clsoe posture() rospy.loginfo('opening the gripper and placing the object') self._gripper.set_joint_value_target('gripper_finger1_joint', [0.0]) self._gripper.go()
class Pick_Place: def __init__(self): self.object_name = "box" self.object_width = 0.03 self.arm_group = "irb_120" self.gripper_group = "robotiq_85" self.arm = moveit_commander.MoveGroupCommander("irb_120") self.gripper = moveit_commander.MoveGroupCommander("robotiq_85") self.scene = PlanningSceneInterface() self.robot = RobotCommander() rospy.sleep(1.0) self.scene.remove_world_object(self.object_name) self.pose_object = self.add_object(self.object_name) rospy.sleep(1.0) self.pose_object.position.z += 0.16 print(self.pose_object.position) self.pose_place = Pose() self.pose_place.position.x = self.pose_object.position.x self.pose_place.position.y = self.pose_object.position.y - 0.1 self.pose_place.position.z = 0 #self.pose_object.position.z - 0.1 self.pose_place.orientation = Quaternion( *quaternion_from_euler(0.0, 0.0, 0.0)) self.approach_retreat_desired_dist = 0.2 self.approach_retreat_min_dist = 0.1 #self.grasp_action = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) # self.pickup_action = SimpleActionClient("/pickup", PickupAction) # self.place_action = SimpleActionClient("/place", PlaceAction) print("start pick and place") # Pick # while not self.pickup(self.arm_group, self.object_name, self.object_width): # rospy.logwarn('Pick up failed! Retrying ...') # rospy.sleep(1.0) grasps = self.generate_grasps(self.pose_object, self.object_width) self.arm.pick(self.object_name, grasps) rospy.loginfo('Pick up successfully') rospy.sleep(1) # self.arm.detach_object(self.object_name) # self.clean_scene() # print("pose_place: ", self.pose_place) # Place # while not self.place(self.arm_group, self.object_name, self.pose_place): # rospy.logwarn('Place failed! Retrying ...') # rospy.sleep(1.0) place = self.generate_places(self.pose_place) self.arm.place(self.object_name, place) rospy.loginfo('Place successfully') self.move_joint_hand(0) rospy.loginfo("Moving arm to HOME point") self.move_pose_arm(0, 0.8, 0, 0.4, 0, 0.6) rospy.sleep(1) self.arm.detach_object(self.object_name) self.clean_scene() def clean_scene(self): self.scene.remove_world_object(self.object_name) def add_object(self, name): p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.015 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) size = (0.03, 0.03, 0.03) self.scene.add_box(name, p, size) return p.pose def move_pose_arm(self, roll, pitch, yaw, x, y, z): pose_goal = geometry_msgs.msg.Pose() quat = quaternion_from_euler(roll, pitch, yaw) pose_goal.orientation.x = quat[0] pose_goal.orientation.y = quat[1] pose_goal.orientation.z = quat[2] pose_goal.orientation.w = quat[3] pose_goal.position.x = x pose_goal.position.y = y pose_goal.position.z = z self.arm.set_pose_target(pose_goal) plan = self.arm.go(wait=True) self.arm.stop() # To guarantee no residual movement self.arm.clear_pose_targets() # Move the Robotiq gripper by master axis def move_joint_hand(self, gripper_finger1_joint): joint_goal = self.gripper.get_current_joint_values() joint_goal[2] = gripper_finger1_joint # Gripper master axis self.gripper.go(joint_goal, wait=True) self.gripper.stop() # To guarantee no residual movement def generate_grasps(self, pose, width): grasps = [] now = rospy.Time.now() for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(90.0)): # Create place location: grasp = Grasp() grasp.grasp_pose.header.stamp = now grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame() grasp.grasp_pose.pose = copy.deepcopy(pose) q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle) grasp.grasp_pose.pose.orientation = Quaternion(*q) # Setting pre-grasp approach grasp.pre_grasp_approach.direction.header.stamp = now grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame( ) grasp.pre_grasp_approach.direction.vector.z = -0.2 grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist # Setting post-grasp retreat grasp.post_grasp_retreat.direction.header.stamp = now grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame( ) grasp.post_grasp_retreat.direction.vector.z = 0.2 grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist grasp.max_contact_force = 100 grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() traj.positions.append(0) traj.time_from_start = rospy.Duration.from_sec(0.5) grasp.pre_grasp_posture.points.append(traj) grasp.grasp_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() traj.positions.append(0.55) #traj.effort.append(100) traj.time_from_start = rospy.Duration.from_sec(1.0) grasp.grasp_posture.points.append(traj) grasps.append(grasp) return grasps def generate_places(self, target): #places = [] now = rospy.Time.now() # for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(30.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self.robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0, 0.0) place.place_pose.pose.orientation = Quaternion(*q) # Generate pre place approach: place.pre_place_approach.desired_distance = self.approach_retreat_desired_dist place.pre_place_approach.min_distance = self.approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self.robot.get_planning_frame( ) place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = -0.2 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self.robot.get_planning_frame( ) place.post_place_retreat.desired_distance = self.approach_retreat_desired_dist place.post_place_retreat.min_distance = self.approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 0.2 place.allowed_touch_objects.append(self.object_name) place.post_place_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() traj.positions.append(0) #traj.effort.append(0) traj.time_from_start = rospy.Duration.from_sec(1.0) place.post_place_posture.points.append(traj) # Add place: #places.append(place) return place def create_pickup_goal(self, group, target, grasps): goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) goal.allowed_planning_time = 3.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def create_place_goal(self, group, target, places): goal = PlaceGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(places) goal.allowed_planning_time = 3.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def pickup(self, group, target, width): print("start grnerating grasps") grasps = self.generate_grasps(self.pose_object, width) print("start creating pickup goal") goal = self.create_pickup_goal(group, target, grasps) state = self.pickup_action.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self.pickup_action.get_goal_status_text()) return None result = self.pickup_action.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def place(self, group, target, place): places = self.generate_places(self.pose_object, width) goal = self.create_place_goal(group, target, places) state = self.place_action.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self.place_action.get_goal_status_text()) return None result = self.place_action.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True
class Pick_Place: def __init__(self): # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param( '~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param( '~approach_retreat_min_dist', 0.1) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander: self._scene = PlanningSceneInterface() self._robot = RobotCommander() # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) rospy.sleep(1.0) # self.setup() # Clean the scene: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: self._pose_table = self._add_table(self._table_object_name) rospy.loginfo("added table") rospy.sleep(2.0) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) rospy.loginfo("added grasping box") rospy.sleep(1.0) # Define target place pose: self._pose_place = Pose() self._pose_place.position.x = self._pose_coke_can.position.x self._pose_place.position.y = self._pose_coke_can.position.y - 0.06 self._pose_place.position.z = self._pose_coke_can.position.z self._pose_place.orientation = Quaternion( *quaternion_from_euler(0.0, 0.0, 0.0)) def _add_table(self, name): rospy.loginfo("entered table") p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.0 p.pose.position.y = 0.0 p.pose.position.z = -0.2 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (9, 9, 0.02)) return p.pose def _add_grasp_block_(self, name): rospy.loginfo("entered box grabber") p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.05 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.1, 0.1, 0.1)) return p.pose
class Pick_Place: def __init__(self): self.scene = PlanningSceneInterface() self.robot = RobotCommander() filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_kinematics', 'joints_setup.yaml') with open(filename) as file: joints_setup = yaml.load(file) jointslimit = joints_setup["joints_limit"] home_value = joints_setup["home_value"] j1 = home_value["joint_1"] j2 = home_value["joint_2"] j3 = home_value["joint_3"] j4 = home_value["joint_4"] j5 = home_value["joint_5"] j6 = home_value["joint_6"] g = home_value["gripper"] self.set_home_value([j1, j2, j3, j4, j5, j6, g]) self.object_list = {} filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_kinematics', 'interfaces', 'models_info.yaml') with open(filename) as file: objects_info = yaml.load(file) robot_x = objects_info["robot"]["pose"]["x"] robot_y = objects_info["robot"]["pose"]["y"] robot_z = objects_info["robot"]["pose"]["z"] robot_roll = objects_info["robot"]["pose"]["roll"] robot_pitch = objects_info["robot"]["pose"]["pitch"] robot_yaw = objects_info["robot"]["pose"]["yaw"] rospy.loginfo("Spawning Objects in Gazebo and planning scene") objects = objects_info["objects"] objects_name = objects.keys() for object_name in objects_name: name = object_name shape = objects[name]["shape"] color = objects[name]["color"] x = objects[name]["pose"]["x"] y = objects[name]["pose"]["y"] z = objects[name]["pose"]["z"] roll = objects[name]["pose"]["roll"] pitch = objects[name]["pose"]["pitch"] yaw = objects[name]["pose"]["yaw"] object_pose = self.pose2msg(x, y, z, roll, pitch, yaw) p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = x - robot_x p.pose.position.y = y - robot_y p.pose.position.z = z - robot_z q = quaternion_from_euler(roll, pitch, yaw) p.pose.orientation = Quaternion(*q) if shape == "box": x = objects[name]["size"]["x"] y = objects[name]["size"]["y"] z = objects[name]["size"]["z"] p.pose.position.z += z / 2 height = z width = y length = x self.object_list[name] = Object(p.pose, object_pose, height, width, length, shape, color) elif shape == "cylinder": height = objects[name]["size"]["height"] radius = objects[name]["size"]["radius"] p.pose.position.z += height / 2 self.object_list[name] = Object(p.pose, object_pose, height, radius * 2, radius * 2, shape, color) elif shape == "sphere": radius = objects[name]["size"] p.pose.position.z += radius self.object_list[name] = Object(p.pose, object_pose, radius * 2, radius * 2, radius * 2, shape, color) # self.object_list = object_list self.goal_list = {} self.set_target_info() self.gripper_width = {} self.set_gripper_width_relationship() self.arm = moveit_commander.MoveGroupCommander("irb_120") self.gripper = moveit_commander.MoveGroupCommander("robotiq_85") self.arm.set_goal_tolerance(0.01) # set default grasp message infos self.set_grasp_distance(0.1, 0.2) self.set_grasp_direction(0, 0, -0.5) self.get_workspace() self.message_pub = rospy.Publisher("/gui_message", String, queue_size=0) self.updatepose_pub = rospy.Publisher("/updatepose", Bool, queue_size=0) def send_message(self, message): msg = String() msg.data = message self.message_pub.publish(msg) def updatepose_trigger(self, value): msg = Bool() msg.data = value self.updatepose_pub.publish(msg) def clean_scene(self, object_name): self.scene.remove_world_object(object_name) def set_target_info(self): filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_kinematics', 'interfaces', 'models_info.yaml') with open(filename) as file: objects_info = yaml.load(file) robot_x = objects_info["robot"]["pose"]["x"] robot_y = objects_info["robot"]["pose"]["y"] robot_z = objects_info["robot"]["pose"]["z"] targets = objects_info["targets"] target_name = targets.keys() for name in target_name: position = Point() position.x = targets[name]["x"] - robot_x position.y = targets[name]["y"] - robot_y position.z = targets[name]["z"] - robot_z self.goal_list[name] = position def set_gripper_width_relationship(self): filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_kinematics', 'interfaces', 'models_info.yaml') with open(filename) as file: objects_info = yaml.load(file) gripper_joint_value = objects_info["gripper_joint_value"] objects_width = gripper_joint_value.keys() for object_width in objects_width: self.gripper_width[object_width] = gripper_joint_value[ object_width] def get_object_list(self): return self.object_list.keys() def get_target_list(self): return self.goal_list.keys() def get_object_pose(self, object_name): return copy.deepcopy(self.object_list[object_name].relative_pose) def get_object_info(self, object_name): this_object = copy.deepcopy(self.object_list[object_name]) pose = this_object.relative_pose height = this_object.height width = this_object.width length = this_object.length shape = this_object.shape color = this_object.color return pose, height, width, length, shape, color def get_target_position(self, target_name): return self.goal_list[target_name] def pose2msg(self, roll, pitch, yaw, x, y, z): pose = geometry_msgs.msg.Pose() quat = quaternion_from_euler(roll, pitch, yaw) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] pose.position.x = x pose.position.y = y pose.position.z = z return pose def msg2pose(self, pose): x = pose.position.x y = pose.position.y z = pose.position.z quaternion = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) euler = euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1] yaw = euler[2] return roll, pitch, yaw, x, y, z def pose2msg_deg(self, roll, pitch, yaw, x, y, z): pose = geometry_msgs.msg.Pose() quat = quaternion_from_euler(numpy.deg2rad(roll), numpy.deg2rad(pitch), numpy.deg2rad(yaw)) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] pose.position.x = x pose.position.y = y pose.position.z = z return pose def msg2pose_deg(self, pose): x = pose.position.x y = pose.position.y z = pose.position.z quaternion = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) euler = euler_from_quaternion(quaternion) roll = numpy.rad2deg(euler[0]) pitch = numpy.rad2deg(euler[1]) yaw = numpy.rad2deg(euler[2]) return roll, pitch, yaw, x, y, z def get_workspace(self): filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_kinematics', 'joints_setup.yaml') with open(filename) as file: joints_setup = yaml.load(file) workspace = joints_setup["workspace"] x = workspace["center"]["x"] y = workspace["center"]["y"] z = workspace["center"]["z"] min_r = workspace["r"]["min"] max_r = workspace["r"]["max"] min_z = workspace["min_z"] self.workspace = WorkSpace(x, y, z, min_r, max_r, min_z) # check if the position is inside workspace def is_inside_workspace(self, x, y, z): if z > self.workspace.min_z: dx = x - self.workspace.x dy = y - self.workspace.y dz = z - self.workspace.z r = math.sqrt(dx**2 + dy**2 + dz**2) if self.workspace.min_r < r < self.workspace.max_r: return True return False def gripper2TCP(self, pose, length=0): roll, pitch, yaw, x, y, z = self.msg2pose(pose) T = euler_matrix(roll, pitch, yaw) T[0:3, 3] = numpy.array([x, y, z]) pos_gripper_tcp = numpy.array([-length, 0, 0, 1]) pos_tcp = T.dot(pos_gripper_tcp) pose.position.x = pos_tcp[0] pose.position.y = pos_tcp[1] pose.position.z = pos_tcp[2] return pose def TCP2gripper(self, pose, length=0): roll, pitch, yaw, x, y, z = self.msg2pose(pose) T = euler_matrix(roll, pitch, yaw) T[0:3, 3] = numpy.array([x, y, z]) pos_gripper_tcp = numpy.array([length, 0, 0, 1]) pos_tcp = T.dot(pos_gripper_tcp) pose.position.x = pos_tcp[0] pose.position.y = pos_tcp[1] pose.position.z = pos_tcp[2] return pose def set_home_value(self, home_value): self.home_value = home_value def back_to_home(self): j1, j2, j3, j4, j5, j6, g = self.home_value self.move_joint_arm(j1, j2, j3, j4, j5, j6) self.move_joint_hand(g) rospy.sleep(1) # Forward Kinematics (FK): move the arm by axis values def move_joint_arm(self, joint_0, joint_1, joint_2, joint_3, joint_4, joint_5): joint_goal = self.arm.get_current_joint_values() joint_goal[0] = joint_0 joint_goal[1] = joint_1 joint_goal[2] = joint_2 joint_goal[3] = joint_3 joint_goal[4] = joint_4 joint_goal[5] = joint_5 self.arm.go(joint_goal, wait=True) self.arm.stop() # To guarantee no residual movement self.updatepose_trigger(True) # Inverse Kinematics: Move the robot arm to desired pose def move_pose_arm(self, pose_goal): position = pose_goal.position if not self.is_inside_workspace(position.x, position.y, position.z): rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****') return self.arm.set_pose_target(pose_goal) self.arm.go(wait=True) self.arm.stop() # To guarantee no residual movement self.arm.clear_pose_targets() self.updatepose_trigger(True) # Move the Robotiq gripper by master axis def move_joint_hand(self, gripper_finger1_joint): joint_goal = self.gripper.get_current_joint_values() joint_goal[2] = gripper_finger1_joint # Gripper master axis self.gripper.go(joint_goal, wait=True) self.gripper.stop() # To guarantee no residual movement self.updatepose_trigger(True) def set_grasp_direction(self, x, y, z): self.approach_direction = Vector3() self.approach_direction.x = x self.approach_direction.y = y self.approach_direction.z = z self.retreat_direction = Vector3() self.retreat_direction.x = -x self.retreat_direction.y = -y self.retreat_direction.z = -z def set_grasp_distance(self, min_distance, desired_distance): self.approach_retreat_min_dist = min_distance self.approach_retreat_desired_dist = desired_distance def count_gripper_width(self, object_name): object_width = self.object_list[object_name].width if object_width in self.gripper_width: return self.gripper_width[object_width] else: rospy.loginfo( "Cannot find suitable gripper joint value for this object width" ) return 0 def generate_grasp(self, object_name, eef_orientation, position, width=0, roll=0, pitch=0, yaw=0, length=0): if width == 0: # need to count gripper joint value if (eef_orientation == "horizontal" and pitch == 0) or (eef_orientation == "vertical" and yaw == 0): width = self.count_gripper_width(object_name) else: rospy.loginfo( "Orientation doesn't meet requirement. Please tune gripper width by yourself" ) return self.generate_grasp_width(eef_orientation, position, width, roll, pitch, yaw, length) def generate_grasp_width(self, eef_orientation, position, width, roll=0, pitch=0, yaw=0, length=0): now = rospy.Time.now() grasp = Grasp() grasp.grasp_pose.header.stamp = now grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame() grasp.grasp_pose.pose.position = position if eef_orientation == "horizontal": q = quaternion_from_euler(0.0, numpy.deg2rad(pitch), 0.0) elif eef_orientation == "vertical": q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), numpy.deg2rad(yaw)) elif eef_orientation == "user_defined": q = quaternion_from_euler(numpy.deg2rad(roll), numpy.deg2rad(pitch), numpy.deg2rad(yaw)) grasp.grasp_pose.pose.orientation = Quaternion(*q) # transform from gripper to TCP grasp.grasp_pose.pose = self.gripper2TCP(grasp.grasp_pose.pose, length) if not self.is_inside_workspace(grasp.grasp_pose.pose.position.x, grasp.grasp_pose.pose.position.y, grasp.grasp_pose.pose.position.z): rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****') #return False # Setting pre-grasp approach grasp.pre_grasp_approach.direction.header.stamp = now grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame( ) grasp.pre_grasp_approach.direction.vector = self.approach_direction grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist # Setting post-grasp retreat grasp.post_grasp_retreat.direction.header.stamp = now grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame( ) grasp.post_grasp_retreat.direction.vector = self.retreat_direction grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist grasp.max_contact_force = 1000 grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() traj.positions.append(0.0) traj.time_from_start = rospy.Duration.from_sec(0.5) grasp.pre_grasp_posture.points.append(traj) grasp.grasp_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() traj.positions.append(width) traj.time_from_start = rospy.Duration.from_sec(5.0) grasp.grasp_posture.points.append(traj) return grasp # pick up object with grasps def pickup(self, object_name, grasps): rospy.loginfo('Start picking ' + object_name) self.arm.pick(object_name, grasps) #self.gripper.stop() self.updatepose_trigger(True) rospy.loginfo('Pick up finished') self.arm.detach_object(object_name) self.clean_scene(object_name) # place object to goal position def place(self, eef_orientation, position, distance=0.1, roll=0, pitch=0, yaw=180): if not self.is_inside_workspace(position.x, position.y, position.z): rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****') rospy.loginfo('Stop placing') return pose = Pose() pose.position = position if eef_orientation == "horizontal": q = quaternion_from_euler(0.0, numpy.deg2rad(pitch), numpy.deg2rad(180)) elif eef_orientation == "vertical": q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), numpy.deg2rad(yaw)) elif eef_orientation == "user_defined": q = quaternion_from_euler(numpy.deg2rad(roll), numpy.deg2rad(pitch), numpy.deg2rad(yaw)) pose.orientation = Quaternion(*q) pose.position.z += distance rospy.loginfo('Start placing') self.move_pose_arm(pose) rospy.sleep(1) # move down waypoints = [] wpose = self.arm.get_current_pose().pose wpose.position.z -= distance waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold self.arm.execute(plan, wait=True) self.updatepose_trigger(True) # place self.move_joint_hand(0) rospy.sleep(1) # move up waypoints = [] wpose = self.arm.get_current_pose().pose wpose.position.z += distance waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold self.arm.execute(plan, wait=True) self.updatepose_trigger(True) rospy.loginfo('Place finished')
class Pick_Place: def __init__(self): # 检索参数: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._table2_object_name = rospy.get_param('~table_object_name', 'Grasp_Table2') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object2_name = rospy.get_param('~grasp_object_name', 'Grasp_Object2') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.1) # 创建(调试)发布者: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # 创建规划现场,机器人指挥官: self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) # 清理现场: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._table2_object_name) self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._grasp_object2_name) # 将表和可乐罐对象添加到计划场景: self._pose_table = self._add_table(self._table_object_name) self._pose_table = self._add_table2(self._table2_object_name) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) self._pose_coke_can2 = self._add_grasp_block2_(self._grasp_object2_name) rospy.sleep(1.0) # 定义目标位置姿势: self._pose_place = Pose() self._pose_place.position.x = self._pose_coke_can.position.x-0.5 self._pose_place.position.y = self._pose_coke_can.position.y-0.85 self._pose_place.position.z = self._pose_coke_can.position.z-0.3 self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)) self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9 # base_link is 0.9 above self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above # 检索组 (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) self._arm.set_named_target('pickup') self._arm.go(wait=True) print("Pickup pose") # 创建抓取生成器“生成”动作客户端: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(1.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown('Grasp generator action client not available!') return # 创建移动组“zhua取”操作客户端: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(1.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # 创建移动组“放置”动作客户端: self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(1.0)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return # Pick Coke can object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') print("pose_place: ", self._pose_place) # 放置可乐可能会在支撑表面(桌子)上的其他地方产生异物: while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Place successfully') print "-------------test1--------------" rospy.sleep(1.0) def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._table2_object_name) print "-------------test2------------------" def _add_table(self, name): """ 创建表并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.85 p.pose.position.y = 0.0 p.pose.position.z = 0.70 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (1, 1, 0.05)) print "-------------test3------------------" return p.pose def _add_table2(self, name): """ 创建表并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.0 p.pose.position.y = 0.85 p.pose.position.z = 0.40 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.5, 0.05)) print "-------------test4------------------" return p.pose def _add_grasp_block_(self, name): """ 创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.74 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.045, 0.045, 0.045)) print "-------------test5------------------" return p.pose def _add_grasp_block2_(self, name): """ 创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.3 p.pose.position.z = 0.74 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.045, 0.045, 0.045)) print "-------------test6------------------" return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. 使用抓握生成器生成动作来生成抓握; 基于moveit_simple_grasps pkg上的server_test.py示例 Generate the grasp Pose Array data for visualization, and then send the grasp goal to the grasp server. 生成抓取姿势阵列数据以进行可视化,然后将抓取目标发送到抓取服务器。 """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # 发送目标并等待结果: # 将目标发送到ActionServer,等待目标完成,并且必须抢占目标. state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # 发布掌握(用于调试/可视化目的): self._publish_grasps(grasps) print "-------------test7------------------" return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp 为该位置的姿势创建姿势数组数据 """ # Generate places: places = [] now = rospy.Time.now() for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle ) place.place_pose.pose.orientation = Quaternion(*q) # 生成预安置方法: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame() place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = 0.1 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame() place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 0.1 # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) print "-------------test8------------------" return places def _create_pickup_goal(self, group, target, grasps): """ 创建一个MoveIt! 接送目标 创建一个捡起抓取物体的目标 """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) goal.support_surface_name = self._table_object_name # 配置目标计划选项: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 print "-------------test9------------------" return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal Create a place goal for MoveIt! """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 print "-------------test10------------------" return goal def _pickup(self, group, target, width): """ 使用计划小组选择目标 """ # 从掌握生成器服务器获取可能的掌握: grasps = self._generate_grasps(self._pose_coke_can, width) # 创建并发送提货目标: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False print "-------------test11------------------" return True def _place(self, group, target, place): """ 使用计划组放置目标 """ # Obtain possible places: places = self._generate_places(place) # Create and send Place goal: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False print "-------------test12------------------" return True def _publish_grasps(self, grasps): """ 使用PoseArray消息将抓取发布为姿势 """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg) print "-------------test13------------------" def _publish_places(self, places): """ 使用PoseArray消息将位置发布为姿势 """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg) print "-------------test14------------------"
def main(argv): #moveit_commander.roscpp_initialize(sys.argv) rospy.init_node("PickPlaceDemo") rospy.on_shutdown(onshutdownhook) transform = tf.TransformListener() global scene scene = PlanningSceneInterface() robot = RobotCommander() group = MoveGroupCommander("manipulator") rospy.sleep(2) #scene.is_diff = True #t = transform.getLatestCommonTime("/tool0", "/world") #tool0_pose, tool0_quaternion = transform.lookupTransform("world", "tool0", t) quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0) p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = -0.700 p.pose.position.y = 0.000 p.pose.position.z = 0.060 p.pose.orientation.x = quaternion[0] p.pose.orientation.y = quaternion[1] p.pose.orientation.z = quaternion[2] p.pose.orientation.w = quaternion[3] scene.add_box("box1", p, (0.15, 0.15, 0.15)) p.pose.position.x = -0.700 p.pose.position.y = -0.250 p.pose.position.z = 0.060 scene.add_box("box2", p, (0.15, 0.15, 0.15)) p.pose.position.x = -0.700 p.pose.position.y = -0.500 p.pose.position.z = 0.060 scene.add_box("box3", p, (0.15, 0.15, 0.15)) rospy.sleep(5) quaternion = tf.transformations.quaternion_from_euler( 0.0, (math.pi / 2.0), 0.0) pose_target = Pose() pose_target.position.x = -0.700 pose_target.position.y = 0.000 pose_target.position.z = 0.155 pose_target.orientation.x = quaternion[0] pose_target.orientation.y = quaternion[1] pose_target.orientation.z = quaternion[2] pose_target.orientation.w = quaternion[3] group.set_pose_target(pose_target) group.go(wait=True) rospy.sleep(1) scene.attach_box("tool0", "box1") rospy.sleep(1) pose_target.position.x = 0.000 pose_target.position.y = -0.700 pose_target.position.z = 0.155 group.set_pose_target(pose_target) group.go(wait=True) rospy.sleep(1) scene.remove_attached_object("tool0") rospy.sleep(1) pose_target.position.x = -0.700 pose_target.position.y = -0.250 pose_target.position.z = 0.155 group.set_pose_target(pose_target) group.go(wait=True) rospy.sleep(1) scene.attach_box("tool0", "box2") rospy.sleep(1) pose_target.position.x = 0.000 pose_target.position.y = -0.700 pose_target.position.z = 0.306 group.set_pose_target(pose_target) group.go(wait=True) rospy.sleep(1) scene.remove_attached_object("tool0") rospy.sleep(1) pose_target.position.x = -0.700 pose_target.position.y = -0.500 pose_target.position.z = 0.155 group.set_pose_target(pose_target) group.go(wait=True) rospy.sleep(1) scene.attach_box("tool0", "box3") rospy.sleep(1) pose_target.position.x = 0.000 pose_target.position.y = -0.700 pose_target.position.z = 0.457 group.set_pose_target(pose_target) group.go(wait=True) rospy.sleep(1) scene.remove_attached_object("tool0") while not rospy.is_shutdown(): pass
class ModelManager: def __init__(self): rospy.wait_for_service("gazebo/spawn_sdf_model") self.spawn_model_srv = rospy.ServiceProxy("gazebo/spawn_sdf_model", SpawnModel) rospy.wait_for_service("gazebo/spawn_sdf_model") self.delete_model_srv = rospy.ServiceProxy("gazebo/delete_model", DeleteModel) self.scene = PlanningSceneInterface() self.robot = RobotCommander() rospy.sleep(1) self.object_list = {} def pose2msg(self, x, y, z, roll, pitch, yaw): pose = Pose() pose.position.x = x pose.position.y = y pose.position.z = z quat = quaternion_from_euler(roll, pitch, yaw) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] return pose def msg2pose(self, pose): x = pose.position.x y = pose.position.y z = pose.position.z quaternion = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) euler = euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1] yaw = euler[2] return roll, pitch, yaw, x, y, z def spawn_model(self, model_name, model_pose): with open( os.path.join(rospkg.RosPack().get_path('ur5_gripper_demo'), 'models', model_name, 'model.sdf'), "r") as f: model_xml = f.read() self.spawn_model_srv(model_name, model_xml, "", model_pose, "world") def delete_model(self, model_name): self.delete_model_srv(model_name) def clean_scene(self, object_name): self.scene.remove_world_object(object_name) def respawn_all_objects(self): objects_name = self.object_list.keys() for object_name in objects_name: this_object = self.object_list[object_name] print("Respawning {}".format(object_name)) # remove old objects in Gazebo self.delete_model(object_name) # respawn new objects in Gazebo roll, pitch, yaw, x, y, z = self.msg2pose(this_object.abs_pose) object_pose = self.pose2msg(x, y, z, roll, pitch, yaw) self.spawn_model(object_name, object_pose) # respawn objects in Rviz p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() self.clean_scene(object_name) p.pose = copy.copy(this_object.relative_pose) shape = this_object.shape if shape == "box": x = this_object.length y = this_object.width z = this_object.height size = (x, y, z) self.scene.add_box(object_name, p, size) elif shape == "cylinder": height = this_object.height radius = this_object.width / 2 self.scene.add_cylinder(object_name, p, height, radius) elif shape == "sphere": radius = this_object.width / 2 self.scene.add_sphere(object_name, p, radius) rospy.sleep(0.5) def spawn_all_model(self): filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_vacuum_gripper', 'interfaces', 'models_info.yaml') with open(filename) as file: objects_info = yaml.load(file) robot_x = objects_info["robot"]["pose"]["x"] robot_y = objects_info["robot"]["pose"]["y"] robot_z = objects_info["robot"]["pose"]["z"] robot_roll = objects_info["robot"]["pose"]["roll"] robot_pitch = objects_info["robot"]["pose"]["pitch"] robot_yaw = objects_info["robot"]["pose"]["yaw"] rospy.loginfo("Spawning Objects in Gazebo") objects = objects_info["objects"] objects_name = objects.keys() for object_name in objects_name: name = object_name shape = objects[name]["shape"] color = objects[name]["color"] # add object in Gazebo # self.delete_model(object_name) x = objects[name]["pose"]["x"] y = objects[name]["pose"]["y"] z = objects[name]["pose"]["z"] roll = objects[name]["pose"]["roll"] pitch = objects[name]["pose"]["pitch"] yaw = objects[name]["pose"]["yaw"] object_pose = self.pose2msg(x, y, z, roll, pitch, yaw) self.spawn_model(object_name, object_pose) # add object in planning scene(Rviz) p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() self.clean_scene(name) p.pose.position.x = x - robot_x p.pose.position.y = y - robot_y p.pose.position.z = z - robot_z q = quaternion_from_euler(roll, pitch, yaw) p.pose.orientation = Quaternion(*q) if shape == "box": x = objects[name]["size"]["x"] y = objects[name]["size"]["y"] z = objects[name]["size"]["z"] p.pose.position.z += z / 2 # size = (x, y, z) # self.scene.add_box(name, p, size) height = z width = y length = x self.object_list[name] = Object(p.pose, object_pose, height, width, length, shape, color) elif shape == "cylinder": height = objects[name]["size"]["height"] radius = objects[name]["size"]["radius"] p.pose.position.z += height / 2 self.scene.add_cylinder(name, p, height, radius) self.object_list[name] = Object(p.pose, object_pose, height, radius * 2, radius * 2, shape, color) elif shape == "sphere": radius = objects[name]["size"] p.pose.position.z += radius self.scene.add_sphere(name, p, radius) self.object_list[name] = Object(p.pose, object_pose, radius * 2, radius * 2, radius * 2, shape, color) rospy.sleep(0.5) rospy.loginfo("Spawning Obstacles in planning scene") obstacles = objects_info["obstacles"] obstacles_name = obstacles.keys() for obstacle_name in obstacles_name: name = obstacle_name p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() self.clean_scene(name) p.pose.position.x = obstacles[name]["pose"]["x"] - robot_x p.pose.position.y = obstacles[name]["pose"]["y"] - robot_y p.pose.position.z = obstacles[name]["pose"]["z"] - robot_z q = quaternion_from_euler(robot_roll, robot_pitch, robot_yaw) p.pose.orientation = Quaternion(*q) x = obstacles[name]["size"]["x"] y = obstacles[name]["size"]["y"] z = obstacles[name]["size"]["z"] size = (x, y, z) self.scene.add_box(name, p, size) rospy.sleep(0.5)
def main(argv): #This should become dynamic, e.g. rosparam offset = [-0.450, -0.450, 0.0] UDP_IP = "127.0.0.1" UDP_PORT = 9000 rospy.init_node("Collision_adder") transform = tf.TransformListener() rospack = rospkg.RosPack() #while not t.canTransform("shoulder_link", "base_link", rospy.Time.now()): # print "." mesh_path = rospack.get_path("ur_description") + "/meshes/ur5/collision/" scene = PlanningSceneInterface() robot = RobotCommander() rospy.sleep(2) sock = socket.socket( socket.AF_INET, # Internet socket.SOCK_DGRAM) # UDP sock.bind((UDP_IP, UDP_PORT)) #I assume that the base can not move, e.g. is mounted in a fixed location. quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0) p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = offset[0] p.pose.position.y = offset[1] p.pose.position.z = offset[2] p.pose.orientation.x = quaternion[0] p.pose.orientation.y = quaternion[1] p.pose.orientation.z = quaternion[2] p.pose.orientation.w = quaternion[3] scene.add_mesh("base", p, mesh_path + "/base.stl") #scene.is_diff = True while not rospy.is_shutdown(): data, addr = sock.recvfrom(2024) # buffer size is 1024 bytes print "received message:", data jointstates = data.split(",") #t = transform.getLatestCommonTime("/shoulder_link", "/base_link") #shoulder_pose, shoulder_quaternion = transform.lookupTransform("base_link", "shoulder_link", t) shoulderstate = jointstates[0].split(";") shoulder = PoseStamped() shoulder.header.frame_id = robot.get_planning_frame() shoulder.pose.position.x = offset[0] + float(shoulderstate[1]) shoulder.pose.position.y = offset[1] + float(shoulderstate[2]) shoulder.pose.position.z = offset[2] + float(shoulderstate[3]) shoulder.pose.orientation.x = float(shoulderstate[4]) shoulder.pose.orientation.y = float(shoulderstate[5]) shoulder.pose.orientation.z = float(shoulderstate[6]) shoulder.pose.orientation.w = float(shoulderstate[7]) scene.add_mesh(shoulderstate[0], shoulder, mesh_path + shoulderstate[0] + ".stl") #t = transform.getLatestCommonTime("/upper_arm_link", "/base_link") #upper_arm_pose, upper_arm_quaternion = transform.lookupTransform("base_link", "upper_arm_link", t) upperarmstate = jointstates[1].split(";") upper_arm = PoseStamped() upper_arm.header.frame_id = robot.get_planning_frame() upper_arm.pose.position.x = offset[0] + float(upperarmstate[1]) upper_arm.pose.position.y = offset[1] + float(upperarmstate[2]) upper_arm.pose.position.z = offset[2] + float(upperarmstate[3]) upper_arm.pose.orientation.x = float(upperarmstate[4]) upper_arm.pose.orientation.y = float(upperarmstate[5]) upper_arm.pose.orientation.z = float(upperarmstate[6]) upper_arm.pose.orientation.w = float(upperarmstate[7]) scene.add_mesh(upperarmstate[0], upper_arm, mesh_path + upperarmstate[0] + ".stl") #t = transform.getLatestCommonTime("/forearm_link", "/base_link") #fore_arm_pose, fore_arm_quaternion = transform.lookupTransform("base_link", "forearm_link", t) forearmstate = jointstates[2].split(";") fore_arm = PoseStamped() fore_arm.header.frame_id = robot.get_planning_frame() fore_arm.pose.position.x = offset[0] + float(forearmstate[1]) fore_arm.pose.position.y = offset[1] + float(forearmstate[2]) fore_arm.pose.position.z = offset[2] + float(forearmstate[3]) fore_arm.pose.orientation.x = float(forearmstate[4]) fore_arm.pose.orientation.y = float(forearmstate[5]) fore_arm.pose.orientation.z = float(forearmstate[6]) fore_arm.pose.orientation.w = float(forearmstate[7]) scene.add_mesh(forearmstate[0], fore_arm, mesh_path + forearmstate[0] + ".stl") #t = transform.getLatestCommonTime("/wrist_1_link", "/base_link") #wrist1_pose, wrist1_quaternion = transform.lookupTransform("base_link", "wrist_1_link", t) wrist1state = jointstates[3].split(";") wrist1 = PoseStamped() wrist1.header.frame_id = robot.get_planning_frame() wrist1.pose.position.x = offset[0] + float(wrist1state[1]) wrist1.pose.position.y = offset[1] + float(wrist1state[2]) wrist1.pose.position.z = offset[2] + float(wrist1state[3]) wrist1.pose.orientation.x = float(wrist1state[4]) wrist1.pose.orientation.y = float(wrist1state[5]) wrist1.pose.orientation.z = float(wrist1state[6]) wrist1.pose.orientation.w = float(wrist1state[7]) scene.add_mesh(wrist1state[0], wrist1, mesh_path + wrist1state[0] + ".stl") #t = transform.getLatestCommonTime("/wrist_2_link", "/base_link") #wrist2_pose, wrist2_quaternion = transform.lookupTransform("base_link", "wrist_2_link", t) wrist2state = jointstates[4].split(";") wrist2 = PoseStamped() wrist2.header.frame_id = robot.get_planning_frame() wrist2.pose.position.x = offset[0] + float(wrist2state[1]) wrist2.pose.position.y = offset[1] + float(wrist2state[2]) wrist2.pose.position.z = offset[2] + float(wrist2state[3]) wrist2.pose.orientation.x = float(wrist2state[4]) wrist2.pose.orientation.y = float(wrist2state[5]) wrist2.pose.orientation.z = float(wrist2state[6]) wrist2.pose.orientation.w = float(wrist2state[7]) scene.add_mesh(wrist2state[0], wrist2, mesh_path + wrist2state[0] + ".stl") #t = transform.getLatestCommonTime("/wrist_3_link", "/base_link") #wrist3_pose, wrist3_quaternion = transform.lookupTransform("base_link", "wrist_3_link", t) wrist3state = jointstates[5].split(";") wrist3 = PoseStamped() wrist3.header.frame_id = robot.get_planning_frame() wrist3.pose.position.x = offset[0] + float(wrist3state[1]) wrist3.pose.position.y = offset[1] + float(wrist3state[2]) wrist3.pose.position.z = offset[2] + float(wrist3state[3]) wrist3.pose.orientation.x = float(wrist3state[4]) wrist3.pose.orientation.y = float(wrist3state[5]) wrist3.pose.orientation.z = float(wrist3state[6]) wrist3.pose.orientation.w = float(wrist3state[7]) scene.add_mesh(wrist3state[0], wrist3, mesh_path + wrist3state[0] + ".stl")
class DaArmServer: """The basic, design problem/tui agnostic arm server """ gestures = {} grasp_height = 0.1 drop_height = 0.2 moving = False paused = False move_group_state = "IDLE" def __init__(self, num_planning_attempts=20): rospy.init_node("daarm_server", anonymous=True) self.init_params() self.init_scene() self.init_publishers() self.init_subscribers() self.init_action_clients() self.init_service_clients() self.init_arm(num_planning_attempts) def init_arm(self, num_planning_attempts=20): self.arm = MoveGroupCommander("arm") self.gripper = MoveGroupCommander("gripper") self.robot = RobotCommander() self.arm.set_num_planning_attempts(num_planning_attempts) self.arm.set_goal_tolerance(0.2) self.init_services() self.init_action_servers() def init_scene(self): world_objects = ["table", "tui", "monitor", "overhead", "wall"] self.robot = RobotCommander() self.scene = PlanningSceneInterface() for obj in world_objects: self.scene.remove_world_object(obj) rospy.sleep(0.5) self.tuiPose = PoseStamped() self.tuiPose.header.frame_id = self.robot.get_planning_frame() self.wallPose = PoseStamped() self.wallPose.header.frame_id = self.robot.get_planning_frame() self.tuiPose.pose.position = Point(0.3556, -0.343, -0.51) self.tuiDimension = (0.9906, 0.8382, 0.8636) self.wallPose.pose.position = Point(-0.508, -0.343, -0.3048) self.wallDimension = (0.6096, 2, 1.35) rospy.sleep(0.5) self.scene.add_box("tui", self.tuiPose, self.tuiDimension) self.scene.add_box("wall", self.wallPose, self.wallDimension) def init_params(self): try: self.grasp_height = get_ros_param("GRASP_HEIGHT", "Grasp height defaulting to 0.1") self.drop_height = get_ros_param("DROP_HEIGHT", "Drop height defaulting to 0.2") except ValueError as e: rospy.loginfo(e) def init_publishers(self): self.calibration_publisher = rospy.Publisher("/calibration_results", CalibrationParams) self.action_belief_publisher = rospy.Publisher("/arm_action_beliefs", String, queue_size=1) rospy.sleep(0.5) def init_subscribers(self): self.joint_angle_subscriber = rospy.Subscriber( '/j2s7s300_driver/out/joint_angles', JointAngles, self.update_joints) self.move_it_feedback_subscriber = rospy.Subscriber( '/move_group/feedback', MoveGroupActionFeedback, self.update_move_group_state) def init_action_servers(self): self.calibration_server = actionlib.SimpleActionServer("calibrate_arm", CalibrateAction, self.calibrate) self.move_block_server = actionlib.SimpleActionServer("move_block", MoveBlockAction, self.handle_move_block) #self.home_arm_server = actionlib.SimpleActionServer("home_arm", HomeArmAction, self.home_arm) def init_services(self): self.home_arm_service = rospy.Service("/home_arm", ArmCommand, self.handle_home_arm) # emergency stop self.stop_arm_service = rospy.Service("/stop_arm", ArmCommand, self.handle_stop_arm) # stop and pause for a bit self.pause_arm_service = rospy.Service("/pause_arm", ArmCommand, self.handle_pause_arm) self.start_arm_service = rospy.Service("/restart_arm", ArmCommand, self.handle_restart_arm) def init_action_clients(self): # Action Client for joint control joint_action_address = '/j2s7s300_driver/joints_action/joint_angles' self.joint_action_client = actionlib.SimpleActionClient( joint_action_address, kinova_msgs.msg.ArmJointAnglesAction) rospy.loginfo('Waiting for ArmJointAnglesAction server...') self.joint_action_client.wait_for_server() rospy.loginfo('ArmJointAnglesAction Server Connected') # Service to move the gripper fingers finger_action_address = '/j2s7s300_driver/fingers_action/finger_positions' self.finger_action_client = actionlib.SimpleActionClient( finger_action_address, kinova_msgs.msg.SetFingersPositionAction) self.finger_action_client.wait_for_server() def init_service_clients(self): self.is_simulation = None try: self.is_simulation = get_ros_param("IS_SIMULATION", "") except: self.is_simulation = False if self.is_simulation is True: # setup alternatives to jaco services for emergency stop, joint control, and finger control pass # Service to get TUI State rospy.wait_for_service('get_tui_blocks') self.get_block_state = rospy.ServiceProxy('get_tui_blocks', TuiState) # Service for homing the arm home_arm_service = '/j2s7s300_driver/in/home_arm' self.home_arm_client = rospy.ServiceProxy(home_arm_service, HomeArm) rospy.loginfo('Waiting for kinova home arm service') rospy.wait_for_service(home_arm_service) rospy.loginfo('Kinova home arm service server connected') # Service for emergency stop emergency_service = '/j2s7s300_driver/in/stop' self.emergency_stop = rospy.ServiceProxy(emergency_service, Stop) rospy.loginfo('Waiting for Stop service') rospy.wait_for_service(emergency_service) rospy.loginfo('Stop service server connected') # Service for restarting the arm start_service = '/j2s7s300_driver/in/start' self.restart_arm = rospy.ServiceProxy(start_service, Start) rospy.loginfo('Waiting for Start service') rospy.wait_for_service(start_service) rospy.loginfo('Start service server connected') def handle_start_arm(self, message): return self.restart_arm() def handle_stop_arm(self, message): return self.stop_motion() def handle_pause_arm(self, message): self.stop_motion(home=True, pause=True) return str(self.paused) def handle_restart_arm(self, message): self.restart_arm() self.paused = False return str(self.paused) def handle_home_arm(self, message): try: status = self.home_arm_kinova() return json.dumps(status) except rospy.ServiceException as e: rospy.loginfo("Homing arm failed") def home_arm(self): # send the arm home # for now, let's just use the kinova home self.home_arm_client() def home_arm_kinova(self): """Takes the arm to the kinova default home if possible """ self.arm.set_named_target("Home") try: self.arm.go() return "successful home" except: return "failed to home" def move_fingers(self, finger1_pct, finger2_pct, finger3_pct): finger_max_turn = 6800 goal = kinova_msgs.msg.SetFingersPositionGoal() goal.fingers.finger1 = float((finger1_pct/100.0)*finger_max_turn) goal.fingers.finger2 = float((finger2_pct/100.0)*finger_max_turn) goal.fingers.finger3 = float((finger3_pct/100.0)*finger_max_turn) self.finger_action_client.send_goal(goal) if self.finger_action_client.wait_for_result(rospy.Duration(5.0)): return self.finger_action_client.get_result() else: self.finger_action_client.cancel_all_goals() rospy.loginfo('the gripper action timed-out') return None def move_joint_angles(self, angle_set): goal = kinova_msgs.msg.ArmJointAnglesGoal() goal.angles.joint1 = angle_set[0] goal.angles.joint2 = angle_set[1] goal.angles.joint3 = angle_set[2] goal.angles.joint4 = angle_set[3] goal.angles.joint5 = angle_set[4] goal.angles.joint6 = angle_set[5] goal.angles.joint7 = angle_set[6] self.joint_action_client.send_goal(goal) if self.joint_action_client.wait_for_result(rospy.Duration(20.0)): return self.joint_action_client.get_result() else: print(' the joint angle action timed-out') self.joint_action_client.cancel_all_goals() return None def handle_move_block(self, message): """msg format: {id: int, source: Point {x: float,y: float}, target: Point {x: float, y: float} """ def open_gripper(self, delay=0): """open the gripper ALL THE WAY, then delay """ if self.is_simulation is True: self.gripper.set_named_target("Open") self.gripper.go() else: self.move_fingers(50, 50, 50) rospy.sleep(delay) def close_gripper(self, delay=0): """close the gripper ALL THE WAY, then delay """ self.gripper.set_named_target("Close") self.gripper.go() rospy.sleep(delay) def handle_gesture(self, gesture): # lookup the gesture from a table? or how about a message? # one possibility, object that contains desired deltas in pos/orientation # as well as specify the arm or gripper choice pass def move_arm_to_pose(self, position, orientation, delay=0, waypoints=[], corrections=1, action_server=None): if len(waypoints) > 0: # this is good for doing gestures plan, fraction = self.arm.compute_cartesian_path(waypoints, eef_step=0.01, jump_threshold=0.0) else: p = self.arm.get_current_pose() p.pose.position = position p.pose.orientation = orientation self.arm.set_pose_target(p) plan = self.arm.plan() if plan: # get the last pose to correct if desired ptPos = plan.joint_trajectory.points[-1].positions # print "==================================" # print "Last point of the current trajectory: " angle_set = list() for i in range(len(ptPos)): tempPos = ptPos[i]*180/np.pi + int(round((self.joint_angles[i] - ptPos[i]*180/np.pi)/(360)))*360 angle_set.append(tempPos) if action_server: action_server.publish_feedback(CalibrateFeedback("Plan Found")) self.arm.execute(plan, wait=False) while self.move_group_state is not "IDLE": rospy.sleep(0.001) print(self.move_group_state) if self.paused is True: self.arm.stop() return rospy.loginfo("LEAVING THE WHILE LOOP") print("LEAVING THE LOOOOOOOOOP!!!!") if corrections > 0: rospy.loginfo("Correcting the pose") self.move_joint_angles(angle_set) rospy.sleep(delay) else: if action_server: action_server.publish_feedback(CalibrateFeedback("Planning Failed")) # Let's have the caller handle sequences instead. # def handle_move_sequence(self, message): # # if the move fails, do we cancel the sequence # cancellable = message.cancellable # moves = message.moves # for move in moves: # try: # self.handle_move_block(move) # except Exception: # if cancellable: # rospy.loginfo("Part of move failed, cancelling the rest.") # break def update_move_group_state(self, message): rospy.loginfo(message.feedback.state) self.move_group_state = message.feedback.state def get_grasp_orientation(self, position): return Quaternion(1, 0, 0, 0) def update_joints(self, joints): self.joint_angles = [joints.joint1, joints.joint2, joints.joint3, joints.joint4, joints.joint5, joints.joint6, joints.joint7] def move_z(self, distance): p = self.arm.get_current_pose() p.pose.position.z += distance self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0) def pick_block(self, location, check_grasp=False, retry_attempts=0, delay=0, action_server=None): # go to a spot and pick up a block # if check_grasp is true, it will check torque on gripper and retry or fail if not holding # open the gripper self.open_gripper() position = Point(location.x, location.y, self.grasp_height) orientation = self.get_grasp_orientation(position) try: self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) except Exception as e: raise(e) if action_server: action_server.publish_feedback() if check_grasp is True: pass # for now, but we want to check finger torques # for i in range(retry_attempts): # self.move_z(0.3) self.close_gripper() self.move_z(0.1) rospy.sleep(delay) def place_block(self, location, check_grasp=False, delay=0, action_server=None): # go to a spot an drop a block # if check_grasp is true, it will check torque on gripper and fail if not holding a block position = Point(location.x, location.y, self.drop_height) orientation = self.get_grasp_orientation(position) try: self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) except Exception as e: raise(e) if action_server: action_server.publish_feedback() self.open_gripper() self.move_z(0.1) self.close_gripper() def stop_motion(self, home=False, pause=False): rospy.loginfo("STOPPING the ARM") # cancel the moveit_trajectory # self.arm.stop() # call the emergency stop on kinova # self.emergency_stop() # rospy.sleep(0.5) if pause is True: self.paused = True if home is True: # self.restart_arm() self.home_arm() def calibrate(self, message): print("calibrating ", message) self.place_calibration_block() rospy.sleep(5) # five seconds to correct the drop if it bounced, etc. self.calibration_server.publish_feedback(CalibrateFeedback("getting the coordinates")) params = self.record_calibration_params() self.set_arm_calibration_params(params[0], params[1]) self.calibration_publisher.publish(CalibrationParams(params[0], params[1])) self.calibration_server.set_succeeded(CalibrateResult(params)) def set_arm_calibration_params(self, arm_x_min, arm_y_min): rospy.set_param("ARM_X_MIN", arm_x_min) rospy.set_param("ARM_Y_MIN", arm_y_min) def place_calibration_block(self): # start by homing the arm (kinova home) self.calibration_server.publish_feedback(CalibrateFeedback("homing")) self.home_arm_kinova() # go to grab a block from a human self.open_gripper() self.close_gripper() self.open_gripper(4) self.close_gripper(2) # move to a predetermined spot self.calibration_server.publish_feedback(CalibrateFeedback("moving to drop")) self.move_arm_to_pose(Point(0.4, -0.4, 0.1), Quaternion(0, 1, 0, 0)) # drop the block self.open_gripper() self.calibration_server.publish_feedback(CalibrateFeedback("dropped the block")) calibration_block = {'x': 0.4, 'y': 0.4, 'id': 0} calibration_action_belief = {"action": "add", "block": calibration_block} self.action_belief_publisher.publish(String(json.dumps(calibration_action_belief))) rospy.loginfo("published arm belief") return def record_calibration_params(self): """ Call the block_tracker service to get the current table config. Find the calibration block and set the appropriate params. """ # make sure we know the dimensions of the table before we start # fixed table dimensions include tuio_min_x,tuio_min_y,tuio_dist_x,tuio_dist_y,arm_dist_x,arm_dist_y tuio_bounds = get_tuio_bounds() arm_bounds = get_arm_bounds(calibrate=False) try: block_state = json.loads(self.get_block_state("tuio").tui_state) except rospy.ServiceException as e: # self.calibration_server.set_aborted() raise(ValueError("Failed getting block state to calibrate: "+str(e))) if len(block_state) != 1: # self.calibration_server.set_aborted() raise(ValueError("Failed calibration, either couldn't find block or > 1 block on TUI!")) # if we made it here, let's continue! # start by comparing the reported tuio coords where we dropped the block # with the arm's localization of the end-effector that dropped it # (we assume that the block fell straight below the drop point) drop_pose = self.arm.get_current_pose() end_effector_x = drop_pose.pose.position.x end_effector_y = drop_pose.pose.position.y block_tuio_x = block_state[0]['x'] block_tuio_y = block_state[0]['y'] x_ratio, y_ratio = tuio_to_ratio(block_tuio_x, block_tuio_y, tuio_bounds) arm_x_min = end_effector_x - x_ratio*arm_bounds["x_dist"] arm_y_min = end_effector_y - y_ratio*arm_bounds["y_dist"] return [arm_x_min, arm_y_min]
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander(GROUP_NAME_ARM) right_arm.set_planner_id("KPIECEkConfigDefault") right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) eef = right_arm.get_end_effector_link() rospy.sleep(2) scene.remove_attached_object(GRIPPER_FRAME, "part") # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") #right_arm.set_named_target("r_start") #right_arm.go() #right_gripper.set_named_target("right_gripper_open") #right_gripper.go() rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add a table #p.pose.position.x = 0.42 #p.pose.position.y = -0.2 #p.pose.position.z = 0.3 #scene.add_box("table", p, (0.5, 1.5, 0.6)) # add an object to be grasped p.pose.position.x = 0.40 p.pose.position.y = -0.0 p.pose.position.z = 0.85 scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) g = Grasp() g.id = "test" start_pose = PoseStamped() start_pose.header.frame_id = FIXED_FRAME # start the gripper in a neutral pose part way to the target start_pose.pose.position.x = 0.37636 start_pose.pose.position.y = 0.21886 start_pose.pose.position.z = 0.9 start_pose.pose.orientation.x = 0.00080331 start_pose.pose.orientation.y = 0.001589 start_pose.pose.orientation.z = -2.4165e-06 start_pose.pose.orientation.w = 1 right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(2) # generate a list of grasps grasps = self.make_grasps(start_pose) result = False n_attempts = 0 # repeat until will succeed while result == False: result = robot.right_arm.pick("part", grasps) n_attempts += 1 print "Attempts: ", n_attempts rospy.sleep(0.3) rospy.spin() roscpp_shutdown()
# clean the scene scene.remove_world_object("table") scene.remove_world_object("part") scene.remove_attached_object(GRIPPER_FRAME, "part") #rospy.logwarn("cleaning world") #right_arm.set_named_target("r_start") #right_arm.go() #right_gripper.set_named_target("open") #right_gripper.go() rospy.sleep(3) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add a table p.pose.position.x = 1.0 p.pose.position.y = 0.2 p.pose.position.z = 0.3 scene.add_box("table", p, (0.7, 1, 0.7)) # add an object to be grasped p.pose.position.x = 0.4 p.pose.position.y = 0 p.pose.position.z = 0.75 scene.add_box("part", p, (0.07, 0.01, 0.2)) # add a position for placement p1 = PoseStamped()
class CreateScene2(object): def __init__(self): self._scene = PlanningSceneInterface() self.robot = RobotCommander() # pause to wait for rviz to load rospy.sleep(4) self.first_stamp = None self.cloud_pub = rospy.Publisher('/camera/depth_registered/points', PointCloud2, queue_size=20, latch=True) self.cloud_sub = rospy.Subscriber( '/camera/depth_registered/points_old', PointCloud2, self.msg_cb) # clear the scene self._scene.remove_world_object() # add floor object floor_pose = [0, 0, -0.12, 0, 0, 0, 1] floor_dimensions = [4, 4, 0.02] self.add_box_object("floor", floor_dimensions, floor_pose) # add collision objects self._kinect_pose_1 = [0.0, 0.815, 0.665, 0, -0.707, 0.707, 0] # stream the kinect tf kinect_cloud_topic = "kinect2_rgb_optical_frame" thread.start_new_thread(self.spin, (kinect_cloud_topic, )) def msg_cb(self, msg): global first_stamp, now if self.first_stamp is None: now = rospy.Time.now() first_stamp = msg.header.stamp msg.header.stamp -= first_stamp msg.header.stamp += now for i in range(3): self.cloud_pub.publish(msg) def add_box_object(self, name, dimensions, pose): p = geometry_msgs.msg.PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = pose[0] p.pose.position.y = pose[1] p.pose.position.z = pose[2] p.pose.orientation.x = pose[3] p.pose.orientation.y = pose[4] p.pose.orientation.z = pose[5] p.pose.orientation.w = pose[6] self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2])) rospy.sleep(0.2) def spin(self, cloud_topic): tf_broadcaster = tf.TransformBroadcaster() rate = rospy.Rate(20) while not rospy.is_shutdown(): tf_broadcaster.sendTransform( (self._kinect_pose_1[0], self._kinect_pose_1[1], self._kinect_pose_1[2]), (self._kinect_pose_1[3], self._kinect_pose_1[4], self._kinect_pose_1[5], self._kinect_pose_1[6]), rospy.Time.now(), cloud_topic, "world") rate.sleep()
class UR5_Gripped_Manipulator: """ Class Attributes Robot : Robot Commander Object Scene : Planning Scene Interface Object (current scene) Man : MoveGroupCommander object to control manipulator EEF : MoveGroupCommander object to control endeffector Target_poses : Pose list of created target objects Picked : List of part indexes to ensure to picked non-picked ones """ def __init__(self, scene=None, manip_name="manipulator", eef_name="endeffector"): self.robot = RobotCommander() self.man = MoveGroupCommander(manip_name) self.eef = MoveGroupCommander(eef_name) self.scene = scene self.target_poses = [] self.picked = [] # self.client = actionlib.SimpleActionClient('gripper_controller/gripper_action', GripperCommandAction) self.client = actionlib.SimpleActionClient( 'gripper_controller/gripper_action', FollowJointTrajectoryAction) # self.client.wait_for_server() if not self.client.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return """ ----------------Function Name: clean_scene---------------- Definition: Clears the target_poses and picked lists in order to establish a new execution. Necessary for test_simulation method """ def clear_poses(self): for i in xrange(len(self.target_poses)): self.target_poses.pop() for i in xrange(len(self.picked)): self.picked.pop() """ ----------------Function Name: clean_scene---------------- Definition: Clears all non-attached objects from planning scene """ def clean_scene(self): rospy.loginfo("Clearing the Scene") self.scene.remove_world_object() """ ----------------Function Name: add_object---------------- Name : Object Name Pose : Pose of the Object (x,y,z,xo,yo,zo,wo) Dimension : Dimensions of the Obhect (Tuple) (d1,d2,d3) Type : Box(0),Sphere(1) d1 is radius for sphere i.e typ==1, """ def add_object(self, name, x, y, z, xo=0.0, yo=0.0, zo=0.0, wo=0.0, d1=0.1, d2=0.1, d3=0.1, typ=0): pos = PoseStamped() pos.header.frame_id = self.robot.get_planning_frame() pos.pose.position.x = x pos.pose.position.y = y pos.pose.position.z = z pos.pose.orientation.x = xo pos.pose.orientation.y = yo pos.pose.orientation.z = zo pos.pose.orientation.w = wo rospy.loginfo("ADDING OBJECT") if (typ == 0): self.scene.add_box(name, pos, (d1, d2, d3)) elif (typ == 1): self.scene.add_sphere(name, pos, d1) else: print("ERROR in Type") return pos """ ----------------Function Name: create_environment---------------- Definition : Creates the simulation environment with non-target collision objects, can be edited to create different default environment """ def create_environment(self): # self.add_object(name="wall",x=0.0,y=0.8,z=0.5,d1=0.1,d2=0.35,d3=1,typ=0) # self.add_object(name="wall_2",x=0.0,y=-0.8,z=0.5,d1=0.1,d2=0.35,d3=1,typ=0) self.add_object(name="table", x=0.0, y=0.0, z=-0.05, d1=2, d2=2, d3=0.0001, typ=0) """ ----------------Function Name: check_targets---------------- Definition : Creates given number of objects within the distance provided. It prevents collision object overlapping. It returns the pose list of the objects Number : Number of collision objects required to spawn Distance : Required minimum distance between each collision objects """ def check_targets(self, number, distance): rospy.loginfo("Creating Boxes!") def al(typ, x=0.0): if typ == 'x': return random.uniform(0.35, 0.65) elif typ == "y": rang = math.sqrt(0.8**2 - x**2) return random.uniform(-rang, rang) hemme = [] dims = [] cnt = 0 while len(hemme) != number: if cnt == 200: hemme = [] cnt = 0 cnt = cnt + 1 # dim = (random.uniform(0.08,0.12),random.uniform(0.08,0.10),random.uniform(0.05,0.2)) # quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0,random.uniform(-math.pi ,math.pi)) dim = (0.09, 0.09, 0.2) box = PoseStamped() box.header.frame_id = self.robot.get_planning_frame() box.pose.position.z = (-0.04 + (dim[2] / 2)) # box.pose.position.x = al('x') # box.pose.position.y = al('y',box.pose.position.x) # box.pose.orientation.x = quaternion[0] # box.pose.orientation.y = quaternion[1] # box.pose.orientation.z = quaternion[2] # box.pose.orientation.w = quaternion[3] box.pose.position.x = 0.35 box.pose.position.y = 0.35 box.pose.position.z = 0.05 box.pose.orientation.x = 0 box.pose.orientation.y = 0 box.pose.orientation.z = 0 box.pose.orientation.w = 1 flag = 1 for i in hemme: if abs(i.pose.position.x - box.pose.position.x) < distance or abs( i.pose.position.y - box.pose.position.y) < distance: flag = 0 if flag == 0: continue hemme.append(box) dims.append(dim) cnt = 0 names = [] for i in xrange(len(hemme)): now = "part" + str(cnt) cnt = cnt + 1 names.append(now) self.add_object(name=now, x=hemme[i].pose.position.x, y=hemme[i].pose.position.y, z=hemme[i].pose.position.z, xo=hemme[i].pose.orientation.x, yo=hemme[i].pose.orientation.y, zo=hemme[i].pose.orientation.z, wo=hemme[i].pose.orientation.w, d1=dims[i][0], d2=dims[i][1], d3=dims[i][2], typ=0) print("End Check!") rospy.loginfo(hemme) return hemme """ ----------------Function Name: clean_scene---------------- Definition : Clears all non-attached objects from planning scene Group : MoveGroupCommander object to control given group """ def default_state_gripper(self, grp): rospy.loginfo("Openning Gripper") joint_vals = grp.get_current_joint_values() joint_vals[0] = 0.0 grp.set_joint_value_target(joint_vals) init_plan = grp.plan() return grp.execute(init_plan) """ ----------------Function Name: closed_state_gripper---------------- Definition : Function that opens gripper and detachs the gripped object Obj : Name of the Object that is needed to detach """ def closed_state_gripper(self, obj): rospy.loginfo("Closing Gripper") def convert(width): return 0.77 - width / 0.15316 * 0.77 width = self.scene.get_objects([obj])[obj].primitives[0].dimensions[0] width = convert(width) now = self.robot.endeffector.get_current_joint_values()[0] cnt = 0 while abs(now - width) > 0.0001: now = self.robot.endeffector.get_current_joint_values()[0] cnt = cnt + 1 tmp = width - abs(now - width) / 2.0 self.robot.endeffector.set_joint_value_target('finger_joint', tmp) pl = self.robot.endeffector.plan() # self.robot.endeffector.go() self.client.wait_for_server() goal = FollowJointTrajectoryAction().action_goal.goal goal.trajectory = pl.joint_trajectory rospy.loginfo(goal) # goal=GripperCommandAction().action_goal.goal # goal.command.position = tmp # goal.command.max_effort=1 state = self.client.send_goal_and_wait(goal) rospy.loginfo(state) if state == 0: rospy.logerr('Pick up goal failed!: %s' % self.client.get_goal_status_text()) return None rospy.sleep(0.05) if cnt == 7: break rospy.sleep(1.0) ret = self.robot.manipulator.attach_object(obj) return ret """ ----------------Function Name: pick_object---------------- Definition : It moves the given group i.e robot to the collision object whose index is given and picks that object. Group : MoveGroupCommander object to control given group Part_index : Index of the target object to obtain pose """ def pick_object(self, group, part_index): rospy.loginfo("Pick Operation starts!") gripped_object = scene.get_objects(["part" + str(part_index) ])["part" + str(part_index)] pos = copy.deepcopy(self.target_poses[part_index]) rospy.loginfo(pos) temp = tf.transformations.euler_from_quaternion( (pos.pose.orientation.x, pos.pose.orientation.y, pos.pose.orientation.z, pos.pose.orientation.w)) quaternion = tf.transformations.quaternion_from_euler( math.pi, temp[1], temp[2]) # pos.pose.position.z += 0.17 + (gripped_object.primitives[0].dimensions[2]/2.0) pos.pose.position.z += 0.14 + ( gripped_object.primitives[0].dimensions[2] / 2.0) # pos.pose.orientation.x = quaternion[0] # pos.pose.orientation.y = quaternion[1] # pos.pose.orientation.z = quaternion[2] # pos.pose.orientation.w = quaternion[3] pos.pose.orientation.x = 0 pos.pose.orientation.y = 0.707 pos.pose.orientation.z = 0 pos.pose.orientation.w = 0.707 rospy.loginfo(pos) group.set_pose_target(pos) move_plan = group.plan() i = 0 while (not move_plan.joint_trajectory.joint_names): move_plan = group.plan() i += 1 if (i == 500): break state = group.execute(move_plan) rospy.loginfo("Execute operation for Object is %s" % str(part_index)) if (state): self.closed_state_gripper("part" + str(part_index)) rospy.sleep(1.0) self.place_object(group, part_index) return else: return """ ----------------Function Name: pick_object---------------- Definition : It places the gripped object to the target location Group : MoveGroupCommander object to control given group Part_index : Index of the target object to obtain pose """ def place_object(self, group, part_index): def al(typ, x=0.0): if typ == 'x': return random.uniform(-0.35, -0.6) elif typ == 'y': rang = math.sqrt(0.75**2 - x**2) x = random.uniform(-rang, rang) return x pos = PoseStamped() pos.header.frame_id = self.robot.get_planning_frame() pos.pose.position.x = al("x") pos.pose.position.y = al("y", pos.pose.position.x) group.set_pose_target(pos) #This line makes placing possible by setting a valid z position for gripped object pos.pose.position.z = -0.04 + 0.17 + ((scene.get_attached_objects([ "part" + str(part_index) ])["part" + str(part_index)]).object.primitives[0].dimensions[2]) pos.pose.orientation.y = 1.0 group.set_pose_target(pos) move_plan = group.plan() state = group.execute(move_plan) rospy.loginfo("placed") if (state): detached = group.detach_object("part" + str(part_index)) rospy.sleep(1.5) if (detached): self.scene.remove_world_object("part" + str(part_index)) rospy.sleep(1.5) self.default_state_gripper(self.eef) self.picked.append(part_index) else: self.default_state_gripper(self.robot.endeffector) group.detach_object("part" + str(part_index)) rospy.sleep(2) """ ----------------Function Name: execute_simulation---------------- Definition : Creates the specified environment , generates motion plans and does the pick and place operation based on these plans. To visualize RViz is needed. Planner ID can be changed based on MoveIT's supported OMPL. """ def execute_simulation(self, num_of_objects=1): is_success = False #Reset State of the Gripper self.default_state_gripper(self.eef) #### #Reset the position of the Arm # Will be implemented if needed #### #Clean the scene self.clean_scene() rospy.sleep(1.0) #Create environment self.create_environment() #Create targets self.target_poses = self.check_targets(num_of_objects, 0.10) rospy.sleep(5) #Planner setup self.man.set_planner_id("RRTConnectkConfigDefault") self.man.set_num_planning_attempts(20) self.man.allow_replanning(True) #Pick and place every object for i in xrange(len(self.target_poses)): if i not in self.picked: self.pick_object(group=self.man, part_index=i) rospy.loginfo("END OF PICK AND PLACE") if (len(self.picked) == len(self.target_poses)): is_success = True self.clear_poses() return is_success """ ----------------Function Name: execute_simulation---------------- Definition : Executes the simulation num_attempts times. Stores the success rate and writes into the specified document Num_Attempts : Limiting number for executing simulation. (Default Value: 10) File_Name : File name (with path if needed) to write test results """ def test_simulation(self, num_attempts=10, num_of_objects=1, file_name="ur5_pick_place_test.txt"): success_rate = 0 file = open(file_name, "a+") file.write("Start Test \n") for case in xrange(1, num_attempts + 1): state = self.execute_simulation(num_of_objects=num_of_objects) if (state): success_rate += 1 rospy.loginfo("Success Rate: " + str(success_rate) + "/" + str(case)) file.write("Test Case %s is successfull" % str(case)) else: rospy.loginfo("Success Rate: " + str(success_rate) + "/" + str(case)) file.write("Test Case %s is failedl" % str(case)) file.write("Success: %" + str(((success_rate / (case - 1) * 100.0)))) file.write("----END OF TEST----") file.close() return
class PigeonPickAndPlace: #Class constructor (parecido com java, inicializa o que foi instanciado) def __init__(self): # Retrieve params: self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016) self._arm_group = rospy.get_param('~arm', 'arm_move_group') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) # Create planning scene where we will add the objects etc. self._scene = PlanningSceneInterface() # Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple self._robot = RobotCommander() rospy.sleep(1.0) # Clean the scene (remove the old objects: self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: # TODO get the position of the detected object self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name) rospy.sleep(1.0) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown('Grasp generator action client not available!') return # Create move group 'pickup' action client: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # Pick object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') def __del__(self): # Clean the scene self._scene.remove_world_object(self._grasp_object_name) def _add_object_grasp(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() #pose p.header.stamp = rospy.Time.now() rospy.loginfo(self._robot.get_planning_frame()) p.pose.position.x = 0.11 # 0.26599 # antigo = 0.75 - 0.01 p.pose.position.y = -0.31 # -0.030892 #antigo = 0.25 - 0.01 p.pose.position.z = 0.41339 #antigo = 1.00 + (0.3 + 0.03) / 2.0 #p.pose.orientation.x = 0.0028925 #p.pose.orientation.y = -0.0019311 #p.pose.orientation.z = -0.71058 #p.pose.orientation.w = 0.70361 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/, # using the measure tape tool from meshlab. # The box is the bounding box of the lego cylinder. # The values are taken from the cylinder base diameter and height. # the size is length (x),thickness(y),height(z) # I don't know if the detector return this values of object self._scene.add_box(name, p, (0.032, 0.016, 0.020)) return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps #rospy.logerr('Generated: %f grasps:' % grasps.size) # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) return grasps def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) #goal.support_surface_name = self._table_object_name # Configure goal planning options: goal.allowed_planning_time = 5.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 10 return goal def _pickup(self, group, target, width): """ Pick up a target using the planning group """ # Obtain possible grasps from the grasp generator server: grasps = self._generate_grasps(self._pose_object_grasp, 0.016) # Create and send Pickup goal: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg)
class Pick_Place: def __init__(self): # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~arm', 'arm') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_distrost', 0.2) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.1) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander: self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) # Clean the scene: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: self._pose_table = self._add_table(self._table_object_name) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) rospy.sleep(1.0) # Define target place pose: self._pose_place = Pose() self._pose_place.position.x = self._pose_coke_can.position.x self._pose_place.position.y = self._pose_coke_can.position.y - 0.06 self._pose_place.position.z = self._pose_coke_can.position.z self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown('Grasp generator action client not available!') return # Create move group 'pickup' action client: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # Create move group 'place' action client: self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return # Pick Coke can object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') # Place Coke can object on another place on the support surface (table): while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Place successfully') def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._table_object_name) def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.45 p.pose.position.y = 0.0 p.pose.position.z = 0.0 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose def _add_grasp_block_(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.25 p.pose.position.y = 0.05 p.pose.position.z = 0.1 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.03, 0.03, 0.09)) return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp """ # Generate places: places = [] now = rospy.Time.now() for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle ) place.place_pose.pose.orientation = Quaternion(*q) # Generate pre place approach: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame() place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = 0.2 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame() place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 0.2 # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) return places def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) goal.support_surface_name = self._table_object_name # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _pickup(self, group, target, width): """ Pick up a target using the planning group """ # Obtain possible grasps from the grasp generator server: grasps = self._generate_grasps(self._pose_coke_can, width) # Create and send Pickup goal: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _place(self, group, target, place): """ Place a target using the planning group """ # Obtain possible places: places = self._generate_places(place) # Create and send Place goal: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg) def _publish_places(self, places): """ Publish places as poses, using a PoseArray message """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg)
class Pick_Place: def __init__(self, x, y, z): self._x = x self._y = y self._z = z # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param( '~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param( '~approach_retreat_min_dist', 0.1) # Create (debugging) publishers:创建(调试)发布者: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander:创建规划场景和机器人命令: self._scene = PlanningSceneInterface() #未知 self._robot = RobotCommander() #未知 self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name, self._x, self._y, self._z) rospy.sleep(0.5) # Define target place pose:定义目标位置姿势 self._pose_place = Pose() self._pose_place.position.x = 0 #self._pose_coke_can.position.x self._pose_place.position.y = -0.85 #self._pose_coke_can.position.y - 0.20 self._pose_place.position.z = 0.7 #self._pose_coke_can.position.z self._pose_place.orientation = Quaternion( *quaternion_from_euler(0.0, 0.0, 0.0)) self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9 #self._pose_coke_can.position.z - 0.9 # base_link is 0.9 above self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) self._arm.set_named_target('up') self._arm.go(wait=True) print("up pose") print("第一部分恢复位置初始") # Create grasp generator 'generate' action client: # #创建抓取生成器“生成”动作客户端: print("开始Action通信") self._grasps_ac = SimpleActionClient( '/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(0.5)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown( 'Grasp generator action client not available!') return print("结束Action通信") # Create move group 'pickup' action client: # 创建移动组“抓取”操作客户端: print("开始pickup通信") self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(0.5)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return print("结束pickup通信") # Create move group 'place' action client: # 创建移动组“放置”动作客户端: print("开始place通信") self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(0.5)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return print("结束place通信") # Pick Coke can object:抓取快 while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(0.5) print("抓取物体") rospy.loginfo('Pick up successfully') print("pose_place: ", self._pose_place) # Place Coke can object on another place on the support surface (table): while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(0.5) rospy.loginfo('Place successfully') def _generate_grasps(self, pose, width): """ 使用抓握生成器生成动作来生成抓握; 基于moveit_simple_grasps pkg上的server_test.py示例。 生成抓取姿势阵列数据以进行可视化, 然后将抓取目标发送到抓取服务器. """ self._grasps_ac.wait_for_server() rospy.loginfo("Successfully connected!") # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: # Sends a goal to the ActionServer, waits for the goal to complete, and preempts goal is necessary. # rospy.loginfo("Sent goal,waiting,目标物体的位置:\n " + str(goal)) #self._grasps_ac.wait_for_result() #发送目标并等待结果: # #将目标发送到ActionServer,等待目标完成,然后抢占目标是必要的。 t_start = rospy.Time.now() state = self._grasps_ac.send_goal_and_wait(goal) t_end = rospy.Time.now() t_toal = t_end - t_start rospy.loginfo("发送时间Result received in " + str(t_toal.to_sec())) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) """ print("---------grasps--start------------") print(grasps) print("-----------test-grasps-----end-----------------") rospy.sleep(2) print("-grasps---sleep----end-----") """ return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp 为该位置的姿势创建姿势数组数据 """ # Generate places: places = [] now = rospy.Time.now() #print("---------------test3--------------------------") for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle) place.place_pose.pose.orientation = Quaternion(*q) # Generate pre place approach:生成前置位置方法: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame( ) place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = 0.1 #print("place1---test=====") # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame( ) place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 0.06 #print("place2---test=====") # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) """ print(places) print("------test4---------------------------------------") """ return places def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal Create a goal for picking up the grasping object创建一个捡起抓取物体的目标 """ # Create goal: goal = PickupGoal() goal.group_name = group #规划的组 goal.target_name = target #要拾取的对象的名称( #可能使用的抓握列表。 必须至少填写一个掌握 goal.possible_grasps.extend(grasps) #障碍物的可选清单,我们掌握有关语义的信息,可以在抓取过程中触摸/推动/移动这些障碍物; 注意:如果使用对象名称“ all”,则在进近和提升过程中禁止与所有对象发生碰撞。 goal.allowed_touch_objects.append(target) #如果没有可用的名称,则在碰撞图中支撑表面(例如桌子)的名称可以留空 goal.support_surface_name = self._table_object_name # Configure goal planning options:配置目标计划选项: #运动计划者可以规划的最长时间 goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 """ print("----goal-------test-----") print(goal) print("----goal---end------test---") """ rospy.sleep(0.5) print("-goal---sleep----end-----") return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal Create a place goal for MoveIt! """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _add_grasp_block_(self, name, _x, _y, _z): """ Create and add block to the scene创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = _x p.pose.position.y = _y p.pose.position.z = _z q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. #self._scene.add_box(name, p, (0.045, 0.045, 0.045)) return p.pose def _pickup(self, group, target, width): """ Pick up a target using the planning group使用规划小组选择目标 """ # Obtain possible grasps from the grasp generator server:从掌握生成器服务器获取可能的抓握: grasps = self._generate_grasps(self._pose_coke_can, width) #print("--goal--start----") # Create and send Pickup goal:创建并发送zhua取目标: goal = self._create_pickup_goal(group, target, grasps) #print("--goal--end----") #print("---pick---up---1----") state = self._pickup_ac.send_goal_and_wait(goal) #print("-----------------state-------------------") #print(state) #print("---pick---up---2----") if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() """ print("-----test1--------") print(result) print("------test1--end------") """ # Check for error: err = result.error_code.val print(err) if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False print("pickup-----end-----") rospy.sleep(0.5) return True def _place(self, group, target, place): """ Place a target using the planning group """ # Obtain possible places:获取可能的位置: places = self._generate_places(place) # Create and send Place goal:创建并发送地方目标: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() """ print("-----test2--------") print(result) print("------test2--end------") """ # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message使用PoseArray消息将抓取发布为姿势 """ print(self._grasps_pub.get_num_connections()) if self._grasps_pub.get_num_connections() != 1: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) print(msg) rospy.loginfo('Publisher ' + str(len(msg.poses)) + ' poses') self._grasps_pub.publish(msg) """ print("11-111") rospy.loginfo('Publisher'+str(len(msg))+'poses') print("11-111") rospy.sleep(2) """ def _publish_places(self, places): """ Publish places as poses, using a PoseArray message """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg)
roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() rospy.sleep(1) # clean the scene #scene.remove_world_object("block1") #scene.remove_world_object("block2") #scene.remove_world_object("block3") #scene.remove_world_object("block4") #scene.remove_world_object("table") #scene.remove_world_object("bowl") #scene.remove_world_object("box") # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0.7 p.pose.position.y = -0.4 p.pose.position.z = 0.85 p.pose.orientation.w = 1.57 scene.add_box("block1", p, (0.044, 0.044, 0.044)) p.pose.position.y = -0.2 p.pose.position.z = 0.175 scene.add_box("block2", p, (0.044, 0.044, 0.044)) p.pose.position.x = 0.6 p.pose.position.y = -0.7 p.pose.position.z = 0.5 scene.add_box("block3", p, (0.044, 0.044, 0.044)) p.pose.position.x = 1 p.pose.position.y = -0.7 p.pose.position.z = 0.5
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() robot = RobotCommander() right_arm = MoveGroupCommander("right_arm") self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() rf = robot.get_planning_frame() print rf open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.087, 100)) close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.03, -1)) self.ac.send_goal(open) self.ac.wait_for_result() #print self.ac.get_result() # Prepare Gazebo Subscriber self.pwh = None self.bl = ['ground_plane','pr2'] self.pa = [] self.idx = [] self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) # Give the scene a chance to catch up rospy.sleep(2) while self.pwh is None: rospy.sleep(0.05) #self.new = [ x for x in self.pwh.name if x not in self.bl ] #for i in self.new: #self.idx.append(self.pwh.name.index(i)) #for j in self.idx: #self.pa.append(self.pwh.pose[j]) ##print self.pa #self.pose_array = PoseArray() #self.pose_array.header.frame_id = REFERENCE_FRAME #self.pose_array.poses = self.pa # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] #obstacle1_size = [0.1, 0.025, 0.01] target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') #obstacle1_id = 'obstacle1' #self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Remove leftover objects from a previous run scene.remove_world_object(target_id) scene.remove_world_object(table_id) #scene.remove_world_object(obstacle1_id) ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.019 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 0.995 scene.add_box(table_id, table_pose, table_size) #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = REFERENCE_FRAME #obstacle1_pose.pose = self.pwh.pose[self.o1id] ## Add the target object to the scene #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() #Publish target frame #self.object_frames_pub.publish(target_pose) # pick an object target_pose.pose.position.z += 0.15 robot.right_arm.pick(target_id) self.ac.send_goal(close) self.ac.wait_for_result() #print self.ac.get_result() rospy.sleep(2)