{ "cells": [ { "cell_type": "code", "execution_count": 1, "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" ] } ], "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": 2, "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" } ], "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": [] } ], "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 }