def get_gl_pose_from_hio(self, component_name, hio_pos, hio_rotmat):
"""
get the global pose of an object from a grasp pose described in an object's local frame
:param hio_pos: a grasp pose described in an object's local frame -- pos
:param hio_rotmat: a grasp pose described in an object's local frame -- rotmat
:return:
author: weiwei
date: 20210302
"""
if component_name != 'arm':
raise ValueError(
"Component name for Xarm7ShuidiRobot must be \'arm\'!")
hnd_pos = self.arm.jnts[-1]['gl_posq']
hnd_rotmat = self.arm.jnts[-1]['gl_rotmatq']
hnd_homomat = rm.homomat_from_posrot(hnd_pos, hnd_rotmat)
hio_homomat = rm.homomat_from_posrot(hio_pos, hio_rotmat)
oih_homomat = rm.homomat_inverse(hio_homomat)
gl_obj_homomat = hnd_homomat.dot(oih_homomat)
return gl_obj_homomat[:3, 3], gl_obj_homomat[:3, :3]
def get_gl_pose_from_hio(self, hio_pos, hio_rotmat, component_name='lft_arm'):
"""
get the loc pose of an object from a grasp pose described in an object's local frame
:param hio_pos: a grasp pose described in an object's local frame -- pos
:param hio_rotmat: a grasp pose described in an object's local frame -- rotmat
:return:
author: weiwei
date: 20210302
"""
if component_name is 'lft_arm':
arm = self.lft_arm
elif component_name is 'rgt_arm':
arm = self.rgt_arm
hnd_pos = arm.jnts[-1]['gl_posq']
hnd_rotmat = arm.jnts[-1]['gl_rotmatq']
hnd_homomat = rm.homomat_from_posrot(hnd_pos, hnd_rotmat)
hio_homomat = rm.homomat_from_posrot(hio_pos, hio_rotmat)
oih_homomat = rm.homomat_inverse(hio_homomat)
gl_obj_homomat = hnd_homomat.dot(oih_homomat)
return gl_obj_homomat[:3, 3], gl_obj_homomat[:3, :3]
# pcd_list = []
# marker_center_list = []
# def update(pk_obj, pcd_list, marker_center_list, task):
# if len(pcd_list) != 0:
# for pcd in pcd_list:
# pcd.detach()
# pcd_list.clear()
# if len(marker_center_list) != 0:
# for marker_center in marker_center_list:
# marker_center.detach()
# marker_center_list.clear()
while True:
pk_obj.device_get_capture()
color_image_handle = pk_obj.capture_get_color_image()
depth_image_handle = pk_obj.capture_get_depth_image()
if color_image_handle and depth_image_handle:
color_image = pk_obj.image_convert_to_numpy(color_image_handle)
# cv2.imshow("test", color_image)
# cv2.waitKey(0)
point_cloud = pk_obj.transform_depth_image_to_point_cloud(depth_image_handle)
point_cloud = rm.homomat_transform_points(rm.homomat_inverse(origin_homomat), point_cloud)
point_cloud[point_cloud[:,0]<-1]=point_cloud[point_cloud[:,0]<-1]*0
mypoint_cloud = gm.GeometricModel(initor=point_cloud)
mypoint_cloud.attach_to(base)
base.run()
pk_obj.image_release(color_image_handle)
pk_obj.image_release(depth_image_handle)
pk_obj.capture_release()
base.run()
