111 lines
5.1 KiB
Python
Executable File
111 lines
5.1 KiB
Python
Executable File
import trimesh
|
|
import json
|
|
import numpy as np
|
|
|
|
|
|
PI = np.pi
|
|
|
|
def create_model_data(id):
|
|
file_path = f"./textured{id}.obj"
|
|
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='obj')
|
|
# 创建一个场景
|
|
scene = trimesh.Scene(mesh)
|
|
|
|
oriented_bounding_box = mesh.bounding_box_oriented
|
|
red_color = [1.0, 0.0, 0.0, 0.5] # 红色, A=1 表示不透明
|
|
|
|
scale = data['scale']
|
|
|
|
target_sphere = trimesh.creation.icosphere(subdivisions=2, radius=0.005)
|
|
target_trans_matrix_sphere = trimesh.transformations.translation_matrix([0,0,0])
|
|
|
|
target_sphere.apply_transform(target_trans_matrix_sphere)
|
|
target_sphere.visual.vertex_colors = np.array([red_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 = []
|
|
orientation_point_list = []
|
|
functional_matrix = []
|
|
|
|
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=0.1,origin_size= 0.005)
|
|
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.03, [0, 0, 0], [PI/2, 0, 0], "Grab the apple from top to bottom.")
|
|
add_contact_point(0.03, [0, 0, 0], [PI/2, 0, PI/2], "Grab the apple from top to bottom.")
|
|
add_contact_point(0.03, [0, 0, 0], [PI/2, 0, PI], "Grab the apple from top to bottom.")
|
|
add_contact_point(0.03, [0, 0, 0], [PI/2, 0, -PI/2], "Grab the apple from top to bottom.")
|
|
|
|
|
|
# 旋转矩阵的参数顺序是 (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,oriented_bounding_box.extents[1]/2,0])
|
|
# orientation_point.apply_transform(orientation_point_sphere)
|
|
# orientation_point.visual.vertex_colors = np.array([red_color] * len(orientation_point.vertices))
|
|
# orientation_point_list = orientation_point_sphere.tolist()
|
|
# scene.add_geometry(orientation_point)
|
|
|
|
# axis1
|
|
axis1 = trimesh.creation.axis(axis_length=0.1, origin_size= 0.005)
|
|
axis1.apply_transform(transform_matrix)
|
|
|
|
functional_sphere = trimesh.creation.icosphere(subdivisions=0.1, radius=0.003)
|
|
functional_trans_matrix_sphere = trimesh.transformations.translation_matrix([0,0,0]) @ trimesh.transformations.euler_matrix(PI,0,0)
|
|
functional_sphere.apply_transform(functional_trans_matrix_sphere)
|
|
functional_sphere.visual.vertex_colors = np.array([red_color] * len(functional_sphere.vertices))
|
|
functional_matrix = functional_trans_matrix_sphere.tolist()
|
|
axis = trimesh.creation.axis(axis_length=0.1,origin_size= 0.005)
|
|
axis.apply_transform(functional_trans_matrix_sphere)
|
|
scene.add_geometry(axis)
|
|
scene.add_geometry(functional_sphere)
|
|
|
|
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], # 功能点矩阵
|
|
'orientation_point': orientation_point_list,
|
|
'contact_points_group': [[0,1,2,3]],
|
|
'contact_points_mask': [True],
|
|
'contact_points_discription': contact_point_discription_list, # 抓取点描述
|
|
'target_point_discription': ["The center of the object."], # 目标点描述
|
|
'functional_point_discription': ["Point0: Apple center point, axis perpendicular to the apple ground downward"],
|
|
'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 = ""
|
|
create_model_data(id) |