From 890edeb97f7183a7d53a6f7bc52d1645b838546b Mon Sep 17 00:00:00 2001 From: Rooholla-KhorramBakht Date: Sat, 4 May 2024 21:14:41 -0400 Subject: [PATCH] dds message problem fixed --- .gitmodules | 3 - Go2Py/robot/interface/dds.py | 4 +- .../idls/BmsState_.idl | 2 - .../idls/Go2pyLowCmd.idl | 4 +- .../idls/IMUState_.idl | 3 - .../idls/LowState_.idl | 4 - .../idls/MotorState_.idl | 3 - dds_messages/make_msgs.sh | 18 +++ examples/00-robot-interface-DDS.ipynb | 99 +++++++++++++ examples/00-robot-interface-dds.ipynb | 132 ------------------ ...s2.ipynb => 01-robot-interface-ROS2.ipynb} | 0 ...2-mujoco-sim.ipynb => 02-MuJoCo-sim.ipynb} | 0 ...owlevel-control-fsm.ipynb => 04-FSM.ipynb} | 76 ++++++++-- .../05-walk-these-ways-RL-controller.ipynb | 19 ++- go2py_common/.idlpy_manifest | 3 + .../unitree_go => go2py_common}/__init__.py | 2 +- go2py_common/msg/.idlpy_manifest | 3 + .../msg/__init__.py | 2 +- go2py_common/msg/dds_/.idlpy_manifest | 3 + .../msg/dds_/_Go2pyLowCmd.py | 7 +- go2py_common/msg/dds_/__init__.py | 9 ++ msg_sources/make_msgs.sh | 29 ---- setup.cfg | 3 +- .../unitree_go => unitree_go}/.idlpy_manifest | 4 - unitree_go/__init__.py | 9 ++ .../msg/.idlpy_manifest | 4 - unitree_go/msg/__init__.py | 9 ++ .../msg/dds_/.idlpy_manifest | 4 - .../msg/dds_/_BmsState_.py | 6 +- .../msg/dds_/_IMUState_.py | 6 +- .../msg/dds_/_LowState_.py | 12 +- .../msg/dds_/_MotorState_.py | 6 +- .../msg/dds_/__init__.py | 5 +- 33 files changed, 261 insertions(+), 232 deletions(-) rename {msg_sources => dds_messages}/idls/BmsState_.idl (95%) rename {msg_sources => dds_messages}/idls/Go2pyLowCmd.idl (89%) rename {msg_sources => dds_messages}/idls/IMUState_.idl (93%) rename {msg_sources => dds_messages}/idls/LowState_.idl (97%) rename {msg_sources => dds_messages}/idls/MotorState_.idl (95%) create mode 100755 dds_messages/make_msgs.sh create mode 100644 examples/00-robot-interface-DDS.ipynb delete mode 100644 examples/00-robot-interface-dds.ipynb rename examples/{01-robot-interface-ros2.ipynb => 01-robot-interface-ROS2.ipynb} (100%) rename examples/{02-mujoco-sim.ipynb => 02-MuJoCo-sim.ipynb} (100%) rename examples/{04-lowlevel-control-fsm.ipynb => 04-FSM.ipynb} (54%) create mode 100644 go2py_common/.idlpy_manifest rename {Go2Py/unitree_go => go2py_common}/__init__.py (83%) create mode 100644 go2py_common/msg/.idlpy_manifest rename {Go2Py/unitree_go => go2py_common}/msg/__init__.py (81%) create mode 100644 go2py_common/msg/dds_/.idlpy_manifest rename {Go2Py/unitree_go => go2py_common}/msg/dds_/_Go2pyLowCmd.py (82%) create mode 100644 go2py_common/msg/dds_/__init__.py delete mode 100755 msg_sources/make_msgs.sh rename {Go2Py/unitree_go => unitree_go}/.idlpy_manifest (78%) create mode 100644 unitree_go/__init__.py rename {Go2Py/unitree_go => unitree_go}/msg/.idlpy_manifest (78%) create mode 100644 unitree_go/msg/__init__.py rename {Go2Py/unitree_go => unitree_go}/msg/dds_/.idlpy_manifest (77%) rename {Go2Py/unitree_go => unitree_go}/msg/dds_/_BmsState_.py (83%) rename {Go2Py/unitree_go => unitree_go}/msg/dds_/_IMUState_.py (82%) rename {Go2Py/unitree_go => unitree_go}/msg/dds_/_LowState_.py (77%) rename {Go2Py/unitree_go => unitree_go}/msg/dds_/_MotorState_.py (83%) rename {Go2Py/unitree_go => unitree_go}/msg/dds_/__init__.py (60%) diff --git a/.gitmodules b/.gitmodules index d60eadf..e69de29 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +0,0 @@ -[submodule "deploy/isaac_ws/src/isaac_ros_common"] - path = deploy/isaac_ws/src/isaac_ros_common - url = https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_common.git diff --git a/Go2Py/robot/interface/dds.py b/Go2Py/robot/interface/dds.py index b811ba9..ea1fc1b 100644 --- a/Go2Py/robot/interface/dds.py +++ b/Go2Py/robot/interface/dds.py @@ -6,7 +6,8 @@ import numpy.linalg as LA from scipy.spatial.transform import Rotation as R from cyclonedds.domain import DomainParticipant -from Go2Py.unitree_go.msg.dds_ import Go2pyLowCmd_ +from go2py_common.msg.dds_ import Go2pyLowCmd_ +from unitree_go.msg.dds_ import LowState_ from cyclonedds.topic import Topic from cyclonedds.pub import DataWriter @@ -14,7 +15,6 @@ from cyclonedds.domain import DomainParticipant from cyclonedds.topic import Topic from cyclonedds.sub import DataReader from cyclonedds.util import duration -from Go2Py.unitree_go.msg.dds_ import LowState_ from threading import Thread from scipy.spatial.transform import Rotation from Go2Py.joy import xKeySwitch, xRockerBtn diff --git a/msg_sources/idls/BmsState_.idl b/dds_messages/idls/BmsState_.idl similarity index 95% rename from msg_sources/idls/BmsState_.idl rename to dds_messages/idls/BmsState_.idl index 8f2c7a0..9efe4c4 100644 --- a/msg_sources/idls/BmsState_.idl +++ b/dds_messages/idls/BmsState_.idl @@ -4,7 +4,6 @@ #ifndef __unitree_go__msg__bms_state__idl__ #define __unitree_go__msg__bms_state__idl__ -module Go2Py { module unitree_go { module msg { @@ -47,5 +46,4 @@ unsigned short cell_vol[15]; //电池内部15节电池的电压。 }; // module msg }; // module unitree_go -}; #endif // __unitree_go__msg__bms_state__idl__ \ No newline at end of file diff --git a/msg_sources/idls/Go2pyLowCmd.idl b/dds_messages/idls/Go2pyLowCmd.idl similarity index 89% rename from msg_sources/idls/Go2pyLowCmd.idl rename to dds_messages/idls/Go2pyLowCmd.idl index 0c1d386..6c944fe 100644 --- a/msg_sources/idls/Go2pyLowCmd.idl +++ b/dds_messages/idls/Go2pyLowCmd.idl @@ -1,4 +1,4 @@ -module unitree_go { +module go2py_common { module msg { module dds_{ @final struct Go2pyLowCmd_ { @@ -10,4 +10,4 @@ module unitree_go { }; }; }; -}; +}; \ No newline at end of file diff --git a/msg_sources/idls/IMUState_.idl b/dds_messages/idls/IMUState_.idl similarity index 93% rename from msg_sources/idls/IMUState_.idl rename to dds_messages/idls/IMUState_.idl index 1e456f7..e1c2afe 100644 --- a/msg_sources/idls/IMUState_.idl +++ b/dds_messages/idls/IMUState_.idl @@ -4,8 +4,6 @@ #ifndef __unitree_go__msg__imu_state__idl__ #define __unitree_go__msg__imu_state__idl__ -module Go2Py { - module unitree_go { module msg { @@ -32,5 +30,4 @@ octet temperature; //IMU 温度信息（摄氏度）。 }; // module msg }; // module unitree_go -}; #endif // __unitree_go__msg__imu_state__idl__ \ No newline at end of file diff --git a/msg_sources/idls/LowState_.idl b/dds_messages/idls/LowState_.idl similarity index 97% rename from msg_sources/idls/LowState_.idl rename to dds_messages/idls/LowState_.idl index df04591..1b26cff 100644 --- a/msg_sources/idls/LowState_.idl +++ b/dds_messages/idls/LowState_.idl @@ -7,9 +7,6 @@ #ifndef __unitree_go__msg__low_state__idl__ #define __unitree_go__msg__low_state__idl__ - -module Go2Py { - module unitree_go { module msg { @@ -70,5 +67,4 @@ unsigned long crc; //数据CRC校验用。 }; // module msg }; // module unitree_go -}; #endif // __unitree_go__msg__low_state__idl__ \ No newline at end of file diff --git a/msg_sources/idls/MotorState_.idl b/dds_messages/idls/MotorState_.idl similarity index 95% rename from msg_sources/idls/MotorState_.idl rename to dds_messages/idls/MotorState_.idl index 478aa2b..a9bbeea 100644 --- a/msg_sources/idls/MotorState_.idl +++ b/dds_messages/idls/MotorState_.idl @@ -4,8 +4,6 @@ #ifndef __unitree_go__msg__motor_state__idl__ #define __unitree_go__msg__motor_state__idl__ -module Go2Py { - module unitree_go { module msg { @@ -36,5 +34,4 @@ unsigned long reserve[2]; //当前电机通信频率+电机错误标志位： }; // module msg }; // module unitree_go -}; #endif // __unitree_go__msg__motor_state__idl__ \ No newline at end of file diff --git a/dds_messages/make_msgs.sh b/dds_messages/make_msgs.sh new file mode 100755 index 0000000..070decf --- /dev/null +++ b/dds_messages/make_msgs.sh @@ -0,0 +1,18 @@ +#!/bin/bash + +GREEN='\033[0;32m' +NC='\033[0m' # No Color + +echo -e "${GREEN} Starting DDS type generation...${NC}" +# Clean +rm -r ../unitree_go +rm -r ../go2py_common + +# Make +for file in idls/*.idl +do + echo "Processing $file file..." + idlc -l py $file +done +mv unitree_go ../ +mv go2py_common ../ \ No newline at end of file diff --git a/examples/00-robot-interface-DDS.ipynb b/examples/00-robot-interface-DDS.ipynb new file mode 100644 index 0000000..d773b42 --- /dev/null +++ b/examples/00-robot-interface-DDS.ipynb @@ -0,0 +1,99 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Lowlevel Control" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import unitree_go\n", + "unitree_go.__file__" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from Go2Py.robot.interface.dds import GO2Real\n", + "import time\n", + "robot = GO2Real(mode='lowlevel')" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "robot.getJointStates()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import time\n", + "for i in range(1000000):\n", + " q = np.zeros(12) \n", + " dq = np.zeros(12)\n", + " kp = np.ones(12)*0.0\n", + " kd = np.ones(12)*0.0\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": "markdown", + "metadata": {}, + "source": [ + "# Highlevel Control" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from Go2Py.robot.interface.dds import GO2Real\n", + "import time\n", + "robot = GO2Real(mode='highlevel')" + ] + } + ], + "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.18" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/examples/00-robot-interface-dds.ipynb b/examples/00-robot-interface-dds.ipynb deleted file mode 100644 index 2c66643..0000000 --- a/examples/00-robot-interface-dds.ipynb +++ /dev/null @@ -1,132 +0,0 @@ -{ - "cells": [ - { - "cell_type": "markdown", - "metadata": {}, - "source": [ - "# Lowlevel Control" - ] - }, - { - "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.18)\n", - "Hello from the pygame community. https://www.pygame.org/contribute.html\n" - ] - } - ], - "source": [ - "from Go2Py.robot.interface.dds import GO2Real\n", - "import time\n", - "robot = GO2Real(mode='lowlevel')" - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "metadata": {}, - "outputs": [ - { - "data": { - "text/plain": [ - "{'q': array([-0.04851204, 1.25695562, -2.80253339, 0.05441612, 1.24787235,\n", - " -2.78187132, -0.34375334, 1.27221012, -2.80291271, 0.32734287,\n", - " 1.27893162, -2.81329131]),\n", - " 'dq': array([ 0.03875524, 0.0155021 , 0.0323522 , 0.07363495, 0.02712867,\n", - " -0.0161761 , 0.01937762, 0.03487971, 0.00808805, -0.03487971,\n", - " 0. , 0.02628616]),\n", - " 'ddq': array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]),\n", - " 'tau_est': array([-0.04947656, 0.04947656, -0.04741504, -0.04947656, -0.04947656,\n", - " 0. , 0. , 0.04947656, -0.04741504, 0.04947656,\n", - " -0.07421485, 0.04741504]),\n", - " 'temperature': array([34., 30., 29., 32., 30., 30., 36., 31., 29., 37., 31., 30.])}" - ] - }, - "execution_count": 3, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "robot.getJointStates()" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "metadata": {}, - "outputs": [], - "source": [ - "import numpy as np\n", - "import time\n", - "for i in range(1000000):\n", - " # q = np.zeros(12) \n", - " dq = np.zeros(12)\n", - " kp = np.ones(12)*0.0\n", - " kd = np.ones(12)*0.0\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": "markdown", - "metadata": {}, - "source": [ - "# Highlevel Control" - ] - }, - { - "cell_type": "code", - "execution_count": 4, - "metadata": {}, - "outputs": [ - { - "ename": "NotImplementedError", - "evalue": "DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.", - "output_type": "error", - "traceback": [ - "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[0;31mNotImplementedError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[0;32mIn[4], line 3\u001b[0m\n\u001b[1;32m 1\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01mGo2Py\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mrobot\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01minterface\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mdds\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m GO2Real\n\u001b[1;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01mtime\u001b[39;00m\n\u001b[0;32m----> 3\u001b[0m robot \u001b[38;5;241m=\u001b[39m \u001b[43mGO2Real\u001b[49m\u001b[43m(\u001b[49m\u001b[43mmode\u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[38;5;124;43mhighlevel\u001b[39;49m\u001b[38;5;124;43m'\u001b[39;49m\u001b[43m)\u001b[49m\n", - "File \u001b[0;32m~/projects/rooholla/locomotion/Go2Py/Go2Py/robot/interface/dds.py:35\u001b[0m, in \u001b[0;36mGO2Real.__init__\u001b[0;34m(self, mode, vx_max, vy_max, ωz_max)\u001b[0m\n\u001b[1;32m 33\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mmode \u001b[38;5;241m=\u001b[39m mode\n\u001b[1;32m 34\u001b[0m \u001b[38;5;28;01mif\u001b[39;00m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mmode \u001b[38;5;241m==\u001b[39m \u001b[38;5;124m'\u001b[39m\u001b[38;5;124mhighlevel\u001b[39m\u001b[38;5;124m'\u001b[39m:\n\u001b[0;32m---> 35\u001b[0m \u001b[38;5;28;01mraise\u001b[39;00m \u001b[38;5;167;01mNotImplementedError\u001b[39;00m(\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mDDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.\u001b[39m\u001b[38;5;124m'\u001b[39m)\n\u001b[1;32m 36\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39msimulated \u001b[38;5;241m=\u001b[39m \u001b[38;5;28;01mFalse\u001b[39;00m\n\u001b[1;32m 37\u001b[0m \u001b[38;5;28mself\u001b[39m\u001b[38;5;241m.\u001b[39mprestanding_q \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39marray([ \u001b[38;5;241m0.0\u001b[39m, \u001b[38;5;241m1.26186061\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.5\u001b[39m,\n\u001b[1;32m 38\u001b[0m \u001b[38;5;241m0.0\u001b[39m, \u001b[38;5;241m1.25883281\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.5\u001b[39m,\n\u001b[1;32m 39\u001b[0m \u001b[38;5;241m0.0\u001b[39m, \u001b[38;5;241m1.27193761\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.6\u001b[39m, \n\u001b[1;32m 40\u001b[0m \u001b[38;5;241m0.0\u001b[39m, \u001b[38;5;241m1.27148342\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m2.6\u001b[39m])\n", - "\u001b[0;31mNotImplementedError\u001b[0m: DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface." - ] - } - ], - "source": [ - "from Go2Py.robot.interface.dds import GO2Real\n", - "import time\n", - "robot = GO2Real(mode='highlevel')" - ] - } - ], - "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.18" - } - }, - "nbformat": 4, - "nbformat_minor": 4 -} diff --git a/examples/01-robot-interface-ros2.ipynb b/examples/01-robot-interface-ROS2.ipynb similarity index 100% rename from examples/01-robot-interface-ros2.ipynb rename to examples/01-robot-interface-ROS2.ipynb diff --git a/examples/02-mujoco-sim.ipynb b/examples/02-MuJoCo-sim.ipynb similarity index 100% rename from examples/02-mujoco-sim.ipynb rename to examples/02-MuJoCo-sim.ipynb diff --git a/examples/04-lowlevel-control-fsm.ipynb b/examples/04-FSM.ipynb similarity index 54% rename from examples/04-lowlevel-control-fsm.ipynb rename to examples/04-FSM.ipynb index 6ab4d4d..5226ccd 100644 --- a/examples/04-lowlevel-control-fsm.ipynb +++ b/examples/04-FSM.ipynb @@ -9,7 +9,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": {}, "outputs": [], "source": [ @@ -21,7 +21,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 2, "metadata": {}, "outputs": [], "source": [ @@ -32,7 +32,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 3, "metadata": {}, "outputs": [], "source": [ @@ -42,11 +42,12 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 5, "metadata": {}, "outputs": [], "source": [ - "fsm.close()" + "fsm.close()\n", + "robot.close()" ] }, { @@ -58,9 +59,18 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "pygame 2.5.2 (SDL 2.28.2, Python 3.8.18)\n", + "Hello from the pygame community. https://www.pygame.org/contribute.html\n" + ] + } + ], "source": [ "from Go2Py.robot.interface.dds import GO2Real\n", "import time\n", @@ -71,22 +81,62 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 2, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'q': array([-0.0441218 , 1.2594384 , -2.81978369, 0.05720162, 1.2381835 ,\n", + " -2.77803254, -0.34687191, 1.26960623, -2.80896306, 0.28916293,\n", + " 1.28592575, -2.83088923]),\n", + " 'dq': array([ 0.03875524, -0.04650629, 0.00202201, 0.0155021 , -0.04263076,\n", + " -0.0323522 , -0.00775105, -0.04650629, 0.00404402, -0.05813286,\n", + " 0.03100419, -0.03841824]),\n", + " 'ddq': array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]),\n", + " 'tau_est': array([-0.07421485, 0.07421485, -0.23707521, -0.17316797, -0.04947656,\n", + " 0.28449023, 0. , 0.02473828, -0.09483008, 0.04947656,\n", + " -0.04947656, 0.09483008]),\n", + " 'temperature': array([35., 31., 30., 33., 30., 30., 37., 31., 30., 37., 31., 30.])}" + ] + }, + "execution_count": 2, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "robot = GO2Real(mode='lowlevel')\n", + "time.sleep(1)\n", + "robot.getJointStates()" + ] + }, + { + "cell_type": "code", + "execution_count": 3, "metadata": {}, "outputs": [], "source": [ - "robot = GO2Real(mode='lowlevel')\n", "remote = KeyboardRemote()\n", "safety_hypervisor = SafetyHypervisor(robot)" ] }, { "cell_type": "code", - "execution_count": null, + "execution_count": 6, "metadata": {}, "outputs": [], "source": [ - "fsm = FSM(robot, remote, safety)" + "fsm = FSM(robot, remote, safety_hypervisor)" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [], + "source": [ + "fsm.close()" ] }, { @@ -94,9 +144,7 @@ "execution_count": null, "metadata": {}, "outputs": [], - "source": [ - "fsm.close()" - ] + "source": [] } ], "metadata": { diff --git a/examples/05-walk-these-ways-RL-controller.ipynb b/examples/05-walk-these-ways-RL-controller.ipynb index 01ad28f..97ed4c4 100644 --- a/examples/05-walk-these-ways-RL-controller.ipynb +++ b/examples/05-walk-these-ways-RL-controller.ipynb @@ -2,7 +2,7 @@ "cells": [ { "cell_type": "code", - "execution_count": 1, + "execution_count": null, "metadata": {}, "outputs": [], "source": [ @@ -178,7 +178,6 @@ "from Go2Py.robot.fsm import FSM\n", "from Go2Py.robot.remote import KeyboardRemote\n", "from Go2Py.robot.safety import SafetyHypervisor\n", - "from Go2Py.sim.mujoco import Go2Sim\n", "from Go2Py.control.walk_these_ways import *" ] }, @@ -214,6 +213,22 @@ "safety_hypervisor = SafetyHypervisor(robot)" ] }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "controller.command_profile.pitch_cmd=0.0\n", + "controller.command_profile.body_height_cmd=0.0\n", + "controller.command_profile.footswing_height_cmd=0.04\n", + "controller.command_profile.roll_cmd=0.0\n", + "controller.command_profile.stance_width_cmd=0.2\n", + "controller.command_profile.x_vel_cmd=-0.2\n", + "controller.command_profile.y_vel_cmd=0.01\n", + "controller.command_profile.setGaitType(\"trotting\")" + ] + }, { "cell_type": "code", "execution_count": null, diff --git a/go2py_common/.idlpy_manifest b/go2py_common/.idlpy_manifest new file mode 100644 index 0000000..4ff480a --- /dev/null +++ b/go2py_common/.idlpy_manifest @@ -0,0 +1,3 @@ +Go2pyLowCmd +msg + diff --git a/Go2Py/unitree_go/__init__.py b/go2py_common/__init__.py similarity index 83% rename from Go2Py/unitree_go/__init__.py rename to go2py_common/__init__.py index cbf3d21..29bab3d 100644 --- a/Go2Py/unitree_go/__init__.py +++ b/go2py_common/__init__.py @@ -1,7 +1,7 @@ """ Generated by Eclipse Cyclone DDS idlc Python Backend Cyclone DDS IDL version: v0.11.0 - Module: Go2Py.unitree_go + Module: go2py_common """ diff --git a/go2py_common/msg/.idlpy_manifest b/go2py_common/msg/.idlpy_manifest new file mode 100644 index 0000000..4ab2cc0 --- /dev/null +++ b/go2py_common/msg/.idlpy_manifest @@ -0,0 +1,3 @@ +Go2pyLowCmd +dds_ + diff --git a/Go2Py/unitree_go/msg/__init__.py b/go2py_common/msg/__init__.py similarity index 81% rename from Go2Py/unitree_go/msg/__init__.py rename to go2py_common/msg/__init__.py index 8b01d8d..9eb7ef5 100644 --- a/Go2Py/unitree_go/msg/__init__.py +++ b/go2py_common/msg/__init__.py @@ -1,7 +1,7 @@ """ Generated by Eclipse Cyclone DDS idlc Python Backend Cyclone DDS IDL version: v0.11.0 - Module: Go2Py.unitree_go.msg + Module: go2py_common.msg """ diff --git a/go2py_common/msg/dds_/.idlpy_manifest b/go2py_common/msg/dds_/.idlpy_manifest new file mode 100644 index 0000000..bae2f4a --- /dev/null +++ b/go2py_common/msg/dds_/.idlpy_manifest @@ -0,0 +1,3 @@ +Go2pyLowCmd + +Go2pyLowCmd_ diff --git a/Go2Py/unitree_go/msg/dds_/_Go2pyLowCmd.py b/go2py_common/msg/dds_/_Go2pyLowCmd.py similarity index 82% rename from Go2Py/unitree_go/msg/dds_/_Go2pyLowCmd.py rename to go2py_common/msg/dds_/_Go2pyLowCmd.py index cd97b13..0f37ccd 100644 --- a/Go2Py/unitree_go/msg/dds_/_Go2pyLowCmd.py +++ b/go2py_common/msg/dds_/_Go2pyLowCmd.py @@ -1,7 +1,7 @@ """ Generated by Eclipse Cyclone DDS idlc Python Backend Cyclone DDS IDL version: v0.11.0 - Module: Go2Py.unitree_go.msg.dds_ + Module: go2py_common.msg.dds_ IDL file: Go2pyLowCmd.idl """ @@ -15,12 +15,13 @@ import cyclonedds.idl.annotations as annotate import cyclonedds.idl.types as types # root module import for resolving types -import Go2Py.unitree_go +import go2py_common + @dataclass @annotate.final @annotate.autoid("sequential") -class Go2pyLowCmd_(idl.IdlStruct, typename="unitree_go.msg.dds_.Go2pyLowCmd_"): +class Go2pyLowCmd_(idl.IdlStruct, typename="go2py_common.msg.dds_.Go2pyLowCmd_"): q: types.array[types.float32, 12] dq: types.array[types.float32, 12] kp: types.array[types.float32, 12] diff --git a/go2py_common/msg/dds_/__init__.py b/go2py_common/msg/dds_/__init__.py new file mode 100644 index 0000000..f4ecbc5 --- /dev/null +++ b/go2py_common/msg/dds_/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: go2py_common.msg.dds_ + +""" + +from ._Go2pyLowCmd import Go2pyLowCmd_ +__all__ = ["Go2pyLowCmd_", ] diff --git a/msg_sources/make_msgs.sh b/msg_sources/make_msgs.sh deleted file mode 100755 index 6a98a71..0000000 --- a/msg_sources/make_msgs.sh +++ /dev/null @@ -1,29 +0,0 @@ -#!/bin/bash - -GREEN='\033[0;32m' -NC='\033[0m' # No Color - -echo -e "${GREEN} Starting DDS type generation...${NC}" -# Clean -# rm -r Go2Py/go2py_msgs/dds_ -# rm -r ../Go2Py/unitree_go - -# Make -for file in idls/*.idl -do - echo "Processing $file file..." - idlc -l py $file -done -mv Go2Py/unitree_go ../Go2Py -# rm -r Go2Py -# mkdir msgs/cpp -# cd msgs/cpp -# for file in ../../idl/*.idl -# do -# echo "Processing $file file..." -# idlc -l ../../idl/libcycloneddsidlcxx.so.0.10.2 $file -# done -# cd ../.. -# # rm -r ../cpp_bridge/include/go2py -# # mv msgs/cpp ../cpp_bridge/include/go2py -# echo -e "${GREEN} Done with DDS type generation${NC}" \ No newline at end of file diff --git a/setup.cfg b/setup.cfg index 1abfcf7..946f6eb 100644 --- a/setup.cfg +++ b/setup.cfg @@ -17,7 +17,8 @@ packages = find: include_package_data = True install_requires = pygame - pyyaml + pyyaml + pynput # [options.package_data] # * = unitree_legged_sdk/amd64/*.so, unitree_legged_sdk/arm64/*.so \ No newline at end of file diff --git a/Go2Py/unitree_go/.idlpy_manifest b/unitree_go/.idlpy_manifest similarity index 78% rename from Go2Py/unitree_go/.idlpy_manifest rename to unitree_go/.idlpy_manifest index 25ab5be..e5dd99d 100644 --- a/Go2Py/unitree_go/.idlpy_manifest +++ b/unitree_go/.idlpy_manifest @@ -2,10 +2,6 @@ BmsState_ msg -Go2pyLowCmd -msg - - IMUState_ msg diff --git a/unitree_go/__init__.py b/unitree_go/__init__.py new file mode 100644 index 0000000..b553006 --- /dev/null +++ b/unitree_go/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go + +""" + +from . import msg +__all__ = ["msg", ] diff --git a/Go2Py/unitree_go/msg/.idlpy_manifest b/unitree_go/msg/.idlpy_manifest similarity index 78% rename from Go2Py/unitree_go/msg/.idlpy_manifest rename to unitree_go/msg/.idlpy_manifest index 8491d25..5ca29a3 100644 --- a/Go2Py/unitree_go/msg/.idlpy_manifest +++ b/unitree_go/msg/.idlpy_manifest @@ -2,10 +2,6 @@ BmsState_ dds_ -Go2pyLowCmd -dds_ - - IMUState_ dds_ diff --git a/unitree_go/msg/__init__.py b/unitree_go/msg/__init__.py new file mode 100644 index 0000000..f8cae9b --- /dev/null +++ b/unitree_go/msg/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: unitree_go.msg + +""" + +from . import dds_ +__all__ = ["dds_", ] diff --git a/Go2Py/unitree_go/msg/dds_/.idlpy_manifest b/unitree_go/msg/dds_/.idlpy_manifest similarity index 77% rename from Go2Py/unitree_go/msg/dds_/.idlpy_manifest rename to unitree_go/msg/dds_/.idlpy_manifest index 2834913..bc9d94d 100644 --- a/Go2Py/unitree_go/msg/dds_/.idlpy_manifest +++ b/unitree_go/msg/dds_/.idlpy_manifest @@ -2,10 +2,6 @@ BmsState_ BmsState_ -Go2pyLowCmd - -Go2pyLowCmd_ - IMUState_ IMUState_ diff --git a/Go2Py/unitree_go/msg/dds_/_BmsState_.py b/unitree_go/msg/dds_/_BmsState_.py similarity index 83% rename from Go2Py/unitree_go/msg/dds_/_BmsState_.py rename to unitree_go/msg/dds_/_BmsState_.py index 4f41d71..baba8cc 100644 --- a/Go2Py/unitree_go/msg/dds_/_BmsState_.py +++ b/unitree_go/msg/dds_/_BmsState_.py @@ -1,7 +1,7 @@ """ Generated by Eclipse Cyclone DDS idlc Python Backend Cyclone DDS IDL version: v0.11.0 - Module: Go2Py.unitree_go.msg.dds_ + Module: unitree_go.msg.dds_ IDL file: BmsState_.idl """ @@ -15,13 +15,13 @@ import cyclonedds.idl.annotations as annotate import cyclonedds.idl.types as types # root module import for resolving types -import Go2Py.unitree_go +import unitree_go @dataclass @annotate.final @annotate.autoid("sequential") -class BmsState_(idl.IdlStruct, typename="Go2Py.unitree_go.msg.dds_.BmsState_"): +class BmsState_(idl.IdlStruct, typename="unitree_go.msg.dds_.BmsState_"): version_high: types.uint8 version_low: types.uint8 status: types.uint8 diff --git a/Go2Py/unitree_go/msg/dds_/_IMUState_.py b/unitree_go/msg/dds_/_IMUState_.py similarity index 82% rename from Go2Py/unitree_go/msg/dds_/_IMUState_.py rename to unitree_go/msg/dds_/_IMUState_.py index 1704375..473d9cb 100644 --- a/Go2Py/unitree_go/msg/dds_/_IMUState_.py +++ b/unitree_go/msg/dds_/_IMUState_.py @@ -1,7 +1,7 @@ """ Generated by Eclipse Cyclone DDS idlc Python Backend Cyclone DDS IDL version: v0.11.0 - Module: Go2Py.unitree_go.msg.dds_ + Module: unitree_go.msg.dds_ IDL file: IMUState_.idl """ @@ -15,13 +15,13 @@ import cyclonedds.idl.annotations as annotate import cyclonedds.idl.types as types # root module import for resolving types -import Go2Py.unitree_go +import unitree_go @dataclass @annotate.final @annotate.autoid("sequential") -class IMUState_(idl.IdlStruct, typename="Go2Py.unitree_go.msg.dds_.IMUState_"): +class IMUState_(idl.IdlStruct, typename="unitree_go.msg.dds_.IMUState_"): quaternion: types.array[types.float32, 4] gyroscope: types.array[types.float32, 3] accelerometer: types.array[types.float32, 3] diff --git a/Go2Py/unitree_go/msg/dds_/_LowState_.py b/unitree_go/msg/dds_/_LowState_.py similarity index 77% rename from Go2Py/unitree_go/msg/dds_/_LowState_.py rename to unitree_go/msg/dds_/_LowState_.py index 3961e3e..27b1175 100644 --- a/Go2Py/unitree_go/msg/dds_/_LowState_.py +++ b/unitree_go/msg/dds_/_LowState_.py @@ -1,7 +1,7 @@ """ Generated by Eclipse Cyclone DDS idlc Python Backend Cyclone DDS IDL version: v0.11.0 - Module: Go2Py.unitree_go.msg.dds_ + Module: unitree_go.msg.dds_ IDL file: LowState_.idl """ @@ -15,22 +15,22 @@ import cyclonedds.idl.annotations as annotate import cyclonedds.idl.types as types # root module import for resolving types -import Go2Py.unitree_go +import unitree_go @dataclass @annotate.final @annotate.autoid("sequential") -class LowState_(idl.IdlStruct, typename="Go2Py.unitree_go.msg.dds_.LowState_"): +class LowState_(idl.IdlStruct, typename="unitree_go.msg.dds_.LowState_"): head: types.array[types.uint8, 2] level_flag: types.uint8 frame_reserve: types.uint8 sn: types.array[types.uint32, 2] version: types.array[types.uint32, 2] bandwidth: types.uint16 - imu_state: 'Go2Py.unitree_go.msg.dds_.IMUState_' - motor_state: types.array['Go2Py.unitree_go.msg.dds_.MotorState_', 20] - bms_state: 'Go2Py.unitree_go.msg.dds_.BmsState_' + imu_state: 'unitree_go.msg.dds_.IMUState_' + motor_state: types.array['unitree_go.msg.dds_.MotorState_', 20] + bms_state: 'unitree_go.msg.dds_.BmsState_' foot_force: types.array[types.int16, 4] foot_force_est: types.array[types.int16, 4] tick: types.uint32 diff --git a/Go2Py/unitree_go/msg/dds_/_MotorState_.py b/unitree_go/msg/dds_/_MotorState_.py similarity index 83% rename from Go2Py/unitree_go/msg/dds_/_MotorState_.py rename to unitree_go/msg/dds_/_MotorState_.py index 48a47f2..2d28c7b 100644 --- a/Go2Py/unitree_go/msg/dds_/_MotorState_.py +++ b/unitree_go/msg/dds_/_MotorState_.py @@ -1,7 +1,7 @@ """ Generated by Eclipse Cyclone DDS idlc Python Backend Cyclone DDS IDL version: v0.11.0 - Module: Go2Py.unitree_go.msg.dds_ + Module: unitree_go.msg.dds_ IDL file: MotorState_.idl """ @@ -15,13 +15,13 @@ import cyclonedds.idl.annotations as annotate import cyclonedds.idl.types as types # root module import for resolving types -import Go2Py.unitree_go +import unitree_go @dataclass @annotate.final @annotate.autoid("sequential") -class MotorState_(idl.IdlStruct, typename="Go2Py.unitree_go.msg.dds_.MotorState_"): +class MotorState_(idl.IdlStruct, typename="unitree_go.msg.dds_.MotorState_"): mode: types.uint8 q: types.float32 dq: types.float32 diff --git a/Go2Py/unitree_go/msg/dds_/__init__.py b/unitree_go/msg/dds_/__init__.py similarity index 60% rename from Go2Py/unitree_go/msg/dds_/__init__.py rename to unitree_go/msg/dds_/__init__.py index 79464f9..a360471 100644 --- a/Go2Py/unitree_go/msg/dds_/__init__.py +++ b/unitree_go/msg/dds_/__init__.py @@ -1,13 +1,12 @@ """ Generated by Eclipse Cyclone DDS idlc Python Backend Cyclone DDS IDL version: v0.11.0 - Module: Go2Py.unitree_go.msg.dds_ + Module: unitree_go.msg.dds_ """ from ._BmsState_ import BmsState_ -from ._Go2pyLowCmd import Go2pyLowCmd_ from ._IMUState_ import IMUState_ from ._LowState_ import LowState_ from ._MotorState_ import MotorState_ -__all__ = ["BmsState_", "Go2pyLowCmd_", "IMUState_", "LowState_", "MotorState_", ] +__all__ = ["BmsState_", "IMUState_", "LowState_", "MotorState_", ]