calibration parameters is added to the xacro file
This commit is contained in:
Rooholla-Khorrambakht
parent
8c364f3805
commit
d1ba6dafcc
|
|
@ -68,7 +68,7 @@
|
||||||
<joint name="imu_joint" type="fixed">
|
<joint name="imu_joint" type="fixed">
|
||||||
<parent link="trunk"/>
|
<parent link="trunk"/>
|
||||||
<child link="imu_link"/>
|
<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>
|
</joint>
|
||||||
|
|
||||||
<link name="imu_link">
|
<link name="imu_link">
|
||||||
|
|
@ -91,6 +91,72 @@
|
||||||
</geometry>
|
</geometry>
|
||||||
</collision>
|
</collision>
|
||||||
</link>
|
</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="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" />
|
<xacro:leg name="FL" mirror="1" mirror_dae="True" front_hind="1" front_hind_dae="True" />
|
||||||
|
|
|
||||||
|
|
@ -16,13 +16,13 @@
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 10,
|
"execution_count": 35,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"outputs": [],
|
"outputs": [],
|
||||||
"source": [
|
"source": [
|
||||||
"from Go2Py.calibration import *\n",
|
"from Go2Py.calibration import *\n",
|
||||||
"import numpy as np\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",
|
"cell_type": "code",
|
||||||
"execution_count": 46,
|
"execution_count": 1,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"outputs": [],
|
"outputs": [],
|
||||||
"source": [
|
"source": [
|
||||||
|
|
@ -548,7 +548,7 @@
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"cell_type": "code",
|
"cell_type": "code",
|
||||||
"execution_count": 47,
|
"execution_count": 4,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"outputs": [
|
"outputs": [
|
||||||
{
|
{
|
||||||
|
|
@ -580,7 +580,7 @@
|
||||||
" [0.000, 0.000, 0.000, 1.000]])}"
|
" [0.000, 0.000, 0.000, 1.000]])}"
|
||||||
]
|
]
|
||||||
},
|
},
|
||||||
"execution_count": 47,
|
"execution_count": 4,
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
"output_type": "execute_result"
|
"output_type": "execute_result"
|
||||||
}
|
}
|
||||||
|
|
@ -632,6 +632,30 @@
|
||||||
"result['ext_wrt_imu']"
|
"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",
|
"cell_type": "markdown",
|
||||||
"metadata": {},
|
"metadata": {},
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue