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)
