d43f data collect node - remove imu

.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/stretch_ros.iml" filepath="$PROJECT_DIR$/.idea/stretch_ros.iml" />
+    </modules>
+  </component>
+</project>

stretch_camera_testrig/.idea/stretch_camera_testrig.iml → .idea/stretch_ros.iml

@@ -5,4 +5,8 @@
     <orderEntry type="inheritedJdk" />
     <orderEntry type="sourceFolder" forTests="false" />
   </component>
+  <component name="PyDocumentationSettings">
+    <option name="format" value="PLAIN" />
+    <option name="myDocStringFormat" value="Plain" />
+  </component>
 </module>

stretch_camera_testrig/.idea/vcs.xml → .idea/vcs.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
   <component name="VcsDirectoryMappings">
-    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
+    <mapping directory="$PROJECT_DIR$" vcs="Git" />
   </component>
 </project>

stretch_camera_testrig/.idea/modules.xml

@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectModuleManager">
-    <modules>
-      <module fileurl="file://$PROJECT_DIR$/.idea/stretch_camera_testrig.iml" filepath="$PROJECT_DIR$/.idea/stretch_camera_testrig.iml" />
-    </modules>
-  </component>
-</project>

stretch_camera_testrig/nodes/collect_head_calibration_data.py

@@ -94,7 +94,8 @@ class CollectHeadCalibrationDataNode:
                 if marker.id == self.shoulder_marker_id:
                     self.shoulder_marker_pose = marker.pose
 
-    def calibration_data_callback_testrig(self, marker_array, accel):
+    # def calibration_data_callback_testrig(self, marker_array, accel):
+    def calibration_data_callback_testrig(self, marker_array):
 
         with self.data_lock:
             self.wrist_inside_marker_pose = None
@@ -102,7 +103,7 @@ class CollectHeadCalibrationDataNode:
             self.base_left_marker_pose = None
             self.base_right_marker_pose = None
             self.shoulder_marker_pose = None
-            self.acceleration = [accel.linear_acceleration.x, accel.linear_acceleration.y, accel.linear_acceleration.z]
+            # self.acceleration = [accel.linear_acceleration.x, accel.linear_acceleration.y, accel.linear_acceleration.z]
 
             self.marker_time = None
             for marker in marker_array.markers:
@@ -829,10 +830,13 @@ class CollectHeadCalibrationDataNode:
 
         else:
             # Setup time synchronization for collecting testrig data. 
-            accel_subscriber = message_filters.Subscriber('/camera/accel/sample_corrected', Imu)
+            # accel_subscriber = message_filters.Subscriber('/camera/accel/sample_corrected', Imu)
             aruco_subscriber = message_filters.Subscriber('/aruco/marker_array', MarkerArray)
             slop_time = 0.1
-            self.synchronizer = message_filters.ApproximateTimeSynchronizer([aruco_subscriber, accel_subscriber], 10,
+            # self.synchronizer = message_filters.ApproximateTimeSynchronizer([aruco_subscriber, accel_subscriber], 10,
+            #                                                                 slop_time,
+            #                                                                 allow_headerless=True)
+            self.synchronizer = message_filters.ApproximateTimeSynchronizer([aruco_subscriber], 10,
                                                                             slop_time,
                                                                             allow_headerless=True)
             self.synchronizer.registerCallback(self.calibration_data_callback_testrig)

stretch_gazebo/config/dex_wrist.yaml

@@ -0,0 +1,15 @@
+stretch_dex_wrist_controller:
+  type: "position_controllers/JointTrajectoryController"
+  joints:
+    - joint_wrist_pitch
+    - joint_wrist_roll
+  allow_partial_joints_goal: true
+
+  constraints:
+    goal_time: 0.6
+    stopped_velocity_tolerance: 0.05
+    joint_gripper_finger_right: {trajectory: 0.1, goal: 0.1}
+    joint_gripper_finger_left: {trajectory: 0.1, goal: 0.1}
+    stop_trajectory_duration: 0.5
+    state_publish_rate:  25
+    action_monitor_rate: 10