def gen_meshmodel(self, toggle_tcpcs=False, toggle_jntscs=False, rgba=None, name='robotiq85_meshmodel'): mm_collection = mc.ModelCollection(name=name) self.coupling.gen_meshmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(mm_collection) self.lft_outer.gen_meshmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(mm_collection) self.lft_inner.gen_meshmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(mm_collection) self.rgt_outer.gen_meshmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(mm_collection) self.rgt_inner.gen_meshmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(mm_collection) if toggle_tcpcs: jaw_center_gl_pos = self.rotmat.dot(self.jaw_center_pos) + self.pos jaw_center_gl_rotmat = self.rotmat.dot(self.jaw_center_rotmat) gm.gen_dashstick(spos=self.pos, epos=jaw_center_gl_pos, thickness=.0062, rgba=[.5, 0, 1, 1], type="round").attach_to(mm_collection) gm.gen_mycframe( pos=jaw_center_gl_pos, rotmat=jaw_center_gl_rotmat).attach_to(mm_collection) return mm_collection
def gen_stickmodel(self, tcp_jntid=None, tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=False, toggle_connjnt=False, name='robotiq140_stickmodel'): sm_collection = mc.ModelCollection(name=name) self.coupling.gen_stickmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs).attach_to(sm_collection) self.lft_outer.gen_stickmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(sm_collection) self.lft_inner.gen_stickmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(sm_collection) self.rgt_outer.gen_stickmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(sm_collection) self.rgt_inner.gen_stickmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(sm_collection) if toggle_tcpcs: jaw_center_gl_pos = self.rotmat.dot(grpr.jaw_center_pos) + self.pos jaw_center_gl_rotmat = self.rotmat.dot(grpr.jaw_center_rotmat) gm.gen_dashstick(spos=self.pos, epos=jaw_center_gl_pos, thickness=.0062, rgba=[.5, 0, 1, 1], type="round").attach_to(sm_collection) gm.gen_mycframe(pos=jaw_center_gl_pos, rotmat=jaw_center_gl_rotmat).attach_to(sm_collection) return sm_collection
def gen_stickmodel(self, tcp_jntid=None, tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=False, toggle_connjnt=False, name='schunkrh918_stickmodel'): stickmodel = mc.ModelCollection(name=name) self.coupling.gen_stickmodel( toggle_tcpcs=False, toggle_jntscs=toggle_jntscs).attach_to(stickmodel) self.lft.gen_stickmodel( toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) self.rgt.gen_stickmodel( toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) if toggle_tcpcs: jaw_center_gl_pos = self.rotmat.dot(self.jaw_center_pos) + self.pos jaw_center_gl_rotmat = self.rotmat.dot(self.jaw_center_rotmat) gm.gen_dashstick(spos=self.pos, epos=jaw_center_gl_pos, thickness=.0062, rgba=[.5, 0, 1, 1], type="round").attach_to(stickmodel) gm.gen_mycframe(pos=jaw_center_gl_pos, rotmat=jaw_center_gl_rotmat).attach_to(stickmodel) return stickmodel
def _toggle_tcpcs(self, parent_model, tcp_jntid, tcp_loc_pos, tcp_loc_rotmat, tcpic_rgba, tcpic_thickness, tcpcs_thickness=None, tcpcs_length=None): """ :param parent_model: where to draw the frames to :param tcp_jntid: single id or a list of ids :param tcp_loc_pos: :param tcp_loc_rotmat: :param tcpic_rgba: color that used to render the tcp indicator :param tcpic_thickness: thickness the tcp indicator :param tcpcs_thickness: thickness the tcp coordinate frame :return: author: weiwei date: 20201125 """ if tcp_jntid is None: tcp_jntid = self.jlobject.tcp_jntid if tcp_loc_pos is None: tcp_loc_pos = self.jlobject.tcp_loc_pos if tcp_loc_rotmat is None: tcp_loc_rotmat = self.jlobject.tcp_loc_rotmat if tcpcs_thickness is None: tcpcs_thickness = tcpic_thickness if tcpcs_length is None: tcpcs_length = tcpcs_thickness * 15 tcp_gl_pos, tcp_gl_rotmat = self.jlobject.get_gl_tcp( tcp_jntid, tcp_loc_pos, tcp_loc_rotmat) if isinstance(tcp_gl_pos, list): for i, jid in enumerate(tcp_jntid): jgpos = self.jlobject.jnts[jid]['gl_posq'] gm.gen_dashstick(spos=jgpos, epos=tcp_gl_pos[i], thickness=tcpic_thickness, rgba=tcpic_rgba, type="round").attach_to(parent_model) gm.gen_mycframe(pos=tcp_gl_pos[i], rotmat=tcp_gl_rotmat[i], length=tcpcs_length, thickness=tcpcs_thickness, alpha=tcpic_rgba[3]).attach_to(parent_model) else: jgpos = self.jlobject.jnts[tcp_jntid]['gl_posq'] gm.gen_dashstick(spos=jgpos, epos=tcp_gl_pos, thickness=tcpic_thickness, rgba=tcpic_rgba, type="round").attach_to(parent_model) gm.gen_mycframe(pos=tcp_gl_pos, rotmat=tcp_gl_rotmat, length=tcpcs_length, thickness=tcpcs_thickness, alpha=tcpic_rgba[3]).attach_to(parent_model)
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 angle = rm.angle_between_vectors(-pn_direction, cnrml) vec = np.cross(-pn_direction, cnrml) rotmat = rm.rotmat_from_axangle(vec, angle) new_plane_homomat = np.eye(4) new_plane_homomat[:3, :3] = rotmat.dot(homomat[:3, :3]) new_plane_homomat[:3, 3] = cpt twod_plane = gm.gen_box(np.array([.2, .2, .001]), homomat=new_plane_homomat, rgba=[1, 1, 1, .3]) twod_plane.attach_to(base)
# # gm.gen_dashstick(pos_start, pos_end, thickness=.001, rgba=[0, 0, 0, .3], lsolid=.005, lspace=.005).attach_to(base) # pos_start = rotmat_a.dot(np.array([0, pos_a[1], pos_a[2]])) # pos_end = rotmat_a.dot(np.array([0, 0, pos_a[2]])) # gm.gen_dashstick(pos_start, pos_end, thickness=.001, rgba=[0,0,0,.3], lsolid=.005, lspace=.005).attach_to(base) # cvt to sigma o pos_o = rotmat_a.dot(pos_a) # gm.gen_dashstick(pos_o, np.array([pos_o[0], pos_o[1], 0]), thickness=.001, rgba=[0,0,0,.3], lsolid=.005, lspace=.005).attach_to(base) gm.gen_stick(pos_o, np.array([pos_o[0], pos_o[1], 0]), thickness=.001, rgba=[0, 0, 0, .3]).attach_to(base) gm.gen_dashstick(np.array([pos_o[0], pos_o[1], 0]), np.array([pos_o[0], 0, 0]), thickness=.001, rgba=[0, 0, 0, .3], lsolid=.005, lspace=.005).attach_to(base) # gm.gen_dashstick(pos_o, np.array([pos_o[0], 0, pos_o[2]]), thickness=.001, rgba=[0,0,0,.3], lsolid=.005, lspace=.005).attach_to(base) gm.gen_stick(pos_o, np.array([pos_o[0], 0, pos_o[2]]), thickness=.001, rgba=[0, 0, 0, .3]).attach_to(base) gm.gen_dashstick(np.array([pos_o[0], 0, pos_o[2]]), np.array([pos_o[0], 0, 0]), thickness=.001, rgba=[0, 0, 0, .3], lsolid=.005, lspace=.005).attach_to(base)