def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), cdmesh_type='aabb', name='yumi_gripper'): self.name = name self.pos = pos self.rotmat = rotmat self.cdmesh_type = cdmesh_type # aabb, convexhull, or triangles # joints # - coupling - No coupling by default self.coupling = jl.JLChain(pos=self.pos, rotmat=self.rotmat, homeconf=np.zeros(0), name='coupling') self.coupling.jnts[1]['loc_pos'] = np.array([0, 0, .0]) self.coupling.lnks[0]['name'] = 'coupling_lnk0' # toggle on the following part to assign an explicit mesh model to a coupling # self.coupling.lnks[0]['meshfile'] = os.path.join(this_dir, "meshes", "xxx.stl") # self.coupling.lnks[0]['rgba'] = [.2, .2, .2, 1] self.coupling.reinitialize() # jaw center self.jaw_center_pos = np.zeros(3) self.jaw_center_rotmat = np.eye(3) # jaw width self.jawwidth_rng = [0.0, 5.0] # collision detection self.cc = None # cd mesh collection for precise collision checking self.cdmesh_collection = mc.ModelCollection()
def gen_meshmodel(self, tcp_jnt_id=None, tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=False, rgba=None, name='xarm_shuidi_mobile_meshmodel'): meshmodel = mc.ModelCollection(name=name) self.agv.gen_meshmodel(tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) self.arm.gen_meshmodel(tcp_jnt_id=tcp_jnt_id, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) self.ft_sensor.gen_meshmodel(tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) self.hnd.gen_meshmodel(tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, rgba=rgba).attach_to(meshmodel) for obj_info in self.oih_infos: objcm = obj_info['collision_model'] objcm.set_pos(obj_info['gl_pos']) objcm.set_rotmat(obj_info['gl_rotmat']) objcm.copy().attach_to(meshmodel) return meshmodel
def gen_stickmodel(self, toggle_tcpcs=False, toggle_jntscs=False, toggle_connjnt=False, name='suction_stickmodel'): mm_collection = mc.ModelCollection(name=name) self.coupling.gen_stickmodel( toggle_tcpcs=False, toggle_jntscs=toggle_jntscs).attach_to(mm_collection) self.jlc.gen_stickmodel( toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(mm_collection) if toggle_tcpcs: suction_center_gl_pos = self.rotmat.dot( self.suction_center_pos) + self.pos suction_center_gl_rotmat = self.rotmat.dot( self.suction_center_rotmat) gm.gen_dashstick(spos=self.pos, epos=suction_center_gl_pos, thickness=.0062, rgba=[.5, 0, 1, 1], type="round").attach_to(mm_collection) gm.gen_mycframe( pos=suction_center_gl_pos, rotmat=suction_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='yumi_gripper_stickmodel'): stickmodel = mc.ModelCollection(name=name) self.coupling.gen_stickmodel( tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs).attach_to(stickmodel) self.lft.gen_stickmodel( tcp_jntid=tcp_jntid, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) self.rgt.gen_stickmodel( tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) return stickmodel
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_fgr.gen_meshmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(mm_collection) self.rgt_fgr.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(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(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='robotiq85_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=toggle_tcpcs, 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_jnt_id=None, tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=False, toggle_connjnt=False, name='yumi'): stickmodel = mc.ModelCollection(name=name) self.central_body.gen_stickmodel( tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs).attach_to(stickmodel) self.lft_arm.gen_stickmodel( tcp_jnt_id=tcp_jnt_id, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) # self.lft_hnd.gen_stickmodel(toggle_tcpcs=False, # toggle_jntscs=toggle_jntscs, # toggle_connjnt=toggle_connjnt).attach_to(stickmodel) self.rgt_arm.gen_stickmodel( tcp_jnt_id=tcp_jnt_id, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) # self.rgt_hnd.gen_stickmodel(toggle_tcpcs=False, # toggle_jntscs=toggle_jntscs, # toggle_connjnt=toggle_connjnt).attach_to(stickmodel) return stickmodel
def gen_meshmodel(self, toggle_tcpcs=False, toggle_jntscs=False, rgba=None, name='xarm_gripper_meshmodel'): meshmodel = mc.ModelCollection(name=name) self.coupling.gen_meshmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) self.lft.gen_meshmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) self.rgt.gen_meshmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) 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(meshmodel) gm.gen_mycframe(pos=jaw_center_gl_pos, rotmat=jaw_center_gl_rotmat).attach_to(meshmodel) return meshmodel
def gen_stickmodel(self, tcp_jnt_id=None, tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=False, toggle_connjnt=False, name='xarm7_shuidi_mobile_stickmodel'): stickmodel = mc.ModelCollection(name=name) self.agv.gen_stickmodel( tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs).attach_to(stickmodel) self.arm.gen_stickmodel( tcp_jnt_id=tcp_jnt_id, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) self.ft_sensor.gen_stickmodel( tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat).attach_to(stickmodel) self.hnd.gen_stickmodel( toggle_tcpcs=False, toggle_jntscs=toggle_jntscs).attach_to(stickmodel) return stickmodel
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 gen_stickmodel(self, tcp_jnt_id=None, tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=False, toggle_connjnt=False, name='cobotta_stickmodel'): stickmodel = mc.ModelCollection(name=name) self.base_plate.gen_stickmodel( tcp_jnt_id=tcp_jnt_id, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) self.arm.gen_stickmodel( tcp_jnt_id=tcp_jnt_id, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) self.hnd.gen_stickmodel( toggle_tcpcs=False, toggle_jntscs=toggle_jntscs).attach_to(stickmodel) return stickmodel
def gen_meshmodel(self, tcp_jntid=None, tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=False, rgba=None, name='yumi_gripper_meshmodel'): meshmodel = mc.ModelCollection(name=name) self.coupling.gen_meshmodel(tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) self.lft.gen_meshmodel(tcp_jntid=tcp_jntid, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) self.rgt.gen_meshmodel(tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) return meshmodel
def gen_mesh_model(self, radius=.005, rgba=np.array([1, 0, 0, 1])) -> mc.ModelCollection: """ Plot markers for rigid body """ markers_mc = mc.ModelCollection() for i in range(len(self)): gm.gen_sphere(self.markers[i], radius=radius, rgba=rgba).attach_to(markers_mc) return markers_mc
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='ur3dual'): stickmodel = mc.ModelCollection(name=name) self.lft_body.gen_stickmodel( tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs).attach_to(stickmodel) self.lft_arm.gen_stickmodel( tcp_jntid=tcp_jntid, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) self.lft_hnd.gen_stickmodel( toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) self.rgt_body.gen_stickmodel( tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs).attach_to(stickmodel) self.rgt_arm.gen_stickmodel( tcp_jntid=tcp_jntid, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) self.rgt_hnd.gen_stickmodel( toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) self.lft_ft_sensor.gen_stickmodel( toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) self.rgt_ft_sensor.gen_stickmodel( toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) return stickmodel
def gen_meshmodel(self, tcp_jntid=None, tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=False, rgba=None, name='xarm_gripper_meshmodel'): meshmodel = mc.ModelCollection(name=name) self.lft_body.gen_meshmodel(tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) self.lft_arm.gen_meshmodel(tcp_jntid=tcp_jntid, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) self.lft_hnd.gen_meshmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) self.rgt_arm.gen_meshmodel(tcp_jntid=tcp_jntid, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) self.rgt_hnd.gen_meshmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) self.lft_ft_sensor.gen_meshmodel(toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) self.rgt_ft_sensor.gen_meshmodel(toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) for obj_info in self.lft_oih_infos: objcm = obj_info['collisionmodel'] objcm.set_pos(obj_info['gl_pos']) objcm.set_rotmat(obj_info['gl_rotmat']) objcm.copy().attach_to(meshmodel) for obj_info in self.rgt_oih_infos: objcm = obj_info['collisionmodel'] objcm.set_pos(obj_info['gl_pos']) objcm.set_rotmat(obj_info['gl_rotmat']) objcm.copy().attach_to(meshmodel) return meshmodel
def gen_meshmodel(self, tcp_jnt_id=None, tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=False, rgba=None, name='xarm_shuidi_mobile_meshmodel', option='full'): """ :param tcp_jnt_id: :param tcp_loc_pos: :param tcp_loc_rotmat: :param toggle_tcpcs: :param toggle_jntscs: :param rgba: :param name: :param option: 'full', 'hand_only', 'body_only' :return: """ meshmodel = mc.ModelCollection(name=name) if option == 'full' or option == 'body_only': self.base_plate.gen_meshmodel(tcp_jnt_id=tcp_jnt_id, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) self.arm.gen_meshmodel(tcp_jnt_id=tcp_jnt_id, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) if option == 'full' or option == 'hand_only': self.hnd.gen_meshmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) if option == 'full': for obj_info in self.oih_infos: objcm = obj_info['collision_model'].copy() objcm.set_pos(obj_info['gl_pos']) objcm.set_rotmat(obj_info['gl_rotmat']) if rgba is not None: objcm.set_rgba(rgba) objcm.attach_to(meshmodel) return meshmodel
def gen_meshmodel(self, tcp_jnt_id=None, tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=False, rgba=None, name='ur3e_dual_meshmodel'): mm_collection = mc.ModelCollection(name=name) self.lft_body.gen_meshmodel(tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(mm_collection) self.lft_arm.gen_meshmodel(tcp_jnt_id=tcp_jnt_id, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(mm_collection) self.lft_hnd.gen_meshmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(mm_collection) self.rgt_arm.gen_meshmodel(tcp_jnt_id=tcp_jnt_id, tcp_loc_pos=tcp_loc_pos, tcp_loc_rotmat=tcp_loc_rotmat, toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(mm_collection) self.rgt_hnd.gen_meshmodel(toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(mm_collection) for obj_info in self.lft_oih_infos: objcm = obj_info['collision_model'] objcm.set_pos(obj_info['gl_pos']) objcm.set_rotmat(obj_info['gl_rotmat']) objcm.copy().attach_to(mm_collection) for obj_info in self.rgt_oih_infos: objcm = obj_info['collision_model'] objcm.set_pos(obj_info['gl_pos']) objcm.set_rotmat(obj_info['gl_rotmat']) objcm.copy().attach_to(mm_collection) return mm_collection
def gen_stickmodel(self, toggle_tcpcs=False, toggle_jntscs=False, toggle_connjnt=False, name='xarm_gripper_stickmodel'): stickmodel = mc.ModelCollection(name=name) self.lft_outer.gen_stickmodel( toggle_tcpcs=toggle_tcpcs, toggle_jntscs=toggle_jntscs, toggle_connjnt=toggle_connjnt).attach_to(stickmodel) self.lft_inner.gen_stickmodel( toggle_tcpcs=False, toggle_jntscs=toggle_jntscs).attach_to(stickmodel) self.rgt_outer.gen_stickmodel( toggle_tcpcs=False, toggle_jntscs=toggle_jntscs).attach_to(stickmodel) self.rgt_inner.gen_stickmodel( toggle_tcpcs=False, toggle_jntscs=toggle_jntscs).attach_to(stickmodel) return stickmodel
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='tbm'): stickmodel = mc.ModelCollection(name=name) self.head.gen_stickmodel( tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs).attach_to(stickmodel) for k in self.cutters.keys(): for cutter in self.cutters[k]: cutter.gen_stickmodel( tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs).attach_to(stickmodel) return stickmodel
def gen_meshmodel(self, tcp_jntid=None, tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=False, rgba=None, name='tbm'): meshmodel = mc.ModelCollection(name=name) self.head.gen_meshmodel(tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) for k in self.cutters.keys(): for cutter in self.cutters[k]: cutter.gen_meshmodel(tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) return meshmodel
def gen_meshmodel(self, tcp_jntid=None, tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=True, toggle_jntscs=False, name='robot_mesh', rgba=None): mm_collection = mc.ModelCollection(name=name) for id in range(self.jlobject.ndof + 1): if self.jlobject.lnks[id]['collisionmodel'] is not None: this_collisionmodel = self.jlobject.lnks[id][ 'collisionmodel'].copy() pos = self.jlobject.lnks[id]['gl_pos'] rotmat = self.jlobject.lnks[id]['gl_rotmat'] this_collisionmodel.set_homomat( rm.homomat_from_posrot(pos, rotmat)) this_rgba = self.jlobject.lnks[id][ 'rgba'] if rgba is None else rgba this_collisionmodel.set_rgba(this_rgba) this_collisionmodel.attach_to(mm_collection) # tool center coord if toggle_tcpcs: self._toggle_tcpcs(mm_collection, tcp_jntid, tcp_loc_pos, tcp_loc_rotmat, tcpic_rgba=np.array([.5, 0, 1, 1]), tcpic_thickness=.0062) # toggle all coord if toggle_jntscs: alpha = 1 if rgba == None else rgba[3] self._toggle_jntcs(mm_collection, jntcs_thickness=.0062, alpha=alpha) return mm_collection
def gen_stickmodel(self, rgba=np.array([.5, 0, 0, 1]), thickness=.01, joint_ratio=1.62, link_ratio=.62, tcp_jntid=None, tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=True, toggle_jntscs=False, toggle_connjnt=False, name='robotstick'): """ generate the stick model for a jntlnk object snp means stick nodepath :param rgba: :param tcp_jntid: :param tcp_loc_pos: :param tcp_loc_rotmat: :param toggle_tcpcs: :param toggle_jntscs: :param toggle_connjnt: draw the connecting joint explicitly or not :param name: :return: author: weiwei date: 20200331, 20201006 """ stickmodel = mc.ModelCollection(name=name) id = 0 loopdof = self.jlobject.ndof + 1 if toggle_connjnt: loopdof = self.jlobject.ndof + 2 while id < loopdof: cjid = self.jlobject.jnts[id]['child'] jgpos = self.jlobject.jnts[id]['gl_posq'] # joint global pos cjgpos = self.jlobject.jnts[cjid][ 'gl_pos0'] # child joint global pos jgmtnax = self.jlobject.jnts[id][ "gl_motionax"] # joint global rot ax gm.gen_stick(spos=jgpos, epos=cjgpos, thickness=thickness, type="rect", rgba=rgba).attach_to(stickmodel) if id > 0: if self.jlobject.jnts[id]['type'] == "revolute": gm.gen_stick(spos=jgpos - jgmtnax * thickness, epos=jgpos + jgmtnax * thickness, type="rect", thickness=thickness * joint_ratio, rgba=np.array([.3, .3, .2, rgba[3] ])).attach_to(stickmodel) if self.jlobject.jnts[id]['type'] == "prismatic": jgpos0 = self.jlobject.jnts[id]['gl_pos0'] gm.gen_stick(spos=jgpos0, epos=jgpos, type="round", thickness=thickness * joint_ratio, rgba=np.array([.2, .3, .3, rgba[3] ])).attach_to(stickmodel) id = cjid # tool center coord if toggle_tcpcs: self._toggle_tcpcs(stickmodel, tcp_jntid, tcp_loc_pos, tcp_loc_rotmat, tcpic_rgba=rgba + np.array([0, 0, 1, 0]), tcpic_thickness=thickness * link_ratio) # toggle all coord if toggle_jntscs: self._toggle_jntcs(stickmodel, jntcs_thickness=thickness * link_ratio, alpha=rgba[3]) return stickmodel