diff --git a/.clang-format b/.clang-format
new file mode 100644
index 00000000..8d8dc719
--- /dev/null
+++ b/.clang-format
@@ -0,0 +1,9 @@
+BasedOnStyle: LLVM
+IndentWidth: 4
+AccessModifierOffset: -4
+CommentPragmas: 'NOLINT'
+PackConstructorInitializers: CurrentLine
+AlignOperands: AlignAfterOperator
+AlignTrailingComments: true
+ColumnLimit: 0
+SeparateDefinitionBlocks: Always
\ No newline at end of file
diff --git a/.clang-tidy b/.clang-tidy
new file mode 100644
index 00000000..e9f4e763
--- /dev/null
+++ b/.clang-tidy
@@ -0,0 +1,10 @@
+Checks: "bugprone-*,performance-*,readability-*,modernize-*,hicpp-*,cppcoreguidelines-*,clang-analyzer-*,-readability-braces-around-statements,-hicpp-braces-around-statements,-cppcoreguidelines-non-private-member-variables-in-classes"
+WarningsAsErrors:    ''
+HeaderFilterRegex:   ''
+CheckOptions:
+  - key: readability-magic-numbers.IgnoredIntegerValues
+    value: '1;'
+  - key: hicpp-signed-bitwise.IgnorePositiveIntegerLiterals
+    value: 'true'
+  - key: readability-function-cognitive-complexity.IgnoreMacros
+    value: 'true'
\ No newline at end of file
diff --git a/.clangd b/.clangd
new file mode 100644
index 00000000..538f880e
--- /dev/null
+++ b/.clangd
@@ -0,0 +1,2 @@
+Diagnostics:
+    UnusedIncludes: Strict
\ No newline at end of file
diff --git a/.gitignore b/.gitignore
index b26d66fe..76a69cc1 100644
--- a/.gitignore
+++ b/.gitignore
@@ -9,6 +9,7 @@ devel/
 /.vscode/*
 # except recommended extensions
 !/.vscode/extensions.json
+!/.vscode/settings.json
 
 # fetched external libs
 # NOTE: these may be removed in the future, tbd
@@ -20,6 +21,11 @@ src/wroboclaw
 src/wrosout
 src/wresponsecontrol
 
+# external lib destination dirs
+src/drive/lib
+src/manager/lib
+src/science/lib
+
 *.pyc
 src/arm/lib
 
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 00000000..772e1688
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,93 @@
+{
+    "clang-tidy.checks": [
+        "bugprone-*",
+        "performance-*",
+        "readability-*",
+        "modernize-*",
+        "hicpp-*",
+        "cppcoreguidelines-*",
+        "clang-analyzer-*",
+        "-readability-braces-around-statements",
+        "-hicpp-braces-around-statements",
+        "-cppcoreguidelines-non-private-member-variables-in-classes"
+    ],
+    "files.associations": {
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "strstream": "cpp",
+        "bit": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "chrono": "cpp",
+        "complex": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "forward_list": "cpp",
+        "list": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "string": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "hash_map": "cpp",
+        "hash_set": "cpp",
+        "fstream": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "ostream": "cpp",
+        "shared_mutex": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cfenv": "cpp",
+        "cinttypes": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "variant": "cpp"
+    },
+    "python.autoComplete.extraPaths": [
+        "/home/nomorenickels/Documents/WRover_Software/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/nomorenickels/Documents/WRover_Software/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "cmake.sourceDirectory": "${workspaceFolder}/src"
+}
\ No newline at end of file
diff --git a/assemble.py b/assemble.py
index 3ae45fe1..744ca328 100755
--- a/assemble.py
+++ b/assemble.py
@@ -165,7 +165,7 @@ def __init__(self, pkg_name):
         self.pkg_name = pkg_name
 
     def execute(self, root_dir: str, dep_dir: str, args: List[str]):
-        if not BuildApt.install_list:
+        if not hasattr(BuildApt,'install_list'):
             print_subsec('Caching dpkg install list...')
             BuildApt.install_list = set()
             with open('/var/lib/dpkg/status') as dpkg_status:
diff --git a/project.json b/project.json
index cdc081da..0bbb2578 100644
--- a/project.json
+++ b/project.json
@@ -27,6 +27,20 @@
       "build": {
         "provider": "noop"
       }
+    },
+    {
+      "name": "moveit",
+      "build": {
+        "package": "ros-noetic-moveit",
+        "provider": "apt"
+      }
+    },
+    {
+      "name": "moveit-visual-tools",
+      "build": {
+        "package": "ros-noetic-moveit-visual-tools",
+        "provider": "apt"
+      }
     }
   ]
 }
diff --git a/reconfig_arm_urdf.sh b/reconfig_arm_urdf.sh
new file mode 100755
index 00000000..1ab412c9
--- /dev/null
+++ b/reconfig_arm_urdf.sh
@@ -0,0 +1,2 @@
+#!/bin/bash
+gnome-terminal -- roslaunch wroboarm_21 demo_test.launch
\ No newline at end of file
diff --git a/setup.zsh b/setup.zsh
new file mode 100755
index 00000000..fba8c5a9
--- /dev/null
+++ b/setup.zsh
@@ -0,0 +1,3 @@
+ROOT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
+. $ROOT_DIR/venv/bin/activate
+. $ROOT_DIR/devel/setup.zsh
diff --git a/src/wr_control_drive_arm/CMakeLists.txt b/src/wr_control_drive_arm/CMakeLists.txt
new file mode 100644
index 00000000..a122dafe
--- /dev/null
+++ b/src/wr_control_drive_arm/CMakeLists.txt
@@ -0,0 +1,227 @@
+cmake_minimum_required(VERSION 3.8)
+project(wr_control_drive_arm)
+
+# # Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++17)
+
+# # 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
+  actionlib
+  actionlib_msgs
+  sensor_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 exec_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 exec_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  # Or other packages containing 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 exec_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 your 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 wr_control_drive_science
+  # CATKIN_DEPENDS roscpp rospy
+  # DEPENDS system_lib
+)
+
+# ##########
+# # Build ##
+# ##########
+
+# # Specify additional locations of header files
+# # Your package locations should be listed before other locations
+include_directories(
+
+  # include
+  ${catkin_INCLUDE_DIRS}
+)
+
+# # Declare a C++ library
+add_library(${PROJECT_NAME}
+  src/DifferentialJointToMotorSpeedConverter.cpp
+  src/DirectJointToMotorSpeedConverter.cpp
+  src/Joint.cpp
+  src/MathUtil.cpp
+  src/Motor.cpp
+  src/SingleEncoderJointPositionMonitor.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(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+# # Declare a C++ executable
+# # With catkin_make all packages are built within a single CMake context
+# # The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/wr_control_drive_arm_node.cpp)
+add_executable(ArmControlActionServer src/ArmControlActionServer.cpp)
+target_link_libraries(ArmControlActionServer ${catkin_LIBRARIES} ${PROJECT_NAME})
+add_executable(JointStatePublisher src/JointStatePublisher.cpp)
+target_link_libraries(JointStatePublisher ${catkin_LIBRARIES} ${PROJECT_NAME})
+add_executable(testDifferential src/testDifferential.cpp)
+target_link_libraries(testDifferential ${catkin_LIBRARIES} ${PROJECT_NAME})
+
+set_property(TARGET ArmControlActionServer PROPERTY CXX_STANDARD 17)
+set_property(TARGET JointStatePublisher PROPERTY CXX_STANDARD 17)
+set_property(TARGET ArmControlActionServer PROPERTY CXX_STANDARD_REQUIRED true)
+set_property(TARGET JointStatePublisher PROPERTY CXX_STANDARD_REQUIRED true)
+set_property(TARGET testDifferential PROPERTY CXX_STANDARD 17)
+set_property(TARGET testDifferential PROPERTY CXX_STANDARD_REQUIRED true)
+
+# # Rename C++ executable without prefix
+# # The above recommended prefix causes long target names, the following renames the
+# # target back to the shorter version for ease of user use
+# # e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+# # Add cmake target dependencies of the executable
+# # same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+# # Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_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
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+# # Mark executables for installation
+# # See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+# # Mark libraries for installation
+# # See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_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}
+# )
+
+# ############
+# # Testing ##
+# ############
+
+# # Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_wr_control_drive_science.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/src/wr_control_drive_arm/config/arm_motor_PID_mock.yaml b/src/wr_control_drive_arm/config/arm_motor_PID_mock.yaml
new file mode 100644
index 00000000..e59fd363
--- /dev/null
+++ b/src/wr_control_drive_arm/config/arm_motor_PID_mock.yaml
@@ -0,0 +1,55 @@
+rate: 50
+controllers:
+  - setpointTopic: "/control/arm/pid/turntable/setpoint"
+    feedbackTopic: "/control/arm/pid/turntable/feedback"
+    outputTopic: "/control/arm/pid/turntable/output"
+    P: 5
+    I: 0.5
+    D: 0
+    min: -1
+    max: 1
+
+  - setpointTopic: "/control/arm/pid/shoulder/setpoint"
+    feedbackTopic: "/control/arm/pid/shoulder/feedback"
+    outputTopic: "/control/arm/pid/shoulder/output"
+    P: 5
+    I: 0.5
+    D: 0
+    min: -1
+    max: 1
+
+  - setpointTopic: "/control/arm/pid/elbow/setpoint"
+    feedbackTopic: "/control/arm/pid/elbow/feedback"
+    outputTopic: "/control/arm/pid/elbow/output"
+    P: 5
+    I: 0.5
+    D: 0
+    min: -1
+    max: 1
+
+  - setpointTopic: "/control/arm/pid/forearmRoll/setpoint"
+    feedbackTopic: "/control/arm/pid/forearmRoll/feedback"
+    outputTopic: "/control/arm/pid/forearmRoll/output"
+    P: 5
+    I: 0.5
+    D: 0
+    min: -1
+    max: 1
+
+  - setpointTopic: "/control/arm/pid/wristPitch/setpoint"
+    feedbackTopic: "/control/arm/pid/wristPitch/feedback"
+    outputTopic: "/control/arm/pid/wristPitch/output"
+    P: 5
+    I: 0.5
+    D: 0
+    min: -1
+    max: 1
+
+  - setpointTopic: "/control/arm/pid/wristRoll/setpoint"
+    feedbackTopic: "/control/arm/pid/wristRoll/feedback"
+    outputTopic: "/control/arm/pid/wristRoll/output"
+    P: 5
+    I: 0.5
+    D: 0
+    min: -1
+    max: 1
diff --git a/src/wr_control_drive_arm/config/arm_motor_PID_real.yaml b/src/wr_control_drive_arm/config/arm_motor_PID_real.yaml
new file mode 100644
index 00000000..a0da7801
--- /dev/null
+++ b/src/wr_control_drive_arm/config/arm_motor_PID_real.yaml
@@ -0,0 +1,55 @@
+rate: 50
+controllers:
+  - setpointTopic: "/control/arm/pid/turntable/setpoint"
+    feedbackTopic: "/control/arm/pid/turntable/feedback"
+    outputTopic: "/control/arm/pid/turntable/output"
+    P: 10
+    I: 0
+    D: 0
+    min: -1
+    max: 1
+
+  - setpointTopic: "/control/arm/pid/shoulder/setpoint"
+    feedbackTopic: "/control/arm/pid/shoulder/feedback"
+    outputTopic: "/control/arm/pid/shoulder/output"
+    P: 10
+    I: 0
+    D: 0
+    min: -1
+    max: 1
+
+  - setpointTopic: "/control/arm/pid/elbow/setpoint"
+    feedbackTopic: "/control/arm/pid/elbow/feedback"
+    outputTopic: "/control/arm/pid/elbow/output"
+    P: 10
+    I: 0
+    D: 0
+    min: -1
+    max: 1
+
+  - setpointTopic: "/control/arm/pid/forearmRoll/setpoint"
+    feedbackTopic: "/control/arm/pid/forearmRoll/feedback"
+    outputTopic: "/control/arm/pid/forearmRoll/output"
+    P: 10
+    I: 0
+    D: 0
+    min: -1
+    max: 1
+
+  - setpointTopic: "/control/arm/pid/wristPitch/setpoint"
+    feedbackTopic: "/control/arm/pid/wristPitch/feedback"
+    outputTopic: "/control/arm/pid/wristPitch/output"
+    P: 10
+    I: 0
+    D: 0
+    min: -1
+    max: 1
+
+  - setpointTopic: "/control/arm/pid/wristRoll/setpoint"
+    feedbackTopic: "/control/arm/pid/wristRoll/feedback"
+    outputTopic: "/control/arm/pid/wristRoll/output"
+    P: 10
+    I: 0
+    D: 0
+    min: -1
+    max: 1
diff --git a/src/wr_control_drive_arm/config/arm_parameters_mock.yaml b/src/wr_control_drive_arm/config/arm_parameters_mock.yaml
new file mode 100644
index 00000000..a8b1e10d
--- /dev/null
+++ b/src/wr_control_drive_arm/config/arm_parameters_mock.yaml
@@ -0,0 +1,37 @@
+arm_parameters: 
+  elbow:
+    encoder_parameters:
+      counts_per_rotation: 1000
+      offset: 500
+    motor_parameters:
+      gear_ratio: 1
+  forearmRoll:
+    encoder_parameters:
+      counts_per_rotation: 1000
+      offset: 500
+    motor_parameters:
+      gear_ratio: 1
+  shoulder:
+    encoder_parameters:
+      counts_per_rotation: 1000
+      offset: 500
+    motor_parameters:
+      gear_ratio: 1
+  turntable:
+    encoder_parameters:
+      counts_per_rotation: 1000
+      offset: 500
+    motor_parameters:
+      gear_ratio: 1
+  wristPitch:
+    encoder_parameters:
+      counts_per_rotation: 1000
+      offset: 500
+    motor_parameters:
+      gear_ratio: 1
+  wristRoll:
+    encoder_parameters:
+      counts_per_rotation: 1000
+      offset: 500
+    motor_parameters:
+      gear_ratio: 1
diff --git a/src/wr_control_drive_arm/config/arm_parameters_real.yaml b/src/wr_control_drive_arm/config/arm_parameters_real.yaml
new file mode 100644
index 00000000..511c0c23
--- /dev/null
+++ b/src/wr_control_drive_arm/config/arm_parameters_real.yaml
@@ -0,0 +1,37 @@
+arm_parameters: 
+  elbow:
+    encoder_parameters:
+      counts_per_rotation: 1850
+      offset: 546
+    motor_parameters:
+      gear_ratio: 3
+  forearmRoll:
+    encoder_parameters:
+      counts_per_rotation: -1850
+      offset: 576
+    motor_parameters:
+      gear_ratio: 3
+  shoulder:
+    encoder_parameters:
+      counts_per_rotation: 5550
+      offset: 1281
+    motor_parameters:
+      gear_ratio: 9
+  turntable:
+    encoder_parameters:
+      counts_per_rotation: 6900 #7200
+      offset: 90
+    motor_parameters:
+      gear_ratio: 3
+  wristPitch:
+    encoder_parameters:
+      counts_per_rotation: -1850
+      offset: 886
+    motor_parameters:
+      gear_ratio: 4.875
+  wristRoll:
+    encoder_parameters:
+      counts_per_rotation: -1850
+      offset: 981
+    motor_parameters:
+      gear_ratio: 4.875
diff --git a/src/wr_control_drive_arm/launch/actionserver.launch b/src/wr_control_drive_arm/launch/actionserver.launch
new file mode 100644
index 00000000..a1c5c9e0
--- /dev/null
+++ b/src/wr_control_drive_arm/launch/actionserver.launch
@@ -0,0 +1,21 @@
+<!--Launches the action server and PID nodes-->
+<launch>
+    <group ns="control">
+        <node name="WResponseControl_ArmMotors" pkg="wresponsecontrol" type="WResponseControl">
+            <rosparam file="$(find wr_control_drive_arm)/config/arm_motor_PID_real.yaml" />
+        </node>
+        <node name="ArmControlActionServer" pkg="wr_control_drive_arm"
+            type="ArmControlActionServer"
+            output="screen">
+            <rosparam
+                file="$(find wr_control_drive_arm)/config/arm_parameters_real.yaml"
+                if="$(eval
+        env('WROVER_HW') == 'REAL')" />
+            <rosparam
+                file="$(find
+        wr_control_drive_arm)/config/arm_parameters_mock.yaml"
+                if="$(eval
+        env('WROVER_HW') == 'MOCK')" />
+        </node>
+    </group>
+</launch>
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/launch/jointstatepublisher.launch b/src/wr_control_drive_arm/launch/jointstatepublisher.launch
new file mode 100644
index 00000000..9b662856
--- /dev/null
+++ b/src/wr_control_drive_arm/launch/jointstatepublisher.launch
@@ -0,0 +1,12 @@
+<!--Launches the joint state publisher node-->
+<launch>
+    <group ns="control">
+        <node name="JointStatePublisher" pkg="wr_control_drive_arm" type="JointStatePublisher"
+            output="screen">
+            <rosparam file="$(find wr_control_drive_arm)/config/arm_parameters_real.yaml"
+                if="$(eval env('WROVER_HW') == 'REAL')" />
+            <rosparam file="$(find wr_control_drive_arm)/config/arm_parameters_mock.yaml"
+                if="$(eval env('WROVER_HW') == 'MOCK')" />
+        </node>
+    </group>
+</launch>
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/launch/std.launch b/src/wr_control_drive_arm/launch/std.launch
new file mode 100644
index 00000000..96c2e726
--- /dev/null
+++ b/src/wr_control_drive_arm/launch/std.launch
@@ -0,0 +1,5 @@
+<!--Launches the action server and the joint state publisher-->
+<launch>
+    <include file="$(find wr_control_drive_arm)/launch/actionserver.launch" />
+    <include file="$(find wr_control_drive_arm)/launch/jointstatepublisher.launch" />
+</launch>
diff --git a/src/wr_control_drive_arm/launch/testDifferential.launch b/src/wr_control_drive_arm/launch/testDifferential.launch
new file mode 100644
index 00000000..b6632b1a
--- /dev/null
+++ b/src/wr_control_drive_arm/launch/testDifferential.launch
@@ -0,0 +1,3 @@
+<launch>
+    <node pkg="wr_control_drive_arm" name="testDifferential" type="testDifferential" output="screen"/>
+</launch>
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/package.xml b/src/wr_control_drive_arm/package.xml
new file mode 100644
index 00000000..9b0ad0c8
--- /dev/null
+++ b/src/wr_control_drive_arm/package.xml
@@ -0,0 +1,67 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>wr_control_drive_arm</name>
+  <version>0.0.0</version>
+  <description>Control systems subsystem package that drives the arm subsystem.</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="egeng@wisc.edu">E. Geng</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>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/wr_control_drive_science</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+    <exec_depend>message_runtime</exec_depend> 
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <depend>actionlib</depend>
+  <depend>actionlib_msgs</depend>
+  <depend>sensor_msgs</depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp
new file mode 100644
index 00000000..7b3f6ae9
--- /dev/null
+++ b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp
@@ -0,0 +1,361 @@
+/**
+ * @file ArmControlActionServer.cpp
+ * @author Ben Nowotny
+ * @brief The exeutable file to run the Arm Control Action Server
+ * @date 2022-12-05
+ */
+#include "DifferentialJointToMotorSpeedConverter.hpp"
+#include "DirectJointToMotorSpeedConverter.hpp"
+#include "Joint.hpp"
+#include "Motor.hpp"
+#include "RoboclawChannel.hpp"
+#include "SingleEncoderJointPositionMonitor.hpp"
+#include "XmlRpcValue.h"
+#include "ros/init.h"
+
+#include <actionlib/server/simple_action_server.h>
+#include <algorithm>
+#include <array>
+#include <chrono>
+#include <cmath>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <csignal>
+#include <memory>
+#include <ros/ros.h>
+#include <sensor_msgs/JointState.h>
+#include <std_msgs/Empty.h>
+#include <std_msgs/Float64.h>
+#include <std_srvs/Trigger.h>
+#include <string>
+#include <thread>
+#include <unordered_map>
+#include <vector>
+
+using XmlRpc::XmlRpcValue;
+
+/**
+ * @brief Refresh rate of ros::Rate
+ */
+constexpr float CLOCK_RATE{50};
+
+constexpr double IK_WARN_RATE{1.0 / 2};
+
+constexpr double JOINT_SAFETY_MAX_SPEED{0.5};
+constexpr double JOINT_SAFETY_HOLD_SPEED{0.3};
+
+/**
+ * @brief Nessage cache size of publisher
+ */
+constexpr std::uint32_t MESSAGE_CACHE_SIZE{10};
+
+/**
+ * @brief Period between timer callback
+ */
+constexpr float TIMER_CALLBACK_DURATION{1.0 / 50.0};
+
+/**
+ * @brief Defines space for all Joint references
+ */
+std::unordered_map<std::string, std::unique_ptr<Joint>> namedJointMap;
+
+/**
+ * @brief Defines space for all joint position monitor references
+ */
+std::unordered_map<std::string, std::shared_ptr<SingleEncoderJointPositionMonitor>> namedPositionMonitors;
+
+/**
+ * @brief Simplify the SimpleActionServer reference name
+ */
+using Server = actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>;
+
+/**
+ * @brief The service server for enabling IK
+ */
+ros::ServiceServer enableServiceServer;
+/**
+ * @brief The status of IK program
+ */
+std::atomic_bool IKEnabled{true};
+/**
+ * @brief The service client for disabling IK
+ */
+ros::ServiceClient enableServiceClient;
+
+/**
+ * @brief Perform the given action as interpreted as moving the arm joints to
+ * specified positions
+ *
+ * @param goal The goal state given
+ * @param as The Action Server this is occuring on
+ */
+void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal,
+             Server *server) {
+    if (!IKEnabled) {
+        server->setAborted();
+        ROS_WARN_THROTTLE( // NOLINT(hicpp-no-array-decay,hicpp,hicpp-vararg,cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg)
+            IK_WARN_RATE,
+            "IK is disabled");
+        return;
+    }
+
+    for (const auto &jointName : goal->trajectory.joint_names) {
+        std::cout << jointName << "\t";
+    }
+    std::cout << std::endl;
+    for (const auto &waypoint : goal->trajectory.points) {
+        for (const auto &jointVal : waypoint.positions) {
+            std::cout << jointVal << "\t";
+        }
+        std::cout << std::endl;
+    }
+
+    for (const auto &currTargetPosition : goal->trajectory.points) {
+
+        if (!IKEnabled) {
+            server->setAborted();
+            std::cout << "Over current fault!" << std::endl;
+            return;
+        }
+
+        // Copy of the velocities received from MoveIt
+        std::vector<double> velocityCopies{currTargetPosition.velocities};
+
+        for (uint32_t i = 0; i < goal->trajectory.joint_names.size(); ++i) {
+
+            // The joint that is currently being scaled
+            auto currJoint = goal->trajectory.joint_names.at(i);
+
+            // The position monitor whose velocity is currently being scaled
+            auto& currPosMtr = *namedPositionMonitors.at(currJoint);
+
+            // Scale by counts per rotation and gear ratio
+            velocityCopies.at(i) *= abs(currPosMtr.getCountsPerRotation()*currPosMtr.getGearRatio());
+        }
+
+        // Get the maximum velocity assigned to any joint
+        const double VELOCITY_MAX = abs(*std::max_element(
+            velocityCopies.begin(),
+            velocityCopies.end(),
+            [](double lhs, double rhs) -> bool { return abs(lhs) < abs(rhs); }));
+
+        for (uint32_t i = 0; i < goal->trajectory.joint_names.size(); ++i) {
+            // Set joint to hold speed in case the greatest velocity comes through as 0
+            auto jointVelocity{JOINT_SAFETY_HOLD_SPEED};
+
+            // Scale all velocities by the safety max speed with respect to the maximum velocity given by MoveIt
+            if (VELOCITY_MAX != 0)
+                jointVelocity = velocityCopies.at(i) / VELOCITY_MAX * JOINT_SAFETY_MAX_SPEED;
+
+            // Set the joint's velocity
+            namedJointMap.at(goal->trajectory.joint_names.at(i))->setTarget(currTargetPosition.positions.at(i), jointVelocity);
+        }
+    }
+
+    auto waypointComplete{false};
+    ros::Rate updateRate{CLOCK_RATE};
+
+    while (!waypointComplete && ros::ok() && !server->isNewGoalAvailable()) {
+
+        if (!IKEnabled) {
+            server->setAborted();
+            std::cout << "Over current fault!" << std::endl;
+            return;
+        }
+
+        waypointComplete = true;
+        for (const auto &[_, joint] : namedJointMap) {
+            waypointComplete &= joint->hasReachedTarget();
+        }
+        updateRate.sleep();
+    }
+
+    // Report preemption if it occurred
+    if (server->isNewGoalAvailable())
+        server->setPreempted();
+    // When all positions have been reached, set the current task as succeeded
+    else
+        server->setSucceeded();
+    std::cout << "Action Complete!" << std::endl;
+}
+
+auto getEncoderConfigFromParams(const XmlRpcValue &params, const std::string &jointName) -> EncoderConfiguration {
+    return {.countsPerRotation = static_cast<int32_t>(params[jointName]["encoder_parameters"]["counts_per_rotation"]),
+            .offset = static_cast<int32_t>(params[jointName]["encoder_parameters"]["offset"])};
+}
+
+auto getMotorConfigFromParams(const XmlRpcValue &params, const std::string &jointName) -> MotorConfiguration {
+    return {.gearRatio = static_cast<double>(params[jointName]["motor_configurations"]["gear_ratio"])};
+}
+
+void checkOverCurrentFaults(const std::vector<std::shared_ptr<Motor>> &motors){
+    for(const auto& motor : motors){
+        if (motor->isOverCurrent()) {
+            IKEnabled = false;
+            std::cout << "Over current fault!" << std::endl;
+
+            for (const auto& joint : namedJointMap) {
+                joint.second->stop();
+            }
+        }
+        // TODO:  arbitrate control to old arm driver
+    }
+}
+
+/**
+ * @brief The main executable method of the node.  Starts the ROS node and the
+ * Action Server for processing Arm Control commands
+ *
+ * @param argc The number of program arguments
+ * @param argv The given program arguments
+ * @return int The status code on exiting the program
+ */
+auto main(int argc, char **argv) -> int {
+    std::cout << "start main" << std::endl;
+    // Initialize the current node as ArmControlSystem
+    ros::init(argc, argv, "ArmControlActionServer");
+    // Create the NodeHandle to the current ROS node
+    ros::NodeHandle n;
+    ros::NodeHandle pn{"~"};
+
+    XmlRpcValue armParams;
+    pn.getParam("arm_parameters", armParams);
+
+    // Initialize all motors with their MoveIt name, WRoboclaw initialization,
+    // and reference to the current node
+    std::cout << "init motors" << std::endl;
+
+    /**
+     * @brief The list of motors
+     */
+    std::vector<std::shared_ptr<Motor>> motors{};
+
+    using std::literals::string_literals::operator""s;
+
+    const auto turntableMotor{std::make_shared<Motor>("aux0"s, RoboclawChannel::A, n)};
+    const auto shoulderMotor{std::make_shared<Motor>("aux0"s, RoboclawChannel::B, n)};
+    const auto elbowMotor{std::make_shared<Motor>("aux1"s, RoboclawChannel::A, n)};
+    const auto forearmRollMotor{std::make_shared<Motor>("aux1"s, RoboclawChannel::B, n)};
+    const auto wristLeftMotor{std::make_shared<Motor>("aux2"s, RoboclawChannel::A, n)};
+    const auto wristRightMotor{std::make_shared<Motor>("aux2"s, RoboclawChannel::B, n)};
+
+    motors.push_back(turntableMotor);
+    motors.push_back(shoulderMotor);
+    motors.push_back(elbowMotor);
+    motors.push_back(forearmRollMotor);
+    motors.push_back(wristLeftMotor);
+    motors.push_back(wristRightMotor);
+
+    namedPositionMonitors.insert({"turntable_joint", std::make_shared<SingleEncoderJointPositionMonitor>(
+                                                            "aux0"s,
+                                                            RoboclawChannel::A,
+                                                            getEncoderConfigFromParams(armParams, "turntable"),
+                                                            getMotorConfigFromParams(armParams, "turntable"),
+                                                            n)});
+
+    namedPositionMonitors.insert({"shoulder_joint", std::make_shared<SingleEncoderJointPositionMonitor>(
+                                                            "aux0"s,
+                                                            RoboclawChannel::B,
+                                                            getEncoderConfigFromParams(armParams, "shoulder"),
+                                                            getMotorConfigFromParams(armParams, "shoulder"),
+                                                            n)});
+
+    namedPositionMonitors.insert({"elbowPitch_joint", std::make_shared<SingleEncoderJointPositionMonitor>(
+                                                            "aux1"s,
+                                                            RoboclawChannel::A,
+                                                            getEncoderConfigFromParams(armParams, "elbow"),
+                                                            getMotorConfigFromParams(armParams, "elbow"),
+                                                            n)});
+
+    namedPositionMonitors.insert({"elbowRoll_joint", std::make_shared<SingleEncoderJointPositionMonitor>(
+                                                            "aux1"s,
+                                                            RoboclawChannel::B,
+                                                            getEncoderConfigFromParams(armParams, "forearmRoll"),
+                                                            getMotorConfigFromParams(armParams, "forearmRoll"),
+                                                            n)});
+
+    namedPositionMonitors.insert({"wristPitch_joint", std::make_shared<SingleEncoderJointPositionMonitor>(
+                                                            "aux2"s,
+                                                            RoboclawChannel::A,
+                                                            getEncoderConfigFromParams(armParams, "wristPitch"),
+                                                            getMotorConfigFromParams(armParams, "wristPitch"),
+                                                            n)});
+
+    namedPositionMonitors.insert({"wristRoll_link", std::make_shared<SingleEncoderJointPositionMonitor>(
+                                                            "aux2"s,
+                                                            RoboclawChannel::B,
+                                                            getEncoderConfigFromParams(armParams, "wristRoll"),
+                                                            getMotorConfigFromParams(armParams, "wristRoll"),
+                                                            n)});
+
+    const auto turntableSpeedConverter{std::make_shared<DirectJointToMotorSpeedConverter>(turntableMotor, MotorSpeedDirection::REVERSE)};
+    const auto shoulderSpeedConverter{std::make_shared<DirectJointToMotorSpeedConverter>(shoulderMotor, MotorSpeedDirection::REVERSE)};
+    const auto elbowSpeedConverter{std::make_shared<DirectJointToMotorSpeedConverter>(elbowMotor, MotorSpeedDirection::REVERSE)};
+    const auto forearmRollSpeedConverter{std::make_shared<DirectJointToMotorSpeedConverter>(forearmRollMotor, MotorSpeedDirection::REVERSE)};
+    const auto differentialSpeedConverter{std::make_shared<DifferentialJointToMotorSpeedConverter>(wristLeftMotor, wristRightMotor)};
+
+    // Initialize all Joints
+
+    std::cout << "init joints" << std::endl;
+
+    namedJointMap.insert({"turntable_joint", std::make_unique<Joint>(
+                                                 "turntable"s,
+                                                 [turntablePositionMonitor=namedPositionMonitors.at("turntable_joint")]() -> double { return (*turntablePositionMonitor)(); },
+                                                 [turntableSpeedConverter](double speed) { (*turntableSpeedConverter)(speed); },
+                                                 n)});
+    namedJointMap.insert({"shoulder_joint", std::make_unique<Joint>(
+                                                "shoulder",
+                                                [shoulderPositionMonitor=namedPositionMonitors.at("shoulder_joint")]() -> double { return (*shoulderPositionMonitor)(); },
+                                                [shoulderSpeedConverter](double speed) { (*shoulderSpeedConverter)(speed); },
+                                                n)});
+    namedJointMap.insert({"elbowPitch_joint", std::make_unique<Joint>(
+                                                  "elbow",
+                                                  [elbowPositionMonitor=namedPositionMonitors.at("elbowPitch_joint")]() -> double { return (*elbowPositionMonitor)(); },
+                                                  [elbowSpeedConverter](double speed) { (*elbowSpeedConverter)(speed); },
+                                                  n)});
+    namedJointMap.insert({"elbowRoll_joint", std::make_unique<Joint>(
+                                                 "forearmRoll",
+                                                 [forearmRollPositionMonitor=namedPositionMonitors.at("elbowRoll_joint")]() -> double { return (*forearmRollPositionMonitor)(); },
+                                                 [forearmRollSpeedConverter](double speed) { (*forearmRollSpeedConverter)(speed); },
+                                                 n)});
+    namedJointMap.insert({"wristPitch_joint", std::make_unique<Joint>(
+                                                  "wristPitch",
+                                                  [wristPitchPositionMonitor=namedPositionMonitors.at("wristPitch_joint")]() -> double { return (*wristPitchPositionMonitor)(); },
+                                                  [converter = differentialSpeedConverter](double speed) { converter->setPitchSpeed(speed); },
+                                                  n)});
+    namedJointMap.insert({"wristRoll_link", std::make_unique<Joint>(
+                                                "wristRoll",
+                                                [wristRollPositionMonitor=namedPositionMonitors.at("wristRoll_link")]() -> double { return (*wristRollPositionMonitor)(); },
+                                                [converter = differentialSpeedConverter](double speed) { converter->setRollSpeed(speed); },
+                                                n)});
+
+    // Initialize the Action Server
+    Server server(
+        n, "/arm_controller/follow_joint_trajectory",
+        [&server](auto goal) { execute(goal, &server); }, false);
+    // Start the Action Server
+    server.start();
+    std::cout << "server started" << std::endl;
+
+    enableServiceServer = n.advertiseService(
+        "start_IK",
+        static_cast<boost::function<bool(std_srvs::Trigger::Request &,
+                                         std_srvs::Trigger::Response &)>>(
+            [](std_srvs::Trigger::Request &req,
+               std_srvs::Trigger::Response &res) -> bool {
+                IKEnabled = true;
+                res.message = "Arm IK Enabled";
+                res.success = static_cast<unsigned char>(true);
+                return true;
+            }));
+
+    enableServiceClient =
+        n.serviceClient<std_srvs::Trigger>("PLACEHOLDER_NAME");
+
+    ros::Timer currentTimer = n.createTimer(ros::Duration{TIMER_CALLBACK_DURATION}, [&motors](const ros::TimerEvent& event) { checkOverCurrentFaults(motors); });
+
+    std::cout << "entering ROS spin..." << std::endl;
+    // ROS spin for communication with other nodes
+    ros::spin();
+    // Return 0 on exit (successful exit)
+    return 0;
+}
diff --git a/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp
new file mode 100644
index 00000000..dfe28450
--- /dev/null
+++ b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.cpp
@@ -0,0 +1,43 @@
+#include "DifferentialJointToMotorSpeedConverter.hpp"
+#include <mutex>
+
+DifferentialJointToMotorSpeedConverter::DifferentialJointToMotorSpeedConverter(
+    std::shared_ptr<Motor> leftMotor,
+    std::shared_ptr<Motor> rightMotor)
+    : cachedPitchSpeed{0},
+      cachedRollSpeed{0},
+      leftMotor{std::move(leftMotor)},
+      rightMotor{std::move(rightMotor)} {}
+
+DifferentialJointToMotorSpeedConverter::DifferentialJointToMotorSpeedConverter(
+    const DifferentialJointToMotorSpeedConverter &other)
+    : cachedPitchSpeed{other.cachedPitchSpeed.load()},
+      cachedRollSpeed{other.cachedRollSpeed.load()},
+      leftMotor{other.leftMotor},
+      rightMotor{other.rightMotor} {}
+
+DifferentialJointToMotorSpeedConverter::DifferentialJointToMotorSpeedConverter(
+    DifferentialJointToMotorSpeedConverter &&other) noexcept
+    : cachedPitchSpeed{other.cachedPitchSpeed.load()},
+      cachedRollSpeed{other.cachedRollSpeed.load()},
+      leftMotor{other.leftMotor},
+      rightMotor{other.rightMotor} {}
+
+void DifferentialJointToMotorSpeedConverter::setPitchSpeed(double speed) {
+    cachedPitchSpeed = speed;
+    dispatchDifferentialSpeed();
+}
+
+void DifferentialJointToMotorSpeedConverter::setRollSpeed(double speed) {
+    cachedRollSpeed = speed;
+    dispatchDifferentialSpeed();
+}
+
+void DifferentialJointToMotorSpeedConverter::dispatchDifferentialSpeed() {
+    // const std::lock_guard<std::recursive_mutex> guard{mutex};
+    auto m1Speed{-cachedPitchSpeed + (cachedRollSpeed * AVERAGE_SCALING_FACTOR)};
+    auto m2Speed{-cachedPitchSpeed - (cachedRollSpeed * AVERAGE_SCALING_FACTOR)};
+    leftMotor->setSpeed(m1Speed);
+    rightMotor->setSpeed(m2Speed);
+    // std::cout << "ptch: " << cachedPitchSpeed.load() << "\troll: " << cachedRollSpeed.load() << "\tlspd: " << m1Speed << "\trspd: " << m2Speed << std::endl;
+}
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.hpp b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.hpp
new file mode 100644
index 00000000..5d096add
--- /dev/null
+++ b/src/wr_control_drive_arm/src/DifferentialJointToMotorSpeedConverter.hpp
@@ -0,0 +1,31 @@
+#ifndef DIFFERENTIAL_JOINT_TO_MOTOR_SPEED_CONVERTER_H
+#define DIFFERENTIAL_JOINT_TO_MOTOR_SPEED_CONVERTER_H
+
+#include "Motor.hpp"
+
+class DifferentialJointToMotorSpeedConverter {
+public:
+    DifferentialJointToMotorSpeedConverter(std::shared_ptr<Motor> leftMotor, std::shared_ptr<Motor> rightMotor);
+    void setPitchSpeed(double speed);
+    void setRollSpeed(double speed);
+
+    DifferentialJointToMotorSpeedConverter(const DifferentialJointToMotorSpeedConverter &);
+    auto operator=(const DifferentialJointToMotorSpeedConverter &) -> DifferentialJointToMotorSpeedConverter & = delete;
+    DifferentialJointToMotorSpeedConverter(DifferentialJointToMotorSpeedConverter &&) noexcept;
+    auto operator=(DifferentialJointToMotorSpeedConverter &&) -> DifferentialJointToMotorSpeedConverter & = delete;
+    ~DifferentialJointToMotorSpeedConverter() = default;
+
+private:
+    std::atomic<double> cachedPitchSpeed;
+    std::atomic<double> cachedRollSpeed;
+    std::recursive_mutex mutex;
+
+    const std::shared_ptr<Motor> leftMotor;
+    const std::shared_ptr<Motor> rightMotor;
+
+    static constexpr double AVERAGE_SCALING_FACTOR{1};
+
+    void dispatchDifferentialSpeed();
+};
+
+#endif
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.cpp b/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.cpp
new file mode 100644
index 00000000..67fd0568
--- /dev/null
+++ b/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.cpp
@@ -0,0 +1,18 @@
+#include "DirectJointToMotorSpeedConverter.hpp"
+
+DirectJointToMotorSpeedConverter::DirectJointToMotorSpeedConverter(std::shared_ptr<Motor> outputMotor, MotorSpeedDirection direction)
+    : outputMotor{std::move(outputMotor)},
+      direction{direction} {}
+
+void DirectJointToMotorSpeedConverter::operator()(double speed) {
+    double actualSpeed{0}; // In event of enum error, stop the motor
+    switch (direction) {
+    case MotorSpeedDirection::FORWARD:
+        actualSpeed = speed;
+        break;
+    case MotorSpeedDirection::REVERSE:
+        actualSpeed = -speed;
+        break;
+    }
+    outputMotor->setSpeed(actualSpeed);
+}
diff --git a/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.hpp b/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.hpp
new file mode 100644
index 00000000..6a117435
--- /dev/null
+++ b/src/wr_control_drive_arm/src/DirectJointToMotorSpeedConverter.hpp
@@ -0,0 +1,28 @@
+#ifndef DIRECT_JOINT_TO_MOTOR_SPEED_CONVERTER_H
+#define DIRECT_JOINT_TO_MOTOR_SPEED_CONVERTER_H
+
+#include "Motor.hpp"
+#include <memory>
+
+enum class MotorSpeedDirection {
+    FORWARD,
+    REVERSE
+};
+
+class DirectJointToMotorSpeedConverter {
+public:
+    DirectJointToMotorSpeedConverter(std::shared_ptr<Motor> outputMotor, MotorSpeedDirection direction);
+    void operator()(double speed);
+
+    DirectJointToMotorSpeedConverter(const DirectJointToMotorSpeedConverter &) = default;
+    auto operator=(const DirectJointToMotorSpeedConverter &) -> DirectJointToMotorSpeedConverter & = delete;
+    DirectJointToMotorSpeedConverter(DirectJointToMotorSpeedConverter &&) = default;
+    auto operator=(DirectJointToMotorSpeedConverter &&) -> DirectJointToMotorSpeedConverter & = delete;
+    ~DirectJointToMotorSpeedConverter() = default;
+
+private:
+    const MotorSpeedDirection direction;
+    const std::shared_ptr<Motor> outputMotor;
+};
+
+#endif
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/src/Joint.cpp b/src/wr_control_drive_arm/src/Joint.cpp
new file mode 100644
index 00000000..08b7c8a3
--- /dev/null
+++ b/src/wr_control_drive_arm/src/Joint.cpp
@@ -0,0 +1,72 @@
+#include "Joint.hpp"
+#include "MathUtil.hpp"
+#include <ros/node_handle.h>
+#include <std_msgs/Float64.h>
+
+using std::literals::string_literals::operator""s;
+using MathUtil::RADIANS_PER_ROTATION;
+
+Joint::Joint(std::string name,
+             std::function<double()> positionMonitor,
+             std::function<void(double)> motorSpeedDispatcher,
+             ros::NodeHandle node)
+    : name{std::move(name)},
+      positionMonitor{std::move(positionMonitor)},
+      motorSpeedDispatcher{std::move(motorSpeedDispatcher)},
+      executeMotion{false},
+      controlLoopUpdateTimer{node.createTimer(ros::Rate{FEEDBACK_UPDATE_FREQUENCY_HZ}, &Joint::onFeedbackUpdateEvent, this, false, false)},
+      controlLoopOutputSubscriber{node.subscribe("/control/arm/pid/"s + this->name + "/output", 1, &Joint::onControlLoopOutput, this)},
+      controlLoopSetpointPublisher{node.advertise<std_msgs::Float64>("/control/arm/pid/"s + this->name + "/setpoint", 1)},
+      controlLoopFeedbackPublisher{node.advertise<std_msgs::Float64>("/control/arm/pid/"s + this->name + "/feedback", 1)} {}
+
+void Joint::setTarget(double target, double maxSpeed) {
+    this->target = target;
+    this->maxSpeed = maxSpeed;
+    executeMotion = true;
+    controlLoopUpdateTimer.start();
+}
+
+auto Joint::hasReachedTarget() const -> bool {
+    // Copy-on-read to avoid inconsistencies on asynchronous change
+    auto currentTarget = target.load();
+
+    // Compute the upper and lower bounds in the finite encoder space
+    auto lBound = MathUtil::corrMod(currentTarget - JOINT_TOLERANCE_RADIANS, RADIANS_PER_ROTATION);
+    auto uBound = MathUtil::corrMod(currentTarget + JOINT_TOLERANCE_RADIANS, RADIANS_PER_ROTATION);
+    auto position = MathUtil::corrMod(positionMonitor(), RADIANS_PER_ROTATION);
+    // If the computed lower bound is lower than the upper bound, perform the computation normally
+    if (lBound < uBound)
+        return position <= uBound && position >= lBound;
+    // Otherwise, check if the value is outside either bound and negate the response
+    return position <= uBound || position >= lBound;
+}
+
+auto Joint::getName() const -> std::string {
+    return name;
+}
+
+void Joint::stop() {
+    executeMotion = false;
+    controlLoopUpdateTimer.stop();
+    motorSpeedDispatcher(0);
+}
+
+void Joint::onControlLoopOutput(const std_msgs::Float64::ConstPtr &msg) {
+    auto cappedPowerUnsigned{std::min(std::abs(msg->data), std::abs(maxSpeed))};
+    double cappedPower{0};
+    if (msg->data != 0)
+        cappedPower = (std::abs(msg->data) / msg->data) * cappedPowerUnsigned;
+    motorSpeedDispatcher(executeMotion ? cappedPower : 0);
+}
+
+void Joint::onFeedbackUpdateEvent(const ros::TimerEvent &event) {
+    // Setpoint
+    std_msgs::Float64 setpointMsg;
+    setpointMsg.data = target;
+    controlLoopSetpointPublisher.publish(setpointMsg);
+
+    // Feedback
+    std_msgs::Float64 feedbackMsg;
+    feedbackMsg.data = positionMonitor();
+    controlLoopFeedbackPublisher.publish(feedbackMsg);
+}
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/src/Joint.hpp b/src/wr_control_drive_arm/src/Joint.hpp
new file mode 100644
index 00000000..659a4f98
--- /dev/null
+++ b/src/wr_control_drive_arm/src/Joint.hpp
@@ -0,0 +1,39 @@
+#ifndef JOINT_H
+#define JOINT_H
+
+#include "ros/publisher.h"
+#include "ros/subscriber.h"
+#include "ros/timer.h"
+#include "std_msgs/Float64.h"
+#include <functional>
+#include <string>
+
+class Joint {
+public:
+    explicit Joint(std::string name, std::function<double()> positionMonitor, std::function<void(double)> motorSpeedDispatcher, ros::NodeHandle node);
+    void setTarget(double target, double maxSpeed);
+    [[nodiscard]] auto hasReachedTarget() const -> bool;
+    [[nodiscard]] auto getName() const -> std::string;
+    void stop();
+
+private:
+    static constexpr double FEEDBACK_UPDATE_FREQUENCY_HZ{50};
+    static constexpr double JOINT_TOLERANCE_RADIANS{3 * M_PI / 180};
+
+    const std::string name;
+    const std::function<double()> positionMonitor;
+    const std::function<void(double)> motorSpeedDispatcher;
+
+    void onControlLoopOutput(const std_msgs::Float64::ConstPtr &msg);
+    void onFeedbackUpdateEvent(const ros::TimerEvent &event);
+
+    std::atomic<double> target;
+    std::atomic<double> maxSpeed;
+    std::atomic<bool> executeMotion;
+    ros::Timer controlLoopUpdateTimer;
+    ros::Subscriber controlLoopOutputSubscriber;
+    ros::Publisher controlLoopSetpointPublisher;
+    ros::Publisher controlLoopFeedbackPublisher;
+};
+
+#endif
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/src/JointStatePublisher.cpp b/src/wr_control_drive_arm/src/JointStatePublisher.cpp
new file mode 100644
index 00000000..3a602563
--- /dev/null
+++ b/src/wr_control_drive_arm/src/JointStatePublisher.cpp
@@ -0,0 +1,140 @@
+/**
+ * @file JointStatePublisher.cpp
+ * @author Ben Nowotny
+ * @author Jack Zautner
+ * @brief The executable file to run the joint state publisher
+ * @date 2022-12-05
+ */
+
+#include "SingleEncoderJointPositionMonitor.hpp"
+#include "XmlRpcValue.h"
+
+#include <actionlib/server/simple_action_server.h>
+#include <algorithm>
+#include <array>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <csignal>
+#include <memory>
+#include <sensor_msgs/JointState.h>
+#include <std_msgs/Empty.h>
+#include <std_msgs/Float64.h>
+#include <std_srvs/Trigger.h>
+#include <string>
+
+using XmlRpc::XmlRpcValue;
+
+/**
+ * @brief Nessage cache size of publisher
+ */
+constexpr std::uint32_t MESSAGE_CACHE_SIZE = 1;
+
+/**
+ * @brief Period between timer callback
+ */
+constexpr float TIMER_CALLBACK_DURATION = 1.0 / 50.0;
+
+/**
+ * @brief The joint state publisher for MoveIt
+ */
+ros::Publisher jointStatePublisher;
+
+std::map<std::string, SingleEncoderJointPositionMonitor> namedJointPositionMonitors;
+
+void publishJointStates(const ros::TimerEvent &event) {
+    std::vector<std::string> names;
+    std::vector<double> positions;
+    sensor_msgs::JointState js_msg;
+
+    for (auto &[name, monitor] : namedJointPositionMonitors) {
+        names.push_back(name);
+        positions.push_back(monitor());
+    }
+
+    js_msg.name = names;
+    js_msg.position = positions;
+    js_msg.header.stamp = ros::Time::now();
+    // Publish the Joint State message
+    jointStatePublisher.publish(js_msg);
+}
+
+auto getEncoderConfigFromParams(const XmlRpcValue &params, const std::string &jointName) -> EncoderConfiguration {
+    return {.countsPerRotation = static_cast<int32_t>(params[jointName]["encoder_parameters"]["counts_per_rotation"]),
+            .offset = static_cast<int32_t>(params[jointName]["encoder_parameters"]["offset"])};
+}
+
+auto getMotorConfigFromParams(const XmlRpcValue &params, const std::string &jointName) -> MotorConfiguration {
+    return {.gearRatio = static_cast<double>(params[jointName]["motor_configurations"]["gear_ratio"])};
+}
+
+/**
+ * @brief The main executable method of the node. Starts the ROS node
+ *
+ * @param argc The number of program arguments
+ * @param argv The given program arguments
+ * @return int The status code on exiting the program
+ */
+auto main(int argc, char **argv) -> int {
+
+    std::cout << "start main" << std::endl;
+    // // Initialize the current node as JointStatePublisherApplication
+    ros::init(argc, argv, "JointStatePublisher");
+    // // Create the NodeHandle to the current ROS node
+    ros::NodeHandle n;
+    ros::NodeHandle pn{"~"};
+
+    XmlRpcValue armParams;
+    pn.getParam("arm_parameters", armParams);
+
+    namedJointPositionMonitors.try_emplace("elbowPitch_joint",
+
+                                           "aux1",
+                                           RoboclawChannel::A,
+                                           getEncoderConfigFromParams(armParams, "elbow"),
+                                           getMotorConfigFromParams(armParams, "elbow"),
+                                           n);
+    namedJointPositionMonitors.try_emplace("elbowRoll_joint",
+
+                                           "aux1",
+                                           RoboclawChannel::B,
+                                           getEncoderConfigFromParams(armParams, "forearmRoll"),
+                                           getMotorConfigFromParams(armParams, "forearmRoll"),
+                                           n);
+    namedJointPositionMonitors.try_emplace("shoulder_joint",
+
+                                           "aux0",
+                                           RoboclawChannel::B,
+                                           getEncoderConfigFromParams(armParams, "shoulder"),
+                                           getMotorConfigFromParams(armParams, "shoulder"),
+                                           n);
+    namedJointPositionMonitors.try_emplace("turntable_joint",
+
+                                           "aux0",
+                                           RoboclawChannel::A,
+                                           getEncoderConfigFromParams(armParams, "turntable"),
+                                           getMotorConfigFromParams(armParams, "turntable"),
+                                           n);
+    namedJointPositionMonitors.try_emplace("wristPitch_joint",
+
+                                           "aux2",
+                                           RoboclawChannel::A,
+                                           getEncoderConfigFromParams(armParams, "wristPitch"),
+                                           getMotorConfigFromParams(armParams, "wristPitch"),
+                                           n);
+    namedJointPositionMonitors.try_emplace("wristRoll_link",
+                                           "aux2",
+                                           RoboclawChannel::B,
+                                           getEncoderConfigFromParams(armParams, "wristRoll"),
+                                           getMotorConfigFromParams(armParams, "wristRoll"),
+                                           n);
+
+    // Initialize the Joint State Data Publisher
+    jointStatePublisher = n.advertise<sensor_msgs::JointState>("/joint_states", MESSAGE_CACHE_SIZE);
+
+    // Timer that will call publishJointStates periodically
+    ros::Timer timer = n.createTimer(ros::Duration(TIMER_CALLBACK_DURATION), publishJointStates);
+
+    // Enter ROS spin
+    ros::spin();
+
+    return 0;
+}
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/src/MathUtil.cpp b/src/wr_control_drive_arm/src/MathUtil.cpp
new file mode 100644
index 00000000..682b0bad
--- /dev/null
+++ b/src/wr_control_drive_arm/src/MathUtil.cpp
@@ -0,0 +1,8 @@
+#include "MathUtil.hpp"
+#include <cmath>
+
+namespace MathUtil {
+auto corrMod(double dividend, double divisor) -> double {
+    return std::fmod(std::fmod(dividend, divisor) + divisor, std::abs(divisor));
+}
+} // namespace MathUtil
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/src/MathUtil.hpp b/src/wr_control_drive_arm/src/MathUtil.hpp
new file mode 100644
index 00000000..07e7b0d0
--- /dev/null
+++ b/src/wr_control_drive_arm/src/MathUtil.hpp
@@ -0,0 +1,11 @@
+#ifndef MATH_UTIL_H
+#define MATH_UTIL_H
+
+#include <cmath>
+
+namespace MathUtil {
+auto corrMod(double dividend, double divisor) -> double;
+static constexpr double RADIANS_PER_ROTATION{2 * M_PI};
+} // namespace MathUtil
+
+#endif
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/src/Motor.cpp b/src/wr_control_drive_arm/src/Motor.cpp
new file mode 100644
index 00000000..bc96cd0f
--- /dev/null
+++ b/src/wr_control_drive_arm/src/Motor.cpp
@@ -0,0 +1,45 @@
+#include "Motor.hpp"
+#include "RoboclawChannel.hpp"
+#include "ros/node_handle.h"
+#include "ros/time.h"
+#include "std_msgs/Int16.h"
+#include <atomic>
+#include <cstdint>
+#include <optional>
+#include <stdexcept>
+
+using std::literals::string_literals::operator""s;
+
+Motor::Motor(const std::string &controllerName, RoboclawChannel channel, ros::NodeHandle node)
+    : motorSpeedPublisher{
+          node.advertise<std_msgs::Int16>(
+              "/hsi/roboclaw/"s + controllerName + "/cmd/"s + (channel == RoboclawChannel::A ? "left" : "right"),
+              1)},
+      currentOverLimitSubscriber{
+          node.subscribe("/hsi/roboclaw/"s + controllerName + "/curr/over_lim/" + (channel == RoboclawChannel::A ? "left" : "right"),
+              OVER_CURRENT_QUEUE_SIZE, &Motor::setCurrentStatus, this)} {}
+
+void Motor::setSpeed(double speed) {
+    if (abs(speed) > 1)
+        throw std::invalid_argument("speed must be between -1 and 1");
+
+    std_msgs::Int16 powerMsg{};
+    powerMsg.data = static_cast<int16_t>(speed * INT16_MAX);
+    motorSpeedPublisher.publish(powerMsg);
+}
+
+void Motor::setCurrentStatus(const std_msgs::Bool::ConstPtr &msg) {
+    bool overCurrent = static_cast<bool>(msg->data);
+    if (overCurrent && !this->beginStallTime.has_value()) {
+        this->beginStallTime = ros::Time::now();
+    } else if (!overCurrent && this->beginStallTime.has_value()) {
+        this->beginStallTime = std::nullopt;
+    }
+}
+
+auto Motor::isOverCurrent() const -> bool {
+    if (this->beginStallTime.has_value()) {
+        return static_cast<bool>((ros::Time::now() - this->beginStallTime.value()).toSec() >= STALL_THRESHOLD_TIME);
+    }
+    return false;
+}
diff --git a/src/wr_control_drive_arm/src/Motor.hpp b/src/wr_control_drive_arm/src/Motor.hpp
new file mode 100644
index 00000000..ff3a6174
--- /dev/null
+++ b/src/wr_control_drive_arm/src/Motor.hpp
@@ -0,0 +1,30 @@
+#ifndef MOTOR_H
+#define MOTOR_H
+
+#include "RoboclawChannel.hpp"
+#include "ros/publisher.h"
+#include "ros/subscriber.h"
+#include "std_msgs/Bool.h"
+#include <string>
+#include <optional>
+
+class Motor {
+public:
+    Motor(const std::string &controllerName, RoboclawChannel channel, ros::NodeHandle node);
+    void setSpeed(double speed);
+    auto isOverCurrent() const -> bool;
+
+private:
+
+    static constexpr float STALL_THRESHOLD_TIME{0.5F};
+    static constexpr int OVER_CURRENT_QUEUE_SIZE{25};
+
+
+    void setCurrentStatus(const std_msgs::Bool::ConstPtr &msg); 
+
+    ros::Publisher motorSpeedPublisher;
+    ros::Subscriber currentOverLimitSubscriber;
+    std::optional<ros::Time> beginStallTime;
+};
+
+#endif
diff --git a/src/wr_control_drive_arm/src/RoboclawChannel.hpp b/src/wr_control_drive_arm/src/RoboclawChannel.hpp
new file mode 100644
index 00000000..fbf7de00
--- /dev/null
+++ b/src/wr_control_drive_arm/src/RoboclawChannel.hpp
@@ -0,0 +1,6 @@
+#ifndef ROBOCLAW_CHANNEL_H
+#define ROBOCLAW_CHANNEL_H
+
+enum class RoboclawChannel { A, B };
+
+#endif
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp
new file mode 100644
index 00000000..d657f1c8
--- /dev/null
+++ b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.cpp
@@ -0,0 +1,59 @@
+#include "SingleEncoderJointPositionMonitor.hpp"
+#include "MathUtil.hpp"
+#include "RoboclawChannel.hpp"
+#include "ros/node_handle.h"
+#include <cstdint>
+
+using std::literals::string_literals::operator""s;
+using MathUtil::RADIANS_PER_ROTATION;
+
+SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor(
+    const std::string &controllerName,
+    RoboclawChannel channel,
+    EncoderConfiguration eConfig,
+    MotorConfiguration mConfig,
+    ros::NodeHandle node)
+    : countsPerRotation{eConfig.countsPerRotation},
+      offset{eConfig.offset},
+      gearRatio{mConfig.gearRatio},
+      position{0},
+      encoderSubscriber{
+          std::make_shared<ros::Subscriber>(node.subscribe(
+              "/hsi/roboclaw/"s + controllerName + "/enc/" + (channel == RoboclawChannel::A ? "left" : "right"),
+              1,
+              &SingleEncoderJointPositionMonitor::onEncoderReceived,
+              this))} {}
+
+SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor(
+    const SingleEncoderJointPositionMonitor &other)
+    : countsPerRotation(other.countsPerRotation),
+      offset(other.offset),
+      gearRatio{other.getGearRatio()},
+      encoderSubscriber{other.encoderSubscriber},
+      position{other.position.load()} {}
+
+SingleEncoderJointPositionMonitor::SingleEncoderJointPositionMonitor(
+    SingleEncoderJointPositionMonitor &&other) noexcept
+    : countsPerRotation(other.countsPerRotation),
+      offset(other.offset),
+      gearRatio(other.getGearRatio()),
+      encoderSubscriber{std::move(other.encoderSubscriber)},
+      position{other.position.load()} {}
+
+auto SingleEncoderJointPositionMonitor::operator()() -> double {
+    return position;
+}
+
+void SingleEncoderJointPositionMonitor::onEncoderReceived(const std_msgs::UInt32::ConstPtr &msg) {
+    auto enc = static_cast<double>(msg->data);
+    auto rotations = MathUtil::corrMod(enc - offset, countsPerRotation) / countsPerRotation;
+    position = MathUtil::corrMod(rotations * RADIANS_PER_ROTATION + M_PI, RADIANS_PER_ROTATION) - M_PI;
+}
+
+auto SingleEncoderJointPositionMonitor::getCountsPerRotation() const -> int32_t {
+    return this->countsPerRotation;
+}
+
+auto SingleEncoderJointPositionMonitor::getGearRatio() const -> double {
+    return this->gearRatio;
+}
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp
new file mode 100644
index 00000000..e57437b1
--- /dev/null
+++ b/src/wr_control_drive_arm/src/SingleEncoderJointPositionMonitor.hpp
@@ -0,0 +1,44 @@
+#ifndef SINGLE_ENCODER_JOINT_POSITION_MONITOR_H
+#define SINGLE_ENCODER_JOINT_POSITION_MONITOR_H
+
+#include "RoboclawChannel.hpp"
+#include <cstdint>
+#include <ros/node_handle.h>
+#include <ros/subscriber.h>
+#include <std_msgs/UInt32.h>
+#include <string>
+
+struct EncoderConfiguration {
+    int32_t countsPerRotation;
+    int32_t offset;
+};
+
+struct MotorConfiguration {
+    double gearRatio;
+};
+
+class SingleEncoderJointPositionMonitor {
+public:
+    SingleEncoderJointPositionMonitor(const std::string &controllerName, RoboclawChannel channel, EncoderConfiguration eConfig, MotorConfiguration mConfig, ros::NodeHandle node);
+    auto operator()() -> double;
+
+    SingleEncoderJointPositionMonitor(const SingleEncoderJointPositionMonitor &);
+    auto operator=(const SingleEncoderJointPositionMonitor &) -> SingleEncoderJointPositionMonitor & = delete;
+    SingleEncoderJointPositionMonitor(SingleEncoderJointPositionMonitor &&) noexcept;
+    auto operator=(SingleEncoderJointPositionMonitor &&) -> SingleEncoderJointPositionMonitor & = delete;
+    ~SingleEncoderJointPositionMonitor() = default;
+
+    [[nodiscard]] auto getCountsPerRotation() const -> int32_t;
+    [[nodiscard]] auto getGearRatio() const -> double;
+
+private:
+    void onEncoderReceived(const std_msgs::UInt32::ConstPtr &msg);
+
+    std::atomic<double> position;
+    std::shared_ptr<ros::Subscriber> encoderSubscriber;
+    const int32_t countsPerRotation;
+    const int32_t offset;
+    const double gearRatio;
+};
+
+#endif
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/src/testDifferential.cpp b/src/wr_control_drive_arm/src/testDifferential.cpp
new file mode 100644
index 00000000..5b2d31a6
--- /dev/null
+++ b/src/wr_control_drive_arm/src/testDifferential.cpp
@@ -0,0 +1,26 @@
+#include <cstdint>
+#include "Motor.hpp"
+#include "DifferentialJointToMotorSpeedConverter.hpp"
+#include "ros/init.h"
+#include <ros/ros.h>
+
+auto main(int32_t argc, char** argv) -> int32_t{
+    ros::init(argc, argv, "testNode");
+    
+    ros::NodeHandle n{};
+    using std::literals::string_literals::operator""s;
+
+    const auto wristLeftMotor{std::make_shared<Motor>("aux2"s, RoboclawChannel::A, n)};
+    const auto wristRightMotor{std::make_shared<Motor>("aux2"s, RoboclawChannel::B, n)};
+
+    const auto differentialSpeedConverter{std::make_shared<DifferentialJointToMotorSpeedConverter>(wristLeftMotor, wristRightMotor)};
+
+    ros::Rate loopRate{50};
+    while(ros::ok()){
+        differentialSpeedConverter->setPitchSpeed(0.3);
+        differentialSpeedConverter->setRollSpeed(0);
+        loopRate.sleep();
+    }
+
+    return 0;
+}
\ No newline at end of file
diff --git a/src/wr_control_drive_arm/structure.wsd b/src/wr_control_drive_arm/structure.wsd
new file mode 100755
index 00000000..ef4f1a40
--- /dev/null
+++ b/src/wr_control_drive_arm/structure.wsd
@@ -0,0 +1,99 @@
+@startuml Structure
+enum RoboclawChannel{
+    + A
+    + B
+}
+
+note right of RoboclawChannel::A
+    Analogous to 'LEFT'
+end note
+
+note right of RoboclawChannel::B
+    Analogous to 'RIGHT'
+end note
+
+class Motor{
+    - powerPublisher : ros::Publisher
+
+    + Motor(string controllerName, RoboclawChannel channel)
+    + void setPower(double power)
+    + bool getOverCurrentStatus()
+}
+
+Motor --> RoboclawChannel
+
+class EncoderJointPositionMonitor {
+    - encoderSubscriber : ros::Subscriber
+    - countsPerRotation : const int
+    - offset : const int
+    
+    + EncoderJointPositionMonitor(string controllerName, RoboclawChannel channel, int countsPerRotation, int offset)
+    - void encoderCallback(std_msgs::Int32)
+}
+
+EncoderJointPositionMonitor --> RoboclawChannel
+
+class DirectJointToMotorSpeedConverter {
+    + DirectJointToMotorSpeedConverter(Motor motor)
+    + void operator()(double)
+}
+
+DirectJointToMotorSpeedConverter "1" --> "1" Motor
+
+class DifferentialJointToMotorSpeedConverter {
+    + void setPitchSpeed(double)
+    + void setRollSpeed(double)
+}
+
+DifferentialJointToMotorSpeedConverter "1" --> "2" Motor
+
+class Joint{
+    + Joint(String name)
+    + void setTarget(double target)
+    + bool hasReachedTarget()
+    + void stop()  
+}
+
+Joint --> EncoderJointPositionMonitor : via function parameter <double()>
+<> speedConversion
+Joint --> speedConversion : via function parameter <void(double)>
+speedConversion --> DirectJointToMotorSpeedConverter : as functor
+speedConversion --> DifferentialJointToMotorSpeedConverter::setPitchSpeed : as function pointer
+speedConversion --> DifferentialJointToMotorSpeedConverter::setRollSpeed : as function pointer
+
+@enduml
+
+@startuml seq
+    participant ROS
+    MoveIt -> ActionServer : Sends a motion path
+    loop For each waypoint in the motion
+        loop For each joint in the arm
+            ActionServer -> Joint : Set the next setpoint per joint
+            Joint -> Joint : Start the execution loop
+        end
+        loop While some joint has not reached its setpoint
+            note over ActionServer : Wait
+            ROS -> Joint : Kicks off Timer Event
+            activate Joint
+            Joint -> "PID Loop" : Feed the setpoint
+            Joint -> JointPositionMonitor : Ask for the joint position
+            activate JointPositionMonitor
+            JointPositionMonitor --> Joint : Sends back joint position
+            deactivate JointPositionMonitor
+            Joint -> "PID Loop" : Feed the feedback
+            deactivate Joint
+
+            note left of Joint : Happens asynchonously
+            "PID Loop" -> Joint : New output available
+            activate Joint
+            Joint -> JointToMotorSpeedConverter : Dispatch new speed
+            activate JointToMotorSpeedConverter
+            JointToMotorSpeedConverter -> JointToMotorSpeedConverter : Determine hardware speeds
+            JointToMotorSpeedConverter -> "Motor(s)" : Dispatch hardware speeds
+            deactivate JointToMotorSpeedConverter
+            deactivate Joint
+        end
+    end
+    ActionServer -> MoveIt : Report motion as complete
+    
+@enduml
diff --git a/src/wr_control_drive_science/CMakeLists.txt b/src/wr_control_drive_science/CMakeLists.txt
index 29768673..529f3eab 100644
--- a/src/wr_control_drive_science/CMakeLists.txt
+++ b/src/wr_control_drive_science/CMakeLists.txt
@@ -13,6 +13,10 @@ find_package(catkin REQUIRED COMPONENTS
   rospy
 )
 
+
+find_package(Boost REQUIRED COMPONENTS python)
+find_package(PythonLibs 2.7 REQUIRED)
+
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 
@@ -118,6 +122,8 @@ catkin_package(
 include_directories(
 # include
   ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${PYTHON_INCLUDE_DIRS}
 )
 
 ## Declare a C++ library
@@ -125,6 +131,17 @@ include_directories(
 #   src/${PROJECT_NAME}/wr_control_drive_science.cpp
 # )
 
+# add_library(testModule
+#   main.cpp
+# )
+
+# target_link_libraries(testModule ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+
+# set_target_properties(testModule PROPERTIES
+#         PREFIX ""
+#         LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}
+#         )
+
 ## 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
diff --git a/src/wr_control_drive_science/package.xml b/src/wr_control_drive_science/package.xml
index de1573a2..f2d77f2c 100644
--- a/src/wr_control_drive_science/package.xml
+++ b/src/wr_control_drive_science/package.xml
@@ -43,7 +43,7 @@
   <!-- Use buildtool_depend for build tool packages: -->
   <!--   <buildtool_depend>catkin</buildtool_depend> -->
   <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
+    <exec_depend>message_runtime</exec_depend> 
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
   <!-- Use doc_depend for packages you need only for building documentation: -->
diff --git a/src/wr_entry_point/launch/comp/comp_arm.launch b/src/wr_entry_point/launch/comp/comp_arm.launch
index 8f356b47..1d537d63 100644
--- a/src/wr_entry_point/launch/comp/comp_arm.launch
+++ b/src/wr_entry_point/launch/comp/comp_arm.launch
@@ -3,7 +3,7 @@
   <include file="$(find wr_hci_hid)/launch/teleop_arm.launch"/>
   <!-- TODO: arm logic -->
   <!-- TODO: arm control systems -->
-  <node name="arm_driver" pkg="wr_logic_teleop_arm" type="bad_arm_driver.py">
+  <node name="bad_arm_driver" pkg="wr_logic_teleop_arm" type="bad_arm_driver.py">
     <param name="controller_ns" value="/hci/arm/gamepad"/>
     <param name="claw_ns_0" value="/hsi/roboclaw/aux0"/>
     <param name="claw_ns_1" value="/hsi/roboclaw/aux1"/>
diff --git a/src/wr_entry_point/launch/comp/comp_init.launch b/src/wr_entry_point/launch/comp/comp_init.launch
index ea3bb01d..36f80295 100644
--- a/src/wr_entry_point/launch/comp/comp_init.launch
+++ b/src/wr_entry_point/launch/comp/comp_init.launch
@@ -4,9 +4,9 @@
   <node name="env_checker" pkg="wr_entry_point" type="env_checker.py" required="true"/>
 
   <!-- Set up machine definitions -->
-  <machine name="base_station" address="localhost" env-loader="/opt/ros/noetic/env.sh" default="false"/>
+  <machine name="base_station" address="localhost" env-loader="/opt/ros/noetic/env.sh" default="true"/>
   <!-- If running in local mode, define "wiscrobo" to also be the local machine (i.e. base station) -->
-  <machine name="wrover" address="wrover-nano.local" env-loader="/home/wiscrobo/catkin_ws/WRover21_Software/env_remote.sh" user="wiscrobo" default="true" unless="$(env WROVER_LOCAL)">
+  <machine name="wrover" address="wrover-nano.local" env-loader="/home/wiscrobo/catkin_ws/WRover21_Software/env_remote.sh" user="wiscrobo" default="false" unless="$(env WROVER_LOCAL)">
 <!-- env name="WROVER_LOCAL" value="false"/ -->
 <!-- env name="WROVER_HW" value="MOCK"/ -->
 </machine>
diff --git a/src/wr_entry_point/launch/test/mock_arm.launch b/src/wr_entry_point/launch/test/mock_arm.launch
new file mode 100644
index 00000000..6dfbb8b1
--- /dev/null
+++ b/src/wr_entry_point/launch/test/mock_arm.launch
@@ -0,0 +1,24 @@
+<!-- Test robot configuration that runs only the arm and manipulator. -->
+<launch>
+  <include file="$(find wr_entry_point)/launch/comp/comp_init.launch" />
+
+  <include file="$(find wr_hsi_roboclaw)/launch/roboclaw_real.launch" if="$(eval env('WROVER_HW') == 'REAL')">
+    <arg name="use_encoders" value="false" />
+  </include>
+  <include file="$(find wr_hsi_roboclaw)/launch/roboclaw_mock.launch" if="$(eval env('WROVER_HW') == 'MOCK')">
+    <arg name="use_encoders" value="true" />
+  </include>
+
+  <include file="$(find wr_hci_hid)/launch/teleop_arm.launch" />
+
+  <include file="$(find wr_entry_point)/launch/comp/comp_ui.launch" />
+
+  <!-- <include file="$(find wr_entry_point)/launch/comp/comp_arm.launch"/> -->
+
+  <include file="$(find wroboarm_23)/launch/demo_test.launch" />
+
+
+  <node name="arm_driver" pkg="wr_logic_teleop_arm" type="ArmTeleopLogic" output="screen" />
+
+  <include file="$(find wr_control_drive_arm)/launch/std.launch" />
+</launch>
\ No newline at end of file
diff --git a/src/wr_entry_point/launch/test/test_arm.launch b/src/wr_entry_point/launch/test/test_arm.launch
index d834e3de..26e9a216 100644
--- a/src/wr_entry_point/launch/test/test_arm.launch
+++ b/src/wr_entry_point/launch/test/test_arm.launch
@@ -1,6 +1,8 @@
 <!-- Test robot configuration that runs only the arm and manipulator. -->
 <launch>
+  <env name="WROVER_LOCAL" value="true"/>
   <env name="WROVER_MODE" value="TEST"/>
+  <env name="WROVER_HW" value="REAL"/>
 
   <include file="$(find wr_entry_point)/launch/comp/comp_init.launch"/>
   
@@ -10,5 +12,9 @@
   
   <include file="$(find wr_entry_point)/launch/comp/comp_ui.launch"/>
   
-  <include file="$(find wr_entry_point)/launch/comp/comp_arm.launch"/>
+  <inclue file="$(find wr_control_drive_arm)/launch/std.launch"/>
+  
+  <include file="$(find wroboarm_22)/launch/demo_test.launch" if="$(eval env('WROVER_HW') == 'MOCK')"/>
+
+  <node name="arm_driver" pkg="wr_logic_teleop_arm" type="ArmTeleopLogic" />
 </launch>
diff --git a/src/wr_entry_point/launch/test/test_controller.launch b/src/wr_entry_point/launch/test/test_controller.launch
new file mode 100644
index 00000000..281ea3cf
--- /dev/null
+++ b/src/wr_entry_point/launch/test/test_controller.launch
@@ -0,0 +1,7 @@
+<!-- Launches the full robot system, configured for the URC extreme retrieval and delivery mission. -->
+<launch>
+
+  <include file="$(find wr_entry_point)/launch/comp/comp_init.launch"/>
+  
+  <include file="$(find wr_hci_hid)/launch/teleop_arm.launch"/>
+</launch>
diff --git a/src/wr_entry_point/launch/test/vis_arm.launch b/src/wr_entry_point/launch/test/vis_arm.launch
new file mode 100644
index 00000000..abc1bf63
--- /dev/null
+++ b/src/wr_entry_point/launch/test/vis_arm.launch
@@ -0,0 +1,22 @@
+<!-- Test robot configuration that runs only the arm and manipulator. -->
+<launch>
+  <env name="WROVER_LOCAL" value="false"/>
+  <env name="WROVER_MODE" value="TEST"/>
+  <env name="WROVER_HW" value="REAL"/>
+
+
+  <!--include file="$(find wr_entry_point)/launch/comp/comp_init.launch"/>
+  
+  <include file="$(find wr_entry_point)/launch/comp/comp_roboclaw.launch">
+    <arg name="use_encoders" value="true"/>
+  </include>
+  
+  <include file="$(find wr_entry_point)/launch/comp/comp_ui.launch"/>
+  
+  <include file="$(find wr_entry_point)/launch/comp/comp_arm.launch"/-->
+  
+  <include file="$(find wr_control_drive_arm)/launch/std.launch" />
+  <include file="$(find wroboarm_22)/launch/demo_test.launch"/>
+
+  <!-- node name="arm_driver" pkg="wr_logic_teleop_arm" type="ArmTeleopLogic" / -->
+</launch>
diff --git a/src/wr_entry_point/launch/test_arm_OLD.launch b/src/wr_entry_point/launch/test_arm_OLD.launch
new file mode 100644
index 00000000..7ae0bf5c
--- /dev/null
+++ b/src/wr_entry_point/launch/test_arm_OLD.launch
@@ -0,0 +1,4 @@
+<launch>
+	<include file="$(find wr_entry_point)/launch/comp/init.launch"/>
+	<include file="$(find wr_hsi_roboclaw)/launch/roboclaw_mock.launch"/>
+</launch>
diff --git a/src/wr_hsi_roboclaw/config/roboclaw_enc.yaml b/src/wr_hsi_roboclaw/config/roboclaw_enc.yaml
index 11268a6f..03b11c22 100644
--- a/src/wr_hsi_roboclaw/config/roboclaw_enc.yaml
+++ b/src/wr_hsi_roboclaw/config/roboclaw_enc.yaml
@@ -6,37 +6,45 @@ claws:
     address: 0x80
     enc_left:
         enabled: true
-        max_speed: 300000000 # TODO probably don't actually need all these encoders
+        max_speed: 100 # TODO probably don't actually need all these encoders
+        offset: 500
     enc_right:
         enabled: true
-        max_speed: 300000000
+        max_speed: 100
+        offset: 500
   aux1: # arm elbow/forearm, science ?
     topic: '/hsi/roboclaw/aux1'
     address: 0x81
     enc_left:
         enabled: true
-        max_speed: 300000000
+        max_speed: 100
+        offset: 500
     enc_right:
         enabled: true
-        max_speed: 300000000
+        max_speed: 100
+        offset: 500
   aux2: # arm wrist, science ?
     topic: '/hsi/roboclaw/aux2'
     address: 0x82
     enc_left:
         enabled: true
-        max_speed: 300000000
+        max_speed: 100
+        offset: 500
     enc_right:
         enabled: true
-        max_speed: 300000000
+        max_speed: 100
+        offset: 500
   aux3: # arm manip, science ?
     topic: '/hsi/roboclaw/aux3'
     address: 0x83
     enc_left:
         enabled: true
-        max_speed: 300000000
+        max_speed: 100
+        offset: 500
     enc_right:
         enabled: true
-        max_speed: 300000000
+        max_speed: 100
+        offset: 500
   drive: # drive train
     topic: '/hsi/roboclaw/drive'
     address: 0x84
diff --git a/src/wr_hsi_roboclaw/launch/roboclaw_mock.launch b/src/wr_hsi_roboclaw/launch/roboclaw_mock.launch
index 396b5b53..6fea09ed 100644
--- a/src/wr_hsi_roboclaw/launch/roboclaw_mock.launch
+++ b/src/wr_hsi_roboclaw/launch/roboclaw_mock.launch
@@ -1,6 +1,6 @@
 <!-- Launches a mock hardware interface for testing the drive system. -->
 <launch>
-  <arg name="use_encoders" default="false"/>
+  <arg name="use_encoders" default="true"/>
 
   <group ns="hsi">
     <group ns="roboclaw">
diff --git a/src/wr_logic_teleop_arm/CMakeLists.txt b/src/wr_logic_teleop_arm/CMakeLists.txt
index c061b7c5..746f428e 100644
--- a/src/wr_logic_teleop_arm/CMakeLists.txt
+++ b/src/wr_logic_teleop_arm/CMakeLists.txt
@@ -10,6 +10,10 @@ project(wr_logic_teleop_arm)
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
+  moveit_core
+  moveit_ros_planning_interface
+  moveit_msgs
+  moveit_visual_tools
 )
 
 ## System dependencies are found with CMake's conventions
@@ -117,6 +121,7 @@ catkin_package(
 include_directories(
 # include
   ${catkin_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIR}
 )
 
 ## Declare a C++ library
@@ -133,6 +138,8 @@ include_directories(
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
 # add_executable(${PROJECT_NAME}_node src/wr_logic_teleop_arm_node.cpp)
+add_executable(ArmTeleopLogic src/ArmTeleopLogic.cpp)
+target_link_libraries(ArmTeleopLogic ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
diff --git a/src/wr_logic_teleop_arm/launch/std.launch b/src/wr_logic_teleop_arm/launch/std.launch
new file mode 100644
index 00000000..a5e0beaa
--- /dev/null
+++ b/src/wr_logic_teleop_arm/launch/std.launch
@@ -0,0 +1,7 @@
+<launch>
+    <group ns="teleop">
+
+        <node name="ArmTeleop" pkg="wr_logic_teleop_arm" type="ArmTeleopLogic">
+        </node>
+    </group>
+</launch>
diff --git a/src/wr_logic_teleop_arm/package.xml b/src/wr_logic_teleop_arm/package.xml
index c2fc4d95..d8351162 100644
--- a/src/wr_logic_teleop_arm/package.xml
+++ b/src/wr_logic_teleop_arm/package.xml
@@ -55,6 +55,10 @@
   <build_export_depend>rospy</build_export_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>rospy</exec_depend>
+  <depend>moveit_core</depend>
+  <depend> moveit_ros_planning_interface</depend>
+  <depend>moveit_msgs</depend>
+  <depend>moveit_visual_tools</depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->
diff --git a/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp
new file mode 100644
index 00000000..f3a31eae
--- /dev/null
+++ b/src/wr_logic_teleop_arm/src/ArmTeleopLogic.cpp
@@ -0,0 +1,233 @@
+#include "boost/function.hpp"
+#include "geometry_msgs/PoseStamped.h"
+#include "geometry_msgs/Quaternion.h"
+#include "moveit_msgs/RobotTrajectory.h"
+#include "ros/rate.h"
+#include "ros/subscriber.h"
+#include "std_msgs/Bool.h"
+#include "std_msgs/Float32.h"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/LinearMath/Vector3.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include <atomic>
+#include <boost/concept_check.hpp>
+#include <cmath>
+#include <cstdint>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit_visual_tools/moveit_visual_tools.h>
+#include <ros/ros.h>
+#include <thread>
+
+using Std_Bool = const std_msgs::BoolConstPtr &;
+using Std_Float32 = const std_msgs::Float32ConstPtr &;
+
+std::atomic_bool isNewPath{true};
+const tf2::Quaternion WORLD_OFFSET{0, sin(M_PI / 2), 0, cos(M_PI / 2)};
+
+auto updateTarget(float x_pos, float y_pos, float z_pos, tf2::Quaternion orientation, ros::Publisher &pub) -> void {
+    geometry_msgs::PoseStamped p{};
+    p.pose.position.x = x_pos;
+    p.pose.position.y = y_pos;
+    p.pose.position.z = z_pos;
+    auto outOrientation = /* tf2::Quaternion(0,sin(M_PI/4), 0, cos(M_PI/4)) * */ orientation;
+    p.pose.orientation = tf2::toMsg(outOrientation);
+    p.header.frame_id = "base_link";
+    pub.publish(p);
+}
+
+auto main(int argc, char **argv) -> int {
+    ros::init(argc, argv, "ArmTeleopLogic");
+    ros::AsyncSpinner spin(1);
+    spin.start();
+    ros::NodeHandle np("~");
+
+    constexpr float PLANNING_TIME = 0.05;
+    constexpr float CLOCK_RATE = 2;
+    constexpr uint32_t MESSAGE_QUEUE_LENGTH = 1000;
+    constexpr float TRIGGER_PRESSED = 0.5;
+    constexpr float JOYSTICK_DEADBAND = 0.1;
+    constexpr float MINIMUM_PATH_ACCURACY = 0.0;
+
+    constexpr float STEP_X = 0.003;
+    constexpr float STEP_Y = 0.003;
+    constexpr float STEP_Z = 0.003;
+
+    constexpr float HOME_X = 0.0;
+    constexpr float HOME_Y = -0.7;
+    constexpr float HOME_Z = 0.2;
+
+    float accel = 1.0;
+    float deaccel = 1.0;
+    float step_mult = 1.0;
+    float x_pos = HOME_X;
+    float y_pos = HOME_Y;
+    float z_pos = HOME_Z;
+
+    const tf2::Quaternion homeOrientation{sin(M_PI / 4), 0, 0, cos(-M_PI / 4)};
+    tf2::Quaternion orientation{homeOrientation};
+
+    const tf2::Quaternion SPIN_X{sin(2 * M_PI / 1000), 0, 0, cos(2 * M_PI / 1000)};
+    const tf2::Quaternion SPIN_Y{0, sin(2 * M_PI / 1000), 0, cos(2 * M_PI / 1000)};
+    const tf2::Quaternion SPIN_Z{0, 0, sin(2 * M_PI / 1000), cos(2 * M_PI / 1000)};
+
+    moveit::planning_interface::MoveGroupInterface move("arm");
+    // move.setPlannerId("RRTStar");
+    move.setPlanningTime(PLANNING_TIME);
+    const moveit::core::JointModelGroup *joint_model_group = move.getCurrentState()->getJointModelGroup("arm");
+    robot_state::RobotState start_state(*move.getCurrentState());
+    moveit_visual_tools::MoveItVisualTools visual_tools("arm");
+
+    ros::Rate loop{CLOCK_RATE};
+
+    ros::Publisher nextTarget = np.advertise<geometry_msgs::PoseStamped>("/logic/arm_teleop/next_target",
+                                                                         MESSAGE_QUEUE_LENGTH);
+
+    ros::Publisher trajectoryPub = np.advertise<visualization_msgs::MarkerArray>("/logic/arm_teleop/trajectory",
+                                                                                 MESSAGE_QUEUE_LENGTH);
+
+    // y axis
+    ros::Subscriber yAxis = np.subscribe("/hci/arm/gamepad/axis/stick_left_y",
+                                         MESSAGE_QUEUE_LENGTH,
+                                         static_cast<boost::function<void(Std_Float32)>>([&](Std_Float32 msg) -> void {
+                                             if (abs(msg->data) >= JOYSTICK_DEADBAND) {
+                                                 y_pos += msg->data * STEP_Y;
+                                                 updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget);
+                                             }
+                                         }));
+
+    // x axis
+    ros::Subscriber xAxis = np.subscribe("/hci/arm/gamepad/axis/stick_left_x",
+                                         MESSAGE_QUEUE_LENGTH,
+                                         static_cast<boost::function<void(Std_Float32)>>([&](Std_Float32 msg) -> void {
+                                             if (abs(msg->data) >= JOYSTICK_DEADBAND) {
+                                                 x_pos += msg->data * STEP_X;
+                                                 updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget);
+                                             }
+                                         }));
+
+    // y up
+    ros::Subscriber zUp = np.subscribe("/hci/arm/gamepad/button/trigger_left",
+                                       MESSAGE_QUEUE_LENGTH,
+                                       static_cast<boost::function<void(Std_Bool)>>([&](Std_Bool msg) -> void {
+                                           if (msg->data) {
+                                               z_pos += STEP_Y * step_mult;
+                                               updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget);
+                                           }
+                                       }));
+
+    // y down
+    ros::Subscriber zDown = np.subscribe("/hci/arm/gamepad/button/trigger_right",
+                                         MESSAGE_QUEUE_LENGTH,
+                                         static_cast<boost::function<void(Std_Bool)>>([&](Std_Bool msg) -> void {
+                                             if (msg->data) {
+                                                 z_pos -= STEP_Y * step_mult;
+                                                 updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget);
+                                             }
+                                         }));
+
+    // roll
+    ros::Subscriber roll = np.subscribe("/hci/arm/gamepad/button/pov_x",
+                                          MESSAGE_QUEUE_LENGTH,
+                                          static_cast<boost::function<void(Std_Bool)>>([&](Std_Bool msg) -> void {
+                                              if (msg->data) {
+                                                  orientation *= SPIN_Z * step_mult;
+                                                  updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget);
+                                              }
+                                          }));
+
+    // pitch
+    ros::Subscriber pitch = np.subscribe("/hci/arm/gamepad/axis/pov_y",
+                                         MESSAGE_QUEUE_LENGTH,
+                                         static_cast<boost::function<void(Std_Float32)>>([&](Std_Float32 msg) -> void {
+                                             if (abs(msg->data) >= 0.5) {
+                                                 orientation *= (msg->data > 0 ? SPIN_Y : SPIN_Y.inverse());
+                                                 updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget);
+                                             }
+                                         }));
+
+    // yaw
+    ros::Subscriber yaw = np.subscribe("/hci/arm/gamepad/axis/stick_right_x",
+                                       MESSAGE_QUEUE_LENGTH,
+                                       static_cast<boost::function<void(Std_Float32)>>([&](Std_Float32 msg) -> void {
+                                           if (abs(msg->data) >= 0.5) {
+                                               orientation *= (msg->data > 0 ? SPIN_X : SPIN_X.inverse());
+                                               updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget);
+                                           }
+                                       }));
+
+    // override path execution
+    ros::Subscriber execPath = np.subscribe("/hci/arm/gamepad/button/start",
+                                            MESSAGE_QUEUE_LENGTH,
+                                            static_cast<boost::function<void(Std_Bool)>>([&](Std_Bool msg) -> void {
+                                                if (msg->data) {
+                                                    isNewPath.store(true);
+                                                }
+                                            }));
+
+    // reset target/cancel path
+    ros::Subscriber resetPose = np.subscribe("/hci/arm/gamepad/button/select",
+                                             MESSAGE_QUEUE_LENGTH,
+                                             static_cast<boost::function<void(Std_Bool)>>([&](Std_Bool msg) -> void {
+                                                 if (msg->data) {
+                                                     move.stop();
+                                                 }
+                                             }));
+
+    ros::Subscriber home = np.subscribe("/hci/arm/gamepad/button/mode",
+                                        MESSAGE_QUEUE_LENGTH,
+                                        static_cast<boost::function<void(Std_Bool)>>([&](Std_Bool msg) -> void {
+                                            if (msg->data) {
+                                                x_pos = HOME_X;
+                                                y_pos = HOME_Y;
+                                                z_pos = HOME_Z;
+                                                orientation = homeOrientation;
+                                                updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget);
+                                            }
+                                        }));
+
+    while (ros::ok()) {
+
+        updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget);
+        if (!isNewPath.load()) {
+            loop.sleep();
+            continue;
+        }
+
+        // stop current path
+        isNewPath.store(false);
+        move.stop();
+
+        // configure target pose
+        geometry_msgs::PoseStamped p{};
+        p.pose.position.x = x_pos;
+        p.pose.position.y = y_pos;
+        p.pose.position.z = z_pos;
+        p.pose.orientation = tf2::toMsg(orientation);
+        p.header.frame_id = "world";
+
+        move.setPoseTarget(p);
+        move.setStartStateToCurrentState();
+
+        // plan and execute path
+        move.asyncMove();
+
+        while (ros::ok()) {
+            updateTarget(x_pos, y_pos, z_pos, orientation, nextTarget);
+
+            if (move.getMoveGroupClient().getState().isDone()) {
+                std::cout << "[INFO] [" << ros::Time::now() << "]: path finished: " << move.getMoveGroupClient().getState().getText() << std::endl;
+                break;
+            }
+
+            else if (isNewPath.load()) {
+                std::cout << "[INFO] [" << ros::Time::now() << "]: path overridden" << std::endl;
+                move.stop();
+                break;
+            }
+
+            loop.sleep();
+        }
+    }
+
+    return 0;
+}
\ No newline at end of file
diff --git a/src/wroboarm_23/.setup_assistant b/src/wroboarm_23/.setup_assistant
new file mode 100644
index 00000000..ccd64ddc
--- /dev/null
+++ b/src/wroboarm_23/.setup_assistant
@@ -0,0 +1,11 @@
+moveit_setup_assistant_config:
+  URDF:
+    package: wroboarm_23
+    relative_path: urdf/ArmAssembly.SLDASM.urdf
+    xacro_args: ""
+  SRDF:
+    relative_path: config/wroboarm_23.srdf
+  CONFIG:
+    author_name: Ben Nowotny
+    author_email: bennowotny65535@gmail.com
+    generated_timestamp: 1669082391
\ No newline at end of file
diff --git a/src/wroboarm_23/CMakeLists.txt b/src/wroboarm_23/CMakeLists.txt
new file mode 100644
index 00000000..f1e67c4f
--- /dev/null
+++ b/src/wroboarm_23/CMakeLists.txt
@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 3.1.3)
+project(wroboarm_23)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  PATTERN "setup_assistant.launch" EXCLUDE)
+install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/src/wroboarm_23/config/cartesian_limits.yaml b/src/wroboarm_23/config/cartesian_limits.yaml
new file mode 100644
index 00000000..7df72f69
--- /dev/null
+++ b/src/wroboarm_23/config/cartesian_limits.yaml
@@ -0,0 +1,5 @@
+cartesian_limits:
+  max_trans_vel: 1
+  max_trans_acc: 2.25
+  max_trans_dec: -5
+  max_rot_vel: 1.57
diff --git a/src/wroboarm_23/config/chomp_planning.yaml b/src/wroboarm_23/config/chomp_planning.yaml
new file mode 100644
index 00000000..eb9c9122
--- /dev/null
+++ b/src/wroboarm_23/config/chomp_planning.yaml
@@ -0,0 +1,18 @@
+planning_time_limit: 10.0
+max_iterations: 200
+max_iterations_after_collision_free: 5
+smoothness_cost_weight: 0.1
+obstacle_cost_weight: 1.0
+learning_rate: 0.01
+smoothness_cost_velocity: 0.0
+smoothness_cost_acceleration: 1.0
+smoothness_cost_jerk: 0.0
+ridge_factor: 0.0
+use_pseudo_inverse: false
+pseudo_inverse_ridge_factor: 1e-4
+joint_update_limit: 0.1
+collision_clearance: 0.2
+collision_threshold: 0.07
+use_stochastic_descent: true
+enable_failure_recovery: false
+max_recovery_attempts: 5
diff --git a/src/wroboarm_23/config/fake_controllers.yaml b/src/wroboarm_23/config/fake_controllers.yaml
new file mode 100644
index 00000000..46de675b
--- /dev/null
+++ b/src/wroboarm_23/config/fake_controllers.yaml
@@ -0,0 +1,15 @@
+controller_list:
+  - name: fake_arm_controller
+    type: $(arg fake_execution_type)
+    joints:
+      - turntable_joint
+      - shoulder_joint
+      - elbowPitch_joint
+      - elbowRoll_joint
+      - wristPitch_joint
+      - wristRoll_link
+initial:  # Define initial robot poses per group
+#  - group: arm
+#    pose: home
+
+  []
\ No newline at end of file
diff --git a/src/wroboarm_23/config/gazebo_controllers.yaml b/src/wroboarm_23/config/gazebo_controllers.yaml
new file mode 100644
index 00000000..e4d2eb00
--- /dev/null
+++ b/src/wroboarm_23/config/gazebo_controllers.yaml
@@ -0,0 +1,4 @@
+# Publish joint_states
+joint_state_controller:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
diff --git a/src/wroboarm_23/config/gazebo_wroboarm_23.urdf b/src/wroboarm_23/config/gazebo_wroboarm_23.urdf
new file mode 100644
index 00000000..e4edc521
--- /dev/null
+++ b/src/wroboarm_23/config/gazebo_wroboarm_23.urdf
@@ -0,0 +1,262 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
+     Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578
+     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot name="wroboarm_23">
+    <link name="base_link">
+        <inertial>
+            <origin xyz="9.1830074983592E-19 -9.80159456515381E-19 0.015606490089144" rpy="0 0 0" />
+            <mass value="0.93972387307473" />
+            <inertia ixx="0.00251429022255044" ixy="7.37485241366082E-20" ixz="8.66637218064247E-21" iyy="0.0021336882069818" iyz="1.14091331088149E-19" izz="0.0044229329573873" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://wroboarm_23/meshes/base_link.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://wroboarm_23/meshes/base_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <link name="turntable_link">
+        <inertial>
+            <origin xyz="-0.01225868218219 0.128577249577976 -0.00370544350042927" rpy="0 0 0" />
+            <mass value="0.996366805002726" />
+            <inertia ixx="0.00652370094208946" ixy="0.0010646616036529" ixz="0.000283127005277363" iyy="0.0050231810993567" iyz="6.82113719161566E-05" izz="0.00863345382595838" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://wroboarm_23/meshes/turntable_link.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://wroboarm_23/meshes/turntable_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="turntable_joint" type="continuous">
+        <origin xyz="0 0 0.035865" rpy="1.5708 0 -0.84509" />
+        <parent link="base_link" />
+        <child link="turntable_link" />
+        <axis xyz="0 1 0" />
+    </joint>
+    <link name="upperArm_link">
+        <inertial>
+            <origin xyz="0.0143973999054599 -3.20061625693202E-05 0.279348384794928" rpy="0 0 0" />
+            <mass value="0.985210343915811" />
+            <inertia ixx="0.0237982224738338" ixy="-8.8797773328906E-07" ixz="-0.00388532856230116" iyy="0.0293594722597601" iyz="-7.129443699684E-06" izz="0.00625034839320102" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://wroboarm_23/meshes/upperArm_link.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://wroboarm_23/meshes/upperArm_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="shoulder_joint" type="continuous">
+        <origin xyz="0 0.18098 0.043071" rpy="-1.5572 0 0" />
+        <parent link="turntable_link" />
+        <child link="upperArm_link" />
+        <axis xyz="-1 0 0" />
+    </joint>
+    <link name="forearm_link">
+        <inertial>
+            <origin xyz="-4.1286418728248E-16 0.0550154986611603 0.120152803034247" rpy="0 0 0" />
+            <mass value="0.51779610173045" />
+            <inertia ixx="0.00404850874710764" ixy="-4.33680868994202E-19" ixz="2.87313575708659E-18" iyy="0.00375548329392275" iyz="-0.000377385363706531" izz="0.000788327838226511" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://wroboarm_23/meshes/forearm_link.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://wroboarm_23/meshes/forearm_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="elbowPitch_joint" type="continuous">
+        <origin xyz="0 0 0.46634" rpy="2.7009 0 0" />
+        <parent link="upperArm_link" />
+        <child link="forearm_link" />
+        <axis xyz="-1 0 0" />
+    </joint>
+    <link name="wrist_link">
+        <inertial>
+            <origin xyz="-2.77555756156289E-17 5.55111512312578E-17 0.096820252622317" rpy="0 0 0" />
+            <mass value="0.301763082175336" />
+            <inertia ixx="0.00210344289702837" ixy="1.30104260698261E-18" ixz="5.14996031930615E-19" iyy="0.00305016250706558" iyz="-1.15196480826585E-18" izz="0.00107609106501786" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://wroboarm_23/meshes/wrist_link.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://wroboarm_23/meshes/wrist_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="elbowRoll_joint" type="continuous">
+        <origin xyz="0 0.04445 0.23038" rpy="0 0 0" />
+        <parent link="forearm_link" />
+        <child link="wrist_link" />
+        <axis xyz="0 0 1" />
+    </joint>
+    <link name="carpus_link">
+        <inertial>
+            <origin xyz="5.55111512312578E-17 5.55111512312578E-17 0" rpy="0 0 0" />
+            <mass value="0.447707117943177" />
+            <inertia ixx="0.000367702266904795" ixy="7.04731412115578E-19" ixz="1.62630325872826E-19" iyy="0.000843427086265753" iyz="0" izz="0.000844746123560661" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://wroboarm_23/meshes/carpus_link.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://wroboarm_23/meshes/carpus_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="wristPitch_joint" type="continuous">
+        <origin xyz="0 0 0.23528" rpy="0 0 0" />
+        <parent link="wrist_link" />
+        <child link="carpus_link" />
+        <axis xyz="-1 0 0" />
+    </joint>
+    <link name="endEffector_link">
+        <inertial>
+            <origin xyz="-0.000725623905114631 -0.000893578002320872 0.0499824104113341" rpy="0 0 0" />
+            <mass value="0.399994299548626" />
+            <inertia ixx="0.00169604127916281" ixy="-3.07411212276679E-06" ixz="2.57918978917245E-05" iyy="0.00173351996343527" iyz="5.0605971705242E-05" izz="0.000256533582960489" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://wroboarm_23/meshes/endEffector_link.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://wroboarm_23/meshes/endEffector_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="wristRoll_link" type="continuous">
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <parent link="carpus_link" />
+        <child link="endEffector_link" />
+        <axis xyz="0 0 1" />
+    </joint>
+    <transmission name="trans_turntable_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="turntable_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="turntable_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_shoulder_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="shoulder_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="shoulder_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_elbowPitch_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="elbowPitch_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="elbowPitch_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_elbowRoll_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="elbowRoll_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="elbowRoll_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_wristPitch_joint">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="wristPitch_joint">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="wristPitch_joint_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_wristRoll_link">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="wristRoll_link">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="wristRoll_link_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <gazebo>
+        <plugin name="gazebo_ros_control">
+            <robotNamespace>/</robotNamespace>
+        </plugin>
+    </gazebo>
+</robot>
+
diff --git a/src/wroboarm_23/config/joint_limits.yaml b/src/wroboarm_23/config/joint_limits.yaml
new file mode 100644
index 00000000..ae579b8e
--- /dev/null
+++ b/src/wroboarm_23/config/joint_limits.yaml
@@ -0,0 +1,52 @@
+# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+
+# For beginners, we downscale velocity and acceleration limits.
+# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
+default_velocity_scaling_factor: 1.0
+default_acceleration_scaling_factor: 1.0
+
+# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
+# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+joint_limits:
+  elbowPitch_joint:
+    has_velocity_limits: true
+    max_velocity: 10
+    has_acceleration_limits: true
+    max_acceleration: 10
+    max_position: !radians 2.2
+    min_position: !radians -0.2
+  elbowRoll_joint:
+    has_velocity_limits: true
+    max_velocity: 10
+    has_acceleration_limits: true
+    max_acceleration: 10
+    max_position: !radians 0.7
+    min_position: !radians -0.7
+  shoulder_joint:
+    has_velocity_limits: true
+    max_velocity: 10
+    has_acceleration_limits: true
+    max_acceleration: 10
+    max_position: !radians 0
+    min_position: !radians -1.3
+  turntable_joint:
+    has_velocity_limits: true
+    max_velocity: 10
+    has_acceleration_limits: true
+    max_acceleration: 10
+    max_position: !radians pi / 2
+    min_position: !radians 0
+  wristPitch_joint:
+    has_velocity_limits: true
+    max_velocity: 10
+    has_acceleration_limits: true
+    max_acceleration: 10
+    max_position: !radians 2.5
+    min_position: !radians -2.5
+  wristRoll_link:
+    has_velocity_limits: true
+    max_velocity: 10
+    has_acceleration_limits: true
+    max_acceleration: 10
+    max_position: !radians pi
+    min_position: !radians -pi
\ No newline at end of file
diff --git a/src/wroboarm_23/config/joint_names_ArmAssembly.SLDASM.yaml b/src/wroboarm_23/config/joint_names_ArmAssembly.SLDASM.yaml
new file mode 100644
index 00000000..05cd2083
--- /dev/null
+++ b/src/wroboarm_23/config/joint_names_ArmAssembly.SLDASM.yaml
@@ -0,0 +1 @@
+controller_joint_names: ['', 'turntable_joint', 'shoulder_joint', 'elbowPitch_joint', 'elbowRoll_joint', 'wristPitch_joint', 'wristRoll_link', ]
diff --git a/src/wroboarm_23/config/kinematics.yaml b/src/wroboarm_23/config/kinematics.yaml
new file mode 100644
index 00000000..05573e5c
--- /dev/null
+++ b/src/wroboarm_23/config/kinematics.yaml
@@ -0,0 +1,4 @@
+arm:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.005
\ No newline at end of file
diff --git a/src/wroboarm_23/config/ompl_planning.yaml b/src/wroboarm_23/config/ompl_planning.yaml
new file mode 100644
index 00000000..5e0528df
--- /dev/null
+++ b/src/wroboarm_23/config/ompl_planning.yaml
@@ -0,0 +1,157 @@
+planner_configs:
+  AnytimePathShortening:
+    type: geometric::AnytimePathShortening
+    shortcut: true  # Attempt to shortcut all new solution paths
+    hybridize: true  # Compute hybrid solution trajectories
+    max_hybrid_paths: 24  # Number of hybrid paths generated per iteration
+    num_planners: 4  # The number of default planners to use for planning
+    planners: ""  # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]"
+  SBL:
+    type: geometric::SBL
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  EST:
+    type: geometric::EST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+  LBKPIECE:
+    type: geometric::LBKPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  BKPIECE:
+    type: geometric::BKPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9
+    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  KPIECE:
+    type: geometric::KPIECE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    border_fraction: 0.9  # Fraction of time focused on boarder default: 0.9 (0.0,1.]
+    failed_expansion_score_factor: 0.5  # When extending motion fails, scale score by factor. default: 0.5
+    min_valid_path_fraction: 0.5  # Accept partially valid moves above fraction. default: 0.5
+  RRT:
+    type: geometric::RRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+  RRTConnect:
+    type: geometric::RRTConnect
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  RRTstar:
+    type: geometric::RRTstar
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1
+  TRRT:
+    type: geometric::TRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
+    max_states_failed: 10  # when to start increasing temp. default: 10
+    temp_change_factor: 2.0  # how much to increase or decrease temp. default: 2.0
+    min_temperature: 10e-10  # lower limit of temp change. default: 10e-10
+    init_temperature: 10e-6  # initial temperature. default: 10e-6
+    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+    frountierNodeRatio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+    k_constant: 0.0  # value used to normalize expresssion. default: 0.0 set in setup()
+  PRM:
+    type: geometric::PRM
+    max_nearest_neighbors: 10  # use k nearest neighbors. default: 10
+  PRMstar:
+    type: geometric::PRMstar
+  FMT:
+    type: geometric::FMT
+    num_samples: 1000  # number of states that the planner should sample. default: 1000
+    radius_multiplier: 1.1  # multiplier used for the nearest neighbors search radius. default: 1.1
+    nearest_k: 1  # use Knearest strategy. default: 1
+    cache_cc: 1  # use collision checking cache. default: 1
+    heuristics: 0  # activate cost to go heuristics. default: 0
+    extended_fmt: 1  # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1
+  BFMT:
+    type: geometric::BFMT
+    num_samples: 1000  # number of states that the planner should sample. default: 1000
+    radius_multiplier: 1.0  # multiplier used for the nearest neighbors search radius. default: 1.0
+    nearest_k: 1  # use the Knearest strategy. default: 1
+    balanced: 0  # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1
+    optimality: 1  # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1
+    heuristics: 1  # activates cost to go heuristics. default: 1
+    cache_cc: 1  # use the collision checking cache. default: 1
+    extended_fmt: 1  # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1
+  PDST:
+    type: geometric::PDST
+  STRIDE:
+    type: geometric::STRIDE
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    use_projected_distance: 0  # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0
+    degree: 16  # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16
+    max_degree: 18  # max degree of a node in the GNAT. default: 12
+    min_degree: 12  # min degree of a node in the GNAT. default: 12
+    max_pts_per_leaf: 6  # max points per leaf in the GNAT. default: 6
+    estimated_dimension: 0.0  # estimated dimension of the free space. default: 0.0
+    min_valid_path_fraction: 0.2  # Accept partially valid moves above fraction. default: 0.2
+  BiTRRT:
+    type: geometric::BiTRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    temp_change_factor: 0.1  # how much to increase or decrease temp. default: 0.1
+    init_temperature: 100  # initial temperature. default: 100
+    frountier_threshold: 0.0  # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
+    frountier_node_ratio: 0.1  # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1
+    cost_threshold: 1e300  # the cost threshold. Any motion cost that is not better will not be expanded. default: inf
+  LBTRRT:
+    type: geometric::LBTRRT
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+    epsilon: 0.4  # optimality approximation factor. default: 0.4
+  BiEST:
+    type: geometric::BiEST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  ProjEST:
+    type: geometric::ProjEST
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+    goal_bias: 0.05  # When close to goal select goal, with this probability. default: 0.05
+  LazyPRM:
+    type: geometric::LazyPRM
+    range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
+  LazyPRMstar:
+    type: geometric::LazyPRMstar
+  SPARS:
+    type: geometric::SPARS
+    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
+    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
+    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
+    max_failures: 1000  # maximum consecutive failure limit. default: 1000
+  SPARStwo:
+    type: geometric::SPARStwo
+    stretch_factor: 3.0  # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0
+    sparse_delta_fraction: 0.25  # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
+    dense_delta_fraction: 0.001  # delta fraction for interface detection. default: 0.001
+    max_failures: 5000  # maximum consecutive failure limit. default: 5000
+arm:
+  planner_configs:
+    - AnytimePathShortening
+    - SBL
+    - EST
+    - LBKPIECE
+    - BKPIECE
+    - KPIECE
+    - RRT
+    - RRTConnect
+    - RRTstar
+    - TRRT
+    - PRM
+    - PRMstar
+    - FMT
+    - BFMT
+    - PDST
+    - STRIDE
+    - BiTRRT
+    - LBTRRT
+    - BiEST
+    - ProjEST
+    - LazyPRM
+    - LazyPRMstar
+    - SPARS
+    - SPARStwo
+  projection_evaluator: joints(turntable_joint,shoulder_joint)
+  longest_valid_segment_fraction: 0.005
diff --git a/src/wroboarm_23/config/ros_controllers.yaml b/src/wroboarm_23/config/ros_controllers.yaml
new file mode 100644
index 00000000..a79ae41f
--- /dev/null
+++ b/src/wroboarm_23/config/ros_controllers.yaml
@@ -0,0 +1,12 @@
+controller_list:
+  - name: arm_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: True
+    joints:
+      - turntable_joint
+      - shoulder_joint
+      - elbowPitch_joint
+      - elbowRoll_joint
+      - wristPitch_joint
+      - wristRoll_link
\ No newline at end of file
diff --git a/src/wroboarm_23/config/sensors_3d.yaml b/src/wroboarm_23/config/sensors_3d.yaml
new file mode 100644
index 00000000..51010a36
--- /dev/null
+++ b/src/wroboarm_23/config/sensors_3d.yaml
@@ -0,0 +1,2 @@
+sensors:
+  []
\ No newline at end of file
diff --git a/src/wroboarm_23/config/simple_moveit_controllers.yaml b/src/wroboarm_23/config/simple_moveit_controllers.yaml
new file mode 100644
index 00000000..a79ae41f
--- /dev/null
+++ b/src/wroboarm_23/config/simple_moveit_controllers.yaml
@@ -0,0 +1,12 @@
+controller_list:
+  - name: arm_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: True
+    joints:
+      - turntable_joint
+      - shoulder_joint
+      - elbowPitch_joint
+      - elbowRoll_joint
+      - wristPitch_joint
+      - wristRoll_link
\ No newline at end of file
diff --git a/src/wroboarm_23/config/stomp_planning.yaml b/src/wroboarm_23/config/stomp_planning.yaml
new file mode 100644
index 00000000..ece56380
--- /dev/null
+++ b/src/wroboarm_23/config/stomp_planning.yaml
@@ -0,0 +1,39 @@
+stomp/arm:
+  group_name: arm
+  optimization:
+    num_timesteps: 60
+    num_iterations: 40
+    num_iterations_after_valid: 0
+    num_rollouts: 30
+    max_rollouts: 30
+    initialization_method: 1  # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]
+    control_cost_weight: 0.0
+  task:
+    noise_generator:
+      - class: stomp_moveit/NormalDistributionSampling
+        stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
+    cost_functions:
+      - class: stomp_moveit/CollisionCheck
+        collision_penalty: 1.0
+        cost_weight: 1.0
+        kernel_window_percentage: 0.2
+        longest_valid_joint_move: 0.05
+    noisy_filters:
+      - class: stomp_moveit/JointLimits
+        lock_start: True
+        lock_goal: True
+      - class: stomp_moveit/MultiTrajectoryVisualization
+        line_width: 0.02
+        rgb: [255, 255, 0]
+        marker_array_topic: stomp_trajectories
+        marker_namespace: noisy
+    update_filters:
+      - class: stomp_moveit/PolynomialSmoother
+        poly_order: 6
+      - class: stomp_moveit/TrajectoryVisualization
+        line_width: 0.05
+        rgb: [0, 191, 255]
+        error_rgb: [255, 0, 0]
+        publish_intermediate: True
+        marker_topic: stomp_trajectory
+        marker_namespace: optimized
\ No newline at end of file
diff --git a/src/wroboarm_23/config/wroboarm_23.srdf b/src/wroboarm_23/config/wroboarm_23.srdf
new file mode 100644
index 00000000..f3d67e92
--- /dev/null
+++ b/src/wroboarm_23/config/wroboarm_23.srdf
@@ -0,0 +1,28 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="wroboarm_23">
+    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <group name="arm">
+        <chain base_link="base_link" tip_link="endEffector_link"/>
+    </group>
+    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+    <end_effector name="hand" parent_link="endEffector_link" group="arm"/>
+    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
+    <virtual_joint name="odom_combined" type="fixed" parent_frame="world" child_link="base_link"/>
+    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <disable_collisions link1="base_link" link2="turntable_link" reason="Adjacent"/>
+    <disable_collisions link1="carpus_link" link2="endEffector_link" reason="Adjacent"/>
+    <disable_collisions link1="carpus_link" link2="forearm_link" reason="Never"/>
+    <disable_collisions link1="carpus_link" link2="wrist_link" reason="Adjacent"/>
+    <disable_collisions link1="endEffector_link" link2="forearm_link" reason="Never"/>
+    <disable_collisions link1="forearm_link" link2="upperArm_link" reason="Adjacent"/>
+    <disable_collisions link1="forearm_link" link2="wrist_link" reason="Adjacent"/>
+    <disable_collisions link1="turntable_link" link2="upperArm_link" reason="Adjacent"/>
+</robot>
diff --git a/src/wroboarm_23/export.log b/src/wroboarm_23/export.log
new file mode 100644
index 00000000..f12be56f
--- /dev/null
+++ b/src/wroboarm_23/export.log
@@ -0,0 +1,705 @@
+2022-04-26 21:16:19,486 INFO  Logger.cs: 70 - 
+--------------------------------------------------------------------------------
+2022-04-26 21:16:19,505 INFO  Logger.cs: 71 - Logging commencing for SW2URDF exporter
+2022-04-26 21:16:19,506 INFO  Logger.cs: 73 - Commit version 1.6.0-4-g7f85cfe
+2022-04-26 21:16:19,506 INFO  Logger.cs: 74 - Build version 1.6.7995.38578
+2022-04-26 21:16:19,508 INFO  SwAddin.cs: 192 - Attempting to connect to SW
+2022-04-26 21:16:19,508 INFO  SwAddin.cs: 197 - Setting up callbacks
+2022-04-26 21:16:19,509 INFO  SwAddin.cs: 201 - Setting up command manager
+2022-04-26 21:16:19,509 INFO  SwAddin.cs: 204 - Adding command manager
+2022-04-26 21:16:19,511 INFO  SwAddin.cs: 263 - Adding Assembly export to file menu
+2022-04-26 21:16:19,512 INFO  SwAddin.cs: 272 - Adding Part export to file menu
+2022-04-26 21:16:19,512 INFO  SwAddin.cs: 210 - Adding event handlers
+2022-04-26 21:16:19,514 INFO  SwAddin.cs: 217 - Connecting plugin to SolidWorks
+2022-04-26 21:19:01,163 INFO  SwAddin.cs: 294 - Assembly export called for file ArmAssembly.SLDASM
+2022-04-26 21:19:03,867 INFO  SwAddin.cs: 313 - Saving assembly
+2022-04-26 21:19:05,013 INFO  SwAddin.cs: 316 - Opening property manager
+2022-04-26 21:19:05,060 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM
+2022-04-26 21:19:05,088 INFO  ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM
+2022-04-26 21:19:05,089 INFO  ExportHelperExtension.cs: 1145 - Found 1 features of type [CoordSys] in ArmAssembly.SLDASM
+2022-04-26 21:19:05,089 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 21:19:05,111 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 21:19:05,122 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1
+2022-04-26 21:19:05,123 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 21:19:05,123 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1
+2022-04-26 21:19:05,123 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1
+2022-04-26 21:19:05,124 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 21:19:05,124 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1
+2022-04-26 21:19:05,125 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1
+2022-04-26 21:19:05,125 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 21:19:05,125 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1
+2022-04-26 21:19:05,126 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1
+2022-04-26 21:19:05,126 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 21:19:05,127 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1
+2022-04-26 21:19:05,127 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1
+2022-04-26 21:19:05,127 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 21:19:05,128 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1
+2022-04-26 21:19:05,128 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1
+2022-04-26 21:19:05,128 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 21:19:05,129 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1
+2022-04-26 21:19:05,129 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1
+2022-04-26 21:19:05,187 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 21:19:05,187 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1
+2022-04-26 21:19:05,188 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM
+2022-04-26 21:19:05,188 INFO  ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM
+2022-04-26 21:19:05,189 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM
+2022-04-26 21:19:05,189 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 21:19:05,189 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 21:19:05,190 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1
+2022-04-26 21:19:05,190 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 21:19:05,190 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1
+2022-04-26 21:19:05,191 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1
+2022-04-26 21:19:05,191 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 21:19:05,191 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1
+2022-04-26 21:19:05,192 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1
+2022-04-26 21:19:05,192 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 21:19:05,192 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1
+2022-04-26 21:19:05,193 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1
+2022-04-26 21:19:05,193 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 21:19:05,193 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1
+2022-04-26 21:19:05,194 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1
+2022-04-26 21:19:05,194 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 21:19:05,194 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1
+2022-04-26 21:19:05,195 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1
+2022-04-26 21:19:05,195 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 21:19:05,195 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1
+2022-04-26 21:19:05,196 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1
+2022-04-26 21:19:05,196 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 21:19:05,196 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1
+2022-04-26 21:19:05,777 INFO  SwAddin.cs: 339 - Loading config tree
+2022-04-26 21:19:05,789 INFO  ConfigurationSerialization.cs: 276 - URDF Configuration found
+<Link z:Id="i1" xmlns="http://schemas.datacontract.org/2004/07/SW2URDF" xmlns:i="http://www.w3.org/2001/XMLSchema-instance" xmlns:z="http://schemas.microsoft.com/2003/10/Serialization/"><Attributes><Attribute z:Id="i2"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">base_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i3" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i4" i:type="Origin"><Attributes><Attribute z:Id="i5"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>9.1830074983591922E-19</a:double><a:double>0.015606490089143955</a:double><a:double>1.5715252581589567E-18</a:double></Value></Attribute><Attribute z:Id="i6"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i6"/><XYZAttribute z:Ref="i5"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i7" i:type="Mass"><Attributes><Attribute z:Id="i8"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.93972387307473026</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i8"/></URDFElement><URDFElement z:Id="i9" i:type="Inertia"><Attributes><Attribute z:Id="i10"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0025142902225504422</Value></Attribute><Attribute z:Id="i11"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">8.02209898558041E-21</Value></Attribute><Attribute z:Id="i12"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">-7.374852413660824E-20</Value></Attribute><Attribute z:Id="i13"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0044229329573873041</Value></Attribute><Attribute z:Id="i14"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">2.2568808327940613E-20</Value></Attribute><Attribute z:Id="i15"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0021336882069818023</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i10"/><IxyAttribute z:Ref="i11"/><IxzAttribute z:Ref="i12"/><IyyAttribute z:Ref="i13"/><IyzAttribute z:Ref="i14"/><IzzAttribute z:Ref="i15"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i9"/><Mass z:Ref="i7"/><Origin z:Ref="i4"/></URDFElement><URDFElement z:Id="i16" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i17" i:type="Origin"><Attributes><Attribute z:Id="i18"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i19"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i19"/><XYZAttribute z:Ref="i18"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i20" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i21" i:type="Mesh"><Attributes><Attribute z:Id="i22"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i22"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i21"/></URDFElement><URDFElement z:Id="i23" i:type="Material"><Attributes><Attribute z:Id="i24"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i25" i:type="Color"><Attributes><Attribute z:Id="i26"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0.792156862745098</a:double><a:double>0.81960784313725488</a:double><a:double>0.93333333333333335</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i26"/></URDFElement><URDFElement z:Id="i27" i:type="Texture"><Attributes><Attribute z:Id="i28"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i28"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i25"/><NameAttribute z:Ref="i24"/><Texture z:Ref="i27"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i20"/><Material z:Ref="i23"/><Origin z:Ref="i17"/></URDFElement><URDFElement z:Id="i29" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i30" i:type="Origin"><Attributes><Attribute z:Id="i31"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i32"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i32"/><XYZAttribute z:Ref="i31"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i33" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i34" i:type="Mesh"><Attributes><Attribute z:Id="i35"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i35"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i34"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i33"/><Origin z:Ref="i30"/></URDFElement><URDFElement z:Id="i36" i:type="Joint"><Attributes><Attribute z:Id="i37"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute><Attribute z:Id="i38"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i39" i:type="Origin"><Attributes><Attribute z:Id="i40"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i41"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i41"/><XYZAttribute z:Ref="i40"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i42" i:type="ParentLink"><Attributes><Attribute z:Id="i43"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i43"/></URDFElement><URDFElement z:Id="i44" i:type="ChildLink"><Attributes><Attribute z:Id="i45"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i45"/></URDFElement><URDFElement z:Id="i46" i:type="Axis"><Attributes><Attribute z:Id="i47"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i47"/></URDFElement><URDFElement z:Id="i48" i:type="Limit"><Attributes><Attribute z:Id="i49"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i50"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i51"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i52"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i51"/><LowerAttribute z:Ref="i49"/><UpperAttribute z:Ref="i50"/><VelocityAttribute z:Ref="i52"/></URDFElement><URDFElement z:Id="i53" i:type="Calibration"><Attributes><Attribute z:Id="i54"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i55"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i55"/><RisingAttribute z:Ref="i54"/></URDFElement><URDFElement z:Id="i56" i:type="Dynamics"><Attributes><Attribute z:Id="i57"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i58"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i57"/><FrictionAttribute z:Ref="i58"/></URDFElement><URDFElement z:Id="i59" i:type="SafetyController"><Attributes><Attribute z:Id="i60"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i61"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i62"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i63"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i62"/><KVelocityAttribute z:Ref="i63"/><SoftLowerAttribute z:Ref="i61"/><SoftUpperAttribute z:Ref="i60"/></URDFElement><URDFElement z:Id="i64" i:type="Mimic"><Attributes><Attribute z:Id="i65"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i66"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i67"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i65"/><MultiplierAttribute z:Ref="i66"/><OffsetAttribute z:Ref="i67"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i46"/><AxisName/><Calibration z:Ref="i53"/><Child z:Ref="i44"/><CoordinateSystemName>Origin_global</CoordinateSystemName><Dynamics z:Ref="i56"/><Limit z:Ref="i48"/><Mimic z:Ref="i64"/><NameAttribute z:Ref="i37"/><Origin z:Ref="i39"/><Parent z:Ref="i42"/><Safety z:Ref="i59"/><TypeAttribute z:Ref="i38"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children/><Collision z:Ref="i29"/><Inertial z:Ref="i3"/><Joint z:Ref="i36"/><NameAttribute z:Ref="i2"/><Parent i:nil="true"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i16"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link>
+2022-04-26 21:19:05,911 INFO  LinkNode.cs: 35 - Building node base_link
+2022-04-26 21:19:05,914 INFO  CommonSwOperations.cs: 221 - Loading SolidWorks components for base_link from E:\Robotics\WR\URDF\ArmAssembly.SLDASM
+2022-04-26 21:19:05,915 INFO  CommonSwOperations.cs: 245 - Loading component with PID ?6???base-1@ArmAssembly


+2022-04-26 21:19:05,935 INFO  CommonSwOperations.cs: 254 - Successfully loaded component E:\Robotics\WR\URDF\base.SLDPRT
+2022-04-26 21:19:05,935 INFO  CommonSwOperations.cs: 230 - Loaded 1 components for link base_link
+2022-04-26 21:19:05,981 INFO  SwAddin.cs: 344 - Showing property manager
+2022-04-26 21:19:16,795 INFO  ExportPropertyManager.cs: 422 - Configuration saved
+2022-04-26 21:19:16,812 INFO  ConfigurationSerialization.cs: 276 - URDF Configuration found
+<Link z:Id="i1" xmlns="http://schemas.datacontract.org/2004/07/SW2URDF" xmlns:i="http://www.w3.org/2001/XMLSchema-instance" xmlns:z="http://schemas.microsoft.com/2003/10/Serialization/"><Attributes><Attribute z:Id="i2"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">base_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i3" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i4" i:type="Origin"><Attributes><Attribute z:Id="i5"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>9.1830074983591922E-19</a:double><a:double>0.015606490089143955</a:double><a:double>1.5715252581589567E-18</a:double></Value></Attribute><Attribute z:Id="i6"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i6"/><XYZAttribute z:Ref="i5"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i7" i:type="Mass"><Attributes><Attribute z:Id="i8"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.93972387307473026</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i8"/></URDFElement><URDFElement z:Id="i9" i:type="Inertia"><Attributes><Attribute z:Id="i10"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0025142902225504422</Value></Attribute><Attribute z:Id="i11"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">8.02209898558041E-21</Value></Attribute><Attribute z:Id="i12"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">-7.374852413660824E-20</Value></Attribute><Attribute z:Id="i13"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0044229329573873041</Value></Attribute><Attribute z:Id="i14"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">2.2568808327940613E-20</Value></Attribute><Attribute z:Id="i15"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0021336882069818023</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i10"/><IxyAttribute z:Ref="i11"/><IxzAttribute z:Ref="i12"/><IyyAttribute z:Ref="i13"/><IyzAttribute z:Ref="i14"/><IzzAttribute z:Ref="i15"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i9"/><Mass z:Ref="i7"/><Origin z:Ref="i4"/></URDFElement><URDFElement z:Id="i16" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i17" i:type="Origin"><Attributes><Attribute z:Id="i18"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i19"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i19"/><XYZAttribute z:Ref="i18"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i20" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i21" i:type="Mesh"><Attributes><Attribute z:Id="i22"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i22"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i21"/></URDFElement><URDFElement z:Id="i23" i:type="Material"><Attributes><Attribute z:Id="i24"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i25" i:type="Color"><Attributes><Attribute z:Id="i26"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0.792156862745098</a:double><a:double>0.81960784313725488</a:double><a:double>0.93333333333333335</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i26"/></URDFElement><URDFElement z:Id="i27" i:type="Texture"><Attributes><Attribute z:Id="i28"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i28"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i25"/><NameAttribute z:Ref="i24"/><Texture z:Ref="i27"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i20"/><Material z:Ref="i23"/><Origin z:Ref="i17"/></URDFElement><URDFElement z:Id="i29" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i30" i:type="Origin"><Attributes><Attribute z:Id="i31"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i32"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i32"/><XYZAttribute z:Ref="i31"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i33" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i34" i:type="Mesh"><Attributes><Attribute z:Id="i35"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i35"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i34"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i33"/><Origin z:Ref="i30"/></URDFElement><URDFElement z:Id="i36" i:type="Joint"><Attributes><Attribute z:Id="i37"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute><Attribute z:Id="i38"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i39" i:type="Origin"><Attributes><Attribute z:Id="i40"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i41"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i41"/><XYZAttribute z:Ref="i40"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i42" i:type="ParentLink"><Attributes><Attribute z:Id="i43"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i43"/></URDFElement><URDFElement z:Id="i44" i:type="ChildLink"><Attributes><Attribute z:Id="i45"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i45"/></URDFElement><URDFElement z:Id="i46" i:type="Axis"><Attributes><Attribute z:Id="i47"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i47"/></URDFElement><URDFElement z:Id="i48" i:type="Limit"><Attributes><Attribute z:Id="i49"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i50"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i51"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i52"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i51"/><LowerAttribute z:Ref="i49"/><UpperAttribute z:Ref="i50"/><VelocityAttribute z:Ref="i52"/></URDFElement><URDFElement z:Id="i53" i:type="Calibration"><Attributes><Attribute z:Id="i54"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i55"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i55"/><RisingAttribute z:Ref="i54"/></URDFElement><URDFElement z:Id="i56" i:type="Dynamics"><Attributes><Attribute z:Id="i57"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i58"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i57"/><FrictionAttribute z:Ref="i58"/></URDFElement><URDFElement z:Id="i59" i:type="SafetyController"><Attributes><Attribute z:Id="i60"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i61"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i62"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i63"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i62"/><KVelocityAttribute z:Ref="i63"/><SoftLowerAttribute z:Ref="i61"/><SoftUpperAttribute z:Ref="i60"/></URDFElement><URDFElement z:Id="i64" i:type="Mimic"><Attributes><Attribute z:Id="i65"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i66"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i67"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i65"/><MultiplierAttribute z:Ref="i66"/><OffsetAttribute z:Ref="i67"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i46"/><AxisName/><Calibration z:Ref="i53"/><Child z:Ref="i44"/><CoordinateSystemName>Origin_global</CoordinateSystemName><Dynamics z:Ref="i56"/><Limit z:Ref="i48"/><Mimic z:Ref="i64"/><NameAttribute z:Ref="i37"/><Origin z:Ref="i39"/><Parent z:Ref="i42"/><Safety z:Ref="i59"/><TypeAttribute z:Ref="i38"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children/><Collision z:Ref="i29"/><Inertial z:Ref="i3"/><Joint z:Ref="i36"/><NameAttribute z:Ref="i2"/><Parent i:nil="true"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i16"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link>
+2022-04-26 21:19:16,903 INFO  ExportPropertyManager.cs: 1142 - AfterClose called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:19:54,237 INFO  SwAddin.cs: 294 - Assembly export called for file ArmAssembly.SLDASM
+2022-04-26 21:19:55,264 INFO  SwAddin.cs: 313 - Saving assembly
+2022-04-26 21:19:55,326 INFO  SwAddin.cs: 316 - Opening property manager
+2022-04-26 21:19:55,326 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM
+2022-04-26 21:19:55,327 INFO  ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM
+2022-04-26 21:19:55,327 INFO  ExportHelperExtension.cs: 1145 - Found 1 features of type [CoordSys] in ArmAssembly.SLDASM
+2022-04-26 21:19:55,328 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 21:19:55,328 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 21:19:55,328 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1
+2022-04-26 21:19:55,329 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 21:19:55,329 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1
+2022-04-26 21:19:55,329 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1
+2022-04-26 21:19:55,330 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 21:19:55,330 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1
+2022-04-26 21:19:55,330 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1
+2022-04-26 21:19:55,331 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 21:19:55,331 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1
+2022-04-26 21:19:55,331 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1
+2022-04-26 21:19:55,332 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 21:19:55,332 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1
+2022-04-26 21:19:55,340 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1
+2022-04-26 21:19:55,360 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 21:19:55,360 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1
+2022-04-26 21:19:55,361 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1
+2022-04-26 21:19:55,361 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 21:19:55,361 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1
+2022-04-26 21:19:55,362 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1
+2022-04-26 21:19:55,362 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 21:19:55,363 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1
+2022-04-26 21:19:55,363 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM
+2022-04-26 21:19:55,364 INFO  ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM
+2022-04-26 21:19:55,364 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM
+2022-04-26 21:19:55,364 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 21:19:55,365 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 21:19:55,365 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1
+2022-04-26 21:19:55,365 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 21:19:55,366 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1
+2022-04-26 21:19:55,366 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1
+2022-04-26 21:19:55,366 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 21:19:55,367 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1
+2022-04-26 21:19:55,367 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1
+2022-04-26 21:19:55,367 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 21:19:55,368 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1
+2022-04-26 21:19:55,368 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1
+2022-04-26 21:19:55,369 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 21:19:55,369 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1
+2022-04-26 21:19:55,369 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1
+2022-04-26 21:19:55,370 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 21:19:55,370 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1
+2022-04-26 21:19:55,370 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1
+2022-04-26 21:19:55,371 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 21:19:55,371 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1
+2022-04-26 21:19:55,371 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1
+2022-04-26 21:19:55,372 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 21:19:55,372 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1
+2022-04-26 21:19:55,413 INFO  SwAddin.cs: 339 - Loading config tree
+2022-04-26 21:19:55,413 INFO  ConfigurationSerialization.cs: 276 - URDF Configuration found
+<Link z:Id="i1" xmlns="http://schemas.datacontract.org/2004/07/SW2URDF" xmlns:i="http://www.w3.org/2001/XMLSchema-instance" xmlns:z="http://schemas.microsoft.com/2003/10/Serialization/"><Attributes><Attribute z:Id="i2"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">base_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i3" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i4" i:type="Origin"><Attributes><Attribute z:Id="i5"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>9.1830074983591922E-19</a:double><a:double>0.015606490089143955</a:double><a:double>1.5715252581589567E-18</a:double></Value></Attribute><Attribute z:Id="i6"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i6"/><XYZAttribute z:Ref="i5"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i7" i:type="Mass"><Attributes><Attribute z:Id="i8"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.93972387307473026</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i8"/></URDFElement><URDFElement z:Id="i9" i:type="Inertia"><Attributes><Attribute z:Id="i10"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0025142902225504422</Value></Attribute><Attribute z:Id="i11"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">8.02209898558041E-21</Value></Attribute><Attribute z:Id="i12"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">-7.374852413660824E-20</Value></Attribute><Attribute z:Id="i13"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0044229329573873041</Value></Attribute><Attribute z:Id="i14"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">2.2568808327940613E-20</Value></Attribute><Attribute z:Id="i15"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0021336882069818023</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i10"/><IxyAttribute z:Ref="i11"/><IxzAttribute z:Ref="i12"/><IyyAttribute z:Ref="i13"/><IyzAttribute z:Ref="i14"/><IzzAttribute z:Ref="i15"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i9"/><Mass z:Ref="i7"/><Origin z:Ref="i4"/></URDFElement><URDFElement z:Id="i16" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i17" i:type="Origin"><Attributes><Attribute z:Id="i18"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i19"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i19"/><XYZAttribute z:Ref="i18"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i20" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i21" i:type="Mesh"><Attributes><Attribute z:Id="i22"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i22"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i21"/></URDFElement><URDFElement z:Id="i23" i:type="Material"><Attributes><Attribute z:Id="i24"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i25" i:type="Color"><Attributes><Attribute z:Id="i26"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0.792156862745098</a:double><a:double>0.81960784313725488</a:double><a:double>0.93333333333333335</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i26"/></URDFElement><URDFElement z:Id="i27" i:type="Texture"><Attributes><Attribute z:Id="i28"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i28"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i25"/><NameAttribute z:Ref="i24"/><Texture z:Ref="i27"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i20"/><Material z:Ref="i23"/><Origin z:Ref="i17"/></URDFElement><URDFElement z:Id="i29" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i30" i:type="Origin"><Attributes><Attribute z:Id="i31"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i32"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i32"/><XYZAttribute z:Ref="i31"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i33" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i34" i:type="Mesh"><Attributes><Attribute z:Id="i35"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i35"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i34"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i33"/><Origin z:Ref="i30"/></URDFElement><URDFElement z:Id="i36" i:type="Joint"><Attributes><Attribute z:Id="i37"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute><Attribute z:Id="i38"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i39" i:type="Origin"><Attributes><Attribute z:Id="i40"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i41"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i41"/><XYZAttribute z:Ref="i40"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i42" i:type="ParentLink"><Attributes><Attribute z:Id="i43"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i43"/></URDFElement><URDFElement z:Id="i44" i:type="ChildLink"><Attributes><Attribute z:Id="i45"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i45"/></URDFElement><URDFElement z:Id="i46" i:type="Axis"><Attributes><Attribute z:Id="i47"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i47"/></URDFElement><URDFElement z:Id="i48" i:type="Limit"><Attributes><Attribute z:Id="i49"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i50"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i51"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i52"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i51"/><LowerAttribute z:Ref="i49"/><UpperAttribute z:Ref="i50"/><VelocityAttribute z:Ref="i52"/></URDFElement><URDFElement z:Id="i53" i:type="Calibration"><Attributes><Attribute z:Id="i54"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i55"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i55"/><RisingAttribute z:Ref="i54"/></URDFElement><URDFElement z:Id="i56" i:type="Dynamics"><Attributes><Attribute z:Id="i57"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i58"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i57"/><FrictionAttribute z:Ref="i58"/></URDFElement><URDFElement z:Id="i59" i:type="SafetyController"><Attributes><Attribute z:Id="i60"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i61"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i62"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i63"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i62"/><KVelocityAttribute z:Ref="i63"/><SoftLowerAttribute z:Ref="i61"/><SoftUpperAttribute z:Ref="i60"/></URDFElement><URDFElement z:Id="i64" i:type="Mimic"><Attributes><Attribute z:Id="i65"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i66"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i67"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i65"/><MultiplierAttribute z:Ref="i66"/><OffsetAttribute z:Ref="i67"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i46"/><AxisName/><Calibration z:Ref="i53"/><Child z:Ref="i44"/><CoordinateSystemName>Origin_global</CoordinateSystemName><Dynamics z:Ref="i56"/><Limit z:Ref="i48"/><Mimic z:Ref="i64"/><NameAttribute z:Ref="i37"/><Origin z:Ref="i39"/><Parent z:Ref="i42"/><Safety z:Ref="i59"/><TypeAttribute z:Ref="i38"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children/><Collision z:Ref="i29"/><Inertial z:Ref="i3"/><Joint z:Ref="i36"/><NameAttribute z:Ref="i2"/><Parent i:nil="true"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i16"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link>
+2022-04-26 21:19:55,414 INFO  LinkNode.cs: 35 - Building node base_link
+2022-04-26 21:19:55,414 INFO  CommonSwOperations.cs: 221 - Loading SolidWorks components for base_link from E:\Robotics\WR\URDF\ArmAssembly.SLDASM
+2022-04-26 21:19:55,415 INFO  CommonSwOperations.cs: 245 - Loading component with PID ?6???base-1@ArmAssembly


+2022-04-26 21:19:55,415 INFO  CommonSwOperations.cs: 254 - Successfully loaded component E:\Robotics\WR\URDF\base.SLDPRT
+2022-04-26 21:19:55,415 INFO  CommonSwOperations.cs: 230 - Loaded 1 components for link base_link
+2022-04-26 21:19:55,433 INFO  SwAddin.cs: 344 - Showing property manager
+2022-04-26 21:20:28,795 INFO  ExportPropertyManager.cs: 422 - Configuration saved
+2022-04-26 21:20:28,797 INFO  ConfigurationSerialization.cs: 276 - URDF Configuration found
+<Link z:Id="i1" xmlns="http://schemas.datacontract.org/2004/07/SW2URDF" xmlns:i="http://www.w3.org/2001/XMLSchema-instance" xmlns:z="http://schemas.microsoft.com/2003/10/Serialization/"><Attributes><Attribute z:Id="i2"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">base_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i3" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i4" i:type="Origin"><Attributes><Attribute z:Id="i5"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>9.1830074983591922E-19</a:double><a:double>0.015606490089143955</a:double><a:double>1.5715252581589567E-18</a:double></Value></Attribute><Attribute z:Id="i6"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i6"/><XYZAttribute z:Ref="i5"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i7" i:type="Mass"><Attributes><Attribute z:Id="i8"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.93972387307473026</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i8"/></URDFElement><URDFElement z:Id="i9" i:type="Inertia"><Attributes><Attribute z:Id="i10"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0025142902225504422</Value></Attribute><Attribute z:Id="i11"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">8.02209898558041E-21</Value></Attribute><Attribute z:Id="i12"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">-7.374852413660824E-20</Value></Attribute><Attribute z:Id="i13"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0044229329573873041</Value></Attribute><Attribute z:Id="i14"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">2.2568808327940613E-20</Value></Attribute><Attribute z:Id="i15"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0021336882069818023</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i10"/><IxyAttribute z:Ref="i11"/><IxzAttribute z:Ref="i12"/><IyyAttribute z:Ref="i13"/><IyzAttribute z:Ref="i14"/><IzzAttribute z:Ref="i15"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i9"/><Mass z:Ref="i7"/><Origin z:Ref="i4"/></URDFElement><URDFElement z:Id="i16" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i17" i:type="Origin"><Attributes><Attribute z:Id="i18"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i19"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i19"/><XYZAttribute z:Ref="i18"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i20" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i21" i:type="Mesh"><Attributes><Attribute z:Id="i22"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i22"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i21"/></URDFElement><URDFElement z:Id="i23" i:type="Material"><Attributes><Attribute z:Id="i24"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i25" i:type="Color"><Attributes><Attribute z:Id="i26"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0.792156862745098</a:double><a:double>0.81960784313725488</a:double><a:double>0.93333333333333335</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i26"/></URDFElement><URDFElement z:Id="i27" i:type="Texture"><Attributes><Attribute z:Id="i28"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i28"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i25"/><NameAttribute z:Ref="i24"/><Texture z:Ref="i27"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i20"/><Material z:Ref="i23"/><Origin z:Ref="i17"/></URDFElement><URDFElement z:Id="i29" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i30" i:type="Origin"><Attributes><Attribute z:Id="i31"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i32"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i32"/><XYZAttribute z:Ref="i31"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i33" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i34" i:type="Mesh"><Attributes><Attribute z:Id="i35"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i35"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i34"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i33"/><Origin z:Ref="i30"/></URDFElement><URDFElement z:Id="i36" i:type="Joint"><Attributes><Attribute z:Id="i37"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute><Attribute z:Id="i38"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i39" i:type="Origin"><Attributes><Attribute z:Id="i40"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i41"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i41"/><XYZAttribute z:Ref="i40"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i42" i:type="ParentLink"><Attributes><Attribute z:Id="i43"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i43"/></URDFElement><URDFElement z:Id="i44" i:type="ChildLink"><Attributes><Attribute z:Id="i45"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i45"/></URDFElement><URDFElement z:Id="i46" i:type="Axis"><Attributes><Attribute z:Id="i47"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i47"/></URDFElement><URDFElement z:Id="i48" i:type="Limit"><Attributes><Attribute z:Id="i49"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i50"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i51"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i52"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i51"/><LowerAttribute z:Ref="i49"/><UpperAttribute z:Ref="i50"/><VelocityAttribute z:Ref="i52"/></URDFElement><URDFElement z:Id="i53" i:type="Calibration"><Attributes><Attribute z:Id="i54"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i55"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i55"/><RisingAttribute z:Ref="i54"/></URDFElement><URDFElement z:Id="i56" i:type="Dynamics"><Attributes><Attribute z:Id="i57"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i58"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i57"/><FrictionAttribute z:Ref="i58"/></URDFElement><URDFElement z:Id="i59" i:type="SafetyController"><Attributes><Attribute z:Id="i60"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i61"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i62"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i63"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i62"/><KVelocityAttribute z:Ref="i63"/><SoftLowerAttribute z:Ref="i61"/><SoftUpperAttribute z:Ref="i60"/></URDFElement><URDFElement z:Id="i64" i:type="Mimic"><Attributes><Attribute z:Id="i65"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i66"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i67"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i65"/><MultiplierAttribute z:Ref="i66"/><OffsetAttribute z:Ref="i67"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i46"/><AxisName/><Calibration z:Ref="i53"/><Child z:Ref="i44"/><CoordinateSystemName>Origin_global</CoordinateSystemName><Dynamics z:Ref="i56"/><Limit z:Ref="i48"/><Mimic z:Ref="i64"/><NameAttribute z:Ref="i37"/><Origin z:Ref="i39"/><Parent z:Ref="i42"/><Safety z:Ref="i59"/><TypeAttribute z:Ref="i38"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children/><Collision z:Ref="i29"/><Inertial z:Ref="i3"/><Joint z:Ref="i36"/><NameAttribute z:Ref="i2"/><Parent i:nil="true"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i16"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link>
+2022-04-26 21:20:29,098 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM
+2022-04-26 21:20:29,110 INFO  ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM
+2022-04-26 21:20:29,111 INFO  ExportHelperExtension.cs: 1145 - Found 1 features of type [CoordSys] in ArmAssembly.SLDASM
+2022-04-26 21:20:29,111 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 21:20:29,112 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 21:20:29,112 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1
+2022-04-26 21:20:29,112 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 21:20:29,113 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1
+2022-04-26 21:20:29,113 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1
+2022-04-26 21:20:29,113 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 21:20:29,114 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1
+2022-04-26 21:20:29,114 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1
+2022-04-26 21:20:29,114 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 21:20:29,115 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1
+2022-04-26 21:20:29,115 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1
+2022-04-26 21:20:29,115 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 21:20:29,116 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1
+2022-04-26 21:20:29,116 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1
+2022-04-26 21:20:29,116 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 21:20:29,117 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1
+2022-04-26 21:20:29,117 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1
+2022-04-26 21:20:29,117 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 21:20:29,118 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1
+2022-04-26 21:20:29,118 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1
+2022-04-26 21:20:29,118 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 21:20:29,119 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1
+2022-04-26 21:20:29,119 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM
+2022-04-26 21:20:29,120 INFO  ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM
+2022-04-26 21:20:29,120 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM
+2022-04-26 21:20:29,120 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 21:20:29,121 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 21:20:29,121 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1
+2022-04-26 21:20:29,121 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 21:20:29,122 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1
+2022-04-26 21:20:29,122 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1
+2022-04-26 21:20:29,122 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 21:20:29,123 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1
+2022-04-26 21:20:29,123 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1
+2022-04-26 21:20:29,138 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 21:20:29,139 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1
+2022-04-26 21:20:29,139 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1
+2022-04-26 21:20:29,140 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 21:20:29,140 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1
+2022-04-26 21:20:29,140 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1
+2022-04-26 21:20:29,141 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 21:20:29,141 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1
+2022-04-26 21:20:29,141 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1
+2022-04-26 21:20:29,142 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 21:20:29,142 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1
+2022-04-26 21:20:29,142 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1
+2022-04-26 21:20:29,143 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 21:20:29,143 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1
+2022-04-26 21:20:29,182 INFO  ExportPropertyManager.cs: 1142 - AfterClose called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:20:45,550 INFO  SwAddin.cs: 294 - Assembly export called for file ArmAssembly.SLDASM
+2022-04-26 21:20:45,550 INFO  SwAddin.cs: 299 - Save is required
+2022-04-26 21:20:45,551 INFO  SwAddin.cs: 313 - Saving assembly
+2022-04-26 21:20:45,614 INFO  SwAddin.cs: 316 - Opening property manager
+2022-04-26 21:20:45,615 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM
+2022-04-26 21:20:45,616 INFO  ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM
+2022-04-26 21:20:45,616 INFO  ExportHelperExtension.cs: 1145 - Found 1 features of type [CoordSys] in ArmAssembly.SLDASM
+2022-04-26 21:20:45,616 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 21:20:45,617 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 21:20:45,617 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1
+2022-04-26 21:20:45,617 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 21:20:45,618 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1
+2022-04-26 21:20:45,618 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1
+2022-04-26 21:20:45,618 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 21:20:45,619 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1
+2022-04-26 21:20:45,619 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1
+2022-04-26 21:20:45,619 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 21:20:45,620 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1
+2022-04-26 21:20:45,620 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1
+2022-04-26 21:20:45,620 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 21:20:45,621 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1
+2022-04-26 21:20:45,621 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1
+2022-04-26 21:20:45,621 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 21:20:45,622 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1
+2022-04-26 21:20:45,622 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1
+2022-04-26 21:20:45,623 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 21:20:45,623 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1
+2022-04-26 21:20:45,624 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1
+2022-04-26 21:20:45,624 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 21:20:45,625 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1
+2022-04-26 21:20:45,625 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM
+2022-04-26 21:20:45,625 INFO  ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM
+2022-04-26 21:20:45,626 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM
+2022-04-26 21:20:45,626 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 21:20:45,626 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 21:20:45,627 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1
+2022-04-26 21:20:45,627 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 21:20:45,628 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1
+2022-04-26 21:20:45,628 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1
+2022-04-26 21:20:45,629 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 21:20:45,629 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1
+2022-04-26 21:20:45,630 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1
+2022-04-26 21:20:45,630 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 21:20:45,630 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1
+2022-04-26 21:20:45,631 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1
+2022-04-26 21:20:45,631 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 21:20:45,632 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1
+2022-04-26 21:20:45,632 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1
+2022-04-26 21:20:45,633 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 21:20:45,633 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1
+2022-04-26 21:20:45,633 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1
+2022-04-26 21:20:45,634 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 21:20:45,634 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1
+2022-04-26 21:20:45,635 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1
+2022-04-26 21:20:45,635 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 21:20:45,636 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1
+2022-04-26 21:20:45,732 INFO  SwAddin.cs: 339 - Loading config tree
+2022-04-26 21:20:45,733 INFO  ConfigurationSerialization.cs: 276 - URDF Configuration found
+<Link z:Id="i1" xmlns="http://schemas.datacontract.org/2004/07/SW2URDF" xmlns:i="http://www.w3.org/2001/XMLSchema-instance" xmlns:z="http://schemas.microsoft.com/2003/10/Serialization/"><Attributes><Attribute z:Id="i2"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">base_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i3" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i4" i:type="Origin"><Attributes><Attribute z:Id="i5"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>9.1830074983591922E-19</a:double><a:double>0.015606490089143955</a:double><a:double>1.5715252581589567E-18</a:double></Value></Attribute><Attribute z:Id="i6"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i6"/><XYZAttribute z:Ref="i5"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i7" i:type="Mass"><Attributes><Attribute z:Id="i8"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.93972387307473026</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i8"/></URDFElement><URDFElement z:Id="i9" i:type="Inertia"><Attributes><Attribute z:Id="i10"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0025142902225504422</Value></Attribute><Attribute z:Id="i11"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">8.02209898558041E-21</Value></Attribute><Attribute z:Id="i12"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">-7.374852413660824E-20</Value></Attribute><Attribute z:Id="i13"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0044229329573873041</Value></Attribute><Attribute z:Id="i14"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">2.2568808327940613E-20</Value></Attribute><Attribute z:Id="i15"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0021336882069818023</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i10"/><IxyAttribute z:Ref="i11"/><IxzAttribute z:Ref="i12"/><IyyAttribute z:Ref="i13"/><IyzAttribute z:Ref="i14"/><IzzAttribute z:Ref="i15"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i9"/><Mass z:Ref="i7"/><Origin z:Ref="i4"/></URDFElement><URDFElement z:Id="i16" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i17" i:type="Origin"><Attributes><Attribute z:Id="i18"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i19"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i19"/><XYZAttribute z:Ref="i18"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i20" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i21" i:type="Mesh"><Attributes><Attribute z:Id="i22"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i22"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i21"/></URDFElement><URDFElement z:Id="i23" i:type="Material"><Attributes><Attribute z:Id="i24"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i25" i:type="Color"><Attributes><Attribute z:Id="i26"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0.792156862745098</a:double><a:double>0.81960784313725488</a:double><a:double>0.93333333333333335</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i26"/></URDFElement><URDFElement z:Id="i27" i:type="Texture"><Attributes><Attribute z:Id="i28"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i28"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i25"/><NameAttribute z:Ref="i24"/><Texture z:Ref="i27"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i20"/><Material z:Ref="i23"/><Origin z:Ref="i17"/></URDFElement><URDFElement z:Id="i29" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i30" i:type="Origin"><Attributes><Attribute z:Id="i31"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i32"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i32"/><XYZAttribute z:Ref="i31"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i33" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i34" i:type="Mesh"><Attributes><Attribute z:Id="i35"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i35"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i34"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i33"/><Origin z:Ref="i30"/></URDFElement><URDFElement z:Id="i36" i:type="Joint"><Attributes><Attribute z:Id="i37"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute><Attribute z:Id="i38"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i39" i:type="Origin"><Attributes><Attribute z:Id="i40"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i41"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i41"/><XYZAttribute z:Ref="i40"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i42" i:type="ParentLink"><Attributes><Attribute z:Id="i43"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i43"/></URDFElement><URDFElement z:Id="i44" i:type="ChildLink"><Attributes><Attribute z:Id="i45"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i45"/></URDFElement><URDFElement z:Id="i46" i:type="Axis"><Attributes><Attribute z:Id="i47"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i47"/></URDFElement><URDFElement z:Id="i48" i:type="Limit"><Attributes><Attribute z:Id="i49"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i50"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i51"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i52"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i51"/><LowerAttribute z:Ref="i49"/><UpperAttribute z:Ref="i50"/><VelocityAttribute z:Ref="i52"/></URDFElement><URDFElement z:Id="i53" i:type="Calibration"><Attributes><Attribute z:Id="i54"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i55"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i55"/><RisingAttribute z:Ref="i54"/></URDFElement><URDFElement z:Id="i56" i:type="Dynamics"><Attributes><Attribute z:Id="i57"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i58"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i57"/><FrictionAttribute z:Ref="i58"/></URDFElement><URDFElement z:Id="i59" i:type="SafetyController"><Attributes><Attribute z:Id="i60"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i61"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i62"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i63"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i62"/><KVelocityAttribute z:Ref="i63"/><SoftLowerAttribute z:Ref="i61"/><SoftUpperAttribute z:Ref="i60"/></URDFElement><URDFElement z:Id="i64" i:type="Mimic"><Attributes><Attribute z:Id="i65"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i66"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i67"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i65"/><MultiplierAttribute z:Ref="i66"/><OffsetAttribute z:Ref="i67"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i46"/><AxisName/><Calibration z:Ref="i53"/><Child z:Ref="i44"/><CoordinateSystemName>Origin_global</CoordinateSystemName><Dynamics z:Ref="i56"/><Limit z:Ref="i48"/><Mimic z:Ref="i64"/><NameAttribute z:Ref="i37"/><Origin z:Ref="i39"/><Parent z:Ref="i42"/><Safety z:Ref="i59"/><TypeAttribute z:Ref="i38"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children/><Collision z:Ref="i29"/><Inertial z:Ref="i3"/><Joint z:Ref="i36"/><NameAttribute z:Ref="i2"/><Parent i:nil="true"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=</a:base64Binary><a:base64Binary>sDYAAAUAAAD//v8WUwBoAG8AdQBsAGQAZQByAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAGQAAAA==</a:base64Binary><a:base64Binary>sDYAAAUAAAD//v8WVQBwAHAAZQByAEEAcgBtAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAGgAAAA==</a:base64Binary><a:base64Binary>sDYAAAUAAAD//v8URgBvAHIAYQByAG0ALQAxAEAAQQByAG0AQQBzAHMAZQBtAGIAbAB5AAQAAAAQAAAAAQAAAAEAAAAjAAAA</a:base64Binary><a:base64Binary>sDYAAAUAAAD//v8cVwByAGkAcwB0AFMAdAByAHUAYwB0AHUAcgBlAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAKAAAAA==</a:base64Binary><a:base64Binary>sDYAAAUAAAD//v8TVwByAGkAcwB0AC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAJwAAAA==</a:base64Binary><a:base64Binary>sDYAAAUAAAD//v8ZTQBhAG4AaQBwAHUAbABhAHQAbwByAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAJAAAAA==</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i16"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link>
+2022-04-26 21:20:45,734 INFO  LinkNode.cs: 35 - Building node base_link
+2022-04-26 21:20:45,734 INFO  CommonSwOperations.cs: 221 - Loading SolidWorks components for base_link from E:\Robotics\WR\URDF\ArmAssembly.SLDASM
+2022-04-26 21:20:45,734 INFO  CommonSwOperations.cs: 245 - Loading component with PID ?6???base-1@ArmAssembly


+2022-04-26 21:20:45,734 INFO  CommonSwOperations.cs: 254 - Successfully loaded component E:\Robotics\WR\URDF\base.SLDPRT
+2022-04-26 21:20:45,735 INFO  CommonSwOperations.cs: 245 - Loading component with PID ?6???Shoulder-1@ArmAssembly


+2022-04-26 21:20:45,735 INFO  CommonSwOperations.cs: 254 - Successfully loaded component E:\Robotics\WR\URDF\Shoulder.SLDPRT
+2022-04-26 21:20:45,735 INFO  CommonSwOperations.cs: 245 - Loading component with PID ?6???UpperArm-1@ArmAssembly


+2022-04-26 21:20:45,736 INFO  CommonSwOperations.cs: 254 - Successfully loaded component E:\Robotics\WR\URDF\UpperArm.SLDPRT
+2022-04-26 21:20:45,736 INFO  CommonSwOperations.cs: 245 - Loading component with PID ?6???Forarm-1@ArmAssembly

#
+2022-04-26 21:20:45,736 INFO  CommonSwOperations.cs: 254 - Successfully loaded component E:\Robotics\WR\URDF\Forarm.SLDPRT
+2022-04-26 21:20:45,737 INFO  CommonSwOperations.cs: 245 - Loading component with PID ?6???WristStructure-1@ArmAssembly

(
+2022-04-26 21:20:45,737 INFO  CommonSwOperations.cs: 254 - Successfully loaded component E:\Robotics\WR\URDF\WristStructure.SLDPRT
+2022-04-26 21:20:45,737 INFO  CommonSwOperations.cs: 245 - Loading component with PID ?6???Wrist-1@ArmAssembly

'
+2022-04-26 21:20:45,738 INFO  CommonSwOperations.cs: 254 - Successfully loaded component E:\Robotics\WR\URDF\Wrist.SLDPRT
+2022-04-26 21:20:45,738 INFO  CommonSwOperations.cs: 245 - Loading component with PID ?6???Manipulator-1@ArmAssembly

$
+2022-04-26 21:20:45,739 INFO  CommonSwOperations.cs: 254 - Successfully loaded component E:\Robotics\WR\URDF\Manipulator.SLDPRT
+2022-04-26 21:20:45,739 INFO  CommonSwOperations.cs: 230 - Loaded 7 components for link base_link
+2022-04-26 21:20:45,909 INFO  SwAddin.cs: 344 - Showing property manager
+2022-04-26 21:20:50,182 INFO  ExportPropertyManager.cs: 1036 - OnListboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:20:50,491 INFO  ExportPropertyManager.cs: 1036 - OnListboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:20:52,134 INFO  ExportPropertyManager.cs: 1036 - OnListboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:20:53,289 INFO  ExportPropertyManager.cs: 1036 - OnListboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:20:53,987 INFO  ExportPropertyManager.cs: 1036 - OnListboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:20:54,678 INFO  ExportPropertyManager.cs: 1036 - OnListboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:20:55,176 INFO  ExportPropertyManager.cs: 1036 - OnListboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:20:55,830 INFO  ExportPropertyManager.cs: 1036 - OnListboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:20:58,315 INFO  ExportPropertyManager.cs: 422 - Configuration saved
+2022-04-26 21:20:58,316 INFO  ConfigurationSerialization.cs: 276 - URDF Configuration found
+<Link z:Id="i1" xmlns="http://schemas.datacontract.org/2004/07/SW2URDF" xmlns:i="http://www.w3.org/2001/XMLSchema-instance" xmlns:z="http://schemas.microsoft.com/2003/10/Serialization/"><Attributes><Attribute z:Id="i2"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">base_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i3" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i4" i:type="Origin"><Attributes><Attribute z:Id="i5"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>9.1830074983591922E-19</a:double><a:double>0.015606490089143955</a:double><a:double>1.5715252581589567E-18</a:double></Value></Attribute><Attribute z:Id="i6"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i6"/><XYZAttribute z:Ref="i5"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i7" i:type="Mass"><Attributes><Attribute z:Id="i8"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.93972387307473026</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i8"/></URDFElement><URDFElement z:Id="i9" i:type="Inertia"><Attributes><Attribute z:Id="i10"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0025142902225504422</Value></Attribute><Attribute z:Id="i11"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">8.02209898558041E-21</Value></Attribute><Attribute z:Id="i12"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">-7.374852413660824E-20</Value></Attribute><Attribute z:Id="i13"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0044229329573873041</Value></Attribute><Attribute z:Id="i14"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">2.2568808327940613E-20</Value></Attribute><Attribute z:Id="i15"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0021336882069818023</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i10"/><IxyAttribute z:Ref="i11"/><IxzAttribute z:Ref="i12"/><IyyAttribute z:Ref="i13"/><IyzAttribute z:Ref="i14"/><IzzAttribute z:Ref="i15"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i9"/><Mass z:Ref="i7"/><Origin z:Ref="i4"/></URDFElement><URDFElement z:Id="i16" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i17" i:type="Origin"><Attributes><Attribute z:Id="i18"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i19"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i19"/><XYZAttribute z:Ref="i18"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i20" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i21" i:type="Mesh"><Attributes><Attribute z:Id="i22"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i22"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i21"/></URDFElement><URDFElement z:Id="i23" i:type="Material"><Attributes><Attribute z:Id="i24"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i25" i:type="Color"><Attributes><Attribute z:Id="i26"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0.792156862745098</a:double><a:double>0.81960784313725488</a:double><a:double>0.93333333333333335</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i26"/></URDFElement><URDFElement z:Id="i27" i:type="Texture"><Attributes><Attribute z:Id="i28"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i28"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i25"/><NameAttribute z:Ref="i24"/><Texture z:Ref="i27"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i20"/><Material z:Ref="i23"/><Origin z:Ref="i17"/></URDFElement><URDFElement z:Id="i29" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i30" i:type="Origin"><Attributes><Attribute z:Id="i31"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i32"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i32"/><XYZAttribute z:Ref="i31"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i33" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i34" i:type="Mesh"><Attributes><Attribute z:Id="i35"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i35"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i34"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i33"/><Origin z:Ref="i30"/></URDFElement><URDFElement z:Id="i36" i:type="Joint"><Attributes><Attribute z:Id="i37"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute><Attribute z:Id="i38"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i39" i:type="Origin"><Attributes><Attribute z:Id="i40"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i41"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i41"/><XYZAttribute z:Ref="i40"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i42" i:type="ParentLink"><Attributes><Attribute z:Id="i43"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i43"/></URDFElement><URDFElement z:Id="i44" i:type="ChildLink"><Attributes><Attribute z:Id="i45"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i45"/></URDFElement><URDFElement z:Id="i46" i:type="Axis"><Attributes><Attribute z:Id="i47"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i47"/></URDFElement><URDFElement z:Id="i48" i:type="Limit"><Attributes><Attribute z:Id="i49"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i50"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i51"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i52"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i51"/><LowerAttribute z:Ref="i49"/><UpperAttribute z:Ref="i50"/><VelocityAttribute z:Ref="i52"/></URDFElement><URDFElement z:Id="i53" i:type="Calibration"><Attributes><Attribute z:Id="i54"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i55"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i55"/><RisingAttribute z:Ref="i54"/></URDFElement><URDFElement z:Id="i56" i:type="Dynamics"><Attributes><Attribute z:Id="i57"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i58"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i57"/><FrictionAttribute z:Ref="i58"/></URDFElement><URDFElement z:Id="i59" i:type="SafetyController"><Attributes><Attribute z:Id="i60"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i61"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i62"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i63"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i62"/><KVelocityAttribute z:Ref="i63"/><SoftLowerAttribute z:Ref="i61"/><SoftUpperAttribute z:Ref="i60"/></URDFElement><URDFElement z:Id="i64" i:type="Mimic"><Attributes><Attribute z:Id="i65"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i66"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i67"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i65"/><MultiplierAttribute z:Ref="i66"/><OffsetAttribute z:Ref="i67"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i46"/><AxisName/><Calibration z:Ref="i53"/><Child z:Ref="i44"/><CoordinateSystemName>Origin_global</CoordinateSystemName><Dynamics z:Ref="i56"/><Limit z:Ref="i48"/><Mimic z:Ref="i64"/><NameAttribute z:Ref="i37"/><Origin z:Ref="i39"/><Parent z:Ref="i42"/><Safety z:Ref="i59"/><TypeAttribute z:Ref="i38"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children/><Collision z:Ref="i29"/><Inertial z:Ref="i3"/><Joint z:Ref="i36"/><NameAttribute z:Ref="i2"/><Parent i:nil="true"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=</a:base64Binary><a:base64Binary>sDYAAAUAAAD//v8WUwBoAG8AdQBsAGQAZQByAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAGQAAAA==</a:base64Binary><a:base64Binary>sDYAAAUAAAD//v8WVQBwAHAAZQByAEEAcgBtAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAGgAAAA==</a:base64Binary><a:base64Binary>sDYAAAUAAAD//v8URgBvAHIAYQByAG0ALQAxAEAAQQByAG0AQQBzAHMAZQBtAGIAbAB5AAQAAAAQAAAAAQAAAAEAAAAjAAAA</a:base64Binary><a:base64Binary>sDYAAAUAAAD//v8cVwByAGkAcwB0AFMAdAByAHUAYwB0AHUAcgBlAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAKAAAAA==</a:base64Binary><a:base64Binary>sDYAAAUAAAD//v8TVwByAGkAcwB0AC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAJwAAAA==</a:base64Binary><a:base64Binary>sDYAAAUAAAD//v8ZTQBhAG4AaQBwAHUAbABhAHQAbwByAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAJAAAAA==</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i16"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link>
+2022-04-26 21:20:58,417 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM
+2022-04-26 21:20:58,418 INFO  ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM
+2022-04-26 21:20:58,418 INFO  ExportHelperExtension.cs: 1145 - Found 1 features of type [CoordSys] in ArmAssembly.SLDASM
+2022-04-26 21:20:58,418 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 21:20:58,419 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 21:20:58,419 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1
+2022-04-26 21:20:58,419 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 21:20:58,420 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1
+2022-04-26 21:20:58,430 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1
+2022-04-26 21:20:58,441 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 21:20:58,442 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1
+2022-04-26 21:20:58,442 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1
+2022-04-26 21:20:58,443 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 21:20:58,443 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1
+2022-04-26 21:20:58,443 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1
+2022-04-26 21:20:58,444 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 21:20:58,444 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1
+2022-04-26 21:20:58,444 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1
+2022-04-26 21:20:58,445 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 21:20:58,445 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1
+2022-04-26 21:20:58,445 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1
+2022-04-26 21:20:58,446 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 21:20:58,446 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1
+2022-04-26 21:20:58,446 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1
+2022-04-26 21:20:58,447 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 21:20:58,447 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1
+2022-04-26 21:20:58,448 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM
+2022-04-26 21:20:58,448 INFO  ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM
+2022-04-26 21:20:58,448 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM
+2022-04-26 21:20:58,449 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 21:20:58,449 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 21:20:58,449 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1
+2022-04-26 21:20:58,449 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 21:20:58,450 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1
+2022-04-26 21:20:58,450 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1
+2022-04-26 21:20:58,450 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 21:20:58,451 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1
+2022-04-26 21:20:58,451 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1
+2022-04-26 21:20:58,451 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 21:20:58,452 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1
+2022-04-26 21:20:58,452 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1
+2022-04-26 21:20:58,452 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 21:20:58,453 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1
+2022-04-26 21:20:58,453 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1
+2022-04-26 21:20:58,453 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 21:20:58,454 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1
+2022-04-26 21:20:58,454 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1
+2022-04-26 21:20:58,454 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 21:20:58,455 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1
+2022-04-26 21:20:58,455 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1
+2022-04-26 21:20:58,455 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 21:20:58,456 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1
+2022-04-26 21:20:58,496 INFO  ExportPropertyManager.cs: 1142 - AfterClose called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:22:40,251 INFO  SwAddin.cs: 294 - Assembly export called for file ArmAssembly.SLDASM
+2022-04-26 21:22:40,252 INFO  SwAddin.cs: 299 - Save is required
+2022-04-26 21:22:40,252 INFO  SwAddin.cs: 313 - Saving assembly
+2022-04-26 21:22:40,312 INFO  SwAddin.cs: 316 - Opening property manager
+2022-04-26 21:22:40,312 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM
+2022-04-26 21:22:40,313 INFO  ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM
+2022-04-26 21:22:40,313 INFO  ExportHelperExtension.cs: 1145 - Found 1 features of type [CoordSys] in ArmAssembly.SLDASM
+2022-04-26 21:22:40,313 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 21:22:40,314 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 21:22:40,314 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1
+2022-04-26 21:22:40,314 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 21:22:40,315 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1
+2022-04-26 21:22:40,315 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1
+2022-04-26 21:22:40,315 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 21:22:40,316 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1
+2022-04-26 21:22:40,316 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1
+2022-04-26 21:22:40,316 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 21:22:40,317 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1
+2022-04-26 21:22:40,317 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1
+2022-04-26 21:22:40,318 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 21:22:40,318 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1
+2022-04-26 21:22:40,318 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1
+2022-04-26 21:22:40,319 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 21:22:40,320 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1
+2022-04-26 21:22:40,320 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1
+2022-04-26 21:22:40,320 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 21:22:40,321 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1
+2022-04-26 21:22:40,321 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1
+2022-04-26 21:22:40,321 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 21:22:40,322 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1
+2022-04-26 21:22:40,322 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM
+2022-04-26 21:22:40,322 INFO  ExportHelperExtension.cs: 1136 - Found 47 in ArmAssembly.SLDASM
+2022-04-26 21:22:40,323 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in ArmAssembly.SLDASM
+2022-04-26 21:22:40,323 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 21:22:40,323 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 21:22:40,324 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1
+2022-04-26 21:22:40,324 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 21:22:40,324 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1
+2022-04-26 21:22:40,325 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1
+2022-04-26 21:22:40,325 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 21:22:40,325 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1
+2022-04-26 21:22:40,326 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1
+2022-04-26 21:22:40,326 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 21:22:40,326 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1
+2022-04-26 21:22:40,327 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1
+2022-04-26 21:22:40,327 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 21:22:40,328 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1
+2022-04-26 21:22:40,328 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1
+2022-04-26 21:22:40,328 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 21:22:40,329 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1
+2022-04-26 21:22:40,329 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1
+2022-04-26 21:22:40,329 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 21:22:40,330 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1
+2022-04-26 21:22:40,331 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1
+2022-04-26 21:22:40,331 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 21:22:40,331 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1
+2022-04-26 21:22:40,462 INFO  SwAddin.cs: 339 - Loading config tree
+2022-04-26 21:22:40,463 INFO  ConfigurationSerialization.cs: 276 - URDF Configuration found
+<Link z:Id="i1" xmlns="http://schemas.datacontract.org/2004/07/SW2URDF" xmlns:i="http://www.w3.org/2001/XMLSchema-instance" xmlns:z="http://schemas.microsoft.com/2003/10/Serialization/"><Attributes><Attribute z:Id="i2"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">base_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i3" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i4" i:type="Origin"><Attributes><Attribute z:Id="i5"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>9.1830074983591922E-19</a:double><a:double>0.015606490089143955</a:double><a:double>1.5715252581589567E-18</a:double></Value></Attribute><Attribute z:Id="i6"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i6"/><XYZAttribute z:Ref="i5"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i7" i:type="Mass"><Attributes><Attribute z:Id="i8"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.93972387307473026</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i8"/></URDFElement><URDFElement z:Id="i9" i:type="Inertia"><Attributes><Attribute z:Id="i10"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0025142902225504422</Value></Attribute><Attribute z:Id="i11"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">8.02209898558041E-21</Value></Attribute><Attribute z:Id="i12"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">-7.374852413660824E-20</Value></Attribute><Attribute z:Id="i13"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0044229329573873041</Value></Attribute><Attribute z:Id="i14"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">2.2568808327940613E-20</Value></Attribute><Attribute z:Id="i15"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0021336882069818023</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i10"/><IxyAttribute z:Ref="i11"/><IxzAttribute z:Ref="i12"/><IyyAttribute z:Ref="i13"/><IyzAttribute z:Ref="i14"/><IzzAttribute z:Ref="i15"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i9"/><Mass z:Ref="i7"/><Origin z:Ref="i4"/></URDFElement><URDFElement z:Id="i16" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i17" i:type="Origin"><Attributes><Attribute z:Id="i18"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i19"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i19"/><XYZAttribute z:Ref="i18"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i20" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i21" i:type="Mesh"><Attributes><Attribute z:Id="i22"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i22"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i21"/></URDFElement><URDFElement z:Id="i23" i:type="Material"><Attributes><Attribute z:Id="i24"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i25" i:type="Color"><Attributes><Attribute z:Id="i26"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0.792156862745098</a:double><a:double>0.81960784313725488</a:double><a:double>0.93333333333333335</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i26"/></URDFElement><URDFElement z:Id="i27" i:type="Texture"><Attributes><Attribute z:Id="i28"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i28"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i25"/><NameAttribute z:Ref="i24"/><Texture z:Ref="i27"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i20"/><Material z:Ref="i23"/><Origin z:Ref="i17"/></URDFElement><URDFElement z:Id="i29" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i30" i:type="Origin"><Attributes><Attribute z:Id="i31"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i32"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i32"/><XYZAttribute z:Ref="i31"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i33" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i34" i:type="Mesh"><Attributes><Attribute z:Id="i35"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i35"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i34"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i33"/><Origin z:Ref="i30"/></URDFElement><URDFElement z:Id="i36" i:type="Joint"><Attributes><Attribute z:Id="i37"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute><Attribute z:Id="i38"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i39" i:type="Origin"><Attributes><Attribute z:Id="i40"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i41"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i41"/><XYZAttribute z:Ref="i40"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i42" i:type="ParentLink"><Attributes><Attribute z:Id="i43"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i43"/></URDFElement><URDFElement z:Id="i44" i:type="ChildLink"><Attributes><Attribute z:Id="i45"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i45"/></URDFElement><URDFElement z:Id="i46" i:type="Axis"><Attributes><Attribute z:Id="i47"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i47"/></URDFElement><URDFElement z:Id="i48" i:type="Limit"><Attributes><Attribute z:Id="i49"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i50"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i51"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i52"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i51"/><LowerAttribute z:Ref="i49"/><UpperAttribute z:Ref="i50"/><VelocityAttribute z:Ref="i52"/></URDFElement><URDFElement z:Id="i53" i:type="Calibration"><Attributes><Attribute z:Id="i54"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i55"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i55"/><RisingAttribute z:Ref="i54"/></URDFElement><URDFElement z:Id="i56" i:type="Dynamics"><Attributes><Attribute z:Id="i57"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i58"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i57"/><FrictionAttribute z:Ref="i58"/></URDFElement><URDFElement z:Id="i59" i:type="SafetyController"><Attributes><Attribute z:Id="i60"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i61"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i62"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i63"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i62"/><KVelocityAttribute z:Ref="i63"/><SoftLowerAttribute z:Ref="i61"/><SoftUpperAttribute z:Ref="i60"/></URDFElement><URDFElement z:Id="i64" i:type="Mimic"><Attributes><Attribute z:Id="i65"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i66"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i67"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i65"/><MultiplierAttribute z:Ref="i66"/><OffsetAttribute z:Ref="i67"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i46"/><AxisName/><Calibration z:Ref="i53"/><Child z:Ref="i44"/><CoordinateSystemName>Origin_global</CoordinateSystemName><Dynamics z:Ref="i56"/><Limit z:Ref="i48"/><Mimic z:Ref="i64"/><NameAttribute z:Ref="i37"/><Origin z:Ref="i39"/><Parent z:Ref="i42"/><Safety z:Ref="i59"/><TypeAttribute z:Ref="i38"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children/><Collision z:Ref="i29"/><Inertial z:Ref="i3"/><Joint z:Ref="i36"/><NameAttribute z:Ref="i2"/><Parent i:nil="true"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i16"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link>
+2022-04-26 21:22:40,464 INFO  LinkNode.cs: 35 - Building node base_link
+2022-04-26 21:22:40,464 INFO  CommonSwOperations.cs: 221 - Loading SolidWorks components for base_link from E:\Robotics\WR\URDF\ArmAssembly.SLDASM
+2022-04-26 21:22:40,464 INFO  CommonSwOperations.cs: 245 - Loading component with PID ?6???base-1@ArmAssembly


+2022-04-26 21:22:40,464 INFO  CommonSwOperations.cs: 254 - Successfully loaded component E:\Robotics\WR\URDF\base.SLDPRT
+2022-04-26 21:22:40,465 INFO  CommonSwOperations.cs: 230 - Loaded 1 components for link base_link
+2022-04-26 21:22:40,495 INFO  SwAddin.cs: 344 - Showing property manager
+2022-04-26 21:22:43,955 INFO  ExportPropertyManager.cs: 1036 - OnListboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:22:44,696 INFO  ExportPropertyManager.cs: 1036 - OnListboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:22:46,553 INFO  ExportPropertyManager.cs: 1036 - OnListboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:22:49,426 INFO  ExportPropertyManager.cs: 1142 - AfterClose called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:25:55,950 INFO  SwAddin.cs: 294 - Assembly export called for file ArmAssembly.SLDASM
+2022-04-26 21:25:55,950 INFO  SwAddin.cs: 299 - Save is required
+2022-04-26 21:25:55,951 INFO  SwAddin.cs: 313 - Saving assembly
+2022-04-26 21:25:56,086 INFO  SwAddin.cs: 316 - Opening property manager
+2022-04-26 21:25:56,087 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM
+2022-04-26 21:25:56,088 INFO  ExportHelperExtension.cs: 1136 - Found 53 in ArmAssembly.SLDASM
+2022-04-26 21:25:56,088 INFO  ExportHelperExtension.cs: 1145 - Found 1 features of type [CoordSys] in ArmAssembly.SLDASM
+2022-04-26 21:25:56,088 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 21:25:56,089 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 21:25:56,089 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1
+2022-04-26 21:25:56,089 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 21:25:56,090 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1
+2022-04-26 21:25:56,090 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1
+2022-04-26 21:25:56,090 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 21:25:56,091 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1
+2022-04-26 21:25:56,091 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1
+2022-04-26 21:25:56,091 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 21:25:56,092 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1
+2022-04-26 21:25:56,092 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1
+2022-04-26 21:25:56,093 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 21:25:56,093 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1
+2022-04-26 21:25:56,094 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1
+2022-04-26 21:25:56,095 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 21:25:56,096 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1
+2022-04-26 21:25:56,096 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1
+2022-04-26 21:25:56,096 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 21:25:56,097 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1
+2022-04-26 21:25:56,097 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1
+2022-04-26 21:25:56,097 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 21:25:56,098 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1
+2022-04-26 21:25:56,098 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM
+2022-04-26 21:25:56,099 INFO  ExportHelperExtension.cs: 1136 - Found 53 in ArmAssembly.SLDASM
+2022-04-26 21:25:56,099 INFO  ExportHelperExtension.cs: 1145 - Found 6 features of type [RefAxis] in ArmAssembly.SLDASM
+2022-04-26 21:25:56,099 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 21:25:56,100 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 21:25:56,100 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1
+2022-04-26 21:25:56,100 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 21:25:56,101 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1
+2022-04-26 21:25:56,101 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1
+2022-04-26 21:25:56,101 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 21:25:56,102 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1
+2022-04-26 21:25:56,102 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1
+2022-04-26 21:25:56,102 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 21:25:56,103 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1
+2022-04-26 21:25:56,103 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1
+2022-04-26 21:25:56,103 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 21:25:56,103 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1
+2022-04-26 21:25:56,104 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1
+2022-04-26 21:25:56,104 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 21:25:56,104 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1
+2022-04-26 21:25:56,105 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1
+2022-04-26 21:25:56,105 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 21:25:56,105 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1
+2022-04-26 21:25:56,106 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1
+2022-04-26 21:25:56,106 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 21:25:56,106 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1
+2022-04-26 21:25:56,235 INFO  SwAddin.cs: 339 - Loading config tree
+2022-04-26 21:25:56,236 INFO  ConfigurationSerialization.cs: 276 - URDF Configuration found
+<Link z:Id="i1" xmlns="http://schemas.datacontract.org/2004/07/SW2URDF" xmlns:i="http://www.w3.org/2001/XMLSchema-instance" xmlns:z="http://schemas.microsoft.com/2003/10/Serialization/"><Attributes><Attribute z:Id="i2"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">base_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i3" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i4" i:type="Origin"><Attributes><Attribute z:Id="i5"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>9.1830074983591922E-19</a:double><a:double>0.015606490089143955</a:double><a:double>1.5715252581589567E-18</a:double></Value></Attribute><Attribute z:Id="i6"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i6"/><XYZAttribute z:Ref="i5"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i7" i:type="Mass"><Attributes><Attribute z:Id="i8"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.93972387307473026</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i8"/></URDFElement><URDFElement z:Id="i9" i:type="Inertia"><Attributes><Attribute z:Id="i10"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0025142902225504422</Value></Attribute><Attribute z:Id="i11"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">8.02209898558041E-21</Value></Attribute><Attribute z:Id="i12"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">-7.374852413660824E-20</Value></Attribute><Attribute z:Id="i13"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0044229329573873041</Value></Attribute><Attribute z:Id="i14"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">2.2568808327940613E-20</Value></Attribute><Attribute z:Id="i15"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0021336882069818023</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i10"/><IxyAttribute z:Ref="i11"/><IxzAttribute z:Ref="i12"/><IyyAttribute z:Ref="i13"/><IyzAttribute z:Ref="i14"/><IzzAttribute z:Ref="i15"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i9"/><Mass z:Ref="i7"/><Origin z:Ref="i4"/></URDFElement><URDFElement z:Id="i16" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i17" i:type="Origin"><Attributes><Attribute z:Id="i18"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i19"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i19"/><XYZAttribute z:Ref="i18"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i20" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i21" i:type="Mesh"><Attributes><Attribute z:Id="i22"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i22"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i21"/></URDFElement><URDFElement z:Id="i23" i:type="Material"><Attributes><Attribute z:Id="i24"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i25" i:type="Color"><Attributes><Attribute z:Id="i26"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0.792156862745098</a:double><a:double>0.81960784313725488</a:double><a:double>0.93333333333333335</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i26"/></URDFElement><URDFElement z:Id="i27" i:type="Texture"><Attributes><Attribute z:Id="i28"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i28"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i25"/><NameAttribute z:Ref="i24"/><Texture z:Ref="i27"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i20"/><Material z:Ref="i23"/><Origin z:Ref="i17"/></URDFElement><URDFElement z:Id="i29" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i30" i:type="Origin"><Attributes><Attribute z:Id="i31"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i32"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i32"/><XYZAttribute z:Ref="i31"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i33" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i34" i:type="Mesh"><Attributes><Attribute z:Id="i35"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i35"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i34"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i33"/><Origin z:Ref="i30"/></URDFElement><URDFElement z:Id="i36" i:type="Joint"><Attributes><Attribute z:Id="i37"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute><Attribute z:Id="i38"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i39" i:type="Origin"><Attributes><Attribute z:Id="i40"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i41"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i41"/><XYZAttribute z:Ref="i40"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i42" i:type="ParentLink"><Attributes><Attribute z:Id="i43"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i43"/></URDFElement><URDFElement z:Id="i44" i:type="ChildLink"><Attributes><Attribute z:Id="i45"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i45"/></URDFElement><URDFElement z:Id="i46" i:type="Axis"><Attributes><Attribute z:Id="i47"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i47"/></URDFElement><URDFElement z:Id="i48" i:type="Limit"><Attributes><Attribute z:Id="i49"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i50"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i51"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i52"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i51"/><LowerAttribute z:Ref="i49"/><UpperAttribute z:Ref="i50"/><VelocityAttribute z:Ref="i52"/></URDFElement><URDFElement z:Id="i53" i:type="Calibration"><Attributes><Attribute z:Id="i54"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i55"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i55"/><RisingAttribute z:Ref="i54"/></URDFElement><URDFElement z:Id="i56" i:type="Dynamics"><Attributes><Attribute z:Id="i57"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i58"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i57"/><FrictionAttribute z:Ref="i58"/></URDFElement><URDFElement z:Id="i59" i:type="SafetyController"><Attributes><Attribute z:Id="i60"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i61"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i62"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i63"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i62"/><KVelocityAttribute z:Ref="i63"/><SoftLowerAttribute z:Ref="i61"/><SoftUpperAttribute z:Ref="i60"/></URDFElement><URDFElement z:Id="i64" i:type="Mimic"><Attributes><Attribute z:Id="i65"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i66"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i67"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i65"/><MultiplierAttribute z:Ref="i66"/><OffsetAttribute z:Ref="i67"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i46"/><AxisName/><Calibration z:Ref="i53"/><Child z:Ref="i44"/><CoordinateSystemName>Origin_global</CoordinateSystemName><Dynamics z:Ref="i56"/><Limit z:Ref="i48"/><Mimic z:Ref="i64"/><NameAttribute z:Ref="i37"/><Origin z:Ref="i39"/><Parent z:Ref="i42"/><Safety z:Ref="i59"/><TypeAttribute z:Ref="i38"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children/><Collision z:Ref="i29"/><Inertial z:Ref="i3"/><Joint z:Ref="i36"/><NameAttribute z:Ref="i2"/><Parent i:nil="true"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i16"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link>
+2022-04-26 21:25:56,236 INFO  LinkNode.cs: 35 - Building node base_link
+2022-04-26 21:25:56,237 INFO  CommonSwOperations.cs: 221 - Loading SolidWorks components for base_link from E:\Robotics\WR\URDF\ArmAssembly.SLDASM
+2022-04-26 21:25:56,237 INFO  CommonSwOperations.cs: 245 - Loading component with PID ?6???base-1@ArmAssembly


+2022-04-26 21:25:56,237 INFO  CommonSwOperations.cs: 254 - Successfully loaded component E:\Robotics\WR\URDF\base.SLDPRT
+2022-04-26 21:25:56,238 INFO  CommonSwOperations.cs: 230 - Loaded 1 components for link base_link
+2022-04-26 21:25:56,271 INFO  SwAddin.cs: 344 - Showing property manager
+2022-04-26 21:26:17,760 INFO  ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:26:21,486 INFO  ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:26:29,220 INFO  ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:26:48,883 INFO  ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:27:03,931 INFO  ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:27:16,584 INFO  ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:27:31,267 INFO  ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:27:44,310 INFO  ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:27:51,841 INFO  ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:28:03,077 INFO  ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:28:11,859 INFO  ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:28:24,385 INFO  ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:28:26,812 INFO  ExportPropertyManager.cs: 1136 - OnNumberBoxTrackingCompleted called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 21:28:35,610 INFO  ExportPropertyManager.cs: 1018 - OnComboboxSelectionChanged called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 22:16:01,745 INFO  ExportPropertyManager.cs: 422 - Configuration saved
+2022-04-26 22:16:01,747 INFO  ConfigurationSerialization.cs: 276 - URDF Configuration found
+<Link z:Id="i1" xmlns="http://schemas.datacontract.org/2004/07/SW2URDF" xmlns:i="http://www.w3.org/2001/XMLSchema-instance" xmlns:z="http://schemas.microsoft.com/2003/10/Serialization/"><Attributes><Attribute z:Id="i2"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">base_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i3" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i4" i:type="Origin"><Attributes><Attribute z:Id="i5"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>9.1830074983591922E-19</a:double><a:double>0.015606490089143955</a:double><a:double>1.5715252581589567E-18</a:double></Value></Attribute><Attribute z:Id="i6"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i6"/><XYZAttribute z:Ref="i5"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i7" i:type="Mass"><Attributes><Attribute z:Id="i8"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.93972387307473026</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i8"/></URDFElement><URDFElement z:Id="i9" i:type="Inertia"><Attributes><Attribute z:Id="i10"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0025142902225504422</Value></Attribute><Attribute z:Id="i11"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">8.02209898558041E-21</Value></Attribute><Attribute z:Id="i12"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">-7.374852413660824E-20</Value></Attribute><Attribute z:Id="i13"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0044229329573873041</Value></Attribute><Attribute z:Id="i14"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">2.2568808327940613E-20</Value></Attribute><Attribute z:Id="i15"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0021336882069818023</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i10"/><IxyAttribute z:Ref="i11"/><IxzAttribute z:Ref="i12"/><IyyAttribute z:Ref="i13"/><IyzAttribute z:Ref="i14"/><IzzAttribute z:Ref="i15"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i9"/><Mass z:Ref="i7"/><Origin z:Ref="i4"/></URDFElement><URDFElement z:Id="i16" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i17" i:type="Origin"><Attributes><Attribute z:Id="i18"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i19"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i19"/><XYZAttribute z:Ref="i18"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i20" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i21" i:type="Mesh"><Attributes><Attribute z:Id="i22"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i22"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i21"/></URDFElement><URDFElement z:Id="i23" i:type="Material"><Attributes><Attribute z:Id="i24"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i25" i:type="Color"><Attributes><Attribute z:Id="i26"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0.792156862745098</a:double><a:double>0.81960784313725488</a:double><a:double>0.93333333333333335</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i26"/></URDFElement><URDFElement z:Id="i27" i:type="Texture"><Attributes><Attribute z:Id="i28"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i28"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i25"/><NameAttribute z:Ref="i24"/><Texture z:Ref="i27"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i20"/><Material z:Ref="i23"/><Origin z:Ref="i17"/></URDFElement><URDFElement z:Id="i29" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i30" i:type="Origin"><Attributes><Attribute z:Id="i31"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i32"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i32"/><XYZAttribute z:Ref="i31"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i33" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i34" i:type="Mesh"><Attributes><Attribute z:Id="i35"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i35"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i34"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i33"/><Origin z:Ref="i30"/></URDFElement><URDFElement z:Id="i36" i:type="Joint"><Attributes><Attribute z:Id="i37"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute><Attribute z:Id="i38"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i39" i:type="Origin"><Attributes><Attribute z:Id="i40"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i41"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i41"/><XYZAttribute z:Ref="i40"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i42" i:type="ParentLink"><Attributes><Attribute z:Id="i43"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i43"/></URDFElement><URDFElement z:Id="i44" i:type="ChildLink"><Attributes><Attribute z:Id="i45"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i45"/></URDFElement><URDFElement z:Id="i46" i:type="Axis"><Attributes><Attribute z:Id="i47"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i47"/></URDFElement><URDFElement z:Id="i48" i:type="Limit"><Attributes><Attribute z:Id="i49"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i50"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i51"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i52"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i51"/><LowerAttribute z:Ref="i49"/><UpperAttribute z:Ref="i50"/><VelocityAttribute z:Ref="i52"/></URDFElement><URDFElement z:Id="i53" i:type="Calibration"><Attributes><Attribute z:Id="i54"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i55"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i55"/><RisingAttribute z:Ref="i54"/></URDFElement><URDFElement z:Id="i56" i:type="Dynamics"><Attributes><Attribute z:Id="i57"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i58"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i57"/><FrictionAttribute z:Ref="i58"/></URDFElement><URDFElement z:Id="i59" i:type="SafetyController"><Attributes><Attribute z:Id="i60"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i61"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i62"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i63"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i62"/><KVelocityAttribute z:Ref="i63"/><SoftLowerAttribute z:Ref="i61"/><SoftUpperAttribute z:Ref="i60"/></URDFElement><URDFElement z:Id="i64" i:type="Mimic"><Attributes><Attribute z:Id="i65"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i66"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i67"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i65"/><MultiplierAttribute z:Ref="i66"/><OffsetAttribute z:Ref="i67"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i46"/><AxisName/><Calibration z:Ref="i53"/><Child z:Ref="i44"/><CoordinateSystemName>Origin_global</CoordinateSystemName><Dynamics z:Ref="i56"/><Limit z:Ref="i48"/><Mimic z:Ref="i64"/><NameAttribute z:Ref="i37"/><Origin z:Ref="i39"/><Parent z:Ref="i42"/><Safety z:Ref="i59"/><TypeAttribute z:Ref="i38"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children/><Collision z:Ref="i29"/><Inertial z:Ref="i3"/><Joint z:Ref="i36"/><NameAttribute z:Ref="i2"/><Parent i:nil="true"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i16"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link>
+2022-04-26 22:16:01,816 INFO  ExportHelperExtension.cs: 347 - Creating joint turntable_link
+2022-04-26 22:16:01,823 INFO  ExportHelperExtension.cs: 1397 - Fixing components for base_link
+2022-04-26 22:16:01,824 INFO  ExportHelperExtension.cs: 1402 - Fixing 24
+2022-04-26 22:16:01,824 INFO  ExportHelperExtension.cs: 1409 - Component 24 is already fixed
+2022-04-26 22:16:02,369 INFO  : 0 - R1: 1, System.__ComObject, 1, System.Double[]
+2022-04-26 22:16:02,373 INFO  ExportHelperExtension.cs: 841 - R2: 0, 0
+2022-04-26 22:16:02,374 INFO  ExportHelperExtension.cs: 849 - L1: 0
+2022-04-26 22:16:02,374 INFO  ExportHelperExtension.cs: 857 - L2: 0
+2022-04-26 22:16:04,962 WARN  ExportHelperExtension.cs: 351 - Creating joint from parent base_link to child turntable_link failed
+2022-04-26 22:16:04,971 INFO  ExportHelperExtension.cs: 347 - Creating joint upperArm_link
+2022-04-26 22:16:04,971 INFO  ExportHelperExtension.cs: 1397 - Fixing components for turntable_link
+2022-04-26 22:16:04,972 INFO  ExportHelperExtension.cs: 1402 - Fixing 25
+2022-04-26 22:16:04,972 INFO  ExportHelperExtension.cs: 1402 - Fixing 24
+2022-04-26 22:16:04,972 INFO  ExportHelperExtension.cs: 1409 - Component 24 is already fixed
+2022-04-26 22:16:05,267 INFO  ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[]
+2022-04-26 22:16:05,267 INFO  ExportHelperExtension.cs: 841 - R2: 0, 0
+2022-04-26 22:16:05,268 INFO  ExportHelperExtension.cs: 849 - L1: 0
+2022-04-26 22:16:05,268 INFO  ExportHelperExtension.cs: 857 - L2: 0
+2022-04-26 22:16:05,268 INFO  ExportHelperExtension.cs: 1352 - Unfixing component 25
+2022-04-26 22:16:06,570 WARN  ExportHelperExtension.cs: 351 - Creating joint from parent turntable_link to child upperArm_link failed
+2022-04-26 22:16:06,575 INFO  ExportHelperExtension.cs: 347 - Creating joint forearm_link
+2022-04-26 22:16:06,576 INFO  ExportHelperExtension.cs: 1397 - Fixing components for upperArm_link
+2022-04-26 22:16:06,576 INFO  ExportHelperExtension.cs: 1402 - Fixing 26
+2022-04-26 22:16:06,576 INFO  ExportHelperExtension.cs: 1402 - Fixing 25
+2022-04-26 22:16:06,577 INFO  ExportHelperExtension.cs: 1402 - Fixing 24
+2022-04-26 22:16:06,577 INFO  ExportHelperExtension.cs: 1409 - Component 24 is already fixed
+2022-04-26 22:16:06,893 INFO  ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[]
+2022-04-26 22:16:06,893 INFO  ExportHelperExtension.cs: 841 - R2: 0, 0
+2022-04-26 22:16:06,893 INFO  ExportHelperExtension.cs: 849 - L1: 0
+2022-04-26 22:16:06,894 INFO  ExportHelperExtension.cs: 857 - L2: 0
+2022-04-26 22:16:06,894 INFO  ExportHelperExtension.cs: 1352 - Unfixing component 26
+2022-04-26 22:16:06,894 INFO  ExportHelperExtension.cs: 1352 - Unfixing component 25
+2022-04-26 22:16:08,241 WARN  ExportHelperExtension.cs: 351 - Creating joint from parent upperArm_link to child forearm_link failed
+2022-04-26 22:16:08,248 INFO  ExportHelperExtension.cs: 347 - Creating joint wrist_link
+2022-04-26 22:16:08,248 INFO  ExportHelperExtension.cs: 1397 - Fixing components for forearm_link
+2022-04-26 22:16:08,248 INFO  ExportHelperExtension.cs: 1402 - Fixing 35
+2022-04-26 22:16:08,249 INFO  ExportHelperExtension.cs: 1402 - Fixing 26
+2022-04-26 22:16:08,249 INFO  ExportHelperExtension.cs: 1402 - Fixing 25
+2022-04-26 22:16:08,249 INFO  ExportHelperExtension.cs: 1402 - Fixing 24
+2022-04-26 22:16:08,250 INFO  ExportHelperExtension.cs: 1409 - Component 24 is already fixed
+2022-04-26 22:16:08,561 INFO  ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[]
+2022-04-26 22:16:08,562 INFO  ExportHelperExtension.cs: 841 - R2: 0, 0
+2022-04-26 22:16:08,562 INFO  ExportHelperExtension.cs: 849 - L1: 0
+2022-04-26 22:16:08,562 INFO  ExportHelperExtension.cs: 857 - L2: 0
+2022-04-26 22:16:08,563 INFO  ExportHelperExtension.cs: 1352 - Unfixing component 35
+2022-04-26 22:16:08,563 INFO  ExportHelperExtension.cs: 1352 - Unfixing component 26
+2022-04-26 22:16:08,563 INFO  ExportHelperExtension.cs: 1352 - Unfixing component 25
+2022-04-26 22:16:09,913 WARN  ExportHelperExtension.cs: 351 - Creating joint from parent forearm_link to child wrist_link failed
+2022-04-26 22:16:09,916 INFO  ExportHelperExtension.cs: 347 - Creating joint carpus_link
+2022-04-26 22:16:09,916 INFO  ExportHelperExtension.cs: 1397 - Fixing components for wrist_link
+2022-04-26 22:16:09,916 INFO  ExportHelperExtension.cs: 1402 - Fixing 40
+2022-04-26 22:16:09,917 INFO  ExportHelperExtension.cs: 1402 - Fixing 35
+2022-04-26 22:16:09,917 INFO  ExportHelperExtension.cs: 1402 - Fixing 26
+2022-04-26 22:16:09,917 INFO  ExportHelperExtension.cs: 1402 - Fixing 25
+2022-04-26 22:16:09,918 INFO  ExportHelperExtension.cs: 1402 - Fixing 24
+2022-04-26 22:16:09,918 INFO  ExportHelperExtension.cs: 1409 - Component 24 is already fixed
+2022-04-26 22:16:10,240 INFO  ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[]
+2022-04-26 22:16:10,241 INFO  ExportHelperExtension.cs: 841 - R2: 0, 0
+2022-04-26 22:16:10,241 INFO  ExportHelperExtension.cs: 849 - L1: 0
+2022-04-26 22:16:10,241 INFO  ExportHelperExtension.cs: 857 - L2: 0
+2022-04-26 22:16:10,242 INFO  ExportHelperExtension.cs: 1352 - Unfixing component 40
+2022-04-26 22:16:10,242 INFO  ExportHelperExtension.cs: 1352 - Unfixing component 35
+2022-04-26 22:16:10,242 INFO  ExportHelperExtension.cs: 1352 - Unfixing component 26
+2022-04-26 22:16:10,242 INFO  ExportHelperExtension.cs: 1352 - Unfixing component 25
+2022-04-26 22:16:11,595 WARN  ExportHelperExtension.cs: 351 - Creating joint from parent wrist_link to child carpus_link failed
+2022-04-26 22:16:11,599 INFO  ExportHelperExtension.cs: 347 - Creating joint endEffector_link
+2022-04-26 22:16:11,599 INFO  ExportHelperExtension.cs: 1397 - Fixing components for carpus_link
+2022-04-26 22:16:11,600 INFO  ExportHelperExtension.cs: 1402 - Fixing 39
+2022-04-26 22:16:11,600 INFO  ExportHelperExtension.cs: 1402 - Fixing 40
+2022-04-26 22:16:11,600 INFO  ExportHelperExtension.cs: 1402 - Fixing 35
+2022-04-26 22:16:11,601 INFO  ExportHelperExtension.cs: 1402 - Fixing 26
+2022-04-26 22:16:11,601 INFO  ExportHelperExtension.cs: 1402 - Fixing 25
+2022-04-26 22:16:11,601 INFO  ExportHelperExtension.cs: 1402 - Fixing 24
+2022-04-26 22:16:11,607 INFO  ExportHelperExtension.cs: 1409 - Component 24 is already fixed
+2022-04-26 22:16:12,003 INFO  ExportHelperExtension.cs: 828 - R1: 1, System.__ComObject, 1, System.Double[]
+2022-04-26 22:16:12,004 INFO  ExportHelperExtension.cs: 841 - R2: 0, 0
+2022-04-26 22:16:12,004 INFO  ExportHelperExtension.cs: 849 - L1: 0
+2022-04-26 22:16:12,005 INFO  ExportHelperExtension.cs: 857 - L2: 0
+2022-04-26 22:16:12,005 INFO  ExportHelperExtension.cs: 1352 - Unfixing component 39
+2022-04-26 22:16:12,005 INFO  ExportHelperExtension.cs: 1352 - Unfixing component 40
+2022-04-26 22:16:12,006 INFO  ExportHelperExtension.cs: 1352 - Unfixing component 35
+2022-04-26 22:16:12,006 INFO  ExportHelperExtension.cs: 1352 - Unfixing component 26
+2022-04-26 22:16:12,006 INFO  ExportHelperExtension.cs: 1352 - Unfixing component 25
+2022-04-26 22:16:13,493 WARN  ExportHelperExtension.cs: 351 - Creating joint from parent carpus_link to child endEffector_link failed
+2022-04-26 22:16:13,569 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from ArmAssembly.SLDASM
+2022-04-26 22:16:13,570 INFO  ExportHelperExtension.cs: 1136 - Found 59 in ArmAssembly.SLDASM
+2022-04-26 22:16:13,570 INFO  ExportHelperExtension.cs: 1145 - Found 7 features of type [CoordSys] in ArmAssembly.SLDASM
+2022-04-26 22:16:13,570 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 22:16:13,571 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 22:16:13,571 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from base-1
+2022-04-26 22:16:13,571 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 22:16:13,572 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in base-1
+2022-04-26 22:16:13,572 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Shoulder-1
+2022-04-26 22:16:13,572 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 22:16:13,573 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Shoulder-1
+2022-04-26 22:16:13,573 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from UpperArm-1
+2022-04-26 22:16:13,573 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 22:16:13,574 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in UpperArm-1
+2022-04-26 22:16:13,574 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Forarm-1
+2022-04-26 22:16:13,575 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 22:16:13,575 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Forarm-1
+2022-04-26 22:16:13,575 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Wrist-1
+2022-04-26 22:16:13,576 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 22:16:13,576 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Wrist-1
+2022-04-26 22:16:13,576 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from WristStructure-1
+2022-04-26 22:16:13,577 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 22:16:13,577 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in WristStructure-1
+2022-04-26 22:16:13,577 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [CoordSys] from Manipulator-1
+2022-04-26 22:16:13,578 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 22:16:13,578 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [CoordSys] in Manipulator-1
+2022-04-26 22:16:13,578 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from ArmAssembly.SLDASM
+2022-04-26 22:16:13,579 INFO  ExportHelperExtension.cs: 1136 - Found 59 in ArmAssembly.SLDASM
+2022-04-26 22:16:13,579 INFO  ExportHelperExtension.cs: 1145 - Found 6 features of type [RefAxis] in ArmAssembly.SLDASM
+2022-04-26 22:16:13,579 INFO  ExportHelperExtension.cs: 1148 - Proceeding through assembly components
+2022-04-26 22:16:13,580 INFO  ExportHelperExtension.cs: 1160 - 7 components to check
+2022-04-26 22:16:13,580 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from base-1
+2022-04-26 22:16:13,580 INFO  ExportHelperExtension.cs: 1136 - Found 27 in base-1
+2022-04-26 22:16:13,580 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in base-1
+2022-04-26 22:16:13,581 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Shoulder-1
+2022-04-26 22:16:13,581 INFO  ExportHelperExtension.cs: 1136 - Found 50 in Shoulder-1
+2022-04-26 22:16:13,581 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Shoulder-1
+2022-04-26 22:16:13,582 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from UpperArm-1
+2022-04-26 22:16:13,582 INFO  ExportHelperExtension.cs: 1136 - Found 41 in UpperArm-1
+2022-04-26 22:16:13,582 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in UpperArm-1
+2022-04-26 22:16:13,583 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Forarm-1
+2022-04-26 22:16:13,583 INFO  ExportHelperExtension.cs: 1136 - Found 35 in Forarm-1
+2022-04-26 22:16:13,583 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Forarm-1
+2022-04-26 22:16:13,584 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Wrist-1
+2022-04-26 22:16:13,584 INFO  ExportHelperExtension.cs: 1136 - Found 30 in Wrist-1
+2022-04-26 22:16:13,584 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Wrist-1
+2022-04-26 22:16:13,585 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from WristStructure-1
+2022-04-26 22:16:13,585 INFO  ExportHelperExtension.cs: 1136 - Found 30 in WristStructure-1
+2022-04-26 22:16:13,585 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in WristStructure-1
+2022-04-26 22:16:13,586 INFO  ExportHelperExtension.cs: 1125 - Retrieving features of type [RefAxis] from Manipulator-1
+2022-04-26 22:16:13,586 INFO  ExportHelperExtension.cs: 1136 - Found 39 in Manipulator-1
+2022-04-26 22:16:13,586 INFO  ExportHelperExtension.cs: 1145 - Found 0 features of type [RefAxis] in Manipulator-1
+2022-04-26 22:16:13,635 INFO  ExportPropertyManager.cs: 1142 - AfterClose called. This method no longer throws an Exception. It just silently does nothing. Ok, except for this logging message
+2022-04-26 22:17:26,131 INFO  AssemblyExportForm.cs: 253 - Completing URDF export
+2022-04-26 22:17:26,137 INFO  ConfigurationSerialization.cs: 276 - URDF Configuration found
+<Link z:Id="i1" xmlns="http://schemas.datacontract.org/2004/07/SW2URDF" xmlns:i="http://www.w3.org/2001/XMLSchema-instance" xmlns:z="http://schemas.microsoft.com/2003/10/Serialization/"><Attributes><Attribute z:Id="i2"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">base_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i3" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i4" i:type="Origin"><Attributes><Attribute z:Id="i5"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>9.1830074983591922E-19</a:double><a:double>0.015606490089143955</a:double><a:double>1.5715252581589567E-18</a:double></Value></Attribute><Attribute z:Id="i6"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i6"/><XYZAttribute z:Ref="i5"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i7" i:type="Mass"><Attributes><Attribute z:Id="i8"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.93972387307473026</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i8"/></URDFElement><URDFElement z:Id="i9" i:type="Inertia"><Attributes><Attribute z:Id="i10"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0025142902225504422</Value></Attribute><Attribute z:Id="i11"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">8.02209898558041E-21</Value></Attribute><Attribute z:Id="i12"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">-7.374852413660824E-20</Value></Attribute><Attribute z:Id="i13"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0044229329573873041</Value></Attribute><Attribute z:Id="i14"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">2.2568808327940613E-20</Value></Attribute><Attribute z:Id="i15"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0.0021336882069818023</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i10"/><IxyAttribute z:Ref="i11"/><IxzAttribute z:Ref="i12"/><IyyAttribute z:Ref="i13"/><IyzAttribute z:Ref="i14"/><IzzAttribute z:Ref="i15"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i9"/><Mass z:Ref="i7"/><Origin z:Ref="i4"/></URDFElement><URDFElement z:Id="i16" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i17" i:type="Origin"><Attributes><Attribute z:Id="i18"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i19"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i19"/><XYZAttribute z:Ref="i18"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i20" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i21" i:type="Mesh"><Attributes><Attribute z:Id="i22"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i22"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i21"/></URDFElement><URDFElement z:Id="i23" i:type="Material"><Attributes><Attribute z:Id="i24"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i25" i:type="Color"><Attributes><Attribute z:Id="i26"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0.792156862745098</a:double><a:double>0.81960784313725488</a:double><a:double>0.93333333333333335</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i26"/></URDFElement><URDFElement z:Id="i27" i:type="Texture"><Attributes><Attribute z:Id="i28"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i28"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i25"/><NameAttribute z:Ref="i24"/><Texture z:Ref="i27"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i20"/><Material z:Ref="i23"/><Origin z:Ref="i17"/></URDFElement><URDFElement z:Id="i29" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i30" i:type="Origin"><Attributes><Attribute z:Id="i31"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i32"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i32"/><XYZAttribute z:Ref="i31"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i33" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i34" i:type="Mesh"><Attributes><Attribute z:Id="i35"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i35"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i34"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i33"/><Origin z:Ref="i30"/></URDFElement><URDFElement z:Id="i36" i:type="Joint"><Attributes><Attribute z:Id="i37"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute><Attribute z:Id="i38"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i39" i:type="Origin"><Attributes><Attribute z:Id="i40"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i41"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i41"/><XYZAttribute z:Ref="i40"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i42" i:type="ParentLink"><Attributes><Attribute z:Id="i43"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i43"/></URDFElement><URDFElement z:Id="i44" i:type="ChildLink"><Attributes><Attribute z:Id="i45"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i45"/></URDFElement><URDFElement z:Id="i46" i:type="Axis"><Attributes><Attribute z:Id="i47"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i47"/></URDFElement><URDFElement z:Id="i48" i:type="Limit"><Attributes><Attribute z:Id="i49"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i50"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i51"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i52"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i51"/><LowerAttribute z:Ref="i49"/><UpperAttribute z:Ref="i50"/><VelocityAttribute z:Ref="i52"/></URDFElement><URDFElement z:Id="i53" i:type="Calibration"><Attributes><Attribute z:Id="i54"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i55"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i55"/><RisingAttribute z:Ref="i54"/></URDFElement><URDFElement z:Id="i56" i:type="Dynamics"><Attributes><Attribute z:Id="i57"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i58"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i57"/><FrictionAttribute z:Ref="i58"/></URDFElement><URDFElement z:Id="i59" i:type="SafetyController"><Attributes><Attribute z:Id="i60"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i61"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i62"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i63"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i62"/><KVelocityAttribute z:Ref="i63"/><SoftLowerAttribute z:Ref="i61"/><SoftUpperAttribute z:Ref="i60"/></URDFElement><URDFElement z:Id="i64" i:type="Mimic"><Attributes><Attribute z:Id="i65"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i66"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i67"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i65"/><MultiplierAttribute z:Ref="i66"/><OffsetAttribute z:Ref="i67"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i46"/><AxisName/><Calibration z:Ref="i53"/><Child z:Ref="i44"/><CoordinateSystemName>Origin_global</CoordinateSystemName><Dynamics z:Ref="i56"/><Limit z:Ref="i48"/><Mimic z:Ref="i64"/><NameAttribute z:Ref="i37"/><Origin z:Ref="i39"/><Parent z:Ref="i42"/><Safety z:Ref="i59"/><TypeAttribute z:Ref="i38"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children><Link z:Id="i68"><Attributes><Attribute z:Id="i69"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">turntable_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i70" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i71" i:type="Origin"><Attributes><Attribute z:Id="i72"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i73"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i73"/><XYZAttribute z:Ref="i72"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i74" i:type="Mass"><Attributes><Attribute z:Id="i75"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i75"/></URDFElement><URDFElement z:Id="i76" i:type="Inertia"><Attributes><Attribute z:Id="i77"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i78"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i79"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i80"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i81"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i82"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i77"/><IxyAttribute z:Ref="i78"/><IxzAttribute z:Ref="i79"/><IyyAttribute z:Ref="i80"/><IyzAttribute z:Ref="i81"/><IzzAttribute z:Ref="i82"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i76"/><Mass z:Ref="i74"/><Origin z:Ref="i71"/></URDFElement><URDFElement z:Id="i83" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i84" i:type="Origin"><Attributes><Attribute z:Id="i85"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i86"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i86"/><XYZAttribute z:Ref="i85"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i87" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i88" i:type="Mesh"><Attributes><Attribute z:Id="i89"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i89"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i88"/></URDFElement><URDFElement z:Id="i90" i:type="Material"><Attributes><Attribute z:Id="i91"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i92" i:type="Color"><Attributes><Attribute z:Id="i93"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>1</a:double><a:double>1</a:double><a:double>1</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i93"/></URDFElement><URDFElement z:Id="i94" i:type="Texture"><Attributes><Attribute z:Id="i95"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i95"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i92"/><NameAttribute z:Ref="i91"/><Texture z:Ref="i94"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i87"/><Material z:Ref="i90"/><Origin z:Ref="i84"/></URDFElement><URDFElement z:Id="i96" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i97" i:type="Origin"><Attributes><Attribute z:Id="i98"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i99"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i99"/><XYZAttribute z:Ref="i98"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i100" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i101" i:type="Mesh"><Attributes><Attribute z:Id="i102"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i102"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i101"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i100"/><Origin z:Ref="i97"/></URDFElement><URDFElement z:Id="i103" i:type="Joint"><Attributes><Attribute z:Id="i104"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">turntable_joint</Value></Attribute><Attribute z:Id="i105"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">Automatically Detect</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i106" i:type="Origin"><Attributes><Attribute z:Id="i107"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i108"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i108"/><XYZAttribute z:Ref="i107"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i109" i:type="ParentLink"><Attributes><Attribute z:Id="i110"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i110"/></URDFElement><URDFElement z:Id="i111" i:type="ChildLink"><Attributes><Attribute z:Id="i112"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i112"/></URDFElement><URDFElement z:Id="i113" i:type="Axis"><Attributes><Attribute z:Id="i114"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i114"/></URDFElement><URDFElement z:Id="i115" i:type="Limit"><Attributes><Attribute z:Id="i116"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i117"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i118"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i119"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i118"/><LowerAttribute z:Ref="i116"/><UpperAttribute z:Ref="i117"/><VelocityAttribute z:Ref="i119"/></URDFElement><URDFElement z:Id="i120" i:type="Calibration"><Attributes><Attribute z:Id="i121"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i122"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i122"/><RisingAttribute z:Ref="i121"/></URDFElement><URDFElement z:Id="i123" i:type="Dynamics"><Attributes><Attribute z:Id="i124"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i125"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i124"/><FrictionAttribute z:Ref="i125"/></URDFElement><URDFElement z:Id="i126" i:type="SafetyController"><Attributes><Attribute z:Id="i127"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i128"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i129"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i130"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i129"/><KVelocityAttribute z:Ref="i130"/><SoftLowerAttribute z:Ref="i128"/><SoftUpperAttribute z:Ref="i127"/></URDFElement><URDFElement z:Id="i131" i:type="Mimic"><Attributes><Attribute z:Id="i132"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i133"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i134"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i132"/><MultiplierAttribute z:Ref="i133"/><OffsetAttribute z:Ref="i134"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i113"/><AxisName>Axis1</AxisName><Calibration z:Ref="i120"/><Child z:Ref="i111"/><CoordinateSystemName>Automatically Generate</CoordinateSystemName><Dynamics z:Ref="i123"/><Limit z:Ref="i115"/><Mimic z:Ref="i131"/><NameAttribute z:Ref="i104"/><Origin z:Ref="i106"/><Parent z:Ref="i109"/><Safety z:Ref="i126"/><TypeAttribute z:Ref="i105"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children><Link z:Id="i135"><Attributes><Attribute z:Id="i136"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">upperArm_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i137" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i138" i:type="Origin"><Attributes><Attribute z:Id="i139"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i140"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i140"/><XYZAttribute z:Ref="i139"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i141" i:type="Mass"><Attributes><Attribute z:Id="i142"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i142"/></URDFElement><URDFElement z:Id="i143" i:type="Inertia"><Attributes><Attribute z:Id="i144"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i145"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i146"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i147"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i148"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i149"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i144"/><IxyAttribute z:Ref="i145"/><IxzAttribute z:Ref="i146"/><IyyAttribute z:Ref="i147"/><IyzAttribute z:Ref="i148"/><IzzAttribute z:Ref="i149"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i143"/><Mass z:Ref="i141"/><Origin z:Ref="i138"/></URDFElement><URDFElement z:Id="i150" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i151" i:type="Origin"><Attributes><Attribute z:Id="i152"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i153"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i153"/><XYZAttribute z:Ref="i152"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i154" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i155" i:type="Mesh"><Attributes><Attribute z:Id="i156"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i156"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i155"/></URDFElement><URDFElement z:Id="i157" i:type="Material"><Attributes><Attribute z:Id="i158"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i159" i:type="Color"><Attributes><Attribute z:Id="i160"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>1</a:double><a:double>1</a:double><a:double>1</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i160"/></URDFElement><URDFElement z:Id="i161" i:type="Texture"><Attributes><Attribute z:Id="i162"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i162"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i159"/><NameAttribute z:Ref="i158"/><Texture z:Ref="i161"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i154"/><Material z:Ref="i157"/><Origin z:Ref="i151"/></URDFElement><URDFElement z:Id="i163" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i164" i:type="Origin"><Attributes><Attribute z:Id="i165"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i166"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i166"/><XYZAttribute z:Ref="i165"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i167" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i168" i:type="Mesh"><Attributes><Attribute z:Id="i169"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i169"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i168"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i167"/><Origin z:Ref="i164"/></URDFElement><URDFElement z:Id="i170" i:type="Joint"><Attributes><Attribute z:Id="i171"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">shoulder_joint</Value></Attribute><Attribute z:Id="i172"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">Automatically Detect</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i173" i:type="Origin"><Attributes><Attribute z:Id="i174"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i175"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i175"/><XYZAttribute z:Ref="i174"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i176" i:type="ParentLink"><Attributes><Attribute z:Id="i177"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i177"/></URDFElement><URDFElement z:Id="i178" i:type="ChildLink"><Attributes><Attribute z:Id="i179"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i179"/></URDFElement><URDFElement z:Id="i180" i:type="Axis"><Attributes><Attribute z:Id="i181"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i181"/></URDFElement><URDFElement z:Id="i182" i:type="Limit"><Attributes><Attribute z:Id="i183"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i184"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i185"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i186"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i185"/><LowerAttribute z:Ref="i183"/><UpperAttribute z:Ref="i184"/><VelocityAttribute z:Ref="i186"/></URDFElement><URDFElement z:Id="i187" i:type="Calibration"><Attributes><Attribute z:Id="i188"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i189"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i189"/><RisingAttribute z:Ref="i188"/></URDFElement><URDFElement z:Id="i190" i:type="Dynamics"><Attributes><Attribute z:Id="i191"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i192"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i191"/><FrictionAttribute z:Ref="i192"/></URDFElement><URDFElement z:Id="i193" i:type="SafetyController"><Attributes><Attribute z:Id="i194"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i195"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i196"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i197"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i196"/><KVelocityAttribute z:Ref="i197"/><SoftLowerAttribute z:Ref="i195"/><SoftUpperAttribute z:Ref="i194"/></URDFElement><URDFElement z:Id="i198" i:type="Mimic"><Attributes><Attribute z:Id="i199"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i200"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i201"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i199"/><MultiplierAttribute z:Ref="i200"/><OffsetAttribute z:Ref="i201"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i180"/><AxisName>Axis2</AxisName><Calibration z:Ref="i187"/><Child z:Ref="i178"/><CoordinateSystemName>Automatically Generate</CoordinateSystemName><Dynamics z:Ref="i190"/><Limit z:Ref="i182"/><Mimic z:Ref="i198"/><NameAttribute z:Ref="i171"/><Origin z:Ref="i173"/><Parent z:Ref="i176"/><Safety z:Ref="i193"/><TypeAttribute z:Ref="i172"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children><Link z:Id="i202"><Attributes><Attribute z:Id="i203"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">forearm_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i204" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i205" i:type="Origin"><Attributes><Attribute z:Id="i206"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i207"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i207"/><XYZAttribute z:Ref="i206"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i208" i:type="Mass"><Attributes><Attribute z:Id="i209"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i209"/></URDFElement><URDFElement z:Id="i210" i:type="Inertia"><Attributes><Attribute z:Id="i211"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i212"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i213"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i214"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i215"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i216"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i211"/><IxyAttribute z:Ref="i212"/><IxzAttribute z:Ref="i213"/><IyyAttribute z:Ref="i214"/><IyzAttribute z:Ref="i215"/><IzzAttribute z:Ref="i216"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i210"/><Mass z:Ref="i208"/><Origin z:Ref="i205"/></URDFElement><URDFElement z:Id="i217" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i218" i:type="Origin"><Attributes><Attribute z:Id="i219"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i220"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i220"/><XYZAttribute z:Ref="i219"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i221" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i222" i:type="Mesh"><Attributes><Attribute z:Id="i223"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i223"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i222"/></URDFElement><URDFElement z:Id="i224" i:type="Material"><Attributes><Attribute z:Id="i225"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i226" i:type="Color"><Attributes><Attribute z:Id="i227"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>1</a:double><a:double>1</a:double><a:double>1</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i227"/></URDFElement><URDFElement z:Id="i228" i:type="Texture"><Attributes><Attribute z:Id="i229"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i229"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i226"/><NameAttribute z:Ref="i225"/><Texture z:Ref="i228"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i221"/><Material z:Ref="i224"/><Origin z:Ref="i218"/></URDFElement><URDFElement z:Id="i230" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i231" i:type="Origin"><Attributes><Attribute z:Id="i232"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i233"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i233"/><XYZAttribute z:Ref="i232"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i234" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i235" i:type="Mesh"><Attributes><Attribute z:Id="i236"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i236"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i235"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i234"/><Origin z:Ref="i231"/></URDFElement><URDFElement z:Id="i237" i:type="Joint"><Attributes><Attribute z:Id="i238"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">elbowPitch_joint</Value></Attribute><Attribute z:Id="i239"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">Automatically Detect</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i240" i:type="Origin"><Attributes><Attribute z:Id="i241"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i242"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i242"/><XYZAttribute z:Ref="i241"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i243" i:type="ParentLink"><Attributes><Attribute z:Id="i244"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i244"/></URDFElement><URDFElement z:Id="i245" i:type="ChildLink"><Attributes><Attribute z:Id="i246"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i246"/></URDFElement><URDFElement z:Id="i247" i:type="Axis"><Attributes><Attribute z:Id="i248"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i248"/></URDFElement><URDFElement z:Id="i249" i:type="Limit"><Attributes><Attribute z:Id="i250"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i251"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i252"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i253"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i252"/><LowerAttribute z:Ref="i250"/><UpperAttribute z:Ref="i251"/><VelocityAttribute z:Ref="i253"/></URDFElement><URDFElement z:Id="i254" i:type="Calibration"><Attributes><Attribute z:Id="i255"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i256"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i256"/><RisingAttribute z:Ref="i255"/></URDFElement><URDFElement z:Id="i257" i:type="Dynamics"><Attributes><Attribute z:Id="i258"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i259"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i258"/><FrictionAttribute z:Ref="i259"/></URDFElement><URDFElement z:Id="i260" i:type="SafetyController"><Attributes><Attribute z:Id="i261"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i262"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i263"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i264"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i263"/><KVelocityAttribute z:Ref="i264"/><SoftLowerAttribute z:Ref="i262"/><SoftUpperAttribute z:Ref="i261"/></URDFElement><URDFElement z:Id="i265" i:type="Mimic"><Attributes><Attribute z:Id="i266"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i267"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i268"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i266"/><MultiplierAttribute z:Ref="i267"/><OffsetAttribute z:Ref="i268"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i247"/><AxisName>Axis3</AxisName><Calibration z:Ref="i254"/><Child z:Ref="i245"/><CoordinateSystemName>Automatically Generate</CoordinateSystemName><Dynamics z:Ref="i257"/><Limit z:Ref="i249"/><Mimic z:Ref="i265"/><NameAttribute z:Ref="i238"/><Origin z:Ref="i240"/><Parent z:Ref="i243"/><Safety z:Ref="i260"/><TypeAttribute z:Ref="i239"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children><Link z:Id="i269"><Attributes><Attribute z:Id="i270"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">wrist_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i271" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i272" i:type="Origin"><Attributes><Attribute z:Id="i273"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i274"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i274"/><XYZAttribute z:Ref="i273"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i275" i:type="Mass"><Attributes><Attribute z:Id="i276"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i276"/></URDFElement><URDFElement z:Id="i277" i:type="Inertia"><Attributes><Attribute z:Id="i278"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i279"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i280"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i281"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i282"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i283"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i278"/><IxyAttribute z:Ref="i279"/><IxzAttribute z:Ref="i280"/><IyyAttribute z:Ref="i281"/><IyzAttribute z:Ref="i282"/><IzzAttribute z:Ref="i283"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i277"/><Mass z:Ref="i275"/><Origin z:Ref="i272"/></URDFElement><URDFElement z:Id="i284" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i285" i:type="Origin"><Attributes><Attribute z:Id="i286"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i287"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i287"/><XYZAttribute z:Ref="i286"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i288" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i289" i:type="Mesh"><Attributes><Attribute z:Id="i290"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i290"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i289"/></URDFElement><URDFElement z:Id="i291" i:type="Material"><Attributes><Attribute z:Id="i292"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i293" i:type="Color"><Attributes><Attribute z:Id="i294"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>1</a:double><a:double>1</a:double><a:double>1</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i294"/></URDFElement><URDFElement z:Id="i295" i:type="Texture"><Attributes><Attribute z:Id="i296"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i296"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i293"/><NameAttribute z:Ref="i292"/><Texture z:Ref="i295"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i288"/><Material z:Ref="i291"/><Origin z:Ref="i285"/></URDFElement><URDFElement z:Id="i297" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i298" i:type="Origin"><Attributes><Attribute z:Id="i299"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i300"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i300"/><XYZAttribute z:Ref="i299"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i301" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i302" i:type="Mesh"><Attributes><Attribute z:Id="i303"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i303"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i302"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i301"/><Origin z:Ref="i298"/></URDFElement><URDFElement z:Id="i304" i:type="Joint"><Attributes><Attribute z:Id="i305"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">elbowRoll_joint</Value></Attribute><Attribute z:Id="i306"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">Automatically Detect</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i307" i:type="Origin"><Attributes><Attribute z:Id="i308"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i309"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i309"/><XYZAttribute z:Ref="i308"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i310" i:type="ParentLink"><Attributes><Attribute z:Id="i311"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i311"/></URDFElement><URDFElement z:Id="i312" i:type="ChildLink"><Attributes><Attribute z:Id="i313"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i313"/></URDFElement><URDFElement z:Id="i314" i:type="Axis"><Attributes><Attribute z:Id="i315"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i315"/></URDFElement><URDFElement z:Id="i316" i:type="Limit"><Attributes><Attribute z:Id="i317"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i318"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i319"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i320"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i319"/><LowerAttribute z:Ref="i317"/><UpperAttribute z:Ref="i318"/><VelocityAttribute z:Ref="i320"/></URDFElement><URDFElement z:Id="i321" i:type="Calibration"><Attributes><Attribute z:Id="i322"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i323"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i323"/><RisingAttribute z:Ref="i322"/></URDFElement><URDFElement z:Id="i324" i:type="Dynamics"><Attributes><Attribute z:Id="i325"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i326"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i325"/><FrictionAttribute z:Ref="i326"/></URDFElement><URDFElement z:Id="i327" i:type="SafetyController"><Attributes><Attribute z:Id="i328"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i329"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i330"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i331"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i330"/><KVelocityAttribute z:Ref="i331"/><SoftLowerAttribute z:Ref="i329"/><SoftUpperAttribute z:Ref="i328"/></URDFElement><URDFElement z:Id="i332" i:type="Mimic"><Attributes><Attribute z:Id="i333"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i334"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i335"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i333"/><MultiplierAttribute z:Ref="i334"/><OffsetAttribute z:Ref="i335"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i314"/><AxisName>Axis4</AxisName><Calibration z:Ref="i321"/><Child z:Ref="i312"/><CoordinateSystemName>Automatically Generate</CoordinateSystemName><Dynamics z:Ref="i324"/><Limit z:Ref="i316"/><Mimic z:Ref="i332"/><NameAttribute z:Ref="i305"/><Origin z:Ref="i307"/><Parent z:Ref="i310"/><Safety z:Ref="i327"/><TypeAttribute z:Ref="i306"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children><Link z:Id="i336"><Attributes><Attribute z:Id="i337"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">carpus_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i338" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i339" i:type="Origin"><Attributes><Attribute z:Id="i340"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i341"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i341"/><XYZAttribute z:Ref="i340"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i342" i:type="Mass"><Attributes><Attribute z:Id="i343"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i343"/></URDFElement><URDFElement z:Id="i344" i:type="Inertia"><Attributes><Attribute z:Id="i345"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i346"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i347"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i348"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i349"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i350"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i345"/><IxyAttribute z:Ref="i346"/><IxzAttribute z:Ref="i347"/><IyyAttribute z:Ref="i348"/><IyzAttribute z:Ref="i349"/><IzzAttribute z:Ref="i350"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i344"/><Mass z:Ref="i342"/><Origin z:Ref="i339"/></URDFElement><URDFElement z:Id="i351" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i352" i:type="Origin"><Attributes><Attribute z:Id="i353"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i354"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i354"/><XYZAttribute z:Ref="i353"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i355" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i356" i:type="Mesh"><Attributes><Attribute z:Id="i357"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i357"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i356"/></URDFElement><URDFElement z:Id="i358" i:type="Material"><Attributes><Attribute z:Id="i359"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i360" i:type="Color"><Attributes><Attribute z:Id="i361"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>1</a:double><a:double>1</a:double><a:double>1</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i361"/></URDFElement><URDFElement z:Id="i362" i:type="Texture"><Attributes><Attribute z:Id="i363"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i363"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i360"/><NameAttribute z:Ref="i359"/><Texture z:Ref="i362"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i355"/><Material z:Ref="i358"/><Origin z:Ref="i352"/></URDFElement><URDFElement z:Id="i364" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i365" i:type="Origin"><Attributes><Attribute z:Id="i366"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i367"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i367"/><XYZAttribute z:Ref="i366"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i368" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i369" i:type="Mesh"><Attributes><Attribute z:Id="i370"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i370"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i369"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i368"/><Origin z:Ref="i365"/></URDFElement><URDFElement z:Id="i371" i:type="Joint"><Attributes><Attribute z:Id="i372"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">wristPitch_joint</Value></Attribute><Attribute z:Id="i373"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">Automatically Detect</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i374" i:type="Origin"><Attributes><Attribute z:Id="i375"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i376"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i376"/><XYZAttribute z:Ref="i375"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i377" i:type="ParentLink"><Attributes><Attribute z:Id="i378"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i378"/></URDFElement><URDFElement z:Id="i379" i:type="ChildLink"><Attributes><Attribute z:Id="i380"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i380"/></URDFElement><URDFElement z:Id="i381" i:type="Axis"><Attributes><Attribute z:Id="i382"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i382"/></URDFElement><URDFElement z:Id="i383" i:type="Limit"><Attributes><Attribute z:Id="i384"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i385"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i386"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i387"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i386"/><LowerAttribute z:Ref="i384"/><UpperAttribute z:Ref="i385"/><VelocityAttribute z:Ref="i387"/></URDFElement><URDFElement z:Id="i388" i:type="Calibration"><Attributes><Attribute z:Id="i389"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i390"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i390"/><RisingAttribute z:Ref="i389"/></URDFElement><URDFElement z:Id="i391" i:type="Dynamics"><Attributes><Attribute z:Id="i392"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i393"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i392"/><FrictionAttribute z:Ref="i393"/></URDFElement><URDFElement z:Id="i394" i:type="SafetyController"><Attributes><Attribute z:Id="i395"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i396"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i397"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i398"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i397"/><KVelocityAttribute z:Ref="i398"/><SoftLowerAttribute z:Ref="i396"/><SoftUpperAttribute z:Ref="i395"/></URDFElement><URDFElement z:Id="i399" i:type="Mimic"><Attributes><Attribute z:Id="i400"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i401"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i402"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i400"/><MultiplierAttribute z:Ref="i401"/><OffsetAttribute z:Ref="i402"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i381"/><AxisName>Axis5</AxisName><Calibration z:Ref="i388"/><Child z:Ref="i379"/><CoordinateSystemName>Automatically Generate</CoordinateSystemName><Dynamics z:Ref="i391"/><Limit z:Ref="i383"/><Mimic z:Ref="i399"/><NameAttribute z:Ref="i372"/><Origin z:Ref="i374"/><Parent z:Ref="i377"/><Safety z:Ref="i394"/><TypeAttribute z:Ref="i373"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children><Link z:Id="i403"><Attributes><Attribute z:Id="i404"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">endEffector_link</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i405" i:type="Inertial"><Attributes/><ChildElements><URDFElement z:Id="i406" i:type="Origin"><Attributes><Attribute z:Id="i407"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i408"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i408"/><XYZAttribute z:Ref="i407"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i409" i:type="Mass"><Attributes><Attribute z:Id="i410"><AttributeType>value</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute></Attributes><ChildElements/><ElementName>mass</ElementName><required>false</required><ValueAttribute z:Ref="i410"/></URDFElement><URDFElement z:Id="i411" i:type="Inertia"><Attributes><Attribute z:Id="i412"><AttributeType>ixx</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i413"><AttributeType>ixy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i414"><AttributeType>ixz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i415"><AttributeType>iyy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i416"><AttributeType>iyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute><Attribute z:Id="i417"><AttributeType>izz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:double" xmlns:a="http://www.w3.org/2001/XMLSchema">0</Value></Attribute></Attributes><ChildElements/><ElementName>inertia</ElementName><required>false</required><IxxAttribute z:Ref="i412"/><IxyAttribute z:Ref="i413"/><IxzAttribute z:Ref="i414"/><IyyAttribute z:Ref="i415"/><IyzAttribute z:Ref="i416"/><IzzAttribute z:Ref="i417"/></URDFElement></ChildElements><ElementName>inertial</ElementName><required>false</required><Inertia z:Ref="i411"/><Mass z:Ref="i409"/><Origin z:Ref="i406"/></URDFElement><URDFElement z:Id="i418" i:type="Visual"><Attributes/><ChildElements><URDFElement z:Id="i419" i:type="Origin"><Attributes><Attribute z:Id="i420"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i421"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i421"/><XYZAttribute z:Ref="i420"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i422" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i423" i:type="Mesh"><Attributes><Attribute z:Id="i424"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i424"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i423"/></URDFElement><URDFElement z:Id="i425" i:type="Material"><Attributes><Attribute z:Id="i426"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements><URDFElement z:Id="i427" i:type="Color"><Attributes><Attribute z:Id="i428"><AttributeType>rgba</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>1</a:double><a:double>1</a:double><a:double>1</a:double><a:double>1</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>color</ElementName><required>false</required><RGBAAttribute z:Ref="i428"/></URDFElement><URDFElement z:Id="i429" i:type="Texture"><Attributes><Attribute z:Id="i430"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>texture</ElementName><required>false</required><FilenameAttribute z:Ref="i430"/><wFilename/></URDFElement></ChildElements><ElementName>material</ElementName><required>false</required><Color z:Ref="i427"/><NameAttribute z:Ref="i426"/><Texture z:Ref="i429"/></URDFElement></ChildElements><ElementName>visual</ElementName><required>false</required><Geometry z:Ref="i422"/><Material z:Ref="i425"/><Origin z:Ref="i419"/></URDFElement><URDFElement z:Id="i431" i:type="Collision"><Attributes/><ChildElements><URDFElement z:Id="i432" i:type="Origin"><Attributes><Attribute z:Id="i433"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i434"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i434"/><XYZAttribute z:Ref="i433"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i435" i:type="Geometry"><Attributes/><ChildElements><URDFElement z:Id="i436" i:type="Mesh"><Attributes><Attribute z:Id="i437"><AttributeType>filename</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mesh</ElementName><required>false</required><FilenameAttribute z:Ref="i437"/></URDFElement></ChildElements><ElementName>geometry</ElementName><required>true</required><Mesh z:Ref="i436"/></URDFElement></ChildElements><ElementName>collision</ElementName><required>false</required><Geometry z:Ref="i435"/><Origin z:Ref="i432"/></URDFElement><URDFElement z:Id="i438" i:type="Joint"><Attributes><Attribute z:Id="i439"><AttributeType>name</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">wristRoll_link</Value></Attribute><Attribute z:Id="i440"><AttributeType>type</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema">Automatically Detect</Value></Attribute></Attributes><ChildElements><URDFElement z:Id="i441" i:type="Origin"><Attributes><Attribute z:Id="i442"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute><Attribute z:Id="i443"><AttributeType>rpy</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>origin</ElementName><required>false</required><RPYAttribute z:Ref="i443"/><XYZAttribute z:Ref="i442"/><isCustomized>false</isCustomized></URDFElement><URDFElement z:Id="i444" i:type="ParentLink"><Attributes><Attribute z:Id="i445"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>parent</ElementName><required>true</required><NameAttribute z:Ref="i445"/></URDFElement><URDFElement z:Id="i446" i:type="ChildLink"><Attributes><Attribute z:Id="i447"><AttributeType>link</AttributeType><IsRequired>true</IsRequired><Value i:type="a:string" xmlns:a="http://www.w3.org/2001/XMLSchema"/></Attribute></Attributes><ChildElements/><ElementName>child</ElementName><required>true</required><NameAttribute z:Ref="i447"/></URDFElement><URDFElement z:Id="i448" i:type="Axis"><Attributes><Attribute z:Id="i449"><AttributeType>xyz</AttributeType><IsRequired>true</IsRequired><Value i:type="a:ArrayOfdouble" xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:double>0</a:double><a:double>0</a:double><a:double>0</a:double></Value></Attribute></Attributes><ChildElements/><ElementName>axis</ElementName><required>false</required><XYZAttribute z:Ref="i449"/></URDFElement><URDFElement z:Id="i450" i:type="Limit"><Attributes><Attribute z:Id="i451"><AttributeType>lower</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i452"><AttributeType>upper</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i453"><AttributeType>effort</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i454"><AttributeType>velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>limit</ElementName><required>false</required><EffortAttribute z:Ref="i453"/><LowerAttribute z:Ref="i451"/><UpperAttribute z:Ref="i452"/><VelocityAttribute z:Ref="i454"/></URDFElement><URDFElement z:Id="i455" i:type="Calibration"><Attributes><Attribute z:Id="i456"><AttributeType>rising</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i457"><AttributeType>falling</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>calibration</ElementName><required>false</required><FallingAttribute z:Ref="i457"/><RisingAttribute z:Ref="i456"/></URDFElement><URDFElement z:Id="i458" i:type="Dynamics"><Attributes><Attribute z:Id="i459"><AttributeType>damping</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i460"><AttributeType>friction</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>dynamics</ElementName><required>false</required><DampingAttribute z:Ref="i459"/><FrictionAttribute z:Ref="i460"/></URDFElement><URDFElement z:Id="i461" i:type="SafetyController"><Attributes><Attribute z:Id="i462"><AttributeType>soft_upper_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i463"><AttributeType>soft_lower_limit</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i464"><AttributeType>k_position</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i465"><AttributeType>k_velocity</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>safety_controller</ElementName><required>false</required><KPositionAttribute z:Ref="i464"/><KVelocityAttribute z:Ref="i465"/><SoftLowerAttribute z:Ref="i463"/><SoftUpperAttribute z:Ref="i462"/></URDFElement><URDFElement z:Id="i466" i:type="Mimic"><Attributes><Attribute z:Id="i467"><AttributeType>joint</AttributeType><IsRequired>true</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i468"><AttributeType>multiplier</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute><Attribute z:Id="i469"><AttributeType>offset</AttributeType><IsRequired>false</IsRequired><Value i:nil="true"/></Attribute></Attributes><ChildElements/><ElementName>mimic</ElementName><required>false</required><JointNameAttribute z:Ref="i467"/><MultiplierAttribute z:Ref="i468"/><OffsetAttribute z:Ref="i469"/></URDFElement></ChildElements><ElementName>joint</ElementName><required>false</required><Axis z:Ref="i448"/><AxisName>Axis6</AxisName><Calibration z:Ref="i455"/><Child z:Ref="i446"/><CoordinateSystemName>Automatically Generate</CoordinateSystemName><Dynamics z:Ref="i458"/><Limit z:Ref="i450"/><Mimic z:Ref="i466"/><NameAttribute z:Ref="i439"/><Origin z:Ref="i441"/><Parent z:Ref="i444"/><Safety z:Ref="i461"/><TypeAttribute z:Ref="i440"/></URDFElement></ChildElements><ElementName>link</ElementName><required>true</required><Children/><Collision z:Ref="i431"/><Inertial z:Ref="i405"/><Joint z:Ref="i438"/><NameAttribute z:Ref="i404"/><Parent z:Ref="i336"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8ZTQBhAG4AaQBwAHUAbABhAHQAbwByAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAJAAAAA==</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i418"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link></Children><Collision z:Ref="i364"/><Inertial z:Ref="i338"/><Joint z:Ref="i371"/><NameAttribute z:Ref="i337"/><Parent z:Ref="i269"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8TVwByAGkAcwB0AC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAJwAAAA==</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i351"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link></Children><Collision z:Ref="i297"/><Inertial z:Ref="i271"/><Joint z:Ref="i304"/><NameAttribute z:Ref="i270"/><Parent z:Ref="i202"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8cVwByAGkAcwB0AFMAdAByAHUAYwB0AHUAcgBlAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAKAAAAA==</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i284"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link></Children><Collision z:Ref="i230"/><Inertial z:Ref="i204"/><Joint z:Ref="i237"/><NameAttribute z:Ref="i203"/><Parent z:Ref="i135"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8URgBvAHIAYQByAG0ALQAxAEAAQQByAG0AQQBzAHMAZQBtAGIAbAB5AAQAAAAQAAAAAQAAAAEAAAAjAAAA</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i217"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link></Children><Collision z:Ref="i163"/><Inertial z:Ref="i137"/><Joint z:Ref="i170"/><NameAttribute z:Ref="i136"/><Parent z:Ref="i68"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8WVQBwAHAAZQByAEEAcgBtAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAGgAAAA==</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i150"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link></Children><Collision z:Ref="i96"/><Inertial z:Ref="i70"/><Joint z:Ref="i103"/><NameAttribute z:Ref="i69"/><Parent z:Ref="i1"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8WUwBoAG8AdQBsAGQAZQByAC0AMQBAAEEAcgBtAEEAcwBzAGUAbQBiAGwAeQAEAAAAEAAAAAEAAAABAAAAGQAAAA==</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i83"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link></Children><Collision z:Ref="i29"/><Inertial z:Ref="i3"/><Joint z:Ref="i36"/><NameAttribute z:Ref="i2"/><Parent i:nil="true"/><STLQualityFine>false</STLQualityFine><SWComponentPIDs xmlns:a="http://schemas.microsoft.com/2003/10/Serialization/Arrays"><a:base64Binary>sDYAAAUAAAD//v8SYgBhAHMAZQAtADEAQABBAHIAbQBBAHMAcwBlAG0AYgBsAHkABAAAABAAAAABAAAAAQAAABgAAAA=</a:base64Binary></SWComponentPIDs><SWMainComponentPID i:nil="true"/><Visual z:Ref="i16"/><isFixedFrame>false</isFixedFrame><isIncomplete>false</isIncomplete></Link>
+2022-04-26 22:17:53,105 INFO  AssemblyExportForm.cs: 309 - Saving URDF package to E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM
+2022-04-26 22:17:53,107 INFO  ExportHelper.cs: 147 - Beginning the export process
+2022-04-26 22:17:53,108 INFO  ExportHelper.cs: 153 - Creating package directories with name ArmAssembly.SLDASM and save path E:\Robotics\WR\URDF\URDF
+2022-04-26 22:17:55,069 INFO  ExportHelper.cs: 162 - Creating CMakeLists.txt at E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\CMakeLists.txt
+2022-04-26 22:17:55,070 INFO  ExportHelper.cs: 166 - Creating joint names config at E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\config\joint_names_ArmAssembly.SLDASM.yaml
+2022-04-26 22:17:55,071 INFO  ExportHelper.cs: 170 - Creating package.xml at E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\package.xml
+2022-04-26 22:17:55,071 INFO  PackageXMLWriter.cs: 21 - Creating package.xml at E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\package.xml
+2022-04-26 22:17:55,076 INFO  ExportHelper.cs: 177 - Creating RVIZ launch file in E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\launch\
+2022-04-26 22:17:55,078 INFO  ExportHelper.cs: 182 - Creating Gazebo launch file in E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\launch\
+2022-04-26 22:17:55,079 INFO  ExportHelper.cs: 187 - Saving existing STL preferences
+2022-04-26 22:17:55,080 INFO  ExportHelper.cs: 450 - Saving users preferences
+2022-04-26 22:17:55,080 INFO  ExportHelper.cs: 190 - Modifying STL preferences
+2022-04-26 22:17:55,081 INFO  ExportHelper.cs: 464 - Setting STL preferences
+2022-04-26 22:17:55,085 INFO  ExportHelper.cs: 196 - Found 0 hidden components 
+2022-04-26 22:17:55,085 INFO  ExportHelper.cs: 197 - Hiding all components
+2022-04-26 22:17:55,336 INFO  ExportHelper.cs: 204 - Beginning individual files export
+2022-04-26 22:17:55,339 INFO  ExportHelper.cs: 270 - Exporting link: base_link
+2022-04-26 22:17:55,339 INFO  ExportHelper.cs: 272 - Link base_link has 1 children
+2022-04-26 22:17:55,340 INFO  ExportHelper.cs: 270 - Exporting link: turntable_link
+2022-04-26 22:17:55,341 INFO  ExportHelper.cs: 272 - Link turntable_link has 1 children
+2022-04-26 22:17:55,342 INFO  ExportHelper.cs: 270 - Exporting link: upperArm_link
+2022-04-26 22:17:55,342 INFO  ExportHelper.cs: 272 - Link upperArm_link has 1 children
+2022-04-26 22:17:55,343 INFO  ExportHelper.cs: 270 - Exporting link: forearm_link
+2022-04-26 22:17:55,343 INFO  ExportHelper.cs: 272 - Link forearm_link has 1 children
+2022-04-26 22:17:55,344 INFO  ExportHelper.cs: 270 - Exporting link: wrist_link
+2022-04-26 22:17:55,344 INFO  ExportHelper.cs: 272 - Link wrist_link has 1 children
+2022-04-26 22:17:55,344 INFO  ExportHelper.cs: 270 - Exporting link: carpus_link
+2022-04-26 22:17:55,345 INFO  ExportHelper.cs: 272 - Link carpus_link has 1 children
+2022-04-26 22:17:55,345 INFO  ExportHelper.cs: 270 - Exporting link: endEffector_link
+2022-04-26 22:17:55,346 INFO  ExportHelper.cs: 272 - Link endEffector_link has 0 children
+2022-04-26 22:17:55,346 INFO  ExportHelper.cs: 317 - endEffector_link: Exporting STL with coordinate frame Origin_wristRoll_link
+2022-04-26 22:17:55,347 INFO  ExportHelper.cs: 322 - endEffector_link: Reference geometry name 
+2022-04-26 22:17:55,506 INFO  ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\meshes\endEffector_link.STL
+2022-04-26 22:17:55,923 INFO  ExportHelper.cs: 405 - Removing SW header in STL file
+2022-04-26 22:17:55,923 INFO  ExportHelper.cs: 317 - carpus_link: Exporting STL with coordinate frame Origin_wristPitch_joint
+2022-04-26 22:17:55,924 INFO  ExportHelper.cs: 322 - carpus_link: Reference geometry name 
+2022-04-26 22:17:55,969 INFO  ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\meshes\carpus_link.STL
+2022-04-26 22:17:56,017 INFO  ExportHelper.cs: 405 - Removing SW header in STL file
+2022-04-26 22:17:56,018 INFO  ExportHelper.cs: 317 - wrist_link: Exporting STL with coordinate frame Origin_elbowRoll_joint
+2022-04-26 22:17:56,018 INFO  ExportHelper.cs: 322 - wrist_link: Reference geometry name 
+2022-04-26 22:17:56,031 INFO  ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\meshes\wrist_link.STL
+2022-04-26 22:17:56,058 INFO  ExportHelper.cs: 405 - Removing SW header in STL file
+2022-04-26 22:17:56,059 INFO  ExportHelper.cs: 317 - forearm_link: Exporting STL with coordinate frame Origin_elbowPitch_joint
+2022-04-26 22:17:56,059 INFO  ExportHelper.cs: 322 - forearm_link: Reference geometry name 
+2022-04-26 22:17:56,112 INFO  ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\meshes\forearm_link.STL
+2022-04-26 22:17:56,168 INFO  ExportHelper.cs: 405 - Removing SW header in STL file
+2022-04-26 22:17:56,169 INFO  ExportHelper.cs: 317 - upperArm_link: Exporting STL with coordinate frame Origin_shoulder_joint
+2022-04-26 22:17:56,169 INFO  ExportHelper.cs: 322 - upperArm_link: Reference geometry name 
+2022-04-26 22:17:56,180 INFO  ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\meshes\upperArm_link.STL
+2022-04-26 22:17:56,216 INFO  ExportHelper.cs: 405 - Removing SW header in STL file
+2022-04-26 22:17:56,217 INFO  ExportHelper.cs: 317 - turntable_link: Exporting STL with coordinate frame Origin_turntable_joint
+2022-04-26 22:17:56,217 INFO  ExportHelper.cs: 322 - turntable_link: Reference geometry name 
+2022-04-26 22:17:56,290 INFO  ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\meshes\turntable_link.STL
+2022-04-26 22:17:56,359 INFO  ExportHelper.cs: 405 - Removing SW header in STL file
+2022-04-26 22:17:56,360 INFO  ExportHelper.cs: 317 - base_link: Exporting STL with coordinate frame Origin_global
+2022-04-26 22:17:56,360 INFO  ExportHelper.cs: 322 - base_link: Reference geometry name 
+2022-04-26 22:17:56,369 INFO  ExportHelper.cs: 330 - Saving STL to E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\meshes\base_link.STL
+2022-04-26 22:17:56,393 INFO  ExportHelper.cs: 405 - Removing SW header in STL file
+2022-04-26 22:17:56,394 INFO  ExportHelper.cs: 145 - Showing all components except previously hidden components
+2022-04-26 22:17:56,648 INFO  ExportHelper.cs: 145 - Resetting STL preferences
+2022-04-26 22:17:56,649 INFO  ExportHelper.cs: 478 - Returning STL preferences to user preferences
+2022-04-26 22:17:56,649 INFO  ExportHelper.cs: 228 - Writing URDF file to E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\urdf\ArmAssembly.SLDASM.urdf
+2022-04-26 22:17:56,675 INFO  CSVImportExport.cs: 32 - Writing CSV file E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\urdf\ArmAssembly.SLDASM.csv
+2022-04-26 22:17:56,718 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link,
+2022-04-26 22:17:56,718 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link,
+2022-04-26 22:17:56,719 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link,
+2022-04-26 22:17:56,719 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link,
+2022-04-26 22:17:56,720 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link,
+2022-04-26 22:17:56,720 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link,
+2022-04-26 22:17:56,726 ERROR CSVImportExport.cs: 130 - The following columns were not written to the CSV: Link.Joint.ChildLink.link,
+2022-04-26 22:17:56,727 INFO  ExportHelper.cs: 234 - Copying log file
+2022-04-26 22:17:56,730 INFO  ExportHelper.cs: 439 - Copying C:\Users\Payton Jackson\sw2urdf_logs\sw2urdf.log to E:\Robotics\WR\URDF\URDF\ArmAssembly.SLDASM\export.log
diff --git a/src/wroboarm_23/launch/chomp_planning_pipeline.launch.xml b/src/wroboarm_23/launch/chomp_planning_pipeline.launch.xml
new file mode 100644
index 00000000..34271389
--- /dev/null
+++ b/src/wroboarm_23/launch/chomp_planning_pipeline.launch.xml
@@ -0,0 +1,24 @@
+<launch>
+  <arg name="start_state_max_bounds_error" default="0.1" />
+  <arg name="jiggle_fraction" default="0.05" />
+  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
+  <arg name="planning_adapters"
+       default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
+                default_planner_request_adapters/AddTimeParameterization
+                default_planner_request_adapters/ResolveConstraintFrames
+                default_planner_request_adapters/FixWorkspaceBounds
+                default_planner_request_adapters/FixStartStateBounds
+                default_planner_request_adapters/FixStartStateCollision
+                default_planner_request_adapters/FixStartStatePathConstraints"
+                />
+
+  <param name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
+
+  <!-- Add MoveGroup capabilities specific to this pipeline -->
+  <!-- <param name="capabilities" value="" /> -->
+
+  <rosparam command="load" file="$(find wroboarm_23)/config/chomp_planning.yaml" />
+</launch>
diff --git a/src/wroboarm_23/launch/default_warehouse_db.launch b/src/wroboarm_23/launch/default_warehouse_db.launch
new file mode 100644
index 00000000..2543e8f8
--- /dev/null
+++ b/src/wroboarm_23/launch/default_warehouse_db.launch
@@ -0,0 +1,15 @@
+<launch>
+
+  <arg name="reset" default="false"/>
+  <!-- If not specified, we'll use a default database location -->
+  <arg name="moveit_warehouse_database_path" default="$(find wroboarm_23)/default_warehouse_mongo_db" />
+
+  <!-- Launch the warehouse with the configured database location -->
+  <include file="$(dirname)/warehouse.launch">
+    <arg name="moveit_warehouse_database_path" value="$(arg moveit_warehouse_database_path)" />
+  </include>
+
+  <!-- If we want to reset the database, run this node -->
+  <node if="$(arg reset)" name="$(anon moveit_default_db_reset)" type="moveit_init_demo_warehouse" pkg="moveit_ros_warehouse" respawn="false" output="screen" />
+
+</launch>
diff --git a/src/wroboarm_23/launch/demo.launch b/src/wroboarm_23/launch/demo.launch
new file mode 100644
index 00000000..9abb2e63
--- /dev/null
+++ b/src/wroboarm_23/launch/demo.launch
@@ -0,0 +1,69 @@
+<launch>
+
+  <!-- specify the planning pipeline -->
+  <arg name="pipeline" default="ompl" />
+
+  <!-- By default, we do not start a database (it can be large) -->
+  <arg name="db" default="false" />
+  <!-- Allow user to specify database location -->
+  <arg name="db_path" default="$(find wroboarm_23)/default_warehouse_mongo_db" />
+
+  <!-- By default, we are not in debug mode -->
+  <arg name="debug" default="false" />
+
+  <!-- By default, we will load or override the robot_description -->
+  <arg name="load_robot_description" default="true"/>
+
+  <!-- Choose controller manager: fake, simple, or ros_control -->
+  <arg name="moveit_controller_manager" default="fake" />
+  <!-- Set execution mode for fake execution controllers -->
+  <arg name="fake_execution_type" default="interpolate" />
+
+  <!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode -->
+  <arg name="use_gui" default="false" />
+  <arg name="use_rviz" default="true" />
+
+  <!-- If needed, broadcast static tf for robot root -->
+  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world base_link" />
+
+
+
+  <group if="$(eval arg('moveit_controller_manager') == 'fake')">
+    <!-- We do not have a real robot connected, so publish fake joint states via a joint_state_publisher
+         MoveIt's fake controller's joint states are considered via the 'source_list' parameter -->
+    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
+      <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
+    </node>
+    <!-- If desired, a GUI version is available allowing to move the simulated robot around manually
+         This corresponds to moving around the real robot without the use of MoveIt. -->
+    <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
+      <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
+    </node>
+
+    <!-- Given the published joint states, publish tf for the robot links -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+  </group>
+
+  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+  <include file="$(dirname)/move_group.launch">
+    <arg name="allow_trajectory_execution" value="true"/>
+    <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
+    <arg name="fake_execution_type" value="$(arg fake_execution_type)"/>
+    <arg name="info" value="true"/>
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="pipeline" value="$(arg pipeline)"/>
+    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
+  </include>
+
+  <!-- Run Rviz and load the default config to see the state of the move_group node -->
+  <include file="$(dirname)/moveit_rviz.launch" if="$(arg use_rviz)">
+    <arg name="rviz_config" value="$(dirname)/moveit.rviz"/>
+    <arg name="debug" value="$(arg debug)"/>
+  </include>
+
+  <!-- If database loading was enabled, start mongodb as well -->
+  <include file="$(dirname)/default_warehouse_db.launch" if="$(arg db)">
+    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
+  </include>
+
+</launch>
diff --git a/src/wroboarm_23/launch/demo_gazebo.launch b/src/wroboarm_23/launch/demo_gazebo.launch
new file mode 100644
index 00000000..a9f320c5
--- /dev/null
+++ b/src/wroboarm_23/launch/demo_gazebo.launch
@@ -0,0 +1,21 @@
+<launch>
+
+  <!-- specify the planning pipeline -->
+  <arg name="pipeline" default="ompl" />
+
+  <!-- Gazebo specific options -->
+  <arg name="gazebo_gui" default="true"/>
+  <arg name="paused" default="false"/>
+
+  <!-- launch the gazebo simulator and spawn the robot -->
+  <include file="$(dirname)/gazebo.launch" >
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="gazebo_gui" value="$(arg gazebo_gui)"/>
+  </include>
+
+  <include file="$(dirname)/demo.launch" pass_all_args="true">
+    <!-- robot description is loaded by gazebo.launch, to enable Gazebo features -->
+    <arg name="load_robot_description" value="false" />
+    <arg name="moveit_controller_manager" value="ros_control" />
+  </include>
+</launch>
diff --git a/src/wroboarm_23/launch/demo_test.launch b/src/wroboarm_23/launch/demo_test.launch
new file mode 100644
index 00000000..8c258868
--- /dev/null
+++ b/src/wroboarm_23/launch/demo_test.launch
@@ -0,0 +1,69 @@
+<launch>
+
+  <!-- specify the planning pipeline -->
+  <arg name="pipeline" default="ompl" />
+
+  <!-- By default, we do not start a database (it can be large) -->
+  <arg name="db" default="false" />
+  <!-- Allow user to specify database location -->
+  <arg name="db_path" default="$(find wroboarm_23)/default_warehouse_mongo_db" />
+
+  <!-- By default, we are not in debug mode -->
+  <arg name="debug" default="false" />
+
+  <!-- By default, we will load or override the robot_description -->
+  <arg name="load_robot_description" default="true"/>
+
+  <!-- Choose controller manager: fake, simple, or ros_control -->
+  <arg name="moveit_controller_manager" default="simple" />
+  <!-- Set execution mode for fake execution controllers -->
+  <arg name="fake_execution_type" default="false" />
+
+  <!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode -->
+  <arg name="use_gui" default="false" />
+  <arg name="use_rviz" default="true" />
+
+  <!-- If needed, broadcast static tf for robot root -->
+  <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world base_link" />
+
+
+
+  <group if="$(eval arg('moveit_controller_manager') == 'fake')">
+    <!-- We do not have a real robot connected, so publish fake joint states via a joint_state_publisher
+         MoveIt's fake controller's joint states are considered via the 'source_list' parameter -->
+    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
+      <rosparam param="source_list">[control/arm_joint_states]</rosparam>
+    </node>
+    <!-- If desired, a GUI version is available allowing to move the simulated robot around manually
+         This corresponds to moving around the real robot without the use of MoveIt. -->
+    <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
+      <rosparam param="source_list">[control/arm_joint_states]</rosparam>
+    </node>
+
+    <!-- Given the published joint states, publish tf for the robot links -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+  </group>
+
+  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
+  <include file="$(dirname)/move_group.launch">
+    <arg name="allow_trajectory_execution" value="true"/>
+    <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
+    <arg name="fake_execution_type" value="$(arg fake_execution_type)"/>
+    <arg name="info" value="true"/>
+    <arg name="debug" value="$(arg debug)"/>
+    <arg name="pipeline" value="$(arg pipeline)"/>
+    <arg name="load_robot_description" value="$(arg load_robot_description)"/>
+  </include>
+
+  <!-- Run Rviz and load the default config to see the state of the move_group node -->
+  <include file="$(dirname)/moveit_rviz.launch" if="$(arg use_rviz)">
+    <arg name="rviz_config" value="$(dirname)/moveit.rviz"/>
+    <arg name="debug" value="$(arg debug)"/>
+  </include>
+
+  <!-- If database loading was enabled, start mongodb as well -->
+  <include file="$(dirname)/default_warehouse_db.launch" if="$(arg db)">
+    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
+  </include>
+
+</launch>
diff --git a/src/wroboarm_23/launch/display.launch b/src/wroboarm_23/launch/display.launch
new file mode 100644
index 00000000..ce58753b
--- /dev/null
+++ b/src/wroboarm_23/launch/display.launch
@@ -0,0 +1,20 @@
+<launch>
+  <arg
+    name="model" />
+  <param
+    name="robot_description"
+    textfile="$(find wroboarm_23)/urdf/ArmAssembly.SLDASM.urdf" />
+  <node
+    name="joint_state_publisher_gui"
+    pkg="joint_state_publisher_gui"
+    type="joint_state_publisher_gui" />
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="robot_state_publisher" />
+  <node
+    name="rviz"
+    pkg="rviz"
+    type="rviz"
+    args="-d $(find wroboarm_23)/urdf.rviz" />
+</launch>
\ No newline at end of file
diff --git a/src/wroboarm_23/launch/fake_moveit_controller_manager.launch.xml b/src/wroboarm_23/launch/fake_moveit_controller_manager.launch.xml
new file mode 100644
index 00000000..46689c86
--- /dev/null
+++ b/src/wroboarm_23/launch/fake_moveit_controller_manager.launch.xml
@@ -0,0 +1,12 @@
+<launch>
+
+  <!-- execute the trajectory in 'interpolate' mode or jump to goal position in 'last point' mode -->
+  <arg name="fake_execution_type" default="interpolate" />
+
+  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
+  <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
+
+  <!-- The rest of the params are specific to this plugin -->
+  <rosparam subst_value="true" file="$(find wroboarm_23)/config/fake_controllers.yaml"/>
+
+</launch>
diff --git a/src/wroboarm_23/launch/gazebo.launch b/src/wroboarm_23/launch/gazebo.launch
new file mode 100644
index 00000000..cb085009
--- /dev/null
+++ b/src/wroboarm_23/launch/gazebo.launch
@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="paused" default="false"/>
+  <arg name="gazebo_gui" default="true"/>
+  <arg name="initial_joint_positions" doc="Initial joint configuration of the robot"
+       default=""/>
+
+  <!-- startup simulated world -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" default="worlds/empty.world"/>
+    <arg name="paused" value="true"/>
+    <arg name="gui" value="$(arg gazebo_gui)"/>
+  </include>
+
+  <!-- send robot urdf to param server -->
+  <param name="robot_description" textfile="$(find wroboarm_23)/config/gazebo_wroboarm_23.urdf" />
+
+  <!-- unpause only after loading robot model -->
+  <arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
+  <!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
+  <arg name="world_pose" value="-x 0 -y 0 -z 0" />
+  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg unpause) $(arg world_pose) $(arg initial_joint_positions)"
+    respawn="false" output="screen" />
+
+  <!-- Load joint controller parameters for Gazebo -->
+  <rosparam file="$(find wroboarm_23)/config/gazebo_controllers.yaml" />
+  <!-- Spawn Gazebo ROS controllers -->
+  <node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />
+  <!-- Load ROS controllers -->
+  <include file="$(dirname)/ros_controllers.launch"/>
+
+</launch>
diff --git a/src/wroboarm_23/launch/joystick_control.launch b/src/wroboarm_23/launch/joystick_control.launch
new file mode 100644
index 00000000..9411f6e6
--- /dev/null
+++ b/src/wroboarm_23/launch/joystick_control.launch
@@ -0,0 +1,17 @@
+<launch>
+  <!-- See moveit_ros/visualization/doc/joystick.rst for documentation -->
+
+  <arg name="dev" default="/dev/input/js0" />
+
+  <!-- Launch joy node -->
+  <node pkg="joy" type="joy_node" name="joy">
+    <param name="dev" value="$(arg dev)" /> <!-- Customize this to match the location your joystick is plugged in on-->
+    <param name="deadzone" value="0.2" />
+    <param name="autorepeat_rate" value="40" />
+    <param name="coalesce_interval" value="0.025" />
+  </node>
+
+  <!-- Launch python interface -->
+  <node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>
+
+</launch>
diff --git a/src/wroboarm_23/launch/move_group.launch b/src/wroboarm_23/launch/move_group.launch
new file mode 100644
index 00000000..bbaa5eb5
--- /dev/null
+++ b/src/wroboarm_23/launch/move_group.launch
@@ -0,0 +1,105 @@
+<launch>
+
+  <!-- GDB Debug Option -->
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix"
+           value="gdb -x $(dirname)/gdb_settings.gdb --ex run --args" />
+
+  <!-- Verbose Mode Option -->
+  <arg name="info" default="$(arg debug)" />
+  <arg unless="$(arg info)" name="command_args" value="" />
+  <arg     if="$(arg info)" name="command_args" value="--debug" />
+
+  <!-- move_group settings -->
+  <arg name="pipeline" default="ompl" />
+  <arg name="allow_trajectory_execution" default="true"/>
+  <arg name="moveit_controller_manager" default="simple" />
+  <arg name="fake_execution_type" default="interpolate"/>
+  <arg name="max_safe_path_cost" default="1"/>
+  <arg name="publish_monitored_planning_scene" default="true"/>
+
+  <arg name="capabilities" default=""/>
+  <arg name="disable_capabilities" default=""/>
+  <!-- load these non-default MoveGroup capabilities (space seperated) -->
+  <!--
+  <arg name="capabilities" value="
+                a_package/AwsomeMotionPlanningCapability
+                another_package/GraspPlanningPipeline
+                " />
+  -->
+
+  <!-- inhibit these default MoveGroup capabilities (space seperated) -->
+  <!--
+  <arg name="disable_capabilities" value="
+                move_group/MoveGroupKinematicsService
+                move_group/ClearOctomapService
+                " />
+  -->
+
+  <arg name="load_robot_description" default="false" />
+  <!-- load URDF, SRDF and joint_limits configuration -->
+  <include file="$(dirname)/planning_context.launch">
+    <arg name="load_robot_description" value="$(arg load_robot_description)" />
+  </include>
+
+  <!-- Planning Pipelines -->
+  <group ns="move_group/planning_pipelines">
+
+    <!-- OMPL -->
+    <include file="$(dirname)/planning_pipeline.launch.xml">
+      <arg name="pipeline" value="ompl" />
+    </include>
+
+    <!-- CHOMP -->
+    <include file="$(dirname)/planning_pipeline.launch.xml">
+      <arg name="pipeline" value="chomp" />
+    </include>
+
+    <!-- Pilz Industrial Motion -->
+    <include file="$(dirname)/planning_pipeline.launch.xml">
+      <arg name="pipeline" value="pilz_industrial_motion_planner" />
+    </include>
+
+    <!-- Support custom planning pipeline -->
+    <include if="$(eval arg('pipeline') not in ['ompl', 'chomp', 'pilz_industrial_motion_planner'])"
+             file="$(dirname)/planning_pipeline.launch.xml">
+      <arg name="pipeline" value="$(arg pipeline)" />
+    </include>
+  </group>
+
+  <!-- Trajectory Execution Functionality -->
+  <include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
+    <arg name="moveit_manage_controllers" value="true" />
+    <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
+    <arg name="fake_execution_type" value="$(arg fake_execution_type)" />
+  </include>
+
+  <!-- Sensors Functionality -->
+  <include ns="move_group" file="$(dirname)/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
+    <arg name="moveit_sensor_manager" value="wroboarm_23" />
+  </include>
+
+  <!-- Start the actual move_group node/action server -->
+  <node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
+    <!-- Set the display variable, in case OpenGL code is used internally -->
+    <env name="DISPLAY" value="$(optenv DISPLAY :0)" />
+
+    <param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
+    <param name="sense_for_plan/max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
+    <param name="default_planning_pipeline" value="$(arg pipeline)" />
+    <param name="capabilities" value="$(arg capabilities)" />
+    <param name="disable_capabilities" value="$(arg disable_capabilities)" />
+
+    <!-- do not copy dynamics information from /joint_states to internal robot monitoring
+         default to false, because almost nothing in move_group relies on this information -->
+    <param name="monitor_dynamics" value="false" />
+
+    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
+    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
+    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
+    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
+    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
+  </node>
+
+</launch>
diff --git a/src/wroboarm_23/launch/moveit.rviz b/src/wroboarm_23/launch/moveit.rviz
new file mode 100644
index 00000000..97d51153
--- /dev/null
+++ b/src/wroboarm_23/launch/moveit.rviz
@@ -0,0 +1,265 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 84
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /MotionPlanning1
+        - /MotionPlanning1/Planning Request1
+        - /Pose1
+      Splitter Ratio: 0.5
+    Tree Height: 295
+  - Class: rviz/Help
+    Name: Help
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+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
+    - Acceleration_Scaling_Factor: 0.1
+      Class: moveit_rviz_plugin/MotionPlanning
+      Enabled: true
+      Move Group Namespace: ""
+      MoveIt_Allow_Approximate_IK: false
+      MoveIt_Allow_External_Program: false
+      MoveIt_Allow_Replanning: false
+      MoveIt_Allow_Sensor_Positioning: false
+      MoveIt_Planning_Attempts: 10
+      MoveIt_Planning_Time: 5
+      MoveIt_Use_Cartesian_Path: false
+      MoveIt_Use_Constraint_Aware_IK: false
+      MoveIt_Workspace:
+        Center:
+          X: 0
+          Y: 0
+          Z: 0
+        Size:
+          X: 2
+          Y: 2
+          Z: 2
+      Name: MotionPlanning
+      Planned Path:
+        Color Enabled: false
+        Interrupt Display: false
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          carpus_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          endEffector_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          forearm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          turntable_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          upperArm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Loop Animation: true
+        Robot Alpha: 0.5
+        Robot Color: 150; 50; 150
+        Show Robot Collision: false
+        Show Robot Visual: false
+        Show Trail: false
+        State Display Time: 0.05 s
+        Trail Step Size: 1
+        Trajectory Topic: move_group/display_planned_path
+        Use Sim Time: false
+      Planning Metrics:
+        Payload: 1
+        Show Joint Torques: false
+        Show Manipulability: false
+        Show Manipulability Index: false
+        Show Weight Limit: false
+        TextHeight: 0.07999999821186066
+      Planning Request:
+        Colliding Link Color: 255; 0; 0
+        Goal State Alpha: 1
+        Goal State Color: 250; 128; 0
+        Interactive Marker Size: 0
+        Joint Violation Color: 255; 0; 255
+        Planning Group: arm
+        Query Goal State: false
+        Query Start State: false
+        Show Workspace: false
+        Start State Alpha: 1
+        Start State Color: 0; 255; 0
+      Planning Scene Topic: move_group/monitored_planning_scene
+      Robot Description: robot_description
+      Scene Geometry:
+        Scene Alpha: 1
+        Scene Color: 50; 230; 50
+        Scene Display Time: 0.009999999776482582
+        Show Scene Geometry: true
+        Voxel Coloring: Z-Axis
+        Voxel Rendering: Occupied Voxels
+      Scene Robot:
+        Attached Body Color: 150; 50; 150
+        Links:
+          All Links Enabled: true
+          Expand Joint Details: false
+          Expand Link Details: false
+          Expand Tree: false
+          Link Tree Style: Links in Alphabetic Order
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          carpus_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          endEffector_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          forearm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          turntable_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          upperArm_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          wrist_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+        Robot Alpha: 0.85
+        Show Robot Collision: false
+        Show Robot Visual: true
+      Value: true
+      Velocity_Scaling_Factor: 0.1
+    - Alpha: 1
+      Axes Length: 0.10000000149011612
+      Axes Radius: 0.02500000037252903
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Queue Size: 10
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Axes
+      Topic: /logic/arm_teleop/next_target
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 2
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.75
+      Focal Point:
+        X: -0.10000000149011612
+        Y: 0.25
+        Z: 0.30000001192092896
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.5
+      Target Frame: base_link
+      Yaw: -0.6232355833053589
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Help:
+    collapsed: false
+  Hide Left Dock: false
+  Hide Right Dock: false
+  MotionPlanning:
+    collapsed: false
+  MotionPlanning - Trajectory Slider:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f30000039efc0200000007fb000000100044006900730070006c006100790073010000003d000001b8000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001fb000001e00000017d00ffffff0000053f0000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27
diff --git a/src/wroboarm_23/launch/moveit_rviz.launch b/src/wroboarm_23/launch/moveit_rviz.launch
new file mode 100644
index 00000000..a4605c03
--- /dev/null
+++ b/src/wroboarm_23/launch/moveit_rviz.launch
@@ -0,0 +1,15 @@
+<launch>
+
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+
+  <arg name="rviz_config" default="" />
+  <arg     if="$(eval rviz_config=='')" name="command_args" value="" />
+  <arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />
+
+  <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
+        args="$(arg command_args)" output="screen">
+  </node>
+
+</launch>
diff --git a/src/wroboarm_23/launch/ompl-chomp_planning_pipeline.launch.xml b/src/wroboarm_23/launch/ompl-chomp_planning_pipeline.launch.xml
new file mode 100644
index 00000000..7318a0b8
--- /dev/null
+++ b/src/wroboarm_23/launch/ompl-chomp_planning_pipeline.launch.xml
@@ -0,0 +1,20 @@
+<launch>
+  <!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->
+  <include file="$(find wroboarm_23)/launch/ompl_planning_pipeline.launch.xml">
+    <arg name="planning_adapters"
+         default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
+                  default_planner_request_adapters/AddTimeParameterization
+                  default_planner_request_adapters/FixWorkspaceBounds
+                  default_planner_request_adapters/FixStartStateBounds
+                  default_planner_request_adapters/FixStartStateCollision
+                  default_planner_request_adapters/FixStartStatePathConstraints
+                  chomp/OptimizerAdapter"
+                  />
+  </include>
+
+  <!-- load chomp config -->
+  <rosparam command="load" file="$(find wroboarm_23)/config/chomp_planning.yaml" />
+
+  <!-- override trajectory_initialization_method: Use OMPL-generated trajectory -->
+  <param name="trajectory_initialization_method" value="fillTrajectory"/>
+</launch>
diff --git a/src/wroboarm_23/launch/ompl_planning_pipeline.launch.xml b/src/wroboarm_23/launch/ompl_planning_pipeline.launch.xml
new file mode 100644
index 00000000..1bdd6cfc
--- /dev/null
+++ b/src/wroboarm_23/launch/ompl_planning_pipeline.launch.xml
@@ -0,0 +1,27 @@
+<launch>
+
+  <!-- The request adapters (plugins) used when planning with OMPL. ORDER MATTERS! -->
+  <arg name="planning_adapters"
+       default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
+                default_planner_request_adapters/AddTimeParameterization
+                default_planner_request_adapters/ResolveConstraintFrames
+                default_planner_request_adapters/FixWorkspaceBounds
+                default_planner_request_adapters/FixStartStateBounds
+                default_planner_request_adapters/FixStartStateCollision
+                default_planner_request_adapters/FixStartStatePathConstraints"
+                />
+
+  <arg name="start_state_max_bounds_error" default="0.1" />
+  <arg name="jiggle_fraction" default="0.05" />
+
+  <param name="planning_plugin" value="ompl_interface/OMPLPlanner" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
+
+  <!-- Add MoveGroup capabilities specific to this pipeline -->
+  <!-- <param name="capabilities" value="" /> -->
+
+  <rosparam command="load" file="$(find wroboarm_23)/config/ompl_planning.yaml"/>
+
+</launch>
diff --git a/src/wroboarm_23/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/src/wroboarm_23/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
new file mode 100644
index 00000000..c7c4cf50
--- /dev/null
+++ b/src/wroboarm_23/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
@@ -0,0 +1,15 @@
+<launch>
+
+  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
+  <arg name="planning_adapters" default="" />
+
+  <param name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+
+  <!-- Define default planner (for all groups) -->
+  <param name="default_planner_config" value="PTP" />
+
+  <!-- MoveGroup capabilities to load for this pipeline, append sequence capability -->
+  <param name="capabilities" value="pilz_industrial_motion_planner/MoveGroupSequenceAction
+                                    pilz_industrial_motion_planner/MoveGroupSequenceService" />
+</launch>
diff --git a/src/wroboarm_23/launch/planning_context.launch b/src/wroboarm_23/launch/planning_context.launch
new file mode 100644
index 00000000..b5f6954c
--- /dev/null
+++ b/src/wroboarm_23/launch/planning_context.launch
@@ -0,0 +1,26 @@
+<launch>
+  <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
+  <arg name="load_robot_description" default="false"/>
+
+  <!-- The name of the parameter under which the URDF is loaded -->
+  <arg name="robot_description" default="robot_description"/>
+
+  <!-- Load universal robot description format (URDF) -->
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find wroboarm_23)/urdf/ArmAssembly.SLDASM.urdf"/>
+
+  <!-- The semantic description that corresponds to the URDF -->
+  <param name="$(arg robot_description)_semantic" textfile="$(find wroboarm_23)/config/wroboarm_23.srdf" />
+
+  <!-- Load updated joint limits (override information from URDF) -->
+  <group ns="$(arg robot_description)_planning">
+    <rosparam command="load" file="$(find wroboarm_23)/config/joint_limits.yaml"/>
+    <rosparam command="load" file="$(find wroboarm_23)/config/cartesian_limits.yaml"/>
+  </group>
+
+  <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
+  <group ns="$(arg robot_description)_kinematics">
+    <rosparam command="load" file="$(find wroboarm_23)/config/kinematics.yaml"/>
+
+  </group>
+
+</launch>
diff --git a/src/wroboarm_23/launch/planning_pipeline.launch.xml b/src/wroboarm_23/launch/planning_pipeline.launch.xml
new file mode 100644
index 00000000..4b4d0d66
--- /dev/null
+++ b/src/wroboarm_23/launch/planning_pipeline.launch.xml
@@ -0,0 +1,10 @@
+<launch>
+
+  <!-- This file makes it easy to include different planning pipelines;
+       It is assumed that all planning pipelines are named XXX_planning_pipeline.launch  -->
+
+  <arg name="pipeline" default="ompl" />
+
+  <include ns="$(arg pipeline)" file="$(dirname)/$(arg pipeline)_planning_pipeline.launch.xml" />
+
+</launch>
diff --git a/src/wroboarm_23/launch/ros_control_moveit_controller_manager.launch.xml b/src/wroboarm_23/launch/ros_control_moveit_controller_manager.launch.xml
new file mode 100644
index 00000000..9ebc91c1
--- /dev/null
+++ b/src/wroboarm_23/launch/ros_control_moveit_controller_manager.launch.xml
@@ -0,0 +1,4 @@
+<launch>
+	<!-- Define MoveIt controller manager plugin -->
+	<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItControllerManager" />
+</launch>
diff --git a/src/wroboarm_23/launch/ros_controllers.launch b/src/wroboarm_23/launch/ros_controllers.launch
new file mode 100644
index 00000000..7c953597
--- /dev/null
+++ b/src/wroboarm_23/launch/ros_controllers.launch
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- Load joint controller configurations from YAML file to parameter server -->
+  <rosparam file="$(find wroboarm_23)/config/ros_controllers.yaml" command="load"/>
+
+  <!-- Load the controllers -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen" args="arm_controller "/>
+
+</launch>
diff --git a/src/wroboarm_23/launch/run_benchmark_ompl.launch b/src/wroboarm_23/launch/run_benchmark_ompl.launch
new file mode 100644
index 00000000..abea39b7
--- /dev/null
+++ b/src/wroboarm_23/launch/run_benchmark_ompl.launch
@@ -0,0 +1,21 @@
+<launch>
+
+  <!-- This argument must specify the list of .cfg files to process for benchmarking -->
+  <arg name="cfg" />
+
+  <!-- Load URDF -->
+  <include file="$(dirname)/planning_context.launch">
+    <arg name="load_robot_description" value="true"/>
+  </include>
+
+  <!-- Start the database -->
+  <include file="$(dirname)/warehouse.launch">
+    <arg name="moveit_warehouse_database_path" value="moveit_ompl_benchmark_warehouse"/>
+  </include>
+
+  <!-- Start Benchmark Executable -->
+  <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
+    <rosparam command="load" file="$(find wroboarm_23)/config/ompl_planning.yaml"/>
+  </node>
+
+</launch>
diff --git a/src/wroboarm_23/launch/sensor_manager.launch.xml b/src/wroboarm_23/launch/sensor_manager.launch.xml
new file mode 100644
index 00000000..c12a65c3
--- /dev/null
+++ b/src/wroboarm_23/launch/sensor_manager.launch.xml
@@ -0,0 +1,17 @@
+<launch>
+
+  <!-- This file makes it easy to include the settings for sensor managers -->
+
+  <!-- Params for 3D sensors config -->
+  <rosparam command="load" file="$(find wroboarm_23)/config/sensors_3d.yaml" />
+
+  <!-- Params for the octomap monitor -->
+  <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
+  <param name="octomap_resolution" type="double" value="0.025" />
+  <param name="max_range" type="double" value="5.0" />
+
+  <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
+  <arg name="moveit_sensor_manager" default="wroboarm_23" />
+  <include file="$(dirname)/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
+
+</launch>
diff --git a/src/wroboarm_23/launch/setup_assistant.launch b/src/wroboarm_23/launch/setup_assistant.launch
new file mode 100644
index 00000000..da7b439c
--- /dev/null
+++ b/src/wroboarm_23/launch/setup_assistant.launch
@@ -0,0 +1,16 @@
+<!-- Re-launch the MoveIt Setup Assistant with this configuration package already loaded -->
+<launch>
+
+  <!-- Debug Info -->
+  <arg name="debug" default="false" />
+  <arg unless="$(arg debug)" name="launch_prefix" value="" />
+  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
+
+  <!-- Run -->
+  <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
+        args="--config_pkg=wroboarm_23"
+        launch-prefix="$(arg launch_prefix)"
+        required="true"
+        output="screen" />
+
+</launch>
diff --git a/src/wroboarm_23/launch/simple_moveit_controller_manager.launch.xml b/src/wroboarm_23/launch/simple_moveit_controller_manager.launch.xml
new file mode 100644
index 00000000..7a92fb9b
--- /dev/null
+++ b/src/wroboarm_23/launch/simple_moveit_controller_manager.launch.xml
@@ -0,0 +1,8 @@
+<launch>
+  <!-- Define the MoveIt controller manager plugin to use for trajectory execution -->
+  <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
+
+  <!-- Load controller list to the parameter server -->
+  <rosparam file="$(find wroboarm_23)/config/simple_moveit_controllers.yaml" />
+  <rosparam file="$(find wroboarm_23)/config/ros_controllers.yaml" />
+</launch>
diff --git a/src/wroboarm_23/launch/stomp_planning_pipeline.launch.xml b/src/wroboarm_23/launch/stomp_planning_pipeline.launch.xml
new file mode 100644
index 00000000..a1676cb5
--- /dev/null
+++ b/src/wroboarm_23/launch/stomp_planning_pipeline.launch.xml
@@ -0,0 +1,26 @@
+<launch>
+  <!-- Stomp Plugin for MoveIt -->
+  <arg name="planning_plugin" value="stomp_moveit/StompPlannerManager" />
+
+  <arg name="start_state_max_bounds_error" value="0.1" />
+  <arg name="jiggle_fraction" value="0.05" />
+  <!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
+  <arg name="planning_adapters"
+       default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
+                default_planner_request_adapters/AddTimeParameterization
+                default_planner_request_adapters/FixWorkspaceBounds
+                default_planner_request_adapters/FixStartStateBounds
+                default_planner_request_adapters/FixStartStateCollision
+                default_planner_request_adapters/FixStartStatePathConstraints" />
+
+
+  <param name="planning_plugin" value="$(arg planning_plugin)" />
+  <param name="request_adapters" value="$(arg planning_adapters)" />
+  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
+  <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
+
+  <!-- Add MoveGroup capabilities specific to this pipeline -->
+  <!-- <param name="capabilities" value="" /> -->
+
+  <rosparam command="load" file="$(find wroboarm_23)/config/stomp_planning.yaml"/>
+</launch>
diff --git a/src/wroboarm_23/launch/trajectory_execution.launch.xml b/src/wroboarm_23/launch/trajectory_execution.launch.xml
new file mode 100644
index 00000000..a2841558
--- /dev/null
+++ b/src/wroboarm_23/launch/trajectory_execution.launch.xml
@@ -0,0 +1,23 @@
+<launch>
+  <!-- This file summarizes all settings required for trajectory execution  -->
+
+  <!-- Define moveit controller manager plugin: fake, simple, or ros_control -->
+  <arg name="moveit_controller_manager" />
+  <arg name="fake_execution_type" default="interpolate" />
+
+  <!-- Flag indicating whether MoveIt is allowed to load/unload  or switch controllers -->
+  <arg name="moveit_manage_controllers" default="true"/>
+  <param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
+
+  <!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
+  <param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
+  <!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
+  <param name="trajectory_execution/allowed_goal_duration_margin" value="99999"/> <!-- default 0.5 -->
+  <!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
+  <param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->
+
+  <!-- We use pass_all_args=true here to pass fake_execution_type, which is required by fake controllers, but not by real-robot controllers.
+       As real-robot controller_manager.launch files shouldn't be required to define this argument, we use the trick of passing all args. -->
+  <include file="$(dirname)/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" pass_all_args="true" />
+
+</launch>
diff --git a/src/wroboarm_23/launch/warehouse.launch b/src/wroboarm_23/launch/warehouse.launch
new file mode 100644
index 00000000..0712e670
--- /dev/null
+++ b/src/wroboarm_23/launch/warehouse.launch
@@ -0,0 +1,15 @@
+<launch>
+
+  <!-- The path to the database must be specified -->
+  <arg name="moveit_warehouse_database_path" />
+
+  <!-- Load warehouse parameters -->
+  <include file="$(dirname)/warehouse_settings.launch.xml" />
+
+  <!-- Run the DB server -->
+  <node name="$(anon mongo_wrapper_ros)" cwd="ROS_HOME" type="mongo_wrapper_ros.py" pkg="warehouse_ros_mongo">
+    <param name="overwrite" value="false"/>
+    <param name="database_path" value="$(arg moveit_warehouse_database_path)" />
+  </node>
+
+</launch>
diff --git a/src/wroboarm_23/launch/warehouse_settings.launch.xml b/src/wroboarm_23/launch/warehouse_settings.launch.xml
new file mode 100644
index 00000000..e473b083
--- /dev/null
+++ b/src/wroboarm_23/launch/warehouse_settings.launch.xml
@@ -0,0 +1,16 @@
+<launch>
+  <!-- Set the parameters for the warehouse and run the mongodb server. -->
+
+  <!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->
+  <arg name="moveit_warehouse_port" default="33829" />
+
+  <!-- The default DB host for moveit -->
+  <arg name="moveit_warehouse_host" default="localhost" />
+
+  <!-- Set parameters for the warehouse -->
+  <param name="warehouse_port" value="$(arg moveit_warehouse_port)"/>
+  <param name="warehouse_host" value="$(arg moveit_warehouse_host)"/>
+  <param name="warehouse_exec" value="mongod" />
+  <param name="warehouse_plugin" value="warehouse_ros_mongo::MongoDatabaseConnection" />
+
+</launch>
diff --git a/src/wroboarm_23/launch/wroboarm_23_moveit_sensor_manager.launch.xml b/src/wroboarm_23/launch/wroboarm_23_moveit_sensor_manager.launch.xml
new file mode 100644
index 00000000..5d02698d
--- /dev/null
+++ b/src/wroboarm_23/launch/wroboarm_23_moveit_sensor_manager.launch.xml
@@ -0,0 +1,3 @@
+<launch>
+
+</launch>
diff --git a/src/wroboarm_23/meshes/base_link.STL b/src/wroboarm_23/meshes/base_link.STL
new file mode 100644
index 00000000..c298f6b5
Binary files /dev/null and b/src/wroboarm_23/meshes/base_link.STL differ
diff --git a/src/wroboarm_23/meshes/carpus_link.STL b/src/wroboarm_23/meshes/carpus_link.STL
new file mode 100644
index 00000000..412f5322
Binary files /dev/null and b/src/wroboarm_23/meshes/carpus_link.STL differ
diff --git a/src/wroboarm_23/meshes/endEffector_link.STL b/src/wroboarm_23/meshes/endEffector_link.STL
new file mode 100644
index 00000000..3efbbee9
Binary files /dev/null and b/src/wroboarm_23/meshes/endEffector_link.STL differ
diff --git a/src/wroboarm_23/meshes/forearm_link.STL b/src/wroboarm_23/meshes/forearm_link.STL
new file mode 100644
index 00000000..c9155a2e
Binary files /dev/null and b/src/wroboarm_23/meshes/forearm_link.STL differ
diff --git a/src/wroboarm_23/meshes/turntable_link.STL b/src/wroboarm_23/meshes/turntable_link.STL
new file mode 100644
index 00000000..80b5b615
Binary files /dev/null and b/src/wroboarm_23/meshes/turntable_link.STL differ
diff --git a/src/wroboarm_23/meshes/upperArm_link.STL b/src/wroboarm_23/meshes/upperArm_link.STL
new file mode 100644
index 00000000..f8a54dcb
Binary files /dev/null and b/src/wroboarm_23/meshes/upperArm_link.STL differ
diff --git a/src/wroboarm_23/meshes/wrist_link.STL b/src/wroboarm_23/meshes/wrist_link.STL
new file mode 100644
index 00000000..f79e328d
Binary files /dev/null and b/src/wroboarm_23/meshes/wrist_link.STL differ
diff --git a/src/wroboarm_23/package.xml b/src/wroboarm_23/package.xml
new file mode 100644
index 00000000..ee8c34af
--- /dev/null
+++ b/src/wroboarm_23/package.xml
@@ -0,0 +1,40 @@
+<package>
+
+  <name>wroboarm_23</name>
+  <version>0.3.0</version>
+  <description>
+     An automatically generated package with all the configuration and launch files for using the wroboarm_23 with the MoveIt Motion Planning Framework
+  </description>
+  <author email="bennowotny65535@gmail.com">Ben Nowotny</author>
+  <maintainer email="bennowotny65535@gmail.com">Ben Nowotny</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://moveit.ros.org/</url>
+  <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
+  <url type="repository">https://github.com/ros-planning/moveit</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>moveit_ros_move_group</run_depend>
+  <run_depend>moveit_fake_controller_manager</run_depend>
+  <run_depend>moveit_kinematics</run_depend>
+  <run_depend>moveit_planners_ompl</run_depend>
+  <run_depend>moveit_ros_visualization</run_depend>
+  <run_depend>moveit_setup_assistant</run_depend>
+  <run_depend>moveit_simple_controller_manager</run_depend>
+  <run_depend>joint_state_publisher</run_depend>
+  <run_depend>joint_state_publisher_gui</run_depend>
+  <run_depend>robot_state_publisher</run_depend>
+  <run_depend>rviz</run_depend>
+  <run_depend>tf2_ros</run_depend>
+  <run_depend>xacro</run_depend>
+  <!-- The next 2 packages are required for the gazebo simulation.
+       We don't include them by default to prevent installing gazebo and all its dependencies. -->
+  <!-- <run_depend>joint_trajectory_controller</run_depend> -->
+  <!-- <run_depend>gazebo_ros_control</run_depend> -->
+  <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
+  <!-- <run_depend>warehouse_ros_mongo</run_depend> -->
+
+
+</package>
diff --git a/src/wroboarm_23/urdf/ArmAssembly.SLDASM.csv b/src/wroboarm_23/urdf/ArmAssembly.SLDASM.csv
new file mode 100644
index 00000000..294889b5
--- /dev/null
+++ b/src/wroboarm_23/urdf/ArmAssembly.SLDASM.csv
@@ -0,0 +1,8 @@
+Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
+base_link,9.1830074983592E-19,-9.80159456515381E-19,0.015606490089144,0,0,0,0.93972387307473,0.00251429022255044,7.37485241366082E-20,8.66637218064247E-21,0.0021336882069818,1.14091331088149E-19,0.0044229329573873,0,0,0,0,0,0,package://wroboarm_23/meshes/base_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_23/meshes/base_link.STL,,base-1,Origin_global,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
+turntable_link,-0.01225868218219,0.128577249577976,-0.00370544350042927,0,0,0,0.996366805002726,0.00652370094208946,0.0010646616036529,0.000283127005277363,0.0050231810993567,6.82113719161566E-05,0.00863345382595838,0,0,0,0,0,0,package://wroboarm_23/meshes/turntable_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_23/meshes/turntable_link.STL,,Shoulder-1,Origin_turntable_joint,Axis1,turntable_joint,continuous,0,0,0.035865,1.5708,0,-0.84509,base_link,0,1,0,,,,,,,,,,,,
+upperArm_link,0.0143973999054599,-3.20061625693202E-05,0.279348384794928,0,0,0,0.985210343915811,0.0237982224738338,-8.8797773328906E-07,-0.00388532856230116,0.0293594722597601,-7.129443699684E-06,0.00625034839320102,0,0,0,0,0,0,package://wroboarm_23/meshes/upperArm_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_23/meshes/upperArm_link.STL,,UpperArm-1,Origin_shoulder_joint,Axis2,shoulder_joint,continuous,0,0.18098,0.043071,-1.5572,0,0,turntable_link,-1,0,0,,,,,,,,,,,,
+forearm_link,-4.1286418728248E-16,0.0550154986611603,0.120152803034247,0,0,0,0.51779610173045,0.00404850874710764,-4.33680868994202E-19,2.87313575708659E-18,0.00375548329392275,-0.000377385363706531,0.000788327838226511,0,0,0,0,0,0,package://wroboarm_23/meshes/forearm_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_23/meshes/forearm_link.STL,,Forarm-1,Origin_elbowPitch_joint,Axis3,elbowPitch_joint,continuous,0,0,0.46634,2.7009,0,0,upperArm_link,-1,0,0,,,,,,,,,,,,
+wrist_link,-2.77555756156289E-17,5.55111512312578E-17,0.096820252622317,0,0,0,0.301763082175336,0.00210344289702837,1.30104260698261E-18,5.14996031930615E-19,0.00305016250706558,-1.15196480826585E-18,0.00107609106501786,0,0,0,0,0,0,package://wroboarm_23/meshes/wrist_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_23/meshes/wrist_link.STL,,WristStructure-1,Origin_elbowRoll_joint,Axis4,elbowRoll_joint,continuous,0,0.04445,0.23038,0,0,0,forearm_link,0,0,1,,,,,,,,,,,,
+carpus_link,5.55111512312578E-17,5.55111512312578E-17,0,0,0,0,0.447707117943177,0.000367702266904795,7.04731412115578E-19,1.62630325872826E-19,0.000843427086265753,0,0.000844746123560661,0,0,0,0,0,0,package://wroboarm_23/meshes/carpus_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_23/meshes/carpus_link.STL,,Wrist-1,Origin_wristPitch_joint,Axis5,wristPitch_joint,continuous,0,0,0.23528,0,0,0,wrist_link,-1,0,0,,,,,,,,,,,,
+endEffector_link,-0.000725623905114631,-0.000893578002320872,0.0499824104113341,0,0,0,0.399994299548626,0.00169604127916281,-3.07411212276679E-06,2.57918978917245E-05,0.00173351996343527,5.0605971705242E-05,0.000256533582960489,0,0,0,0,0,0,package://wroboarm_23/meshes/endEffector_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://wroboarm_23/meshes/endEffector_link.STL,,Manipulator-1,Origin_wristRoll_link,Axis6,wristRoll_link,continuous,0,0,0,0,0,0,carpus_link,0,0,1,,,,,,,,,,,,
diff --git a/src/wroboarm_23/urdf/ArmAssembly.SLDASM.urdf b/src/wroboarm_23/urdf/ArmAssembly.SLDASM.urdf
new file mode 100644
index 00000000..3bd1ee46
--- /dev/null
+++ b/src/wroboarm_23/urdf/ArmAssembly.SLDASM.urdf
@@ -0,0 +1,371 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
+     Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578
+     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot
+  name="wroboarm_23">
+  <link
+    name="base_link">
+    <inertial>
+      <origin
+        xyz="9.1830074983592E-19 -9.80159456515381E-19 0.015606490089144"
+        rpy="0 0 0" />
+      <mass
+        value="0.93972387307473" />
+      <inertia
+        ixx="0.00251429022255044"
+        ixy="7.37485241366082E-20"
+        ixz="8.66637218064247E-21"
+        iyy="0.0021336882069818"
+        iyz="1.14091331088149E-19"
+        izz="0.0044229329573873" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://wroboarm_23/meshes/base_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://wroboarm_23/meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="turntable_link">
+    <inertial>
+      <origin
+        xyz="-0.01225868218219 0.128577249577976 -0.00370544350042927"
+        rpy="0 0 0" />
+      <mass
+        value="0.996366805002726" />
+      <inertia
+        ixx="0.00652370094208946"
+        ixy="0.0010646616036529"
+        ixz="0.000283127005277363"
+        iyy="0.0050231810993567"
+        iyz="6.82113719161566E-05"
+        izz="0.00863345382595838" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://wroboarm_23/meshes/turntable_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://wroboarm_23/meshes/turntable_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="turntable_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 0.035865"
+      rpy="1.5708 0 -0.84509" />
+    <parent
+      link="base_link" />
+    <child
+      link="turntable_link" />
+    <axis
+      xyz="0 1 0" />
+    <limit effort="50" velocity="1.5" lower="0" upper="1.57" />
+  </joint>
+  <link
+    name="upperArm_link">
+    <inertial>
+      <origin
+        xyz="0.0143973999054599 -3.20061625693202E-05 0.279348384794928"
+        rpy="0 0 0" />
+      <mass
+        value="0.985210343915811" />
+      <inertia
+        ixx="0.0237982224738338"
+        ixy="-8.8797773328906E-07"
+        ixz="-0.00388532856230116"
+        iyy="0.0293594722597601"
+        iyz="-7.129443699684E-06"
+        izz="0.00625034839320102" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://wroboarm_23/meshes/upperArm_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://wroboarm_23/meshes/upperArm_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="shoulder_joint"
+    type="revolute">
+    <origin
+      xyz="0 0.18098 0.043071"
+      rpy="-1.5572 0 0" />
+    <parent
+      link="turntable_link" />
+    <child
+      link="upperArm_link" />
+    <axis
+      xyz="-1 0 0" />
+    <limit effort="50" velocity="1.5" lower="0" upper="1.57" />
+  </joint>
+  <link
+    name="forearm_link">
+    <inertial>
+      <origin
+        xyz="-4.1286418728248E-16 0.0550154986611603 0.120152803034247"
+        rpy="0 0 0" />
+      <mass
+        value="0.51779610173045" />
+      <inertia
+        ixx="0.00404850874710764"
+        ixy="-4.33680868994202E-19"
+        ixz="2.87313575708659E-18"
+        iyy="0.00375548329392275"
+        iyz="-0.000377385363706531"
+        izz="0.000788327838226511" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://wroboarm_23/meshes/forearm_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://wroboarm_23/meshes/forearm_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="elbowPitch_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 0.46634"
+      rpy="2.7009 0 0" />
+    <parent
+      link="upperArm_link" />
+    <child
+      link="forearm_link" />
+    <axis
+      xyz="-1 0 0" />
+    <limit effort="50" velocity="1.5" lower="0" upper="1.57" />
+  </joint>
+  <link
+    name="wrist_link">
+    <inertial>
+      <origin
+        xyz="-2.77555756156289E-17 5.55111512312578E-17 0.096820252622317"
+        rpy="0 0 0" />
+      <mass
+        value="0.301763082175336" />
+      <inertia
+        ixx="0.00210344289702837"
+        ixy="1.30104260698261E-18"
+        ixz="5.14996031930615E-19"
+        iyy="0.00305016250706558"
+        iyz="-1.15196480826585E-18"
+        izz="0.00107609106501786" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://wroboarm_23/meshes/wrist_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://wroboarm_23/meshes/wrist_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="elbowRoll_joint"
+    type="revolute">
+    <origin
+      xyz="0 0.04445 0.23038"
+      rpy="0 0 0" />
+    <parent
+      link="forearm_link" />
+    <child
+      link="wrist_link" />
+    <axis
+      xyz="0 0 1" />
+    <limit effort="50" velocity="1.5" lower="0" upper="1.57" />
+  </joint>
+  <link
+    name="carpus_link">
+    <inertial>
+      <origin
+        xyz="5.55111512312578E-17 5.55111512312578E-17 0"
+        rpy="0 0 0" />
+      <mass
+        value="0.447707117943177" />
+      <inertia
+        ixx="0.000367702266904795"
+        ixy="7.04731412115578E-19"
+        ixz="1.62630325872826E-19"
+        iyy="0.000843427086265753"
+        iyz="0"
+        izz="0.000844746123560661" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://wroboarm_23/meshes/carpus_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://wroboarm_23/meshes/carpus_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="wristPitch_joint"
+    type="revolute">
+    <origin
+      xyz="0 0 0.23528"
+      rpy="0 0 0" />
+    <parent
+      link="wrist_link" />
+    <child
+      link="carpus_link" />
+    <axis
+      xyz="-1 0 0" />
+    <limit effort="50" velocity="1.5" lower="0" upper="1.57" />
+  </joint>
+  <link
+    name="endEffector_link">
+    <inertial>
+      <origin
+        xyz="-0.000725623905114631 -0.000893578002320872 0.0499824104113341"
+        rpy="0 0 0" />
+      <mass
+        value="0.399994299548626" />
+      <inertia
+        ixx="0.00169604127916281"
+        ixy="-3.07411212276679E-06"
+        ixz="2.57918978917245E-05"
+        iyy="0.00173351996343527"
+        iyz="5.0605971705242E-05"
+        izz="0.000256533582960489" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://wroboarm_23/meshes/endEffector_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://wroboarm_23/meshes/endEffector_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="wristRoll_link"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="0 0 0" />
+    <parent
+      link="carpus_link" />
+    <child
+      link="endEffector_link" />
+    <axis
+      xyz="0 0 1" />
+    <limit effort="50" velocity="1.5" lower="0" upper="1.57" />
+  </joint>
+</robot>
\ No newline at end of file
diff --git a/test_arm_control_stack.sh b/test_arm_control_stack.sh
new file mode 100755
index 00000000..af265954
--- /dev/null
+++ b/test_arm_control_stack.sh
@@ -0,0 +1,18 @@
+#!/bin/bash
+ROOT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
+cd $ROOT_DIR
+./assemble.py build
+#gnome-terminal -- roscore
+#rostopic list &>./out.temp;
+#COUNT=$(grep -Ec "Unable to communicate with master" out.temp)
+#while [ $COUNT -ne 0 ]; do
+#	rostopic list &>./out.temp;
+#	COUNT=$(grep -Ec "Unable to communicate with master" out.temp)
+#done
+#rm ./out.temp;
+export WROVER_LOCAL=true
+export WROVER_HW=MOCK
+MOTOR_TO_INSPECT=11
+# (sleep 2 && rqt_plot /control/arm/$MOTOR_TO_INSPECT/output /control/arm/$MOTOR_TO_INSPECT/setpoint /control/arm/$MOTOR_TO_INSPECT/feedback) & #/hsi/roboclaw/aux1/cmd/left 
+roslaunch wr_entry_point mock_arm.launch
+
diff --git a/testscrpt.py b/testscrpt.py
new file mode 100755
index 00000000..dd6cb4c3
--- /dev/null
+++ b/testscrpt.py
@@ -0,0 +1,41 @@
+#!/usr/bin/env python3
+import rospy
+import sensor_msgs.msg as sensor_msgs
+import matplotlib.pyplot as plt
+from threading import Thread
+
+positions = None
+names = None
+
+def newJS(jsMsg: sensor_msgs.JointState):
+    global positions, names
+    if positions is None:
+        positions = []
+        names = [name for name in jsMsg.name]
+        for pos in jsMsg.position:
+            positions.append([pos])
+    else:
+        for l, pos in zip(positions, jsMsg.position):
+            l.append(pos)
+            if len(l) > 500:
+                del l[0]
+
+def plot():
+    global positions, names
+    while not rospy.is_shutdown():
+        if positions is not None and names is not None:
+            for l in positions:
+                plt.plot(l)
+            plt.legend(names)
+            plt.pause(0.05)
+            plt.clf()
+
+
+if __name__=="__main__":
+    rospy.init_node("testNode")
+    sub = rospy.Subscriber("/joint_states", sensor_msgs.JointState, newJS)
+    t1 = Thread(target=plot)
+    t1.start()
+    t1.join()
+    rospy.spin()
+    
\ No newline at end of file