rotmat = rm.rotmat_from_euler(math.pi / 3, -math.pi / 6, math.pi / 3) ax, angle = rm.axangle_between_rotmat(np.eye(3), rotmat) rotmat2 = rm.rotmat_from_axangle(ax, math.pi/6) # cross_vec = rm.unit_vector(np.cross(np.array([0.1,0,1]), rotmat[:3,2])) cross_vec = rotmat2.dot(rotmat[:3,0]) base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, 0], toggle_debug=True) # base = wd.World(cam_pos=ax*2, lookat_pos=[0, 0, 0], toggle_debug=True) gm.gen_arrow(epos=ax*.3, rgba=[0,0,0,.3]).attach_to(base) gm.gen_frame(length=.2).attach_to(base) # gm.gen_dasharrow(epos=cross_vec*.3, rgba=[1,1,0,1], lsolid=.01, lspace=.0077).attach_to(base) gm.gen_arrow(epos=cross_vec*.3, rgba=[1,1,0,1]).attach_to(base) gm.gen_sphere(radius=.005, pos=cross_vec*.3, rgba=[0,0,0,1]).attach_to(base) nxt_vec_uvw = rotmat2.dot(cross_vec) gm.gen_dasharrow(epos=nxt_vec_uvw*.3, rgba=[1,1,0,1]).attach_to(base) # gm.gen_arrow(epos=nxt_vec_uvw*.3, rgba=[1,1,0,1]).attach_to(base) gm.gen_sphere(radius=.005, pos=nxt_vec_uvw*.3, rgba=[0,0,0,1]).attach_to(base) radius, _ = rm.unit_vector(cross_vec * .3 - cross_vec.dot(ax) * ax * .3, toggle_length=True) gm.gen_arrow(spos=cross_vec.dot(ax)*ax*.3, epos = cross_vec*.3, rgba=[1,.47,0,.5]).attach_to(base) gm.gen_dasharrow(spos=cross_vec.dot(ax)*ax*.3, epos = nxt_vec_uvw*.3, rgba=[1,.47,0,.5]).attach_to(base) gm.gen_arrow(epos=ax*math.sqrt(.3**2-radius**2), rgba=[0,0,0,1]).attach_to(base) ## projections # gm.gen_dasharrow(spos = ax*math.sqrt(.3**2-radius**2), # epos=ax*math.sqrt(.3**2-radius**2)+np.cross(ax, cross_vec*.3)*math.sin(math.pi/6), # rgba=[1,0,1,.5]).attach_to(base) # gm.gen_dasharrow(spos = ax*math.sqrt(.3**2-radius**2), # epos=ax*math.sqrt(.3**2-radius**2)+(cross_vec*.3-cross_vec.dot(ax)*ax*.3)*math.cos(math.pi/6), # rgba=[0,1,1,.5]).attach_to(base)
homomat[:3, 3] + pt_direction * .05 + tmp_direction * .05,
homomat[:3, 3] + tmp_direction * .05
], [homomat[:3, 3] + tmp_direction * .05, homomat[:3, 3]]]
# gm.gen_linesegs(line_segs).attach_to(base)
for sec in line_segs:
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])
end_rotmat = start_rotmat
jnt_values_list, tcp_list = inik_svlr.gen_circular_motion(
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)