allen variance dataset collection and results added for the GO2 body IMU

.gitignore

@@ -173,4 +173,5 @@ deploy/workspaces/nav2_ws/log
 
 _isaac_sim
 .vscode
-deploy/sgiaun-umut
+deploy/sgiaun-umut
+examples/datasets

Go2Py/assets/calibration/imu/acceleration.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8b306743e3775c2b000f0ba490b99415e388f631731940066354fb8389e08e49
+size 1867195

Go2Py/assets/calibration/imu/gyro.png

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bb6a2b3f3090c86bc82e1c0728e7d2b96b57c1f6921f0298974fcafb8313a6ef
+size 1672152

Go2Py/assets/calibration/imu/imu.yaml

@@ -0,0 +1,11 @@
+#Accelerometer
+accelerometer_noise_density: 0.004059677310589936 
+accelerometer_random_walk: 7.228500058270378e-05 
+
+#Gyroscope
+gyroscope_noise_density: 0.0010796822189602655 
+gyroscope_random_walk: 9.093779634369827e-06 
+
+rostopic: '/go2/imu' #Make sure this is correct
+update_rate: 500.0 #Make sure this is correct
+

examples/00-robot-interface.ipynb

@@ -13,7 +13,7 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "from Go2Py.robot.interface.dds import GO2Real\n",
+    "from Go2Py.robot.interface import GO2Real\n",
     "import time\n",
     "robot = GO2Real(mode='lowlevel')"
    ]
@@ -68,7 +68,7 @@
     }
    ],
    "source": [
-    "from Go2Py.robot.interface.dds import GO2Real\n",
+    "from Go2Py.robot.interface import GO2Real\n",
     "import time\n",
     "robot = GO2Real(mode='highlevel')"
    ]

examples/imu-noise-id-dataset-collector.py

@@ -0,0 +1,55 @@
+from Go2Py.robot.interface import GO2Real
+import time
+import numpy as np
+
+from rosbags.rosbag2 import Writer
+from rosbags.typesys import Stores, get_typestore
+from rosbags.typesys.stores.ros2_foxy import (
+    builtin_interfaces__msg__Time as Time,
+    sensor_msgs__msg__Imu as Imu,
+    std_msgs__msg__Header as Header,
+    geometry_msgs__msg__Vector3 as Vector3, 
+    geometry_msgs__msg__Quaternion as Quaternion,
+)
+
+
+def main():
+    robot = GO2Real(mode='lowlevel')
+    time.sleep(1)
+    soc = robot.getBatteryState()
+    imu = robot.getIMU()
+    accel = imu['accel']
+    gyro = imu['gyro']
+    """Iterate over IMAGES and save to output bag."""
+    typestore = get_typestore(Stores.ROS2_FOXY)
+    with Writer('imu_noise_identification_dataset') as writer:
+        conn = writer.add_connection('/go2/imu', Imu.__msgtype__)
+
+        while(soc>10):
+            soc = robot.getBatteryState()
+            imu = robot.getIMU()
+            a = imu['accel']
+            g = imu['gyro']
+            accel = Vector3(a[0], a[1], a[2])
+            gyro = Vector3(g[0], g[1], g[2])
+            q = Quaternion(0,0,0,1)
+            timestamp = time.time()*10**9
+            msg = Imu(
+            Header(
+                    stamp=Time(sec=int(timestamp // 10**9), nanosec=int(timestamp % 10**9)),
+                    frame_id='base_link'),
+            linear_acceleration=accel, 
+            angular_velocity= gyro, 
+            orientation=q, 
+            orientation_covariance=np.zeros(9),
+            linear_acceleration_covariance=np.zeros(9),
+            angular_velocity_covariance=np.zeros(9)
+            )
+            writer.write(conn, timestamp, typestore.serialize_cdr(msg, msg.__msgtype__))
+            time.sleep(0.02)
+            print(soc)
+    robot.close()
+    exit()
+
+if __name__=='__main__':
+    main()