diff --git a/borunte_support/CMakeLists.txt b/borunte_support/CMakeLists.txt
new file mode 100644
index 0000000..85ca9ca
--- /dev/null
+++ b/borunte_support/CMakeLists.txt
@@ -0,0 +1,200 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(borunte_support)
+
+#~ Find catkin macros and libraries
+#~ if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+#~ is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+)
+
+#~ System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+#~ Uncomment this if the package has a setup.py. This macro ensures
+#~ modules and global scripts declared therein get installed
+#~ See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+#~ Declare ROS messages, services and actions ##
+#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+#~ To declare and build messages, services or actions from within this
+#~ package, follow these steps:
+#~ * Let MSG_DEP_SET be the set of packages whose message types you use in
+#~   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+#~ * In the file package.xml:
+#~   * add a build_depend tag for "message_generation"
+#~   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+#~   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+#~     but can be declared for certainty nonetheless:
+#~     * add a run_depend tag for "message_runtime"
+#~ * In this file (CMakeLists.txt):
+#~   * add "message_generation" and every package in MSG_DEP_SET to
+#~     find_package(catkin REQUIRED COMPONENTS ...)
+#~   * add "message_runtime" and every package in MSG_DEP_SET to
+#~     catkin_package(CATKIN_DEPENDS ...)
+#~   * uncomment the add_*_files sections below as needed
+#~     and list every .msg/.srv/.action file to be processed
+#~   * uncomment the generate_messages entry below
+#~   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+#~ Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+#~ Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+#~ Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+#~ Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs
+# )
+
+#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+#~ Declare ROS dynamic reconfigure parameters ##
+#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+#~ To declare and build dynamic reconfigure parameters within this
+#~ package, follow these steps:
+#~ * In the file package.xml:
+#~   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+#~ * In this file (CMakeLists.txt):
+#~   * add "dynamic_reconfigure" to
+#~     find_package(catkin REQUIRED COMPONENTS ...)
+#~   * uncomment the "generate_dynamic_reconfigure_options" section below
+#~     and list every .cfg file to be processed
+
+#~ Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+#~ catkin specific configuration ##
+#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+#~ The catkin_package macro generates cmake config files for your package
+#~ Declare things to be passed to dependent projects
+#~ INCLUDE_DIRS: uncomment this if you package contains header files
+#~ LIBRARIES: libraries you create in this project that dependent projects also need
+#~ CATKIN_DEPENDS: catkin_packages dependent projects also need
+#~ DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES borunte_support
+    CATKIN_DEPENDS
+        roscpp
+        rospy
+        std_msgs
+#  DEPENDS system_lib
+)
+
+#~~~~~~~~~~
+#~ Build ##
+#~~~~~~~~~~
+
+#~ Specify additional locations of header files
+#~ Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+#~ Declare a C++ library
+# add_library(borunte_support
+#   src/${PROJECT_NAME}/borunte_support.cpp
+# )
+
+#~ Add cmake target dependencies of the library
+#~ as an example, code may need to be generated before libraries
+#~ either from message generation or dynamic reconfigure
+# add_dependencies(borunte_support ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+#~ Declare a C++ executable
+# add_executable(borunte_support_node src/borunte_support_node.cpp)
+
+#~ Add cmake target dependencies of the executable
+#~ same as for the library above
+# add_dependencies(borunte_support_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+#~ Specify libraries to link a library or executable target against
+# target_link_libraries(borunte_support_node
+#   ${catkin_LIBRARIES}
+# )
+
+#~~~~~~~~~~~~
+#~ Install ##
+#~~~~~~~~~~~~
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+#~ Mark executable scripts (Python etc.) for installation
+#~ in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+#~ Mark executables and/or libraries for installation
+# install(TARGETS borunte_support borunte_support_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+#~ Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+#~ Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+# Install python executables
+catkin_install_python(
+  PROGRAMS
+        scripts/run_sim_ui.py
+        scripts/run_machinetalk.py
+
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  )
+
+#~~~~~~~~~~~~
+#~ Testing ##
+#~~~~~~~~~~~~
+
+#~ Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_borunte_support.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+#~ Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/borunte_support/config/borunte.rviz b/borunte_support/config/borunte.rviz
new file mode 100644
index 0000000..e691383
--- /dev/null
+++ b/borunte_support/config/borunte.rviz
@@ -0,0 +1,179 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /RobotModel1/Links1
+      Splitter Ratio: 0.5
+    Tree Height: 466
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Class: rviz/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.1
+      Reference Frame: world
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        arm_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_link_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        arm_link_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        milling_machine:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 4.1036
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 2.12118
+        Y: 0.100075
+        Z: 0.851119
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.329797
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 6.07196
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 744
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000002a200000261fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000002800000261000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e0067000000010b000001d70000000000000000000000010000010f0000025efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000025e000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005150000003bfc0100000002fb0000000800540069006d0065010000000000000515000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005150000026100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1301
+  X: 65
+  Y: 24
diff --git a/borunte_support/config/borunte_controllers.yaml b/borunte_support/config/borunte_controllers.yaml
new file mode 100644
index 0000000..08055e2
--- /dev/null
+++ b/borunte_support/config/borunte_controllers.yaml
@@ -0,0 +1,109 @@
+# Moveit
+controller_list:
+  - name: /hardware_interface/position_trajectory_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6]
+
+  # Settings for ros_control hardware interface
+hardware_interface:
+  joints:
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
+  # choose the control mode
+  # 0: position
+  # 1: velocity
+  control_mode: 0
+  sim_control_mode: 0 # 0: position, 1: velocity
+
+# Publish all joint states
+# Creates the /joint_states topic necessary in ROS
+joint_state_controller:
+  type:         joint_state_controller/JointStateController
+  publish_rate: 50
+
+# Joint Trajectory Controller
+# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
+position_trajectory_controller:
+  type: position_controllers/JointTrajectoryController
+  # These joints can likely just be copied from the
+  # hardware_interface list above
+  joints:
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
+  constraints:
+    goal_time: 5.0
+    #stopped_position_tolerance: 0.4 # Defaults to 0.01
+    joint_1:
+      trajectory: 0.60
+      goal:       0.15
+    joint_2:
+      trajectory: 0.60
+      goal:       0.15
+    joint_3:
+      trajectory: 0.60
+      goal:       0.15
+    joint_4:
+      trajectory: 0.60
+      goal:       0.15
+    joint_5:
+      trajectory: 0.60
+      goal:       0.15
+    joint_6:
+      trajectory: 0.60
+      goal:       0.15
+  # gains:
+  #     joint1: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
+  #     joint2: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
+
+  # state_publish_rate:  50 # Defaults to 50
+  # action_monitor_rate: 20 # Defaults to 20
+  #hold_trajectory_duration: 0 # Defaults to 0.5
+
+# Individual Position Controllers
+# Allows to send individual ROS msg of Float64 to each joint separately
+joint_1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint_1
+  pid: {p: 100.0, i: 0.01, d: 10.0}
+joint_2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint_2
+  pid: {p: 100.0, i: 0.01, d: 10.0}
+joint_3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint_3
+  pid: {p: 100.0, i: 0.01, d: 10.0}
+joint_4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint_4
+  pid: {p: 100.0, i: 0.01, d: 10.0}
+joint_5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint_4
+  pid: {p: 100.0, i: 0.01, d: 10.0}
+joint_6_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint_6
+  pid: {p: 100.0, i: 0.01, d: 10.0}
+
+# Group Position Controllers ---------------------------------------
+# Allows to send single ROS msg of Float64MultiArray to all joints
+joint_position_controller:
+  type: position_controllers/JointGroupPositionController
+  joints:
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
diff --git a/borunte_support/config/borunte_stand.rviz b/borunte_support/config/borunte_stand.rviz
new file mode 100644
index 0000000..6f3db4f
--- /dev/null
+++ b/borunte_support/config/borunte_stand.rviz
@@ -0,0 +1,328 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 138
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /TF1
+        - /TF1/Frames1
+      Splitter Ratio: 0.5
+    Tree Height: 1368
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base:
+          Value: true
+        base_link:
+          Value: true
+        borunte_ezgripper_adapter:
+          Value: true
+        borunte_stand_flange:
+          Value: true
+        borunte_stand_link:
+          Value: true
+        depth_camera_link:
+          Value: true
+        flange:
+          Value: true
+        link_1:
+          Value: true
+        link_2:
+          Value: true
+        link_3:
+          Value: true
+        link_4:
+          Value: true
+        link_5:
+          Value: true
+        link_6:
+          Value: true
+        main_ezgripper_finger_L1_1:
+          Value: true
+        main_ezgripper_finger_L1_2:
+          Value: true
+        main_ezgripper_finger_L2_1:
+          Value: true
+        main_ezgripper_finger_L2_2:
+          Value: true
+        main_ezgripper_finger_L2_pad_1:
+          Value: true
+        main_ezgripper_finger_L2_pad_2:
+          Value: true
+        main_ezgripper_palm_link:
+          Value: true
+        rgb_camera_link:
+          Value: true
+        tool0:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 0.20000000298023224
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        borunte_stand_link:
+          borunte_stand_flange:
+            base_link:
+              base:
+                {}
+              link_1:
+                link_2:
+                  link_3:
+                    link_4:
+                      link_5:
+                        link_6:
+                          flange:
+                            borunte_ezgripper_adapter:
+                              depth_camera_link:
+                                {}
+                              main_ezgripper_palm_link:
+                                main_ezgripper_finger_L1_1:
+                                  main_ezgripper_finger_L2_1:
+                                    main_ezgripper_finger_L2_pad_1:
+                                      {}
+                                main_ezgripper_finger_L1_2:
+                                  main_ezgripper_finger_L2_2:
+                                    main_ezgripper_finger_L2_pad_2:
+                                      {}
+                              rgb_camera_link:
+                                {}
+                            tool0:
+                              {}
+          world:
+            {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        borunte_ezgripper_adapter:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        borunte_stand_flange:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        borunte_stand_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        depth_camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        flange:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        link_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        main_ezgripper_finger_L1_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        main_ezgripper_finger_L1_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        main_ezgripper_finger_L2_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        main_ezgripper_finger_L2_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        main_ezgripper_finger_L2_pad_1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        main_ezgripper_finger_L2_pad_2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        main_ezgripper_palm_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        rgb_camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        tool0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: borunte_stand_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 3.2491230964660645
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.1167607307434082
+        Y: 0.004363108426332474
+        Z: 0.720638632774353
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5397963523864746
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 5.2585954666137695
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1732
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001d50000061cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a0000061c000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002aafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000040000002aa0000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000b0f0000004efc0100000002fb0000000800540069006d0065010000000000000b0f0000024600fffffffb0000000800540069006d00650100000000000004500000000000000000000009340000061c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 2831
+  X: -200
+  Y: 261
diff --git a/borunte_support/config/create_dummy_signals.py b/borunte_support/config/create_dummy_signals.py
new file mode 100644
index 0000000..b79a15e
--- /dev/null
+++ b/borunte_support/config/create_dummy_signals.py
@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+# very small helper script for short-cutting
+# input and output pins when passing data
+# thru HAL shared memory setup
+import sys
+import time
+import subprocess
+
+from machinekit import launcher
+from machinekit import hal
+
+try:
+    launcher.check_installation()
+    launcher.cleanup_session()  # kill any running Machinekit instances
+    launcher.start_realtime()  # start Machinekit realtime environment
+
+    while 'ul_iface_component.joint_6.pos-cmd' not in hal.pins:
+        time.sleep(0.5)
+
+    print('found HAL pin, connecting')
+    # create 6 signals between pos-cmd and pos-fb pins
+    # when machinekit_ros_control has started with
+    # parameter machinekit_interface: 0
+    for i in range(1, 7):
+        hal.Pin('ul_iface_component.joint_%s.pos-cmd' % i).link(
+            'ul_iface_component.joint_%s.pos-fb' % i
+        )
+
+    launcher.register_exit_handler()  # enable on ctrl-C, needs to executed after HAL files
+
+    while True:
+        launcher.check_processes()
+        time.sleep(1)
+
+except subprocess.CalledProcessError:
+    launcher.end_session()
+    sys.exit(1)
+
+sys.exit(0)
diff --git a/borunte_support/config/defaults/digital_io_names.yaml b/borunte_support/config/defaults/digital_io_names.yaml
new file mode 100644
index 0000000..0bc6e18
--- /dev/null
+++ b/borunte_support/config/defaults/digital_io_names.yaml
@@ -0,0 +1,3 @@
+io:
+  digital_in_names: {}
+  digital_out_names: {}
diff --git a/borunte_support/config/defaults/global_waypoints.yaml b/borunte_support/config/defaults/global_waypoints.yaml
new file mode 100644
index 0000000..347e39d
--- /dev/null
+++ b/borunte_support/config/defaults/global_waypoints.yaml
@@ -0,0 +1,4 @@
+global_waypoints:
+  home:
+    - JOINTS
+    - [0, 0, 0, 0, 0, 0]
diff --git a/borunte_support/config/defaults/machinetalk.yaml b/borunte_support/config/defaults/machinetalk.yaml
new file mode 100644
index 0000000..a624a4b
--- /dev/null
+++ b/borunte_support/config/defaults/machinetalk.yaml
@@ -0,0 +1,2 @@
+machinetalk:
+  nodes: []
diff --git a/borunte_support/config/defaults/user_config.yaml b/borunte_support/config/defaults/user_config.yaml
new file mode 100644
index 0000000..e3fc2a5
--- /dev/null
+++ b/borunte_support/config/defaults/user_config.yaml
@@ -0,0 +1,9 @@
+user_config:
+  jograte: 0.1
+  feedrate: 0.1
+  maximum_velocity: 0.1
+  jog:
+    continuous: false
+    step_size: 0
+  recent_paths: []
+  optional_stop: false
diff --git a/borunte_support/config/hal_hw_interface.yaml b/borunte_support/config/hal_hw_interface.yaml
new file mode 100644
index 0000000..a985782
--- /dev/null
+++ b/borunte_support/config/hal_hw_interface.yaml
@@ -0,0 +1,106 @@
+hal_mgr:
+  hal_files:
+    # Load a HAL file that sets up the realtime configuration
+    - setup.py
+    - hal_io.py
+    - robot_io_pins.py
+    - haltalk.py
+
+# Settings for ros_control hardware interface
+hardware_interface:
+  joints:
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
+  sim_control_mode: 0 # 0: position, 1: velocity
+
+# Publish all joint states ----------------------------------
+# Creates the /joint_states topic necessary in ROS
+joint_state_controller:
+  type:         joint_state_controller/JointStateController
+  publish_rate: 50
+
+# Joint Trajectory Controller -------------------------------
+# For detailed explanations of parameter see
+# http://wiki.ros.org/joint_trajectory_controller
+position_trajectory_controller:
+  type: position_controllers/JointTrajectoryController
+  # These joints can likely just be copied from the hardware_interface
+  # list above
+  joints:
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
+  constraints:
+    goal_time: 5.0
+    #stopped_position_tolerance: 0.4 # Defaults to 0.01
+    joint_1:
+      trajectory: 0.60
+      goal:       0.15
+    joint_2:
+      trajectory: 0.60
+      goal:       0.15
+    joint_3:
+      trajectory: 0.60
+      goal:       0.15
+    joint_4:
+      trajectory: 0.60
+      goal:       0.15
+    joint_5:
+      trajectory: 0.60
+      goal:       0.15
+    joint_6:
+      trajectory: 0.60
+      goal:       0.15
+  # gains:
+  #   joint_1: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
+  #   joint_2: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
+
+  state_publish_rate:  50 # Defaults to 50
+  action_monitor_rate: 20 # Defaults to 20
+  stop_trajectory_duration: 0 # Defaults to 0.5
+
+# Individual Position Controllers ---------------------------------------
+# Allows to send individual ROS msg of Float64 to each joint separately
+joint_1_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint_1
+  pid: {p: 100.0, i: 0.01, d: 10.0}
+joint_2_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint_2
+  pid: {p: 100.0, i: 0.01, d: 10.0}
+joint_3_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint_3
+  pid: {p: 100.0, i: 0.01, d: 10.0}
+joint_4_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint_4
+  pid: {p: 100.0, i: 0.01, d: 10.0}
+joint_5_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint_5
+  pid: {p: 100.0, i: 0.01, d: 10.0}
+joint_6_position_controller:
+  type: position_controllers/JointPositionController
+  joint: joint_6
+  pid: {p: 100.0, i: 0.01, d: 10.0}
+
+# Group Position Controllers ---------------------------------------
+# Allows to send single ROS msg of Float64MultiArray to all joints
+joint_position_controller:
+  type: position_controllers/JointGroupPositionController
+  joints:
+    - joint_1
+    - joint_2
+    - joint_3
+    - joint_4
+    - joint_5
+    - joint_6
diff --git a/borunte_support/config/hal_io.yaml b/borunte_support/config/hal_io.yaml
new file mode 100644
index 0000000..2e28b7a
--- /dev/null
+++ b/borunte_support/config/hal_io.yaml
@@ -0,0 +1,58 @@
+hal_io:
+
+  # I/O main loop publish rate in Hz
+  update_rate: 10
+
+  # Pin definitions
+  #
+  # output_pins: are read from a topic (default) or service
+
+  # HAL pins are named hal_io.<pin_name>
+
+  # valid keys for all pins:
+  #   hal_type:  BIT, FLOAT, S32, U32
+
+  publish_pins:
+    # publish_pins values from HAL pin published to ROS topic
+    # valid keys:
+    #   hal_dir:  IN (default), IO
+    #   pub_topic:  (any valid ROS topic name, default /hal_io/<pin_name>)
+    digital_in_1:
+      hal_type: BIT
+    digital_in_2:
+      hal_type: BIT
+    digital_in_3:
+      hal_type: BIT
+    digital_in_4:
+      hal_type: BIT
+    state_fb:
+      hal_type: S32
+  subscribe_pins:
+    # subscribe_pins values from ROS topic copied to HAL output pin
+    # valid keys:
+    #   hal_dir:  OUT (default), IO
+    #   sub_topic:  (any valid ROS topic name, default /hal_io/<pin_name>)
+    digital_out_1:
+      hal_type: BIT
+    digital_out_2:
+      hal_type: BIT
+    digital_out_3:
+      hal_type: BIT
+    digital_out_4:
+      hal_type: BIT
+    zero_all_joints:
+      hal_type: BIT
+      hal_dir: IO
+    state_cmd:
+      hal_type: U32
+      hal_dir: IO
+    reset:
+      hal_type: BIT
+      hal_dir: IO
+# service_pins:
+    # service_pins values from ROS service copied to HAL output pin
+    #   and published topic
+    # valid keys:
+    #   hal_dir:  OUT (default), IO
+    #   service_name:  (any valid ROS service name, default /hal_io/<pin_name>)
+    #   pub_topic:  (any valid ROS topic name, default /hal_io/<pin_name>)
diff --git a/borunte_support/config/io_config.yaml b/borunte_support/config/io_config.yaml
new file mode 100644
index 0000000..f0702e9
--- /dev/null
+++ b/borunte_support/config/io_config.yaml
@@ -0,0 +1,12 @@
+# hal_io publish/subscribe topics for IO pins
+io:
+  digital_in_topics:
+    - /hal_io/digital_in_1
+    - /hal_io/digital_in_2
+    - /hal_io/digital_in_3
+    - /hal_io/digital_in_4
+  digital_out_topics:
+    - /hal_io/digital_out_1
+    - /hal_io/digital_out_2
+    - /hal_io/digital_out_3
+    - /hal_io/digital_out_4
diff --git a/borunte_support/config/jog_arm_settings_gui.yaml b/borunte_support/config/jog_arm_settings_gui.yaml
new file mode 100644
index 0000000..d9a0828
--- /dev/null
+++ b/borunte_support/config/jog_arm_settings_gui.yaml
@@ -0,0 +1,34 @@
+gazebo: false # Whether the robot is started in simulation environment
+collision_check: false # Check collisions?
+command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+scale: # Only used if command_in_type=="unitless"
+  linear:  0.008 # Max linear velocity. Meters per publish_period. Units is [m/s]
+  rotational:  0.016 # Max angular velocity. Rads per publish_period. Units is [rad/s]
+  joint: 0.04 # Max joint angular/linear velocity. Rads or Meters per publish period. Units is [rad/s] or [m/s].
+cartesian_command_in_topic:  jog_arm_server/frame_delta_jog_cmds # Topic for xyz commands
+joint_command_in_topic: jog_arm_server/joint_delta_jog_cmds # Topic for angle commands
+command_frame:  world # TF frame that incoming cmds are given in
+incoming_command_timeout:  1 # Stop jogging if X seconds elapse without a new cmd
+joint_topic:  joint_states
+move_group_name:  manipulator # Often 'manipulator' or 'arm'
+lower_singularity_threshold:  30 # Start decelerating when the condition number hits this (close to singularity). Larger --> closer to singularity
+hard_stop_singularity_threshold: 45 # Stop when the condition number hits this. Larger --> closer to singularity
+lower_collision_proximity_threshold: 0.1 # Start decelerating when a collision is this far [m]
+hard_stop_collision_proximity_threshold: 0.005 # Stop when a collision is this far [m]
+planning_frame:  world # The MoveIt! planning frame. Often 'base_link'
+# Larger c --> trust the filtered data more, trust the measurements less.
+low_pass_filter_coeff:  2. # Larger-> more smoothing to jog commands, but more lag.
+publish_period:  0.008 # 1/Nominal publish rate [seconds]
+publish_delay: 0.005 # delay between calculation and execution start of command
+collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+warning_topic:  jog_arm_server/warning # Publish boolean warnings to this topic
+joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+command_out_topic: position_trajectory_controller/command
+# What type of topic does your robot driver expect?
+# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController)
+# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots)
+command_out_type: trajectory_msgs/JointTrajectory
+# Can save some bandwidth as most robots only require positions or velocities
+publish_joint_positions: true
+publish_joint_velocities: true
+publish_joint_accelerations: false
diff --git a/borunte_support/config/jog_arm_settings_spacemouse.yaml b/borunte_support/config/jog_arm_settings_spacemouse.yaml
new file mode 100644
index 0000000..86e42ab
--- /dev/null
+++ b/borunte_support/config/jog_arm_settings_spacemouse.yaml
@@ -0,0 +1,34 @@
+gazebo: false # Whether the robot is started in simulation environment
+collision_check: false # Check collisions?
+command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+scale: # Only used if command_in_type=="unitless"
+  linear:  0.002 # Max linear velocity. Meters per publish_period. Units is [m/s]
+  rotational:  0.004 # Max angular velocity. Rads per publish_period. Units is [rad/s]
+  joint: 0.01 # Max joint angular/linear velocity. Rads or Meters per publish period. Units is [rad/s] or [m/s].
+cartesian_command_in_topic:  jog_arm_server/frame_delta_jog_cmds_spacemouse # Topic for xyz commands
+joint_command_in_topic: jog_arm_server/joint_delta_jog_cmds_spacemouse # Topic for angle commands
+command_frame:  world # TF frame that incoming cmds are given in
+incoming_command_timeout:  0.5  # Stop jogging if X seconds elapse without a new cmd
+joint_topic:  joint_states
+move_group_name:  manipulator # Often 'manipulator' or 'arm'
+lower_singularity_threshold:  30 # Start decelerating when the condition number hits this (close to singularity). Larger --> closer to singularity
+hard_stop_singularity_threshold: 45 # Stop when the condition number hits this. Larger --> closer to singularity
+lower_collision_proximity_threshold: 0.1 # Start decelerating when a collision is this far [m]
+hard_stop_collision_proximity_threshold: 0.005 # Stop when a collision is this far [m]
+planning_frame:  world # The MoveIt! planning frame. Often 'base_link'
+# Larger c --> trust the filtered data more, trust the measurements less.
+low_pass_filter_coeff:  2. # Larger-> more smoothing to jog commands, but more lag.
+publish_period:  0.008 # 1/Nominal publish rate [seconds]
+publish_delay: 0.005 # delay between calculation and execution start of command
+collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+warning_topic:  jog_arm_server/warning # Publish boolean warnings to this topic
+joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+command_out_topic: position_trajectory_controller/command
+# What type of topic does your robot driver expect?
+# Currently supported are std_msgs/Float64MultiArray (for ros_control JointGroupVelocityController)
+# or trajectory_msgs/JointTrajectory (for Universal Robots and other non-ros_control robots)
+command_out_type: trajectory_msgs/JointTrajectory
+# Can save some bandwidth as most robots only require positions or velocities
+publish_joint_positions: true
+publish_joint_velocities: true
+publish_joint_accelerations: false
diff --git a/borunte_support/halfiles/hal_io.py b/borunte_support/halfiles/hal_io.py
new file mode 100644
index 0000000..28b999e
--- /dev/null
+++ b/borunte_support/halfiles/hal_io.py
@@ -0,0 +1,11 @@
+# -*- coding: utf-8 -*-
+from machinekit import hal
+
+
+def load_hal_io_component():
+    # Load hal_io user component
+    cmd = 'rosrun hal_hw_interface hal_io'
+    hal.loadusr(cmd, wait=True, wait_name='hal_io', wait_timeout=10.0)
+
+
+load_hal_io_component()
diff --git a/borunte_support/halfiles/haltalk.py b/borunte_support/halfiles/haltalk.py
new file mode 100644
index 0000000..0ae0472
--- /dev/null
+++ b/borunte_support/halfiles/haltalk.py
@@ -0,0 +1,4 @@
+# -*- coding: utf-8 -*-
+from machinekit import hal
+
+hal.loadusr('haltalk', wait=True)
diff --git a/borunte_support/halfiles/robot_io_pins.py b/borunte_support/halfiles/robot_io_pins.py
new file mode 100644
index 0000000..001d983
--- /dev/null
+++ b/borunte_support/halfiles/robot_io_pins.py
@@ -0,0 +1,45 @@
+# -*- coding: utf-8 -*-
+
+import rospy
+from machinekit import hal
+
+
+def setup_sim_pins():
+    rospy.loginfo('Creating io-rcomp HAL remote component')
+    # mirror hal_io to rcomp
+    hal_io = hal.components['hal_io']
+    rcomp = hal.RemoteComponent('io-rcomp', timer=100)
+    for pin in hal_io.pins():
+        name = '.'.join(pin.name.split('.')[1:])
+        if not name.startswith('digital_'):
+            continue
+        dir_map = {hal.HAL_IN: hal.HAL_OUT, hal.HAL_OUT: hal.HAL_IN}
+        rcomp.newpin(name, pin.type, dir_map.get(pin.dir, hal.HAL_IO))
+        if pin.linked:
+            continue
+        if pin.dir == hal.HAL_IN:
+            rcomp.pin(name).link(hal.pins[pin.name])
+        else:
+            hal.pins[pin.name].link(rcomp.pin(name))
+    rcomp.ready()
+
+
+def setup_gripper_pins():
+    hal.Pin('hal_io.digital_out_1').link('gripper-open-close')
+    hal.Pin('hal_io.digital_in_1').link('gripper-opened')
+
+
+def setup_robot_control_pins():
+    hal.Pin('hal_io.state_cmd').link(hal.Signal('state-cmd'))
+    hal.Pin('hal_io.state_fb').link(hal.Signal('state-fb'))
+    hal.Pin('hal_io.reset').link(hal.Signal('reset-in'))
+
+
+def setup_pins():
+    sim_mode = rospy.get_param('/hal_hardware/sim_mode', True)
+    if sim_mode:
+        setup_sim_pins()
+    setup_robot_control_pins()
+
+
+setup_pins()
diff --git a/borunte_support/halfiles/setup.py b/borunte_support/halfiles/setup.py
new file mode 100644
index 0000000..f926a41
--- /dev/null
+++ b/borunte_support/halfiles/setup.py
@@ -0,0 +1,72 @@
+# -*- coding: utf-8 -*-
+import os
+import sys
+import time
+
+import rospy
+from machinekit import launcher
+from machinekit import rtapi as rt
+from machinekit import hal
+
+HARDWARE_RESET_DELAY_S = 0.1
+HARDWARE_SETTLE_TIME_S = 1.0
+NUM_JOINTS = 6
+
+HAL_CONFIG_PATH = rospy.get_param('/hal_config_path', '')
+sys.path.append(HAL_CONFIG_PATH)
+os.environ['PYTHONPATH'] += ':{}'.format(HAL_CONFIG_PATH)
+
+
+def install_rt_comps():
+    launcher.install_comp(
+        os.path.join(HAL_CONFIG_PATH, 'components', 'absolute_joint.icomp')
+    )
+
+
+def create_hw_interface(thread):
+    rt.loadrt('{}/hal_hw_interface'.format(os.environ['COMP_DIR']))
+    hal.addf('hal_hw_interface', thread.name)
+
+    oneshot = rt.newinst('oneshot', 'oneshot.hw_reset')
+    hal.addf(oneshot.name, thread.name)
+    oneshot.pin('in').link('power-on')
+    oneshot.pin('out').link('hw-reset')
+    oneshot.pin('width').set(HARDWARE_RESET_DELAY_S)
+
+
+def connect_hw_interface():
+    for nr in range(1, NUM_JOINTS + 1):
+        hal.Pin('hal_hw_interface.joint_{}.pos-cmd'.format(nr)).link(
+            'joint-{}-cmd-pos'.format(nr)
+        )
+        hal.Pin('hal_hw_interface.joint_{}.pos-fb'.format(nr)).link(
+            'joint-{}-fb-out-pos'.format(nr)
+        )
+
+        hal.Pin('hal_hw_interface.reset').link('hw-reset')
+
+
+def setup_hal():
+    sim_mode = rospy.get_param('/hal_hardware/sim_mode', True)
+    tool = rospy.get_param('/hal_hardware/tool', 'none')
+    rospy.loginfo('tool {}'.format(tool))
+    if sim_mode:
+        os.environ['SIM_MODE'] = '1'
+    os.environ['ROBOT_TOOL'] = tool
+
+    from borunte_hal.robot import setup_thread, configure_hal
+
+    cgname = rospy.get_param('/hal_mgr/rt_cgname', None)
+    thread = setup_thread(cgname)
+    create_hw_interface(thread)
+    configure_hal(thread)
+    time.sleep(HARDWARE_SETTLE_TIME_S)
+    connect_hw_interface()
+
+    # start the sim config "powered on"
+    # if sim:
+    #    hal.Signal('state-cmd').set(1)
+
+
+install_rt_comps()
+setup_hal()
diff --git a/borunte_support/launch/display_borunte.launch b/borunte_support/launch/display_borunte.launch
new file mode 100644
index 0000000..3e30f1a
--- /dev/null
+++ b/borunte_support/launch/display_borunte.launch
@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<launch>
+<!-- <arg name="model" default="$(find borunte)/config/borunte.xacro meshDirectory:=$(find borunte)/mesh/" /> -->
+<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find borunte_support)/urdf/borunte.xacro'" />
+<arg name="gui" default="True" />
+
+<!-- <param name="robot_description" textfile="$(arg model)" /> -->
+
+<param name="use_gui" value="$(arg gui)" />
+
+<node name="joint_state_publisher" pkg="joint_state_publisher"
+type="joint_state_publisher" />
+
+<node name="robot_state_publisher" pkg="robot_state_publisher"
+type="state_publisher" />
+
+<node pkg="rviz" type="rviz" name="rviz" args="-d $(find borunte_support)/config/borunte.rviz"/>
+
+</launch>
diff --git a/borunte_support/launch/display_borunte_stand.launch b/borunte_support/launch/display_borunte_stand.launch
new file mode 100644
index 0000000..c64d6f7
--- /dev/null
+++ b/borunte_support/launch/display_borunte_stand.launch
@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<launch>
+
+<arg name="tool" default="ezgripper_gen2_plus" />
+<arg name="gui" default="True" />
+
+<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find borunte_support)/urdf/borunte_on_stand.xacro' tool:=$(arg tool)"/>
+
+<param name="use_gui" value="$(arg gui)" />
+
+<node name="joint_state_publisher" pkg="joint_state_publisher"
+type="joint_state_publisher" />
+
+<node name="robot_state_publisher" pkg="robot_state_publisher"
+type="state_publisher" />
+
+<node pkg="rviz" type="rviz" name="rviz" args="-d $(find borunte_support)/config/borunte_stand.rviz"/>
+
+</launch>
diff --git a/borunte_support/launch/hal_hardware.launch b/borunte_support/launch/hal_hardware.launch
new file mode 100644
index 0000000..9d2655c
--- /dev/null
+++ b/borunte_support/launch/hal_hardware.launch
@@ -0,0 +1,76 @@
+<?xml version="1.0"?>
+<launch>
+  <!--
+      This .launch file brings up the borunte controller
+  -->
+  <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
+  <arg name="load_robot_description" default="true"/>
+
+  <!-- The name of the parameter under which the URDF is loaded -->
+  <arg name="robot_description" default="robot_description"/>
+
+  <!-- HAL debug output -->
+  <!-- - Output HAL debug messages to screen (console) -->
+  <arg name="hal_debug_output" default="false"/>
+  <!-- - Set HAL debug level, 0-5 -->
+  <arg name="hal_debug_level" default="1"/>
+
+  <!-- - Run in simulated hardware mode -->
+  <arg name="sim" default="$(eval optenv('HARDWARE_MODE', 'sim') == 'sim')"/>
+
+  <!-- - Tool to use -->
+  <arg name="tool" default="none" />
+
+  <!-- Set search path for robot HAL config -->
+  <param
+    name="hal_config_path"
+    value="$(find borunte_hal)"
+    />
+
+  <!-- cpuset cgroup for RT threads -->
+  <arg unless="$(arg sim)" name="cgroup" default="/rt"/>
+  <arg     if="$(arg sim)" name="cgroup" default=""/>
+
+  <rosparam
+      command="load"
+      file="$(find borunte_support)/config/hal_hw_interface.yaml"
+      />
+
+  <rosparam
+      command="load"
+      file="$(find borunte_support)/config/hal_io.yaml"
+    />
+
+  <param
+      name="hal_mgr/hal_file_dir"
+      value="$(find borunte_support)/halfiles"
+      />
+  <param name="hal_hardware/sim_mode" value="$(arg sim)"/>
+  <param name="hal_hardware/tool" value="$(arg tool)"/>
+
+  <include file="$(find hal_hw_interface)/launch/hal_hw_interface.launch">
+    <arg name="hal_debug_output" value="$(arg hal_debug_output)"/>
+    <arg name="hal_debug_level" value="$(arg hal_debug_level)"/>
+  </include>
+
+  <rosparam
+      file="$(find borunte_support)/config/borunte_controllers.yaml"
+      command="load"
+    />
+
+  <node
+      name="ros_control_controller_manager"
+      pkg="controller_manager"
+      type="controller_manager"
+      respawn="false"
+      output="screen"
+      args="spawn joint_state_controller position_trajectory_controller"
+    />
+
+  <node
+    name="machinetalk"
+    pkg="borunte_support"
+    type="run_machinetalk.py"
+  />
+
+</launch>
diff --git a/borunte_support/launch/hw_bringup.launch b/borunte_support/launch/hw_bringup.launch
new file mode 100644
index 0000000..91cbabc
--- /dev/null
+++ b/borunte_support/launch/hw_bringup.launch
@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- HAL debug output -->
+  <!-- - Output HAL debug messages to screen (console) -->
+  <arg name="hal_debug_output" default="false"/>
+  <!-- - Set HAL debug level, 0-5 -->
+  <arg name="hal_debug_level" default="1"/>
+  <!-- start in sim mode -->
+  <arg name="sim" default="$(eval optenv('HARDWARE_MODE', 'sim') == 'sim')"/>
+  <!-- ID of the tool to use -->
+  <arg name="tool" default="ezgripper_gen2_plus"/>
+  <!-- The name of the parameter under which the URDF is loaded -->
+  <arg name="robot_description" default="robot_description"/>
+  <!-- The camera nodelet manager to start -->
+  <arg name="manager" default="camera_nodelet_manager"/>
+
+  <!-- set sim param -->
+  <param name="sim" value="$(arg sim)" />
+
+  <!-- Load universal robot description format (URDF) -->
+  <param name="$(arg robot_description)" command="$(find xacro)/xacro --inorder '$(find borunte_support)/urdf/borunte_on_stand.xacro' tool:=$(arg tool)"/>
+
+  <!-- load I/O config -->
+  <rosparam
+      command="load"
+      file="$(find borunte_support)/config/io_config.yaml"
+      />
+
+  <!-- run the "real robot" interface nodes -->
+  <include file="$(find borunte_support)/launch/hal_hardware.launch">
+    <arg name="hal_debug_output" value="$(arg hal_debug_output)"/>
+    <arg name="hal_debug_level" value="$(arg hal_debug_level)"/>
+    <arg name="sim" value="$(arg sim)"/>
+    <arg name="tool" value="$(arg tool)" />
+  </include>
+
+  <!-- Start tool/gripper nodes -->
+  <node pkg="ezgripper_driver" type="ezgripper.py" name="ezgripper" output="screen" if="$(eval tool == 'ezgripper_gen2_plus' and not sim)">
+    <param name="port" value="hwgrep://0403:6015"/>
+    <param name="baud" value="57600"/>
+    <rosparam param="grippers">{main:[1]}</rosparam>
+  </node>
+  <node pkg="ezgripper_driver" type="ezgripper_sim.py" name="ezgripper" output="screen" if="$(eval tool == 'ezgripper_gen2_plus' and sim)">
+    <rosparam param="grippers">{main:[1]}</rosparam>
+  </node>
+
+  <!-- publish the robot state (tf transforms) -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
+
+  <!-- launch vision chain -->
+  <include file="$(find tag_tracking)/launch/combine_rgbd.launch">
+    <arg name="manager" value="$(arg manager)"/>
+  </include>
+
+  <include file="$(find tag_tracking)/launch/elp_camera_13mp_cv_stream.launch" unless="$(arg sim)">
+    <arg name="manager" value="$(arg manager)"/>
+    <arg name="use_manager" value="true"/>
+  </include>
+
+  <include file="$(find tag_tracking)/launch/pico_flexx_camera.launch" unless="$(arg sim)">
+    <arg name="manager" value="$(arg manager)"/>
+    <arg name="use_manager" value="true"/>
+  </include>
+</launch>
diff --git a/borunte_support/launch/jogging.launch b/borunte_support/launch/jogging.launch
new file mode 100644
index 0000000..8088b76
--- /dev/null
+++ b/borunte_support/launch/jogging.launch
@@ -0,0 +1,21 @@
+<launch>
+
+  <node name="joy_node" pkg="joy" type="joy_node" required="true"/>
+
+  <node name="spacemouse_to_twist" pkg="jog_arm" type="spacemouse_to_twist" output="screen" required="true">
+    <param name="output_topic" type="string" value="jog_arm_server/frame_delta_jog_cmds_spacemouse" />
+  </node>
+
+  <node name="jog_arm_server_spacemouse" pkg="jog_arm" type="jog_arm_server" output="screen" required="true">
+    <param name="parameter_ns" type="string" value="jog_arm_server_spacemouse" />
+    <rosparam command="load" file="$(find borunte_support)/config/jog_arm_settings_spacemouse.yaml" />
+  </node>
+
+  <node name="jog_arm_server_gui" pkg="jog_arm" type="jog_arm_server" output="screen" required="true">
+    <param name="parameter_ns" type="string" value="jog_arm_server_gui" />
+    <rosparam command="load" file="$(find borunte_support)/config/jog_arm_settings_gui.yaml" />
+  </node>
+
+  <node pkg="robot_jog" type="jog_node" name="jog_node" required="true"/>
+
+</launch>
diff --git a/borunte_support/launch/parameter_persistence.launch b/borunte_support/launch/parameter_persistence.launch
new file mode 100644
index 0000000..61f9cfd
--- /dev/null
+++ b/borunte_support/launch/parameter_persistence.launch
@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<launch>
+
+  <include file="$(find redis_store)/launch/redis_store.launch">
+    <arg name="defaults_path" value="$(find borunte_support)/config/defaults"/>
+  </include>
+
+</launch>
diff --git a/borunte_support/launch/tracking_tools.launch b/borunte_support/launch/tracking_tools.launch
new file mode 100644
index 0000000..17e8f5c
--- /dev/null
+++ b/borunte_support/launch/tracking_tools.launch
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="manager" default="camera_nodelet_manager"/>
+
+  <include file="$(find tag_tracking)/launch/ar_track_alvar.launch">
+    <arg name="output_frame" value="world"/>
+    <arg name="use_cloud" value="false"/>
+  </include>
+
+  <include file="$(find barcode_tracking)/launch/barcode_detector.launch">
+    <arg name="output_frame" value="world" />
+    <arg name="manager" value="$(arg manager)"/>
+    <arg name="use_manager" value="true"/>
+  </include>
+</launch>
diff --git a/borunte_support/meshes/base_link.stl b/borunte_support/meshes/base_link.stl
new file mode 100644
index 0000000..8eefc8b
Binary files /dev/null and b/borunte_support/meshes/base_link.stl differ
diff --git a/borunte_support/meshes/base_link_collision.stl b/borunte_support/meshes/base_link_collision.stl
new file mode 100644
index 0000000..bc53ba1
Binary files /dev/null and b/borunte_support/meshes/base_link_collision.stl differ
diff --git a/borunte_support/meshes/borunte_carriage.stl b/borunte_support/meshes/borunte_carriage.stl
new file mode 100644
index 0000000..8471871
Binary files /dev/null and b/borunte_support/meshes/borunte_carriage.stl differ
diff --git a/borunte_support/meshes/borunte_ezgripper_adapter.stl b/borunte_support/meshes/borunte_ezgripper_adapter.stl
new file mode 100644
index 0000000..5714cf2
Binary files /dev/null and b/borunte_support/meshes/borunte_ezgripper_adapter.stl differ
diff --git a/borunte_support/meshes/borunte_ezgripper_adapter_collision.stl b/borunte_support/meshes/borunte_ezgripper_adapter_collision.stl
new file mode 100644
index 0000000..5fa7e14
Binary files /dev/null and b/borunte_support/meshes/borunte_ezgripper_adapter_collision.stl differ
diff --git a/borunte_support/meshes/collision/create_collision_invert.mlx b/borunte_support/meshes/collision/create_collision_invert.mlx
new file mode 100644
index 0000000..f473dc4
--- /dev/null
+++ b/borunte_support/meshes/collision/create_collision_invert.mlx
@@ -0,0 +1,37 @@
+<!DOCTYPE FilterScript>
+<FilterScript>
+ <filter name="Uniform Mesh Resampling">
+  <Param type="RichAbsPerc" value="0.009234" min="0" name="CellSize" max="0.461702"/>
+  <Param type="RichAbsPerc" value="0.0049993" min="-0.0923404" name="Offset" max="0.0923404"/>
+  <Param type="RichBool" value="false" name="mergeCloseVert"/>
+  <Param type="RichBool" value="false" name="discretize"/>
+  <Param type="RichBool" value="false" name="multisample"/>
+  <Param type="RichBool" value="true" name="absDist"/>
+ </filter>
+ <filter name="Convex Hull">
+  <Param type="RichBool" value="false" name="reorient"/>
+ </filter>
+ <filter name="Quadric Edge Collapse Decimation">
+  <Param type="RichInt" value="377" name="TargetFaceNum"/>
+  <Param type="RichFloat" value="0.3" name="TargetPerc"/>
+  <Param type="RichFloat" value="0.3" name="QualityThr"/>
+  <Param type="RichBool" value="false" name="PreserveBoundary"/>
+  <Param type="RichFloat" value="1" name="BoundaryWeight"/>
+  <Param type="RichBool" value="false" name="PreserveNormal"/>
+  <Param type="RichBool" value="false" name="PreserveTopology"/>
+  <Param type="RichBool" value="true" name="OptimalPlacement"/>
+  <Param type="RichBool" value="false" name="PlanarQuadric"/>
+  <Param type="RichBool" value="false" name="QualityWeight"/>
+  <Param type="RichBool" value="true" name="AutoClean"/>
+  <Param type="RichBool" value="false" name="Selected"/>
+ </filter>
+ <filter name="Re-Orient all faces coherentely"/>
+ <filter name="Invert Faces Orientation">
+  <Param type="RichBool" value="true" name="forceFlip"/>
+ </filter>
+ <filter name="Recompute Vertex Normals">
+  <Param enum_val0="None (avg)" enum_val1="By Angle" enum_cardinality="4" enum_val2="By Area" enum_val3="As defined by N. Max" type="RichEnum" value="0" name="weightMode"/>
+ </filter>
+ <filter name="Normalize Vertex Normals"/>
+ <filter name="Normalize Face Normals"/>
+</FilterScript>
diff --git a/borunte_support/meshes/collision/create_collision_no_invert.mlx b/borunte_support/meshes/collision/create_collision_no_invert.mlx
new file mode 100644
index 0000000..6a09984
--- /dev/null
+++ b/borunte_support/meshes/collision/create_collision_no_invert.mlx
@@ -0,0 +1,34 @@
+<!DOCTYPE FilterScript>
+<FilterScript>
+ <filter name="Uniform Mesh Resampling">
+  <Param type="RichAbsPerc" value="0.009234" min="0" name="CellSize" max="0.461702"/>
+  <Param type="RichAbsPerc" value="0.0049993" min="-0.0923404" name="Offset" max="0.0923404"/>
+  <Param type="RichBool" value="false" name="mergeCloseVert"/>
+  <Param type="RichBool" value="false" name="discretize"/>
+  <Param type="RichBool" value="false" name="multisample"/>
+  <Param type="RichBool" value="true" name="absDist"/>
+ </filter>
+ <filter name="Convex Hull">
+  <Param type="RichBool" value="false" name="reorient"/>
+ </filter>
+ <filter name="Quadric Edge Collapse Decimation">
+  <Param type="RichInt" value="377" name="TargetFaceNum"/>
+  <Param type="RichFloat" value="0.3" name="TargetPerc"/>
+  <Param type="RichFloat" value="0.3" name="QualityThr"/>
+  <Param type="RichBool" value="false" name="PreserveBoundary"/>
+  <Param type="RichFloat" value="1" name="BoundaryWeight"/>
+  <Param type="RichBool" value="false" name="PreserveNormal"/>
+  <Param type="RichBool" value="false" name="PreserveTopology"/>
+  <Param type="RichBool" value="true" name="OptimalPlacement"/>
+  <Param type="RichBool" value="false" name="PlanarQuadric"/>
+  <Param type="RichBool" value="false" name="QualityWeight"/>
+  <Param type="RichBool" value="true" name="AutoClean"/>
+  <Param type="RichBool" value="false" name="Selected"/>
+ </filter>
+ <filter name="Re-Orient all faces coherentely"/>
+ <filter name="Recompute Vertex Normals">
+  <Param enum_val0="None (avg)" enum_val1="By Angle" enum_cardinality="4" enum_val2="By Area" enum_val3="As defined by N. Max" type="RichEnum" value="0" name="weightMode"/>
+ </filter>
+ <filter name="Normalize Vertex Normals"/>
+ <filter name="Normalize Face Normals"/>
+</FilterScript>
diff --git a/borunte_support/meshes/fish_finger.stl b/borunte_support/meshes/fish_finger.stl
new file mode 100644
index 0000000..062641f
Binary files /dev/null and b/borunte_support/meshes/fish_finger.stl differ
diff --git a/borunte_support/meshes/fish_finger_adapter_left.stl b/borunte_support/meshes/fish_finger_adapter_left.stl
new file mode 100644
index 0000000..84ac093
Binary files /dev/null and b/borunte_support/meshes/fish_finger_adapter_left.stl differ
diff --git a/borunte_support/meshes/fish_finger_adapter_left_collision.stl b/borunte_support/meshes/fish_finger_adapter_left_collision.stl
new file mode 100644
index 0000000..8a68df1
Binary files /dev/null and b/borunte_support/meshes/fish_finger_adapter_left_collision.stl differ
diff --git a/borunte_support/meshes/fish_finger_adapter_right.stl b/borunte_support/meshes/fish_finger_adapter_right.stl
new file mode 100644
index 0000000..16932ed
Binary files /dev/null and b/borunte_support/meshes/fish_finger_adapter_right.stl differ
diff --git a/borunte_support/meshes/fish_finger_adapter_right_collision.stl b/borunte_support/meshes/fish_finger_adapter_right_collision.stl
new file mode 100644
index 0000000..07d43dd
Binary files /dev/null and b/borunte_support/meshes/fish_finger_adapter_right_collision.stl differ
diff --git a/borunte_support/meshes/fish_finger_collision.stl b/borunte_support/meshes/fish_finger_collision.stl
new file mode 100644
index 0000000..d93df7f
Binary files /dev/null and b/borunte_support/meshes/fish_finger_collision.stl differ
diff --git a/borunte_support/meshes/link_1.stl b/borunte_support/meshes/link_1.stl
new file mode 100644
index 0000000..2fd4754
Binary files /dev/null and b/borunte_support/meshes/link_1.stl differ
diff --git a/borunte_support/meshes/link_1_collision.stl b/borunte_support/meshes/link_1_collision.stl
new file mode 100644
index 0000000..234e715
Binary files /dev/null and b/borunte_support/meshes/link_1_collision.stl differ
diff --git a/borunte_support/meshes/link_2.stl b/borunte_support/meshes/link_2.stl
new file mode 100644
index 0000000..cccabb4
Binary files /dev/null and b/borunte_support/meshes/link_2.stl differ
diff --git a/borunte_support/meshes/link_2_collision.stl b/borunte_support/meshes/link_2_collision.stl
new file mode 100644
index 0000000..de57635
Binary files /dev/null and b/borunte_support/meshes/link_2_collision.stl differ
diff --git a/borunte_support/meshes/link_3.stl b/borunte_support/meshes/link_3.stl
new file mode 100644
index 0000000..1c273ea
Binary files /dev/null and b/borunte_support/meshes/link_3.stl differ
diff --git a/borunte_support/meshes/link_3_collision.stl b/borunte_support/meshes/link_3_collision.stl
new file mode 100644
index 0000000..929cb81
Binary files /dev/null and b/borunte_support/meshes/link_3_collision.stl differ
diff --git a/borunte_support/meshes/link_4.stl b/borunte_support/meshes/link_4.stl
new file mode 100644
index 0000000..091800c
Binary files /dev/null and b/borunte_support/meshes/link_4.stl differ
diff --git a/borunte_support/meshes/link_4_collision.stl b/borunte_support/meshes/link_4_collision.stl
new file mode 100644
index 0000000..d77e912
Binary files /dev/null and b/borunte_support/meshes/link_4_collision.stl differ
diff --git a/borunte_support/meshes/link_5.stl b/borunte_support/meshes/link_5.stl
new file mode 100644
index 0000000..f9ab009
Binary files /dev/null and b/borunte_support/meshes/link_5.stl differ
diff --git a/borunte_support/meshes/link_5_collision.stl b/borunte_support/meshes/link_5_collision.stl
new file mode 100644
index 0000000..1a1bb1c
Binary files /dev/null and b/borunte_support/meshes/link_5_collision.stl differ
diff --git a/borunte_support/meshes/link_6.stl b/borunte_support/meshes/link_6.stl
new file mode 100644
index 0000000..5ab43e4
Binary files /dev/null and b/borunte_support/meshes/link_6.stl differ
diff --git a/borunte_support/meshes/link_6_collision.stl b/borunte_support/meshes/link_6_collision.stl
new file mode 100644
index 0000000..5ab43e4
Binary files /dev/null and b/borunte_support/meshes/link_6_collision.stl differ
diff --git a/borunte_support/meshes/robotiq_adapter.stl b/borunte_support/meshes/robotiq_adapter.stl
new file mode 100644
index 0000000..21bc9d5
Binary files /dev/null and b/borunte_support/meshes/robotiq_adapter.stl differ
diff --git a/borunte_support/meshes/robotiq_adapter_collision.stl b/borunte_support/meshes/robotiq_adapter_collision.stl
new file mode 100644
index 0000000..dc3611e
Binary files /dev/null and b/borunte_support/meshes/robotiq_adapter_collision.stl differ
diff --git a/borunte_support/meshes/robotiq_hand-e_gripper.stl b/borunte_support/meshes/robotiq_hand-e_gripper.stl
new file mode 100644
index 0000000..e593826
Binary files /dev/null and b/borunte_support/meshes/robotiq_hand-e_gripper.stl differ
diff --git a/borunte_support/meshes/robotiq_hand-e_gripper_collision.stl b/borunte_support/meshes/robotiq_hand-e_gripper_collision.stl
new file mode 100644
index 0000000..5f4860b
Binary files /dev/null and b/borunte_support/meshes/robotiq_hand-e_gripper_collision.stl differ
diff --git a/borunte_support/meshes/simple_grip_finger.stl b/borunte_support/meshes/simple_grip_finger.stl
new file mode 100644
index 0000000..36b28b0
Binary files /dev/null and b/borunte_support/meshes/simple_grip_finger.stl differ
diff --git a/borunte_support/meshes/simple_grip_finger_collision.stl b/borunte_support/meshes/simple_grip_finger_collision.stl
new file mode 100644
index 0000000..a044a5c
Binary files /dev/null and b/borunte_support/meshes/simple_grip_finger_collision.stl differ
diff --git a/borunte_support/package.xml b/borunte_support/package.xml
new file mode 100644
index 0000000..a2692d5
--- /dev/null
+++ b/borunte_support/package.xml
@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<package>
+  <name>borunte_support</name>
+  <version>1.0.0</version>
+  <description>Borunte robot support package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="alex@machinekoder.com">Alexander Rössler</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>BSD</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/borunte_support</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <author email="alex@machinekoder.com">Alexander Rössler</author>
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>xacro</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>hal_hw_interface</run_depend>
+  <run_depend>borunte_hal</run_depend>
+  <run_depend>ezgripper_driver</run_depend>
+  <run_depend>joint_state_publisher</run_depend>
+  <run_depend>robot_state_publisher</run_depend>
+  <run_depend>rviz</run_depend>
+  <run_depend>controller_manager</run_depend>
+  <run_depend>joy</run_depend>
+  <run_depend>jog_arm</run_depend>
+  <run_depend>robot_jog</run_depend>
+  <run_depend>redis_store</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/borunte_support/scripts/run_machinetalk.py b/borunte_support/scripts/run_machinetalk.py
new file mode 100755
index 0000000..6b12d8e
--- /dev/null
+++ b/borunte_support/scripts/run_machinetalk.py
@@ -0,0 +1,22 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+import subprocess
+import sys
+import time
+
+from machinekit import launcher
+import rospy
+
+try:
+    launcher.register_exit_handler()  # enable on ctrl-C, needs to executed after HAL files
+    path = rospy.get_param('/hal_config_path', '')
+    launcher.start_process('mklauncher')
+    launcher.start_process('configserver -n Borunte-Control {}'.format(path))
+
+    while True:
+        launcher.check_processes()
+        time.sleep(1)
+
+except subprocess.CalledProcessError:
+    launcher.end_session()
+    sys.exit(1)
diff --git a/borunte_support/scripts/run_sim_ui.py b/borunte_support/scripts/run_sim_ui.py
new file mode 100755
index 0000000..016ebe9
--- /dev/null
+++ b/borunte_support/scripts/run_sim_ui.py
@@ -0,0 +1,27 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+import subprocess
+import sys
+import time
+
+from machinekit import launcher
+import rospkg
+
+PACKAGE_NAME = 'borunte_support'
+
+rospack = rospkg.RosPack()
+
+try:
+    launcher.register_exit_handler()  # enable on ctrl-C, needs to executed after HAL files
+    launcher.ensure_mklauncher()  # ensure mklauncher is started
+    launcher.start_process(
+        'configserver -n HAL-IO {}'.format(rospack.get_path(PACKAGE_NAME))
+    )
+
+    while True:
+        launcher.check_processes()
+        time.sleep(1)
+
+except subprocess.CalledProcessError:
+    launcher.end_session()
+    sys.exit(1)
diff --git a/borunte_support/src/ros.HAL-IO/HAL-IO.qml b/borunte_support/src/ros.HAL-IO/HAL-IO.qml
new file mode 100644
index 0000000..99d36aa
--- /dev/null
+++ b/borunte_support/src/ros.HAL-IO/HAL-IO.qml
@@ -0,0 +1,87 @@
+import QtQuick 2.0
+import QtQuick.Controls 1.1
+import QtQuick.Layouts 1.1
+import Machinekit.Controls 1.0
+import Machinekit.HalRemote.Controls 1.0
+import Machinekit.HalRemote 1.0
+
+HalApplicationWindow {
+  id: main
+
+  name: "io-rcomp"
+  title: qsTr("HAL-IO")
+
+  ColumnLayout {
+    anchors.fill: parent
+    anchors.margins: 10
+
+    Item {
+      Layout.fillHeight: true
+    }
+    HalButton {
+      Layout.alignment: Layout.Center
+      name: "digital_in_1"
+      text: "Digital In 1"
+      checkable: true
+    }
+    HalButton {
+      Layout.alignment: Layout.Center
+      name: "digital_in_2"
+      text: "Digital In 2"
+      checkable: true
+    }
+    HalButton {
+      Layout.alignment: Layout.Center
+      name: "digital_in_3"
+      text: "Digital In 3"
+      checkable: true
+    }
+    HalButton {
+      Layout.alignment: Layout.Center
+      name: "digital_in_4"
+      text: "Digital In 4"
+      checkable: true
+    }
+
+    RowLayout {
+      Layout.alignment: Layout.Center
+      Label {
+        text: "Digital Out 1: "
+      }
+      HalLed {
+        name: "digital_out_1"
+      }
+    }
+    RowLayout {
+      Layout.alignment: Layout.Center
+      Label {
+        text: "Digital Out 2: "
+      }
+      HalLed {
+        name: "digital_out_2"
+      }
+    }
+    RowLayout {
+      Layout.alignment: Layout.Center
+      Label {
+        text: "Digital Out 3: "
+      }
+      HalLed {
+        name: "digital_out_3"
+      }
+    }
+    RowLayout {
+      Layout.alignment: Layout.Center
+      Label {
+        text: "Digital Out 4: "
+      }
+      HalLed {
+        name: "digital_out_4"
+      }
+    }
+
+    Item {
+      Layout.fillHeight: true
+    }
+  }
+}
diff --git a/borunte_support/src/ros.HAL-IO/description.ini b/borunte_support/src/ros.HAL-IO/description.ini
new file mode 100644
index 0000000..1671ee3
--- /dev/null
+++ b/borunte_support/src/ros.HAL-IO/description.ini
@@ -0,0 +1,4 @@
+[Default]
+name=HAL-IO
+description=HAL IO Interface
+type=QT5_QML
diff --git a/borunte_support/urdf/borunte.xacro b/borunte_support/urdf/borunte.xacro
new file mode 100644
index 0000000..420b901
--- /dev/null
+++ b/borunte_support/urdf/borunte.xacro
@@ -0,0 +1,14 @@
+<?xml version="1.0" ?>
+<robot name="borunte" xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:include filename="$(find borunte_support)/urdf/borunte_none_macro.xacro"/>
+    <xacro:borunte_none_macro prefix=""/>
+
+    <!-- 'world' frame: default pose reference frame -->
+    <link name="world" />
+    <joint name="world-base_link" type="fixed">
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <axis xyz="0 0 1"/>
+      <child link="world"/>
+      <parent link="base_link"/>
+    </joint>
+</robot>
diff --git a/borunte_support/urdf/borunte_ezgripper_gen2_plus_macro.xacro b/borunte_support/urdf/borunte_ezgripper_gen2_plus_macro.xacro
new file mode 100644
index 0000000..26008ee
--- /dev/null
+++ b/borunte_support/urdf/borunte_ezgripper_gen2_plus_macro.xacro
@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="ezgripper_gen2_plus_macro" params="prefix">
+    <xacro:include filename="$(find borunte_support)/urdf/borunte_macro.xacro"/>
+    <xacro:borunte_macro prefix="${prefix}"/>
+
+    <xacro:include filename="$(find ezgripper_driver)/urdf/ezgripper_dual_gen2_plus_articulated.urdf.xacro"/>
+    <!-- Add the gripper -->
+    <ezgripper_dual prefix="main" parent_link="${prefix}borunte_ezgripper_adapter">
+      <origin xyz="0 0 0.012" rpy="${pi} ${-pi/2.0} 0"/>
+    </ezgripper_dual>
+
+    <material name="${prefix}Black_Plastic">
+      <color rgba="0.1 0.1 0.1 0.99"/>
+    </material>
+
+    <link name="${prefix}borunte_ezgripper_adapter">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/borunte_ezgripper_adapter.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <material name="${prefix}Black_Plastic"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/borunte_ezgripper_adapter_collision.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+      </collision>
+    </link>
+
+    <!-- Camera frame: location of wrist cameras -->
+    <link name="${prefix}rgb_camera_link" />
+    <joint name="${prefix}borunte_ezgripper_adapter-rgb_camera_link" type="fixed">
+      <origin xyz="0.042 0 0.027" rpy="0 0 ${pi/2.0}" />
+      <parent link="${prefix}borunte_ezgripper_adapter" />
+      <child link="${prefix}rgb_camera_link" />
+    </joint>
+
+    <link name="${prefix}depth_camera_link" />
+    <joint name="${prefix}borunte_ezgripper_adapter-depth_camera_link" type="fixed">
+      <origin xyz="0.074 0 0.0255" rpy="0 0 ${pi/2.0}" />
+      <parent link="${prefix}borunte_ezgripper_adapter" />
+      <child link="${prefix}depth_camera_link" />
+    </joint>
+
+    <!-- attach hand_e to eef -->
+    <!-- tool0 flange is set to correct dimensions in the robotiq_hand_e_macro.xacro -->
+    <!-- this assures that planning is done from the perspective of the fingers -->
+    <joint name="${prefix}flange-borunte_ezgripper_adapter" type="fixed">
+      <origin xyz="0 0 0" rpy="0 0 ${pi}" />
+      <parent link="${prefix}flange" />
+      <child link="${prefix}borunte_ezgripper_adapter" />
+    </joint>
+
+    <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+    <link name="${prefix}tool0" />
+    <joint name="${prefix}flange-tool0" type="fixed">
+      <!-- marker -->
+      <origin xyz="0 0 0.195" rpy="0 ${pi*1} ${-pi/2.0}" />
+      <parent link="${prefix}flange" />
+      <child link="${prefix}tool0" />
+    </joint>
+
+  </xacro:macro>
+</robot>
diff --git a/borunte_support/urdf/borunte_hand_e.xacro b/borunte_support/urdf/borunte_hand_e.xacro
new file mode 100644
index 0000000..7277a6d
--- /dev/null
+++ b/borunte_support/urdf/borunte_hand_e.xacro
@@ -0,0 +1,14 @@
+<?xml version="1.0" ?>
+<robot name="borunte" xmlns:xacro="http://ros.org/wiki/xacro">
+    <xacro:include filename="$(find borunte_support)/urdf/borunte_hand_e_macro.xacro"/>
+    <xacro:borunte_hand_e_macro prefix=""/>
+
+        <!-- 'world' frame: default pose reference frame -->
+    <link name="world" />
+    <joint name="world-base_link" type="fixed">
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <axis xyz="0 0 1"/>
+      <child link="world"/>
+      <parent link="base_link"/>
+    </joint>
+</robot>
diff --git a/borunte_support/urdf/borunte_hand_e_fish_fingers_macro.xacro b/borunte_support/urdf/borunte_hand_e_fish_fingers_macro.xacro
new file mode 100644
index 0000000..7fe1b12
--- /dev/null
+++ b/borunte_support/urdf/borunte_hand_e_fish_fingers_macro.xacro
@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="borunte_hand_e_macro" params="prefix">
+
+    <xacro:include filename="$(find borunte_support)/urdf/borunte_macro.xacro"/>
+    <xacro:borunte_macro prefix="${prefix}"/>
+
+    <xacro:include filename="$(find borunte_support)/urdf/robotiq_hand_e_fish_fingers_macro.xacro"/>
+    <xacro:robotiq_hand_e_macro prefix=""/>
+
+    <!-- attach hand_e to eef -->
+    <!-- tool0 flange is set to correct dimensions in the robotiq_hand_e_macro.xacro -->
+    <!-- this assures that planning is done from the perspective of the fingers -->
+    <joint name="${prefix}flange-borunte_robotiq_adapter" type="fixed">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <parent link="${prefix}flange" />
+      <child link="${prefix}borunte_robotiq_adapter" />
+    </joint>
+
+      <!-- Camera frame: location of wrist camera -->
+    <link name="${prefix}rgb_camera_link" />
+    <joint name="${prefix}borunte_robotiq_adapter-rgb_camera_link" type="fixed">
+      <origin xyz="0.06 0 0.03" rpy="0 0 ${pi/2.0}" />
+      <parent link="${prefix}borunte_robotiq_adapter" />
+      <child link="${prefix}rgb_camera_link" />
+    </joint>
+
+    <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+    <link name="${prefix}tool0" />
+    <joint name="${prefix}flange-tool0" type="fixed">
+      <!-- marker -->
+      <origin xyz="0.0 0.0 0.0" rpy="0 ${pi*1} 0" />
+      <parent link="${prefix}flange" />
+      <child link="${prefix}tool0" />
+    </joint>
+
+    <link name="${prefix}eef_tcp_simple_grip" />
+    <joint name="${prefix}flange-eef_tcp_simple_grip" type="fixed">
+      <!-- marker -->
+      <origin xyz="0.0468199 0.0 0.158734" rpy="0 ${pi*5/4} 0" />
+      <parent link="${prefix}flange" />
+      <child link="${prefix}eef_tcp_simple_grip" />
+    </joint>
+
+    <link name="${prefix}eef_tcp_fish_grip" />
+    <joint name="${prefix}flange-eef_tcp_fish_grip" type="fixed">
+      <!-- marker -->
+      <origin xyz="-0.119652 0.0 0.230152" rpy="0 ${pi*3/4} 0" />
+      <parent link="${prefix}flange" />
+      <child link="${prefix}eef_tcp_fish_grip" />
+    </joint>
+
+  </xacro:macro>
+</robot>
diff --git a/borunte_support/urdf/borunte_hand_e_macro.xacro b/borunte_support/urdf/borunte_hand_e_macro.xacro
new file mode 100644
index 0000000..a00a8c4
--- /dev/null
+++ b/borunte_support/urdf/borunte_hand_e_macro.xacro
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="borunte_hand_e_macro" params="prefix">
+
+    <xacro:include filename="$(find borunte_support)/urdf/borunte_macro.xacro"/>
+    <xacro:borunte_macro prefix="${prefix}"/>
+
+    <xacro:include filename="$(find borunte_support)/urdf/robotiq_hand_e_macro.xacro"/>
+    <xacro:robotiq_hand_e_macro prefix=""/>
+
+    <!-- attach hand_e to eef -->
+    <!-- tool0 flange is set to correct dimensions in the robotiq_hand_e_macro.xacro -->
+    <!-- this assures that planning is done from the perspective of the fingers -->
+    <joint name="${prefix}flange-borunte_robotiq_adapter" type="fixed">
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <parent link="${prefix}flange" />
+      <child link="${prefix}borunte_robotiq_adapter" />
+    </joint>
+
+    <!-- Camera frame: location of wrist camera -->
+    <link name="${prefix}rgb_camera_link" />
+    <joint name="${prefix}borunte_robotiq_adapter-rgb_camera_link" type="fixed">
+      <origin xyz="0.06 0 0.03" rpy="0 0 ${pi/2.0}" />
+      <parent link="${prefix}borunte_robotiq_adapter" />
+      <child link="${prefix}rgb_camera_link" />
+    </joint>
+
+    <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+    <link name="${prefix}tool0" />
+    <joint name="${prefix}flange-tool0" type="fixed">
+      <!-- marker -->
+      <origin xyz="0 0 0.1605" rpy="0 ${pi*1} ${-pi/2.0}" />
+      <parent link="${prefix}flange" />
+      <child link="${prefix}tool0" />
+    </joint>
+
+  </xacro:macro>
+</robot>
diff --git a/borunte_support/urdf/borunte_macro.xacro b/borunte_support/urdf/borunte_macro.xacro
new file mode 100644
index 0000000..0d4180c
--- /dev/null
+++ b/borunte_support/urdf/borunte_macro.xacro
@@ -0,0 +1,218 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="borunte_macro" params="prefix">
+    <material name="${prefix}Black">
+      <color rgba="0.1 0.1 0.1 0.99"/>
+    </material>
+    <material name="${prefix}DarkRed">
+      <color rgba="0.5 0.0 0.0 0.99"/>
+    </material>
+
+    <!-- link list -->
+    <link name="${prefix}base_link">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/base_link.stl"/>
+        </geometry>
+        <origin rpy="0.0 0 0.0" xyz="0 0 0"/>
+        <material name="${prefix}Black"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/base_link_collision.stl"/>
+        </geometry>
+        <origin rpy="0.0 0 0.0" xyz="0 0 0"/>
+      </collision>
+    </link>
+
+    <link name="${prefix}link_1">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/link_1.stl"/>
+        </geometry>
+        <origin rpy="0.0 0 0.0" xyz="0 0 0"/>
+        <material name="${prefix}DarkRed"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/link_1_collision.stl"/>
+        </geometry>
+        <origin rpy="0.0 0 0.0" xyz="0 0 0"/>
+      </collision>
+      <inertial>
+        <mass value="100"/>
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+      </inertial>
+    </link>
+
+    <link name="${prefix}link_2">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/link_2.stl"/>
+        </geometry>
+        <origin rpy="0.0 0 0.0" xyz="0 0 0"/>
+        <material name="${prefix}DarkRed"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/link_2_collision.stl"/>
+        </geometry>
+        <origin rpy="0.0 0 0.0" xyz="0 0 0"/>
+      </collision>
+      <inertial>
+        <mass value="100"/>
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+      </inertial>
+    </link>
+
+    <link name="${prefix}link_3">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/link_3.stl"/>
+        </geometry>
+        <origin rpy="0.0 0 0.0" xyz="0 0.01 0"/>
+        <material name="${prefix}DarkRed"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/link_3_collision.stl"/>
+        </geometry>
+        <origin rpy="0.0 0 0.0" xyz="0 0.01 0"/>
+      </collision>
+      <inertial>
+        <mass value="100"/>
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+      </inertial>
+    </link>
+
+    <link name="${prefix}link_4">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/link_4.stl"/>
+        </geometry>
+        <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
+        <material name="${prefix}DarkRed"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/link_4_collision.stl"/>
+        </geometry>
+        <origin rpy="0.0 0 0.0" xyz="0 0 0"/>
+      </collision>
+      <inertial>
+        <mass value="100"/>
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+      </inertial>
+    </link>
+
+    <link name="${prefix}link_5">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/link_5.stl"/>
+        </geometry>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <material name="${prefix}DarkRed"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/link_5_collision.stl"/>
+        </geometry>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+      </collision>
+      <inertial>
+        <mass value="100"/>
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+      </inertial>
+    </link>
+
+    <link name="${prefix}link_6">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/link_6.stl"/>
+        </geometry>
+        <origin rpy="0 0 0.0" xyz="0 0 0"/>
+        <material name="${prefix}DarkRed"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/link_6_collision.stl"/>
+        </geometry>
+        <origin rpy="0 0 0.0" xyz="0 0 0"/>
+      </collision>
+      <inertial>
+        <mass value="100"/>
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+      </inertial>
+    </link>
+
+    <!-- joint list -->
+    <joint name="${prefix}joint_1" type="revolute">
+      <limit effort="1000.0" lower="-3.076" upper="3.076" velocity="3.14"/>
+      <parent link="base_link"/>
+      <child link="link_1"/>
+      <axis xyz="0 0 1"/>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0.24"/>
+    </joint>
+
+    <joint name="${prefix}joint_2" type="revolute">
+      <!-- actual limit -1.74, reduced to reduce config problems -->
+      <limit effort="1000.0" lower="-1.0" upper="2.35" velocity="1.0"/>
+      <parent link="link_1"/>
+      <child link="link_2"/>
+      <axis xyz="0 1 0"/>
+      <origin rpy="0.0 0.0 0.0" xyz="0.07 0.09 0.165"/>
+    </joint>
+
+    <joint name="${prefix}joint_3" type="revolute">
+      <limit effort="1000.0" lower="-2.094" upper="1.570" velocity="1.3"/>
+      <parent link="link_2"/>
+      <child link="link_3"/>
+      <axis xyz="0 1 0"/>
+      <origin rpy="0.0 ${pi/2.0} 0.0" xyz="0.0 0.0 0.39"/>
+    </joint>
+
+    <joint name="${prefix}joint_4" type="revolute">
+      <!-- actual limit +-2.094, reduced to reduce config problems -->
+      <limit effort="1000.0" lower="${-pi/2.0}" upper="${pi/2.0}" velocity="2.0"/>
+      <parent link="link_3"/>
+      <child link="link_4"/>
+      <axis xyz="0 0 1"/>
+      <origin rpy="0.0 0.0 0.0" xyz="-0.125 -0.09 0.075"/>
+    </joint>
+
+    <joint name="${prefix}joint_5" type="revolute">
+      <limit effort="1000.0" lower="-1.919" upper="2.094" velocity="2.35"/>
+      <parent link="link_4"/>
+      <child link="link_5"/>
+      <axis xyz="0 1 0"/>
+      <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.0 0.31"/>
+    </joint>
+
+    <joint name="${prefix}joint_6" type="revolute">
+      <limit effort="1000.0" lower="${-pi}" upper="${pi}" velocity="2.35"/>
+      <parent link="link_5"/>
+      <child link="link_6"/>
+      <axis xyz="0 0 1"/>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0.104"/>
+    </joint>
+
+
+    <!-- ROS-Industrial 'base' frame: base_link to Fanuc World Coordinates transform -->
+    <link name="${prefix}base" />
+    <joint name="${prefix}base_link-base" type="fixed">
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <axis xyz="0 0 1"/>
+      <parent link="${prefix}base_link"/>
+      <child link="${prefix}base"/>
+    </joint>
+
+    <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
+    <link name="${prefix}flange"/>
+    <joint name="${prefix}joint_6-flange" type="fixed">
+      <origin xyz="0 0 0.015" rpy="0 0 0" />
+      <parent link="${prefix}link_6" />
+      <child link="${prefix}flange" />
+    </joint>
+
+  </xacro:macro>
+</robot>
diff --git a/borunte_support/urdf/borunte_none_macro.xacro b/borunte_support/urdf/borunte_none_macro.xacro
new file mode 100644
index 0000000..c539274
--- /dev/null
+++ b/borunte_support/urdf/borunte_none_macro.xacro
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="borunte_none_macro" params="prefix">
+
+    <xacro:include filename="$(find borunte_support)/urdf/borunte_macro.xacro"/>
+    <xacro:borunte_macro prefix="${prefix}"/>
+
+    <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+    <link name="${prefix}tool0" />
+    <joint name="${prefix}flange-tool0" type="fixed">
+      <!-- marker -->
+      <origin xyz="0 0 0" rpy="0 ${pi*1} 0" />
+      <parent link="${prefix}flange" />
+      <child link="${prefix}tool0" />
+    </joint>
+
+  </xacro:macro>
+</robot>
diff --git a/borunte_support/urdf/borunte_on_stand.xacro b/borunte_support/urdf/borunte_on_stand.xacro
new file mode 100644
index 0000000..119d9f7
--- /dev/null
+++ b/borunte_support/urdf/borunte_on_stand.xacro
@@ -0,0 +1,14 @@
+<?xml version="1.0" ?>
+<robot name="borunte_on_stand" xmlns:xacro="http://ros.org/wiki/xacro">
+	<xacro:include filename="$(find borunte_support)/urdf/borunte_on_stand_macro.xacro"/>
+	<xacro:borunte_on_stand_macro robotprefix="" standprefix="" tool="$(arg tool)"/>
+
+    <!-- 'world' frame: default pose reference frame -->
+    <link name="world" />
+    <joint name="world-borunte_stand_link" type="fixed">
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <axis xyz="0 0 1"/>
+      <child link="world"/>
+      <parent link="borunte_stand_link"/>
+    </joint>
+</robot>
diff --git a/borunte_support/urdf/borunte_on_stand_macro.xacro b/borunte_support/urdf/borunte_on_stand_macro.xacro
new file mode 100644
index 0000000..e330ab6
--- /dev/null
+++ b/borunte_support/urdf/borunte_on_stand_macro.xacro
@@ -0,0 +1,37 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="borunte_on_stand_macro" params="robotprefix standprefix tool">
+
+    <xacro:if value="${tool == 'none'}">
+      <xacro:include filename="$(find borunte_support)/urdf/borunte_none_macro.xacro"/>
+      <xacro:borunte_none_macro prefix="${robotprefix}"/>
+    </xacro:if>
+
+    <xacro:if value="${tool == 'hand_e'}">
+      <xacro:include filename="$(find borunte_support)/urdf/borunte_hand_e_macro.xacro"/>
+      <xacro:borunte_hand_e_macro prefix="${robotprefix}"/>
+    </xacro:if>
+
+    <xacro:if value="${tool == 'hand_e_fish_fingers'}">
+      <xacro:include filename="$(find borunte_support)/urdf/borunte_hand_e_fish_fingers_macro.xacro"/>
+      <xacro:borunte_hand_e_macro prefix="${robotprefix}"/>
+    </xacro:if>
+
+    <xacro:if value="${tool == 'ezgripper_gen2_plus'}">
+      <xacro:include filename="$(find borunte_support)/urdf/borunte_ezgripper_gen2_plus_macro.xacro"/>
+      <xacro:ezgripper_gen2_plus_macro prefix="${robotprefix}"/>
+    </xacro:if>
+
+    <xacro:include filename="$(find borunte_support)/urdf/borunte_stand_macro.xacro"/>
+    <xacro:borunte_stand_macro prefix="${standprefix}"/>
+
+    <!-- attach robot to stand flange -->
+    <joint name="${robotprefix}robot_attach_joint" type="fixed">
+      <parent link="borunte_stand_flange"/>
+      <child link="${robotprefix}base_link"/>
+      <axis xyz="0 0 0"/>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
+    </joint>
+
+  </xacro:macro>
+</robot>
diff --git a/borunte_support/urdf/borunte_stand_macro.xacro b/borunte_support/urdf/borunte_stand_macro.xacro
new file mode 100644
index 0000000..a63eec2
--- /dev/null
+++ b/borunte_support/urdf/borunte_stand_macro.xacro
@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="borunte_stand_macro" params="prefix">
+    <material name="${prefix}DarkGray">
+      <color rgba="0.5 0.5 0.5 0.75"/>
+    </material>
+
+    <!-- link list  -->
+    <link name="${prefix}borunte_stand_link">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/borunte_carriage.stl"/>
+        </geometry>
+        <origin rpy="0.0 0 0.0" xyz="0 0 0"/>
+        <material name="${prefix}DarkGray"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/borunte_carriage.stl"/>
+        </geometry>
+        <origin rpy="0.0 0 0.0" xyz="0 0 0.0"/>
+      </collision>
+    </link>
+
+    <!-- link the robot is going to be attached to -->
+    <link name="${prefix}borunte_stand_flange" />
+
+    <joint name="${prefix}borunte_stand_joint" type="fixed">
+      <parent link="${prefix}borunte_stand_link"/>
+      <child link="${prefix}borunte_stand_flange"/>
+      <axis xyz="0 0 0"/>
+      <origin rpy="0.0 0.0 ${pi/4.0}" xyz="0.0 0.0 0.735"/>
+    </joint>
+
+  </xacro:macro>
+</robot>
diff --git a/borunte_support/urdf/robotiq_hand_e.xacro b/borunte_support/urdf/robotiq_hand_e.xacro
new file mode 100644
index 0000000..a80b58c
--- /dev/null
+++ b/borunte_support/urdf/robotiq_hand_e.xacro
@@ -0,0 +1,5 @@
+<?xml version="1.0" ?>
+<robot name="borunte" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:include filename="$(find borunte_support)/urdf/robotiq_hand_e_macro.xacro"/>
+  <xacro:robotiq_hand_e_macro prefix=""/>
+</robot>
diff --git a/borunte_support/urdf/robotiq_hand_e_base_macro.xacro b/borunte_support/urdf/robotiq_hand_e_base_macro.xacro
new file mode 100644
index 0000000..55aa5d8
--- /dev/null
+++ b/borunte_support/urdf/robotiq_hand_e_base_macro.xacro
@@ -0,0 +1,52 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="robotiq_hand_e_base_macro" params="prefix">
+
+    <material name="${prefix}Black_Aluminum">
+      <color rgba="0.1 0.1 0.1 0.99"/>
+    </material>
+
+    <material name="${prefix}White_Plastic">
+      <color rgba="0.9 0.9 0.9 0.99"/>
+    </material>
+
+    <link name="${prefix}borunte_robotiq_adapter">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/robotiq_adapter.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <material name="${prefix}White_Plastic"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/robotiq_adapter_collision.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+      </collision>
+    </link>
+
+    <link name="${prefix}robotiq_hand_e">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/robotiq_hand-e_gripper.stl"/>
+        </geometry>
+        <origin xyz="0 0 -0.003" rpy="0 0 0"/>
+        <material name="${prefix}Black_Aluminum"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/robotiq_hand-e_gripper_collision.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+      </collision>
+    </link>
+    <joint name="${prefix}borunte_robotiq_adapter-robotiq_hand_e" type="fixed">
+      <parent link="${prefix}borunte_robotiq_adapter" />
+      <child link="${prefix}robotiq_hand_e" />
+      <axis xyz="0 0 1"/>
+      <origin xyz="0 0 0.015" rpy="${pi} ${pi} ${pi/2.0}" />
+    </joint>
+
+    </xacro:macro>
+</robot>
diff --git a/borunte_support/urdf/robotiq_hand_e_fish_fingers_macro.xacro b/borunte_support/urdf/robotiq_hand_e_fish_fingers_macro.xacro
new file mode 100644
index 0000000..a1afa6d
--- /dev/null
+++ b/borunte_support/urdf/robotiq_hand_e_fish_fingers_macro.xacro
@@ -0,0 +1,145 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="robotiq_hand_e_macro" params="prefix">
+
+    <xacro:include filename="$(find borunte_support)/urdf/robotiq_hand_e_base_macro.xacro"/>
+    <xacro:robotiq_hand_e_base_macro prefix="${prefix}"/>
+
+    <material name="${prefix}Blue_Rubber">
+      <color rgba="0.1 0.1 1.0 0.99"/>
+    </material>
+
+    <link name="${prefix}fish_finger_adapter_left">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/fish_finger_adapter_left.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <material name="${prefix}Black_Aluminum"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/fish_finger_adapter_left_collision.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0.0"/>
+      </collision>
+    </link>
+    <joint name="${prefix}robotiq_hand_e-fish_finger_adapter_left" type="fixed">
+      <parent link="${prefix}robotiq_hand_e" />
+      <child link="${prefix}fish_finger_adapter_left" />
+      <axis xyz="0 0 1"/>
+      <origin xyz="0 0 0.0965" rpy="0 0 ${pi/2.0}" />
+    </joint>
+
+    <link name="${prefix}fish_finger_adapter_right">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/fish_finger_adapter_right.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+        <material name="${prefix}Black_Aluminum"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/fish_finger_adapter_right_collision.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 ${pi}"/>
+      </collision>
+    </link>
+    <joint name="${prefix}robotiq_hand_e-fish_finger_adapter_right" type="fixed">
+      <parent link="${prefix}robotiq_hand_e" />
+      <child link="${prefix}fish_finger_adapter_right" />
+      <axis xyz="0 0 1"/>
+      <origin xyz="0 0 0.0965" rpy="0 0 -${pi/2.0}" />
+    </joint>
+
+    <link name="${prefix}simple_grip_finger_left">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/simple_grip_finger.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <material name="${prefix}Black_Aluminum"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/simple_grip_finger_collision.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0.0"/>
+      </collision>
+    </link>
+    <joint name="${prefix}robotiq_hand_e-simple_grip_finger_left" type="fixed">
+      <parent link="${prefix}fish_finger_adapter_left" />
+      <child link="${prefix}simple_grip_finger_left" />
+      <axis xyz="0 0 1"/>
+      <origin xyz="0.014293 0.0005 0.014707" rpy="0 ${pi/4.0} 0" />
+    </joint>
+
+    <link name="${prefix}fish_finger_left">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/fish_finger.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <material name="${prefix}Blue_Rubber"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/fish_finger_collision.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0.0"/>
+      </collision>
+    </link>
+    <joint name="${prefix}fish_finger_adapter_left-fish_finger_left" type="fixed">
+      <parent link="${prefix}fish_finger_adapter_left" />
+      <child link="${prefix}fish_finger_left" />
+      <axis xyz="0 0 1"/>
+      <origin xyz="-0.015 -0.026 0.014" rpy="0 ${pi/4.0} ${pi}" />
+    </joint>
+
+    <link name="${prefix}simple_grip_finger_right">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/simple_grip_finger.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <material name="${prefix}Black_Aluminum"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/simple_grip_finger_collision.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0.0"/>
+      </collision>
+    </link>
+    <joint name="${prefix}fish_finger_adapter_right-simple_grip_finger_right" type="fixed">
+      <parent link="${prefix}fish_finger_adapter_right" />
+      <child link="${prefix}simple_grip_finger_right" />
+      <axis xyz="0 0 1"/>
+      <origin xyz="-0.014293 0.0005 0.014707" rpy="0 -${pi/4.0} 0" />
+    </joint>
+
+        <link name="${prefix}fish_finger_right">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/fish_finger.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <material name="${prefix}Blue_Rubber"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/fish_finger_collision.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0.0"/>
+      </collision>
+    </link>
+    <joint name="${prefix}fish_finger_adapter_right-fish_finger_right" type="fixed">
+      <parent link="${prefix}fish_finger_adapter_right" />
+      <child link="${prefix}fish_finger_right" />
+      <axis xyz="0 0 1"/>
+      <origin xyz="0.015 -0.026 0.014" rpy="0 -${pi/4.0} ${pi}" />
+    </joint>
+
+  </xacro:macro>
+</robot>
diff --git a/borunte_support/urdf/robotiq_hand_e_macro.xacro b/borunte_support/urdf/robotiq_hand_e_macro.xacro
new file mode 100644
index 0000000..76218f6
--- /dev/null
+++ b/borunte_support/urdf/robotiq_hand_e_macro.xacro
@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:macro name="robotiq_hand_e_macro" params="prefix">
+
+    <xacro:include filename="$(find borunte_support)/urdf/robotiq_hand_e_base_macro.xacro"/>
+    <xacro:robotiq_hand_e_base_macro prefix="${prefix}"/>
+
+    <link name="${prefix}simple_grip_finger_left">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/simple_grip_finger.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <material name="${prefix}Black_Aluminum"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/simple_grip_finger_collision.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0.0"/>
+      </collision>
+    </link>
+    <joint name="${prefix}robotiq_hand_e-simple_grip_finger_left" type="fixed">
+      <parent link="${prefix}robotiq_hand_e" />
+      <child link="${prefix}simple_grip_finger_left" />
+      <axis xyz="0 0 1"/>
+      <origin xyz="0 0 0.0965" rpy="0 0 ${pi/2.0}" />
+    </joint>
+
+    <link name="${prefix}simple_grip_finger_right">
+      <visual>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/simple_grip_finger.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <material name="${prefix}Black_Aluminum"/>
+      </visual>
+      <collision>
+        <geometry>
+          <mesh filename="package://borunte_support/meshes/simple_grip_finger_collision.stl"/>
+        </geometry>
+        <origin xyz="0 0 0" rpy="0 0 0.0"/>
+      </collision>
+    </link>
+    <joint name="${prefix}robotiq_hand_e-simple_grip_finger_right" type="fixed">
+      <parent link="${prefix}robotiq_hand_e" />
+      <child link="${prefix}simple_grip_finger_right" />
+      <axis xyz="0 0 1"/>
+      <origin xyz="0 0 0.0965" rpy="0 0 -${pi/2.0}" />
+    </joint>
+
+  </xacro:macro>
+</robot>
diff --git a/client_socket.py b/client_socket.py
index 1926c61..57ca963 100644
--- a/client_socket.py
+++ b/client_socket.py
@@ -24,8 +24,9 @@ class SocketRobotArm:
     # laser_id = 15
     laser_id = 14
     # filename = 'half-sphere-no-angle'
-    filename = "half-sphere"
+    filename = "test"
     urdf_filename = "sample"
+    # urdf_filename = "fanucM16ib"
 
     pass_size = 4
 
@@ -166,6 +167,8 @@ class SocketRobotArm:
     def set_joint(self, coordinates):
         num_joints = p.getNumJoints(self.body_id)
         for joint_index in range(0, num_joints):
+            if not joint_index in coordinates:
+                return
             p.setJointMotorControl2(
                 bodyUniqueId=self.body_id,
                 jointIndex=joint_index,
diff --git a/data/test.nc.result b/data/test.nc.result
new file mode 100644
index 0000000..ca0c8ff
--- /dev/null
+++ b/data/test.nc.result
@@ -0,0 +1,4 @@
+X0 Y100
+X100 Y100
+X100 Y0
+X0 Y0
\ No newline at end of file
diff --git a/fanuc_lrmate200ib_support/fanucLRMate200ib.rights b/fanuc_lrmate200ib_support/fanucLRMate200ib.rights
new file mode 100644
index 0000000..81669a6
--- /dev/null
+++ b/fanuc_lrmate200ib_support/fanucLRMate200ib.rights
@@ -0,0 +1,32 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2012-2015, TU Delft Robotics Institute
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+ * Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above
+   copyright notice, this list of conditions and the following
+   disclaimer in the documentation and/or other materials provided
+   with the distribution.
+ * Neither the name of the TU Delft Robotics Institute nor the names 
+   of its contributors may be used to endorse or promote products 
+   derived from this software without specific prior written 
+   permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/base_link.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/base_link.stl
new file mode 100644
index 0000000..1fec063
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/base_link.stl differ
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_1.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_1.stl
new file mode 100644
index 0000000..b1fb657
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_1.stl differ
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_2.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_2.stl
new file mode 100644
index 0000000..2793ae6
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_2.stl differ
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_3.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_3.stl
new file mode 100644
index 0000000..94e4542
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_3.stl differ
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_4.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_4.stl
new file mode 100644
index 0000000..b273c24
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_4.stl differ
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_5.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_5.stl
new file mode 100644
index 0000000..8b5cf8a
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_5.stl differ
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_6.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_6.stl
new file mode 100644
index 0000000..249ea6c
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_6.stl differ
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/base_link.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/base_link.stl
new file mode 100644
index 0000000..1fec063
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/base_link.stl differ
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_1.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_1.stl
new file mode 100644
index 0000000..b1fb657
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_1.stl differ
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_2.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_2.stl
new file mode 100644
index 0000000..2793ae6
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_2.stl differ
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_3.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_3.stl
new file mode 100644
index 0000000..94e4542
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_3.stl differ
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_4.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_4.stl
new file mode 100644
index 0000000..b273c24
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_4.stl differ
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_5.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_5.stl
new file mode 100644
index 0000000..8b5cf8a
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_5.stl differ
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_6.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_6.stl
new file mode 100644
index 0000000..249ea6c
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_6.stl differ
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib3l/collision/link_4.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib3l/collision/link_4.stl
new file mode 100644
index 0000000..7ddb9f9
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib3l/collision/link_4.stl differ
diff --git a/fanuc_lrmate200ib_support/meshes/lrmate200ib3l/visual/link_4.stl b/fanuc_lrmate200ib_support/meshes/lrmate200ib3l/visual/link_4.stl
new file mode 100644
index 0000000..7ddb9f9
Binary files /dev/null and b/fanuc_lrmate200ib_support/meshes/lrmate200ib3l/visual/link_4.stl differ
diff --git a/fanuc_lrmate200ib_support/meta-information.json b/fanuc_lrmate200ib_support/meta-information.json
new file mode 100644
index 0000000..93082eb
--- /dev/null
+++ b/fanuc_lrmate200ib_support/meta-information.json
@@ -0,0 +1,16 @@
+{
+    "robots": [
+        {
+            "name": "LR Mate 200iB",
+            "type": "robotic arm",
+            "manufacturer": "Fanuc",
+            "xacro-generated": false,
+            "urdf": [
+                "urdf/fanucLRMate200ib.urdf"
+            ],
+            "variant": null,
+            "source-link": "https://se.mathworks.com/matlabcentral/fileexchange/98714-robotics-system-toolbox-robot-library-data",
+            "id": 12
+        }
+    ]
+}
\ No newline at end of file
diff --git a/fanuc_lrmate200ib_support/urdf/fanucLRMate200ib.urdf b/fanuc_lrmate200ib_support/urdf/fanucLRMate200ib.urdf
new file mode 100644
index 0000000..023de79
--- /dev/null
+++ b/fanuc_lrmate200ib_support/urdf/fanucLRMate200ib.urdf
@@ -0,0 +1,191 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from lrmate200ib.xacro              | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="fanuc_lrmate200ib" xmlns:xacro="http://ros.org/wiki/xacro">
+  <!-- links: main serial chain -->
+  <link name="base_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/lrmate200ib/visual/base_link.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/lrmate200ib/collision/base_link.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_1">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/lrmate200ib/visual/link_1.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/lrmate200ib/collision/link_1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_2">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/lrmate200ib/visual/link_2.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/lrmate200ib/collision/link_2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_3">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/lrmate200ib/visual/link_3.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/lrmate200ib/collision/link_3.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_4">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/lrmate200ib/visual/link_4.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/lrmate200ib/collision/link_4.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_5">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/lrmate200ib/visual/link_5.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/lrmate200ib/collision/link_5.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_6">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/lrmate200ib/visual/link_6.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.15 0.15 0.15 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/lrmate200ib/collision/link_6.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- joints: main serial chain -->
+  <joint name="joint_1" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0 0.350"/>
+    <parent link="base_link"/>
+    <child link="link_1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0" lower="-2.7925" upper="2.7925" velocity="3.1415"/>
+  </joint>
+  <joint name="joint_2" type="revolute">
+    <origin rpy="0 0 0" xyz="0.150 0 0"/>
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0" lower="-0.5759" upper="2.6529" velocity="3.1415"/>
+  </joint>
+  <joint name="joint_3" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0 0.250"/>
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="0" lower="-2.6145" upper="2.8797" velocity="3.9269"/>
+  </joint>
+  <joint name="joint_4" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0 0.075"/>
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="0" lower="-3.3161" upper="3.3161" velocity="6.9813"/>
+  </joint>
+  <joint name="joint_5" type="revolute">
+    <origin rpy="0 0 0" xyz="0.290 0 0"/>
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="0" lower="-2.0943" upper="2.0943" velocity="5.7595"/>
+  </joint>
+  <joint name="joint_6" type="revolute">
+    <origin rpy="0 0 0" xyz="0.080 0 0"/>
+    <parent link="link_5"/>
+    <child link="link_6"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="0" lower="-6.2831" upper="6.2831" velocity="8.3775"/>
+  </joint>
+  <!-- ROS base_link to Fanuc World Coordinates transform -->
+  <link name="base"/>
+  <joint name="base_link-base" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.350"/>
+    <parent link="base_link"/>
+    <child link="base"/>
+  </joint>
+  <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
+  <link name="flange"/>
+  <joint name="joint_6-flange" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="link_6"/>
+    <child link="flange"/>
+  </joint>
+  <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+  <link name="tool0"/>
+  <joint name="link_6-tool0" type="fixed">
+    <origin rpy="3.14159265359 -1.57079632679 0" xyz="0 0 0"/>
+    <parent link="link_6"/>
+    <child link="tool0"/>
+  </joint>
+</robot>
diff --git a/fanuc_m16ib_support/fanucM16ib.rights b/fanuc_m16ib_support/fanucM16ib.rights
new file mode 100644
index 0000000..e4277af
--- /dev/null
+++ b/fanuc_m16ib_support/fanucM16ib.rights
@@ -0,0 +1,32 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2012, 2013, TU Delft Robotics Institute
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+ * Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above
+   copyright notice, this list of conditions and the following
+   disclaimer in the documentation and/or other materials provided
+   with the distribution.
+ * Neither the name of the TU Delft Robotics Institute nor the names 
+   of its contributors may be used to endorse or promote products 
+   derived from this software without specific prior written 
+   permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
\ No newline at end of file
diff --git a/fanuc_m16ib_support/meshes/m16ib20/collision/base_link.stl b/fanuc_m16ib_support/meshes/m16ib20/collision/base_link.stl
new file mode 100644
index 0000000..605b8fa
Binary files /dev/null and b/fanuc_m16ib_support/meshes/m16ib20/collision/base_link.stl differ
diff --git a/fanuc_m16ib_support/meshes/m16ib20/collision/link_1.stl b/fanuc_m16ib_support/meshes/m16ib20/collision/link_1.stl
new file mode 100644
index 0000000..5e31465
Binary files /dev/null and b/fanuc_m16ib_support/meshes/m16ib20/collision/link_1.stl differ
diff --git a/fanuc_m16ib_support/meshes/m16ib20/collision/link_2.stl b/fanuc_m16ib_support/meshes/m16ib20/collision/link_2.stl
new file mode 100644
index 0000000..44d3e1e
Binary files /dev/null and b/fanuc_m16ib_support/meshes/m16ib20/collision/link_2.stl differ
diff --git a/fanuc_m16ib_support/meshes/m16ib20/collision/link_3.stl b/fanuc_m16ib_support/meshes/m16ib20/collision/link_3.stl
new file mode 100644
index 0000000..658d54c
Binary files /dev/null and b/fanuc_m16ib_support/meshes/m16ib20/collision/link_3.stl differ
diff --git a/fanuc_m16ib_support/meshes/m16ib20/collision/link_4.stl b/fanuc_m16ib_support/meshes/m16ib20/collision/link_4.stl
new file mode 100644
index 0000000..8a921ad
Binary files /dev/null and b/fanuc_m16ib_support/meshes/m16ib20/collision/link_4.stl differ
diff --git a/fanuc_m16ib_support/meshes/m16ib20/collision/link_5.stl b/fanuc_m16ib_support/meshes/m16ib20/collision/link_5.stl
new file mode 100644
index 0000000..582b60f
Binary files /dev/null and b/fanuc_m16ib_support/meshes/m16ib20/collision/link_5.stl differ
diff --git a/fanuc_m16ib_support/meshes/m16ib20/collision/link_6.stl b/fanuc_m16ib_support/meshes/m16ib20/collision/link_6.stl
new file mode 100644
index 0000000..b2684ba
Binary files /dev/null and b/fanuc_m16ib_support/meshes/m16ib20/collision/link_6.stl differ
diff --git a/fanuc_m16ib_support/meshes/m16ib20/visual/base_link.stl b/fanuc_m16ib_support/meshes/m16ib20/visual/base_link.stl
new file mode 100644
index 0000000..605b8fa
Binary files /dev/null and b/fanuc_m16ib_support/meshes/m16ib20/visual/base_link.stl differ
diff --git a/fanuc_m16ib_support/meshes/m16ib20/visual/link_1.stl b/fanuc_m16ib_support/meshes/m16ib20/visual/link_1.stl
new file mode 100644
index 0000000..5e31465
Binary files /dev/null and b/fanuc_m16ib_support/meshes/m16ib20/visual/link_1.stl differ
diff --git a/fanuc_m16ib_support/meshes/m16ib20/visual/link_2.stl b/fanuc_m16ib_support/meshes/m16ib20/visual/link_2.stl
new file mode 100644
index 0000000..44d3e1e
Binary files /dev/null and b/fanuc_m16ib_support/meshes/m16ib20/visual/link_2.stl differ
diff --git a/fanuc_m16ib_support/meshes/m16ib20/visual/link_3.stl b/fanuc_m16ib_support/meshes/m16ib20/visual/link_3.stl
new file mode 100644
index 0000000..658d54c
Binary files /dev/null and b/fanuc_m16ib_support/meshes/m16ib20/visual/link_3.stl differ
diff --git a/fanuc_m16ib_support/meshes/m16ib20/visual/link_4.stl b/fanuc_m16ib_support/meshes/m16ib20/visual/link_4.stl
new file mode 100644
index 0000000..8a921ad
Binary files /dev/null and b/fanuc_m16ib_support/meshes/m16ib20/visual/link_4.stl differ
diff --git a/fanuc_m16ib_support/meshes/m16ib20/visual/link_5.stl b/fanuc_m16ib_support/meshes/m16ib20/visual/link_5.stl
new file mode 100644
index 0000000..582b60f
Binary files /dev/null and b/fanuc_m16ib_support/meshes/m16ib20/visual/link_5.stl differ
diff --git a/fanuc_m16ib_support/meshes/m16ib20/visual/link_6.stl b/fanuc_m16ib_support/meshes/m16ib20/visual/link_6.stl
new file mode 100644
index 0000000..b2684ba
Binary files /dev/null and b/fanuc_m16ib_support/meshes/m16ib20/visual/link_6.stl differ
diff --git a/fanuc_m16ib_support/meta-information.json b/fanuc_m16ib_support/meta-information.json
new file mode 100644
index 0000000..0dd62f5
--- /dev/null
+++ b/fanuc_m16ib_support/meta-information.json
@@ -0,0 +1,16 @@
+{
+    "robots": [
+        {
+            "name": "M-16iB",
+            "type": "robotic arm",
+            "manufacturer": "Fanuc",
+            "xacro-generated": false,
+            "urdf": [
+                "urdf/fanucM16ib.urdf"
+            ],
+            "variant": null,
+            "source-link": "https://se.mathworks.com/matlabcentral/fileexchange/98714-robotics-system-toolbox-robot-library-data",
+            "id": 13
+        }
+    ]
+}
\ No newline at end of file
diff --git a/fanuc_m16ib_support/urdf/fanucM16ib.urdf b/fanuc_m16ib_support/urdf/fanucM16ib.urdf
new file mode 100644
index 0000000..5ad9dc1
--- /dev/null
+++ b/fanuc_m16ib_support/urdf/fanucM16ib.urdf
@@ -0,0 +1,174 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from m16ib20.xacro                  | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="fanuc_m16ib20" xmlns:xacro="http://ros.org/wiki/xacro">
+  <link name="base_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/fanuc_m16ib_support/meshes/m16ib20/visual/base_link.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/fanuc_m16ib_support/meshes/m16ib20/collision/base_link.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_1">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/fanuc_m16ib_support/meshes/m16ib20/visual/link_1.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/fanuc_m16ib_support/meshes/m16ib20/collision/link_1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_2">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/fanuc_m16ib_support/meshes/m16ib20/visual/link_2.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/fanuc_m16ib_support/meshes/m16ib20/collision/link_2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_3">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/fanuc_m16ib_support/meshes/m16ib20/visual/link_3.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/fanuc_m16ib_support/meshes/m16ib20/collision/link_3.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_4">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/fanuc_m16ib_support/meshes/m16ib20/visual/link_4.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/fanuc_m16ib_support/meshes/m16ib20/collision/link_4.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_5">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/fanuc_m16ib_support/meshes/m16ib20/visual/link_5.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/fanuc_m16ib_support/meshes/m16ib20/collision/link_5.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_6">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/fanuc_m16ib_support/meshes/m16ib20/visual/link_6.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.15 0.15 0.15 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../meshes/fanuc_m16ib_support/meshes/m16ib20/collision/link_6.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="tool0"/>
+  <joint name="joint_1" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0 0.245"/>
+    <parent link="base_link"/>
+    <child link="link_1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0" lower="-2.9671" upper="2.9671" velocity="2.8798"/>
+  </joint>
+  <joint name="joint_2" type="revolute">
+    <origin rpy="0 0 0" xyz="0.15 0.18759 0.28"/>
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0" lower="-1.5708" upper="2.7925" velocity="2.8798"/>
+  </joint>
+  <joint name="joint_3" type="revolute">
+    <origin rpy="0 0 0" xyz="0 -0.18759 0.77"/>
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="0" lower="-2.9671" upper="5.0615" velocity="3.0543"/>
+  </joint>
+  <joint name="joint_4" type="revolute">
+    <origin rpy="0 0 0" xyz="0.18425 0 0.1"/>
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="0" lower="-3.4907" upper="3.4907" velocity="6.1087"/>
+  </joint>
+  <joint name="joint_5" type="revolute">
+    <origin rpy="0 0 0" xyz="0.55575 0 0"/>
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="0" lower="-2.4435" upper="2.4435" velocity="5.9341"/>
+  </joint>
+  <joint name="joint_6" type="revolute">
+    <origin rpy="0 0 0" xyz="0.1 0 0"/>
+    <parent link="link_5"/>
+    <child link="link_6"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="0" lower="-7.8540" upper="7.8540" velocity="9.0757"/>
+  </joint>
+  <joint name="joint_6-tool0" type="fixed">
+    <parent link="link_6"/>
+    <child link="tool0"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+</robot>
diff --git a/test.py b/simple_test.py
similarity index 100%
rename from test.py
rename to simple_test.py
diff --git a/test_socket_server.py b/test_socket_server.py
index d564703..f95127c 100644
--- a/test_socket_server.py
+++ b/test_socket_server.py
@@ -18,13 +18,21 @@ with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
                     res = {"queryData": ["ok"]}
 
                     if "queryAddr" in req.keys() and "axis-0" in req["queryAddr"]:
-                        res["queryData"] = [-59.696, 39.438, -7.478, -58.198, -76.606, 198.533]
+                        res["queryData"] = [-59.696, 39.438, -7.478, -58.198, -76.606, 0]
+                        # res["queryData"] = [-60, 40, -10, -60, -75, 0]
 
                     if "queryAddr" in req and "world-0" in req["queryAddr"]:
-                        res["queryData"] = [643.622, -1289.604, 254.682, 124.70, 24.209, -58.492]
+                        res["queryData"] = [
+                            643.622,
+                            -1289.604,
+                            254.682,
+                            124.70,
+                            24.209,
+                            -58.492,
+                        ]
 
                     if req["reqType"] == "command":
-                        res["cmdReply"] = ['ok']
+                        res["cmdReply"] = ["ok"]
                     conn.sendall(json.dumps(res).encode())
             except Exception as e:
                 print(f"by {addr} error {e}")
diff --git a/urdf/borunte_macro.urdf b/urdf/borunte_macro.urdf
new file mode 100644
index 0000000..a2a5404
--- /dev/null
+++ b/urdf/borunte_macro.urdf
@@ -0,0 +1,215 @@
+<?xml version="1.0" ?>
+<robot name="borunte">
+  <material name="Black">
+    <color rgba="0.1 0.1 0.1 0.99"/>
+  </material>
+  <material name="DarkRed">
+    <color rgba="0.5 0.0 0.0 0.99"/>
+  </material>
+  <!-- link list -->
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <mesh filename="/home/aarizona/projects/modbus_test/borunte_support/meshes/base_link.stl"/>
+      </geometry>
+      <origin rpy="0.0 0 0.0" xyz="0 0 0"/>
+      <material name="Black"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="/home/aarizona/projects/modbus_test/borunte_support/meshes/base_link_collision.stl"/>
+      </geometry>
+      <origin rpy="0.0 0 0.0" xyz="0 0 0"/>
+    </collision>
+  </link>
+  <link name="link_1">
+    <visual>
+      <geometry>
+        <mesh filename="/home/aarizona/projects/modbus_test/borunte_support/meshes/link_1.stl"/>
+      </geometry>
+      <origin rpy="0.0 0 0.0" xyz="0 0 0"/>
+      <material name="DarkRed"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="/home/aarizona/projects/modbus_test/borunte_support/meshes/link_1_collision.stl"/>
+      </geometry>
+      <origin rpy="0.0 0 0.0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <mass value="100"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+  <link name="link_2">
+    <visual>
+      <geometry>
+        <mesh filename="/home/aarizona/projects/modbus_test/borunte_support/meshes/link_2.stl"/>
+      </geometry>
+      <origin rpy="0.0 0 0.0" xyz="0 0 0"/>
+      <material name="DarkRed"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="/home/aarizona/projects/modbus_test/borunte_support/meshes/link_2_collision.stl"/>
+      </geometry>
+      <origin rpy="0.0 0 0.0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <mass value="100"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+  <link name="link_3">
+    <visual>
+      <geometry>
+        <mesh filename="/home/aarizona/projects/modbus_test/borunte_support/meshes/link_3.stl"/>
+      </geometry>
+      <origin rpy="0.0 0 0.0" xyz="0 0.01 0"/>
+      <material name="DarkRed"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="/home/aarizona/projects/modbus_test/borunte_support/meshes/link_3_collision.stl"/>
+      </geometry>
+      <origin rpy="0.0 0 0.0" xyz="0 0.01 0"/>
+    </collision>
+    <inertial>
+      <mass value="100"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+  <link name="link_4">
+    <visual>
+      <geometry>
+        <mesh filename="/home/aarizona/projects/modbus_test/borunte_support/meshes/link_4.stl"/>
+      </geometry>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
+      <material name="DarkRed"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="/home/aarizona/projects/modbus_test/borunte_support/meshes/link_4_collision.stl"/>
+      </geometry>
+      <origin rpy="0.0 0 0.0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <mass value="100"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+  <link name="link_5">
+    <visual>
+      <geometry>
+        <mesh filename="/home/aarizona/projects/modbus_test/borunte_support/meshes/link_5.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <material name="DarkRed"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="/home/aarizona/projects/modbus_test/borunte_support/meshes/link_5_collision.stl"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <mass value="100"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+  <link name="link_6">
+    <visual>
+      <geometry>
+        <mesh filename="/home/aarizona/projects/modbus_test/borunte_support/meshes/link_6.stl"/>
+      </geometry>
+      <origin rpy="0 0 0.0" xyz="0 0 0"/>
+      <material name="DarkRed"/>
+    </visual>
+    <collision>
+      <geometry>
+        <mesh filename="/home/aarizona/projects/modbus_test/borunte_support/meshes/link_6_collision.stl"/>
+      </geometry>
+      <origin rpy="0 0 0.0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <mass value="100"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+  <!-- joint list -->
+  <joint name="joint_1" type="revolute">
+    <limit effort="1000.0" lower="-3.076" upper="3.076" velocity="3.14"/>
+    <parent link="base_link"/>
+    <child link="link_1"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0 0 0.24"/>
+  </joint>
+  <joint name="joint_2" type="revolute">
+    <!-- actual limit -1.74, reduced to reduce config problems -->
+    <limit effort="1000.0" lower="-1.0" upper="2.35" velocity="1.0"/>
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    <axis xyz="0 1 0"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.07 0.09 0.165"/>
+  </joint>
+  <joint name="joint_3" type="revolute">
+    <limit effort="1000.0" lower="-2.094" upper="1.570" velocity="1.3"/>
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    <axis xyz="0 1 0"/>
+    <origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0 0.0 0.39"/>
+  </joint>
+  <joint name="joint_4" type="revolute">
+    <!-- actual limit +-2.094, reduced to reduce config problems -->
+    <limit effort="1000.0" lower="-1.5707963267948966" upper="1.5707963267948966" velocity="2.0"/>
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0.0 0.0 0.0" xyz="-0.125 -0.09 0.075"/>
+  </joint>
+  <joint name="joint_5" type="revolute">
+    <limit effort="1000.0" lower="-1.919" upper="2.094" velocity="2.35"/>
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <axis xyz="0 1 0"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.0 0.31"/>
+  </joint>
+  <joint name="joint_6" type="revolute">
+    <limit effort="1000.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="2.35"/>
+    <parent link="link_5"/>
+    <child link="link_6"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0.0 0.0 0.0" xyz="0 0 0.104"/>
+  </joint>
+  <!-- ROS-Industrial 'base' frame: base_link to Fanuc World Coordinates transform -->
+  <link name="base"/>
+  <joint name="base_link-base" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="base_link"/>
+    <child link="base"/>
+  </joint>
+  <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
+  <link name="flange"/>
+  <joint name="joint_6-flange" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.015"/>
+    <parent link="link_6"/>
+    <child link="flange"/>
+  </joint>
+  <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+  <link name="tool0"/>
+  <joint name="flange-tool0" type="fixed">
+    <!-- marker -->
+    <origin rpy="0 3.141592653589793 0" xyz="0 0 0"/>
+    <parent link="flange"/>
+    <child link="tool0"/>
+  </joint>
+  <!-- 'world' frame: default pose reference frame -->
+  <link name="world"/>
+  <joint name="world-base_link" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <child link="world"/>
+    <parent link="base_link"/>
+  </joint>
+</robot>
diff --git a/urdf/fanucLRMate200ib.urdf b/urdf/fanucLRMate200ib.urdf
new file mode 100644
index 0000000..e69d99a
--- /dev/null
+++ b/urdf/fanucLRMate200ib.urdf
@@ -0,0 +1,191 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from lrmate200ib.xacro              | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="fanuc_lrmate200ib">
+  <!-- links: main serial chain -->
+  <link name="base_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/base_link.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/base_link.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_1">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_1.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_2">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_2.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_3">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_3.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_3.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_4">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_4.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_4.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_5">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_5.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_5.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_6">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/visual/link_6.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.15 0.15 0.15 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_lrmate200ib_support/meshes/lrmate200ib/collision/link_6.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- joints: main serial chain -->
+  <joint name="joint_1" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0 0.350"/>
+    <parent link="base_link"/>
+    <child link="link_1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0" lower="-2.7925" upper="2.7925" velocity="3.1415"/>
+  </joint>
+  <joint name="joint_2" type="revolute">
+    <origin rpy="0 0 0" xyz="0.150 0 0"/>
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0" lower="-0.5759" upper="2.6529" velocity="3.1415"/>
+  </joint>
+  <joint name="joint_3" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0 0.250"/>
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="0" lower="-2.6145" upper="2.8797" velocity="3.9269"/>
+  </joint>
+  <joint name="joint_4" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0 0.075"/>
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="0" lower="-3.3161" upper="3.3161" velocity="6.9813"/>
+  </joint>
+  <joint name="joint_5" type="revolute">
+    <origin rpy="0 0 0" xyz="0.290 0 0"/>
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="0" lower="-2.0943" upper="2.0943" velocity="5.7595"/>
+  </joint>
+  <joint name="joint_6" type="revolute">
+    <origin rpy="0 0 0" xyz="0.080 0 0"/>
+    <parent link="link_5"/>
+    <child link="link_6"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="0" lower="-6.2831" upper="6.2831" velocity="8.3775"/>
+  </joint>
+  <!-- ROS base_link to Fanuc World Coordinates transform -->
+  <link name="base"/>
+  <joint name="base_link-base" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.350"/>
+    <parent link="base_link"/>
+    <child link="base"/>
+  </joint>
+  <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
+  <link name="flange"/>
+  <joint name="joint_6-flange" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="link_6"/>
+    <child link="flange"/>
+  </joint>
+  <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+  <link name="tool0"/>
+  <joint name="link_6-tool0" type="fixed">
+    <origin rpy="3.14159265359 -1.57079632679 0" xyz="0 0 0"/>
+    <parent link="link_6"/>
+    <child link="tool0"/>
+  </joint>
+</robot>
diff --git a/urdf/fanucM16ib.urdf b/urdf/fanucM16ib.urdf
new file mode 100644
index 0000000..0dc52ea
--- /dev/null
+++ b/urdf/fanucM16ib.urdf
@@ -0,0 +1,174 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from m16ib20.xacro                  | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="fanuc_m16ib20">
+  <link name="base_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_m16ib_support/meshes/m16ib20/visual/base_link.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_m16ib_support/meshes/m16ib20/collision/base_link.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_1">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_m16ib_support/meshes/m16ib20/visual/link_1.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_m16ib_support/meshes/m16ib20/collision/link_1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_2">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_m16ib_support/meshes/m16ib20/visual/link_2.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_m16ib_support/meshes/m16ib20/collision/link_2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_3">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_m16ib_support/meshes/m16ib20/visual/link_3.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_m16ib_support/meshes/m16ib20/collision/link_3.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_4">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_m16ib_support/meshes/m16ib20/visual/link_4.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_m16ib_support/meshes/m16ib20/collision/link_4.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_5">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_m16ib_support/meshes/m16ib20/visual/link_5.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_m16ib_support/meshes/m16ib20/collision/link_5.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_6">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_m16ib_support/meshes/m16ib20/visual/link_6.stl"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.15 0.15 0.15 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="../fanuc_m16ib_support/meshes/m16ib20/collision/link_6.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="tool0"/>
+  <joint name="joint_1" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0 0.245"/>
+    <parent link="base_link"/>
+    <child link="link_1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0" lower="-2.9671" upper="2.9671" velocity="2.8798"/>
+  </joint>
+  <joint name="joint_2" type="revolute">
+    <origin rpy="0 0 0" xyz="0.15 0.18759 0.28"/>
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0" lower="-1.5708" upper="2.7925" velocity="2.8798"/>
+  </joint>
+  <joint name="joint_3" type="revolute">
+    <origin rpy="0 0 0" xyz="0 -0.18759 0.77"/>
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="0" lower="-2.9671" upper="5.0615" velocity="3.0543"/>
+  </joint>
+  <joint name="joint_4" type="revolute">
+    <origin rpy="0 0 0" xyz="0.18425 0 0.1"/>
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="0" lower="-3.4907" upper="3.4907" velocity="6.1087"/>
+  </joint>
+  <joint name="joint_5" type="revolute">
+    <origin rpy="0 0 0" xyz="0.55575 0 0"/>
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="0" lower="-2.4435" upper="2.4435" velocity="5.9341"/>
+  </joint>
+  <joint name="joint_6" type="revolute">
+    <origin rpy="0 0 0" xyz="0.1 0 0"/>
+    <parent link="link_5"/>
+    <child link="link_6"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="0" lower="-7.8540" upper="7.8540" velocity="9.0757"/>
+  </joint>
+  <joint name="joint_6-tool0" type="fixed">
+    <parent link="link_6"/>
+    <child link="tool0"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+</robot>
diff --git a/urdf/sample.urdf b/urdf/sample.urdf
index b17f2af..3cbcd51 100644
--- a/urdf/sample.urdf
+++ b/urdf/sample.urdf
@@ -1,136 +1,145 @@
 <?xml version="1.0"?>
-<robot name="five_axis_robot" xmlns:xacro="http://ros.org/wiki/xacro">
-
-  <!-- Определение ссылок -->
+<robot name="sample_robot">
+  <!-- links: main serial chain -->
   <link name="base_link">
-    <inertial>
-      <mass value="1.0" />
-      <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
-    </inertial>
     <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.1"/>
       <geometry>
-        <box size="0.5 0.5 0.4" />
+        <box size="0.2 0.2 0.2"/>
       </geometry>
-      <material name="grey">
-        <color rgba="0.5 0.5 0.5 1.0" />
+      <material name="">
+        <color rgba="0.0 0 0 1.0"/>
       </material>
     </visual>
   </link>
-
-  <link name="link1">
-    <inertial>
-      <mass value="1.0" />
-      <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
-    </inertial>
+  <link name="link_1">
     <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.2473"/>
       <geometry>
-        <cylinder radius="0.05" length="0.3" />
+        <box size="0.1 0.1 0.4946"/>
       </geometry>
-      <material name="blue">
-        <color rgba="0.0 0.0 1.0 1.0" />
+      <material name="">
+        <color rgba="1.0 0.0 0.0 1.0"/>
       </material>
     </visual>
   </link>
-
-  <link name="link2">
-    <inertial>
-      <mass value="1.0" />
-      <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
-    </inertial>
+  <link name="link_2">
     <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.3649605"/>
       <geometry>
-        <cylinder radius="0.05" length="0.3" />
+        <box size="0.1 0.1 0.729921"/>
       </geometry>
-      <material name="green">
-        <color rgba="0.0 1.0 0.0 1.0" />
+      <material name="">
+        <color rgba="0 0.0 1.0 1.0"/>
       </material>
     </visual>
   </link>
-
-  <link name="link3">
-    <inertial>
-      <mass value="1.0" />
-      <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
-    </inertial>
+  <link name="link_3">
     <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.049"/>
       <geometry>
-        <cylinder radius="0.05" length="0.3" />
+        <box size="0.1 0.1 0.098277"/>
       </geometry>
-      <material name="red">
-        <color rgba="1.0 0.0 0.0 1.0" />
+      <material name="">
+        <color rgba="0. 0.22 1.00 1.0"/>
       </material>
     </visual>
   </link>
-
-  <link name="link4">
-    <inertial>
-      <mass value="1.0" />
-      <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
-    </inertial>
+  <link name="link_4">
     <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.5545195"/>
       <geometry>
-        <cylinder radius="0.05" length="0.3" />
+        <box size="0.1 0.1 1.109039"/>
       </geometry>
-      <material name="yellow">
-        <color rgba="1.0 1.0 0.0 1.0" />
+      <material name="">
+        <color rgba="0.96 0.76 0.13 1.0"/>
       </material>
     </visual>
   </link>
-
-  <link name="end_effector">
-    <inertial>
-      <mass value="1.0" />
-      <inertia ixx="0.005" ixy="0.0" ixz="0.0" iyy="0.005" iyz="0.0" izz="0.005"/>
-    </inertial>
+  <link name="link_5">
     <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.0585"/>
       <geometry>
-        <box size="0.1 0.1 0.1" />
+        <box size="0.1 0.1 0.117"/>
       </geometry>
-      <material name="black">
-        <color rgba="0.0 0.0 0.0 1.0" />
+      <material name="">
+        <color rgba="1.0 0.5 0.0 1.0"/>
       </material>
     </visual>
   </link>
-
-  <!-- Определение суставов -->
-  <joint name="joint1" type="revolute">
-    <parent link="base_link" />
-    <child link="link1" />
-    <origin xyz="0 0 0.494600" rpy="0 0 0" />
-    <axis xyz="0 0 1" />
-    <limit lower="-3.14" upper="3.14" effort="10" velocity="1" />
+  <link name="link_6">
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.05"/>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.15 0.15 0.15 1.0"/>
+      </material>
+    </visual>
+  </link>
+  <!-- joints: main serial chain -->
+  <joint name="joint_1" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0 0.2"/>
+    <parent link="base_link"/>
+    <child link="link_1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0" lower="-2.7925" upper="2.7925" velocity="3.1415"/>
   </joint>
-
-  <joint name="joint2" type="revolute">
-    <parent link="link1" />
-    <child link="link2" />
-    <origin xyz="0 0 0.729921" rpy="0 0 0" />
-    <axis xyz="0 1 0" />
-    <limit lower="-3.14" upper="3.14" effort="10" velocity="1" />
+  <joint name="joint_2" type="revolute">
+    <origin rpy="0 0 0" xyz="0.0170450 0 0.494600"/>
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0" lower="-0.5759" upper="2.6529" velocity="3.1415"/>
   </joint>
-
-  <joint name="joint3" type="revolute">
-    <parent link="link2" />
-    <child link="link3" />
-    <origin xyz="0 0 0.098277" rpy="0 0 0" />
-    <axis xyz="0 1 0" />
-    <limit lower="-3.14" upper="3.14" effort="10" velocity="1" />
+  <joint name="joint_3" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0 0.729921"/>
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="0" lower="-2.6145" upper="2.8797" velocity="3.9269"/>
   </joint>
-
-  <joint name="joint4" type="revolute">
-    <parent link="link3" />
-    <child link="link4" />
-    <origin xyz="0 0 1.109039" rpy="0 0 0" />
-    <axis xyz="0 1 0" />
-    <limit lower="-3.14" upper="3.14" effort="10" velocity="1" />
+  <joint name="joint_4" type="revolute">
+    <origin rpy="0 0 0" xyz="0 0 0.098277"/>
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="0" lower="-3.3161" upper="3.3161" velocity="6.9813"/>
   </joint>
-
-  <joint name="end_effector_joint" type="revolute">
-    <parent link="link4" />
-    <child link="end_effector" />
-    <origin xyz="0 0 0.0170450" rpy="0 0 0" />
-    <axis xyz="0 1 0" />
-    <limit lower="-3.14" upper="3.14" effort="10" velocity="1" />
+  <joint name="joint_5" type="revolute">
+    <origin rpy="0 0 0" xyz="0.0 0 1.109039"/>
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="0" lower="-2.0943" upper="2.0943" velocity="5.7595"/>
   </joint>
-
-</robot>
\ No newline at end of file
+  <joint name="joint_6" type="revolute">
+    <origin rpy="0 0 0" xyz="0.0 0 0.117"/>
+    <parent link="link_5"/>
+    <child link="link_6"/>
+    <axis xyz="-1 0 0"/>
+    <limit effort="0" lower="-6.2831" upper="6.2831" velocity="8.3775"/>
+  </joint>
+  <!-- ROS base_link to Fanuc World Coordinates transform -->
+  <link name="base"/>
+  <joint name="base_link-base" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.350"/>
+    <parent link="base_link"/>
+    <child link="base"/>
+  </joint>
+  <!-- ROS-Industrial 'flange' frame: attachment point for EEF models -->
+  <link name="flange"/>
+  <joint name="joint_6-flange" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="link_6"/>
+    <child link="flange"/>
+  </joint>
+  <!-- ROS-Industrial 'tool0' frame: all-zeros tool frame -->
+  <link name="tool0"/>
+  <joint name="link_6-tool0" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="link_6"/>
+    <child link="tool0"/>
+  </joint>
+</robot>
diff --git a/urdf_convert.py b/urdf_convert.py
new file mode 100644
index 0000000..cbd0e45
--- /dev/null
+++ b/urdf_convert.py
@@ -0,0 +1,4 @@
+from xacrodoc import XacroDoc
+doc = XacroDoc.from_file(path="fanuc_m16ib_support/urdf/fanucM16ib.urdf")
+urdf_str = doc.to_urdf_string()
+doc.to_urdf_file("urdf/fanucM16ib.urdf")
\ No newline at end of file