{ "cells": [ { "cell_type": "code", "execution_count": 2, "metadata": {}, "outputs": [], "source": [ "# 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()" ] }, { "cell_type": "code", "execution_count": 3, "metadata": {}, "outputs": [ { "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": [ "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": [], "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": { "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 }