def _init_jlchain(self): """ init joints and links chains there are two lists of dictionaries where the first one is joints, the second one is links links: a list of dictionaries with each dictionary holding the properties of a link joints: a list of dictionaries with each dictionary holding the properties of a joint njoints is assumed to be equal to nlinks+1 joint i connects link i-1 and link i :return: author: weiwei date: 20161202tsukuba, 20190328toyonaka, 20200330toyonaka """ lnks = [dict() for i in range(self.ndof + 1)] jnts = [dict() for i in range(self.ndof + 2)] for id in range(self.ndof + 1): lnks[id]['name'] = 'link0' lnks[id]['loc_pos'] = np.array([0, 0, 0]) lnks[id]['loc_rotmat'] = rm.rotmat_from_euler(0, 0, 0) lnks[id]['com'] = np.zeros(3) lnks[id]['inertia'] = np.eye(3) lnks[id][ 'mass'] = 0 # the visual adjustment is ignored for simplisity lnks[id]['meshfile'] = None lnks[id]['collisionmodel'] = None lnks[id][ 'cdprimit_childid'] = -1 # id of the CollisionChecker.np.Child lnks[id]['scale'] = [1, 1, 1] # 3 list lnks[id]['rgba'] = [.7, .7, .7, 1] # 4 list for id in range(self.ndof + 2): jnts[id]['type'] = 'revolute' jnts[id]['parent'] = id - 1 jnts[id]['child'] = id + 1 jnts[id]['loc_pos'] = np.array([0, .1, 0]) if id > 0 else np.array( [0, 0, 0]) jnts[id]['loc_rotmat'] = np.eye(3) jnts[id]['loc_motionax'] = np.array( [0, 0, 1]) # rot ax for rev joint, linear ax for pris joint jnts[id]['gl_pos0'] = jnts[id][ 'loc_pos'] # to be updated by self._update_fk jnts[id]['gl_rotmat0'] = jnts[id][ 'loc_rotmat'] # to be updated by self._update_fk jnts[id]['gl_motionax'] = jnts[id][ 'loc_motionax'] # to be updated by self._update_fk jnts[id]['gl_posq'] = jnts[id][ 'gl_pos0'] # to be updated by self._update_fk jnts[id]['gl_rotmatq'] = jnts[id][ 'gl_rotmat0'] # to be updated by self._update_fk jnts[id]['motion_rng'] = [-math.pi, math.pi] # min, max jnts[id]['motion_val'] = 0 jnts[0]['gl_pos0'] = self.pos # This is not necessary, for easy read jnts[0]['gl_rotmat0'] = self.rotmat jnts[0]['type'] = 'end' jnts[self.ndof + 1]['loc_pos'] = np.array([0, 0, 0]) jnts[self.ndof + 1]['child'] = -1 jnts[self.ndof + 1]['type'] = 'end' return lnks, jnts
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 set_rpy(self, roll, pitch, yaw): """ set the pose of the object using rpy :param roll: radian :param pitch: radian :param yaw: radian :return: author: weiwei date: 20190513 """ npmat3 = rm.rotmat_from_euler(roll, pitch, yaw, axes="sxyz") self.set_rotmat(npmat3)
def loadObjitem(f_name, pos=(0, 0, 0), rot=(0, 0, 0), sample_num=10000, type="box"): if f_name[-3:] != 'stl': f_name += '.stl' objcm = cm.CollisionModel(objinit=os.path.join(config.ROOT, "obstacles", f_name), type=type) objmat4 = np.zeros([4, 4]) objmat4[:3, :3] = rm.rotmat_from_euler(rot[0], rot[1], rot[2], axes="sxyz") objmat4[:3, 3] = pos objcm.sethomomat(objmat4) return item.Item(objcm=objcm, objmat4=objmat4, sample_num=sample_num)
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 setRPY(self, roll, pitch, yaw): """ set the pose of the object using rpy :param roll: degree :param pitch: degree :param yaw: degree :return: author: weiwei date: 20190513 """ currentmat = self._bdb.gethomomat() # currentmatnp = base.pg.mat4ToNp(currentmat)# motified by hu 2020/06/30 currentmatnp = currentmat # motified by hu 2020/06/30 newmatnp = rm.rotmat_from_euler(roll, pitch, yaw, axes="sxyz") self.set(base.pg.npToMat4(newmatnp, currentmatnp[:, 3]))
def set_rpy(self, roll, pitch, yaw): """ set the pose of the object using rpy :param roll: degree :param pitch: degree :param yaw: degree :return: author: weiwei date: 20190513 """ currentmatnp = self._gm.get_homomat() # currentmatnp = da.pdmat4_to_npmat4(currentmat) newmatnp = rm.rotmat_from_euler(roll, pitch, yaw, axes="sxyz") # self.__objnp.setMat(p3du.npToMat4(newmatnp, currentmatnp[:,3])) self._bdb.set_homomat( rm.homomat_from_posrot(currentmatnp[:3, 3], newmatnp)) self._gm.set_homomat( rm.homomat_from_posrot(currentmatnp[:3, 3], newmatnp))
def loadObjpcd(f_name, pos=(0, 0, 0), rot=(0, 0, 0), sample_num=100000, toggledebug=False): obj = cm.CollisionModel( objinit=os.path.join(config.ROOT, "obstacles", f_name)) rotmat4 = np.zeros([4, 4]) rotmat4[:3, :3] = rm.rotmat_from_euler(rot[0], rot[1], rot[2], axes="sxyz") rotmat4[:3, 3] = pos obj_surface = np.asarray(obj.sample_surface(nsample=sample_num)) # obj_surface = obj_surface[obj_surface[:, 2] > 2] obj_surface_real = __pcd_trans(obj_surface, rotmat4) if toggledebug: # obj_surface = o3d_helper.nparray2o3dpcd(copy.deepcopy(obj_surface)) # obj_surface.paint_uniform_color([1, 0.706, 0]) # o3d.visualization.draw_geometries([obj_surface], window_name='loadObjpcd') # pcddnp = base.pg.genpointcloudnp(obj_surface_real) # pcddnp.reparentTo(base.render) pass return obj_surface_real
def update(bunnycm, task): if base.inputmgr.keymap['space'] is True: for i in range(1): bunnycm1 = bunnycm.copy() bunnycm1.set_mass(.1) rndcolor = np.random.rand(4) rndcolor[-1] = 1 bunnycm1.set_rgba(rndcolor) rotmat = rm.rotmat_from_euler(0, 0, math.pi / 12) z = math.floor(i / 100) y = math.floor((i - z * 100) / 10) x = i - z * 100 - y * 10 print(x, y, z, "\n") bunnycm1.set_homomat( rm.homomat_from_posrot( np.array( [x * 0.015 - 0.07, y * 0.015 - 0.07, 0.35 + z * 0.015]), rotmat)) base.attach_internal_update_obj(bunnycm1) bunnycm1.start_physics() base.inputmgr.keymap['space'] = False return task.cont
def update(bunnycm, task): if base.inputmgr.keyMap['space'] is True: for i in range(300): bunnycm1 = bunnycm.copy() bunnycm1.setMass(.1) # bunnycm1.setColor(0.7, 0, 0.7, 1.0) bunnycm1.setColor(random.random(), random.random(), random.random(), 1.0) # bunnycm1.reparentTo(base.render) # rotmat = rm.rodrigues([0,0,1], 15) rotmat = rm.rotmat_from_euler(0, 0, 15) z = math.floor(i / 100) y = math.floor((i - z * 100) / 10) x = i - z * 100 - y * 10 print(x, y, z, "\n") bunnycm1.setMat( base.pg.npToMat4( rotmat, np.array([x * 15 - 70, y * 15 - 70, 150 + z * 15]))) base.attachRUD(bunnycm1) base.inputmgr.keyMap['space'] = False return task.cont
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
object1.set_pos(np.array([.45, .48, 1.11])) object1.set_rgba([.8, .8, .8, 1]) object1.attach_to(base) # マグカップの生成 object2 = cm.CollisionModel("./objects/mug.stl") object2.set_pos(np.array([.3, .48, 1.11])) object2.set_rgba([.5, .7, .3, 1]) object2.attach_to(base) # ロボットの生成 component_name = 'lft_arm' robot_s = ur3d.UR3Dual() start_pos = np.array([.4, .3, 1.11]) start_rotmat = rm.rotmat_from_euler(ai=math.pi / 2, aj=math.pi, ak=0, axes='szxz') start_conf = robot_s.ik(component_name=component_name, tgt_pos=start_pos, tgt_rotmat=start_rotmat) goal_pos = np.array([.4, .7, 1.11]) goal_rotmat = rm.rotmat_from_euler(ai=math.pi / 2, aj=math.pi, ak=0, axes='szxz') goal_conf = robot_s.ik(component_name=component_name, tgt_pos=goal_pos, tgt_rotmat=goal_rotmat) # 経路計画 rrtc_planner = rrtc.RRTConnect(robot_s)
self.rgt.gen_meshmodel(tcp_loc_pos=None, tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) return meshmodel if __name__ == '__main__': import visualization.panda.world as wd import modeling.geometric_model as gm base = wd.World(cam_pos=[.5, .5, .5], lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) grpr = YumiGripper(enable_cc=True) grpr.fix_to(pos=np.array([0, .3, .2]), rotmat=rm.rotmat_from_euler(math.pi/3, math.pi/3, math.pi/3)) grpr.jaw_to(.02) print(grpr.get_jawwidth()) grpr.gen_stickmodel().attach_to(base) grpr.gen_meshmodel(rgba=[0, .5, 0, .5]).attach_to(base) # grpr.gen_stickmodel(togglejntscs=False).attach_to(base) # grpr.fix_to(pos=np.array([0, .3, .2]), rotmat=rm.rotmat_from_axangle([1, 0, 0], math.pi/3)) grpr.fix_to(pos=np.zeros(3), rotmat=np.eye(3)) grpr.gen_meshmodel().attach_to(base) grpr2 = grpr.copy() grpr2.fix_to(pos=np.array([.3,.3,.2]), rotmat=rm.rotmat_from_axangle([0,1,0],.01)) model = grpr2.gen_meshmodel(rgba=[0.5, .5, 0, .5]) model.attach_to(base) grpr2.show_cdprimit() grpr2.show_cdmesh()
import visualization.panda.world as wd base = wd.World(cam_pos=[.3, .3, .3], lookat_pos=[0, 0, 0], toggle_debug=True) objpath = os.path.join(basis.__path__[0], 'objects', 'bunnysim.stl') bunnycm = CollisionModel(objpath, cdprimit_type='polygons') bunnycm.set_rgba([0.7, 0.7, 0.0, .2]) bunnycm.show_localframe() rotmat = rm.rotmat_from_axangle([1, 0, 0], math.pi / 2.0) bunnycm.set_rotmat(rotmat) bunnycm.show_cdprimit() bunnycm1 = CollisionModel(objpath, cdprimit_type="cylinder") bunnycm1.set_rgba([0.7, 0, 0.7, 1.0]) rotmat = rm.rotmat_from_euler(0, 0, math.radians(15)) bunnycm1.set_pos(np.array([0, .01, 0])) bunnycm1.set_rotmat(rotmat) bunnycm2 = bunnycm1.copy() bunnycm2.change_cdprimitive_type(cdprimitive_type='surface_balls') bunnycm2.set_rgba([0, 0.7, 0.7, 1.0]) rotmat = rm.rotmat_from_axangle([1, 0, 0], -math.pi / 4.0) bunnycm2.set_pos(np.array([0, .2, 0])) bunnycm2.set_rotmat(rotmat) bunnycm.attach_to(base) bunnycm1.attach_to(base) bunnycm2.attach_to(base) bunnycm.show_cdprimit() bunnycm1.show_cdprimit()
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), 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)
import robot_sim.robots.xarm7_shuidi_mobile.xarm7_shuidi_mobile as xsm import manipulation.pick_place_planner as ppp import motion.probabilistic.rrt_connect as rrtc base = wd.World(cam_pos=[2.1, -2.1, 2.1], lookat_pos=[.0, 0, .3]) gm.gen_frame().attach_to(base) # ground ground = cm.gen_box(extent=[5, 5, 1], rgba=[.57, .57, .5, .7]) ground.set_pos(np.array([0, 0, -.51])) ground.attach_to(base) # object box object_box = cm.gen_box(extent=[.02, .06, .7], rgba=[.7, .5, .3, .7]) # object_box_gl_pos = np.array([.3, -.4, .35]) # object_box_gl_rotmat = np.eye(3) object_box_gl_pos = np.array([.3, .4, .35]) object_box_gl_rotmat = rm.rotmat_from_euler(0, 0, math.pi / 3) obgl_start_homomat = rm.homomat_from_posrot(object_box_gl_pos, object_box_gl_rotmat) object_box.set_pos(object_box_gl_pos) object_box.set_rotmat(object_box_gl_rotmat) gm.gen_frame().attach_to(object_box) object_box_copy = object_box.copy() # object_box_copy.attach_to(base) # object box goal # object_box_gl_goal_pos = np.array([.6, -.1, .1]) # object_box_gl_goal_rotmat = rm.rotmat_from_euler(0, math.pi / 2, math.pi / 2) object_box_gl_goal_pos = np.array([.3, -.4, .1]) object_box_gl_goal_rotmat = rm.rotmat_from_euler(math.pi / 3, math.pi / 2, math.pi / 2) obgl_goal_homomat = rm.homomat_from_posrot(object_box_gl_goal_pos, object_box_gl_goal_rotmat)
ground.attach_to(base) # object holder object_holder = cm.CollisionModel("objects/holder.stl") object_holder.set_rgba([.5, .5, .5, 1]) object_holder_gl_pos = np.array([-.15, -.3, .0]) object_holder_gl_rotmat = np.eye(3) obgl_start_homomat = rm.homomat_from_posrot(object_holder_gl_pos, object_holder_gl_rotmat) object_holder.set_pos(object_holder_gl_pos) object_holder.set_rotmat(object_holder_gl_rotmat) gm.gen_frame().attach_to(object_holder) object_holder_copy = object_holder.copy() object_holder_copy.attach_to(base) # object holder goal object_holder_gl_goal_pos = np.array([.25, -.05, .05]) object_holder_gl_goal_rotmat = rm.rotmat_from_euler(0, 0, -math.pi / 2) obgl_goal_homomat = rm.homomat_from_posrot(object_holder_gl_goal_pos, object_holder_gl_goal_rotmat) object_holder_goal_copy = object_holder.copy() object_holder_goal_copy.set_homomat(obgl_goal_homomat) object_holder_goal_copy.attach_to(base) robot_s = cbt.Cobotta() # robot_s.gen_meshmodel().attach_to(base) rrtc_s = rrtc.RRTConnect(robot_s) ppp_s = ppp.PickPlacePlanner(robot_s) original_grasp_info_list = gpa.load_pickle_file('holder', './', 'cobg_holder_grasps.pickle') manipulator_name = "arm" hand_name = "hnd"
base = wd.World(cam_pos=[3, 1, 2], lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) # object object = cm.CollisionModel("./objects/bunnysim.stl") object.set_pos(np.array([.85, 0, .37])) object.set_rgba([.5, .7, .3, 1]) object.attach_to(base) # robot_s component_name = 'arm' robot_s = xss.XArmShuidi() robot_s.gen_meshmodel(toggle_tcpcs=True).attach_to(base) # base.run() seed_jnt_values = robot_s.get_jnt_values(component_name=component_name) for y in range(-5, 5): tgt_pos = np.array([.3, y * .1, 1]) tgt_rotmat = rm.rotmat_from_euler(0, math.pi / 2, 0) gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat).attach_to(base) tic = time.time() jnt_values = robot_s.ik(component_name=component_name, tgt_pos=tgt_pos, tgt_rotmat=tgt_rotmat, max_niter=500, toggle_debug=False, seed_jnt_values=seed_jnt_values) toc = time.time() print(f"time cost: {toc-tic}") seed_jnt_values = jnt_values if jnt_values is not None: robot_s.fk(component_name=component_name, jnt_values=jnt_values) robot_s.gen_meshmodel(toggle_tcpcs=True).attach_to(base) base.run()
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)
import numpy as np import modeling.geometric_model as gm import modeling.collision_model as cm import visualization.panda.world as wd import basis.robot_math as rm import math from scipy.spatial import cKDTree import vision.depth_camera.surface.rbf_surface as rbfs base = wd.World(cam_pos=np.array([-.3, -.7, .42]), lookat_pos=np.array([0, 0, 0])) # gm.gen_frame().attach_to(base) bowl_model = cm.CollisionModel(initor="./objects/bowl.stl") bowl_model.set_rgba([.3, .3, .3, .3]) bowl_model.set_rotmat(rm.rotmat_from_euler(math.pi, 0, 0)) # bowl_model.attach_to(base) pn_direction = np.array([0, 0, -1]) bowl_samples, bowl_sample_normals = bowl_model.sample_surface( toggle_option='normals', radius=.002) selection = bowl_sample_normals.dot(-pn_direction) > .1 bowl_samples = bowl_samples[selection] bowl_sample_normals = bowl_sample_normals[selection] tree = cKDTree(bowl_samples) surface = rbfs.RBFSurface(bowl_samples[:, :2], bowl_samples[:, 2]) surface.get_gometricmodel(rgba=[.3, .3, .3, .3]).attach_to(base) pt_direction = rm.orthogonal_vector(pn_direction, toggle_unit=True) tmp_direction = np.cross(pn_direction, pt_direction) plane_rotmat = np.column_stack((pt_direction, tmp_direction, pn_direction))
import os
tcp_loc_rotmat=None, toggle_tcpcs=False, toggle_jntscs=toggle_jntscs, rgba=rgba).attach_to(meshmodel) return meshmodel if __name__ == '__main__': import visualization.panda.world as wd import modeling.geometric_model as gm base = wd.World(cam_pos=[.5, .5, .5], lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) grpr = YumiGripper(enable_cc=True) grpr.fix_to(pos=np.array([0, .3, .2]), rotmat=rm.rotmat_from_euler(math.pi / 3, math.pi / 3, math.pi / 3)) grpr.jaw_to(.02) print(grpr.get_jawwidth()) grpr.gen_stickmodel().attach_to(base) grpr.gen_meshmodel(rgba=[0, .5, 0, .5]).attach_to(base) # grpr.gen_stickmodel(togglejntscs=False).attach_to(base) # grpr.fix_to(pos=np.array([0, .3, .2]), rotmat=rm.rotmat_from_axangle([1, 0, 0], math.pi/3)) grpr.fix_to(pos=np.zeros(3), rotmat=np.eye(3)) grpr.gen_meshmodel().attach_to(base) grpr2 = grpr.copy() grpr2.fix_to(pos=np.array([.3, .3, .2]), rotmat=rm.rotmat_from_axangle([0, 1, 0], .01)) model = grpr2.gen_meshmodel(rgba=[0.5, .5, 0, .5]) model.attach_to(base) grpr2.show_cdprimit()
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)
gm.gen_frame().attach_to(base) xav = XArmShuidi(enable_cc=True) xav.fk(component_name='all', jnt_values=np.array( [0, 0, 0, 0, 0, 0, math.pi, 0, -math.pi / 6, 0, 0])) xav.jaw_to(jawwidth=.08) tgt_pos = np.array([.55, 0, .55]) tgt_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2) gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat).attach_to(base) jnt_values = xav.ik(component_name='arm', tgt_pos=tgt_pos, tgt_rotmat=tgt_rotmat) print(jnt_values) tgt_pos2 = np.array([.45, 0, .07]) tgt_rotmat2 = rm.rotmat_from_euler(0, math.pi, 0) jnt_values2 = xav.ik(component_name='arm', tgt_pos=tgt_pos2, tgt_rotmat=tgt_rotmat2, seed_jnt_values=jnt_values, max_niter=10000) print(jnt_values2) xav.fk(component_name='arm', jnt_values=jnt_values) # xss.fk(component_name='agv', jnt_values=np.array([.2, -.5, math.radians(30)])) xav_meshmodel = xav.gen_meshmodel(toggle_tcpcs=True) xav_meshmodel.attach_to(base) # xss.show_cdprimit() xav.gen_stickmodel().attach_to(base) tic = time.time() result = xav.is_collided() toc = time.time()
import robot_sim.robots.nextage.nextage as nxt import motion.probabilistic.rrt_connect as rrtc base = wd.World(cam_pos=[4, -1, 2], lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) # object object_box = cm.gen_box(extent=[.15, .15, .15]) object_box.set_pos(np.array([.4, .3, .4])) object_box.set_rgba([.5, .7, .3, 1]) object_box.attach_to(base) # robot_s component_name = 'lft_arm_waist' robot_s = nxt.Nextage() start_pos = np.array([.4, 0, .2]) start_rotmat = rm.rotmat_from_euler(0, math.pi * 2 / 3, -math.pi / 4) start_conf = robot_s.ik(component_name, start_pos, start_rotmat) goal_pos = np.array([.3, .5, .7]) goal_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi) goal_conf = robot_s.ik(component_name, goal_pos, goal_rotmat) rrtc_planner = rrtc.RRTConnect(robot_s) path = rrtc_planner.plan(component_name=component_name, start_conf=start_conf, goal_conf=goal_conf, obstacle_list=[object_box], ext_dist=.05, smoothing_iterations=150, max_time=300) print(path) for pose in path[1:-2]: print(pose)
component_name, gl_jaw_center_pos, gl_jaw_center_rotmat, start_conf=robot_s.get_jnt_values(component_name), approach_direction=gl_jaw_center_rotmat[:, 2], approach_distance=.2) if conf_path is None: continue else: robot_s.fk(component_name, conf_path[-1]) rel_obj_pos, rel_obj_rotmat = robot_s.hold(object_box, jawwidth=jaw_width) # robot_s.gen_meshmodel().attach_to(base) # base.run() goal_jaw_center_pos = gl_jaw_center_pos + np.array([-.3, -.2, .3]) goal_jaw_center_rotmat = rm.rotmat_from_euler(math.pi / 2, math.pi / 2, math.pi / 2) jvs = robot_s.ik(component_name, goal_jaw_center_pos, goal_jaw_center_rotmat) robot_s.fk(component_name, jvs) # robot_s.gen_meshmodel().attach_to(base) # base.run() path = rrtc_s.plan(component_name, start_conf=conf_path[-1], goal_conf=jvs, obstacle_list=[], ext_dist=.1, rand_rate=70, max_time=300) for jvp in path: robot_s.fk(component_name, jvp) gl_obj_pos, gl_obj_rotmat = robot_s.cvt_loc_tcp_to_gl(
rel_pos = np.zeros(3) rel_rotmat = np.eye(3) if pressed_keys['r']: rel_pos = np.array([arm_linear_speed * .5, 0, 0]) elif pressed_keys['t']: rel_pos = np.array([-arm_linear_speed * .5, 0, 0]) elif pressed_keys['f']: rel_pos = np.array([0, arm_linear_speed * .5, 0]) elif pressed_keys['g']: rel_pos = np.array([0, -arm_linear_speed * .5, 0]) elif pressed_keys['v']: rel_pos = np.array([0, 0, arm_linear_speed * .5]) elif pressed_keys['b']: rel_pos = np.array([0, 0, -arm_linear_speed * .5]) elif pressed_keys['y']: rel_rotmat = rm.rotmat_from_euler(arm_angular_speed * .5, 0, 0) elif pressed_keys['u']: rel_rotmat = rm.rotmat_from_euler(-arm_angular_speed * .5, 0, 0) elif pressed_keys['h']: rel_rotmat = rm.rotmat_from_euler(0, arm_angular_speed * .5, 0) elif pressed_keys['j']: rel_rotmat = rm.rotmat_from_euler(0, -arm_angular_speed * .5, 0) elif pressed_keys['n']: rel_rotmat = rm.rotmat_from_euler(0, 0, arm_angular_speed * .5) elif pressed_keys['m']: rel_rotmat = rm.rotmat_from_euler(0, 0, -arm_angular_speed * .5) new_arm_tcp_pos = current_arm_tcp_pos + rel_pos new_arm_tcp_rotmat = rel_rotmat.dot(current_arm_tcp_rotmat) last_jnt_values = rbt_s.get_jnt_values() new_jnt_values = rbt_s.ik(tgt_pos=new_arm_tcp_pos, tgt_rotmat=new_arm_tcp_rotmat,
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(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()