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

velocity estimator notebooks updated

This commit is contained in:
Rooholla-Khorrambakht 2024-04-15 01:05:35 -04:00
parent a43e0bbc16
commit e637d6529c
2 changed files with 181 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
examples/legged_inertial_ekf.ipynb
View File

@ -2,48 +2,35 @@
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"execution_count": 2,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"pygame 2.5.2 (SDL 2.28.2, Python 3.8.10)\n",
"Hello from the pygame community. https://www.pygame.org/contribute.html\n"
]
}
],
"outputs": [],
"source": [
"from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
"# from Go2Py.robot.interface.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n",
"from Go2Py.robot.model import Go2Model\n",
"import time\n",
"ros2_init()\n",
"robot = GO2Real(mode='highlevel')\n",
"ros2_exec_manager = ROS2ExecutorManager()\n",
"ros2_exec_manager.add_node(robot)\n",
"ros2_exec_manager.start()"
"# ros2_init()\n",
"# robot = GO2Real(mode='highlevel')\n",
"# ros2_exec_manager = ROS2ExecutorManager()\n",
"# ros2_exec_manager.add_node(robot)\n",
"# ros2_exec_manager.start()"
]
},
{
"cell_type": "code",
"execution_count": 2,
"execution_count": 3,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"{'q': array([-0.04339516, 1.257864 , -2.80321264, 0.01644808, 1.26709867,\n",
" -2.79090714, -0.36422092, 1.27348173, -2.80375004, 0.34087694,\n",
" 1.27862883, -2.80913687]),\n",
" 'dq': array([-0.02712867, 0.00387552, -0.00202201, -0.02712867, 0.0658839 ,\n",
" -0.02022013, 0.00775105, -0.00775105, 0.01415409, 0.01937762,\n",
" 0.10463915, -0.05055031])}"
]
},
"execution_count": 2,
"metadata": {},
"output_type": "execute_result"
"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"
]
}
],
"source": [
@ -239,7 +226,22 @@
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
"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']"
]
}
],
"metadata": {

147
examples/velocity_estimator.ipynb Normal file
View File

@ -0,0 +1,147 @@
{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"from Go2Py.sim.mujoco import Go2Sim\n",
"from Go2Py.robot.model import Go2Model\n",
"import pinocchio as pin \n",
"import numpy as np\n",
"import time"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[0.3868 0. 0. ]\n"
]
}
],
"source": [
"model = Go2Model()"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"robot = Go2Sim()"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"robot.step()"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [],
"source": [
"omega = robot.getIMU()['gyro']\n",
"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": [
"robot.standUpReset()\n",
"while True:\n",
" time.sleep(0.001)\n",
" robot.setCommands(robot.standing_q, np.zeros(12), np.ones(12)*100, np.ones(12)*0.1, np.zeros(12))\n",
" robot.step() \n",
" trans, quat = robot.getPose()\n",
" state = robot.getJointStates()\n",
" quat = pin.Quaternion(np.hstack([quat[1:],quat[0]]))\n",
" R = quat.matrix()\n",
" T = np.eye(4)\n",
" T[0:3,0:3] = R\n",
" vel = np.zeros(6)\n",
" model.update(state['q'], state['dq'],T,vel)\n",
"\n",
" J = model.getInfo()['J']['FR_foot'][:,6:]\n",
" nle = model.getInfo()['nle'][6:]\n",
" # tau = (robot.data.qfrc_smooth+robot.data.qfrc_constraint)[6:]\n",
" tau = state['tau_est'].squeeze()\n",
" print(np.linalg.pinv(J.T)[0:3]@(tau - nle))"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"question: What exactly is the difference between the local world aligned and local jacobians in the pinocchio? Why it fits my expectations for the FR3 and not the Go2?"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"robot.getJointStates()\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"J[3:,:].T"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"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": 2
}