diff --git a/Go2Py/robot/interface/dds.py b/Go2Py/robot/interface/dds.py index 8b8843f..55261ff 100644 --- a/Go2Py/robot/interface/dds.py +++ b/Go2Py/robot/interface/dds.py @@ -71,10 +71,10 @@ class GO2Real(): def getJointStates(self): """Returns the joint angles (q) and velocities (dq) of the robot""" motor_state = np.array([[self.state.motor_state[i].q, - robot.state.motor_state[i].dq, - robot.state.motor_state[i].ddq, - robot.state.motor_state[i].tau_est, - robot.state.motor_state[i].temperature] for i in range(12)]) + self.state.motor_state[i].dq, + self.state.motor_state[i].ddq, + self.state.motor_state[i].tau_est, + self.state.motor_state[i].temperature] for i in range(12)]) return {'q':motor_state[:,0], 'dq':motor_state[:,1], 'ddq':motor_state[:,2], diff --git a/dds_test.ipynb b/examples/dds_draft.ipynb similarity index 93% rename from dds_test.ipynb rename to examples/dds_draft.ipynb index 09020b5..cc38445 100644 --- a/dds_test.ipynb +++ b/examples/dds_draft.ipynb @@ -124,7 +124,7 @@ }, { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "metadata": {}, "outputs": [], "source": [ @@ -150,7 +150,7 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": null, "metadata": {}, "outputs": [], "source": [ @@ -312,52 +312,45 @@ }, { "cell_type": "code", - "execution_count": 10, + "execution_count": null, "metadata": {}, "outputs": [], - "source": [ - "robot = GO2Real()" - ] + "source": [] }, { "cell_type": "code", - "execution_count": 12, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "64" - ] - }, - "execution_count": 12, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ "robot.getBatteryState()" ] }, { "cell_type": "code", - "execution_count": 8, + "execution_count": null, "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "BmsState_(version_high=1, version_low=9, status=8, soc=64, current=-1716, cycle=9, bq_ntc=b'\\x1e\\x1c', mcu_ntc=b' \\x1f', cell_vol=[3843, 3848, 3849, 3849, 3848, 3846, 3845, 3839, 0, 0, 0, 0, 0, 0, 0])" - ] - }, - "execution_count": 8, - "metadata": {}, - "output_type": "execute_result" - } - ], + "outputs": [], "source": [ - "robot.state.bms_state" + "from Go2Py.robot.interface.dds import GO2Real\n", + "robot = GO2Real()" ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "robot.getIMU()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] } ], "metadata": { diff --git a/examples/lowlevel_dds_interface.ipynb b/examples/lowlevel_dds_interface.ipynb new file mode 100644 index 0000000..8417324 --- /dev/null +++ b/examples/lowlevel_dds_interface.ipynb @@ -0,0 +1,92 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "from Go2Py.robot.interface.dds import GO2Real\n", + "import time\n", + "robot = GO2Real(mode='lowlevel')" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import time\n", + "for i in range(1000):\n", + " q = np.zeros(12) \n", + " dq = np.zeros(12)\n", + " kp = np.zeros(12)\n", + " kd = np.zeros(12)\n", + " tau = np.zeros(12)\n", + " tau[0] = -0.0\n", + " robot.setCommands(q, dq, kp, kd, tau)\n", + " time.sleep(0.01) " + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'q': array([-0.04657429, 1.2569859 , -2.80100107, 0.02410829, 1.27012634,\n", + " -2.79052782, -0.37551445, 1.27423871, -2.79706788, 0.34611496,\n", + " 1.28171718, -2.81962585]),\n", + " 'dq': array([ 0.00387552, -0.00775105, 0.00404402, -0.03100419, -0.00775105,\n", + " -0.01011006, -0.04263076, -0.05425733, 0.01011006, -0.04263076,\n", + " -0.00387552, -0.00808805]),\n", + " 'ddq': array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]),\n", + " 'tau_est': array([-0.07421485, 0.12369141, -0.23707521, -0.12369141, -0.12369141,\n", + " 0.33190528, 0.02473828, -0.02473828, -0.14224511, 0.02473828,\n", + " -0.04947656, 0.09483008]),\n", + " 'temperature': array([35., 31., 30., 34., 30., 30., 38., 32., 31., 38., 32., 31.])}" + ] + }, + "execution_count": 3, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "robot.getJointStates()" + ] + }, + { + "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 +}