From 92f942b7ee6ba7ac7ca6e0108cfa0c2fd840dd08 Mon Sep 17 00:00:00 2001
From: Aaron Edsinger <aedsinger@hello-robot.com>
Date: Wed, 10 Mar 2021 09:29:23 -0800
Subject: [PATCH] ready for j2 test of dex_wrist

---
 .../nodes/collect_head_calibration_data       |  3 ++-
 stretch_core/nodes/keyboard_teleop            |  2 +-
 stretch_core/rviz/stretch_simple_test.rviz    | 22 +++++++++----------
 .../urdf/stretch_dex_wrist_beta.xacro         |  4 ++--
 4 files changed, 16 insertions(+), 15 deletions(-)

diff --git a/stretch_calibration/nodes/collect_head_calibration_data b/stretch_calibration/nodes/collect_head_calibration_data
index cbdf9c2..572a31e 100755
--- a/stretch_calibration/nodes/collect_head_calibration_data
+++ b/stretch_calibration/nodes/collect_head_calibration_data
@@ -589,7 +589,8 @@ class CollectHeadCalibrationDataNode:
             'joint_head_tilt': -0.4}
         initial_pose = {}
         for j in self.joints:
-            initial_pose[j] = initial_pose_map[j]
+            if initial_pose_map.has_key(j):
+                initial_pose[j] = initial_pose_map[j]
 
         rospy.loginfo('Move to the calibration start pose.')
         self.move_to_pose(initial_pose)
diff --git a/stretch_core/nodes/keyboard_teleop b/stretch_core/nodes/keyboard_teleop
index 51ddcfb..bed322e 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('v PITCH FORWARD', 'c PITCH BACK'))
+            print('|{:^22}{:^21}|'.format('c PITCH FORWARD', 'v 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 063cdca..823110b 100644
--- a/stretch_core/rviz/stretch_simple_test.rviz
+++ b/stretch_core/rviz/stretch_simple_test.rviz
@@ -9,8 +9,8 @@ Panels:
         - /RobotModel1
         - /RobotModel1/Links1
         - /RobotModel1/Links1/link_arm_l01
-        - /RobotModel1/Links1/link_gripper_finger_left1
-        - /RobotModel1/Links1/link_gripper_fingertip_right1
+        - /RobotModel1/Links1/link_wrist_pitch1
+        - /RobotModel1/Links1/link_wrist_yaw_bottom1
       Splitter Ratio: 0.5
     Tree Height: 1484
   - Class: rviz/Selection
@@ -120,7 +120,7 @@ Visualization Manager:
           Value: true
         link_arm_l0:
           Alpha: 1
-          Show Axes: true
+          Show Axes: false
           Show Trail: false
           Value: true
         link_arm_l1:
@@ -185,7 +185,7 @@ Visualization Manager:
           Value: true
         link_gripper_fingertip_right:
           Alpha: 1
-          Show Axes: true
+          Show Axes: false
           Show Trail: false
           Value: true
         link_head:
@@ -230,7 +230,7 @@ Visualization Manager:
           Value: true
         link_wrist_pitch:
           Alpha: 1
-          Show Axes: false
+          Show Axes: true
           Show Trail: false
           Value: true
         link_wrist_roll:
@@ -378,25 +378,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 0.9523301124572754
+      Distance: 1.160446047782898
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.060830965638160706
-        Y: -0.08009408414363861
-        Z: 0.6235455274581909
+        X: -0.09076250344514847
+        Y: -0.09887726604938507
+        Z: 0.6882548928260803
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.5397967100143433
+      Pitch: 0.4297966957092285
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 5.230452060699463
+      Yaw: 5.720452308654785
     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 d8e786b..db2df9e 100644
--- a/stretch_description/urdf/stretch_dex_wrist_beta.xacro
+++ b/stretch_description/urdf/stretch_dex_wrist_beta.xacro
@@ -107,8 +107,8 @@
     <child
       link="link_wrist_pitch" />
     <axis
-      xyz="0 0 1" />
-    <limit effort="100" lower="-0.56" upper="1.56" velocity="1.0"/>
+      xyz="0 0 -1" />
+    <limit effort="100" lower="-1.57" upper="0.56" velocity="1.0"/>
   </joint>
   <link
     name="link_wrist_roll">