def showmultiface(self, base, num): length = 0.02 for i, facet in enumerate(self.pFaces[num]): color = (random.random(), random.random(), random.random(), 1) for j, face in enumerate(facet): # color = (random.random(), random.random(), random.random(), 1) gm.gen_sphere(face[0][0:3], radius=.005, rgba=color).attach_to(base) gm.gen_sphere(face[1][0:3], radius=.005, rgba=color).attach_to(base) gm.gen_sphere(face[2][0:3], radius=.005, rgba=color).attach_to(base) hufunc.drawanySingleSurface(base, vertices=np.array(face), color=color) gm.gen_arrow(spos=np.array([ self.pFacetpairscenter[num][i][0], self.pFacetpairscenter[num][i][1], self.pFacetpairscenter[num][i][2] ]), epos=np.array([ self.pFacetpairscenter[num][i][0] + length * self.pNormals[num][i][0], self.pFacetpairscenter[num][i][1] + length * self.pNormals[num][i][1], self.pFacetpairscenter[num][i][2] + length * self.pNormals[num][i][2] ]), thickness=0.005, rgba=color)
def showsingleNormal(self, num): color = (random.random(), random.random(), random.random(), 1) # length = random.randint(60,100) length = 0.05 self.__showSptPnt(self.holdingpairsSptPnt[num], color=color) for i, facetpair in enumerate(self.hpairs[num]): gm.gen_arrow(spos=np.array([ self.largefacetscenter[facetpair][0], self.largefacetscenter[facetpair][1], self.largefacetscenter[facetpair][2] ]), epos=([ self.largefacetscenter[facetpair][0] + length * self.largefacet_normals[facetpair][0], self.largefacetscenter[facetpair][1] + length * self.largefacet_normals[facetpair][1], self.largefacetscenter[facetpair][2] + length * self.largefacet_normals[facetpair][2] ]), thickness=0.003, rgba=color).attach_to(base) for face in self.facets[facetpair]: self.drawSingleFaceSurface(base, vertices=self.vertices, faces=self.faces[face], color=color)
def update(textNode, obj_show_node, counter, task): if base.inputmgr.keymap['space'] is True: print("space") # time.sleep(0.5) if counter[0] >= len(freehold.placementRotMat): counter[0] = 0 if obj_show_node[0] is not None: # obj_show_node[0].detach() obj_show_node[1].detachNode() obj_show_node[2].detachNode() obj_show_node[3].detachNode() obj_show_node[0] = cm.CollisionModel(objpath) obj_show_node[0].set_scale((0.001, 0.001, 0.001)) obj_show_node[0].set_rgba((0, 191 / 255, 1, 0.5)) obj_show_node[0].set_homomat(freehold.placementRotMat[counter[0]]) # obj_show_node[0].attach_to(base) obj_show_node[1] = NodePath("com") obj_show_node[2] = NodePath("vertice") obj_show_node[3] = NodePath("normal") if obj_show_node[0].is_mcdwith(fixturecm): obj_show_node[0].set_rgba((1, 0, 0, 0.5)) else: obj_show_node[0].attach_to(base) # gm.gen_sphere(pos = freehold.placementCoM[counter[0]]).attach_to(obj_show_node[1]) obj_show_node[1].reparentTo(base.render) # for vertace in freehold.placementVertices[counter[0]]: # gm.gen_sphere(pos=vertace, radius=0.005).attach_to(obj_show_node[2]) obj_show_node[2].reparentTo(base.render) for i, normal in enumerate(freehold.placementNormals[counter[0]]): print(normal) gm.gen_arrow( spos=freehold.placementfacetpairscenter[counter[0]][i], epos=freehold.placementfacetpairscenter[counter[0]][i] + normal * 0.1).attach_to(obj_show_node[3]) obj_show_node[3].reparentTo(base.render) counter[0] += 1 if textNode[0] is not None: textNode[0].detachNode() textNode[1].detachNode() textNode[2].detachNode() cam_pos = base.cam.getPos() textNode[0] = OnscreenText(text=str(cam_pos[0])[0:5], fg=(1, 0, 0, 1), pos=(1.0, 0.8), align=TextNode.ALeft) textNode[1] = OnscreenText(text=str(cam_pos[1])[0:5], fg=(0, 1, 0, 1), pos=(1.3, 0.8), align=TextNode.ALeft) textNode[2] = OnscreenText(text=str(cam_pos[2])[0:5], fg=(0, 0, 1, 1), pos=(1.6, 0.8), align=TextNode.ALeft) return task.again
def showfeatures(self, node, normal, placementfacetpairscenter, placementVertices, placementCoM, showCoM, showNormal, showVertices): np.random.seed(0) rgb = np.random.rand(3) # color = (random.random(),random.random(),random.random(),1) color = (rgb[0], rgb[1], rgb[2], 1) # length = random.randint(60,100) length = 0.050 # self.__showSptPnt(self.holdingpairsSptPnt[i], color=color) if showNormal == True: for i in range(len(normal)): gm.gen_arrow( spos=np.array([ placementfacetpairscenter[i][0], placementfacetpairscenter[i][1], placementfacetpairscenter[i][2] ]), epos=np.array([ placementfacetpairscenter[i][0] + length * normal[i][0], placementfacetpairscenter[i][1] + length * normal[i][1], placementfacetpairscenter[i][2] + length * normal[i][2] ]), thickness=0.005, rgba=color).attach_to(node) if showCoM == True: gm.gen_sphere(pos=(placementCoM[0], placementCoM[1], placementCoM[2]), radius=.003, rgba=color).attach_to(node) gm.gen_arrow(spos=np.array( [placementCoM[0], placementCoM[1], placementCoM[2]]), epos=np.array([ placementCoM[0], placementCoM[1], placementCoM[2] - 0.100 ]), thickness=0.001, rgba=color).attach_to(node) if showVertices == True: for vertices in placementVertices: color2 = (random.random(), random.random(), random.random(), 1) for i in range(len(vertices)): gm.gen_sphere(pos=(vertices[i][0], vertices[i][1], vertices[i][2]), radius=.005, rgba=color2).attach_to(node)
def showallNormal(self): for i in range(len(self.hpairs)): color = (random.random(), random.random(), random.random(), 1) # length = random.randint(60,100) length = 0.05 self.__showSptPnt(self.holdingpairsSptPnt[i], color=color) for j, facetpair in enumerate(self.hpairs[i]): gm.gen_arrow( spos=np.array([ self.largefacetscenter[facetpair][0], self.largefacetscenter[facetpair][1], self.largefacetscenter[facetpair][2] ]), epos=np.array([ self.largefacetscenter[facetpair][0] + length * self.largefacet_normals[facetpair][0], self.largefacetscenter[facetpair][1] + length * self.largefacet_normals[facetpair][1], self.largefacetscenter[facetpair][2] + length * self.largefacet_normals[facetpair][2] ]), thickness=0.01, rgba=color).attach_to(base)
homomat[:3, 3] + pt_direction * .05, homomat[:3, 3] + pt_direction * .05 + tmp_direction * .05 ], [ homomat[:3, 3] + pt_direction * .05 + tmp_direction * .05, homomat[:3, 3] + tmp_direction * .05 ], [homomat[:3, 3] + tmp_direction * .05, 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] gm.gen_arrow(spos=line_segs[0][0], epos=epos, thickness=0.004).attach_to(base) spt = homomat[:3, 3] # gm.gen_stick(spt, spt + pn_direction * 10, rgba=[0,1,0,1]).attach_to(base) # base.run() gm.gen_dasharrow(spt, spt - pn_direction * .07, thickness=.004).attach_to(base) # p0 cpt, cnrml = bowl_model.ray_hit(spt, spt + pn_direction * 10000, option='closest') gm.gen_dashstick(spt, cpt, rgba=[.57, .57, .57, .7], thickness=0.003).attach_to(base) gm.gen_sphere(pos=cpt, radius=.005).attach_to(base) gm.gen_dasharrow(cpt, cpt - pn_direction * .07, thickness=.004).attach_to(base) # p0 gm.gen_dasharrow(cpt, cpt + cnrml * .07, thickness=.004).attach_to(base) # p0
objcm1.set_rgba([1, 1, .3, .2]) objcm2 = objcm1.copy() objcm2.set_pos(objcm1.get_pos() + np.array([.05, .02, .0])) objcm1.change_cdmesh_type('convex_hull') objcm2.change_cdmesh_type('obb') iscollided, contact_points = is_collided(objcm1, objcm2) objcm1.show_cdmesh() objcm2.show_cdmesh() objcm1.attach_to(base) objcm2.attach_to(base) print(iscollided) for ctpt in contact_points: gm.gen_sphere(ctpt, radius=.001).attach_to(base) pfrom = np.array([0, 0, 0]) + np.array([1.0, 1.0, 1.0]) # pto = np.array([0, 0, 0]) + np.array([-1.0, -1.0, -1.0]) pto = np.array([0, 0, 0]) + np.array([0.02, 0.02, 0.02]) # pfrom = np.array([0, 0, 0]) + np.array([0.0, 0.0, 1.0]) # pto = np.array([0, 0, 0]) + np.array([0.0, 0.0, -1.0]) # hit_point, hit_normal = rayhit_closet(pfrom=pfrom, pto=pto, objcm=objcm1) hit_points, hit_normals = rayhit_all(pfrom=pfrom, pto=pto, objcm=objcm1) # objcm.attach_to(base) # objcm.show_cdmesh(type='box') # objcm.show_cdmesh(type='convex_hull') # for hitpos, hitnormal in zip([hit_point], [hit_normal]): for hitpos, hitnormal in zip(hit_points, hit_normals): gm.gen_sphere(hitpos, radius=.003, rgba=np.array([0, 1, 1, 1])).attach_to(base) gm.gen_arrow(hitpos, epos=hitpos+hitnormal*.03, thickness=.002, rgba=np.array([0, 1, 1, 1])).attach_to(base) gm.gen_stick(spos=pfrom, epos=pto, thickness=.002).attach_to(base) # gm.gen_arrow(spos=hitpos, epos=hitpos + hitnrml * .07, thickness=.002, rgba=np.array([0, 1, 0, 1])).attach_to(base) base.run()
import motion.probabilistic.rrt_connect as rrtc import math import manipulation.pick_place_planner as ppp import motion.probabilistic.rrt_differential_wheel_connect as rrtdwc base = wd.World(cam_pos=[2, -2, 2], lookat_pos=[.0, 0, .3]) gm.gen_frame().attach_to(base) ground = cm.gen_box(extent=[5, 5, 1], rgba=[.57, .57, .5, .7]) ground.set_pos(np.array([0, 0, -.5])) ground.attach_to(base) ## show human human = nxt.Nextage() rotmat = rm.rotmat_from_axangle([0, 0, 1], math.pi / 2) human.fix_to(np.array([0, -1.5, 1]), rotmat=rotmat) gm.gen_arrow(spos=np.array([0, -1.5, 1]), epos=np.array([0, -1.5, 0]), thickness=0.01, rgba=np.array([.5, 0, 0, 1])).attach_to(base) human.gen_stickmodel().attach_to(base) ## show table2 table2_center = np.array([-1.3, .6, .483]) table2_bottom = cm.gen_box(extent=[1.08, .42, .06], rgba=[150 / 255, 154 / 255, 152 / 255, 1]) rotmat = rm.rotmat_from_axangle([0, 0, 1], math.pi / 2) table2_bottom.set_rotmat(rotmat) table2_bottom.set_pos(table2_center - np.array([0, 0, .03])) table2_bottom.attach_to(base) table2_bottom2 = table2_bottom.copy() table2_bottom2.set_pos(table2_center + np.array([0, 0, -.483 + .03])) table2_bottom2.attach_to(base) table2_top = table2_bottom.copy() table2_top.set_pos(table2_center + np.array([0, 0, -.483 + 1 - .03]))
import visualization.panda.world as wd import modeling.geometric_model as gm import basis.robot_math as rm import math import numpy as np rotmat = rm.rotmat_from_euler(math.pi / 3, -math.pi / 6, math.pi / 3) ax, angle = rm.axangle_between_rotmat(np.eye(3), rotmat) rotmat2 = rm.rotmat_from_axangle(ax, math.pi/6) # 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),
# max_samples=10000, # min_dist_between_sampled_contact_points=.014, # angle_between_contact_normals=math.radians(160), # toggle_sampled_points=True) # for p in contact_points: # gm.gen_sphere(p, radius=.002).attach_to(base) # base.run() # pickle.dump(contact_pairs, open( "save.p", "wb" )) contact_pairs = pickle.load(open("save.p", "rb")) for i, cp in enumerate(contact_pairs): contact_p0, contact_n0 = cp[0] contact_p1, contact_n1 = cp[1] rgba = rm.get_rgba_from_cmap(i) gm.gen_sphere(contact_p0, radius=.002, rgba=rgba).attach_to(base) gm.gen_arrow(contact_p0, contact_p0 + contact_n0 * .01, thickness=.0012, rgba=rgba).attach_to(base) # gm.gen_arrow(contact_p0, contact_p0-contact_n0*.1, thickness=.0012, rgba = rgba).attach_to(base) gm.gen_sphere(contact_p1, radius=.002, rgba=rgba).attach_to(base) # gm.gen_dashstick(contact_p0, contact_p1, thickness=.0012, rgba=rgba).attach_to(base) gm.gen_arrow(contact_p1, contact_p1 + contact_n1 * .01, thickness=.0012, 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!")
def agv_move(task): global onscreen, onscreen_agv, operation_count, pre_pos, pre_angle # agv_linear_speed = .002 agv_linear_speed = 0 agv_angular_speed = .5 arm_linear_speed = .03 arm_angular_speed = .1 for item in onscreen: item.detach() onscreen.clear() for item in onscreen_agv: item.detach() onscreen_agv.clear() agv_pos = rbt_s.get_jnt_values("agv") agv_loc_rotmat = rm.rotmat_from_axangle(axis=[0, 0, 1], angle=-1 * agv_pos[2]) print(agv_pos) agv_direction = np.dot(np.array([1, 0, 0]), agv_loc_rotmat) # print(agv_direction) # onscreen_agv.append(gm.gen_dasharrow(spos=np.array([agv_pos[0], agv_pos[1], 0]), epos=np.array([agv_pos[0], agv_pos[1], 0]))+np.array(agv_direction)) onscreen_agv.append( gm.gen_arrow(epos=np.array( [agv_direction[0], agv_direction[1], agv_direction[2]]), rgba=[1, 0, 1, 1])) # onscreen_agv.append(gm.gen_frame(pos=np.array([agv_pos[0], agv_pos[1], 0]), rotmat=agv_loc_rotmat, length=3)) onscreen_agv[-1].attach_to(base) # print(agv_pos) pressed_keys = { 'w': keyboard.is_pressed('w'), 'a': keyboard.is_pressed('a'), 's': keyboard.is_pressed('s'), 'd': keyboard.is_pressed('d'), 'r': keyboard.is_pressed('r'), # x+ global 't': keyboard.is_pressed('t'), # x- global 'f': keyboard.is_pressed('f'), # y+ global 'g': keyboard.is_pressed('g'), # y- global 'v': keyboard.is_pressed('v'), # z+ global 'b': keyboard.is_pressed('b'), # z- gglobal 'y': keyboard.is_pressed('y'), # r+ global 'u': keyboard.is_pressed('u'), # r- global 'h': keyboard.is_pressed('h'), # p+ global 'j': keyboard.is_pressed('j'), # p- global 'n': keyboard.is_pressed('n'), # yaw+ global 'm': keyboard.is_pressed('m'), # yaw- global 'o': keyboard.is_pressed('o'), # gripper open 'p': keyboard.is_pressed('p') } # gripper close values_list = list(pressed_keys.values()) if pressed_keys["w"] and pressed_keys["a"]: print("Invalid Operation!!") # rbt_x.agv_move(linear_speed=agv_linear_speed, angular_speed=agv_angular_speed, time_interval=.5) elif pressed_keys["w"] and pressed_keys["d"]: print("Invalid Operation!!") # rbt_x.agv_move(linear_speed=agv_linear_speed, angular_speed=-agv_angular_speed, time_interval=.5) elif pressed_keys["s"] and pressed_keys["a"]: print("Invalid Operation!!") # rbt_x.agv_move(linear_speed=-agv_linear_speed, angular_speed=-agv_angular_speed, time_interval=.5) elif pressed_keys["s"] and pressed_keys["d"]: print("Invalid Operation!!") # rbt_x.agv_move(linear_speed=-agv_linear_speed, angular_speed=agv_angular_speed, time_interval=.5) elif pressed_keys["w"] and sum(values_list) == 1: # if key 'q' is pressed rbt_s.fk( component_name='agv', jnt_values=np.array( pre_pos + [agv_linear_speed, 0, math.radians(0.5)])) pre_pos = np.array(pre_pos + [agv_linear_speed, 0, math.radians(0.5)]) # rbt_x.agv_move(linear_speed=agv_linear_speed, angular_speed=0, time_interval=.5) elif pressed_keys["s"] and sum(values_list) == 1: # if key 'q' is pressed rbt_s.fk( component_name='agv', jnt_values=np.array(pre_pos + [-1 * agv_linear_speed, 0, math.radians(-0.5)])) pre_pos = np.array(pre_pos + [-1 * agv_linear_speed, 0, math.radians(-0.5)]) # rbt_x.agv_move(linear_speed=-agv_linear_speed, angular_speed=0, time_interval=.5) elif pressed_keys["a"] and sum(values_list) == 1: # if key 'q' is pressed rbt_s.fk( component_name='agv', jnt_values=np.array( pre_pos + [0, agv_linear_speed, math.radians(0)])) pre_pos = np.array(pre_pos + [0, agv_linear_speed, math.radians(0)]) # rbt_x.agv_move(linear_speed=0, angular_speed=agv_angular_speed, time_interval=.5) elif pressed_keys["d"] and sum(values_list) == 1: # if key 'q' is pressed rbt_s.fk( component_name='agv', jnt_values=np.array(pre_pos + [0, -1 * agv_linear_speed, math.radians(0)])) pre_pos = np.array(pre_pos + [0, -1 * agv_linear_speed, math.radians(0)]) # rbt_x.agv_move(linear_speed=0, angular_speed=-agv_angular_speed, time_interval=.5) elif pressed_keys["o"] and sum(values_list) == 1: # if key 'q' is pressed print("Invalid Operation!!") # rbt_x.arm_jaw_to(jawwidth=100) elif pressed_keys["p"] and sum(values_list) == 1: # if key 'q' is pressed print("Invalid Operation!!") # rbt_x.arm_jaw_to(jawwidth=0) elif any(pressed_keys[item] for item in ['r', 't', 'f', 'g', 'v', 'b', 'y', 'u', 'h', 'j', 'n', 'm']) and\ sum(values_list) == 1: # global tic = time.time() current_jnt_values = rbt_s.get_jnt_values() current_arm_tcp_pos, current_arm_tcp_rotmat = rbt_s.get_gl_tcp() rel_pos = np.zeros(3) rel_rotmat = np.eye(3) if pressed_keys['r']: rel_pos = np.array([arm_linear_speed * .5, 0, 0]) elif pressed_keys['t']: rel_pos = np.array([-arm_linear_speed * .5, 0, 0]) elif pressed_keys['f']: rel_pos = np.array([0, arm_linear_speed * .5, 0]) elif pressed_keys['g']: rel_pos = np.array([0, -arm_linear_speed * .5, 0]) elif pressed_keys['v']: rel_pos = np.array([0, 0, arm_linear_speed * .5]) elif pressed_keys['b']: rel_pos = np.array([0, 0, -arm_linear_speed * .5]) elif pressed_keys['y']: rel_rotmat = rm.rotmat_from_euler(arm_angular_speed * .5, 0, 0) elif pressed_keys['u']: rel_rotmat = rm.rotmat_from_euler(-arm_angular_speed * .5, 0, 0) elif pressed_keys['h']: rel_rotmat = rm.rotmat_from_euler(0, arm_angular_speed * .5, 0) elif pressed_keys['j']: rel_rotmat = rm.rotmat_from_euler(0, -arm_angular_speed * .5, 0) elif pressed_keys['n']: rel_rotmat = rm.rotmat_from_euler(0, 0, arm_angular_speed * .5) elif pressed_keys['m']: rel_rotmat = rm.rotmat_from_euler(0, 0, -arm_angular_speed * .5) new_arm_tcp_pos = current_arm_tcp_pos + rel_pos new_arm_tcp_rotmat = rel_rotmat.dot(current_arm_tcp_rotmat) last_jnt_values = rbt_s.get_jnt_values() new_jnt_values = rbt_s.ik(tgt_pos=new_arm_tcp_pos, tgt_rotmat=new_arm_tcp_rotmat, seed_jnt_values=current_jnt_values) # if new_jnt_values is None: # continue rbt_s.fk(jnt_values=new_jnt_values) toc = time.time() start_frame_id = math.ceil((toc - tic) / .01) # rbt_x.arm_move_jspace_path([last_jnt_values, new_jnt_values], time_interval=.1, start_frame_id=start_frame_id) onscreen.append(rbt_s.gen_meshmodel()) onscreen[-1].attach_to(base) # print(pre_pos) # print(onscreen) operation_count += 1 # time.sleep(1/30) return task.cont
tmp_gm.set_rgba(rm.random_rgba()) # edge edge_list = (np.array(seg_nested_edge_list[i]) + offset_pos).tolist() gm.gen_linesegs(edge_list, thickness=.05, rgba=[1, 0, 0, 1]).attach_to(base) # seed segment tmp_trm = tg.extract_subtrimesh(bunnycm.objtrm, facet_seed_list[i], offset_pos) tmp_gm = gm.StaticGeometricModel(tmp_trm, btwosided=True) tmp_gm.attach_to(base) tmp_gm.set_rgba([1, 0, 0, 1]) # face center and normal seed_center = np.mean(tmp_trm.vertices, axis=0) gm.gen_sphere(pos=seed_center, radius=.001).attach_to(base) gm.gen_arrow(spos=seed_center, epos=seed_center + tmp_trm.face_normals[0] * .01, thickness=.0006).attach_to(base) for face_id_for_curvature in face_id_pair_list_for_curvature[i]: rgba = [1, 1, 1, 1] tmp_trm = tg.extract_subtrimesh(bunnycm.objtrm, face_id_for_curvature, offset_pos) tmp_gm = gm.StaticGeometricModel(tmp_trm, btwosided=True) tmp_gm.attach_to(base) tmp_gm.set_rgba(rgba) seed_center = np.mean(tmp_trm.vertices, axis=0) gm.gen_sphere(pos=seed_center, radius=.001, rgba=rgba).attach_to(base) gm.gen_arrow(spos=seed_center, epos=seed_center + tmp_trm.face_normals[0] * .01, thickness=.0006, rgba=rgba).attach_to(base)
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 ]] gm.gen_linesegs(line_segs).attach_to(base) gm.gen_arrow(spos=line_segs[0][0], epos=line_segs[0][1], thickness=0.004).attach_to(base) spt = homomat[:3, 3] # gm.gen_stick(spt, spt + pn_direction * 10, rgba=[0,1,0,1]).attach_to(base) # base.run() gm.gen_dasharrow(spt, spt - pn_direction * .07, thickness=.004).attach_to(base) # p0 cpt, cnrml = bowl_model.ray_hit(spt, spt + pn_direction * 10000, option='closest') gm.gen_dashstick(spt, cpt, rgba=[.57, .57, .57, .7], thickness=0.003).attach_to(base) gm.gen_sphere(pos=cpt, radius=.005).attach_to(base) gm.gen_dasharrow(cpt, cpt - pn_direction * .07, thickness=.004).attach_to(base) # p0 gm.gen_dasharrow(cpt, cpt + cnrml * .07, thickness=.004).attach_to(base) # p0
# cross_vec = rm.unit_vector(np.cross(np.array([0,0,1]), rotmat[:3,2])) # # gm.gen_dasharrow(epos=cross_vec*.2, rgba=[1,1,0,1], lsolid=.01, lspace=.0077).attach_to(base) # gm.gen_arrow(epos=cross_vec*.2, rgba=[1,1,0,1]).attach_to(base) # gm.gen_sphere(radius=.005, pos=cross_vec*.2, rgba=[0,0,0,1]).attach_to(base) # # # nxt_vec_uvw = rotmat.dot(cross_vec) # # gm.gen_dasharrow(epos=nxt_vec_uvw*.2, rgba=[1,1,0,1], lsolid=.015, lspace=.007).attach_to(base) # gm.gen_arrow(epos=nxt_vec_uvw*.2, rgba=[1,1,0,1]).attach_to(base) # gm.gen_sphere(radius=.005, pos=nxt_vec_uvw*.2, rgba=[0,0,0,1]).attach_to(base) # # # pre_vec_xyz = rotmat.T.dot(cross_vec) # gm.gen_arrow(epos=pre_vec_xyz*.2, rgba=[1,1,0,1]).attach_to(base) # gm.gen_sphere(radius=.005, pos=pre_vec_xyz*.2, rgba=[0,0,0,1]).attach_to(base) # ax, angle = rm.axangle_between_rotmat(np.eye(3), rotmat) gm.gen_arrow(epos=ax * .4, rgba=[0, 0, 0, 1]).attach_to(base) for step_angle in np.linspace(0, angle, 10).tolist(): rotmat = rm.rotmat_from_axangle(ax, step_angle) gm.gen_dashframe(length=.2, rotmat=rotmat).attach_to(base) gm.gen_dashtorus(axis=rotmat[:3, 2], portion=1, radius=.2, thickness=.003, rgba=[1, 1, 0, 1], lspace=.007, lsolid=.01, sections=16, discretization=64).attach_to(base) gm.gen_sphere(radius=.2, rgba=[.67, .67, .67, .9], subdivisions=5).attach_to(base) # radius, _ = rm.unit_vector(cross_vec*.2-cross_vec.dot(ax)*ax*.2, toggle_length=True)