degrees=True).as_rotvec() # tool_orientation = [0., -np.pi, 0.] # [0,-2.22,2.22] # [2.22,2.22,0] # pose0 = np.array([-0.511, 0.294, 0.237, -0.032, -1.666, 0.138]) pose0 = np.hstack([[-0.505, 0.06, 0.2], tool_orientation]) pose_up = pose0.copy() urc.movel_wait(pose0, a=a, v=v) # --------------------------------------------- # Start gripper grc = Gripper_Controller() grc.gripper_helper.set_gripper_current_limit(0.3) grc.start() grc.follow_gripper_pos = 0. time.sleep(0.5) # Move robot to home pose # robot = Robot(False, None, None, workspace_limits, # tcp_host_ip, tcp_port, rtc_host_ip, rtc_port, # False, None, None) # robot.open_gripper() # Slow down robot # robot.joint_acc = 1.4 # robot.joint_vel = 1.05 # Callback function for clicking on OpenCV window click_point_pix = () # camera_color_img, camera_depth_img = robot.get_camera_data()
urc = UR_Controller() grc = Gripper_Controller() urc.start() grc.start() # pose0 = np.array([-0.252, -0.138, 0.067, 0.013, -2.121, 2.313]) pose0 = np.array([-0.539, 0.312, 0.321, -1.787, -1.604, -0.691]) pose1 = np.array([-0.481, 0.283, 0.359, -1.6, -1.480, -1.031]) pose2 = np.array([-0.481, 0.283, 0.359, -1.216, -1.480, -.8]) # grc.gripper_helper.set_gripper_current_limit(0.4) grc.follow_gripper_pos = 0 # c = input() # grc.follow_gripper_pos = 1 # # # a = 0.05 # v = 0.05 # urc.movel_wait(pose0, a=a, v=v) # # # c = input() # urc.movel_wait(pose1, a=a, v=v) # c = input() # urc.movel_wait(pose0, a=a, v=v) # c = input()
# tool_orientation_euler = [180, 0, 0]
# tool_orientation_euler = [180, 0, 180]
tool_orientation = R.from_euler('xyz', tool_orientation_euler, degrees=True).as_rotvec()
# tool_orientation = [0., -np.pi, 0.] # [0,-2.22,2.22] # [2.22,2.22,0]
# pose0 = np.array([-0.511, 0.294, 0.237, -0.032, -1.666, 0.138])
pose0 = np.hstack([[-0.505, 0.06, 0.2], tool_orientation])
pose_up = pose0.copy()
pose_transfer = np.array([-0.431, 0.092, 0.232, -2.230, -2.194, -0.019])
urc.movel_wait(pose0, a=a, v=v)
# ---------------------------------------------
# Start gripper
grc = Gripper_Controller()
grc.follow_gripper_pos = 0
grc.gripper_helper.set_gripper_current_limit(0.3)
grc.start()
# grc.follow_gripper_pos = 0.
time.sleep(0.5)
# Move robot to home pose
# robot = Robot(False, None, None, workspace_limits,
# tcp_host_ip, tcp_port, rtc_host_ip, rtc_port,
# False, None, None)
# robot.open_gripper()
# Slow down robot
# robot.joint_acc = 1.4
# robot.joint_vel = 1.05