open_pub/Uni-Lab-OS
1
0
Fork 0
mirror of https://github.com/dptech-corp/Uni-Lab-OS.git synced 2026-02-06 23:15:10 +00:00
Code Issues Packages Projects Releases Wiki Activity

Device visualization (#39)

Browse Source 
* Update README and MQTTClient for installation instructions and code improvements

* feat: 支持local_config启动
add: 增加对crt path的说明，为传入config.py的相对路径
move: web component

* add: registry description

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

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

* feat: node_info_update srv
fix: OTDeck cant create

* close #12
feat: slave node registry

* feat: show machine name
fix: host node registry not uploaded

* feat: add hplc registry

* feat: add hplc registry

* fix: hplc status typo

* fix: devices/

* 完成启动OT并联动rviz

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

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

* 完成启动OT并联动rviz

* fix: device.class possible null

* fix: HPLC additions with online service

* fix: slave mode spin not working

* fix: slave mode spin not working

* 修复rviz位置问题,

修复rviz位置问题,
在无tf变动时减缓发送频率
在backend中添加物料跟随方法

* feat: 多ProtocolNode 允许子设备ID相同
feat: 上报发现的ActionClient
feat: Host重启动，通过discover机制要求slaveNode重新注册，实现信息及时上报

* feat: 支持env设置config

* fix: running logic

* fix: running logic

* fix: missing ot

* 在main中直接初始化republisher和物料的mesh节点

* 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中

* Device visualization (#14)

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

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

* 完成启动OT并联动rviz

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

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

* 完成启动OT并联动rviz

* 修复rviz位置问题,

修复rviz位置问题,
在无tf变动时减缓发送频率
在backend中添加物料跟随方法

* fix: running logic

* fix: running logic

* fix: missing ot

* 在main中直接初始化republisher和物料的mesh节点

* 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中

---------

Co-authored-by: zhangshixiang <@zhangshixiang>
Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com>

* fix: missing hostname in devices_names
fix: upload_file for model file

* fix: missing paho-mqtt package
bump version to 0.9.0

* fix startup
add ResourceCreateFromOuter.action

* fix type hint

* update actions

* update actions

* host node add_resource_from_outer
fix cmake list

* pass device config to device class

* add: bind_parent_ids to resource create action
fix: message convert string

* fix: host node should not be re_discovered

* feat: resource tracker support dict

* feat: add more necessary params

* feat: fix boolean null in registry action data

* feat: add outer resource

* 编写mesh添加action

* feat: append resource

* add action

* feat: vis 2d for plr

* fix

* fix: browser on rviz

* fix: cloud bridge error fallback to local

* fix: salve auto run rviz

* 初始化两个plate

* Device visualization (#22)

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

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

* 完成启动OT并联动rviz

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

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

* 完成启动OT并联动rviz

* 修复rviz位置问题,

修复rviz位置问题,
在无tf变动时减缓发送频率
在backend中添加物料跟随方法

* fix: running logic

* fix: running logic

* fix: missing ot

* 在main中直接初始化republisher和物料的mesh节点

* 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中

* 编写mesh添加action

* add action

* fix

* fix: browser on rviz

* fix: cloud bridge error fallback to local

* fix: salve auto run rviz

* 初始化两个plate

---------

Co-authored-by: zhangshixiang <@zhangshixiang>
Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com>

* fix: multi channel

* fix: aspirate

* fix: aspirate

* fix: aspirate

* fix: aspirate

* 提交

* fix: jobadd

* fix: jobadd

* fix: msg converter

* tijiao

* add resource creat easy action

* identify debug msg

* mq client id

* 提取lh的joint发布

* unify liquid_handler definition

* 修改物料跟随与物料添加逻辑

修改物料跟随与物料添加逻辑
将joint_publisher类移出lh的backends，但仍需要对lh的backends进行一些改写

* Revert "修改物料跟随与物料添加逻辑"

This reverts commit 498c997ad7.

* Reapply "修改物料跟随与物料添加逻辑"

This reverts commit 3a60d2ae81.

* Revert "Merge remote-tracking branch 'upstream/dev' into device_visualization"

This reverts commit fa727220af, reversing
changes made to 498c997ad7.

* 修改物料放下时的方法，如果选择

修改物料放下时的方法，
如果选择drop_trash，则删除物料显示
如果选择drop，则让其解除连接

* add biomek.py demo implementation

* 更新LiquidHandlerBiomek类，添加资源创建功能，优化协议创建方法，修复部分代码格式问题，更新YAML配置以支持新功能。

* Test

* fix biomek success type

* Convert LH action to biomek.

* Update biomek.py

* 注册表上报handle和schema (param input)

* 修复biomek缺少的字段

* delete 's'

* Remove warnings

* Update biomek.py

* Biomek test

* Update biomek.py

* 新增transfer_biomek的msg

* New transfer_biomek

* Updated transfer_biomek

* 更新transfer_biomek的msg

* 更新transfer_biomek的msg

* 支持Biomek创建

* new action

* fix key name typo

* New parameter for biomek to run.

* Refine

* Update

* new actions

* new actions

* 1

* registry

* fix biomek startup
add action handles

* fix handles not as default entry

* unilab添加moveit启动

1,整合所有moveit节点到一个move_group中，并整合所有的controller依次激活
2,添加pymoveit2的节点，使用json可直接启动
3,修改机械臂规划方式，添加约束，让冗余关节不会进行过多移动

* biomek_test.py

biomek_test.py是最新的版本，运行它会生成complete_biomek_protocol.json

* Update biomek.py

* biomek_test.py

* fix liquid_handler.biomek handles

* 修改物体attach时，多次赋值当前时间导致卡顿问题，

* Revert "修改物体attach时，多次赋值当前时间导致卡顿问题，"

This reverts commit 56d45b94f5.

* Reapply "修改物体attach时，多次赋值当前时间导致卡顿问题，"

This reverts commit 07d9db20c3.

* 添加缺少物料："plate_well_G12",

* host node新增resource add时间统计
create_resource新增handle
bump version to 0.9.2

* 修正物料上传时间
改用biomek_test
增加ResultInfoEncoder
支持返回结果上传

* 正确发送return_info结果

* 同步执行状态信息

* 取消raiseValueError提示

* Update biomek_test.py

* 0608 DONE

* 同步了Biomek.py 现在应可用

* biomek switch back to non-test

* temp disable initialize resource

* add

* fix tip resource data

* liquid states

* change to debug level

* Revert "change to debug level"

This reverts commit 5d9953c3e5.

* Reapply "change to debug level"

This reverts commit 2487bb6ffc.

* fix tip resource data

* add full device

* add moveit yaml

* 修复moveit
增加post_init阶段，给予ros_node反向

* remove necessary node

* fix moveit action client

* remove necessary imports

* Update moveit_interface.py

* fix handler_key uppercase

* json add liquids

* fix setup

* add

* change to "sources" and "targets" for lh

* bump version

* remove parent's parent link

---------

Co-authored-by: Harvey Que <Q-Query@outlook.com>
Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com>
Co-authored-by: zhangshixiang <@zhangshixiang>
Co-authored-by: Junhan Chang <changjh@pku.edu.cn>
Co-authored-by: Guangxin Zhang <guangxin.zhang.bio@gmail.com>
Co-authored-by: qxw138 <qxw@stu.pku.edu.cn>
This commit is contained in:
q434343
2025-06-09 17:06:04 +08:00
committed by GitHub
parent 729a0fcf0c
commit 133ffaac17
104 changed files with 20893 additions and 2712 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
unilabos/app/mq.py
View File

@@ -162,7 +162,7 @@ class MQTTClient:
status = {"data": device_status.get(device_id, {}), "device_id": device_id}
address = f"labs/{MQConfig.lab_id}/devices/"
self.client.publish(address, json.dumps(status), qos=2)
logger.critical(f"Device status published: address: {address}, {status}")
logger.debug(f"Device status published: address: {address}, {status}")
def publish_job_status(self, feedback_data: dict, job_id: str, status: str, return_info: Optional[str] = None):
if self.mqtt_disable:

4
unilabos/app/web/client.py
View File

@@ -60,7 +60,7 @@ class HTTPClient:
Dict: 返回的资源数据
"""
response = requests.get(
f"{self.remote_addr}/lab/resource/",
f"{self.remote_addr}/lab/resource/?edge_format=1",
params={"id": id, "with_children": with_children},
headers={"Authorization": f"lab {self.auth}"},
timeout=5,
@@ -96,7 +96,7 @@ class HTTPClient:
Response: API响应对象
"""
response = requests.patch(
f"{self.remote_addr}/lab/resource/batch_update/",
f"{self.remote_addr}/lab/resource/batch_update/?edge_format=1",
json=resources,
headers={"Authorization": f"lab {self.auth}"},
timeout=5,

9
unilabos/device_mesh/devices/benyao_arm/config/initial_positions.yaml Normal file
View File

@@ -0,0 +1,9 @@
# Default initial positions for full_dev's ros2_control fake system
initial_positions:
arm_base_joint: 0
arm_link_1_joint: 0
arm_link_2_joint: 0
arm_link_3_joint: 0
gripper_base_joint: 0
gripper_right_joint: 0.03

40
unilabos/device_mesh/devices/benyao_arm/config/joint_limits.yaml Normal file
View File

@@ -0,0 +1,40 @@
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
# For beginners, we downscale velocity and acceleration limits.
# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
default_velocity_scaling_factor: 0.1
default_acceleration_scaling_factor: 0.1
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
arm_base_joint:
has_velocity_limits: true
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0
arm_link_1_joint:
has_velocity_limits: true
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0
arm_link_2_joint:
has_velocity_limits: true
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0
arm_link_3_joint:
has_velocity_limits: true
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0
gripper_base_joint:
has_velocity_limits: true
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0
gripper_right_joint:
has_velocity_limits: true
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0

4
unilabos/device_mesh/devices/benyao_arm/config/kinematics.yaml Normal file
View File

@@ -0,0 +1,4 @@
arm:
kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin
kinematics_solver_search_resolution: 0.0050000000000000001
kinematics_solver_timeout: 0.0050000000000000001

56
unilabos/device_mesh/devices/benyao_arm/config/macro.ros2_control.xacro Normal file
View File

@@ -0,0 +1,56 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="benyao_arm_ros2_control" params="device_name mesh_path">
<xacro:property name="initial_positions" value="${load_yaml(mesh_path + '/devices/benyao_arm/config/initial_positions.yaml')['initial_positions']}"/>
<ros2_control name="${device_name}benyao_arm" type="system">
<hardware>
<!-- By default, set up controllers for simulation. This won't work on real hardware -->
<plugin>mock_components/GenericSystem</plugin>
</hardware>
<joint name="${device_name}arm_base_joint">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['arm_base_joint']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="${device_name}arm_link_1_joint">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['arm_link_1_joint']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="${device_name}arm_link_2_joint">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['arm_link_2_joint']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="${device_name}arm_link_3_joint">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['arm_link_3_joint']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="${device_name}gripper_base_joint">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['gripper_base_joint']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="${device_name}gripper_right_joint">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['gripper_right_joint']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
</ros2_control>
</xacro:macro>
</robot>

46
unilabos/device_mesh/devices/benyao_arm/config/macro.srdf.xacro Normal file
View File

@@ -0,0 +1,46 @@
<?xml version="1.0" encoding="UTF-8"?>
<!--This does not replace URDF, and is not an extension of URDF.
This is a format for representing semantic information about the robot structure.
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="benyao_arm_srdf" params="device_name">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
<group name="${device_name}arm">
<chain base_link="${device_name}arm_slideway" tip_link="${device_name}gripper_base"/>
</group>
<group name="${device_name}arm_gripper">
<joint name="${device_name}gripper_right_joint"/>
</group>
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="${device_name}arm_base" link2="${device_name}arm_link_2" reason="Adjacent"/>
<disable_collisions link1="${device_name}arm_base" link2="${device_name}arm_link_1" reason="Adjacent"/>
<disable_collisions link1="${device_name}arm_base" link2="${device_name}arm_link_3" reason="Never"/>
<disable_collisions link1="${device_name}arm_base" link2="${device_name}arm_slideway" reason="Adjacent"/>
<disable_collisions link1="${device_name}arm_link_1" link2="${device_name}arm_link_2" reason="Adjacent"/>
<disable_collisions link1="${device_name}arm_link_1" link2="${device_name}arm_link_3" reason="Never"/>
<disable_collisions link1="${device_name}arm_link_1" link2="${device_name}arm_slideway" reason="Never"/>
<disable_collisions link1="${device_name}arm_link_1" link2="${device_name}gripper_base" reason="Never"/>
<disable_collisions link1="${device_name}arm_link_1" link2="${device_name}gripper_left" reason="Never"/>
<disable_collisions link1="${device_name}arm_link_1" link2="${device_name}gripper_right" reason="Never"/>
<disable_collisions link1="${device_name}arm_link_2" link2="${device_name}arm_link_3" reason="Adjacent"/>
<disable_collisions link1="${device_name}arm_link_2" link2="${device_name}arm_slideway" reason="Never"/>
<disable_collisions link1="${device_name}arm_link_2" link2="${device_name}gripper_base" reason="Never"/>
<disable_collisions link1="${device_name}arm_link_2" link2="${device_name}gripper_left" reason="Never"/>
<disable_collisions link1="${device_name}arm_link_2" link2="${device_name}gripper_right" reason="Never"/>
<disable_collisions link1="${device_name}arm_link_3" link2="${device_name}arm_slideway" reason="Never"/>
<disable_collisions link1="${device_name}arm_link_3" link2="${device_name}gripper_base" reason="Adjacent"/>
<disable_collisions link1="${device_name}arm_link_3" link2="${device_name}gripper_left" reason="Never"/>
<disable_collisions link1="${device_name}arm_link_3" link2="${device_name}gripper_right" reason="Never"/>
<disable_collisions link1="${device_name}arm_slideway" link2="${device_name}gripper_base" reason="Never"/>
<disable_collisions link1="${device_name}arm_slideway" link2="${device_name}gripper_left" reason="Never"/>
<disable_collisions link1="${device_name}arm_slideway" link2="${device_name}gripper_right" reason="Never"/>
<disable_collisions link1="${device_name}gripper_base" link2="${device_name}gripper_left" reason="Adjacent"/>
<disable_collisions link1="${device_name}gripper_base" link2="${device_name}gripper_right" reason="Adjacent"/>
<disable_collisions link1="${device_name}gripper_left" link2="${device_name}gripper_right" reason="Never"/>
</xacro:macro>
</robot>

14
unilabos/device_mesh/devices/benyao_arm/config/move_group.json Normal file
View File

@@ -0,0 +1,14 @@
{
"arm":
{
"joint_names": [
"arm_base_joint",
"arm_link_1_joint",
"arm_link_2_joint",
"arm_link_3_joint",
"gripper_base_joint"
],
"base_link_name": "device_link",
"end_effector_name": "gripper_base"
}
}

29
unilabos/device_mesh/devices/benyao_arm/config/moveit_controllers.yaml Normal file
View File

@@ -0,0 +1,29 @@
# MoveIt uses this configuration for controller management
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager:
controller_names:
- arm_controller
- gripper_controller
arm_controller:
type: FollowJointTrajectory
action_ns: follow_joint_trajectory
default: true
joints:
- arm_base_joint
- arm_link_1_joint
- arm_link_2_joint
- arm_link_3_joint
- gripper_base_joint
action_ns: follow_joint_trajectory
default: true
gripper_controller:
type: FollowJointTrajectory
action_ns: follow_joint_trajectory
default: true
joints:
- gripper_right_joint
action_ns: follow_joint_trajectory
default: true

2
unilabos/device_mesh/devices/benyao_arm/config/moveit_planners.yaml Normal file
View File

@@ -0,0 +1,2 @@
planner_configs:
- ompl_interface/OMPLPlanner

6
unilabos/device_mesh/devices/benyao_arm/config/pilz_cartesian_limits.yaml Normal file
View File

@@ -0,0 +1,6 @@
# Limits for the Pilz planner
cartesian_limits:
max_trans_vel: 1.0
max_trans_acc: 2.25
max_trans_dec: -5.0
max_rot_vel: 1.57

39
unilabos/device_mesh/devices/benyao_arm/config/ros2_controllers.yaml Normal file
View File

@@ -0,0 +1,39 @@
# This config file is used by ros2_control
controller_manager:
ros__parameters:
update_rate: 100 # Hz
arm_controller:
type: joint_trajectory_controller/JointTrajectoryController
gripper_controller:
type: joint_trajectory_controller/JointTrajectoryController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
arm_controller:
ros__parameters:
joints:
- arm_base_joint
- arm_link_1_joint
- arm_link_2_joint
- arm_link_3_joint
- gripper_base_joint
command_interfaces:
- position
state_interfaces:
- position
- velocity
gripper_controller:
ros__parameters:
joints:
- gripper_right_joint
command_interfaces:
- position
state_interfaces:
- position
- velocity

44
unilabos/device_mesh/devices/benyao_arm/joint_limit.yaml Normal file
View File

@@ -0,0 +1,44 @@
joint_limits:
arm_base_joint:
effort: 50
velocity: 1.0
lower: 0
upper: 1.5
arm_link_1_joint:
effort: 50
velocity: 1.0
lower: 0
upper: 0.6
arm_link_2_joint:
effort: 50
velocity: 1.0
lower: !degrees -95
upper: !degrees 95
arm_link_3_joint:
effort: 50
velocity: 1.0
lower: !degrees -195
upper: !degrees 195
gripper_base_joint:
effort: 50
velocity: 1.0
lower: !degrees -95
upper: !degrees 95
gripper_right_joint:
effort: 50
velocity: 1.0
lower: 0
upper: 0.03
gripper_left_joint:
effort: 50
velocity: 1.0
lower: 0
upper: 0.03

293
unilabos/device_mesh/devices/benyao_arm/macro_device.xacro Normal file
View File

@@ -0,0 +1,293 @@
<?xml version="1.0" ?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="benyao_arm">
<xacro:macro name="benyao_arm" params="mesh_path:='' parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 rx:=0 ry:=0 r:=0">
<!-- Read .yaml files from disk, load content into properties -->
<xacro:property name= "joint_limit_parameters" value="${xacro.load_yaml(mesh_path + '/devices/benyao_arm/joint_limit.yaml')}"/>
<!-- Extract subsections from yaml dictionaries -->
<xacro:property name= "sec_limits" value="${joint_limit_parameters['joint_limits']}"/>
<joint name="${station_name}${device_name}base_link_joint" type="fixed">
<origin xyz="${x} ${y} ${z}" rpy="${rx} ${ry} ${r}" />
<parent link="${parent_link}"/>
<child link="${station_name}${device_name}device_link"/>
<axis xyz="0 0 0"/>
</joint>
<link name="${station_name}${device_name}device_link"/>
<joint name="${station_name}${device_name}device_link_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="${station_name}${device_name}device_link"/>
<child link="${station_name}${device_name}arm_slideway"/>
<axis xyz="0 0 0"/>
</joint>
<!-- JOINTS LIMIT PARAMETERS -->
<xacro:property name="limit_arm_base_joint" value="${sec_limits['arm_base_joint']}" />
<xacro:property name="limit_arm_link_1_joint" value="${sec_limits['arm_link_1_joint']}" />
<xacro:property name="limit_arm_link_2_joint" value="${sec_limits['arm_link_2_joint']}" />
<xacro:property name="limit_arm_link_3_joint" value="${sec_limits['arm_link_3_joint']}" />
<xacro:property name="limit_gripper_base_joint" value="${sec_limits['gripper_base_joint']}" />
<xacro:property name="limit_gripper_right_joint" value="${sec_limits['gripper_right_joint']}"/>
<xacro:property name="limit_gripper_left_joint" value="${sec_limits['gripper_left_joint']}" />
<link name="${station_name}${device_name}arm_slideway">
<inertial>
<origin rpy="0 0 0" xyz="-0.913122246354019 -0.00141851388483838 0.0416079172839272"/>
<mass value="13.6578107753627"/>
<inertia ixx="0.0507627640890578" ixy="0.0245166532634714" ixz="-0.0112656803168519" iyy="5.2550852314372" iyz="0.000302974193920367" izz="5.26892263696439"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_slideway.STL"/>
</geometry>
<material name="">
<color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_slideway.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}arm_base_joint" type="prismatic">
<origin rpy="0 0 0" xyz="0.307 0 0.1225"/>
<parent link="${station_name}${device_name}arm_slideway"/>
<child link="${station_name}${device_name}arm_base"/>
<axis xyz="1 0 0"/>
<limit
effort="${limit_arm_base_joint['effort']}"
lower="${limit_arm_base_joint['lower']}"
upper="${limit_arm_base_joint['upper']}"
velocity="${limit_arm_base_joint['velocity']}"/>
</joint>
<link name="${station_name}${device_name}arm_base">
<inertial>
<origin rpy="0 0 0" xyz="1.48458338655733E-06 -0.00831873687136486 0.351728466012153"/>
<mass value="16.1341586205194"/>
<inertia ixx="0.54871651759045" ixy="7.65476367433116E-07" ixz="2.0515139488158E-07" iyy="0.55113098995396" iyz="-5.13261457726806E-07" izz="0.0619081867727048"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_base.STL"/>
</geometry>
<material name="">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_base.STL"/>
</geometry>
</collision>
</link>
<link name="${station_name}${device_name}arm_link_1">
<inertial>
<origin rpy="0 0 0" xyz="0 -0.0102223856758559 0.0348505130779933"/>
<mass value="0.828629227096429"/>
<inertia ixx="0.00119703598787112" ixy="-2.46083048832131E-19" ixz="1.43864352731199E-19" iyy="0.00108355785790042" iyz="1.88092240278693E-06" izz="0.00160914803816438"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_link_1.STL"/>
</geometry>
<material name="">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_link_1.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}arm_link_1_joint" type="prismatic">
<origin rpy="0 0 0" xyz="0 0.1249 0.15"/>
<parent link="${station_name}${device_name}arm_base"/>
<child link="${station_name}${device_name}arm_link_1"/>
<axis xyz="0 0 1"/>
<limit
effort="${limit_arm_link_1_joint['effort']}"
lower="${limit_arm_link_1_joint['lower']}"
upper="${limit_arm_link_1_joint['upper']}"
velocity="${limit_arm_link_1_joint['velocity']}"/>
</joint>
<link name="${station_name}${device_name}arm_link_2">
<inertial>
<origin rpy="0 0 0" xyz="-3.33066907387547E-16 0.100000000000003 -0.0325000000000004"/>
<mass value="2.04764861029349"/>
<inertia ixx="0.0150150059448827" ixy="-1.28113733272213E-17" ixz="6.7561418872754E-19" iyy="0.00262980501315445" iyz="7.44451536320152E-18" izz="0.0162030186138787"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_link_2.STL"/>
</geometry>
<material name="">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_link_2.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}arm_link_2_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="${station_name}${device_name}arm_link_1"/>
<child link="${station_name}${device_name}arm_link_2"/>
<axis xyz="0 0 1"/>
<limit
effort="${limit_arm_link_2_joint['effort']}"
lower="${limit_arm_link_2_joint['lower']}"
upper="${limit_arm_link_2_joint['upper']}"
velocity="${limit_arm_link_2_joint['velocity']}"/>
</joint>
<link name="${station_name}${device_name}arm_link_3">
<inertial>
<origin rpy="0 0 0" xyz="4.77395900588817E-15 0.0861257730831348 -0.0227999999999999"/>
<mass value="1.19870202871083"/>
<inertia ixx="0.00780783223764428" ixy="7.26567379579506E-18" ixz="1.02766851352053E-18" iyy="0.00109642607170081" iyz="-9.73775385060067E-18" izz="0.0084997384510058"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_link_3.STL"/>
</geometry>
<material name="">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/arm_link_3.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}arm_link_3_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0.2 -0.0647"/>
<parent link="${station_name}${device_name}arm_link_2"/>
<child link="${station_name}${device_name}arm_link_3"/>
<axis xyz="0 0 1"/>
<limit
effort="${limit_arm_link_3_joint['effort']}"
lower="${limit_arm_link_3_joint['lower']}"
upper="${limit_arm_link_3_joint['upper']}"
velocity="${limit_arm_link_3_joint['velocity']}"/>
</joint>
<link name="${station_name}${device_name}gripper_base">
<inertial>
<origin rpy="0 0 0" xyz="-6.05365748571618E-05 0.0373027483464434 -0.0264392017534612"/>
<mass value="0.511925198394943"/>
<inertia ixx="0.000640463815051467" ixy="1.08132229596356E-06" ixz="7.165124649009E-07" iyy="0.000552164156414554" iyz="9.80000237347941E-06" izz="0.00103553457812823"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/gripper_base.STL"/>
</geometry>
<material name="">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/gripper_base.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}gripper_base_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0.2 -0.045"/>
<parent link="${station_name}${device_name}arm_link_3"/>
<child link="${station_name}${device_name}gripper_base"/>
<axis xyz="0 0 1"/>
<limit
effort="${limit_gripper_base_joint['effort']}"
lower="${limit_gripper_base_joint['lower']}"
upper="${limit_gripper_base_joint['upper']}"
velocity="${limit_gripper_base_joint['velocity']}"/>
</joint>
<link name="${station_name}${device_name}gripper_right">
<inertial>
<origin rpy="0 0 0" xyz="0.0340005471193899 0.0339655085140826 -0.0325252119823062"/>
<mass value="0.013337481136229"/>
<inertia ixx="2.02427962974094E-05" ixy="1.78442722292145E-06" ixz="-4.36485961300289E-07" iyy="1.4816483393622E-06" iyz="2.60539468115799E-06" izz="1.96629693098755E-05"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/gripper_right.STL"/>
</geometry>
<material name="">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/gripper_right.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}gripper_right_joint" type="prismatic">
<origin rpy="0 0 0" xyz="0 0.0942 -0.022277"/>
<parent link="${station_name}${device_name}gripper_base"/>
<child link="${station_name}${device_name}gripper_right"/>
<axis xyz="1 0 0"/>
<limit
effort="${limit_gripper_right_joint['effort']}"
lower="${limit_gripper_right_joint['lower']}"
upper="${limit_gripper_right_joint['upper']}"
velocity="${limit_gripper_right_joint['velocity']}"/>
</joint>
<link name="${station_name}${device_name}gripper_left">
<inertial>
<origin rpy="0 3.1416 0" xyz="-0.0340005471193521 0.0339655081029604 -0.0325252119827364"/>
<mass value="0.0133374811362292"/>
<inertia ixx="2.02427962974094E-05" ixy="-1.78442720812615E-06" ixz="4.36485961300305E-07" iyy="1.48164833936224E-06" iyz="2.6053946859901E-06" izz="1.96629693098755E-05"/>
</inertial>
<visual>
<origin rpy="0 3.1416 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/gripper_left.STL"/>
</geometry>
<material name="">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 3.1416 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/benyao_arm/meshes/gripper_left.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}gripper_left_joint" type="prismatic">
<origin rpy="0 3.1416 0" xyz="0 0.0942 -0.022277"/>
<parent link="${station_name}${device_name}gripper_base"/>
<child link="${station_name}${device_name}gripper_left"/>
<axis xyz="1 0 0"/>
<limit
effort="${limit_gripper_left_joint['effort']}"
lower="${limit_gripper_left_joint['lower']}"
upper="${limit_gripper_left_joint['upper']}"
velocity="${limit_gripper_left_joint['velocity']}"/>
<mimic joint="${station_name}${device_name}gripper_right_joint" multiplier="1" />
</joint>
</xacro:macro>
</robot>

BIN
unilabos/device_mesh/devices/benyao_arm/meshes/arm_base.STL Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_1.STL Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_2.STL Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/benyao_arm/meshes/arm_link_3.STL Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/benyao_arm/meshes/arm_slideway.STL Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/benyao_arm/meshes/gripper_base.STL Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/benyao_arm/meshes/gripper_left.STL Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/benyao_arm/meshes/gripper_right.STL Normal file
View File

Binary file not shown.

5
unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro
View File

@@ -3,11 +3,10 @@
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="opentrons_liquid_handler"
params="parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 r:=0 mesh_path:=''">
params="parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 rx:=0 ry:=0 r:=0 mesh_path:=''">
<joint name="${station_name}${device_name}base_link_joint" type="fixed">
<origin xyz="${x} ${y} ${z}" rpy="0 0 ${r}" />
<origin xyz="${x} ${y} ${z}" rpy="${rx} ${ry} ${r}" />
<parent link="${parent_link}"/>
<child link="${station_name}${device_name}device_link"/>
<axis xyz="0 0 0"/>

4
unilabos/device_mesh/devices/slide_w140/macro_device.xacro
View File

@@ -8,11 +8,11 @@
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="slide_w140" params="mesh_path:='' length:=0.1 min_d:=0.1 max_d:=0.1 slider_d:=0.14 parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 r:=0" >
<xacro:macro name="slide_w140" params="mesh_path:='' length:=0.1 min_d:=0.1 max_d:=0.1 slider_d:=0.14 parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 rx:=0 ry:=0 r:=0" >
<joint name="${station_name}${device_name}base_link_joint" type="fixed">
<origin xyz="${x} ${y} ${z}" rpy="0 0 ${r}" />
<origin xyz="${x} ${y} ${z}" rpy="${rx} ${ry} ${r}" />
<parent link="${parent_link}"/>
<child link="${station_name}${device_name}device_link"/>
<axis xyz="0 0 0"/>

BIN
unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/hotel.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 59 KiB

59
unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/macro_device.xacro Normal file
View File

@@ -0,0 +1,59 @@
<?xml version='1.0'?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="thermo_orbitor_rs2_hotel"
params="parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 rx:=0 ry:=0 r:=0
mesh_path:=''">
<joint name="${station_name}${device_name}base_link_joint" type="fixed">
<origin xyz="${x} ${y} ${z}" rpy="${rx} ${ry} ${r}" />
<parent link="${parent_link}"/>
<child link="${station_name}${device_name}device_link"/>
<axis xyz="0 0 0"/>
</joint>
<link name="${station_name}${device_name}device_link"/>
<joint name="${station_name}${device_name}device_link_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="${station_name}${device_name}device_link"/>
<child link="${station_name}${device_name}base_link"/>
<axis xyz="0 0 0"/>
</joint>
<link name='${station_name}${device_name}base_link'>
<visual name='visual'>
<origin rpy="-${pi/2} 0 ${pi}" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/thermo_orbitor_rs2_hotel/meshes/hotel.stl"/>
</geometry>
<material name="clay">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="-${pi/2} 0 ${pi}" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/thermo_orbitor_rs2_hotel/meshes/hotel.stl"/>
</geometry>
</collision>
</link>
<link name='${station_name}${device_name}socketTypeGenericSbsFootprint'/>
<xacro:property name="delta" value="0.073" />
<xacro:macro name='platetargets' params='num'>
<joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_${num}_60_1' type='fixed'>
<parent link="${station_name}${device_name}base_link"/>
<child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
<origin rpy="0 0 ${pi/2}" xyz="-0.002 0.011 ${(num - 1) * delta + 0.038}"/>
</joint>
</xacro:macro>
<xacro:platetargets num='1' />
<xacro:platetargets num='2' />
<xacro:platetargets num='3' />
<xacro:platetargets num='4' />
<xacro:platetargets num='5' />
<xacro:platetargets num='6' />
<xacro:platetargets num='7' />
<xacro:platetargets num='8' />
</xacro:macro>
</robot>

BIN
unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meshes/hotel.glb Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meshes/hotel.stl Normal file
View File

Binary file not shown.

10
unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meta.json Normal file
View File

@@ -0,0 +1,10 @@
{
"fileName": "thermo_orbitor_rs2_hotel",
"related": [
"thermo_skyline_stacker",
"thermo_cytomat2c_stacker_15",
"thermo_fisher_cytomat_stacker_16",
"thermo_cytomat2c_stacker_21",
"thermo_orbitor_rs2_hotel"
]
}

10
unilabos/device_mesh/devices/toyo_xyz/config/full_dev.urdf.xacro Normal file
View File

@@ -0,0 +1,10 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="full_dev">
<xacro:arg name="device_name" default=""/>
<xacro:arg name="mesh_path" default="/home/z43/git_pj/Uni-Lab-OS/unilabos/device_mesh" />
<xacro:include filename="macro.ros2_control.xacro" />
<xacro:toyo_xyz_ros2_control device_name="$(arg device_name)" mesh_path="$(arg mesh_path)" />
</robot>

6
unilabos/device_mesh/devices/toyo_xyz/config/initial_positions.yaml Normal file
View File

@@ -0,0 +1,6 @@
# Default initial positions for full_dev's ros2_control fake system
initial_positions:
slider1_joint: 0
slider2_joint: 0
slider3_joint: 0

25
unilabos/device_mesh/devices/toyo_xyz/config/joint_limits.yaml Normal file
View File

@@ -0,0 +1,25 @@
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
# For beginners, we downscale velocity and acceleration limits.
# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
default_velocity_scaling_factor: 0.1
default_acceleration_scaling_factor: 0.1
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
slider1_joint:
has_velocity_limits: false
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0
slider2_joint:
has_velocity_limits: false
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0
slider3_joint:
has_velocity_limits: false
max_velocity: 0
has_acceleration_limits: false
max_acceleration: 0

4
unilabos/device_mesh/devices/toyo_xyz/config/kinematics.yaml Normal file
View File

@@ -0,0 +1,4 @@
toyo_xyz:
kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin
kinematics_solver_search_resolution: 0.0050000000000000001
kinematics_solver_timeout: 0.0050000000000000001

35
unilabos/device_mesh/devices/toyo_xyz/config/macro.ros2_control.xacro Normal file
View File

@@ -0,0 +1,35 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="toyo_xyz_ros2_control" params="device_name mesh_path">
<xacro:property name="initial_positions" value="${load_yaml(mesh_path + '/devices/toyo_xyz/config/initial_positions.yaml')['initial_positions']}"/>
<ros2_control name="${device_name}toyo_xyz" type="system">
<hardware>
<!-- By default, set up controllers for simulation. This won't work on real hardware -->
<plugin>mock_components/GenericSystem</plugin>
</hardware>
<joint name="${device_name}slider1_joint">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['slider1_joint']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="${device_name}slider2_joint">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['slider2_joint']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
<joint name="${device_name}slider3_joint">
<command_interface name="position"/>
<state_interface name="position">
<param name="initial_value">${initial_positions['slider3_joint']}</param>
</state_interface>
<state_interface name="velocity"/>
</joint>
</ros2_control>
</xacro:macro>
</robot>

109
unilabos/device_mesh/devices/toyo_xyz/config/macro.srdf.xacro Normal file
View File

@@ -0,0 +1,109 @@
<?xml version="1.0" encoding="UTF-8"?>
<!--This does not replace URDF, and is not an extension of URDF.
This is a format for representing semantic information about the robot structure.
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="toyo_xyz_srdf" params="device_name">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
<group name="${device_name}toyo_xyz">
<chain base_link="${device_name}base_link" tip_link="${device_name}slider3_link"/>
</group>
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="${device_name}base2_link" link2="${device_name}base3_link" reason="Never"/>
<disable_collisions link1="${device_name}base2_link" link2="${device_name}base_link" reason="Never"/>
<disable_collisions link1="${device_name}base2_link" link2="${device_name}chain_link" reason="Never"/>
<disable_collisions link1="${device_name}base2_link" link2="${device_name}end2_link" reason="Never"/>
<disable_collisions link1="${device_name}base2_link" link2="${device_name}end3_link" reason="Never"/>
<disable_collisions link1="${device_name}base2_link" link2="${device_name}end_link" reason="Never"/>
<disable_collisions link1="${device_name}base2_link" link2="${device_name}fixed_link" reason="Never"/>
<disable_collisions link1="${device_name}base2_link" link2="${device_name}length1_link" reason="Never"/>
<disable_collisions link1="${device_name}base2_link" link2="${device_name}length2_link" reason="Default"/>
<disable_collisions link1="${device_name}base2_link" link2="${device_name}length3_link" reason="Never"/>
<disable_collisions link1="${device_name}base2_link" link2="${device_name}slider1_link" reason="Adjacent"/>
<disable_collisions link1="${device_name}base2_link" link2="${device_name}slider2_link" reason="Never"/>
<disable_collisions link1="${device_name}base2_link" link2="${device_name}slider3_link" reason="Never"/>
<disable_collisions link1="${device_name}base3_link" link2="${device_name}base_link" reason="Never"/>
<disable_collisions link1="${device_name}base3_link" link2="${device_name}chain_link" reason="Never"/>
<disable_collisions link1="${device_name}base3_link" link2="${device_name}end2_link" reason="Never"/>
<disable_collisions link1="${device_name}base3_link" link2="${device_name}end3_link" reason="Never"/>
<disable_collisions link1="${device_name}base3_link" link2="${device_name}end_link" reason="Never"/>
<disable_collisions link1="${device_name}base3_link" link2="${device_name}fixed_link" reason="Never"/>
<disable_collisions link1="${device_name}base3_link" link2="${device_name}length1_link" reason="Never"/>
<disable_collisions link1="${device_name}base3_link" link2="${device_name}length2_link" reason="Never"/>
<disable_collisions link1="${device_name}base3_link" link2="${device_name}length3_link" reason="Never"/>
<disable_collisions link1="${device_name}base3_link" link2="${device_name}slider1_link" reason="Never"/>
<disable_collisions link1="${device_name}base3_link" link2="${device_name}slider2_link" reason="Adjacent"/>
<disable_collisions link1="${device_name}base3_link" link2="${device_name}slider3_link" reason="Never"/>
<disable_collisions link1="${device_name}base_link" link2="${device_name}chain_link" reason="Never"/>
<disable_collisions link1="${device_name}base_link" link2="${device_name}end2_link" reason="Never"/>
<disable_collisions link1="${device_name}base_link" link2="${device_name}end3_link" reason="Never"/>
<disable_collisions link1="${device_name}base_link" link2="${device_name}end_link" reason="Adjacent"/>
<disable_collisions link1="${device_name}base_link" link2="${device_name}fixed_link" reason="Never"/>
<disable_collisions link1="${device_name}base_link" link2="${device_name}length1_link" reason="Adjacent"/>
<disable_collisions link1="${device_name}base_link" link2="${device_name}length2_link" reason="Never"/>
<disable_collisions link1="${device_name}base_link" link2="${device_name}length3_link" reason="Never"/>
<disable_collisions link1="${device_name}base_link" link2="${device_name}slider1_link" reason="Adjacent"/>
<disable_collisions link1="${device_name}base_link" link2="${device_name}slider2_link" reason="Never"/>
<disable_collisions link1="${device_name}base_link" link2="${device_name}slider3_link" reason="Never"/>
<disable_collisions link1="${device_name}chain_link" link2="${device_name}end2_link" reason="Never"/>
<disable_collisions link1="${device_name}chain_link" link2="${device_name}end3_link" reason="Never"/>
<disable_collisions link1="${device_name}chain_link" link2="${device_name}end_link" reason="Never"/>
<disable_collisions link1="${device_name}chain_link" link2="${device_name}fixed_link" reason="Default"/>
<disable_collisions link1="${device_name}chain_link" link2="${device_name}length1_link" reason="Never"/>
<disable_collisions link1="${device_name}chain_link" link2="${device_name}length2_link" reason="Never"/>
<disable_collisions link1="${device_name}chain_link" link2="${device_name}length3_link" reason="Never"/>
<disable_collisions link1="${device_name}chain_link" link2="${device_name}slider1_link" reason="Adjacent"/>
<disable_collisions link1="${device_name}chain_link" link2="${device_name}slider2_link" reason="Never"/>
<disable_collisions link1="${device_name}chain_link" link2="${device_name}slider3_link" reason="Never"/>
<disable_collisions link1="${device_name}end2_link" link2="${device_name}end3_link" reason="Never"/>
<disable_collisions link1="${device_name}end2_link" link2="${device_name}end_link" reason="Never"/>
<disable_collisions link1="${device_name}end2_link" link2="${device_name}fixed_link" reason="Never"/>
<disable_collisions link1="${device_name}end2_link" link2="${device_name}length1_link" reason="Never"/>
<disable_collisions link1="${device_name}end2_link" link2="${device_name}length2_link" reason="Default"/>
<disable_collisions link1="${device_name}end2_link" link2="${device_name}length3_link" reason="Never"/>
<disable_collisions link1="${device_name}end2_link" link2="${device_name}slider1_link" reason="Adjacent"/>
<disable_collisions link1="${device_name}end2_link" link2="${device_name}slider2_link" reason="Never"/>
<disable_collisions link1="${device_name}end2_link" link2="${device_name}slider3_link" reason="Never"/>
<disable_collisions link1="${device_name}end3_link" link2="${device_name}end_link" reason="Never"/>
<disable_collisions link1="${device_name}end3_link" link2="${device_name}fixed_link" reason="Never"/>
<disable_collisions link1="${device_name}end3_link" link2="${device_name}length1_link" reason="Never"/>
<disable_collisions link1="${device_name}end3_link" link2="${device_name}length2_link" reason="Never"/>
<disable_collisions link1="${device_name}end3_link" link2="${device_name}length3_link" reason="Default"/>
<disable_collisions link1="${device_name}end3_link" link2="${device_name}slider1_link" reason="Never"/>
<disable_collisions link1="${device_name}end3_link" link2="${device_name}slider2_link" reason="Adjacent"/>
<disable_collisions link1="${device_name}end3_link" link2="${device_name}slider3_link" reason="Never"/>
<disable_collisions link1="${device_name}end_link" link2="${device_name}fixed_link" reason="Never"/>
<disable_collisions link1="${device_name}end_link" link2="${device_name}length1_link" reason="Default"/>
<disable_collisions link1="${device_name}end_link" link2="${device_name}length2_link" reason="Never"/>
<disable_collisions link1="${device_name}end_link" link2="${device_name}length3_link" reason="Never"/>
<disable_collisions link1="${device_name}end_link" link2="${device_name}slider1_link" reason="Never"/>
<disable_collisions link1="${device_name}end_link" link2="${device_name}slider2_link" reason="Never"/>
<disable_collisions link1="${device_name}end_link" link2="${device_name}slider3_link" reason="Never"/>
<disable_collisions link1="${device_name}fixed_link" link2="${device_name}length1_link" reason="Never"/>
<disable_collisions link1="${device_name}fixed_link" link2="${device_name}length2_link" reason="Never"/>
<disable_collisions link1="${device_name}fixed_link" link2="${device_name}length3_link" reason="Default"/>
<disable_collisions link1="${device_name}fixed_link" link2="${device_name}slider1_link" reason="Never"/>
<disable_collisions link1="${device_name}fixed_link" link2="${device_name}slider2_link" reason="Adjacent"/>
<disable_collisions link1="${device_name}fixed_link" link2="${device_name}slider3_link" reason="Never"/>
<disable_collisions link1="${device_name}length1_link" link2="${device_name}length2_link" reason="Never"/>
<disable_collisions link1="${device_name}length1_link" link2="${device_name}length3_link" reason="Never"/>
<disable_collisions link1="${device_name}length1_link" link2="${device_name}slider1_link" reason="Never"/>
<disable_collisions link1="${device_name}length1_link" link2="${device_name}slider2_link" reason="Never"/>
<disable_collisions link1="${device_name}length1_link" link2="${device_name}slider3_link" reason="Never"/>
<disable_collisions link1="${device_name}length2_link" link2="${device_name}length3_link" reason="Never"/>
<disable_collisions link1="${device_name}length2_link" link2="${device_name}slider1_link" reason="Adjacent"/>
<disable_collisions link1="${device_name}length2_link" link2="${device_name}slider2_link" reason="Never"/>
<disable_collisions link1="${device_name}length2_link" link2="${device_name}slider3_link" reason="Never"/>
<disable_collisions link1="${device_name}length3_link" link2="${device_name}slider1_link" reason="Never"/>
<disable_collisions link1="${device_name}length3_link" link2="${device_name}slider2_link" reason="Adjacent"/>
<disable_collisions link1="${device_name}length3_link" link2="${device_name}slider3_link" reason="Never"/>
<disable_collisions link1="${device_name}slider1_link" link2="${device_name}slider2_link" reason="Adjacent"/>
<disable_collisions link1="${device_name}slider1_link" link2="${device_name}slider3_link" reason="Never"/>
<disable_collisions link1="${device_name}slider2_link" link2="${device_name}slider3_link" reason="Adjacent"/>
</xacro:macro>
</robot>

12
unilabos/device_mesh/devices/toyo_xyz/config/move_group.json Normal file
View File

@@ -0,0 +1,12 @@
{
"toyo_xyz":
{
"joint_names": [
"slider1_joint",
"slider2_joint",
"slider3_joint"
],
"base_link_name": "device_link",
"end_effector_name": "slider3_link"
}
}

16
unilabos/device_mesh/devices/toyo_xyz/config/moveit_controllers.yaml Normal file
View File

@@ -0,0 +1,16 @@
# MoveIt uses this configuration for controller management
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager:
controller_names:
- toyo_xyz_controller
toyo_xyz_controller:
type: FollowJointTrajectory
action_ns: follow_joint_trajectory
default: true
joints:
- slider1_joint
- slider2_joint
- slider3_joint

6
unilabos/device_mesh/devices/toyo_xyz/config/pilz_cartesian_limits.yaml Normal file
View File

@@ -0,0 +1,6 @@
# Limits for the Pilz planner
cartesian_limits:
max_trans_vel: 1.0
max_trans_acc: 2.25
max_trans_dec: -5.0
max_rot_vel: 1.57

34
unilabos/device_mesh/devices/toyo_xyz/config/ros2_controllers.yaml Normal file
View File

@@ -0,0 +1,34 @@
# This config file is used by ros2_control
controller_manager:
ros__parameters:
update_rate: 100 # Hz
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
# action_ns: $(var device_id)
toyo_xyz_controller:
type: joint_trajectory_controller/JointTrajectoryController
# joint_state_broadcaster:
# ros__parameters: {}
toyo_xyz_controller:
ros__parameters:
joints:
- slider1_joint
- slider2_joint
- slider3_joint
command_interfaces:
- position
state_interfaces:
- position
- velocity
allow_partial_joints_goal: false
open_loop_control: true
allow_integration_in_goal_trajectories: true
action_monitor_rate: 20.0
# goal_time: 0.0
# constraints:
# stopped_velocity_tolerance: 0.01
# goal_time: 0.0

14
unilabos/device_mesh/devices/toyo_xyz/joint_config.json Normal file
View File

@@ -0,0 +1,14 @@
{
"slider1_joint": {
"child":"slider1_link",
"axis" : "x"
},
"slider2_joint": {
"child":"slider2_link",
"axis" : "x"
},
"slider3_joint": {
"child":"slider3_link",
"axis" : "x"
}
}

465
unilabos/device_mesh/devices/toyo_xyz/macro_device.xacro Normal file
View File

@@ -0,0 +1,465 @@
<?xml version="1.0" ?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="toyo_xyz"
params="length1:=0.1 min_d1:=0.01 max_d1:=0.01 slider_d1:=0.135
length2:=0.1 min_d2:=0.01 max_d2:=0.01 slider_d2:=0.116
length3:=0.1 min_d3:=0.01 max_d3:=0.01 slider_d3:=0.090
parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 rx:=0 ry:=0 r:=0
mesh_path:=''">
<joint name="${station_name}${device_name}base_link_joint" type="fixed">
<origin xyz="${x} ${y} ${z}" rpy="${rx} ${ry} ${r}" />
<parent link="${parent_link}"/>
<child link="${station_name}${device_name}device_link"/>
<axis xyz="0 0 0"/>
</joint>
<link name="${station_name}${device_name}device_link"/>
<joint name="${station_name}${device_name}device_link_joint" type="fixed">
<origin xyz="${-min_d1} 0 0" rpy="0 0 0" />
<parent link="${station_name}${device_name}device_link"/>
<child link="${station_name}${device_name}base_link"/>
<axis xyz="0 0 0"/>
</joint>
<link name="${station_name}${device_name}base_link">
<inertial>
<origin rpy="0 0 0" xyz="-0.0120236017581094 -0.0517106842624209 0.0409192044637189"/>
<mass value="0.69481680376074"/>
<inertia ixx="0.000936712501344659" ixy="7.23677144617562E-06" ixz="-1.24166862138852E-06" iyy="0.000379264833590696" iyz="9.52650081379508E-07" izz="0.000898185165939333"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base_link.STL"/>
</geometry>
<material name="">
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base_link.STL"/>
</geometry>
</collision>
</link>
<link name="${station_name}${device_name}length1_link">
<inertial>
<origin rpy="0 0 0" xyz="0.494579219297241 -0.0104662192149046 0.020043711867505"/>
<mass value="5.94417515905243"/>
<inertia ixx="0.00674507129097306" ixy="9.38238612394185E-09" ixz="4.35684774317116E-06" iyy="0.485770401104387" iyz="2.57353070463078E-06" izz="0.489757831272925"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/length1_link.STL" scale="${(length1 + min_d1 + max_d1 + slider_d1)} 1 1"/>
</geometry>
<material name="">
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/length1_link.STL" scale="${(length1 + min_d1 + max_d1 + slider_d1)} 1 1"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}length1_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="${station_name}${device_name}base_link"/>
<child link="${station_name}${device_name}length1_link"/>
<axis xyz="0 0 1"/>
</joint>
<link name="${station_name}${device_name}slider1_link">
<inertial>
<origin rpy="0 0 0" xyz="0 0 -0.00730412052530301"/>
<mass value="0.118155140657232"/>
<inertia ixx="6.43342434854928E-05" ixy="-2.31210361323828E-11" ixz="3.06922820322201E-10" iyy="0.000154199389116579" iyz="1.20524516927812E-08" izz="0.000215806482689218"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider1_link.STL"/>
</geometry>
<material name="">
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider1_link.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}slider1_joint" type="prismatic">
<origin rpy="0 0 0" xyz="${min_d1 + slider_d1/2} 0 0.078"/>
<parent link="${station_name}${device_name}base_link"/>
<child link="${station_name}${device_name}slider1_link"/>
<axis xyz="1 0 0"/>
<limit effort="0" lower="0" upper="${length1}" velocity="0"/>
</joint>
<link name="${station_name}${device_name}chain_link">
<inertial>
<origin rpy="0 0 0" xyz="0.49922930196331 -0.00327071484975672 0.0207539916460562"/>
<mass value="2.00787563607981"/>
<inertia ixx="0.00240786362662634" ixy="1.44801915606072E-05" ixz="-3.1726019259437E-05" iyy="0.167850452350525" iyz="-0.000138217054257536" izz="0.168373005725347"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/chain_link.STL" scale="${(length2 + min_d2 + max_d2 + slider_d2)} 1 1"/>
</geometry>
<material name="">
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/chain_link.STL" scale="${(length2 + min_d2 + max_d2 + slider_d2)} 1 1"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}chain_joint" type="fixed">
<origin rpy="0 0 1.5708" xyz="-0.02 -0.041 -0.0005"/>
<parent link="${station_name}${device_name}slider1_link"/>
<child link="${station_name}${device_name}chain_link"/>
<axis xyz="0 0 0"/>
</joint>
<link name="${station_name}${device_name}base2_link">
<inertial>
<origin rpy="0 0 0" xyz="-0.00645898766177746 0.0354023221709439 0.0277640039757229"/>
<mass value="0.341816215620884"/>
<inertia ixx="0.000246380769664854" ixy="-3.5162220656232E-06" ixz="3.72156984182819E-08" iyy="0.00014938476423929" iyz="-2.01567401863811E-06" izz="0.000271539741067036"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base2_link.STL"/>
</geometry>
<material name="">
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base2_link.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}base2_joint" type="fixed">
<origin rpy="1.5708 0 1.5708" xyz="-0.0145 0.0295 0.04915"/>
<parent link="${station_name}${device_name}slider1_link"/>
<child link="${station_name}${device_name}base2_link"/>
<axis xyz="0 0 0"/>
</joint>
<link name="${station_name}${device_name}slider2_link">
<inertial>
<origin rpy="0 0 0" xyz="-1.94863440558524E-06 -4.2412482447185E-05 -0.00600639156586869"/>
<mass value="0.0568102419437891"/>
<inertia ixx="1.29719509999494E-05" ixy="-2.84670156002291E-09" ixz="-1.99529353027075E-10" iyy="5.43277686573641E-05" iyz="-5.94709690026503E-09" izz="6.6299564390705E-05"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider2_link.STL"/>
</geometry>
<material name="">
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider2_link.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}slider2_joint" type="prismatic">
<origin rpy="1.5708 0 1.5708" xyz="0.0455 ${min_d2 + slider_d2/2 + 0.0295} 0.049"/>
<parent link="${station_name}${device_name}slider1_link"/>
<child link="${station_name}${device_name}slider2_link"/>
<axis xyz="1 0 0"/>
<limit effort="0" lower="0" upper="${length2}" velocity="0"/>
</joint>
<link name="${station_name}${device_name}length2_link">
<inertial>
<origin
xyz="0.495 -0.00012803097983341 0.0216248093421714"
rpy="0 0 0" />
<mass value="2.0281266583716" />
<inertia
ixx="0.000735867784216875"
ixy="4.46823528502648E-18"
ixz="1.62564733709183E-17"
iyy="0.166018547616233"
iyz="8.35811182464874E-07"
izz="0.166011809812984" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/length2_link.STL" scale="${(length2 + min_d2 + max_d2 + slider_d2)} 1 1"/>
</geometry>
<material name="">
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/length2_link.STL" scale="${(length2 + min_d2 + max_d2 + slider_d2)} 1 1"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}length2_joint" type="fixed">
<origin xyz="-0.0145 0.0295 0.04915" rpy="1.5708 0 1.5708" />
<parent link="${station_name}${device_name}slider1_link" />
<child link="${station_name}${device_name}length2_link" />
<axis xyz="0 0 0" />
</joint>
<link name="${station_name}${device_name}fixed_link">
<inertial>
<origin rpy="0 0 0" xyz="0.0841027924880233 -1.38777878078145E-17 -0.00753342821771308"/>
<mass value="0.114714657110459"/>
<inertia ixx="4.09226560926746E-05" ixy="-1.19246814602201E-20" ixz="1.10812310425511E-07" iyy="0.000219464025912457" iyz="-3.47441993328604E-22" izz="0.000257639354963189"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/fixed_link.STL"/>
</geometry>
<material name="">
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/fixed_link.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}fixed_joint" type="fixed">
<origin rpy="0 0 -1.5708" xyz="0 0.113 0.013"/>
<parent link="${station_name}${device_name}slider2_link"/>
<child link="${station_name}${device_name}fixed_link"/>
<axis xyz="0 0 0"/>
</joint>
<link name="${station_name}${device_name}base3_link">
<inertial>
<origin rpy="0 0 0" xyz="-0.0898266891415951 -0.000686554465181816 0.0214841690633115"/>
<mass value="0.253768676399038"/>
<inertia ixx="7.3143781299483E-05" ixy="-9.27468544438197E-07" ixz="-6.25920202213005E-07" iyy="0.000222729105415939" iyz="1.73297633775367E-06" izz="0.000226940773440045"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base3_link.STL"/>
</geometry>
<material name="">
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/base3_link.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}base3_joint" type="fixed">
<origin rpy="0 0 -1.5708" xyz="0 0.113 0.013"/>
<parent link="${station_name}${device_name}slider2_link"/>
<child link="${station_name}${device_name}base3_link"/>
<axis xyz="0 0 0"/>
</joint>
<link name="${station_name}${device_name}length3_link">
<inertial>
<origin rpy="0 0 0" xyz="0.495 2.22485346140083E-05 0.017182121490278"/>
<mass value="1.46660245378617"/>
<inertia ixx="0.000378412905544255" ixy="6.68641853450019E-20" ixz="1.30946080093724E-18" iyy="0.119979432170624" iyz="5.03952092605041E-07" izz="0.11996849156089"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/length3_link.STL" scale="${(length3 + min_d3 + max_d3 + slider_d3)} 1 1"/>
</geometry>
<material name="">
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/length3_link.STL" scale="${(length3 + min_d3 + max_d3 + slider_d3)} 1 1"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}length3_joint" type="fixed">
<origin rpy="0 0 -1.5708" xyz="0 0.113 0.013"/>
<parent link="${station_name}${device_name}slider2_link"/>
<child link="${station_name}${device_name}length3_link"/>
<axis xyz="0 0 0"/>
</joint>
<link name="${station_name}${device_name}slider3_link">
<inertial>
<origin rpy="0 0 0" xyz="6.92336766672241E-06 2.33762226242717E-09 -0.0056316911606703"/>
<mass value="0.0319537658681183"/>
<inertia ixx="4.74895305241407E-06" ixy="-2.68838717157416E-13" ixz="4.74105113238926E-10" iyy="1.78967089054364E-05" iyz="-4.41481885417567E-13" izz="2.21974556051535E-05"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider3_link.STL"/>
</geometry>
<material name="">
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/slider3_link.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}slider3_joint" type="prismatic">
<origin rpy="0 0 -1.5708" xyz="0 ${0.113 - min_d3 - slider_d3/2} 0.0635"/>
<parent link="${station_name}${device_name}slider2_link"/>
<child link="${station_name}${device_name}slider3_link"/>
<axis xyz="1 0 0"/>
<limit effort="0" lower="0" upper="${length3}" velocity="0"/>
</joint>
<link name="${station_name}${device_name}end3_link">
<inertial>
<origin rpy="0 0 0" xyz="0.00497294496885192 -2.44043265976157E-06 0.0183349877681029"/>
<mass value="0.0140489551253671"/>
<inertia ixx="3.43932725883609E-06" ixy="4.00452842192135E-11" ixz="-1.53817578472123E-08" iyy="1.94727500633638E-06" iyz="-3.82376052540752E-10" izz="1.72263562631362E-06"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end3_link.STL"/>
</geometry>
<material name="">
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end3_link.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}end3_joint" type="fixed">
<origin rpy="0 0 -1.5708" xyz="0 -${min_d3 + slider_d3 + length3 + max_d3 -0.113} 0.013"/>
<parent link="${station_name}${device_name}slider2_link"/>
<child link="${station_name}${device_name}end3_link"/>
<axis xyz="0 0 0"/>
</joint>
<link name="${station_name}${device_name}end2_link">
<inertial>
<origin rpy="0 0 0" xyz="0.00495670363919842 -8.74633492434218E-06 0.0214414396703796"/>
<mass value="0.0200788183514264"/>
<inertia ixx="7.16397592830092E-06" ixy="4.75643325623778E-11" ixz="-2.20469268570818E-08" iyy="3.90488015971723E-06" iyz="-3.17180365916489E-09" izz="3.58779761734039E-06"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end2_link.STL"/>
</geometry>
<material name="">
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end2_link.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}end2_joint" type="fixed">
<origin rpy="1.5708 0 1.5708" xyz="-0.0135 ${min_d2 + slider_d2 + length2 + max_d2 + 0.0295} 0.0492"/>
<parent link="${station_name}${device_name}slider1_link"/>
<child link="${station_name}${device_name}end2_link"/>
<axis xyz="0 0 0"/>
</joint>
<link name="${station_name}${device_name}end_link">
<inertial>
<origin rpy="0 0 0" xyz="0.00744127217617607 -2.30544170541074E-06 0.0287548952899474"/>
<mass value="0.0648209796507272"/>
<inertia ixx="4.85939921252094E-05" ixy="1.05486327324319E-09" ixz="-4.17427158603031E-08" iyy="2.1549381051207E-05" iyz="2.1129085201095E-09" izz="2.9433994127647E-05"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end_link.STL"/>
</geometry>
<material name="">
<color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="file://${mesh_path}/devices/toyo_xyz/meshes/end_link.STL"/>
</geometry>
</collision>
</link>
<joint name="${station_name}${device_name}end_joint" type="fixed">
<origin rpy="0 0 0" xyz="${min_d1 + length1 + max_d1 + slider_d1} 0 0.001"/>
<parent link="${station_name}${device_name}base_link"/>
<child link="${station_name}${device_name}end_link"/>
<axis xyz="0 0 0"/>
</joint>
</xacro:macro>
</robot>

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/base2_link.STL Executable file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/base2_link.fbx Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/base3_link.STL Executable file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/base3_link.fbx Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/base_link.STL Executable file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/base_link.fbx Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/chain_link.STL Executable file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/chain_link.fbx Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/end2_link.STL Executable file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/end2_link.fbx Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/end3_link.STL Executable file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/end3_link.fbx Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/end_link.STL Executable file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/end_link.fbx Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/fixed_link.STL Executable file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/fixed_link.fbx Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/length1_link.STL Executable file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/length1_link.fbx Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/length2_link.STL Executable file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/length2_link.fbx Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/length3_link.STL Executable file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/length3_link.fbx Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/slider1_link.STL Executable file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/slider1_link.fbx Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/slider2_link.STL Executable file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/slider2_link.fbx Normal file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/slider3_link.STL Executable file
View File

Binary file not shown.

BIN
unilabos/device_mesh/devices/toyo_xyz/meshes/slider3_link.fbx Normal file
View File

Binary file not shown.

20
unilabos/device_mesh/devices/toyo_xyz/param_config.json Normal file
View File

@@ -0,0 +1,20 @@
{
"private_param":
{
"min_d1": 0.01 ,
"max_d1": 0.01 ,
"slider_d1": 0.135,
"min_d2": 0.01 ,
"max_d2": 0.01 ,
"slider_d2": 0.116,
"min_d3": 0.01 ,
"max_d3": 0.01 ,
"slider_d3": 0.09
},
"public_param":
{
"length1" :0.5,
"length2" :0.2,
"length3" :0.2
}
}

266
unilabos/device_mesh/resource_visalization.py
View File

@@ -1,13 +1,38 @@
import json
import os
from pathlib import Path
import re
import yaml
from launch import LaunchService
from launch import LaunchDescription
from launch_ros.actions import Node as nd
import xacro
from lxml import etree
from launch_param_builder import load_yaml
from launch_ros.parameter_descriptions import ParameterFile
from unilabos.registry.registry import lab_registry
from ament_index_python.packages import get_package_share_directory
def get_pattern_matches(folder, pattern):
"""Given all the files in the folder, find those that match the pattern.
If there are groups defined, the groups are returned. Otherwise the path to the matches are returned.
"""
matches = []
if not folder.exists():
return matches
for child in folder.iterdir():
if not child.is_file():
continue
m = pattern.search(child.name)
if m:
groups = m.groups()
if groups:
matches.append(groups[0])
else:
matches.append(child)
return matches
class ResourceVisualization:
def __init__(self, device: dict, resource: dict, enable_rviz: bool = True):
@@ -31,9 +56,8 @@ class ResourceVisualization:
self.enable_rviz = enable_rviz
registry = lab_registry
self.srdf_str = '''
<?xml version="1.0" encoding="UTF-8"?>
<robot name="minimal">
self.srdf_str = '''<?xml version="1.0" ?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="full_dev">
</robot>
'''
@@ -43,23 +67,46 @@ class ResourceVisualization:
</robot>
'''
self.root = etree.fromstring(self.robot_state_str)
self.root_srdf = etree.fromstring(self.srdf_str)
xacro_uri = self.root.nsmap["xacro"]
self.moveit_nodes = {}
self.moveit_nodes_kinematics = {}
self.moveit_controllers_yaml = {
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
"moveit_simple_controller_manager": {
"controller_names": []
}
}
self.ros2_controllers_yaml = {
"controller_manager": {
"ros__parameters": {
"update_rate": 100,
"joint_state_broadcaster": {
"type": "joint_state_broadcaster/JointStateBroadcaster",
}
}
}
}
# 遍历设备节点
for node in device.values():
if node['type'] == 'device' and node['class'] != '':
device_class = node['class']
# 检查设备类型是否在注册表中
if device_class not in registry.device_type_registry.keys():
raise ValueError(f"设备类型 {device_class} 未在注册表中注册")
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} 未在注册表中注册")
elif "model" in registry.resource_type_registry[resource_class].keys():
model_config = registry.resource_type_registry[resource_class]['model']
if node['type'] in self.resource_type or (node['type'] == 'device' and node['class'] != ''):
model_config = {}
if node['type'] in self.resource_type:
resource_class = node['class']
if resource_class not in registry.resource_type_registry.keys():
raise ValueError(f"资源类型 {resource_class} 未在注册表中注册")
elif "model" in registry.resource_type_registry[resource_class].keys():
model_config = registry.resource_type_registry[resource_class]['model']
elif node['type'] == 'device' and node['class'] != '':
device_class = node['class']
if device_class not in registry.device_type_registry.keys():
raise ValueError(f"设备类型 {device_class} 未在注册表中注册")
elif "model" in registry.device_type_registry[device_class].keys():
model_config = registry.device_type_registry[device_class]['model']
if model_config:
if model_config['type'] == 'resource':
self.resource_model[node['id']] = {
'mesh': f"{str(self.mesh_path)}/resources/{model_config['mesh']}",
@@ -71,18 +118,45 @@ class ResourceVisualization:
'mesh_tf': model_config['children_mesh_tf']
}
elif model_config['type'] == 'device':
new_include = etree.SubElement(self.root, f"{{{xacro_uri}}}include")
new_include.set("filename", f"{str(self.mesh_path)}/devices/{model_config['mesh']}/macro_device.xacro")
new_dev = etree.SubElement(self.root, f"{{{xacro_uri}}}{model_config['mesh']}")
new_dev.set("parent_link", "world")
new_dev.set("mesh_path", str(self.mesh_path))
new_dev.set("device_name", node["id"]+"_")
new_dev.set("station_name", node["parent"]+'_')
# if node["parent"] is not None:
# new_dev.set("station_name", node["parent"]+'_')
print('o'*20)
node["parent"]
node["id"]
print('o'*20)
new_dev.set("x",str(float(node["position"]["x"])/1000))
new_dev.set("y",str(float(node["position"]["y"])/1000))
new_dev.set("z",str(float(node["position"]["z"])/1000))
if "rotation" in node["config"]:
new_dev.set("r",str(float(node["config"]["rotation"]["z"])/1000))
new_dev.set("rx",str(float(node["config"]["rotation"]["x"])))
new_dev.set("ry",str(float(node["config"]["rotation"]["y"])))
new_dev.set("r",str(float(node["config"]["rotation"]["z"])))
if "device_config" in node["config"]:
for key, value in node["config"]["device_config"].items():
new_dev.set(key, str(float(value)))
# 添加ros2_controller
if node['class'].startswith('moveit.'):
new_include_controller = etree.SubElement(self.root, f"{{{xacro_uri}}}include")
new_include_controller.set("filename", f"{str(self.mesh_path)}/devices/{model_config['mesh']}/config/macro.ros2_control.xacro")
new_controller = etree.SubElement(self.root, f"{{{xacro_uri}}}{model_config['mesh']}_ros2_control")
new_controller.set("device_name", node["id"]+"_")
new_controller.set("mesh_path", str(self.mesh_path))
# 添加moveit的srdf
new_include_srdf = etree.SubElement(self.root_srdf, f"{{{xacro_uri}}}include")
new_include_srdf.set("filename", f"{str(self.mesh_path)}/devices/{model_config['mesh']}/config/macro.srdf.xacro")
new_srdf = etree.SubElement(self.root_srdf, f"{{{xacro_uri}}}{model_config['mesh']}_srdf")
new_srdf.set("device_name", node["id"]+"_")
self.moveit_nodes[node["id"]] = model_config['mesh']
else:
print("错误的注册表类型！")
re = etree.tostring(self.root, encoding="unicode")
@@ -90,8 +164,37 @@ class ResourceVisualization:
xacro.process_doc(doc)
self.urdf_str = doc.toxml()
re_srdf = etree.tostring(self.root_srdf, encoding="unicode")
doc_srdf = xacro.parse(re_srdf)
xacro.process_doc(doc_srdf)
self.urdf_str_srdf = doc_srdf.toxml()
if self.moveit_nodes:
self.moveit_init()
def create_launch_description(self, urdf_str: str) -> LaunchDescription:
def moveit_init(self):
for name, config in self.moveit_nodes.items():
controller_dict = yaml.safe_load(open(f"{str(self.mesh_path)}/devices/{config}/config/ros2_controllers.yaml", "r"))
moveit_dict = yaml.safe_load(open(f"{str(self.mesh_path)}/devices/{config}/config/moveit_controllers.yaml", "r"))
kinematics_dict = yaml.safe_load(open(f"{str(self.mesh_path)}/devices/{config}/config/kinematics.yaml", "r"))
for key_kinematics, value_kinematics in kinematics_dict.items():
self.moveit_nodes_kinematics[f'{name}_{key_kinematics}'] = value_kinematics
for key, value in controller_dict['controller_manager']['ros__parameters'].items():
if key == 'update_rate' or key == 'joint_state_broadcaster':
continue
self.ros2_controllers_yaml['controller_manager']['ros__parameters'][f"{name}_{key}"] = value
controller_dict[key]['ros__parameters']['joints'] = [f"{name}_{joint}" for joint in controller_dict[key]['ros__parameters']['joints']]
self.ros2_controllers_yaml[f"{name}_{key}"] = controller_dict[key]
for controller_name in moveit_dict['moveit_simple_controller_manager']['controller_names']:
self.moveit_controllers_yaml['moveit_simple_controller_manager']['controller_names'].append(f"{name}_{controller_name}")
moveit_dict['moveit_simple_controller_manager'][controller_name]['joints'] = [f"{name}_{joint}" for joint in moveit_dict['moveit_simple_controller_manager'][controller_name]['joints']]
self.moveit_controllers_yaml['moveit_simple_controller_manager'][f"{name}_{controller_name}"] = moveit_dict['moveit_simple_controller_manager'][controller_name]
def create_launch_description(self) -> LaunchDescription:
"""
创建launch描述，包含robot_state_publisher和move_group节点
@@ -101,10 +204,93 @@ class ResourceVisualization:
Returns:
LaunchDescription: launch描述对象
"""
moveit_configs_utils_path = Path(get_package_share_directory("moveit_configs_utils"))
default_folder = moveit_configs_utils_path / "default_configs"
planning_pattern = re.compile("^(.*)_planning.yaml$")
pipelines = []
for pipeline in get_pattern_matches(default_folder, planning_pattern):
if pipeline not in pipelines:
pipelines.append(pipeline)
if "ompl" in pipelines:
default_planning_pipeline = "ompl"
else:
default_planning_pipeline = pipelines[0]
planning_pipelines = {
"planning_pipelines": pipelines,
"default_planning_pipeline": default_planning_pipeline,
}
for pipeline in pipelines:
planning_pipelines[pipeline] = load_yaml(
default_folder / f"{pipeline}_planning.yaml"
)
if "ompl" in planning_pipelines:
ompl_config = planning_pipelines["ompl"]
if "planner_configs" not in ompl_config:
ompl_config.update(load_yaml(default_folder / "ompl_defaults.yaml"))
yaml.safe_dump(self.ros2_controllers_yaml, open(f"{str(self.mesh_path)}/ros2_controllers.yaml", "w"))
robot_description_planning = {
"default_velocity_scaling_factor": 0.1,
"default_acceleration_scaling_factor": 0.1,
"cartesian_limits": {
"max_trans_vel": 1.0,
"max_trans_acc": 2.25,
"max_trans_dec": -5.0,
"max_rot_vel": 1.57
}
}
# 解析URDF文件
robot_description = urdf_str
robot_description = self.urdf_str
urdf_str_srdf = self.urdf_str_srdf
kinematics_dict = self.moveit_nodes_kinematics
if self.moveit_nodes:
controllers = []
ros2_controllers = ParameterFile(f"{str(self.mesh_path)}/ros2_controllers.yaml", allow_substs=True)
controllers.append(
nd(
package="controller_manager",
executable="ros2_control_node",
output='screen',
parameters=[
{"robot_description": robot_description},
ros2_controllers,
]
)
)
for controller in self.moveit_controllers_yaml['moveit_simple_controller_manager']['controller_names']:
controllers.append(
nd(
package="controller_manager",
executable="spawner",
arguments=[f"{controller}", "--controller-manager", f"controller_manager"],
output="screen",
)
)
controllers.append(
nd(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster", "--controller-manager", f"controller_manager"],
output="screen",
)
)
for i in controllers:
self.launch_description.add_action(i)
else:
ros2_controllers = None
# 创建robot_state_publisher节点
robot_state_publisher = nd(
@@ -115,23 +301,21 @@ class ResourceVisualization:
parameters=[{
'robot_description': robot_description,
'use_sim_time': False
}]
},
# kinematics_dict
]
)
# joint_state_publisher_node = nd(
# package='joint_state_publisher_gui', # 或 joint_state_publisher
# executable='joint_state_publisher_gui',
# name='joint_state_publisher',
# output='screen'
# )
# 创建move_group节点
move_group = nd(
package='moveit_ros_move_group',
executable='move_group',
output='screen',
parameters=[{
'allow_trajectory_execution': True,
'robot_description': robot_description,
'robot_description_semantic': self.srdf_str,
'robot_description_semantic': urdf_str_srdf,
'robot_description_kinematics': kinematics_dict,
'capabilities': '',
'disable_capabilities': '',
'monitor_dynamics': False,
@@ -141,7 +325,13 @@ class ResourceVisualization:
'publish_geometry_updates': True,
'publish_state_updates': True,
'publish_transforms_updates': True,
}]
# 'robot_description_planning': robot_description_planning,
},
self.moveit_controllers_yaml,
# ompl_planning_pipeline_config,
robot_description_planning,
planning_pipelines,
]
)
# 将节点添加到launch描述中
@@ -156,7 +346,14 @@ class ResourceVisualization:
executable='rviz2',
name='rviz2',
arguments=['-d', f"{str(self.mesh_path)}/view_robot.rviz"],
output='screen'
output='screen',
parameters=[
{'robot_description_kinematics': kinematics_dict,
},
robot_description_planning,
planning_pipelines,
]
)
self.launch_description.add_action(rviz_node)
@@ -169,6 +366,11 @@ class ResourceVisualization:
Args:
urdf_str: URDF文件路径
"""
launch_description = self.create_launch_description(self.urdf_str)
launch_description = self.create_launch_description()
# print('--------------------------------')
# print(self.moveit_controllers_yaml)
# print('--------------------------------')
# print(self.urdf_str)
# print('--------------------------------')
self.launch_service.include_launch_description(launch_description)
self.launch_service.run()

31
unilabos/device_mesh/ros2_controllers.yaml Normal file
View File

@@ -0,0 +1,31 @@
benyao_arm_controller:
ros__parameters:
command_interfaces:
- position
joints:
- benyao_arm_base_joint
- benyao_arm_link_1_joint
- benyao_arm_link_2_joint
- benyao_arm_link_3_joint
- benyao_gripper_base_joint
state_interfaces:
- position
- velocity
benyao_gripper_controller:
ros__parameters:
command_interfaces:
- position
joints:
- benyao_gripper_right_joint
state_interfaces:
- position
- velocity
controller_manager:
ros__parameters:
benyao_arm_controller:
type: joint_trajectory_controller/JointTrajectoryController
benyao_gripper_controller:
type: joint_trajectory_controller/JointTrajectoryController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
update_rate: 100

262
unilabos/device_mesh/view_robot.rviz
View File

@@ -1,23 +1,15 @@
Panels:
- Class: rviz_common/Displays
Help Height: 138
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /TF1
- /TF1/Tree1
- /RobotModel1
- /PlanningScene1
- /PlanningScene1/Scene Geometry1
- /RobotState1
- /RobotState1/Links1
- /MotionPlanning1
- /MotionPlanning1/Scene Geometry1
- /MotionPlanning1/Scene Robot1
- /MotionPlanning1/Planning Request1
Splitter Ratio: 0.5
Tree Height: 345
Splitter Ratio: 0.5016146302223206
Tree Height: 1112
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@@ -93,7 +85,7 @@ Visualization Manager:
Value: false
Visual Enabled: true
- Class: moveit_rviz_plugin/PlanningScene
Enabled: false
Enabled: true
Move Group Namespace: ""
Name: PlanningScene
Planning Scene Topic: /monitored_planning_scene
@@ -104,7 +96,7 @@ Visualization Manager:
Scene Display Time: 0.009999999776482582
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Voxel Rendering: Disabled
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
@@ -113,10 +105,108 @@ Visualization Manager:
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
PLR_STATION_deck_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
PLR_STATION_deck_first_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
PLR_STATION_deck_fourth_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
PLR_STATION_deck_main_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
PLR_STATION_deck_second_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
PLR_STATION_deck_socketTypeGenericSbsFootprint:
Alpha: 1
Show Axes: false
Show Trail: false
PLR_STATION_deck_socketTypeHEPAModule:
Alpha: 1
Show Axes: false
Show Trail: false
PLR_STATION_deck_third_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_link_1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_link_2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_link_3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_slideway:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
benyao_gripper_base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_gripper_left:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_gripper_right:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
hotel_base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
hotel_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
hotel_socketTypeGenericSbsFootprint:
Alpha: 1
Show Axes: false
Show Trail: false
world:
Alpha: 1
Show Axes: false
Show Trail: false
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: false
Value: false
Value: true
- Attached Body Color: 150; 50; 150
Class: moveit_rviz_plugin/RobotState
Collision Enabled: false
@@ -203,6 +293,63 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
benyao_arm_base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_link_1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_link_2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_link_3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_slideway:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
benyao_gripper_base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_gripper_left:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_gripper_right:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
hotel_base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
hotel_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
hotel_socketTypeGenericSbsFootprint:
Alpha: 1
Show Axes: false
Show Trail: false
world:
Alpha: 1
Show Axes: false
@@ -230,7 +377,7 @@ Visualization Manager:
Goal State Color: 250; 128; 0
Interactive Marker Size: 0
Joint Violation Color: 255; 0; 255
Planning Group: ""
Planning Group: benyao_arm
Query Goal State: false
Query Start State: false
Show Workspace: false
@@ -242,9 +389,9 @@ Visualization Manager:
Scene Alpha: 0.8999999761581421
Scene Color: 50; 230; 50
Scene Display Time: 0.009999999776482582
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Show Scene Geometry: false
Voxel Coloring: Cell Probability
Voxel Rendering: All Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
@@ -290,6 +437,63 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
benyao_arm_base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_link_1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_link_2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_link_3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_arm_slideway:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
benyao_gripper_base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_gripper_left:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
benyao_gripper_right:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
hotel_base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
hotel_device_link:
Alpha: 1
Show Axes: false
Show Trail: false
hotel_socketTypeGenericSbsFootprint:
Alpha: 1
Show Axes: false
Show Trail: false
world:
Alpha: 1
Show Axes: false
@@ -345,43 +549,43 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 1.0284695625305176
Distance: 2.622864246368408
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.29730814695358276
Y: 0.21228469908237457
Z: 0.20008830726146698
X: -0.2880733013153076
Y: -0.16004444658756256
Z: -0.16730672121047974
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.38979560136795044
Pitch: 0.48479583859443665
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.06074193865060806
Yaw: 0.042561568319797516
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1656
Height: 2032
Hide Left Dock: false
Hide Right Dock: true
MotionPlanning:
collapsed: false
MotionPlanning - Trajectory Slider:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000400000000000003a3000005dcfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000002510000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000002cb0000037f000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000627000005dc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000003a30000079bfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000027000004c60000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000002600000026fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000004f9000002c9000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000bc50000079b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 2518
X: 385
Y: 120
Width: 3956
X: 140
Y: 54

3760
unilabos/devices/liquid_handling/complete_biomek_protocol.json Normal file
View File

File diff suppressed because it is too large Load Diff

279
unilabos/devices/liquid_handling/complete_biomek_protocol_0608.json
View File

@@ -643,6 +643,222 @@
]
}
],
"P12": [
{
"Properties": {
"Name": "working plate on P12",
"Device": "",
"liquidtype": "Water",
"BarCode": "",
"SenseEveryTime": false
},
"Known": true,
"Class": "LabwareClasses\\BCDeep96Round",
"DataSets": {
"Volume": {}
},
"RuntimeDataSets": {
"Volume": {}
},
"EvalAmounts": [
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,
0
],
"Nominal": false,
"EvalLiquids": [
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water",
"Water"
]
}
],
"P13": [
{
"Properties": {
@@ -1110,9 +1326,9 @@
"Pod": "Pod1",
"items": [
{
"Position": "P1",
"Position": "P12",
"Height": -2.0,
"Volume": "50",
"Volume": "40",
"liquidtype": "Well Contents",
"WellsX": 12,
"LabwareClass": "Matrix96_750uL",
@@ -1219,7 +1435,6 @@
true,
true,
true,
true,
true
],
"Prototype": "MC P300 High",
@@ -1236,9 +1451,9 @@
"UseExpression": false
},
{
"Position": "P11",
"Position": "P14",
"Height": -2.0,
"Volume": "50",
"Volume": "40",
"liquidtype": "Tip Contents",
"WellsX": 12,
"LabwareClass": "Matrix96_750uL",
@@ -1411,9 +1626,9 @@
"Pod": "Pod1",
"items": [
{
"Position": "P2",
"Position": "P12",
"Height": -2.0,
"Volume": "100",
"Volume": "40",
"liquidtype": "Well Contents",
"WellsX": 12,
"LabwareClass": "Matrix96_750uL",
@@ -1520,7 +1735,6 @@
true,
true,
true,
true,
true
],
"Prototype": "MC P300 High",
@@ -1537,9 +1751,9 @@
"UseExpression": false
},
{
"Position": "P11",
"Position": "P14",
"Height": -2.0,
"Volume": "100",
"Volume": "40",
"liquidtype": "Tip Contents",
"WellsX": 12,
"LabwareClass": "Matrix96_750uL",
@@ -1743,7 +1957,7 @@
{
"Position": "P12",
"Height": -2.0,
"Volume": "150",
"Volume": "40",
"liquidtype": "Well Contents",
"WellsX": 12,
"LabwareClass": "Matrix96_750uL",
@@ -1850,7 +2064,6 @@
true,
true,
true,
true,
true
],
"Prototype": "MC P300 High",
@@ -1867,9 +2080,9 @@
"UseExpression": false
},
{
"Position": "P22",
"Position": "P14",
"Height": -2.0,
"Volume": "150",
"Volume": "40",
"liquidtype": "Tip Contents",
"WellsX": 12,
"LabwareClass": "Matrix96_750uL",
@@ -2042,9 +2255,9 @@
"Pod": "Pod1",
"items": [
{
"Position": "P3",
"Position": "P12",
"Height": -2.0,
"Volume": "400",
"Volume": "40",
"liquidtype": "Well Contents",
"WellsX": 12,
"LabwareClass": "Matrix96_750uL",
@@ -2151,7 +2364,6 @@
true,
true,
true,
true,
true
],
"Prototype": "MC P300 High",
@@ -2168,9 +2380,9 @@
"UseExpression": false
},
{
"Position": "P12",
"Position": "P14",
"Height": -2.0,
"Volume": "400",
"Volume": "40",
"liquidtype": "Tip Contents",
"WellsX": 12,
"LabwareClass": "Matrix96_750uL",
@@ -2383,7 +2595,7 @@
{
"Position": "P12",
"Height": -2.0,
"Volume": "400",
"Volume": "40",
"liquidtype": "Well Contents",
"WellsX": 12,
"LabwareClass": "Matrix96_750uL",
@@ -2490,7 +2702,6 @@
true,
true,
true,
true,
true
],
"Prototype": "MC P300 High",
@@ -2507,9 +2718,9 @@
"UseExpression": false
},
{
"Position": "P22",
"Position": "P14",
"Height": -2.0,
"Volume": "400",
"Volume": "40",
"liquidtype": "Tip Contents",
"WellsX": 12,
"LabwareClass": "Matrix96_750uL",
@@ -2682,9 +2893,9 @@
"Pod": "Pod1",
"items": [
{
"Position": "P3",
"Position": "P12",
"Height": -2.0,
"Volume": "400",
"Volume": "40",
"liquidtype": "Well Contents",
"WellsX": 12,
"LabwareClass": "Matrix96_750uL",
@@ -2791,7 +3002,6 @@
true,
true,
true,
true,
true
],
"Prototype": "MC P300 High",
@@ -2808,9 +3018,9 @@
"UseExpression": false
},
{
"Position": "P12",
"Position": "P14",
"Height": -2.0,
"Volume": "400",
"Volume": "40",
"liquidtype": "Tip Contents",
"WellsX": 12,
"LabwareClass": "Matrix96_750uL",
@@ -3023,7 +3233,7 @@
{
"Position": "P12",
"Height": -2.0,
"Volume": "400",
"Volume": "40",
"liquidtype": "Well Contents",
"WellsX": 12,
"LabwareClass": "Matrix96_750uL",
@@ -3130,7 +3340,6 @@
true,
true,
true,
true,
true
],
"Prototype": "MC P300 High",
@@ -3147,9 +3356,9 @@
"UseExpression": false
},
{
"Position": "P22",
"Position": "P14",
"Height": -2.0,
"Volume": "400",
"Volume": "40",
"liquidtype": "Tip Contents",
"WellsX": 12,
"LabwareClass": "Matrix96_750uL",
@@ -3339,7 +3548,7 @@
"Pod": "Pod1",
"items": [
{
"Position": "P4",
"Position": "P12",
"Height": -2.0,
"Volume": "40",
"liquidtype": "Well Contents",
@@ -3448,7 +3657,6 @@
true,
true,
true,
true,
true
],
"Prototype": "MC P300 High",
@@ -3465,7 +3673,7 @@
"UseExpression": false
},
{
"Position": "P13",
"Position": "P14",
"Height": -2.0,
"Volume": "40",
"liquidtype": "Tip Contents",
@@ -3804,7 +4012,6 @@
true,
true,
true,
true,
true
],
"Prototype": "MC P300 High",

9417
unilabos/devices/liquid_handling/converted protocol/sci-lucif-assay4_plr_background_tested.ipynb
View File

File diff suppressed because it is too large Load Diff

0
unilabos/devices/resource_container/__init__.py Normal file
View File

9
unilabos/devices/resource_container/container.py Normal file
View File

@@ -0,0 +1,9 @@
class HotelContainer:
def __init__(self, rotation: dict, device_config: dict):
self.rotation = rotation
self.device_config = device_config
self.status = 'idle'
def get_rotation(self):
return self.rotation

38
unilabos/devices/ros_dev/lh_joint_config.json Normal file
View File

@@ -0,0 +1,38 @@
{
"OTDeck":{
"joint_names":[
"first_joint",
"second_joint",
"third_joint",
"fourth_joint"
],
"link_names":[
"first_link",
"second_link",
"third_link",
"fourth_link"
],
"y":{
"first_joint":{
"factor":-0.001,
"offset":0.166
}
},
"x":{
"second_joint":{
"factor":-0.001,
"offset":0.1775
}
},
"z":{
"third_joint":{
"factor":0.001,
"offset":0.0
},
"fourth_joint":{
"factor":0.001,
"offset":0.0
}
}
}
}

346
unilabos/devices/ros_dev/liquid_handler_joint_publisher.py
View File

@@ -1,9 +1,12 @@
import asyncio
import copy
from pathlib import Path
import threading
import rclpy
import json
import time
from rclpy.executors import MultiThreadedExecutor
from rclpy.action import ActionServer
from rclpy.action import ActionServer,ActionClient
from sensor_msgs.msg import JointState
from unilabos_msgs.action import SendCmd
from rclpy.action.server import ServerGoalHandle
@@ -11,9 +14,11 @@ from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
from tf_transformations import quaternion_from_euler
from tf2_ros import TransformBroadcaster, Buffer, TransformListener
from rclpy.node import Node
import re
class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
def __init__(self,device_id:str, joint_config:dict, lh_id:str,resource_tracker, rate=50):
def __init__(self,resources_config:list, resource_tracker, rate=50, device_id:str = "lh_joint_publisher"):
super().__init__(
driver_instance=self,
device_id=device_id,
@@ -23,60 +28,118 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
print_publish=False,
resource_tracker=resource_tracker,
)
# joint_config_dict = {
# "joint_names":[
# "first_joint",
# "second_joint",
# "third_joint",
# "fourth_joint"
# ],
# "y":{
# "first_joint":{
# "factor":-1,
# "offset":0.0
# }
# },
# "x":{
# "second_joint":{
# "factor":-1,
# "offset":0.0
# }
# },
# "z":{
# "third_joint":{
# "factor":1,
# "offset":0.0
# },
# "fourth_joint":{
# "factor":1,
# "offset":0.0
# }
# }
# }
# 初始化参数
self.j_msg = JointState()
self.lh_id = lh_id
# self.j_msg.name = joint_names
self.joint_config = joint_config
self.j_msg.position = [0.0 for i in range(len(joint_config['joint_names']))]
self.j_msg.name = [f"{self.lh_id}_{x}" for x in joint_config['joint_names']]
# self.joint_config = joint_config_dict
# self.j_msg.position = [0.0 for i in range(len(joint_config_dict['joint_names']))]
# self.j_msg.name = [f"{self.lh_id}_{x}" for x in joint_config_dict['joint_names']]
joint_config = json.load(open(f"{Path(__file__).parent.absolute()}/lh_joint_config.json", encoding="utf-8"))
self.resources_config = {x['id']:x for x in resources_config}
self.rate = rate
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
self.j_pub = self.create_publisher(JointState,'/joint_states',10)
self.create_timer(0.02,self.lh_joint_pub_callback)
self.create_timer(1,self.lh_joint_pub_callback)
self.resource_action = None
while self.resource_action is None:
self.resource_action = self.check_tf_update_actions()
time.sleep(1)
self.resource_action_client = ActionClient(self, SendCmd, self.resource_action)
while not self.resource_action_client.wait_for_server(timeout_sec=1.0):
self.get_logger().info('等待 TfUpdate 服务器...')
self.deck_list = []
self.lh_devices = {}
# 初始化设备ID与config信息
for resource in resources_config:
if resource['class'] == 'liquid_handler':
deck_id = resource['config']['data']['children'][0]['_resource_child_name']
deck_class = resource['config']['data']['children'][0]['_resource_type'].split(':')[-1]
key = f'{deck_id}'
# key = f'{resource["id"]}_{deck_id}'
self.lh_devices[key] = {
'joint_msg':JointState(
name=[f'{key}_{x}' for x in joint_config[deck_class]['joint_names']],
position=[0.0 for _ in joint_config[deck_class]['joint_names']]
),
'joint_config':joint_config[deck_class]
}
self.deck_list.append(deck_id)
print('='*20)
print(self.lh_devices)
print('='*20)
self.j_action = ActionServer(
self,
SendCmd,
"joint",
"hl_joint_action",
self.lh_joint_action_callback,
result_timeout=5000
)
def check_tf_update_actions(self):
topics = self.get_topic_names_and_types()
for topic_item in topics:
topic_name, topic_types = topic_item
if 'action_msgs/msg/GoalStatusArray' in topic_types:
# 删除 /_action/status 部分
base_name = topic_name.replace('/_action/status', '')
# 检查最后一个部分是否为 tf_update
parts = base_name.split('/')
if parts and parts[-1] == 'tf_update':
return base_name
return None
def find_resource_parent(self, resource_id:str):
# 遍历父辈，找到父辈的父辈，直到找到设备ID
parent_id = self.resources_config[resource_id]['parent']
try:
if parent_id in self.deck_list:
p_ = self.resources_config[parent_id]['parent']
str_ = f'{parent_id}'
return str(str_)
else:
return self.find_resource_parent(parent_id)
except Exception as e:
return None
def send_resource_action(self, resource_id_list:list[str], link_name:str):
goal_msg = SendCmd.Goal()
str_dict = {}
for resource in resource_id_list:
str_dict[resource] = link_name
goal_msg.command = json.dumps(str_dict)
self.resource_action_client.send_goal(goal_msg)
def resource_move(self, resource_id:str, link_name:str, channels:list[int]):
resource = resource_id.rsplit("_",1)
channel_list = ['A','B','C','D','E','F','G','H']
resource_list = []
match = re.match(r'([a-zA-Z_]+)(\d+)', resource[1])
if match:
number = match.group(2)
for channel in channels:
resource_list.append(f"{resource[0]}_{channel_list[channel]}{number}")
if len(resource_list) > 0:
self.send_resource_action(resource_list, link_name)
def lh_joint_action_callback(self,goal_handle: ServerGoalHandle):
"""Move a single joint
@@ -101,12 +164,13 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
goal_handle.succeed()
except Exception as e:
print(e)
print(f'Liquid handler action error: \n{e}')
goal_handle.abort()
result.success = False
return result
def inverse_kinematics(self, x, y, z,
parent_id,
x_joint:dict,
y_joint:dict,
z_joint:dict ):
@@ -117,77 +181,102 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
x (float): x坐标
y (float): y坐标
z (float): z坐标
x_joint (dict): x轴关节配置，包含plus和offset
y_joint (dict): y轴关节配置，包含plus和offset
z_joint (dict): z轴关节配置，包含plus和offset
x_joint (dict): x轴关节配置，包含factor和offset
y_joint (dict): y轴关节配置，包含factor和offset
z_joint (dict): z轴关节配置，包含factor和offset
Returns:
dict: 关节名称和对应位置的字典
"""
joint_positions = copy.deepcopy(self.j_msg.position)
joint_positions = copy.deepcopy(self.lh_devices[parent_id]['joint_msg'].position)
z_index = 0
# 处理x轴关节
for joint_name, config in x_joint.items():
index = self.j_msg.name.index(f"{self.lh_id}_{joint_name}")
index = self.lh_devices[parent_id]['joint_msg'].name.index(f"{parent_id}_{joint_name}")
joint_positions[index] = x * config["factor"] + config["offset"]
# 处理y轴关节
for joint_name, config in y_joint.items():
index = self.j_msg.name.index(f"{self.lh_id}_{joint_name}")
index = self.lh_devices[parent_id]['joint_msg'].name.index(f"{parent_id}_{joint_name}")
joint_positions[index] = y * config["factor"] + config["offset"]
# 处理z轴关节
for joint_name, config in z_joint.items():
index = self.j_msg.name.index(f"{self.lh_id}_{joint_name}")
index = self.lh_devices[parent_id]['joint_msg'].name.index(f"{parent_id}_{joint_name}")
joint_positions[index] = z * config["factor"] + config["offset"]
return joint_positions
z_index = index
return joint_positions ,z_index
def move_joints(self, resource_name, link_name, speed, x_joint=None, y_joint=None, z_joint=None):
def move_joints(self, resource_names, x, y, z, option, speed = 0.1 ,x_joint=None, y_joint=None, z_joint=None):
if isinstance(resource_names, list):
resource_name_ = resource_names[0]
else:
resource_name_ = resource_names
transform = self.tf_buffer.lookup_transform(
link_name,
resource_name,
rclpy.time.Time()
)
x,y,z = transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z
parent_id = self.find_resource_parent(resource_name_)
print('!'*20)
print(parent_id)
print('!'*20)
if x_joint is None:
x_joint_config = next(iter(self.joint_config['x'].items()))
elif x_joint in self.joint_config['x']:
x_joint_config = self.joint_config['x'][x_joint]
xa,xb = next(iter(self.lh_devices[parent_id]['joint_config']['x'].items()))
x_joint_config = {xa:xb}
elif x_joint in self.lh_devices[parent_id]['joint_config']['x']:
x_joint_config = self.lh_devices[parent_id]['joint_config']['x'][x_joint]
else:
raise ValueError(f"x_joint {x_joint} not in joint_config['x']")
if y_joint is None:
y_joint_config = next(iter(self.joint_config['y'].items()))
elif y_joint in self.joint_config['y']:
y_joint_config = self.joint_config['y'][y_joint]
ya,yb = next(iter(self.lh_devices[parent_id]['joint_config']['y'].items()))
y_joint_config = {ya:yb}
elif y_joint in self.lh_devices[parent_id]['joint_config']['y']:
y_joint_config = self.lh_devices[parent_id]['joint_config']['y'][y_joint]
else:
raise ValueError(f"y_joint {y_joint} not in joint_config['y']")
if z_joint is None:
z_joint_config = next(iter(self.joint_config['z'].items()))
elif z_joint in self.joint_config['z']:
z_joint_config = self.joint_config['z'][z_joint]
za, zb = next(iter(self.lh_devices[parent_id]['joint_config']['z'].items()))
z_joint_config = {za :zb}
elif z_joint in self.lh_devices[parent_id]['joint_config']['z']:
z_joint_config = self.lh_devices[parent_id]['joint_config']['z'][z_joint]
else:
raise ValueError(f"z_joint {z_joint} not in joint_config['z']")
joint_positions_target = self.inverse_kinematics(x,y,z,x_joint_config,y_joint_config,z_joint_config)
joint_positions_target, z_index = self.inverse_kinematics(x,y,z,parent_id,x_joint_config,y_joint_config,z_joint_config)
joint_positions_target_zero = copy.deepcopy(joint_positions_target)
joint_positions_target_zero[z_index] = 0
self.move_to(joint_positions_target_zero, speed, parent_id)
self.move_to(joint_positions_target, speed, parent_id)
time.sleep(1)
if option == "pick":
link_name = self.lh_devices[parent_id]['joint_config']['link_names'][z_index]
link_name = f'{parent_id}_{link_name}'
self.resource_move(resource_name_, link_name, [0,1,2,3,4,5,6,7])
elif option == "drop_trash":
self.resource_move(resource_name_, "__trash", [0,1,2,3,4,5,6,7])
elif option == "drop":
self.resource_move(resource_name_, "world", [0,1,2,3,4,5,6,7])
self.move_to(joint_positions_target_zero, speed, parent_id)
def move_to(self, joint_positions ,speed, parent_id):
loop_flag = 0
while loop_flag < len(self.joint_config['joint_names']):
while loop_flag < len(joint_positions):
loop_flag = 0
for i in range(len(self.joint_config['joint_names'])):
distance = joint_positions_target[i] - self.j_msg.position[i]
for i in range(len(joint_positions)):
distance = joint_positions[i] - self.lh_devices[parent_id]['joint_msg'].position[i]
if distance == 0:
loop_flag += 1
continue
minus_flag = distance/abs(distance)
if abs(distance) > speed/self.rate:
self.j_msg.position[i] += minus_flag * speed/self.rate
self.lh_devices[parent_id]['joint_msg'].position[i] += minus_flag * speed/self.rate
else :
self.j_msg.position[i] = joint_positions_target[i]
self.lh_devices[parent_id]['joint_msg'].position[i] = joint_positions[i]
loop_flag += 1
@@ -195,10 +284,103 @@ class LiquidHandlerJointPublisher(BaseROS2DeviceNode):
self.lh_joint_pub_callback()
time.sleep(1/self.rate)
def lh_joint_pub_callback(self):
self.j_msg.header.stamp = self.get_clock().now().to_msg()
self.j_pub.publish(self.j_msg)
for id, config in self.lh_devices.items():
config['joint_msg'].header.stamp = self.get_clock().now().to_msg()
self.j_pub.publish(config['joint_msg'])
class JointStatePublisher(Node):
def __init__(self):
super().__init__('joint_state_publisher')
self.lh_action = None
while self.lh_action is None:
self.lh_action = self.check_hl_joint_actions()
time.sleep(1)
self.lh_action_client = ActionClient(self, SendCmd, self.lh_action)
while not self.lh_action_client.wait_for_server(timeout_sec=1.0):
self.get_logger().info('等待 TfUpdate 服务器...')
def check_hl_joint_actions(self):
topics = self.get_topic_names_and_types()
for topic_item in topics:
topic_name, topic_types = topic_item
if 'action_msgs/msg/GoalStatusArray' in topic_types:
# 删除 /_action/status 部分
base_name = topic_name.replace('/_action/status', '')
# 检查最后一个部分是否为 tf_update
parts = base_name.split('/')
if parts and parts[-1] == 'hl_joint_action':
return base_name
return None
def send_resource_action(self, resource_name, x,y,z,option, speed = 0.1,x_joint=None, y_joint=None, z_joint=None):
goal_msg = SendCmd.Goal()
str_dict = {
'resource_names':resource_name,
'x':x,
'y':y,
'z':z,
'option':option,
'speed':speed,
'x_joint':x_joint,
'y_joint':y_joint,
'z_joint':z_joint
}
goal_msg.command = json.dumps(str_dict)
if not self.lh_action_client.wait_for_server(timeout_sec=5.0):
self.get_logger().error('Action server not available')
return None
try:
# 创建新的executor
executor = rclpy.executors.MultiThreadedExecutor()
executor.add_node(self)
# 发送目标
future = self.lh_action_client.send_goal_async(goal_msg)
# 使用executor等待结果
while not future.done():
executor.spin_once(timeout_sec=0.1)
handle = future.result()
if not handle.accepted:
self.get_logger().error('Goal was rejected')
return None
# 等待最终结果
result_future = handle.get_result_async()
while not result_future.done():
executor.spin_once(timeout_sec=0.1)
result = result_future.result()
return result
except Exception as e:
self.get_logger().error(f'Error during action execution: {str(e)}')
return None
finally:
# 清理executor
executor.remove_node(self)
def main():

2442
unilabos/devices/ros_dev/moveit2.py Normal file
View File

File diff suppressed because it is too large Load Diff

376
unilabos/devices/ros_dev/moveit_interface.py Normal file
View File

@@ -0,0 +1,376 @@
import json
import time
from copy import deepcopy
from pathlib import Path
from moveit_msgs.msg import JointConstraint, Constraints
from rclpy.action import ActionClient
from tf2_ros import Buffer, TransformListener
from unilabos_msgs.action import SendCmd
from unilabos.devices.ros_dev.moveit2 import MoveIt2
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
class MoveitInterface:
_ros_node: BaseROS2DeviceNode
tf_buffer: Buffer
tf_listener: TransformListener
def __init__(self, moveit_type, joint_poses, rotation=None, device_config=None):
self.device_config = device_config
self.rotation = rotation
self.data_config = json.load(
open(
f"{Path(__file__).parent.parent.parent.absolute()}/device_mesh/devices/{moveit_type}/config/move_group.json",
encoding="utf-8",
)
)
self.arm_move_flag = False
self.move_option = ["pick", "place", "side_pick", "side_place"]
self.joint_poses = joint_poses
self.cartesian_flag = False
self.mesh_group = ["reactor", "sample", "beaker"]
self.moveit2 = {}
self.resource_action = None
self.resource_client = None
self.resource_action_ok = False
def post_init(self, ros_node: BaseROS2DeviceNode):
self._ros_node = ros_node
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self._ros_node)
for move_group, config in self.data_config.items():
base_link_name = f"{self._ros_node.device_id}_{config['base_link_name']}"
end_effector_name = f"{self._ros_node.device_id}_{config['end_effector_name']}"
joint_names = [f"{self._ros_node.device_id}_{name}" for name in config["joint_names"]]
self.moveit2[f"{move_group}"] = MoveIt2(
node=self._ros_node,
joint_names=joint_names,
base_link_name=base_link_name,
end_effector_name=end_effector_name,
group_name=f"{self._ros_node.device_id}_{move_group}",
callback_group=self._ros_node.callback_group,
use_move_group_action=True,
ignore_new_calls_while_executing=True,
)
self.moveit2[f"{move_group}"].allowed_planning_time = 3.0
self._ros_node.create_timer(1, self.wait_for_resource_action, callback_group=self._ros_node.callback_group)
def wait_for_resource_action(self):
if not self.resource_action_ok:
while self.resource_action is None:
self.resource_action = self.check_tf_update_actions()
time.sleep(1)
self.resource_client = ActionClient(self._ros_node, SendCmd, self.resource_action)
self.resource_action_ok = True
while not self.resource_client.wait_for_server(timeout_sec=5.0):
self._ros_node.lab_logger().info("等待 TfUpdate 服务器...")
def check_tf_update_actions(self):
topics = self._ros_node.get_topic_names_and_types()
for topic_item in topics:
topic_name, topic_types = topic_item
if "action_msgs/msg/GoalStatusArray" in topic_types:
# 删除 /_action/status 部分
base_name = topic_name.replace("/_action/status", "")
# 检查最后一个部分是否为 tf_update
parts = base_name.split("/")
if parts and parts[-1] == "tf_update":
return base_name
return None
def set_position(self, command):
"""使用moveit 移动到指定点
Args:
command: A JSON-formatted string that includes quaternion, speed, position
position (list) : 点的位置 [x,y,z]
quaternion (list) : 点的姿态(四元数) [x,y,z,w]
move_group (string) : The move group moveit will plan
speed (float) : The speed of the movement, speed > 0
retry (int) : Retry times when moveit plan fails
Returns:
None
"""
result = SendCmd.Result()
cmd_str = command.replace("'", '"')
cmd_dict = json.loads(cmd_str)
self.moveit_task(**cmd_dict)
return result
def moveit_task(
self, move_group, position, quaternion, speed=1, retry=10, cartesian=False, target_link=None, offsets=[0, 0, 0]
):
speed_ = float(max(0.1, min(speed, 1)))
self.moveit2[move_group].max_velocity = speed_
self.moveit2[move_group].max_acceleration = speed_
re_ = False
pose_result = [x + y for x, y in zip(position, offsets)]
# print(pose_result)
while retry > -1 and not re_:
self.moveit2[move_group].move_to_pose(
target_link=target_link,
position=pose_result,
quat_xyzw=quaternion,
cartesian=cartesian,
# cartesian_fraction_threshold=0.0,
cartesian_max_step=0.01,
weight_position=1.0,
)
re_ = self.moveit2[move_group].wait_until_executed()
retry += -1
return re_
def moveit_joint_task(self, move_group, joint_positions, joint_names=None, speed=1, retry=10):
re_ = False
joint_positions_ = [float(x) for x in joint_positions]
speed_ = float(max(0.1, min(speed, 1)))
self.moveit2[move_group].max_velocity = speed_
self.moveit2[move_group].max_acceleration = speed_
while retry > -1 and not re_:
self.moveit2[move_group].move_to_configuration(joint_positions=joint_positions_, joint_names=joint_names)
re_ = self.moveit2[move_group].wait_until_executed()
retry += -1
print(self.moveit2[move_group].compute_fk(joint_positions))
return re_
def resource_manager(self, resource, parent_link):
goal_msg = SendCmd.Goal()
str_dict = {}
str_dict[resource] = parent_link
goal_msg.command = json.dumps(str_dict)
assert self.resource_client is not None
self.resource_client.send_goal(goal_msg)
return True
def pick_and_place(self, command: str):
"""
Using MoveIt to make the robotic arm pick or place materials to a target point.
Args:
command: A JSON-formatted string that includes option, target, speed, lift_height, mt_height
*option (string) : Action type: pick/place/side_pick/side_place
*move_group (string): The move group moveit will plan
*status(string) : Target pose
resource(string) : The target resource
x_distance (float) : The distance to the target in x direction(meters)
y_distance (float) : The distance to the target in y direction(meters)
lift_height (float) : The height at which the material should be lifted(meters)
retry (float) : Retry times when moveit plan fails
speed (float) : The speed of the movement, speed > 0
Returns:
None
"""
result = SendCmd.Result()
try:
cmd_str = str(command).replace("'", '"')
cmd_dict = json.loads(cmd_str)
if cmd_dict["option"] in self.move_option:
option_index = self.move_option.index(cmd_dict["option"])
place_flag = option_index % 2
config = {}
function_list = []
status = cmd_dict["status"]
joint_positions_ = self.joint_poses[cmd_dict["move_group"]][status]
config.update({k: cmd_dict[k] for k in ["speed", "retry", "move_group"] if k in cmd_dict})
# 夹取
if not place_flag:
if "target" in cmd_dict.keys():
function_list.append(lambda: self.resource_manager(cmd_dict["resource"], cmd_dict["target"]))
else:
function_list.append(
lambda: self.resource_manager(
cmd_dict["resource"], self.moveit2[cmd_dict["move_group"]].end_effector_name
)
)
else:
function_list.append(lambda: self.resource_manager(cmd_dict["resource"], "world"))
constraints = []
if "constraints" in cmd_dict.keys():
for i in range(len(cmd_dict["constraints"])):
v = float(cmd_dict["constraints"][i])
if v > 0:
constraints.append(
JointConstraint(
joint_name=self.moveit2[cmd_dict["move_group"]].joint_names[i],
position=joint_positions_[i],
tolerance_above=v,
tolerance_below=v,
weight=1.0,
)
)
if "lift_height" in cmd_dict.keys():
retval = None
retry = config.get("retry", 10)
while retval is None and retry > 0:
retval = self.moveit2[cmd_dict["move_group"]].compute_fk(joint_positions_)
time.sleep(0.1)
retry -= 1
if retval is None:
result.success = False
return result
pose = [retval.pose.position.x, retval.pose.position.y, retval.pose.position.z]
quaternion = [
retval.pose.orientation.x,
retval.pose.orientation.y,
retval.pose.orientation.z,
retval.pose.orientation.w,
]
function_list = [
lambda: self.moveit_task(
position=[retval.pose.position.x, retval.pose.position.y, retval.pose.position.z],
quaternion=quaternion,
**config,
cartesian=self.cartesian_flag,
)
] + function_list
pose[2] += float(cmd_dict["lift_height"])
function_list.append(
lambda: self.moveit_task(
position=pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag
)
)
end_pose = pose
if "x_distance" in cmd_dict.keys() or "y_distance" in cmd_dict.keys():
if "x_distance" in cmd_dict.keys():
deep_pose = deepcopy(pose)
deep_pose[0] += float(cmd_dict["x_distance"])
elif "y_distance" in cmd_dict.keys():
deep_pose = deepcopy(pose)
deep_pose[1] += float(cmd_dict["y_distance"])
function_list = [
lambda: self.moveit_task(
position=pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag
)
] + function_list
function_list.append(
lambda: self.moveit_task(
position=deep_pose, quaternion=quaternion, **config, cartesian=self.cartesian_flag
)
)
end_pose = deep_pose
retval_ik = self.moveit2[cmd_dict["move_group"]].compute_ik(
position=end_pose, quat_xyzw=quaternion, constraints=Constraints(joint_constraints=constraints)
)
position_ = [
retval_ik.position[retval_ik.name.index(i)]
for i in self.moveit2[cmd_dict["move_group"]].joint_names
]
function_list = [
lambda: self.moveit_joint_task(
joint_positions=position_,
joint_names=self.moveit2[cmd_dict["move_group"]].joint_names,
**config,
)
] + function_list
else:
function_list = [
lambda: self.moveit_joint_task(**config, joint_positions=joint_positions_)
] + function_list
for i in range(len(function_list)):
if i == 0:
self.cartesian_flag = False
else:
self.cartesian_flag = True
re = function_list[i]()
if not re:
print(i, re)
result.success = False
return result
result.success = True
except Exception as e:
print(e)
self.cartesian_flag = False
result.success = False
return result
def set_status(self, command: str):
"""
Goto home position
Args:
command: A JSON-formatted string that includes speed
*status (string) : The joint status moveit will plan
*move_group (string): The move group moveit will plan
separate (list) : The joint index to be separated
lift_height (float) : The height at which the material should be lifted(meters)
x_distance (float) : The distance to the target in x direction(meters)
y_distance (float) : The distance to the target in y direction(meters)
speed (float) : The speed of the movement, speed > 0
retry (float) : Retry times when moveit plan fails
Returns:
None
"""
result = SendCmd.Result()
try:
cmd_str = command.replace("'", '"')
cmd_dict = json.loads(cmd_str)
config = {}
config["move_group"] = cmd_dict["move_group"]
if "speed" in cmd_dict.keys():
config["speed"] = cmd_dict["speed"]
if "retry" in cmd_dict.keys():
config["retry"] = cmd_dict["retry"]
status = cmd_dict["status"]
joint_positions_ = self.joint_poses[cmd_dict["move_group"]][status]
re = self.moveit_joint_task(**config, joint_positions=joint_positions_)
if not re:
result.success = False
return result
result.success = True
except Exception as e:
print(e)
result.success = False
return result

9
unilabos/registry/devices/hotel.yaml Normal file
View File

@@ -0,0 +1,9 @@
hotel.thermo_orbitor_rs2_hotel:
description: Thermo Orbitor RS2 Hotel
class:
module: unilabos.devices.resource_container.container:HotelContainer
type: python
model:
type: device
mesh: thermo_orbitor_rs2_hotel

8
unilabos/registry/devices/liquid_handler.yaml
View File

@@ -358,8 +358,8 @@ liquid_handler.biomek:
transfer_biomek:
type: LiquidHandlerTransferBiomek
goal:
source: source
target: target
sources: sources
targets: targets
tip_rack: tip_rack
volume: volume
aspirate_techniques: aspirate_techniques
@@ -417,8 +417,8 @@ liquid_handler.biomek:
move_biomek:
type: LiquidHandlerMoveBiomek
goal:
source: resource
target: target
source: sources
target: targets
feedback: {}
result:
name: name

56
unilabos/registry/devices/moveit_config.yaml Normal file
View File

@@ -0,0 +1,56 @@
moveit.toyo_xyz:
description: Toyo XYZ
class:
module: unilabos.devices.ros_dev.moveit_interface:MoveitInterface
type: python
action_value_mappings:
set_position:
type: SendCmd
goal:
command: command
feedback: { }
result: { }
pick_and_place:
type: SendCmd
goal:
command: command
feedback: { }
result: { }
set_status:
type: SendCmd
goal:
command: command
feedback: { }
result: { }
model:
type: device
mesh: toyo_xyz
moveit.benyao_arm:
description: Benyao Arm
model:
type: device
mesh: benyao_arm
class:
module: unilabos.devices.ros_dev.moveit_interface:MoveitInterface
type: python
action_value_mappings:
set_position:
type: SendCmd
goal:
command: command
feedback: {}
result: {}
pick_and_place:
type: SendCmd
goal:
command: command
feedback: {}
result: {}
set_status:
type: SendCmd
goal:
command: command
feedback: {}
result: {}

2
unilabos/registry/registry.py
View File

@@ -87,7 +87,7 @@ class Registry:
),
"handles": {
"output": [{
"handler_key": "Labware",
"handler_key": "labware",
"label": "Labware",
"data_type": "resource",
"data_source": "handle",

23
unilabos/resources/graphio.py
View File

@@ -9,7 +9,7 @@ try:
from pylabrobot.resources.resource import Resource as ResourcePLR
except ImportError:
pass
from typing import Union, get_origin, get_args
physical_setup_graph: nx.Graph = None
@@ -298,7 +298,6 @@ def nested_dict_to_list(nested_dict: dict) -> list[dict]: # FIXME 是tree？
return result
def convert_resources_to_type(
resources_list: list[dict], resource_type: type, *, plr_model: bool = False
) -> Union[list[dict], dict, None, "ResourcePLR"]:
@@ -320,9 +319,13 @@ def convert_resources_to_type(
return resource_ulab_to_plr(resources_list, plr_model)
resources_tree = dict_to_tree({r["id"]: r for r in resources_list})
return resource_ulab_to_plr(resources_tree[0], plr_model)
elif isinstance(resource_type, list) and all(issubclass(t, ResourcePLR) for t in resource_type):
resources_tree = dict_to_tree({r["id"]: r for r in resources_list})
return [resource_ulab_to_plr(r, plr_model) for r in resources_tree]
elif isinstance(resource_type, list) :
if all((get_origin(t) is Union) for t in resource_type):
resources_tree = dict_to_tree({r["id"]: r for r in resources_list})
return [resource_ulab_to_plr(r, plr_model) for r in resources_tree]
elif all(issubclass(t, ResourcePLR) for t in resource_type):
resources_tree = dict_to_tree({r["id"]: r for r in resources_list})
return [resource_ulab_to_plr(r, plr_model) for r in resources_tree]
else:
return None
@@ -343,9 +346,13 @@ def convert_resources_from_type(resources_list, resource_type: type) -> Union[li
elif isinstance(resource_type, type) and issubclass(resource_type, ResourcePLR):
resources_tree = [resource_plr_to_ulab(resources_list)]
return tree_to_list(resources_tree)
elif isinstance(resource_type, list) and all(issubclass(t, ResourcePLR) for t in resource_type):
resources_tree = [resource_plr_to_ulab(r) for r in resources_list]
return tree_to_list(resources_tree)
elif isinstance(resource_type, list) :
if all((get_origin(t) is Union) for t in resource_type):
resources_tree = [resource_plr_to_ulab(r) for r in resources_list]
return tree_to_list(resources_tree)
elif all(issubclass(t, ResourcePLR) for t in resource_type):
resources_tree = [resource_plr_to_ulab(r) for r in resources_list]
return tree_to_list(resources_tree)
else:
return None

13
unilabos/ros/main_slave_run.py
View File

@@ -6,9 +6,9 @@ import time
from typing import Optional, Dict, Any, List
import rclpy
from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
from unilabos.ros.nodes.presets.resource_mesh_manager import ResourceMeshManager
from unilabos.ros.nodes.resource_tracker import DeviceNodeResourceTracker
from unilabos.devices.ros_dev.liquid_handler_joint_publisher import LiquidHandlerJointPublisher
from unilabos_msgs.msg import Resource # type: ignore
from unilabos_msgs.srv import ResourceAdd, SerialCommand # type: ignore
from rclpy.executors import MultiThreadedExecutor
@@ -69,19 +69,23 @@ def main(
)
if visual != "disable":
from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
resource_mesh_manager = ResourceMeshManager(
resources_mesh_config,
resources_config,
resource_tracker= DeviceNodeResourceTracker(),
resource_tracker = host_node.resource_tracker,
device_id = 'resource_mesh_manager',
)
joint_republisher = JointRepublisher(
'joint_republisher',
DeviceNodeResourceTracker()
host_node.resource_tracker
)
lh_joint_pub = LiquidHandlerJointPublisher(resources_config=resources_config,
resource_tracker=host_node.resource_tracker)
executor.add_node(resource_mesh_manager)
executor.add_node(joint_republisher)
executor.add_node(lh_joint_pub)
thread = threading.Thread(target=executor.spin, daemon=True, name="host_executor_thread")
thread.start()
@@ -121,6 +125,7 @@ def slave(
executor.add_node(n)
if visual != "disable":
from unilabos.ros.nodes.presets.joint_republisher import JointRepublisher
resource_mesh_manager = ResourceMeshManager(
resources_mesh_config,
resources_config,

4
unilabos/ros/msgs/message_converter.py
View File

@@ -131,7 +131,7 @@ _msg_converter: Dict[Type, Any] = {
Bool: lambda x: Bool(data=bool(x)),
str: str,
String: lambda x: String(data=str(x)),
Point: lambda x: Point(x=x.x, y=x.y, z=x.z) if not isinstance(x, dict) else Point(x=x.get("x", 0), y=x.get("y", 0), z=x.get("z", 0)),
Point: lambda x: Point(x=x.x, y=x.y, z=x.z) if not isinstance(x, dict) else Point(x=x.get("x", 0.0), y=x.get("y", 0.0), z=x.get("z", 0.0)),
Resource: lambda x: Resource(
id=x.get("id", ""),
name=x.get("name", ""),
@@ -141,7 +141,7 @@ _msg_converter: Dict[Type, Any] = {
type=x.get("type", ""),
category=x.get("class", "") or x.get("type", ""),
pose=(
Pose(position=Point(x=float(x.get("position", {}).get("x", 0)), y=float(x.get("position", {}).get("y", 0)), z=float(x.get("position", {}).get("z", 0))))
Pose(position=Point(x=float(x.get("position", {}).get("x", 0.0)), y=float(x.get("position", {}).get("y", 0.0)), z=float(x.get("position", {}).get("z", 0.0))))
if x.get("position", None) is not None
else Pose()
),

23
unilabos/ros/nodes/base_device_node.py
View File

@@ -740,16 +740,23 @@ class BaseROS2DeviceNode(Node, Generic[T]):
self.lab_logger().info(f"更新资源状态: {k}")
r = ResourceUpdate.Request()
# 仅当action_kwargs[k]不为None时尝试转换
akv = action_kwargs[k]
akv = action_kwargs[k] # 已经是完成转换的物料了，只需要转换成ros msg Resource了
apv = action_paramtypes[k]
final_type = get_type_class(apv)
if final_type is None:
continue
try:
r.resources = [
convert_to_ros_msg(Resource, self.resource_tracker.root_resource(rs))
for rs in convert_resources_from_type(akv, final_type) # type: ignore # FIXME # 考虑反查到最大的
]
seen = set()
unique_resources = []
for rs in akv:
res = self.resource_tracker.parent_resource(rs) # 获取 resource 对象
if id(res) not in seen:
seen.add(id(res))
converted_list = convert_resources_from_type([res], final_type)
unique_resources.extend([convert_to_ros_msg(Resource, converted) for converted in converted_list])
r.resources = unique_resources
response = await self._resource_clients["resource_update"].call_async(r)
self.lab_logger().debug(f"资源更新结果: {response}")
except Exception as e:
@@ -917,6 +924,12 @@ class ROS2DeviceNode:
)
self._ros_node: BaseROS2DeviceNode
self._ros_node.lab_logger().info(f"初始化完成 {self._ros_node.uuid} {self.driver_is_ros}")
self.driver_instance._ros_node = self._ros_node # type: ignore
if hasattr(self.driver_instance, "post_init"):
try:
self.driver_instance.post_init(self._ros_node) # type: ignore
except Exception as e:
self._ros_node.lab_logger().error(f"设备后初始化失败: {e}")
def _start_loop(self):
def run_event_loop():

128
unilabos/ros/nodes/presets/resource_mesh_manager.py
View File

@@ -91,8 +91,11 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.__collision_object_publisher = self.create_publisher(
CollisionObject, "/collision_object", 10
)
self.__planning_scene_publisher = self.create_publisher(
PlanningScene, "/planning_scene", 10
)
self.__attached_collision_object_publisher = self.create_publisher(
AttachedCollisionObject, "/attached_collision_object", 10
AttachedCollisionObject, "/attached_collision_object", 0
)
# 创建一个Action Server用于修改resource_tf_dict
@@ -121,7 +124,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
"""检查move_group节点是否已初始化完成"""
# 获取当前可用的节点列表
if len(self.resource_tf_dict) == 0:
return
tf_ready = self.tf_buffer.can_transform("world", next(iter(self.resource_tf_dict.keys())), rclpy.time.Time(),rclpy.duration.Duration(seconds=2))
# if tf_ready:
@@ -129,8 +133,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.move_group_ready = True
self.publish_resource_tf()
self.add_resource_collision_meshes(self.resource_tf_dict)
# time.sleep(1)
def add_resource_mesh_callback(self, goal_handle : ServerGoalHandle):
tf_update_msg = goal_handle.request
@@ -147,7 +150,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
"""刷新资源配置"""
registry = lab_registry
resource_config = json.loads(resource_config_str)
resource_config = json.loads(resource_config_str.replace("'",'"'))
if resource_config['id'] in self.resource_config_dict:
self.get_logger().info(f'资源 {resource_config["id"]} 已存在')
@@ -158,7 +161,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.resource_model[resource_config['id']] = {
'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['mesh']}",
'mesh_tf': model_config['mesh_tf']}
if model_config['children_mesh'] is not None:
if 'children_mesh' in model_config.keys():
self.resource_model[f"{resource_config['id']}_"] = {
'mesh': f"{str(self.mesh_path)}/device_mesh/resources/{model_config['children_mesh']}",
'mesh_tf': model_config['children_mesh_tf']
@@ -187,7 +190,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
pass
elif parent is not None and resource_id in self.resource_model:
parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None","")
# parent_link = f"{self.resource_config_dict[parent]['parent']}_{parent}_device_link".replace("None_","")
parent_link = f"{parent}_device_link".replace("None_","")
else:
@@ -297,7 +301,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
"world",
resource_id,
rclpy.time.Time(seconds=0),
rclpy.duration.Duration(seconds=5)
# rclpy.duration.Duration(seconds=5)
)
# 提取当前位姿信息
@@ -344,9 +348,7 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.resource_pose_publisher.publish(changed_poses_msg)
self.zero_count += 1
def _is_pose_equal(self, pose1, pose2, tolerance=1e-7):
"""
比较两个位姿是否相等（考虑浮点数精度）
@@ -386,14 +388,24 @@ class ResourceMeshManager(BaseROS2DeviceNode):
self.__planning_scene = self._get_planning_scene_service.call(
GetPlanningScene.Request()
).scene
for resource_id, target_parent in cmd_dict.items():
self.__planning_scene.is_diff = True
planning_scene = PlanningScene()
planning_scene.is_diff = True
planning_scene.robot_state.is_diff = True
# time_start = self.get_clock().now()
time_start = rclpy.time.Time(seconds=0)
count = 0
for resource_id, target_parent in cmd_dict.items():
parent_id = target_parent
if target_parent == '__trash':
parent_id = 'world'
# 获取从resource_id到target_parent的转换
transform = self.tf_buffer.lookup_transform(
target_parent,
parent_id,
resource_id,
rclpy.time.Time(seconds=0)
time_start,
timeout=rclpy.duration.Duration(seconds=10)
)
# 提取转换中的位置和旋转信息
@@ -411,26 +423,62 @@ class ResourceMeshManager(BaseROS2DeviceNode):
}
self.resource_tf_dict[resource_id] = {
"parent": target_parent,
"parent": parent_id,
"position": position,
"rotation": rotation
}
# self.attach_collision_object(id=resource_id,link_name=target_parent)
collision_object = AttachedCollisionObject(
# time.sleep(0.02)
operation_attach = CollisionObject.ADD
operation_world = CollisionObject.REMOVE
if target_parent == 'world':
operation_attach = CollisionObject.REMOVE
operation_world = CollisionObject.ADD
elif target_parent == '__trash':
operation_attach = CollisionObject.REMOVE
world_object = CollisionObject(
id=resource_id,
link_name=target_parent,
operation=operation_world
)
if target_parent != '__trash':
planning_scene.world.collision_objects.append(world_object)
collision_object = AttachedCollisionObject(
object=CollisionObject(
id=resource_id,
operation=CollisionObject.ADD
operation=operation_attach
)
)
self.__planning_scene.robot_state.attached_collision_objects.append(collision_object)
if target_parent != 'world' and target_parent != '__trash':
collision_object.link_name = target_parent
planning_scene.robot_state.attached_collision_objects.append(collision_object)
count += 1
if count > 30:
req = ApplyPlanningScene.Request()
req.scene = planning_scene
self.publish_resource_tf()
self._apply_planning_scene_service.call(req)
self.__planning_scene_publisher.publish(planning_scene)
count = 0
planning_scene = PlanningScene()
planning_scene.is_diff = True
planning_scene.robot_state.is_diff = True
req = ApplyPlanningScene.Request()
req.scene = self.__planning_scene
self._apply_planning_scene_service.call_async(req)
req.scene = planning_scene
self.publish_resource_tf()
self._apply_planning_scene_service.call(req)
self.__planning_scene_publisher.publish(planning_scene)
# self.__collision_object_publisher.publish(CollisionObject())
except Exception as e:
self.get_logger().error(f"更新资源TF字典失败: {e}")
@@ -440,18 +488,22 @@ class ResourceMeshManager(BaseROS2DeviceNode):
return SendCmd.Result(success=True)
def add_resource_collision_meshes(self,resource_tf_dict:dict):
"""
遍历资源配置字典，为每个在resource_model中有对应模型的资源添加碰撞网格
该方法检查每个资源ID是否在self.resource_model中有对应的3D模型文件路径，
如果有，则调用add_collision_mesh方法将其添加到碰撞环境中。
"""
self.get_logger().info('开始添加资源碰撞网格')
self.__planning_scene = self._get_planning_scene_service.call(
GetPlanningScene.Request()
).scene
planning_scene = PlanningScene()
planning_scene.is_diff = True
count = 0
for resource_id, tf_info in resource_tf_dict.items():
if resource_id in self.resource_model:
@@ -479,7 +531,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
quat_xyzw=q,
frame_id=resource_id
)
self.__planning_scene.world.collision_objects.append(collision_object)
count += 1
planning_scene.world.collision_objects.append(collision_object)
elif f"{tf_info['parent']}_" in self.resource_model:
# 获取资源的父级框架ID
id_ = f"{tf_info['parent']}_"
@@ -507,14 +560,26 @@ class ResourceMeshManager(BaseROS2DeviceNode):
quat_xyzw=q,
frame_id=resource_id
)
count += 1
planning_scene.world.collision_objects.append(collision_object)
self.__planning_scene.world.collision_objects.append(collision_object)
if count > 30:
req = ApplyPlanningScene.Request()
req.scene = planning_scene
self.publish_resource_tf()
self._apply_planning_scene_service.call(req)
self.__planning_scene_publisher.publish(planning_scene)
count = 0
planning_scene = PlanningScene()
planning_scene.is_diff = True
req = ApplyPlanningScene.Request()
req.scene = self.__planning_scene
self._apply_planning_scene_service.call_async(req)
req.scene = planning_scene
self.publish_resource_tf()
self._apply_planning_scene_service.call(req)
self.__planning_scene_publisher.publish(planning_scene)
self.get_logger().info('资源碰撞网格添加完成')
@@ -959,9 +1024,6 @@ class ResourceMeshManager(BaseROS2DeviceNode):
Attach collision object to the robot.
"""
if link_name is None:
link_name = self.__end_effector_name
msg = AttachedCollisionObject(
object=CollisionObject(id=id, operation=CollisionObject.ADD)
)

21
unilabos/ros/nodes/resource_tracker.py
View File

@@ -1,3 +1,5 @@
from typing import List, Tuple, Any
from unilabos.utils.log import logger
@@ -5,12 +7,12 @@ class DeviceNodeResourceTracker(object):
def __init__(self):
self.resources = []
self.root_resource2resource = {}
self.resource2parent_resource = {}
pass
def root_resource(self, resource):
if id(resource) in self.root_resource2resource:
return self.root_resource2resource[id(resource)]
def parent_resource(self, resource):
if id(resource) in self.resource2parent_resource:
return self.resource2parent_resource[id(resource)]
else:
return resource
@@ -44,20 +46,21 @@ class DeviceNodeResourceTracker(object):
self.loop_find_resource(r, resource_cls_type, identifier_key, getattr(query_resource, identifier_key))
)
assert len(res_list) == 1, f"{query_resource} 找到多个资源，请检查资源是否唯一: {res_list}"
self.root_resource2resource[id(query_resource)] = res_list[0]
self.resource2parent_resource[id(query_resource)] = res_list[0][0]
self.resource2parent_resource[id(res_list[0][1])] = res_list[0][0]
# 后续加入其他对比方式
return res_list[0]
return res_list[0][1]
def loop_find_resource(self, resource, target_resource_cls_type, identifier_key, compare_value):
def loop_find_resource(self, resource, target_resource_cls_type, identifier_key, compare_value, parent_res=None) -> List[Tuple[Any, Any]]:
res_list = []
# print(resource, target_resource_cls_type, identifier_key, compare_value)
children = getattr(resource, "children", [])
for child in children:
res_list.extend(self.loop_find_resource(child, target_resource_cls_type, identifier_key, compare_value))
res_list.extend(self.loop_find_resource(child, target_resource_cls_type, identifier_key, compare_value, resource))
if target_resource_cls_type == type(resource) or target_resource_cls_type == dict:
if hasattr(resource, identifier_key):
if getattr(resource, identifier_key) == compare_value:
res_list.append(resource)
res_list.append((parent_res, resource))
return res_list
def filter_find_list(self, res_list, compare_std_dict):

50
unilabos/ros/utils/driver_creator.py
View File

@@ -105,40 +105,41 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
return nested_dict_to_list(resource), Resource
return resource, source_type
def _process_resource_references(self, data: Any, to_dict=False) -> Any:
def _process_resource_references(self, data: Any, to_dict=False, states=None, prefix_path="") -> Any:
"""
递归处理资源引用，替换_resource_child_name对应的资源
Args:
data: 需要处理的数据，可能是字典、列表或其他类型
to_dict: 转换成对应的实例，还是转换成对应的字典
to_dict: 是否返回字典形式的资源
states: 用于保存所有资源状态
prefix_path: 当前递归路径
Returns:
处理后的数据
"""
from pylabrobot.resources import Deck, Resource
if states is None:
states = {}
if isinstance(data, dict):
# 检查是否包含资源引用
if "_resource_child_name" in data:
child_name = data["_resource_child_name"]
if child_name in self.children:
# 找到了对应的资源
resource = self.children[child_name]
# 检查是否需要转换资源类型
if "_resource_type" in data:
type_path = data["_resource_type"]
try:
# 尝试导入指定的类型
target_type = import_manager.get_class(type_path)
contain_model = not issubclass(target_type, Deck)
resource, target_type = self._process_resource_mapping(resource, target_type)
# 在截图中格式，是deserialize，所以这里要转成plr resource可deserialize的字典
# 这样后面执行deserialize的时候能够正确反序列化对应的物料
resource_instance: Resource = resource_ulab_to_plr(resource, contain_model)
# 使用 prefix_path 作为 key 存储资源状态
if to_dict:
return resource_instance.serialize()
serialized = resource_instance.serialize()
states[prefix_path] = resource_instance.serialize_all_state()
return serialized
else:
self.resource_tracker.add_resource(resource_instance)
return resource_instance
@@ -151,18 +152,21 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
else:
logger.warning(f"找不到资源引用 '{child_name}'，保持原值不变")
# 递归处理字典的每个值
# 递归处理每个键值
result = {}
for key, value in data.items():
result[key] = self._process_resource_references(value, to_dict)
new_prefix = f"{prefix_path}.{key}" if prefix_path else key
result[key] = self._process_resource_references(value, to_dict, states, new_prefix)
return result
# 处理列表类型
elif isinstance(data, list):
return [self._process_resource_references(item, to_dict) for item in data]
return [
self._process_resource_references(item, to_dict, states, f"{prefix_path}[{i}]")
for i, item in enumerate(data)
]
# 其他类型直接返回
return data
else:
return data
def create_instance(self, data: Dict[str, Any]) -> Optional[T]:
"""
@@ -187,10 +191,18 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
logger.debug(f"自动补充 _resource_type: {data[param_name]['_resource_type']}")
# 首先处理资源引用
processed_data = self._process_resource_references(data, to_dict=True)
states = {}
processed_data = self._process_resource_references(data, to_dict=True, states=states)
try:
self.device_instance = deserialize_method(**processed_data)
all_states = self.device_instance.serialize_all_state()
for k, v in states.items():
logger.debug(f"PyLabRobot反序列化设置状态：{k}")
for kk, vv in all_states.items():
if kk not in v:
v[kk] = vv
self.device_instance.deck.load_all_state(v)
self.resource_tracker.add_resource(self.device_instance)
self.post_create()
return self.device_instance # type: ignore
@@ -225,6 +237,10 @@ class PyLabRobotCreator(DeviceClassCreator[T]):
if hasattr(self.device_instance, "setup") and asyncio.iscoroutinefunction(getattr(self.device_instance, "setup")):
from unilabos.ros.nodes.base_device_node import ROS2DeviceNode
def done_cb(*args):
from pylabrobot.resources import set_volume_tracking
# from pylabrobot.resources import set_tip_tracking
set_volume_tracking(enabled=True)
# set_tip_tracking(enabled=True) # 序列化tip_spot has为False
logger.debug(f"PyLabRobot设备实例 {self.device_instance} 设置完成")
from unilabos.config.config import BasicConfig
if BasicConfig.vis_2d_enable: