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='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_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 _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)
def showCoodinate(self, id):
axis1 = self.coordinate[2*id]
# print("check1")
# print(axis1, "11")
gm.gen_mycframe(rotmat=axis1, pos=self.origin[2*id], thickness=0.01).attach_to(base)
# axis2 = self.coordinate[2*id+1]
# gm.gen_frame(rotmat=axis2).attach_to(base)
print(self.endpairs[2*id][0],"cccc")
# gm.gen_sphere(pos=self.endpairs[2*id][0], radius=0.0051).attach_to(base)
# gm.gen_sphere(pos=self.endpairs[2 * id][1], radius=0.0051).attach_to(base)
# gm.gen_sphere(pos=self.temporigin[2 * id], radius=0.01).attach_to(base)
gm.gen_sphere(pos=self.origin[2 * id], radius=0.01).attach_to(base)
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()
jnt_values = jlinstance.ik(tgt_pos0,
tgt_rotmat0,
seed_jnt_values=None,
local_minima="accept",
toggle_debug=False)
toc = time.time()
print('ik cost: ', toc - tic, jnt_values)
jlinstance.fk(jnt_values=jnt_values)
jlinstance.gen_stickmodel(toggle_jntscs=True).attach_to(base)
component_name,
circle_center_pos,
circle_ax,
start_rotmat,
end_rotmat,
radius=radius,
toggle_tcp_list=True)
for i in range(len(tcp_list) - 1):
spos = tcp_list[i][0]
srotmat = tcp_list[i][1]
epos = tcp_list[i + 1][0]
erotmat = tcp_list[i + 1][1]
print(spos, epos)
gm.gen_dasharrow(spos, epos, thickness=.01, rgba=[1, 0, 0,
1]).attach_to(base)
gm.gen_mycframe(epos, erotmat, alpha=.7).attach_to(base)
# robot_s.fk(hnd_name, jnt_values_list[1])
# robot_s.gen_meshmodel(toggle_tcpcs=False, rgba=[.7,.3,.3,.57]).attach_to(base)
# robot_s.fk(hnd_name, jnt_values_list[2])
# robot_s.gen_meshmodel(toggle_tcpcs=False, rgba=[.3,.7,.3,.57]).attach_to(base)
# robot_s.fk(hnd_name, jnt_values_list[3])
# robot_s.gen_meshmodel(toggle_tcpcs=False, rgba=[.3,.3,.7,.57]).attach_to(base)
yumi_s.fk(component_name, jnt_values_list[0])
yumi_s.gen_meshmodel(toggle_tcpcs=True).attach_to(base)
# base.run()
x = np.arange(len(jnt_values_list))
print(x)
plt.figure(figsize=(10, 5))
plt.plot(x, jnt_values_list, '-o')
plt.xticks(x)
plt.show()
(q4, jnt_values[3]),
(q5, jnt_values[4]),
(q6, jnt_values[5])])
resultant_rotmat = np.array(resultant_rotmat.tolist()).astype(np.float64)
toc = time.time_ns()
print(f"sympy eval cost {toc - tic}")
tic = time.time_ns()
rbt_s.fk(jnt_values=jnt_values)
actual_pos, actual_rotmat = rbt_s.get_gl_tcp(manipulator_name='arm')
toc = time.time_ns()
print(f"direct fk cost {toc - tic}")
#
# print(resultant_rotmat, actual_rotmat)
print(resultant_pos, resultant_rotmat)
gm.gen_frame(pos=resultant_pos, rotmat=resultant_rotmat).attach_to(base)
gm.gen_mycframe(pos=actual_pos, rotmat=actual_rotmat).attach_to(base)
rbt_s.gen_meshmodel(toggle_tcpcs=True, toggle_jntscs=True, rgba=[.3, .3, .3, .3]).attach_to(base)
pos2 = pos2.subs([(q1, jnt_values[0]),
(q2, jnt_values[1])])
resultant_pos2 = np.array(pos2.tolist()).ravel().astype(np.float64)
gm.gen_sphere(pos=resultant_pos2).attach_to(base)
pos3 = pos3.subs([(q1, jnt_values[0]),
(q2, jnt_values[1]),
(q3, jnt_values[2])])
resultant_pos3 = np.array(pos3.tolist()).ravel().astype(np.float64)
gm.gen_sphere(pos=resultant_pos3).attach_to(base)
pos4 = pos4.subs([(q1, jnt_values[0]),
(q3, jnt_values[2]), (q4, jnt_values[3]),
(q5, jnt_values[4]),
(q6, jnt_values[5])])
resultant_rotmat = np.array(resultant_rotmat.tolist()).astype(np.float64)
toc = time.time_ns()
print(f"sympy eval cost {toc - tic}")
tic = time.time_ns()
rbt_s.fk(jnt_values=jnt_values)
actual_pos, actual_rotmat = rbt_s.get_gl_tcp(manipulator_name='arm')
toc = time.time_ns()
print(f"direct fk cost {toc - tic}")
#
# print(resultant_rotmat, actual_rotmat)
print(resultant_pos, resultant_rotmat)
gm.gen_frame(pos=resultant_pos, rotmat=resultant_rotmat).attach_to(base)
gm.gen_mycframe(pos=actual_pos, rotmat=actual_rotmat).attach_to(base)
rbt_s.gen_meshmodel(toggle_tcpcs=True,
toggle_jntscs=True,
rgba=[.3, .3, .3, .3]).attach_to(base)
pos2 = pos2.subs([(q1, jnt_values[0]), (q2, jnt_values[1])])
resultant_pos2 = np.array(pos2.tolist()).ravel().astype(np.float64)
gm.gen_sphere(pos=resultant_pos2).attach_to(base)
pos3 = pos3.subs([(q1, jnt_values[0]), (q2, jnt_values[1]),
(q3, jnt_values[2])])
resultant_pos3 = np.array(pos3.tolist()).ravel().astype(np.float64)
gm.gen_sphere(pos=resultant_pos3).attach_to(base)
pos4 = pos4.subs([(q1, jnt_values[0]), (q2, jnt_values[1]),
