Initial pose for dex wrist joints in collect calib data

stretch_calibration/nodes/collect_head_calibration_data

@@ -577,12 +577,21 @@ class CollectHeadCalibrationDataNode:
     def move_to_initial_configuration(self):
         # The robot is commanded to move to this pose prior to
         # beginning to collect calibration data.
-        initial_pose = {'joint_wrist_yaw': 0.0,
-                        'wrist_extension': 0.0,
-                        'joint_lift': 0.3,
-                        'gripper_aperture': 0.0,
-                        'joint_head_pan': -1.6947147036864942,
-                        'joint_head_tilt': -0.4}
+        initial_pose_map = {
+            'joint_wrist_yaw': 0.0,
+            'joint_wrist_pitch': 0.0,
+            'joint_wrist_roll': 0.0,
+            'wrist_extension': 0.0,
+            'joint_lift': 0.3,
+            'gripper_aperture': 0.0,
+            'joint_head_pan': -1.6947147036864942,
+            'joint_head_tilt': -0.4
+        }
+
+        initial_pose = {}
+        for j in self.joint_state.name:
+            if j in initial_pose_map:
+                initial_pose[j] = initial_pose_map[j]
 
         rospy.loginfo('Move to the calibration start pose.')
         self.move_to_pose(initial_pose)
@@ -641,6 +650,9 @@ class CollectHeadCalibrationDataNode:
             rospy.signal_shutdown('Unable to connect to arm action server. Timeout exceeded.')
             sys.exit()
 
+        rate = rospy.Rate(self.rate)
+        while self.joint_state is None:
+            rate.sleep()
         self.move_to_initial_configuration()
 
         self.calibrate_pan_and_tilt(collect_check_data)