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

This commit is contained in:
Rooholla-KhorramBakht 2024-05-19 15:53:24 -04:00
parent e5df8e3d36
commit 6202393369
6 changed files with 76 additions and 3 deletions

3
.gitignore vendored
View File

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

View File

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

View File

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

View File

@ -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

View File

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

View File

@ -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()