diff --git a/CMakeLists.txt b/CMakeLists.txt
index ac70d09ad..adae17b4c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -259,8 +259,11 @@ else ()
             target_precompile_headers(zed_nodelet PRIVATE src/perception/zed_wrapper/pch.hpp)
             target_include_directories(zed_nodelet SYSTEM PRIVATE ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS})
             target_link_libraries(zed_nodelet PRIVATE ${ZED_LIBRARIES} ${OpenCV_LIBRARIES} ${SPECIAL_OS_LIBS} lie)
-            target_compile_definitions(zed_nodelet PRIVATE MROVER_IS_NODELET)
-            target_compile_options(zed_nodelet PRIVATE $<$<COMPILE_LANGUAGE:CUDA>:--expt-extended-lambda>)
+            target_compile_definitions(zed_nodelet PRIVATE
+                    MROVER_IS_NODELET # Generate code relevant to nodelet
+                    ALLOW_BUILD_DEBUG # Ignore ZED warnings about Debug mode
+                    __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__ # Eigen includes some files it should not, ignore
+            )
         endif ()
     endmacro()
 endif ()
diff --git a/config/rviz/arm.rviz b/config/rviz/arm.rviz
index c1aab6175..f7dac412f 100644
--- a/config/rviz/arm.rviz
+++ b/config/rviz/arm.rviz
@@ -4,12 +4,12 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /Global Options1
+        - /Status1
         - /Grid1
-        - /MotionPlanning1
-        - /MotionPlanning1/Scene Geometry1
-        - /MotionPlanning1/Scene Robot1
+        - /RobotModel1
       Splitter Ratio: 0.5
-    Tree Height: 336
+    Tree Height: 871
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -51,184 +51,60 @@ Visualization Manager:
         Z: 0
       Plane: XY
       Plane Cell Count: 100
-      Reference Frame: world
+      Reference Frame: base_link
       Value: true
-    - Acceleration_Scaling_Factor: 0.1
-      Class: moveit_rviz_plugin/MotionPlanning
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
       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
-          aTob:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bToc:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          cTod:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          chassisToa:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          dToe:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          eTof:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          hand:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          world:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-        Loop Animation: false
-        Robot Alpha: 0.5
-        Robot Color: 150; 50; 150
-        Show Robot Collision: false
-        Show Robot Visual: true
-        Show Trail: false
-        State Display Time: 3x
-        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: ""
-        Query Goal State: true
-        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
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        a_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        b_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        c_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        d_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        e_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
       Robot Description: robot_description
-      Scene Geometry:
-        Scene Alpha: 0.8999999761581421
-        Scene Color: 50; 230; 50
-        Scene Display Time: 0.009999999776482582
-        Show Scene Geometry: true
-        Voxel Coloring: Z-Axis
-        Voxel Rendering: Occupied Voxels
-      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
-          aTob:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          bToc:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          base_link:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          cTod:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          chassisToa:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          dToe:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          eTof:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          hand:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-            Value: true
-          world:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-        Robot Alpha: 0.5
-        Show Robot Collision: true
-        Show Robot Visual: false
+      TF Prefix: ""
+      Update Interval: 0
       Value: true
-      Velocity_Scaling_Factor: 0.1
+      Visual Enabled: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: chassisToa
-    Frame Rate: 30
+    Fixed Frame: base_link
+    Frame Rate: 60
   Name: root
   Tools:
     - Class: rviz/Interact
@@ -251,7 +127,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 103.44278717041016
+      Distance: 65.38457489013672
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -259,29 +135,25 @@ Visualization Manager:
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: -1.1320419311523438
-        Y: 0.6890945434570312
-        Z: -1.7296075820922852
+        X: 5.594668865203857
+        Y: 0.6798133850097656
+        Z: 3.844665288925171
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.13748864829540253
+      Pitch: 0.42248857021331787
       Target Frame: <Fixed Frame>
-      Yaw: 2.7814881801605225
+      Yaw: 1.0014894008636475
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 948
+  Height: 1085
   Hide Left Dock: false
   Hide Right Dock: true
-  MotionPlanning:
-    collapsed: false
-  MotionPlanning - Trajectory Slider:
-    collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -290,6 +162,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1920
-  X: 0
-  Y: 27
+  Width: 1574
+  X: 177
+  Y: 89
diff --git a/src/perception/tag_detector/tag_detector.cpp b/src/perception/tag_detector/tag_detector.cpp
index 7daea870b..0fb90f3fa 100644
--- a/src/perception/tag_detector/tag_detector.cpp
+++ b/src/perception/tag_detector/tag_detector.cpp
@@ -5,8 +5,8 @@ namespace mrover {
     void TagDetectorNodelet::onInit() {
         mNh = getMTNodeHandle();
         mPnh = getMTPrivateNodeHandle();
-        mDetectorParams = new cv::aruco::DetectorParameters();
-        auto defaultDetectorParams = cv::aruco::DetectorParameters::create();
+        mDetectorParams = cv::makePtr<cv::aruco::DetectorParameters>();
+        auto defaultDetectorParams = cv::makePtr<cv::aruco::DetectorParameters>();
         int dictionaryNumber;
 
         mNh.param<bool>("use_odom_frame", mUseOdom, false);
@@ -15,8 +15,7 @@ namespace mrover {
         mNh.param<std::string>("camera_frame", mCameraFrameId, "zed2i_left_camera_frame");
 
         mPnh.param<bool>("publish_images", mPublishImages, true);
-        using DictEnumType = std::underlying_type_t<cv::aruco::PREDEFINED_DICTIONARY_NAME>;
-        mPnh.param<int>("dictionary", dictionaryNumber, static_cast<DictEnumType>(cv::aruco::DICT_4X4_50));
+        mPnh.param<int>("dictionary", dictionaryNumber, static_cast<int>(cv::aruco::DICT_4X4_50));
         mPnh.param<int>("min_hit_count_before_publish", mMinHitCountBeforePublish, 5);
         mPnh.param<int>("max_hit_count", mMaxHitCount, 5);
         mPnh.param<int>("tag_increment_weight", mTagIncrementWeight, 2);
@@ -24,7 +23,7 @@ namespace mrover {
 
         mIt.emplace(mNh);
         mImgPub = mIt->advertise("tag_detection", 1);
-        mDictionary = cv::aruco::getPredefinedDictionary(dictionaryNumber);
+        mDictionary = cv::makePtr<cv::aruco::Dictionary>(cv::aruco::getPredefinedDictionary(dictionaryNumber));
 
         mPcSub = mNh.subscribe("camera/left/points", 1, &TagDetectorNodelet::pointCloudCallback, this);
         mServiceEnableDetections = mNh.advertiseService("enable_detections", &TagDetectorNodelet::enableDetectionsCallback, this);
diff --git a/src/teleoperation/frontend/.gitignore b/src/teleoperation/frontend/.gitignore
index 38adffa64..692d47f6d 100644
--- a/src/teleoperation/frontend/.gitignore
+++ b/src/teleoperation/frontend/.gitignore
@@ -13,6 +13,7 @@ dist
 dist-ssr
 coverage
 *.local
+bun.lockb
 
 /cypress/videos/
 /cypress/screenshots/
diff --git a/starter_project/autonomy/src/perception.cpp b/starter_project/autonomy/src/perception.cpp
index 2185a5fe2..d9b0aa7b9 100644
--- a/starter_project/autonomy/src/perception.cpp
+++ b/starter_project/autonomy/src/perception.cpp
@@ -28,8 +28,8 @@ namespace mrover {
         // TODO: uncomment me!
         // mTagPublisher = mNodeHandle.advertise<StarterProjectTag>("tag", 1);
 
-        mTagDetectorParams = cv::aruco::DetectorParameters::create();
-        mTagDictionary = cv::aruco::getPredefinedDictionary(0);
+        mTagDetectorParams = cv::makePtr<cv::aruco::DetectorParameters>();
+        mTagDictionary = cv::makePtr<cv::aruco::Dictionary>(cv::aruco::getPredefinedDictionary(0));
     }
 
     void Perception::imageCallback(sensor_msgs::ImageConstPtr const& image) {
diff --git a/urdf/arm/arm.urdf.xacro b/urdf/arm/arm.urdf.xacro
index 13617b804..b5f93cf28 100644
--- a/urdf/arm/arm.urdf.xacro
+++ b/urdf/arm/arm.urdf.xacro
@@ -1,175 +1,148 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="arm">
-    <xacro:property name="PI" value="3.141592"/>
-    <xacro:property name="base_width" value="7"/>
-    <xacro:property name="base_len" value="7"/>
-    <xacro:property name="base_height" value="19"/>
+  <xacro:property name="PI" value="3.141592"/>
+  <xacro:property name="base_width" value="7"/>
+  <xacro:property name="base_len" value="7"/>
+  <xacro:property name="base_height" value="19"/>
 
-    <!-- Functions -->
+  <!-- Functions -->
 
-    <xacro:macro name="joints"
-                 params="name type:=revolute parent child roll:=0 pitch:=0 yaw:=0 origin_x origin_y origin_z axis_x:=0 axis_y:=0 axis_z:=0 lower upper damp:=1.0">
-        <joint name="${name}" type="${type}">
-            <origin xyz="${origin_x} ${origin_y} ${origin_z}" rpy="${roll} ${pitch} ${yaw}"/>
-            <parent link="${parent}"/>
-            <child link="${child}"/>
-            <axis xyz="${axis_x} ${axis_y} ${axis_z}"/>
-            <!-- effort in N-m, velocity in rad/s -->
-            <limit lower="${lower}" upper="${upper}" effort="30" velocity="1"/>
-            <!-- damping in N-m-s/rad -->
-            <dynamics damping="${damp}"/>
-        </joint>
-    </xacro:macro>
+  <xacro:macro name="joints"
+               params="name type:=revolute parent child x y z rx:=0 ry:=0 rz:=0 axis_x:=0 axis_y:=1 axis_z:=0 lower upper damp:=1.0">
+    <joint name="${name}" type="${type}">
+      <origin xyz="${x} ${y} ${z}" rpy="${rx} ${ry} ${rz}"/>
+      <parent link="${parent}"/>
+      <child link="${child}"/>
+      <axis xyz="${axis_x} ${axis_y} ${axis_z}"/>
+      <!-- effort in N-m, velocity in rad/s -->
+      <limit lower="${lower}" upper="${upper}" effort="30" velocity="1"/>
+      <!-- damping in N-m-s/rad -->
+      <dynamics damping="${damp}"/>
+    </joint>
+  </xacro:macro>
 
-    <xacro:macro name="link-visual" params="fileName x y z col r g b roll:=0 pitch:=0 yaw:=0">
-        <visual>
-            <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}"/>
-            <geometry>
-                <mesh filename="package://mrover/urdf/meshes/${fileName}"/>
-            </geometry>
-            <material name="${col}">
-                <color rgba="${r} ${g} ${b} 1"/>
-            </material>
-        </visual>
-    </xacro:macro>
+  <xacro:macro name="link-visual" params="fileName col r g b x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0">
+    <visual>
+      <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}"/>
+      <geometry>
+        <mesh filename="package://mrover/urdf/meshes/${fileName}"/>
+      </geometry>
+      <material name="${col}">
+        <color rgba="${r} ${g} ${b} 1"/>
+      </material>
+    </visual>
+  </xacro:macro>
 
-    <xacro:macro name="link-inertial"
-                 params="x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0 mass ixx iyy izz ixy:=0 iyz:=0 ixz:=0">
-        <inertial>
-            <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}"/>
-            <!-- mass in kg -->
-            <mass value="${mass}"/>
-            <inertia ixx="${ixx}" iyy="${iyy}" izz="${izz}" ixy="${ixy}" iyz="${iyz}" ixz="${ixz}"/>
-        </inertial>
-    </xacro:macro>
+  <xacro:macro name="link-inertial"
+               params="x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0 mass ixx iyy izz ixy:=0 iyz:=0 ixz:=0">
+    <inertial>
+      <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}"/>
+      <!-- mass in kg -->
+      <mass value="${mass}"/>
+      <inertia ixx="${ixx}" iyy="${iyy}" izz="${izz}" ixy="${ixy}" iyz="${iyz}" ixz="${ixz}"/>
+    </inertial>
+  </xacro:macro>
 
-    <xacro:macro name="box_inertia" params="m w h d">
-        <inertial>
-            <mass value="${m}"/>
-            <inertia ixx="${m / 12.0 * (d*d + h*h)}" ixy="0.0" ixz="0.0" iyy="${m / 12.0 * (w*w + h*h)}" iyz="0.0"
-                     izz="${m / 12.0 * (w*w + d*d)}"/>
-        </inertial>
-    </xacro:macro>
+  <xacro:macro name="box_inertia" params="m w h d">
+    <inertial>
+      <mass value="${m}"/>
+      <inertia ixx="${m / 12.0 * (d*d + h*h)}" ixy="0.0" ixz="0.0" iyy="${m / 12.0 * (w*w + h*h)}" iyz="0.0"
+               izz="${m / 12.0 * (w*w + d*d)}"/>
+    </inertial>
+  </xacro:macro>
 
-    <!-- defines a cylinder collision object with two endpoints -->
-    <xacro:macro name="link-collision-cylinder" params="radius x1 y1 z1 x2 y2 z2 roll:=0 pitch:=0 yaw:=0">
-        <xacro:property name="dx" value="${x2-x1}"/>
-        <xacro:property name="dy" value="${y2-y1}"/>
-        <xacro:property name="dz" value="${z2-z1}"/>
+  <!-- defines a cylinder collision object with two endpoints -->
+  <xacro:macro name="link-collision-cylinder" params="radius x1 y1 z1 x2 y2 z2 roll:=0 pitch:=0 yaw:=0">
+    <xacro:property name="dx" value="${x2-x1}"/>
+    <xacro:property name="dy" value="${y2-y1}"/>
+    <xacro:property name="dz" value="${z2-z1}"/>
 
-        <xacro:if value="${dx != 0}">
-            <xacro:property name="length" value="${fabs(dx)}"/>
-            <xacro:property name="rpy" value="${roll} ${pitch+PI/2} ${yaw}"/>
-        </xacro:if>
-        <xacro:if value="${dy != 0}">
-            <xacro:property name="length" value="${fabs(dy)}"/>
-            <xacro:property name="rpy" value="${roll+PI/2} ${pitch} ${yaw}"/>
-        </xacro:if>
-        <xacro:if value="${dz != 0}">
-            <xacro:property name="length" value="${fabs(dz)}"/>
-            <xacro:property name="rpy" value="${roll} ${pitch} ${yaw}"/>
-        </xacro:if>
+    <xacro:if value="${dx != 0}">
+      <xacro:property name="length" value="${fabs(dx)}"/>
+      <xacro:property name="rpy" value="${roll} ${pitch+PI/2} ${yaw}"/>
+    </xacro:if>
+    <xacro:if value="${dy != 0}">
+      <xacro:property name="length" value="${fabs(dy)}"/>
+      <xacro:property name="rpy" value="${roll+PI/2} ${pitch} ${yaw}"/>
+    </xacro:if>
+    <xacro:if value="${dz != 0}">
+      <xacro:property name="length" value="${fabs(dz)}"/>
+      <xacro:property name="rpy" value="${roll} ${pitch} ${yaw}"/>
+    </xacro:if>
 
-        <collision>
-            <origin xyz="${x1+dx/2} ${y1+dy/2} ${z1+dz/2}" rpy="${rpy}"/>
-            <geometry>
-                <cylinder radius="${radius}" length="${length}"/>
-            </geometry>
-        </collision>
-    </xacro:macro>
+    <collision>
+      <origin xyz="${x1+dx/2} ${y1+dy/2} ${z1+dz/2}" rpy="${rpy}"/>
+      <geometry>
+        <cylinder radius="${radius}" length="${length}"/>
+      </geometry>
+    </collision>
+  </xacro:macro>
 
-    <!-- defines a box collision object with the center and dimensions -->
-    <xacro:macro name="link-collision-box" params="x:=0 y:=0 z:=0 lx ly lz roll:=0 pitch:=0 yaw:=0">
-        <collision>
-            <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}"/>
-            <geometry>
-                <box size="${lx} ${ly} ${lz}"/>
-            </geometry>
-        </collision>
-    </xacro:macro>
+  <!-- defines a box collision object with the center and dimensions -->
+  <xacro:macro name="link-collision-box" params="x:=0 y:=0 z:=0 lx ly lz roll:=0 pitch:=0 yaw:=0">
+    <collision>
+      <origin xyz="${x} ${y} ${z}" rpy="${roll} ${pitch} ${yaw}"/>
+      <geometry>
+        <box size="${lx} ${ly} ${lz}"/>
+      </geometry>
+    </collision>
+  </xacro:macro>
 
-    <!-- Joints -->
+  <!-- Joints -->
 
-    <xacro:joints name="base_link_connection" type="fixed" parent="base_link" child="a_link"
-                  origin_x="0" origin_y="15.55" origin_z="11" axis_z="0"
-                  lower="0" upper="0"/>
-    <xacro:joints name="a_joint" type="continuous" parent="a_link" child="b_link"
-                  origin_x="0" origin_y="0" origin_z="0" axis_z="1"
-                  lower="-3.14" upper="3.14"/>
-    <xacro:joints name="b_joint" parent="b_link" child="c_link"
-                  origin_x="0" origin_y="1.33" origin_z="0.99" axis_x="1"
-                  lower="-1.064651" upper="0"
-                  roll="0.593412"/>
-    <xacro:joints name="c_joint" parent="c_link" child="d_link"
-                  origin_x="0" origin_y="20.11" origin_z="10.75" axis_x="1"
-                  lower="-0.087267" upper="4.886922"
-                  roll="-0.436332"/>
-    <xacro:joints name="d_joint" parent="d_link" child="e_link"
-                  origin_x="-4.4" origin_y="-0.42" origin_z="-2.535" axis_z="1" axis_y="0"
-                  lower="-3.14" upper="3.14"
-                  roll="-0.15708" pitch="-0.0174533" yaw="0"/>
-    <!-- <xacro:joints name="e_joint" parent="dToe" child="eTof"
-                  origin_x="2.55" origin_y="0.2" origin_z="-17" axis_y="1"
-                  lower="-3.59538" upper="0.610865"
-                  roll="0" pitch="0" yaw="-1.649336"/>
-    <xacro:joints name="f_joint" parent="eTof" child="hand"
-                  origin_x="-6.3" origin_y="1.75" origin_z="0" axis_x="1"
-                  lower="-0.523599" upper="6.28"/> -->
+  <xacro:joints name="base_link_joint" type="prismatic" parent="base_link" child="a_link"
+                x="0.031359" y="9.9173" z="0"
+                lower="0" upper="20"/>
+  <xacro:joints name="a_joint" parent="a_link" child="b_link"
+                x="1.32728" y="0.02613" z="1.99693"
+                ry="-1.04509"
+                lower="-3.14" upper="3.14"/>
+  <xacro:joints name="b_joint" parent="b_link" child="c_link"
+                x="22.8544" y="1.81734" z="0.378175"
+                ry="2.61595"
+                lower="-1.064651" upper="0"/>
+  <xacro:joints name="c_joint" parent="c_link" child="d_link"
+                x="19.5129" y="0.264652" z="0.03498"
+                ry="-1.5708"
+                lower="-0.087267" upper="4.886922"/>
+  <xacro:joints name="d_joint" parent="d_link" child="e_link"
+                x="5.59352" y="-2.05037" z="0.040519"
+                lower="-3.14" upper="3.14"/>
 
-    <!-- Links -->
+  <!-- Links -->
 
-    <link name="base_link">
-        <!-- <xacro:box_inertia m="3.0" w="${base_len}" h="${base_width}" d="${base_height}"/>
-        <xacro:link-visual fileName="chasis.dae" x="0" y="0" z="-9.5" col="red" r="1" g="1" b="1"/>
-        <collision>
-            <origin rpy="0 0 0" xyz="0 0 -9.5"/>
-            <geometry>
-                <mesh filename="package://robo_impls/urdf/rover.dae"/>
-            </geometry>
-        </collision> -->
-    </link>
-    <link name="a_link">
-        <xacro:link-visual fileName="arm.a.dae" x="0" y="0" z="0" col="red" r="1" g="0" b="0"/>
-        <xacro:link-collision-cylinder radius="3.75" x1="0" y1="0" z1="-1.25" x2="0" y2="0" z2="-3.75"/>
-        <xacro:link-collision-cylinder radius="1.75" x1="3.6467207" y1="3.6590503" z1="-0.75" x2="3.6467207"
-                                       y2="3.6590503" z2="-2.6913988"/>
-        <xacro:link-inertial z="-2.5" mass="0.5" ixx="2.3" iyy="2.3" izz="3.5"/>
-    </link>
-    <link name="b_link">
-        <xacro:link-visual fileName="arm.b.dae" x="0" y="0" z="0" col="orange" r="1" g="0.43" b="0" roll="0"/>
-        <xacro:link-collision-box y="-1.75" z="3" lx="2" ly="3" lz="6"/>
-        <xacro:link-collision-cylinder radius="2" x1="0" y1="0" z1="-0.25" x2="0" y2="0" z2="0.75"/>
-        <xacro:link-inertial y="-1.75" z="3" mass="0.5" ixx="1.9" iyy="0.7" izz="0.5"/>
-    </link>
-    <link name="c_link">
-        <xacro:link-visual fileName="arm.c.dae" x="0" y="0" z="0" col="yellow" r="1" g="1" b="0" roll="0"/>
-        <xacro:link-collision-box y="9" z="5" lx="2" ly="20" lz="2" roll="0.48"/>
-        <xacro:link-collision-cylinder radius="2" x1="-2" y1="20.25" z1="10.75" x2="4.5" y2="20.25" z2="10.75"/>
-        <xacro:link-collision-cylinder radius="1" x1="0" y1="-2" z1="8.5" x2="0" y2="17" z2="8.5" roll="0.3"/>
-        <xacro:link-inertial y="12" mass="0.5" ixx="24" iyy="0.75" izz="24"/>
-    </link>
-    <link name="d_link">
-        <xacro:link-visual fileName="arm.d.dae" x="0" y="0" z="0" col="green" r="0.43" g="1" b="0"/>
-        <xacro:link-collision-cylinder radius="3.75" x1="-4.5" y1="0" z1="-2.5" x2="-4.5" y2="0" z2="3" roll="-0.17"/>
-        <xacro:link-inertial x="-1" mass="0.5" ixx="3.2" iyy="3.2" izz="16"/>
-    </link>
-    <link name="e_link">
-        <xacro:link-visual fileName="arm.e.dae" x="0" y="0" z="0" col="blue" r="0" g="0" b="1" pitch="0" roll="0"/>
-        <xacro:link-collision-box y="0" z="-7" lx="4" ly="20" lz="4" roll="${(-PI/2.0)+0.05}"/>
-        <xacro:link-collision-cylinder radius="2.5" x1="-3" y1="0" z1="-17" x2="3" y2="0" z2="-17"/>
-        <xacro:link-inertial y="-7.6324" z="4.7693" roll="-0.558505" mass="0.5" ixx="17" iyy="0.5" izz="17"/>
-    </link>
-    <!-- <link name="eTof">
-        <xacro:link-visual fileName="e-f.dae" x="0" y="0" z="0" col="violet" r="0.5" g="0" b="1" roll="0"/>
-        <xacro:link-collision-cylinder radius="3" x1="3" y1="1.8" z1="0" x2="-6.5" y2="1.8" z2="0"/>
-        <xacro:link-inertial z="-3" mass="0.5" ixx="1.6" iyy="1.6" izz="0.25"/>
-    </link>
-    <link name="hand">
-        <xacro:link-visual fileName="hand.dae" x="0" y="0" z="0" col="pink" r="1" g="0" b="1"/>
-        <xacro:link-collision-box x="-2.25" y="1" z="0.5" lx="8" ly="3.5" lz="8.5" roll="${PI/2.0}"/>
-        <xacro:link-collision-box z="8.75" y="3.5" x="-1" lx="3.5" ly="3.5" lz="5" roll="${PI/2.0}"/>
-        <xacro:link-collision-cylinder radius="0.75" x1="0" y1="2.3" z1="0" x2="0" y2="2.3" z2="9"/>
-        <xacro:link-collision-box x="-1.6" y="-1.5" z="-2" lx="1" ly="2.5" lz="2.5"/>
-        <xacro:link-inertial z="-2.5" mass="0.5" ixx="2.2" iyy="4" izz="2.2"/>
-    </link> -->
+  <link name="base_link">
+    <xacro:box_inertia m="3.0" w="${base_len}" h="${base_width}" d="${base_height}"/>
+    <xacro:link-visual fileName="chasis.dae" col="red" r="1" g="1" b="1"/>
+    <!--    <collision>-->
+    <!--        <origin rpy="0 0 0" xyz="0 0 -9.5"/>-->
+    <!--        <geometry>-->
+    <!--            <mesh filename="package://robo_impls/urdf/rover.dae"/>-->
+    <!--        </geometry>-->
+    <!--    </collision> -->
+  </link>
+  <link name="a_link">
+    <xacro:link-visual fileName="link.a.dae"
+                       col="white" r="1" g="1" b="1"/>
+    <!--    <xacro:link-collision-cylinder radius="3.75" x1="0" y1="0" z1="-1.25" x2="0" y2="0" z2="-3.75"/>-->
+    <!--    <xacro:link-collision-cylinder radius="1.75" x1="3.6467207" y1="3.6590503" z1="-0.75" x2="3.6467207"-->
+    <!--                                   y2="3.6590503" z2="-2.6913988"/>-->
+    <!--    <xacro:link-inertial z="-2.5" mass="0.5" ixx="2.3" iyy="2.3" izz="3.5"/>-->
+  </link>
+  <link name="b_link">
+    <xacro:link-visual fileName="link.b.dae"
+                       col="orange" r="1" g="0.43" b="0"/>
+  </link>
+  <link name="c_link">
+    <xacro:link-visual fileName="link.c.dae"
+                       col="yellow" r="1" g="1" b="0"/>
+  </link>
+  <link name="d_link">
+    <xacro:link-visual fileName="link.d.dae"
+                       col="green" r="0.43" g="1" b="0"/>
+  </link>
+  <link name="e_link">
+    <xacro:link-visual fileName="link.e.dae"
+                       col="blue" r="0" g="0" b="1"/>
+  </link>
 </robot>
diff --git a/urdf/meshes/arm.a.dae b/urdf/meshes/arm.a.dae
deleted file mode 100644
index 509539a91..000000000
--- a/urdf/meshes/arm.a.dae
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:f2cb97f962fd294157d98fec9c2cfde1519ced082f0761712581c78fce581472
-size 10781736
diff --git a/urdf/meshes/arm.b.dae b/urdf/meshes/arm.b.dae
deleted file mode 100644
index 456fb7fb0..000000000
--- a/urdf/meshes/arm.b.dae
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:d78614ab70cf07e518a3f946560428c065550a9d883b5dba08ecbcd361de4fca
-size 4161111
diff --git a/urdf/meshes/arm.c.dae b/urdf/meshes/arm.c.dae
deleted file mode 100644
index 6dd26a934..000000000
--- a/urdf/meshes/arm.c.dae
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:f7ba6f190eb3583dd4d00c2dc93261b3b0b49b614d0805db9088139808c47c5a
-size 8991259
diff --git a/urdf/meshes/arm.d.dae b/urdf/meshes/arm.d.dae
deleted file mode 100644
index 5dbb55c94..000000000
--- a/urdf/meshes/arm.d.dae
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:e43c792bd73ca2634b7825682267b6336e661123a42059b0a0cae7dd7615a230
-size 1203228
diff --git a/urdf/meshes/arm.dae b/urdf/meshes/arm.dae
new file mode 100644
index 000000000..1c7af67d9
--- /dev/null
+++ b/urdf/meshes/arm.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:48ebba2feeaf3e4acd22c6b46deb6e0cf3237c185b48f142ffc26913e00cdf06
+size 6492
diff --git a/urdf/meshes/arm.e.dae b/urdf/meshes/arm.e.dae
deleted file mode 100644
index fe503b009..000000000
--- a/urdf/meshes/arm.e.dae
+++ /dev/null
@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:9c26f1c637b2547a4c0780e82ebbb66e1a2c1efa9afc1085ba3fbef56acf7b57
-size 3575991
diff --git a/urdf/meshes/chasis.dae b/urdf/meshes/chasis.dae
index 942a38298..a2dd3f711 100644
--- a/urdf/meshes/chasis.dae
+++ b/urdf/meshes/chasis.dae
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:6f7cfcc6e1786b4e1e3591579ab8014e5a0d0f982f3eb9a142df20dc6d0f2e89
-size 5250
+oid sha256:ea6d616980906bf035d90e563685f2aa3c218be2f47b1c8924884922114e81aa
+size 6491
diff --git a/urdf/meshes/link.a.dae b/urdf/meshes/link.a.dae
new file mode 100644
index 000000000..192bf4e09
--- /dev/null
+++ b/urdf/meshes/link.a.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5be28f75de929d3ee54a86985c9d38fc34f8ae3f92ab87b3fe7ae7aef0bee797
+size 10815714
diff --git a/urdf/meshes/link.b.dae b/urdf/meshes/link.b.dae
new file mode 100644
index 000000000..4d93afdbf
--- /dev/null
+++ b/urdf/meshes/link.b.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bfaa73edb0cb66f9298f9b68323386d6a203a49b78e8a2b12f5d349754b74126
+size 4232741
diff --git a/urdf/meshes/link.c.dae b/urdf/meshes/link.c.dae
new file mode 100644
index 000000000..db082c9e6
--- /dev/null
+++ b/urdf/meshes/link.c.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:480da3567990aa3302b406789b5cce05c113e4da0353d79180fb46afb612bc23
+size 9013569
diff --git a/urdf/meshes/link.d.dae b/urdf/meshes/link.d.dae
new file mode 100644
index 000000000..3716bf02a
--- /dev/null
+++ b/urdf/meshes/link.d.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c1c0a38fa9857a76eae2f4eadbd78f0c56e49eb8ee61e4bceb3470b999b6ca4b
+size 1219217
diff --git a/urdf/meshes/link.e.dae b/urdf/meshes/link.e.dae
new file mode 100644
index 000000000..fe9dfbc0c
--- /dev/null
+++ b/urdf/meshes/link.e.dae
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:90ac9bb7d464ca899e8022218b8a647fb5030a63b06d1afeacfe9aba4a7ad106
+size 3614763
diff --git a/urdf/staging/arm.blend b/urdf/staging/arm.blend
index b6b11f249..d27cfb594 100644
--- a/urdf/staging/arm.blend
+++ b/urdf/staging/arm.blend
@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:39fa4e5a34062d9692a13e360b4237f83eb5a8a8ed34494527911ecd7e6c4500
-size 21299380
+oid sha256:afd38aac88bcf414c76519181a80bb72c76edcf8189c54a6e581ba0e4944cd29
+size 21299488
diff --git a/urdf/staging/arm.dae.blend b/urdf/staging/arm.dae.blend
new file mode 100644
index 000000000..151a87bac
--- /dev/null
+++ b/urdf/staging/arm.dae.blend
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:37022548c19e5c9ba79e791e8f0ae06ab91c5e6d8ecb6baa4832951d9f1b8605
+size 1186800