diff --git a/examples/legged_inertial_ekf.ipynb b/examples/legged_inertial_ekf.ipynb index 97aa105..8560553 100644 --- a/examples/legged_inertial_ekf.ipynb +++ b/examples/legged_inertial_ekf.ipynb @@ -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\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": { diff --git a/examples/velocity_estimator.ipynb b/examples/velocity_estimator.ipynb new file mode 100644 index 0000000..a94a91b --- /dev/null +++ b/examples/velocity_estimator.ipynb @@ -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 +}