目录
1. 功能介绍
2. 准备
3. 编写思路
1.基础准备
2.运动算法
3.最终代码
4. 结果展示
1. 功能介绍
本次blog将实现使用电脑自带键盘进行对机器🐶的精细操控。玩过🐶的都知道，手柄操控机器🐶实现精准操控十分困难，本程序用键盘实现机器🐶的前进后退，向左转右转，以及很多动作的实现。
移动按键
w：向前移动
e：向后并向左移动（普通模式下）
a：向左转
d：向右转
q：向前并向右转动（普通模式下）
s：向后移动
c：向后并向左移动（全向模式下，和e相对）
z：向前并向右转动（全向模式下，和q相对）
当按下shift键时，机器狗进入全向模式（Holonmic mode），允许进行侧向移动（strafing）
W：向前移动（全向模式与w相同）
E：向左移动
A：向后并向左移动
D：向后并向右移动
Q：向右移动
S：向后移动
C：向前并向左移动
Z：向前并向右转动
速度调整按键
r：增加最大速度和转向速度10%
t：减少最大速度和转向速度10%
f：仅增加线性速度10%（不影响转向速度）
g：仅减少线性速度10%
v：仅增加转向速度10%（线性速度不变）
b：仅减少转向速度10%
运动模式切换
h：打招呼
j：前跳
k：伸懒腰
n：坐下
m：坐下后恢复站立
y：跳舞1
u：跳舞2
2. 准备
由于python的兼容性和语法简单，使用python来实现功能。在vscode的命令行中跳转到/unitree_go2_ws/src/base目录下，输入以下指令，创建功能包：
ros2 pkg create go2_teleop_ctrl_keyboard –build-type ament_python –dependencies rclpy unitree_api –node-name go2_teleop_ctrl_keyboard
然后打开go2_teleop_ctrl_keyboard.py文件，进行编写文件。
3. 编写思路
1.基础准备
首先创立常量字典，将动作和对应的编号联系起来，更具有易读性：
ROBOT_SPORT_API_IDS = {
"DAMP":1001, #阻尼控制
"BALANCESTAND":1002, #平衡站立
"STOPMOVE":1003, #停止运动
"STANDUP":1004, #站立
"STANDDOWN":1005, #站立下降
"RECOVERYSTAND":1006, #恢复站立
"EULER":1007, #欧拉角控制
"MOVE":1008, #移动
"SIT":1009, #坐下
"RISESIT":1010, #从坐下恢复站立
"SWITCHGAIT":1011, #切换步态
"TRIGGER":1012, #触发
"BODYHEIGHT":1013, #身体高度调整
"FOOTTRAISEHEIGHT":1014, #脚步抬起高度调整
"SPEEDLEVEL":1015, #速度级别调整
"HELLO":1016, #打招呼
"STRETCH":1017, #伸懒腰
"TRAJECTORYEOLLOW":1018, #轨迹跟随
"CONTINUOUSGAIT":1019, #连续步态
"CONTENT":1020, #内容
"WALLOW":1021, #打滚
"DANCE1":1022, #舞蹈1
"DANCE2":1023, #舞蹈2
"GETBODYHEIGHT":1024, #获取身体高度
"GETFOOTRAISEHEIGHT":1025, #获取脚部抬起高度
"GETSPEEDLEVEL":1026, #获取速度级别
"SWITCHJOYSTICK":1027, #切换操纵杆
"POSE":1028, #姿态
"SCRAPE":1029, #刮擦
"FRONTFLIP":1030, #前空翻
"FRONTJUMP":1031, #前跳
"FRONTPOUNCE":1032, #前扑
}
SPORT_MODE = {
# h：打招呼
# j：前跳
# k：伸懒腰
# n：坐下
# m：坐下后恢复站立
# y：跳舞1
# u：跳舞2
'h':ROBOT_SPORT_API_IDS["HELLO"],
'j':ROBOT_SPORT_API_IDS["FRONTJUMP"],
'k':ROBOT_SPORT_API_IDS["STRETCH"],
'n':ROBOT_SPORT_API_IDS["SIT"],
'm':ROBOT_SPORT_API_IDS["RISESIT"],
'y':ROBOT_SPORT_API_IDS["DANCE1"],
'u':ROBOT_SPORT_API_IDS["DANCE2"],
}
moveBindings = {
# w：向前移动
# e：向后并向左移动（普通模式下）
# a：向左转
# d：向右转
# q：向前并向右转动（普通模式下）
# s：向后移动
# c：向后并向左移动（全向模式下，和e相对）
# z：向前并向右转动（全向模式下，和q相对）
# 当按下shift键时，机器狗进入全向模式（Holonmic mode），允许进行侧向移动（strafing）
# W：向前移动（全向模式与w相同）
# E：向前并向左移动
# A：向左移动
# D：向右移动
# Q：向前并向左移动
# S：停止移动
# C：向后并向右移动
# Z：向后并向左转动
#(x,y,z,theta)的比例系数
'w':(1,0,0,0),
'e':(1,0,0,-1),
'a':(0,0,0,1),
'd':(0,0,0,-1),
'q':(1,0,0,1),
's':(-1,0,0,0),
'c':(-1,0,0,1),
'z':(-1,0,0,-1),
'W':(1,0,0,0),
'E':(1,-1,0,0),
'A':(0,1,0,0),
'D':(0,-1,0,0),
'Q':(1,1,0,0),
'S':(-1,0,0,0),
'C':(-1,-1,0,0),
'Z':(-1,1,0,0),
}
speedBindings = {
# r：增加最大速度和转向速度10%
# t：减少最大速度和转向速度10%
# f：仅增加线性速度10%（不影响转向速度）
# g：仅减少线性速度10%
# v：仅增加转向速度10%（线性速度不变）
# b：仅减少转向速度10%
'r':(1.1,1.1), #speed * 1.1 ， angular * 1.1
't':(.9,.9),
'f':(1.1,1),
'g':(.9,1),
'v':(1,1.1),
'b':(1,.9),
}
这些动作在宇树的开发文档中都能找到，还有其他动作请自己去看开发文档哦
然后要创建文档提示，不然拿到手一脸蒙：
msg = """
this node takes keypreess from keyboard and pulishers them
as unitree_api/'msg/Request messages.It works best with a US keyboard layout.
————————————-
Moving around:
q w e
—————————
a s d
—————————
z x c
For Holonomic mode (strafing),hold down the shift key:
————————————-
Q W E
—————————
A S D
—————————
Z X C
anything else：stop
r/t：increase/decrease max speed by 10%
f/g：increase/decrease only linear speed by 10%
v/b：increase/decrease only angular speed by 10%
h：Greet
j：Front jump
k：Stretch
n：Sit Down
m：Stand Up from Sitting
y：Dance 1
u：Dance 2
CTRL-C to Quit
"""
在main()函数开头打印，让人一目了然。
2.运动算法
首先初始化线速度和角速度：
self.declare_parameter("speed",0.2) #设初始 x 和 y 线速度一致
self.declare_parameter("angular",1)
self.speed = self.get_parameter("speed").value
self.angular = self.get_parameter("angular").value
这是ROS2的语法哦，创建参数，不记得去复习去。
这个算法中对机器狗运动学建模，坐标系设置为：
使用的运动控制算法类似差速控制，分别给初始线速度和角速度赋三进制值1,0,-1，从而达到不同的控制效果。机器🐶的速度为(x,y,z,theta),为x，y，z方向的线速度和角速度。由于机器🐶没装引擎，飞不起来，在本程序中z轴一直为0，所以控制运动方向的算法为：
#3.运动控制 (设置运动消息和api_id) 虽然运动模式多，但基本原理为： x 线速度 + y 线速度 + z 角速度
elif key in moveBindings.keys():
x_bind = moveBindings[key][0]
y_bind = moveBindings[key][1]
z_bind = moveBindings[key][3]
teleopNode.publish(ROBOT_SPORT_API_IDS["MOVE"],
x = x_bind * teleopNode.speed,
y = y_bind * teleopNode.speed,
z = z_bind * teleopNode.angular
)
本程序还用了键盘读取的导包文件，这个不多讲，去deepseek一下。从上面代码可以显而易见的看出键盘按下不同的按键，会有不同的方向有速度值。
还可以进行运动模式的切换，当小写键时，为普通模式，a和d键为左右旋转。当大写键时，为全向模式，即蟹行模式，可以左右，正反对角线移动，切换代码如下：
#2.运动模式切换（设置api_id）
elif key in SPORT_MODE.keys():
teleopNode.publish(SPORT_MODE[key])
这里还要讲一下publish函数，这是封装好的向机器🐶发送指令的函数，采用ROS2发布和订阅的知识点，不会的也要去复习了。
#发布速度指令
def publish(self,api_id,x=0.0,y=0.0,z=0.0):
req = Request()
req.header.identity.api_id = api_id
js = {"x":x,"y":y,"z":z}
req.parameter = json.dumps(js);
self.pub.publish(req)
由于，机器🐶的发布消息格式Request，其parameter为元组格式，故需要用json包进行转换。
还有些粗枝细节就不讲了，自己理解一下代码吧。
3.最终代码
'''
需求：编写键盘控制节点，控制go2运动
规则：
移动按键
w：向前移动
e：向后并向左移动（普通模式下）
a：向左转
d：向右转
q：向前并向右转动（普通模式下）
s：向后移动
c：向后并向左移动（全向模式下，和e相对）
z：向前并向右转动（全向模式下，和q相对）
当按下shift键时，机器狗进入全向模式（Holonmic mode），允许进行侧向移动（strafing）
W：向前移动（全向模式与w相同）
E：向左移动
A：向后并向左移动
D：向后并向右移动
Q：向右移动
S：停止移动
C：向前并向左移动
Z：向前并向右转动
速度调整按键
r：增加最大速度和转向速度10%
t：减少最大速度和转向速度10%
f：仅增加线性速度10%（不影响转向速度）
g：仅减少线性速度10%
v：仅增加转向速度10%（线性速度不变）
b：仅减少转向速度10%
运动模式切换
h：打招呼
j：前跳
k：伸懒腰
n：坐下
m：坐下后恢复站立
y：跳舞1
u：跳舞2
流程：
1.引入读取键盘录入的第三方
2.编写ROS2节点
3.编写按键映射逻辑
'''
#1.导包
import rclpy
from rclpy.node import Node
import termios
import sys
import tty
import threading
from unitree_api.msg import Request
import json
msg = """
this node takes keypreess from keyboard and pulishers them
as unitree_api/'msg/Request messages.It works best with a US keyboard layout.
————————————-
Moving around:
q w e
—————————
a s d
—————————
z x c
For Holonomic mode (strafing),hold down the shift key:
————————————-
Q W E
—————————
A S D
—————————
Z X C
anything else：stop
r/t：increase/decrease max speed by 10%
f/g：increase/decrease only linear speed by 10%
v/b：increase/decrease only angular speed by 10%
h：Greet
j：Front jump
k：Stretch
n：Sit Down
m：Stand Up from Sitting
y：Dance 1
u：Dance 2
CTRL-C to Quit
"""
#定义常量字典
ROBOT_SPORT_API_IDS = {
"DAMP":1001, #阻尼控制
"BALANCESTAND":1002, #平衡站立
"STOPMOVE":1003, #停止运动
"STANDUP":1004, #站立
"STANDDOWN":1005, #站立下降
"RECOVERYSTAND":1006, #恢复站立
"EULER":1007, #欧拉角控制
"MOVE":1008, #移动
"SIT":1009, #坐下
"RISESIT":1010, #从坐下恢复站立
"SWITCHGAIT":1011, #切换步态
"TRIGGER":1012, #触发
"BODYHEIGHT":1013, #身体高度调整
"FOOTTRAISEHEIGHT":1014, #脚步抬起高度调整
"SPEEDLEVEL":1015, #速度级别调整
"HELLO":1016, #打招呼
"STRETCH":1017, #伸懒腰
"TRAJECTORYEOLLOW":1018, #轨迹跟随
"CONTINUOUSGAIT":1019, #连续步态
"CONTENT":1020, #内容
"WALLOW":1021, #打滚
"DANCE1":1022, #舞蹈1
"DANCE2":1023, #舞蹈2
"GETBODYHEIGHT":1024, #获取身体高度
"GETFOOTRAISEHEIGHT":1025, #获取脚部抬起高度
"GETSPEEDLEVEL":1026, #获取速度级别
"SWITCHJOYSTICK":1027, #切换操纵杆
"POSE":1028, #姿态
"SCRAPE":1029, #刮擦
"FRONTFLIP":1030, #前空翻
"FRONTJUMP":1031, #前跳
"FRONTPOUNCE":1032, #前扑
}
SPORT_MODE = {
# h：打招呼
# j：前跳
# k：伸懒腰
# n：坐下
# m：坐下后恢复站立
# y：跳舞1
# u：跳舞2
'h':ROBOT_SPORT_API_IDS["HELLO"],
'j':ROBOT_SPORT_API_IDS["FRONTJUMP"],
'k':ROBOT_SPORT_API_IDS["STRETCH"],
'n':ROBOT_SPORT_API_IDS["SIT"],
'm':ROBOT_SPORT_API_IDS["RISESIT"],
'y':ROBOT_SPORT_API_IDS["DANCE1"],
'u':ROBOT_SPORT_API_IDS["DANCE2"],
}
moveBindings = {
# w：向前移动
# e：向后并向左移动（普通模式下）
# a：向左转
# d：向右转
# q：向前并向右转动（普通模式下）
# s：向后移动
# c：向后并向左移动（全向模式下，和e相对）
# z：向前并向右转动（全向模式下，和q相对）
# 当按下shift键时，机器狗进入全向模式（Holonmic mode），允许进行侧向移动（strafing）
# W：向前移动（全向模式与w相同）
# E：向前并向左移动
# A：向左移动
# D：向右移动
# Q：向前并向左移动
# S：停止移动
# C：向后并向右移动
# Z：向后并向左转动
#(x,y,z,theta)的比例系数
'w':(1,0,0,0),
'e':(1,0,0,-1),
'a':(0,0,0,1),
'd':(0,0,0,-1),
'q':(1,0,0,1),
's':(-1,0,0,0),
'c':(-1,0,0,1),
'z':(-1,0,0,-1),
'W':(1,0,0,0),
'E':(1,-1,0,0),
'A':(0,1,0,0),
'D':(0,-1,0,0),
'Q':(1,1,0,0),
'S':(-1,0,0,0),
'C':(-1,-1,0,0),
'Z':(-1,1,0,0),
}
speedBindings = {
# r：增加最大速度和转向速度10%
# t：减少最大速度和转向速度10%
# f：仅增加线性速度10%（不影响转向速度）
# g：仅减少线性速度10%
# v：仅增加转向速度10%（线性速度不变）
# b：仅减少转向速度10%
'r':(1.1,1.1), #speed * 1.1 ， angular * 1.1
't':(.9,.9),
'f':(1.1,1),
'g':(.9,1),
'v':(1,1.1),
'b':(1,.9),
}
# 3.自定义节点类：
class TeleopNode(Node):
def __init__(self):
super().__init__("teleopnode_node_py")
self.pub = self.create_publisher(Request,"/api/sport/request",10)
#以参数的方式设置线速度角速度初始值
self.declare_parameter("speed",0.2) #设初始 x 和 y 线速度一致
self.declare_parameter("angular",1)
self.speed = self.get_parameter("speed").value
self.angular = self.get_parameter("angular").value
#发布速度指令
def publish(self,api_id,x=0.0,y=0.0,z=0.0):
req = Request()
req.header.identity.api_id = api_id
js = {"x":x,"y":y,"z":z}
req.parameter = json.dumps(js);
self.pub.publish(req)
def get_Key(settings):
#设置读取模式
tty.setraw(sys.stdin.fileno())
#获取一个按键
key = sys.stdin.read(1)
#恢复终端原始设置
termios.tcsetattr(sys.stdin,termios.TCSADRAIN,settings)
#返回按键值
return key
def main():
print(msg)
#读取键盘录入实现
#1.获取标准输入流的属性并返回
settings = termios.tcgetattr(sys.stdin)
#2.ros2 node实现，需要单独子线程处理
rclpy.init()
#子线程处理
teleopNode = TeleopNode()
spinner = threading.Thread(target=rclpy.spin,args=(teleopNode,))
spinner.start()
#3.循环读取按键，并映射
#加个异常处理，利用finally保证资源释放
try:
while True:
key = get_Key(settings)
# if key == 'h':
# teleopNode.publish(1016)
#1.结束终端判断 Ctrl + c —> 🐶站立平衡
if key == '\\x03':
teleopNode.publish(ROBOT_SPORT_API_IDS["BALANCESTAND"])
break
#2.运动模式切换（设置api_id）
elif key in SPORT_MODE.keys():
teleopNode.publish(SPORT_MODE[key])
#3.运动控制 (设置运动消息和api_id) 虽然运动模式多，但基本原理为： x 线速度 + y 线速度 + z 角速度
elif key in moveBindings.keys():
x_bind = moveBindings[key][0]
y_bind = moveBindings[key][1]
z_bind = moveBindings[key][3]
teleopNode.publish(ROBOT_SPORT_API_IDS["MOVE"],
x = x_bind * teleopNode.speed,
y = y_bind * teleopNode.speed,
z = z_bind * teleopNode.angular
)
#4.速度调整 (设置运动消息和api_id)
elif key in speedBindings.keys():
s_bind = speedBindings[key][0] #线速度比例值
a_bind = speedBindings[key][1] #角速度比例值
teleopNode.speed = s_bind * teleopNode.speed
teleopNode.angular = a_bind * teleopNode.angular
print("current_speed:%.5f，angular:%.5f" %(teleopNode.speed ,teleopNode.angular))
else:
#停止运动
teleopNode.publish(ROBOT_SPORT_API_IDS["BALANCESTAND"])
finally:
rclpy.shutdown()
#🐶进入站立平衡状态
teleopNode.publish(ROBOT_SPORT_API_IDS["BALANCESTAND"])
if __name__ == '__main__':
main()
然后在code的终端回到/unitree_go2_ws下，输入以下指令进行编译并执行：
colcon build –packages-select go2_teleop_ctrl_keyboard
. install/setup.bash
ros2 run go2_teleop_ctrl_keyboard go2_teleop_ctrl_keyboard
随后就可以开始用键盘控制机器🐶了。
4. 结果展示
按下k：
机器🐶伸懒腰
蟹行模式：
蟹行机器🐶
按下y，跳舞1：
机器🐶跳舞1
按下u，跳舞2：
机器🐶跳舞2
其他的就不展示了。
敬请期待后续开发。
