def show(self, thickness = 0.0003):
for y in range(self.wid_num):
for x in range(self.len_num):
gm.gen_sphere(pos = self.position_matrix[y][x], radius = 0.0006, rgba=(0,0,0,1)).attach_to(base)
if x == self.len_num - 1 and y < self.wid_num - 1:
print("check")
gm.gen_stick(self.position_matrix[x][y], self.position_matrix[x][y + 1],
thickness=thickness).attach_to(base)
elif y == self.wid_num - 1 and x < self.len_num-1:
gm.gen_stick(self.position_matrix[x][y], self.position_matrix[x + 1][y],
thickness=thickness).attach_to(base)
elif x < self.len_num-1 and y < self.wid_num - 1:
gm.gen_stick(self.position_matrix[x][y], self.position_matrix[x+1][y], thickness=thickness).attach_to(
base)
gm.gen_stick(self.position_matrix[x][y], self.position_matrix[x][y+1], thickness=thickness).attach_to(
base)
def gen_stick(spos=np.array([.0, .0, .0]),
epos=np.array([.0, .0, .1]),
thickness=.005, type="round",
rgba=[1, 0, 0, 1],
sections=8):
"""
:param spos:
:param epos:
:param thickness:
:param rgba:
:return:
author: weiwei
date: 20210328
"""
stick_sgm = gm.gen_stick(spos=spos, epos=epos, thickness=thickness, type=type, rgba=rgba, sections=sections)
stick_cm = CollisionModel(stick_sgm)
return stick_cm
base = wd.World(cam_pos=np.array([.7, -.7, 1]),
lookat_pos=np.array([0.3, 0, 0]))
gm.gen_frame().attach_to(base)
sec1_spos = np.array([0, 0, 0])
sec_len = np.array([.2, 0, 0])
sec1_epos = sec1_spos + sec_len
sec2_spos = sec1_epos
angle = math.pi / 5.7
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_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)
if goal_joint_values_attachment is not None:
rbt_s.fk(component_name=component_name, jnt_values=goal_joint_values_attachment)
rbt_s.gen_meshmodel().attach_to(base)
path = planner.plan(component_name=component_name,
start_conf=init_joint_values,
goal_conf=goal_joint_values_attachment,
ext_dist=.02)
n_path = len(path)
previous_pos = None
for id, jnt_values in enumerate(path):
rbt_s.fk(component_name=component_name, jnt_values=jnt_values)
rgba = rm.get_rgba_from_cmap(int(id * 256 / n_path), cm_name='cool', step=256)
rgba[-1] = .3
pos, rotmat = rbt_s.get_gl_tcp(manipulator_name=component_name)
if previous_pos is not None:
gm.gen_stick(previous_pos, pos, rgba=rgba).attach_to(base)
previous_pos = pos
# rbt_s.gen_meshmodel(rgba=rgba).attach_to(base)
# interpolated_confs = \
# traj_gen.interpolate_by_max_spdacc(path,
# control_frequency=.008,
# max_vels=max_vels)
# n_path = len(interpolated_confs)
# print(n_path)
# previous_pos = None
# for id, jnt_values in enumerate(interpolated_confs):
# rbt_s.fk(component_name=component_name, jnt_values=jnt_values)
# rgba = rm.get_rgba_from_cmap(int(id * 256 / n_path), cm_name='cool', step=256)
# # rgba[-1] = 1
# pos, rotmat = rbt_s.get_gl_tcp(manipulator_name=component_name)
# # rgbmatrix = np.eye(3)
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
],
[
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)
def update(pk_obj, pcd_list, ball_center_list, counter, task):
if len(pcd_list) != 0:
for pcd in pcd_list:
pcd.detach()
pcd_list.clear()
# if len(ball_center_list) != 0:
# for ball_center in ball_center_list:
# ball_center.detach()
# ball_center_list.clear()
while True:
pk_obj.device_get_capture()
color_image_handle = pk_obj.capture_get_color_image()
depth_image_handle = pk_obj.capture_get_depth_image()
if color_image_handle and depth_image_handle:
break
point_cloud = pk_obj.transform_depth_image_to_point_cloud(
depth_image_handle)
point_cloud = rm.homomat_transform_points(affine_matrix, point_cloud)
ball = []
for id, point_cloud_sub in enumerate(point_cloud):
if 0.3 < point_cloud_sub[0] < 3.3 and -1.3 < point_cloud_sub[
1] < .3 and 0.5 < point_cloud_sub[2] < 2.5:
ball.append(point_cloud_sub)
mypoint_cloud = gm.GeometricModel(initor=point_cloud)
mypoint_cloud.attach_to(base)
pcd_list.append(mypoint_cloud)
if len(ball) > 20:
center = np.mean(np.array(ball), axis=0)
ball_center_list.append([center, counter[0]])
ball = gm.gen_sphere(center, radius=.1)
ball.attach_to(base)
ball_list.append(ball)
if len(ball_center_list) > 2:
x = []
y = []
z = []
t = []
for cen, f_id in ball_center_list:
x.append(cen[0])
y.append(cen[1])
z.append(cen[2])
t.append(f_id)
para_x = np.polyfit(t, x, 1)
para_y = np.polyfit(t, y, 1)
para_z = np.polyfit(t, z, 2)
f_x = np.poly1d(para_x)
f_y = np.poly1d(para_y)
f_z = np.poly1d(para_z)
orbit = []
for t in np.linspace(ball_center_list[0][1],
ball_center_list[0][1] + 5, 100):
orbit.append(np.array([f_x(t), f_y(t), f_z(t)]))
for id in range(len(orbit)):
if id > 0:
tmp_stick = gm.gen_stick(spos=orbit[id - 1],
epos=orbit[id],
thickness=.01,
type="round")
tmp_stick.attach_to(base)
para_list.append(tmp_stick)
return task.done
pk_obj.image_release(color_image_handle)
pk_obj.image_release(depth_image_handle)
pk_obj.capture_release()
counter[0] += 1
return task.cont
import numpy as np import basis.robot_math as rm import visualization.panda.world as wd import modeling.geometric_model as gm leaf_rgba = [45 / 255, 90 / 255, 39 / 255, 1] stem_rgba = [97 / 255, 138 / 255, 61 / 255, 1] base = wd.World(cam_pos=[1, 1, .7]) sb_leaf = gm.GeometricModel(initor="objects/soybean_leaf.stl") sb_leaf.set_rgba(rgba=leaf_rgba) stem0_spos = np.array([0, 0, 0]) stem0_epos = np.array([0, 0, .05]) stem0 = gm.gen_stick(spos=stem0_spos, epos=stem0_epos, rgba=stem_rgba) stem0.attach_to(base) sbl0 = sb_leaf.copy() sbl0.set_pos(stem0_epos) sbl0.attach_to(base) sbl1 = sb_leaf.copy() sbl1.set_pos(stem0_epos - np.array([0, 0, 0.005])) sbl1.set_rotmat(rm.rotmat_from_axangle([0, 0, 1], np.pi)) sbl1.attach_to(base) base.run()
def gen_stickmodel(self,
rgba=np.array([.5, 0, 0, 1]),
thickness=.01,
joint_ratio=1.62,
link_ratio=.62,
tcp_jntid=None,
tcp_loc_pos=None,
tcp_loc_rotmat=None,
toggle_tcpcs=True,
toggle_jntscs=False,
toggle_connjnt=False,
name='robotstick'):
"""
generate the stick model for a jntlnk object
snp means stick nodepath
:param rgba:
:param tcp_jntid:
:param tcp_loc_pos:
:param tcp_loc_rotmat:
:param toggle_tcpcs:
:param toggle_jntscs:
:param toggle_connjnt: draw the connecting joint explicitly or not
:param name:
:return:
author: weiwei
date: 20200331, 20201006
"""
stickmodel = mc.ModelCollection(name=name)
id = 0
loopdof = self.jlobject.ndof + 1
if toggle_connjnt:
loopdof = self.jlobject.ndof + 2
while id < loopdof:
cjid = self.jlobject.jnts[id]['child']
jgpos = self.jlobject.jnts[id]['gl_posq'] # joint global pos
cjgpos = self.jlobject.jnts[cjid][
'gl_pos0'] # child joint global pos
jgmtnax = self.jlobject.jnts[id][
"gl_motionax"] # joint global rot ax
gm.gen_stick(spos=jgpos,
epos=cjgpos,
thickness=thickness,
type="rect",
rgba=rgba).attach_to(stickmodel)
if id > 0:
if self.jlobject.jnts[id]['type'] == "revolute":
gm.gen_stick(spos=jgpos - jgmtnax * thickness,
epos=jgpos + jgmtnax * thickness,
type="rect",
thickness=thickness * joint_ratio,
rgba=np.array([.3, .3, .2, rgba[3]
])).attach_to(stickmodel)
if self.jlobject.jnts[id]['type'] == "prismatic":
jgpos0 = self.jlobject.jnts[id]['gl_pos0']
gm.gen_stick(spos=jgpos0,
epos=jgpos,
type="round",
thickness=thickness * joint_ratio,
rgba=np.array([.2, .3, .3, rgba[3]
])).attach_to(stickmodel)
id = cjid
# tool center coord
if toggle_tcpcs:
self._toggle_tcpcs(stickmodel,
tcp_jntid,
tcp_loc_pos,
tcp_loc_rotmat,
tcpic_rgba=rgba + np.array([0, 0, 1, 0]),
tcpic_thickness=thickness * link_ratio)
# toggle all coord
if toggle_jntscs:
self._toggle_jntcs(stickmodel,
jntcs_thickness=thickness * link_ratio,
alpha=rgba[3])
return stickmodel
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()
# pos_start = rotmat_a.dot(np.array([pos_a[0], 0, 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)
# # pos_start = rotmat_a.dot(pos_a)
# # pos_end = rotmat_a.dot(np.array([0, pos_a[1], pos_a[2]]))
# # 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]),
ee_s = cbtg.CobottaPipette()
id_x = 7
id_y = 11
tip_pos, tip_rotmat = tip_rack.get_rack_hole_pose(id_x=id_x, id_y=id_y)
z_offset = np.array([0, 0, .02])
base.change_campos_and_lookat_pos(cam_pos=[.4, .0, .15],
lookat_pos=tip_pos + z_offset)
spiral_points = rm.gen_3d_equilateral_verts(pos=tip_pos + z_offset,
rotmat=tip_rotmat)
print(spiral_points)
pre_point = None
for point in spiral_points:
gm.gen_sphere(point, radius=.00016).attach_to(base)
if pre_point is not None:
gm.gen_stick(pre_point, point, thickness=.00012).attach_to(base)
pre_point = point
goal_joint_values_attachment = utils.search_reachable_configuration(
rbt_s=rbt_s,
ee_s=ee_s,
component_name=component_name,
tgt_pos=spiral_points[0],
cone_axis=-tip_rotmat[:3, 2],
rotation_interval=np.radians(15),
obstacle_list=[frame_bottom],
toggle_debug=False)
if goal_joint_values_attachment is not None:
rbt_s.fk(component_name=component_name,
jnt_values=goal_joint_values_attachment)
rbt_s.gen_meshmodel(rgba=[1, 1, 1, .9]).attach_to(base)
def update(pkx, rbtx, background_image, pcd_list, ball_center_list, counter,
task):
if len(pcd_list) != 0:
for pcd in pcd_list:
pcd.detach()
pcd_list.clear()
while True:
pkx.device_get_capture()
color_image_handle = pkx.capture_get_color_image()
depth_image_handle = pkx.capture_get_depth_image()
if color_image_handle and depth_image_handle:
break
point_cloud = pkx.transform_depth_image_to_point_cloud(depth_image_handle)
point_cloud = rm.homomat_transform_points(affine_matrix, point_cloud)
ball = []
for id, point_cloud_sub in enumerate(point_cloud):
if 0.3 < point_cloud_sub[0] < 3.3 and -1.3 < point_cloud_sub[
1] < .3 and 0.5 < point_cloud_sub[2] < 2.5:
ball.append(point_cloud_sub)
mypoint_cloud = gm.GeometricModel(initor=point_cloud)
mypoint_cloud.attach_to(base)
pcd_list.append(mypoint_cloud)
if len(ball) > 20:
center = np.mean(np.array(ball), axis=0)
ball_center_list.append([center, counter[0]])
ball = gm.gen_sphere(center, radius=.1)
ball.attach_to(base)
ball_list.append(ball)
if len(ball_center_list) > 2:
x = []
y = []
z = []
t = []
for cen, f_id in ball_center_list:
x.append(cen[0])
y.append(cen[1])
z.append(cen[2])
t.append(f_id)
para_x = np.polyfit(t, x, 1)
para_y = np.polyfit(t, y, 1)
para_z = np.polyfit(t, z, 2)
f_x = np.poly1d(para_x)
f_y = np.poly1d(para_y)
f_z = np.poly1d(para_z)
orbit = []
for t in np.linspace(ball_center_list[0][1],
ball_center_list[0][1] + 15, 100):
point = np.array([f_x(t), f_y(t), f_z(t)])
orbit.append(point)
if abs(point[0]) < .7 and abs(
point[1]) < .7 and abs(point[2] - 1.1) < .3:
# last_point = orbit[-1]
jnt_values = rbts.ik(tgt_pos=point,
tgt_rotmat=np.array([[-1, 0, 0],
[0, -1, 0],
[0, 0, 1]]))
if jnt_values is None:
continue
rbts.fk(component_name="arm", jnt_values=jnt_values)
rbts.gen_meshmodel().attach_to(base)
rbtx.arm_move_jspace_path(
[rbtx.get_jnt_values(), jnt_values],
max_jntspeed=math.pi * 1.3)
break
for id in range(len(orbit)):
if id > 0:
tmp_stick = gm.gen_stick(spos=orbit[id - 1],
epos=orbit[id],
thickness=.01,
type="round")
tmp_stick.attach_to(base)
para_list.append(tmp_stick)
return task.done
pkx.image_release(color_image_handle)
pkx.image_release(depth_image_handle)
pkx.capture_release()
counter[0] += 1
return task.cont
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)
# rectangle
epos_vec = rm.unit_vector(cross_vec*.3-cross_vec.dot(ax)*ax*.3)
gm.gen_stick(spos=cross_vec.dot(ax)*ax*.3-ax*.03,
epos =cross_vec.dot(ax)*ax*.3-ax*.03+epos_vec*.03,
rgba=[0,0,0,1],
thickness=.001).attach_to(base)
gm.gen_stick(spos=cross_vec.dot(ax)*ax*.3-ax*.03+epos_vec*.03,
epos=cross_vec.dot(ax)*ax*.3-ax*.03+epos_vec*.03+ax*.03-ax*.005,
rgba=[0,0,0,1],
thickness=.001).attach_to(base)
epos_vec = rm.unit_vector(nxt_vec_uvw*.3-nxt_vec_uvw.dot(ax)*ax*.3)
gm.gen_stick(spos=nxt_vec_uvw.dot(ax)*ax*.3-ax*.03,
epos =nxt_vec_uvw.dot(ax)*ax*.3-ax*.03+epos_vec*.03,
rgba=[0,0,0,1],
thickness=.001).attach_to(base)
gm.gen_stick(spos=nxt_vec_uvw.dot(ax)*ax*.3-ax*.03+epos_vec*.03,
epos=nxt_vec_uvw.dot(ax)*ax*.3-ax*.03+epos_vec*.03+ax*.03-ax*.0045,
rgba=[0,0,0,1],
thickness=.001).attach_to(base)
gm.gen_torus(ax,
# box.set_rgba([153 / 255, 183 / 255, 1, 1])
# box.set_pos(np.array([0, 0, .32]))
# box.set_rotmat(rm.rotmat_from_axangle([0, 1, 0], -math.pi / 12))
# box.attach_to(base)
suction_s = suction.MVFLN40()
loc_pos_box = np.array([.1, 0, .02])
loc_rotmat_box = rm.rotmat_from_euler(math.pi, 0, 0)
gl_pos_box = box.get_pos() + box.get_rotmat().dot(loc_pos_box)
gl_rotmat_box = box.get_rotmat().dot(loc_rotmat_box)
suction_s.suction_to_with_scpose(gl_pos_box, gl_rotmat_box)
suction_s.gen_meshmodel().attach_to(base)
# gm.gen_stick(
# suction_s.pos,
# suction_s.pos - suction_s.rotmat[:,2]*10).attach_to(base)
gm.gen_stick(suction_s.pos, np.array([0, 0, 1])).attach_to(base)
# loc_pos_box = np.array([-.12, .12, .02])
# loc_rotmat_box = rm.rotmat_from_euler(math.pi*2/3, -math.pi/6, 0)
loc_pos_box = np.array([-.12, .03, .02])
loc_rotmat_box = rm.rotmat_from_euler(math.pi * 5 / 7, -math.pi / 3,
math.pi / 3)
# loc_pos_box = np.array([-.12, .03, -.02])
# loc_rotmat_box = rm.rotmat_from_euler(0, -math.pi / 3, math.pi/3)
# loc_pos_box = np.array([0, -.12, -.02])
# loc_rotmat_box = rm.rotmat_from_euler(0, math.pi / 3, math.pi/3)
gl_pos_box = box.get_pos() + box.get_rotmat().dot(loc_pos_box)
gl_rotmat_box = box.get_rotmat().dot(loc_rotmat_box)
print(gl_pos_box)
rtqgel_s = rtqgel.Robotiq85GelsightPusher()
box.set_rotmat(rm.rotmat_from_axangle([1, 0, 0], math.pi / 2)) # box.set_rotmat(rm.rotmat_from_euler(math.pi / 2, -math.pi/12, 0)) box.set_pos(np.array([.57, -.17, 1.35])) box.set_rotmat(rm.rotmat_from_euler(math.pi / 2, -math.pi / 12, 0)) suction_s = suction.MVFLN40() loc_pos_box = np.array([.1, .15, .0]) loc_rotmat_box = rm.rotmat_from_euler(math.pi / 2, 0, 0) gl_pos_box = box.get_pos() + box.get_rotmat().dot(loc_pos_box) gl_rotmat_box = box.get_rotmat().dot(loc_rotmat_box) suction_s.suction_to_with_scpose(gl_pos_box, gl_rotmat_box) suction_s.gen_meshmodel().attach_to(base) # gm.gen_stick( # suction_s.pos, # suction_s.pos - suction_s.rotmat[:,2]*10).attach_to(base) gm.gen_stick(suction_s.pos, np.array([.5, 0, 2.4])).attach_to(base) box.set_pos(np.array([.57, -.17, 1.3])) box.set_rotmat(rm.rotmat_from_axangle([1, 0, 0], math.pi / 2)) # loc_pos_box = np.array([-.125, .03, .02]) # loc_rotmat_box = rm.rotmat_from_axangle([0,1,0], math.pi*11/12).dot(rm.rotmat_from_axangle([0,0,1], math.pi/2)) # loc_rotmat_box=rm.rotmat_from_axangle([1,0,0], -math.pi/6).dot(loc_rotmat_box) # # loc_pos_box = np.array([-.12, .03, .02]) # # loc_rotmat_box = rm.rotmat_from_euler(math.pi*5/7, -math.pi / 3, math.pi/3) # # loc_pos_box = np.array([-.12, .03, -.02]) # # loc_rotmat_box = rm.rotmat_from_euler(0, -math.pi / 3, math.pi/3) # # loc_pos_box = np.array([0, -.12, -.02]) # # loc_rotmat_box = rm.rotmat_from_euler(0, math.pi / 3, math.pi/3) # gl_pos_box = box.get_pos() + box.get_rotmat().dot(loc_pos_box) # gl_rotmat_box = box.get_rotmat().dot(loc_rotmat_box) # jnt_angles = ur3d_s.ik(component_name='lft_arm', tgt_pos=gl_pos_box, tgt_rotmat=gl_rotmat_box)
def gen_stickmodel(self,
rgba=np.array([.5, 0, 0, 1]),
thickness=.01,
jointratio=1.62,
linkratio=.62,
tcp_jntid=None,
tcp_localpos=None,
tcp_localrotmat=None,
toggletcpcs=True,
togglejntscs=False,
togglecntjnt=False,
name='robotstick'):
"""
generate a stick model for self.jltree_obj
:param rgba:
:param tcp_jntid:
:param tcp_localpos:
:param tcp_localrotmat:
:param toggletcpcs:
:param togglejntscs:
:param togglecntjnt: draw the connecting joint explicitly or not
:param name:
:return:
author: weiwei
date: 20200331, 20201006, 20201205
"""
stickmodel = gm.StaticGeometricModel(name=name)
id = 0
loopdof = self.jlobject.ndof + 1
if togglecntjnt:
loopdof = self.jlobject.ndof + 2
while id < loopdof:
cjid = self.jlobject.joints[id]['child']
jgpos = self.jlobject.joints[id]['g_posq'] # joint global pos
cjgpos = self.jlobject.joints[cjid][
'g_pos0'] # child joint global pos
jgmtnax = self.jlobject.joints[id][
"g_mtnax"] # joint global rot ax
gm.gen_stick(spos=jgpos,
epos=cjgpos,
thickness=thickness,
type="rect",
rgba=rgba).attach_to(stickmodel)
if id > 0:
if self.jlobject.joints[id]['type'] == "revolute":
gm.gen_stick(spos=jgpos - jgmtnax * thickness,
epos=jgpos + jgmtnax * thickness,
type="rect",
thickness=thickness * jointratio,
rgba=np.array([.3, .3, .2,
1])).attach_to(stickmodel)
if self.jlobject.joints[id]['type'] == "prismatic":
jgpos0 = self.jlobject.joints[id]['g_pos0']
gm.gen_stick(spos=jgpos0,
epos=jgpos,
type="round",
hickness=thickness * jointratio,
rgba=np.array([.2, .3, .3,
1])).attach_to(stickmodel)
id = cjid
# tool center coord
if toggletcpcs:
self._toggle_tcpcs(stickmodel,
tcp_jntid,
tcp_localpos,
tcp_localrotmat,
tcpic_rgba=rgba + np.array([0, 0, 1, 0]),
tcpic_thickness=thickness * linkratio)
# toggle all coord
if togglejntscs:
self._toggle_jntcs(stickmodel,
jntcs_thickness=thickness * linkratio)
return stickmodel
