键盘控制案例

描述

使用键盘控制机器人的运动
使用键盘控制机器人手臂移动
示例代码路径:<kuavo-ros-opensource>/src/kuavo_sdk/scripts/keyboard_control/robot_keyboard_control.py

程序逻辑

程序初始化
- 程序启动时，初始化ROS节点，节点名称为simulated_joystick
- 创建一个SimulatedJoystick类的实例
- 订阅/stop_robot话题，用于接收停止机器人的信号
- 使用termios库设置键盘输入为非阻塞模式，以便实时读取键盘输入
- 手臂移动控制
  - 读取传感器数据得到当前各关节角度,再通过fk正解,得到初始末端位姿
  - 初始化程序参数，储存机器人的末端位姿期望信息
  - 调用termios库设置键盘输入模式，实时读取键盘输入
  - 手臂切换至控制模式二(外部控制)
  - 发布/kuavo_arm_target_poses话题，用于控制手臂目标姿态
- 整机运动控制
  - 发布/joy话题，用于发送模拟的遥控器信号
键盘输入处理
- getKey函数：
  使用select库监听键盘输入，设置超时时间为0.1秒
  如果有键盘输入，读取单个字符并返回；否则返回空字符串
  如果键盘输入ctrl+c 则恢复键盘的原始设置并退出程序
- update_joy函数：
  - 手臂移动控制
    根据键盘输入更新末端执行器位置控制(XYZ)与姿态控制(RPY)的参数值
  - 整机运动控制
    根据键盘输入更新模拟遥控器的状态（按钮和摇杆信号）
控制指令发布
- 主循环：
  在run函数中，程序进入主循环，不断监听键盘输入。
- 手臂移动控制
  - 自适应处理：
    根据设定的末端位置，计算得出一个相对合理的末端姿态
    自适应处理能确保在自动控制末端姿态模式下，最大的可达空间。
  - 手动修改：
    若处在手动控制末端姿态模式下，则启用输入的角度参数。
    以自适应处理得到的末端姿态为参照，进行二次旋转。
    将最后的结果作为最终参数发给ik进行求解
- 整机运动控制
  如果有键盘输入，调用update_joy函数更新遥控器状态。
  将更新后的遥控器信号（joy_msg）发布到/joy话题。
- 退出条件：
  如果按下Ctrl-C，程序退出主循环。
  如果接收到/stop_robot话题的消息，程序调用rospy.signal_shutdown关闭ROS节点。
程序退出
- 资源清理：
  程序退出时，恢复键盘的原始设置，避免键盘输入异常。

参数说明

joystick_sensitivity:灵敏度,无单位.
修改路径:<kuavo-ros-opensource>/src/humanoid-control/humanoid_controllers/launch/joy/joy_control_sim.launch

编译

cd <kuavo-ros-opensource>
sudo su
catkin build kuavo_sdk motion_capture_ik

执行

启动机器人

cd kuavo-ros-opensource  # 进入下位机工作空间
sudo su
source devel/setup.bash
# 仿真环境运行
roslaunch humanoid_controllers load_kuavo_mujoco_sim.launch 
# 实物运行
roslaunch humanoid_controllers load_kuavo_real.launch 

启动ik逆解服务器

cd kuavo-ros-opensource  # 进入下位机工作空间
sudo su
source devel/setup.bash
roslaunch motion_capture_ik ik_node.launch 

启动键盘控制案例

cd kuavo-ros-opensource  # 进入下位机工作空间
sudo su
source devel/setup.bash
python3 src/kuavo_sdk/scripts/keyboard_control/robot_keyboard_control.py 

操作说明
- 初始为键盘控制手臂模式
- 按v在键盘控制手臂模式和键盘控制机器人移动模式间切换
- 键盘控制机器人移动模式:
  按键盘WASD为左操纵杆，控制前进/后退，左/右横移；IKJL/QE为右操纵杆、上/下、左转/右转,每按动一次值增加或减小10%；R是walk；C是stance；T是trot；B是back；O/F是start；空格是将左右操纵杆的输入重置为零。
- 键盘控制手臂模式:
  根据键盘输入更新末端执行器位置控制(XYZ)与姿态控制(RPY)的参数值:
  控制 X Y Z Roll Pitch Yaw
  加 w d q u i j
  减 s a e o k l
  按G切换控制模式,在手动控制和自动控制两者间切换
  位置控制(XYZ)的变化会相应修改self.eef_target_xyz的值
  姿态控制(RPY)的变化会相应修改self.eef_angle_manual的值
  注:
  - self.eef_target_ypr的值为自动生成,不要手动修改
  - Yaw的推荐赋值范围为负九十度 ~ 正九十度
  - Pitch和Roll的推荐赋值范围为负三十度 ~ 正三十度

控制	X	Y	Z	Roll	Pitch	Yaw
加	`w`	`d`	`q`	`u`	`i`	`j`
减	`s`	`a`	`e`	`o`	`k`	`l`

运行效果

键盘控制机器人移动:
键盘控制手臂移动到指定位置:
键盘按G切换至手动控制模式 , 再控制手臂末端期望姿态绕新x轴旋转四十五度:
键盘控制手臂末端期望姿态绕新x轴旋转九十度:

键盘控制案例

描述​

程序逻辑​

参数说明​

编译​

执行​

运行效果​

描述

程序逻辑

参数说明

编译

执行

运行效果