From c57c81290c370cf61f7debd5c112fe995604020f Mon Sep 17 00:00:00 2001 From: Rooholla-KhorramBakht Date: Thu, 8 Feb 2024 22:02:42 -0500 Subject: [PATCH] ROS2 lowstate interface and corresponding example added. --- .gitignore | 2 +- Go2Py/robot/interface/ros2.py | 24 +-- deploy/robot_ws/src/go2py_node/src/bridge.cpp | 19 ++- .../src/unitree/unitree_go/CMakeLists.txt | 1 + .../unitree/unitree_go/msg/Go2pyLowCmd.msg | 5 + examples/highlevel_ros2_interface.ipynb | 47 +++++- examples/lowlevel_ros2_interface.ipynb | 156 ++++++++++++++++++ 7 files changed, 224 insertions(+), 30 deletions(-) create mode 100644 deploy/robot_ws/src/unitree/unitree_go/msg/Go2pyLowCmd.msg create mode 100644 examples/lowlevel_ros2_interface.ipynb diff --git a/.gitignore b/.gitignore index 792aee9..39508c2 100644 --- a/.gitignore +++ b/.gitignore @@ -15,7 +15,7 @@ downloads/ eggs/ .eggs/ # lib/ -lib64/ +# lib64/ parts/ sdist/ var/ diff --git a/Go2Py/robot/interface/ros2.py b/Go2Py/robot/interface/ros2.py index d653e2f..05ed19d 100644 --- a/Go2Py/robot/interface/ros2.py +++ b/Go2Py/robot/interface/ros2.py @@ -12,7 +12,7 @@ from rclpy.executors import MultiThreadedExecutor from geometry_msgs.msg import TransformStamped from Go2Py.joy import xKeySwitch, xRockerBtn from geometry_msgs.msg import TwistStamped -from unitree_go.msg import LowState, LowCmd +from unitree_go.msg import LowState, Go2pyLowCmd from nav_msgs.msg import Odometry @@ -77,7 +77,7 @@ class GO2Real(Node): self.lowstate_subscriber = self.create_subscription( LowState, self.lowstate_topic, self.lowstate_callback, 1 ) - self.lowcmd_publisher = self.create_publisher(LowCmd, self.lowcmd_topic, 1) + self.lowcmd_publisher = self.create_publisher(Go2pyLowCmd, self.lowcmd_topic, 1) self.odometry_subscriber = self.create_subscription( Odometry, "/utlidar/robot_odom", self.odom_callback, 1 @@ -95,8 +95,8 @@ class GO2Real(Node): self.ωz_max = ωz_max self.ωz_min = -ωz_max self.running = True - self.setCommands = {'lowstate':self.setCommandsLow, - 'highstate':self.setCommandsHigh}[self.mode] + self.setCommands = {'lowlevel':self.setCommandsLow, + 'highlevel':self.setCommandsHigh}[self.mode] def lowstate_callback(self, msg): """ @@ -203,14 +203,14 @@ class GO2Real(Node): self.highcmd.twist.angular.z = _ω_z self.highcmd_publisher.publish(self.highcmd) - def setCommandsLow(q, dq, kp, kd, tau_ff): - assert q.size == qd.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12" - lowcmd = LowCmd() - lowcmd.motor_cmd.q = q.tolist() - lowcmd.motor_cmd.dq = dq.tolist() - lowcmd.motor_cmd.kp = kp.tolist() - lowcmd.motor_cmd.kd = kd.tolist() - lowcmd.motor_cmd.tau_ff = tau_ff.tolist() + def setCommandsLow(self, q, dq, kp, kd, tau_ff): + assert q.size == dq.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12" + lowcmd = Go2pyLowCmd() + lowcmd.q = q.tolist() + lowcmd.dq = dq.tolist() + lowcmd.kp = kp.tolist() + lowcmd.kd = kd.tolist() + lowcmd.tau = tau_ff.tolist() self.lowcmd_publisher.publish(lowcmd) def close(self): diff --git a/deploy/robot_ws/src/go2py_node/src/bridge.cpp b/deploy/robot_ws/src/go2py_node/src/bridge.cpp index 5c9607f..532c5b9 100644 --- a/deploy/robot_ws/src/go2py_node/src/bridge.cpp +++ b/deploy/robot_ws/src/go2py_node/src/bridge.cpp @@ -1,6 +1,7 @@ #include "unitree_go/msg/low_state.hpp" #include "unitree_go/msg/imu_state.hpp" #include "unitree_go/msg/motor_state.hpp" +#include "unitree_go/msg/go2py_low_cmd.hpp" #include "unitree_go/msg/low_cmd.hpp" #include "unitree_go/msg/motor_cmd.hpp" @@ -44,7 +45,7 @@ class Custom: public rclcpp::Node lowstate_suber = this->create_subscription( "lowstate", 1, std::bind(&Custom::lowstate_callback, this, std::placeholders::_1)); - lowcmd_suber = this->create_subscription( + lowcmd_suber = this->create_subscription( "/go2/lowcmd", 1, std::bind(&Custom::lowcmd_callback, this, std::placeholders::_1)); lowcmd_puber = this->create_publisher("/lowcmd", 10); @@ -71,9 +72,9 @@ class Custom: public rclcpp::Node // Lowlevel interface void lowstate_callback(unitree_go::msg::LowState::SharedPtr data); - void lowcmd_callback(unitree_go::msg::LowCmd::SharedPtr data); + void lowcmd_callback(unitree_go::msg::Go2pyLowCmd::SharedPtr data); rclcpp::Subscription::SharedPtr lowstate_suber; - rclcpp::Subscription::SharedPtr lowcmd_suber; + rclcpp::Subscription::SharedPtr lowcmd_suber; // A struct to store the highlevel states for later use rclcpp::Publisher::SharedPtr lowcmd_puber; unitree_go::msg::LowCmd lowcmd_msg; @@ -231,15 +232,15 @@ void Custom::twistCmdCallback(const geometry_msgs::msg::TwistStamped::SharedPtr highreq_puber->publish(highreq); } -void Custom::lowcmd_callback(unitree_go::msg::LowCmd::SharedPtr data) +void Custom::lowcmd_callback(unitree_go::msg::Go2pyLowCmd::SharedPtr data) { for(int i=0; i<12; i++) { - lowcmd_msg.motor_cmd[i].q = data->motor_cmd[i].q; // Taregt angular(rad) - lowcmd_msg.motor_cmd[i].kp = data->motor_cmd[i].kp; // Poinstion(rad) control kp gain - lowcmd_msg.motor_cmd[i].dq = data->motor_cmd[i].dq; // Taregt angular velocity(rad/ss) - lowcmd_msg.motor_cmd[i].kd = data->motor_cmd[i].kd; // Poinstion(rad) control kd gain - lowcmd_msg.motor_cmd[i].tau = data->motor_cmd[i].tau; // Feedforward toque 1N.m + lowcmd_msg.motor_cmd[i].q = data->q[i]; // Taregt angular(rad) + lowcmd_msg.motor_cmd[i].kp = data->kp[i]; // Poinstion(rad) control kp gain + lowcmd_msg.motor_cmd[i].dq = data->dq[i]; // Taregt angular velocity(rad/ss) + lowcmd_msg.motor_cmd[i].kd = data->kd[i]; // Poinstion(rad) control kd gain + lowcmd_msg.motor_cmd[i].tau = data->tau[i]; // Feedforward toque 1N.m get_crc(lowcmd_msg); //Compute the CRC and load it into the message lowcmd_puber->publish(lowcmd_msg); //Publish lowcmd message } diff --git a/deploy/robot_ws/src/unitree/unitree_go/CMakeLists.txt b/deploy/robot_ws/src/unitree/unitree_go/CMakeLists.txt index 2cab5df..9621ad6 100755 --- a/deploy/robot_ws/src/unitree/unitree_go/CMakeLists.txt +++ b/deploy/robot_ws/src/unitree/unitree_go/CMakeLists.txt @@ -48,6 +48,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/UwbState.msg" "msg/UwbSwitch.msg" "msg/WirelessController.msg" + "msg/Go2pyLowCmd.msg" DEPENDENCIES geometry_msgs ) diff --git a/deploy/robot_ws/src/unitree/unitree_go/msg/Go2pyLowCmd.msg b/deploy/robot_ws/src/unitree/unitree_go/msg/Go2pyLowCmd.msg new file mode 100644 index 0000000..216daf6 --- /dev/null +++ b/deploy/robot_ws/src/unitree/unitree_go/msg/Go2pyLowCmd.msg @@ -0,0 +1,5 @@ +float32[12] q +float32[12] dq +float32[12] kp +float32[12] kd +float32[12] tau \ No newline at end of file diff --git a/examples/highlevel_ros2_interface.ipynb b/examples/highlevel_ros2_interface.ipynb index 7c30b2d..0013b86 100644 --- a/examples/highlevel_ros2_interface.ipynb +++ b/examples/highlevel_ros2_interface.ipynb @@ -12,9 +12,18 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 2, "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.joy import Logitech3DPro\n", "# joy = Logitech3DPro(joy_id=0) " @@ -31,14 +40,23 @@ }, { "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.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n", "import time\n", "ros2_init()\n", - "robot = GO2Real()\n", + "robot = GO2Real(mode='lowlevel')\n", "ros2_exec_manager = ROS2ExecutorManager()\n", "ros2_exec_manager.add_node(robot)\n", "ros2_exec_manager.start()" @@ -71,11 +89,24 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 17, "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "text/plain": [ + "{'stamp_nanosec': 1707447716.9414463,\n", + " 'position': array([0.01655228, 0.00701126, 0.30197507]),\n", + " 'orientation': array([-0.00573182, 0.0074235 , 0.08994701, 0.99590236])}" + ] + }, + "execution_count": 17, + "metadata": {}, + "output_type": "execute_result" + } + ], "source": [ - "robot.getJointStates()" + "robot.getOdometry()" ] } ], diff --git a/examples/lowlevel_ros2_interface.ipynb b/examples/lowlevel_ros2_interface.ipynb new file mode 100644 index 0000000..c0cf79a --- /dev/null +++ b/examples/lowlevel_ros2_interface.ipynb @@ -0,0 +1,156 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Joystick Control\n", + "As the first step after successfully communicating with the robot's onboard controller, we use a USB joystick (a Logitech Extreme 3D Pro) to command the robot with desired $x$, $y$, and $\\Psi$ velocities.\n", + "\n", + "First, use the Pygame-based joystick class provided with B1Py to connect to the joystick:" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "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.joy import Logitech3DPro\n", + "# joy = Logitech3DPro(joy_id=0) " + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "***Note:*** Initially when you instantiate the joystick, the class performs an offset calibration to maker sure the neutral configuration of the device reads zero. It is important to leave the device untouched during the couple of seconds after calling the cell above. The prompts tells you when you can continue. \n", + "\n", + "Then as explained in [hardware interface documentation](unitree_locomotion_controller_interface.ipynb), instantiate and communication link to high-level controller on the robot. Note that the robot should be standing and ready to walk before we can control it. Furthermore, the the highlevel node in b1py_node ROS2 package (in `deploy/ros2_ws`) should be launched on the robot. " + ] + }, + { + "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.ros2 import GO2Real, ros2_init, ROS2ExecutorManager\n", + "import time\n", + "ros2_init()\n", + "robot = GO2Real(mode='lowlevel')\n", + "ros2_exec_manager = ROS2ExecutorManager()\n", + "ros2_exec_manager.add_node(robot)\n", + "ros2_exec_manager.start()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Finally, read the joystick values and send them to the robot in a loop:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "Duration = 20\n", + "N = Duration//0.01\n", + "for i in range(500):\n", + " cmd = joy.readAnalog()\n", + " vx = cmd['x']\n", + " vy = cmd['y']\n", + " w = cmd['z'] / 2\n", + " body_height = cmd['aux'] / 10\n", + " robot.setCommands(vx,vy,w, bodyHeight=body_height)\n", + " time.sleep(0.01)" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import time\n", + "for i in range(10000):\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.8\n", + " robot.setCommands(q, dq, kp, kd, tau)\n", + " time.sleep(0.01) " + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "{'q': array([-0.06477112, 1.24850821, -2.8002429 , 0.04539341, 1.25765204,\n", + " -2.78191853, -0.34121001, 1.27112007, -2.79908991, 0.34284496,\n", + " 1.26930332, -2.8161664 ]),\n", + " 'dq': array([ 0. , 0.01937762, -0.00606604, -0.01162657, 0. ,\n", + " 0.01819811, 0.01937762, 0. , 0.03033019, -0.05813286,\n", + " 0.00775105, 0.00808805])}" + ] + }, + "execution_count": 2, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "robot.getJointStates()" + ] + } + ], + "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 +}