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_outer.gen_meshmodel(toggle_tcpcs=False,
toggle_jntscs=toggle_jntscs,
rgba=rgba).attach_to(mm_collection)
self.lft_inner.gen_meshmodel(toggle_tcpcs=False,
toggle_jntscs=toggle_jntscs,
rgba=rgba).attach_to(mm_collection)
self.rgt_outer.gen_meshmodel(toggle_tcpcs=False,
toggle_jntscs=toggle_jntscs,
rgba=rgba).attach_to(mm_collection)
self.rgt_inner.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(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(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='robotiq140_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=False,
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_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 _toggle_tcpcs(self,
parent_model,
tcp_jntid,
tcp_loc_pos,
tcp_loc_rotmat,
tcpic_rgba,
tcpic_thickness,
tcpcs_thickness=None,
tcpcs_length=None):
"""
:param parent_model: where to draw the frames to
:param tcp_jntid: single id or a list of ids
:param tcp_loc_pos:
:param tcp_loc_rotmat:
:param tcpic_rgba: color that used to render the tcp indicator
:param tcpic_thickness: thickness the tcp indicator
:param tcpcs_thickness: thickness the tcp coordinate frame
:return:
author: weiwei
date: 20201125
"""
if tcp_jntid is None:
tcp_jntid = self.jlobject.tcp_jntid
if tcp_loc_pos is None:
tcp_loc_pos = self.jlobject.tcp_loc_pos
if tcp_loc_rotmat is None:
tcp_loc_rotmat = self.jlobject.tcp_loc_rotmat
if tcpcs_thickness is None:
tcpcs_thickness = tcpic_thickness
if tcpcs_length is None:
tcpcs_length = tcpcs_thickness * 15
tcp_gl_pos, tcp_gl_rotmat = self.jlobject.get_gl_tcp(
tcp_jntid, tcp_loc_pos, tcp_loc_rotmat)
if isinstance(tcp_gl_pos, list):
for i, jid in enumerate(tcp_jntid):
jgpos = self.jlobject.jnts[jid]['gl_posq']
gm.gen_dashstick(spos=jgpos,
epos=tcp_gl_pos[i],
thickness=tcpic_thickness,
rgba=tcpic_rgba,
type="round").attach_to(parent_model)
gm.gen_mycframe(pos=tcp_gl_pos[i],
rotmat=tcp_gl_rotmat[i],
length=tcpcs_length,
thickness=tcpcs_thickness,
alpha=tcpic_rgba[3]).attach_to(parent_model)
else:
jgpos = self.jlobject.jnts[tcp_jntid]['gl_posq']
gm.gen_dashstick(spos=jgpos,
epos=tcp_gl_pos,
thickness=tcpic_thickness,
rgba=tcpic_rgba,
type="round").attach_to(parent_model)
gm.gen_mycframe(pos=tcp_gl_pos,
rotmat=tcp_gl_rotmat,
length=tcpcs_length,
thickness=tcpcs_thickness,
alpha=tcpic_rgba[3]).attach_to(parent_model)
gm.gen_stick(spos=sec[0],
epos=sec[1],
rgba=[0, 0, 0, 1],
thickness=.002,
type='round').attach_to(base)
epos = (line_segs[0][1] - line_segs[0][0]) * .7 + line_segs[0][0]
gm.gen_arrow(spos=line_segs[0][0], epos=epos, thickness=0.004).attach_to(base)
spt = homomat[:3, 3]
# gm.gen_stick(spt, spt + pn_direction * 10, rgba=[0,1,0,1]).attach_to(base)
# base.run()
gm.gen_dasharrow(spt, spt - pn_direction * .07,
thickness=.004).attach_to(base) # p0
cpt, cnrml = bowl_model.ray_hit(spt,
spt + pn_direction * 10000,
option='closest')
gm.gen_dashstick(spt, cpt, rgba=[.57, .57, .57, .7],
thickness=0.003).attach_to(base)
gm.gen_sphere(pos=cpt, radius=.005).attach_to(base)
gm.gen_dasharrow(cpt, cpt - pn_direction * .07,
thickness=.004).attach_to(base) # p0
gm.gen_dasharrow(cpt, cpt + cnrml * .07, thickness=.004).attach_to(base) # p0
angle = rm.angle_between_vectors(-pn_direction, cnrml)
vec = np.cross(-pn_direction, cnrml)
rotmat = rm.rotmat_from_axangle(vec, angle)
new_plane_homomat = np.eye(4)
new_plane_homomat[:3, :3] = rotmat.dot(homomat[:3, :3])
new_plane_homomat[:3, 3] = cpt
twod_plane = gm.gen_box(np.array([.2, .2, .001]),
homomat=new_plane_homomat,
rgba=[1, 1, 1, .3])
twod_plane.attach_to(base)
# # gm.gen_dashstick(pos_start, pos_end, thickness=.001, rgba=[0, 0, 0, .3], lsolid=.005, lspace=.005).attach_to(base)
# pos_start = rotmat_a.dot(np.array([0, pos_a[1], pos_a[2]]))
# pos_end = rotmat_a.dot(np.array([0, 0, pos_a[2]]))
# gm.gen_dashstick(pos_start, pos_end, thickness=.001, rgba=[0,0,0,.3], lsolid=.005, lspace=.005).attach_to(base)
# cvt to sigma o
pos_o = rotmat_a.dot(pos_a)
# gm.gen_dashstick(pos_o, np.array([pos_o[0], pos_o[1], 0]), thickness=.001, rgba=[0,0,0,.3], lsolid=.005, lspace=.005).attach_to(base)
gm.gen_stick(pos_o,
np.array([pos_o[0], pos_o[1], 0]),
thickness=.001,
rgba=[0, 0, 0, .3]).attach_to(base)
gm.gen_dashstick(np.array([pos_o[0], pos_o[1], 0]),
np.array([pos_o[0], 0, 0]),
thickness=.001,
rgba=[0, 0, 0, .3],
lsolid=.005,
lspace=.005).attach_to(base)
# gm.gen_dashstick(pos_o, np.array([pos_o[0], 0, pos_o[2]]), thickness=.001, rgba=[0,0,0,.3], lsolid=.005, lspace=.005).attach_to(base)
gm.gen_stick(pos_o,
np.array([pos_o[0], 0, pos_o[2]]),
thickness=.001,
rgba=[0, 0, 0, .3]).attach_to(base)
gm.gen_dashstick(np.array([pos_o[0], 0, pos_o[2]]),
np.array([pos_o[0], 0, 0]),
thickness=.001,
rgba=[0, 0, 0, .3],
lsolid=.005,
lspace=.005).attach_to(base)
