EmbodiedAI
/
Go2Py_SIM
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

unitree lidar placement is fixed

This commit is contained in:
Rooholla-Khorrambakht 2024-05-27 23:07:35 -04:00
parent d1ba6dafcc
commit 1bed642bd0
2 changed files with 114 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Go2Py/assets/calibration/extrinsics/crrl-robot-may27.pkl
View File

Binary file not shown.

180
examples/calibration_post_processing.ipynb
View File

@ -537,7 +537,7 @@
},
{
"cell_type": "code",
"execution_count": 1,
"execution_count": 19,
"metadata": {},
"outputs": [],
"source": [
@ -548,39 +548,47 @@
},
{
"cell_type": "code",
"execution_count": 4,
"execution_count": 20,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'infra1_wrt_base': array([[-0.046, -0.071, 0.996, 0.322],\n",
" [-0.999, -0.011, -0.047, 0.033],\n",
" [0.014, -0.997, -0.071, 0.082],\n",
" [0.000, 0.000, 0.000, 1.000]]),\n",
" 'imu_wrt_base': array([[1.000, -0.000, -0.021, -0.027],\n",
" [-0.000, 1.000, -0.002, -0.005],\n",
" [0.021, 0.002, 1.000, 0.040],\n",
" [0.000, 0.000, 0.000, 1.000]]),\n",
" 'infra2_wrt_base': array([[-0.040, -0.070, 0.997, 0.318],\n",
" [-0.999, -0.011, -0.041, -0.062],\n",
" [0.014, -0.997, -0.070, 0.083],\n",
" [0.000, 0.000, 0.000, 1.000]]),\n",
" 'color_wrt_base': array([[-0.040, -0.070, 0.997, 0.319],\n",
" [-0.999, -0.008, -0.040, -0.026],\n",
" [0.011, -0.998, -0.070, 0.082],\n",
" [0.000, 0.000, 0.000, 1.000]]),\n",
" 'marker_wrt_base': array([[-0.888, 0.453, 0.076, 0.023],\n",
" [-0.454, -0.891, -0.004, -0.000],\n",
" [0.066, -0.038, 0.997, 0.058],\n",
" [0.000, 0.000, 0.000, 1.000]]),\n",
" 'lidar_wrt_base': array([[-0.966, -0.087, 0.243, 0.191],\n",
" [-0.088, 0.996, 0.005, -0.001],\n",
" [-0.243, -0.016, -0.970, 0.008],\n",
" [0.000, 0.000, 0.000, 1.000]])}"
"{'infra1_wrt_base': array([[-0.04633745, -0.07139989, 0.99637086, 0.32161791],\n",
" [-0.99882303, -0.01099835, -0.04723963, 0.03330488],\n",
" [ 0.01433134, -0.99738713, -0.07080622, 0.08162181],\n",
" [ 0. , 0. , 0. , 1. ]]),\n",
" 'imu_wrt_base': array([[ 9.99781597e-01, -2.84572838e-05, -2.08987443e-02,\n",
" -2.73533177e-02],\n",
" [-7.00270663e-06, 9.99998561e-01, -1.69667706e-03,\n",
" -5.49944525e-03],\n",
" [ 2.08987625e-02, 1.69645285e-03, 9.99780158e-01,\n",
" 3.99126757e-02],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 1.00000000e+00]]),\n",
" 'infra2_wrt_base': array([[-0.04036617, -0.07029462, 0.9967092 , 0.31796855],\n",
" [-0.99908557, -0.01122962, -0.0412544 , -0.06191919],\n",
" [ 0.01409262, -0.99746306, -0.06977704, 0.08277337],\n",
" [ 0. , 0. , 0. , 1. ]]),\n",
" 'color_wrt_base': array([[-0.03955514, -0.07002279, 0.99676085, 0.31941529],\n",
" [-0.99915546, -0.00833421, -0.04023564, -0.02578968],\n",
" [ 0.01112463, -0.99751058, -0.06963399, 0.08231088],\n",
" [ 0. , 0. , 0. , 1. ]]),\n",
" 'marker_wrt_base': array([[-8.88383872e-01, 4.52720962e-01, 7.62746821e-02,\n",
" 2.29437353e-02],\n",
" [-4.54285606e-01, -8.90848853e-01, -3.59301716e-03,\n",
" -6.99694933e-06],\n",
" [ 6.63225789e-02, -3.78424687e-02, 9.97080369e-01,\n",
" 5.84578170e-02],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 1.00000000e+00]]),\n",
" 'lidar_wrt_base': array([[-0.96602083, -0.08677698, 0.24346151, 0.19088012],\n",
" [-0.08810804, 0.99609608, 0.00543825, -0.00108608],\n",
" [-0.24298297, -0.01619745, -0.96989531, 0.00789635],\n",
" [ 0. , 0. , 0. , 1. ]])}"
]
},
"execution_count": 4,
"execution_count": 20,
"metadata": {},
"output_type": "execute_result"
}
@ -591,39 +599,47 @@
},
{
"cell_type": "code",
"execution_count": 57,
"execution_count": 21,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'infra1_wrt_base': array([[-0.046, -0.071, 0.996, 0.322],\n",
" [-0.999, -0.011, -0.047, 0.033],\n",
" [0.014, -0.997, -0.071, 0.082],\n",
" [0.000, 0.000, 0.000, 1.000]]),\n",
" 'imu_wrt_base': array([[1.000, -0.000, -0.021, -0.027],\n",
" [-0.000, 1.000, -0.002, -0.005],\n",
" [0.021, 0.002, 1.000, 0.040],\n",
" [0.000, 0.000, 0.000, 1.000]]),\n",
" 'infra2_wrt_base': array([[-0.040, -0.070, 0.997, 0.318],\n",
" [-0.999, -0.011, -0.041, -0.062],\n",
" [0.014, -0.997, -0.070, 0.083],\n",
" [0.000, 0.000, 0.000, 1.000]]),\n",
" 'color_wrt_base': array([[-0.040, -0.070, 0.997, 0.319],\n",
" [-0.999, -0.008, -0.040, -0.026],\n",
" [0.011, -0.998, -0.070, 0.082],\n",
" [0.000, 0.000, 0.000, 1.000]]),\n",
" 'marker_wrt_base': array([[-0.888, 0.453, 0.076, 0.023],\n",
" [-0.454, -0.891, -0.004, -0.000],\n",
" [0.066, -0.038, 0.997, 0.058],\n",
" [0.000, 0.000, 0.000, 1.000]]),\n",
" 'lidar_wrt_base': array([[-0.966, -0.087, 0.243, 0.191],\n",
" [-0.088, 0.996, 0.005, -0.001],\n",
" [-0.243, -0.016, -0.970, 0.008],\n",
" [0.000, 0.000, 0.000, 1.000]])}"
"{'infra1_wrt_base': array([[-0.04633745, -0.07139989, 0.99637086, 0.32161791],\n",
" [-0.99882303, -0.01099835, -0.04723963, 0.03330488],\n",
" [ 0.01433134, -0.99738713, -0.07080622, 0.08162181],\n",
" [ 0. , 0. , 0. , 1. ]]),\n",
" 'imu_wrt_base': array([[ 9.99781597e-01, -2.84572838e-05, -2.08987443e-02,\n",
" -2.73533177e-02],\n",
" [-7.00270663e-06, 9.99998561e-01, -1.69667706e-03,\n",
" -5.49944525e-03],\n",
" [ 2.08987625e-02, 1.69645285e-03, 9.99780158e-01,\n",
" 3.99126757e-02],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 1.00000000e+00]]),\n",
" 'infra2_wrt_base': array([[-0.04036617, -0.07029462, 0.9967092 , 0.31796855],\n",
" [-0.99908557, -0.01122962, -0.0412544 , -0.06191919],\n",
" [ 0.01409262, -0.99746306, -0.06977704, 0.08277337],\n",
" [ 0. , 0. , 0. , 1. ]]),\n",
" 'color_wrt_base': array([[-0.03955514, -0.07002279, 0.99676085, 0.31941529],\n",
" [-0.99915546, -0.00833421, -0.04023564, -0.02578968],\n",
" [ 0.01112463, -0.99751058, -0.06963399, 0.08231088],\n",
" [ 0. , 0. , 0. , 1. ]]),\n",
" 'marker_wrt_base': array([[-8.88383872e-01, 4.52720962e-01, 7.62746821e-02,\n",
" 2.29437353e-02],\n",
" [-4.54285606e-01, -8.90848853e-01, -3.59301716e-03,\n",
" -6.99694933e-06],\n",
" [ 6.63225789e-02, -3.78424687e-02, 9.97080369e-01,\n",
" 5.84578170e-02],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 1.00000000e+00]]),\n",
" 'lidar_wrt_base': array([[-0.96602083, -0.08677698, 0.24346151, 0.19088012],\n",
" [-0.08810804, 0.99609608, 0.00543825, -0.00108608],\n",
" [-0.24298297, -0.01619745, -0.96989531, 0.00789635],\n",
" [ 0. , 0. , 0. , 1. ]])}"
]
},
"execution_count": 57,
"execution_count": 21,
"metadata": {},
"output_type": "execute_result"
}
@ -634,17 +650,17 @@
},
{
"cell_type": "code",
"execution_count": 41,
"execution_count": 22,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"(array([0.319415, -0.025790, 0.082311]),\n",
" array([-1.640491, -0.011125, -1.610364]))"
"(array([ 0.31941529, -0.02578968, 0.08231088]),\n",
" array([-1.64049103, -0.01112486, -1.61036424]))"
]
},
"execution_count": 41,
"execution_count": 22,
"metadata": {},
"output_type": "execute_result"
}
@ -665,27 +681,28 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 23,
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"[2024-05-27T22:53:16Z INFO re_sdk_comms::server] Hosting a SDK server over TCP at 0.0.0.0:9876. Connect with the Rerun logging SDK.\n",
"[2024-05-27T22:53:17Z INFO winit::platform_impl::platform::x11::window] Guessed window scale factor: 1\n"
"[2024-05-28T02:56:54Z WARN re_sdk_comms::tcp_client] Tried to flush while TCP stream was still Pending. Data was possibly dropped.\n",
"[2024-05-28T02:56:54Z INFO re_sdk_comms::server] Hosting a SDK server over TCP at 0.0.0.0:9876. Connect with the Rerun logging SDK.\n",
"[2024-05-28T02:56:54Z INFO winit::platform_impl::platform::x11::window] Guessed window scale factor: 1\n",
"[2024-05-28T02:56:54Z INFO re_sdk_comms::server] New SDK client connected: 127.0.0.1:37256\n",
"[2024-05-28T02:56:54Z INFO re_sdk_comms::server] New SDK client connected: 127.0.0.1:37250\n"
]
},
{
"name": "stderr",
"output_type": "stream",
"text": [
"[2024-05-27T22:53:17Z INFO re_sdk_comms::server] New SDK client connected: 127.0.0.1:60992\n",
"[2024-05-27T22:53:17Z INFO re_sdk_comms::server] New SDK client connected: 127.0.0.1:32770\n",
"[2024-05-27T22:53:17Z WARN wgpu_hal::vulkan::instance] Unable to find extension: VK_EXT_swapchain_colorspace\n",
"[2024-05-28T02:56:54Z WARN wgpu_hal::vulkan::instance] Unable to find extension: VK_EXT_swapchain_colorspace\n",
"MESA-INTEL: warning: Performance support disabled, consider sysctl dev.i915.perf_stream_paranoid=0\n",
"\n",
"[2024-05-27T22:53:17Z INFO egui_wgpu] There were 3 available wgpu adapters: {backend: Vulkan, device_type: DiscreteGpu, name: \"NVIDIA GeForce GTX 1050\", driver: \"NVIDIA\", driver_info: \"535.171.04\", vendor: 0x10DE, device: 0x1C8D}, {backend: Vulkan, device_type: IntegratedGpu, name: \"Intel(R) HD Graphics 630 (KBL GT2)\", driver: \"Intel open-source Mesa driver\", driver_info: \"Mesa 21.2.6\", vendor: 0x8086, device: 0x591B}, {backend: Vulkan, device_type: Cpu, name: \"llvmpipe (LLVM 12.0.0, 256 bits)\", driver: \"llvmpipe\", driver_info: \"Mesa 21.2.6 (LLVM 12.0.0)\", vendor: 0x10005}\n"
"[2024-05-28T02:56:54Z INFO egui_wgpu] There were 3 available wgpu adapters: {backend: Vulkan, device_type: DiscreteGpu, name: \"NVIDIA GeForce GTX 1050\", driver: \"NVIDIA\", driver_info: \"535.171.04\", vendor: 0x10DE, device: 0x1C8D}, {backend: Vulkan, device_type: IntegratedGpu, name: \"Intel(R) HD Graphics 630 (KBL GT2)\", driver: \"Intel open-source Mesa driver\", driver_info: \"Mesa 21.2.6\", vendor: 0x8086, device: 0x591B}, {backend: Vulkan, device_type: Cpu, name: \"llvmpipe (LLVM 12.0.0, 256 bits)\", driver: \"llvmpipe\", driver_info: \"Mesa 21.2.6 (LLVM 12.0.0)\", vendor: 0x10005}\n"
]
}
],
@ -697,7 +714,7 @@
},
{
"cell_type": "code",
"execution_count": 56,
"execution_count": 24,
"metadata": {},
"outputs": [],
"source": [
@ -706,7 +723,31 @@
},
{
"cell_type": "code",
"execution_count": 52,
"execution_count": 25,
"metadata": {},
"outputs": [],
"source": [
"t=[0.28945, 0, -0.046825]\n",
"R = Rotation.from_euler(seq='xyz', angles=[0, 2.8782, 0])\n",
"q = R.as_quat()\n",
"rr.log(\"world/base/lidar\", rr.Transform3D(translation=t, rotation=rr.Quaternion(xyzw=q)))"
]
},
{
"cell_type": "code",
"execution_count": 26,
"metadata": {},
"outputs": [],
"source": [
"t=[-0.02557, 0, 0.04232]\n",
"R = Rotation.from_euler(seq='xyz', angles=[0, 0, 0])\n",
"q = R.as_quat()\n",
"rr.log(\"world/base/imu_bad\", rr.Transform3D(translation=t, rotation=rr.Quaternion(xyzw=q)))"
]
},
{
"cell_type": "code",
"execution_count": 27,
"metadata": {},
"outputs": [
{
@ -730,6 +771,13 @@
" qq = pin.Quaternion(T[0:3,0:3])\n",
" rr.log(f\"{parent}/{child}\", rr.Transform3D(translation=T[0:3,-1].tolist(), rotation=rr.Quaternion(xyzw=[qq.x,qq.y,qq.z, qq.w])))"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {