def sample_surface(self, radius=0.005, nsample=None, toggle_option='face_ids'): """ :param raidus: :param toggle_option; 'face_ids', 'normals', None :return: author: weiwei date: 20191228 """ if self._objtrm is None: raise ValueError("Only applicable to models with a trimesh!") if nsample is None: nsample = int(round(self.objtrm.area / ((radius * 0.3)**2))) points, face_ids = self.objtrm.sample_surface(nsample, radius=radius, toggle_faceid=True) # transform points = rm.homomat_transform_points(self.get_homomat(), points) if toggle_option is None: return np.array(points) elif toggle_option == 'face_ids': return np.array(points), np.array(face_ids) elif toggle_option == 'normals': return np.array(points), rm.homomat_transform_points( self.get_homomat(), self.objtrm.face_normals[face_ids]) else: print( "toggle_option must be None, point_face_ids, or point_nromals")
def gettwoend(self): self.endpairs = [] self.temporigin = [] self.origin = [] self.homomat = [] for i, coordinate in enumerate(self.coordinate): endpair = [] axis = coordinate.T[2] large = 0 large_vec = np.array([0, 0, 0]) small = 0 small_vec = np.array([0, 0, 0]) facet_1_ID = self.doubleholdpair[i][0] facet_2_ID = self.doubleholdpair[i][1] vertices = np.append(self.largeface_vertices[facet_1_ID], self.largeface_vertices[facet_2_ID], axis=0) for vertice in vertices: value = np.dot(axis, vertice) if value >= large: large = value large_vec = vertice elif value < small: small = value small_vec = vertice endpair.append(large_vec) endpair.append(small_vec) self.temporigin.append((large_vec + small_vec) * 0.5) self.endpairs.append(endpair) origin = self.getorigin(normal_list=[ self.largeface_normals[facet_1_ID], self.largeface_normals[facet_2_ID], axis ], point_list=[ self.largefacescenter[facet_1_ID], self.largefacescenter[facet_2_ID], (large_vec + small_vec) * 0.5 ]) self.origin.append(origin) homomat = np.linalg.inv(rm.homomat_from_posrot(origin, coordinate)) self.pRotMat.append(homomat) self.pCoM.append(rm.homomat_transform_points(homomat, self.com)) tempvertice = [] for vertice in vertices: tempvertice.append( rm.homomat_transform_points(homomat, vertice)) self.pVertices.append(tempvertice) self.pNormals.append([ np.dot(coordinate.T, self.largeface_normals[facet_1_ID]), np.dot(coordinate.T, self.largeface_normals[facet_2_ID]) ]) self.pFacetpairscenter.append([ rm.homomat_transform_points(homomat, self.largefacescenter[facet_1_ID]), rm.homomat_transform_points(homomat, self.largefacescenter[facet_2_ID]) ])
def genHexahedralVectors(coefficient,polygon = 6, normal = np.array([0,0,1]), Fdistributed=1): '''generate vectors for simulating the friction cone''' initalMatrix = np.zeros([polygon,3]) # initVector =pg.rm.unit_vector(np.array([0,coefficient,1])) initVector = Fdistributed*np.array([0, coefficient, 1]) # tfMatrix = pg.trigeom.align_vectors(vector_start=np.array([0,0,1]),vector_end=-normal) tfMatrix = trigeom.align_vectors(vector_start=np.array([0, 0, 1]), vector_end=normal) initalMatrix[0,:] = rm.homomat_transform_points(tfMatrix,initVector) angle = 360.0/ polygon for i in range(1,polygon): rotMat = rm.rotmat_from_axangle([0,0,1],angle*i) initalMatrix[i,:] = rm.homomat_transform_points(tfMatrix,np.dot(rotMat,initVector)) return initalMatrix
def extract_rotated_vvnf(self): if self.cdmesh_type == 'aabb': objtrm = self.objtrm.bounding_box elif self.cdmesh_type == 'obb': objtrm = self.objtrm.bounding_box_oriented elif self.cdmesh_type == 'convex_hull': objtrm = self.objtrm.convex_hull elif self.cdmesh_type == 'triangles': objtrm = self.objtrm homomat = self.get_homomat() vertices = rm.homomat_transform_points(homomat, objtrm.vertices) vertex_normals = rm.homomat_transform_points(homomat, objtrm.vertex_normals) faces = objtrm.faces return vertices, vertex_normals, faces
def cdmesh(self): vertices = [] vertex_normals = [] faces = [] for objcm in self._cm_list: if objcm.cdmesh_type == 'aabb': objtrm = objcm.objtrm.bounding_box elif objcm.cdmesh_type == 'obb': objtrm = objcm.objtrm.bounding_box_oriented elif objcm.cdmesh_type == 'convexhull': objtrm = objcm.objtrm.convex_hull elif objcm.cdmesh_type == 'triangles': objtrm = objcm.objtrm homomat = objcm.get_homomat() vertices += rm.homomat_transform_points(homomat, objtrm.vertices) vertex_normals += rm.homomat_transform_points(homomat, objtrm.vertex_normals) faces += (objtrm.faces+len(faces)) return mcd.gen_cdmesh_vvnf(vertices, vertex_normals, faces)
def sethomomat(self, homomat): """ set the pose of the dynamic body :param homomat: the homomat of the original frame (the collision model) :return: author: weiwei date: 2019?, 20201119 """ pos = rm.homomat_transform_points(homomat, self.com) rotmat = homomat[:3, :3] self.setTransform(TransformState.makeMat(dh.npv3mat3_to_pdmat4(pos, rotmat)))
def set_homomat(self, homomat): """ set the pose of the dynamic body :param homomat: the homomat of the original frame (the collision model) :return: author: weiwei date: 2019?, 20201119 """ tmp_homomat = copy.deepcopy(homomat) tmp_homomat[:3, 3] = tmp_homomat[:3, 3] * base.physics_scale pos = rm.homomat_transform_points(tmp_homomat, self.com) rotmat = tmp_homomat[:3, :3] self.setTransform( TransformState.makeMat(dh.npv3mat3_to_pdmat4(pos, rotmat)))
def extract_rotated_vvnf(self, cdmesh_type=None): """ allow either extract a vvnf following the specified cdmesh_type or the value of self.cdmesh_type :param cdmesh_type: :return: author: weiwei date: 20211215 """ if cdmesh_type is None: cdmesh_type = self.cdmesh_type if cdmesh_type == 'aabb': objtrm = self.objtrm.bounding_box elif cdmesh_type == 'obb': objtrm = self.objtrm.bounding_box_oriented elif cdmesh_type == 'convex_hull': objtrm = self.objtrm.convex_hull elif cdmesh_type == 'triangles': objtrm = self.objtrm homomat = self.get_homomat() vertices = rm.homomat_transform_points(homomat, objtrm.vertices) vertex_normals = rm.homomat_transform_points(homomat, objtrm.vertex_normals) faces = objtrm.faces return vertices, vertex_normals, faces
def update(pk_obj, pcd_list, ball_center_list, counter, task): if len(pcd_list) != 0: for pcd in pcd_list: pcd.detach() pcd_list.clear() # if len(ball_center_list) != 0: # for ball_center in ball_center_list: # ball_center.detach() # ball_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: break point_cloud = pk_obj.transform_depth_image_to_point_cloud( depth_image_handle) point_cloud = rm.homomat_transform_points(affine_matrix, point_cloud) ball = [] for id, point_cloud_sub in enumerate(point_cloud): if 0.3 < point_cloud_sub[0] < 3.3 and -1.3 < point_cloud_sub[ 1] < .3 and 0.5 < point_cloud_sub[2] < 2.5: ball.append(point_cloud_sub) mypoint_cloud = gm.GeometricModel(initor=point_cloud) mypoint_cloud.attach_to(base) pcd_list.append(mypoint_cloud) if len(ball) > 20: center = np.mean(np.array(ball), axis=0) ball_center_list.append([center, counter[0]]) ball = gm.gen_sphere(center, radius=.1) ball.attach_to(base) ball_list.append(ball) if len(ball_center_list) > 2: x = [] y = [] z = [] t = [] for cen, f_id in ball_center_list: x.append(cen[0]) y.append(cen[1]) z.append(cen[2]) t.append(f_id) para_x = np.polyfit(t, x, 1) para_y = np.polyfit(t, y, 1) para_z = np.polyfit(t, z, 2) f_x = np.poly1d(para_x) f_y = np.poly1d(para_y) f_z = np.poly1d(para_z) orbit = [] for t in np.linspace(ball_center_list[0][1], ball_center_list[0][1] + 5, 100): orbit.append(np.array([f_x(t), f_y(t), f_z(t)])) for id in range(len(orbit)): if id > 0: tmp_stick = gm.gen_stick(spos=orbit[id - 1], epos=orbit[id], thickness=.01, type="round") tmp_stick.attach_to(base) para_list.append(tmp_stick) return task.done pk_obj.image_release(color_image_handle) pk_obj.image_release(depth_image_handle) pk_obj.capture_release() counter[0] += 1 return task.cont
# action_rotmat_list = [] # for i in range(10): # action_pos_list.append(pos[i] + pos_start) # action_rotmat_list.append(np.dot(rm.rotmat_from_axangle(rot[i][0], rot[i][1]), # rot_start)) # # print(marker_pos_in_hnd) # pos_arm_tcp, rot_arm_tcp = xss.arm.jlc.lnks[7]["gl_pos"], xss.arm.jlc.lnks[7]["gl_rotmat"] # pos_hnd_tcp, rot_hand_tcp = xss.get_gl_tcp(manipulator_name="arm") # # gm.gen_frame(pos_arm_tcp, rot_arm_tcp).attach_to(base) # # gm.gen_frame(pos_hnd_tcp, rot_hand_tcp).attach_to(base) # marker_pos_in_hnd = np.array([-0.026, 0, 0.069]) - np.array([0, 0, pos_hnd_tcp[2] - pos_arm_tcp[2]]) # # print("marker in hand", marker_pos_in_hnd) # calibration_r = dc.calibrate(component_name="arm", # sensor_marker_handler=sensor_handler, # marker_pos_in_hnd=marker_pos_in_hnd, # action_pos_list=action_pos_list, # action_rotmat_list=action_rotmat_list) # print(calibration_r) # base.run() # validate pcd from vision.depth_camera.calibrator import load_calibration_data affine_matrix, _, _ = load_calibration_data() gm.GeometricModel(initor=rm.homomat_transform_points( affine_matrix, sensor_handler.get_point_cloud())).attach_to(base) # # planner # rrtc_planner = rrtc.RRTConnect(xss) base.run()
# 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()
def update(pkx, rbtx, background_image, pcd_list, ball_center_list, counter, task): if len(pcd_list) != 0: for pcd in pcd_list: pcd.detach() pcd_list.clear() while True: pkx.device_get_capture() color_image_handle = pkx.capture_get_color_image() depth_image_handle = pkx.capture_get_depth_image() if color_image_handle and depth_image_handle: break point_cloud = pkx.transform_depth_image_to_point_cloud(depth_image_handle) point_cloud = rm.homomat_transform_points(affine_matrix, point_cloud) ball = [] for id, point_cloud_sub in enumerate(point_cloud): if 0.3 < point_cloud_sub[0] < 3.3 and -1.3 < point_cloud_sub[ 1] < .3 and 0.5 < point_cloud_sub[2] < 2.5: ball.append(point_cloud_sub) mypoint_cloud = gm.GeometricModel(initor=point_cloud) mypoint_cloud.attach_to(base) pcd_list.append(mypoint_cloud) if len(ball) > 20: center = np.mean(np.array(ball), axis=0) ball_center_list.append([center, counter[0]]) ball = gm.gen_sphere(center, radius=.1) ball.attach_to(base) ball_list.append(ball) if len(ball_center_list) > 2: x = [] y = [] z = [] t = [] for cen, f_id in ball_center_list: x.append(cen[0]) y.append(cen[1]) z.append(cen[2]) t.append(f_id) para_x = np.polyfit(t, x, 1) para_y = np.polyfit(t, y, 1) para_z = np.polyfit(t, z, 2) f_x = np.poly1d(para_x) f_y = np.poly1d(para_y) f_z = np.poly1d(para_z) orbit = [] for t in np.linspace(ball_center_list[0][1], ball_center_list[0][1] + 15, 100): point = np.array([f_x(t), f_y(t), f_z(t)]) orbit.append(point) if abs(point[0]) < .7 and abs( point[1]) < .7 and abs(point[2] - 1.1) < .3: # last_point = orbit[-1] jnt_values = rbts.ik(tgt_pos=point, tgt_rotmat=np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]])) if jnt_values is None: continue rbts.fk(component_name="arm", jnt_values=jnt_values) rbts.gen_meshmodel().attach_to(base) rbtx.arm_move_jspace_path( [rbtx.get_jnt_values(), jnt_values], max_jntspeed=math.pi * 1.3) break for id in range(len(orbit)): if id > 0: tmp_stick = gm.gen_stick(spos=orbit[id - 1], epos=orbit[id], thickness=.01, type="round") tmp_stick.attach_to(base) para_list.append(tmp_stick) return task.done pkx.image_release(color_image_handle) pkx.image_release(depth_image_handle) pkx.capture_release() counter[0] += 1 return task.cont