186 lines
7.5 KiB
Python
Executable File
186 lines
7.5 KiB
Python
Executable File
# import trimesh
|
|
# import json
|
|
# import numpy as np
|
|
|
|
|
|
# file_path = "./base.glb"
|
|
# with open(file_path, 'rb') as file_obj:
|
|
# mesh = trimesh.load(file_obj, file_type='glb')
|
|
# # mesh = trimesh.load(file_path, file_type='glb')
|
|
# oriented_bounding_box = mesh.bounding_box_oriented
|
|
# red_color = [1.0, 0.0, 0.0, 0.5] # 红色, A=1 表示不透明
|
|
|
|
|
|
# target_sphere = trimesh.creation.icosphere(subdivisions=2, radius=0.05)
|
|
# target_trans_matrix_sphere = trimesh.transformations.translation_matrix([0, 0.3,0.17])
|
|
# target_sphere.apply_transform(target_trans_matrix_sphere)
|
|
# target_sphere.visual.vertex_colors = np.array([red_color] * len(target_sphere.vertices))
|
|
# target_trans_list = target_trans_matrix_sphere.tolist()
|
|
|
|
|
|
|
|
# contact_sphere = trimesh.creation.icosphere(subdivisions=2, radius=0.08)
|
|
# contact_trans_matrix_sphere = trimesh.transformations.translation_matrix([0, -0.4, -0.1])
|
|
# contact_sphere.apply_transform(contact_trans_matrix_sphere)
|
|
# contact_sphere.visual.vertex_colors = np.array([red_color] * len(contact_sphere.vertices))
|
|
# contact_trans_list = contact_trans_matrix_sphere.tolist()
|
|
# # 创建一个场景
|
|
# scene = trimesh.Scene(mesh)
|
|
|
|
# # axis1
|
|
# axis1 = trimesh.creation.axis(axis_length=1)
|
|
# # 旋转矩阵的参数顺序是 (X, Y, Z) to endpose axis
|
|
# trans_matrix1 = trimesh.transformations.euler_matrix(-1.57, 0,1.57)
|
|
# # trans_matrix1[0,3] = 1
|
|
# # trans_matrix1[1,3] = 1
|
|
# # trans_matrix1[2,3] = -0.3
|
|
# # 应用旋转矩阵到坐标轴
|
|
# axis1.apply_transform(trans_matrix1)
|
|
# # scene.add_geometry(axis1)
|
|
# transform_matrix_list1 = trans_matrix1.tolist()
|
|
|
|
# # trans_matrix2 = trimesh.transformations.euler_matrix(-1.57,0,1.57)
|
|
# # axis1.apply_transform(trans_matrix2)
|
|
|
|
|
|
|
|
|
|
# data = {
|
|
# 'center': oriented_bounding_box.centroid.tolist(), # 中心点
|
|
# 'extents': oriented_bounding_box.extents.tolist(), # 尺寸
|
|
# # 'grasp_pose1': transform_matrix_list1,
|
|
# 'scale': 0.1,
|
|
# 'contact_trans_list' : contact_trans_list,
|
|
# 'target_trans_list' : contact_trans_list
|
|
# }
|
|
# with open('./model_data.json', 'w') as json_file:
|
|
# json.dump(data, json_file, indent=4)
|
|
|
|
|
|
|
|
# # 将坐标轴添加到场景
|
|
# axis = trimesh.creation.axis(axis_length=1.5,origin_size= 0.005)
|
|
# # axis.apply_transform(trimesh.transformations.euler_matrix(-1.57, 1.57, 0))
|
|
# # scene.add_geometry(axis)
|
|
# scene.add_geometry(axis1)
|
|
# # 可视化网格和坐标轴
|
|
# scene.add_geometry(target_sphere)
|
|
# scene.add_geometry(contact_sphere)
|
|
# scene.show()
|
|
|
|
|
|
import trimesh
|
|
import urdfpy
|
|
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 = [0.31,0.31,0.31]
|
|
|
|
target_sphere = trimesh.creation.icosphere(subdivisions=2, radius=0.15)
|
|
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=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.2, [0, -0.4, -0.1], [-PI/2, 0, PI/2], "Grasping the handle of knife.")
|
|
# add_contact_point(0.3, [0, 0, 0], [0, PI/2, 0], "Grasping the side of the bottle.")
|
|
# add_contact_point(0.3, [0, 0, 0], [0, PI, 0], "Grasping the side of the bottle.")
|
|
# add_contact_point(0.3, [0, 0, 0], [0, -PI/2, 0], "Grasping the side of the bottle.")
|
|
|
|
|
|
# 旋转矩阵的参数顺序是 (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([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_sphere = trimesh.creation.icosphere(subdivisions=2, radius=0.1)
|
|
functional_trans_matrix_sphere = trimesh.transformations.translation_matrix([0,0.3,0.17]) @ trimesh.transformations.euler_matrix(-0.05,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)
|
|
|
|
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]],
|
|
'contact_points_mask': [True],
|
|
'contact_points_discription': contact_point_discription_list, # 抓取点描述
|
|
'target_point_discription': ["The center of the object."], # 目标点描述
|
|
'functional_point_discription': [""],
|
|
'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) |