def showmultiface(self, base, num):
length = 0.02
for i, facet in enumerate(self.pFaces[num]):
color = (random.random(), random.random(), random.random(), 1)
for j, face in enumerate(facet):
# color = (random.random(), random.random(), random.random(), 1)
gm.gen_sphere(face[0][0:3], radius=.005,
rgba=color).attach_to(base)
gm.gen_sphere(face[1][0:3], radius=.005,
rgba=color).attach_to(base)
gm.gen_sphere(face[2][0:3], radius=.005,
rgba=color).attach_to(base)
hufunc.drawanySingleSurface(base,
vertices=np.array(face),
color=color)
gm.gen_arrow(spos=np.array([
self.pFacetpairscenter[num][i][0],
self.pFacetpairscenter[num][i][1],
self.pFacetpairscenter[num][i][2]
]),
epos=np.array([
self.pFacetpairscenter[num][i][0] +
length * self.pNormals[num][i][0],
self.pFacetpairscenter[num][i][1] +
length * self.pNormals[num][i][1],
self.pFacetpairscenter[num][i][2] +
length * self.pNormals[num][i][2]
]),
thickness=0.005,
rgba=color)
def showsingleNormal(self, num):
color = (random.random(), random.random(), random.random(), 1)
# length = random.randint(60,100)
length = 0.05
self.__showSptPnt(self.holdingpairsSptPnt[num], color=color)
for i, facetpair in enumerate(self.hpairs[num]):
gm.gen_arrow(spos=np.array([
self.largefacetscenter[facetpair][0],
self.largefacetscenter[facetpair][1],
self.largefacetscenter[facetpair][2]
]),
epos=([
self.largefacetscenter[facetpair][0] +
length * self.largefacet_normals[facetpair][0],
self.largefacetscenter[facetpair][1] +
length * self.largefacet_normals[facetpair][1],
self.largefacetscenter[facetpair][2] +
length * self.largefacet_normals[facetpair][2]
]),
thickness=0.003,
rgba=color).attach_to(base)
for face in self.facets[facetpair]:
self.drawSingleFaceSurface(base,
vertices=self.vertices,
faces=self.faces[face],
color=color)
def update(textNode, obj_show_node, counter, task):
if base.inputmgr.keymap['space'] is True:
print("space")
# time.sleep(0.5)
if counter[0] >= len(freehold.placementRotMat):
counter[0] = 0
if obj_show_node[0] is not None:
# obj_show_node[0].detach()
obj_show_node[1].detachNode()
obj_show_node[2].detachNode()
obj_show_node[3].detachNode()
obj_show_node[0] = cm.CollisionModel(objpath)
obj_show_node[0].set_scale((0.001, 0.001, 0.001))
obj_show_node[0].set_rgba((0, 191 / 255, 1, 0.5))
obj_show_node[0].set_homomat(freehold.placementRotMat[counter[0]])
# obj_show_node[0].attach_to(base)
obj_show_node[1] = NodePath("com")
obj_show_node[2] = NodePath("vertice")
obj_show_node[3] = NodePath("normal")
if obj_show_node[0].is_mcdwith(fixturecm):
obj_show_node[0].set_rgba((1, 0, 0, 0.5))
else:
obj_show_node[0].attach_to(base)
# gm.gen_sphere(pos = freehold.placementCoM[counter[0]]).attach_to(obj_show_node[1])
obj_show_node[1].reparentTo(base.render)
# for vertace in freehold.placementVertices[counter[0]]:
# gm.gen_sphere(pos=vertace, radius=0.005).attach_to(obj_show_node[2])
obj_show_node[2].reparentTo(base.render)
for i, normal in enumerate(freehold.placementNormals[counter[0]]):
print(normal)
gm.gen_arrow(
spos=freehold.placementfacetpairscenter[counter[0]][i],
epos=freehold.placementfacetpairscenter[counter[0]][i] +
normal * 0.1).attach_to(obj_show_node[3])
obj_show_node[3].reparentTo(base.render)
counter[0] += 1
if textNode[0] is not None:
textNode[0].detachNode()
textNode[1].detachNode()
textNode[2].detachNode()
cam_pos = base.cam.getPos()
textNode[0] = OnscreenText(text=str(cam_pos[0])[0:5],
fg=(1, 0, 0, 1),
pos=(1.0, 0.8),
align=TextNode.ALeft)
textNode[1] = OnscreenText(text=str(cam_pos[1])[0:5],
fg=(0, 1, 0, 1),
pos=(1.3, 0.8),
align=TextNode.ALeft)
textNode[2] = OnscreenText(text=str(cam_pos[2])[0:5],
fg=(0, 0, 1, 1),
pos=(1.6, 0.8),
align=TextNode.ALeft)
return task.again
def showfeatures(self, node, normal, placementfacetpairscenter,
placementVertices, placementCoM, showCoM, showNormal,
showVertices):
np.random.seed(0)
rgb = np.random.rand(3)
# color = (random.random(),random.random(),random.random(),1)
color = (rgb[0], rgb[1], rgb[2], 1)
# length = random.randint(60,100)
length = 0.050
# self.__showSptPnt(self.holdingpairsSptPnt[i], color=color)
if showNormal == True:
for i in range(len(normal)):
gm.gen_arrow(
spos=np.array([
placementfacetpairscenter[i][0],
placementfacetpairscenter[i][1],
placementfacetpairscenter[i][2]
]),
epos=np.array([
placementfacetpairscenter[i][0] +
length * normal[i][0],
placementfacetpairscenter[i][1] +
length * normal[i][1],
placementfacetpairscenter[i][2] + length * normal[i][2]
]),
thickness=0.005,
rgba=color).attach_to(node)
if showCoM == True:
gm.gen_sphere(pos=(placementCoM[0], placementCoM[1],
placementCoM[2]),
radius=.003,
rgba=color).attach_to(node)
gm.gen_arrow(spos=np.array(
[placementCoM[0], placementCoM[1], placementCoM[2]]),
epos=np.array([
placementCoM[0], placementCoM[1],
placementCoM[2] - 0.100
]),
thickness=0.001,
rgba=color).attach_to(node)
if showVertices == True:
for vertices in placementVertices:
color2 = (random.random(), random.random(), random.random(), 1)
for i in range(len(vertices)):
gm.gen_sphere(pos=(vertices[i][0], vertices[i][1],
vertices[i][2]),
radius=.005,
rgba=color2).attach_to(node)
def showallNormal(self):
for i in range(len(self.hpairs)):
color = (random.random(), random.random(), random.random(), 1)
# length = random.randint(60,100)
length = 0.05
self.__showSptPnt(self.holdingpairsSptPnt[i], color=color)
for j, facetpair in enumerate(self.hpairs[i]):
gm.gen_arrow(
spos=np.array([
self.largefacetscenter[facetpair][0],
self.largefacetscenter[facetpair][1],
self.largefacetscenter[facetpair][2]
]),
epos=np.array([
self.largefacetscenter[facetpair][0] +
length * self.largefacet_normals[facetpair][0],
self.largefacetscenter[facetpair][1] +
length * self.largefacet_normals[facetpair][1],
self.largefacetscenter[facetpair][2] +
length * self.largefacet_normals[facetpair][2]
]),
thickness=0.01,
rgba=color).attach_to(base)
homomat[:3, 3] + pt_direction * .05,
homomat[:3, 3] + pt_direction * .05 + tmp_direction * .05
],
[
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
homomat[:3, 3] + pt_direction * .05,
homomat[:3, 3] + pt_direction * .05 + tmp_direction * .05
],
[
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
objcm1.set_rgba([1, 1, .3, .2])
objcm2 = objcm1.copy()
objcm2.set_pos(objcm1.get_pos() + np.array([.05, .02, .0]))
objcm1.change_cdmesh_type('convex_hull')
objcm2.change_cdmesh_type('obb')
iscollided, contact_points = is_collided(objcm1, objcm2)
objcm1.show_cdmesh()
objcm2.show_cdmesh()
objcm1.attach_to(base)
objcm2.attach_to(base)
print(iscollided)
for ctpt in contact_points:
gm.gen_sphere(ctpt, radius=.001).attach_to(base)
pfrom = np.array([0, 0, 0]) + np.array([1.0, 1.0, 1.0])
# pto = np.array([0, 0, 0]) + np.array([-1.0, -1.0, -1.0])
pto = np.array([0, 0, 0]) + np.array([0.02, 0.02, 0.02])
# pfrom = np.array([0, 0, 0]) + np.array([0.0, 0.0, 1.0])
# pto = np.array([0, 0, 0]) + np.array([0.0, 0.0, -1.0])
# hit_point, hit_normal = rayhit_closet(pfrom=pfrom, pto=pto, objcm=objcm1)
hit_points, hit_normals = rayhit_all(pfrom=pfrom, pto=pto, objcm=objcm1)
# objcm.attach_to(base)
# objcm.show_cdmesh(type='box')
# objcm.show_cdmesh(type='convex_hull')
# for hitpos, hitnormal in zip([hit_point], [hit_normal]):
for hitpos, hitnormal in zip(hit_points, hit_normals):
gm.gen_sphere(hitpos, radius=.003, rgba=np.array([0, 1, 1, 1])).attach_to(base)
gm.gen_arrow(hitpos, epos=hitpos+hitnormal*.03, thickness=.002, rgba=np.array([0, 1, 1, 1])).attach_to(base)
gm.gen_stick(spos=pfrom, epos=pto, thickness=.002).attach_to(base)
# gm.gen_arrow(spos=hitpos, epos=hitpos + hitnrml * .07, thickness=.002, rgba=np.array([0, 1, 0, 1])).attach_to(base)
base.run()
import motion.probabilistic.rrt_connect as rrtc
import math
import manipulation.pick_place_planner as ppp
import motion.probabilistic.rrt_differential_wheel_connect as rrtdwc
base = wd.World(cam_pos=[2, -2, 2], lookat_pos=[.0, 0, .3])
gm.gen_frame().attach_to(base)
ground = cm.gen_box(extent=[5, 5, 1], rgba=[.57, .57, .5, .7])
ground.set_pos(np.array([0, 0, -.5]))
ground.attach_to(base)
## show human
human = nxt.Nextage()
rotmat = rm.rotmat_from_axangle([0, 0, 1], math.pi / 2)
human.fix_to(np.array([0, -1.5, 1]), rotmat=rotmat)
gm.gen_arrow(spos=np.array([0, -1.5, 1]),
epos=np.array([0, -1.5, 0]),
thickness=0.01,
rgba=np.array([.5, 0, 0, 1])).attach_to(base)
human.gen_stickmodel().attach_to(base)
## show table2
table2_center = np.array([-1.3, .6, .483])
table2_bottom = cm.gen_box(extent=[1.08, .42, .06],
rgba=[150 / 255, 154 / 255, 152 / 255, 1])
rotmat = rm.rotmat_from_axangle([0, 0, 1], math.pi / 2)
table2_bottom.set_rotmat(rotmat)
table2_bottom.set_pos(table2_center - np.array([0, 0, .03]))
table2_bottom.attach_to(base)
table2_bottom2 = table2_bottom.copy()
table2_bottom2.set_pos(table2_center + np.array([0, 0, -.483 + .03]))
table2_bottom2.attach_to(base)
table2_top = table2_bottom.copy()
table2_top.set_pos(table2_center + np.array([0, 0, -.483 + 1 - .03]))
import visualization.panda.world as wd import modeling.geometric_model as gm import basis.robot_math as rm import math import numpy as np 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),
# max_samples=10000,
# min_dist_between_sampled_contact_points=.014,
# angle_between_contact_normals=math.radians(160),
# toggle_sampled_points=True)
# for p in contact_points:
# gm.gen_sphere(p, radius=.002).attach_to(base)
# base.run()
# pickle.dump(contact_pairs, open( "save.p", "wb" ))
contact_pairs = pickle.load(open("save.p", "rb"))
for i, cp in enumerate(contact_pairs):
contact_p0, contact_n0 = cp[0]
contact_p1, contact_n1 = cp[1]
rgba = rm.get_rgba_from_cmap(i)
gm.gen_sphere(contact_p0, radius=.002, rgba=rgba).attach_to(base)
gm.gen_arrow(contact_p0,
contact_p0 + contact_n0 * .01,
thickness=.0012,
rgba=rgba).attach_to(base)
# gm.gen_arrow(contact_p0, contact_p0-contact_n0*.1, thickness=.0012, rgba = rgba).attach_to(base)
gm.gen_sphere(contact_p1, radius=.002, rgba=rgba).attach_to(base)
# gm.gen_dashstick(contact_p0, contact_p1, thickness=.0012, rgba=rgba).attach_to(base)
gm.gen_arrow(contact_p1,
contact_p1 + contact_n1 * .01,
thickness=.0012,
rgba=rgba).attach_to(base)
# gm.gen_dasharrow(contact_p1, contact_p1+contact_n1*.03, thickness=.0012, rgba=rgba).attach_to(base)
# base.run()
gripper_s = rtq85.Robotiq85()
contact_offset = .002
grasp_info_list = []
for i, cp in enumerate(contact_pairs):
print(f"{i} of {len(contact_pairs)} done!")
def agv_move(task):
global onscreen, onscreen_agv, operation_count, pre_pos, pre_angle
# agv_linear_speed = .002
agv_linear_speed = 0
agv_angular_speed = .5
arm_linear_speed = .03
arm_angular_speed = .1
for item in onscreen:
item.detach()
onscreen.clear()
for item in onscreen_agv:
item.detach()
onscreen_agv.clear()
agv_pos = rbt_s.get_jnt_values("agv")
agv_loc_rotmat = rm.rotmat_from_axangle(axis=[0, 0, 1],
angle=-1 * agv_pos[2])
print(agv_pos)
agv_direction = np.dot(np.array([1, 0, 0]), agv_loc_rotmat)
# print(agv_direction)
# onscreen_agv.append(gm.gen_dasharrow(spos=np.array([agv_pos[0], agv_pos[1], 0]), epos=np.array([agv_pos[0], agv_pos[1], 0]))+np.array(agv_direction))
onscreen_agv.append(
gm.gen_arrow(epos=np.array(
[agv_direction[0], agv_direction[1], agv_direction[2]]),
rgba=[1, 0, 1, 1]))
# onscreen_agv.append(gm.gen_frame(pos=np.array([agv_pos[0], agv_pos[1], 0]), rotmat=agv_loc_rotmat, length=3))
onscreen_agv[-1].attach_to(base)
# print(agv_pos)
pressed_keys = {
'w': keyboard.is_pressed('w'),
'a': keyboard.is_pressed('a'),
's': keyboard.is_pressed('s'),
'd': keyboard.is_pressed('d'),
'r': keyboard.is_pressed('r'), # x+ global
't': keyboard.is_pressed('t'), # x- global
'f': keyboard.is_pressed('f'), # y+ global
'g': keyboard.is_pressed('g'), # y- global
'v': keyboard.is_pressed('v'), # z+ global
'b': keyboard.is_pressed('b'), # z- gglobal
'y': keyboard.is_pressed('y'), # r+ global
'u': keyboard.is_pressed('u'), # r- global
'h': keyboard.is_pressed('h'), # p+ global
'j': keyboard.is_pressed('j'), # p- global
'n': keyboard.is_pressed('n'), # yaw+ global
'm': keyboard.is_pressed('m'), # yaw- global
'o': keyboard.is_pressed('o'), # gripper open
'p': keyboard.is_pressed('p')
} # gripper close
values_list = list(pressed_keys.values())
if pressed_keys["w"] and pressed_keys["a"]:
print("Invalid Operation!!")
# rbt_x.agv_move(linear_speed=agv_linear_speed, angular_speed=agv_angular_speed, time_interval=.5)
elif pressed_keys["w"] and pressed_keys["d"]:
print("Invalid Operation!!")
# rbt_x.agv_move(linear_speed=agv_linear_speed, angular_speed=-agv_angular_speed, time_interval=.5)
elif pressed_keys["s"] and pressed_keys["a"]:
print("Invalid Operation!!")
# rbt_x.agv_move(linear_speed=-agv_linear_speed, angular_speed=-agv_angular_speed, time_interval=.5)
elif pressed_keys["s"] and pressed_keys["d"]:
print("Invalid Operation!!")
# rbt_x.agv_move(linear_speed=-agv_linear_speed, angular_speed=agv_angular_speed, time_interval=.5)
elif pressed_keys["w"] and sum(values_list) == 1: # if key 'q' is pressed
rbt_s.fk(
component_name='agv',
jnt_values=np.array(
pre_pos +
[agv_linear_speed, 0, math.radians(0.5)]))
pre_pos = np.array(pre_pos + [agv_linear_speed, 0, math.radians(0.5)])
# rbt_x.agv_move(linear_speed=agv_linear_speed, angular_speed=0, time_interval=.5)
elif pressed_keys["s"] and sum(values_list) == 1: # if key 'q' is pressed
rbt_s.fk(
component_name='agv',
jnt_values=np.array(pre_pos +
[-1 * agv_linear_speed, 0,
math.radians(-0.5)]))
pre_pos = np.array(pre_pos +
[-1 * agv_linear_speed, 0,
math.radians(-0.5)])
# rbt_x.agv_move(linear_speed=-agv_linear_speed, angular_speed=0, time_interval=.5)
elif pressed_keys["a"] and sum(values_list) == 1: # if key 'q' is pressed
rbt_s.fk(
component_name='agv',
jnt_values=np.array(
pre_pos +
[0, agv_linear_speed, math.radians(0)]))
pre_pos = np.array(pre_pos + [0, agv_linear_speed, math.radians(0)])
# rbt_x.agv_move(linear_speed=0, angular_speed=agv_angular_speed, time_interval=.5)
elif pressed_keys["d"] and sum(values_list) == 1: # if key 'q' is pressed
rbt_s.fk(
component_name='agv',
jnt_values=np.array(pre_pos +
[0, -1 * agv_linear_speed,
math.radians(0)]))
pre_pos = np.array(pre_pos +
[0, -1 * agv_linear_speed,
math.radians(0)])
# rbt_x.agv_move(linear_speed=0, angular_speed=-agv_angular_speed, time_interval=.5)
elif pressed_keys["o"] and sum(values_list) == 1: # if key 'q' is pressed
print("Invalid Operation!!")
# rbt_x.arm_jaw_to(jawwidth=100)
elif pressed_keys["p"] and sum(values_list) == 1: # if key 'q' is pressed
print("Invalid Operation!!")
# rbt_x.arm_jaw_to(jawwidth=0)
elif any(pressed_keys[item] for item in ['r', 't', 'f', 'g', 'v', 'b', 'y', 'u', 'h', 'j', 'n', 'm']) and\
sum(values_list) == 1: # global
tic = time.time()
current_jnt_values = rbt_s.get_jnt_values()
current_arm_tcp_pos, current_arm_tcp_rotmat = rbt_s.get_gl_tcp()
rel_pos = np.zeros(3)
rel_rotmat = np.eye(3)
if pressed_keys['r']:
rel_pos = np.array([arm_linear_speed * .5, 0, 0])
elif pressed_keys['t']:
rel_pos = np.array([-arm_linear_speed * .5, 0, 0])
elif pressed_keys['f']:
rel_pos = np.array([0, arm_linear_speed * .5, 0])
elif pressed_keys['g']:
rel_pos = np.array([0, -arm_linear_speed * .5, 0])
elif pressed_keys['v']:
rel_pos = np.array([0, 0, arm_linear_speed * .5])
elif pressed_keys['b']:
rel_pos = np.array([0, 0, -arm_linear_speed * .5])
elif pressed_keys['y']:
rel_rotmat = rm.rotmat_from_euler(arm_angular_speed * .5, 0, 0)
elif pressed_keys['u']:
rel_rotmat = rm.rotmat_from_euler(-arm_angular_speed * .5, 0, 0)
elif pressed_keys['h']:
rel_rotmat = rm.rotmat_from_euler(0, arm_angular_speed * .5, 0)
elif pressed_keys['j']:
rel_rotmat = rm.rotmat_from_euler(0, -arm_angular_speed * .5, 0)
elif pressed_keys['n']:
rel_rotmat = rm.rotmat_from_euler(0, 0, arm_angular_speed * .5)
elif pressed_keys['m']:
rel_rotmat = rm.rotmat_from_euler(0, 0, -arm_angular_speed * .5)
new_arm_tcp_pos = current_arm_tcp_pos + rel_pos
new_arm_tcp_rotmat = rel_rotmat.dot(current_arm_tcp_rotmat)
last_jnt_values = rbt_s.get_jnt_values()
new_jnt_values = rbt_s.ik(tgt_pos=new_arm_tcp_pos,
tgt_rotmat=new_arm_tcp_rotmat,
seed_jnt_values=current_jnt_values)
# if new_jnt_values is None:
# continue
rbt_s.fk(jnt_values=new_jnt_values)
toc = time.time()
start_frame_id = math.ceil((toc - tic) / .01)
# rbt_x.arm_move_jspace_path([last_jnt_values, new_jnt_values], time_interval=.1, start_frame_id=start_frame_id)
onscreen.append(rbt_s.gen_meshmodel())
onscreen[-1].attach_to(base)
# print(pre_pos)
# print(onscreen)
operation_count += 1
# time.sleep(1/30)
return task.cont
tmp_gm.set_rgba(rm.random_rgba())
# edge
edge_list = (np.array(seg_nested_edge_list[i]) + offset_pos).tolist()
gm.gen_linesegs(edge_list, thickness=.05, rgba=[1, 0, 0,
1]).attach_to(base)
# seed segment
tmp_trm = tg.extract_subtrimesh(bunnycm.objtrm, facet_seed_list[i],
offset_pos)
tmp_gm = gm.StaticGeometricModel(tmp_trm, btwosided=True)
tmp_gm.attach_to(base)
tmp_gm.set_rgba([1, 0, 0, 1])
# face center and normal
seed_center = np.mean(tmp_trm.vertices, axis=0)
gm.gen_sphere(pos=seed_center, radius=.001).attach_to(base)
gm.gen_arrow(spos=seed_center,
epos=seed_center + tmp_trm.face_normals[0] * .01,
thickness=.0006).attach_to(base)
for face_id_for_curvature in face_id_pair_list_for_curvature[i]:
rgba = [1, 1, 1, 1]
tmp_trm = tg.extract_subtrimesh(bunnycm.objtrm,
face_id_for_curvature, offset_pos)
tmp_gm = gm.StaticGeometricModel(tmp_trm, btwosided=True)
tmp_gm.attach_to(base)
tmp_gm.set_rgba(rgba)
seed_center = np.mean(tmp_trm.vertices, axis=0)
gm.gen_sphere(pos=seed_center, radius=.001,
rgba=rgba).attach_to(base)
gm.gen_arrow(spos=seed_center,
epos=seed_center + tmp_trm.face_normals[0] * .01,
thickness=.0006,
rgba=rgba).attach_to(base)
homomat = np.eye(4)
homomat[:3, :3] = plane_rotmat
homomat[:3, 3] = np.array([-.07, -.03, .1])
twod_plane = gm.gen_box(np.array([.2, .2, .001]),
homomat=homomat,
rgba=[1, 1, 1, .3])
twod_plane.attach_to(base)
circle_radius = .05
line_segs = [[homomat[:3, 3], homomat[:3, 3] + pt_direction * .05],
[
homomat[:3, 3] + pt_direction * .05,
homomat[:3, 3] + pt_direction * .05 + tmp_direction * .05
]]
gm.gen_linesegs(line_segs).attach_to(base)
gm.gen_arrow(spos=line_segs[0][0], epos=line_segs[0][1],
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
# 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)
# #
# nxt_vec_uvw = rotmat.dot(cross_vec)
# # gm.gen_dasharrow(epos=nxt_vec_uvw*.2, rgba=[1,1,0,1], lsolid=.015, lspace=.007).attach_to(base)
# gm.gen_arrow(epos=nxt_vec_uvw*.2, rgba=[1,1,0,1]).attach_to(base)
# gm.gen_sphere(radius=.005, pos=nxt_vec_uvw*.2, rgba=[0,0,0,1]).attach_to(base)
# #
# pre_vec_xyz = rotmat.T.dot(cross_vec)
# gm.gen_arrow(epos=pre_vec_xyz*.2, rgba=[1,1,0,1]).attach_to(base)
# gm.gen_sphere(radius=.005, pos=pre_vec_xyz*.2, rgba=[0,0,0,1]).attach_to(base)
#
ax, angle = rm.axangle_between_rotmat(np.eye(3), rotmat)
gm.gen_arrow(epos=ax * .4, rgba=[0, 0, 0, 1]).attach_to(base)
for step_angle in np.linspace(0, angle, 10).tolist():
rotmat = rm.rotmat_from_axangle(ax, step_angle)
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)
# radius, _ = rm.unit_vector(cross_vec*.2-cross_vec.dot(ax)*ax*.2, toggle_length=True)
