allen variance dataset collection and results added for the GO2 body IMU
This commit is contained in:
parent
e5df8e3d36
commit
6202393369
|
@ -173,4 +173,5 @@ deploy/workspaces/nav2_ws/log
|
||||||
|
|
||||||
_isaac_sim
|
_isaac_sim
|
||||||
.vscode
|
.vscode
|
||||||
deploy/sgiaun-umut
|
deploy/sgiaun-umut
|
||||||
|
examples/datasets
|
|
@ -0,0 +1,3 @@
|
||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:8b306743e3775c2b000f0ba490b99415e388f631731940066354fb8389e08e49
|
||||||
|
size 1867195
|
|
@ -0,0 +1,3 @@
|
||||||
|
version https://git-lfs.github.com/spec/v1
|
||||||
|
oid sha256:bb6a2b3f3090c86bc82e1c0728e7d2b96b57c1f6921f0298974fcafb8313a6ef
|
||||||
|
size 1672152
|
|
@ -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
|
||||||
|
|
|
@ -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')"
|
||||||
]
|
]
|
||||||
|
|
|
@ -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()
|
Loading…
Reference in New Issue