open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/dptech-corp/Uni-Lab-OS.git synced 2025-12-17 04:51:10 +00:00
Code Issues Packages Projects Releases Wiki Activity

使用json启动plr与3D模型仿真

Browse Source
This commit is contained in:
zhangshixiang
2025-04-29 10:08:34 +08:00
committed by Junhan Chang
parent d407423aaa
commit 49bb11b2a3
8 changed files with 150 additions and 620 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
unilabos/app/main.py
View File

@@ -134,6 +134,7 @@ def main():
args_dict["resources_config"] = initialize_resources(list(deepcopy(devices_and_resources).values()))
args_dict["devices_config"] = dict_to_nested_dict(deepcopy(devices_and_resources), devices_only=False)
# args_dict["resources_config"] = dict_to_tree(devices_and_resources, devices_only=False)
args_dict["graph"] = graph_res.physical_setup_graph
else:
if args_dict["devices"] is None or args_dict["resources"] is None:
@@ -171,9 +172,10 @@ def main():
if args_dict["visual"] != "None":
if args_dict["visual"] == "rviz":
resource_visualization = ResourceVisualization(args_dict["devices_config"], args_dict["resources_config"],registry_dict)
enable_rviz=True
elif args_dict["visual"] == "web":
resource_visualization = ResourceVisualization(args_dict["devices_config"], args_dict["resources_config"],registry_dict,enable_rviz=False )
enable_rviz=False
resource_visualization = ResourceVisualization(devices_and_resources, args_dict["resources_config"],registry_dict,enable_rviz=enable_rviz)
devices_config_add = add_resource_mesh_manager_node(resource_visualization.resource_model, args_dict["resources_config"])
args_dict["devices_config"] = {**args_dict["devices_config"], **devices_config_add}

6
unilabos/device_mesh/resource_visalization.py
View File

@@ -50,6 +50,10 @@ class ResourceVisualization:
# 检查设备类型是否在注册表中
if device_class not in registry.device_type_registry.keys():
print("="*20)
print(device_class)
print(registry.device_type_registry.keys())
print("="*20)
raise ValueError(f"设备类型 {device_class} 未在注册表中注册")
elif "model" in registry.device_type_registry[device_class].keys():
@@ -71,6 +75,7 @@ class ResourceVisualization:
elif node['type'] in self.resource_type:
# print(registry.resource_type_registry)
resource_class = node['class']
if resource_class not in registry.resource_type_registry.keys():
raise ValueError(f"资源类型 {resource_class} 未在注册表中注册")
@@ -85,6 +90,7 @@ class ResourceVisualization:
'mesh': f"{str(self.mesh_path)}/resources/{model_config['children_mesh']}",
'mesh_tf': model_config['children_mesh_tf']
}
re = etree.tostring(self.root, encoding="unicode")
doc = xacro.parse(re)

576
unilabos/device_mesh/view_robot.rviz
View File

@@ -13,7 +13,7 @@ Panels:
- /RobotState1
- /RobotState1/Links1
Splitter Ratio: 0.5
Tree Height: 275
Tree Height: 345
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@@ -49,436 +49,19 @@ Visualization Manager:
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz_default_plugins/TF
Enabled: true
Enabled: false
Frame Timeout: 15
Frames:
All Enabled: false
Gripper1_device_link:
Value: false
Gripper1_first_link:
Value: true
Gripper1_fourth_link:
Value: false
Gripper1_main_link:
Value: false
Gripper1_second_link:
Value: false
Gripper1_socketTypeGenericSbsFootprint:
Value: false
Gripper1_socketTypeHEPAModule:
Value: false
Gripper1_third_link:
Value: false
Plate1:
Value: false
Plate1_A1:
Value: false
Plate1_A10:
Value: false
Plate1_A11:
Value: false
Plate1_A12:
Value: false
Plate1_A2:
Value: false
Plate1_A3:
Value: false
Plate1_A4:
Value: false
Plate1_A5:
Value: false
Plate1_A6:
Value: false
Plate1_A7:
Value: false
Plate1_A8:
Value: false
Plate1_A9:
Value: false
Plate1_B1:
Value: false
Plate1_B10:
Value: false
Plate1_B11:
Value: false
Plate1_B12:
Value: false
Plate1_B2:
Value: false
Plate1_B3:
Value: false
Plate1_B4:
Value: false
Plate1_B5:
Value: false
Plate1_B6:
Value: false
Plate1_B7:
Value: false
Plate1_B8:
Value: false
Plate1_B9:
Value: false
Plate1_C1:
Value: false
Plate1_C10:
Value: false
Plate1_C11:
Value: false
Plate1_C12:
Value: false
Plate1_C2:
Value: false
Plate1_C3:
Value: false
Plate1_C4:
Value: false
Plate1_C5:
Value: false
Plate1_C6:
Value: false
Plate1_C7:
Value: false
Plate1_C8:
Value: false
Plate1_C9:
Value: false
Plate1_D1:
Value: false
Plate1_D10:
Value: false
Plate1_D11:
Value: false
Plate1_D12:
Value: false
Plate1_D2:
Value: false
Plate1_D3:
Value: false
Plate1_D4:
Value: false
Plate1_D5:
Value: false
Plate1_D6:
Value: false
Plate1_D7:
Value: false
Plate1_D8:
Value: false
Plate1_D9:
Value: false
Plate1_E1:
Value: false
Plate1_E10:
Value: false
Plate1_E11:
Value: false
Plate1_E12:
Value: false
Plate1_E2:
Value: false
Plate1_E3:
Value: false
Plate1_E4:
Value: false
Plate1_E5:
Value: false
Plate1_E6:
Value: false
Plate1_E7:
Value: false
Plate1_E8:
Value: false
Plate1_E9:
Value: false
Plate1_F1:
Value: false
Plate1_F10:
Value: false
Plate1_F11:
Value: false
Plate1_F12:
Value: false
Plate1_F2:
Value: false
Plate1_F3:
Value: false
Plate1_F4:
Value: false
Plate1_F5:
Value: false
Plate1_F6:
Value: false
Plate1_F7:
Value: false
Plate1_F8:
Value: false
Plate1_F9:
Value: false
Plate1_G1:
Value: false
Plate1_G10:
Value: false
Plate1_G11:
Value: false
Plate1_G12:
Value: false
Plate1_G2:
Value: false
Plate1_G3:
Value: false
Plate1_G4:
Value: false
Plate1_G5:
Value: false
Plate1_G6:
Value: false
Plate1_G7:
Value: false
Plate1_G8:
Value: false
Plate1_G9:
Value: false
Plate1_H1:
Value: false
Plate1_H10:
Value: false
Plate1_H11:
Value: false
Plate1_H12:
Value: false
Plate1_H2:
Value: false
Plate1_H3:
Value: false
Plate1_H4:
Value: false
Plate1_H5:
Value: false
Plate1_H6:
Value: false
Plate1_H7:
Value: false
Plate1_H8:
Value: false
Plate1_H9:
Value: false
world:
Value: false
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Show Names: false
Tree:
world:
Gripper1_device_link:
Gripper1_main_link:
Gripper1_first_link:
Gripper1_second_link:
Gripper1_fourth_link:
{}
Gripper1_third_link:
{}
Plate1_C6:
{}
Gripper1_socketTypeGenericSbsFootprint:
{}
Gripper1_socketTypeHEPAModule:
{}
Plate1:
Plate1_A1:
{}
Plate1_A10:
{}
Plate1_A11:
{}
Plate1_A12:
{}
Plate1_A2:
{}
Plate1_A3:
{}
Plate1_A4:
{}
Plate1_A5:
{}
Plate1_A6:
{}
Plate1_A7:
{}
Plate1_A8:
{}
Plate1_A9:
{}
Plate1_B1:
{}
Plate1_B10:
{}
Plate1_B11:
{}
Plate1_B12:
{}
Plate1_B2:
{}
Plate1_B3:
{}
Plate1_B4:
{}
Plate1_B5:
{}
Plate1_B6:
{}
Plate1_B7:
{}
Plate1_B8:
{}
Plate1_B9:
{}
Plate1_C1:
{}
Plate1_C10:
{}
Plate1_C11:
{}
Plate1_C12:
{}
Plate1_C2:
{}
Plate1_C3:
{}
Plate1_C4:
{}
Plate1_C7:
{}
Plate1_C8:
{}
Plate1_C9:
{}
Plate1_D1:
{}
Plate1_D10:
{}
Plate1_D11:
{}
Plate1_D12:
{}
Plate1_D2:
{}
Plate1_D3:
{}
Plate1_D4:
{}
Plate1_D5:
{}
Plate1_D6:
{}
Plate1_D7:
{}
Plate1_D8:
{}
Plate1_D9:
{}
Plate1_E1:
{}
Plate1_E10:
{}
Plate1_E11:
{}
Plate1_E12:
{}
Plate1_E2:
{}
Plate1_E3:
{}
Plate1_E4:
{}
Plate1_E5:
{}
Plate1_E6:
{}
Plate1_E7:
{}
Plate1_E8:
{}
Plate1_E9:
{}
Plate1_F1:
{}
Plate1_F10:
{}
Plate1_F11:
{}
Plate1_F12:
{}
Plate1_F2:
{}
Plate1_F3:
{}
Plate1_F4:
{}
Plate1_F5:
{}
Plate1_F6:
{}
Plate1_F7:
{}
Plate1_F8:
{}
Plate1_F9:
{}
Plate1_G1:
{}
Plate1_G10:
{}
Plate1_G11:
{}
Plate1_G12:
{}
Plate1_G2:
{}
Plate1_G3:
{}
Plate1_G4:
{}
Plate1_G5:
{}
Plate1_G6:
{}
Plate1_G7:
{}
Plate1_G8:
{}
Plate1_G9:
{}
Plate1_H1:
{}
Plate1_H10:
{}
Plate1_H11:
{}
Plate1_H12:
{}
Plate1_H2:
{}
Plate1_H3:
{}
Plate1_H4:
{}
Plate1_H5:
{}
Plate1_H6:
{}
Plate1_H7:
{}
Plate1_H8:
{}
Plate1_H9:
{}
Plate1_C5:
{}
{}
Update Interval: 0
Value: true
Value: false
- Alpha: 1
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
@@ -490,61 +73,20 @@ Visualization Manager:
History Policy: Keep Last
Reliability Policy: Reliable
Value: /robot_description
Enabled: true
Enabled: false
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Gripper1_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_first_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_fourth_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_main_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_second_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_socketTypeGenericSbsFootprint:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_socketTypeHEPAModule:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_third_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
world:
Alpha: 1
Show Axes: false
Show Trail: false
Mass Properties:
Inertia: false
Mass: false
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Value: true
Value: false
Visual Enabled: true
- Class: moveit_rviz_plugin/PlanningScene
Enabled: false
@@ -566,48 +108,7 @@ Visualization Manager:
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Gripper1_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_first_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_fourth_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_main_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_second_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_socketTypeGenericSbsFootprint:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_socketTypeHEPAModule:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_third_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
world:
Alpha: 1
Show Axes: false
Show Trail: false
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: false
@@ -621,48 +122,7 @@ Visualization Manager:
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Gripper1_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_first_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_fourth_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_main_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_second_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Gripper1_socketTypeGenericSbsFootprint:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_socketTypeHEPAModule:
Alpha: 1
Show Axes: false
Show Trail: false
Gripper1_third_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
world:
Alpha: 1
Show Axes: false
Show Trail: false
Name: RobotState
Robot Alpha: 1
Robot Description: robot_description
@@ -888,30 +348,30 @@ Visualization Manager:
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 1.2573390007019043
Y: 1.1951926946640015
Z: 0.23975235223770142
X: 1.0440783500671387
Y: 0.27775266766548157
Z: 0.42559614777565
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.5797955989837646
Pitch: 0.40479576587677
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 6.250748634338379
Yaw: 6.070750713348389
Saved: ~
Window Geometry:
"":
collapsed: false
" - Trajectory Slider":
collapsed: false
Displays:
collapsed: false
Height: 1656
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000003a3000005dcfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e0000020b0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007a00fffffffbffffffff0100000285000003c5000002b700ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000627000005dc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
MotionPlanning:
collapsed: false
MotionPlanning - Trajectory Slider:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000400000000000003a3000005dcfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000002510000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000002cb0000037f000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000627000005dc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
@@ -919,5 +379,5 @@ Window Geometry:
Views:
collapsed: true
Width: 2518
X: 125
X: 140
Y: 145

64
unilabos/devices/ros_dev/resource_mesh_manager.py
View File

@@ -125,7 +125,6 @@ class ResourceMeshManager(BaseROS2DeviceNode):
def resource_mesh_setup(self):
"""move_group初始化完成后的设置"""
self.get_logger().info('开始设置资源网格管理器')
#遍历resource_config中的资源配置，判断panent是否在resource_model中，
for resource_id, resource_config in self.resource_config_dict.items():
@@ -162,7 +161,12 @@ class ResourceMeshManager(BaseROS2DeviceNode):
float(rotation_dict['y']),
float(rotation_dict['z'])
)
# print("-"*20)
# print(f"resource_id: {resource_id}")
# print(f"parent: {parent}")
# print(f"resource_config: {self.resource_model}")
# print(f"parent_link: {parent_link}")
# print("-"*20)
rotation = {
"x": q[0],
"y": q[1],
@@ -358,62 +362,6 @@ class ResourceMeshManager(BaseROS2DeviceNode):
goal_handle.succeed()
return SendCmd.Result(success=True)
def resource_mesh_setup(self):
"""move_group初始化完成后的设置"""
self.get_logger().info('开始设置资源网格管理器')
#遍历resource_config中的资源配置，判断panent是否在resource_model中，
for resource_id, resource_config in self.resource_config_dict.items():
parent = resource_config['parent']
parent_link = 'world'
if parent in self.resource_model:
parent_link = parent
elif parent is None and resource_id in self.resource_model:
pass
elif parent not in self.resource_model and parent is not None:
parent_link = f"{self.resource_config_dict[parent]['parent']}{parent}_device_link".replace("None","")
else:
continue
# 提取位置信息并转换单位
position = {
"x": float(resource_config['position']['x'])/1000,
"y": float(resource_config['position']['y'])/1000,
"z": float(resource_config['position']['z'])/1000
}
rotation_dict = {
"x": 0,
"y": 0,
"z": 0
}
if 'rotation' in resource_config['config']:
rotation_dict = resource_config['config']['rotation']
# 从欧拉角转换为四元数
q = quaternion_from_euler(
float(rotation_dict['x']),
float(rotation_dict['y']),
float(rotation_dict['z'])
)
rotation = {
"x": q[0],
"y": q[1],
"z": q[2],
"w": q[3]
}
# 更新资源TF字典
self.resource_tf_dict[resource_id] = {
"parent": parent_link,
"position": position,
"rotation": rotation
}
def publish_resource_tf(self):
"""
发布资源之间的TF关系

8
unilabos/registry/devices/deck.yaml Normal file
View File

@@ -0,0 +1,8 @@
OTDeck:
description: Opentrons deck
class:
module: pylabrobot.resources.opentrons.deck:OTDeck
type: pylabrobot
model:
type: device
mesh: opentrons_liquid_handler

5
unilabos/registry/resources/opentrons/deck.yaml
View File

@@ -2,4 +2,7 @@ OTDeck:
description: Opentrons deck
class:
module: pylabrobot.resources.opentrons.deck:OTDeck
type: pylabrobot
type: pylabrobot
model:
type: device
mesh: opentrons_liquid_handler