ROS2 lowstate interface and corresponding example added.

.gitignore

@@ -15,7 +15,7 @@ downloads/
 eggs/
 .eggs/
 # lib/
-lib64/
+# lib64/
 parts/
 sdist/
 var/

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,
+        self.setCommands = {'lowlevel':self.setCommandsLow,
-                            'highstate':self.setCommandsHigh}[self.mode]
+                            '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):
+    def setCommandsLow(self, 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"
+        assert q.size == dq.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12"
-        lowcmd = LowCmd()
+        lowcmd = Go2pyLowCmd()
-        lowcmd.motor_cmd.q = q.tolist()
+        lowcmd.q = q.tolist()
-        lowcmd.motor_cmd.dq = dq.tolist()
+        lowcmd.dq = dq.tolist()
-        lowcmd.motor_cmd.kp = kp.tolist()
+        lowcmd.kp = kp.tolist()
-        lowcmd.motor_cmd.kd = kd.tolist()
+        lowcmd.kd = kd.tolist()
-        lowcmd.motor_cmd.tau_ff = tau_ff.tolist()
+        lowcmd.tau = tau_ff.tolist()
         self.lowcmd_publisher.publish(lowcmd)
 
     def close(self):

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<unitree_go::msg::LowState>(
             "lowstate", 1, std::bind(&Custom::lowstate_callback, this, std::placeholders::_1));
             
-            lowcmd_suber = this->create_subscription<unitree_go::msg::LowCmd>(
+            lowcmd_suber = this->create_subscription<unitree_go::msg::Go2pyLowCmd>(
             "/go2/lowcmd", 1, std::bind(&Custom::lowcmd_callback, this, std::placeholders::_1));
 
             lowcmd_puber = this->create_publisher<unitree_go::msg::LowCmd>("/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<unitree_go::msg::LowState>::SharedPtr lowstate_suber;
-        rclcpp::Subscription<unitree_go::msg::LowCmd>::SharedPtr lowcmd_suber;
+        rclcpp::Subscription<unitree_go::msg::Go2pyLowCmd>::SharedPtr lowcmd_suber;
         // A struct to store the highlevel states for later use
         rclcpp::Publisher<unitree_go::msg::LowCmd>::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].q =   data->q[i];   // Taregt angular(rad)
-        lowcmd_msg.motor_cmd[i].kp =  data->motor_cmd[i].kp;  // Poinstion(rad) control kp gain
+        lowcmd_msg.motor_cmd[i].kp =  data->kp[i];  // 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].dq =  data->dq[i];  // 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].kd =  data->kd[i];  // 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].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
     }

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
 )
 

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    

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()"
    ]
   }
  ],

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
+}