补充日志

调整protocol node以更好支持多种类型的read和write

.gitignore

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

README.md

@@ -49,7 +49,7 @@ conda env update --file unilabos-[YOUR_OS].yml -n environment_name
 
 # Currently, you need to install the `unilabos_msgs` package
 # You can download the system-specific package from the Release page
-conda install ros-humble-unilabos-msgs-0.9.1-xxxxx.tar.bz2
+conda install ros-humble-unilabos-msgs-0.9.4-xxxxx.tar.bz2
 
 # Install PyLabRobot and other prerequisites
 git clone https://github.com/PyLabRobot/pylabrobot plr_repo

README_zh.md

@@ -49,7 +49,7 @@ conda env update --file unilabos-[YOUR_OS].yml -n 环境名
 
 # 现阶段，需要安装 `unilabos_msgs` 包
 # 可以前往 Release 页面下载系统对应的包进行安装
-conda install ros-humble-unilabos-msgs-0.9.1-xxxxx.tar.bz2
+conda install ros-humble-unilabos-msgs-0.9.4-xxxxx.tar.bz2
 
 # 安装PyLabRobot等前置
 git clone https://github.com/PyLabRobot/pylabrobot plr_repo

recipes/ros-humble-unilabos-msgs/recipe.yaml

@@ -1,6 +1,6 @@
 package:
   name: ros-humble-unilabos-msgs
-  version: 0.9.1
+  version: 0.9.4
 source:
   path: ../../unilabos_msgs
   folder: ros-humble-unilabos-msgs/src/work

recipes/unilabos/recipe.yaml

@@ -1,6 +1,6 @@
 package:
   name: unilabos
-  version: "0.9.1"
+  version: "0.9.4"
 
 source:
   path: ../..

setup.cfg

@@ -1,4 +1,2 @@
-[develop]
-script_dir=$base/lib/unilabos
 [install]
 install_scripts=$base/lib/unilabos

setup.py

@@ -4,7 +4,7 @@ package_name = 'unilabos'
 
 setup(
     name=package_name,
-    version='0.9.1',
+    version='0.9.4',
     packages=find_packages(),
     include_package_data=True,
     install_requires=['setuptools'],

test/experiments/biomek.json

@@ -0,0 +1,22 @@
+{
+    "nodes": [
+        {
+            "id": "BIOMEK",
+            "name": "BIOMEK",
+            "parent": null,
+            "type": "device",
+            "class": "liquid_handler.biomek",
+            "position": {
+                "x": 620.6111111111111,
+                "y": 171,
+                "z": 0
+            },
+            "config": {
+            },
+            "data": {},
+            "children": [
+            ]
+        }
+    ],
+    "links": []
+}

test/experiments/test_moveit.json

@@ -0,0 +1,35 @@
+{
+    "nodes": [
+
+        {
+            "id": "benyao",
+            "name": "benyao",
+            "children": [
+            ],
+            "parent": null,
+            "type": "device",
+            "class": "moveit.arm_slider",
+            "position": {
+                "x": 0,
+                "y": 0,
+                "z": 0
+            },
+            "config": {
+                "moveit_type": "arm_slider",
+                "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": [
+        
+    ]
+}

unilabos/app/main.py

@@ -10,6 +10,8 @@ from copy import deepcopy
 
 import yaml
 
+from unilabos.resources.graphio import tree_to_list
+
 # 首先添加项目根目录到路径
 current_dir = os.path.dirname(os.path.abspath(__file__))
 unilabos_dir = os.path.dirname(os.path.dirname(current_dir))
@@ -144,19 +146,19 @@ def main():
             else read_graphml(args_dict["graph"])
         )
         devices_and_resources = dict_from_graph(graph_res.physical_setup_graph)
-        args_dict["resources_config"] = initialize_resources(list(deepcopy(devices_and_resources).values()))
+        # args_dict["resources_config"] = initialize_resources(list(deepcopy(devices_and_resources).values()))
+        args_dict["resources_config"] = list(devices_and_resources.values())
         args_dict["devices_config"] = dict_to_nested_dict(deepcopy(devices_and_resources), devices_only=False)
-        # args_dict["resources_config"] = dict_to_tree(devices_and_resources, devices_only=False)
-
         args_dict["graph"] = graph_res.physical_setup_graph
     else:
         if args_dict["devices"] is None or args_dict["resources"] is None:
             print_status("Either graph or devices and resources must be provided.", "error")
             sys.exit(1)
         args_dict["devices_config"] = json.load(open(args_dict["devices"], encoding="utf-8"))
-        args_dict["resources_config"] = initialize_resources(
+        # args_dict["resources_config"] = initialize_resources(
-            list(json.load(open(args_dict["resources"], encoding="utf-8")).values())
+        #     list(json.load(open(args_dict["resources"], encoding="utf-8")).values())
-        )
+        # )
+        args_dict["resources_config"] = list(json.load(open(args_dict["resources"], encoding="utf-8")).values())
 
     print_status(f"{len(args_dict['resources_config'])} Resources loaded:", "info")
     for i in args_dict["resources_config"]:

unilabos/app/mq.py

@@ -1,6 +1,7 @@
 import json
 import time
 import traceback
+from typing import Optional
 import uuid
 
 import paho.mqtt.client as mqtt
@@ -161,12 +162,14 @@ class MQTTClient:
         status = {"data": device_status.get(device_id, {}), "device_id": device_id}
         address = f"labs/{MQConfig.lab_id}/devices/"
         self.client.publish(address, json.dumps(status), qos=2)
-        logger.critical(f"Device status published: address: {address}, {status}")
+        logger.debug(f"Device status published: address: {address}, {status}")
 
-    def publish_job_status(self, feedback_data: dict, job_id: str, status: str):
+    def publish_job_status(self, feedback_data: dict, job_id: str, status: str, return_info: Optional[str] = None):
         if self.mqtt_disable:
             return
-        jobdata = {"job_id": job_id, "data": feedback_data, "status": status}
+        if return_info is None:
+            return_info = "{}"
+        jobdata = {"job_id": job_id, "data": feedback_data, "status": status, "return_info": return_info}
         self.client.publish(f"labs/{MQConfig.lab_id}/job/list/", json.dumps(jobdata), qos=2)
 
     def publish_registry(self, device_id: str, device_info: dict):

unilabos/app/web/client.py

@@ -30,18 +30,18 @@ class HTTPClient:
             self.auth = MQConfig.lab_id
         info(f"HTTPClient 初始化完成: remote_addr={self.remote_addr}")
 
-    def resource_add(self, resources: List[Dict[str, Any]]) -> requests.Response:
+    def resource_add(self, resources: List[Dict[str, Any]], database_process_later:bool) -> requests.Response:
         """
         添加资源
 
         Args:
             resources: 要添加的资源列表
-
+            database_process_later: 后台处理资源
         Returns:
             Response: API响应对象
         """
         response = requests.post(
-            f"{self.remote_addr}/lab/resource/",
+            f"{self.remote_addr}/lab/resource/?database_process_later={1 if database_process_later else 0}",
             json=resources,
             headers={"Authorization": f"lab {self.auth}"},
             timeout=5,
@@ -60,7 +60,7 @@ class HTTPClient:
             Dict: 返回的资源数据
         """
         response = requests.get(
-            f"{self.remote_addr}/lab/resource/",
+            f"{self.remote_addr}/lab/resource/?edge_format=1",
             params={"id": id, "with_children": with_children},
             headers={"Authorization": f"lab {self.auth}"},
             timeout=5,
@@ -96,7 +96,7 @@ class HTTPClient:
             Response: API响应对象
         """
         response = requests.patch(
-            f"{self.remote_addr}/lab/resource/batch_update/",
+            f"{self.remote_addr}/lab/resource/batch_update/?edge_format=1",
             json=resources,
             headers={"Authorization": f"lab {self.auth}"},
             timeout=5,

unilabos/device_mesh/devices/arm_slider/config/initial_positions.yaml

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

unilabos/device_mesh/devices/arm_slider/config/joint_limits.yaml

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

unilabos/device_mesh/devices/arm_slider/config/kinematics.yaml

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

unilabos/device_mesh/devices/arm_slider/config/macro.ros2_control.xacro

@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="arm_slider_ros2_control" params="device_name mesh_path">
+        <xacro:property name="initial_positions" value="${load_yaml(mesh_path + '/devices/arm_slider/config/initial_positions.yaml')['initial_positions']}"/>
+
+        <ros2_control name="${device_name}arm_slider" 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>

unilabos/device_mesh/devices/arm_slider/config/macro.srdf.xacro

@@ -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="arm_slider_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>

unilabos/device_mesh/devices/arm_slider/config/move_group.json

@@ -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"
+    }
+}

unilabos/device_mesh/devices/arm_slider/config/moveit_controllers.yaml

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

unilabos/device_mesh/devices/arm_slider/config/moveit_planners.yaml

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

unilabos/device_mesh/devices/arm_slider/config/pilz_cartesian_limits.yaml

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

unilabos/device_mesh/devices/arm_slider/config/ros2_controllers.yaml

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

unilabos/device_mesh/devices/arm_slider/joint_limit.yaml

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

unilabos/device_mesh/devices/arm_slider/macro_device.xacro

@@ -0,0 +1,293 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro" name="arm_slider">
+
+  <xacro:macro name="arm_slider" 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/arm_slider/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/arm_slider/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/arm_slider/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/arm_slider/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/arm_slider/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/arm_slider/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/arm_slider/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/arm_slider/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/arm_slider/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/arm_slider/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/arm_slider/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/arm_slider/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/arm_slider/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/arm_slider/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/arm_slider/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/arm_slider/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/arm_slider/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>

unilabos/device_mesh/devices/benyao_arm/config/initial_positions.yaml

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

unilabos/device_mesh/devices/benyao_arm/config/joint_limits.yaml

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

unilabos/device_mesh/devices/benyao_arm/config/kinematics.yaml

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

unilabos/device_mesh/devices/benyao_arm/config/macro.ros2_control.xacro

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

unilabos/device_mesh/devices/benyao_arm/config/macro.srdf.xacro

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

unilabos/device_mesh/devices/benyao_arm/config/move_group.json

@@ -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"
+    }
+}

unilabos/device_mesh/devices/benyao_arm/config/moveit_controllers.yaml

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

unilabos/device_mesh/devices/benyao_arm/config/moveit_planners.yaml

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

unilabos/device_mesh/devices/benyao_arm/config/pilz_cartesian_limits.yaml

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

unilabos/device_mesh/devices/benyao_arm/config/ros2_controllers.yaml

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

unilabos/device_mesh/devices/benyao_arm/joint_limit.yaml

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

unilabos/device_mesh/devices/benyao_arm/macro_device.xacro

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

unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro

@@ -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"/>

unilabos/device_mesh/devices/slide_w140/macro_device.xacro

@@ -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"/>

unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/macro_device.xacro

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

unilabos/device_mesh/devices/thermo_orbitor_rs2_hotel/meta.json

@@ -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"
+    ]
+}

unilabos/device_mesh/devices/toyo_xyz/config/full_dev.urdf.xacro

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

unilabos/device_mesh/devices/toyo_xyz/config/initial_positions.yaml

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

unilabos/device_mesh/devices/toyo_xyz/config/joint_limits.yaml

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

unilabos/device_mesh/devices/toyo_xyz/config/kinematics.yaml

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

unilabos/device_mesh/devices/toyo_xyz/config/macro.ros2_control.xacro

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

unilabos/device_mesh/devices/toyo_xyz/config/macro.srdf.xacro

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

unilabos/device_mesh/devices/toyo_xyz/config/move_group.json

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

unilabos/device_mesh/devices/toyo_xyz/config/moveit_controllers.yaml

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

unilabos/device_mesh/devices/toyo_xyz/config/pilz_cartesian_limits.yaml

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

unilabos/device_mesh/devices/toyo_xyz/config/ros2_controllers.yaml

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

unilabos/device_mesh/devices/toyo_xyz/joint_config.json

@@ -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"
+    }
+}

unilabos/device_mesh/devices/toyo_xyz/macro_device.xacro

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