跳到主要内容

单步控制案例(落足点规划)

示例代码​

  • 路径：<kuavo-ros-opensource>/src/demo/examples_code/step_control/simStepControl.py

说明​

下面是对代码中各个函数的详细说明，包括它们的作用、功能、输入参数以及执行逻辑：

1. get_foot_pose_traj_msg​

作用与功能​

  • 该函数用于创建并返回一个 footPoseTargetTrajectories 消息对象，该对象包含了机器人的步态信息。

输入参数​

  • time_traj: 时间轨迹列表，表示每一步的时间点。
  • foot_idx_traj: 脚索引轨迹列表，指示哪只脚在当前步态中移动。
  • foot_traj: 脚姿态轨迹列表，包含每一步的脚位置和姿态。
  • torso_traj: 躯干姿态轨迹列表，包含每一步的躯干位置和姿态。

执行逻辑​

  1. 创建一个 footPoseTargetTrajectories 消息实例。
  2. 将输入的时间轨迹、脚索引轨迹和初始化的脚姿态轨迹赋值给消息对象。
  3. 遍历每个时间点，创建对应的 footPose 消息，设置脚和躯干的姿态。
  4. 将每个 footPose 消息添加到 footPoseTrajectory 列表中。
  5. 返回构建好的消息对象。

2. generate_steps​

作用与功能​

  • 根据给定的躯干位置和偏航角，计算并返回左右脚的位置。

输入参数​

  • torso_pos: 躯干位置的数组，包含x、y、z坐标。
  • torso_yaw: 躯干的偏航角，以弧度表示。
  • foot_bias: 脚的横向偏移量，用于计算左右脚的位置。

执行逻辑​

  1. 计算左右脚相对于躯干的偏移量。
  2. 构建绕z轴旋转的旋转矩阵 R_z。
  3. 使用旋转矩阵计算左右脚的实际位置。
  4. 返回计算得到的左右脚位置。

3. get_multiple_steps_msg​

作用与功能​

  • 生成多步步态的消息对象，包含多个步态的时间、脚索引、脚姿态和躯干姿态。

输入参数​

  • body_poses: 身体姿态的列表，每个姿态包含x、y、z坐标和偏航角。
  • dt: 每一步的时间间隔。
  • is_left_first: 布尔值，指示是否左脚先行。
  • collision_check: 布尔值，指示是否进行碰撞检测。

执行逻辑​

  1. 初始化步态相关的轨迹列表。
  2. 创建用于碰撞检测的左右脚旋转矩形。
  3. 遍历每个步态，计算时间轨迹和身体姿态。
  4. 使用 generate_steps 计算左右脚的位置。
  5. 如果启用了碰撞检测，每两步进行一次碰撞检测，调整步态顺序以避免碰撞。
  6. 根据当前步态顺序，更新脚索引和脚姿态轨迹。
  7. 打印生成的轨迹信息。
  8. 调用 get_foot_pose_traj_msg 生成并返回步态消息对象。

主程序逻辑​

  • 初始化ROS节点：使用 rospy.init_node 初始化一个名为 foot_pose_publisher 的ROS节点。
  • 创建发布者：通过 rospy.Publisher 创建一个发布者，用于发布步态消息到 /humanoid_mpc_foot_pose_target_trajectories 话题。
  • 设置默认参数：定义默认的步态顺序和碰撞检测选项。
  • 定义身体姿态序列：设置一系列的身体姿态，每个姿态对应两步。
  • 生成并发布步态消息：调用 get_multiple_steps_msg 生成步态消息，并通过发布者发布该消息。

通过这些函数和逻辑，代码实现了根据给定的身体姿态序列生成机器人步态，并通过ROS发布这些步态信息的功能。

执行​

  • 启动下位机主程序
source devel/setup.bash
roslaunch humanoid_controllers load_kuavo_real.launch
  • source SDK环境变量
source devel/setup.zsh # 或bash

  • 示例程序

    • <kuavo-ros-opensource>/src/demo/examples_code/step_control/simStepControl.py

  • 运行效果