def sample_surface(self,
radius=0.005,
nsample=None,
toggle_option='face_ids'):
"""
:param raidus:
:param toggle_option; 'face_ids', 'normals', None
:return:
author: weiwei
date: 20191228
"""
if self._objtrm is None:
raise ValueError("Only applicable to models with a trimesh!")
if nsample is None:
nsample = int(round(self.objtrm.area / ((radius * 0.3)**2)))
points, face_ids = self.objtrm.sample_surface(nsample,
radius=radius,
toggle_faceid=True)
# transform
points = rm.homomat_transform_points(self.get_homomat(), points)
if toggle_option is None:
return np.array(points)
elif toggle_option == 'face_ids':
return np.array(points), np.array(face_ids)
elif toggle_option == 'normals':
return np.array(points), rm.homomat_transform_points(
self.get_homomat(), self.objtrm.face_normals[face_ids])
else:
print(
"toggle_option must be None, point_face_ids, or point_nromals")
def gettwoend(self):
self.endpairs = []
self.temporigin = []
self.origin = []
self.homomat = []
for i, coordinate in enumerate(self.coordinate):
endpair = []
axis = coordinate.T[2]
large = 0
large_vec = np.array([0, 0, 0])
small = 0
small_vec = np.array([0, 0, 0])
facet_1_ID = self.doubleholdpair[i][0]
facet_2_ID = self.doubleholdpair[i][1]
vertices = np.append(self.largeface_vertices[facet_1_ID],
self.largeface_vertices[facet_2_ID],
axis=0)
for vertice in vertices:
value = np.dot(axis, vertice)
if value >= large:
large = value
large_vec = vertice
elif value < small:
small = value
small_vec = vertice
endpair.append(large_vec)
endpair.append(small_vec)
self.temporigin.append((large_vec + small_vec) * 0.5)
self.endpairs.append(endpair)
origin = self.getorigin(normal_list=[
self.largeface_normals[facet_1_ID],
self.largeface_normals[facet_2_ID], axis
],
point_list=[
self.largefacescenter[facet_1_ID],
self.largefacescenter[facet_2_ID],
(large_vec + small_vec) * 0.5
])
self.origin.append(origin)
homomat = np.linalg.inv(rm.homomat_from_posrot(origin, coordinate))
self.pRotMat.append(homomat)
self.pCoM.append(rm.homomat_transform_points(homomat, self.com))
tempvertice = []
for vertice in vertices:
tempvertice.append(
rm.homomat_transform_points(homomat, vertice))
self.pVertices.append(tempvertice)
self.pNormals.append([
np.dot(coordinate.T, self.largeface_normals[facet_1_ID]),
np.dot(coordinate.T, self.largeface_normals[facet_2_ID])
])
self.pFacetpairscenter.append([
rm.homomat_transform_points(homomat,
self.largefacescenter[facet_1_ID]),
rm.homomat_transform_points(homomat,
self.largefacescenter[facet_2_ID])
])
def genHexahedralVectors(coefficient,polygon = 6, normal = np.array([0,0,1]), Fdistributed=1):
'''generate vectors for simulating the friction cone'''
initalMatrix = np.zeros([polygon,3])
# initVector =pg.rm.unit_vector(np.array([0,coefficient,1]))
initVector = Fdistributed*np.array([0, coefficient, 1])
# tfMatrix = pg.trigeom.align_vectors(vector_start=np.array([0,0,1]),vector_end=-normal)
tfMatrix = trigeom.align_vectors(vector_start=np.array([0, 0, 1]), vector_end=normal)
initalMatrix[0,:] = rm.homomat_transform_points(tfMatrix,initVector)
angle = 360.0/ polygon
for i in range(1,polygon):
rotMat = rm.rotmat_from_axangle([0,0,1],angle*i)
initalMatrix[i,:] = rm.homomat_transform_points(tfMatrix,np.dot(rotMat,initVector))
return initalMatrix
def extract_rotated_vvnf(self):
if self.cdmesh_type == 'aabb':
objtrm = self.objtrm.bounding_box
elif self.cdmesh_type == 'obb':
objtrm = self.objtrm.bounding_box_oriented
elif self.cdmesh_type == 'convex_hull':
objtrm = self.objtrm.convex_hull
elif self.cdmesh_type == 'triangles':
objtrm = self.objtrm
homomat = self.get_homomat()
vertices = rm.homomat_transform_points(homomat, objtrm.vertices)
vertex_normals = rm.homomat_transform_points(homomat,
objtrm.vertex_normals)
faces = objtrm.faces
return vertices, vertex_normals, faces
def cdmesh(self):
vertices = []
vertex_normals = []
faces = []
for objcm in self._cm_list:
if objcm.cdmesh_type == 'aabb':
objtrm = objcm.objtrm.bounding_box
elif objcm.cdmesh_type == 'obb':
objtrm = objcm.objtrm.bounding_box_oriented
elif objcm.cdmesh_type == 'convexhull':
objtrm = objcm.objtrm.convex_hull
elif objcm.cdmesh_type == 'triangles':
objtrm = objcm.objtrm
homomat = objcm.get_homomat()
vertices += rm.homomat_transform_points(homomat, objtrm.vertices)
vertex_normals += rm.homomat_transform_points(homomat, objtrm.vertex_normals)
faces += (objtrm.faces+len(faces))
return mcd.gen_cdmesh_vvnf(vertices, vertex_normals, faces)
def sethomomat(self, homomat):
"""
set the pose of the dynamic body
:param homomat: the homomat of the original frame (the collision model)
:return:
author: weiwei
date: 2019?, 20201119
"""
pos = rm.homomat_transform_points(homomat, self.com)
rotmat = homomat[:3, :3]
self.setTransform(TransformState.makeMat(dh.npv3mat3_to_pdmat4(pos, rotmat)))
def set_homomat(self, homomat):
"""
set the pose of the dynamic body
:param homomat: the homomat of the original frame (the collision model)
:return:
author: weiwei
date: 2019?, 20201119
"""
tmp_homomat = copy.deepcopy(homomat)
tmp_homomat[:3, 3] = tmp_homomat[:3, 3] * base.physics_scale
pos = rm.homomat_transform_points(tmp_homomat, self.com)
rotmat = tmp_homomat[:3, :3]
self.setTransform(
TransformState.makeMat(dh.npv3mat3_to_pdmat4(pos, rotmat)))
def extract_rotated_vvnf(self, cdmesh_type=None):
"""
allow either extract a vvnf following the specified cdmesh_type or the value of self.cdmesh_type
:param cdmesh_type:
:return:
author: weiwei
date: 20211215
"""
if cdmesh_type is None:
cdmesh_type = self.cdmesh_type
if cdmesh_type == 'aabb':
objtrm = self.objtrm.bounding_box
elif cdmesh_type == 'obb':
objtrm = self.objtrm.bounding_box_oriented
elif cdmesh_type == 'convex_hull':
objtrm = self.objtrm.convex_hull
elif cdmesh_type == 'triangles':
objtrm = self.objtrm
homomat = self.get_homomat()
vertices = rm.homomat_transform_points(homomat, objtrm.vertices)
vertex_normals = rm.homomat_transform_points(homomat, objtrm.vertex_normals)
faces = objtrm.faces
return vertices, vertex_normals, faces
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
# 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()
# 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()
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
