def gen_dashstick(spos=np.array([0, 0, 0]), epos=np.array([0.1, 0, 0]), thickness=0.005, lsolid=None, lspace=None, sections=8, sticktype="rect"): """ :param spos: 1x3 nparray :param epos: 1x3 nparray :param thickness: 0.005 m by default :param lsolid: length of the solid section, 1*thickness if None :param lspace: length of the empty section, 1.5*thickness if None :return: author: weiwei date: 20191228osaka """ solidweight = 1.6 spaceweight = 1.07 if not lsolid: lsolid = thickness * solidweight if not lspace: lspace = thickness * spaceweight length, direction = rm.unit_vector(epos - spos, toggle_length=True) nstick = math.floor(length / (lsolid + lspace)) vertices = np.empty((0, 3)) faces = np.empty((0, 3)) for i in range(0, nstick): tmp_spos = spos + (lsolid * direction + lspace * direction) * i tmp_stick = gen_stick(spos=tmp_spos, epos=tmp_spos + lsolid * direction, thickness=thickness, type=sticktype, sections=sections) tmp_stick_faces = tmp_stick.faces + len(vertices) vertices = np.vstack((vertices, tmp_stick.vertices)) faces = np.vstack((faces, tmp_stick_faces)) # wrap up the last segment tmp_spos = spos + (lsolid * direction + lspace * direction) * nstick tmp_epos = tmp_spos + lsolid * direction final_length, _ = rm.unit_vector(tmp_epos - spos, toggle_length=True) if final_length > length: tmp_epos = epos tmp_stick = gen_stick(spos=tmp_spos, epos=tmp_epos, thickness=thickness, type=sticktype, sections=sections) tmp_stick_faces = tmp_stick.faces + len(vertices) vertices = np.vstack((vertices, tmp_stick.vertices)) faces = np.vstack((faces, tmp_stick_faces)) return trm.Trimesh(vertices=vertices, faces=faces)
def suction_to_with_sczy(self, gl_suction_center_pos, gl_suction_center_z, gl_suction_center_y): """ :param gl_suction_center_pos: :param gl_suction_center_z: jaw_center's approaching direction :param gl_suction_center_y: jaw_center's opening direction :param jaw_width: :return: author: weiwei date: 20220127 """ gl_jaw_center_rotmat = np.eye(3) gl_jaw_center_rotmat[:, 2] = rm.unit_vector(gl_suction_center_z) gl_jaw_center_rotmat[:, 1] = rm.unit_vector(gl_suction_center_y) gl_jaw_center_rotmat[:, 0] = np.cross(gl_jaw_center_rotmat[:3, 1], gl_jaw_center_rotmat[:3, 2]) return self.suction_to_with_scpose(gl_suction_center_pos, gl_suction_center_z)
def gen_arrow(spos=np.array([0, 0, 0]), epos=np.array([0.1, 0, 0]), thickness=0.005, sections=8, sticktype="rect"): """ :param spos: 1x3 nparray :param epos: 1x3 nparray :param thickness: 0.005 m by default :param sections: # of discretized sectors used to approximate a cylinder :param sticktype: The shape at the end of the arrow stick, round or rect :param radius: :return: author: weiwei date: 20191228osaka """ direction = rm.unit_vector(epos - spos) stick = gen_stick(spos=spos, epos=epos - direction * thickness * 4, thickness=thickness, type=sticktype, sections=sections) cap = gen_cone(spos=epos - direction * thickness * 4, epos=epos, radius=thickness, sections=sections) vertices = np.vstack((stick.vertices, cap.vertices)) capfaces = cap.faces + len(stick.vertices) faces = np.vstack((stick.faces, capfaces)) return trm.Trimesh(vertices=vertices, faces=faces)
def drawTheCovexHull(constraints, pos=[150, 150, 150]): print("The constraints are:",constraints) print("The len is",len(constraints)) base.pggen.plotAxis(base.render, spos=np.array(pos), length=80, thickness=3) convexVetices = [np.array(pos)] convexVeticesNormal = [] for i in range(0, len(constraints)): base.pggen.plotArrow(base.render, spos=pos, epos=pos + constraints[i] * 40, thickness=4, rgba=[0,0,0, 0.7]) convexVetices.append((pos + constraints[i] * 40)) convexVeticesNormal.append(constraints[i]) trimeshNew = trimesh.Trimesh(vertices=np.array(convexVetices)) try: convexHull = trimeshNew.convex_hull convexHullpd = pg.trimeshtonp(convexHull) convexHullpd.setColor(0,0,0,0.2 ) convexHullpd.setTransparency(TransparencyAttrib.MAlpha) convexHullpd.reparentTo(base.render) except: print("Cannot generate the convex hull") bestDirection = np.array([0, 0, 0]) for z in constraints: bestDirection = bestDirection + z bestDirection = rm.unit_vector(bestDirection) base.pggen.plotArrow(base.render, spos=pos, epos=pos + bestDirection * 40, thickness=4, rgba=[147.0 / 255, 112.0 / 255, 219.0 / 255, 0.7]) print("The best value is ", bestDirection)
def gen_dasharrow(spos=np.array([0, 0, 0]), epos=np.array([0.1, 0, 0]), thickness=0.005, lsolid=None, lspace=None, sections=8, sticktype="rect"): """ :param spos: 1x3 nparray :param epos: 1x3 nparray :param thickness: 0.005 m by default :param lsolid: length of the solid section, 1*thickness if None :param lspace: length of the empty section, 1.5*thickness if None :return: author: weiwei date: 20191228osaka """ length, direction = rm.unit_vector(epos - spos, toggle_length=True) cap = gen_cone(spos=epos - direction * thickness * 4, epos=epos, radius=thickness, sections=sections) dash_stick = gen_dashstick(spos=spos, epos=epos - direction * thickness * 4, thickness=thickness, lsolid=lsolid, lspace=lspace, sections=sections, sticktype=sticktype) tmp_stick_faces = dash_stick.faces + len(cap.vertices) vertices = np.vstack((cap.vertices, dash_stick.vertices)) faces = np.vstack((cap.faces, tmp_stick_faces)) return trm.Trimesh(vertices=vertices, faces=faces)
def rayhit_closet(pfrom, pto, objcm): """ :param pfrom: :param pto: :param objcm: :return: author: weiwei date: 20190805 """ tgt_cdmesh = gen_cdmesh_vvnf(*objcm.extract_rotated_vvnf()) ray = OdeRayGeom(length=1) length, dir = rm.unit_vector(pto - pfrom, toggle_length=True) ray.set(pfrom[0], pfrom[1], pfrom[2], dir[0], dir[1], dir[2]) ray.setLength(length) contact_entry = OdeUtil.collide(ray, tgt_cdmesh, max_contacts=10) contact_points = [ da.pdv3_to_npv3(point) for point in contact_entry.getContactPoints() ] min_id = np.argmin(np.linalg.norm(pfrom - np.array(contact_points), axis=1)) contact_normals = [ da.pdv3_to_npv3(contact_entry.getContactGeom(i).getNormal()) for i in range(contact_entry.getNumContacts()) ] return contact_points[min_id], contact_normals[min_id]
def grip_at_with_jczy(self, gl_jaw_center_pos, gl_jaw_center_z, gl_jaw_center_y, jaw_width): """ :param gl_jaw_center_pos: :param gl_jaw_center_z: jaw_center's approaching direction :param gl_jaw_center_y: jaw_center's opening direction :param jaw_width: :return: """ gl_jaw_center_rotmat = np.eye(3) gl_jaw_center_rotmat[:, 2] = rm.unit_vector(gl_jaw_center_z) gl_jaw_center_rotmat[:, 1] = rm.unit_vector(gl_jaw_center_y) gl_jaw_center_rotmat[:, 0] = np.cross(gl_jaw_center_rotmat[:3, 1], gl_jaw_center_rotmat[:3, 2]) return self.grip_at_with_jcpose(gl_jaw_center_pos, gl_jaw_center_rotmat, jaw_width)
def _extend_conf(self, conf1, conf2, ext_dist): """ :param conf1: :param conf2: :param ext_dist: :return: a list of 1xn nparray """ len, vec = rm.unit_vector(conf2 - conf1, toggle_length=True) return conf1 + ext_dist * vec if len > 1e-6 else None
def normal_from_3point(p1, p2, p3): x1, y1, z1 = p1[0], p1[1], p1[2] x2, y2, z2 = p2[0], p2[1], p2[2] x3, y3, z3 = p3[0], p3[1], p3[2] # print(x3, y3, z3) a = (y2 - y1) * (z3 - z1) - (y3 - y1) * (z2 - z1) b = (z2 - z1) * (x3 - x1) - (z3 - z1) * (x2 - x1) c = (x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1) return rm.unit_vector(np.array([a, b, c]))
def cylinder_link_start_end(start, end, radius = 0.0003): start = start end = end height = np.linalg.norm(end - start, ord = 2, axis = 0, keepdims=True)[0] vector = rm.unit_vector(end - start) rot3 = rm.rotmat_between_vectors(np.array([0,0,1]),vector) rot4 = rm.homomat_from_posrot(pos = start, rot = rot3) cylinder = cm.gen_cylinder(radius=radius, height=height, section=30, homomat=rot4) return cylinder
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)
def specialMax(a,b,n): """ :param a: 1 by 3 np array :param b: 1 by 3 np array :return: """ crosspd = np.cross(a, b) if np.allclose(rm.unit_vector(crosspd),n): return a else: return b
def gen_rel_linear_motion_with_given_conf(self, component_name, goal_jnt_values, direction, distance, obstacle_list=[], granularity=0.03, seed_jnt_values=None, type='sink', toggle_debug=False): """ :param goal_info: :param direction: :param distance: :param obstacle_list: :param granularity: :param type: 'sink', or 'source' :return: author: weiwei date: 20210114 """ goal_tcp_pos, goal_tcp_rotmat = self.robot_s.cvt_conf_to_tcp( component_name, goal_jnt_values) if type == 'sink': start_tcp_pos = goal_tcp_pos - rm.unit_vector(direction) * distance start_tcp_rotmat = goal_tcp_rotmat return self.gen_linear_motion(component_name, start_tcp_pos, start_tcp_rotmat, goal_tcp_pos, goal_tcp_rotmat, obstacle_list, granularity, seed_jnt_values, toggle_debug=toggle_debug) elif type == 'source': start_tcp_pos = goal_tcp_pos start_tcp_rotmat = goal_tcp_rotmat goal_tcp_pos = goal_tcp_pos + direction * distance goal_tcp_rotmat = goal_tcp_rotmat return self.gen_linear_motion(component_name, start_tcp_pos, start_tcp_rotmat, goal_tcp_pos, goal_tcp_rotmat, obstacle_list, granularity, seed_jnt_values, toggle_debug=toggle_debug) else: raise ValueError("Type must be sink or source!")
def rayhit_all(pfrom, pto, objcm): """ :param pfrom: :param pto: :param objcm: :return: author: weiwei date: 20190805 """ tgt_cdmesh = gen_cdmesh_vvnf(*objcm.extract_rotated_vvnf()) ray = OdeRayGeom(length=1) length, dir = rm.unit_vector(pto-pfrom, toggle_length=True) ray.set(pfrom[0], pfrom[1], pfrom[2], dir[0], dir[1], dir[2]) ray.setLength(length) hit_entry = OdeUtil.collide(ray, tgt_cdmesh) hit_points = [da.pdv3_to_npv3(point) for point in hit_entry.getContactPoints()] hit_normals = [da.pdv3_to_npv3(hit_entry.getContactGeom(i).getNormal()) for i in range(hit_entry.getNumContacts())] return hit_points, hit_normals
def _extend_conf(self, conf1, conf2, ext_dist, exact_end=True): """ :param conf1: :param conf2: :param ext_dist: :return: a list of 1xn nparray """ len, vec = rm.unit_vector(conf2 - conf1, toggle_length=True) # one step extension: not adopted because it is slower than full extensions, 20210523, weiwei # return [conf1 + ext_dist * vec] # switch to the following code for ful extensions if not exact_end: nval = math.ceil(len / ext_dist) nval = 1 if nval == 0 else nval # at least include itself conf_array = np.linspace(conf1, conf1 + nval * ext_dist * vec, nval) else: nval = math.floor(len / ext_dist) nval = 1 if nval == 0 else nval # at least include itself conf_array = np.linspace(conf1, conf1 + nval * ext_dist * vec, nval) conf_array = np.vstack((conf_array, conf2)) return list(conf_array)
def _extend_conf(self, conf1, conf2, ext_dist): """ WARNING: This extend_conf is specially designed for differential-wheel robots :param conf1: :param conf2: :param ext_dist: :return: a list of 1xn nparray author: weiwei date: 20210530 """ angle_ext_dist = ext_dist len, vec = rm.unit_vector(conf2[:2] - conf1[:2], toggle_length=True) if len > 0: translational_theta = rm.angle_between_2d_vectors( np.array([1, 0]), vec) conf1_theta_to_translational_theta = translational_theta - conf1[2] else: conf1_theta_to_translational_theta = (conf2[2] - conf1[2]) translational_theta = conf2[2] # rotate nval = abs( math.ceil(conf1_theta_to_translational_theta / angle_ext_dist)) linear_conf1 = np.array([conf1[0], conf1[1], translational_theta]) conf1_angular_arary = np.linspace(conf1, linear_conf1, nval) # translate nval = math.ceil(len / ext_dist) linear_conf2 = np.array([conf2[0], conf2[1], translational_theta]) conf12_linear_arary = np.linspace(linear_conf1, linear_conf2, nval) # rotate translational_theta_to_conf2_theta = conf2[2] - translational_theta nval = abs( math.ceil(translational_theta_to_conf2_theta / angle_ext_dist)) conf2_angular_arary = np.linspace(linear_conf2, conf2, nval) conf_array = np.vstack( (conf1_angular_arary, conf12_linear_arary, conf2_angular_arary)) return list(conf_array)
def generate_matrix(self): for id in self.node_infos.keys(): matrix_id_infos = {} matrix_id_infos["parity"] = self.node_infos[id]["parity"] if self.node_infos[id]["parity"] == "even-even": # matrix_id_infos["top"] = self.node_infos[id]["rgt"] + np.array([-self.origin_offset, 0, 0]) # matrix_id_infos["bottom"] = self.node_infos[id]["lft"] + np.array([self.origin_offset, 0, 0]) matrix_id_infos["top"] = self.node_infos[id]["rgt"] + rm.unit_vector(self.node_infos[id]["origin"]- self.node_infos[id]["rgt"]) * self.origin_offset matrix_id_infos["bottom"] = self.node_infos[id]["lft"] + rm.unit_vector(self.node_infos[id]["origin"]- self.node_infos[id]["lft"]) * self.origin_offset matrix_id_infos["center1"] = self.node_infos[id]["up"] + np.array([0, -self.origin_offset, 0]) matrix_id_infos["center2"] = self.node_infos[id]["origin"] + np.array([0, 0, self.height-self.origin_offset]) matrix_id_infos["center3"] = self.node_infos[id]["down"] + np.array([0, self.origin_offset, 0]) matrix_id_infos["center4"] = self.node_infos[id]["origin"] + np.array([0, 0, -self.height+self.origin_offset]) matrix_id_infos["center1"] = self.node_infos[id]["up"] + rm.unit_vector(self.node_infos[id]["origin"]- self.node_infos[id]["up"]) * self.origin_offset matrix_id_infos["center2"] = self.node_infos[id]["height"] + rm.unit_vector(self.node_infos[id]["origin"]- self.node_infos[id]["height"]) * self.origin_offset matrix_id_infos["center3"] = self.node_infos[id]["down"] + rm.unit_vector(self.node_infos[id]["origin"]- self.node_infos[id]["down"]) * self.origin_offset matrix_id_infos["center4"] = self.node_infos[id]["low"] + rm.unit_vector(self.node_infos[id]["origin"]- self.node_infos[id]["low"]) * self.origin_offset elif self.node_infos[id]["parity"] == "odd-even": # matrix_id_infos["top"] = self.node_infos[id]["rgt"] + np.array([-self.origin_offset, 0, 0]) # matrix_id_infos["bottom"] = self.node_infos[id]["lft"] + np.array([self.origin_offset, 0, 0]) matrix_id_infos["top"] = self.node_infos[id]["rgt"] + rm.unit_vector(self.node_infos[id]["origin"] - self.node_infos[id]["rgt"]) * self.origin_offset matrix_id_infos["bottom"] = self.node_infos[id]["lft"] + rm.unit_vector(self.node_infos[id]["origin"] - self.node_infos[id]["lft"]) * self.origin_offset # matrix_id_infos["center1"] = self.node_infos[id]["up"] + np.array([0, -(0.005-self.origin_offset)/1.414, (0.005-self.origin_offset)/1.414]) # matrix_id_infos["center2"] = self.node_infos[id]["origin"] + np.array([0, -(0.005-self.origin_offset)/1.414, (0.005-self.origin_offset)/1.414]) # matrix_id_infos["center3"] = self.node_infos[id]["down"] + np.array([0, (0.005-self.origin_offset)/1.414, -(0.005-self.origin_offset)/1.414]) # matrix_id_infos["center4"] = self.node_infos[id]["origin"] + np.array([0, (0.005-self.origin_offset)/1.414, -(0.005-self.origin_offset)/1.414]) matrix_id_infos["center1"] = self.node_infos[id]["origin"] + ( self.node_infos[id]["up"] - self.node_infos[id]["origin"] + np.array( [0, -self.origin_offset, self.height - self.origin_offset])) / 1.414 matrix_id_infos["center2"] = self.node_infos[id]["origin"] + ( self.node_infos[id]["down"] - self.node_infos[id]["origin"] + np.array( [0, self.origin_offset, self.height - self.origin_offset])) / 1.414 matrix_id_infos["center3"] = self.node_infos[id]["origin"] + ( self.node_infos[id]["down"] - self.node_infos[id]["origin"] + np.array( [0, self.origin_offset, -self.height + self.origin_offset])) / 1.414 matrix_id_infos["center4"] = self.node_infos[id]["origin"] + ( self.node_infos[id]["up"] - self.node_infos[id]["origin"] + np.array( [0, -self.origin_offset, -self.height + self.origin_offset])) / 1.414 elif self.node_infos[id]["parity"] == "even-odd": # matrix_id_infos["top"] = self.node_infos[id]["up"] + np.array([0, -self.origin_offset, 0]) # matrix_id_infos["bottom"] = self.node_infos[id]["down"] + np.array([0, self.origin_offset, 0]) matrix_id_infos["top"] = self.node_infos[id]["up"] + rm.unit_vector( self.node_infos[id]["origin"] - self.node_infos[id]["up"]) * self.origin_offset matrix_id_infos["bottom"] = self.node_infos[id]["down"] + rm.unit_vector( self.node_infos[id]["origin"] - self.node_infos[id]["down"]) * self.origin_offset matrix_id_infos["center1"] = self.node_infos[id]["origin"] + (self.node_infos[id]["rgt"] - self.node_infos[id]["origin"] + np.array([-self.origin_offset, 0, self.height-self.origin_offset]))/ 1.414 matrix_id_infos["center2"] = self.node_infos[id]["origin"] + (self.node_infos[id]["lft"] - self.node_infos[id]["origin"] + np.array([self.origin_offset, 0, self.height -self.origin_offset])) / 1.414 matrix_id_infos["center3"] = self.node_infos[id]["origin"] + (self.node_infos[id]["lft"] - self.node_infos[id]["origin"] + np.array([self.origin_offset, 0, -self.height +self.origin_offset])) / 1.414 matrix_id_infos["center4"] = self.node_infos[id]["origin"] + (self.node_infos[id]["rgt"] - self.node_infos[id]["origin"] + np.array([-self.origin_offset, 0, -self.height + self.origin_offset])) / 1.414 # matrix_id_infos["center1"] = self.node_infos[id]["origin"] + np.array([-(0.005-self.origin_offset)/1.414, 0, (0.005-self.origin_offset)/1.414]) # matrix_id_infos["center2"] = self.node_infos[id]["origin"] + np.array([-(0.005-self.origin_offset)/1.414, 0, -(0.005-self.origin_offset)/1.414]) # matrix_id_infos["center3"] = self.node_infos[id]["origin"] + np.array([(0.005-self.origin_offset)/1.414, 0, -(0.005-self.origin_offset)/1.414]) # matrix_id_infos["center4"] = self.node_infos[id]["origin"] + np.array([(0.005-self.origin_offset)/1.414, 0, (0.005-self.origin_offset)/1.414]) else: pass self.node_matrix_infos[id] = matrix_id_infos
t_npt = cpt+direction*.07/n # gm.gen_arrow(spos=t_npt, epos=t_npt+last_normal*.015, thickness=0.001, rgba=[1, 1, 0, 1]).attach_to(base) nearby_sample_ids = tree.query_ball_point(t_npt, .005) nearby_samples = bowl_samples[nearby_sample_ids] # gm.GeometricModel(nearby_samples).attach_to(base) 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)) homomat = np.eye(4) homomat[:3,:3]=plane_rotmat homomat[:3,3]=plane_center # twod_plane = gm.gen_box(np.array([.2, .2, .001]), homomat=homomat, rgba=[.5,.7,1,.3]).attach_to(base) projected_point = rm.project_to_plane(t_npt, plane_center, plane_normal) # gm.gen_stick(t_npt, projected_point, thickness=.002).attach_to(base) new_normal = rm.unit_vector(t_npt-projected_point) if pn_direction.dot(new_normal) > .1: new_normal = -new_normal # gm.gen_arrow(spos=projected_point, epos=projected_point+new_normal*.015, thickness=0.001).attach_to(base) angle = rm.angle_between_vectors(last_normal, new_normal) vec = rm.unit_vector(np.cross(last_normal, new_normal)) new_rotmat = rm.rotmat_from_axangle(vec, angle) direction = new_rotmat.dot(direction) tmp_direction = new_rotmat.dot(tmp_direction) # new_line_segs = [[cpt, cpt+direction*(.07-tick*.07/n)], # [cpt+direction*(.07-tick*.07/n), cpt+direction*(.07-tick*.07/n)+tmp_direction*.07]] # gm.gen_linesegs(new_line_segs).attach_to(base) gm.gen_stick(spos=cpt, epos=projected_point, rgba=[1,.6,0,1], thickness=.002, type='round').attach_to(base) cpt=projected_point last_normal = new_normal # break
bpgm.set_vert_size(.01) bpgm1.attach_to(base) bpgm2.attach_to(base) lsgm = gen_linesegs([[np.array([.1, 0, .01]), np.array([.01, 0, .01])], [np.array([.01, 0, .01]), np.array([.1, 0, .1])], [np.array([.1, 0, .1]), np.array([.1, 0, .01])]]) lsgm.attach_to(base) gen_circarrow(radius=.1, portion=.8).attach_to(base) gen_dasharrow(spos=np.array([0, 0, 0]), epos=np.array([0, 0, 2])).attach_to(base) gen_dashframe(pos=np.array([0, 0, 0]), rotmat=np.eye(3)).attach_to(base) axmat = rm.rotmat_from_axangle([1, 1, 1], math.pi / 4) gen_frame(rotmat=axmat).attach_to(base) axmat[:, 0] = .1 * axmat[:, 0] axmat[:, 1] = .07 * axmat[:, 1] axmat[:, 2] = .3 * axmat[:, 2] gen_ellipsoid(pos=np.array([0, 0, 0]), axmat=axmat).attach_to(base) print(rm.unit_vector(np.array([0, 0, 0]))) pos = np.array([.3, 0, 0]) rotmat = rm.rotmat_from_euler(math.pi / 6, 0, 0) homomat = rm.homomat_from_posrot(pos, rotmat) gen_frame_box([.1, .2, .3], homomat).attach_to(base) base.run()
break pcd_pnt_list.append(pcd_pnt) if flag or len(pcd_pnt_list) < 3: continue print(pcd_pnt_list) id_origin = 2 id_x = 0 id_minus_y = 3 for i, id in enumerate(ids): if id == 2: origin_xyz = pcd_pnt_list[i] if id == 0: plusx_xyz = pcd_pnt_list[i] if id == 3: minusy_xyz = pcd_pnt_list[i] axis_x = rm.unit_vector(plusx_xyz - origin_xyz) axis_y = rm.unit_vector(origin_xyz - minusy_xyz) axis_z = rm.unit_vector(np.cross(axis_x, axis_y)) origin = origin_xyz origin_rotmat = np.eye(3) origin_rotmat[:, 0] = axis_x origin_rotmat[:, 1] = axis_y origin_rotmat[:, 2] = axis_z pickle.dump([origin, origin_rotmat], open("origin.pkl", "wb")) gm.gen_frame(pos=origin_xyz, rotmat=origin_rotmat, length=1, thickness=.03).attach_to(base) # for image_xy in image_xy_list: # cv2.circle(color_image, tuple(image_xy), 10, (255, 0, 0), -1) # cv2.imshow("test", color_image)
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()
def generate_matrix(self): for y in range(2, self.grid.get_wid() - 2): for x in range(2, self.grid.get_len() - 2): id = f"{x}-{y}" matrix_id_infos = {} matrix_id_infos["parity"] = self.node_infos[id]["parity"] matrix_id_infos["origin"] = self.node_infos[id]["origin"] if self.node_infos[id]["parity"] == "even-even": matrix_id_infos[ "top"] = self.node_infos[id]["rgt"] + rm.unit_vector( self.node_infos[id]["origin"] - self.node_infos[id]["rgt"]) * self.origin_offset matrix_id_infos["bottom"] = self.node_infos[id][ "lft"] + rm.unit_vector( self.node_infos[id]["origin"] - self.node_infos[id]["lft"]) * self.origin_offset matrix_id_infos["center1"] = self.node_infos[id][ "up"] + rm.unit_vector( self.node_infos[id]["origin"] - self.node_infos[id]["up"]) * self.origin_offset matrix_id_infos["center2"] = self.node_infos[id][ "height"] + rm.unit_vector( self.node_infos[id]["origin"] - self.node_infos[id]["height"]) * self.origin_offset matrix_id_infos["center3"] = self.node_infos[id][ "down"] + rm.unit_vector( self.node_infos[id]["origin"] - self.node_infos[id]["down"]) * self.origin_offset matrix_id_infos["center4"] = self.node_infos[id][ "low"] + rm.unit_vector( self.node_infos[id]["origin"] - self.node_infos[id]["low"]) * self.origin_offset elif self.node_infos[id]["parity"] == "odd-even": matrix_id_infos[ "top"] = self.node_infos[id]["rgt"] + rm.unit_vector( self.node_infos[id]["origin"] - self.node_infos[id]["rgt"]) * self.origin_offset matrix_id_infos["bottom"] = self.node_infos[id][ "lft"] + rm.unit_vector( self.node_infos[id]["origin"] - self.node_infos[id]["lft"]) * self.origin_offset xy_list = id.split("-") x = int(xy_list[0]) y = int(xy_list[1]) matrix_id_infos[ "center1"] = self.node_infos[id]["origin"] + cpt_vec( self.node_infos[f"{x}-{y+1}"]["height"], self.node_infos[f"{x}-{y}"]["origin"], self.origin_offset) matrix_id_infos[ "center2"] = self.node_infos[id]["origin"] + cpt_vec( self.node_infos[f"{x}-{y -1}"]["height"], self.node_infos[f"{x}-{y}"]["origin"], self.origin_offset) matrix_id_infos[ "center3"] = self.node_infos[id]["origin"] + cpt_vec( self.node_infos[f"{x}-{y-1}"]["low"], self.node_infos[f"{x}-{y}"]["origin"], self.origin_offset) matrix_id_infos[ "center4"] = self.node_infos[id]["origin"] + cpt_vec( self.node_infos[f"{x}-{y+1}"]["low"], self.node_infos[f"{x}-{y}"]["origin"], self.origin_offset) elif self.node_infos[id]["parity"] == "even-odd": matrix_id_infos[ "top"] = self.node_infos[id]["up"] + rm.unit_vector( self.node_infos[id]["origin"] - self.node_infos[id]["up"]) * self.origin_offset matrix_id_infos["bottom"] = self.node_infos[id][ "down"] + rm.unit_vector( self.node_infos[id]["origin"] - self.node_infos[id]["down"]) * self.origin_offset xy_list = id.split("-") x = int(xy_list[0]) y = int(xy_list[1]) matrix_id_infos[ "center1"] = self.node_infos[id]["origin"] + cpt_vec( self.node_infos[f"{x + 1}-{y}"]["height"], self.node_infos[f"{x}-{y}"]["origin"], self.origin_offset) matrix_id_infos[ "center2"] = self.node_infos[id]["origin"] + cpt_vec( self.node_infos[f"{x-1}-{y}"]["height"], self.node_infos[f"{x}-{y}"]["origin"], self.origin_offset) matrix_id_infos[ "center3"] = self.node_infos[id]["origin"] + cpt_vec( self.node_infos[f"{x-1}-{y}"]["low"], self.node_infos[f"{x}-{y}"]["origin"], self.origin_offset) matrix_id_infos[ "center4"] = self.node_infos[id]["origin"] + cpt_vec( self.node_infos[f"{x+1}-{y}"]["low"], self.node_infos[f"{x}-{y}"]["origin"], self.origin_offset) else: pass self.node_matrix_infos[id] = matrix_id_infos
def cpt_vec(vec_a, vec_b, offset): result_vec = (vec_a - vec_b - rm.unit_vector(vec_a - vec_b) * offset) / 1.414 return result_vec
def checkSymbol(a,b,n): crosspd = np.cross(a, b) if np.allclose(rm.unit_vector(crosspd), n,atol=0.00001) or np.allclose(crosspd, np.array([0,0,0]),atol=0.00001): return 1 else: return -1
# cross_vec = rm.unit_vector(np.cross(np.array([0.1,0,1]), rotmat[:3,2])) cross_vec = rotmat2.dot(rotmat[:3,0]) base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0], toggle_debug=True) # base = wd.World(cam_pos=ax*2, lookat_pos=[0, 0, 0], toggle_debug=True) gm.gen_arrow(epos=ax*.3, rgba=[0,0,0,.3]).attach_to(base) gm.gen_frame(length=.2).attach_to(base) # gm.gen_dasharrow(epos=cross_vec*.3, rgba=[1,1,0,1], lsolid=.01, lspace=.0077).attach_to(base) gm.gen_arrow(epos=cross_vec*.3, rgba=[1,1,0,1]).attach_to(base) gm.gen_sphere(radius=.005, pos=cross_vec*.3, rgba=[0,0,0,1]).attach_to(base) nxt_vec_uvw = rotmat2.dot(cross_vec) gm.gen_dasharrow(epos=nxt_vec_uvw*.3, rgba=[1,1,0,1]).attach_to(base) # gm.gen_arrow(epos=nxt_vec_uvw*.3, rgba=[1,1,0,1]).attach_to(base) gm.gen_sphere(radius=.005, pos=nxt_vec_uvw*.3, rgba=[0,0,0,1]).attach_to(base) radius, _ = rm.unit_vector(cross_vec * .3 - cross_vec.dot(ax) * ax * .3, toggle_length=True) gm.gen_arrow(spos=cross_vec.dot(ax)*ax*.3, epos = cross_vec*.3, rgba=[1,.47,0,.5]).attach_to(base) gm.gen_dasharrow(spos=cross_vec.dot(ax)*ax*.3, epos = nxt_vec_uvw*.3, rgba=[1,.47,0,.5]).attach_to(base) gm.gen_arrow(epos=ax*math.sqrt(.3**2-radius**2), rgba=[0,0,0,1]).attach_to(base) ## projections # gm.gen_dasharrow(spos = ax*math.sqrt(.3**2-radius**2), # epos=ax*math.sqrt(.3**2-radius**2)+np.cross(ax, cross_vec*.3)*math.sin(math.pi/6), # rgba=[1,0,1,.5]).attach_to(base) # gm.gen_dasharrow(spos = ax*math.sqrt(.3**2-radius**2), # epos=ax*math.sqrt(.3**2-radius**2)+(cross_vec*.3-cross_vec.dot(ax)*ax*.3)*math.cos(math.pi/6), # rgba=[0,1,1,.5]).attach_to(base) # rectangle epos_vec = rm.unit_vector(cross_vec*.3-cross_vec.dot(ax)*ax*.3) gm.gen_stick(spos=cross_vec.dot(ax)*ax*.3-ax*.03,
break projected_point = tmp_projected_point t_npt = tmp_t_npt domain_grid = np.meshgrid(np.linspace(-.005, .005, 100, endpoint=True), np.linspace(-.005, .005, 100, endpoint=True)) domain_0, domain_1 = domain_grid domain = np.column_stack( (domain_0.ravel() + t_npt[0], domain_1.ravel() + t_npt[1])) codomain = surface.get_zdata(domain) vertices = np.column_stack((domain, codomain)) plane_center, plane_normal = rm.fit_plane(vertices) new_normal = plane_normal if pn_direction.dot(new_normal) > .1: new_normal = -new_normal angle = rm.angle_between_vectors(-pn_direction, new_normal) vec = rm.unit_vector(np.cross(-pn_direction, new_normal)) new_rotmat = rm.rotmat_from_axangle(vec, angle) direction = new_rotmat.dot(direction) gm.gen_stick(spos=cpt, epos=projected_point, rgba=[1, .6, 0, 1], thickness=.002, type='round').attach_to(base) cpt = projected_point # last_normal = new_normal direction = new_rotmat.dot(tmp_direction) for tick in range(1, n + 1): len = .05 / n tmp_cpt = cpt extended_len = 0
z_surface = surface.get_zdata(np.array([t_npt[:2]])) projected_point = np.array([t_npt[0], t_npt[1], z_surface[0]]) domain_grid = np.meshgrid(np.linspace(-.005, .005, 100, endpoint=True), np.linspace(-.005, .005, 100, endpoint=True)) domain_0, domain_1 = domain_grid domain = np.column_stack( (domain_0.ravel() + t_npt[0], domain_1.ravel() + t_npt[1])) codomain = surface.get_zdata(domain) vertices = np.column_stack((domain, codomain)) plane_center, plane_normal = rm.fit_plane(vertices) new_normal = plane_normal if pn_direction.dot(new_normal) > .1: new_normal = -new_normal # gm.gen_arrow(spos=projected_point, epos=projected_point+plane_normal*.015).attach_to(base) angle = rm.angle_between_vectors(last_normal, new_normal) vec = rm.unit_vector(np.cross(last_normal, new_normal)) new_rotmat = rm.rotmat_from_axangle(vec, angle) direction = new_rotmat.dot(direction) gm.gen_stick(spos=cpt, epos=projected_point, rgba=[1, .6, 0, 1], thickness=.002, type='round').attach_to(base) tmp_len = np.linalg.norm(projected_point - cpt) extended_len += tmp_len cpt = projected_point last_normal = new_normal print(tick, extended_len) if extended_len > .05: break # break
def pushpose(self, axisvec, pushpoint, toggle_debug=False): pushposelist = [] pushpose_rotmatlist = [] zaxis = np.array([0, 0, 1]) axisvec_norm = np.linalg.norm(axisvec) ## 円錐の中心のベクトル theta = 5 degree = 90 ## 30 handrotate = 180 ## 30 thetamax = 30 ## 60 thetarange = int(thetamax / theta) degreerange = int(360 / degree) handrotaterange = int(360 / handrotate) for i in range(thetarange): referencevec = axisvec + (axisvec_norm * math.tan(math.radians(theta * (i + 1)))) * zaxis referencepoint = pushpoint + referencevec ## プッシングする点からの相対座標に変換して、クォータニオンを求める q_refvec = Quaternion(0, referencepoint[0] - pushpoint[0], referencepoint[1] - pushpoint[1], referencepoint[2] - pushpoint[2]) for j in range(degreerange): q_axis = Quaternion(axis=rm.unit_vector(axisvec), degrees=degree * (j + 1)) ## 回転クォータニオン q_new = q_axis * q_refvec * q_axis.inverse ## 絶対座標に戻す point = np.array([ q_new[1] + pushpoint[0], q_new[2] + pushpoint[1], q_new[3] + pushpoint[2] ]) # base.pggen.plotSphere(base.render, pos=point, radius=10, rgba=[0,0,1,1]) handdir = pushpoint - point handdir_projection = copy.copy(handdir) ## xy平面への正射影 handdir_projection[2] = 0 handdir_projection = rm.unit_vector(handdir_projection) ## ハンド座標系の各要素となるベクトル handdir = rm.unit_vector(handdir) ## z thumb_verticalvec = np.cross(zaxis, handdir_projection) ## x zaxis_hand = np.cross(handdir, thumb_verticalvec) ## y # pushposelist.append(self.rtq85.approachAt(5,5,5,thumb_verticalvec[0], thumb_verticalvec[1], thumb_verticalvec[2], # handdir[0], handdir[1], handdir[2], jawwidth=0)) ## ハンドの方向を軸に-90度ずつ回転した姿勢を生成 for k in range(handrotaterange): handrotmat = np.empty((0, 3)) ## test # handrotmat = np.append(handrotmat, np.array([handdir]), axis=0) # handrotmat = np.append(handrotmat, np.array([thumb_verticalvec]), axis=0) # handrotmat = np.append(handrotmat, np.array([zaxis_hand]), axis=0) handrotmat = np.append(handrotmat, np.array([thumb_verticalvec]), axis=0) handrotmat = np.append(handrotmat, np.array([zaxis_hand]), axis=0) handrotmat = np.append(handrotmat, np.array([handdir]), axis=0) handrotmat = handrotmat.T handrotmat = np.dot( rm.rotmat_from_axangle(handrotmat[:, 2], -math.radians(handrotate * k)), handrotmat) pushposelist.append( self.rtq85.grip_at_with_jczy( np.array([.005, .005, .005]), np.array([ handrotmat[:, 2][0], handrotmat[:, 2][1], handrotmat[:, 2][2] ]), np.array([ handrotmat[:, 0][0], handrotmat[:, 0][1], handrotmat[:, 0][2] ]), jaw_width=self.rtq85.jawwidth_rng[0])) if toggle_debug: self.rtq85.copy().gen_meshmodel().attach_to(base) pushpose_rotmatlist.append(handrotmat) return pushpose_rotmatlist
thickness=0.001, rgba=[1, 1, 0, 1]).attach_to(base) nearby_sample_ids = tree.query_ball_point(t_npt, .005) nearby_samples = bowl_samples[nearby_sample_ids] # gm.GeometricModel(nearby_samples).attach_to(base) 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) homomat = np.eye(4) homomat[:3, :3] = plane_rotmat homomat[:3, 3] = plane_center # twod_plane = gm.gen_box(np.array([.2, .2, .001]), homomat=homomat, rgba=[.5,.7,1,.1]).attach_to(base) projected_point = rm.project_to_plane(t_npt, plane_center, plane_normal) new_normal = rm.unit_vector(t_npt - projected_point) gm.gen_arrow(spos=projected_point, epos=projected_point + new_normal * .015, thickness=0.001).attach_to(base) angle = rm.angle_between_vectors(-pn_direction, new_normal) vec = np.cross(-pn_direction, new_normal) new_rotmat = rm.rotmat_from_axangle(vec, angle) direction = new_rotmat.dot(pt_direction) # new_tmp_direction = new_rotmat.dot(tmp_direction) new_line_segs = [[cpt, projected_point]] # gm.gen_stick(spos=cpt, epos=t_npt, rgba=[1,0,1,1], thickness=.002, type='round').attach_to(base) gm.gen_stick(spos=cpt, epos=projected_point, rgba=[1, .6, 0, 1], thickness=.002, type='round').attach_to(base)