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 move_to_position(self,x,y,z,qx,qy,qz,qw):
ori = tf_c.quaternion_to_euler(qx, qy, qz,qw)
self.urx_wait_move(x,y,z,ori[0],ori[1],ori[2])
def urx_wait_move(self,x,y,z,rr,rp,ry):
#force tool angle to not wrap wire around itself
#IF(P18+180>180,-180-(180-(P18+180)),P18+180)
if (ry+tf_c.d2r(180)>tf_c.d2r(180)):
ry = -tf_c.d2r(180)-(tf_c.d2r(180)-(ry+tf_c.d2r(180)))
else:
ry = ry + tf_c.d2r(180)
#IF(AND(Q4>-45,Q4<135),Q4+180,Q4)
if (ry>tf_c.d2r(-45) and ry<tf_c.d2r(135)):
ry = ry + tf_c.d2r(180)
#IF(R4>180,-180-(180-R4),R4)
if (ry>tf_c.d2r(180)):
ry = tf_c.d2r(-180) - (tf_c.d2r(180)-ry)
rx, ry, rz = tf_c.rpy2rv(rr,rp,ry)
while(True):
self.process_frame()
try:
if not self.robot.is_program_running():
self.robot.movel((x, y, z, rx, ry, rz), self.robot_vel, self.robot_acc,wait=True)
time.sleep(0.1)
except Exception as e:
print(e)
if not self.robot.is_program_running():
print("Robot movement script complete")
break
def move_to_object(self,
plan_only=False,
x=None,
y=None,
z=None,
qx=None,
qy=None,
qz=None,
qw=None):
if (self.move_mode == self.MOVE_MODE_URX):
#get pose and use as target if variables aren't set
pose_current = self.robot.get_pose()
if (x == None):
x = pose_current.pos.x
if (y == None):
y = pose_current.pos.y
if (z == None):
z = pose_current.pos.z
#TODO do the same for orienation (pose only returns rotaion matrix so need to convert to quaternion)
'''
if (qx == None):
qx = pose_current.orientation.x
if (qy == None):
qy = pose_current.orientation.y
if (qz == None):
qz = pose_current.orientation.z
if (qw == None):
qw = pose_current.orientation.w
'''
if (not plan_only):
ori = tf_c.quaternion_to_euler(qx, qy, qz, qw)
self.urx_wait_move(x, y, z, ori[0], ori[1], ori[2])
return True
elif (self.move_mode == self.MOVE_MODE_MOVEIT):
try:
print "============ Starting Move Group Interface ..."
pose_current = self.moveGroupInterface.get_current_pose()
if (x == None):
x = pose_current.position.x
if (y == None):
y = pose_current.position.y
if (z == None):
z = pose_current.position.z
if (qx == None):
qx = pose_current.orientation.x
if (qy == None):
qy = pose_current.orientation.y
if (qz == None):
qz = pose_current.orientation.z
if (qw == None):
qw = pose_current.orientation.w
pose_goal = self.moveGroupInterface.create_pose(
x, y, z, qx, qy, qz, qw)
print("current")
print(pose_current)
print("goal")
print(pose_goal)
self.isRobotStopped = False
self.moveGroupInterface.go_to_pose_goal(pose_goal, plan_only)
self.isRobotStopped = True
return True
except rospy.ROSInterruptException as e:
print(e)
return False
def create_rect_mat(self, x, y, angle):
position = [x, y, 0]
orienation = [0, 0, angle]
return tf_c.euler_translation_to_transformation_matrix(
orienation, position)
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])