def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), cdmesh_type='convex_hull', name='yumi_gripper', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, cdmesh_type=cdmesh_type, name=name) this_dir, this_filename = os.path.split(__file__) cpl_end_pos = self.coupling.jnts[-1]['gl_posq'] cpl_end_rotmat = self.coupling.jnts[-1]['gl_rotmatq'] # - lft self.lft = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(1), name='base_lft_finger') self.lft.jnts[1]['loc_pos'] = np.array([0, 0.0065, 0.0837]) self.lft.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, math.pi) self.lft.jnts[1]['type'] = 'prismatic' self.lft.jnts[1]['motion_rng'] = [.0, .025] self.lft.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) self.lft.lnks[0]['name'] = "base" self.lft.lnks[0]['loc_pos'] = np.zeros(3) self.lft.lnks[0]['meshfile'] = os.path.join(this_dir, "meshes", "base.stl") self.lft.lnks[0]['rgba'] = [.5, .5, .5, 1] self.lft.lnks[1]['name'] = "finger1" self.lft.lnks[1]['meshfile'] = os.path.join(this_dir, "meshes", "finger.stl") self.lft.lnks[1]['rgba'] = [.2, .2, .2, 1] # - rgt self.rgt = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(1), name='rgt_finger') self.rgt.jnts[1]['loc_pos'] = np.array([0, -0.0065, 0.0837]) self.rgt.jnts[1]['type'] = 'prismatic' self.rgt.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) self.rgt.lnks[1]['name'] = "finger2" self.rgt.lnks[1]['meshfile'] = os.path.join(this_dir, "meshes", "finger.stl") self.rgt.lnks[1]['rgba'] = [.2, .2, .2, 1] # reinitialize self.lft.reinitialize(cdmesh_type=cdmesh_type) self.rgt.reinitialize(cdmesh_type=cdmesh_type) # jaw width self.jawwidth_rng = [0.0, .05] # jaw center self.jaw_center_pos = np.array([0, 0, .13]) # collision detection self.all_cdelements = [] self.enable_cc(toggle_cdprimit=enable_cc)
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), cdmesh_type='box', name='robotiqhe', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, cdmesh_type=cdmesh_type, name=name) this_dir, this_filename = os.path.split(__file__) cpl_end_pos = self.coupling.jnts[-1]['gl_posq'] cpl_end_rotmat = self.coupling.jnts[-1]['gl_rotmatq'] # - lft self.lft = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(1), name='base_lft_finger') self.lft.jnts[1]['loc_pos'] = np.array([-.025, .0, .11]) self.lft.jnts[1]['type'] = 'prismatic' self.lft.jnts[1]['motion_rng'] = [0, .025] self.lft.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) self.lft.lnks[0]['name'] = "base" self.lft.lnks[0]['loc_pos'] = np.zeros(3) self.lft.lnks[0]['meshfile'] = os.path.join(this_dir, "meshes", "base_cvt.stl") self.lft.lnks[0]['rgba'] = [.2, .2, .2, 1] self.lft.lnks[1]['name'] = "finger1" self.lft.lnks[1]['meshfile'] = os.path.join(this_dir, "meshes", "finger1_cvt.stl") self.lft.lnks[1]['rgba'] = [.5, .5, .5, 1] # - rgt self.rgt = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(1), name='rgt_finger') self.rgt.jnts[1]['loc_pos'] = np.array([.025, .0, .11]) self.rgt.jnts[1]['type'] = 'prismatic' self.rgt.jnts[1]['loc_motionax'] = np.array([-1, 0, 0]) self.rgt.lnks[1]['name'] = "finger2" self.rgt.lnks[1]['meshfile'] = os.path.join(this_dir, "meshes", "finger2_cvt.stl") self.rgt.lnks[1]['rgba'] = [.5, .5, .5, 1] # jaw center self.jaw_center_pos = np.array([0, 0, .14]) # reinitialize self.lft.reinitialize() self.rgt.reinitialize() # collision detection self.all_cdelements = [] self.enable_cc(toggle_cdprimit=enable_cc)
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), name="tbm_changer", enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) # base plate self.base_plate = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(0), name='base_plate') # self.base_plate.jnts[1]['loc_pos'] = np.array([0, 0, 0.035]) # self.base_plate.lnks[0]['mesh_file'] = os.path.join(this_dir, "meshes", "base_plate.stl") # self.base_plate.lnks[0]['rgba'] = [.35,.35,.35,1] # self.base_plate.reinitialize() # arm self.arm = tbma.TBMArm(pos=self.base_plate.jnts[-1]['gl_posq'], rotmat=self.base_plate.jnts[-1]['gl_rotmatq'], name='arm', enable_cc=False) # gripper self.hnd = tbmg.TBMGripper(pos=self.arm.jnts[-1]['gl_posq'], rotmat=self.arm.jnts[-1]['gl_rotmatq'], name='hnd', enable_cc=False) # tool center point self.arm.jlc.tcp_jnt_id = -1 self.arm.jlc.tcp_loc_pos = self.hnd.jaw_center_pos self.arm.jlc.tcp_loc_rotmat = self.hnd.jaw_center_rotmat # a list of detailed information about objects in hand, see CollisionChecker.add_objinhnd self.oih_infos = [] # collision detection if enable_cc: self.enable_cc() # component map self.manipulator_dict['arm'] = self.arm self.manipulator_dict['hnd'] = self.arm self.hnd_dict['hnd'] = self.hnd self.hnd_dict['arm'] = self.hnd
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), cdmesh_type='box', name='mvfln40', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, cdmesh_type=cdmesh_type, name=name) this_dir, this_filename = os.path.split(__file__) cpl_end_pos = self.coupling.jnts[-1]['gl_posq'] cpl_end_rotmat = self.coupling.jnts[-1]['gl_rotmatq'] self.jlc = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(0), name='mvfln40_jlc') self.jlc.jnts[1]['loc_pos'] = np.array([0, .0, .068]) self.jlc.lnks[0]['name'] = "mvfln40" self.jlc.lnks[0]['loc_pos'] = np.zeros(3) self.jlc.lnks[0]['mesh_file'] = os.path.join(this_dir, "meshes", "mvfln40.stl") self.jlc.lnks[0]['rgba'] = [.55, .55, .55, 1] # reinitialize self.jlc.reinitialize() # suction center self.suction_center_pos = np.array([0, 0, .068]) # collision detection self.all_cdelements = [] self.enable_cc(toggle_cdprimit=enable_cc)
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), cdmesh_type='box', name='robotiqhe', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, cdmesh_type=cdmesh_type, name=name) this_dir, this_filename = os.path.split(__file__) cpl_end_pos = self.coupling.jnts[-1]['gl_posq'] cpl_end_rotmat = self.coupling.jnts[-1]['gl_rotmatq'] self.jlc = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(2), name='base_lft_finger') self.jlc.jnts[1]['loc_pos'] = np.array([0, .0, .06]) self.jlc.jnts[1]['type'] = 'prismatic' self.jlc.jnts[1]['motion_rng'] = [0, .015] self.jlc.jnts[1]['loc_motionax'] = np.array([0, 1, 0]) self.jlc.jnts[2]['loc_pos'] = np.array([0, .0, .0]) self.jlc.jnts[2]['type'] = 'prismatic' self.jlc.jnts[2]['loc_motionax'] = np.array([0, 1, 0]) self.jlc.lnks[0]['name'] = "base" self.jlc.lnks[0]['loc_pos'] = np.zeros(3) self.jlc.lnks[0]['mesh_file'] = os.path.join(this_dir, "meshes", "gripper_base.dae") self.jlc.lnks[0]['rgba'] = [.35, .35, .35, 1] self.jlc.lnks[1]['name'] = "finger1" self.jlc.lnks[1]['loc_pos'] = np.array([0, 0, -.06]) self.jlc.lnks[1]['mesh_file'] = os.path.join(this_dir, "meshes", "left_finger.dae") self.jlc.lnks[1]['rgba'] = [.5, .5, .5, 1] self.jlc.lnks[2]['name'] = "finger2" self.jlc.lnks[2]['loc_pos'] = np.array([0, 0, -.06]) self.jlc.lnks[2]['mesh_file'] = os.path.join(this_dir, "meshes", "right_finger.dae") self.jlc.lnks[2]['rgba'] = [.5, .5, .5, 1] # jaw width self.jawwidth_rng = [0.0, .03] # jaw center self.jaw_center_pos = np.array([0,0,.05]) # reinitialize self.jlc.reinitialize() # collision detection self.all_cdelements=[] self.enable_cc(toggle_cdprimit=enable_cc)
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 __init__(self, pos=np.zeros(3), rotmat=np.eye(3), homeconf=np.zeros(7), name='irb14050', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) self.jlc = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=homeconf, name=name) # seven joints, n_jnts = 7+2 (tgt ranges from 1-7), nlinks = 7+1 jnt_safemargin = math.pi / 18.0 # self.jlc.jnts[1]['loc_pos'] = np.array([0.05355, -0.0725, 0.41492]) # self.jlc.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler(-0.9795, -0.5682, -2.3155) self.jlc.jnts[1]['loc_pos'] = np.array([0., 0., 0.]) self.jlc.jnts[1]['motion_rng'] = [-2.94087978961 + jnt_safemargin, 2.94087978961 - jnt_safemargin] self.jlc.jnts[2]['loc_pos'] = np.array([0.03, 0.0, 0.1]) self.jlc.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler(1.57079632679, 0.0, 0.0) self.jlc.jnts[2]['motion_rng'] = [-2.50454747661 + jnt_safemargin, 0.759218224618 - jnt_safemargin] self.jlc.jnts[3]['loc_pos'] = np.array([-0.03, 0.17283, 0.0]) self.jlc.jnts[3]['loc_rotmat'] = rm.rotmat_from_euler(-1.57079632679, 0.0, 0.0) self.jlc.jnts[3]['motion_rng'] = [-2.94087978961 + jnt_safemargin, 2.94087978961 - jnt_safemargin] self.jlc.jnts[4]['loc_pos'] = np.array([-0.04188, 0.0, 0.07873]) self.jlc.jnts[4]['loc_rotmat'] = rm.rotmat_from_euler(1.57079632679, -1.57079632679, 0.0) self.jlc.jnts[4]['motion_rng'] = [-2.15548162621 + jnt_safemargin, 1.3962634016 - jnt_safemargin] self.jlc.jnts[5]['loc_pos'] = np.array([0.0405, 0.16461, 0.0]) self.jlc.jnts[5]['loc_rotmat'] = rm.rotmat_from_euler(-1.57079632679, 0.0, 0.0) self.jlc.jnts[5]['motion_rng'] = [-5.06145483078 + jnt_safemargin, 5.06145483078 - jnt_safemargin] self.jlc.jnts[6]['loc_pos'] = np.array([-0.027, 0, 0.10039]) self.jlc.jnts[6]['loc_rotmat'] = rm.rotmat_from_euler(1.57079632679, 0.0, 0.0) self.jlc.jnts[6]['motion_rng'] = [-1.53588974176 + jnt_safemargin, 2.40855436775 - jnt_safemargin] self.jlc.jnts[7]['loc_pos'] = np.array([0.027, 0.029, 0.0]) self.jlc.jnts[7]['loc_rotmat'] = rm.rotmat_from_euler(-1.57079632679, 0.0, 0.0) self.jlc.jnts[7]['motion_rng'] = [-3.99680398707 + jnt_safemargin, 3.99680398707 - jnt_safemargin] # links self.jlc.lnks[1]['name'] = "link_1" self.jlc.lnks[1]['meshfile'] = os.path.join(this_dir, "meshes", "link_1.stl") self.jlc.lnks[1]['rgba'] = [.5, .5, .5, 1] self.jlc.lnks[2]['name'] = "link_2" self.jlc.lnks[2]['meshfile'] = os.path.join(this_dir, "meshes", "link_2.stl") self.jlc.lnks[2]['rgba'] = [.929, .584, .067, 1] self.jlc.lnks[3]['name'] = "link_3" self.jlc.lnks[3]['meshfile'] = os.path.join(this_dir, "meshes", "link_3.stl") self.jlc.lnks[3]['rgba'] = [.7, .7, .7, 1] self.jlc.lnks[4]['name'] = "link_4" self.jlc.lnks[4]['meshfile'] = os.path.join(this_dir, "meshes", "link_4.stl") self.jlc.lnks[4]['rgba'] = [0.180, .4, 0.298, 1] self.jlc.lnks[5]['name'] = "link_5" self.jlc.lnks[5]['meshfile'] = os.path.join(this_dir, "meshes", "link_5.stl") self.jlc.lnks[5]['rgba'] = [.7, .7, .7, 1] self.jlc.lnks[6]['name'] = "link_6" self.jlc.lnks[6]['meshfile'] = os.path.join(this_dir, "meshes", "link_6.stl") self.jlc.lnks[6]['rgba'] = [0.180, .4, 0.298, 1] self.jlc.lnks[7]['name'] = "link_7" # self.jlc.lnks[7]['meshfile'] = os.path.join(this_dir, "meshes", "link_7.stl") # not really needed to visualize # self.jlc.lnks[7]['rgba'] = [.5,.5,.5,1] # reinitialization self.jlc.reinitialize() # collision detection if enable_cc: self.enable_cc()
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), name='XYBot'): super().__init__(pos=pos, rotmat=rotmat, name=name) self.jlc = jl.JLChain(homeconf=np.zeros(2), name='XYBot') self.jlc.jnts[1]['type'] = 'prismatic' self.jlc.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) self.jlc.jnts[1]['loc_pos'] = np.zeros(3) self.jlc.jnts[1]['motion_rng'] = [-2.0, 15.0] self.jlc.jnts[2]['type'] = 'prismatic' self.jlc.jnts[2]['loc_motionax'] = np.array([0, 1, 0]) self.jlc.jnts[2]['loc_pos'] = np.zeros(3) self.jlc.jnts[2]['motion_rng'] = [-2.0, 15.0] self.jlc.reinitialize()
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), name="cobotta", enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) # base plate self.base_plate = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(0), name='base_plate_ripps') self.base_plate.jnts[1]['loc_pos'] = np.array([0, 0, 0.01]) self.base_plate.lnks[0]['mesh_file'] = os.path.join( this_dir, "meshes", "base_plate_ripps.stl") self.base_plate.lnks[0]['rgba'] = [.55, .55, .55, 1] self.base_plate.reinitialize() # arm arm_homeconf = np.zeros(6) arm_homeconf[1] = -math.pi / 6 arm_homeconf[2] = math.pi / 2 arm_homeconf[4] = math.pi / 6 self.arm = cbta.CobottaArm( pos=self.base_plate.jnts[-1]['gl_posq'], rotmat=self.base_plate.jnts[-1]['gl_rotmatq'], homeconf=arm_homeconf, name='arm', enable_cc=False) # gripper self.gripper_loc_rotmat = rm.rotmat_from_axangle( [0, 0, 1], np.pi) # 20220607 rotate the pipetting end with 180^o. self.hnd = cbtp.CobottaPipette( pos=self.arm.jnts[-1]['gl_posq'], rotmat=self.arm.jnts[-1]['gl_rotmatq'].dot( self.gripper_loc_rotmat), name='hnd_s', enable_cc=False) # tool center point self.arm.jlc.tcp_jnt_id = -1 self.arm.jlc.tcp_loc_pos = self.gripper_loc_rotmat.dot( self.hnd.jaw_center_pos) self.arm.jlc.tcp_loc_rotmat = self.gripper_loc_rotmat.dot( self.hnd.jaw_center_rotmat) # a list of detailed information about objects in hand, see CollisionChecker.add_objinhnd self.oih_infos = [] # collision detection if enable_cc: self.enable_cc() # component map self.manipulator_dict['arm'] = self.arm self.manipulator_dict['hnd'] = self.arm self.hnd_dict['hnd'] = self.hnd self.hnd_dict['arm'] = self.hnd
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), cdmesh_type='box', name='tbm_gripper', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, cdmesh_type=cdmesh_type, name=name) this_dir, this_filename = os.path.split(__file__) cpl_end_pos = self.coupling.jnts[-1]['gl_posq'] cpl_end_rotmat = self.coupling.jnts[-1]['gl_rotmatq'] # left finger self.lft_fgr = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(1), name='lft_outer') self.lft_fgr.jnts[1]['loc_pos'] = np.array([0.119, 0, -.058]) self.lft_fgr.jnts[1]['motion_rng'] = [-.8, .8] self.lft_fgr.jnts[1]['loc_motionax'] = np.array([0, 1, 0]) # right finger self.rgt_fgr = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(1), name='lft_outer') self.rgt_fgr.jnts[1]['loc_pos'] = np.array([0.119, 0, 0.058]) self.rgt_fgr.jnts[1]['motion_rng'] = [-.8, .8] self.rgt_fgr.jnts[1]['loc_motionax'] = np.array([0, 1, 0]) # links # palm and left finger self.lft_fgr.lnks[0]['name'] = "palm" self.lft_fgr.lnks[0]['loc_pos'] = np.zeros(3) self.lft_fgr.lnks[0]['meshfile'] = os.path.join(this_dir, "meshes", "palm.stl") self.lft_fgr.lnks[0]['rgba'] = [.5, .5, .5, 1] self.lft_fgr.lnks[1]['name'] = "finger1" self.lft_fgr.lnks[1]['loc_pos'] = np.zeros(3) self.lft_fgr.lnks[1]['meshfile'] = os.path.join(this_dir, "meshes", "finger1.stl") self.lft_fgr.lnks[1]['rgba'] = [0.792156862745098, 0.819607843137255, 0.933333333333333, 1] # right finger self.rgt_fgr.lnks[1]['name'] = "finger2" self.rgt_fgr.lnks[1]['loc_pos'] = np.zeros(3) self.rgt_fgr.lnks[1]['meshfile'] = os.path.join(this_dir, "meshes", "finger2.stl") self.rgt_fgr.lnks[1]['rgba'] = [0.792156862745098, 0.819607843137255, 0.933333333333333, 1] # reinitialize self.lft_fgr.reinitialize() self.rgt_fgr.reinitialize() # jaw width self.jawwidth_rng = [-.5, .5] # jaw center self.jaw_center_pos = np.array([.325, 0, 0]) # collision detection self.all_cdelements = [] self.enable_cc(toggle_cdprimit=enable_cc)
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), ndof=5, base_thickness=.005, base_length=.3, name='stem'): self.pos = pos self.rotmat = rotmat self.jlc = jlc.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(ndof), name=name + "jlchain") for i in range(1, self.jlc.ndof+1): self.jlc.jnts[i]['loc_pos']=np.array([0,0, base_length/5]) self.jlc.jnts[i]['loc_motionax']=np.array([1,0,0]) self.jlc.reinitialize() for link_id in range(self.jlc.ndof + 1): self.jlc.lnks[link_id]['collision_model'] = cm.gen_stick(spos=np.zeros(3), epos=rotmat.T.dot(self.jlc.jnts[link_id + 1]['gl_posq']-self.jlc.jnts[link_id]['gl_posq']), thickness=base_thickness/(link_id+1)**(1/3), sections=24)
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), name='nextage', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) central_homeconf = np.radians(np.array([.0, .0, .0])) lft_arm_homeconf = np.radians( np.array([central_homeconf[0], 15, 0, -143, 0, 0, 0])) rgt_arm_homeconf = np.radians( np.array([central_homeconf[0], -15, 0, -143, 0, 0, 0])) # central self.central_body = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=central_homeconf, name='centeral_body') self.central_body.jnts[1]['loc_pos'] = np.array([0, 0, 0]) self.central_body.jnts[1]['loc_motionax'] = np.array([0, 0, 1]) self.central_body.jnts[1]['motion_rng'] = [-2.84489, 2.84489] self.central_body.jnts[2]['loc_pos'] = np.array([0, 0, 0.5695]) self.central_body.jnts[2]['loc_motionax'] = np.array([0, 0, 1]) self.central_body.jnts[2]['motion_rng'] = [-1.22173, 1.22173] self.central_body.jnts[3]['loc_pos'] = np.array([0, 0, 0]) self.central_body.jnts[3]['loc_motionax'] = np.array([0, 1, 0]) self.central_body.jnts[3]['motion_rng'] = [-0.349066, 1.22173] self.central_body.lnks[0]['name'] = "nextage_base" self.central_body.lnks[0]['loc_pos'] = np.array([0, 0, 0.97]) self.central_body.lnks[0]['collision_model'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "waist_link_mesh.dae"), cdprimit_type="user_defined", expand_radius=.005, userdefined_cdprimitive_fn=self._waist_combined_cdnp) self.central_body.lnks[0]['rgba'] = [.77, .77, .77, 1.0] self.central_body.lnks[1]['name'] = "nextage_chest" self.central_body.lnks[1]['loc_pos'] = np.array([0, 0, 0]) self.central_body.lnks[1]['collision_model'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "chest_joint0_link_mesh.dae"), cdprimit_type="user_defined", expand_radius=.005, userdefined_cdprimitive_fn=self._chest_combined_cdnp) self.central_body.lnks[1]['rgba'] = [1, .65, .5, 1] self.central_body.lnks[2]['name'] = "head_joint0_link_mesh" self.central_body.lnks[2]['loc_pos'] = np.array([0, 0, 0.5695]) self.central_body.lnks[2]['mesh_file'] = os.path.join( this_dir, "meshes", "head_joint0_link_mesh.dae") self.central_body.lnks[2]['rgba'] = [.35, .35, .35, 1] self.central_body.lnks[3]['name'] = "nextage_chest" self.central_body.lnks[3]['loc_pos'] = np.array([0, 0, 0]) self.central_body.lnks[3]['mesh_file'] = os.path.join( this_dir, "meshes", "head_joint1_link_mesh.dae") self.central_body.lnks[3]['rgba'] = [.63, .63, .63, 1] self.central_body.reinitialize() # lft self.lft_arm = jl.JLChain( pos=self.central_body.jnts[1]['gl_posq'], rotmat=self.central_body.jnts[1]['gl_rotmatq'], homeconf=lft_arm_homeconf, name='lft_arm') self.lft_arm.jnts[1]['loc_pos'] = np.array([0, 0, 0]) self.lft_arm.jnts[1]['loc_motionax'] = np.array([0, 0, 1]) self.lft_arm.jnts[2]['loc_pos'] = np.array([0, 0.145, 0.370296]) self.lft_arm.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler( -0.261799, 0, 0) self.lft_arm.jnts[2]['loc_motionax'] = np.array([0, 0, 1]) self.lft_arm.jnts[2]['motion_rng'] = [-1.53589, 1.53589] self.lft_arm.jnts[3]['loc_pos'] = np.array([0, 0, 0]) self.lft_arm.jnts[3]['loc_motionax'] = np.array([0, 1, 0]) self.lft_arm.jnts[3]['motion_rng'] = [-2.44346, 1.0472] self.lft_arm.jnts[4]['loc_pos'] = np.array([0, 0.095, -0.25]) self.lft_arm.jnts[4]['loc_motionax'] = np.array([0, 1, 0]) self.lft_arm.jnts[4]['motion_rng'] = [-2.75762, 0] self.lft_arm.jnts[5]['loc_pos'] = np.array([-0.03, 0, 0]) self.lft_arm.jnts[5]['loc_motionax'] = np.array([0, 0, 1]) self.lft_arm.jnts[5]['motion_rng'] = [-1.8326, 2.87979] self.lft_arm.jnts[6]['loc_pos'] = np.array([0, 0, -0.235]) self.lft_arm.jnts[6]['loc_motionax'] = np.array([0, 1, 0]) self.lft_arm.jnts[6]['motion_rng'] = [-1.74533, 1.74533] self.lft_arm.jnts[7]['loc_pos'] = np.array([-0.047, 0, -0.09]) self.lft_arm.jnts[7]['loc_motionax'] = np.array([1, 0, 0]) self.lft_arm.jnts[7]['motion_rng'] = [-2.84489, 2.84489] self.lft_arm.lnks[2]['name'] = "lft_arm_joint0" self.lft_arm.lnks[2]['loc_pos'] = np.array([0, 0.145, 0.370296]) self.lft_arm.lnks[2]['loc_rotmat'] = rm.rotmat_from_euler( -0.261799, 0, 0) self.lft_arm.lnks[2]['mesh_file'] = os.path.join( this_dir, "meshes", "larm_joint0_link_mesh.dae") self.lft_arm.lnks[2]['rgba'] = [.35, .35, .35, 1] self.lft_arm.lnks[3]['name'] = "lft_arm_joint1" self.lft_arm.lnks[3]['loc_pos'] = np.array([0, 0, 0]) self.lft_arm.lnks[3]['mesh_file'] = os.path.join( this_dir, "meshes", "larm_joint1_link_mesh.dae") self.lft_arm.lnks[3]['rgba'] = [.57, .57, .57, 1] self.lft_arm.lnks[4]['name'] = "lft_arm_joint2" self.lft_arm.lnks[4]['loc_pos'] = np.array([0, 0.095, -0.25]) self.lft_arm.lnks[4]['mesh_file'] = os.path.join( this_dir, "meshes", "larm_joint2_link_mesh.dae") self.lft_arm.lnks[4]['rgba'] = [.35, .35, .35, 1] self.lft_arm.lnks[5]['name'] = "lft_arm_joint3" self.lft_arm.lnks[5]['loc_pos'] = np.array([-0.03, 0, 0]) self.lft_arm.lnks[5]['mesh_file'] = os.path.join( this_dir, "meshes", "larm_joint3_link_mesh.dae") self.lft_arm.lnks[5]['rgba'] = [.35, .35, .35, 1] self.lft_arm.lnks[6]['name'] = "lft_arm_joint4" self.lft_arm.lnks[6]['loc_pos'] = np.array([0, 0, -0.235]) self.lft_arm.lnks[6]['mesh_file'] = os.path.join( this_dir, "meshes", "larm_joint4_link_mesh.dae") self.lft_arm.lnks[6]['rgba'] = [.7, .7, .7, 1] self.lft_arm.lnks[7]['name'] = "lft_arm_joint5" self.lft_arm.lnks[7]['loc_pos'] = np.array([-0.047, 0, -0.09]) self.lft_arm.lnks[7]['mesh_file'] = os.path.join( this_dir, "meshes", "larm_joint5_link_mesh.dae") self.lft_arm.lnks[7]['rgba'] = [.57, .57, .57, 1] self.lft_arm.reinitialize() # rgt self.rgt_arm = jl.JLChain( pos=self.central_body.jnts[1]['gl_posq'], rotmat=self.central_body.jnts[1]['gl_rotmatq'], homeconf=rgt_arm_homeconf, name='rgt_arm') self.rgt_arm.jnts[1]['loc_pos'] = np.array([0, 0, 0]) self.rgt_arm.jnts[1]['loc_motionax'] = np.array([0, 0, 1]) self.rgt_arm.jnts[2]['loc_pos'] = np.array([0, -0.145, 0.370296]) self.rgt_arm.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler( 0.261799, 0, 0) self.rgt_arm.jnts[2]['loc_motionax'] = np.array([0, 0, 1]) self.rgt_arm.jnts[2]['motion_rng'] = [-1.53589, 1.53589] self.rgt_arm.jnts[3]['loc_pos'] = np.array([0, 0, 0]) self.rgt_arm.jnts[3]['loc_motionax'] = np.array([0, 1, 0]) self.rgt_arm.jnts[3]['motion_rng'] = [-2.44346, 1.0472] self.rgt_arm.jnts[4]['loc_pos'] = np.array([0, -0.095, -0.25]) self.rgt_arm.jnts[4]['loc_motionax'] = np.array([0, 1, 0]) self.rgt_arm.jnts[4]['motion_rng'] = [-2.75762, 0] self.rgt_arm.jnts[5]['loc_pos'] = np.array([-0.03, 0, 0]) self.rgt_arm.jnts[5]['loc_motionax'] = np.array([0, 0, 1]) self.rgt_arm.jnts[5]['motion_rng'] = [-1.8326, 2.87979] self.rgt_arm.jnts[6]['loc_pos'] = np.array([0, 0, -0.235]) self.rgt_arm.jnts[6]['loc_motionax'] = np.array([0, 1, 0]) self.rgt_arm.jnts[6]['motion_rng'] = [-1.74533, 1.74533] self.rgt_arm.jnts[7]['loc_pos'] = np.array([-0.047, 0, -0.09]) self.rgt_arm.jnts[7]['loc_motionax'] = np.array([1, 0, 0]) self.rgt_arm.jnts[7]['motion_rng'] = [-2.84489, 2.84489] self.rgt_arm.lnks[2]['name'] = "rgt_arm_joint0" self.rgt_arm.lnks[2]['loc_pos'] = np.array([0, -0.145, 0.370296]) self.rgt_arm.lnks[2]['loc_rotmat'] = rm.rotmat_from_euler( 0.261799, 0, 0) self.rgt_arm.lnks[2]['mesh_file'] = os.path.join( this_dir, "meshes", "rarm_joint0_link_mesh.dae") self.rgt_arm.lnks[2]['rgba'] = [.35, .35, .35, 1] self.rgt_arm.lnks[3]['name'] = "rgt_arm_joint1" self.rgt_arm.lnks[3]['loc_pos'] = np.array([0, 0, 0]) self.rgt_arm.lnks[3]['mesh_file'] = os.path.join( this_dir, "meshes", "rarm_joint1_link_mesh.dae") self.rgt_arm.lnks[3]['rgba'] = [.57, .57, .57, 1] self.rgt_arm.lnks[4]['name'] = "rgt_arm_joint2" self.rgt_arm.lnks[4]['loc_pos'] = np.array([0, -0.095, -0.25]) self.rgt_arm.lnks[4]['mesh_file'] = os.path.join( this_dir, "meshes", "rarm_joint2_link_mesh.dae") self.rgt_arm.lnks[4]['rgba'] = [.35, .35, .35, 1] self.rgt_arm.lnks[5]['name'] = "rgt_arm_joint3" self.rgt_arm.lnks[5]['loc_pos'] = np.array([-0.03, 0, 0]) self.rgt_arm.lnks[5]['mesh_file'] = os.path.join( this_dir, "meshes", "rarm_joint3_link_mesh.dae") self.rgt_arm.lnks[5]['rgba'] = [.35, .35, .35, 1] self.rgt_arm.lnks[6]['name'] = "rgt_arm_joint4" self.rgt_arm.lnks[6]['loc_pos'] = np.array([0, 0, -0.235]) self.rgt_arm.lnks[6]['mesh_file'] = os.path.join( this_dir, "meshes", "rarm_joint4_link_mesh.dae") self.rgt_arm.lnks[6]['rgba'] = [.7, .7, .7, 1] self.rgt_arm.lnks[7]['name'] = "rgt_arm_joint5" self.rgt_arm.lnks[7]['loc_pos'] = np.array([-0.047, 0, -0.09]) self.rgt_arm.lnks[7]['mesh_file'] = os.path.join( this_dir, "meshes", "rarm_joint5_link_mesh.dae") self.rgt_arm.lnks[7]['rgba'] = [.57, .57, .57, 1] self.rgt_arm.reinitialize() # tool center point # lft self.lft_arm.tcp_jnt_id = -1 # self.lft_arm.tcp_loc_pos = self.lft_hnd.jaw_center_pos # self.lft_arm.tcp_loc_rotmat = self.lft_hnd.jaw_center_rotmat self.lft_arm.tcp_loc_pos = np.zeros(3) self.lft_arm.tcp_loc_rotmat = np.eye(3) # rgt self.rgt_arm.tcp_jnt_id = -1 # self.rgt_arm.tcp_loc_pos = self.rgt_hnd.jaw_center_pos # self.rgt_arm.tcp_loc_rotmat = self.rgt_hnd.jaw_center_rotmat self.rgt_arm.tcp_loc_pos = np.zeros(3) self.rgt_arm.tcp_loc_rotmat = np.eye(3) # a list of detailed information about objects in hand, see CollisionChecker.add_objinhnd self.lft_oih_infos = [] self.rgt_oih_infos = [] # collision detection if enable_cc: self.enable_cc() # component map self.manipulator_dict['rgt_arm'] = self.rgt_arm self.manipulator_dict['lft_arm'] = self.lft_arm self.manipulator_dict['rgt_arm_waist'] = self.rgt_arm self.manipulator_dict['lft_arm_waist'] = self.lft_arm
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), homeconf=np.zeros(6), name='tbm_arm', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) self.jlc = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=homeconf, name=name) # six joints, n_jnts = 6+2 (tgt ranges from 1-6), nlinks = 6+1 # self.jlc.jnts[1]['loc_pos'] = np.array([0.2, 0, 0]) # self.jlc.jnts[1]['type'] = 'prismatic' # self.jlc.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) # self.jlc.jnts[1]['motion_rng'] = [-.1, 1.25] self.jlc.jnts[1]['loc_pos'] = np.array([0, 0, 0.346]) self.jlc.jnts[1]['loc_motionax'] = np.array([0, 0, 1]) self.jlc.jnts[1]['motion_rng'] = [-math.radians(60), math.radians(60)] self.jlc.jnts[2]['loc_pos'] = np.array([0.645, .0, .0]) self.jlc.jnts[2]['loc_motionax'] = np.array([1, 0, 0]) # self.jlc.jnts[2]['motion_rng'] = [-2*math.pi, 2*math.pi] self.jlc.jnts[3]['loc_pos'] = np.array([.425, .0, .0]) self.jlc.jnts[3]['loc_motionax'] = np.array([0, 0, 1]) self.jlc.jnts[3]['motion_rng'] = [-math.radians(90), math.radians(90)] self.jlc.jnts[4]['loc_pos'] = np.array([0.587, .0, .0]) self.jlc.jnts[4]['loc_motionax'] = np.array([1, 0, 0]) # self.jlc.jnts[4]['motion_rng'] = [-2*math.pi, 2*math.pi] self.jlc.jnts[5]['loc_pos'] = np.array([.63, .0, .0]) self.jlc.jnts[5]['loc_motionax'] = np.array([0, 1, 0]) self.jlc.jnts[5]['motion_rng'] = [-math.radians(115), math.radians(115)] self.jlc.jnts[6]['loc_pos'] = np.array([.329, .0, .0]) self.jlc.jnts[6]['loc_motionax'] = np.array([1, 0, 0]) # self.jlc.jnts[6]['motion_rng'] = [-2*math.pi, 2*math.pi] # links self.jlc.lnks[0]['name'] = "base" self.jlc.lnks[0]['loc_pos'] = np.zeros(3) # self.jlc.lnks[0]['mass'] = 1.4 # self.jlc.lnks[0]['com'] = np.array([-.02131, .000002, .044011]) self.jlc.lnks[0]['meshfile'] = os.path.join(this_dir, "meshes", "base.stl") self.jlc.lnks[0]['rgba'] = [.5, .5, .5, 1.0] self.jlc.lnks[1]['name'] = "j1" self.jlc.lnks[1]['loc_pos'] = np.zeros(3) self.jlc.lnks[1]['com'] = np.array([.0, .0, .15]) self.jlc.lnks[1]['mass'] = 1.29 self.jlc.lnks[1]['meshfile'] = os.path.join(this_dir, "meshes", "joint1.stl") self.jlc.lnks[1]['rgba'] = [.7, .7, .7, 1.0] self.jlc.lnks[2]['name'] = "j2" self.jlc.lnks[2]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[2]['com'] = np.array([-.02, .1, .07]) self.jlc.lnks[2]['mass'] = 0.39 self.jlc.lnks[2]['meshfile'] = os.path.join(this_dir, "meshes", "joint2.stl") self.jlc.lnks[2]['rgba'] = [.77, .77, .60, 1] self.jlc.lnks[3]['name'] = "j3" self.jlc.lnks[3]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[3]['com'] = np.array([-.01, .02, .03]) self.jlc.lnks[3]['mass'] = .35 self.jlc.lnks[3]['meshfile'] = os.path.join(this_dir, "meshes", "joint3.stl") self.jlc.lnks[3]['rgba'] = [.35, .35, .35, 1.0] self.jlc.lnks[4]['name'] = "j4" self.jlc.lnks[4]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[4]['com'] = np.array([.0, .0, 0.055]) self.jlc.lnks[4]['mass'] = 0.35 self.jlc.lnks[4]['meshfile'] = os.path.join(this_dir, "meshes", "joint4.stl") self.jlc.lnks[4]['rgba'] = [.7, .7, .7, 1.0] self.jlc.lnks[5]['name'] = "j5" self.jlc.lnks[5]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[5]['com'] = np.array([.0, -.04, .015]) self.jlc.lnks[5]['mass'] = 0.19 self.jlc.lnks[5]['meshfile'] = os.path.join(this_dir, "meshes", "joint5.stl") self.jlc.lnks[5]['rgba'] = [.77, .77, .60, 1] self.jlc.lnks[6]['name'] = "j6" self.jlc.lnks[6]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[6]['com'] = np.array([.0, .0, 0]) self.jlc.lnks[6]['mass'] = 0.03 self.jlc.lnks[6]['meshfile'] = None self.jlc.lnks[6]['rgba'] = [.5, .5, .5, 1.0] self.jlc.reinitialize() # collision detection if enable_cc: self.enable_cc()
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), homeconf=np.zeros(6), name='ur3', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) self.jlc = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=homeconf, name=name) # six joints, n_jnts = 6+2 (tgt ranges from 1-6), nlinks = 6+1 self.jlc.jnts[1]['loc_pos'] = np.array([0, 0, .1519]) self.jlc.jnts[2]['loc_pos'] = np.array([0, 0.1198, 0]) self.jlc.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler( .0, math.pi / 2.0, .0) self.jlc.jnts[2]['loc_motionax'] = np.array([0, 1, 0]) self.jlc.jnts[3]['loc_pos'] = np.array([0, -.0925, .24365]) self.jlc.jnts[3]['loc_rotmat'] = rm.rotmat_from_euler(.0, .0, .0) self.jlc.jnts[3]['loc_motionax'] = np.array([0, 1, 0]) self.jlc.jnts[4]['loc_pos'] = np.array([.0, .0, 0.21325]) self.jlc.jnts[4]['loc_rotmat'] = rm.rotmat_from_euler( .0, math.pi / 2.0, 0) self.jlc.jnts[4]['loc_motionax'] = np.array([0, 1, 0]) self.jlc.jnts[5]['loc_pos'] = np.array([0, .11235 + .0925 - .1198, 0]) self.jlc.jnts[5]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, 0) self.jlc.jnts[5]['loc_motionax'] = np.array([0, 0, 1]) self.jlc.jnts[6]['loc_pos'] = np.array([0, 0, .08535]) self.jlc.jnts[6]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, 0) self.jlc.jnts[6]['loc_motionax'] = np.array([0, 1, 0]) self.jlc.jnts[7]['loc_pos'] = np.array([0, .0819, 0]) self.jlc.jnts[7]['loc_rotmat'] = rm.rotmat_from_euler( -math.pi / 2.0, math.pi / 2.0, 0) # self.jlc.jnts[7]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, 0) # links self.jlc.lnks[0]['name'] = "base" self.jlc.lnks[0]['loc_pos'] = np.zeros(3) self.jlc.lnks[0]['mass'] = 2.0 self.jlc.lnks[0]['mesh_file'] = os.path.join(this_dir, "meshes", "base.stl") self.jlc.lnks[0]['rgba'] = [.5, .5, .5, 1.0] self.jlc.lnks[1]['name'] = "shoulder" self.jlc.lnks[1]['loc_pos'] = np.zeros(3) self.jlc.lnks[1]['com'] = np.array([.0, -.02, .0]) self.jlc.lnks[1]['mass'] = 1.95 self.jlc.lnks[1]['mesh_file'] = os.path.join(this_dir, "meshes", "shoulder.stl") self.jlc.lnks[1]['rgba'] = [.1, .3, .5, 1.0] self.jlc.lnks[2]['name'] = "upperarm" self.jlc.lnks[2]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[2]['com'] = np.array([.13, 0, .1157]) self.jlc.lnks[2]['mass'] = 3.42 self.jlc.lnks[2]['mesh_file'] = os.path.join(this_dir, "meshes", "upperarm.stl") # self.jlc.lnks[2]['rgba'] = [.7,.7,.7, 1.0] self.jlc.lnks[3]['name'] = "forearm" self.jlc.lnks[3]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[3]['com'] = np.array([.05, .0, .0238]) self.jlc.lnks[3]['mass'] = 1.437 self.jlc.lnks[3]['mesh_file'] = os.path.join(this_dir, "meshes", "forearm.stl") self.jlc.lnks[3]['rgba'] = [.35, .35, .35, 1.0] self.jlc.lnks[4]['name'] = "wrist1" self.jlc.lnks[4]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[4]['com'] = np.array([.0, .0, 0.01]) self.jlc.lnks[4]['mass'] = 0.871 self.jlc.lnks[4]['mesh_file'] = os.path.join(this_dir, "meshes", "wrist1.stl") # self.jlc.lnks[4]['rgba'] = [.7,.7,.7, 1.0] self.jlc.lnks[5]['name'] = "wrist2" self.jlc.lnks[5]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[5]['com'] = np.array([.0, .0, 0.01]) self.jlc.lnks[5]['mass'] = 0.8 self.jlc.lnks[5]['mesh_file'] = os.path.join(this_dir, "meshes", "wrist2.stl") self.jlc.lnks[5]['rgba'] = [.1, .3, .5, 1.0] self.jlc.lnks[6]['name'] = "wrist3" self.jlc.lnks[6]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[6]['com'] = np.array([.0, .0, -0.02]) self.jlc.lnks[6]['mass'] = 0.8 self.jlc.lnks[6]['mesh_file'] = os.path.join(this_dir, "meshes", "wrist3.stl") self.jlc.lnks[6]['rgba'] = [.5, .5, .5, 1.0] self.jlc.reinitialize() # collision detection if enable_cc: self.enable_cc()
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), cdmesh_type='box', name='robotiq85', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, cdmesh_type=cdmesh_type, name=name) this_dir, this_filename = os.path.split(__file__) cpl_end_pos = self.coupling.jnts[-1]['gl_posq'] cpl_end_rotmat = self.coupling.jnts[-1]['gl_rotmatq'] # - lft_outer self.lft_outer = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(4), name='lft_outer') self.lft_outer.jnts[1]['loc_pos'] = np.array([0, -.0306011, .054904]) self.lft_outer.jnts[1]['motion_rng'] = [.0, .8] self.lft_outer.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler( 0, 0, math.pi) self.lft_outer.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) self.lft_outer.jnts[2]['loc_pos'] = np.array([0, .0315, -.0041]) # passive self.lft_outer.jnts[2]['loc_motionax'] = np.array([1, 0, 0]) self.lft_outer.jnts[3]['loc_pos'] = np.array([0, .0061, .0471]) self.lft_outer.jnts[3]['loc_motionax'] = np.array([1, 0, 0]) self.lft_outer.jnts[4]['loc_pos'] = np.zeros(3) # https://github.com/Danfoa uses geometry instead of the dae mesh. The following coordiante is needed # self.lft_outer.jnts[4]['loc_pos'] = np.array([0, -0.0220203446692936, .03242]) # - lft_inner self.lft_inner = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(1), name='lft_inner') self.lft_inner.jnts[1]['loc_pos'] = np.array([0, -.0127, .06142]) self.lft_inner.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler( 0, 0, math.pi) self.lft_inner.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) # - rgt_outer self.rgt_outer = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(4), name='rgt_outer') self.rgt_outer.jnts[1]['loc_pos'] = np.array([0, .0306011, .054904]) self.rgt_outer.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) self.rgt_outer.jnts[2]['loc_pos'] = np.array([0, .0315, -.0041]) # passive self.rgt_outer.jnts[2]['loc_motionax'] = np.array([1, 0, 0]) self.rgt_outer.jnts[3]['loc_pos'] = np.array([0, .0061, .0471]) self.rgt_outer.jnts[3]['loc_motionax'] = np.array([1, 0, 0]) self.rgt_outer.jnts[4]['loc_pos'] = np.zeros(3) # https://github.com/Danfoa uses geometry instead of the dae mesh. The following coordiante is needed # self.rgt_outer.jnts[4]['loc_pos'] = np.array([0, -0.0220203446692936, .03242]) # - rgt_inner self.rgt_inner = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(1), name='rgt_inner') self.rgt_inner.jnts[1]['loc_pos'] = np.array([0, .0127, .06142]) self.rgt_inner.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) # links # - lft_outer self.lft_outer.lnks[0]['name'] = "robotiq85_gripper_base" self.lft_outer.lnks[0]['loc_pos'] = np.zeros(3) self.lft_outer.lnks[0]['com'] = np.array( [8.625e-08, -4.6583e-06, 0.03145]) self.lft_outer.lnks[0]['mass'] = 0.22652 self.lft_outer.lnks[0]['meshfile'] = os.path.join( this_dir, "meshes", "robotiq_arg2f_85_base_link_cvt.stl") self.lft_outer.lnks[0]['rgba'] = [.2, .2, .2, 1] self.lft_outer.lnks[1]['name'] = "left_outer_knuckle" self.lft_outer.lnks[1]['loc_pos'] = np.zeros(3) self.lft_outer.lnks[1]['com'] = np.array( [-0.000200000000003065, 0.0199435877845359, 0.0292245259211331]) self.lft_outer.lnks[1]['mass'] = 0.00853198276973456 self.lft_outer.lnks[1]['meshfile'] = os.path.join( this_dir, "meshes", "robotiq_arg2f_85_outer_knuckle.stl") self.lft_outer.lnks[1]['rgba'] = [ 0.792156862745098, 0.819607843137255, 0.933333333333333, 1 ] self.lft_outer.lnks[2]['name'] = "left_outer_finger" self.lft_outer.lnks[2]['loc_pos'] = np.zeros(3) self.lft_outer.lnks[2]['com'] = np.array( [0.00030115855001899, 0.0373907951953854, -0.0208027427000385]) self.lft_outer.lnks[2]['mass'] = 0.022614240507152 self.lft_outer.lnks[2]['meshfile'] = os.path.join( this_dir, "meshes", "robotiq_arg2f_85_outer_finger_cvt.stl") self.lft_outer.lnks[2]['rgba'] = [.2, .2, .2, 1] self.lft_outer.lnks[3]['name'] = "left_inner_finger" self.lft_outer.lnks[3]['loc_pos'] = np.zeros(3) self.lft_outer.lnks[3]['com'] = np.array( [0.000299999999999317, 0.0160078233491243, -0.0136945669206257]) self.lft_outer.lnks[3]['mass'] = 0.0104003125914103 self.lft_outer.lnks[3]['meshfile'] = os.path.join( this_dir, "meshes", "robotiq_85_inner_finger_gelsight_cvt.stl") self.lft_outer.lnks[3]['rgba'] = [0.57, 0.57, 0.57, 1] self.lft_outer.lnks[4]['name'] = "left_inner_finger_pad" self.lft_outer.lnks[4]['loc_pos'] = np.zeros(3) # self.lft_outer.lnks[4]['meshfile'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_85_pad.dae") # self.lft_outer.lnks[4]['scale'] = [1e-3, 1e-3, 1e-3] # self.lft_outer.lnks[4]['rgba'] = [0.792156862745098, 0.819607843137255, 0.933333333333333, 1] # - lft_inner self.lft_inner.lnks[1]['name'] = "left_inner_knuckle" self.lft_inner.lnks[1]['loc_pos'] = np.zeros(3) self.lft_inner.lnks[1]['com'] = np.array( [0.000123011831763771, 0.0507850843201817, 0.00103968640075166]) self.lft_inner.lnks[1]['mass'] = 0.0271177346495152 self.lft_inner.lnks[1]['meshfile'] = os.path.join( this_dir, "meshes", "robotiq_arg2f_85_inner_knuckle_cvt.stl") self.lft_inner.lnks[1]['rgba'] = [.2, .2, .2, 1] # - rgt_outer self.rgt_outer.lnks[1]['name'] = "left_outer_knuckle" self.rgt_outer.lnks[1]['loc_pos'] = np.zeros(3) self.rgt_outer.lnks[1]['com'] = np.array( [-0.000200000000003065, 0.0199435877845359, 0.0292245259211331]) self.rgt_outer.lnks[1]['mass'] = 0.00853198276973456 self.rgt_outer.lnks[1]['meshfile'] = os.path.join( this_dir, "meshes", "robotiq_arg2f_85_outer_knuckle.stl") self.rgt_outer.lnks[1]['rgba'] = [ 0.792156862745098, 0.819607843137255, 0.933333333333333, 1 ] self.rgt_outer.lnks[2]['name'] = "left_outer_finger" self.rgt_outer.lnks[2]['loc_pos'] = np.zeros(3) self.rgt_outer.lnks[2]['com'] = np.array( [0.00030115855001899, 0.0373907951953854, -0.0208027427000385]) self.rgt_outer.lnks[2]['mass'] = 0.022614240507152 self.rgt_outer.lnks[2]['meshfile'] = os.path.join( this_dir, "meshes", "robotiq_arg2f_85_outer_finger_cvt.stl") self.rgt_outer.lnks[2]['rgba'] = [.2, .2, .2, 1] self.rgt_outer.lnks[3]['name'] = "left_inner_finger" self.rgt_outer.lnks[3]['loc_pos'] = np.zeros(3) self.rgt_outer.lnks[3]['com'] = np.array( [0.000299999999999317, 0.0160078233491243, -0.0136945669206257]) self.rgt_outer.lnks[3]['mass'] = 0.0104003125914103 self.rgt_outer.lnks[3]['meshfile'] = os.path.join( this_dir, "meshes", "robotiq_85_inner_finger_gelsight_cvt.stl") self.rgt_outer.lnks[3]['rgba'] = [0.57, 0.57, 0.57, 1] self.rgt_outer.lnks[4]['name'] = "left_inner_finger_pad" self.rgt_outer.lnks[4]['loc_pos'] = np.zeros(3) # self.rgt_outer.lnks[4]['meshfile'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_85_pad.dae") # self.rgt_outer.lnks[4]['scale'] = [1e-3, 1e-3, 1e-3] # self.rgt_outer.lnks[4]['rgba'] = [0.792156862745098, 0.819607843137255, 0.933333333333333, 1] # - rgt_inner self.rgt_inner.lnks[1]['name'] = "left_inner_knuckle" self.rgt_inner.lnks[1]['loc_pos'] = np.zeros(3) self.rgt_inner.lnks[1]['com'] = np.array( [0.000123011831763771, 0.0507850843201817, 0.00103968640075166]) self.rgt_inner.lnks[1]['mass'] = 0.0271177346495152 self.rgt_inner.lnks[1]['meshfile'] = os.path.join( this_dir, "meshes", "robotiq_arg2f_85_inner_knuckle_cvt.stl") self.rgt_inner.lnks[1]['rgba'] = [.2, .2, .2, 1] # reinitialize self.lft_outer.reinitialize() self.lft_inner.reinitialize() self.rgt_outer.reinitialize() self.rgt_inner.reinitialize() # jaw width self.jawwidth_rng = [0.0, .085] # jaw center self.jaw_center_pos = np.array([0, 0, .180]) # collision detection self.all_cdelements = [] self.enable_cc(toggle_cdprimit=enable_cc)
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), cdmesh_type='box', name='robotiq140', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, cdmesh_type=cdmesh_type, name=name) this_dir, this_filename = os.path.split(__file__) cpl_end_pos = self.coupling.jnts[-1]['gl_posq'] cpl_end_rotmat = self.coupling.jnts[-1]['gl_rotmatq'] # - lft_outer self.lft_outer = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(4), name='lft_outer') self.lft_outer.jnts[1]['loc_pos'] = np.array([0, -.0306011, .054904]) self.lft_outer.jnts[1]['motion_rng'] = [.0, .7] self.lft_outer.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler((math.pi / 2.0 + .725), 0, 0) self.lft_outer.jnts[1]['loc_motionax'] = np.array([-1, 0, 0]) self.lft_outer.jnts[2]['loc_pos'] = np.array([0, 0.01821998610742, 0.0260018192872234]) # passive self.lft_outer.jnts[2]['loc_motionax'] = np.array([1, 0, 0]) self.lft_outer.jnts[3]['loc_pos'] = np.array([0, 0.0817554015893473, -0.0282203446692936]) self.lft_outer.jnts[3]['loc_rotmat'] = rm.rotmat_from_euler(-0.725, 0, 0) self.lft_outer.jnts[3]['loc_motionax'] = np.array([1, 0, 0]) self.lft_outer.jnts[4]['loc_pos'] = np.array([0, 0.0420203446692936, -.03242]) # - lft_inner self.lft_inner = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(1), name='lft_inner') self.lft_inner.jnts[1]['loc_pos'] = np.array([0, -.0127, .06142]) self.lft_inner.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler((math.pi / 2.0 + .725), 0, 0) self.lft_inner.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) # - rgt_outer self.rgt_outer = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(4), name='rgt_outer') self.rgt_outer.jnts[1]['loc_pos'] = np.array([0, .0306011, .054904]) self.rgt_outer.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler((math.pi / 2.0 + .725), 0, math.pi) self.rgt_outer.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) self.rgt_outer.jnts[2]['loc_pos'] = np.array([0, 0.01821998610742, 0.0260018192872234]) # passive self.rgt_outer.jnts[2]['loc_motionax'] = np.array([1, 0, 0]) self.rgt_outer.jnts[3]['loc_pos'] = np.array([0, 0.0817554015893473, -0.0282203446692936]) self.rgt_outer.jnts[3]['loc_rotmat'] = rm.rotmat_from_euler(-0.725, 0, 0) self.rgt_outer.jnts[3]['loc_motionax'] = np.array([1, 0, 0]) self.rgt_outer.jnts[4]['loc_pos'] = np.array([0, 0.0420203446692936, -.03242]) # - rgt_inner self.rgt_inner = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(1), name='rgt_inner') self.rgt_inner.jnts[1]['loc_pos'] = np.array([0, 0.0127, 0.06142]) self.rgt_inner.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler((math.pi / 2.0 + .725), 0, math.pi) self.rgt_inner.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) # links # - lft_outer self.lft_outer.lnks[0]['name'] = "robotiq140_gripper_base" self.lft_outer.lnks[0]['loc_pos'] = np.zeros(3) self.lft_outer.lnks[0]['com'] = np.array([8.625e-08, -4.6583e-06, 0.03145]) self.lft_outer.lnks[0]['mass'] = 0.22652 self.lft_outer.lnks[0]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_base_link.stl") self.lft_outer.lnks[0]['rgba'] = [.2, .2, .2, 1] self.lft_outer.lnks[1]['name'] = "left_outer_knuckle" self.lft_outer.lnks[1]['loc_pos'] = np.zeros(3) self.lft_outer.lnks[1]['com'] = np.array([-0.000200000000003065, 0.0199435877845359, 0.0292245259211331]) self.lft_outer.lnks[1]['mass'] = 0.00853198276973456 self.lft_outer.lnks[1]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_outer_knuckle.stl") self.lft_outer.lnks[1]['rgba'] = [0.792156862745098, 0.819607843137255, 0.933333333333333, 1] self.lft_outer.lnks[2]['name'] = "left_outer_finger" self.lft_outer.lnks[2]['loc_pos'] = np.zeros(3) self.lft_outer.lnks[2]['com'] = np.array([0.00030115855001899, 0.0373907951953854, -0.0208027427000385]) self.lft_outer.lnks[2]['mass'] = 0.022614240507152 self.lft_outer.lnks[2]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_outer_finger.stl") self.lft_outer.lnks[2]['rgba'] = [.2, .2, .2, 1] self.lft_outer.lnks[3]['name'] = "left_inner_finger" self.lft_outer.lnks[3]['loc_pos'] = np.zeros(3) self.lft_outer.lnks[3]['com'] = np.array([0.000299999999999317, 0.0160078233491243, -0.0136945669206257]) self.lft_outer.lnks[3]['mass'] = 0.0104003125914103 self.lft_outer.lnks[3]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_inner_finger.stl") self.lft_outer.lnks[3]['rgba'] = [.2, .2, .2, 1] self.lft_outer.lnks[4]['name'] = "left_inner_finger_pad" self.lft_outer.lnks[4]['loc_pos'] = np.zeros(3) self.lft_outer.lnks[4]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_pad.stl") self.lft_outer.lnks[4]['scale'] = [1e-3, 1e-3, 1e-3] self.lft_outer.lnks[4]['rgba'] = [0.792156862745098, 0.819607843137255, 0.933333333333333, 1] # - lft_inner self.lft_inner.lnks[1]['name'] = "left_inner_knuckle" self.lft_inner.lnks[1]['loc_pos'] = np.zeros(3) self.lft_inner.lnks[1]['com'] = np.array([0.000123011831763771, 0.0507850843201817, 0.00103968640075166]) self.lft_inner.lnks[1]['mass'] = 0.0271177346495152 self.lft_inner.lnks[1]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_inner_knuckle.stl") self.lft_inner.lnks[1]['rgba'] = [.2, .2, .2, 1] # - rgt_outer self.rgt_outer.lnks[1]['name'] = "right_outer_knuckle" self.rgt_outer.lnks[1]['loc_pos'] = np.zeros(3) self.rgt_outer.lnks[1]['com'] = np.array([-0.000200000000003065, 0.0199435877845359, 0.0292245259211331]) self.rgt_outer.lnks[1]['mass'] = 0.00853198276973456 self.rgt_outer.lnks[1]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_outer_knuckle.stl") self.rgt_outer.lnks[1]['rgba'] = [0.792156862745098, 0.819607843137255, 0.933333333333333, 1] self.rgt_outer.lnks[2]['name'] = "right_outer_finger" self.rgt_outer.lnks[2]['loc_pos'] = np.zeros(3) self.rgt_outer.lnks[2]['com'] = np.array([0.00030115855001899, 0.0373907951953854, -0.0208027427000385]) self.rgt_outer.lnks[2]['mass'] = 0.022614240507152 self.rgt_outer.lnks[2]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_outer_finger.stl") self.rgt_outer.lnks[2]['rgba'] = [.2, .2, .2, 1] self.rgt_outer.lnks[3]['name'] = "right_inner_finger" self.rgt_outer.lnks[3]['loc_pos'] = np.zeros(3) self.rgt_outer.lnks[3]['com'] = np.array([0.000299999999999317, 0.0160078233491243, -0.0136945669206257]) self.rgt_outer.lnks[3]['mass'] = 0.0104003125914103 self.rgt_outer.lnks[3]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_inner_finger.stl") self.rgt_outer.lnks[3]['rgba'] = [.2, .2, .2, 1] self.rgt_outer.lnks[4]['name'] = "right_inner_finger_pad" self.rgt_outer.lnks[4]['loc_pos'] = np.zeros(3) self.rgt_outer.lnks[4]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_pad.stl") self.rgt_outer.lnks[4]['scale'] = [1e-3, 1e-3, 1e-3] self.rgt_outer.lnks[4]['rgba'] = [0.792156862745098, 0.819607843137255, 0.933333333333333, 1] # - rgt_inner self.rgt_inner.lnks[1]['name'] = "right_inner_knuckle" self.rgt_inner.lnks[1]['loc_pos'] = np.zeros(3) self.rgt_inner.lnks[1]['com'] = np.array([0.000123011831763771, 0.0507850843201817, 0.00103968640075166]) self.rgt_inner.lnks[1]['mass'] = 0.0271177346495152 self.rgt_inner.lnks[1]['mesh_file'] = os.path.join(this_dir, "meshes", "robotiq_arg2f_140_inner_knuckle.stl") self.rgt_inner.lnks[1]['rgba'] = [.2, .2, .2, 1] # reinitialize self.lft_outer.reinitialize() self.lft_inner.reinitialize() self.rgt_outer.reinitialize() self.rgt_inner.reinitialize() # jaw width self.jawwidth_rng = [0.0, .140] # jaw center self.jaw_center_pos = np.array([0, 0, .19]) # position for initial state (fully open) # relative jaw center pos self.jaw_center_pos_rel = self.jaw_center_pos - self.lft_outer.jnts[4]['gl_posq'] # collision detection self.all_cdelements = [] self.enable_cc(toggle_cdprimit=enable_cc)
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), name='ur3dual', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) # left side self.lft_body = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(12), name='lft_body_jl') self.lft_body.jnts[0]['loc_pos'] = np.array([0.0, 0.0, 0.0]) self.lft_body.jnts[1]['loc_pos'] = np.array([-0.0, 0.0, 0.0]) self.lft_body.jnts[2]['loc_pos'] = np.array([0.0, 0.0, 0.0]) self.lft_body.jnts[3]['loc_pos'] = np.array([0.0, 0.0, 0.0]) self.lft_body.jnts[4]['loc_pos'] = np.array([-0.0, 0.0, 0.0]) self.lft_body.jnts[5]['loc_pos'] = np.array([0.0, 0.0, 0.0]) self.lft_body.jnts[6]['loc_pos'] = np.array([0.0, 0.0, 0.0]) self.lft_body.jnts[7]['loc_pos'] = np.array([-0.0, 0.0, 0.0]) self.lft_body.jnts[8]['loc_pos'] = np.array([0.0, 0.0, 0.0]) self.lft_body.jnts[9]['loc_pos'] = np.array([0.0, 0.0, 0.0]) self.lft_body.jnts[10]['loc_pos'] = np.array([-0.0, 0.0, 0.0]) self.lft_body.jnts[11]['loc_pos'] = np.array([0.0, 0.0, 0.0]) self.lft_body.jnts[12]['loc_pos'] = np.array([0.0, 0.0, 0.0]) self.lft_body.jnts[13]['loc_pos'] = np.array( [.0, .258485281374, 1.61051471863]) self.lft_body.jnts[13]['loc_rotmat'] = rm.rotmat_from_euler( -3.0 * math.pi / 4.0, 0, math.pi, 'rxyz') # body self.lft_body.lnks[0]['name'] = "ur3_dual_lft_body" self.lft_body.lnks[0]['loc_pos'] = np.array([0, 0, 0]) self.lft_body.lnks[0]['collisionmodel'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "ur3_dual_base.stl"), cdprimit_type="user_defined", expand_radius=.005, userdefined_cdprimitive_fn=self._base_combined_cdnp) self.lft_body.lnks[0]['rgba'] = [.3, .3, .3, 1.0] # columns self.lft_body.lnks[1]['name'] = "ur3_dual_back_rgt_column" self.lft_body.lnks[1]['loc_pos'] = np.array([-0.41, -0.945, 0]) self.lft_body.lnks[1]['collisionmodel'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "ur3_dual_column2400x60x60.stl")) self.lft_body.lnks[2]['name'] = "ur3_dual_back_lft_column" self.lft_body.lnks[2]['loc_pos'] = np.array([-0.41, 0.945, 0]) self.lft_body.lnks[2]['collisionmodel'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "ur3_dual_column2400x60x60.stl")) self.lft_body.lnks[3]['name'] = "ur3_dual_front_rgt_column" self.lft_body.lnks[3]['loc_pos'] = np.array([0.73, -0.945, 0]) self.lft_body.lnks[3]['collisionmodel'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "ur3_dual_column2400x60x60.stl")) self.lft_body.lnks[4]['name'] = "ur3_dual_front_lft_column" self.lft_body.lnks[4]['loc_pos'] = np.array([0.73, 0.945, 0]) self.lft_body.lnks[4]['collisionmodel'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "ur3_dual_column2400x60x60.stl")) # x_rows self.lft_body.lnks[5]['name'] = "ur3_dual_up_rgt_xrow" self.lft_body.lnks[5]['loc_pos'] = np.array([-0.38, -0.945, 2.37]) self.lft_body.lnks[5]['loc_rotmat'] = rm.rotmat_from_euler( 0, math.pi / 2, 0) self.lft_body.lnks[5]['collisionmodel'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "ur3_dual_column1080x60x60.stl")) self.lft_body.lnks[6]['name'] = "ur3_dual_bottom_rgt_xrow" self.lft_body.lnks[6]['loc_pos'] = np.array([-0.38, -0.945, 1.07]) self.lft_body.lnks[6]['loc_rotmat'] = rm.rotmat_from_euler( 0, math.pi / 2, 0) self.lft_body.lnks[6]['collisionmodel'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "ur3_dual_column1080x60x60.stl")) self.lft_body.lnks[7]['name'] = "ur3_dual_up_lft_xrow" self.lft_body.lnks[7]['loc_pos'] = np.array([-0.38, 0.945, 2.37]) self.lft_body.lnks[7]['loc_rotmat'] = rm.rotmat_from_euler( 0, math.pi / 2, 0) self.lft_body.lnks[7]['collisionmodel'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "ur3_dual_column1080x60x60.stl")) self.lft_body.lnks[8]['name'] = "ur3_dual_bottom_lft_xrow" self.lft_body.lnks[8]['loc_pos'] = np.array([-0.38, 0.945, 1.07]) self.lft_body.lnks[8]['loc_rotmat'] = rm.rotmat_from_euler( 0, math.pi / 2, 0) self.lft_body.lnks[8]['collisionmodel'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "ur3_dual_column1080x60x60.stl")) # y_rows self.lft_body.lnks[9]['name'] = "ur3_dual_back_up_yrow" self.lft_body.lnks[9]['loc_pos'] = np.array([-0.41, -0.915, 2.37]) self.lft_body.lnks[9]['loc_rotmat'] = rm.rotmat_from_euler( -math.pi / 2, 0, 0) self.lft_body.lnks[9]['collisionmodel'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "ur3_dual_column1830x60x60.stl")) self.lft_body.lnks[10]['name'] = "ur3_dual_back_bottom_yrow" self.lft_body.lnks[10]['loc_pos'] = np.array([-0.41, -0.915, 0.35]) self.lft_body.lnks[10]['loc_rotmat'] = rm.rotmat_from_euler( -math.pi / 2, 0, 0) self.lft_body.lnks[10]['collisionmodel'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "ur3_dual_column1830x60x60.stl")) self.lft_body.lnks[11]['name'] = "ur3_dual_front_up_yrow" self.lft_body.lnks[11]['loc_pos'] = np.array([0.73, -0.915, 2.37]) self.lft_body.lnks[11]['loc_rotmat'] = rm.rotmat_from_euler( -math.pi / 2, 0, 0) self.lft_body.lnks[11]['collisionmodel'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "ur3_dual_column1830x60x60.stl")) # table TODO update using vision sensors self.lft_body.lnks[12]['name'] = "ur3_dual_table" self.lft_body.lnks[12]['loc_pos'] = np.array([0.36, 0.0, 1.046]) self.lft_body.lnks[12]['loc_rotmat'] = rm.rotmat_from_euler( 0, 0, math.pi / 2) self.lft_body.lnks[12]['collisionmodel'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "ur3_dual_table1820x54x800.stl")) self.lft_body.lnks[12]['rgba'] = [.9, .77, .52, 1.0] self.lft_body.reinitialize() lft_arm_homeconf = np.zeros(6) lft_arm_homeconf[0] = math.pi / 3.0 lft_arm_homeconf[1] = -math.pi * 1.0 / 3.0 lft_arm_homeconf[2] = -math.pi * 2.0 / 3.0 lft_arm_homeconf[3] = math.pi lft_arm_homeconf[4] = -math.pi / 2.0 self.lft_arm = ur.UR3(pos=self.lft_body.jnts[-1]['gl_posq'], rotmat=self.lft_body.jnts[-1]['gl_rotmatq'], homeconf=lft_arm_homeconf, enable_cc=False) # lft hand ftsensor self.lft_ft_sensor = jl.JLChain( pos=self.lft_arm.jnts[-1]['gl_posq'], rotmat=self.lft_arm.jnts[-1]['gl_rotmatq'], homeconf=np.zeros(0), name='lft_ft_sensor_jl') self.lft_ft_sensor.jnts[1]['loc_pos'] = np.array([.0, .0, .0484]) self.lft_ft_sensor.lnks[0]['name'] = "ur3_dual_lft_ft_sensor" self.lft_ft_sensor.lnks[0]['loc_pos'] = np.array([0, 0, 0]) self.lft_ft_sensor.lnks[0]['collisionmodel'] = cm.gen_stick( spos=self.lft_ft_sensor.jnts[0]['loc_pos'], epos=self.lft_ft_sensor.jnts[1]['loc_pos'], thickness=.067, rgba=[.2, .3, .3, 1], sections=24) self.lft_ft_sensor.reinitialize() # lft hand self.lft_hnd = rtq.Robotiq85( pos=self.lft_ft_sensor.jnts[-1]['gl_posq'], rotmat=self.lft_ft_sensor.jnts[-1]['gl_rotmatq'], enable_cc=False) # rigth side self.rgt_body = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(0), name='rgt_body_jl') self.rgt_body.jnts[1]['loc_pos'] = np.array( [.0, -.258485281374, 1.61051471863]) # right from robot_s view self.rgt_body.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler( 3.0 * math.pi / 4.0, .0, .0) # left from robot_s view self.rgt_body.lnks[0]['name'] = "ur3_dual_rgt_body" self.rgt_body.lnks[0]['loc_pos'] = np.array([0, 0, 0]) self.rgt_body.lnks[0]['meshfile'] = None self.rgt_body.lnks[0]['rgba'] = [.3, .3, .3, 1.0] self.rgt_body.reinitialize() rgt_arm_homeconf = np.zeros(6) rgt_arm_homeconf[0] = -math.pi * 1.0 / 3.0 rgt_arm_homeconf[1] = -math.pi * 2.0 / 3.0 rgt_arm_homeconf[2] = math.pi * 2.0 / 3.0 rgt_arm_homeconf[4] = math.pi / 2.0 self.rgt_arm = ur.UR3(pos=self.rgt_body.jnts[-1]['gl_posq'], rotmat=self.rgt_body.jnts[-1]['gl_rotmatq'], homeconf=rgt_arm_homeconf, enable_cc=False) # rgt hand ft sensor self.rgt_ft_sensor = jl.JLChain( pos=self.rgt_arm.jnts[-1]['gl_posq'], rotmat=self.rgt_arm.jnts[-1]['gl_rotmatq'], homeconf=np.zeros(0), name='rgt_ft_sensor_jl') self.rgt_ft_sensor.jnts[1]['loc_pos'] = np.array([.0, .0, .0484]) self.rgt_ft_sensor.lnks[0]['name'] = "ur3_dual_rgt_ft_sensor" self.rgt_ft_sensor.lnks[0]['loc_pos'] = np.array([0, 0, 0]) self.rgt_ft_sensor.lnks[0]['collisionmodel'] = cm.gen_stick( spos=self.rgt_ft_sensor.jnts[0]['loc_pos'], epos=self.rgt_ft_sensor.jnts[1]['loc_pos'], thickness=.067, rgba=[.2, .3, .3, 1], sections=24) self.rgt_ft_sensor.reinitialize() # TODO replace using copy self.rgt_hnd = rtq.Robotiq85( pos=self.rgt_ft_sensor.jnts[-1]['gl_posq'], rotmat=self.rgt_ft_sensor.jnts[-1]['gl_rotmatq'], enable_cc=False) # tool center point # lft self.lft_arm.tcp_jntid = -1 self.lft_arm.tcp_loc_pos = self.lft_ft_sensor.jnts[-1][ 'loc_pos'] + self.lft_hnd.jaw_center_pos self.lft_arm.tcp_loc_rotmat = self.lft_ft_sensor.jnts[-1][ 'loc_rotmat'].dot(self.lft_hnd.jaw_center_rotmat) # rgt self.rgt_arm.tcp_jntid = -1 self.rgt_arm.tcp_loc_pos = self.lft_ft_sensor.jnts[-1][ 'loc_pos'] + self.lft_hnd.jaw_center_pos self.rgt_arm.tcp_loc_rotmat = self.lft_ft_sensor.jnts[-1][ 'loc_rotmat'].dot(self.lft_hnd.jaw_center_rotmat) # a list of detailed information about objects in hand, see CollisionChecker.add_objinhnd self.lft_oih_infos = [] self.rgt_oih_infos = [] # collision detection if enable_cc: self.enable_cc() # component map self.manipulator_dict['rgt_arm'] = self.rgt_arm self.manipulator_dict['lft_arm'] = self.lft_arm self.manipulator_dict[ 'rgt_hnd'] = self.rgt_arm # specify which hand is a gripper installed to self.manipulator_dict[ 'lft_hnd'] = self.lft_arm # specify which hand is a gripper installed to self.manipulator_dict[ 'rgt_ftsensor'] = self.rgt_arm # specify which hand is a gripper installed to self.manipulator_dict[ 'lft_ftsensor'] = self.lft_arm # specify which hand is a gripper installed to self.hnd_dict['rgt_hnd'] = self.rgt_hnd self.hnd_dict['lft_hnd'] = self.lft_hnd self.hnd_dict['rgt_arm'] = self.rgt_hnd self.hnd_dict['lft_arm'] = self.lft_hnd self.hnd_dict['rgt_ftsensor'] = self.rgt_hnd self.hnd_dict['lft_ftsensor'] = self.lft_hnd self.ft_sensor_dict['rgt_ftsensor'] = self.rgt_ft_sensor self.ft_sensor_dict['lft_ftsensor'] = self.lft_ft_sensor self.ft_sensor_dict['rgt_arm'] = self.rgt_ft_sensor self.ft_sensor_dict['lft_arm'] = self.lft_ft_sensor self.ft_sensor_dict['rgt_hnd'] = self.rgt_ft_sensor self.ft_sensor_dict['lft_hnd'] = self.lft_ft_sensor
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), homeconf=np.zeros(7), name='tbm_arm', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) self.jlc = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=homeconf, name=name) self.jlc.jnts[1]['loc_pos'] = np.array([0.0, 0.0, 0.0]) self.jlc.jnts[1]['type'] = 'prismatic' self.jlc.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) self.jlc.jnts[1]['motion_rng'] = [-.5, .0] self.jlc.jnts[2]['loc_pos'] = np.array([0, 0, 0.396]) self.jlc.jnts[2]['loc_motionax'] = np.array([0, 0, 1]) self.jlc.jnts[2]['motion_rng'] = [-math.radians(20), math.radians(20)] self.jlc.jnts[3]['loc_pos'] = np.array([0.654, .0, .0]) self.jlc.jnts[3]['loc_motionax'] = np.array([1, 0, 0]) self.jlc.jnts[4]['loc_pos'] = np.array([.625, .0, .0]) self.jlc.jnts[4]['loc_motionax'] = np.array([0, 0, 1]) self.jlc.jnts[4]['motion_rng'] = [-math.radians(90), math.radians(90)] self.jlc.jnts[5]['loc_pos'] = np.array([0.687, .0, .0]) self.jlc.jnts[5]['loc_motionax'] = np.array([1, 0, 0]) self.jlc.jnts[6]['loc_pos'] = np.array([.83, .0, .0]) self.jlc.jnts[6]['loc_motionax'] = np.array([0, 1, 0]) self.jlc.jnts[6]['motion_rng'] = [-math.radians(115), math.radians(115)] self.jlc.jnts[7]['loc_pos'] = np.array([.223, .0, .0]) self.jlc.jnts[7]['loc_motionax'] = np.array([1, 0, 0]) # links self.jlc.lnks[1]['name'] = "base" self.jlc.lnks[1]['loc_pos'] = np.zeros(3) self.jlc.lnks[1]['mesh_file'] = os.path.join(this_dir, "meshes", "base_r.stl") self.jlc.lnks[1]['rgba'] = [.5, .5, .5, 1.0] self.jlc.lnks[2]['name'] = "j1" self.jlc.lnks[2]['loc_pos'] = np.zeros(3) self.jlc.lnks[2]['com'] = np.array([.0, .0, .15]) self.jlc.lnks[2]['mass'] = 1.29 self.jlc.lnks[2]['mesh_file'] = os.path.join(this_dir, "meshes", "joint1_r.stl") self.jlc.lnks[2]['rgba'] = [.7, .7, .7, 1.0] self.jlc.lnks[3]['name'] = "j2" self.jlc.lnks[3]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[3]['com'] = np.array([-.02, .1, .07]) self.jlc.lnks[3]['mass'] = 0.39 self.jlc.lnks[3]['mesh_file'] = os.path.join(this_dir, "meshes", "joint2_r.stl") self.jlc.lnks[3]['rgba'] = [.77, .77, .60, 1] self.jlc.lnks[4]['name'] = "j3" self.jlc.lnks[4]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[4]['com'] = np.array([-.01, .02, .03]) self.jlc.lnks[4]['mass'] = .35 self.jlc.lnks[4]['mesh_file'] = os.path.join(this_dir, "meshes", "joint3_r.stl") self.jlc.lnks[4]['rgba'] = [.35, .35, .35, 1.0] self.jlc.lnks[5]['name'] = "j4" self.jlc.lnks[5]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[5]['com'] = np.array([.0, .0, 0.055]) self.jlc.lnks[5]['mass'] = 0.35 self.jlc.lnks[5]['mesh_file'] = os.path.join(this_dir, "meshes", "joint4_r.stl") self.jlc.lnks[5]['rgba'] = [.7, .7, .7, 1.0] self.jlc.lnks[6]['name'] = "j5" self.jlc.lnks[6]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[6]['com'] = np.array([.0, -.04, .015]) self.jlc.lnks[6]['mass'] = 0.19 self.jlc.lnks[6]['mesh_file'] = os.path.join(this_dir, "meshes", "joint5_r.stl") self.jlc.lnks[6]['rgba'] = [.77, .77, .60, 1] self.jlc.lnks[7]['name'] = "j6" self.jlc.lnks[7]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[7]['com'] = np.array([.0, .0, 0]) self.jlc.lnks[7]['mass'] = 0.03 self.jlc.lnks[7]['mesh_file'] = None self.jlc.lnks[7]['rgba'] = [.5, .5, .5, 1.0] self.jlc.reinitialize() # collision detection if enable_cc: self.enable_cc()
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), homeconf=np.zeros(6), name='cobotta', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) self.jlc = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=homeconf, name=name) # six joints, njnts = 6+2 (tgt ranges from 1-6), nlinks = 6+1 self.jlc.jnts[1]['loc_pos'] = np.array([0, 0, 0]) self.jlc.jnts[1]['motion_rng'] = [-2.617994, 2.617994] self.jlc.jnts[2]['loc_pos'] = np.array([0, 0, 0.18]) self.jlc.jnts[2]['loc_motionax'] = np.array([0, 1, 0]) self.jlc.jnts[2]['motion_rng'] = [-1.047198, 1.745329] self.jlc.jnts[3]['loc_pos'] = np.array([0, 0, 0.165]) self.jlc.jnts[3]['loc_motionax'] = np.array([0, 1, 0]) self.jlc.jnts[3]['motion_rng'] = [0.3141593, 2.443461] self.jlc.jnts[4]['loc_pos'] = np.array([-0.012, 0.02, 0.088]) self.jlc.jnts[4]['loc_motionax'] = np.array([0, 0, 1]) self.jlc.jnts[4]['motion_rng'] = [-2.96706, 2.96706] self.jlc.jnts[5]['loc_pos'] = np.array([0, -.02, .0895]) self.jlc.jnts[5]['loc_motionax'] = np.array([0, 1, 0]) self.jlc.jnts[5]['motion_rng'] = [-1.658063, 2.356194] self.jlc.jnts[6]['loc_pos'] = np.array([0, -.0445, 0.042]) self.jlc.jnts[6]['loc_motionax'] = np.array([0, 0, 1]) self.jlc.jnts[6]['motion_rng'] = [-2.96706, 2.96706] # links self.jlc.lnks[0]['name'] = "base" self.jlc.lnks[0]['loc_pos'] = np.zeros(3) self.jlc.lnks[0]['mass'] = 1.4 self.jlc.lnks[0]['com'] = np.array([-.02131, .000002, .044011]) self.jlc.lnks[0]['meshfile'] = os.path.join(this_dir, "meshes", "base_link.dae") self.jlc.lnks[0]['rgba'] = [.5, .5, .5, 1.0] self.jlc.lnks[1]['name'] = "j1" self.jlc.lnks[1]['loc_pos'] = np.zeros(3) self.jlc.lnks[1]['com'] = np.array([.0, .0, .15]) self.jlc.lnks[1]['mass'] = 1.29 self.jlc.lnks[1]['meshfile'] = os.path.join(this_dir, "meshes", "j1.dae") self.jlc.lnks[1]['rgba'] = [.7, .7, .7, 1.0] self.jlc.lnks[2]['name'] = "j2" self.jlc.lnks[2]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[2]['com'] = np.array([-.02, .1, .07]) self.jlc.lnks[2]['mass'] = 0.39 self.jlc.lnks[2]['meshfile'] = os.path.join(this_dir, "meshes", "j2.dae") self.jlc.lnks[2]['rgba'] = [0, .55, .60, 1] self.jlc.lnks[3]['name'] = "j3" self.jlc.lnks[3]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[3]['com'] = np.array([-.01, .02, .03]) self.jlc.lnks[3]['mass'] = .35 self.jlc.lnks[3]['meshfile'] = os.path.join(this_dir, "meshes", "j3.dae") self.jlc.lnks[3]['rgba'] = [.35, .35, .35, 1.0] self.jlc.lnks[4]['name'] = "j4" self.jlc.lnks[4]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[4]['com'] = np.array([.0, .0, 0.055]) self.jlc.lnks[4]['mass'] = 0.35 self.jlc.lnks[4]['meshfile'] = os.path.join(this_dir, "meshes", "j4.dae") self.jlc.lnks[4]['rgba'] = [.7, .7, .7, 1.0] self.jlc.lnks[5]['name'] = "j5" self.jlc.lnks[5]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[5]['com'] = np.array([.0, -.04, .015]) self.jlc.lnks[5]['mass'] = 0.19 self.jlc.lnks[5]['meshfile'] = os.path.join(this_dir, "meshes", "j5.dae") self.jlc.lnks[5]['rgba'] = [0, .55, .60, 1] self.jlc.lnks[6]['name'] = "j6" self.jlc.lnks[6]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[6]['com'] = np.array([.0, .0, 0]) self.jlc.lnks[6]['mass'] = 0.03 self.jlc.lnks[6]['meshfile'] = os.path.join(this_dir, "meshes", "j6.dae") self.jlc.lnks[6]['rgba'] = [.5, .5, .5, 1.0] self.jlc.reinitialize() # collision detection if enable_cc: self.enable_cc()
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), name="xarm7_shuidi_mobile", enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) # agv self.agv = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(5), name='agv') # 1-3 x,y,theta; 4-5 dummy self.agv.jnts[1]['loc_pos'] = np.zeros(3) self.agv.jnts[1]['type'] = 'prismatic' self.agv.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) self.agv.jnts[1]['motion_rng'] = [0.0, 5.0] self.agv.jnts[2]['loc_pos'] = np.zeros(3) self.agv.jnts[2]['type'] = 'prismatic' self.agv.jnts[2]['loc_motionax'] = np.array([0, 1, 0]) self.agv.jnts[2]['motion_rng'] = [-3.0, 3.0] self.agv.jnts[3]['loc_pos'] = np.zeros(3) self.agv.jnts[3]['loc_motionax'] = np.array([0, 0, 1]) self.agv.jnts[3]['motion_rng'] = [-math.pi, math.pi] self.agv.jnts[4]['loc_pos'] = np.array([0, .0, .277]) # dummy self.agv.jnts[5]['loc_pos'] = np.zeros(3) # dummy self.agv.jnts[6]['loc_pos'] = np.array([0, .0, .168862]) self.agv.lnks[3]['name'] = 'agv' self.agv.lnks[3]['loc_pos'] = np.array([0, 0, 0]) self.agv.lnks[3]['mesh_file'] = os.path.join(this_dir, 'meshes', 'shuidi_agv.stl') self.agv.lnks[3]['rgba'] = [.7, .7, .7, 1.0] self.agv.lnks[4]['mesh_file'] = os.path.join(this_dir, 'meshes', 'battery.stl') self.agv.lnks[4]['rgba'] = [.35, .35, .35, 1.0] self.agv.lnks[5]['mesh_file'] = os.path.join(this_dir, 'meshes', 'battery_fixture.stl') self.agv.lnks[5]['rgba'] = [.55, .55, .55, 1.0] self.agv.tgtjnts = [1, 2, 3] self.agv.reinitialize() # arm arm_homeconf = np.zeros(7) arm_homeconf[1] = -math.pi / 3 arm_homeconf[3] = math.pi / 12 arm_homeconf[5] = -math.pi / 12 self.arm = xa.XArm7(pos=self.agv.jnts[-1]['gl_posq'], rotmat=self.agv.jnts[-1]['gl_rotmatq'], homeconf=arm_homeconf, name='arm', enable_cc=False) # ft sensor self.ft_sensor = jl.JLChain(pos=self.arm.jnts[-1]['gl_posq'], rotmat=self.arm.jnts[-1]['gl_rotmatq'], homeconf=np.zeros(0), name='ft_sensor_jl') self.ft_sensor.jnts[1]['loc_pos'] = np.array([.0, .0, .065]) self.ft_sensor.lnks[0]['name'] = "xs_ftsensor" self.ft_sensor.lnks[0]['loc_pos'] = np.array([0, 0, 0]) self.ft_sensor.lnks[0]['collision_model'] = cm.gen_stick( spos=self.ft_sensor.jnts[0]['loc_pos'], epos=self.ft_sensor.jnts[1]['loc_pos'], thickness=.075, rgba=[.2, .3, .3, 1], sections=24) self.ft_sensor.reinitialize() # gripper self.hnd = xag.XArmGripper( pos=self.ft_sensor.jnts[-1]['gl_posq'], rotmat=self.ft_sensor.jnts[-1]['gl_rotmatq'], name='hnd_s', enable_cc=False) # tool center point self.arm.jlc.tcp_jnt_id = -1 self.arm.jlc.tcp_loc_rotmat = self.ft_sensor.jnts[-1][ 'loc_rotmat'].dot(self.hnd.jaw_center_rotmat) self.arm.jlc.tcp_loc_pos = self.ft_sensor.jnts[-1][ 'loc_pos'] + self.arm.jlc.tcp_loc_rotmat.dot( self.hnd.jaw_center_pos) # a list of detailed information about objects in hand, see CollisionChecker.add_objinhnd self.oih_infos = [] # collision detection if enable_cc: self.enable_cc() # component map self.manipulator_dict['arm'] = self.arm self.manipulator_dict['ftsensor'] = self.arm self.manipulator_dict[ 'hnd'] = self.arm # specify which hand is a gripper installed to self.hnd_dict['arm'] = self.hnd self.hnd_dict['ftsensor'] = self.hnd self.hnd_dict['hnd'] = self.hnd self.ft_sensor_dict['arm'] = self.ft_sensor self.ft_sensor_dict['ftsensor'] = self.ft_sensor self.ft_sensor_dict['hnd'] = self.ft_sensor
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), name="ur5e_conveyorbelt", enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) # base plate self.base_stand = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(3), name='base_stand') self.base_stand.jnts[1]['loc_pos'] = np.array([.9, -1.5, -0.06]) self.base_stand.jnts[2]['loc_pos'] = np.array([0, 1.23, 0]) self.base_stand.jnts[3]['loc_pos'] = np.array([0, 0, 0]) self.base_stand.jnts[4]['loc_pos'] = np.array([-.9, .27, 0.06]) self.base_stand.lnks[0]['collision_model'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "ur5e_base.stl"), cdprimit_type="user_defined", expand_radius=.005, userdefined_cdprimitive_fn=self._base_combined_cdnp) self.base_stand.lnks[0]['rgba'] = [.35, .35, .35, 1] self.base_stand.lnks[1]['mesh_file'] = os.path.join( this_dir, "meshes", "conveyor.stl") self.base_stand.lnks[1]['rgba'] = [.35, .45, .35, 1] self.base_stand.lnks[2]['mesh_file'] = os.path.join( this_dir, "meshes", "camera_stand.stl") self.base_stand.lnks[2]['rgba'] = [.55, .55, .55, 1] self.base_stand.lnks[3]['mesh_file'] = os.path.join( this_dir, "meshes", "cameras.stl") self.base_stand.lnks[3]['rgba'] = [.55, .55, .55, 1] self.base_stand.reinitialize() # arm arm_homeconf = np.zeros(6) # arm_homeconf[0] = math.pi / 2 arm_homeconf[1] = -math.pi * 2 / 3 arm_homeconf[2] = math.pi / 3 arm_homeconf[3] = -math.pi / 2 arm_homeconf[4] = -math.pi / 2 self.arm = rbt.UR5E(pos=self.base_stand.jnts[-1]['gl_posq'], rotmat=self.base_stand.jnts[-1]['gl_rotmatq'], homeconf=arm_homeconf, name='arm', enable_cc=False) # gripper self.hnd = hnd.Robotiq140(pos=self.arm.jnts[-1]['gl_posq'], rotmat=self.arm.jnts[-1]['gl_rotmatq'], name='hnd_s', enable_cc=False) # tool center point self.arm.jlc.tcp_jnt_id = -1 self.arm.jlc.tcp_loc_pos = self.hnd.jaw_center_pos self.arm.jlc.tcp_loc_rotmat = self.hnd.jaw_center_rotmat # a list of detailed information about objects in hand, see CollisionChecker.add_objinhnd self.oih_infos = [] # collision detection if enable_cc: self.enable_cc() # component map self.manipulator_dict['arm'] = self.arm self.manipulator_dict['hnd'] = self.arm self.hnd_dict['hnd'] = self.hnd self.hnd_dict['arm'] = self.hnd
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), name='ur3edual', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) # left side self.lft_body = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(0), name='lft_body_jl') self.lft_body.jnts[1]['loc_pos'] = np.array( [.365, .345, 1.33]) # left from robot_s view self.lft_body.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler( math.pi / 2.0, 0, math.pi / 2.0) # left from robot_s view self.lft_body.lnks[0]['name'] = "ur3e_dual_base" self.lft_body.lnks[0]['loc_pos'] = np.array([0, 0, 0]) self.lft_body.lnks[0]['collision_model'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "ur3e_dual_base.stl"), cdprimit_type="user_defined", expand_radius=.005, userdefined_cdprimitive_fn=self._base_combined_cdnp) self.lft_body.lnks[0]['rgba'] = [.55, .55, .55, 1.0] self.lft_body.reinitialize() lft_arm_homeconf = np.zeros(6) lft_arm_homeconf[0] = -math.pi * 2.0 / 3.0 lft_arm_homeconf[1] = -math.pi * 2.0 / 3.0 lft_arm_homeconf[2] = math.pi / 2.0 lft_arm_homeconf[3] = math.pi lft_arm_homeconf[4] = -math.pi / 2.0 self.lft_arm = ur.UR3E(pos=self.lft_body.jnts[-1]['gl_posq'], rotmat=self.lft_body.jnts[-1]['gl_rotmatq'], homeconf=lft_arm_homeconf, enable_cc=False) self.lft_hnd = rtq.RobotiqHE( pos=self.lft_arm.jnts[-1]['gl_posq'], rotmat=self.lft_arm.jnts[-1]['gl_rotmatq'], coupling_offset_pos=np.array([0.0, 0.0, 0.0331]), enable_cc=False) # rigth side self.rgt_body = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(0), name='rgt_body_jl') self.rgt_body.jnts[1]['loc_pos'] = np.array( [.365, -.345, 1.33]) # right from robot_s view self.rgt_body.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler( math.pi / 2.0, 0, math.pi / 2.0) # left from robot_s view self.rgt_body.lnks[0]['name'] = "ur3e_dual_base" self.rgt_body.lnks[0]['loc_pos'] = np.array([0, 0, 0]) self.rgt_body.lnks[0]['mesh_file'] = None self.rgt_body.lnks[0]['rgba'] = [.3, .3, .3, 1.0] self.rgt_body.reinitialize() rgt_arm_homeconf = np.zeros(6) rgt_arm_homeconf[0] = math.pi * 2.0 / 3.0 rgt_arm_homeconf[1] = -math.pi / 3.0 rgt_arm_homeconf[2] = -math.pi / 2.0 rgt_arm_homeconf[4] = math.pi / 2.0 self.rgt_arm = ur.UR3E(pos=self.rgt_body.jnts[-1]['gl_posq'], rotmat=self.rgt_body.jnts[-1]['gl_rotmatq'], homeconf=rgt_arm_homeconf, enable_cc=False) self.rgt_hnd = rtq.RobotiqHE( pos=self.rgt_arm.jnts[-1]['gl_posq'], rotmat=self.rgt_arm.jnts[-1]['gl_rotmatq'], coupling_offset_pos=np.array([0.0, 0.0, 0.0331]), enable_cc=False) # tool center point # lft self.lft_arm.tcp_jnt_id = -1 self.lft_arm.tcp_loc_pos = self.lft_hnd.jaw_center_pos self.lft_arm.tcp_loc_rotmat = self.lft_hnd.jaw_center_rotmat # rgt self.rgt_arm.tcp_jnt_id = -1 self.rgt_arm.tcp_loc_pos = self.lft_hnd.jaw_center_pos self.rgt_arm.tcp_loc_rotmat = self.lft_hnd.jaw_center_rotmat # a list of detailed information about objects in hand, see CollisionChecker.add_objinhnd self.lft_oih_infos = [] self.rgt_oih_infos = [] # collision detection if enable_cc: self.enable_cc() # component map self.manipulator_dict['rgt_arm'] = self.rgt_arm self.manipulator_dict['lft_arm'] = self.lft_arm self.manipulator_dict[ 'rgt_hnd'] = self.rgt_arm # specify which hand is a gripper installed to self.manipulator_dict[ 'lft_hnd'] = self.lft_arm # specify which hand is a gripper installed to self.hnd_dict['rgt_hnd'] = self.rgt_hnd self.hnd_dict['lft_hnd'] = self.lft_hnd self.hnd_dict['rgt_arm'] = self.rgt_hnd self.hnd_dict['lft_arm'] = self.lft_hnd
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), name="tbm"): self.pos = pos self.rotmat = rotmat self.name = name this_dir, this_filename = os.path.split(__file__) # head self.head = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(1), name='head') self.head.jnts[1]['loc_pos'] = np.zeros(3) self.head.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) self.head.lnks[0]['name'] = 'tbm_front_shield' self.head.lnks[0]['loc_pos'] = np.array([0, 0, 0]) self.head.lnks[0]['meshfile'] = os.path.join(this_dir, 'meshes', 'tbm_front_shield.stl') self.head.lnks[0]['rgba'] = [.7, .7, .7, .3] self.head.lnks[1]['name'] = 'tbm_cutter_head' self.head.lnks[1]['loc_pos'] = np.array([0, 0, 0]) self.head.lnks[1]['meshfile'] = os.path.join(this_dir, 'meshes', 'tbm_cutter_head.stl') self.head.lnks[1]['rgba'] = [.7, .3, .3, 1] self.head.tgtjnts = [1] self.head.reinitialize() # cutter self.cutters = [] self.cutter_pos_dict = { '0': [ np.array([0.410, 0.0, 0.341 + 0.175]), np.array([0.410, 0.0, 0.590 + 0.175]), np.array([0.410, 0.0, 0.996 + 0.175]), np.array([0.410, 0.0, 1.245 + 0.175]), np.array([0.410, 0.0, 2.162 + 0.175]), np.array([0.410, 0.0, 2.802 + 0.175]), np.array([0.410, 0.0, 3.442 + 0.175]), np.array([0.410, 0.0, 4.082 + 0.175]), np.array([0.410, 0.0, 4.662 + 0.175]), np.array([0.410, 0.0, 5.712 + 0.175]) ], '6': [ np.array([0.410, 0.0, -0.341 - 0.175]), np.array([0.410, 0.0, -0.590 - 0.175]), np.array([0.410, 0.0, -0.996 - 0.175]), np.array([0.410, 0.0, -1.245 - 0.175]), np.array([0.410, 0.0, -2.020 - 0.175]), np.array([0.410, 0.0, -2.660 - 0.175]), np.array([0.410, 0.0, -3.300 - 0.175]), np.array([0.410, 0.0, -3.915 - 0.175]), np.array([0.410, 0.0, -4.515 - 0.175]), np.array([0.410, 0.0, -5.560 - 0.175]) ], '3': [ np.array([0.410, -0.050 + 0.175, 0.0]), np.array([0.410, 0.177 + 0.175, 0.0]), np.array([0.410, 0.605 + 0.175, 0.0]), np.array([0.410, 0.854 + 0.175, 0.0]), np.array([0.410, 1.450 + 0.175, 0.0]), np.array([0.410, 1.699 + 0.175, 0.0]), np.array([0.410, 2.562 + 0.175, 0.0]), np.array([0.410, 3.202 + 0.175, 0.0]), np.array([0.410, 3.837 + 0.175, 0.0]), np.array([0.410, 4.437 + 0.175, 0.0]), np.array([0.410, 5.037 + 0.175, 0.0]), np.array([0.410, 5.862 + 0.175, 0.0]), np.array([0.410, 6.912 + 0.175, 0.0]) ], '9': [ np.array([0.410, 0.050 - 0.175, 0.0]), np.array([0.410, -0.177 - 0.175, 0.0]), np.array([0.410, -0.605 - 0.175, 0.0]), np.array([0.410, -0.854 - 0.175, 0.0]), np.array([0.410, -1.450 - 0.175, 0.0]), np.array([0.410, -1.699 - 0.175, 0.0]), np.array([0.410, -2.260 - 0.175, 0.0]), np.array([0.410, -2.900 - 0.175, 0.0]), np.array([0.410, -3.540 - 0.175, 0.0]), np.array([0.410, -4.140 - 0.175, 0.0]), np.array([0.410, -4.740 - 0.175, 0.0]), np.array([0.410, -5.715 - 0.175, 0.0]), np.array([0.410, -6.765 - 0.175, 0.0]), np.array([ 0.417 - 0.175 * math.sin(math.pi / 6), -7.637 - 0.175 * math.cos(math.pi / 6), 0.0 ]) ], '1.5': [ np.array([ 0.410, 1.259 + 0.175 * math.sin(math.pi / 4), 1.259 + 0.175 * math.cos(math.pi / 4) ]), np.array([ 0.410, 1.711 + 0.175 * math.sin(math.pi / 4), 1.711 + 0.175 * math.cos(math.pi / 4) ]), np.array([ 0.410, 2.164 + 0.175 * math.sin(math.pi / 4), 2.164 + 0.175 * math.cos(math.pi / 4) ]), np.array([ 0.410, 2.609 + 0.175 * math.sin(math.pi / 4), 2.609 + 0.175 * math.cos(math.pi / 4) ]), np.array([ 0.410, 3.033 + 0.175 * math.sin(math.pi / 4), 3.033 + 0.175 * math.cos(math.pi / 4) ]), np.array([ 0.410, 3.882 + 0.175 * math.sin(math.pi / 4), 3.882 + 0.175 * math.cos(math.pi / 4) ]), np.array([ 0.410, 4.731 + 0.175 * math.sin(math.pi / 4), 4.731 + 0.175 * math.cos(math.pi / 4) ]), np.array([ 0.410, 5.101 + 0.175 * math.sin(math.pi / 4), 5.101 + 0.175 * math.cos(math.pi / 4) ]) ], '4.5': [ np.array([ 0.410, 1.541 + 0.175 * math.sin(3 * math.pi / 4), -1.541 + 0.175 * math.cos(3 * math.pi / 4) ]), np.array([ 0.410, 1.994 + 0.175 * math.sin(3 * math.pi / 4), -1.994 + 0.175 * math.cos(3 * math.pi / 4) ]), np.array([ 0.410, 2.447 + 0.175 * math.sin(3 * math.pi / 4), -2.447 + 0.175 * math.cos(3 * math.pi / 4) ]), np.array([ 0.410, 2.874 + 0.175 * math.sin(3 * math.pi / 4), -2.874 + 0.175 * math.cos(3 * math.pi / 4) ]), np.array([ 0.410, 3.299 + 0.175 * math.sin(3 * math.pi / 4), -3.299 + 0.175 * math.cos(3 * math.pi / 4) ]), np.array([ 0.410, 4.253 + 0.175 * math.sin(3 * math.pi / 4), -4.253 + 0.175 * math.cos(3 * math.pi / 4) ]), np.array([ 0.410, 4.996 + 0.175 * math.sin(3 * math.pi / 4), -4.996 + 0.175 * math.cos(3 * math.pi / 4) ]) ], '7.5': [ np.array([ 0.410, -1.315 + 0.175 * math.sin(-3 * math.pi / 4), -1.315 + 0.175 * math.cos(-3 * math.pi / 4) ]), np.array([ 0.410, -1.768 + 0.175 * math.sin(-3 * math.pi / 4), -1.768 + 0.175 * math.cos(-3 * math.pi / 4) ]), np.array([ 0.410, -2.220 + 0.175 * math.sin(-3 * math.pi / 4), -2.220 + 0.175 * math.cos(-3 * math.pi / 4) ]), np.array([ 0.410, -2.662 + 0.175 * math.sin(-3 * math.pi / 4), -2.662 + 0.175 * math.cos(-3 * math.pi / 4) ]), np.array([ 0.410, -3.086 + 0.175 * math.sin(-3 * math.pi / 4), -3.086 + 0.175 * math.cos(-3 * math.pi / 4) ]), np.array([ 0.410, -4.094 + 0.175 * math.sin(-3 * math.pi / 4), -4.094 + 0.175 * math.cos(-3 * math.pi / 4) ]), np.array([ 0.410, -4.837 + 0.175 * math.sin(-3 * math.pi / 4), -4.837 + 0.175 * math.cos(-3 * math.pi / 4) ]), np.array([ 0.436 + 0.175 * math.sin(-math.pi / 4), -5.429 + 0.175 * math.sin(-3 * math.pi / 4) * math.sin(3 * math.pi / 4), -5.429 + 0.175 * math.cos(-3 * math.pi / 4) * math.sin(3 * math.pi / 4) ]) ], '10.5': [ np.array([ 0.410, -1.485 + 0.175 * math.sin(-math.pi / 4), 1.485 + 0.175 * math.cos(-math.pi / 4) ]), np.array([ 0.410, -1.937 + 0.175 * math.sin(-math.pi / 4), 1.937 + 0.175 * math.cos(-math.pi / 4) ]), np.array([ 0.410, -2.390 + 0.175 * math.sin(-math.pi / 4), 2.390 + 0.175 * math.cos(-math.pi / 4) ]), np.array([ 0.410, -2.821 + 0.175 * math.sin(-math.pi / 4), 2.821 + 0.175 * math.cos(-math.pi / 4) ]), np.array([ 0.410, -3.246 + 0.175 * math.sin(-math.pi / 4), 3.246 + 0.175 * math.cos(-math.pi / 4) ]), np.array([ 0.410, -3.723 + 0.175 * math.sin(-math.pi / 4), 3.723 + 0.175 * math.cos(-math.pi / 4) ]), np.array([ 0.410, -4.200 + 0.175 * math.sin(-math.pi / 4), 4.200 + 0.175 * math.cos(-math.pi / 4) ]), np.array([ 0.410, -4.943 + 0.175 * math.sin(-math.pi / 4), 4.943 + 0.175 * math.cos(-math.pi / 4) ]) ], '0.75': [ np.array([ 0.410, 2.072 + 0.175 * math.sin(math.pi / 8), 5.003 + 0.175 * math.cos(math.pi / 8) ]), np.array([ 0.410, 2.503 + 0.175 * math.sin(math.pi / 8), 6.042 + 0.175 * math.cos(math.pi / 8) ]), np.array([ 0.410, 2.818 + 0.175 * math.sin(math.pi / 8), 6.804 + 0.175 * math.cos(math.pi / 8) ]) ], '2.25': [ np.array([ 0.410, 4.656 + 0.175 * math.sin(3 * math.pi / 8), 1.929 + 0.175 * math.cos(3 * math.pi / 8) ]), np.array([ 0.410, 5.626 + 0.175 * math.sin(3 * math.pi / 8), 2.331 + 0.175 * math.cos(3 * math.pi / 8) ]), np.array([ 0.410, 6.596 + 0.175 * math.sin(3 * math.pi / 8), 2.732 + 0.175 * math.cos(3 * math.pi / 8) ]) ], '3.75': [ np.array([ 0.410, 4.795 + 0.175 * math.sin(5 * math.pi / 8), -1.986 + 0.175 * math.cos(5 * math.pi / 8) ]), np.array([ 0.410, 5.973 + 0.175 * math.sin(5 * math.pi / 8), -2.474 + 0.175 * math.cos(5 * math.pi / 8) ]), np.array([ 0.410, 6.735 + 0.175 * math.sin(5 * math.pi / 8), -2.790 + 0.175 * math.cos(5 * math.pi / 8) ]) ], '5.25': [ np.array([ 0.410, 1.900 + 0.175 * math.sin(7 * math.pi / 8), -4.587 + 0.175 * math.cos(7 * math.pi / 8) ]), np.array([ 0.410, 2.388 + 0.175 * math.sin(7 * math.pi / 8), -5.765 + 0.175 * math.cos(7 * math.pi / 8) ]), np.array([ 0.410, 2.761 + 0.175 * math.sin(7 * math.pi / 8), -6.666 + 0.175 * math.cos(7 * math.pi / 8) ]) ], '6.75': [ np.array([ 0.410, -1.986 + 0.175 * math.sin(-7 * math.pi / 8), -4.795 + 0.175 * math.cos(-7 * math.pi / 8) ]), np.array([ 0.410, -2.445 + 0.175 * math.sin(-7 * math.pi / 8), -5.904 + 0.175 * math.cos(-7 * math.pi / 8) ]), np.array([ 0.410, -2.790 + 0.175 * math.sin(-7 * math.pi / 8), -6.735 + 0.175 * math.cos(-7 * math.pi / 8) ]) ], '8.25': [ np.array([ 0.410, -4.726 + 0.175 * math.sin(-5 * math.pi / 8), -1.957 + 0.175 * math.cos(-5 * math.pi / 8) ]), np.array([ 0.410, -5.696 + 0.175 * math.sin(-5 * math.pi / 8), -2.359 + 0.175 * math.cos(-5 * math.pi / 8) ]), np.array([ 0.410, -6.596 + 0.175 * math.sin(-5 * math.pi / 8), -2.732 + 0.175 * math.cos(-5 * math.pi / 8) ]) ], '9.75': [ np.array([ 0.410, -5.044 + 0.175 * math.sin(-3 * math.pi / 8), 2.089 + 0.175 * math.cos(-3 * math.pi / 8) ]), np.array([ 0.410, -6.153 + 0.175 * math.sin(-3 * math.pi / 8), 2.549 + 0.175 * math.cos(-3 * math.pi / 8) ]), np.array([ 0.410, -6.984 + 0.175 * math.sin(-3 * math.pi / 8), 2.893 + 0.175 * math.cos(-3 * math.pi / 8) ]) ], '11.25': [ np.array([ 0.410, -1.871 + 0.175 * math.sin(-math.pi / 8), 4.518 + 0.175 * math.cos(-math.pi / 8) ]), np.array([ 0.410, -2.417 + 0.175 * math.sin(-math.pi / 8), 5.834 + 0.175 * math.cos(-math.pi / 8) ]), np.array([ 0.410, -2.761 + 0.175 * math.sin(-math.pi / 8), 6.666 + 0.175 * math.cos(-math.pi / 8) ]) ] } self.cutter_rotmat_dict = { '0': [ np.eye(3), rm.rotmat_from_euler(0, math.pi, 0), np.eye(3), rm.rotmat_from_euler(0, math.pi, 0), np.eye(3), np.eye(3), np.eye(3), np.eye(3), np.eye(3), np.eye(3) ], '6': [ rm.rotmat_from_euler(0, math.pi, 0), np.eye(3), rm.rotmat_from_euler(0, math.pi, 0), np.eye(3), rm.rotmat_from_euler(0, math.pi, 0), rm.rotmat_from_euler(0, math.pi, 0), rm.rotmat_from_euler(0, math.pi, 0), rm.rotmat_from_euler(0, math.pi, 0), rm.rotmat_from_euler(0, math.pi, 0), rm.rotmat_from_euler(0, math.pi, 0) ], '3': [ rm.rotmat_from_euler(-math.pi / 2, 0, 0), rm.rotmat_from_euler(math.pi / 2, 0, 0), rm.rotmat_from_euler(-math.pi / 2, 0, 0), rm.rotmat_from_euler(math.pi / 2, 0, 0), rm.rotmat_from_euler(-math.pi / 2, 0, 0), rm.rotmat_from_euler(math.pi / 2, 0, 0), rm.rotmat_from_euler(-math.pi / 2, 0, 0), rm.rotmat_from_euler(-math.pi / 2, 0, 0), rm.rotmat_from_euler(-math.pi / 2, 0, 0), rm.rotmat_from_euler(-math.pi / 2, 0, 0), rm.rotmat_from_euler(-math.pi / 2, 0, 0), rm.rotmat_from_euler(-math.pi / 2, 0, 0), rm.rotmat_from_euler(-math.pi / 2, 0, 0) ], '9': [ rm.rotmat_from_euler(math.pi / 2, 0, 0), rm.rotmat_from_euler(-math.pi / 2, 0, 0), rm.rotmat_from_euler(math.pi / 2, 0, 0), rm.rotmat_from_euler(-math.pi / 2, 0, 0), rm.rotmat_from_euler(math.pi / 2, 0, 0), rm.rotmat_from_euler(-math.pi / 2, 0, 0), rm.rotmat_from_euler(math.pi / 2, 0, 0), rm.rotmat_from_euler(math.pi / 2, 0, 0), rm.rotmat_from_euler(math.pi / 2, 0, 0), rm.rotmat_from_euler(math.pi / 2, 0, 0), rm.rotmat_from_euler(math.pi / 2, 0, 0), rm.rotmat_from_euler(math.pi / 2, 0, 0), rm.rotmat_from_euler(math.pi / 2, 0, 0), rm.rotmat_from_euler(math.pi / 2, -math.pi / 6, 0, 'rxyz') ], '1.5': [ rm.rotmat_from_euler(3 * math.pi / 4, 0, 0), rm.rotmat_from_euler(3 * math.pi / 4, 0, 0), rm.rotmat_from_euler(3 * math.pi / 4, 0, 0), rm.rotmat_from_euler(3 * math.pi / 4, 0, 0), rm.rotmat_from_euler(3 * math.pi / 4, 0, 0), rm.rotmat_from_euler(3 * math.pi / 4, 0, 0), rm.rotmat_from_euler(3 * math.pi / 4, 0, 0), rm.rotmat_from_euler(3 * math.pi / 4, 0, 0) ], '4.5': [ rm.rotmat_from_euler(math.pi / 4, 0, 0), rm.rotmat_from_euler(math.pi / 4, 0, 0), rm.rotmat_from_euler(math.pi / 4, 0, 0), rm.rotmat_from_euler(math.pi / 4, 0, 0), rm.rotmat_from_euler(math.pi / 4, 0, 0), rm.rotmat_from_euler(math.pi / 4, 0, 0), rm.rotmat_from_euler(math.pi / 4, 0, 0) ], '7.5': [ rm.rotmat_from_euler(-math.pi / 4, 0, 0), rm.rotmat_from_euler(-math.pi / 4, 0, 0), rm.rotmat_from_euler(-math.pi / 4, 0, 0), rm.rotmat_from_euler(-math.pi / 4, 0, 0), rm.rotmat_from_euler(-math.pi / 4, 0, 0), rm.rotmat_from_euler(-math.pi / 4, 0, 0), rm.rotmat_from_euler(-math.pi / 4, 0, 0), rm.rotmat_from_euler(-math.pi / 4, math.pi / 4, 0, 'rxyz') ], '10.5': [ rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0), rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0), rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0), rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0), rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0), rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0), rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0), rm.rotmat_from_euler(-3 * math.pi / 4, 0, 0) ], '0.75': [ rm.rotmat_from_euler(7 * math.pi / 8, 0, 0), rm.rotmat_from_euler(7 * math.pi / 8, 0, 0), rm.rotmat_from_euler(7 * math.pi / 8, 0, 0) ], '2.25': [ rm.rotmat_from_euler(5 * math.pi / 8, 0, 0), rm.rotmat_from_euler(5 * math.pi / 8, 0, 0), rm.rotmat_from_euler(5 * math.pi / 8, 0, 0) ], '3.75': [ rm.rotmat_from_euler(3 * math.pi / 8, 0, 0), rm.rotmat_from_euler(3 * math.pi / 8, 0, 0), rm.rotmat_from_euler(3 * math.pi / 8, 0, 0) ], '5.25': [ rm.rotmat_from_euler(math.pi / 8, 0, 0), rm.rotmat_from_euler(math.pi / 8, 0, 0), rm.rotmat_from_euler(math.pi / 8, 0, 0) ], '6.75': [ rm.rotmat_from_euler(-math.pi / 8, 0, 0), rm.rotmat_from_euler(-math.pi / 8, 0, 0), rm.rotmat_from_euler(-math.pi / 8, 0, 0) ], '8.25': [ rm.rotmat_from_euler(-3 * math.pi / 8, 0, 0), rm.rotmat_from_euler(-3 * math.pi / 8, 0, 0), rm.rotmat_from_euler(-3 * math.pi / 8, 0, 0) ], '9.75': [ rm.rotmat_from_euler(-5 * math.pi / 8, 0, 0), rm.rotmat_from_euler(-5 * math.pi / 8, 0, 0), rm.rotmat_from_euler(-5 * math.pi / 8, 0, 0) ], '11.25': [ rm.rotmat_from_euler(-7 * math.pi / 8, 0, 0), rm.rotmat_from_euler(-7 * math.pi / 8, 0, 0), rm.rotmat_from_euler(-7 * math.pi / 8, 0, 0) ] } self.cutters = {} for k in self.cutter_pos_dict.keys(): self.cutters[k] = [] for i, pos in enumerate(self.cutter_pos_dict[k]): tmp_jlc = jl.JLChain(pos=pos, rotmat=self.cutter_rotmat_dict[k][i], homeconf=np.zeros(1), name='cutter_' + str(k) + '_' + str(i)) tmp_jlc.jnts[1]['loc_pos'] = np.zeros(3) tmp_jlc.jnts[1]['loc_motionax'] = np.array([0, 0, 1]) tmp_jlc.lnks[1]['name'] = 'cutter' tmp_jlc.lnks[1]['loc_pos'] = np.array([0, 0, 0]) tmp_jlc.lnks[1]['meshfile'] = os.path.join( this_dir, 'meshes', 'cutter.stl') tmp_jlc.lnks[1]['rgba'] = [1, 1, .0, 1.0] tmp_jlc.tgtjnts = [1] tmp_jlc.reinitialize() self.cutters[k].append(tmp_jlc)
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), cdmesh_type='box', name='xarm_gripper', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, cdmesh_type=cdmesh_type, name=name) this_dir, this_filename = os.path.split(__file__) cpl_end_pos = self.coupling.jnts[-1]['gl_posq'] cpl_end_rotmat = self.coupling.jnts[-1]['gl_rotmatq'] # - lft_outer self.lft_outer = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(2), name='jlc_lft_outer') self.lft_outer.jnts[1]['loc_pos'] = np.array([0, .035, .059098]) self.lft_outer.jnts[1]['motion_rng'] = [.0, .85] self.lft_outer.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) self.lft_outer.jnts[2]['loc_pos'] = np.array([0, .035465, .042039]) # passive self.lft_outer.jnts[2]['loc_motionax'] = np.array([-1, 0, 0]) # - lft_inner self.lft_inner = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(1), name='jlc_lft_inner') self.lft_inner.jnts[1]['loc_pos'] = np.array([0, .02, .074098]) self.lft_inner.jnts[1]['loc_motionax'] = np.array([1, 0, 0]) # - rgt_outer self.rgt_outer = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(2), name='jlc_rgt_outer') self.rgt_outer.jnts[1]['loc_pos'] = np.array([0, -.035, .059098]) self.rgt_outer.jnts[1]['loc_motionax'] = np.array([-1, 0, 0]) self.rgt_outer.jnts[2]['loc_pos'] = np.array([0, -.035465, .042039]) # passive self.rgt_outer.jnts[2]['loc_motionax'] = np.array([1, 0, 0]) # - rgt_inner self.rgt_inner = jl.JLChain(pos=cpl_end_pos, rotmat=cpl_end_rotmat, homeconf=np.zeros(1), name='jlc_rgt_inner') self.rgt_inner.jnts[1]['loc_pos'] = np.array([0, -.02, .074098]) self.rgt_inner.jnts[1]['loc_motionax'] = np.array([-1, 0, 0]) # links # - lft_outer self.lft_outer.lnks[0]['name'] = 'lnk_base' self.lft_outer.lnks[0]['loc_pos'] = np.zeros(3) self.lft_outer.lnks[0]['com'] = np.array( [-0.00065489, -0.0018497, 0.048028]) self.lft_outer.lnks[0]['mass'] = 0.5415 self.lft_outer.lnks[0]['meshfile'] = os.path.join( this_dir, "meshes", "base_link.stl") self.lft_outer.lnks[1]['name'] = 'lnk_left_outer_knuckle' self.lft_outer.lnks[1]['loc_pos'] = np.zeros(3) self.lft_outer.lnks[1]['com'] = np.array( [2.9948e-14, 0.021559, 0.015181]) self.lft_outer.lnks[1]['mass'] = 0.033618 self.lft_outer.lnks[1]['meshfile'] = os.path.join( this_dir, "meshes", "left_outer_knuckle.stl") self.lft_outer.lnks[1]['rgba'] = [.2, .2, .2, 1] self.lft_outer.lnks[2]['name'] = 'lnk_left_finger' self.lft_outer.lnks[2]['loc_pos'] = np.zeros(3) self.lft_outer.lnks[2]['com'] = np.array( [-2.4536e-14, -0.016413, 0.029258]) self.lft_outer.lnks[2]['mass'] = 0.048304 self.lft_outer.lnks[2]['meshfile'] = os.path.join( this_dir, "meshes", "left_finger.stl") self.lft_outer.lnks[2]['rgba'] = [.2, .2, .2, 1] # - lft_inner self.lft_inner.lnks[1]['name'] = 'lnk_left_inner_knuckle' self.lft_inner.lnks[1]['loc_pos'] = np.zeros(3) self.lft_inner.lnks[1]['com'] = np.array( [2.9948e-14, 0.021559, 0.015181]) self.lft_inner.lnks[1]['mass'] = 0.033618 self.lft_inner.lnks[1]['meshfile'] = os.path.join( this_dir, "meshes", "left_inner_knuckle.stl") self.lft_inner.lnks[1]['rgba'] = [.2, .2, .2, 1] # - rgt_outer self.rgt_outer.lnks[1]['name'] = 'lnk_right_outer_knuckle' self.rgt_outer.lnks[1]['loc_pos'] = np.zeros(3) self.rgt_outer.lnks[1]['com'] = np.array( [-3.1669e-14, -0.021559, 0.015181]) self.rgt_outer.lnks[1]['mass'] = 0.033618 self.rgt_outer.lnks[1]['meshfile'] = os.path.join( this_dir, "meshes", "right_outer_knuckle.stl") self.rgt_outer.lnks[1]['rgba'] = [.2, .2, .2, 1] self.rgt_outer.lnks[2]['name'] = 'lnk_right_finger' self.rgt_outer.lnks[2]['loc_pos'] = np.zeros(3) self.rgt_outer.lnks[2]['com'] = np.array( [2.5618e-14, 0.016413, 0.029258]) self.rgt_outer.lnks[2]['mass'] = 0.048304 self.rgt_outer.lnks[2]['meshfile'] = os.path.join( this_dir, "meshes", "right_finger.stl") self.rgt_outer.lnks[2]['rgba'] = [.2, .2, .2, 1] # - rgt_inner self.rgt_inner.lnks[1]['name'] = 'lnk_right_inner_knuckle' self.rgt_inner.lnks[1]['loc_pos'] = np.zeros(3) self.rgt_inner.lnks[1]['com'] = np.array( [1.866e-06, -0.022047, 0.026133]) self.rgt_inner.lnks[1]['mass'] = 0.023013 self.rgt_inner.lnks[1]['meshfile'] = os.path.join( this_dir, "meshes", "right_inner_knuckle.stl") self.rgt_inner.lnks[1]['rgba'] = [.2, .2, .2, 1] # reinitialize self.lft_outer.reinitialize() self.lft_inner.reinitialize() self.rgt_outer.reinitialize() self.rgt_inner.reinitialize() # jaw center self.jaw_center_pos = np.array([0, 0, .15]) # jaw width self.jawwidth_rng = [0.0, .085] # collision detection self.all_cdelements = [] self.enable_cc(toggle_cdprimit=enable_cc)
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), name='ur3dual', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) # left side self.lft_body = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(1), name='lft_body_jl') self.lft_body.jnts[1]['loc_pos'] = np.array([0.045, 0, 0.7296]) self.lft_body.jnts[2]['loc_pos'] = np.array([0.15, 0.101, 0.1704]) self.lft_body.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler(-math.pi / 2.0, -math.pi / 2.0, 0) self.lft_body.lnks[0]['name'] = "sda5f_lft_body" self.lft_body.lnks[0]['loc_pos'] = np.array([0, 0, 0]) self.lft_body.lnks[0]['collisionmodel'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "base_link.stl"), cdprimit_type="user_defined", expand_radius=.005, userdefined_cdprimitive_fn=self._base_combined_cdnp) self.lft_body.lnks[0]['rgba'] = [.7, .7, .7, 1.0] self.lft_body.lnks[1]['name'] = "sda5f_lft_torso" self.lft_body.lnks[1]['loc_pos'] = np.array([0, 0, 0]) self.lft_body.lnks[1]['collisionmodel'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "torso_link.stl"), cdprimit_type="user_defined", expand_radius=.005, userdefined_cdprimitive_fn=self._torso_combined_cdnp) self.lft_body.lnks[1]['rgba'] = [.7, .7, .7, 1.0] self.lft_body.reinitialize() lft_arm_homeconf = np.zeros(7) # lft_arm_homeconf[0] = math.pi / 3.0 # lft_arm_homeconf[1] = -math.pi * 1.0 / 3.0 # lft_arm_homeconf[2] = -math.pi * 2.0 / 3.0 # lft_arm_homeconf[3] = math.pi # lft_arm_homeconf[4] = -math.pi / 2.0 self.lft_arm = sia.SIA5(pos=self.lft_body.jnts[-1]['gl_posq'], rotmat=self.lft_body.jnts[-1]['gl_rotmatq'], homeconf=lft_arm_homeconf, enable_cc=False) # lft hand offset (if needed) self.lft_hnd_offset = np.zeros(3) lft_hnd_pos, lft_hnd_rotmat = self.lft_arm.cvt_loc_tcp_to_gl(loc_pos=self.lft_hnd_offset) self.lft_hnd = rtq.Robotiq85(pos=lft_hnd_pos, rotmat=self.lft_arm.jnts[-1]['gl_rotmatq'], enable_cc=False) # right side self.rgt_body = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(1), name='rgt_body_jl') self.rgt_body.jnts[1]['loc_pos'] = np.array([0.045, 0, 0.7296]) # right from robot_s view self.rgt_body.jnts[2]['loc_pos'] = np.array([0.15, -0.101, 0.1704]) self.rgt_body.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler(math.pi / 2.0, -math.pi / 2.0, 0) self.rgt_body.lnks[0]['name'] = "sda5f_rgt_body" self.rgt_body.lnks[0]['loc_pos'] = np.array([0, 0, 0]) self.rgt_body.lnks[0]['meshfile'] = None self.rgt_body.lnks[1]['name'] = "sda5f_rgt_torso" self.rgt_body.lnks[1]['loc_pos'] = np.array([0, 0, 0]) self.rgt_body.lnks[1]['meshfile'] = None self.rgt_body.reinitialize() rgt_arm_homeconf = np.zeros(7) # rgt_arm_homeconf[0] = -math.pi * 1.0 / 3.0 # rgt_arm_homeconf[1] = -math.pi * 2.0 / 3.0 # rgt_arm_homeconf[2] = math.pi * 2.0 / 3.0 # rgt_arm_homeconf[4] = math.pi / 2.0 self.rgt_arm = sia.SIA5(pos=self.rgt_body.jnts[-1]['gl_posq'], rotmat=self.rgt_body.jnts[-1]['gl_rotmatq'], homeconf=rgt_arm_homeconf, enable_cc=False) # rgt hand offset (if needed) self.rgt_hnd_offset = np.zeros(3) rgt_hnd_pos, rgt_hnd_rotmat = self.rgt_arm.cvt_loc_tcp_to_gl(loc_pos=self.rgt_hnd_offset) # TODO replace using copy self.rgt_hnd = rtq.Robotiq85(pos=rgt_hnd_pos, rotmat=self.rgt_arm.jnts[-1]['gl_rotmatq'], enable_cc=False) # tool center point # lft self.lft_arm.tcp_jntid = -1 self.lft_arm.tcp_loc_pos = np.array([0, 0, .145]) self.lft_arm.tcp_loc_rotmat = np.eye(3) # rgt self.rgt_arm.tcp_jntid = -1 self.rgt_arm.tcp_loc_pos = np.array([0, 0, .145]) self.rgt_arm.tcp_loc_rotmat = np.eye(3) # a list of detailed information about objects in hand, see CollisionChecker.add_objinhnd self.lft_oih_infos = [] self.rgt_oih_infos = [] # collision detection if enable_cc: self.enable_cc() self.manipulator_dict['rgt_arm'] = self.rgt_arm self.manipulator_dict['lft_arm'] = self.lft_arm self.manipulator_dict['rgt_hnd'] = self.rgt_arm # specify which hand is a gripper installed to self.manipulator_dict['lft_hnd'] = self.lft_arm # specify which hand is a gripper installed to self.hnd_dict['rgt_hnd'] = self.rgt_hnd self.hnd_dict['lft_hnd'] = self.lft_hnd
import time import math import numpy as np import basis.robot_math as rm import visualization.panda.world as wd import robot_sim._kinematics.jlchain as jlc import modeling.geometric_model as gm if __name__ == "__main__": base = wd.World(cam_pos=[2, 0, 2], lookat_pos=[0, 0, 0]) gm.gen_frame(length=.2).attach_to(base) jlinstance = jlc.JLChain(homeconf=np.array([0, 0, 0, 0, 0])) jlinstance.jnts[4]['type'] = 'prismatic' jlinstance.jnts[4]['loc_motionax'] = np.array([1, 0, 0]) jlinstance.jnts[4]['motion_val'] = .2 jlinstance.jnts[4]['motion_rng'] = [-.5, .5] jlinstance.reinitialize() jlinstance.gen_stickmodel(toggle_jntscs=True, rgba=[1, 0, 0, .15]).attach_to(base) tgt_pos0 = np.array([.3, .1, 0]) tgt_rotmat0 = np.eye(3) gm.gen_mycframe(pos=tgt_pos0, rotmat=tgt_rotmat0, length=.15, thickness=.01).attach_to(base) jlinstance.set_tcp(tcp_jntid=4, tcp_loc_pos=np.array([.2, -.13, 0]), tcp_loc_rotmat=rm.rotmat_from_axangle( np.array([0, 0, 1]), math.pi / 8)) tic = time.time()
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), name='yumi', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) # lft self.lft_body = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(7), name='lft_body') self.lft_body.jnts[1]['loc_pos'] = np.array([0, 0, 0]) self.lft_body.jnts[2]['loc_pos'] = np.array([0, 0, 0]) self.lft_body.jnts[3]['loc_pos'] = np.array([0, 0, 0]) self.lft_body.jnts[4]['loc_pos'] = np.array([0, 0, 0]) self.lft_body.jnts[5]['loc_pos'] = np.array([0, 0, 0]) self.lft_body.jnts[6]['loc_pos'] = np.array([0, 0, 0]) self.lft_body.jnts[7]['loc_pos'] = np.array([0, 0, 0]) self.lft_body.jnts[8]['loc_pos'] = np.array( [0.05355, 0.07250, 0.41492]) self.lft_body.jnts[8]['loc_rotmat'] = rm.rotmat_from_euler( 0.9781, -0.5716, 2.3180) # left from robot_s view self.lft_body.lnks[0]['name'] = "yumi_lft_stand" self.lft_body.lnks[0]['loc_pos'] = np.array([0, 0, 0]) self.lft_body.lnks[0]['mesh_file'] = os.path.join( this_dir, "meshes", "yumi_tablenotop.stl") self.lft_body.lnks[0]['rgba'] = [.55, .55, .55, 1.0] self.lft_body.lnks[1]['name'] = "yumi_lft_body" self.lft_body.lnks[1]['loc_pos'] = np.array([0, 0, 0]) self.lft_body.lnks[1]['collision_model'] = cm.CollisionModel( os.path.join(this_dir, "meshes", "body.stl"), cdprimit_type="user_defined", expand_radius=.005, userdefined_cdprimitive_fn=self._base_combined_cdnp) self.lft_body.lnks[1]['rgba'] = [.7, .7, .7, 1] self.lft_body.lnks[2]['name'] = "yumi_lft_column" self.lft_body.lnks[2]['loc_pos'] = np.array([-.327, -.24, -1.015]) self.lft_body.lnks[2]['mesh_file'] = os.path.join( this_dir, "meshes", "yumi_column60602100.stl") self.lft_body.lnks[2]['rgba'] = [.35, .35, .35, 1.0] self.lft_body.lnks[3]['name'] = "yumi_lft_column" self.lft_body.lnks[3]['loc_pos'] = np.array([-.327, .24, -1.015]) self.lft_body.lnks[3]['mesh_file'] = os.path.join( this_dir, "meshes", "yumi_column60602100.stl") self.lft_body.lnks[3]['rgba'] = [.35, .35, .35, 1.0] self.lft_body.lnks[4]['name'] = "yumi_top_back" self.lft_body.lnks[4]['loc_pos'] = np.array([-.327, 0, 1.085]) self.lft_body.lnks[4]['mesh_file'] = os.path.join( this_dir, "meshes", "yumi_column6060540.stl") self.lft_body.lnks[4]['rgba'] = [.35, .35, .35, 1.0] self.lft_body.lnks[5]['name'] = "yumi_top_lft" self.lft_body.lnks[5]['loc_pos'] = np.array([-.027, -.24, 1.085]) self.lft_body.lnks[5]['loc_rotmat'] = rm.rotmat_from_axangle( [0, 0, 1], -math.pi / 2) self.lft_body.lnks[5]['mesh_file'] = os.path.join( this_dir, "meshes", "yumi_column6060540.stl") self.lft_body.lnks[5]['rgba'] = [.35, .35, .35, 1.0] self.lft_body.lnks[6]['name'] = "yumi_top_lft" self.lft_body.lnks[6]['loc_pos'] = np.array([-.027, .24, 1.085]) self.lft_body.lnks[6]['loc_rotmat'] = rm.rotmat_from_axangle( [0, 0, 1], -math.pi / 2) self.lft_body.lnks[6]['mesh_file'] = os.path.join( this_dir, "meshes", "yumi_column6060540.stl") self.lft_body.lnks[6]['rgba'] = [.35, .35, .35, 1.0] self.lft_body.lnks[7]['name'] = "yumi_top_front" self.lft_body.lnks[7]['loc_pos'] = np.array([.273, 0, 1.085]) self.lft_body.lnks[7]['mesh_file'] = os.path.join( this_dir, "meshes", "yumi_column6060540.stl") self.lft_body.lnks[7]['rgba'] = [.35, .35, .35, 1.0] self.lft_body.reinitialize() lft_arm_homeconf = np.radians(np.array([20, -90, 120, 30, 0, 40, 0])) self.lft_arm = ya.IRB14050(pos=self.lft_body.jnts[-1]['gl_posq'], rotmat=self.lft_body.jnts[-1]['gl_rotmatq'], homeconf=lft_arm_homeconf, enable_cc=False) self.lft_arm.fix_to(pos=self.lft_body.jnts[-1]['gl_posq'], rotmat=self.lft_body.jnts[-1]['gl_rotmatq']) self.lft_hnd = yg.YumiGripper( pos=self.lft_arm.jnts[-1]['gl_posq'], rotmat=self.lft_arm.jnts[-1]['gl_rotmatq'], enable_cc=False, name='lft_hnd') self.lft_hnd.fix_to(pos=self.lft_arm.jnts[-1]['gl_posq'], rotmat=self.lft_arm.jnts[-1]['gl_rotmatq']) # rgt self.rgt_body = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=np.zeros(0), name='rgt_body') self.rgt_body.jnts[1]['loc_pos'] = np.array( [0.05355, -0.0725, 0.41492]) self.rgt_body.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler( -0.9795, -0.5682, -2.3155) # left from robot_s view self.rgt_body.lnks[0]['name'] = "yumi_rgt_body" self.rgt_body.lnks[0]['loc_pos'] = np.array([0, 0, 0]) self.rgt_body.lnks[0]['rgba'] = [.35, .35, .35, 1.0] self.rgt_body.reinitialize() rgt_arm_homeconf = np.radians(np.array([-20, -90, -120, 30, .0, 40, 0])) self.rgt_arm = self.lft_arm.copy() self.rgt_arm.fix_to(pos=self.rgt_body.jnts[-1]['gl_posq'], rotmat=self.rgt_body.jnts[-1]['gl_rotmatq']) self.rgt_arm.set_homeconf(rgt_arm_homeconf) self.rgt_arm.goto_homeconf() self.rgt_hnd = self.lft_hnd.copy() self.rgt_hnd.name = 'rgt_hnd' self.rgt_hnd.fix_to(pos=self.rgt_arm.jnts[-1]['gl_posq'], rotmat=self.rgt_arm.jnts[-1]['gl_rotmatq']) # tool center point # lft self.lft_arm.tcp_jnt_id = -1 self.lft_arm.tcp_loc_pos = self.lft_hnd.jaw_center_pos self.lft_arm.tcp_loc_rotmat = self.lft_hnd.jaw_center_rotmat # rgt self.rgt_arm.tcp_jnt_id = -1 self.rgt_arm.tcp_loc_pos = self.rgt_hnd.jaw_center_pos self.rgt_arm.tcp_loc_rotmat = self.rgt_hnd.jaw_center_rotmat # a list of detailed information about objects in hand, see CollisionChecker.add_objinhnd self.lft_oih_infos = [] self.rgt_oih_infos = [] # collision detection if enable_cc: self.enable_cc() # component map self.manipulator_dict['rgt_arm'] = self.rgt_arm self.manipulator_dict['lft_arm'] = self.lft_arm self.manipulator_dict['rgt_hnd'] = self.rgt_arm self.manipulator_dict['lft_hnd'] = self.lft_arm self.hnd_dict['rgt_hnd'] = self.rgt_hnd self.hnd_dict['lft_hnd'] = self.lft_hnd self.hnd_dict['rgt_arm'] = self.rgt_hnd self.hnd_dict['lft_arm'] = self.lft_hnd
import os
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), homeconf=np.zeros(7), name='sia5', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) self.jlc = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=homeconf, name=name) # seven joints, n_jnts = 7+2 (tgt ranges from 1-7), nlinks = 7+1 jnt_safemargin = math.pi / 18.0 self.jlc.jnts[1]['loc_pos'] = np.array([0, 0, 0]) self.jlc.jnts[1]['motion_rng'] = [-math.pi + jnt_safemargin, math.pi] self.jlc.jnts[2]['loc_pos'] = np.array([0, 0, .168]) self.jlc.jnts[2]['motion_rng'] = [ -math.radians(110) + jnt_safemargin, math.radians(110) - jnt_safemargin ] self.jlc.jnts[2]['loc_motionax'] = np.array([0, 1, 0]) self.jlc.jnts[3]['loc_pos'] = np.array([0, 0, .27]) self.jlc.jnts[3]['motion_rng'] = [ -math.radians(170) + jnt_safemargin, math.radians(170) - jnt_safemargin ] self.jlc.jnts[4]['loc_pos'] = np.array([.085, 0, 0]) self.jlc.jnts[4]['motion_rng'] = [ -math.radians(90) + jnt_safemargin, math.radians(115) - jnt_safemargin ] self.jlc.jnts[4]['loc_motionax'] = np.array([0, -1, 0]) self.jlc.jnts[5]['loc_pos'] = np.array([.27, -0, .06]) self.jlc.jnts[5]['motion_rng'] = [ -math.radians(90) + jnt_safemargin, math.radians(90) - jnt_safemargin ] self.jlc.jnts[5]['loc_motionax'] = np.array([-1, 0, 0]) self.jlc.jnts[6]['loc_pos'] = np.array([0, 0, 0]) self.jlc.jnts[6]['motion_rng'] = [ -math.radians(110) + jnt_safemargin, math.radians(110) - jnt_safemargin ] self.jlc.jnts[6]['loc_motionax'] = np.array([0, -1, 0]) self.jlc.jnts[7]['loc_pos'] = np.array([.134, .0, .0]) self.jlc.jnts[7]['loc_rotmat'] = rm.rotmat_from_euler( -math.pi / 2, 0, 0) self.jlc.jnts[7]['motion_rng'] = [ -math.radians(90) + jnt_safemargin, math.radians(90) - jnt_safemargin ] self.jlc.jnts[7]['loc_motionax'] = np.array([-1, 0, 0]) self.jlc.jnts[8]['loc_pos'] = np.array([.011, .0, .0]) self.jlc.jnts[8]['loc_rotmat'] = rm.rotmat_from_euler( -math.pi / 2, -math.pi / 2, -math.pi / 2) # links # self.jlc.lnks[0]['name'] = "base" # self.jlc.lnks[0]['loc_pos'] = np.zeros(3) # self.jlc.lnks[0]['meshfile'] = os.path.join(this_dir, "meshes", "base.dae") # self.jlc.lnks[0]['rgba'] = [.5,.5,.5, 1.0] self.jlc.lnks[1]['name'] = "link_s" self.jlc.lnks[1]['loc_pos'] = np.array([0, 0, .142]) self.jlc.lnks[1]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, math.pi) self.jlc.lnks[1]['meshfile'] = os.path.join(this_dir, "meshes", "link_s.dae") self.jlc.lnks[1]['rgba'] = [.55, .55, .55, 1.0] self.jlc.lnks[2]['name'] = "link_l" self.jlc.lnks[2]['loc_pos'] = np.zeros(3) self.jlc.lnks[2]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, math.pi) self.jlc.lnks[2]['meshfile'] = os.path.join(this_dir, "meshes", "link_l.dae") self.jlc.lnks[2]['rgba'] = [.1, .3, .5, 1.0] self.jlc.lnks[3]['name'] = "link_e" self.jlc.lnks[3]['loc_pos'] = np.zeros(3) self.jlc.lnks[3]['meshfile'] = os.path.join(this_dir, "meshes", "link_e.dae") self.jlc.lnks[3]['rgba'] = [.7, .7, .7, 1.0] self.jlc.lnks[4]['name'] = "link_u" self.jlc.lnks[4]['loc_pos'] = np.zeros(3) self.jlc.lnks[4]['meshfile'] = os.path.join(this_dir, "meshes", "link_u.dae") self.jlc.lnks[4]['rgba'] = [.1, .3, .5, 1.0] self.jlc.lnks[5]['name'] = "link_r" self.jlc.lnks[5]['loc_pos'] = np.zeros(3) self.jlc.lnks[5]['meshfile'] = os.path.join(this_dir, "meshes", "link_r.dae") self.jlc.lnks[5]['rgba'] = [.7, .7, .7, 1.0] self.jlc.lnks[6]['name'] = "link_b" self.jlc.lnks[6]['loc_pos'] = np.zeros(3) self.jlc.lnks[6]['meshfile'] = os.path.join(this_dir, "meshes", "link_b.dae") self.jlc.lnks[6]['rgba'] = [.1, .3, .5, 1.0] self.jlc.lnks[7]['name'] = "link_t" self.jlc.lnks[7]['loc_pos'] = np.zeros(3) self.jlc.lnks[7]['meshfile'] = os.path.join(this_dir, "meshes", "link_t.dae") self.jlc.lnks[7]['rgba'] = [.7, .7, .7, 1.0] # reinitialization # self.jlc.setinitvalues(np.array([-math.pi/2, math.pi/3, math.pi/6, 0, 0, 0, 0])) # self.jlc.setinitvalues(np.array([-math.pi/2, 0, math.pi/3, math.pi/10, 0, 0, 0])) self.jlc.reinitialize() # collision detection if enable_cc: self.enable_cc()
def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), homeconf=np.zeros(3), name='cobotta', enable_cc=True): super().__init__(pos=pos, rotmat=rotmat, name=name) this_dir, this_filename = os.path.split(__file__) # the last joint is passive new_homeconf = self._mimic_jnt_values(homeconf) self.jlc = jl.JLChain(pos=pos, rotmat=rotmat, homeconf=new_homeconf, name=name) # six joints, n_jnts = 6+2 (tgt ranges from 1-6), nlinks = 6+1 self.jlc.jnts[1]['loc_pos'] = np.array([0, 0, 0.024]) self.jlc.jnts[1]['loc_rotmat'] = rm.rotmat_from_euler( 0, 0, -1.570796325) self.jlc.jnts[1]['loc_motionax'] = np.array([0, 0, 1]) self.jlc.jnts[1]['motion_rng'] = [-3.14159265, 3.14159265] self.jlc.jnts[2]['loc_pos'] = np.array([-0.01175, 0, 0.114]) self.jlc.jnts[2]['loc_rotmat'] = rm.rotmat_from_euler( 1.570796325, -0.20128231967888244, -1.570796325) self.jlc.jnts[2]['loc_motionax'] = np.array([0, 0, 1]) self.jlc.jnts[2]['motion_rng'] = [0, 1.570796325] self.jlc.jnts[3]['loc_pos'] = np.array([0.02699, 0.13228, -0.01175]) self.jlc.jnts[3]['loc_rotmat'] = rm.rotmat_from_euler( 0, 3.14159265, -1.24211575528) self.jlc.jnts[3]['loc_motionax'] = np.array([0, 0, -1]) self.jlc.jnts[3]['motion_rng'] = [0, 1.570796325] self.jlc.jnts[4]['loc_pos'] = np.array([0.07431, -0.12684, 0.0]) self.jlc.jnts[4]['loc_rotmat'] = rm.rotmat_from_euler(0, 3.14159265, 0) self.jlc.jnts[4]['loc_motionax'] = np.array([0, 0, 1]) # self.jlc.jnts[5]['loc_pos'] = np.array([-0.0328, 0.02871, 0]) self.jlc.jnts[5]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, 3.14159265) # links self.jlc.lnks[0]['name'] = "base" self.jlc.lnks[0]['loc_pos'] = np.zeros(3) self.jlc.lnks[0]['mesh_file'] = os.path.join(this_dir, "meshes", "base_link.stl") self.jlc.lnks[0]['rgba'] = [.5, .5, .5, 1.0] self.jlc.lnks[1]['name'] = "link1" self.jlc.lnks[1]['loc_pos'] = np.zeros(3) self.jlc.lnks[1]['mesh_file'] = os.path.join(this_dir, "meshes", "link_1.stl") self.jlc.lnks[1]['rgba'] = [.55, .55, .55, 1.0] self.jlc.lnks[2]['name'] = "link2" self.jlc.lnks[2]['loc_pos'] = np.zeros(3) self.jlc.lnks[2]['mesh_file'] = os.path.join(this_dir, "meshes", "link_2.stl") self.jlc.lnks[2]['rgba'] = [.15, .15, .15, 1] self.jlc.lnks[3]['name'] = "link3" self.jlc.lnks[3]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[3]['mesh_file'] = os.path.join(this_dir, "meshes", "link_5.stl") self.jlc.lnks[3]['rgba'] = [.55, .55, .55, 1] self.jlc.lnks[4]['name'] = "link4" self.jlc.lnks[4]['loc_pos'] = np.array([.0, .0, .0]) self.jlc.lnks[4]['mesh_file'] = os.path.join(this_dir, "meshes", "link_6.stl") self.jlc.lnks[4]['rgba'] = [.35, .35, .35, 1.0] self.jlc.reinitialize() # prepare parameters for analytical ik upper_arm_vec = self.jlc.jnts[3]['gl_posq'] - np.array([0, 0, 0.138]) lower_arm_vec = self.jlc.jnts[4]['gl_posq'] - self.jlc.jnts[3][ 'gl_posq'] self._init_j2_angle = math.asin( np.sqrt(upper_arm_vec[0]**2 + upper_arm_vec[1]**2) / 0.135) self._init_j3_angle = rm.angle_between_vectors(lower_arm_vec, -upper_arm_vec) print(self._init_j3_angle, self._init_j2_angle) # collision detection if enable_cc: self.enable_cc()