From 5b1fc78d17d64c772ab63bdea6b7dda74adc21fb Mon Sep 17 00:00:00 2001
From: Aaron Edsinger <aedsinger@hello-robot.com>
Date: Tue, 9 Mar 2021 09:47:02 -0800
Subject: [PATCH] fix keyboard teleop for wrist

---
 stretch_core/nodes/keyboard_teleop            |  2 +-
 stretch_core/rviz/stretch_simple_test.rviz    | 42 +++++++++++--------
 .../urdf/stretch_dex_wrist_beta.xacro         |  2 +-
 3 files changed, 27 insertions(+), 19 deletions(-)

diff --git a/stretch_core/nodes/keyboard_teleop b/stretch_core/nodes/keyboard_teleop
index bed322e..51ddcfb 100755
--- a/stretch_core/nodes/keyboard_teleop
+++ b/stretch_core/nodes/keyboard_teleop
@@ -105,7 +105,7 @@ class GetKeyboardCommands:
 
         # dex joints
         if 'joint_wrist_pitch' in self.joints:
-            print('|{:^22}{:^21}|'.format('c PITCH FORWARD', 'v PITCH BACK'))
+            print('|{:^22}{:^21}|'.format('v PITCH FORWARD', 'c PITCH BACK'))
         if 'joint_wrist_roll' in self.joints:
             print('|{:^22}{:^21}|'.format('o ROLL FORWARD', 'p ROLL BACK'))
 
diff --git a/stretch_core/rviz/stretch_simple_test.rviz b/stretch_core/rviz/stretch_simple_test.rviz
index fd6eb98..063cdca 100644
--- a/stretch_core/rviz/stretch_simple_test.rviz
+++ b/stretch_core/rviz/stretch_simple_test.rviz
@@ -8,7 +8,9 @@ Panels:
         - /Status1
         - /RobotModel1
         - /RobotModel1/Links1
+        - /RobotModel1/Links1/link_arm_l01
         - /RobotModel1/Links1/link_gripper_finger_left1
+        - /RobotModel1/Links1/link_gripper_fingertip_right1
       Splitter Ratio: 0.5
     Tree Height: 1484
   - Class: rviz/Selection
@@ -60,7 +62,7 @@ Visualization Manager:
       Collision Enabled: false
       Enabled: true
       Links:
-        All Links Enabled: false
+        All Links Enabled: true
         Expand Joint Details: false
         Expand Link Details: false
         Expand Tree: false
@@ -118,7 +120,7 @@ Visualization Manager:
           Value: true
         link_arm_l0:
           Alpha: 1
-          Show Axes: false
+          Show Axes: true
           Show Trail: false
           Value: true
         link_arm_l1:
@@ -166,15 +168,6 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        link_grasp_center:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        link_gripper:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         link_gripper_finger_left:
           Alpha: 1
           Show Axes: false
@@ -192,7 +185,7 @@ Visualization Manager:
           Value: true
         link_gripper_fingertip_right:
           Alpha: 1
-          Show Axes: false
+          Show Axes: true
           Show Trail: false
           Value: true
         link_head:
@@ -225,12 +218,22 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        link_puller:
+        link_right_wheel:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: false
-        link_right_wheel:
+          Value: true
+        link_straight_gripper:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_pitch:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_roll:
           Alpha: 1
           Show Axes: false
           Show Trail: false
@@ -240,6 +243,11 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        link_wrist_yaw_bottom:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         respeaker_base:
           Alpha: 1
           Show Axes: false
@@ -385,10 +393,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.8447968363761902
+      Pitch: 0.5397967100143433
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 5.8704514503479
+      Yaw: 5.230452060699463
     Saved: ~
 Window Geometry:
   Displays:
diff --git a/stretch_description/urdf/stretch_dex_wrist_beta.xacro b/stretch_description/urdf/stretch_dex_wrist_beta.xacro
index 5484680..d8e786b 100644
--- a/stretch_description/urdf/stretch_dex_wrist_beta.xacro
+++ b/stretch_description/urdf/stretch_dex_wrist_beta.xacro
@@ -108,7 +108,7 @@
       link="link_wrist_pitch" />
     <axis
       xyz="0 0 1" />
-    <limit effort="100" lower="-1.14" upper="3.14" velocity="1.0"/>
+    <limit effort="100" lower="-0.56" upper="1.56" velocity="1.0"/>
   </joint>
   <link
     name="link_wrist_roll">