1:实验准备
1:ros安装
确保已经安装好了ros，如果没有安装好，可以使用以下指令进行安装：
wget http://fishros.com/install -O fishros && . fishros
1.选1
2.选1(第一次选1,二次往后可1可2)
3.选择ros1 melodic/ros1 noetic (有哪个选哪个,20.04选noetic)
4.选桌面版,选1
5.ros安装完毕(要验证可以输入roscore验证)
ROS moetic版本效果如下
ROS noetic版本如下(如果是20.04效果如下)
2:依赖安装
1:依赖1
sudo apt-get install liblcm-dev
未安装可能出现的问题:后面编译部分报错但依然可进入拟环境,但模块控制不可用
2:依赖2
sudo apt-get install ros-noetic-controller-interface ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-effort-controllers ros-noetic-joint-trajectory-controller
可能出现的问题:Ubuntu版本>20.04则可能无法安装(因为g++等依赖版本太低)
3:创建工作空间
mkdir -p test_ws/src
4.进入工作空间的src
cd test_ws/src
5.unitree_legged_sdk
1.获取功能包
git clone https://github.com/unitreerobotics/unitree_legged_sdk.git
2.进入功能包并创建build文件
cd unitree_legged_sdk && mkdir build
3.进入build文件夹
cd build
4.编译功能包
cmake ..
make
5.构建 python安装包
cmake -DPYTHON_BUILD=TRUE ..
6.unitree_ros_to_real
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros_to_real.git
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
1:ros安装
确保已经安装好了ros，如果没有安装好，可以使用以下指令进行安装：
wget http://fishros.com/install -O fishros && . fishros
1.选1
2.选1(第一次选1,二次往后可1可2)
3.选择ros1 melodic/ros1 noetic (有哪个选哪个,20.04选noetic)
4.选桌面版,选1
5.ros安装完毕(要验证可以输入roscore验证)
ROS moetic版本效果如下
ROS noetic版本如下(如果是20.04效果如下)
2:依赖安装
1:依赖1
sudo apt-get install liblcm-dev
未安装可能出现的问题:后面编译部分报错但依然可进入拟环境,但模块控制不可用
2:依赖2
sudo apt-get install ros-noetic-controller-interface ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-effort-controllers ros-noetic-joint-trajectory-controller
可能出现的问题:Ubuntu版本>20.04则可能无法安装(因为g++等依赖版本太低)
3:创建工作空间
mkdir -p test_ws/src
4.进入工作空间的src
cd test_ws/src
5.unitree_legged_sdk
1.获取功能包
git clone https://github.com/unitreerobotics/unitree_legged_sdk.git
2.进入功能包并创建build文件
cd unitree_legged_sdk && mkdir build
3.进入build文件夹
cd build
4.编译功能包
cmake ..
make
5.构建 python安装包
cmake -DPYTHON_BUILD=TRUE ..
6.unitree_ros_to_real
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros_to_real.git
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
1.选1
2.选1(第一次选1,二次往后可1可2)
3.选择ros1 melodic/ros1 noetic (有哪个选哪个,20.04选noetic)
4.选桌面版,选1
5.ros安装完毕(要验证可以输入roscore验证)
ROS moetic版本效果如下
ROS noetic版本如下(如果是20.04效果如下)
2:依赖安装
1:依赖1
sudo apt-get install liblcm-dev
未安装可能出现的问题:后面编译部分报错但依然可进入拟环境,但模块控制不可用
2:依赖2
sudo apt-get install ros-noetic-controller-interface ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-effort-controllers ros-noetic-joint-trajectory-controller
可能出现的问题:Ubuntu版本>20.04则可能无法安装(因为g++等依赖版本太低)
3:创建工作空间
mkdir -p test_ws/src
4.进入工作空间的src
cd test_ws/src
5.unitree_legged_sdk
1.获取功能包
git clone https://github.com/unitreerobotics/unitree_legged_sdk.git
2.进入功能包并创建build文件
cd unitree_legged_sdk && mkdir build
3.进入build文件夹
cd build
4.编译功能包
cmake ..
make
5.构建 python安装包
cmake -DPYTHON_BUILD=TRUE ..
6.unitree_ros_to_real
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros_to_real.git
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
1:依赖1
sudo apt-get install liblcm-dev
未安装可能出现的问题:后面编译部分报错但依然可进入拟环境,但模块控制不可用
2:依赖2
sudo apt-get install ros-noetic-controller-interface ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-effort-controllers ros-noetic-joint-trajectory-controller
可能出现的问题:Ubuntu版本>20.04则可能无法安装(因为g++等依赖版本太低)
3:创建工作空间
mkdir -p test_ws/src
4.进入工作空间的src
cd test_ws/src
5.unitree_legged_sdk
1.获取功能包
git clone https://github.com/unitreerobotics/unitree_legged_sdk.git
2.进入功能包并创建build文件
cd unitree_legged_sdk && mkdir build
3.进入build文件夹
cd build
4.编译功能包
cmake ..
make
5.构建 python安装包
cmake -DPYTHON_BUILD=TRUE ..
6.unitree_ros_to_real
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros_to_real.git
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
未安装可能出现的问题:后面编译部分报错但依然可进入拟环境,但模块控制不可用
2:依赖2
sudo apt-get install ros-noetic-controller-interface ros-noetic-gazebo-ros-control ros-noetic-joint-state-controller ros-noetic-effort-controllers ros-noetic-joint-trajectory-controller
可能出现的问题:Ubuntu版本>20.04则可能无法安装(因为g++等依赖版本太低)
3:创建工作空间
mkdir -p test_ws/src
4.进入工作空间的src
cd test_ws/src
5.unitree_legged_sdk
1.获取功能包
git clone https://github.com/unitreerobotics/unitree_legged_sdk.git
2.进入功能包并创建build文件
cd unitree_legged_sdk && mkdir build
3.进入build文件夹
cd build
4.编译功能包
cmake ..
make
5.构建 python安装包
cmake -DPYTHON_BUILD=TRUE ..
6.unitree_ros_to_real
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros_to_real.git
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
可能出现的问题:Ubuntu版本>20.04则可能无法安装(因为g++等依赖版本太低)
3:创建工作空间
mkdir -p test_ws/src
4.进入工作空间的src
cd test_ws/src
5.unitree_legged_sdk
1.获取功能包
git clone https://github.com/unitreerobotics/unitree_legged_sdk.git
2.进入功能包并创建build文件
cd unitree_legged_sdk && mkdir build
3.进入build文件夹
cd build
4.编译功能包
cmake ..
make
5.构建 python安装包
cmake -DPYTHON_BUILD=TRUE ..
6.unitree_ros_to_real
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros_to_real.git
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
4.进入工作空间的src
cd test_ws/src
5.unitree_legged_sdk
1.获取功能包
git clone https://github.com/unitreerobotics/unitree_legged_sdk.git
2.进入功能包并创建build文件
cd unitree_legged_sdk && mkdir build
3.进入build文件夹
cd build
4.编译功能包
cmake ..
make
5.构建 python安装包
cmake -DPYTHON_BUILD=TRUE ..
6.unitree_ros_to_real
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros_to_real.git
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
5.unitree_legged_sdk
1.获取功能包
git clone https://github.com/unitreerobotics/unitree_legged_sdk.git
2.进入功能包并创建build文件
cd unitree_legged_sdk && mkdir build
3.进入build文件夹
cd build
4.编译功能包
cmake ..
make
5.构建 python安装包
cmake -DPYTHON_BUILD=TRUE ..
6.unitree_ros_to_real
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros_to_real.git
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
1.获取功能包
git clone https://github.com/unitreerobotics/unitree_legged_sdk.git
2.进入功能包并创建build文件
cd unitree_legged_sdk && mkdir build
3.进入build文件夹
cd build
4.编译功能包
cmake ..
make
5.构建 python安装包
cmake -DPYTHON_BUILD=TRUE ..
6.unitree_ros_to_real
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros_to_real.git
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
2.进入功能包并创建build文件
cd unitree_legged_sdk && mkdir build
3.进入build文件夹
cd build
4.编译功能包
cmake ..
make
5.构建 python安装包
cmake -DPYTHON_BUILD=TRUE ..
6.unitree_ros_to_real
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros_to_real.git
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
3.进入build文件夹
cd build
4.编译功能包
cmake ..
make
5.构建 python安装包
cmake -DPYTHON_BUILD=TRUE ..
6.unitree_ros_to_real
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros_to_real.git
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
4.编译功能包
cmake ..
make
5.构建 python安装包
cmake -DPYTHON_BUILD=TRUE ..
6.unitree_ros_to_real
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros_to_real.git
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
make
5.构建 python安装包
cmake -DPYTHON_BUILD=TRUE ..
6.unitree_ros_to_real
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros_to_real.git
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
6.unitree_ros_to_real
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros_to_real.git
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros_to_real.git
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
7.unitree_ros
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_ros.git
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
8.unitree_guide
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
1.获取功能包
注意：是在工作空间的src文件下执行
git clone https://github.com/unitreerobotics/unitree_guide.git
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
9.编译工作空间
catkin_make
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
1.如果一切顺利，效果将如下：
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
2.问题及解决：
我在test_ws工作空间下进行编译时就出现了以下问题
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
如何解决：
rm -rf build devel/
随后编译 ：
catkin_make
3.source
source devel/setup.bash
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
至此，准备工作就已经全部完成 。
2.仿真启动
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
1.启动
roslaunch unitree_guide gazeboSim.launch rname:=go2
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
2.问题与解决:
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
1.问题
在执行的时候如果在anconda的虚拟环境当中，就有可能会运行失败：
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
2.解决方法
conda deactivate
roslaunch unitree_guide gazeboSim.launch rname:=go2
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
3.文件修改
尽管使用最开始的命令也能使用，但我更推荐对gazeboSim.launch文件进行修改:
cd src/unitree_guide/unitree_guide/launch/
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
打开文件：
1.手动寻找文件
2.gedit法
gedit gazeboSim.launch
3.vim法（对不熟悉的人较麻烦的方法）
未安装vim是无法使用的
安装如下：
sudo apt install vim
vim gazeboSim.launch
4.将go1修改为go2
3.启动控制
./devel/lib/unitree_guide/junior_ctrl
按键2是站立，按键4是行走模式，按下4键后就可以使用方向wasd键进行运动控制了，jl进行转向控制。
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
问题：
在我使用虚拟机进行腿部控制的时候会出现如下问题，但使用实体机并未出现，暂时还未知是何种原因导致。
初步判断为虚拟机性能导致，因此在这里推荐使用实体机进行实验。
4.排错
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
以上错误由于test_ws/src/unitree_ros-master/robots/go2_description/xacro/robot.xacro中的mech标签使用了一个不存在的三维模型路径,将其中的
mesh filename="package://go2_description/meshes/trunk.dae" scale="1 1 1"/>
改为
mesh filename="package://go2_description/meshes/base.dae" scale="1 1 1"/>
即可
4.进阶控制
先前所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist 类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。
宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~test_ws/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py
将第1行中 version4.14和第11行中的set(MOVE_BASE OFF) 改为 version3.14和第11行中的set(MOVE_BASE ON)，然后重新编译该包：
cd ~/test_ws
# 单独编译 unitree guide 包
catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"
以下为控制脚本go2.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import select
import termios
import tty
import rospy
from geometry_msgs.msg import Twist
HELP_TEXT = """
=========================================================
键盘控制说明 Keyboard Control Help
=========================================================
移动控制（平移）:
q w e 左前 前进 右前
a s d 左移 停止 右移
z x c 左后 后退 右后
对角线移动（45°平移）:
f g h b 左前45° 右前45° 左后45° 右后45°
旋转控制（原地旋转）:
r t 左转 右转
速度调节:
u / i : 增加 / 减少 线速度
j / k : 增加 / 减少 角速度
m / , : 同时调整 线速度和角速度
其他:
其他按键 : 停止运动
CTRL + C : 退出程序
=========================================================
"""
class KeyboardTeleop:
def __init__(self):
rospy.init_node("keyboard_control", anonymous=True)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
# 参数
self.max_lin = rospy.get_param("~speed", 0.8)
self.max_ang = rospy.get_param("~turn", 0.8)
# 速度状态
self.target = [0.0, 0.0, 0.0, 0.0]
self.current = [0.0, 0.0, 0.0, 0.0]
self.acc_step = 0.1
self.dec_step = 0.1
# 键位绑定
self.motion_map = {
'w': (1, 0, 0, 0),
'e': (1, 0, 0, -1),
'a': (0, 1, 0, 0),
'd': (0, -1, 0, 0),
'q': (1, 0, 0, 1),
'x': (-1, 0, 0, 0),
'c': (-1, 0, 0, 1),
'z': (-1, 0, 0, -1),
'r': (0, 0, 0, 1),
't': (0, 0, 0, -1),
'f': (1, 1, 0, 0),
'h': (-1, 1, 0, 0),
'g': (1, -1, 0, 0),
'b': (-1, -1, 0, 0),
}
self.speed_map = {
'u': (1.1, 1.0),
'i': (0.9, 1.0),
'j': (1.0, 1.1),
'k': (1.0, 0.9),
'm': (1.1, 1.1),
',': (0.9, 0.9),
}
self.settings = termios.tcgetattr(sys.stdin)
self.last_key = None
print("\033[2J\033[H")
print(HELP_TEXT)
# ---------------- 键盘读取 ----------------
def read_key(self):
tty.setraw(sys.stdin.fileno())
readable, _, _ = select.select([sys.stdin], [], [], 0.1)
key = sys.stdin.read(1) if readable else ""
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
return key
# ---------------- 插值平滑 ----------------
@staticmethod
def smooth(curr, tgt, acc, dec):
step = acc if curr < tgt else dec
if abs(curr - tgt) <= step:
return tgt
return curr + step if curr < tgt else curr - step
# ---------------- 状态显示 ----------------
def display(self, key):
print("\033[25;0H", end="")
print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang))
print("\033[K[ Key ] %s" % (key if key else "None"))
print("\033[K--------------------------------------------------")
print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target))
print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current))
print("\033[K--------------------------------------------------", end="")
# ---------------- Twist 发布 ----------------
def publish_twist(self):
msg = Twist()
msg.linear.x = self.current[0] * self.max_lin
msg.linear.y = self.current[1] * self.max_lin
msg.linear.z = self.current[2] * self.max_lin
msg.angular.z = self.current[3] * self.max_ang
self.pub.publish(msg)
# ---------------- 主循环 ----------------
def spin(self):
rate = rospy.Rate(20)
while not rospy.is_shutdown():
key = self.read_key()
if key in self.motion_map:
self.target = list(self.motion_map[key])
self.dec_step = 0.1
elif key in self.speed_map:
self.max_lin *= self.speed_map[key][0]
self.max_ang *= self.speed_map[key][1]
else:
self.target = [0.0] * 4
self.dec_step = 0.15
if key == "\x03":
break
for i in range(4):
self.current[i] = self.smooth(
self.current[i],
self.target[i],
self.acc_step,
self.dec_step
)
self.display(key)
self.publish_twist()
rate.sleep()
self.shutdown()
# ---------------- 退出清理 ----------------
def shutdown(self):
stop = Twist()
self.pub.publish(stop)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
rospy.loginfo("[keyboard_control] Exit.")
if __name__ == "__main__":
try:
KeyboardTeleop().spin()
except Exception as e:
rospy.logerr(e)
以下给出整个运动控制流程：
chmod +x go.py
# 开启gazebo仿真器
roslaunch unitree_guide gazeboSim.launch rname:=go2
# 开启底层运动控制节点
# 在该终端按下按键2站立，然后按下按键5进入导航模式
rosrun unitree_guide junior_ctrl
# 启动控制命令脚本
python3 go.py