def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('Three_box') control = arm_base_control() sce = moveit_python.PlanningSceneInterface('odom') sce.clear() rospy.sleep(1) control.add_three_box_obstacle() start = time.clock() startime = datetime.datetime.now() point_list = [] for i in range(11): point_list.append((i * 0.2 - 1, 3, 0.1)) control.print_pointlist(point_list) finish = time.clock() finshtime = datetime.datetime.now() print('time:', finish - start, (finshtime - startime).seconds)
def __init__(self, use_moveit=False): self.use_moveit = use_moveit self.baxter = URDF.from_parameter_server(key='robot_description') self.kdl_tree = kdl_tree_from_urdf_model(self.baxter) self.base_link = self.baxter.get_root() self.right_limb_interface = baxter_interface.Limb('right') self.left_limb_interface = baxter_interface.Limb('left') self.get_link_state = rospy.ServiceProxy("/gazebo/get_link_state", GetLinkState) self.get_model_state = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState) self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) self.set_model_config = rospy.ServiceProxy('/gazebo/set_model_configuration', SetModelConfiguration) # Listen to collision information # self.collision_getter = InfoGetter() # self.collision_topic = "/hug_collision" # Verify robot is enabled print("Getting robot state... ") self._rs = baxter_interface.RobotEnable() self._init_state = self._rs.state().enabled if not self._init_state: print("Enabling robot... ") self._rs.enable() if self.use_moveit: # Moveit group setup moveit_commander.roscpp_initialize(sys.argv) self.robot = moveit_commander.RobotCommander() self.scene = moveit_python.PlanningSceneInterface(self.robot.get_planning_frame()) # self.scene = moveit_commander.PlanningSceneInterface() self.group_name = "right_arm" self.group = moveit_commander.MoveGroupCommander(self.group_name) # ######################## Create Hugging target ############################# # humanoid hugging target # remember to make sure target_line correct + - y self.target_pos_start = np.asarray([0.5, 0, -0.93]) # robot /base frame, z = -0.93 w.r.t /world frame self.target_line_start = np.empty([22, 3], float) for i in range(11): self.target_line_start[i] = self.target_pos_start + [0, -0.0, 1.8] - (asarray([0, -0.0, 1.8]) - asarray([0, -0.0, 0.3]))/10*i self.target_line_start[i+11] = self.target_pos_start + [0, -0.5, 1.3] + (asarray([0, 0.5, 1.3]) - asarray([0, -0.5, 1.3]))/10*i self.target_line = self.target_line_start # Build line point graph for interaction mesh # reset right arm start_point_right = [-0.3, 1.0, 0.0, 0.5, 0.0, 0.027, 0.0] t01 = threading.Thread(target=resetarm_job, args=(self.right_limb_interface, start_point_right)) t01.start() # reset left arm start_point_left = [0.3, 1.0, 0.0, 0.5, 0.0, 0.027, 0.0] t02 = threading.Thread(target=resetarm_job, args=(self.left_limb_interface, start_point_left)) t02.start() t02.join() t01.join() right_limb_pose, _ = limbPose(self.kdl_tree, self.base_link, self.right_limb_interface, 'right') left_limb_pose, _ = limbPose(self.kdl_tree, self.base_link, self.left_limb_interface, 'left') graph_points = np.concatenate((right_limb_pose[5:], left_limb_pose[5:], self.target_line_start), 0) self.triangulation = Delaunay(graph_points)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('Three_box') control = arm_base_control() sce = moveit_python.PlanningSceneInterface('odom') sce.clear() rospy.sleep(1) point_list = [] point_list = control.straight_line_sample((-1, 1), (3, 1)) control.print_pointlist(point_list)
def __init__(self, limb): self._baxter = URDF.from_parameter_server(key='robot_description') self._kdl_tree = kdl_tree_from_urdf_model(self._baxter) self._right_limb_interface = baxter_interface.Limb('right') self._base_link = self._baxter.get_root() self._tip_link = limb + '_gripper' self.solver = KDLKinematics(self._baxter, self._base_link, self._tip_link) self.rs = RobotState() self.rs.joint_state.name = [ 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2' ] self.rs.joint_state.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Moveit group setup moveit_commander.roscpp_initialize(sys.argv) self.robot = moveit_commander.RobotCommander() self.scene = moveit_python.PlanningSceneInterface( self.robot.get_planning_frame()) # self.scene = moveit_commander.PlanningSceneInterface() self.group_name = "right_arm" self.group = moveit_commander.MoveGroupCommander(self.group_name) # service self.set_model_config = rospy.ServiceProxy( '/gazebo/set_model_configuration', SetModelConfiguration) self.get_link_state = rospy.ServiceProxy("/gazebo/get_link_state", GetLinkState) self.sv_srv = rospy.ServiceProxy('/check_state_validity', GetStateValidity) # human target self.target_pos_start = np.asarray( [0.5, 0, -0.93]) # robot /base frame, z = -0.93 w.r.t /world frame self.target_line_start = np.empty([22, 3], float) for i in range(11): self.target_line_start[i] = self.target_pos_start + [ 0, -0.0, 1.8 ] - (np.asarray([0, -0.0, 1.8]) - np.asarray([0, -0.0, 0.5])) / 10 * i self.target_line_start[i + 11] = self.target_pos_start + [ 0, -0.5, 1.3 ] + (np.asarray([0, 0.5, 1.3]) - np.asarray([0, -0.5, 1.3])) / 10 * i self.target_line = self.target_line_start
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('dynamic_obstacle_demo') scene = Scene_obstacle() control = arm_base_control() sce = moveit_python.PlanningSceneInterface('odom') sce.clear() # scene.scene.remove_world_object() rospy.sleep(1) point_list = scene.straight_line_sample((-1,1), (3,1)) point_list_ob = scene.straight_line_sample((-1,1), (3,1), height = 0.05) scene.print_list_visualize(point_list_ob) control.print_pointlist(point_list)
def __init__(self): rospy.init_node('general_printing') name = ['1', '2'] for na in name: control = arm_base_control(name=na) sce = moveit_python.PlanningSceneInterface('odom') sce.clear() rospy.sleep(1) point_list = [] point_list += control.straight_line_sample((0, 1), (1, 1)) # control.print_list_visualize(point_list) control.print_pointlist(point_list)
def __init__(self): rospy.init_node('general_printing') for name in range(1, 8): control = arm_base_control() sce = moveit_python.PlanningSceneInterface('odom') sce.clear() rospy.sleep(5) point_list = [] point_list += control.straight_line_sample((0, 1), (1, 1)) point_list += control.straight_line_sample((1, 1), (1, 2)) point_list += control.straight_line_sample((1, 2), (3, 1)) point_list += control.straight_line_sample((3, 1), (1, 0)) point_list += control.straight_line_sample((1, 0), (1, 1)) # control.print_list_visualize(point_list) control.print_pointlist(point_list, name='f**k' + str(name))
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('general_printing') control = arm_base_control() sce = moveit_python.PlanningSceneInterface('odom') sce.clear() rospy.sleep(1) point_list = [] point_list += control.get_circle_point((-1, 1), 1) point_list += control.straight_line_sample((0, 1), (1, 1)) point_list += control.straight_line_sample((1, 1), (1, 2)) point_list += control.straight_line_sample((1, 2), (3, 1)) point_list += control.straight_line_sample((3, 1), (1, 0)) point_list += control.straight_line_sample((1, 0), (1, 1)) # control.print_list_visualize(point_list) control.print_pointlist(point_list)
def __init__(self, name=None): moveit_commander.roscpp_initialize(sys.argv) self.robot = moveit_commander.RobotCommander() # Init moveit group self.group = moveit_commander.MoveGroupCommander('robot') self.arm_group = moveit_commander.MoveGroupCommander('arm') self.base_group = moveit_commander.MoveGroupCommander('base') self.scene = moveit_commander.PlanningSceneInterface() self.sce = moveit_python.PlanningSceneInterface('odom') self.pub_co = rospy.Publisher('collision_object', CollisionObject, queue_size=100) self.msg_print = SetBoolRequest() self.request_fk = rospy.ServiceProxy('/compute_fk', GetPositionFK) self.pub_co = rospy.Publisher('collision_object', CollisionObject, queue_size=100) sub_waypoint_status = rospy.Subscriber('execute_trajectory/status', GoalStatusArray, self.waypoint_execution_cb) sub_movegroup_status = rospy.Subscriber('execute_trajectory/status', GoalStatusArray, self.move_group_execution_cb) # sub_movegroup_status = rospy.Subscriber('move_group/status', GoalStatusArray, self.move_group_execution_cb) rospy.Subscriber("joint_states", JointState, self.further_ob_printing) ######################################################################### rospy.Subscriber("/joint_states", JointState, self.joint_callback) rospy.Subscriber("/joint_states", JointState, self.eef_callback) self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) self.base_position_x = [] self.base_position_y = [] self.base_position_z = [] self.eef_position_x = [] self.eef_position_y = [] self.eef_position_z = [] self.position_time = [] self.name = name self.js_0 = [] self.js_1 = [] self.js_2 = [] self.js_time = [] self.startime = datetime.datetime.now() self.mkdir('experiment_data' + '/' + name) ############################################################################# msg_print = SetBoolRequest() msg_print.data = True self.re_position_x = [] self.re_position_y = [] self.waypoint_execution_status = 0 self.move_group_execution_status = 0 self.further_printing_number = 0 self.pre_further_printing_number = 0 # initial printing number self._printing_number = 0 self._further_printing_number = 0 self.future_printing_status = False self.current_printing_pose = None self.previous_printing_pose = None self.target_list = None self.group.allow_looking(1) self.group.allow_replanning(1) self.group.set_planning_time(10) # Initialize time record self.travel_time = 0 self.planning_time = 0 self.printing_time = 0 # Initialize extruder extruder_publisher = rospy.Publisher('set_extruder_rate', Float32, queue_size=20) msg_extrude = Float32() msg_extrude = 5.0 extruder_publisher.publish(msg_extrude) self.pub_rviz_marker = rospy.Publisher('/visualization_marker', Marker, queue_size=100) self.remove_all_rviz_marker() self.printing_number_rviz = 0
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('enclosure_demo') control = arm_base_control() sce = moveit_python.PlanningSceneInterface('odom') sce.clear() rospy.sleep(0.5) box_pose = geometry_msgs.msg.PoseStamped() box_pose.header.frame_id = "odom" box_pose.pose.position.x = -0.5 box_pose.pose.position.y = 2.5 box_pose.pose.position.z = 0.2 box_one = "box_1" control.scene.add_box(box_one, box_pose, size=(0.5, 0.5, 0.5)) rospy.sleep(0.5) # scene.scene.remove_world_object() rospy.sleep(1) point_list = [] point_list += control.straight_line_sample((-1, 1), (2, 1)) point_list += control.straight_line_sample((2, 1), (2, 3)) point_list += control.straight_line_sample((1, 3), (0, 3)) point_list += control.straight_line_sample((1, 3), (0, 2)) point_list_0 = control.straight_line_sample((0, 2), (0, 1)) control.group.set_position_target( [1, 2, 0.1], control.group.get_end_effector_link()) control.group.go(wait=True) control.print_list_visualize(point_list) control.print_list_visualize(point_list_0, name='future_ob') while not rospy.is_shutdown(): # Check for enclosure rospy.sleep(0.05) control.base_group.set_position_target( [0, 0, 0.05], control.base_group.get_end_effector_link()) result = control.base_group.plan() control.base_group.clear_pose_targets() if len(result.joint_trajectory.points) == 0: print('Check enclosure failed') # Remove future obstacle current_future_ob_list = sce.getKnownCollisionObjects() for point_name in current_future_ob_list: result = re.match('future_ob', point_name) if result: sce.removeCollisionObject(point_name) rospy.sleep(0.05) random_pose = control.group.get_current_pose( control.group.get_end_effector_link()) (roll, pitch, yaw) = euler_from_quaternion([ random_pose.pose.orientation.x, random_pose.pose.orientation.y, random_pose.pose.orientation.z, random_pose.pose.orientation.w ]) [random_pose.pose.orientation.x, \ random_pose.pose.orientation.y, \ random_pose.pose.orientation.z, \ random_pose.pose.orientation.w] = quaternion_from_euler(roll, pitch, yaw - pi/2) (random_pose.pose.position.x, random_pose.pose.position.y, random_pose.pose.position.z) = (0, 2, 0.1) control.group.set_pose_target(random_pose) control.group.go(wait=True) control.group.clear_pose_targets() # control.group.set_position_target([0, 2, 0.1], control.group.get_end_effector_link()) # control.group.go(wait = True) control.print_list_visualize(point_list_0, name='future_ob') rospy.sleep(0.05) else: print('Check enclosure successful') break
#!/usr/bin/python import rospy import moveit_python from moveit_msgs.msg import CollisionObject from geometry_msgs.msg import Pose, Point, Quaternion from shape_msgs.msg import SolidPrimitive from std_msgs.msg import Header rospy.init_node("moveit_table_scene") scene = moveit_python.PlanningSceneInterface("base_link") #pub = rospy.Publisher("/move_group/monitored_planning_scene", CollisionObject, queue_size=1) table = CollisionObject() # Header h = Header() h.stamp = rospy.Time.now() # Shape box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = [0.808, 1.616, 2.424] # Pose position = Point(*[1.44, 0.0, 0.03]) orient = Quaternion(*[-0.510232, 0.49503, 0.515101, 0.478832]) # Create Collision Object scene.addSolidPrimitive("table", box, Pose(*[position, orient]))
def __init__(self, name_space): moveit_commander.roscpp_initialize(sys.argv) # current_robot_ns = rospy.get_namespace() # current_robot_ns=current_robot_ns[1:-1] current_robot_ns = name_space self.robot = moveit_commander.RobotCommander( current_robot_ns + '/robot_description', current_robot_ns) # Init moveit group self.group = moveit_commander.MoveGroupCommander( 'robot', current_robot_ns + '/robot_description', current_robot_ns) self.arm_group = moveit_commander.MoveGroupCommander( 'arm', current_robot_ns + '/robot_description', current_robot_ns) self.base_group = moveit_commander.MoveGroupCommander( 'base', current_robot_ns + '/robot_description', current_robot_ns) self.scene = moveit_commander.PlanningSceneInterface() self.sce = moveit_python.PlanningSceneInterface( 'odom', current_robot_ns) self.pub_co = rospy.Publisher('/' + current_robot_ns + '/collision_object', CollisionObject, queue_size=100) self.msg_print = SetBoolRequest() self.request_fk = rospy.ServiceProxy( '/' + current_robot_ns + '/compute_fk', GetPositionFK) self.pub_co = rospy.Publisher('/' + current_robot_ns + '/collision_object', CollisionObject, queue_size=10) sub_waypoint_status = rospy.Subscriber( '/' + current_robot_ns + '/execute_trajectory/status', GoalStatusArray, self.waypoint_execution_cb) sub_movegroup_status = rospy.Subscriber( '/' + current_robot_ns + '/move_group/status', GoalStatusArray, self.move_group_execution_cb) sub_movegroup_status = rospy.Subscriber( '/' + current_robot_ns + '/move_group/status', GoalStatusArray, self.move_group_execution_cb) rospy.Subscriber('/' + current_robot_ns + "/joint_states", JointState, self.further_ob_printing) # Initialize extruder print_request = rospy.ServiceProxy( '/' + current_robot_ns + '/set_extruder_printing', SetBool) self.extruder_publisher = rospy.Publisher('/' + current_robot_ns + '/set_extruder_rate', Float32, queue_size=20) # switch the extruder ON, control via set_extruder_rate msg_print = SetBoolRequest() msg_print.data = True print_request(msg_print) self.msg_extrude = Float32() self.waypoint_execution_status = 0 self.move_group_execution_status = 0 self.further_printing_number = 0 self.pre_further_printing_number = 0 # initial printing number self._printing_number = 0 self._further_printing_number = 0 self.future_printing_status = False self.current_printing_pose = None self.previous_printing_pose = None self.target_list = None self.group.allow_looking(1) self.group.allow_replanning(1) self.group.set_planning_time(10) # Initialize extruder extruder_publisher = rospy.Publisher('set_extruder_rate', Float32, queue_size=20) msg_extrude = Float32() msg_extrude = 5.0 extruder_publisher.publish(msg_extrude) self.pub_rviz_marker = rospy.Publisher('/visualization_marker', Marker, queue_size=100) self.remove_all_rviz_marker() self.printing_number_rviz = 0
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('arm_base_printing') self.robot = moveit_commander.RobotCommander() # Initialize moveit scene interface (woeld surrounding the robot) # self.scene = moveit_commander.PlanningSceneInterface() self.scene = moveit_python.PlanningSceneInterface('odom') self.scene.clear() self.group = moveit_commander.MoveGroupCommander('robot') self.base_group = moveit_commander.MoveGroupCommander('robot') self.arm_group = moveit_commander.MoveGroupCommander('arm') self.msg_print = SetBoolRequest() self.request_fk = rospy.ServiceProxy('/compute_fk', GetPositionFK) self.pub_co = rospy.Publisher('collision_object', CollisionObject, queue_size=10) sub_waypoint_status = rospy.Subscriber('execute_trajectory/status', GoalStatusArray, self.waypoint_execution_cb) # check_inside = rospy.Subscriber("joint_states", JointState, self.check_inside_callback) self.waypoint_execution_status = 0 self.group.allow_looking(1) self.group.allow_replanning(1) self.group.set_planning_time(10) # Initialize extruder extruder_publisher = rospy.Publisher('set_extruder_rate', Float32, queue_size=20) msg_extrude = Float32() msg_extrude = 5.0 extruder_publisher.publish(msg_extrude) self.printing_number = 0 self.further_printing_number = 0 # self.add_three_box_obstacle() point_list = [] # for i in range(11): # point_list.append((i * 0.2 -1, 3, 0.1)) # print(point_list) point_list = self.straight_line_sample((-1, 1), (3, 1)) point_list_ob = self.straight_line_sample((-1, 1), (3, 1), height=0.05) # # print(point_list) # self.print_list_visualize(point_list_ob) # self.print_pointlist(point_list) # point_list = self.get_circle_point((2,2), 1) # point_list_ob = self.get_circle_point((2,2), 1, height = 0.05) # self.print_future_visualize(point_list_ob[3:8]) self.print_pointlist(point_list)
def pick_and_place(grasp_pose, object_dims): ## First initialize moveit_commander and rospy. print "============ Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) # interface to the robot robot = moveit_commander.RobotCommander() # interface to the world surrounding the robot. scene = moveit_python.PlanningSceneInterface("base_link") # interface to a group of joints. group = moveit_commander.MoveGroupCommander("manipulator") # publisher to RVIZ for visualization display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=10) ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up. print "============ Waiting for RVIZ..." rospy.sleep(1) # name of the reference frame for this robot print "============ Reference frame: %s" % group.get_planning_frame() # name of the end-effector link for this group print "============ Reference frame: %s" % group.get_end_effector_link() ## MAIN ## pregrasp_pose = compute_pregrasp(grasp_pose) # compute pre-grasp move_to_pose(pregrasp_pose, group, robot, display_trajectory_publisher) # move to pregrasp move_to_pose(grasp_pose, group, robot, display_trajectory_publisher) # move to grasp """ To Do Close Gripper """ #close_gripper() To Do # attach object to ur5 in order to have successful motion planning attach_object(object_dims,pregrasp_pose, grasp_pose, scene, "tool0", "mybox", 0.01) move_to_pose(pregrasp_pose, group, robot, display_trajectory_publisher) # move ur5 to a neutral position up_pose = Pose() quaternion = tf.transformations.quaternion_from_euler(0, 0, 0) up_pose.orientation.x = quaternion[0] up_pose.orientation.y = quaternion[1] up_pose.orientation.z = quaternion[2] up_pose.orientation.w = quaternion[3] up_pose.position.x = 0 up_pose.position.y = .2 up_pose.position.z = 0.8 print "MOVING TO NEUTRAL UP POSITION" move_to_pose(up_pose, group, robot, display_trajectory_publisher) # X and Y coordinates of where object should be placed PLACE_X = 0.5 PLACE_Y = -0.3 print "MOVING TO PRE RELEASE POSE" # even though we are releasing the object, still use grasp poses # change the x and y coordinates of the pregrasp pose pregrasp_pose.position.x = PLACE_X pregrasp_pose.position.y = PLACE_Y move_to_pose(pregrasp_pose, group, robot, display_trajectory_publisher) print "MOVING TO RELEASE POSE" # change the x and y coordinates of the grasp pose pregrasp_pose.position.z=pregrasp_pose.position.z-0.1 move_to_pose(pregrasp_pose, group, robot, display_trajectory_publisher) """ To Do Open Gripper """ # "remove" the object from the robot scene.removeAttachedObject("mybox") #open_gripper() To Do print "MOVING TO NEUTRAL UP POSITION" # move back up to a neutral position awaiting next command move_to_pose(up_pose, group, robot, display_trajectory_publisher) print "SUCCESS" print "============ STOPPING" moveit_commander.roscpp_shutdown()
from tf.transformations import euler_from_quaternion, quaternion_from_euler from math import pi from math import cos, sin from moveit_msgs.msg import CollisionObject from shape_msgs.msg import SolidPrimitive, Plane, Mesh, MeshTriangle import geometry_msgs.msg from geometry_msgs.msg import * import moveit_python # There two obstacle move between (-1,1) and (3, 1) if __name__ == '__main__': rospy.init_node('dynamic_obstacle') # scene = moveit_python.PlanningSceneInterface() scene = moveit_python.PlanningSceneInterface('odom') scene.clear() box1_x = -1 box1_y = 1.5 box1_z = 0.25 box2_x = 3 box2_y = 0.5 box2_z = 0.25 direction_1 = 1 direction_2 = -1 while not rospy.is_shutdown():
#!/usr/bin/env python # This code is used for checking precision of extrinsic callibration import moveit_python import moveit_commander import rospy from geometry_msgs.msg import * if __name__ == "__main__": rospy.init_node('insert_spike') robot = moveit_commander.RobotCommander() scene = moveit_python.PlanningSceneInterface('ra_base') scene.addCylinder("spike1", 0.306, 0.0063, -0.25, -0.68235, 0.153)
def pick_and_place(): ## First initialize moveit_commander and rospy. print "============ Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_wrist_to_pregrasp', anonymous=True) ## Instantiate a RobotCommander object. This object is an interface to ## the robot as a whole. robot = moveit_commander.RobotCommander() ## Instantiate a PlanningSceneInterface object. This object is an interface ## to the world surrounding the robot. scene = moveit_python.PlanningSceneInterface("base_link") ## Instantiate a MoveGroupCommander object. This object is an interface ## to one group of joints. In this case the group is the joints in the left ## arm. This interface can be used to plan and execute motions on the left ## arm. group = moveit_commander.MoveGroupCommander("manipulator") #group = moveit_python.MoveGroupCommander("manipulator") ## 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, queue_size=10) ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up. print "============ Waiting for RVIZ..." rospy.sleep(1) print "============ Starting tutorial " ## Getting Basic Information ## ^^^^^^^^^^^^^^^^^^^^^^^^^ ## ## We can get the name of the reference frame for this robot print "============ Reference frame: %s" % group.get_planning_frame() ## We can also print the name of the end-effector link for this group print "============ Reference frame: %s" % group.get_end_effector_link() ## We can get a list of all the groups in the robot #print "============ Robot Groups:" #print robot.get_group_names() ## Sometimes for debugging it is useful to print the entire state of the ## robot. #print "============ Printing robot state" #print robot.get_current_state() print "============" ## Wait for test_pr2_gripper_grasp_planner_cluster to publish selected grasp #rospy.Subscriber('selected_grasp', Pose, selected_grasp_callback) #rospy.spin() # keeps python from exiting until this node is stopped grasp_pose, object_dimensions = get_selected_grasp( ) #get best pre-grasp pose #print "object dimensions:" #print object_dimensions.x, object_dimensions.y, object_dimensions.z #compute the pregrasp pregrasp_pose = compute_pregrasp(grasp_pose) move_to_pose(pregrasp_pose, group, robot, display_trajectory_publisher) #move to pregrasp move_to_pose(grasp_pose, group, robot, display_trajectory_publisher) #move to grasp """ To Do Close Gripper """ #close_gripper() To Do #"attach object to ur5 in order to have successful motion planning" attach_object(object_dimensions, pregrasp_pose, grasp_pose, scene, "tool0", "mybox", 0.01) move_to_pose(pregrasp_pose, group, robot, display_trajectory_publisher) #move back to pregrasp pose #move ur5 to a neutral position up_pose = Pose() quaternion = tf.transformations.quaternion_from_euler(0, 0, 0) up_pose.orientation.x = quaternion[0] up_pose.orientation.y = quaternion[1] up_pose.orientation.z = quaternion[2] up_pose.orientation.w = quaternion[3] up_pose.position.x = 0 up_pose.position.y = .2 up_pose.position.z = 0.8 print "MOVING TO NEUTRAL UP POSITION" move_to_pose(up_pose, group, robot, display_trajectory_publisher) # X and Y coordinates of where object should be placed PLACE_X = 0.5 PLACE_Y = -0.3 print "MOVING TO PRE RELEASE POSE" # even though we are releasing the object, still use grasp poses #change the x and y coordinates of the pregrasp pose pregrasp_pose.position.x = PLACE_X pregrasp_pose.position.y = PLACE_Y move_to_pose(pregrasp_pose, group, robot, display_trajectory_publisher) print "MOVING TO RELEASE POSE" #change the x and y coordinates of the grasp pose pregrasp_pose.position.z = pregrasp_pose.position.z - 0.1 move_to_pose(pregrasp_pose, group, robot, display_trajectory_publisher) # "remove" the object from the robot scene.removeAttachedObject("mybox") """ To Do Open Gripper """ #open_gripper() To Do print "MOVING TO NEUTRAL UP POSITION" #move back up to a neutral position awaiting next command move_to_pose(up_pose, group, robot, display_trajectory_publisher) print "SUCCESS" moveit_commander.roscpp_shutdown() ## END_TUTORIAL print "============ STOPPING"