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