def create_scene(self): scene = PlanningSceneInterface() rospy.sleep(1) # clean the scene scene.remove_world_object('box') scene.remove_world_object('object') #return obj_pose = PoseStamped() #p.header.frame_id = robot.get_planning_frame() obj_pose.header.frame_id = 'world' obj_pose.pose.position = self.obj_pos obj_pose.pose.orientation = self.obj_ort scene.add_mesh( 'grasping_object', obj_pose, '/home/kai/Workspace/urlg_robot_ws/src/urlg_robots_gazebo/worlds/objects/pringle/optimized_poisson_texture_mapped_mesh.dae' ) box_pose = PoseStamped() #p.header.frame_id = robot.get_planning_frame() box_pose.header.frame_id = 'world' box_pose.pose.position = self.box_pos box_pose.pose.orientation = self.box_ort scene.add_box('box', box_pose, (0.5, 0.5, 0.5)) rospy.sleep(1)
def setup_environment(lower_arm, upper_arm, botharms): # add object to Planning Scene rospy.loginfo( "Planning Scene Settings") scene = PlanningSceneInterface() rospy.sleep(2) # Waiting for PlanningSceneInterface scene.remove_world_object() # upper tool box_pose = PoseStamped() box_pose.header.frame_id = upper_arm.get_planning_frame() pos = upper_arm.get_current_pose() rot_o = 90.0/180.0*math.pi rot_a = 0.0/180.0*math.pi rot_t = -90.0/180.0*math.pi q = tf.transformations.quaternion_from_euler(rot_o, rot_a, rot_t, 'szyx') # setting origin of upper tool box_pose.pose.position.x = pos.pose.position.x box_pose.pose.position.y = pos.pose.position.y box_pose.pose.position.z = pos.pose.position.z box_pose.pose.orientation.x = q[0] box_pose.pose.orientation.y = q[1] box_pose.pose.orientation.z = q[2] box_pose.pose.orientation.w = q[3] rospack = rospkg.RosPack() resourcepath = rospack.get_path('khi_robot_sample')+"/config/work/" meshpath = resourcepath + "upper_tool_001.stl" scene.attach_mesh('upper_link_j4', 'upper_tool', box_pose, meshpath, (1,1,1),['upper_link_j3','upper_link_j4']) rospy.sleep(1) rospy.loginfo( "Planning Scene Settings Finish")
class CollisionSceneExample(object): def __init__(self): self._scene = PlanningSceneInterface() # clear the scene self._scene.remove_world_object() self.robot = RobotCommander() # pause to wait for rviz to load print( "============ Waiting while RVIZ displays the scene with obstacles..." ) # TODO: need to replace this sleep by explicitly waiting for the scene to be updated. rospy.sleep(2) def add_one_box(self): box1_pose = [0.25, 0.25, 0.0, 0, 0, 0, 1] box1_dimensions = [0.25, 0.25, 0.75] self.add_box_object("box1", box1_dimensions, box1_pose) print("============ Added one obstacle to RViz!!") def add_four_boxes(self): box1_pose = [0.20, 0.50, 0.25, 0, 0, 0, 1] box1_dimensions = [0.2, 0.2, 0.5] box2_pose = [-0.55, -0.55, 0, 0, 0, 0, 1] box2_dimensions = [0.25, 0.25, 1.75] box3_pose = [0.5, -0.55, 0.14, 0, 0, 0, 1] box3_dimensions = [0.28, 0.28, 0.22] box4_pose = [-0.4, 0.4, 0.5, 0, 0, 0, 1] box4_dimensions = [0.25, 0.25, 1.1] self.add_box_object("box1", box1_dimensions, box1_pose) self.add_box_object("box2", box2_dimensions, box2_pose) self.add_box_object("box3", box3_dimensions, box3_pose) self.add_box_object("box4", box4_dimensions, box4_pose) print("========== Added 4 obstacles to the scene!!") def add_box_object(self, name, dimensions, pose): p = geometry_msgs.msg.PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = pose[0] p.pose.position.y = pose[1] p.pose.position.z = pose[2] p.pose.orientation.x = pose[3] p.pose.orientation.y = pose[4] p.pose.orientation.z = pose[5] p.pose.orientation.w = pose[6] self._scene.add_box(name, p, (dimensions[0], dimensions[1], dimensions[2]))
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_ik_demo') # 初始化需要使用move group控制的机械臂中的arm group rospy.Subscriber("obj_pose", Pose, obj_pose_get) # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 设置场景物体的名称 table_id = 'table' box1_id = 'box1' box2_id = 'box2' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) rospy.sleep(1) # 设置table、box1和box2的三维尺寸 table_size = [0.2, 0.3, 0.01] box1_size = [0.03, 0.001, 0.08] box2_size = [0.03, 0.03, 0.05] table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = table_size[0]/2.0+obj_pose.position.x - 0.05 table_pose.pose.position.y = obj_pose.position.y table_pose.pose.position.z = obj_pose.position.z - table_size[2] / 2.0 - box1_size[2]/2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = obj_pose.position.x box1_pose.pose.position.y = obj_pose.position.y box1_pose.pose.position.z = obj_pose.position.z box1_pose.pose.orientation.w = 1.0 # scene.add_box(box1_id, box1_pose, box1_size) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0, 0, 0, 1) self.setColor(box1_id, 0.8, 0, 0, 1.0) self.setColor(box2_id, 0.8, 0, 0, 1.0) # self.setColor(table_id, 0.8, 0, 0, 1.0) red # self.setColor(box1_id, 0.8, 0.4, 0, 1.0) yellow # self.setColor(box1_id, 1, 1, 1, 1.0) white # self.setColor(sphere_id, 0, 0, 0, 1) black # 将场景中的颜色设置发布 self.sendColors()
def handle_clean_moveit_scene(self, req): scene = PlanningSceneInterface() rospy.sleep(1) # clean the scene #scene.remove_world_object('table_box') scene.remove_world_object('obj_mesh') response = ManageMoveitSceneResponse() response.success = True return response
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_ik_demo') # 初始化需要使用move group控制的机械臂中的arm group rospy.Subscriber("obj_pose", Pose, obj_pose_get) # 设置场景物体的颜色 # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 设置场景物体的名称 table_id = 'table' box1_id = 'box1' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) rospy.sleep(1) reference_frame = 'base_link' # 设置桌面的高度 table_ground = 0.37 # 设置table、box1和box2的三维尺寸 table_size = [0.2, 0.3, 0.01] box1_size = [0.01, 0.01, 0.19] table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = -table_size[0] / 2.0 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = -0.09 box1_pose.pose.position.y = table_size[0] / 2.0 box1_pose.pose.position.z = 0.18 + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size)
class CreateScene1(object): def __init__(self): self._scene = PlanningSceneInterface() # clear the scene self._scene.remove_world_object() self.robot = RobotCommander() # pause to wait for rviz to load rospy.sleep(4) floor_pose = [0, 0, -0.12, 0, 0, 0, 1] floor_dimensions = [4, 4, 0.02] box1_pose = [0.70, 0.70, 0.25, 0, 0, 0, 1] box1_dimensions = [0.5, 0.5, 0.5] box2_pose = [0.5, -0.5, 0.60, 0, 0, 0, 1] box2_dimensions = [0.25, 0.25, 0.25] box3_pose = [0.5, -0.5, 0.24, 0, 0, 0, 1] box3_dimensions = [0.48, 0.48, 0.48] box4_pose = [-0.8, 0.7, 0.5, 0, 0, 0, 1] box4_dimensions = [0.4, 0.4, 0.4] self.add_box_object("floor", floor_dimensions, floor_pose) self.add_box_object("box1", box1_dimensions, box1_pose) self.add_box_object("box2", box2_dimensions, box2_pose) self.add_box_object("box3", box3_dimensions, box3_pose) self.add_box_object("box4", box4_dimensions, box4_pose) 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)
class ObjectServer(object): def __init__(self): self.scene = PlanningSceneInterface() self.robot = RobotCommander() self.p = PoseStamped() self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) rospy.loginfo("Connecting to clear octomap service...") self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty) self.clear_octomap_srv.wait_for_service() rospy.loginfo("Connected!") def wait_for_planning_scene_object(self, object_name='part'): rospy.loginfo("Waiting for object '" + object_name + "'' to appear in planning scene...") gps_req = GetPlanningSceneRequest() gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES part_in_scene = False while not rospy.is_shutdown() and not part_in_scene: # This call takes a while when rgbd sensor is set gps_resp = self.scene_srv.call(gps_req) # check if 'part' is in the answer for collision_obj in gps_resp.scene.world.collision_objects: if collision_obj.id == object_name: part_in_scene = True break else: rospy.sleep(1.0) rospy.loginfo("'" + object_name + "'' is in scene!") def make_object(self,object_name,x,y,z,sx,sy,sz): rospy.loginfo("Removing any previous 'part' object") self.scene.remove_attached_object("arm_tool_link") self.scene.remove_world_object(object_name) rospy.loginfo("Clearing octomap") self.clear_octomap_srv.call(EmptyRequest()) rospy.sleep(2.0) # Removing is fast rospy.loginfo("Adding new 'part' object") rospy.loginfo("Making "+object_name+"...") self.p.header.frame_id = self.robot.get_planning_frame() self.p.pose.position.x = x self.p.pose.position.y = y self.p.pose.position.z = z self.scene.add_box(object_name, self.p, (sx,sy,sz)) self.wait_for_planning_scene_object(object_name)
def init_mp(cls): movegroup_ns = ManipulatorActions.get_ns() robot_description = ManipulatorActions.get_robdesc_ns() sc = PlanningSceneInterface(ns=movegroup_ns) mg = MoveGroupCommander('arm', ns=movegroup_ns, robot_description=robot_description) mg.set_max_acceleration_scaling_factor(0.5) mg.set_max_velocity_scaling_factor(0.6) mg.set_end_effector_link('tool_frame') rospy.sleep(1.0) sc.remove_world_object() ManipulatorActions.rc = RobotCommander(ns=movegroup_ns) ManipulatorActions.mg = mg ManipulatorActions.sc = sc ManipulatorActions.add_table_plane() ManipulatorActions.measure_weight(calibrate=True) ManipulatorActions.move_up(home=True) ManipulatorActions.measure_weight(calibrate=True) return mg, sc
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)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('paintingrobot_planningscene') # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('/renov_up_level/planning_scene', PlanningScene, queue_size=5) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 设置目标位置所使用的参考坐标系 reference_frame = 'map' stl_id = 'stl' scene.remove_world_object(stl_id) stl_size = [1.0, 1.0, 1.0] stl_pose = PoseStamped() stl_pose.header.frame_id = reference_frame stl_pose.pose.position.x = 0.0 stl_pose.pose.position.y = 0.0 stl_pose.pose.position.z = -2.4701 stl_pose.pose.orientation.w = 1.0 scene.add_mesh( stl_id, stl_pose, '/home/zy/catkin_ws/src/paintingrobot/paintingrobot_underusing/painting_robot_demo/matlab/bim_document/second_scan_2.stl' ) self.setColor(stl_id, 0.8, 0.4, 0, 1.0) # 将场景中的颜色设置发布 while not rospy.is_shutdown(): self.sendColors() # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) #rospy.init_node('moveit_demo') # Initialize the ROS node rospy.init_node('moveit_demo', anonymous=True) #cartesian = rospy.get_param('~cartesian', True) print "===== It is OK ====" rospy.sleep(3) # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=1) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the left arm left_arm = MoveGroupCommander('left_arm') left_gripper = MoveGroupCommander('left_gripper') # Get the name of the end-effector link left_eef = left_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) left_arm.set_goal_position_tolerance(0.01) left_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution left_arm.allow_replanning(True) left_reference_frame = left_arm.get_planning_frame() # Set the left arm reference frame left_arm.set_pose_reference_frame('base') # Allow 5 seconds per planning attempt left_arm.set_planning_time(10) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 10 # Set a limit on the number of place attempts max_place_attempts = 10 # Give the scene a chance to catch up rospy.sleep(2) #object1_id = 'object1' table_id = 'table' target_id = 'target' #tool_id = 'tool' #obstacle1_id = 'obstacle1' # Remove leftover objects from a previous run #scene.remove_world_object(object1_id) scene.remove_world_object(table_id) scene.remove_world_object(target_id) #scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object('base', target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file left_arm.set_named_target('left_arm_zero') left_arm.go() rospy.sleep(1) left_gripper.set_joint_value_target(GRIPPER_OPEN) left_gripper.go() rospy.sleep(1) # Set the height of the table off the ground table_ground = 0.0 #object1_size = [0.088, 0.04, 0.02] # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] # Add a table top and two boxes to the scene #obstacle1_size = [0.3, 0.05, 0.45] # Add a table top and two boxes to the scene #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = left_reference_frame #obstacle1_pose.pose.position.x = 0.96 #obstacle1_pose.pose.position.y = 0.24 #obstacle1_pose.pose.position.z = 0.04 #obstacle1_pose.pose.orientation.w = 1.0 #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) #self.setColor(obstacle1_id, 0.8, 0.4, 0, 1.0) #object1_pose = PoseStamped() #object1_pose.header.frame_id = left_reference_frame #object1_pose.pose.position.x = 0.80 #object1_pose.pose.position.y = 0.04 #object1_pose.pose.position.z = table_ground + table_size[2] + object1_size[2] / 2.0 #object1_pose.pose.orientation.w = 1.0 #scene.add_box(object1_id, object1_pose, object1_size) # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = left_reference_frame table_pose.pose.position.x = 1 table_pose.pose.position.y = 0.7 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = left_reference_frame target_pose.pose.position.x = 1 target_pose.pose.position.y = 0.7 target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table red and the boxes orange #self.setColor(object1_id, 0.8, 0, 0, 1.0) self.setColor(table_id, 0.8, 0, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object left_arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = left_reference_frame place_pose.pose.position.x = 0.18 place_pose.pose.position.y = -0.18 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it #grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = left_arm.pick(target_id, grasps) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # Generate valid place poses places = self.make_places(place_pose) # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = left_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") # Return the arm to the "resting" pose stored in the SRDF file left_arm.set_named_target('left_arm_zero') left_arm.go() # Open the gripper to the neutral position left_gripper.set_joint_value_target(GRIPPER_OPEN) left_gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown from geometry_msgs.msg import PoseStamped from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation from trajectory_msgs.msg import JointTrajectoryPoint if __name__ == "__main__": roscpp_initialize(sys.argv) rospy.init_node("moveit_py_demo", anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander("right_arm") rospy.sleep(1) # clean the scene scene.remove_world_object("table") scene.remove_world_object("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
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm right_arm = MoveGroupCommander('arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) right_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the reference frame for pose targets reference_frame = 'base_link' # Set the right arm reference frame accordingly right_arm.set_pose_reference_frame(reference_frame) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right') right_arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.4 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.6 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.60 box1_pose.pose.position.y = -0.2 box1_pose.pose.position.z = table_ground + table_size[ 2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = 0.60 box2_pose.pose.position.y = 0.25 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() rospy.sleep(2) # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.40 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_pose.pose.position.z + table_size[ 2] + 0.1 target_pose.pose.orientation.w = 1.0 # Set the target pose for the arm right_arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) right_arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right') right_arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
#glovebox pose p2.pose.position.x = 0 p2.pose.position.y = 0 p2.pose.position.z = 0.56 b2 = tf.transformations.quaternion_from_euler(1.57,0,0) p2.pose.orientation.x = b2[0] p2.pose.orientation.y = b2[1] p2.pose.orientation.z = b2[2] p2.pose.orientation.w = b2[3] #detach/remove current scene objects print 'removing objects...' robot.detach_object("bowl") rospy.sleep(1) scene.remove_world_object("bowl") scene.remove_world_object("punch") scene.remove_world_object("glovebox") rospy.sleep(2) #reset the gripper and arm position to home robot.set_start_state_to_current_state() robot.set_named_target("start_glove") robot.go() gripper.set_start_state_to_current_state() gripper.go() #add scene objects print 'adding scene objects' scene.add_mesh("bowl", p, "8inhemi.STL")
class scene_generator: def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('scene_generator') # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) ############## CLEAR THE SCENE ################ # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c timerThread = threading.Thread(target=self.scene_generator) timerThread.daemon = True timerThread.start() ################################################################# FUNCTIONS ################################################################################# def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg def scene_generator(self): # print obj_att global target_pose global target_id global target_size next_call = time.time() while True: next_call = next_call+1 target_id = 'target' self.taid = self.pwh.name.index('custom_1') table_id = 'table' self.tid = self.pwh.name.index('table') obstacle1_id = 'obstacle1' self.o1id = self.pwh.name.index('wood_cube_5cm') obstacle2_id = 'obstacle2' self.o2id = self.pwh.name.index('custom_2') # Set the sizes [l, w, h] table_size = [1.5, 0.8, 0.03] target_size = [0.05, 0.05, 0.15] obstacle1_size = [0.05, 0.05, 0.05] obstacle2_size = [0.05, 0.05, 0.10] ## 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] if self.obj_att is None: # Add the target object to the scene self.scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 self.scene.add_box(table_id, table_pose, table_size) obstacle1_pose = PoseStamped() obstacle1_pose.header.frame_id = REFERENCE_FRAME obstacle1_pose.pose = self.pwh.pose[self.o1id] obstacle1_pose.pose.position.z += 0.025 # Add the target object to the scene self.scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) obstacle2_pose = PoseStamped() obstacle2_pose.header.frame_id = REFERENCE_FRAME obstacle2_pose.pose = self.pwh.pose[self.o2id] # Add the target object to the scene self.scene.add_box(obstacle2_id, obstacle2_pose, obstacle2_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() else: self.scene.remove_world_object('target') time.sleep(next_call - time.time()) #threading.Timer(0.5, self.scene_generator).start() # Set the color of an object def setColor(self, name, r, g, b, a = 0.9): # Initialize a MoveIt color object color = ObjectColor() # Set the id to the name given as an argument color.id = name # Set the rgb and alpha values given as input color.color.r = r color.color.g = g color.color.b = b color.color.a = a # Update the global color dictionary self.colors[name] = color # Actually send the colors to MoveIt! def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p)
scene = PlanningSceneInterface() robot = RobotCommander() rospy.sleep(1) p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.orientation.w = 1.0 p.pose.position.x = 0.15 p.pose.position.y = -0.3 p.pose.position.z = 0.55 id_part = 1 while not rospy.is_shutdown(): # clean the scene scene.remove_world_object("part" + str(id_part - 1)) # publish a demo scene new_p = copy.deepcopy(p) new_p.pose.position.x += random.random() * 0.07 * random.choice([1, -1]) new_p.pose.position.y += random.random() * 0.07 * random.choice([1, -1]) new_p.pose.position.z += random.random() * 0.07 * random.choice([1, -1]) scene.add_box("part" + str(id_part), new_p, (0.05, 0.05, 0.05)) print "Picking " + "part" + str(id_part) + " at: " + str(new_p.pose.position) rospy.sleep(0.5) # pick an object robot.right_arm.pick("part" + str(id_part)) id_part += 1 roscpp_shutdown()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_obstacles_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame accordingly arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file arm.set_named_target('l_arm_init') arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.65 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.35 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.3 box1_pose.pose.position.y = 0 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.3 box2_pose.pose.position.y = 0.25 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.22 target_pose.pose.position.y = 0.14 target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 q = quaternion_from_euler(0, 0, -1.57079633) target_pose.pose.orientation.x = q[0] target_pose.pose.orientation.y = q[1] target_pose.pose.orientation.z = q[2] target_pose.pose.orientation.w = q[3] # Set the target pose for the arm arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file arm.set_named_target('l_arm_init') arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
if __name__ == '__main__': roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) rospy.wait_for_service('grasp_hand') scene = PlanningSceneInterface() robot = RobotCommander() pose = PoseStamped() rospy.sleep(1) # clean the scene scene.remove_world_object("grasp_point") scene.remove_world_object("right_shoe") # move_group group_right = MoveGroupCommander("r_arm") group_left = MoveGroupCommander("l_arm") #Get Current state state = robot.get_current_variable_values() # Link off contact links_off=[] links_off.append("r_thumb1") links_off.append("r_thumb2") links_off.append("r_thumb3")
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link eef = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) # right_arm.set_goal_position_tolerance(0.05) # right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) # Prepare Gripper and open it self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1)) self.ac.send_goal(g_open) rospy.sleep(2) # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) 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) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] #obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 scene.add_box(table_id, table_pose, table_size) #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = REFERENCE_FRAME #obstacle1_pose.pose = self.pwh.pose[self.o1id] ## Add the target object to the scene #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.50 place_pose.pose.position.y = -0.30 place_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() #print target_pose self.object_frames_pub.publish(target_pose) rospy.sleep(2) # Initialize the grasp pose to the target pose grasp_pose = target_pose #grasp_pose.header.frame_id = 'gazebo_wolrd' # Shift the grasp pose by half the width of the target to center it # grasp_pose.pose.position.y -= target_size[1] / 2.0 # grasp_pose.pose.position.x = target_pose.pose.position.x / 2.0 # grasp_pose.pose.position.x = target_pose.pose.position.x -0.07 # grasp_pose.pose.position.z += 0.18 #Allowed touch object list # ato = [target_id, 'r_forearm_link'] # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) #### [target_id] # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: # print grasp.grasp_pose self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation success = False n_attempts = 0 #Allowed touch link list atl = ['r_forearm_link'] # Repeat until we succeed or run out of attempts while success == False and n_attempts < max_pick_attempts: success = right_arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(0.2) if success: self.ac.send_goal(g_close) rospy.sleep(3) ## If the pick was successful, attempt the place operation #if success: #success = False #n_attempts = 0 ## Generate valid place poses #places = self.make_places(place_pose) ## Repeat until we succeed or run out of attempts #while success == False and n_attempts < max_place_attempts: #for place in places: #success = right_arm.place(target_id, place) #if success: #break #n_attempts += 1 #rospy.loginfo("Place attempt: " + str(n_attempts)) #rospy.sleep(0.2) #if not success: #rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") #else: #rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") ## Return the arm to the "resting" pose stored in the SRDF file ##right_arm.set_named_target('resting') ##right_arm.go() ## Open the gripper to the neutral position #right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) #right_gripper.go() #rospy.sleep(1) #rospy.spin() # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class MoveItDemo: def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ### Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) self.left_arm = MoveGroupCommander('left_arm') # Initialize the MoveIt! commander for the gripper self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.left_gripper = MoveGroupCommander('left_gripper') # eel = len(self.right_arm.get_end_effector_link()) # print eel # Allow 5 seconds per planning attempt # self.right_arm.set_planning_time(5) # Prepare Action Controller for gripper self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) ### OPEN THE GRIPPER ### self.open_gripper() # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) ### Attach / Remove Object Flag ### self.aro = None # Run and keep in the BG the scene generator with ctrl^c kill ### timerThread = threading.Thread(target=self.scene_generator) timerThread.daemon = True timerThread.start() ## Give some time to ensure the thread starts!! ## rospy.sleep(5) ### GENERATE THE BLACKLIST AND REMOVE ATTACHED OBJECTS FROM PREVIOUS RUNS ### self.idx_list = self.bl() ### GIVE SCENE TIME TO CATCH UP ### rospy.sleep(5) ################################## GRASP EXECUTION ##################################### print "==================== Executing ===========================" start_time = time.time() ### PERSONAL REMINDER!!! WHAT IS WHAT!!! ### # print obj_id[obj_id.index('target')] # print obj_id.index('target') ### MOVE LEFT ARM OUT OF THE WAY ### self.lasp() success = False while success == False and len(self.idx_list)>0: success, pgr_target = self.grasp_attempt() print ("GA Returns:", success) if success is not False: self.flag = 0 # To let the planning scene know when to remove the object self.post_grasp(pgr_target, obj_id.index('target'),'true') self.place_object(obj_id.index('target')) break else: idx = self.idx_list[0] ds, pgr_col_obj = self.declutter_scene(idx) print ("DS Returns:", ds) if ds == True: self.flag = 0 # To let the planning scene know when to remove the object self.post_grasp(pgr_col_obj, obj_id.index(obj_id[idx]),'true') self.place_object(obj_id.index(obj_id[idx])) self.idx_list.pop(0) print "==================== THE END! ======================" print("--- %s seconds ---" % (time.time() - start_time)) rospy.sleep(5) # # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # # Exit the script moveit_commander.os._exit(0) ################################################################# FUNCTIONS ################################################################################# def grasp_attempt(self): # start_time = time.time() retreat = None init_poses = [] grasp_poses = [] for axis in range(0,6): # while obj_id[obj_id.index('target')] is not 'target': # print '!!!!!' # rospy.sleep(0.05) pg = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'pg') gp = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'gp') init_poses.append(pg) grasp_poses.append(gp) pre_grasps = self.grasp_generator(init_poses) grasps = self.grasp_generator(grasp_poses) for grasp in grasps: self.gripper_pose_pub.publish(grasp) rospy.sleep(0.05) success = False i = 1 for pg, gr in izip(pre_grasps, grasps): self.gripper_pose_pub.publish(gr) print ("G Attempt: ", i) plp = self.right_arm.plan(pg.pose) if len(plp.joint_trajectory.points) == 0: print "No valid pregrasp Position, continuing on next one" i+=1 continue i+=1 self.right_arm.plan(pg.pose) self.right_arm.go(wait=True) rospy.sleep(5) plg = self.right_arm.plan(gr.pose) if len(plg.joint_trajectory.points) >= 10: self.right_arm.go() success = True retreat = gr print "Grasping" break # print("--- %s seconds ---" % (time.time() - start_time)) return success , retreat def declutter_scene(self,index): retreat = None init_poses = [] grasp_poses = [] for axis in range(0,6): pg = self.grasp_pose(obj_pose[index], axis, 'pg') gp = self.grasp_pose(obj_pose[index], axis, 'gp') init_poses.append(pg) grasp_poses.append(gp) pre_grasps = self.grasp_generator(init_poses) grasps = self.grasp_generator(grasp_poses) for grasp in grasps: self.gripper_pose_pub.publish(grasp) rospy.sleep(0.05) success = False i= 1 for pg, gr in izip(pre_grasps, grasps): plp = self.right_arm.plan(pg.pose) print (" DS Attempt: ", i) self.gripper_pose_pub.publish(gr) self.right_arm.plan(pg.pose) if len(plp.joint_trajectory.points) == 0: print "No valid pregrasp Position, continuing on next one" i+=1 continue i+=1 self.right_arm.plan(pg.pose) self.right_arm.go() rospy.sleep(5) plg = self.right_arm.plan(gr.pose) if len(plg.joint_trajectory.points) >= 10: self.right_arm.go() print "Grasping" success = True retreat = gr break return success, retreat def place_object(self, obj_idx): self.aro = obj_idx ### GENERATE PLACE POSES ### places = self.place_generator() ### TRY THESE POSES ### i = 1 for place in places: print (" Place Attempt: ", i) plpl = self.right_arm.plan(place.pose) print len(plpl.joint_trajectory.points) if len(plpl.joint_trajectory.points) == 0: i+=1 continue self.right_arm.plan(plpl) self.right_arm.go(wait=True) ### INFORM SCENE ### # self.open_gripper() # self.aro = None ### RETURN HAND TO STARTING POSITION ### self.post_grasp(place,obj_idx, 'false') self.rasp() break def post_grasp(self,new_pose, obj_idx, fl): ######### GRASP OBJECT/ REMOVE FROM SCENE ######. if fl == 'true': self.close_gripper() self.aro = obj_idx else: self.open_gripper() self.aro = None rospy.sleep(2) ### POST GRASP RETREAT ### M1 = transformations.quaternion_matrix([new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z, new_pose.pose.orientation.w]) M1[0,3] = new_pose.pose.position.x M1[1,3] = new_pose.pose.position.y M1[2,3] = new_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.25 # about z T1 = np.dot(M2, M1) npo = deepcopy(new_pose) npo.pose.position.x = T1[0,3] npo.pose.position.y = T1[1,3] npo.pose.position.z = T1[2,3] quat = transformations.quaternion_from_matrix(T1) npo.pose.orientation.x = quat[0] npo.pose.orientation.y = quat[1] npo.pose.orientation.z = quat[2] npo.pose.orientation.w = quat[3] npo.header.frame_id = REFERENCE_FRAME self.right_arm.plan(npo.pose) self.right_arm.go(wait=True) def place_generator(self): place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.57 place_pose.pose.position.y = 0.16 place_pose.pose.position.z = 0.56 place_pose.pose.orientation.w = 1.0 P = transformations.quaternion_matrix([place_pose.pose.orientation.x, place_pose.pose.orientation.y, place_pose.pose.orientation.z, place_pose.pose.orientation.w]) P[0,3] = place_pose.pose.position.x P[1,3] = place_pose.pose.position.y P[2,3] = place_pose.pose.position.z places =[] yaw_angles = [0, 1,57, -1,57 , 3,14] x_vals = [0, 0.05 ,0.1 , 0.15] z_vals = [0.05 ,0.1 , 0.15] for y in yaw_angles: G = transformations.euler_matrix(0, 0, y) G[0,3] = 0.0 # offset about x G[1,3] = 0.0 # about y G[2,3] = 0.0 # about z for z in z_vals: for x in x_vals: TM = np.dot(P, G) pl = deepcopy(place_pose) pl.pose.position.x = TM[0,3] +x pl.pose.position.y = TM[1,3] pl.pose.position.z = TM[2,3] +z quat = transformations.quaternion_from_matrix(TM) pl.pose.orientation.x = quat[0] pl.pose.orientation.y = quat[1] pl.pose.orientation.z = quat[2] pl.pose.orientation.w = quat[3] pl.header.frame_id = REFERENCE_FRAME places.append(deepcopy(pl)) return places def grasp_pose(self, target_pose, axis, stage): ############ TODO : GENERATE AUTOMATED PRE-GRASPING POSITIONS BASED ON THE PRIMITIVE ######### if axis == 0: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 0) if stage == 'pg': M2[0,3] = -0.25 # offset about x elif stage == 'gp': M2[0,3] = -0.18 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.0 # about z elif axis == 1: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 1.57) M2[0,3] = 0.0 # offset about x if stage == 'pg': M2[1,3] = -0.25 # about y elif stage == 'gp': M2[1,3] = -0.18 # about y M2[2,3] = 0.0 # about z elif axis == 2: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, -1.57) M2[0,3] = 0.0 # offset about x if stage == 'pg': M2[1,3] = 0.25 # about y elif stage == 'gp': M2[1,3] = 0.18 # about y M2[2,3] = 0.0 # about z elif axis == 3: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 3.14) if stage == 'pg': M2[0,3] = 0.25 # offset about x elif stage == 'gp': M2[0,3] = 0.18 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.0 # about z elif axis == 4: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y if stage == 'pg': M2[2,3] = 0.30 # about z elif stage == 'gp': M2[2,3] = 0.23 # about z else: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(1.57, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y if stage == 'pg': M2[2,3] = 0.30 # about z elif stage == 'gp': M2[2,3] = 0.23 # about z T1 = np.dot(M1, M2) grasp_pose = deepcopy(target_pose) grasp_pose.pose.position.x = T1[0,3] grasp_pose.pose.position.y = T1[1,3] grasp_pose.pose.position.z = T1[2,3] quat = transformations.quaternion_from_matrix(T1) grasp_pose.pose.orientation.x = quat[0] grasp_pose.pose.orientation.y = quat[1] grasp_pose.pose.orientation.z = quat[2] grasp_pose.pose.orientation.w = quat[3] grasp_pose.header.frame_id = REFERENCE_FRAME return grasp_pose def grasp_generator(self, initial_poses): # A list to hold the grasps grasps = [] o = [] # Original Pose of the object (o) O=[] i= 0 while i < len(initial_poses): o.append(initial_poses[i]) i+=1 G = transformations.euler_matrix(0, 0, 0) # Generate a grasps for along z axis (x and y directions) k = 0 while k <= 5: O.append(transformations.quaternion_matrix([o[k].pose.orientation.x, o[k].pose.orientation.y, o[k].pose.orientation.z, o[k].pose.orientation.w])) O[k][0,3] = o[k].pose.position.x O[k][1,3] = o[k].pose.position.y O[k][2,3] = o[k].pose.position.z if k in range(0,4): for z in self.drange(0.005-obj_size[obj_id.index('target')][2]/2,-0.005 + obj_size[obj_id.index('target')][2]/2, 0.02): ### TODO: USE EACH OBJECTS SIZE NOT ONLY THE TARGETS ### # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] grasp.pose.position.y = T[1,3] grasp.pose.position.z = T[2,3] +z quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) elif k == 4: for x in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02): # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] +x grasp.pose.position.y = T[1,3] grasp.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) else: for y in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02): # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] grasp.pose.position.y = T[1,3] +y grasp.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) k+=1 print len(grasps) # Return the list return grasps def scene_generator(self): while True: # print "happening" obj_pose =[] obj_id = [] obj_size = [] bl = ['ground_plane','pr2'] global obj_pose, obj_id , obj_size ops = PoseStamped() ops.header.frame_id = REFERENCE_FRAME for model_name in self.pwh.name: if model_name not in bl: obj_id.append(model_name) ops.pose = self.pwh.pose[self.pwh.name.index(model_name)] obj_pose.append(deepcopy(ops)) obj_size.append([0.05, 0.05, 0.15]) # obj_id[obj_id.index('custom_1')] = 'target' obj_size[obj_id.index('custom_2')] = [0.05, 0.05, 0.10] obj_size[obj_id.index('custom_3')] = [0.05, 0.05, 0.05] obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03] if self.aro is None: for i in range(0, len(obj_id)): ### CREATE THE SCENE ### self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i]) self.setColor(obj_id[i], 1, 0.623, 0, 1.0) ### Make the target purple and table green ### self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0) self.setColor(obj_id[obj_id.index('custom_table')], 0.3, 1, 0.3, 1.0) self.scene.remove_attached_object(GRIPPER_FRAME) # Send the colors to the planning scene self.sendColors() else: if self.flag == 0: touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link', 'r_wrist_roll_link', 'r_upper_arm_link'] #print touch_links self.scene.attach_box(GRIPPER_FRAME, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links) ### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ### self.scene.remove_world_object(obj_id[self.aro]) self.flag +=1 time.sleep(0.5) def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg def bl(self): blist = ['target','custom_2','custom_3', 'custom_table'] self.blist = [] for name in obj_id: if name not in blist: self.blist.append(obj_id.index(name)) # Remove any attached objects from a previous session self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index(name)]) self.scene.remove_attached_object(GRIPPER_FRAME, 'target') return self.blist def drange(self, start, stop, step): r = start while r < stop: yield r r += step def close_gripper(self): g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.044, 100)) self.ac.send_goal(g_close) self.ac.wait_for_result() rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object def open_gripper(self): g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0899, 100)) self.ac.send_goal(g_open) self.ac.wait_for_result() rospy.sleep(5) # And up to 20 to detach it # Set the color of an object def setColor(self, name, r, g, b, a = 0.9): # Initialize a MoveIt color object color = ObjectColor() # Set the id to the name given as an argument color.id = name # Set the rgb and alpha values given as input color.color.r = r color.color.g = g color.color.b = b color.color.a = a # Update the global color dictionary self.colors[name] = color # Actually send the colors to MoveIt! def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p) def lasp(self): sp = PoseStamped() sp.header.frame_id = REFERENCE_FRAME sp.pose.position.x = 0.3665 sp.pose.position.y = 0.74094 sp.pose.position.z = 1.1449 sp.pose.orientation.x = 0.80503 sp.pose.orientation.y = -0.18319 sp.pose.orientation.z = 0.31988 sp.pose.orientation.w = 0.46481 self.left_arm.plan(sp) self.left_arm.go(wait=True) def rasp(self): sp = PoseStamped() sp.header.frame_id = REFERENCE_FRAME sp.pose.position.x = 0.39571 sp.pose.position.y = -0.40201 sp.pose.position.z = 1.1128 sp.pose.orientation.x =0.00044829 sp.pose.orientation.y = 0.57956 sp.pose.orientation.z = 9.4878e-05 sp.pose.orientation.w = 0.81493 self.right_arm.plan(sp) self.right_arm.go(wait=True)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('MotionSequence') # 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) # Create a publisher for displaying left wam finger poses self.left_wam_finger_1_pub = rospy.Publisher('left_wam_finger_1', PoseStamped) self.left_wam_finger_2_pub = rospy.Publisher('left_wam_finger_2', PoseStamped) self.left_wam_finger_3_pub = rospy.Publisher('left_wam_finger_3', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Define move group comander for each moveit group left_wam = moveit_commander.MoveGroupCommander('left_wam') right_wam = moveit_commander.MoveGroupCommander('right_wam') left_wam_finger_1 = moveit_commander.MoveGroupCommander('left_wam_finger_1') left_wam_finger_2 = moveit_commander.MoveGroupCommander('left_wam_finger_2') left_wam_finger_3 = moveit_commander.MoveGroupCommander('left_wam_finger_3') right_wam_finger_1 = moveit_commander.MoveGroupCommander('right_wam_finger_1') right_wam_finger_2 = moveit_commander.MoveGroupCommander('right_wam_finger_2') right_wam_finger_3 = moveit_commander.MoveGroupCommander('right_wam_finger_3') left_wam.set_planner_id("PRMstarkConfigDefault") right_wam.set_planner_id("PRMstarkConfigDefault") #left_wam_finger_1.set_planner_id("RRTstarkConfigDefault") #left_wam_finger_2.set_planner_id("RRTstarkConfigDefault") #left_wam_finger_3.set_planner_id("RRTstarkConfigDefault") #right_wam_finger_1.set_planner_id("RRTstarkConfigDefault") #right_wam_finger_2.set_planner_id("RRTstarkConfigDefault") #right_wam_finger_3.set_planner_id("RRTstarkConfigDefault") # Get the name of the end-effector link left_end_effector_link = left_wam.get_end_effector_link() right_end_effector_link = right_wam.get_end_effector_link() # Display the name of the end_effector link rospy.loginfo("The end effector link of left wam is: " + str(left_end_effector_link)) rospy.loginfo("The end effector link of right wam is: " + str(right_end_effector_link)) # Allow some leeway in position (meters) and orientation (radians) right_wam.set_goal_position_tolerance(0.01) right_wam.set_goal_orientation_tolerance(0.05) left_wam.set_goal_position_tolerance(0.01) left_wam.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_wam.allow_replanning(True) left_wam.allow_replanning(True) # Allow 5 seconds per planning attempt right_wam.set_planning_time(15) left_wam.set_planning_time(25) # Allow replanning to increase the odds of a solution right_wam.allow_replanning(True) left_wam.allow_replanning(True) # Set the reference frame for wam arms left_wam.set_pose_reference_frame(REFERENCE_FRAME) right_wam.set_pose_reference_frame(REFERENCE_FRAME) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' bowl_id = 'bowl' pitcher_id = 'pitcher' spoon_id = 'spoon' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(bowl_id) scene.remove_world_object(pitcher_id) scene.remove_world_object(spoon_id) # Remove leftover objects from a previous run scene.remove_attached_object(right_end_effector_link, 'spoon') #right_wam.set_named_target('right_wam_start') #right_wam.go() #rospy.sleep(2) # Closing the hand first # Closing the hand #right_wam_finger_1.set_named_target("right_wam_finger_1_grasp") #right_wam_finger_2.set_named_target("right_wam_finger_2_grasp") #right_wam_finger_3.set_named_target("right_wam_finger_3_grasp") #right_wam_finger_1.execute(right_wam_finger_1.plan()) #rospy.sleep(5) #right_wam_finger_2.execute(right_wam_finger_2.plan()) #rospy.sleep(5) #right_wam_finger_3.execute(right_wam_finger_3.plan()) #rospy.sleep(5) # Create a pose for the tool relative to the end-effector p = PoseStamped() p.header.frame_id = right_end_effector_link # Place the end of the object within the grasp of the gripper p.pose.position.x = 0.0 p.pose.position.y = 0.0 p.pose.position.z = -0.02 # Align the object with the gripper (straight out) p.pose.orientation.x = -0.5 p.pose.orientation.y = 0.5 p.pose.orientation.z = -0.5 p.pose.orientation.w = 0.5 # Attach the tool to the end-effector # Set the height of the table off the ground table_ground = 0.5762625 # Set the length, width and height of the table and boxes table_size = [0.90128, 0.381, 0.0238125] table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0 table_pose.pose.position.y = 0.847725 table_pose.pose.position.z = table_ground scene.add_box(table_id, table_pose, table_size) # Set the height of the bowl bowl_ground = 0.57816875 bowl_pose = PoseStamped() bowl_pose.header.frame_id = REFERENCE_FRAME bowl_pose.pose.position.x = 0.015 bowl_pose.pose.position.y = 0.847725 bowl_pose.pose.position.z = bowl_ground scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl') # Set the height of the pitcher #pitcher_ground = 0.57816875 #pitcher_pose = PoseStamped() #pitcher_pose.header.frame_id = REFERENCE_FRAME #pitcher_pose.pose.position.x = 0.25 #pitcher_pose.pose.position.y = 0.847725 #pitcher_pose.pose.position.z = pitcher_ground #pitcher_pose.pose.orientation.w = -0.5 #pitcher_pose.pose.orientation.z = 0.707 #scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl') # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0.4, 0, 1.0) self.setColor(bowl_id, 0, 0.4, 0.8, 1.0) #self.setColor(pitcher_id, 0.9, 0.9, 0, 1.0) self.sendColors() rospy.sleep(2) start = input("Start left_wam planning ? ") # Set the support surface name to the table object #left_wam.set_support_surface_name(table_id) #right_wam.set_support_surface_name(table_id) # Set the target pose. target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = 0.40363476287 target_pose.pose.position.y = 0.847725 target_pose.pose.position.z = 0.721472317843 target_pose.pose.orientation.x = 0.707 target_pose.pose.orientation.y = 0 target_pose.pose.orientation.z = -0.707 target_pose.pose.orientation.w = 0 # Set the start state to the current state left_wam.set_start_state_to_current_state() # Set the goal pose of the end effector to the stored pose left_wam.set_pose_target(target_pose, left_end_effector_link) left_wam.execute(left_wam.plan()) left_wam.shift_pose_target(5, 0, left_end_effector_link) start = input("Left wam starting position ") # Pause for a second # Closing the hand left_wam_finger_1.set_named_target("left_wam_finger_1_grasp") left_wam_finger_2.set_named_target("left_wam_finger_2_grasp") left_wam_finger_3.set_named_target("left_wam_finger_3_grasp") left_wam_finger_1.execute(left_wam_finger_1.plan()) #rospy.sleep(3) left_wam_finger_2.execute(left_wam_finger_2.plan()) #rospy.sleep(3) left_wam_finger_3.execute(left_wam_finger_3.plan()) #rospy.sleep(3) start = input("Left wam hand closing ") end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link).pose) intermidiate_pose = deepcopy(end_pose) intermidiate_pose.position.z = intermidiate_pose.position.z + 0.05 plan = self.StraightPath(end_pose, intermidiate_pose, left_wam) left_wam.set_start_state_to_current_state() left_wam.execute(plan) start = input("Hold up the Pitcher ") end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link).pose) intermidiate_pose = deepcopy(end_pose) intermidiate_pose.position.x = intermidiate_pose.position.x - 0.1 plan = self.StraightPath(end_pose, intermidiate_pose, left_wam) left_wam.set_start_state_to_current_state() left_wam.execute(plan) start = input("left_wam into pouring position ") end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link)) back_pose = deepcopy(end_pose) end_pose.pose.orientation.x = 0.97773401145 end_pose.pose.orientation.y = 0 end_pose.pose.orientation.z = -0.209726592658 end_pose.pose.orientation.w = 0 # Set the start state to the current state left_wam.set_start_state_to_current_state() # Set the goal pose of the end effector to the stored pose left_wam.set_pose_target(end_pose, left_end_effector_link) left_wam.execute(left_wam.plan()) start = input("Pour the water ") # Set the start state to the current state left_wam.set_start_state_to_current_state() # Set the goal pose of the end effector to the stored pose left_wam.set_pose_target(back_pose, left_end_effector_link) left_wam.execute(left_wam.plan()) end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link)) end_pose.pose.position.x = end_pose.pose.position.x + 0.1 # Set the start state to the current state left_wam.set_start_state_to_current_state() # Set the goal pose of the end effector to the stored pose left_wam.set_pose_target(end_pose, left_end_effector_link) left_wam.execute(left_wam.plan()) start = input("Left_wam back to prep position") target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = -0.600350195463908 target_pose.pose.position.y = 0.80576308041 target_pose.pose.position.z = 0.794775212132 target_pose.pose.orientation.x = 3.01203251908e-05 target_pose.pose.orientation.y = 0.705562870053 target_pose.pose.orientation.z = 4.55236739937e-05 target_pose.pose.orientation.w = 0.708647326547 start = input("Start right_wam planning ? ") right_wam.set_start_state_to_current_state() right_wam.set_pose_target(target_pose, right_end_effector_link) right_wam.execute(right_wam.plan()) start = input("right_wam into position. ") start_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) intermidiate_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) intermidiate_pose.position.x = intermidiate_pose.position.x + 0.6 plan = self.StraightPath(start_pose, intermidiate_pose, right_wam) right_wam.set_start_state_to_current_state() right_wam.execute(plan) rospy.sleep(3) # Closing the hand right_wam_finger_1.set_named_target("right_wam_finger_1_grasp") right_wam_finger_2.set_named_target("right_wam_finger_2_grasp") right_wam_finger_3.set_named_target("right_wam_finger_3_grasp") right_wam_finger_1.execute(right_wam_finger_1.plan()) #rospy.sleep(3) right_wam_finger_2.execute(right_wam_finger_2.plan()) #rospy.sleep(3) right_wam_finger_3.execute(right_wam_finger_3.plan()) rospy.sleep(1) scene.attach_mesh(right_end_effector_link, 'spoon', p, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/spoon.stl') #create a circle path circles = input("How many circles you want the wam to mix ? ") start_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) plan = self.CircularPath(start_pose, circles, right_wam) #execute the circle path right_wam.set_start_state_to_current_state() right_wam.execute(plan) pause = input("Mix the oatmeal ") #put the right_wam back to preparation pose end_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) intermidiate_pose1 = deepcopy(end_pose) intermidiate_pose1.position.z = intermidiate_pose1.position.z + 0.1 plan = self.StraightPath(end_pose, intermidiate_pose1, right_wam) right_wam.set_start_state_to_current_state() right_wam.execute(plan) pause = input("wait for the execution of straight path ") end_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) intermidiate_pose2 = deepcopy(end_pose) intermidiate_pose2.position.x = intermidiate_pose2.position.x - 0.25 plan = self.StraightPath(end_pose, intermidiate_pose2, right_wam) right_wam.set_start_state_to_current_state() right_wam.execute(plan) pause = input("right_wam back into prep position ") #left_wam.shift_pose_target(5, 0, left_end_effector_link) #left_wam.go() #rospy.sleep(2) #left_wam.shift_pose_target(0, -0.05, left_end_effector_link) #left_wam.go() #rospy.sleep(2) # Initialize the grasp pose to the target pose #grasp_pose = target_pose # Generate a list of grasps #grasps = self.make_grasps(grasp_pose, [pitcher_id]) # Publish the grasp poses so they can be viewed in RViz #for grasp in grasps: # self.left_wam_finger_1_pub.publish(grasp.grasp_pose) # rospy.sleep(0.2) # self.left_wam_finger_2_pub.publish(grasp.grasp_pose) # rospy.sleep(0.2) # self.left_wam_finger_3_pub.publish(grasp.grasp_pose) # rospy.sleep(0.2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
grasp.post_grasp_retreat.direction.header.frame_id = frame grasp.post_place_retreat.direction.header.frame_id = frame return grasp if __name__ == "__main__": roscpp_initialize(sys.argv) rospy.init_node('moveit_regrasping_app', anonymous=True) rospy.loginfo("Starting grasp app") # Access the planning scene scene = PlanningSceneInterface() rospy.sleep(1) # clean the scene scene.remove_world_object("ground") scene.remove_world_object("cup") scene.remove_world_object("pen") rospy.sleep(1) # create a pose p = PoseStamped() p.header.frame_id = "world" #robot.get_planning_frame() p.pose.position.x = 0 p.pose.position.y = 0 p.pose.position.z = -0.05 p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1
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()
if __name__ == '__main__': roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) print "Esperando al servicio ... grasp_service ... from hermes_grasp_service" rospy.wait_for_service('grasp_service') scene = PlanningSceneInterface() robot = RobotCommander() pose = PoseStamped() rospy.sleep(1) # clean the scene scene.remove_world_object("right_shoe") # move_group group_right = MoveGroupCommander("r_arm") group_left = MoveGroupCommander("l_arm") #Get Current state state = robot.get_current_variable_values() # Link off contact links_off=[] links_off.append("r_thumb1") links_off.append("r_thumb2") links_off.append("r_thumb3") links_off.append("r_index1")
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 MoveItDemo: def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ### Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Allow 5 seconds per planning attempt self.right_arm.set_planning_time(5) # Prepare Action Controller for gripper self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) ### OPEN THE GRIPPER ### self.open_gripper() self.obj_att = None # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) ############## CLEAR THE SCENE ################ # planning_scene.world.collision_objects.remove('target') # Remove leftover objects from a previous run self.scene.remove_world_object('target') self.scene.remove_world_object('table') # self.scene.remove_world_object(obstacle1_id) # Remove any attached objects from a previous session self.scene.remove_attached_object(GRIPPER_FRAME, 'target') # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c timerThread = threading.Thread(target=self.scene_generator) timerThread.daemon = True timerThread.start() initial_pose = target_pose initial_pose.header.frame_id = 'gazebo_world' print "==================== Generating Transformations ===========================" #################### PRE GRASPING POSE ######################### # M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) # M1[0,3] = target_pose.pose.position.x # M1[1,3] = target_pose.pose.position.y # M1[2,3] = target_pose.pose.position.z # M2 = transformations.euler_matrix(0, 1.57, 0) # M2[0,3] = 0.0 # offset about x # M2[1,3] = 0.0 # about y # M2[2,3] = 0.25 # about z # T = np.dot(M1, M2) # pre_grasping = deepcopy(target_pose) # pre_grasping.pose.position.x = T[0,3] # pre_grasping.pose.position.y = T[1,3] # pre_grasping.pose.position.z = T[2,3] # quat = transformations.quaternion_from_matrix(T) # pre_grasping.pose.orientation.x = quat[0] # pre_grasping.pose.orientation.y = quat[1] # pre_grasping.pose.orientation.z = quat[2] # pre_grasping.pose.orientation.w = quat[3] # pre_grasping.header.frame_id = 'gazebo_world' # self.plan_exec(pre_grasping) ################# GENERATE GRASPS ################### # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Track success/failure and number of attempts for pick operation success = False n_attempts = 0 grasps = self.grasp_generator(initial_pose) possible_grasps = [] for grasp in grasps: self.gripper_pose_pub.publish(grasp) possible_grasps.append(grasp.pose) rospy.sleep(0.2) #print possible_grasps self.right_arm.pick(target_id, grasps) # target_name = target_id # group_name = GROUP_NAME_ARM # end_effector = GROUP_NAME_GRIPPER # attached_object_touch_links = ['r_forearm_link'] # #print (target_name, group_name, end_effector) # PickupGoal(target_name, group_name ,end_effector, possible_grasps ) # # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # # Exit the script moveit_commander.os._exit(0) ################################################################# FUNCTIONS ################################################################################# def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg def grasp_generator(self, initial_pose): # Pitch angles to try pitch_vals = [1, 1.57,0, 1,2 ] # Yaw angles to try yaw_vals = [0]#, 1.57, -1.57] # A list to hold the grasps grasps = [] g = PoseStamped() g.header.frame_id = REFERENCE_FRAME g.pose = initial_pose.pose #g.pose.position.z += 0.18 # Generate a grasp for each pitch and yaw angle for y in yaw_vals: for p in pitch_vals: # Create a quaternion from the Euler angles q = transformations.quaternion_from_euler(0, p, y) # Set the grasp pose orientation accordingly g.pose.orientation.x = q[0] g.pose.orientation.y = q[1] g.pose.orientation.z = q[2] g.pose.orientation.w = q[3] # Append the grasp to the list grasps.append(deepcopy(g)) # Return the list return grasps def plan_exec(self, pose): self.right_arm.clear_pose_targets() self.right_arm.set_pose_target(pose, GRIPPER_FRAME) self.right_arm.plan() rospy.sleep(5) self.right_arm.go(wait=True) def close_gripper(self): g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100)) self.ac.send_goal(g_close) self.ac.wait_for_result() rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object def open_gripper(self): g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) self.ac.send_goal(g_open) self.ac.wait_for_result() rospy.sleep(5) # And up to 20 to detach it def scene_generator(self): # print obj_att global target_pose global target_id next_call = time.time() while True: next_call = next_call+1 target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') # obstacle1_id = 'obstacle1' # self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] # obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 if self.obj_att is None: # Add the target object to the scene self.scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 self.scene.add_box(table_id, table_pose, table_size) # obstacle1_pose = PoseStamped() # obstacle1_pose.header.frame_id = REFERENCE_FRAME # obstacle1_pose.pose = self.pwh.pose[self.o1id] # # Add the target object to the scene # scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() else: self.scene.remove_world_object('target') time.sleep(next_call - time.time()) #threading.Timer(0.5, self.scene_generator).start() # Set the color of an object def setColor(self, name, r, g, b, a = 0.9): # Initialize a MoveIt color object color = ObjectColor() # Set the id to the name given as an argument color.id = name # Set the rgb and alpha values given as input color.color.r = r color.color.g = g color.color.b = b color.color.a = a # Update the global color dictionary self.colors[name] = color # Actually send the colors to MoveIt! def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p)
class BTMotion: def __init__(self, name): # create messages that are used to publish feedback/result self._feedback = amazon_challenge_bt_actions.msg.BTFeedback() self._result = amazon_challenge_bt_actions.msg.BTResult() self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, amazon_challenge_bt_actions.msg.BTAction, execute_cb=self.execute_cb, auto_start = False) self.pub_posed = rospy.Publisher('arm_posed', String, queue_size=10) self.pub_rate = rospy.Rate(30) self._planning_scene = PlanningSceneInterface() # get ROS parameters rospy.loginfo('Getting parameters...') while not rospy.is_shutdown(): try: self._base_move_params = rospy.get_param('/base_move') self._timeout = rospy.get_param(name + '/timeout') self._sim = rospy.get_param(name + '/sim') self._base_pos_dict = rospy.get_param('/base_pos_dict') self._left_arm_joint_pos_dict = rospy.get_param('/left_arm_joint_pos_dict') self._right_arm_joint_pos_dict = rospy.get_param('/right_arm_joint_pos_dict') break except: rospy.sleep(random.uniform(0,2)) continue self._exit = False while not rospy.is_shutdown(): try: self._robot = moveit_commander.RobotCommander() self._left_arm = self._robot.get_group('left_arm') self._right_arm = self._robot.get_group('right_arm') self._arms = self._robot.get_group('arms') self._torso = self._robot.get_group('torso') self._head = self._robot.get_group('head') self._arms_dict = {'left_arm': self._left_arm, 'right_arm': self._right_arm} break except: rospy.sleep(random.uniform(0,2)) continue self._tf_listener = tf.TransformListener() self._next_task_sub = rospy.Subscriber("/amazon_next_task", String, self.get_task) self._shelf_pose_sub = rospy.Subscriber("/pubShelfSep", PoseStamped, self.get_shelf_pose) self._got_shelf_pose = False self._l_gripper_pub = rospy.Publisher('/l_gripper_controller/command', Pr2GripperCommand) while not rospy.is_shutdown(): try: self._tool_size = rospy.get_param('/tool_size', [0.16, 0.02, 0.04]) self._contest = rospy.get_param('/contest', True) break except: rospy.sleep(random.uniform(0,1)) continue if self._contest: self._length_tool = 0.18 + self._tool_size[0] else: self._length_tool = 0.216 + self._tool_size[0] self._as.start() rospy.loginfo('['+rospy.get_name()+']: ready!') def get_shelf_pose(self, msg): self._shelf_pose = msg self._got_shelf_pose = True def get_bm_srv(self): while not rospy.is_shutdown(): try: rospy.wait_for_service('/base_move_server/move', 5.0) rospy.wait_for_service('/base_move_server/preempt', 5.0) break except: rospy.loginfo('[' + rospy.get_name() + ']: waiting for base move server') continue self._bm_move_srv = rospy.ServiceProxy('/base_move_server/move', BaseMove) self._bm_preempt_srv = rospy.ServiceProxy('/base_move_server/preempt', Empty) def timer_callback(self, event): self._timer_started = True rospy.logerr('[' + rospy.get_name() + ']: TIMED OUT!') self._planning_scene.remove_attached_object('l_wrist_roll_link', 'grasped_object') self._planning_scene.remove_world_object('grasped_object') # pull the base back 60 cm self._left_arm.stop() self._right_arm.stop() r = rospy.Rate(1.0) while not self._got_shelf_pose: rospy.loginfo('[' + rospy.get_name() + ']: waiting for shelf pose') r.sleep() base_pos_goal = [-1.42, -self._shelf_pose.pose.position.y, 0.0, 0.0, 0.0, 0.0] self.get_bm_srv() self._bm_preempt_srv.call(EmptyRequest()) while not self.go_base_pos_async(base_pos_goal): rospy.sleep(1.0) left_arm_joint_pos_goal = copy.deepcopy(self._left_arm_joint_pos_dict['start']) right_arm_joint_pos_goal = copy.deepcopy(self._right_arm_joint_pos_dict['start']) joint_pos_goal = left_arm_joint_pos_goal + right_arm_joint_pos_goal self._arms.set_joint_value_target(joint_pos_goal) self._arms.go() self._exit = True def execute_exit(self): if self._exit: self._success = False self.set_status('FAILURE') self._timer.shutdown() return True return False def execute_cb(self, goal): print 'bt motion execute callback' def shutdown_simtrack(self): # get simtrack switch objects service while not rospy.is_shutdown(): try: rospy.wait_for_service('/simtrack/switch_objects', 10.0) break except: rospy.loginfo('[' + rospy.get_name() + ']: waiting for simtrack switch object service') continue simtrack_switch_objects_srv = rospy.ServiceProxy('/simtrack/switch_objects', SwitchObjects) simtrack_switch_objects_srv.call() def init_as(self): self._planning_scene.remove_attached_object('l_wrist_roll_link', 'grasped_object') self._planning_scene.remove_world_object('grasped_object') self._timer_started = False self._exit=False self._timer = rospy.Timer(rospy.Duration(self._timeout), self.timer_callback, oneshot=True) self.shutdown_simtrack() rospy.sleep(2.0) def get_task(self, msg): text = msg.data text = text.replace('[','') text = text.replace(']','') words = text.split(',') self._bin = words[0] self._item = words[1] def get_row(self): ''' For setting the torso height and arm pose ''' while not rospy.is_shutdown(): try: if self._bin=='bin_A' or self._bin=='bin_B' or self._bin=='bin_C': return 'row_1' elif self._bin=='bin_D' or self._bin=='bin_E' or self._bin=='bin_F': return 'row_2' elif self._bin=='bin_G' or self._bin=='bin_H' or self._bin=='bin_I': return 'row_3' elif self._bin=='bin_J' or self._bin=='bin_K' or self._bin=='bin_L': return 'row_4' except: pass def get_column(self): ''' For setting the base pose ''' while not rospy.is_shutdown(): try: if self._bin=='bin_A' or self._bin=='bin_D' or self._bin=='bin_G' or self._bin=='bin_J': return 'column_1' elif self._bin=='bin_B' or self._bin=='bin_E' or self._bin=='bin_H' or self._bin=='bin_K': return 'column_2' elif self._bin=='bin_C' or self._bin=='bin_F' or self._bin=='bin_I' or self._bin=='bin_L': return 'column_3' except: pass def go_joint_goal_async(self, group, joint_pos_goal, normalize_angles=False): q_goal = np.array(joint_pos_goal) if normalize_angles: q_goal = self.normalize_angles(joint_pos_goal) group.set_joint_value_target(joint_pos_goal) if not group.go(wait=False): return False q_now = np.array(group.get_current_joint_values()) if normalize_angles: q_now = self.normalize_angles(q_now) q_tol = group.get_goal_joint_tolerance() if group.get_name()=='left_arm' or group.get_name()=='right_arm' or group.get_name()=='arms' or group.get_name()=='head': q_tol = 0.04 elif group.get_name()=='torso': q_tol = 0.003 t_print = rospy.Time.now() r = rospy.Rate(1.0) # check for preemption while the arm hasn't reach goal configuration while np.max(np.abs(q_goal-q_now)) > q_tol and not rospy.is_shutdown(): if self.execute_exit(): return False q_now = np.array(group.get_current_joint_values()) if normalize_angles: q_now = self.normalize_angles(q_now) # check that preempt has not been requested by the client if self._as.is_preempt_requested(): #HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION group.stop() rospy.loginfo('action halted') self._as.set_preempted() self._exit = True if self.execute_exit(): return False if (rospy.Time.now()-t_print).to_sec()>3.0: t_print = rospy.Time.now() rospy.loginfo('[' + rospy.get_name() + ']: executing action') #HERE THE CODE TO EXECUTE AS LONG AS THE BEHAVIOR TREE DOES NOT HALT THE ACTION r.sleep() if rospy.is_shutdown(): return False return True def normalize_angles(self, q): ''' normalize angles to -pi, pi ''' q_normalized = np.mod(q, 2*np.pi) for i in xrange(np.size(q)): if q_normalized[i] > np.pi: q_normalized[i] = -(2*np.pi - q_normalized[i]) return q_normalized def go_base_pos_async(self, base_pos_goal): angle = base_pos_goal[5] pos = base_pos_goal[0:2] r = rospy.Rate(20.0) req = BaseMoveRequest() req.x = pos[0] req.y = pos[1] req.theta = angle self.get_bm_srv() res = self._bm_move_srv.call(req) if self.execute_exit(): return False # check that preempt has not been requested by the client if self._as.is_preempt_requested(): #HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION rospy.loginfo('action halted while moving base') self._as.set_preempted() self._exit = True if self.execute_exit(): return False return res.result def go_base_moveit_group_pos_async(self, base_pos_goal, group, joint_pos_goal, normalize_angles=False): angle = base_pos_goal[5] pos = base_pos_goal[0:2] r = rospy.Rate(20.0) q_goal = np.array(joint_pos_goal) if normalize_angles: q_goal = self.normalize_angles(joint_pos_goal) group.set_joint_value_target(joint_pos_goal) group.go(wait=False) q_now = np.array(group.get_current_joint_values()) if normalize_angles: q_now = self.normalize_angles(q_now) q_tol = group.get_goal_joint_tolerance() if group.get_name()=='left_arm' or group.get_name()=='right_arm' or group.get_name()=='arms' or group.get_name()=='head': q_tol = 0.04 elif group.get_name()=='torso': q_tol = 0.003 t_print = rospy.Time.now() req = BaseMoveRequest() req.x = pos[0] req.y = pos[1] req.theta = angle self.get_bm_srv() res = self._bm_move_srv.call(req) if self.execute_exit(): return False # check that preempt has not been requested by the client if self._as.is_preempt_requested(): #HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION rospy.loginfo('action halted while moving base') self._as.set_preempted() self._exit = True if self.execute_exit(): return False # check for preemption while the arm hasn't reach goal configuration while np.max(np.abs(q_goal-q_now)) > q_tol and not rospy.is_shutdown(): if self.execute_exit(): return False q_now = np.array(group.get_current_joint_values()) if normalize_angles: q_now = self.normalize_angles(q_now) # check that preempt has not been requested by the client if self._as.is_preempt_requested(): #HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION group.stop() rospy.loginfo('action halted') self._as.set_preempted() self._exit = True if self.execute_exit(): return False if (rospy.Time.now()-t_print).to_sec()>3.0: t_print = rospy.Time.now() rospy.loginfo('[' + rospy.get_name() + ']: executing action') #HERE THE CODE TO EXECUTE AS LONG AS THE BEHAVIOR TREE DOES NOT HALT THE ACTION r.sleep() if rospy.is_shutdown(): return False return res.result def request_detection(self): client = actionlib.SimpleActionClient('amazon_detector', amazon_challenge_bt_actions.msg.DetectorAction) # Waits until the action server has started up and started # listening for goals. rospy.loginfo('Start Waiting') client.wait_for_server() # Creates a goal to send to the action server. goal = amazon_challenge_bt_actions.msg.DetectorGoal(parameter=1) # Sends the goal to the action server. client.send_goal(goal) rospy.loginfo('Goal Sent') # Waits for the server to finish performing the action. client.wait_for_result() def set_status(self,status): if status == 'SUCCESS': self.pub_posed.publish("SUCCESS") rospy.sleep(1) self._feedback.status = 1 self._result.status = self._feedback.status rospy.loginfo('Action %s: Succeeded' % self._action_name) self._as.set_succeeded(self._result) elif status == 'FAILURE': self._feedback.status = 2 self._result.status = self._feedback.status rospy.loginfo('Action %s: Failed' % self._action_name) self._as.set_succeeded(self._result) else: rospy.logerr('Action %s: has a wrong return status' % self._action_name) def open_l_gripper(self): gripper_command_msg = Pr2GripperCommand() gripper_command_msg.max_effort = 40.0 gripper_command_msg.position = 10.0 r = rospy.Rate(10.0) t_init = rospy.Time.now() while (rospy.Time.now()-t_init).to_sec()<3.0 and not rospy.is_shutdown(): if self.execute_exit(): return False self._l_gripper_pub.publish(gripper_command_msg) # check that preempt has not been requested by the client if self._as.is_preempt_requested(): #HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION rospy.loginfo('action halted while opening gripper') self._as.set_preempted() self._exit = True if self.execute_exit(): return False r.sleep() return True def move_l_arm_z(self, z_desired): ''' computes straight line cartesian path in z direction :param z_desired: of tool tip w.r.t. odom_combined :return: ''' waypoints = [] # waypoints.append(self._left_arm.get_current_pose().pose) wpose = copy.deepcopy(self._left_arm.get_current_pose().pose) wpose.position.z = z_desired + self._length_tool waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self._left_arm.compute_cartesian_path(waypoints, 0.05, 0.0) # TODO make this asynchronous self._left_arm.execute(plan) return True
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()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ## Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Prepare Gripper and open it self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1)) self.ac.send_goal(g_open) # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) rospy.sleep(2) # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) 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) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] #obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 scene.add_box(table_id, table_pose, table_size) #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = REFERENCE_FRAME #obstacle1_pose.pose = self.pwh.pose[self.o1id] ## Add the target object to the scene #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.50 place_pose.pose.position.y = -0.30 place_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() #print target_pose self.object_frames_pub.publish(target_pose) rospy.sleep(2) print "==================== Generating Transformations ===========================" M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.25 # about z T = np.dot(M1, M2) pre_grasping = deepcopy(target_pose) pre_grasping.pose.position.x = T[0,3] pre_grasping.pose.position.y = T[1,3] pre_grasping.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) pre_grasping.pose.orientation.x = quat[0] pre_grasping.pose.orientation.y = quat[1] pre_grasping.pose.orientation.z = quat[2] pre_grasping.pose.orientation.w = quat[3] pre_grasping.header.frame_id = 'gazebo_world' # # Initialize the grasp object # g = Grasp() # grasps = [] # # Set the first grasp pose to the input pose # g.grasp_pose = pre_grasping # g.allowed_touch_objects = [target_id] # grasps.append(deepcopy(g)) # right_arm.pick(target_id, grasps) #Change the frame_id for the planning to take place! #target_pose.header.frame_id = 'gazebo_world' self.p_pub.publish(pre_grasping) right_arm.set_pose_target(pre_grasping, GRIPPER_FRAME) plan = right_arm.plan() rospy.sleep(2) right_arm.go(wait=True) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
pr_place.position.x = 0.315 pr_place.position.y = -0.855 pr_place.position.z = -0.1 pr_place.orientation.x = 0.505 pr_place.orientation.y = 0.485 pr_place.orientation.z = -0.497 pr_place.orientation.w = 0.512 right = MoveGroupCommander("right_arm") left = MoveGroupCommander("left_arm") #group.set_random_target() #move(right, pr) move(left, pl) # clean the scene scene.remove_world_object("pole") scene.remove_world_object("table") scene.remove_world_object("bowl") # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0.4 p.pose.position.y = -0.9 p.pose.position.z = -0.4 scene.add_box("table", p, (1.6, 0.8, 0.4)) # p.pose.position.x = 0.8 # p.pose.position.y = 0.0 # p.pose.position.z = -0.02
class MoveItDemo: def __init__(self): global obj_att # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ### Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Allow 5 seconds per planning attempt self.right_arm.set_planning_time(5) # Prepare Action Controller for gripper self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) ### OPEN THE GRIPPER ### self.open_gripper() # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) ############## CLEAR THE SCENE ################ # planning_scene.world.collision_objects.remove('target') # Remove leftover objects from a previous run self.scene.remove_world_object('target') self.scene.remove_world_object('table') # self.scene.remove_world_object(obstacle1_id) # Remove any attached objects from a previous session self.scene.remove_attached_object(GRIPPER_FRAME, 'target') # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c timerThread = threading.Thread(target=self.scene_generator) timerThread.daemon = True timerThread.start() initial_pose = PoseStamped() initial_pose.header.frame_id = 'gazebo_world' initial_pose.pose = target_pose.pose print "==================== Generating Transformations ===========================" #################### PRE GRASPING POSE ######################### M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.25 # about z T = np.dot(M1, M2) pre_grasping = deepcopy(target_pose) pre_grasping.pose.position.x = T[0,3] pre_grasping.pose.position.y = T[1,3] pre_grasping.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) pre_grasping.pose.orientation.x = quat[0] pre_grasping.pose.orientation.y = quat[1] pre_grasping.pose.orientation.z = quat[2] pre_grasping.pose.orientation.w = quat[3] pre_grasping.header.frame_id = 'gazebo_world' self.plan_exec(pre_grasping) #################### GRASPING POSE ######################### M3 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M3[0,3] = target_pose.pose.position.x M3[1,3] = target_pose.pose.position.y M3[2,3] = target_pose.pose.position.z M4 = transformations.euler_matrix(0, 1.57, 0) M4[0,3] = 0.0 # offset about x M4[1,3] = 0.0 # about y M4[2,3] = 0.18 # about z T2 = np.dot(M3, M4) grasping = deepcopy(target_pose) grasping.pose.position.x = T2[0,3] grasping.pose.position.y = T2[1,3] grasping.pose.position.z = T2[2,3] quat2 = transformations.quaternion_from_matrix(T2) grasping.pose.orientation.x = quat2[0] grasping.pose.orientation.y = quat2[1] grasping.pose.orientation.z = quat2[2] grasping.pose.orientation.w = quat2[3] grasping.header.frame_id = 'gazebo_world' self.plan_exec(grasping) #Close the gripper print "========== Waiting for gazebo to catch up ==========" self.close_gripper() #################### ATTACH OBJECT ###################### touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link'] #print touch_links self.scene.attach_box(GRIPPER_FRAME, target_id, target_pose, target_size, touch_links) # counter to let the planning scene know when to remove the object obj_att = 1 #self.scene.remove_world_object(target_id) #################### POST-GRASP RETREAT ######################### M5 = transformations.quaternion_matrix([initial_pose.pose.orientation.x, initial_pose.pose.orientation.y, initial_pose.pose.orientation.z, initial_pose.pose.orientation.w]) M5[0,3] = initial_pose.pose.position.x M5[1,3] = initial_pose.pose.position.y M5[2,3] = initial_pose.pose.position.z M6 = transformations.euler_matrix(0, 1.57, 0) M6[0,3] = 0.0 # offset about x M6[1,3] = 0.0 # about y M6[2,3] = 0.3 # about z T3 = np.dot(M5, M6) post_grasping = deepcopy(initial_pose) post_grasping.pose.position.x = T3[0,3] post_grasping.pose.position.y = T3[1,3] post_grasping.pose.position.z = T3[2,3] quat3 = transformations.quaternion_from_matrix(T3) post_grasping.pose.orientation.x = quat3[0] post_grasping.pose.orientation.y = quat3[1] post_grasping.pose.orientation.z = quat3[2] post_grasping.pose.orientation.w = quat3[3] post_grasping.header.frame_id = 'gazebo_world' self.plan_exec(post_grasping) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.52 place_pose.pose.position.y = -0.48 place_pose.pose.position.z = 0.48 place_pose.pose.orientation.w = 1.0 n_attempts = 0 max_place_attempts = 2 # Generate valid place poses places = self.make_places(place_pose) success = False # Repeat until we succeed or run out of attempts while success == False and n_attempts < max_place_attempts: for place in places: success = self.right_arm.place(target_id, place) if success: break n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) rospy.sleep(0.2) self.open_gripper() obj_att = None rospy.sleep(3) ## # Initialize the grasp object ## g = Grasp() ## grasps = [] ## # Set the first grasp pose to the input pose ## g.grasp_pose = pre_grasping ## g.allowed_touch_objects = [target_id] ## grasps.append(deepcopy(g)) ## right_arm.pick(target_id, grasps) # #Change the frame_id for the planning to take place! # #target_pose.header.frame_id = 'gazebo_world' # # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # # Exit the script moveit_commander.os._exit(0) ################################################################################################################## #Get pose from Gazebo def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg # Generate a list of possible place poses def make_places(self, init_pose): # Initialize the place location as a PoseStamped message place = PoseStamped() # Start with the input place pose place = init_pose # A list of x shifts (meters) to try x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015] # A list of y shifts (meters) to try y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015] # A list of pitch angles to try #pitch_vals = [0, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02] pitch_vals = [0] # A list of yaw angles to try yaw_vals = [0] # A list to hold the places places = [] # Generate a place pose for each angle and translation for y in yaw_vals: for p in pitch_vals: for y in y_vals: for x in x_vals: place.pose.position.x = init_pose.pose.position.x + x place.pose.position.y = init_pose.pose.position.y + y # Create a quaternion from the Euler angles q = quaternion_from_euler(0, p, y) # Set the place pose orientation accordingly place.pose.orientation.x = q[0] place.pose.orientation.y = q[1] place.pose.orientation.z = q[2] place.pose.orientation.w = q[3] # Append this place pose to the list places.append(deepcopy(place)) # Return the list return places def plan_exec(self, pose): self.right_arm.clear_pose_targets() self.right_arm.set_pose_target(pose, GRIPPER_FRAME) self.right_arm.plan() rospy.sleep(5) self.right_arm.go(wait=True) def close_gripper(self): g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100)) self.ac.send_goal(g_close) self.ac.wait_for_result() rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object def open_gripper(self): g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) self.ac.send_goal(g_open) self.ac.wait_for_result() rospy.sleep(5) # And up to 20 to detach it def scene_generator(self): # print obj_att global target_pose global target_id global target_size target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') #obstacle1_id = 'obstacle1' #self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] #obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 # Add the target object to the scene if obj_att is None: self.scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 self.scene.add_box(table_id, table_pose, table_size) #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = REFERENCE_FRAME #obstacle1_pose.pose = self.pwh.pose[self.o1id] ## Add the target object to the scene #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.50 place_pose.pose.position.y = -0.30 place_pose.pose.orientation.w = 1.0 # Add the target object to the scene self.scene.add_box(target_id, target_pose, target_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() else: self.scene.remove_world_object('target') # Publish targe's frame #self.object_frames_pub.publish(target_pose) threading.Timer(0.5, self.scene_generator).start() # Set the color of an object def setColor(self, name, r, g, b, a = 0.9): # Initialize a MoveIt color object color = ObjectColor() # Set the id to the name given as an argument color.id = name # Set the rgb and alpha values given as input color.color.r = r color.color.g = g color.color.b = b color.color.a = a # Update the global color dictionary self.colors[name] = color # Actually send the colors to MoveIt! def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm right_arm = MoveGroupCommander('right_arm') # Initialize the move group for the left arm left_arm = MoveGroupCommander('left_arm') right_arm.set_planner_id("KPIECEkConfigDefault"); left_arm.set_planner_id("KPIECEkConfigDefault"); rospy.sleep(1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) right_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the reference frame for pose targets reference_frame = 'base_footprint' # Set the right arm reference frame accordingly right_arm.set_pose_reference_frame(reference_frame) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file #left_arm.set_named_target('left_start') #left_arm.go() # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.75 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.56 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.51 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = 0.49 box2_pose.pose.position.y = 0.15 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.4 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 target_pose.pose.orientation.w = 1.0 # Set the target pose for the arm right_arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) right_arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class GraspObjectServer: def __init__(self, name): # stuff for grasp planning self.tf_listener = tf.TransformListener() self.tf_broadcaster = tf.TransformBroadcaster() self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link") self.last_objects = RecognizedObjectArray() #rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback) self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback) self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray) rospy.loginfo("Connecting to pickup AS") self.pickup_ac = SimpleActionClient('/pickup', PickupAction) #pickup_ac.wait_for_server() # needed? rospy.loginfo("Connecting to grasp generator AS") self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction) #grasps_ac.wait_for_server() # needed? #planning scene for motion planning self.scene = PlanningSceneInterface() # blocking action server self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False) self.feedback = GraspObjectFeedback() self.result = GraspObjectResult() self.current_goal = None self.grasp_obj_as.start() def objects_callback(self, data): rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects)) self.last_objects = data def goal_callback(self, goal): if self.current_goal: goal.set_rejected() # "Server busy" return elif len(self.last_objects.objects) - 1 < goal.get_goal().target_id: goal.set_rejected() # "No objects to grasp were received on the objects topic." return else: #store and accept new goal self.current_goal = goal self.current_goal.set_accepted() #run grasping state machine self.grasping_sm() #finished, get rid of goal self.current_goal = None def cancel_callback(self, goal): #TODO stop motions? self.current_goal.set_canceled() def grasping_sm(self): if self.current_goal: self.update_feedback("running clustering") (object_points, obj_bbox_dims, object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(self.last_objects.objects[self.current_goal.get_goal().target_id].point_clouds[0]) #TODO visualize bbox #TODO publish filtered pointcloud? print obj_bbox_dims ######## self.update_feedback("check reachability") ######## self.update_feedback("generate grasps") #transform pose to base_link self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(5)) trans_pose = self.tf_listener.transformPose("base_link", object_pose) object_pose = trans_pose #HACK remove orientation -> pose is aligned with parent(base_link) object_pose.pose.orientation.w = 1.0 object_pose.pose.orientation.x = 0.0 object_pose.pose.orientation.y = 0.0 object_pose.pose.orientation.z = 0.0 # shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects) # TODO remove this hack, fix it in table filtering object_pose.pose.position.z += obj_bbox_dims[2]/2.0 grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x # check if there are grasps, if not, abort if len(grasp_list) == 0: self.update_aborted("no grasps received") return self.publish_grasps_as_poses(grasp_list) self.feedback.grasps = grasp_list self.current_goal.publish_feedback(self.feedback) self.result.grasps = grasp_list ######## self.update_feedback("setup planning scene") #remove old objects self.scene.remove_world_object("object_to_grasp") # add object to grasp to planning scene self.scene.add_box("object_to_grasp", object_pose, (obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2])) self.result.object_scene_name = "object_to_grasp" ######## if self.current_goal.get_goal().execute_grasp: self.update_feedback("execute grasps") pug = self.createPickupGoal("object_to_grasp", grasp_list) rospy.loginfo("Sending goal") self.pickup_ac.send_goal(pug) rospy.loginfo("Waiting for result") self.pickup_ac.wait_for_result() result = self.pickup_ac.get_result() rospy.loginfo("Result is:") print result rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val])) ######## self.update_feedback("finished") self.result.object_pose = object_pose #bounding box in a point message self.result.bounding_box = Point() self.result.bounding_box.x = obj_bbox_dims[0] self.result.bounding_box.y = obj_bbox_dims[1] self.result.bounding_box.z = obj_bbox_dims[2] self.current_goal.set_succeeded(result = self.result) #self.current_goal.set_aborted() def update_feedback(self, text): self.feedback.last_state = text self.current_goal.publish_feedback(self.feedback) def update_aborted(self, text = ""): self.update_feedback("aborted." + text) self.current_goal.set_aborted() def generate_grasps(self, pose, width): #send request to block grasp generator service if not self.grasps_ac.wait_for_server(rospy.Duration(3.0)): return [] rospy.loginfo("Successfully connected.") goal = GenerateBlockGraspsGoal() goal.pose = pose.pose goal.width = width self.grasps_ac.send_goal(goal) rospy.loginfo("Sent goal, waiting:\n" + str(goal)) t_start = rospy.Time.now() self.grasps_ac.wait_for_result() t_end = rospy.Time.now() t_total = t_end - t_start rospy.loginfo("Result received in " + str(t_total.to_sec())) grasp_list = self.grasps_ac.get_result().grasps return grasp_list def publish_grasps_as_poses(self, grasps): rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose") graspmsg = Grasp() grasp_PA = PoseArray() header = Header() header.frame_id = "base_link" header.stamp = rospy.Time.now() grasp_PA.header = header for graspmsg in grasps: p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation) grasp_PA.poses.append(p) self.grasp_publisher.publish(grasp_PA) rospy.sleep(0.1) def createPickupGoal(self, target, possible_grasps, group="right_arm_torso_grasping"): """ Create a PickupGoal with the provided data""" pug = PickupGoal() pug.target_name = target pug.group_name = group pug.possible_grasps.extend(possible_grasps) pug.allowed_planning_time = 5.0 pug.planning_options.planning_scene_diff.is_diff = True pug.planning_options.planning_scene_diff.robot_state.is_diff = True pug.planning_options.plan_only = False pug.planning_options.replan = True pug.planning_options.replan_attempts = 10 pug.attached_object_touch_links = ['arm_right_5_link', "arm_right_grasping_frame"] pug.allowed_touch_objects.append(target) #pug.attached_object_touch_links.append('all') return pug
class PickAndPlace: def setColor(self, name, r, g, b, a=0.9): color = ObjectColor() color.id = name color.color.r = r color.color.g = g color.color.b = b color.color.a = a self.colors[name] = color def sendColors(self): p = PlanningScene() p.is_diff = True for color in self.colors.values(): p.object_colors.append(color) self.scene_pub.publish(p) def add_point(self, traj, time, positions, velocities=None): point = trajectory_msgs.msg.JointTrajectoryPoint() point.positions = copy.deepcopy(positions) if velocities is not None: point.velocities = copy.deepcopy(velocities) point.time_from_start = rospy.Duration(time) traj.points.append(point) def FollowQTraj(self, q_traj, t_traj): assert (len(q_traj) == len(t_traj)) #Insert current position to beginning. if t_traj[0] > 1.0e-2: t_traj.insert(0, 0.0) q_traj.insert(0, self.Q(arm=arm)) self.dq_traj = self.QTrajToDQTraj(q_traj, t_traj) def QTrajToDQTraj(self, q_traj, t_traj): dof = len(q_traj[0]) #Modeling the trajectory with spline. splines = [TCubicHermiteSpline() for d in range(dof)] for d in range(len(splines)): data_d = [[t, q[d]] for q, t in zip(q_traj, t_traj)] splines[d].Initialize(data_d, tan_method=splines[d].CARDINAL, c=0.0, m=0.0) #NOTE: We don't have to make spline models as we just want velocities at key points. # They can be obtained by computing tan_method, which will be more efficient. with_tan=True dq_traj = [] for t in t_traj: dq = [splines[d].Evaluate(t, with_tan=True)[1] for d in range(dof)] dq_traj.append(dq) #print dq_traj return dq_traj def JointNames(self): #0arm= 0 return self.joint_names[0] def ROSGetJTP(self, q, t, dq=None): jp = trajectory_msgs.msg.JointTrajectoryPoint() jp.positions = q jp.time_from_start = rospy.Duration(t) if dq is not None: jp.velocities = dq return jp def ToROSTrajectory(self, joint_names, q_traj, t_traj, dq_traj=None): assert (len(q_traj) == len(t_traj)) if dq_traj is not None: (len(dq_traj) == len(t_traj)) #traj= trajectory_msgs.msg.JointTrajectory() self.traj.joint_names = joint_names if dq_traj is not None: self.traj.points = [ self.ROSGetJTP(q, t, dq) for q, t, dq in zip(q_traj, t_traj, dq_traj) ] else: self.traj.points = [ self.ROSGetJTP(q, t) for q, t in zip(q_traj, t_traj) ] self.traj.header.stamp = rospy.Time.now() #print self.traj return self.traj def SmoothQTraj(self, q_traj): if len(q_traj) == 0: return q_prev = np.array(q_traj[0]) q_offset = np.array([0] * len(q_prev)) for q in q_traj: q_diff = np.array(q) - q_prev for d in range(len(q_prev)): if q_diff[d] < -math.pi: q_offset[d] += 1 elif q_diff[d] > math.pi: q_offset[d] -= 1 q_prev = copy.deepcopy(q) q[:] = q + q_offset * 2.0 * math.pi def add_target(self, target_pose, target_size, frame, x, y, o1, o2, o3, o4): target_pose.header.frame_id = frame target_pose.pose.position.x = x target_pose.pose.position.y = y target_pose.pose.position.z = self.table_ground + self.table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.x = o1 target_pose.pose.orientation.y = o2 target_pose.pose.orientation.z = o3 target_pose.pose.orientation.w = o4 #self.scene.add_box(f_target_id,f_target_pose,f_target_size) def cts(self, start_pose, mid_pose, end_pose, maxtries, exe_signal=False): waypoints = [] fraction = 0.0 attempts = 0 waypoints.append(start_pose.pose) if mid_pose != 0: waypoints.append(mid_pose.pose) waypoints.append(end_pose.pose) while fraction != 1 and attempts < maxtries: (plan, fraction) = self.arm.compute_cartesian_path( waypoints, 0.005, 0.0, True) attempts += 1 if (attempts % maxtries == 0 and fraction != 1): rospy.loginfo("path planning failed with " + str(fraction * 100) + "% success.") return 0, 0 continue elif fraction == 1: rospy.loginfo("path compute successfully with " + str(attempts) + " attempts.") if exe_signal: q_traj = [self.arm.get_current_joint_values()] t_traj = [0.0] for i in range(2, len(plan.joint_trajectory.points)): q_traj.append( plan.joint_trajectory.points[i].positions) t_traj.append(t_traj[-1] + 0.07) self.FollowQTraj(q_traj, t_traj) self.sub_jpc.publish( self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj)) rospy.sleep(5) end_joint_state = plan.joint_trajectory.points[-1].positions signal = 1 return signal, end_joint_state #move and rotate def cts_rotate(self, start_pose, end_pose, angle_r, maxtries, exe_signal=False): angle = angle_r * 3.14 / 180.0 waypoints = [] fraction = 0.0 attempts = 0 waypoints.append(start_pose.pose) waypoints.append(end_pose.pose) while fraction != 1 and attempts < maxtries: (plan, fraction) = self.arm.compute_cartesian_path( waypoints, 0.005, 0.0, True) attempts += 1 if (attempts % maxtries == 0 and fraction != 1): rospy.loginfo("path planning failed with " + str(fraction * 100) + "% success.") return 0, 0.0 continue elif fraction == 1: rospy.loginfo("path compute successfully with " + str(attempts) + " attempts.") if exe_signal: q_traj = [self.arm.get_current_joint_values()] t_traj = [0.0] per_angle = angle / (len(plan.joint_trajectory.points) - 2) for i in range(2, len(plan.joint_trajectory.points)): joint_inc_list = [ j for j in plan.joint_trajectory.points[i].positions ] #plan.joint_trajectory.points[i].positions[6]-=per_angle*(i-1) joint_inc_list[6] -= per_angle * (i - 1) q_traj.append(joint_inc_list) t_traj.append(t_traj[-1] + 0.05) self.FollowQTraj(q_traj, t_traj) self.sub_jpc.publish( self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj)) rospy.sleep(5) end_joint_state = plan.joint_trajectory.points[-1].positions signal = 1 return signal, end_joint_state # shaking function: # freq : shaking freqence # times : shaking time per action def shaking(self, initial_state, end_joint_state, freq, times): q_traj = [initial_state] t_traj = [0.0] for i in range(times): q_traj.append(end_joint_state) t_traj.append(t_traj[-1] + 0.5 / freq) q_traj.append(initial_state) t_traj.append(t_traj[-1] + 0.5 / freq) self.FollowQTraj(q_traj, t_traj) self.sub_jpc.publish( self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj)) rospy.sleep(6) #shaking_a: vertical bottle axis def shake_a(self, pre_p_angle, r_angle, amp): start_shake_pose = self.arm.get_current_pose( self.arm_end_effector_link) # for trajectory of shaking start_joint_state = self.arm.get_current_joint_values( ) # for state[6] to rotate shift_pose = deepcopy(start_shake_pose) r_angle = (r_angle / 180.0) * 3.14 pre_p_angle = (pre_p_angle / 180.0) * 3.14 shift_pose.pose.position.x += amp * math.sin(r_angle) * math.cos( pre_p_angle) # in verticle direction shift_pose.pose.position.y -= amp * math.sin(r_angle) * math.sin( pre_p_angle) shift_pose.pose.position.z += amp * math.cos(r_angle) #... signal, end_joint_state = self.cts(start_shake_pose, shift_pose, 300) return signal, end_joint_state def shake_b(self, pre_p_angle, r_angle, amp): start_shake_pose = self.arm.get_current_pose( self.arm_end_effector_link) # for trajectory of shaking start_joint_state = self.arm.get_current_joint_values( ) # for state[6] to rotate shift_pose = deepcopy(start_shake_pose) r_angle = (r_angle / 180.0) * 3.14 pre_p_angle = (pre_p_angle / 180.0) * 3.14 shift_pose.pose.position.x -= amp * math.cos(r_angle) * math.cos( pre_p_angle) # in verticle direction shift_pose.pose.position.y += amp * math.cos(r_angle) * math.sin( pre_p_angle) shift_pose.pose.position.z += amp * math.sin(r_angle) #... signal, end_joint_state = self.cts(start_shake_pose, shift_pose, 300) return signal, end_joint_state def setupSence(self): r_tool_size = [0.03, 0.02, 0.18] l_tool_size = [0.03, 0.02, 0.18] #real scene table table_pose = PoseStamped() table_pose.header.frame_id = self.reference_frame table_pose.pose.position.x = -0.0 table_pose.pose.position.y = 0.65 table_pose.pose.position.z = self.table_ground + self.table_size[ 2] / 2.0 table_pose.pose.orientation.w = 1.0 self.scene.add_box(self.table_id, table_pose, self.table_size) #left gripper l_p = PoseStamped() l_p.header.frame_id = self.arm_end_effector_link l_p.pose.position.x = 0.00 l_p.pose.position.y = 0.057 l_p.pose.position.z = 0.09 l_p.pose.orientation.w = 1 self.scene.attach_box(self.arm_end_effector_link, self.l_id, l_p, l_tool_size) #right gripper r_p = PoseStamped() r_p.header.frame_id = self.arm_end_effector_link r_p.pose.position.x = 0.00 r_p.pose.position.y = -0.057 r_p.pose.position.z = 0.09 r_p.pose.orientation.w = 1 self.scene.attach_box(self.arm_end_effector_link, self.r_id, r_p, r_tool_size) #Params: pose of bottle, grasp_radius, grasp_height, grasp_theta def get_grasp_pose(self, pose, r, theta): g_p = PoseStamped() g_p.header.frame_id = self.arm_end_effector_link g_p.pose.position.x = pose.pose.position.x + r * math.sin(theta) g_p.pose.position.y = pose.pose.position.y - r * math.cos(theta) g_p.pose.position.z = pose.pose.position.z g_p.pose.orientation.w = 0.5 * (math.cos(0.5 * theta) - math.sin(0.5 * theta)) g_p.pose.orientation.x = -0.5 * (math.cos(0.5 * theta) + math.sin(0.5 * theta)) g_p.pose.orientation.y = 0.5 * (math.cos(0.5 * theta) - math.sin(0.5 * theta)) g_p.pose.orientation.z = 0.5 * (math.sin(0.5 * theta) + math.cos(0.5 * theta)) return g_p def get_pour_pose(self, pose, h, r, theta): p_p = PoseStamped() p_p.header.frame_id = self.arm_end_effector_link p_p.pose.position.x = pose.pose.position.x - r * math.cos(theta) p_p.pose.position.y = pose.pose.position.y + r * math.sin(theta) p_p.pose.position.z = pose.pose.position.z + h theta *= -1 p_p.pose.orientation.w = 0.5 * (math.cos(0.5 * theta) - math.sin(0.5 * theta)) p_p.pose.orientation.x = -0.5 * (math.cos(0.5 * theta) + math.sin(0.5 * theta)) p_p.pose.orientation.y = 0.5 * (math.cos(0.5 * theta) - math.sin(0.5 * theta)) p_p.pose.orientation.z = 0.5 * (math.sin(0.5 * theta) + math.cos(0.5 * theta)) return p_p def pour_rotate(self, angle_pre, angle_r, r, maxtries): angle_pre = (angle_pre / 180.0) * 3.14 angle_r_1 = (angle_r / 180.0) * 3.14 initial_state = self.arm.get_current_joint_values() initial_pose = self.arm.get_current_pose(self.arm_end_effector_link) final_pose = deepcopy(initial_pose) final_pose.pose.position.x += r * ( 1 - math.cos(angle_r_1)) * math.cos(angle_pre) final_pose.pose.position.y += r * ( 1 - math.cos(angle_r_1)) * math.sin(angle_pre) final_pose.pose.position.z += r * math.sin(angle_r_1) signal, e_j_s = self.cts_rotate(initial_pose, final_pose, angle_r, maxtries, True) return signal def pg_g_pp(self, pose, r, pre_r): start_pose = self.arm.get_current_pose(self.arm_end_effector_link) p_i_radian = np.arctan(abs(pose.pose.position.x / pose.pose.position.y)) p_i_angle = p_i_radian * 180.0 / 3.14 pick_list = [ 0.0, p_i_angle, 5.0, 25.0, 45.0, 65.0, 15.0, 35.0, 55.0, 75.0, 10.0, 20.0, 30.0, 40.0, 50.0, 60.0 ] for i in range(len(pick_list)): theta = (pick_list[i] / 180.0) * 3.14 if pose.pose.position.x > 0: theta *= -1.0 pre_grasp_pose = self.get_grasp_pose(pose, r + pre_r, theta) #pick up grasp_pose = pre_grasp_pose grasp_pose.pose.position.x -= pre_r * math.sin(theta) grasp_pose.pose.position.y += pre_r * math.cos(theta) signal, e_j_s = self.cts(start_pose, pre_grasp_pose, grasp_pose, 300, True) if signal == 0: continue break rospy.sleep(1) self.gripperCtrl(0) rospy.sleep(2) #move to ori_pose current_pose = self.arm.get_current_pose(self.arm_end_effector_link) signal, e_j_s = self.cts(current_pose, pre_grasp_pose, start_pose, 300, True) rospy.sleep(2) rospy.loginfo("Grasping done.") return start_pose, pre_grasp_pose, grasp_pose def pp_ps(self, target_pose, pour_angle, r_pour, h_pour): for i in range(len(pour_angle)): maxtries = 300 start_pose = self.arm.get_current_pose(self.arm_end_effector_link) theta = (pour_angle[i] / 180.0) * 3.14 pour_pose = self.get_pour_pose(target_pose, h_pour, r_pour, theta) #move to pose signal_1, e_j_s = self.cts_rotate(start_pose, pour_pose, 90.0, maxtries, True) if signal_1 == 0: continue pre_pour_angle = pour_angle[i] rospy.loginfo("Ready for pouring.") return pre_pour_angle def go_back(self, ori_pose, pre_grasp_pose, grasp_pose): cur_pose = self.arm.get_current_pose(self.arm_end_effector_link) signal, e1 = self.cts(cur_pose, 0, ori_pose, 300, True) rospy.loginfo("back to pre_grasp pose, ready for placing bottle..") signal_1, e2 = self.cts(ori_pose, pre_grasp_pose, grasp_pose, 300, True) rospy.loginfo("back to grasp pose, open gripper..") self.gripperCtrl(255) rospy.sleep(1) signal_2, e3 = self.cts(grasp_pose, pre_grasp_pose, ori_pose, 300, True) rospy.loginfo("back to pre_grasp pose, ready for next kind of source.") def rotate_back(self, angle): angle = angle * 3.14 / 180.0 current_j_state = self.arm.get_current_joint_values() current_j_state[6] += angle + 1.57 self.arm.go() def last_step_go_back(self, ori_pose): cur_pose = self.arm.get_current_pose(self.arm_end_effector_link) signal, e1 = self.cts(cur_pose, 0, ori_pose, 300, True) rospy.loginfo("back to last step...") rospy.sleep(1) def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') self.scene = PlanningSceneInterface() pub_traj = rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory, queue_size=10) #pub_ratio_sig= rospy.Publisher('/enable_source_change',Int64,queue_size=1) self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) self.gripperCtrl = rospy.ServiceProxy( "/two_finger/gripper/gotoPositionUntilTouch", SetPosition) self.colors = dict() rospy.sleep(1) self.robot = moveit_commander.robot.RobotCommander() self.arm = MoveGroupCommander('arm') self.arm.allow_replanning(True) cartesian = rospy.get_param('~cartesian', True) self.arm_end_effector_link = self.arm.get_end_effector_link() self.arm.set_goal_position_tolerance(0.005) self.arm.set_goal_orientation_tolerance(0.025) self.arm.allow_replanning(True) self.reference_frame = 'base_link' self.arm.set_pose_reference_frame(self.reference_frame) self.arm.set_planning_time(5) #shaking self.joint_names = [[]] self.joint_names[0] = rospy.get_param('controller_joint_names') self.traj = trajectory_msgs.msg.JointTrajectory() self.sub_jpc = rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory, queue_size=10) #scene planning self.l_id = 'l_tool' self.r_id = 'r_tool' self.table_id = 'table' self.target1_id = 'target1_object' self.target2_id = 'target2_object' self.target3_id = 'target3_object' self.target4_id = 'target4_object' self.f_target_id = 'receive_container' self.scene.remove_world_object(self.l_id) self.scene.remove_world_object(self.r_id) self.scene.remove_world_object(self.table_id) self.scene.remove_world_object(self.target1_id) self.scene.remove_world_object(self.target2_id) self.scene.remove_world_object(self.target3_id) self.scene.remove_world_object(self.target4_id) self.scene.remove_world_object(self.f_target_id) #self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id) self.table_ground = 0.13 self.table_size = [0.9, 0.6, 0.018] self.setupSence() target1_size = [0.035, 0.035, 0.19] target2_size = target1_size target3_size = target1_size target4_size = target1_size self.f_target_size = [0.2, 0.2, 0.04] f_target_pose = PoseStamped() pre_pour_pose = PoseStamped() target1_pose = PoseStamped() target2_pose = PoseStamped() target3_pose = PoseStamped() target4_pose = PoseStamped() joint_names = [ 'joint_' + jkey for jkey in ('s', 'l', 'e', 'u', 'r', 'b', 't') ] joint_names = rospy.get_param('controller_joint_names') traj = trajectory_msgs.msg.JointTrajectory() traj.joint_names = joint_names #final target #self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1) self.add_target(f_target_pose, self.f_target_size, self.reference_frame, 0.2, 0.6, 0, 0, 0, 1) self.scene.add_box(self.f_target_id, f_target_pose, self.f_target_size) #self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1) #target localization #msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped) self.add_target(target1_pose, target1_size, self.reference_frame, -0.25, 0.8, 0, 0, 0, 1) self.scene.add_box(self.target1_id, target1_pose, target1_size) self.add_target(target2_pose, target2_size, self.reference_frame, -0.12, 0.87, 0, 0, 0, 1) self.scene.add_box(self.target2_id, target2_pose, target2_size) self.add_target(target3_pose, target3_size, self.reference_frame, 0.02, 0.88, 0, 0, 0, 1) self.scene.add_box(self.target3_id, target3_pose, target3_size) self.add_target(target4_pose, target4_size, self.reference_frame, 0.15, 0.80, 0, 0, 0, 1) self.scene.add_box(self.target4_id, target4_pose, target4_size) #attach_pose g_p = PoseStamped() g_p.header.frame_id = self.arm_end_effector_link g_p.pose.position.x = 0.00 g_p.pose.position.y = -0.00 g_p.pose.position.z = 0.025 g_p.pose.orientation.w = 0.707 g_p.pose.orientation.x = 0 g_p.pose.orientation.y = -0.707 g_p.pose.orientation.z = 0 #set color self.setColor(self.target1_id, 0.8, 0, 0, 1.0) self.setColor(self.target2_id, 0.8, 0, 0, 1.0) self.setColor(self.target3_id, 0.8, 0, 0, 1.0) self.setColor(self.target4_id, 0.8, 0, 0, 1.0) self.setColor(self.f_target_id, 0.8, 0.3, 0, 1.0) self.setColor('r_tool', 0.8, 0, 0) self.setColor('l_tool', 0.8, 0, 0) self.sendColors() self.gripperCtrl(255) rospy.sleep(3) self.arm.set_named_target("initial_arm") self.arm.go() rospy.sleep(3) #j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156] #j_ori_state=[-2.161055326461792, -0.6802523136138916, -1.7733728885650635, -2.3315746784210205, -0.5292841196060181, 1.4411976337432861, -2.2327845096588135] j_ori_state = [ -1.2628753185272217, -0.442996621131897, -0.1326361745595932, 2.333048105239868, -0.15598002076148987, -1.2167049646377563, 3.1414425373077393 ] #signal= True self.arm.set_joint_value_target(j_ori_state) self.arm.go() rospy.sleep(3) #parameter setup tar_num = 4 maxtries = 300 r_grasp = 0.15 r_pre_g = 0.1 r_bottle = 0.1 for i in range(0, tar_num): #grasp target rospy.loginfo("Choosing source...") if i == 0: target_pose = target1_pose target_id = self.target1_id target_size = target1_size amp = 0.1 freq = 2.75 r_angle = 45.0 elif i == 1: target_pose = target2_pose target_id = self.target2_id target_size = target2_size amp = 0.15 freq = 2.0 r_angle = 30.0 elif i == 2: target_pose = target3_pose target_id = self.target3_id target_size = target3_size amp = 0.1 freq = 2.75 r_angle = 45.0 elif i == 3: target_pose = target4_pose target_id = self.target4_id target_size = target4_size amp = 0.1 freq = 2.75 r_angle = 45.0 r_pour = 0.1 h_pour = 0.1 #pour_angle=[15.0,30.0,45.0,60.0,75.0] pour_angle = [ 0.0, -15.0, -10.0, -5.0, 0.0, 10.0, 15.0, 30.0, 45.0, 60.0, 75.0 ] rospy.loginfo("Params loaded.") rospy.loginfo("Current Source: " + str(i + 1) + " ") #grasp and back ori_pose, mid_pose, g_pose = self.pg_g_pp(target_pose, r_grasp, r_pre_g) #move to target position for pouring pre_p_angle = self.pp_ps(f_target_pose, pour_angle, r_pour, h_pour) rospy.loginfo("pre_pour_angle : " + str(pre_p_angle)) #rotation and shaking ps_signal = self.pour_rotate(pre_p_angle, r_angle, r_bottle, maxtries) if ps_signal != 1: rospy.loginfo( "Pre_shaking_rotation failed, back to ori_pose...") self.last_step_go_back(ori_pose) rospy.sleep(3) continue else: rospy.loginfo("Shaking planning...") shake_per_times = 3 shake_times = 0 shake_times_tgt = 1 signal = True rospy.loginfo("Parameter loaded") rospy.sleep(1) signal, end_joint_state = self.shake_a(pre_p_angle, r_angle, amp) while signal == 1: #area_ratio= rospy.wait_for_message('/color_area_ratio',Float64) #if (area_ratio>ratio_table[i]) or (shake_times>shake_times_tgt): if (shake_times < shake_times_tgt): self.shaking(start_joint_state, end_joint_state, freq, shake_per_times) shake_times += 1 rospy.loginfo("shaking times : " + str(shake_times) + " .") else: signal = 0 self.rotate_back(r_angle) self.go_back(ori_pose, mid_pose, g_pose) rospy.sleep(2) continue #remove and shut down self.scene.remove_attached_object(self.arm_end_effector_link, 'l_tool') self.scene.remove_attached_object(self.arm_end_effector_link, 'r_tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
# specify the planner group.set_planner_id("RRTkConfigDefault") rospy.sleep(3) ## We create this DisplayTrajectory publisher which is used below to publish ## trajectories for RVIZ to visualize. # display_trajectory_publisher = rospy.Publisher( # '/move_group/display_planned_path', # moveit_msgs.msg.DisplayTrajectory) print ">>>>> remove scenes" # clean the scene scene.remove_world_object("pole") scene.remove_world_object("ball") scene.remove_world_object("table") scene.remove_world_object("ground_plane") rospy.sleep(5) print ">>>>> add scenes" # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0 p.pose.position.y = -1 p.pose.position.z = 0.2 p.pose.orientation.w = 1.0 scene.add_box("pole", p, (0.4, 0.1, 0.4))
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_attached_object_demo') # 初始化场景对象 scene = PlanningSceneInterface() rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('manipulator') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() # 设置每次运动规划的时间限制:10s arm.set_planning_time(10) # 移除场景中之前运行残留的物体 scene.remove_attached_object(end_effector_link, 'tool') scene.remove_world_object('table') # 设置桌面的高度 table_ground = 0.7 # 设置table和tool的三维尺寸 table_size = [0.1, 0.3, 0.02] tool_size = [0.2, 0.02, 0.02] # 设置tool的位姿 p = PoseStamped() p.header.frame_id = end_effector_link p.pose.position.x = tool_size[0] / 2.0 p.pose.position.z = -0.015 p.pose.orientation.w = 1 # 将tool附着到机器人的终端 scene.attach_box(end_effector_link, 'tool', p, tool_size) # 将table加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = 'base_link' table_pose.pose.position.x = 0.4 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] table_pose.pose.orientation.w = 1.0 scene.add_box('table', table_pose, table_size) rospy.sleep(2) # 更新当前的位姿 arm.set_start_state_to_current_state() # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) joint_positions = [ -0.5110620856285095, 0.32814791798591614, 0.5454912781715393, 1.0146701335906982, 0.8498637080192566, -0.45695754885673523 ] arm.set_joint_value_target(joint_positions) # 控制机械臂完成运动 arm.go() rospy.sleep(1) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class ObjectManipulationAS: def __init__(self, name): # stuff for grasp planning rospy.loginfo("Getting a TransformListener...") self.tf_listener = tf.TransformListener() rospy.loginfo("Getting a TransformBroadcaster...") self.tf_broadcaster = tf.TransformBroadcaster() rospy.loginfo("Initializing a ClusterBoundingBoxFinder...") self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link") self.last_clusters = None rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...") self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback) if DEBUG_MODE: self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped) rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...") self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction) self.pickup_ac.wait_for_server() rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...") self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction) self.place_ac.wait_for_server() rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...") self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction) self.grasps_ac.wait_for_server() rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...") self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty) self.depth_service.wait_for_service() rospy.loginfo("Getting a PlanningSceneInterface instance...") self.scene = PlanningSceneInterface() # blocking action server rospy.loginfo("Creating Action Server '" + name + "'...") self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False) self.as_feedback = ObjectManipulationFeedback() self.as_result = ObjectManipulationActionResult() self.current_goal = None # Take care of left and right arm grasped stuff self.right_hand_object = None self.left_hand_object = None self.current_side = 'right' rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!") self.grasp_obj_as.start() def objects_callback(self, data): rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects)) self.last_clusters = data def goal_callback(self, goal): if self.current_goal: goal.set_rejected() # "Server busy" return # TODO: Check if pose is not empty, if it is, reject # elif len(self.last_clusters.objects) - 1 < goal.get_goal().target_id: # goal.set_rejected() # "No objects to grasp were received on the objects topic." # return else: #store and accept new goal self.current_goal = goal self.current_goal.set_accepted() #run the corresponding operation if self.current_goal.get_goal().operation == ObjectManipulationGoal.PICK: rospy.loginfo("ObjectManipulation: PICK!") self.pick_operation() elif self.current_goal.get_goal().operation == ObjectManipulationGoal.PLACE: rospy.loginfo("ObjectManipulation: PLACE!") self.place_operation() else: rospy.logerr("ObjectManipulation: Erroneous operation!") #finished, get rid of goal self.current_goal = None def cancel_callback(self, goal): #TODO stop motions? self.current_goal.set_canceled() def pick_operation(self): """Execute pick operation""" if self.message_fields_ok(): self.as_result = ObjectManipulationResult() goal_message_field = self.current_goal.get_goal() self.update_feedback("Checking hand to use") # Check which arm group was requested and if it's currently free of objects if 'right' in goal_message_field.group: if self.right_hand_object: # Something already in the hand self.update_aborted("Right hand already contains an object.") return # necessary? self.current_side = 'right' elif 'left' in goal_message_field.group: if self.left_hand_object: self.update_aborted("Left hand already contains an object.") return self.current_side = 'left' # Publish pose of goal position if DEBUG_MODE: self.to_grasp_object_pose_pub.publish(goal_message_field.target_pose) self.update_feedback("Detecting clusters") if not self.wait_for_recognized_array(wait_time=5, timeout_time=10): # wait until we get clusters published self.update_aborted("Failed detecting clusters") # Search closer cluster # transform pose to base_link if needed if goal_message_field.target_pose.header.frame_id != "base_link": self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5)) object_to_grasp_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose) else: object_to_grasp_pose = goal_message_field.target_pose.pose self.update_feedback("Searching closer cluster while clustering") (closest_cluster_id, (object_points, obj_bbox_dims, object_bounding_box, object_pose)) = self.get_id_of_closest_cluster_to_pose(object_to_grasp_pose) rospy.logdebug("in AS: Closest cluster id is: " + str(closest_cluster_id)) #TODO visualize bbox #TODO publish filtered pointcloud? rospy.logdebug("BBOX: " + str(obj_bbox_dims)) ######## self.update_feedback("Check reachability") ######## self.update_feedback("Generating grasps") rospy.logdebug("Object pose before tf thing is: " + str(object_pose)) #transform pose to base_link, IS THIS NECESSARY?? should be a function in any case self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15)) trans_pose = self.tf_listener.transformPose("base_link", object_pose) object_pose = trans_pose #HACK remove orientation -> pose is aligned with parent(base_link) object_pose.pose.orientation.w = 1.0 object_pose.pose.orientation.x = 0.0 object_pose.pose.orientation.y = 0.0 object_pose.pose.orientation.z = 0.0 # shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects) # TODO remove this hack, fix it in table filtering object_pose.pose.position.z += obj_bbox_dims[2] / 2.0 grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x # check if there are grasps, if not, abort if len(grasp_list) == 0: self.update_aborted("No grasps received") return if DEBUG_MODE: # TODO: change to dynamic param publish_grasps_as_poses(grasp_list) self.current_goal.publish_feedback(self.as_feedback) ######## self.update_feedback("Setup planning scene") # Add object to grasp to planning scene self.scene.add_box(self.current_side + "_hand_object", object_pose, (obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2])) self.as_result.object_scene_name = self.current_side + "_hand_object" ######## self.update_feedback("Execute grasps") pug = createPickupGoal(self.current_side + "_hand_object", grasp_list, group=goal_message_field.group) rospy.loginfo("Sending pickup goal") self.pickup_ac.send_goal(pug) rospy.loginfo("Waiting for result...") self.pickup_ac.wait_for_result() result = self.pickup_ac.get_result() rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val])) ######## self.update_feedback("finished") self.as_result.object_pose = object_pose self.as_result.error_code = result.error_code self.as_result.error_message = str(moveit_error_dict[result.error_code.val]) if result.error_code.val == MoveItErrorCodes.SUCCESS: if self.current_side == 'right': self.right_hand_object = self.current_side + "_hand_object" elif self.current_side == 'left': self.left_hand_object = self.current_side + "_hand_object" self.current_goal.set_succeeded(result=self.as_result) else: # Remove object as it has not been picked self.scene.remove_world_object(self.current_side + "_hand_object") self.update_aborted(text="MoveIt pick failed", manipulation_result=self.as_result) else: self.update_aborted("Goal fields not correctly fulfilled") def place_operation(self): """Execute the place operation""" if self.message_fields_ok(): self.as_result = ObjectManipulationResult() goal_message_field = self.current_goal.get_goal() self.update_feedback("Checking arm to use") # Check which arm group was requested and if it currently has an object if 'right' in goal_message_field.group: if not self.right_hand_object: # Something already in the hand self.update_aborted("Right hand does not contain an object.") return # necessary? self.current_side = 'right' current_target = self.right_hand_object elif 'left' in goal_message_field.group: if not self.left_hand_object: self.update_aborted("Left hand does not contain an object.") return self.current_side = 'left' current_target = self.left_hand_object # transform pose to base_link if needed if goal_message_field.target_pose.header.frame_id != "base_link": self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5)) placing_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose) else: placing_pose = goal_message_field.target_pose.pose #### TODO: ADD HERE LOGIC ABOUT SEARCHING GOOD PLACE POSE #### self.update_feedback("Sending place order to MoveIt!") placing_pose = PoseStamped(header=Header(frame_id="base_link"), pose=placing_pose) goal = createPlaceGoal(placing_pose, group=goal_message_field.group, target=current_target) rospy.loginfo("Sending place goal") self.place_ac.send_goal(goal) rospy.loginfo("Waiting for result...") self.place_ac.wait_for_result() result = self.place_ac.get_result() rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val])) self.as_result.object_pose = placing_pose self.as_result.error_code = result.error_code self.as_result.error_message = str(moveit_error_dict[result.error_code.val]) if result.error_code.val == MoveItErrorCodes.SUCCESS: # Emptying hand self.update_feedback("Emptying hand") if self.current_side == 'right': self.as_result.object_scene_name = current_target self.right_hand_object = None elif self.current_side == 'left': self.as_result.object_scene_name = current_target self.left_hand_object = None # Removing object from the collision world self.scene.remove_world_object(current_target) self.current_goal.set_succeeded(result=self.as_result) else: self.update_aborted(text="MoveIt place failed", manipulation_result=self.as_result) else: self.update_aborted("Goal fields not correctly fulfilled") def message_fields_ok(self): """Check if the minimal fields for the message are fulfilled""" if self.current_goal: goal_message_field = self.current_goal.get_goal() if len(goal_message_field.group) == 0: rospy.logwarn("Group field empty.") return False if goal_message_field.operation != ObjectManipulationGoal.PICK and goal_message_field.operation != ObjectManipulationGoal.PLACE: rospy.logwarn("Asked for an operation different from PICK or PLACE: " + str(goal_message_field.operation)) return False if len(goal_message_field.target_pose.header.frame_id) == 0: rospy.logwarn("Target pose frame_id is empty") return False return True def update_feedback(self, text): """Publish feedback with given text""" self.as_feedback.last_state = text self.current_goal.publish_feedback(self.as_feedback) def update_aborted(self, text="", manipulation_result=None): """Publish feedback and abort with the text give and optionally an ObjectManipulationResult already fulfilled""" self.update_feedback("aborted." + text) if manipulation_result == None: aborted_result = ObjectManipulationResult() aborted_result.error_message = text self.current_goal.set_aborted(result=aborted_result) else: self.current_goal.set_aborted(result=manipulation_result) def generate_grasps(self, pose, width): """Send request to block grasp generator service""" goal = GenerateGraspsGoal() goal.pose = pose.pose goal.width = width grasp_options = GraspGeneratorOptions() grasp_options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Y grasp_options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_DOWN grasp_options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_HALF goal.options.append(grasp_options) self.grasps_ac.send_goal(goal) if DEBUG_MODE: rospy.loginfo("Sent goal, waiting:\n" + str(goal)) self.grasps_ac.wait_for_result() grasp_list = self.grasps_ac.get_result().grasps return grasp_list def get_id_of_closest_cluster_to_pose(self, input_pose): """Returns the id of the closest cluster to the pose provided (in Pose or PoseStamped) and all the associated clustering information""" current_id = 0 closest_pose = None closest_object_points = None closest_id = 0 closest_bbox = None closest_bbox_dims = None closest_distance = 99999.9 for myobject in self.last_clusters.objects: (object_points, obj_bbox_dims, object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(myobject.point_clouds[0]) self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15)) trans_pose = self.tf_listener.transformPose("base_link", object_pose) object_pose = trans_pose rospy.loginfo("id: " + str(current_id) + "\n pose:\n" + str(object_pose)) if closest_pose == None: # first loop iteration closest_object_points = object_points closest_pose = object_pose closest_bbox = object_bounding_box closest_bbox_dims = obj_bbox_dims closest_distance = dist_between_poses(closest_pose, input_pose) else: if dist_between_poses(object_pose, input_pose) < closest_distance: closest_object_points = object_points closest_distance = dist_between_poses(object_pose, input_pose) closest_pose = object_pose closest_bbox = object_bounding_box closest_bbox_dims = obj_bbox_dims closest_id = current_id current_id += 1 rospy.loginfo("Closest id is: " + str(closest_id) + " at " + str(closest_pose)) return closest_id, (closest_object_points, closest_bbox_dims, closest_bbox_dims, closest_pose) def wait_for_recognized_array(self, wait_time=6, timeout_time=10): """Ask for depth images until we get a recognized array wait for wait_time between depth throtle calls stop if timeout_time is achieved If we dont find it in the correspondent time return false, true otherwise""" initial_time = rospy.Time.now() self.last_clusters = None count = 0 num_calls = 1 self.depth_service.call(EmptyRequest()) rospy.loginfo("Depth throtle server call #" + str(num_calls)) rospy.loginfo("Waiting for a recognized array...") while rospy.Time.now() - initial_time < rospy.Duration(timeout_time) and self.last_clusters == None: rospy.sleep(0.1) count += 1 if count >= wait_time / 10: self.depth_service.call(EmptyRequest()) num_calls += 1 rospy.loginfo("Depth throtle server call #" + str(num_calls)) if self.last_clusters == None: return False else: return True
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt right_arm.set_planning_time(60) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "grasp" pose stored in the SRDF file right_arm.set_named_target('right_arm_up') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_OPEN) right_gripper.go() rospy.sleep(5) # Set the height of the table off the ground table_ground = 0.04 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] target_x = 0.135 #target_y = -0.32 target_y = -0.285290879999 # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.25 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = target_x target_pose.pose.position.y = target_y target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table blue and the boxes orange self.setColor(table_id, 0, 0, 0.8, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object right_arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.18 place_pose.pose.position.y = 0 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 p = PoseStamped() p.header.frame_id = "up1_footprint" p.pose.position.x = 0.12792118579 p.pose.position.y = -0.285290879999 p.pose.position.z = 0.120301181892 p.pose.orientation.x = 0.0 p.pose.orientation.y = 0.0 p.pose.orientation.z = -0.706825181105 p.pose.orientation.w = 0.707388269167 right_gripper.set_pose_target(p.pose) # pick an object right_arm.allow_replanning(True) right_arm.allow_looking(True) right_arm.set_goal_tolerance(0.05) right_arm.set_planning_time(60) print "arm grasp" success = 0 attempt = 0 while not success: p_plan = right_arm.plan() attempt = attempt + 1 print "Planning attempt: " + str(attempt) if p_plan.joint_trajectory.points != []: success = 1 print "arm grasp" right_arm.execute(p_plan) rospy.sleep(5) right_gripper.set_joint_value_target(GRIPPER_GRASP) right_gripper.go() print "gripper closed" rospy.sleep(5) scene.attach_box(GRIPPER_FRAME, target_id) print "object attached" right_arm.set_named_target('right_arm_up') right_arm.go() print "arm up" rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class 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('irb120_robotiq85_gazebo'), '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) rospy.loginfo("All objects are respawned") def spawn_all_model(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"] 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"] # 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)
class DaArmServer: """The basic, design problem/tui agnostic arm server """ gestures = {} pointing_height = 0.06 grasp_height = 0.05 drop_height = 0.07 cruising_height = 0.1 START_TOLERANCE = 0.05 # this is for moveit to check for change in joint angles before moving GOAL_TOLERANCE = 0.005 moving = False paused = False move_group_state = "IDLE" last_joint_trajectory_goal = "" last_joint_trajectory_result = "" def __init__(self, num_planning_attempts=100, safe_zone=None): rospy.init_node("daarm_server", anonymous=True) self.safe_zone = safe_zone # this is a fallback zone to drop a block on fail if nothing is passed: [{x,y},{xT,yT}] 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=700): rospy.set_param( "/move_group/trajectory_execution/allowed_start_tolerance", self.START_TOLERANCE) self.arm = MoveGroupCommander("arm") self.gripper = MoveGroupCommander("gripper") self.robot = RobotCommander() self.arm.set_num_planning_attempts(num_planning_attempts) self.arm.set_goal_position_tolerance(self.GOAL_TOLERANCE) self.arm.set_goal_orientation_tolerance(0.02) self.init_services() self.init_action_servers() def init_scene(self): world_objects = [ "table", "tui", "monitor", "overHead", "wall", "farWall", "frontWall", "backWall", "blockProtector", "rearCamera" ] 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.tuiPose.pose.position = Point(0.0056, -0.343, -0.51) self.tuiDimension = (0.9906, 0.8382, 0.8836) self.overHeadPose = PoseStamped() self.overHeadPose.header.frame_id = self.robot.get_planning_frame() self.overHeadPose.pose.position = Point(0.0056, 0.0, 0.97) self.overHeadDimension = (0.9906, 0.8382, 0.05) self.blockProtectorPose = PoseStamped() self.blockProtectorPose.header.frame_id = self.robot.get_planning_frame( ) self.blockProtectorPose.pose.position = Point( 0.0056, -0.343, -0.51 + self.cruising_height) self.blockProtectorDimension = (0.9906, 0.8382, 0.8636) self.wallPose = PoseStamped() self.wallPose.header.frame_id = self.robot.get_planning_frame() self.wallPose.pose.position = Point(-0.858, -0.343, -0.3048) self.wallDimension = (0.6096, 2, 1.35) self.farWallPose = PoseStamped() self.farWallPose.header.frame_id = self.robot.get_planning_frame() self.farWallPose.pose.position = Point(0.9, -0.343, -0.3048) self.farWallDimension = (0.6096, 2, 3.35) self.frontWallPose = PoseStamped() self.frontWallPose.header.frame_id = self.robot.get_planning_frame() self.frontWallPose.pose.position = Point(0.0056, -0.85, -0.51) self.frontWallDimension = (1, 0.15, 4) self.backWallPose = PoseStamped() self.backWallPose.header.frame_id = self.robot.get_planning_frame() self.backWallPose.pose.position = Point(0.0056, 0.55, -0.51) self.backWallDimension = (1, 0.005, 4) self.rearCameraPose = PoseStamped() self.rearCameraPose.header.frame_id = self.robot.get_planning_frame() self.rearCameraPose.pose.position = Point(0.65, 0.45, -0.51) self.rearCameraDimension = (0.5, 0.5, 2) rospy.sleep(0.5) self.scene.add_box("tui", self.tuiPose, self.tuiDimension) self.scene.add_box("wall", self.wallPose, self.wallDimension) self.scene.add_box("farWall", self.farWallPose, self.farWallDimension) self.scene.add_box("overHead", self.overHeadPose, self.overHeadDimension) self.scene.add_box("backWall", self.backWallPose, self.backWallDimension) self.scene.add_box("frontWall", self.frontWallPose, self.frontWallDimension) self.scene.add_box("rearCamera", self.rearCameraPose, self.rearCameraDimension) def raise_table(self): #raises the table obstacle to protect blocks on the table during transport self.scene.remove_world_object("blockProtector") self.scene.add_box("blockProtector", self.blockProtectorPose, self.blockProtectorDimension) def lower_table(self): #lowers the table to allow grasping into it self.scene.remove_world_object("blockProtector") def init_params(self): try: self.grasp_height = get_ros_param( "GRASP_HEIGHT", "Grasp height defaulting to 0.01") self.drop_height = get_ros_param("DROP_HEIGHT", "Drop height defaulting to 0.07") self.cruising_height = get_ros_param( "CRUISING_HEIGHT", "Cruising height defaulting to 0.1") self.pointing_height = get_ros_param( "POINT_HEIGHT", "Pointing height defaulting to 0.06") except ValueError as e: rospy.loginfo(e) def handle_param_update(self, message): self.init_params() def init_publishers(self): self.calibration_publisher = rospy.Publisher("/calibration_results", CalibrationParams, queue_size=1) 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.joint_trajectory_subscriber = rospy.Subscriber( '/j2s7s300/follow_joint_trajectory/status', GoalStatusArray, self.update_joint_trajectory_state) self.joint_trajectory_goal_subscriber = rospy.Subscriber( '/j2s7s300/follow_joint_trajectory/goal', FollowJointTrajectoryActionGoal, self.update_joint_trajectory_goal) self.joint_trajectory_result_subscriber = rospy.Subscriber( '/j2s7s300/follow_joint_trajectory/result', FollowJointTrajectoryActionResult, self.update_joint_trajectory_result) self.finger_position_subscriber = rospy.Subscriber( '/j2s7s300_driver/out/finger_position', FingerPosition, self.update_finger_position) self.param_update_subscriber = rospy.Subscriber( "/param_update", String, self.handle_param_update) self.moveit_status_subscriber = rospy.Subscriber( '/move_group/status', GoalStatusArray, self.update_move_group_status) self.move_it_feedback_subscriber = rospy.Subscriber( '/move_group/feedback', MoveGroupActionFeedback, self.update_move_group_state) #Topic for getting joint torques rospy.Subscriber('/j2s7s300_driver/out/joint_torques', JointAngles, self.monitorJointTorques) #Topic for getting cartesian force on end effector rospy.Subscriber('/j2s7s300_driver/out/tool_wrench', geometry_msgs.msg.WrenchStamped, self.monitorToolWrench) def init_action_servers(self): self.calibration_server = actionlib.SimpleActionServer( "calibrate_arm", CalibrateAction, self.calibrate, auto_start=False) self.calibration_server.start() self.move_block_server = actionlib.SimpleActionServer( "move_block", MoveBlockAction, self.handle_move_block, auto_start=False) self.move_block_server.start() #self.home_arm_server = actionlib.SimpleActionServer("home_arm", HomeArmAction, self.home_arm) self.move_pose_server = actionlib.SimpleActionServer( "move_pose", MovePoseAction, self.handle_move_pose, auto_start=False) self.move_pose_server.start() 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 = False 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() 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() self.home_arm_kinova() return "done" def custom_home_arm(self): angles_set = [ 629.776062012, 150.076568694, -0.13603515923, 29.8505859375, 0.172727271914, 212.423721313, 539.743164062 ] goal = kinova_msgs.msg.ArmJointAnglesGoal() goal.angles.joint1 = angles_set[0] goal.angles.joint2 = angles_set[1] goal.angles.joint3 = angles_set[2] goal.angles.joint4 = angles_set[3] goal.angles.joint5 = angles_set[4] goal.angles.joint6 = angles_set[5] goal.angles.joint7 = angles_set[6] self.joint_action_client.send_goal(goal) def home_arm_kinova(self): """Takes the arm to the kinova default home if possible """ # self.arm.set_named_target("Home") angles_set = map(np.deg2rad, [ 629.776062012, 150.076568694, -0.13603515923, 29.8505859375, 0.172727271914, 212.423721313, 269.743164062 ]) self.arm.clear_pose_targets() try: self.arm.set_joint_value_target(angles_set) except MoveItCommanderException as e: pass #stupid bug in movegroupcommander wrapper throws an exception when trying to set joint angles try: self.arm.go() return "successful home" except: return "failed to home" # This callback function monitors the Joint Torques and stops the current execution if the Joint Torques exceed certain value def monitorJointTorques(self, torques): if abs(torques.joint1) > 1: return #self.emergency_stop() #Stop arm driver #rospy.sleep(1.0) #self.group.stop() #Stop moveit execution # This callback function monitors the Joint Wrench and stops the current # execution if the Joint Wrench exceeds certain value def monitorToolWrench(self, wrenchStamped): return #toolwrench = abs(wrenchStamped.wrench.force.x**2 + wrenchStamped.wrench.force.y**2 + wrenchStamped.wrench.force.z**2) ##print toolwrench #if toolwrench > 100: # self.emergency_stop() # Stop arm driver 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} """ print(message) pick_x = message.source.x pick_y = message.source.y pick_x_threshold = message.source_x_tolerance pick_y_threshold = message.source_y_tolerance block_id = message.id place_x = message.target.x place_y = message.target.y place_x_threshold = message.target_x_tolerance place_y_threshold = message.target_y_tolerance self.move_block(block_id, pick_x, pick_y, pick_x_threshold, pick_y_threshold, place_x, place_y, place_x_threshold, place_y_threshold, message.block_size) def handle_pick_failure(self, exception): rospy.loginfo("Pick failed, going home.") self.open_gripper() self.home_arm() raise exception def handle_place_failure(self, safe_zone, block_size, exception): #should probably figure out if I'm holding the block first so it doesn't look weird #figure out how to drop the block somewhere safe #pass none and none if you are certain you haven't picked up a block yet if safe_zone is None and block_size is None: self.home_arm() raise exception rospy.loginfo("HANDLING PLACE FAILURE") block_response = json.loads(self.get_block_state('tuio').tui_state) current_block_state = block_response drop_location = self.calculate_drop_location( safe_zone[0]['x'], safe_zone[0]['y'], safe_zone[1]['x_tolerance'], safe_zone[1]['y_tolerance'], current_block_state, block_size, num_attempts=1000) try: arm_drop_location = tuio_to_arm(drop_location.x, drop_location.y, get_tuio_bounds(), get_arm_bounds()) rospy.loginfo("panic arm drop: " + str(arm_drop_location)) self.place_block( Point(arm_drop_location[0], arm_drop_location[1], 0)) except Exception as e: rospy.loginfo( "ERROR: Cannot panic place the block! Get ready to catch it!") self.open_gripper() self.home_arm() raise exception def get_candidate_blocks(self, block_id, pick_x, pick_y, pick_x_tolerance, pick_y_tolerance): block_response = json.loads(self.get_block_state('tuio').tui_state) current_block_state = block_response candidate_blocks = [] print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance, pick_y_tolerance) # get candidate blocks -- blocks with the same id and within the error bounds/threshold given print(current_block_state) for block in current_block_state: print( block, self.check_block_bounds(block['x'], block['y'], pick_x, pick_y, pick_x_tolerance, pick_y_tolerance)) if block['id'] == block_id and self.check_block_bounds( block['x'], block['y'], pick_x, pick_y, pick_x_tolerance, pick_y_tolerance): candidate_blocks.append(block) return candidate_blocks def move_block(self, block_id, pick_x, pick_y, pick_x_tolerance, pick_y_tolerance, place_x, place_y, place_x_tolerance, place_y_tolerance, block_size=None, safe_zone=None, pick_tries=2, place_tries=1, point_at_block=True): if block_size is None: block_size = get_ros_param('DEFAULT_BLOCK_SIZE') block_response = json.loads(self.get_block_state('tuio').tui_state) current_block_state = block_response candidate_blocks = [] print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance, pick_y_tolerance) # get candidate blocks -- blocks with the same id and within the error bounds/threshold given print(current_block_state) # check for a drop location before trying to pick, do this before checking source to prevent cases where we go for a block user # moved while we are checking for a drop location drop_location = self.calculate_drop_location(place_x, place_y, place_x_tolerance, place_y_tolerance, current_block_state, block_size, num_attempts=2000) if drop_location == None: self.handle_place_failure( None, None, Exception("no room in the target zone to drop the block")) # here we are actually building a set of candidate blocks to pick for block in current_block_state: print( block, self.check_block_bounds(block['x'], block['y'], pick_x, pick_y, pick_x_tolerance, pick_y_tolerance)) if block['id'] == block_id and self.check_block_bounds( block['x'], block['y'], pick_x, pick_y, pick_x_tolerance, pick_y_tolerance): candidate_blocks.append(block) # select best block to pick and pick up pick_location = None if len(candidate_blocks) < 1: raise Exception("no block of id " + str(block_id) + " found within the source zone") elif len(candidate_blocks) == 1: pick_location = Point(candidate_blocks[0]['x'], candidate_blocks[0]['y'], 0) else: pick_location = Point(*self.find_most_isolated_block( candidate_blocks, current_block_state)) if point_at_block == True: try: arm_pick_location = tuio_to_arm(pick_location.x, pick_location.y, get_tuio_bounds(), get_arm_bounds()) arm_drop_location = tuio_to_arm(drop_location.x, drop_location.y, get_tuio_bounds(), get_arm_bounds()) self.close_gripper() self.point_at_block(location=Point(arm_pick_location[0], arm_pick_location[1], 0)) self.point_at_block(location=Point(arm_drop_location[0], arm_drop_location[1], 0)) self.home_arm() except Exception as e: rospy.loginfo(str(e)) rospy.loginfo("failed trying to point at block. giving up.") self.home_arm() self.move_block_server.set_succeeded( MoveBlockResult(drop_location)) return for i in range(pick_tries): try: arm_pick_location = tuio_to_arm(pick_location.x, pick_location.y, get_tuio_bounds(), get_arm_bounds()) self.pick_block(location=Point(arm_pick_location[0], arm_pick_location[1], 0), check_grasp=True) break except Exception as e: if i < pick_tries - 1: rospy.loginfo("pick failed and trying again..." + str(e)) else: rospy.loginfo("pick failed and out of attempts..." + str(e)) self.handle_pick_failure(e) if safe_zone == None: if self.safe_zone == None: safe_zone = [{ 'x': pick_x, 'y': pick_y }, { 'x_tolerance': pick_x_tolerance, 'y_tolerance': pick_y_tolerance }] else: safe_zone = self.safe_zone # calculate drop location block_response = json.loads(self.get_block_state('tuio').tui_state) current_block_state = block_response drop_location = self.calculate_drop_location(place_x, place_y, place_x_tolerance, place_y_tolerance, current_block_state, block_size, num_attempts=2000) if drop_location == None: self.handle_place_failure( safe_zone, block_size, Exception("no room in the target zone to drop the block")) rospy.loginfo("tuio drop" + str(drop_location)) for i in range(place_tries): try: arm_drop_location = tuio_to_arm(drop_location.x, drop_location.y, get_tuio_bounds(), get_arm_bounds()) rospy.loginfo("arm drop: " + str(arm_drop_location)) self.place_block( Point(arm_drop_location[0], arm_drop_location[1], 0)) break except Exception as e: if i < place_tries - 1: rospy.loginfo("place failed and trying again..." + str(e)) else: rospy.loginfo("place failed and out of attempts..." + str(e)) # check to see if we've defined a safe zone to drop the blocks self.handle_place_failure(safe_zone, block_size, e) # assume success if we made it this far self.move_block_server.set_succeeded(MoveBlockResult(drop_location)) # check if a certain x, y position is within the bounds of another x,y position @staticmethod def check_block_bounds(x, y, x_origin, y_origin, x_threshold, y_threshold): if x <= x_origin + x_threshold and x >= x_origin - x_threshold \ and y <= y_origin + y_threshold and y >= y_origin - y_threshold: return True return False # calculate the best location to drop the block def calculate_drop_location(self, x, y, x_threshold, y_threshold, blocks, block_size, num_attempts=1000): attempt = 0 x_original, y_original = x, y while (attempt < num_attempts): valid = True for block in blocks: if self.check_block_bounds(block['x'], block['y'], x, y, 0.8 * block_size, block_size): valid = False break if valid: return Point(x, y, 0) else: x = random.uniform(x_original - x_threshold, x_original + x_threshold) y = random.uniform(y_original - y_threshold, y_original + y_threshold) attempt += 1 #if none found in num_attempts, return none return None # candidates should have more than one element in it @staticmethod def find_most_isolated_block(candidates, all_blocks): print(candidates) min_distances = [] # tuples of candidate, distance to closest block for candidate in candidates: min_dist = -1 for block in all_blocks: if block['x'] == candidate['x'] and block['y'] == candidate[ 'y']: continue else: dist = DaArmServer.block_dist(candidate, block) if min_dist == -1 or dist < min_dist: min_dist = dist min_distances.append([candidate, min_dist]) most_isolated, _ = max(min_distances, key=lambda x: x[ 1]) # get most isolated candidate, and min_distance return most_isolated['x'], most_isolated['y'], 0 @staticmethod def block_dist(block_1, block_2): return np.sqrt((block_2['x'] - block_1['x'])**2 + (block_2['y'] - block_1['y'])**2) def update_finger_position(self, message): self.finger_positions = [ message.finger1, message.finger2, message.finger3 ] def check_grasp(self): closed_pos = 0.95 * 6800 distance_from_closed = 0 for fp in self.finger_positions: distance_from_closed += (closed_pos - fp)**2 if np.sqrt(distance_from_closed ) > 130: #this is just some magic number for now return True #if the fingers aren't fully closed, then grasp is good else: return False 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: try: rospy.loginfo("Opening Gripper!!") self.move_fingers(50, 50, 50) except Exception as e: rospy.loginfo("Caught it!!" + str(e)) 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() try: rospy.loginfo("Closing Gripper!!") self.move_fingers(95, 95, 95) except Exception as e: rospy.loginfo("Caught it!!" + str(e)) 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 handle_move_pose(self, message): # takes a geometry_msgs/Pose message self.move_arm_to_pose(message.target.position, message.target.orientation, action_server=self.move_pose_server) self.move_pose_server.set_succeeded() def check_plan_result(self, target_pose, cur_pose): #we'll do a very lenient check, this is to find failures, not error #also only checking position, not orientation rospy.loginfo("checking pose:" + str(target_pose) + str(cur_pose)) if np.abs(target_pose.pose.position.x - cur_pose.pose.position.x) > self.GOAL_TOLERANCE * 2: print("x error too far") return False if np.abs(target_pose.pose.position.y - cur_pose.pose.position.y) > self.GOAL_TOLERANCE * 2: print("y error too far") return False if np.abs(target_pose.pose.position.z - cur_pose.pose.position.z) > self.GOAL_TOLERANCE * 2: print("z error too far") return False print("tolerable error") return True # expects cooridinates for position to be in arm space def move_arm_to_pose(self, position, orientation, delay=0.5, waypoints=[], corrections=4, action_server=None): for i in range(corrections + 1): rospy.loginfo("TRY NUMBER " + str(i)) if len(waypoints) > 0 and i < 1: # 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 rospy.loginfo("PLANNING TO " + str(p)) self.arm.set_pose_target(p) last_traj_goal = self.last_joint_trajectory_goal rospy.loginfo("EXECUTING!") plan = self.arm.go(wait=False) timeout = 5 / 0.001 while self.last_joint_trajectory_goal == last_traj_goal: rospy.sleep(0.001) timeout -= 1 if timeout <= 0: raise (Exception( "Timeout waiting for kinova to accept movement goal." )) rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL") current_goal = self.last_joint_trajectory_goal # then loop until a result for it gets published timeout = 90 / 0.001 while self.last_joint_trajectory_result != current_goal: rospy.sleep(0.001) timeout -= 1 if timeout <= 0: raise (Exception( "Motion took longer than 90 seconds. aborting...")) if self.paused is True: self.arm.stop() # cancel the moveit goals return rospy.loginfo("LEAVING THE WHILE LOOP") # for i in range(corrections): # rospy.loginfo("Correcting the pose") # self.move_joint_angles(angle_set) rospy.sleep(delay) if (self.check_plan_result(p, self.arm.get_current_pose())): break #we're there, no need to retry #rospy.loginfo("OK GOT THROUGH THE PLANNING PHASE") if False: # # 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: pass #action_server.publish_feedback(CalibrateFeedback("Plan Found")) last_traj_goal = self.last_joint_trajectory_goal rospy.loginfo("EXECUTING!") self.arm.execute(plan, wait=False) # this is a bit naive, but I'm going to loop until a new trajectory goal gets published timeout = 5 / 0.001 while self.last_joint_trajectory_goal == last_traj_goal: rospy.sleep(0.001) timeout -= 1 if timeout <= 0: raise (Exception( "Timeout waiting for kinova to accept movement goal." )) rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL") current_goal = self.last_joint_trajectory_goal # then loop until a result for it gets published timeout = 15 / 0.001 while self.last_joint_trajectory_result != current_goal: rospy.sleep(0.001) timeout -= 1 if timeout <= 0: raise (Exception( "Motion took longer than 15 seconds. aborting...")) if self.paused is True: self.arm.stop() # cancel the moveit goals return rospy.loginfo("LEAVING THE WHILE LOOP") # for i in range(corrections): # 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")) pass # 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 update_move_group_status(self, message): if message.status_list: #rospy.loginfo("MoveGroup Status for "+str(message.status_list[0].goal_id.id)+": "+str(message.status_list[0].status)) self.move_group_status = message.status_list[0].status def update_joint_trajectory_state(self, message): # print(message.status_list) if len(message.status_list) > 0: self.joint_trajectory_state = message.status_list[0].status else: self.joint_trajectory_state = 0 def update_joint_trajectory_goal(self, message): #print("goalisasis", message.goal_id.id) self.last_joint_trajectory_goal = message.goal_id.id def update_joint_trajectory_result(self, message): #print("resultisasis", message.status.goal_id.id) self.last_joint_trajectory_result = message.status.goal_id.id def get_grasp_orientation(self, position): #return Quaternion(0, 0, 1/np.sqrt(2), 1/np.sqrt(2)) return Quaternion(-0.707388, -0.706825, 0.0005629, 0.0005633) 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_relative(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 move_z_absolute(self, height): p = self.arm.get_current_pose() p.pose.position.z = height 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 # print('Position: ', position) self.open_gripper() position = Point(location.x, location.y, self.cruising_height) orientation = self.get_grasp_orientation(position) try: self.raise_table() self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) self.lower_table() position = Point(location.x, location.y, self.grasp_height) self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) except Exception as e: current_pose = self.arm.get_current_pose() if current_pose.pose.position.z - self.cruising_height < 0: self.move_z_absolute(self.crusing_height) self.raise_table() raise ( e ) #problem because sometimes we get exception e.g. if we're already there # and it will skip the close if so. if action_server: action_server.publish_feedback() self.close_gripper() self.move_z_absolute(self.cruising_height) #try to wait until we're up to check grasp rospy.sleep(0.5) if check_grasp is True: if (self.check_grasp() is False): print("Grasp failed!") self.raise_table() raise (Exception("grasp failed!")) # for now, but we want to check finger torques # for i in range(retry_attempts): # self.move_z(0.3) self.raise_table() 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 # print('Position: ', position) current_pose = self.arm.get_current_pose() if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct self.move_z_absolute(self.cruising_height) position = Point(location.x, location.y, self.cruising_height) rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " + str(self.drop_height)) orientation = self.get_grasp_orientation(position) try: self.raise_table( ) # only do this as a check in case it isn't already raised self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) self.lower_table() self.move_z_absolute(self.drop_height) except Exception as e: current_pose = self.arm.get_current_pose() if current_pose.pose.position.z - self.cruising_height < 0: self.move_z_absolute(self.cruising_height) self.raise_table() raise (e) if action_server: action_server.publish_feedback() self.open_gripper() self.move_z_absolute(self.cruising_height) self.raise_table() self.close_gripper() def point_at_block(self, location, 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 # print('Position: ', position) current_pose = self.arm.get_current_pose() if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct self.move_z_absolute(self.cruising_height) position = Point(location.x, location.y, self.cruising_height) rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " + str(self.drop_height)) orientation = self.get_grasp_orientation(position) try: self.raise_table( ) # only do this as a check in case it isn't already raised self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) self.lower_table() self.move_z_absolute(self.pointing_height) self.move_z_absolute(self.cruising_height) except Exception as e: current_pose = self.arm.get_current_pose() if current_pose.pose.position.z - self.cruising_height < 0: self.move_z_absolute(self.cruising_height) self.raise_table() raise (e) if action_server: action_server.publish_feedback() self.raise_table() 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. print("moving on...") 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")) try: self.move_arm_to_pose(Point(0.4, -0.4, 0.1), Quaternion(1, 0, 0, 0), corrections=0) except Exception as e: rospy.loginfo("THIS IS TH PRoblem" + str(e)) # 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]
class MoveIt(object): def __init__(self): moveit_commander.roscpp_initialize(sys.argv) self.scene = PlanningSceneInterface() self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.arm = MoveGroupCommander("arm") # self.arm.set_goal_joint_tolerance(0.1) self.gripper = MoveGroupCommander("gripper") # already default self.arm.set_planner_id("RRTConnectkConfigDefault") self.end_effector_link = self.arm.get_end_effector_link() self.arm.allow_replanning(True) self.arm.set_planning_time(5) self.transformer = tf.TransformListener() rospy.sleep(2) # allow some time for initialization of moveit def __del__(self): moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) def _open_gripper(self): joint_trajectory = JointTrajectory() joint_trajectory.header.stamp = rospy.get_rostime() joint_trajectory.joint_names = [ "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2" ] joint_trajectory_point = JointTrajectoryPoint() joint_trajectory_point.positions = [0, 0] joint_trajectory_point.time_from_start = rospy.Duration(5.0) joint_trajectory.points.append(joint_trajectory_point) return joint_trajectory def _close_gripper(self): joint_trajectory = JointTrajectory() joint_trajectory.header.stamp = rospy.get_rostime() joint_trajectory.joint_names = [ "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2" ] joint_trajectory_point = JointTrajectoryPoint() joint_trajectory_point.positions = [1.2, 1.2] joint_trajectory_point.time_from_start = rospy.Duration(5.0) joint_trajectory.points.append(joint_trajectory_point) return joint_trajectory # Template function for creating the Grasps def _create_grasps(self, x, y, z, z_max, rotation): grasps = [] # You can create multiple grasps and add them to the grasps list grasp = Grasp() # create a new grasp # Set the pre grasp posture (the fingers) grasp.pre_grasp_posture = self._open_gripper() # Set the grasp posture (the fingers) grasp.grasp_posture = self._close_gripper() # Set the position of where to grasp grasp.grasp_pose.pose.position.x = x grasp.grasp_pose.pose.position.y = y grasp.grasp_pose.pose.position.z = z # Set the orientation of the end effector q = quaternion_from_euler(math.pi, 0.0, rotation) grasp.grasp_pose.pose.orientation.x = q[0] grasp.grasp_pose.pose.orientation.y = q[1] grasp.grasp_pose.pose.orientation.z = q[2] grasp.grasp_pose.pose.orientation.w = q[3] grasp.grasp_pose.header.frame_id = "m1n6s200_link_base" # Set the pre_grasp_approach grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link grasp.pre_grasp_approach.direction.vector.z = 1.0 grasp.pre_grasp_approach.direction.vector.y = 0.0 grasp.pre_grasp_approach.direction.vector.x = 0.0 grasp.pre_grasp_approach.min_distance = 0.05 grasp.pre_grasp_approach.desired_distance = 0.1 # # Set the post_grasp_approach grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link grasp.post_grasp_retreat.direction.vector.z = -1.0 grasp.post_grasp_retreat.direction.vector.x = 0.0 grasp.post_grasp_retreat.direction.vector.y = 0.0 grasp.post_grasp_retreat.min_distance = 0.05 grasp.post_grasp_retreat.desired_distance = 0.25 grasp.grasp_pose.header.frame_id = "m1n6s200_link_base" # setting the planning frame (Positive x is to the left, negative Y is to the front of the arm) grasps.append( grasp ) # add all your grasps in the grasps list, MoveIT will pick the best one for z_offset in np.arange(z + 0.02, z_max, 0.01): new_grasp = copy.deepcopy(grasp) new_grasp.grasp_pose.pose.position.z = z_offset grasps.append(new_grasp) return grasps # Template function, you can add parameters if needed! def grasp(self, x, y, z, z_max, rotation, size): print '******************* grasp' # Object distance: obj_dist = np.linalg.norm(np.asarray((x, y, z))) if obj_dist > 0.5: rospy.loginfo( "Object too far appart ({} m), skipping pick".format(obj_dist)) return False # Add collision object, easiest to name the object, "object" object_pose = PoseStamped() object_pose.header.frame_id = "m1n6s200_link_base" object_pose.pose.position.x = x object_pose.pose.position.y = y object_pose.pose.position.z = z q = quaternion_from_euler(math.pi, 0.0, rotation) object_pose.pose.orientation.x = q[0] object_pose.pose.orientation.y = q[1] object_pose.pose.orientation.z = q[2] object_pose.pose.orientation.w = q[3] self.scene.add_box("object", object_pose, size) rospy.sleep(0.5) self.clear_octomap() rospy.sleep(1.0) # Create and return grasps # z += size[2]/2 # Focus on the top of the object only # z += size[2]/2 + 0.02 # Focus on the top of the object only grasps = self._create_grasps(x, y, z, z_max, rotation) print '******************************************************************************' result = self.arm.pick( 'object', grasps) # Perform pick on "object", returns result print '******************************************************************************' # self.move_to(x, y, z + 0.15, rotation) if result == MoveItErrorCodes.SUCCESS: print 'Success grasp' return True else: print 'Failed grasp' return False def clear_object(self, x, y, z, z_max, rotation, size): print '******************* clear_object' self.move_to_waypoint() success = self.grasp(x, y, z, z_max, rotation, size) if success: self.move_to_waypoint() success = self.move_to_drop_zone() if success: print 'success move to drop zone' else: print 'failed move to drop zone' self.open_fingers() self.remove_object() rospy.sleep(1.0) self.close_fingers() return success def open_fingers(self): print '******************* open_fingers' self.gripper.set_joint_value_target([0.0, 0.0]) self.gripper.go(wait=True) rospy.sleep(2.0) def close_fingers(self): print '******************* close_fingers' self.gripper.set_joint_value_target([1.3, 1.3]) self.gripper.go(wait=True) rospy.sleep(2.0) def move_to(self, x, y, z, rotation, frame_id="m1n6s200_link_base"): print '******************* move_to' q = quaternion_from_euler(math.pi, 0.0, rotation) pose = PoseStamped() pose.header.frame_id = frame_id pose.pose.position.x = x pose.pose.position.y = y pose.pose.position.z = z pose.pose.orientation.x = q[0] pose.pose.orientation.y = q[1] pose.pose.orientation.z = q[2] pose.pose.orientation.w = q[3] self.arm.set_pose_target(pose, self.end_effector_link) plan = self.arm.plan() success = self.arm.go(wait=True) self.arm.stop() self.arm.clear_pose_targets() return success def move_to_waypoint(self): print '******************* move_to_waypoint' return self.move_to(0.35, 0, 0.25, 1.57) def rtb(self): print '******************* rtb' self.move_to_drop_zone() # pose = PoseStamped() # pose.header.frame_id = 'base_footprint' # pose.pose.position.x = -0.191258927921 # pose.pose.position.y = 0.1849306168113 # pose.pose.position.z = 0.813729734732 # pose.pose.orientation.x = -0.934842026356 # pose.pose.orientation.y = 0.350652799078 # pose.pose.orientation.z = -0.00168532388516 # pose.pose.orientation.w = 0.0557688079539 # # self.arm.set_pose_target(pose, self.end_effector_link) # plan = self.arm.plan() # self.arm.go(wait=True) # self.arm.stop() # self.arm.clear_pose_targets() def move_to_drop_zone(self): print '******************* move_to_drop_zone' pose = PoseStamped() pose.header.frame_id = "m1n6s200_link_base" pose.pose.position.x = 0.2175546259709541 pose.pose.position.y = 0.18347985269448372 pose.pose.position.z = 0.16757751444136426 pose.pose.orientation.x = 0.6934210704552356 pose.pose.orientation.y = 0.6589390059796749 pose.pose.orientation.z = -0.23223137602833943 pose.pose.orientation.w = -0.17616808290725341 self.arm.set_pose_target(pose, self.end_effector_link) plan = self.arm.plan() success = self.arm.go(wait=True) self.arm.stop() self.arm.clear_pose_targets() return success def print_position(self): pose = self.arm.get_current_pose() self.transformer.waitForTransform("m1n6s200_link_base", "base_footprint", rospy.Time.now(), rospy.Duration(10)) eef_pose = self.transformer.transformPose("m1n6s200_link_base", pose) orientation = eef_pose.pose.orientation orientation = [ orientation.x, orientation.y, orientation.z, orientation.w ] euler = euler_from_quaternion(orientation) print "z:", eef_pose.pose.position.x print "y:", eef_pose.pose.position.y print "z:", eef_pose.pose.position.z print "yaw (degrees):", math.degrees(euler[2]) def remove_object(self): self.scene.remove_attached_object(self.end_effector_link, "object") self.scene.remove_world_object("object") rospy.loginfo("Object removed")
def __init__(self): """ Setup ROS communication between the Kinect and Baxter :return: """ # Initialise variables self.rgb_img = None self.depth_img = None self.marker_box = None self.marker_center_x = None self.marker_center_y = None self.marker_depth = None self.bridge = CvBridge() # First initialize moveit_commander and rospy. print "============ Initialising Baxter" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('can_node', anonymous=True) self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.left_arm = moveit_commander.MoveGroupCommander("left_arm") self.right_arm = moveit_commander.MoveGroupCommander("right_arm") self.right_arm_navigator = Navigator('right') # Setup grippers self.right_gripper = baxter_interface.Gripper('right') # Setup the table in the scene scene = PlanningSceneInterface() self.scene_pub = rospy.Publisher('/move_group/monitored_planning_scene', PlanningScene) table_id = 'table' scene.remove_world_object(table_id) rospy.sleep(1) table_ground = -0.3 table_size = [4.0, 4.6, 0.1] table_pose = PoseStamped() table_pose.header.frame_id = self.robot.get_planning_frame() table_pose.pose.position.x = 0.7 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) # Create a dictionary to hold object colors self.colors = dict() self.setColor(table_id, 0.8, 0, 0, 1.0) self.sendColors() self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=10) # We can get a list of all the groups in the robot print "============ Right Pose:" print self.right_arm.get_current_pose() print "============ Left Pose:" print self.left_arm.get_current_pose() # Setup the subscribers and publishers self.rgb_sub = rospy.Subscriber("/camera/rgb/image_rect_color", Image, self.callback_rgb) self.points_sub = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.callback_points) self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=5) self.screen_pub = rospy.Publisher('robot/xdisplay', Image, latch=True, queue_size=10) self.right_hand_range_pub = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self.callback_range, queue_size=1) self.left_cam_sub = rospy.Subscriber("/cameras/left_hand_camera/image", Image, self.callback_left_hand)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') rospy.Subscriber('/aruco_single/pose', PoseStamped, self.pose_cb,queue_size=1) scene=PlanningSceneInterface() self.scene_pub=rospy.Publisher('planning_scene',PlanningScene) self.colors=dict() rospy.sleep(1) arm=MoveGroupCommander('arm') #gripper=MoveGroupCommander('gripper') end_effector_link=arm.get_end_effector_link() arm.set_goal_position_tolerance(0.005) arm.set_goal_orientation_tolerance(0.025) arm.allow_replanning(True) #gripper.set_goal_position_tolerance(0.005) #gripper.set_goal_orientation_tolerance(0.025) #gripper.allow_replanning(True) reference_frame='base_link' arm.set_pose_reference_frame(reference_frame) arm.set_planning_time(5) #scene planning table_id='table' #cylinder_id='cylinder' box2_id='box2' target_id='target_object' #scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(table_id) scene.remove_world_object(target_id) rospy.sleep(2) table_ground=0.59 table_size=[0.5,1,0.01] #box1_size=[0.1,0.05,0.03] box2_size=[0.15,0.15,0.02] r_tool_size=[0.05,0.04,0.22] l_tool_size=[0.05,0.04,0.22] target_size=[0.05,0.05,0.1] table_pose=PoseStamped() table_pose.header.frame_id=reference_frame table_pose.pose.position.x=0.7 table_pose.pose.position.y=0.0 table_pose.pose.position.z=table_ground+table_size[2]/2.0 table_pose.pose.orientation.w=1.0 scene.add_box(table_id,table_pose,table_size) ''' box1_pose=PoseStamped() box1_pose.header.frame_id=reference_frame box1_pose.pose.position.x=0.7 box1_pose.pose.position.y=-0.2 box1_pose.pose.position.z=table_ground+table_size[2]+box1_size[2]/2.0 box1_pose.pose.orientation.w=1.0 scene.add_box(box1_id,box1_pose,box1_size) ''' box2_pose=PoseStamped() box2_pose.header.frame_id=reference_frame box2_pose.pose.position.x=0.55 box2_pose.pose.position.y=-0.12 box2_pose.pose.position.z=table_ground+table_size[2]+box2_size[2]/2.0 box2_pose.pose.orientation.w=1.0 scene.add_box(box2_id,box2_pose,box2_size) target_pose=PoseStamped() target_pose.header.frame_id=reference_frame target_pose.pose.position.x=0.58 target_pose.pose.position.y=0.05 target_pose.pose.position.z=table_ground+table_size[2]+target_size[2]/2.0 target_pose.pose.orientation.x=0 target_pose.pose.orientation.y=0 target_pose.pose.orientation.z=0 target_pose.pose.orientation.w=1 scene.add_box(target_id,target_pose,target_size) #left gripper l_p=PoseStamped() l_p.header.frame_id=end_effector_link l_p.pose.position.x=0.00 l_p.pose.position.y=0.06 l_p.pose.position.z=0.11 l_p.pose.orientation.w=1 scene.attach_box(end_effector_link,'l_tool',l_p,l_tool_size) #right gripper r_p=PoseStamped() r_p.header.frame_id=end_effector_link r_p.pose.position.x=0.00 r_p.pose.position.y= -0.06 r_p.pose.position.z=0.11 r_p.pose.orientation.w=1 scene.attach_box(end_effector_link,'r_tool',r_p,r_tool_size) #grasp g_p=PoseStamped() g_p.header.frame_id=end_effector_link g_p.pose.position.x=0.00 g_p.pose.position.y= -0.00 g_p.pose.position.z=0.025 g_p.pose.orientation.w=0.707 g_p.pose.orientation.x=0 g_p.pose.orientation.y=-0.707 g_p.pose.orientation.z=0 self.setColor(table_id,0.8,0,0,1.0) #self.setColor(box1_id,0.8,0.4,0,1.0) self.setColor(box2_id,0.8,0.4,0,1.0) self.setColor('r_tool',0.8,0,0) self.setColor('l_tool',0.8,0,0) self.setColor('target_object',0,1,0) self.sendColors() #motion planning arm.set_named_target("initial_arm") arm.go() rospy.sleep(2) grasp_pose=target_pose grasp_pose.pose.position.x-=0.15 #grasp_pose.pose.position.z= grasp_pose.pose.orientation.x=0 grasp_pose.pose.orientation.y=0.707 grasp_pose.pose.orientation.z=0 grasp_pose.pose.orientation.w=0.707 #arm.set_start_state_to_current_state() ''' arm.set_pose_target(grasp_pose,end_effector_link) traj=arm.plan() arm.execute(traj) print arm.get_current_joint_values() ''' pre_joint_state=[0.16588150906995922, 1.7060146047438647, -0.00961761728757362, 1.8614674591892713, -2.9556667436476847, 1.7432451233907822, 3.1415] arm.set_joint_value_target(pre_joint_state) traj=arm.plan() arm.execute(traj) rospy.sleep(2) arm.shift_pose_target(0,0.09,end_effector_link) arm.go() rospy.sleep(2) scene.attach_box(end_effector_link,target_id,g_p,target_size) rospy.sleep(2) #grasping is over , from now is pouring arm.shift_pose_target(2,0.15,end_effector_link) arm.go() rospy.sleep(2) joint_state_1=arm.get_current_joint_values() joint_state_1[0]-=0.17 arm.set_joint_value_target(joint_state_1) arm.go() rospy.sleep(1) joint_state_2=arm.get_current_joint_values() joint_state_2[6]-=1.8 arm.set_joint_value_target(joint_state_2) arm.go() rospy.sleep(1) #print arm.get_current_joint_values() #pouring test for i in range(1,5): joint_state_2[6]+=0.087 arm.set_joint_value_target(joint_state_2) arm.go() time.sleep(0.05) joint_state_2[6]-=0.087 arm.set_joint_value_target(joint_state_2) arm.go() time.sleep(0.05) print i joint_state_2[6]+=1.8 arm.set_joint_value_target(joint_state_2) arm.go() rospy.sleep(2) joint_state_1[0]+=0.17 arm.set_joint_value_target(joint_state_1) arm.go() rospy.sleep(1) arm.shift_pose_target(2,-0.15,end_effector_link) arm.go() rospy.sleep(2) scene.remove_attached_object(end_effector_link,target_id) rospy.sleep(2) arm.set_named_target("initial_arm") arm.go() rospy.sleep(2) #remove and shut down scene.remove_attached_object(end_effector_link,'l_tool') rospy.sleep(1) scene.remove_attached_object(end_effector_link,'r_tool') rospy.sleep(1) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_obstacles_demo') # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('ur_arm') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置场景物体的名称 table_id = 'table' box1_id = 'box1' box2_id = 'box2' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(2) # 设置桌面的高度 table_ground = 0.25 # 设置table、box1和box2的三维尺寸 table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.26 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.21 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = 0.19 box2_pose.pose.position.y = 0.15 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # 设置机械臂的运动目标位置,位于桌面之上两个盒子之间 target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.2 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 target_pose.pose.orientation.w = 1.0 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(2) # 设置机械臂的运动目标位置,进行避障规划 target_pose2 = PoseStamped() target_pose2.header.frame_id = reference_frame target_pose2.pose.position.x = 0.2 target_pose2.pose.position.y = -0.25 target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 target_pose2.pose.orientation.w = 1.0 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose2, end_effector_link) arm.go() rospy.sleep(2) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening")] self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening")] self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral")] self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten") # We need a tf listener to convert poses into arm reference base self.tf_listener = tf.TransformListener() # 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 gripper poses self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.04) arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 3 # Set a limit on the number of place attempts max_place_attempts = 3 rospy.loginfo("Scaling for MoveIt timeout=" + str( rospy.get_param( '/move_group/trajectory_execution/allowed_execution_duration_scaling' ))) # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name #table_id = 'table' We also remove the table object in order to run a test #box1_id = 'box1' These boxes are commented as we do not need them #box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run #scene.remove_world_object(table_id) #scene.remove_world_object(box1_id) These boxes are commented as we do not need them #scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "arm_up" pose stored in the SRDF file rospy.loginfo("Set Arm: right_up") arm.set_named_target('right_up') if arm.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Move the gripper to the closed position rospy.loginfo("Set Gripper: Close " + str(self.gripper_closed)) gripper.set_joint_value_target(self.gripper_closed) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Move the gripper to the neutral position rospy.loginfo("Set Gripper: Neutral " + str(self.gripper_neutral)) gripper.set_joint_value_target(self.gripper_neutral) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Move the gripper to the open position rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened)) gripper.set_joint_value_target(self.gripper_opened) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Set the height of the table off the ground #table_ground = 0.4 # Set the dimensions of the scene objects [l, w, h] #table_size = [0.2, 0.7, 0.01] #box1_size = [0.1, 0.05, 0.05] commented for the same reasons as previously #box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [ 0.018, 0.018, 0.018 ] #[0.02, 0.005, 0.12] original object dimensions in meters # Add a table top and two boxes to the scene #table_pose = PoseStamped() #table_pose.header.frame_id = REFERENCE_FRAME #table_pose.pose.position.x = 0.36 #table_pose.pose.position.y = 0.0 #table_pose.pose.position.z = table_ground + table_size[2] -0.08 / 2.0 #0.4+0.01/2 aka table_ground + table_size[2] + target_size[2] / 2.0 #table_pose.pose.orientation.w = 1.0 #scene.add_box(table_id, table_pose, table_size) #box1_pose = PoseStamped() These two blocks of code are commented as they assign postion to unwanted boxes #box1_pose.header.frame_id = REFERENCE_FRAME #box1_pose.pose.position.x = table_pose.pose.position.x - 0.04 #box1_pose.pose.position.y = 0.0 #box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 #box1_pose.pose.orientation.w = 1.0 #scene.add_box(box1_id, box1_pose, box1_size) #box2_pose = PoseStamped() #box2_pose.header.frame_id = REFERENCE_FRAME #box2_pose.pose.position.x = table_pose.pose.position.x - 0.06 #box2_pose.pose.position.y = 0.2 #box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 #box2_pose.pose.orientation.w = 1.0 #scene.add_box(box2_id, box2_pose, box2_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = -0.03 #table_pose.pose.position.x - 0.03 target_pose.pose.position.y = 0.1 target_pose.pose.position.z = 0.4 + 0.01 + 0.018 - 0.08 / 2 #table_ground + table_size[2] + target_size[2] / 2.0 table_ground + table_size[2] + target_size[2] -0.08 / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table red and the boxes orange #self.setColor(table_id, 0.8, 0, 0, 1.0) #self.setColor(box1_id, 0.8, 0.4, 0, 1.0) #self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object #arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = -0.03 #table_pose.pose.position.x - 0.03 place_pose.pose.position.y = -0.15 place_pose.pose.position.z = 0.4 + 0.01 + 0.018 - 0.08 / 2 #table_ground + table_size[2] + target_size[2] -0.08 / 2.0 0.4+0.01+0.018/2 aka table_ground + table_size[2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten]) # Track success/failure and number of attempts for pick operation result = MoveItErrorCodes.FAILURE n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: rospy.loginfo("Pick attempt #" + str(n_attempts)) for grasp in grasps: # Publish the grasp poses so they can be viewed in RViz self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) result = arm.pick(target_id, grasps) if result == MoveItErrorCodes.SUCCESS: break n_attempts += 1 rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: rospy.loginfo(" Pick: Done!") # Generate valid place poses places = self.make_places(place_pose) success = False #from here we are forcing the success cases by stting and making it always true it makes an error appear and the arm stays to the middle position n_attempts = 0 # Repeat until we succeed or run out of attempts while not success and n_attempts < max_place_attempts: #while not has been removed added = after operator < rospy.loginfo("Place attempt #" + str(n_attempts)) for place in places: # Publish the place poses so they can be viewed in RViz self.gripper_pose_pub.publish(place) rospy.sleep(0.2) success = arm.place(target_id, place) break if success: break n_attempts += 1 rospy.sleep(0.2) if not success: rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.") else: #end of forcing rospy.loginfo(" Place: Done!") else: rospy.logerr("Pick operation failed after " + str(n_attempts) + " attempts.") # Return the arm to the "resting" pose stored in the SRDF file (passing through right_up) arm.set_named_target('right_up') arm.go() arm.set_named_target('resting') arm.go() # Open the gripper to the neutral position gripper.set_joint_value_target(self.gripper_neutral) gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
from control_msgs.msg import GripperCommandAction, FollowJointTrajectoryAction from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse if __name__ == '__main__': roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() arm = MoveGroupCommander("manipulator") eef = MoveGroupCommander("gripper") rospy.sleep(1) # clean the scene scene.remove_world_object("pole") scene.remove_world_object("table") scene.remove_world_object("part") client = actionlib.SimpleActionClient('gripper_controller/gripper_action', GripperCommandAction) if not 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!') # publish a demo scene k = PoseStamped() k.header.frame_id = robot.get_planning_frame() k.pose.position.x = 0 k.pose.position.y = 0 k.pose.position.z = -0.05 - 0.025
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)
class Pick_Place: def __init__(self): self.object_list = {} self.arm = moveit_commander.MoveGroupCommander("irb_120") self.gripper = moveit_commander.MoveGroupCommander("robotiq_85") self.arm.set_goal_tolerance(0.05) self.gripper.set_goal_tolerance(0.02) self.scene = PlanningSceneInterface() self.robot = RobotCommander() rospy.sleep(1) self.add_objects() self.add_table() #self.add_ground() self.approach_retreat_desired_dist = 0.2 self.approach_retreat_min_dist = 0.1 rospy.sleep(1.0) # pick up object in pose def pickup(self, object_name, pose): grasps = self.generate_grasps(object_name, pose) self.arm.pick(object_name, grasps) #self.gripper.stop() rospy.loginfo('Pick up successfully') self.arm.detach_object(object_name) self.clean_scene(object_name) #rospy.sleep(1) # place object to goal pose def place(self, pose): self.move_pose_arm(pose) rospy.sleep(1) # pose.position.z -= 0.1 # self.move_pose_arm(pose) waypoints = [] wpose = self.arm.get_current_pose().pose wpose.position.z -= 0.15 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.move_joint_hand(0) rospy.sleep(1) # pose.position.z += 0.1 # self.move_pose_arm(pose) waypoints = [] wpose = self.arm.get_current_pose().pose wpose.position.z += 0.15 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) rospy.loginfo('Place successfully') def get_object_pose(self, object_name): pose = self.object_list[object_name].pose return pose def clean_scene(self, object_name): self.scene.remove_world_object(object_name) def add_ground(self): self.clean_scene("ground") 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.01 size = (5, 5, 0.02) #self.scene.add_box("ground",p, size) rospy.sleep(2) def add_table(self): self.clean_scene("table") p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.8 p.pose.position.y = 0.0 p.pose.position.z = 0.1 size = (0.8, 1.5, 0.028) self.scene.add_box("table", p, size) rospy.sleep(2) def add_objects(self): p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() # add box name = "box" self.clean_scene(name) p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.025 + 0.115 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) size = (0.05, 0.05, 0.05) self.scene.add_box(name, p, size) height = 0.05 width = 0.05 shape = "cube" color = "red" self.object_list[name] = Object(p.pose, height, width, shape, color) print("add box") rospy.sleep(1) p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() # add cylinder name = "cylinder" self.clean_scene(name) p.pose.position.x = 0.5 p.pose.position.y = 0.2 p.pose.position.z = 0.05 + 0.115 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) height = 0.1 radius = 0.03 self.scene.add_cylinder(name, p, height, radius) height = 0.1 width = 0.03 shape = "cylinder" color = "green" self.object_list[name] = Object(p.pose, height, width, shape, color) print("add cylinder") rospy.sleep(1) p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() # add sphere name = "ball" self.clean_scene(name) p.pose.position.x = 0.5 p.pose.position.y = -0.2 p.pose.position.z = 0.03 + 0.115 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) radius = 0.03 self.scene.add_sphere(name, p, radius) height = 0.03 width = 0.03 shape = "sphere" color = "red" self.object_list[name] = Object(p.pose, height, width, shape, color) print("add ball") rospy.sleep(1) #print(self.object_list) 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 back_to_home(self): self.move_joint_arm(0, 0, 0, 0, 0, 0) self.move_joint_hand(0) 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 # Inverse Kinematics: Move the robot arm to desired pose def move_pose_arm(self, pose_goal): 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() # 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, name, pose): grasps = [] now = rospy.Time.now() angle = 0 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) # 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.5 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.5 grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle) grasp.grasp_pose.pose.orientation = Quaternion(*q) 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() if name == "box": traj.positions.append(0.4) elif name == "ball": traj.positions.append(0.3) elif name == "cylinder": traj.positions.append(0.3) #traj.velocities.append(0.2) #traj.effort.append(100) traj.time_from_start = rospy.Duration.from_sec(5.0) grasp.grasp_posture.points.append(traj) grasps.append(grasp) return grasps # Implement the main algorithm here def MyAlgorithm(self): self.back_to_home() # pick cylinder object_name = "cylinder" pose = self.get_object_pose(object_name) print(pose.position.y) pose.position.z += 0.16 self.pickup(object_name, pose) roll = 0.0 pitch = numpy.deg2rad(90.0) yaw = 0.0 x = 0 y = 0.6 z = pose.position.z + 0.01 place_pose = self.pose2msg(roll, pitch, yaw, x, y, z) self.place(place_pose) self.back_to_home() # pick box object_name = "box" pose = self.get_object_pose(object_name) print(pose.position.y) pose.position.z += 0.15 self.pickup(object_name, pose) roll = 0.0 pitch = numpy.deg2rad(90.0) yaw = 0.0 x = 0.1 y = 0.6 z = pose.position.z + 0.01 place_pose = self.pose2msg(roll, pitch, yaw, x, y, z) self.place(place_pose) self.back_to_home() # pick ball object_name = "ball" pose = self.get_object_pose(object_name) print(pose.position.y) pose.position.z += 0.14 self.pickup(object_name, pose) roll = 0.0 pitch = numpy.deg2rad(90.0) yaw = 0.0 x = -0.1 y = 0.6 z = pose.position.z + 0.01 place_pose = self.pose2msg(roll, pitch, yaw, x, y, z) self.place(place_pose) self.back_to_home()
class Pick_Place: def __init__(self): self.objects_name = ["cylinder", "box", "sphere"] self.object_width = 0.03 self.arm = moveit_commander.MoveGroupCommander("irb_120") self.gripper = moveit_commander.MoveGroupCommander("robotiq_85") self.arm.set_goal_tolerance(0.2) self.gripper.set_goal_tolerance(0.05) self.scene = PlanningSceneInterface() self.robot = RobotCommander() self.add_ground() rospy.sleep(1.0) for i in range(3): rospy.loginfo("Moving arm to HOME point") self.move_pose_arm(0, 1.57, 0, 0.4, 0, 0.6) rospy.sleep(1) self.object_name = self.objects_name[i] self.scene.remove_world_object(self.object_name) self.pose_object, dx, dy, dz = self.add_object(self.object_name) rospy.sleep(1.0) self.pose_object.position.x += dx self.pose_object.position.y += dy self.pose_object.position.z += dz print(self.objects_name[i], self.pose_object.position) self.approach_retreat_desired_dist = 0.2 self.approach_retreat_min_dist = 0.1 print("start pick and place") # Pick grasps = self.generate_grasps(self.object_name, self.pose_object) self.arm.pick(self.object_name, grasps) self.gripper.stop() rospy.loginfo('Pick up successfully') self.arm.detach_object(self.object_name) self.clean_scene(self.object_name) rospy.sleep(1) curr_pose = self.arm.get_current_pose().pose x = curr_pose.position.x y = curr_pose.position.y z = curr_pose.position.z # quaternion = (curr_pose.orientation.x, # curr_pose.orientation.y, # curr_pose.orientation.z, # curr_pose.orientation.w) # euler = euler_from_quaternion(quaternion) # roll = euler[0] # pitch = euler[1] # yaw = euler[2] roll = 0.0 pitch = numpy.deg2rad(90.0) yaw = 0.0 z += 0.1 self.move_pose_arm(roll, pitch, yaw, x, y, z) x = -curr_pose.position.y y = 0.6 z -= 0.1 #z = self.pose_object.position.z+0.05 # if self.object_name == "cylinder": # yaw = numpy.deg2rad(90.0) #z+= 0.001 self.move_pose_arm(roll, pitch, yaw, x, y, z) rospy.sleep(0.5) z -= 0.1 self.move_pose_arm(roll, pitch, yaw, x, y, z) self.move_joint_hand(0) #if self.object_name == "cylinder": z += 0.1 self.move_pose_arm(roll, pitch, yaw, x, y, z) # target_pose = curr_pose # target_pose.position.x = x # target_pose.position.y = y # target_pose.position.z = z # q = quaternion_from_euler(roll, pitch, yaw) # target_pose.orientation = Quaternion(*q) # place = self.generate_places(target_pose) # self.arm.place(self.object_name, place) # self.arm.detach_object(self.object_name) # self.clean_scene(self.object_name) rospy.loginfo('Place successfully') rospy.loginfo("Moving arm to HOME point") self.move_pose_arm(0, 0.8, 0, 0.4, 0, 0.6) rospy.sleep(1) def clean_scene(self, object_name): self.scene.remove_world_object(object_name) def add_ground(self): 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.01 size = (5, 5, 0.02) self.scene.add_box("ground", p, size) def add_object(self, name): p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() dx = 0 dy = 0 dz = 0 if name == "box": p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.015 + 0.115 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) dz = 0.16 elif name == "cylinder": p.pose.position.x = 0.5 p.pose.position.y = 0.2 p.pose.position.z = 0.05 + 0.115 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) height = 0.1 radius = 0.03 self.scene.add_cylinder(name, p, height, radius) dz = 0.16 #dx = -0.135 elif name == "sphere": p.pose.position.x = 0.5 p.pose.position.y = -0.2 p.pose.position.z = 0.03 + 0.115 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) radius = 0.03 self.scene.add_sphere(name, p, radius) dz = 0.15 return p.pose, dx, dy, dz 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) 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, name, pose): grasps = [] now = rospy.Time.now() #for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(10.0)): # Create place location: angle = 0 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) # 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 # if name != "cylinder": q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle) grasp.grasp_pose.pose.orientation = Quaternion(*q) # else: # #grasp.pre_grasp_approach.direction.vector.z = -0.05 # grasp.pre_grasp_approach.direction.vector.x = 0.1 # #grasp.post_grasp_retreat.direction.vector.z = 0.05 # grasp.post_grasp_retreat.direction.vector.x = -0.1 grasp.max_contact_force = 100 grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() traj.positions.append(0.03) 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() if name == "box": traj.positions.append(0.57) elif name == "sphere": traj.positions.append(0.3) else: traj.positions.append(0.3) #traj.velocities.append(0.2) traj.effort.append(800) traj.time_from_start = rospy.Duration.from_sec(5.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 __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_pick_and_place_demo') # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # 创建一个发布抓取姿态的发布者 self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander(GROUP_NAME_ARM) # 初始化需要使用move group控制的机械臂中的gripper group gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame(REFERENCE_FRAME) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置pick和place阶段的最大尝试次数 max_pick_attempts = 5 max_place_attempts = 5 rospy.sleep(2) # 设置场景物体的名称 table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) # 移除场景中之前与机器臂绑定的物体 scene.remove_attached_object(GRIPPER_FRAME, target_id) rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() # 控制夹爪张开 gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() rospy.sleep(1) # 设置桌面的高度 table_ground = 0.2 # 设置table、box1和box2的三维尺寸[长, 宽, 高] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.35 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.31 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[ 2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.29 box2_pose.pose.position.y = -0.4 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # 设置目标物体的尺寸 target_size = [0.04, 0.04, 0.05] # 设置目标物体的位置,位于桌面之上两个盒子之间 target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.32 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # 将抓取的目标物体加入场景中 scene.add_box(target_id, target_pose, target_size) # 将目标物体设置为黄色 self.setColor(target_id, 0.9, 0.9, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # 设置支持的外观 arm.set_support_surface_name(table_id) # 设置一个place阶段需要放置物体的目标位置 place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.32 place_pose.pose.position.y = -0.2 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # 将目标位置设置为机器人的抓取目标位置 grasp_pose = target_pose # 生成抓取姿态 grasps = self.make_grasps(grasp_pose, [target_id]) # 将抓取姿态发布,可以在rviz中显示 for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # 追踪抓取成功与否,以及抓取的尝试次数 result = None n_attempts = 0 # 重复尝试抓取,直道成功或者超多最大尝试次数 while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = arm.pick(target_id, grasps) rospy.sleep(0.2) # 如果pick成功,则进入place阶段 if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # 生成放置姿态 places = self.make_places(place_pose) # 重复尝试放置,直道成功或者超多最大尝试次数 while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() # 控制夹爪回到张开的状态 gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_obstacles_demo') # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) rospy.Subscriber("chatter", Float64MultiArray, callback) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') gripper = MoveGroupCommander('gripper') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) gripper.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.2) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置场景物体的名称 table_id = 'table' # cy_id = 'cy' box1_id = 'box1' box2_id = 'box2' box3_id = 'box3' sphere_id = 'sphere' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(box3_id) scene.remove_world_object(sphere_id) rospy.sleep(1) #控制机械臂先回到初始化位置 arm.set_named_target('init') arm.go() rospy.sleep(1) # arm.set_named_target('start') # arm.go() # rospy.sleep(1) gripper.set_joint_value_target([0.05]) gripper.go() rospy.sleep(0) # 设置桌面的高度 table_ground = 0.37 # 设置table、box1和box2的三维尺寸 table_size = [0.2, 0.3, 0.01] box1_size = [0.01, 0.01, 0.19] box2_size = [0.01, 0.01, 0.19] box3_size = [0.005, 0.01, 0.3] sphere_R = 0.01 error = 0.03 # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = -table_size[0] / 2.0 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) #scene.add_cylinder box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = -0.09 box1_pose.pose.position.y = table_size[0] / 2.0 box1_pose.pose.position.z = 0.18 + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = -0.09 box2_pose.pose.position.y = -table_size[0] / 2.0 box2_pose.pose.position.z = 0.18 + box1_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # box3_pose = PoseStamped() # box3_pose.header.frame_id = reference_frame # box3_pose.pose.position.x = pos_aim[0] # box3_pose.pose.position.y = pos_aim[1] # box3_pose.pose.position.z = box3_size[2]/2.0+0.1 # box3_pose.pose.orientation.w = 1.0 # scene.add_box(box3_id, box3_pose, box3_size) sphere_pose = PoseStamped() sphere_pose.header.frame_id = reference_frame sphere_pose.pose.position.x = pos_aim[0] + 0 sphere_pose.pose.position.y = pos_aim[1] sphere_pose.pose.position.z = pos_aim[2] sphere_pose.pose.orientation.w = 1.0 scene.add_sphere(sphere_id, sphere_pose, sphere_R) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0, 0, 0, 1) # self.setColor(table_id, 0.8, 0, 0, 1.0) # self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box1_id, 1, 1, 1, 1.0) self.setColor(box2_id, 1, 1, 1, 1.0) self.setColor(box3_id, 1, 1, 1, 1.0) self.setColor(sphere_id, 0.8, 0, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # rospy.INFO("waiting...") # if(Recive_FLAG==1): # rospy.INFO("OK!") # 设置机械臂的运动目标位置 target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = pos_aim[0] - error target_pose.pose.position.y = pos_aim[1] target_pose.pose.position.z = pos_aim[2] # target_pose.pose.orientation.w = 1.0 #####0.3 # 0.2 0.2 0.2 0.15 0.15 # 0.12 0.11 0.12 0.15 0.15 # 0.30 0.245 0.35 0.35 0.25 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) gripper.set_joint_value_target([0.03]) gripper.go() rospy.sleep(1) # # 控制机械臂终端向x移动5cm arm.shift_pose_target(0, 0.01, end_effector_link) arm.go() rospy.sleep(1) gripper.set_joint_value_target([0.05]) gripper.go() rospy.sleep(1) # # 设置机械臂的运动目标位置,进行避障规划 # target_pose2 = PoseStamped() # target_pose2.header.frame_id = reference_frame # target_pose2.pose.position.x = 0.15 # target_pose2.pose.position.y = 0 #-0.25 # target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 # target_pose2.pose.orientation.w = 1.0 # # 控制机械臂运动到目标位置 # arm.set_pose_target(target_pose2, end_effector_link) # arm.go() # rospy.sleep(2) #控制机械臂回到初始化位置 arm.set_named_target('init') arm.go() # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) rospy.spin()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('SceneSetup') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Set the reference frame for pose targets reference_frame = 'rack1' table_id = 'table' bowl_id = 'bowl' pitcher_id = 'pitcher' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(bowl_id) scene.remove_world_object(pitcher_id) # Set the height of the table off the ground table_ground = 0.5762625 # Set the length, width and height of the table and boxes table_size = [1.0128, 0.481, 0.0238125] table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0 table_pose.pose.position.y = 0.847725 table_pose.pose.position.z = table_ground # Set the height of the bowl bowl_ground = 0.57816875 bowl_pose = PoseStamped() bowl_pose.header.frame_id = reference_frame bowl_pose.pose.position.x = 0 bowl_pose.pose.position.y = 0.847725 bowl_pose.pose.position.z = bowl_ground # Set the height of the pitcher pitcher_ground = 0.57816875 pitcher_pose = PoseStamped() pitcher_pose.header.frame_id = reference_frame pitcher_pose.pose.position.x = 0.4 pitcher_pose.pose.position.y = 0.847725 pitcher_pose.pose.position.z = pitcher_ground pitcher_pose.pose.orientation.w = -0.5 pitcher_pose.pose.orientation.z = 0.707 # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0.4, 0, 1.0) self.setColor(bowl_id, 0, 0.4, 0.8, 1.0) self.setColor(pitcher_id, 0, 0.4, 0.8, 1.0) self.sendColors() scene.add_box(table_id, table_pose, table_size) scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl') #scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class PlanningSceneHelper: def __init__(self, package=None, mesh_folder_path=None): # interface to the planning and collision scenes self.psi = PlanningSceneInterface() # self.scene_pub = Publisher("/collision_object", CollisionObject, # queue_size=1, latch=True) self.vis_pub = Publisher("/collision_scene", MarkerArray, queue_size=10) self.marker_array = MarkerArray() sleep(1.0) if package is not None and mesh_folder_path is not None: # Build folder path for use in load rospack = RosPack() self.package = package self.mesh_folder_path = mesh_folder_path self.package_path = rospack.get_path(package) self.folder_path = os.path.join(self.package_path, mesh_folder_path)\ + os.sep else: loginfo( "Missing package or mesh folder path supplied to planning_scene_helper; no meshes can be added" ) # Create dict of objects, for removing markers later self.objects = {} self.next_id = 1 # Loads the selected mesh file into collision scene at the desired location. def add_mesh(self, object_id, frame, filename, visual=None, pose=None, position=None, orientation=None, rpy=None, color=None): if self.package is None: logwarn("No package was provided; no meshes can be loaded.") if visual is None: visual = filename filename = self.folder_path + filename stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy) try: self.psi.add_mesh(object_id, stamped_pose, filename) loginfo("Loaded " + object_id + " from" + filename + " as collision object.") marker = self.make_marker_stub(stamped_pose, color=color) marker.type = Marker.MESH_RESOURCE # marker.mesh_resource = "file:/" + self.folder_path + visual marker.mesh_resource = "package://" + self.package + os.sep + self.mesh_folder_path + os.sep + visual self.marker_array = self.make_new_marker_array_msg() self.marker_array.markers.append(marker) self.vis_pub.publish(self.marker_array) self.objects[object_id] = marker loginfo("Loaded " + object_id + " from " + marker.mesh_resource + " as marker.") except ROSException as e: loginfo("Problem loading " + object_id + " collision object: " + str(e)) except AssimpError as ae: loginfo("Problem loading " + object_id + " collision object: " + str(ae)) def add_cylinder(self, object_id, frame, size, pose=None, position=None, orientation=None, rpy=None, color=None): try: stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy) # Cylinders are special - they are not supported directly by # moveit_commander. It must be published manually to collision scene cyl = CollisionObject() cyl.operation = CollisionObject.ADD cyl.id = object_id cyl.header = stamped_pose.header prim = SolidPrimitive() prim.type = SolidPrimitive.CYLINDER prim.dimensions = [size[0], size[1]] cyl.primitives = [prim] cyl.primitive_poses = [stamped_pose.pose] # self.scene_pub.publish(cyl) marker = self.make_marker_stub(stamped_pose, [size[1] * 2, size[2] * 2, size[0]], color=color) marker.type = Marker.CYLINDER self.publish_marker(object_id, marker) sleep(0.5) logdebug("Loaded " + object_id + " as cylindrical collision object.") except ROSException as e: loginfo("Problem loading " + object_id + " collision object: " + str(e)) def add_sphere(self, object_id, frame, size, pose=None, position=None, orientation=None, rpy=None, color=None): try: stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy) self.psi.add_sphere(object_id, stamped_pose, size[0]) loginfo("got past adding collision scene object") marker = self.make_marker_stub( stamped_pose, [size[0] * 2, size[1] * 2, size[2] * 2], color=color) marker.type = Marker.SPHERE self.publish_marker(object_id, marker) sleep(0.5) loginfo("Loaded " + object_id + " as collision object.") except ROSException as e: loginfo("Problem loading " + object_id + " collision object: " + str(e)) def add_box(self, object_id, frame, size, pose=None, position=None, orientation=None, rpy=None, color=None): try: stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy) self.psi.add_box(object_id, stamped_pose, size) marker = self.make_marker_stub(stamped_pose, size, color=color) marker.type = Marker.CUBE self.publish_marker(object_id, marker) sleep(0.5) loginfo("Loaded " + object_id + " as collision object.") except ROSException as e: loginfo("Problem loading " + object_id + " collision object: " + str(e)) def attach_box(self, link, object_id, frame, size, attach_to_link, pose=None, position=None, orientation=None, rpy=None): """TODO: color is not yet supported, since it's not internally supported by psi.attach_box. Basically duplicate this method but with color support.""" try: stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy), self.psi.attach_box(link, object_id, stamped_pose, size, attach_to_link) sleep(0.5) loginfo("Attached " + object_id + " as collision object.") except ROSException as e: loginfo("Problem attaching " + object_id + " collision object: " + str(e)) @staticmethod def make_stamped_pose(frame, pose=None, position=None, orientation=None, rpy=None): if orientation is not None and rpy is not None: logwarn("Collision object has both orientation (quaternion) and " "Rotation (rpy) defined! Defaulting to quaternion " "representation") stamped_pose = PoseStamped() stamped_pose.header.frame_id = frame stamped_pose.header.stamp = Time.now() # use a pose if one is provided, otherwise, make your own from the # position and orientation. if pose is not None: stamped_pose.pose = pose else: stamped_pose.pose.position.x = position[0] stamped_pose.pose.position.y = position[1] stamped_pose.pose.position.z = position[2] # for orientation, allow either quaternion or RPY specification if orientation is not None: stamped_pose.pose.orientation.x = orientation[0] stamped_pose.pose.orientation.y = orientation[1] stamped_pose.pose.orientation.z = orientation[2] stamped_pose.pose.orientation.w = orientation[3] elif rpy is not None: quaternion = transformations.quaternion_from_euler( rpy[0], rpy[1], rpy[2]) stamped_pose.pose.orientation.x = quaternion[0] stamped_pose.pose.orientation.y = quaternion[1] stamped_pose.pose.orientation.z = quaternion[2] stamped_pose.pose.orientation.w = quaternion[3] else: stamped_pose.pose.orientation.w = 1 # make basic quaternion return stamped_pose # Fill a ROS Marker message with the provided data def make_marker_stub(self, stamped_pose, size=None, color=None): if color is None: color = (0.5, 0.5, 0.5, 1.0) if size is None: size = (1, 1, 1) marker = Marker() marker.header = stamped_pose.header # marker.ns = "collision_scene" marker.id = self.next_id marker.lifetime = Time(0) # forever marker.action = Marker.ADD marker.pose = stamped_pose.pose marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] if len(color) == 4: alpha = color[3] else: alpha = 1.0 marker.color.a = alpha marker.scale.x = size[0] marker.scale.y = size[1] marker.scale.z = size[2] self.next_id += 1 return marker def make_new_marker_array_msg(self): ma = MarkerArray() return ma # publish a single marker def publish_marker(self, object_id, marker): loginfo("Publishing marker for object {}".format(object_id)) self.marker_array = self.make_new_marker_array_msg() self.marker_array.markers.append(marker) self.vis_pub.publish(self.marker_array) self.objects[object_id] = marker # Remove the provided object from the world. def remove(self, object_id): # if object_id not in self.objects: # logwarn("PlanningSceneHelper was not used to create object with id " # + object_id + ". Attempting to remove it anyway.") try: # first remove the actual collision object self.psi.remove_world_object(object_id) if object_id in self.objects: # then remove the marker associated with it marker = self.objects[object_id] marker.action = Marker.DELETE self.publish_marker(object_id, marker) self.objects.pop(object_id) logdebug("Marker for collision object " + object_id + " removed.") except ROSException as e: loginfo("Problem removing " + object_id + " from collision scene:" + str(e)) return except KeyError as e: loginfo("Problem removing " + object_id + " from collision scene:" + str(e)) return loginfo("Model " + object_id + " removed from collision scene.") # Remove the provided attached collision object. def remove_attached(self, link, object_id): try: self.psi.remove_attached_object(link=link, name=object_id) loginfo("Attached object '" + object_id + "' removed from collision scene.") except: loginfo("Problem attached object '" + object_id + "' removing from collision scene.")
print "============ Robot Groups:" print robot.get_group_names() print "============ Robot Links for arm:" print robot.get_link_names("arm") print "============ Robot Links for gripper:" print robot.get_link_names("gripper") print right_arm.get_end_effector_link() print "============ Printing robot state" #print robot.get_current_state() print "============" rospy.sleep(1) # clean the scene scene.remove_world_object("pole") scene.remove_world_object("table") #scene.remove_world_object("part") scene.remove_world_object("grasp_marker") # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() print p.header.frame_id p.pose.position.x = 0.4 p.pose.position.y = -0.4 p.pose.position.z = 0.85 p.pose.orientation.w = 1.0 #scene.add_box("pole", p, (0.3, 0.1, 1.0)) p.pose.position.y = -0.2 p.pose.position.z = 0.175
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "grasp" pose stored in the SRDF file arm.set_named_target('left_arm_up') arm.go() # Open the gripper to the neutral position gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() rospy.sleep(1) # Set the height of the table off the ground table_ground = 0.04 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.25 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.21 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[ 2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 #scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.19 box2_pose.pose.position.y = 0.13 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 #scene.add_box(box2_id, box2_pose, box2_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.24 target_pose.pose.position.y = 0.275 target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table blue and the boxes orange self.setColor(table_id, 0, 0, 0.8, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.18 place_pose.pose.position.y = 0 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it #grasp_pose.pose.position.y -= target_size[1] / 2.0 grasp_pose.pose.position.x = 0.12792118579 + .1 grasp_pose.pose.position.y = 0.285290879999 + 0.05 grasp_pose.pose.position.z = 0.120301181892 #grasp_pose.pose.orientation = # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id, table_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) break # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = arm.pick(target_id, grasps) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # Generate valid place poses places = self.make_places(place_pose) # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: # Return the arm to the "resting" pose stored in the SRDF file arm.set_named_target('left_arm_rest') arm.go() # Open the gripper to the open position gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
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) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 10 seconds per planning attempt right_arm.set_planning_time(10) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 10 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set the height of the table off the ground table_ground = 0.65 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.55 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.55 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.54 box2_pose.pose.position.y = 0.13 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.60 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object right_arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.50 place_pose.pose.position.y = -0.25 place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = right_arm.pick(target_id, grasps) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 #_------------------------now we move to the other table__________------------------------------------------- #_------------------------now we move to the other table__________------------------------------------------- # Generate valid place poses places = self.make_places(place_pose) # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = right_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
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() 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) # # self._pose_table = self._scene.get_object_poses(self._table_object_name) # self._pose_coke_can = self._scene.get_object_poses(self._grasp_object_name) # self.setup() # Clean the scene: self._scene.remove_world_object(self._table_object_name) rospy.sleep(1.0) self._scene.remove_world_object(self._grasp_object_name) rospy.sleep(1.0) # # Add table and Coke can objects to the planning scene: self._pose_table = self._add_table(self._table_object_name) rospy.sleep(2.0) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) rospy.sleep(2.0) # 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)) # 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 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) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) 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_attached_object() 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 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) def _add_objects(self): """ Here add all the objects that you want to add to the _scene """ self._pose_table = self._add_table(self._table_object_name) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
gripper_pose = arm.get_current_pose(arm.get_end_effector_link()) # part p.pose.position.x = gripper_pose.pose.position.x p.pose.position.y = gripper_pose.pose.position.y p.pose.position.z = gripper_pose.pose.position.z # add part scene.add_box(PICK_OBJECT, p, (0.07, 0.07, 0.1)) rospy.loginfo("Added object to world") # attach object manually arm.attach_object(PICK_OBJECT, arm.get_end_effector_link(), GRIPPER_JOINTS) rospy.sleep(1) # ===== place start ==== # place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose()) rospy.loginfo("Place Result is:") rospy.loginfo("Human readable error: " + str(moveit_error_dict[place_result.error_code.val])) rospy.sleep(5) # remove part scene.remove_world_object(PICK_OBJECT) scene.remove_attached_object(arm.get_end_effector_link(), PICK_OBJECT) rospy.sleep(2) i += 1 # end moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)