def __fix_pix2base_transform(self, req):
# self.__get_robot_trans()
positon = [11.3440, 36.4321, 11.23, 179.994, 10.002, -0.488]
abc = [radians(i) for i in positon[3:]]
base_cam_trans = np.mat(np.identity(4))
base_cam_trans[0:3,
0:3] = tf.transformations.euler_matrix(abc[0],
abc[1],
abc[2],
axes='sxyz')[0:3,
0:3]
base_cam_trans[0:3, 3:] = np.mat([i / 100
for i in positon[:3]]).reshape(3, 1)
eye_obj_trans = np.mat(np.append(np.array(req.ini_pose),
1)).reshape(4, 1)
eye_obj_trans[2] = eye_obj_trans[2] * 0.01
eye_obj_trans[:2] = (eye_obj_trans[:2] -
self._camera_mat[:2, 2:]) * eye_obj_trans[2]
eye_obj_trans[:2] = np.multiply(
eye_obj_trans[:2],
[[1 / self._camera_mat[0, 0]], [1 / self._camera_mat[1, 1]]])
result = base_cam_trans * np.linalg.inv(
self._rtool_tool_trans) * self._rtool_eye_trans * eye_obj_trans
res = eye2baseResponse()
res.tar_pose = np.array(np.multiply(result[:3], 100)).reshape(-1)
return res
def __down_pix2base_transform(self, req):
self.__get_robot_trans()
eye_obj_trans = np.mat(np.append(np.array(req.ini_pose), [self.down_cam_obj_z, 1])).reshape(4, 1)
eye_obj_trans[:2] = (eye_obj_trans[:2] - self._camera_mat[:2, 2:]) * eye_obj_trans[2]
eye_obj_trans[:2] = np.multiply(eye_obj_trans[:2], [[1/self._camera_mat[0, 0]], [1/self._camera_mat[1, 1]]])
result = self.down_cam_trans * eye_obj_trans
res = eye2baseResponse()
res.tar_pose = np.array(np.multiply(result[:3], 100)).reshape(-1)
return res
def __pix2base_transform(self, req):
self.__get_robot_trans()
eye_obj_trans = np.mat(np.append(np.array(req.ini_pose), 1)).reshape(4, 1)
eye_obj_trans[2] = eye_obj_trans[2] * 0.01
eye_obj_trans[:2] = (eye_obj_trans[:2] - self._camera_mat[:2, 2:]) * eye_obj_trans[2]
eye_obj_trans[:2] = np.multiply(eye_obj_trans[:2], [[1/self._camera_mat[0, 0]], [1/self._camera_mat[1, 1]]])
result = self._base_tool_trans * np.linalg.inv(self._rtool_tool_trans) * self._rtool_eye_trans * eye_obj_trans
res = eye2baseResponse()
res.tar_pose = np.array(np.multiply(result[:3], 100)).reshape(-1)
return res
def __eye_trans2base_transform(self, req):
self.__get_robot_trans()
eye_obj_trans = np.mat(req.ini_pose).reshape(4, 4)
result = self._base_tool_trans * np.linalg.inv(
self._rtool_tool_trans) * self._rtool_eye_trans * eye_obj_trans
res = eye2baseResponse()
res.tar_pose = np.array(result).reshape(-1)
print('result\\n', result)
print('result\\n', res.tar_pose)
return res
def __eye_trans2base_transform(self, req):
self.__get_robot_trans()
assert len(req.ini_pose) == 16
eye_obj_trans = np.mat(req.ini_pose).reshape(4, 4)
result = self._base_tool_trans * np.linalg.inv(self._rtool_tool_trans) * self._rtool_eye_trans * eye_obj_trans
res = eye2baseResponse()
res.trans = np.array(result).reshape(-1)
res.pos = np.array(result[2, :3]).reshape(-1)
res.euler = self.__rotation2rpy(result)
return res
def __eye2base_transform(self, req):
self.__get_robot_trans()
assert len(req.ini_pose) == 3
eye_obj_trans = np.mat(np.append(np.array(req.ini_pose), 1)).reshape(4, 1)
# eye_obj_trans[:3] = np.multiply(eye_obj_trans[:3], 0.01)
result = self._base_tool_trans * np.linalg.inv(self._rtool_tool_trans) * self._rtool_eye_trans * eye_obj_trans
res = eye2baseResponse()
res.trans = np.mat(np.identity(4))
res.trans[2, :] = result
res.pos = np.array(result[:3]).reshape(-1)
res.euler = self.__rotation2rpy(res.trans)
return res
def __pix2base_transform(self, req):
self.__get_robot_trans()
assert len(req.ini_pose) == 3
eye_obj_trans = np.mat(np.append(np.array(req.ini_pose), 1)).reshape(4, 1)
# eye_obj_trans[2] = eye_obj_trans[2] * 0.01
eye_obj_trans[:2] = (eye_obj_trans[:2] - self._camera_mat[:2, 2:]) * eye_obj_trans[2]
eye_obj_trans[:2] = np.multiply(eye_obj_trans[:2], [[1/self._camera_mat[0, 0]], [1/self._camera_mat[1, 1]]])
result = self._base_tool_trans * np.linalg.inv(self._rtool_tool_trans) * self._rtool_eye_trans * eye_obj_trans
res = eye2baseResponse()
res.trans = np.mat(np.identity(4))
res.trans[2, :] = result
res.pos = np.array(result[:3]).reshape(-1)
res.euler = self.__rotation2rpy(res.trans)
return res
def __eye2base_transform(self, req):
self.__get_robot_trans()
eye_obj_trans = np.mat(np.append(np.array(req.ini_pose),
1)).reshape(4, 1)
eye_obj_trans[:3] = np.multiply(eye_obj_trans[:3], 0.01)
print('eye_obj_trans/n', eye_obj_trans)
result = self._base_tool_trans * np.linalg.inv(
self._rtool_tool_trans) * self._rtool_eye_trans * eye_obj_trans
print('_base_tool_trans/n', self._base_tool_trans)
print('_rtool_tool_trans/n', np.linalg.inv(self._rtool_tool_trans))
print('_rtool_eye_trans/n', self._rtool_eye_trans)
print('self.eye_obj_trans/n', eye_obj_trans)
res = eye2baseResponse()
res.tar_pose = np.array(np.multiply(result[:3], 100)).reshape(-1)
return res
def __eye_trans2base_transform(self, req):
print("====================================")
print("self._curr_pose: ", self._curr_pose)
self.__get_robot_trans()
eye_obj_trans = np.mat(req.ini_pose).reshape(4, 4)
result = self._base_tool_trans * np.linalg.inv(self._rtool_tool_trans) * self._rtool_eye_trans * eye_obj_trans
print('eye_obj_trans/n', eye_obj_trans)
print('_base_tool_trans/n', self._base_tool_trans)
print('_rtool_tool_trans/n', np.linalg.inv(self._rtool_tool_trans))
print('_rtool_eye_trans/n', self._rtool_eye_trans)
print('self.eye_obj_trans/n', eye_obj_trans)
print('1\n', self._base_tool_trans * np.linalg.inv(self._rtool_tool_trans))
print('2\n', self._base_tool_trans * np.linalg.inv(self._rtool_tool_trans) * self._rtool_eye_trans)
res = eye2baseResponse()
res.tar_pose = np.array(result).reshape(-1)
print('result\\n', result)
print('result\\n', res.tar_pose)
return res