def drawanySingleSurface(a, vertices, color):
'''
draw a surface using a calculated fourth point to creat a hull
:param base:
:param vertices:
:param faces:
:param color:
:return:
'''
# print("faces in plotsurface",faces)
surface_vertices = vertices
# surface = humath.centerPoftrangle(surface_vertices[0][:3], surface_vertices[1][:3], surface_vertices[2][:3])
surface = trimesh.Trimesh(surface_vertices)
surface = surface.convex_hull
surface = gm.GeometricModel(surface)
if color == "red":
rgba = (1, 0, 0, 1)
elif color == "green":
rgba = (61 / 255, 145 / 255, 64 / 255, 1)
elif color == "orange":
rgba = (255 / 255, 100 / 255, 24 / 255, 1)
elif color == "white":
rgba = (1, 1, 1, 1)
elif color == "blue":
# rgba = (3/255,168/255,158/255,1)
rgba = (0 / 255, 0 / 255, 255 / 255, 1)
elif color == "yellow":
rgba = (1, 1, 0, 1)
else:
rgba = (0, 0, 0, 1)
surface.set_rgba(rgba)
surface.objpdnp.reparentTo(a)
def update(pcd_list, task):
if len(pcd_list) == 0:
return task.again
if len(shown_pcd_list) != 0:
for pcd in shown_pcd_list:
pcd.detach()
shown_pcd_list.clear()
pcd = gm.GeometricModel(pcd_list[0])
pcd.attach_to(base)
shown_pcd_list.append(pcd)
pcd_list.pop(0)
return task.again
def __init__(self,
objinit,
mass=None,
restitution=0,
allowdeactivation=False,
allowccd=True,
friction=.2,
dynamic=True,
type="convex",
name="bdm"):
"""
:param objinit: GeometricModel (CollisionModel also work)
:param mass:
:param restitution:
:param allowdeactivation:
:param allowccd:
:param friction:
:param dynamic:
:param type: "convex", "triangle", "box"
:param name:
"""
if isinstance(objinit, BDModel):
self._gm = copy.deepcopy(objinit.gm)
self._bdb = objinit.bdb.copy()
self._cm = copy.deepcopy(objinit.cm)
elif isinstance(objinit, gm.GeometricModel):
if mass is None:
mass = 0
self._gm = objinit
self._bdb = bdb.BDBody(self._gm,
type,
mass,
restitution,
allow_deactivation=allowdeactivation,
allow_ccd=allowccd,
friction=friction,
dynamic=dynamic,
name=name)
else:
if mass is None:
mass = 0
self._cm = cm.CollisionModel(objinit)
self._gm = gm.GeometricModel(objinit)
self._bdb = bdb.BDBody(self._gm,
type,
mass,
restitution,
allow_deactivation=allowdeactivation,
allow_ccd=allowccd,
friction=friction,
dynamic=dynamic,
name=name)
def update(video1, pcdm, task):
if len(pcdm) != 0:
for one_pcdm in pcdm:
one_pcdm.detach()
else:
pcdm.append(None)
key = cv2.waitKey(1)
if int(key) == 113:
video1.release()
return task.done
ret, frame = video1.read()
frame = cv2.resize(frame, (int(width), int(height)),
interpolation=cv2.INTER_CUBIC)
depth, hm = itd_cvter.convert(frame)
pcdm[0] = gm.GeometricModel(depth * .001)
pcdm[0].attach_to(base)
return task.again
def drawSingleFaceSurface(self, base, vertices, faces, color):
'''
draw a surface using a calculated fourth point to creat a hull
:param base:
:param vertices:
:param faces:
:param color:
:return:
'''
# print("faces in plotsurface",faces)
surface_vertices = np.array([vertices[faces[0]], vertices[faces[1]], vertices[faces[2]]])
surface = humath.centerPoftrangle(surface_vertices[0], surface_vertices[1], surface_vertices[2])
surface = trimesh.Trimesh(surface)
surface = surface.convex_hull
surface = gm.GeometricModel(surface)
surface.set_rgba(color)
surface.attach_to(base)
def update(pk_obj, pcd_list, marker_center_list, task):
if len(pcd_list) != 0:
for pcd in pcd_list:
pcd.detach()
pcd_list.clear()
if len(marker_center_list) != 0:
for marker_center in marker_center_list:
marker_center.detach()
marker_center_list.clear()
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:
color_image = pk_obj.image_convert_to_numpy(color_image_handle)
point_cloud = pk_obj.transform_depth_image_to_point_cloud(
depth_image_handle)
parameters = aruco.DetectorParameters_create()
color_image = cv2.cvtColor(color_image, cv2.COLOR_BGRA2BGR)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
color_image,
dictionary=aruco.Dictionary_get(aruco.DICT_4X4_250),
parameters=parameters)
if len(corners) == 0:
return task.cont
image_xy = np.mean(np.mean(corners[0], axis=0),
axis=0).astype(np.int16)
pcd_pnt = pk_obj.transform_color_xy_to_pcd_xyz(
input_color_image_handle=color_image_handle,
input_depth_image_handle=depth_image_handle,
color_xy=image_xy)
# cv2.circle(color_image, tuple(image_xy), 10, (255, 0, 0), -1)
# cv2.imshow("test", color_image)
# cv2.waitKey(0)
mypoint_cloud = gm.GeometricModel(initor=point_cloud)
mypoint_cloud.attach_to(base)
pcd_list.append(mypoint_cloud)
marker_center = gm.gen_sphere(pos=pcd_pnt, radius=.1)
marker_center.attach_to(base)
marker_center_list.append(marker_center)
pk_obj.image_release(color_image_handle)
pk_obj.image_release(depth_image_handle)
pk_obj.capture_release()
return task.cont
# select_id = 3
# if height > .1:
# select_id = 4
# map_color = map_list[select_id]
if height < .25:
this_rotmat = wither_rotmat_list[id % len(wither_rotmat_list)]
branch = Stem(ndof=1,
pos=branch_pos,
rotmat=this_rotmat,
base_length=.1 / (height)**(1 / 2),
base_thickness=.002)
branch.gen_meshmodel(rgba=wither_rgba).attach_to(base)
# main_stem.fk(jnt_values=[math.pi/36,math.pi/36, 0,-math.pi/36,-math.pi/36,0])
# stem1.gen_meshmodel().attach_to(base)
sb_leaf = gm.GeometricModel(initor="objects/soybean_leaf.stl")
sb_leaf.set_rgba(rgba=leaf_rgba)
sbl = sb_leaf.copy()
sbl.set_rgba(rgba=wither_rgba)
# sbl.set_scale(np.array([1,1,1])/(int(id/3)%(main_stem.jlc.ndof+1)+1))
sbl.set_scale(np.array([1, 1, 1]))
jnt_pos = branch.jlc.jnts[-1]['gl_posq']
sbl.set_pos(jnt_pos)
sbl.set_rotmat(this_rotmat)
# gm.gen_frame(pos=jnt_pos, rotmat=this_rotmat).attach_to(base)
sbl.attach_to(base)
else:
branch = Stem(ndof=1,
pos=branch_pos,
rotmat=rotmat,
base_length=.1 / (height)**(1 / 2),
# action_rotmat_list = []
# for i in range(10):
# action_pos_list.append(pos[i] + pos_start)
# action_rotmat_list.append(np.dot(rm.rotmat_from_axangle(rot[i][0], rot[i][1]),
# rot_start))
# # print(marker_pos_in_hnd)
# pos_arm_tcp, rot_arm_tcp = xss.arm.jlc.lnks[7]["gl_pos"], xss.arm.jlc.lnks[7]["gl_rotmat"]
# pos_hnd_tcp, rot_hand_tcp = xss.get_gl_tcp(manipulator_name="arm")
# # gm.gen_frame(pos_arm_tcp, rot_arm_tcp).attach_to(base)
# # gm.gen_frame(pos_hnd_tcp, rot_hand_tcp).attach_to(base)
# marker_pos_in_hnd = np.array([-0.026, 0, 0.069]) - np.array([0, 0, pos_hnd_tcp[2] - pos_arm_tcp[2]])
# # print("marker in hand", marker_pos_in_hnd)
# calibration_r = dc.calibrate(component_name="arm",
# sensor_marker_handler=sensor_handler,
# marker_pos_in_hnd=marker_pos_in_hnd,
# action_pos_list=action_pos_list,
# action_rotmat_list=action_rotmat_list)
# print(calibration_r)
# base.run()
# validate pcd
from vision.depth_camera.calibrator import load_calibration_data
affine_matrix, _, _ = load_calibration_data()
gm.GeometricModel(initor=rm.homomat_transform_points(
affine_matrix, sensor_handler.get_point_cloud())).attach_to(base)
# # planner
# rrtc_planner = rrtc.RRTConnect(xss)
base.run()
def show_pcd(pcd, rgba=(1, 1, 1, 1)):
pcd_gm = gm.GeometricModel(initor=pcd)
pcd_gm.set_rgba(rgba)
pcd_gm.attach_to(base)
# pcd_list = []
# marker_center_list = []
# def update(pk_obj, pcd_list, marker_center_list, task):
# if len(pcd_list) != 0:
# for pcd in pcd_list:
# pcd.detach()
# pcd_list.clear()
# if len(marker_center_list) != 0:
# for marker_center in marker_center_list:
# marker_center.detach()
# marker_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:
color_image = pk_obj.image_convert_to_numpy(color_image_handle)
# cv2.imshow("test", color_image)
# cv2.waitKey(0)
point_cloud = pk_obj.transform_depth_image_to_point_cloud(depth_image_handle)
point_cloud = rm.homomat_transform_points(rm.homomat_inverse(origin_homomat), point_cloud)
point_cloud[point_cloud[:,0]<-1]=point_cloud[point_cloud[:,0]<-1]*0
mypoint_cloud = gm.GeometricModel(initor=point_cloud)
mypoint_cloud.attach_to(base)
base.run()
pk_obj.image_release(color_image_handle)
pk_obj.image_release(depth_image_handle)
pk_obj.capture_release()
base.run()
base = wd.World(cam_pos=[.5, .2, .3],
lookat_pos=[0, 0, 0],
auto_cam_rotate=False)
# objcm = gm.WireFrameModel(gen_torus())
# objcm.set_rgba([1, 0, 0, 1])
# objcm.attach_to(base)
# objcm = gm.StaticGeometricModel(gen_axis())
# objcm.set_rgba([1, 0, 0, 1])
# objcm.attach_to(base)
import time
tic = time.time()
for i in range(100):
gen_dumbbell()
toc = time.time()
print("mine", toc - tic)
objcm = gm.GeometricModel(gen_dashstick(lsolid=.005, lspace=.005))
objcm = gm.GeometricModel(gen_dashtorus(portion=1))
objcm.set_rgba([1, 0, 0, 1])
objcm.attach_to(base)
objcm = gm.GeometricModel(gen_stick())
objcm.set_rgba([1, 0, 0, 1])
objcm.set_pos(np.array([0, .01, 0]))
objcm.attach_to(base)
objcm = gm.GeometricModel(gen_dasharrow())
objcm.set_rgba([1, 0, 0, 1])
objcm.set_pos(np.array([0, -.01, 0]))
objcm.attach_to(base)
base.run()
bowl_model.attach_to(base)
# print(model_pcd)
bowl_model.attach_to(base)
# base.run()
import random
for point_id in range(3000, 10000, 100):
tree = cKDTree(bowl_samples)
# point_id = random.randint(3000, 10000)
nearby_sample_ids = tree.query_ball_point(bowl_samples[point_id, :], .03)
nearby_samples = bowl_samples[nearby_sample_ids]
colors = np.tile(np.array([1, 0, 0, 1]), (len(nearby_samples), 1))
print(nearby_samples.shape)
print(colors.shape)
nearby_samples_withcolor = np.column_stack((nearby_samples, colors))
gm.GeometricModel(nearby_samples_withcolor).attach_to(base)
plane_center, plane_normal = rm.fit_plane(nearby_samples)
plane_tangential = rm.orthogonal_vector(plane_normal)
plane_tmp = np.cross(plane_normal, plane_tangential)
plane_rotmat = np.column_stack((plane_tangential, plane_tmp, plane_normal))
nearby_samples_on_xy = plane_rotmat.T.dot(
(nearby_samples - plane_center).T).T
surface = gs.MixedGaussianSurface(nearby_samples_on_xy[:, :2],
nearby_samples_on_xy[:, 2],
n_mix=1)
# t_npt_on_xy = plane_rotmat.T.dot(t_npt - plane_center)
# projected_t_npt_z_on_xy = surface.get_zdata(np.array([t_npt_on_xy[:2]]))
# projected_t_npt_on_xy = np.array([t_npt_on_xy[0], t_npt_on_xy[1], projected_t_npt_z_on_xy[0]])
# projected_point = plane_rotmat.dot(projected_t_npt_on_xy) + plane_center
surface_gm = surface.get_gometricmodel([[-.05, .05], [-.05, .05]],
rgba=[.5, .7, 1, 1])
rotmat.dot(tmp_direction) * .05
]]
gm.gen_linesegs(new_line_segs).attach_to(base)
# gm.gen_arrow(spos=new_line_segs[0][0], epos=new_line_segs[0][1], thickness=0.004).attach_to(base)
t_cpt = cpt
last_normal = cnrml
direction = rotmat.dot(pt_direction)
n = 10
for tick in range(1, n + 1):
t_npt = cpt + direction * .05 / n
gm.gen_arrow(spos=t_npt, epos=t_npt + last_normal * .015,
thickness=0.001).attach_to(base)
nearby_sample_ids = tree.query_ball_point(t_npt, .03)
nearby_samples = bowl_samples[nearby_sample_ids]
gm.GeometricModel(nearby_samples).attach_to(base)
plane_center, plane_normal = rm.fit_plane(nearby_samples)
plane_tangential = rm.orthogonal_vector(plane_normal)
plane_tmp = np.cross(plane_normal, plane_tangential)
plane_rotmat = np.column_stack((plane_tangential, plane_tmp, plane_normal))
nearby_samples_on_xy = plane_rotmat.T.dot(
(nearby_samples - plane_center).T).T
surface = qs.QuadraticSurface(nearby_samples_on_xy[:, :2],
nearby_samples_on_xy[:, 2])
# surface = gs.MixedGaussianSurface(nearby_samples_on_xy[:, :2], nearby_samples_on_xy[:,2],n_mix=1)
t_npt_on_xy = plane_rotmat.T.dot(t_npt - plane_center)
projected_t_npt_z_on_xy = surface.get_zdata(np.array([t_npt_on_xy[:2]]))
projected_t_npt_on_xy = np.array(
[t_npt_on_xy[0], t_npt_on_xy[1], projected_t_npt_z_on_xy[0]])
projected_point = plane_rotmat.dot(projected_t_npt_on_xy) + plane_center
surface_gm = surface.get_gometricmodel([[-.05, .05], [-.05, .05]],
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
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
# fixture_start_homomat = rm.homomat_from_posrot(fixture_start_pos, fixture_start_rotmat)
# fixture_start_rotmat = fixture_start_homomat[:3,:3]
# fixture_start.set_pos(fixture_start_pos)
# fixture_start.set_rotmat(fixture_start_rotmat)
# fixture_start.attach_to(base)
fixture_trimesh = fixture.objtrm
v = fixture_trimesh.voxelized(0.01)
# b = triv.Voxel(fixture_trimesh, 0.01)
c= triv.mesh_to_run(fixture_trimesh, 0.01)
fixture_voxel = fixture_trimesh.voxelized(0.001)
a = gm.GeometricModel(fixture_voxel)
a.attach_to(base)
# fixture_voxel = voxelizer.creation.local_voxelize
base.run()
slopename = "tc71.stl"
slope_high = slope.Slope(z=-0.005, placement="ping", size=sinfo.Sloperate[slopename], show=False)
slopeforcd_high = slope_high.getSlope()
for i, item in enumerate(slopeforcd_high):
item.set_homomat(fixture_start_homomat.dot(item.get_homomat()))
slopeforcd_high[i] = item
# slopeforcd_high[0].attach_to(base)
# slopeforcd_high[1].attach_to(base)
# slopeforcd_high[2].attach_to(base)
robot = ur3ed.UR3EDual()
3:] = self._return_value[:,
3:] / 255.0 # regulate to 0-1
self.buffer = bytearray()
if __name__ == '__main__':
import time
client = EtherSenseClient(address="10.2.0.202", port=18360)
cv2.namedWindow("window")
while True:
cv2.imshow("window", client.get_rgb_image())
cv2.waitKey(1)
# time.sleep(3)
# cv2.imshow("window2", client.get_rgb_image())
# cv2.waitKey(1)
# time.sleep(3)
import visualization.panda.rpc.rviz_client as rv_client
import modeling.geometric_model as gm
rvc = rv_client.RVizClient(host="localhost:182001")
rvc.reset()
last_rmt = None
while True:
pcd = gm.GeometricModel(initor=client.get_rgb_pointcloud())
current_rmt = rvc.showmodel_to_remote(pcd)
if last_rmt is not None:
rvc.unshow_model(last_rmt)
last_rmt = current_rmt
base.run()
if __name__ == '__main__':
import numpy as np
import visualization.panda.world as wd
import modeling.geometric_model as gm
base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, .1])
pm_s = gm.GeometricModel("./meshes/p1000g.stl")
pm_s.attach_to(base)
pm_b_s = gm.GeometricModel("./meshes/p1000g_body.stl")
pm_b_s.set_scale(scale=[1.03,1.03,1.01])
pm_b_s.set_pos(np.array([0,0, 0.1463]))
pm_b_s.set_rgba(rgba=[.3, .4, .6, 1])
pm_b_s.attach_to(base)
base.run()
import numpy as np
import basis.trimesh_generator as tg
import modeling.geometric_model as gm
import visualization.panda.world as wd
if __name__ == '__main__':
base = wd.World(cam_pos=np.array([.3,.3,.3]), lookat_pos=np.zeros(3))
tg_sphere = tg.gen_sphere(pos=np.zeros(3), radius=.05)
gm_sphere = gm.GeometricModel(tg_sphere)
gm_sphere.attach_to(base)
base.run()
bunnycm2.set_rgba([0, 0.7, 0.7, 1.0])
rotmat = rm.rotmat_from_axangle([1, 0, 0], -math.pi / 4.0)
bunnycm2.set_pos(np.array([0, .2, 0]))
bunnycm2.set_rotmat(rotmat)
bunnycm.attach_to(base)
bunnycm1.attach_to(base)
bunnycm2.attach_to(base)
bunnycm.show_cdprimit()
bunnycm1.show_cdprimit()
bunnycm2.show_cdprimit()
bunnycmpoints, _ = bunnycm.sample_surface()
bunnycm1points, _ = bunnycm1.sample_surface()
bunnycm2points, _ = bunnycm2.sample_surface()
bpcm = gm.GeometricModel(bunnycmpoints)
bpcm1 = gm.GeometricModel(bunnycm1points)
bpcm2 = gm.GeometricModel(bunnycm2points)
bpcm.attach_to(base)
bpcm1.attach_to(base)
bpcm2.attach_to(base)
# bunnycm2.show_cdmesh(type='box')
# bunnycm.show_cdmesh(type='box')
# bunnycm1.show_cdmesh(type='convexhull')
# tic = time.time()
# bunnycm2.is_mcdwith([bunnycm, bunnycm1])
# toc = time.time()
# print("mesh cd cost: ", toc - tic)
# tic = time.time()
# bunnycm2.is_pcdwith([bunnycm, bunnycm1])
# toc = time.time()
self.earth.set_rgba(rgba=earth_rgba)
def set_pos(self, pos):
self.cup.set_pos(pos=pos)
self.earth.set_pos(pos=pos)
def attach_to(self, base):
self.cup.attach_to(base)
self.earth.attach_to(base)
def copy(self):
return copy.deepcopy(self)
base = wd.World(cam_pos=[4.2, 4.2, 2.5], lookat_pos=[0, 0, .7], auto_cam_rotate=True)
frame = gm.GeometricModel(initor="meshes/frame.stl")
frame.set_rgba(rgba=aluminium_rgba)
frame.attach_to(base)
bottom_box = cm.gen_box(extent=[.88, 1.68, .45], rgba=board_rgba)
bottom_box.set_pos(pos=np.array([0, 0, .22]))
bottom_box.attach_to(base)
top_box = cm.gen_box(extent=[.88, 1.68, .3], rgba=board_rgba)
top_box.set_pos(pos=np.array([0, 0, 1.65]))
top_box.attach_to(base)
cup = Cup()
cup_pos_x = [0.09 - .44, 0.23 - .44, 0.37 - .44, 0.51 - .44, 0.65 - .44, 0.79 - .44]
cup_pos_y = [.09 - .84, .24 - .84, .39 - .84, .54 - .84, .69 - .84, .84 - .84, .99 - .84, 1.14 - .84, 1.29 - .84,
1.44 - .84, 1.59 - .84]
# a_mesh.export('a_mesh_1.stl')
# print("a", a)
# print("a_mesh", a_mesh)
# b_mesh = b.objtrm
# b_mesh.export('b_mesh.stl')
# a_trimesh = tri.load("a_mesh.stl")
# b_trimesh = tri.load("b_mesh.stl")
# c_mesh = tb.union([a_trimesh, b_trimesh], engine = "blender")
# c = gm.GeometricModel(c_mesh)
# print("c", c)
# c = cm.CollisionModel(c_mesh)
# c = gm.GeometricModel(c_mesh)
# c.attach_to(base)
base.run()
m = [[.050, 0, 0], [.050, .100, 0], [.050, 0, .100], [.150, 0, 0],
[.150, .100, .100], [.150, .100, 0], [.150, 0, .100], [.050, .100, .100]]
# base.pggen.plotAxis(base.render)
# print(Vec3(100, 100, 100), type(Vec3(100, 100, 100)))
b.set_rgba((0, 1, 0, 1))
#
# b.setPos(Vec3(100, 100, 100))
b.attach_to(base)
mesh_b = trimesh.Trimesh(m)
body_m = mesh_b.convex_hull
bm = cm.CollisionModel(body_m)
bm.attach_to(base.render)
a = tb.union([mesh, mesh_b], engine="blender")
am = gm.GeometricModel(a)
am.attach_to(base)
# taskMgr.doMethodLater(0.5, update, 'update', extraArgs=[b,counter], appendTask=True)
base.run()
import numpy as np
import modeling.geometric_model as gm
import visualization.panda.world as wd
import basis.robot_math as rm
import math
# import vision.depth_camera.surface.gaussian_surface as gs
# import vision.depth_camera.surface.quadrantic_surface as qs
import vision.depth_camera.surface.bibspline_surface as bs
import vision.depth_camera.surface.plane_surface as ps
base = wd.World(cam_pos=np.array([.5,.1,.3]), lookat_pos=np.array([0,0,0.05]))
gm.gen_frame().attach_to(base)
tube_model = gm.GeometricModel(initor="./objects/bowl.stl")
tube_model.set_rgba([.3,.3,.3,.3])
tube_model.attach_to(base)
points, points_normals = tube_model.sample_surface(radius=.002, nsample=10000, toggle_option='normals')
sampled_points = []
for id, p in enumerate(points.tolist()):
if np.dot(np.array([1,0,0]), points_normals[id]) > .3 and p[0]>0:
gm.gen_sphere(pos=p, radius=.001).attach_to(base)
sampled_points.append(p)
# x - v
# y - u
rotmat_uv = rm.rotmat_from_euler(0, math.pi/2, 0)
sampled_points = rotmat_uv.dot(np.array(sampled_points).T).T
# surface = rbfs.RBFSurface(sampled_points[:, :2], sampled_points[:,2])
# surface = gs.MixedGaussianSurface(sampled_points[:, :2], sampled_points[:,2], n_mix=1)
# surface = qs.QuadraticSurface(sampled_points[:, :2], sampled_points[:,2])
# surface = bs.BiBSpline(sampled_points[:, :2], sampled_points[:,2])
surface = ps.PlaneSurface(sampled_points[:,:2], sampled_points[:,2])
