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

calibration parameters is added to the xacro file

This commit is contained in:
Rooholla-Khorrambakht 2024-05-27 19:38:33 -04:00
parent 8c364f3805
commit d1ba6dafcc
2 changed files with 96 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
deploy/ros2_nodes/go2_description/xacro/robot.xacro
View File

@ -68,7 +68,7 @@
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<origin rpy="0.001697 -0.020900 -0.000007" xyz="-0.027353 -0.005499 0.039913"/>
</joint>
<link name="imu_link">
@ -91,6 +91,72 @@
</geometry>
</collision>
</link>
<joint name="vicon_joint" type="fixed">
<parent link="trunk"/>
<child link="vicon_link"/>
<origin rpy="-0.037935 -0.066371 -2.668897" xyz="0.022944 -0.000007 0.058458"/>
</joint>
<link name="vicon_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<!-- <material name="red"/> -->
</visual>
</link>
<joint name="infra1_joint" type="fixed">
<parent link="trunk"/>
<child link="infra1_link"/>
<origin rpy="-1.641669 -0.014332 -1.617155" xyz="0.321618 0.033305 0.081622"/>
</joint>
<link name="infra1_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<!-- <material name="red"/> -->
</visual>
</link>
<joint name="infra2_joint" type="fixed">
<parent link="trunk"/>
<child link="infra1_link"/>
<origin rpy="-1.640637 -0.014093 -1.611177" xyz="0.317969 -0.061919 0.082773"/>
</joint>
<link name="infra2_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<!-- <material name="red"/> -->
</visual>
</link>
<joint name="color_joint" type="fixed">
<parent link="trunk"/>
<child link="color_link"/>
<origin rpy="-1.640491 -0.011125 -1.610364" xyz="0.319415 -0.025790 0.082311"/>
</joint>
<link name="color_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<!-- <material name="red"/> -->
</visual>
</link>
<xacro:leg name="FR" mirror="-1" mirror_dae= "False" front_hind="1" front_hind_dae="True" />
<xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True" />

34
examples/calibration_post_processing.ipynb
View File

@ -16,13 +16,13 @@
},
{
"cell_type": "code",
"execution_count": 10,
"execution_count": 35,
"metadata": {},
"outputs": [],
"source": [
"from Go2Py.calibration import *\n",
"import numpy as np\n",
"np.set_printoptions(formatter={'float': lambda x: \"{0:0.3f}\".format(x)})"
"np.set_printoptions(formatter={'float': lambda x: \"{0:0.6f}\".format(x)})"
]
},
{
@ -537,7 +537,7 @@
},
{
"cell_type": "code",
"execution_count": 46,
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
@ -548,7 +548,7 @@
},
{
"cell_type": "code",
"execution_count": 47,
"execution_count": 4,
"metadata": {},
"outputs": [
{
@ -580,7 +580,7 @@
" [0.000, 0.000, 0.000, 1.000]])}"
]
},
"execution_count": 47,
"execution_count": 4,
"metadata": {},
"output_type": "execute_result"
}
@ -632,6 +632,30 @@
"result['ext_wrt_imu']"
]
},
{
"cell_type": "code",
"execution_count": 41,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"(array([0.319415, -0.025790, 0.082311]),\n",
" array([-1.640491, -0.011125, -1.610364]))"
]
},
"execution_count": 41,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"import pinocchio as pin\n",
"T = result['ext_wrt_imu']['color_wrt_base']\n",
"from scipy.spatial.transform import Rotation \n",
"T[0:3,-1], Rotation.from_matrix(T[0:3,0:3]).as_euler(seq='xyz')"
]
},
{
"cell_type": "markdown",
"metadata": {},