import GraspingHelperClass as Gp import graspObjectImageFunc as gi import interceptHelper as iH import gc debugMode = 1800 ### Establish ROS connection and initialize robot rospy.init_node("FrankZihanSu") global limb limb = intera_interface.Limb('right') global gripper gripper = intera_interface.Gripper('right_gripper') gripper.open() headDisplay = head.HeadDisplay() ################################################################################## # Ignore this crazyMode = False ################################################################################## dQ = Gp.euler_to_quaternion(z=0) # print dQ operation_height = 0.443 # # x = 5 inch = 0.13m y = 33 inch = 0.83m # camera_center_human_right = Gp.ik_service_client(limb='right', use_advanced_options=True, # p_x=0.13, p_y=0.55, p_z=operation_height, # q_x=dQ[0], q_y=dQ[1], q_z=dQ[2], q_w=dQ[3]) # # camera_center_human_right = [0.7406572265625, -0.67773046875, 1.38037890625, 0.7668037109375, -0.9584423828125, 1.9225859375, -2.98408203125]
def point_action(x, y): rospy.init_node("GraspingDemo") global limb limb = intera_interface.Limb('right') global gripper gripper = intera_interface.Gripper('right_gripper') # command gripper.close() headDisplay = head.HeadDisplay() rospy.sleep(1) # operation_x = Gp.in_to_m(25) operation_x = x # operation_y_1 = Gp.in_to_m(-19) operation_y_1 = y operation_y_2 = -1 * operation_y_1 # Pre-grasping joint angles dQ = Gp.euler_to_quaternion(z=0) # print dQ operation_height_upper = 0.300 # x = 36 inch, y = -18 inch camera_center_human_right = Gp.ik_service_client( limb='right', use_advanced_options=True, p_x=operation_x, p_y=operation_y_1, p_z=operation_height_upper, q_x=dQ[0], q_y=dQ[1], q_z=dQ[2], q_w=dQ[3]) # x = 36 inch, y = 18 inch camera_center_human_left = Gp.ik_service_client(limb='right', use_advanced_options=True, p_x=operation_x, p_y=operation_y_2, p_z=operation_height_upper, q_x=dQ[0], q_y=dQ[1], q_z=dQ[2], q_w=dQ[3]) moveComm = Gp.moveit_commander moveComm.roscpp_initialize(sys.argv) robot = moveComm.RobotCommander() scene = moveComm.PlanningSceneInterface() group_name = "right_arm" group = moveComm.MoveGroupCommander(group_name) display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', Gp.moveit_msgs.msg.DisplayTrajectory, queue_size=20) planning_frame = group.get_planning_frame() eef_link = group.get_end_effector_link() Gp.load_objects(scene, planning_frame) Gp.load_camera_w_mount(scene) # print limb.joint_angles() dQ = Gp.euler_to_quaternion(z=3.1415 / 2) # print dQ drop_block_pos = camera_center_human_right start = time.time() # command Gp.move_move(limb, group, drop_block_pos, 0.1) rospy.sleep(1)