EmbodiedAI
/
Go2Py_SIM
mirror of https://github.com/machines-in-motion/Go2Py.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ROS2 lowstate interface and corresponding example added.

This commit is contained in:
Rooholla-KhorramBakht 2024-02-08 22:02:42 -05:00
parent f163798f20
commit c57c81290c
7 changed files with 224 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

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

24
Go2Py/robot/interface/ros2.py
View File

@ -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):

19
deploy/robot_ws/src/go2py_node/src/bridge.cpp
View File

@ -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].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
}

1
deploy/robot_ws/src/unitree/unitree_go/CMakeLists.txt
View File

@ -48,6 +48,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/UwbState.msg"
"msg/UwbSwitch.msg"
"msg/WirelessController.msg"
"msg/Go2pyLowCmd.msg"
DEPENDENCIES geometry_msgs
)

5
deploy/robot_ws/src/unitree/unitree_go/msg/Go2pyLowCmd.msg Normal file
View File

@ -0,0 +1,5 @@
float32[12] q
float32[12] dq
float32[12] kp
float32[12] kd
float32[12] tau

47
examples/highlevel_ros2_interface.ipynb
View File

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

156
examples/lowlevel_ros2_interface.ipynb Normal file
View File

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