def _toggle_jntcs(self,
parentmodel,
jntcs_thickness,
jntcs_length=None,
alpha=1):
"""
:param parentmodel: where to draw the frames to
:return:
author: weiwei
date: 20201125
"""
if jntcs_length is None:
jntcs_length = jntcs_thickness * 15
for id in self.jlobject.tgtjnts:
gm.gen_dashframe(pos=self.jlobject.jnts[id]['gl_pos0'],
rotmat=self.jlobject.jnts[id]['gl_rotmat0'],
length=jntcs_length,
thickness=jntcs_thickness,
alpha=alpha).attach_to(parentmodel)
gm.gen_frame(pos=self.jlobject.jnts[id]['gl_posq'],
rotmat=self.jlobject.jnts[id]['gl_rotmatq'],
length=jntcs_length,
thickness=jntcs_thickness,
alpha=alpha).attach_to(parentmodel)
gm.gen_frame(pos=sec2_spos, alpha=.2).attach_to(base)
# gm.gen_stick(spos=sec2_spos, epos=sec2_spos+sec_len, rgba=rgba, thickness=.015).attach_to(base)
# gm.gen_frame(pos=sec2_spos, rotmat=sec2_rotmat, alpha=.2).attach_to(base)
gm.gen_stick(spos=sec2_spos, epos=sec2_epos, rgba=rgba,
thickness=.015).attach_to(base)
# gm.gen_circarrow(axis=sec2_rotaxis, center=sec2_rotaxis / 13+sec2_spos, starting_vector=np.array([-0,-1,0]),radius=.025, portion=.5, thickness=.003, rgba=[1,.4,0,1]).attach_to(base)
#
sec2_rotaxis2 = np.array([1, 0, 0])
sec2_rotmat2 = rm.rotmat_from_axangle(sec2_rotaxis2, angle)
sec2_epos = sec2_spos + sec2_rotmat2.dot(sec2_epos - sec2_spos)
# # sec2_rotmat2 = rm.rotmat_from_axangle([1,0,0], math.pi/6)
# # sec2_epos2 = sec2_spos + sec2_rotmat.dot(np.array([.3, 0, 0]))
# # rgba = [1, .4, 0, .3]
# # gm.gen_stick(spos=sec1_spos, epos=sec1_epos, rgba=rgba, thickness=.015).attach_to(base)
# # gm.gen_dashframe(pos=sec2_spos).attach_to(base)
gm.gen_dashframe(pos=sec2_spos, rotmat=sec2_rotmat, alpha=.2).attach_to(base)
gm.gen_dashframe(pos=sec2_spos,
rotmat=sec2_rotmat2.dot(sec2_rotmat)).attach_to(base)
gm.gen_stick(spos=sec2_spos, epos=sec2_epos, rgba=rgba,
thickness=.015).attach_to(base)
gm.gen_circarrow(axis=sec2_rotaxis2,
center=sec2_rotaxis2 / 13 + sec2_spos,
starting_vector=np.array([0, 0, -1]),
radius=.025,
portion=.6,
thickness=.003,
rgba=[1, .4, 0, 1]).attach_to(base)
# #
# sec2_rotaxis3 = np.array([0, 1, 0])
# sec2_rotmat3 = rm.rotmat_from_axangle(sec2_rotaxis3, math.pi/3)
# sec2_epos = sec2_spos + sec2_rotmat3.dot(sec2_epos-sec2_spos)
import visualization.panda.world as wd import modeling.geometric_model as gm import basis.robot_math as rm import math import numpy as np base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0], toggle_debug=True) frame_o = gm.gen_frame(length=.2) frame_o.attach_to(base) # rotmat = rm.rotmat_from_axangle([1,1,1],math.pi/4) rotmat_a = rm.rotmat_from_euler(math.pi / 3, -math.pi / 6, math.pi / 3) # frame_a = gm.gen_mycframe(length=.2, rotmat=rotmat) frame_a = gm.gen_dashframe(length=.2, rotmat=rotmat_a) frame_a.attach_to(base) # point in a pos_a = np.array([.15, .07, .05]) # pos_start = rotmat_a.dot(pos_a) # pos_end = rotmat_a.dot(np.array([pos_a[0], pos_a[1], 0])) # # gm.gen_dashstick(pos_start, pos_end, thickness=.001, rgba=[0, 0, 0, .3], lsolid=.005, lspace=.005).attach_to(base) # gm.gen_stick(pos_start, pos_end, thickness=.001, rgba=[0, 0, 0, .3]).attach_to(base) # pos_start = rotmat_a.dot(np.array([pos_a[0], pos_a[1], 0])) # pos_end = rotmat_a.dot(np.array([pos_a[0], 0, 0])) # 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(pos_a) # pos_end = rotmat_a.dot(np.array([pos_a[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) # gm.gen_stick(pos_start, pos_end, thickness=.001, rgba=[0, 0, 0, .3]).attach_to(base) # pos_start = rotmat_a.dot(np.array([pos_a[0], 0, pos_a[2]]))
sec2_spos = sec1_epos
angle = math.pi / 5
sec2_rotaxis = np.array([0, 0, 1])
sec2_rotmat = rm.rotmat_from_axangle(sec2_rotaxis, angle)
sec2_epos = sec2_spos + sec2_rotmat.dot(sec_len)
rgba = [1, .4, 0, .3]
gm.gen_stick(spos=sec1_spos, epos=sec1_epos, rgba=rgba,
thickness=.015).attach_to(base)
gm.gen_frame(pos=sec2_spos, alpha=.2).attach_to(base)
gm.gen_stick(spos=sec2_spos,
epos=sec2_spos + sec_len,
rgba=rgba,
thickness=.015).attach_to(base)
gm.gen_dashframe(pos=sec2_spos, rotmat=sec2_rotmat).attach_to(base)
gm.gen_stick(spos=sec2_spos, epos=sec2_epos, rgba=rgba,
thickness=.015).attach_to(base)
gm.gen_circarrow(axis=sec2_rotaxis,
center=sec2_rotaxis / 13 + sec2_spos,
starting_vector=np.array([-0, -1, 0]),
radius=.025,
portion=.5,
thickness=.003,
rgba=[1, .4, 0, 1]).attach_to(base)
#
# sec2_rotaxis2 = np.array([1, 0, 0])
# sec2_rotmat2 = rm.rotmat_from_axangle(sec2_rotaxis2, math.pi/3)
# sec2_epos = sec2_spos + sec2_rotmat2.dot(sec2_epos-sec2_spos)
# # sec2_rotmat2 = rm.rotmat_from_axangle([1,0,0], math.pi/6)
# # sec2_epos2 = sec2_spos + sec2_rotmat.dot(np.array([.3, 0, 0]))
import visualization.panda.world as wd import modeling.geometric_model as gm import basis.robot_math as rm import math import numpy as np base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0], toggle_debug=True) frame_o = gm.gen_frame(length=.2) frame_o.attach_to(base) rotmat = rm.rotmat_from_euler(math.pi / 3, 0, 0) frame_a = gm.gen_dashframe(length=.2, rotmat=rotmat, lsolid=.06, lspace=.01) frame_a.attach_to(base) # gm.gen_circarrow(axis=np.array([1,0,0]), # portion = .9, # center = np.array([.1,0,0]), # radius=.03, # thickness=.003, # rgba=[.3,.3,.3,1]).attach_to(base) rotmat = rm.rotmat_from_euler(math.pi / 3, -math.pi / 6, 0) frame_a = gm.gen_dashframe(length=.2, rotmat=rotmat, lsolid=.025, lspace=.01) frame_a.attach_to(base) # gm.gen_circarrow(axis=np.array([0,-1,0]), # portion = .9, # center = np.array([0,.1,0]), # radius=.03, # thickness=.003, # rgba=[.3,.3,.3,1]).attach_to(base) rotmat = rm.rotmat_from_euler(math.pi / 3, -math.pi / 6, math.pi / 3) frame_a = gm.gen_dashframe(length=.2, rotmat=rotmat) frame_a.attach_to(base) gm.gen_circarrow(axis=np.array([0, 0, 1]),
import modeling.geometric_model as gm
import basis.robot_math as rm
import math
import numpy as np
base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0], toggle_debug=True)
gm.gen_frame(length=.2).attach_to(base)
gm.gen_torus(axis=[0, 0, 1],
portion=1,
radius=.2,
thickness=.003,
rgba=[1, 1, 0, 1],
sections=16,
discretization=64).attach_to(base)
rotmat = rm.rotmat_from_euler(math.pi / 3, -math.pi / 6, math.pi / 3)
gm.gen_dashframe(length=.2, rotmat=rotmat).attach_to(base)
gm.gen_dashtorus(axis=rotmat[:3, 2],
portion=1,
radius=.2,
thickness=.003,
rgba=[1, 1, 0, 1],
lspace=.007,
lsolid=.01,
sections=16,
discretization=64).attach_to(base)
# gm.gen_sphere(radius=.2, rgba=[.67,.67,.67,.9], subdivisions=5).attach_to(base)
#
# cross_vec = rm.unit_vector(np.cross(np.array([0,0,1]), rotmat[:3,2]))
# # gm.gen_dasharrow(epos=cross_vec*.2, rgba=[1,1,0,1], lsolid=.01, lspace=.0077).attach_to(base)
# gm.gen_arrow(epos=cross_vec*.2, rgba=[1,1,0,1]).attach_to(base)
# gm.gen_sphere(radius=.005, pos=cross_vec*.2, rgba=[0,0,0,1]).attach_to(base)