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

269 lines
13 KiB
Plaintext
Raw Normal View History

robot model is updated
2024-04-14 11:00:01 +08:00
{
"cells": [
{
"cell_type": "code",
velocity estimator notebooks updated
2024-04-15 13:05:35 +08:00
"execution_count": 2,
robot model is updated
2024-04-14 11:00:01 +08:00
"metadata": {},
velocity estimator notebooks updated
2024-04-15 13:05:35 +08:00
"outputs": [],
robot model is updated
2024-04-14 11:00:01 +08:00
"source": [
velocity estimator notebooks updated
2024-04-15 13:05:35 +08:00
"# from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
robot model is updated
2024-04-14 11:00:01 +08:00
"from Go2Py.robot.model import Go2Model\n",
"import time\n",
velocity estimator notebooks updated
2024-04-15 13:05:35 +08:00
"# ros2_init()\n",
"# robot = GO2Real(mode='highlevel')\n",
"# ros2_exec_manager = ROS2ExecutorManager()\n",
"# ros2_exec_manager.add_node(robot)\n",
"# ros2_exec_manager.start()"
robot model is updated
2024-04-14 11:00:01 +08:00
]
},
{
"cell_type": "code",
velocity estimator notebooks updated
2024-04-15 13:05:35 +08:00
"execution_count": 3,
robot model is updated
2024-04-14 11:00:01 +08:00
"metadata": {},
"outputs": [
{
velocity estimator notebooks updated
2024-04-15 13:05:35 +08:00
"ename": "NameError",
"evalue": "name 'robot' is not defined",
"output_type": "error",
"traceback": [
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[0;31mNameError\u001b[0m Traceback (most recent call last)",
"Input \u001b[0;32mIn [3]\u001b[0m, in \u001b[0;36m<cell line: 1>\u001b[0;34m()\u001b[0m\n\u001b[0;32m----> 1\u001b[0m state \u001b[38;5;241m=\u001b[39m \u001b[43mrobot\u001b[49m\u001b[38;5;241m.\u001b[39mgetJointStates()\n\u001b[1;32m 2\u001b[0m state\n",
"\u001b[0;31mNameError\u001b[0m: name 'robot' is not defined"
]
robot model is updated
2024-04-14 11:00:01 +08:00
}
],
"source": [
"state = robot.getJointStates()\n",
"state"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"import pinocchio as pin\n",
"import numpy as np"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"model = Go2Model()"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'FR_foot': array([[ 2.54449360e-02, 4.33674972e-02, 9.98735108e-01,\n",
" -1.41910262e-01, -2.05117030e-01, 1.25221489e-02,\n",
" -9.54690795e-02, -1.20784475e-02, -2.13000000e-01,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
" [ 0.00000000e+00, 9.99058578e-01, -4.33815430e-02,\n",
" 7.30090366e-02, 8.83575282e-03, 2.03483648e-01,\n",
" 7.09917949e-02, 7.04731412e-19, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
" [-9.99676225e-01, 1.10384059e-03, 2.54209816e-02,\n",
" -3.61206704e-03, 6.15827735e-02, -1.44718059e-01,\n",
" -2.42999139e-03, 7.07073528e-02, -1.04083409e-17,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 2.54449360e-02, 4.33674972e-02, 9.98735108e-01,\n",
" 2.54449360e-02, -6.93889390e-18, -6.93889390e-18,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 9.99058578e-01, -4.33815430e-02,\n",
" 0.00000000e+00, 1.00000000e+00, 1.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" -9.99676225e-01, 1.10384059e-03, 2.54209816e-02,\n",
" -9.99676225e-01, -4.33680869e-19, -4.33680869e-19,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]]),\n",
" 'FL_foot': array([[ 4.69705590e-02, -1.64291856e-02, 9.98761157e-01,\n",
" 1.41836988e-01, -2.06048603e-01, -1.00598359e-02,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 9.53945942e-02, -1.29637269e-02, -2.13000000e-01,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
" [ 0.00000000e+00, 9.99864733e-01, 1.64473389e-02,\n",
" 7.44673272e-02, -3.33736863e-03, 2.02884930e-01,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 7.37025259e-02, 5.42101086e-20, 4.33680869e-19,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
" [-9.98896274e-01, -7.72540702e-04, 4.69642054e-02,\n",
" 6.66952394e-03, 6.25126142e-02, 1.42884482e-01,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 4.48568838e-03, 7.31743768e-02, 5.20417043e-18,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 4.69705590e-02, -1.64291856e-02, 9.98761157e-01,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 4.69705590e-02, 3.46944695e-18, 3.46944695e-18,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 9.99864733e-01, 1.64473389e-02,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 1.00000000e+00, 1.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" -9.98896274e-01, -7.72540702e-04, 4.69642054e-02,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" -9.98896274e-01, 1.08420217e-19, 1.08420217e-19,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]]),\n",
" 'RR_foot': array([[ 4.05169215e-02, 3.55928916e-01, 9.33634289e-01,\n",
" -1.38835575e-01, 1.70692580e-01, -5.90479974e-02,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" -9.54215804e-02, -1.20404787e-02, -2.13000000e-01,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
" [ 0.00000000e+00, 9.34401571e-01, -3.56221426e-01,\n",
" 8.75935353e-02, -6.56266297e-02, -1.72144687e-01,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 7.10292389e-02, -4.33680869e-19, 2.77555756e-17,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
" [-9.99178852e-01, 1.44329956e-02, 3.78590751e-02,\n",
" -5.62981300e-03, 3.92988943e-02, -1.63564240e-01,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" -3.86936601e-03, 7.05993683e-02, -1.56125113e-17,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 4.05169215e-02, 3.55928916e-01, 9.33634289e-01,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 4.05169215e-02, -5.55111512e-17, -5.55111512e-17,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 9.34401571e-01, -3.56221426e-01,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 1.00000000e+00, 1.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" -9.99178852e-01, 1.44329956e-02, 3.78590751e-02,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" -9.99178852e-01, 2.60208521e-17, 2.60208521e-17,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00]]),\n",
" 'RL_foot': array([[ 4.02773873e-02, -3.34042417e-01, 9.41697083e-01,\n",
" 1.39211420e-01, 1.72418133e-01, 5.52066037e-02,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 9.54225053e-02, -1.16630895e-02, -2.13000000e-01],\n",
" [ 0.00000000e+00, 9.42461856e-01, 3.34313700e-01,\n",
" 8.54747509e-02, 6.16963531e-02, -1.73927839e-01,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 6.99291638e-02, 1.12757026e-17, 0.00000000e+00],\n",
" [-9.99188537e-01, -1.34652824e-02, 3.79599012e-02,\n",
" 5.61162590e-03, 4.09563977e-02, 1.62238613e-01,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 3.84649049e-03, 6.95158146e-02, -6.93889390e-18],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 4.02773873e-02, -3.34042417e-01, 9.41697083e-01,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 4.02773873e-02, -5.55111512e-17, -5.55111512e-17],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 9.42461856e-01, 3.34313700e-01,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 1.00000000e+00, 1.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" -9.99188537e-01, -1.34652824e-02, 3.79599012e-02,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" -9.99188537e-01, -5.20417043e-18, -5.20417043e-18]])}"
]
},
"execution_count": 5,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"model.updateKinematicsPose(state['q'], np.eye(4))\n",
"model.getInfo()['Jb']"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
velocity estimator notebooks updated
2024-04-15 13:05:35 +08:00
"source": [
"def hat(w):\n",
" return np.array([[0., -w[2], w[1]],\n",
" [w[2], 0., -w[0]],\n",
" [-w[1], w[0], 0.]])"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"model.updateKinematicsPose(state['q'], np.eye(4))\n",
"J = model.getInfo()['Jb']"
]
robot model is updated
2024-04-14 11:00:01 +08:00
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.8.10"
}
},
"nbformat": 4,
"nbformat_minor": 4
}