diff --git a/.gitignore b/.gitignore
index 12557c5..b5da835 100644
--- a/.gitignore
+++ b/.gitignore
@@ -9,3 +9,6 @@ experiments.py
 
 # Gedit stuff
 *~
+
+# Ignore PDFs on master
+literature/
diff --git a/launch/rviz_config/human.rviz b/launch/rviz_config/human.rviz
new file mode 100644
index 0000000..b39c597
--- /dev/null
+++ b/launch/rviz_config/human.rviz
@@ -0,0 +1,438 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Grid1
+        - /Grid1/Offset1
+        - /TF1
+        - /TF1/Frames1
+      Splitter Ratio: 0.5
+    Tree Height: 359
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: odom
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        Aruco_0_frame:
+          Value: true
+        Aruco_1_frame:
+          Value: true
+        Aruco_2_frame:
+          Value: true
+        CameraBottom_frame:
+          Value: false
+        CameraBottom_optical_frame:
+          Value: false
+        CameraTop_frame:
+          Value: false
+        CameraTop_optical_frame:
+          Value: false
+        ChestButton_frame:
+          Value: false
+        Head:
+          Value: false
+        HeadTouchFront_frame:
+          Value: false
+        HeadTouchMiddle_frame:
+          Value: false
+        HeadTouchRear_frame:
+          Value: false
+        ImuTorsoAccelerometer_frame:
+          Value: false
+        ImuTorsoGyrometer_frame:
+          Value: false
+        LAnklePitch:
+          Value: false
+        LBicep:
+          Value: false
+        LElbow:
+          Value: false
+        LFinger11_link:
+          Value: false
+        LFinger12_link:
+          Value: false
+        LFinger13_link:
+          Value: false
+        LFinger21_link:
+          Value: false
+        LFinger22_link:
+          Value: false
+        LFinger23_link:
+          Value: false
+        LFootBumperLeft_frame:
+          Value: false
+        LFootBumperRight_frame:
+          Value: false
+        LForeArm:
+          Value: false
+        LFsrFL_frame:
+          Value: false
+        LFsrFR_frame:
+          Value: false
+        LFsrRL_frame:
+          Value: false
+        LFsrRR_frame:
+          Value: false
+        LHandTouchBack_frame:
+          Value: false
+        LHandTouchLeft_frame:
+          Value: false
+        LHandTouchRight_frame:
+          Value: false
+        LHip:
+          Value: false
+        LInfraRed_frame:
+          Value: false
+        LPelvis:
+          Value: false
+        LShoulder:
+          Value: false
+        LSonar_frame:
+          Value: false
+        LThigh:
+          Value: false
+        LThumb1_link:
+          Value: false
+        LThumb2_link:
+          Value: false
+        LTibia:
+          Value: false
+        MicroFrontCenter_frame:
+          Value: false
+        MicroRearCenter_frame:
+          Value: false
+        MicroSurroundLeft_frame:
+          Value: false
+        MicroSurroundRight_frame:
+          Value: false
+        Neck:
+          Value: false
+        RAnklePitch:
+          Value: false
+        RBicep:
+          Value: false
+        RElbow:
+          Value: false
+        RFinger11_link:
+          Value: false
+        RFinger12_link:
+          Value: false
+        RFinger13_link:
+          Value: false
+        RFinger21_link:
+          Value: false
+        RFinger22_link:
+          Value: false
+        RFinger23_link:
+          Value: false
+        RFootBumperLeft_frame:
+          Value: false
+        RFootBumperRight_frame:
+          Value: false
+        RForeArm:
+          Value: false
+        RFsrFL_frame:
+          Value: false
+        RFsrFR_frame:
+          Value: false
+        RFsrRL_frame:
+          Value: false
+        RFsrRR_frame:
+          Value: false
+        RHandTouchBack_frame:
+          Value: false
+        RHandTouchLeft_frame:
+          Value: false
+        RHandTouchRight_frame:
+          Value: false
+        RHip:
+          Value: false
+        RInfraRed_frame:
+          Value: false
+        RPelvis:
+          Value: false
+        RShoulder:
+          Value: false
+        RSonar_frame:
+          Value: false
+        RThigh:
+          Value: false
+        RThumb1_link:
+          Value: false
+        RThumb2_link:
+          Value: false
+        RTibia:
+          Value: false
+        base_footprint:
+          Value: false
+        base_link:
+          Value: false
+        gaze:
+          Value: false
+        l_ankle:
+          Value: false
+        l_gripper:
+          Value: false
+        l_sole:
+          Value: false
+        l_wrist:
+          Value: false
+        odom:
+          Value: false
+        r_ankle:
+          Value: false
+        r_gripper:
+          Value: false
+        r_sole:
+          Value: false
+        r_wrist:
+          Value: false
+        torso:
+          Value: false
+      Marker Scale: 0.1
+      Name: TF
+      Show Arrows: true
+      Show Axes: false
+      Show Names: true
+      Tree:
+        odom:
+          base_link:
+            base_footprint:
+              {}
+            torso:
+              ChestButton_frame:
+                {}
+              ImuTorsoAccelerometer_frame:
+                {}
+              ImuTorsoGyrometer_frame:
+                {}
+              LPelvis:
+                LHip:
+                  LThigh:
+                    LTibia:
+                      LAnklePitch:
+                        l_ankle:
+                          LFootBumperLeft_frame:
+                            {}
+                          LFootBumperRight_frame:
+                            {}
+                          LFsrFL_frame:
+                            {}
+                          LFsrFR_frame:
+                            {}
+                          LFsrRL_frame:
+                            {}
+                          LFsrRR_frame:
+                            {}
+                          l_sole:
+                            {}
+              LShoulder:
+                LBicep:
+                  LElbow:
+                    LForeArm:
+                      l_wrist:
+                        LFinger11_link:
+                          LFinger12_link:
+                            LFinger13_link:
+                              {}
+                        LFinger21_link:
+                          LFinger22_link:
+                            LFinger23_link:
+                              {}
+                        LHandTouchBack_frame:
+                          {}
+                        LHandTouchLeft_frame:
+                          {}
+                        LHandTouchRight_frame:
+                          {}
+                        LThumb1_link:
+                          LThumb2_link:
+                            {}
+                        l_gripper:
+                          {}
+              LSonar_frame:
+                {}
+              Neck:
+                Head:
+                  CameraBottom_frame:
+                    CameraBottom_optical_frame:
+                      {}
+                  CameraTop_frame:
+                    CameraTop_optical_frame:
+                      Aruco_0_frame:
+                        {}
+                      Aruco_1_frame:
+                        {}
+                      Aruco_2_frame:
+                        {}
+                  HeadTouchFront_frame:
+                    {}
+                  HeadTouchMiddle_frame:
+                    {}
+                  HeadTouchRear_frame:
+                    {}
+                  LInfraRed_frame:
+                    {}
+                  MicroFrontCenter_frame:
+                    {}
+                  MicroRearCenter_frame:
+                    {}
+                  MicroSurroundLeft_frame:
+                    {}
+                  MicroSurroundRight_frame:
+                    {}
+                  RInfraRed_frame:
+                    {}
+                  gaze:
+                    {}
+              RPelvis:
+                RHip:
+                  RThigh:
+                    RTibia:
+                      RAnklePitch:
+                        r_ankle:
+                          RFootBumperLeft_frame:
+                            {}
+                          RFootBumperRight_frame:
+                            {}
+                          RFsrFL_frame:
+                            {}
+                          RFsrFR_frame:
+                            {}
+                          RFsrRL_frame:
+                            {}
+                          RFsrRR_frame:
+                            {}
+                          r_sole:
+                            {}
+              RShoulder:
+                RBicep:
+                  RElbow:
+                    RForeArm:
+                      r_wrist:
+                        RFinger11_link:
+                          RFinger12_link:
+                            RFinger13_link:
+                              {}
+                        RFinger21_link:
+                          RFinger22_link:
+                            RFinger23_link:
+                              {}
+                        RHandTouchBack_frame:
+                          {}
+                        RHandTouchLeft_frame:
+                          {}
+                        RHandTouchRight_frame:
+                          {}
+                        RThumb1_link:
+                          RThumb2_link:
+                            {}
+                        r_gripper:
+                          {}
+              RSonar_frame:
+                {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: torso
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.25714
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.280398
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 6.11041
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 663
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000279fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000000000000279000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002f60000003efc0100000002fb0000000800540069006d00650000000000000002f6000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002f80000027900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 760
+  X: 1015
+  Y: 14
diff --git a/launch/rviz_config/nao.rviz b/launch/rviz_config/nao.rviz
new file mode 100644
index 0000000..585da34
--- /dev/null
+++ b/launch/rviz_config/nao.rviz
@@ -0,0 +1,426 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Grid1
+        - /Grid1/Offset1
+        - /TF1
+      Splitter Ratio: 0.5
+    Tree Height: 286
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: odom
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        CameraBottom_frame:
+          Value: true
+        CameraBottom_optical_frame:
+          Value: true
+        CameraTop_frame:
+          Value: true
+        CameraTop_optical_frame:
+          Value: true
+        ChestButton_frame:
+          Value: true
+        Head:
+          Value: true
+        HeadTouchFront_frame:
+          Value: true
+        HeadTouchMiddle_frame:
+          Value: true
+        HeadTouchRear_frame:
+          Value: true
+        ImuTorsoAccelerometer_frame:
+          Value: true
+        ImuTorsoGyrometer_frame:
+          Value: true
+        LAnklePitch:
+          Value: true
+        LBicep:
+          Value: true
+        LElbow:
+          Value: true
+        LFinger11_link:
+          Value: true
+        LFinger12_link:
+          Value: true
+        LFinger13_link:
+          Value: true
+        LFinger21_link:
+          Value: true
+        LFinger22_link:
+          Value: true
+        LFinger23_link:
+          Value: true
+        LFootBumperLeft_frame:
+          Value: true
+        LFootBumperRight_frame:
+          Value: true
+        LForeArm:
+          Value: true
+        LFsrFL_frame:
+          Value: true
+        LFsrFR_frame:
+          Value: true
+        LFsrRL_frame:
+          Value: true
+        LFsrRR_frame:
+          Value: true
+        LHandTouchBack_frame:
+          Value: true
+        LHandTouchLeft_frame:
+          Value: true
+        LHandTouchRight_frame:
+          Value: true
+        LHip:
+          Value: true
+        LInfraRed_frame:
+          Value: true
+        LPelvis:
+          Value: true
+        LShoulder:
+          Value: true
+        LSonar_frame:
+          Value: true
+        LThigh:
+          Value: true
+        LThumb1_link:
+          Value: true
+        LThumb2_link:
+          Value: true
+        LTibia:
+          Value: true
+        MicroFrontCenter_frame:
+          Value: true
+        MicroRearCenter_frame:
+          Value: true
+        MicroSurroundLeft_frame:
+          Value: true
+        MicroSurroundRight_frame:
+          Value: true
+        Neck:
+          Value: true
+        RAnklePitch:
+          Value: true
+        RBicep:
+          Value: true
+        RElbow:
+          Value: true
+        RFinger11_link:
+          Value: true
+        RFinger12_link:
+          Value: true
+        RFinger13_link:
+          Value: true
+        RFinger21_link:
+          Value: true
+        RFinger22_link:
+          Value: true
+        RFinger23_link:
+          Value: true
+        RFootBumperLeft_frame:
+          Value: true
+        RFootBumperRight_frame:
+          Value: true
+        RForeArm:
+          Value: true
+        RFsrFL_frame:
+          Value: true
+        RFsrFR_frame:
+          Value: true
+        RFsrRL_frame:
+          Value: true
+        RFsrRR_frame:
+          Value: true
+        RHandTouchBack_frame:
+          Value: true
+        RHandTouchLeft_frame:
+          Value: true
+        RHandTouchRight_frame:
+          Value: true
+        RHip:
+          Value: true
+        RInfraRed_frame:
+          Value: true
+        RPelvis:
+          Value: true
+        RShoulder:
+          Value: true
+        RSonar_frame:
+          Value: true
+        RThigh:
+          Value: true
+        RThumb1_link:
+          Value: true
+        RThumb2_link:
+          Value: true
+        RTibia:
+          Value: true
+        base_footprint:
+          Value: true
+        base_link:
+          Value: true
+        gaze:
+          Value: true
+        l_ankle:
+          Value: true
+        l_gripper:
+          Value: true
+        l_sole:
+          Value: true
+        l_wrist:
+          Value: true
+        odom:
+          Value: true
+        r_ankle:
+          Value: true
+        r_gripper:
+          Value: true
+        r_sole:
+          Value: true
+        r_wrist:
+          Value: true
+        torso:
+          Value: true
+      Marker Scale: 0.1
+      Name: TF
+      Show Arrows: true
+      Show Axes: false
+      Show Names: false
+      Tree:
+        odom:
+          base_link:
+            base_footprint:
+              {}
+            torso:
+              ChestButton_frame:
+                {}
+              ImuTorsoAccelerometer_frame:
+                {}
+              ImuTorsoGyrometer_frame:
+                {}
+              LPelvis:
+                LHip:
+                  LThigh:
+                    LTibia:
+                      LAnklePitch:
+                        l_ankle:
+                          LFootBumperLeft_frame:
+                            {}
+                          LFootBumperRight_frame:
+                            {}
+                          LFsrFL_frame:
+                            {}
+                          LFsrFR_frame:
+                            {}
+                          LFsrRL_frame:
+                            {}
+                          LFsrRR_frame:
+                            {}
+                          l_sole:
+                            {}
+              LShoulder:
+                LBicep:
+                  LElbow:
+                    LForeArm:
+                      l_wrist:
+                        LFinger11_link:
+                          LFinger12_link:
+                            LFinger13_link:
+                              {}
+                        LFinger21_link:
+                          LFinger22_link:
+                            LFinger23_link:
+                              {}
+                        LHandTouchBack_frame:
+                          {}
+                        LHandTouchLeft_frame:
+                          {}
+                        LHandTouchRight_frame:
+                          {}
+                        LThumb1_link:
+                          LThumb2_link:
+                            {}
+                        l_gripper:
+                          {}
+              LSonar_frame:
+                {}
+              Neck:
+                Head:
+                  CameraBottom_frame:
+                    CameraBottom_optical_frame:
+                      {}
+                  CameraTop_frame:
+                    CameraTop_optical_frame:
+                      {}
+                  HeadTouchFront_frame:
+                    {}
+                  HeadTouchMiddle_frame:
+                    {}
+                  HeadTouchRear_frame:
+                    {}
+                  LInfraRed_frame:
+                    {}
+                  MicroFrontCenter_frame:
+                    {}
+                  MicroRearCenter_frame:
+                    {}
+                  MicroSurroundLeft_frame:
+                    {}
+                  MicroSurroundRight_frame:
+                    {}
+                  RInfraRed_frame:
+                    {}
+                  gaze:
+                    {}
+              RPelvis:
+                RHip:
+                  RThigh:
+                    RTibia:
+                      RAnklePitch:
+                        r_ankle:
+                          RFootBumperLeft_frame:
+                            {}
+                          RFootBumperRight_frame:
+                            {}
+                          RFsrFL_frame:
+                            {}
+                          RFsrFR_frame:
+                            {}
+                          RFsrRL_frame:
+                            {}
+                          RFsrRR_frame:
+                            {}
+                          r_sole:
+                            {}
+              RShoulder:
+                RBicep:
+                  RElbow:
+                    RForeArm:
+                      r_wrist:
+                        RFinger11_link:
+                          RFinger12_link:
+                            RFinger13_link:
+                              {}
+                        RFinger21_link:
+                          RFinger22_link:
+                            RFinger23_link:
+                              {}
+                        RHandTouchBack_frame:
+                          {}
+                        RHandTouchLeft_frame:
+                          {}
+                        RHandTouchRight_frame:
+                          {}
+                        RThumb1_link:
+                          RThumb2_link:
+                            {}
+                        r_gripper:
+                          {}
+              RSonar_frame:
+                {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: torso
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.11097
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.300398
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.1404
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 459
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000001adfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000000000001ad000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002f60000003efc0100000002fb0000000800540069006d00650000000000000002f6000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000019b000001ad00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 411
+  X: 55
+  Y: 14
diff --git a/launch/rviz_config/nao2.rviz b/launch/rviz_config/nao2.rviz
new file mode 100644
index 0000000..30fef26
--- /dev/null
+++ b/launch/rviz_config/nao2.rviz
@@ -0,0 +1,767 @@
+anels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Grid1/Offset1
+        - /Sonar_Left1
+        - /Sonar_Right1
+        - /Image1
+        - /Imu1
+      Splitter Ratio: 0.476667
+    Tree Height: 517
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: true
+    Name: Time
+    SyncMode: 2
+    SyncSource: Image
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 0.1
+      Class: rviz/Grid
+      Color: 141; 141; 141
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0.5
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 36
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        CameraBottom_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        CameraBottom_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        CameraTop_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        CameraTop_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ChestButton_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Head:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        HeadTouchFront_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        HeadTouchMiddle_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        HeadTouchRear_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ImuTorsoAccelerometer_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        ImuTorsoGyrometer_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        LAnklePitch:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LBicep:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LElbow:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        LFinger11_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LFinger12_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LFinger13_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LFinger21_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LFinger22_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LFinger23_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LFootBumperLeft_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        LFootBumperRight_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        LForeArm:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LFsrFL_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        LFsrFR_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        LFsrRL_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        LFsrRR_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        LHandTouchBack_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        LHandTouchLeft_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        LHandTouchRight_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        LHip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LInfraRed_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        LPelvis:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LShoulder:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LSonar_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        LThigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LThumb1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LThumb2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        LTibia:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        Link Tree Style: Links in Alphabetic Order
+        Neck:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RAnklePitch:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RBicep:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RElbow:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        RFinger11_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RFinger12_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RFinger13_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RFinger21_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RFinger22_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RFinger23_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RFootBumperLeft_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        RFootBumperRight_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        RForeArm:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RFsrFL_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        RFsrFR_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        RFsrRL_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        RFsrRR_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        RHandTouchBack_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        RHandTouchLeft_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        RHandTouchRight_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        RHip:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RInfraRed_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        RPelvis:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RShoulder:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RSonar_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        RThigh:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RThumb1_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RThumb2_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        RTibia:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        gaze:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        l_ankle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        l_gripper:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        l_sole:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        l_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        r_ankle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        r_gripper:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        r_sole:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        r_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        torso:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 1500
+      Frames:
+        All Enabled: false
+        CameraBottom_frame:
+          Value: true
+        CameraBottom_optical_frame:
+          Value: true
+        CameraTop_frame:
+          Value: true
+        CameraTop_optical_frame:
+          Value: true
+        ChestButton_frame:
+          Value: true
+        Head:
+          Value: true
+        HeadTouchFront_frame:
+          Value: true
+        HeadTouchMiddle_frame:
+          Value: true
+        HeadTouchRear_frame:
+          Value: true
+        ImuTorsoAccelerometer_frame:
+          Value: true
+        ImuTorsoGyrometer_frame:
+          Value: true
+        LAnklePitch:
+          Value: true
+        LBicep:
+          Value: true
+        LElbow:
+          Value: true
+        LFootBumperLeft_frame:
+          Value: true
+        LFootBumperRight_frame:
+          Value: true
+        LForeArm:
+          Value: true
+        LFsrFL_frame:
+          Value: true
+        LFsrFR_frame:
+          Value: true
+        LFsrRL_frame:
+          Value: true
+        LFsrRR_frame:
+          Value: true
+        LHandTouchBack_frame:
+          Value: true
+        LHandTouchLeft_frame:
+          Value: true
+        LHandTouchRight_frame:
+          Value: true
+        LHip:
+          Value: true
+        LInfraRed_frame:
+          Value: true
+        LPelvis:
+          Value: true
+        LShoulder:
+          Value: true
+        LSonar_frame:
+          Value: true
+        LThigh:
+          Value: true
+        LTibia:
+          Value: true
+        Neck:
+          Value: true
+        RAnklePitch:
+          Value: true
+        RBicep:
+          Value: true
+        RElbow:
+          Value: true
+        RFootBumperLeft_frame:
+          Value: true
+        RFootBumperRight_frame:
+          Value: true
+        RForeArm:
+          Value: true
+        RFsrFL_frame:
+          Value: true
+        RFsrFR_frame:
+          Value: true
+        RFsrRL_frame:
+          Value: true
+        RFsrRR_frame:
+          Value: true
+        RHandTouchBack_frame:
+          Value: true
+        RHandTouchLeft_frame:
+          Value: true
+        RHandTouchRight_frame:
+          Value: true
+        RHip:
+          Value: true
+        RInfraRed_frame:
+          Value: true
+        RPelvis:
+          Value: true
+        RShoulder:
+          Value: true
+        RSonar_frame:
+          Value: true
+        RThigh:
+          Value: true
+        RTibia:
+          Value: true
+        base_footprint:
+          Value: true
+        base_link:
+          Value: true
+        gaze:
+          Value: true
+        l_ankle:
+          Value: true
+        l_gripper:
+          Value: true
+        l_sole:
+          Value: true
+        l_wrist:
+          Value: true
+        odom:
+          Value: true
+        r_ankle:
+          Value: true
+        r_gripper:
+          Value: true
+        r_sole:
+          Value: true
+        r_wrist:
+          Value: true
+        torso:
+          Value: true
+      Marker Scale: 0.1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        odom:
+          base_link:
+            base_footprint:
+              {}
+            torso:
+              ChestButton_frame:
+                {}
+              ImuTorsoAccelerometer_frame:
+                {}
+              ImuTorsoGyrometer_frame:
+                {}
+              LPelvis:
+                LHip:
+                  LThigh:
+                    LTibia:
+                      LAnklePitch:
+                        l_ankle:
+                          LFootBumperLeft_frame:
+                            {}
+                          LFootBumperRight_frame:
+                            {}
+                          LFsrFL_frame:
+                            {}
+                          LFsrFR_frame:
+                            {}
+                          LFsrRL_frame:
+                            {}
+                          LFsrRR_frame:
+                            {}
+                          l_sole:
+                            {}
+              LShoulder:
+                LBicep:
+                  LElbow:
+                    LForeArm:
+                      l_wrist:
+                        LHandTouchBack_frame:
+                          {}
+                        LHandTouchLeft_frame:
+                          {}
+                        LHandTouchRight_frame:
+                          {}
+                        l_gripper:
+                          {}
+              LSonar_frame:
+                {}
+              Neck:
+                Head:
+                  CameraBottom_frame:
+                    CameraBottom_optical_frame:
+                      {}
+                  CameraTop_frame:
+                    CameraTop_optical_frame:
+                      {}
+                  HeadTouchFront_frame:
+                    {}
+                  HeadTouchMiddle_frame:
+                    {}
+                  HeadTouchRear_frame:
+                    {}
+                  LInfraRed_frame:
+                    {}
+                  RInfraRed_frame:
+                    {}
+                  gaze:
+                    {}
+              RPelvis:
+                RHip:
+                  RThigh:
+                    RTibia:
+                      RAnklePitch:
+                        r_ankle:
+                          RFootBumperLeft_frame:
+                            {}
+                          RFootBumperRight_frame:
+                            {}
+                          RFsrFL_frame:
+                            {}
+                          RFsrFR_frame:
+                            {}
+                          RFsrRL_frame:
+                            {}
+                          RFsrRR_frame:
+                            {}
+                          r_sole:
+                            {}
+              RShoulder:
+                RBicep:
+                  RElbow:
+                    RForeArm:
+                      r_wrist:
+                        RHandTouchBack_frame:
+                          {}
+                        RHandTouchLeft_frame:
+                          {}
+                        RHandTouchRight_frame:
+                          {}
+                        r_gripper:
+                          {}
+              RSonar_frame:
+                {}
+      Update Interval: 0
+      Value: true
+    - Alpha: 0.5
+      Buffer Length: 1
+      Class: rviz/Range
+      Color: 255; 255; 255
+      Enabled: true
+      Name: Sonar_Left
+      Queue Size: 100
+      Topic: /nao_robot/sonar/left
+      Value: true
+    - Alpha: 0.5
+      Buffer Length: 1
+      Class: rviz/Range
+      Color: 255; 255; 255
+      Enabled: true
+      Name: Sonar_Right
+      Queue Size: 100
+      Topic: /nao_robot/sonar/right
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /nao_robot/camera/front/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Value: true
+    - Alpha: 0.2
+      Class: rviz_plugin_tutorials/Imu
+      Color: 204; 51; 204
+      Enabled: true
+      History Length: 1
+      Name: Imu
+      Topic: /nao_robot/imu/torso
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 170; 170; 170
+    Fixed Frame: base_footprint
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/XYOrbit
+      Distance: 1.29287
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.411995
+        Y: 0.032867
+        Z: -1.41923e-06
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.129797
+      Target Frame: <Fixed Frame>
+      Value: XYOrbit (rviz)
+      Yaw: 6.23044
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1059
+  Hide Left Dock: false
+  Hide Right Dock: true
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000002830000038ffc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003600000290000000b700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002cc000000f90000001600fffffffb0000000c00430061006d00650072006101000003af000000160000000000000000fb0000000a0049006d00610067006501000001ea000001db0000000000000000000000010000010f0000038ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000360000038f0000009b00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000003efc0100000002fb0000000800540069006d006501000000000000077e000002c700fffffffb0000000800540069006d00650100000000000004500000000000000000000004f50000038f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1918
+  X: 1920
+Y: 24
diff --git a/launch/rviz_config/nao_right.rviz b/launch/rviz_config/nao_right.rviz
new file mode 100644
index 0000000..3bf48ab
--- /dev/null
+++ b/launch/rviz_config/nao_right.rviz
@@ -0,0 +1,426 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Grid1
+        - /Grid1/Offset1
+        - /TF1
+      Splitter Ratio: 0.5
+    Tree Height: 285
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: odom
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        CameraBottom_frame:
+          Value: true
+        CameraBottom_optical_frame:
+          Value: true
+        CameraTop_frame:
+          Value: true
+        CameraTop_optical_frame:
+          Value: true
+        ChestButton_frame:
+          Value: true
+        Head:
+          Value: true
+        HeadTouchFront_frame:
+          Value: true
+        HeadTouchMiddle_frame:
+          Value: true
+        HeadTouchRear_frame:
+          Value: true
+        ImuTorsoAccelerometer_frame:
+          Value: true
+        ImuTorsoGyrometer_frame:
+          Value: true
+        LAnklePitch:
+          Value: true
+        LBicep:
+          Value: true
+        LElbow:
+          Value: true
+        LFinger11_link:
+          Value: true
+        LFinger12_link:
+          Value: true
+        LFinger13_link:
+          Value: true
+        LFinger21_link:
+          Value: true
+        LFinger22_link:
+          Value: true
+        LFinger23_link:
+          Value: true
+        LFootBumperLeft_frame:
+          Value: true
+        LFootBumperRight_frame:
+          Value: true
+        LForeArm:
+          Value: true
+        LFsrFL_frame:
+          Value: true
+        LFsrFR_frame:
+          Value: true
+        LFsrRL_frame:
+          Value: true
+        LFsrRR_frame:
+          Value: true
+        LHandTouchBack_frame:
+          Value: true
+        LHandTouchLeft_frame:
+          Value: true
+        LHandTouchRight_frame:
+          Value: true
+        LHip:
+          Value: true
+        LInfraRed_frame:
+          Value: true
+        LPelvis:
+          Value: true
+        LShoulder:
+          Value: true
+        LSonar_frame:
+          Value: true
+        LThigh:
+          Value: true
+        LThumb1_link:
+          Value: true
+        LThumb2_link:
+          Value: true
+        LTibia:
+          Value: true
+        MicroFrontCenter_frame:
+          Value: true
+        MicroRearCenter_frame:
+          Value: true
+        MicroSurroundLeft_frame:
+          Value: true
+        MicroSurroundRight_frame:
+          Value: true
+        Neck:
+          Value: true
+        RAnklePitch:
+          Value: true
+        RBicep:
+          Value: true
+        RElbow:
+          Value: true
+        RFinger11_link:
+          Value: true
+        RFinger12_link:
+          Value: true
+        RFinger13_link:
+          Value: true
+        RFinger21_link:
+          Value: true
+        RFinger22_link:
+          Value: true
+        RFinger23_link:
+          Value: true
+        RFootBumperLeft_frame:
+          Value: true
+        RFootBumperRight_frame:
+          Value: true
+        RForeArm:
+          Value: true
+        RFsrFL_frame:
+          Value: true
+        RFsrFR_frame:
+          Value: true
+        RFsrRL_frame:
+          Value: true
+        RFsrRR_frame:
+          Value: true
+        RHandTouchBack_frame:
+          Value: true
+        RHandTouchLeft_frame:
+          Value: true
+        RHandTouchRight_frame:
+          Value: true
+        RHip:
+          Value: true
+        RInfraRed_frame:
+          Value: true
+        RPelvis:
+          Value: true
+        RShoulder:
+          Value: true
+        RSonar_frame:
+          Value: true
+        RThigh:
+          Value: true
+        RThumb1_link:
+          Value: true
+        RThumb2_link:
+          Value: true
+        RTibia:
+          Value: true
+        base_footprint:
+          Value: true
+        base_link:
+          Value: true
+        gaze:
+          Value: true
+        l_ankle:
+          Value: true
+        l_gripper:
+          Value: true
+        l_sole:
+          Value: true
+        l_wrist:
+          Value: true
+        odom:
+          Value: true
+        r_ankle:
+          Value: true
+        r_gripper:
+          Value: true
+        r_sole:
+          Value: true
+        r_wrist:
+          Value: true
+        torso:
+          Value: true
+      Marker Scale: 0.1
+      Name: TF
+      Show Arrows: true
+      Show Axes: false
+      Show Names: false
+      Tree:
+        odom:
+          base_link:
+            base_footprint:
+              {}
+            torso:
+              ChestButton_frame:
+                {}
+              ImuTorsoAccelerometer_frame:
+                {}
+              ImuTorsoGyrometer_frame:
+                {}
+              LPelvis:
+                LHip:
+                  LThigh:
+                    LTibia:
+                      LAnklePitch:
+                        l_ankle:
+                          LFootBumperLeft_frame:
+                            {}
+                          LFootBumperRight_frame:
+                            {}
+                          LFsrFL_frame:
+                            {}
+                          LFsrFR_frame:
+                            {}
+                          LFsrRL_frame:
+                            {}
+                          LFsrRR_frame:
+                            {}
+                          l_sole:
+                            {}
+              LShoulder:
+                LBicep:
+                  LElbow:
+                    LForeArm:
+                      l_wrist:
+                        LFinger11_link:
+                          LFinger12_link:
+                            LFinger13_link:
+                              {}
+                        LFinger21_link:
+                          LFinger22_link:
+                            LFinger23_link:
+                              {}
+                        LHandTouchBack_frame:
+                          {}
+                        LHandTouchLeft_frame:
+                          {}
+                        LHandTouchRight_frame:
+                          {}
+                        LThumb1_link:
+                          LThumb2_link:
+                            {}
+                        l_gripper:
+                          {}
+              LSonar_frame:
+                {}
+              Neck:
+                Head:
+                  CameraBottom_frame:
+                    CameraBottom_optical_frame:
+                      {}
+                  CameraTop_frame:
+                    CameraTop_optical_frame:
+                      {}
+                  HeadTouchFront_frame:
+                    {}
+                  HeadTouchMiddle_frame:
+                    {}
+                  HeadTouchRear_frame:
+                    {}
+                  LInfraRed_frame:
+                    {}
+                  MicroFrontCenter_frame:
+                    {}
+                  MicroRearCenter_frame:
+                    {}
+                  MicroSurroundLeft_frame:
+                    {}
+                  MicroSurroundRight_frame:
+                    {}
+                  RInfraRed_frame:
+                    {}
+                  gaze:
+                    {}
+              RPelvis:
+                RHip:
+                  RThigh:
+                    RTibia:
+                      RAnklePitch:
+                        r_ankle:
+                          RFootBumperLeft_frame:
+                            {}
+                          RFootBumperRight_frame:
+                            {}
+                          RFsrFL_frame:
+                            {}
+                          RFsrFR_frame:
+                            {}
+                          RFsrRL_frame:
+                            {}
+                          RFsrRR_frame:
+                            {}
+                          r_sole:
+                            {}
+              RShoulder:
+                RBicep:
+                  RElbow:
+                    RForeArm:
+                      r_wrist:
+                        RFinger11_link:
+                          RFinger12_link:
+                            RFinger13_link:
+                              {}
+                        RFinger21_link:
+                          RFinger22_link:
+                            RFinger23_link:
+                              {}
+                        RHandTouchBack_frame:
+                          {}
+                        RHandTouchLeft_frame:
+                          {}
+                        RHandTouchRight_frame:
+                          {}
+                        RThumb1_link:
+                          RThumb2_link:
+                            {}
+                        r_gripper:
+                          {}
+              RSonar_frame:
+                {}
+      Update Interval: 0
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: torso
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.1627
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: -0.019602
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.77541
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 458
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000001acfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000000000001ac000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002f60000003efc0100000002fb0000000800540069006d00650000000000000002f6000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000190000001ac00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 400
+  X: 457
+  Y: 14
diff --git a/launch/teleoperation.launch b/launch/teleoperation.launch
index 8d8ae54..73bfaa2 100644
--- a/launch/teleoperation.launch
+++ b/launch/teleoperation.launch
@@ -1,32 +1,31 @@
 <?xml version="1.0"?>
 
-<!--
-Launch file for Teleoperating NAO project
-
--->
-
+<!--Launch file for Teleoperating NAO project-->
 
 <launch>
-
-    <!-- roscore is automatically started if none exists-->
-
     <!-- condition for rviz to be started at launch-->
     <?ignore
     <arg name="gui" default="false" />
     <param name="use_gui" value="$(arg gui)"/>
-    ?>
 
     <!-- aruco detector node-->
-    <node pkg="teleoperation" type="aruco_detector" name="aruco_detector_node"/>
+    <node name="aruco_detector" pkg="teleoperation" type="aruco_detector"/>
 
     <!-- aruco effector node, currently commented out-->
     <?ignore
-    <node pkg="teleoperation" type="aruco_effector" name="aruco_detector_node"/>
+    <node name="aruco_effector" pkg="teleoperation" type="aruco_effector"/>
     ?>
 
     <!-- speech recognition and response node-->
-    <node pkg="teleoperation" type="speech" name="speech_node"/>
+    <node name="speech" pkg="teleoperation" type="speech"/>
 
-    <node name="rviz" pkg="rviz" type="rviz"  required="true"/>
+    <!-- open rviz window showing nao -->
+    <node name="rviz_back" pkg="rviz" type="rviz" args = " -d $(find teleoperation)/launch/rviz_config/nao.rviz"/>
+
+    <!-- open rviz window showing nao from its right side -->
+    <node name="rviz_right" pkg="rviz" type="rviz" args = " -d $(find teleoperation)/launch/rviz_config/nao_right.rviz"/>
+
+    <!-- open rviz window showing human -->
+    <node name="rviz_human" pkg="rviz" type="rviz" args = " -d $(find teleoperation)/launch/rviz_config/human.rviz"/>
 
 </launch>
diff --git a/scripts/teleoperation.sh b/scripts/teleoperation.sh
index 01f6268..83616a2 100755
--- a/scripts/teleoperation.sh
+++ b/scripts/teleoperation.sh
@@ -8,11 +8,11 @@ commands=(
     # Start roscore
     'cd ~/catkin_ws;source devel/setup.bash ;roscore'
     # Bringup Nao
-    'sleep 1;cd ~/catkin_ws;source devel/setup.bash ;roslaunch nao_bringup nao_full.launch '
+    'sleep 1;cd ~/catkin_ws;source devel/setup.bash ;roslaunch nao_bringup nao_full.launch'
     # Start speech recognition server
-    'sleep 3;cd ~/catkin_ws;source devel/setup.bash ;roslaunch nao_apps speech.launch'
+    'sleep 4;cd ~/catkin_ws;source devel/setup.bash ;roslaunch nao_apps speech.launch'
     # Launch nodes in teleoperation package
-    'sleep 5;cd ~/catkin_ws;source devel/setup.bash ;roslaunch teleoperation teleoperation.launch'
+    'sleep 4;cd ~/catkin_ws;source devel/setup.bash ;roslaunch teleoperation teleoperation.launch'
 )
 
 # Build final command with all the tabs to launch
diff --git a/src/aruco_detector.cpp b/src/aruco_detector.cpp
index 3743413..e7a60b6 100644
--- a/src/aruco_detector.cpp
+++ b/src/aruco_detector.cpp
@@ -11,7 +11,6 @@
 #include <opencv2/video/tracking.hpp>
 #include <aruco/aruco.h>
 
-// static const std::string OPENCV_WINDOW = "Original image";
 static const std::string ARUCO_WINDOW = "Aruco window";
 
 class ImageConverter {
@@ -31,13 +30,11 @@ class ImageConverter {
                 &ImageConverter::imageCb, this);
 
         // Create output windows
-        // cv::namedWindow(OPENCV_WINDOW);
         cv::namedWindow(ARUCO_WINDOW);
     }
 
     ~ImageConverter() {
         // Clean Up
-        // cv::destroyWindow(OPENCV_WINDOW);
         cv::destroyWindow(ARUCO_WINDOW);
     }
 
@@ -55,9 +52,6 @@ class ImageConverter {
             return;
         }
 
-        // Output Original Image (bcs y not)
-        // cv::imshow(OPENCV_WINDOW, cv_ptr->image);
-
         // Do Aruco
         cv::Mat cam_mat = (cv::Mat_<double>(3, 3) <<
                 690., 0.0, 320,
diff --git a/src/speech.cpp b/src/speech.cpp
index 77add51..4e6fc8c 100644
--- a/src/speech.cpp
+++ b/src/speech.cpp
@@ -182,8 +182,9 @@ public:
     void commandRecognition()
     {
         //recognition has to be started and ended once a valid command was found
-
-        initializeVocabulary();
+        this->initializeVocabulary();
+        ros::Rate loop_rate(1);
+        loop_rate.sleep();
         std_srvs::Empty srv;
 
         if (recog_start_srv.call(srv)) {
@@ -199,7 +200,8 @@ int main(int argc, char** argv) {
     ros::init(argc, argv, "speech");
 
     Nao_control TermiNAOtor;
-    ros::Rate loop_rate(0.5);
+    //TermiNAOtor.initializeVocabulary();
+    ros::Rate loop_rate(1);
     loop_rate.sleep();
     TermiNAOtor.commandRecognition();
     ROS_INFO("SPIN");