def coord_to_robot(self,x,y,z,angle):
z_offset = -1.0
#calculate object pose relative to robot base
robot_x, robot_y, robot_z = self.add_robot_offset(x,y,z+z_offset)
robot_ori = tf_c.quaternion_to_euler(self.robot_zero_q[0], self.robot_zero_q[1], self.robot_zero_q[2],self.robot_zero_q[3])
robot_ori[2] = robot_ori[2] - tf_c.d2r(angle)
robot_q = tf_c.euler_to_quaternion(robot_ori[0],robot_ori[1],robot_ori[2])
robot_qx = robot_q[0]
robot_qy = robot_q[1]
robot_qz = robot_q[2]
robot_qw = robot_q[3]
return robot_x,robot_y,robot_z,robot_qx,robot_qy,robot_qz,robot_qw
def __init__(self,robot_ip,robot_speed,gripper_ip,left_camera_serial,right_camera_serial,left_cal_file,right_cal_file,binning):
self.connected = False
self.binning = binning
self.cam_l = BaslerCapture()
self.cam_r = BaslerCapture()
self.robot = urx.Robot(robot_ip)
self.robot_vel = robot_speed
self.robot_acc = robot_speed
self.gripper = RobotiqControl(gripper_port)
self.sqDetect = SquareDetect(binning=self.binning,isShowThreshold=False)
print("Loading calibration...")
self.stereo3D = Stereo3D(left_cal_file, right_cal_file)
self.camera_calibrated = False
self.robot_calibrated = False
self.image_zero = [-1,-1]
self.image_zero_anti_binning = [-1,-1]
self.robot_zero = [-0.6852423340444724, -0.26550360706314924, 0.5740333751316029]
self.robot_zero_orientation = [3.14, 0, 3.14]
self.robot_zero_q = tf_c.euler_to_quaternion(self.robot_zero_orientation[0],self.robot_zero_orientation[1],self.robot_zero_orientation[2])
def __init__(self,
detect_mode="squares",
robot_speed=0.05,
isUseMoveit=False,
binning=1.0,
isStereo=True,
left_cal_file="",
right_cal_file=""):
self.MOVE_MODE_MOVEIT = "MOVEIT"
self.MOVE_MODE_URX = "URX"
if (isUseMoveit):
self.move_mode = self.MOVE_MODE_MOVEIT
else:
self.move_mode = self.MOVE_MODE_URX
self.detect_mode = detect_mode
self.robot_vel = robot_speed
self.robot_acc = robot_speed
self.isConnected = False
self.isRobotStopped = True
self.isPoseValid = False
self.capture_index = 0
self.PROCESS_MODE_PICK_PLACE = "PROCESS_MODE_PICK_PLACE"
self.PROCESS_MODE_CALIBRATE = "PROCESS_MODE_CALIBRATE"
self.process_mode = self.PROCESS_MODE_CALIBRATE
self.moveGroupInterface = None
self.robot_image_zero_position = None
self.robot_image_zero_q = None
self.robot_image_zero_orientation = None
self.binning = binning
self.sqDetect = SquareDetect(binning=self.binning,
isShowThreshold=True)
self.stCal = StereoCalib("")
self.isStereo = isStereo
if (isStereo):
self.cam2 = BaslerCapture()
self.stereo3D = Stereo3D(left_cal_file, right_cal_file)
self.sqDetect.fx = self.stereo3D.fx
self.sqDetect.baseline = self.stereo3D.T
else:
self.cam2 = None
self.cam = BaslerCapture()
if (self.move_mode == self.MOVE_MODE_MOVEIT):
#define robot base to image zero transform
self.robot_image_zero_position = [0.69888, 0.28909, 0.74546]
self.robot_image_zero_q = [
-0.509778982573, 0.495952045388, 0.491642672896, 0.50243849354
]
elif (self.move_mode == self.MOVE_MODE_URX):
#define robot base to image zero transform
#self.robot_image_zero_position = [-0.69000, -0.29291, 0.59005]
self.robot_image_zero_position = [
-0.6015036167497396, -0.3202075274552616, 0.5948835542537181
]
#robot_image_zero_q = [0.99993, 0.00062475, 0.01081, 0.0046892]
self.robot_image_zero_orientation = [3.14, 0, 3.14]
self.robot_image_zero_q = tf_c.euler_to_quaternion(
self.robot_image_zero_orientation[0],
self.robot_image_zero_orientation[1],
self.robot_image_zero_orientation[2])