137 lines
6.8 KiB
Python
Executable File
137 lines
6.8 KiB
Python
Executable File
import trimesh
|
|
import json
|
|
import numpy as np
|
|
|
|
|
|
PI = np.pi
|
|
red_color = [1.0, 0.0, 0.0, 0.5] # red, A=1 表示不透明
|
|
green_color = [0.0, 1.0, 0.0, 0.5] # green, A=1 表示不透明
|
|
blue_color = [0.0, 0.0, 1.0, 0.5] # blue, A=1 表示不透明
|
|
|
|
def create_model_data(id):
|
|
file_path = f"./base{id}.glb"
|
|
save_path = f"./model_data{id}.json"
|
|
|
|
with open(save_path, 'r') as json_file:
|
|
data = json.load(json_file)
|
|
|
|
with open(file_path, 'rb') as file_obj:
|
|
mesh = trimesh.load(file_obj, file_type='glb')
|
|
# 创建一个场景
|
|
scene = trimesh.Scene(mesh)
|
|
|
|
oriented_bounding_box = mesh.bounding_box_oriented
|
|
|
|
scale = data['scale']
|
|
|
|
target_sphere = trimesh.creation.icosphere(subdivisions=2, radius=0.15)
|
|
target_trans_matrix_sphere = trimesh.transformations.translation_matrix([0,-oriented_bounding_box.extents[1]/2,0])
|
|
|
|
target_sphere.apply_transform(target_trans_matrix_sphere)
|
|
target_sphere.visual.vertex_colors = np.array([blue_color] * len(target_sphere.vertices))
|
|
target_points_list = [target_trans_matrix_sphere.tolist()]
|
|
|
|
# # 可视化网格和坐标轴
|
|
# scene.add_geometry(target_sphere)
|
|
|
|
contact_points_list = []
|
|
contact_point_discription_list = []
|
|
|
|
def add_contact_point(point_radius, pose:list, euler:list, discription: str):
|
|
contact_sphere = trimesh.creation.icosphere(subdivisions=2, radius=point_radius)
|
|
contact_trans_matrix_sphere = trimesh.transformations.translation_matrix(pose) @ trimesh.transformations.euler_matrix(euler[0], euler[1], euler[2])
|
|
contact_sphere.apply_transform(contact_trans_matrix_sphere)
|
|
contact_sphere.visual.vertex_colors = np.array([red_color] * len(contact_sphere.vertices))
|
|
contact_points_list.append(contact_trans_matrix_sphere.tolist())
|
|
|
|
axis = trimesh.creation.axis(axis_length=1.5)
|
|
axis.apply_transform(contact_trans_matrix_sphere)
|
|
scene.add_geometry(axis)
|
|
scene.add_geometry(contact_sphere)
|
|
contact_point_discription_list.append(discription)
|
|
|
|
add_contact_point(0.1, np.array(data['contact_points_pose'][0])[:3,3], [0, 0, 0], "Grasping the side of the cup.")
|
|
add_contact_point(0.1, np.array(data['contact_points_pose'][1])[:3,3], [0, PI, 0], "Grasping the side of the cup.")
|
|
add_contact_point(0.1, np.array(data['contact_points_pose'][2])[:3,3], [0, -PI/2, 0], "Grasping the side of the cup.")
|
|
add_contact_point(0.1, np.array(data['contact_points_pose'][3])[:3,3], [0, PI/2, 0], "Grasping the side of the cup.")
|
|
add_contact_point(0.1, np.array(data['contact_points_pose'][4])[:3,3] * [-1,1,1], [0, -PI/2, 0], "Grasping the side of the cup.")
|
|
add_contact_point(0.1, np.array(data['contact_points_pose'][5])[:3,3] * [-1,1,1], [0, PI/2, 0], "Grasping the side of the cup.")
|
|
# add_contact_point(0.1, np.array(data['contact_pose'][1])[:3,3], [0, PI/2, 0], "Grasping the side of the cup.")
|
|
# add_contact_point(0.1, np.array(data['contact_pose'][2])[:3,3], [0, -PI/2, 0], "Grasping the side of the cup.")
|
|
|
|
|
|
# 旋转矩阵的参数顺序是 (X, Y, Z) to endpose axis
|
|
transform_matrix = trimesh.transformations.euler_matrix(0,0,0)
|
|
transform_matrix = transform_matrix.tolist()
|
|
|
|
# 方向点
|
|
orientation_point = trimesh.creation.icosphere(subdivisions=2, radius=0.1)
|
|
orientation_point_sphere = trimesh.transformations.translation_matrix([0,0,0])
|
|
orientation_point.apply_transform(orientation_point_sphere)
|
|
orientation_point.visual.vertex_colors = np.array([green_color] * len(orientation_point.vertices))
|
|
orientation_point_list = orientation_point_sphere.tolist()
|
|
# scene.add_geometry(orientation_point)
|
|
|
|
# axis1
|
|
axis1 = trimesh.creation.axis(axis_length=1.5)
|
|
axis1.apply_transform(transform_matrix)
|
|
|
|
functional_matrix_list = []
|
|
|
|
functional_sphere = trimesh.creation.icosphere(subdivisions=2, radius=0.15)
|
|
functional_trans_matrix_sphere = trimesh.transformations.translation_matrix(np.array(data['functional_matrix'][0])[:3,3]) @ trimesh.transformations.euler_matrix(0,PI/2,0)
|
|
functional_sphere.apply_transform(functional_trans_matrix_sphere)
|
|
functional_sphere.visual.vertex_colors = np.array([blue_color] * len(functional_sphere.vertices))
|
|
functional_matrix = functional_trans_matrix_sphere.tolist()
|
|
axis = trimesh.creation.axis(axis_length=1.5)
|
|
axis.apply_transform(functional_trans_matrix_sphere)
|
|
scene.add_geometry(axis)
|
|
scene.add_geometry(functional_sphere)
|
|
functional_matrix_list.append(functional_matrix)
|
|
|
|
functional_sphere = trimesh.creation.icosphere(subdivisions=2, radius=0.15)
|
|
functional_trans_matrix_sphere = trimesh.transformations.translation_matrix(target_trans_matrix_sphere[:3,3]) @ trimesh.transformations.euler_matrix(PI/2,0,0)
|
|
functional_sphere.apply_transform(functional_trans_matrix_sphere)
|
|
functional_sphere.visual.vertex_colors = np.array([blue_color] * len(functional_sphere.vertices))
|
|
functional_matrix = functional_trans_matrix_sphere.tolist()
|
|
axis = trimesh.creation.axis(axis_length=1.5)
|
|
axis.apply_transform(functional_trans_matrix_sphere)
|
|
scene.add_geometry(axis)
|
|
scene.add_geometry(functional_sphere)
|
|
functional_matrix_list.append(functional_matrix)
|
|
|
|
data = {
|
|
'center': oriented_bounding_box.centroid.tolist(), # 中心点
|
|
'extents': oriented_bounding_box.extents.tolist(), # 尺寸
|
|
'scale': scale, # 缩放
|
|
'target_pose': target_points_list, # 目标点矩阵
|
|
'contact_points_pose' : contact_points_list, # 抓取点矩阵（多个）
|
|
'transform_matrix': transform_matrix, # 模型到标轴的旋转矩阵
|
|
"functional_matrix": functional_matrix_list, # 功能点矩阵
|
|
'orientation_point': orientation_point_list,
|
|
'contact_points_group': [[0,1],[2,3],[4,5]],
|
|
'contact_points_mask': [True,True,True],
|
|
'contact_points_discription': contact_point_discription_list, # 抓取点描述
|
|
'target_point_discription': [""], # 目标点描述
|
|
'functional_point_discription': ["Point0: The function point is inside the handle of the mug, and the function axis is perpendicular to the plane of the handle.",
|
|
"Point1: The function point is at the bottom of the mug, and the function axis is perpendicular to the bottom of the cup, Used to put down the cup.",],
|
|
'orientation_point_discription': [""]
|
|
}
|
|
with open(save_path, 'w') as json_file:
|
|
json.dump(data, json_file, indent=4, separators=(',', ': '))
|
|
|
|
# 将坐标轴添加到场景
|
|
axis = trimesh.creation.axis(axis_length=1.5,origin_size= 0.05)
|
|
# scene.add_geometry(axis1)
|
|
scene.show()
|
|
|
|
if __name__ == "__main__":
|
|
# id = ""
|
|
for i in range(10):
|
|
# i = 0
|
|
create_model_data(i)
|
|
|
|
|
|
|
|
|