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()
