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

unilab添加moveit启动

Browse Source 
1,整合所有moveit节点到一个move_group中，并整合所有的controller依次激活
2,添加pymoveit2的节点，使用json可直接启动
3,修改机械臂规划方式，添加约束，让冗余关节不会进行过多移动
This commit is contained in:
zhangshixiang
2025-06-06 22:09:35 +08:00
parent 554766924e
commit 3d11a0cc50
80 changed files with 2378 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@@ -232,3 +232,4 @@ CATKIN_IGNORE
/**/local_config.py
*.graphml
unilabos/device_mesh/view_robot.rviz

65
test/experiments/plr_test_converted.json
View File

@@ -163,7 +163,7 @@
"type": "plate",
"class": "opentrons_96_filtertiprack_1000ul",
"position": {
"x": 0,
"x": 265.0,
"y": 0,
"z": 69
},
@@ -5762,8 +5762,8 @@
"type": "plate",
"class": "nest_96_wellplate_2ml_deep",
"position": {
"x": 265.0,
"y": 0,
"x": 0,
"y": 90.5,
"z": 69
},
"config": {
@@ -9623,6 +9623,65 @@
"pending_liquids": [],
"liquid_history": []
}
},
{
"id": "benyao",
"name": "benyao",
"children": [
],
"parent": null,
"type": "device",
"class": "moveit.benyao_arm",
"position": {
"x": -500,
"y": 1000,
"z": -100
},
"config": {
"moveit_type": "benyao_arm",
"joint_poses": {
"arm": {
"hotel_1": [1.05, 0.568, -1.0821, 0.0, 1.0821],
"home": [0.865, 0.09, 0.8727, 0.0, -0.8727]
}
},
"rotation": {
"x": 0,
"y": 0,
"z": -1.5708,
"type": "Rotation"
},
"device_config": {
}
},
"data": {
}
},
{
"id": "hotel",
"name": "hotel",
"children": [
],
"parent": null,
"type": "device",
"class": "hotel.thermo_orbitor_rs2_hotel",
"position": {
"x": 0,
"y": -700,
"z": -10
},
"config": {
"rotation": {
"x": 0,
"y": 0,
"z": 0,
"type": "Rotation"
},
"device_config": {
}
},
"data": {
}
}
],
"links": []

35
test/experiments/test_moveit.json Normal file
View File

@@ -0,0 +1,35 @@
{
"nodes": [
{
"id": "benyao",
"name": "benyao",
"children": [
],
"parent": null,
"type": "device",
"class": "moveit.benyao_arm",
"position": {
"x": 0,
"y": 0,
"z": 0
},
"config": {
"moveit_type": "benyao_arm",
"joint_poses": {
"arm": {
"home": [0.0, 0.2, 0.0, 0.0, 0.0],
"pick": [1.2, 0.0, 0.0, 0.0, 0.0]
}
},
"device_config": {
}
},
"data": {
}
}
],
"links": [
]
}

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
}
}

259
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,40 @@ 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"]+'_')
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 +159,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 +199,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 +296,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 +320,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 +341,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 +361,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

159
unilabos/device_mesh/view_robot.rviz
View File

@@ -1,23 +1,18 @@
Panels:
- Class: rviz_common/Displays
Help Height: 138
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /TF1
- /TF1/Frames1
- /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.4940796494483948
Tree Height: 602
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@@ -61,7 +56,7 @@ Visualization Manager:
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Show Names: true
Tree:
{}
Update Interval: 0
@@ -203,6 +198,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 +282,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
@@ -290,6 +342,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 +454,43 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 1.595012903213501
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.634795606136322
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.05074193701148033
Yaw: 5.590744495391846
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 2160
Hide Left Dock: false
collapsed: true
Height: 1897
Hide Left Dock: true
Hide Right Dock: true
MotionPlanning:
collapsed: false
collapsed: true
MotionPlanning - Trajectory Slider:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000400000000000003a3000005dcfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000006e000002510000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002cb0000037f000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000010000000087000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd0000000400000000000003a300000714fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000027000002c80000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000002600000026fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002fb00000440000002b800ffffff000000010000010f00000387fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003870000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000ddb0000071400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 4096
X: 0
Y: 0
Width: 3547
X: -857
Y: 149

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

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

@@ -0,0 +1,435 @@
from pathlib import Path
from threading import Thread
import time
import rclpy
import rclpy.duration
import rclpy.time
from unilabos_msgs.action import SendCmd
from rclpy.action import ActionServer, ActionClient
from rclpy.action.server import ServerGoalHandle
from rclpy.callback_groups import ReentrantCallbackGroup
from moveit_msgs.msg import JointConstraint, Constraints
from copy import deepcopy
from pymoveit2 import MoveIt2, GripperInterface
import numpy as np
from unilabos.ros.nodes.base_device_node import BaseROS2DeviceNode
# from tf_transformations import quaternion_from_euler
from tf2_ros import Buffer, TransformListener
import json
class MoveitInterface(BaseROS2DeviceNode):
def __init__(self, device_id, moveit_type, joint_poses, resource_tracker, rotation = None, device_config = None):
super().__init__(
driver_instance=self,
device_id=device_id,
status_types={},
action_value_mappings={},
hardware_interface={},
print_publish=False,
resource_tracker=resource_tracker,
)
self.callback_group = ReentrantCallbackGroup()
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.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
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
for move_group, config in data_config.items():
base_link_name = f"{device_id}_{config['base_link_name']}"
end_effector_name = f"{device_id}_{config['end_effector_name']}"
joint_names = [f"{device_id}_{name}" for name in config['joint_names']]
self.moveit2[f"{move_group}"] = MoveIt2(
node=self,
joint_names=joint_names,
base_link_name=base_link_name,
end_effector_name=end_effector_name,
group_name=f"{device_id}_{move_group}",
callback_group=self.callback_group,
use_move_group_action= True,
ignore_new_calls_while_executing = True
)
self.moveit2[f"{move_group}"].allowed_planning_time = 3.0
self.moveit_pose_server = ActionServer(
self,
SendCmd,
f'set_position',
execute_callback=self.callback,
callback_group=self.callback_group,
result_timeout=5000
)
self.moveit_pick_server = ActionServer(
self,
SendCmd,
f'pick_and_place',
execute_callback=self.pick_and_place_callback,
callback_group=self.callback_group,
result_timeout=5000
)
self.moveit_joint_server = ActionServer(
self,
SendCmd,
f'set_status',
execute_callback=self.set_status_callback,
callback_group=self.callback_group,
result_timeout=5000
)
self.create_timer(1, self.wait_for_resource_action,callback_group=ReentrantCallbackGroup())
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, SendCmd, self.resource_action)
self.resource_action_ok = True
while not self.resource_client.wait_for_server(timeout_sec=5.0):
self.get_logger().info('等待 TfUpdate 服务器...')
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 callback(self,goal_handle: ServerGoalHandle):
"""使用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()
try:
cmd_str = str(goal_handle.request.command).replace('\'','\"')
cmd_dict = json.loads(cmd_str)
self.moveit_task(**cmd_dict)
goal_handle.succeed()
result.success=True
except Exception as e:
print(e)
goal_handle.abort()
result.success=False
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)
self.resource_client.send_goal(goal_msg)
return True
def pick_and_place_callback(self,goal_handle: ServerGoalHandle):
"""
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(goal_handle.request.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 = self.moveit2[cmd_dict["move_group"]].compute_fk(joint_positions_)
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)
goal_handle.abort()
result.success=False
return result
goal_handle.succeed()
result.success=True
except Exception as e:
print(e)
goal_handle.abort()
self.cartesian_flag = False
result.success=False
return result
def set_status_callback(self,goal_handle: ServerGoalHandle):
"""
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 = str(goal_handle.request.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"]
if "separate" in cmd_dict.keys():
separate = cmd_dict["separate"]
else:
separate = None
status = cmd_dict["status"]
joint_positions_ = self.joint_poses[cmd_dict["move_group"]][status]
if separate is not None :
separate_j = [joint_positions_[i] for i in separate]
separate_n = [self.joint_names[i] for i in separate]
re = self.moveit_joint_task(**config, joint_positions=separate_j,joint_names=separate_n)
if not re:
goal_handle.abort()
result.success=False
return result
re = self.moveit_joint_task(**config,joint_positions=joint_positions_)
if not re:
goal_handle.abort()
result.success=False
return result
goal_handle.succeed()
result.success=True
except Exception as e:
print(e)
goal_handle.abort()
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

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

@@ -0,0 +1,17 @@
moveit.toyo_xyz:
description: Toyo XYZ
class:
module: unilabos.devices.ros_dev.moveit_interface:MoveitInterface
type: ros2
model:
type: device
mesh: toyo_xyz
moveit.benyao_arm:
description: Benyao Arm
class:
module: unilabos.devices.ros_dev.moveit_interface:MoveitInterface
type: ros2
model:
type: device
mesh: benyao_arm

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

@@ -124,6 +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:
@@ -397,7 +399,8 @@ class ResourceMeshManager(BaseROS2DeviceNode):
transform = self.tf_buffer.lookup_transform(
parent_id,
resource_id,
rclpy.time.Time(seconds=0)
self.get_clock().now(),
timeout=rclpy.duration.Duration(seconds=10)
)
# 提取转换中的位置和旋转信息