def plan_grasps(hnd_s, objcm, angle_between_contact_normals=math.radians(160), openning_direction = 'loc_x', rotation_interval=math.radians(22.5), max_samples=100, min_dist_between_sampled_contact_points=.005, contact_offset=.002): """ :param objcm: :param hnd_s: :param angle_between_contact_normals: :param openning_direction: 'loc_x' or 'loc_y' depending on gripper types :param rotation_granularity: :param max_samples: :param min_dist_between_sampled_contact_points: :param contact_offset: offset at the cotnact to avoid being closely in touch with object surfaces :return: a list [[jawwidth, gl_jaw_center_pos, pos, rotmat], ...] """ contact_pairs = plan_contact_pairs(objcm, max_samples=max_samples, min_dist_between_sampled_contact_points=min_dist_between_sampled_contact_points, angle_between_contact_normals=angle_between_contact_normals) grasp_info_list = [] import modeling.geometric_model as gm for i, cp in enumerate(contact_pairs): print(f"{i} of {len(contact_pairs)} done!") contact_p0, contact_n0 = cp[0] contact_p1, contact_n1 = cp[1] contact_center = (contact_p0 + contact_p1) / 2 jaw_width = np.linalg.norm(contact_p0 - contact_p1) + contact_offset * 2 if jaw_width > hnd_s.jawwidth_rng[1]: continue if openning_direction == 'loc_x': jaw_center_x = contact_n0 jaw_center_z = rm.orthogonal_vector(contact_n0) jaw_center_y = np.cross(jaw_center_z, jaw_center_x) elif openning_direction == 'loc_y': jaw_center_y = contact_n0 jaw_center_z = rm.orthogonal_vector(contact_n0) else: raise ValueError("Openning direction must be loc_x or loc_y!") grasp_info_list += gu.define_grasp_with_rotation(hnd_s, objcm, gl_jaw_center_pos=contact_center, gl_jaw_center_z=jaw_center_z, gl_jaw_center_y=jaw_center_y, jaw_width=jaw_width, gl_rotation_ax=contact_n0, rotation_interval=rotation_interval, toggle_flip=True) return grasp_info_list
def gen_dashtorus(axis=np.array([1, 0, 0]), portion=.5, center=np.array([0, 0, 0]), radius=0.1, thickness=0.005, lsolid=None, lspace=None, sections=8, discretization=24): """ :param axis: the circ arrow will rotate around this axis 1x3 nparray :param portion: 0.0~1.0 :param center: the center position of the circ 1x3 nparray :param radius: :param thickness: :param lsolid: length of solid :param lspace: length of space :param sections: # of discretized sectors used to approximate a cylindrical stick :param discretization: number sticks used for approximating a torus :return: author: weiwei date: 20200602 """ assert (0 <= portion <= 1) solidweight = 1.6 spaceweight = 1.07 if not lsolid: lsolid = thickness * solidweight if not lspace: lspace = thickness * spaceweight unit_axis = rm.unit_vector(axis) starting_vector = rm.orthogonal_vector(unit_axis) min_discretization_value = math.ceil(2 * math.pi / (lsolid + lspace)) if discretization < min_discretization_value: discretization = min_discretization_value nsections = math.floor(portion * 2 * math.pi * radius / (lsolid + lspace)) vertices = np.empty((0, 3)) faces = np.empty((0, 3)) for i in range(0, nsections): # TODO wrap up end torus_sec = gen_torus( axis=axis, starting_vector=rm.rotmat_from_axangle( axis, 2 * math.pi * portion / nsections * i).dot(starting_vector), portion=portion / nsections * lsolid / (lsolid + lspace), center=center, radius=radius, thickness=thickness, sections=sections, discretization=discretization) torus_sec_faces = torus_sec.faces + len(vertices) vertices = np.vstack((vertices, torus_sec.vertices)) faces = np.vstack((faces, torus_sec_faces)) return trm.Trimesh(vertices=vertices, faces=faces)
bowl_model.set_rgba([.3, .3, .3, .3]) bowl_model.set_rotmat(rm.rotmat_from_euler(math.pi, 0, 0)) # bowl_model.attach_to(base) pn_direction = np.array([0, 0, -1]) bowl_samples, bowl_sample_normals = bowl_model.sample_surface( toggle_option='normals', radius=.002) selection = bowl_sample_normals.dot(-pn_direction) > .1 bowl_samples = bowl_samples[selection] bowl_sample_normals = bowl_sample_normals[selection] tree = cKDTree(bowl_samples) surface = rbfs.RBFSurface(bowl_samples[:, :2], bowl_samples[:, 2]) surface.get_gometricmodel(rgba=[.3, .3, .3, .3]).attach_to(base) pt_direction = rm.orthogonal_vector(pn_direction, toggle_unit=True) tmp_direction = np.cross(pn_direction, pt_direction) plane_rotmat = np.column_stack((pt_direction, tmp_direction, pn_direction)) homomat = np.eye(4) homomat[:3, :3] = plane_rotmat homomat[:3, 3] = np.array([-.07, -.03, .1]) twod_plane = gm.gen_box(np.array([.2, .2, .001]), homomat=homomat, rgba=[1, 1, 1, .3]) twod_plane.attach_to(base) circle_radius = .05 line_segs = [[homomat[:3, 3], homomat[:3, 3] + pt_direction * .05], [ homomat[:3, 3] + pt_direction * .05, homomat[:3, 3] + pt_direction * .05 + tmp_direction * .05
base = wd.World(cam_pos=np.array([-.3,-.7,.42]), lookat_pos=np.array([0,0,0])) # gm.gen_frame().attach_to(base) bowl_model = cm.CollisionModel(initor="./objects/bowl.stl") bowl_model.set_rgba([.3,.3,.3,1]) bowl_model.set_rotmat(rm.rotmat_from_euler(math.pi,0,0)) bowl_model.attach_to(base) pn_direction = np.array([0, 0, -1]) bowl_samples, bowl_sample_normals = bowl_model.sample_surface(toggle_option='normals', radius=.002) selection = bowl_sample_normals.dot(-pn_direction)>.1 bowl_samples = bowl_samples[selection] bowl_sample_normals=bowl_sample_normals[selection] tree = cKDTree(bowl_samples) pt_direction = rm.orthogonal_vector(pn_direction, toggle_unit=True) tmp_direction = np.cross(pn_direction, pt_direction) plane_rotmat = np.column_stack((pt_direction, tmp_direction, pn_direction)) homomat=np.eye(4) homomat[:3,:3] = plane_rotmat homomat[:3,3] = np.array([-.07,-.03,.1]) twod_plane = gm.gen_box(np.array([.2, .2, .001]), homomat=homomat, rgba=[1,1,1,.3]) twod_plane.attach_to(base) circle_radius=.05 line_segs = [[homomat[:3,3], homomat[:3,3]+pt_direction*.07], [homomat[:3,3]+pt_direction*.07, homomat[:3,3]+pt_direction*.07+tmp_direction*.07], [homomat[:3,3]+pt_direction*.07+tmp_direction*.07, homomat[:3,3]+tmp_direction*.07], [homomat[:3,3]+tmp_direction*.07, homomat[:3,3]]] # gm.gen_linesegs(line_segs).attach_to(base) for sec in line_segs: gm.gen_stick(spos=sec[0], epos=sec[1], rgba=[0, 0, 0, 1], thickness=.002, type='round').attach_to(base) epos = (line_segs[0][1]-line_segs[0][0])*.7+line_segs[0][0]
def gen_circarrow(axis=np.array([1, 0, 0]), starting_vector=None, portion=0.3, center=np.array([0, 0, 0]), radius=0.005, thickness=0.0015, sections=8, discretization=24): """ :param axis: the circ arrow will rotate around this axis 1x3 nparray :param portion: 0.0~1.0 :param center: the center position of the circ 1x3 nparray :param radius: :param thickness: :param rgba: :param discretization: number sticks used for approximation :return: author: weiwei date: 20200602 """ unitaxis = rm.unit_vector(axis) if starting_vector is None: starting_vector = rm.orthogonal_vector(unitaxis) else: starting_vector = rm.unit_vector(starting_vector) starting_pos = starting_vector * radius + center discretizedangle = 2 * math.pi / discretization ndist = int(portion * discretization) # gen the last arrow first # gen the remaining torus if ndist > 0: lastpos = center + np.dot( rm.rotmat_from_axangle(unitaxis, (ndist - 1) * discretizedangle), starting_vector) * radius nxtpos = center + np.dot( rm.rotmat_from_axangle(unitaxis, ndist * discretizedangle), starting_vector) * radius arrow = gen_arrow(spos=lastpos, epos=nxtpos, thickness=thickness, sections=sections, sticktype="round") vertices = arrow.vertices faces = arrow.faces lastpos = starting_pos for i in range(1 * np.sign(ndist), ndist, 1 * np.sign(ndist)): nxtpos = center + np.dot( rm.rotmat_from_axangle(unitaxis, i * discretizedangle), starting_vector) * radius stick = gen_stick(spos=lastpos, epos=nxtpos, thickness=thickness, sections=sections, type="round") stickfaces = stick.faces + len(vertices) vertices = np.vstack((vertices, stick.vertices)) faces = np.vstack((faces, stickfaces)) lastpos = nxtpos return trm.Trimesh(vertices=vertices, faces=faces) else: return trm.Trimesh()
# bowl_model.attach_to(base) # base.run() tree = cKDTree(bowl_samples) point_id = 7000 nearby_sample_ids = tree.query_ball_point(bowl_samples[point_id, :], 10) nearby_samples = bowl_samples[nearby_sample_ids] colors = np.tile(np.array([1, 0, 0, 1]), (len(nearby_samples), 1)) print(nearby_samples.shape) print(colors.shape) # nearby_samples_withcolor = np.column_stack((nearby_samples, colors)) # gm.GeometricModel(nearby_samples_withcolor).attach_to(base) pcdu.show_pcd(nearby_samples, rgba=(1, 0, 0, 1)) plane_center, plane_normal = rm.fit_plane(nearby_samples) plane_tangential = rm.orthogonal_vector(plane_normal) plane_tmp = np.cross(plane_normal, plane_tangential) plane_rotmat = np.column_stack((plane_tangential, plane_tmp, plane_normal)) nearby_samples_on_xy = plane_rotmat.T.dot((nearby_samples - plane_center).T).T surface = gs.MixedGaussianSurface(nearby_samples_on_xy[:, :2], nearby_samples_on_xy[:, 2], n_mix=1) # t_npt_on_xy = plane_rotmat.T.dot(t_npt - plane_center) # projected_t_npt_z_on_xy = surface.get_zdata(np.array([t_npt_on_xy[:2]])) # projected_t_npt_on_xy = np.array([t_npt_on_xy[0], t_npt_on_xy[1], projected_t_npt_z_on_xy[0]]) # projected_point = plane_rotmat.dot(projected_t_npt_on_xy) + plane_center surface_gm = surface.get_gometricmodel([[-50, 50], [-50, 50]], rgba=[.5, .7, 1, 1]) surface_gm.setpos(plane_center) surface_gm.setrotmat(plane_rotmat) surface_gm.reparentTo(base.render)
rgba=rgba).attach_to(base) # gm.gen_dasharrow(contact_p1, contact_p1+contact_n1*.03, thickness=.0012, rgba=rgba).attach_to(base) # base.run() gripper_s = rtq85.Robotiq85() contact_offset = .002 grasp_info_list = [] for i, cp in enumerate(contact_pairs): print(f"{i} of {len(contact_pairs)} done!") contact_p0, contact_n0 = cp[0] contact_p1, contact_n1 = cp[1] contact_center = (contact_p0 + contact_p1) / 2 jaw_width = np.linalg.norm(contact_p0 - contact_p1) + contact_offset * 2 if jaw_width > gripper_s.jawwidth_rng[1]: continue hndy = contact_n0 hndz = rm.orthogonal_vector(contact_n0) grasp_info_list += gu.define_grasp_with_rotation( gripper_s, object_bunny, gl_jaw_center_pos=contact_center, gl_jaw_center_z=hndz, gl_jaw_center_y=hndy, jaw_width=jaw_width, gl_rotation_ax=hndy, rotation_interval=math.radians(30), toggle_flip=True) for grasp_info in grasp_info_list: aw_width, gl_jaw_center, hnd_pos, hnd_rotmat = grasp_info gripper_s.fix_to(hnd_pos, hnd_rotmat) gripper_s.jaw_to(aw_width) gripper_s.gen_meshmodel().attach_to(base)