这个程序和上面的程序差不多，都是控制机器人的，这里面的动作就不会越来越快，请你详细的剖析这个程序是怎么做到的
#include <booster/robot/b1/b1_loco_client.hpp>
#include <booster/idl/b1/RemoteControllerState.h>
#include <booster/robot/channel/channel_subscriber.hpp>
#include <chrono>
#include <iostream>
#include <thread>
void AdvanceMove(booster::robot::b1::B1LocoClient &client) {
int32_t res = 0;
float forward_speed = 0.5f;
int forward_duration_ms = 16000;
float turn_speed = 1.2f;
int turn_duration_ms = 6300;
int wave_duration_ms = 9000;
int handshake_duration_ms = 9000;
std::cout << "开始..." << std::endl;
res = client.ChangeMode(booster::robot::RobotMode::kWalking);
if (res != 0) {
std::cout << "切换到行走模式失败: error = " << res << std::endl;
return;
}
std::cout << "已切换到行走模式，等待模式变更生效..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
std::cout << "开始行走..." << std::endl;
res = client.Move(forward_speed, 0.0f,0.0f);
if (res != 0) {
std::cout << "行走失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(forward_duration_ms));
res = client.Move(0.0f, 0.0f, 0.0f);
if (res != 0) {
std::cout << "停止前进失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1500)); // 等待停止稳定
std::cout << "第二步：开始挥手..." << std::endl;
res = client.WaveHand(booster::robot::b1::HandAction::kHandOpen);
if (res != 0) {
std::cout << "挥手动作失败: error = " << res << std::endl;
return;
}
std::cout << "挥手持续中..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(wave_duration_ms));
// 放下手
std::cout << "放下手..." << std::endl;
res = client.WaveHand(booster::robot::b1::HandAction::kHandClose);
if (res != 0) {
std::cout << "放下手臂失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2000)); // 等待手臂放下
// 第三步：向左转身90度
std::cout << "第三步：向左转身90度..." << std::endl;
res = client.Move(0.0f, 0.0f, turn_speed);
if (res != 0) {
std::cout << "开始转向失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(turn_duration_ms));
// 停止转向
res = client.Move(0.0f, 0.0f, 0.0f);
if (res != 0) {
std::cout << "停止转向失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1500)); // 等待转向稳定
// 第四步：再次前进相同距离
std::cout << "第四步：再次前进相同距离..." << std::endl;
res = client.Move(forward_speed, 0.0f, 0.0f);
if (res != 0) {
std::cout << "再次前进失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(forward_duration_ms));
// 停止前进
res = client.Move(0.0f, 0.0f, 0.0f);
if (res != 0) {
std::cout << "停止前进失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1500)); // 等待停止稳定
// 第五步：握手一段时间
std::cout << "第五步：开始握手..." << std::endl;
res = client.Handshake(booster::robot::b1::HandAction::kHandOpen);
if (res != 0) {
std::cout << "握手打开失败: error = " << res << std::endl;
return;
}
std::cout << "握手持续中..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(handshake_duration_ms));
// 放下手
std::cout << "放下手..." << std::endl;
res = client.Handshake(booster::robot::b1::HandAction::kHandClose);
if (res != 0) {
std::cout << "握手关闭失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2000)); // 等待手臂放下
std::cout << "向左转身90度..." << std::endl;
res = client.Move(0.0f, 0.0f, turn_speed);
if (res != 0) {
std::cout << "开始转向失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(turn_duration_ms));
// 停止转向
res = client.Move(0.0f, 0.0f, 0.0f);
if (res != 0) {
std::cout << "停止转向失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1500)); // 等待转向稳定
std::cout << "再次前进相同距离..." << std::endl;
res = client.Move(forward_speed, 0.0f, 0.0f);
if (res != 0) {
std::cout << "再次前进失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(forward_duration_ms));
// 停止前进
res = client.Move(0.0f, 0.0f, 0.0f);
if (res != 0) {
std::cout << "停止前进失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1500)); // 等待停止稳定
std::cout << "准备开启手末端控制模式..." << std::endl;
res = client.SwitchHandEndEffectorControlMode(true);
if (res != 0) {
std::cout << "开启手末端控制模式失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
std::cout << "右手上举..." << std::endl;
booster::robot::Posture right_hand_up;
right_hand_up.position_ = booster::robot::Position(0.25, -0.3, 0.25);
right_hand_up.orientation_ = booster::robot::Orientation(0., -1.0, 0.);
res = client.MoveHandEndEffector(
right_hand_up, 1000, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右手上举失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(4000)); // 保持2秒
// 右手放下
std::cout << "右手放下..." << std::endl;
booster::robot::Posture right_hand_down;
right_hand_down.position_ = booster::robot::Position(0.28, -0.25, 0.05);
right_hand_down.orientation_ = booster::robot::Orientation(0., 0., 0.);
res = client.MoveHandEndEffector(
right_hand_down, 1000, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右手放下失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// ===== 新增 handl-up（左手上举）动作 =====
std::cout << "左手上举..." << std::endl;
booster::robot::Posture left_hand_up;
left_hand_up.position_ = booster::robot::Position(0.25, 0.3, 0.25);
left_hand_up.orientation_ = booster::robot::Orientation(0., -1.0, 0.);
res = client.MoveHandEndEffector(
left_hand_up, 1000, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "左手上举失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(4000)); // 保持2秒
// 左手放下
std::cout << "左手放下..." << std::endl;
booster::robot::Posture left_hand_down;
left_hand_down.position_ = booster::robot::Position(0.28, -0.25, 0.05);
left_hand_down.orientation_ = booster::robot::Orientation(0., 0., 0.);
res = client.MoveHandEndEffector(
left_hand_down, 1000, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "左手放下失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::cout << "关闭手末端控制模式..." << std::endl;
res = client.SwitchHandEndEffectorControlMode(false);
if (res != 0) {
std::cout << "关闭手末端控制模式失败: error = " << res << std::endl;
}
std::cout << "高级组合动作完成!" << std::endl;
}
void RobotInteractiveShow(booster::robot::b1::B1LocoClient &client) {
int32_t res = 0;
// 确保处于行走模式
res = client.ChangeMode(booster::robot::RobotMode::kWalking);
if (res != 0) {
std::cout << "切换到行走模式失败: error = " << res << std::endl;
return;
}
std::cout << "已切换到行走模式，等待模式变更生效..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
// 开启手末端控制模式
res = client.SwitchHandEndEffectorControlMode(true);
if (res != 0) {
std::cout << "启用手末端控制模式失败: error = " << res << std::endl;
return;
}
std::cout << "已启用手末端控制模式，等待生效..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
std::cout << "开始互动表演序列..." << std::endl;
// 序列1: 欢迎动作
std::cout << "序列1: 欢迎动作" << std::endl;
// 头部向前点头致意
res = client.RotateHead(0.5, 0.0);
if (res != 0) {
std::cout << "头部点头失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
res = client.RotateHead(0.0, 0.0);
if (res != 0) {
std::cout << "头部回中失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(500));
// 双手展开欢迎
booster::robot::Posture left_arm_welcome;
left_arm_welcome.position_ = booster::robot::Position(0.4, 0.5, 0.4);
left_arm_welcome.orientation_ = booster::robot::Orientation(-1.57, -0.8, 0.0);
booster::robot::Posture right_arm_welcome;
right_arm_welcome.position_ = booster::robot::Position(0.4, -0.5, 0.4);
right_arm_welcome.orientation_ = booster::robot::Orientation(1.57, -0.8, 0.0);
res = client.MoveHandEndEffectorV2(left_arm_welcome, 2500, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "左臂欢迎姿势失败: error = " << res << std::endl;
return;
}
res = client.MoveHandEndEffectorV2(right_arm_welcome, 2500, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右臂欢迎姿势失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
// 序列2: 猜拳游戏准备
std::cout << "序列2: 猜拳游戏准备" << std::endl;
// 右手放到合适位置
booster::robot::Posture right_arm_game;
right_arm_game.position_ = booster::robot::Position(0.5, -0.3, 0.4);
right_arm_game.orientation_ = booster::robot::Orientation(1.57, -0.5, 0.0);
res = client.MoveHandEndEffectorV2(right_arm_game, 2500, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右臂游戏姿势失败: error = " << res << std::endl;
return;
}
// 左手放到身侧
booster::robot::Posture left_arm_side;
left_arm_side.position_ = booster::robot::Position(0.3, 0.3, 0.2);
left_arm_side.orientation_ = booster::robot::Orientation(-1.57, -1.57, 0.0);
res = client.MoveHandEndEffectorV2(left_arm_side, 2500, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "左臂侧放失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
// 序列3: 猜拳游戏
std::cout << "序列3: 猜拳游戏" << std::endl;
std::cout << "准备猜拳..." << std::endl;
// 头部左右看，表示准备
res = client.RotateHead(0.0, 0.5);
if (res != 0) {
std::cout << "头部旋转失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
res = client.RotateHead(0.0, -0.5);
if (res != 0) {
std::cout << "头部旋转失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
res = client.RotateHead(0.0, 0.0);
if (res != 0) {
std::cout << "头部回中失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
// 序列4: 互动表演 - 跟随头部
std::cout << "序列4: 互动表演 - 跟随头部" << std::endl;
// 手臂放到准备位置
booster::robot::Posture left_arm_ready;
left_arm_ready.position_ = booster::robot::Position(0.3, 0.4, 0.5);
left_arm_ready.orientation_ = booster::robot::Orientation(-1.57, -1.0, 0.0);
booster::robot::Posture right_arm_ready;
right_arm_ready.position_ = booster::robot::Position(0.3, -0.4, 0.5);
right_arm_ready.orientation_ = booster::robot::Orientation(1.57, -1.0, 0.0);
res = client.MoveHandEndEffectorV2(left_arm_ready, 2500, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "左臂准备位置失败: error = " << res << std::endl;
return;
}
res = client.MoveHandEndEffectorV2(right_arm_ready, 2500, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右臂准备位置失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
// 头部和手臂协调动作
std::cout << "开始头部和手臂协调动作..." << std::endl;
// 向右看，左手举高
res = client.RotateHead(0.0, -0.7);
if (res != 0) {
std::cout << "头部右转失败: error = " << res << std::endl;
return;
}
booster::robot::Posture left_arm_high;
left_arm_high.position_ = booster::robot::Position(0.3, 0.4, 0.7);
left_arm_high.orientation_ = booster::robot::Orientation(-1.57, -1.0, 0.0);
res = client.MoveHandEndEffectorV2(left_arm_high, 1500, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "左臂举高失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 向左看，右手举高
res = client.RotateHead(0.0, 0.7);
if (res != 0) {
std::cout << "头部左转失败: error = " << res << std::endl;
return;
}
booster::robot::Posture right_arm_high;
right_arm_high.position_ = booster::robot::Position(0.3, -0.4, 0.7);
right_arm_high.orientation_ = booster::robot::Orientation(1.57, -1.0, 0.0);
res = client.MoveHandEndEffectorV2(right_arm_high, 1500, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右臂举高失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 回中，双手放下
res = client.RotateHead(0.0, 0.0);
if (res != 0) {
std::cout << "头部回中失败: error = " << res << std::endl;
return;
}
res = client.MoveHandEndEffectorV2(left_arm_ready, 1500, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "左臂放下失败: error = " << res << std::endl;
return;
}
res = client.MoveHandEndEffectorV2(right_arm_ready, 1500, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右臂放下失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 序列5: 手势表演
std::cout << "序列5: 手势表演" << std::endl;
// 右手放到前方便于观察
booster::robot::Posture right_arm_front;
right_arm_front.position_ = booster::robot::Position(0.5, -0.2, 0.4);
right_arm_front.orientation_ = booster::robot::Orientation(1.57, -0.5, 0.0);
res = client.MoveHandEndEffectorV2(right_arm_front, 2000, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右臂前伸失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 握拳手势
std::cout << "握拳手势" << std::endl;
HandGrasp(client);
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 序列6: 告别动作
std::cout << "序列6: 告别动作" << std::endl;
// 双手挥手告别
booster::robot::Posture left_arm_wave;
left_arm_wave.position_ = booster::robot::Position(0.4, 0.5, 0.6);
left_arm_wave.orientation_ = booster::robot::Orientation(-1.57, -0.5, 0.0);
booster::robot::Posture right_arm_wave;
right_arm_wave.position_ = booster::robot::Position(0.4, -0.5, 0.6);
right_arm_wave.orientation_ = booster::robot::Orientation(1.57, -0.5, 0.0);
res = client.MoveHandEndEffectorV2(left_arm_wave, 2000, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "左臂挥手失败: error = " << res << std::endl;
return;
}
res = client.MoveHandEndEffectorV2(right_arm_wave, 2000, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右臂挥手失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 左右摆动手臂模拟挥手
for (int i = 0; i < 3; i++) {
// 左手向左摆
booster::robot::Posture left_wave_left;
left_wave_left.position_ = booster::robot::Position(0.35, 0.55, 0.6);
left_wave_left.orientation_ = booster::robot::Orientation(-1.57, -0.5, 0.0);
res = client.MoveHandEndEffectorV2(left_wave_left, 800, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "左臂挥手左摆失败: error = " << res << std::endl;
return;
}
// 右手向右摆
booster::robot::Posture right_wave_right;
right_wave_right.position_ = booster::robot::Position(0.35, -0.55, 0.6);
right_wave_right.orientation_ = booster::robot::Orientation(1.57, -0.5, 0.0);
res = client.MoveHandEndEffectorV2(right_wave_right, 800, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右臂挥手右摆失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 左手向右摆
booster::robot::Posture left_wave_right;
left_wave_right.position_ = booster::robot::Position(0.45, 0.45, 0.6);
left_wave_right.orientation_ = booster::robot::Orientation(-1.57, -0.5, 0.0);
res = client.MoveHandEndEffectorV2(left_wave_right, 800, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "左臂挥手右摆失败: error = " << res << std::endl;
return;
}
// 右手向左摆
booster::robot::Posture right_wave_left;
right_wave_left.position_ = booster::robot::Position(0.45, -0.45, 0.6);
right_wave_left.orientation_ = booster::robot::Orientation(1.57, -0.5, 0.0);
res = client.MoveHandEndEffectorV2(right_wave_left, 800, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右臂挥手左摆失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}
// 恢复到中立姿势
booster::robot::Posture left_arm_neutral;
left_arm_neutral.position_ = booster::robot::Position(0.35, 0.25, 0.3);
left_arm_neutral.orientation_ = booster::robot::Orientation(-1.57, -1.57, 0.0);
booster::robot::Posture right_arm_neutral;
right_arm_neutral.position_ = booster::robot::Position(0.35, -0.25, 0.3);
right_arm_neutral.orientation_ = booster::robot::Orientation(1.57, -1.57, 0.0);
res = client.MoveHandEndEffectorV2(left_arm_neutral, 2500, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "左臂回中失败: error = " << res << std::endl;
return;
}
res = client.MoveHandEndEffectorV2(right_arm_neutral, 2500, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右臂回中失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
// 关闭手末端控制模式
res = client.SwitchHandEndEffectorControlMode(false);
if (res != 0) {
std::cout << "关闭手末端控制模式失败: error = " << res << std::endl;
}
std::cout << "互动表演完成!" << std::endl;
}
void RobotArmHeadCombo(booster::robot::b1::B1LocoClient &client) {
int32_t res = 0;
std::cout << "准备开始手臂头部组合表演，首先确保机器人处于正确模式..." << std::endl;
// 先尝试切换到准备模式，然后再切换到行走模式，这样可能更稳定
res = client.ChangeMode(booster::robot::RobotMode::kPrepare);
if (res != 0) {
std::cout << "切换到准备模式失败: error = " << res << std::endl;
return;
}
std::cout << "已切换到准备模式，等待模式变更生效..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(5000)); // 增加等待时间到5秒
// 确保处于行走模式
res = client.ChangeMode(booster::robot::RobotMode::kWalking);
if (res != 0) {
std::cout << "切换到行走模式失败: error = " << res << std::endl;
return;
}
std::cout << "已切换到行走模式，等待模式变更生效..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(5000)); // 增加等待时间到5秒
// 开启手末端控制模式
std::cout << "正在启用手末端控制模式..." << std::endl;
res = client.SwitchHandEndEffectorControlMode(true);
if (res != 0) {
std::cout << "启用手末端控制模式失败: error = " << res << std::endl;
return;
}
std::cout << "已启用手末端控制模式，等待生效..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(5000)); // 增加等待时间到5秒
// 测试手臂控制是否正常工作
std::cout << "测试手臂控制是否正常工作..." << std::endl;
// 使用已知可工作的参数进行初始测试
booster::robot::Posture test_posture_left;
test_posture_left.position_ = booster::robot::Position(0.35, 0.25, 0.1);
test_posture_left.orientation_ = booster::robot::Orientation(-1.57, -1.57, 0.);
std::cout << "尝试移动左臂到测试位置..." << std::endl;
res = client.MoveHandEndEffectorV2(
test_posture_left, 2000, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "测试左臂移动失败: error = " << res << std::endl;
// 尝试重新启用手末端控制模式
std::cout << "尝试重新启用手末端控制模式..." << std::endl;
client.SwitchHandEndEffectorControlMode(false);
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
res = client.SwitchHandEndEffectorControlMode(true);
if (res != 0) {
std::cout << "重新启用手末端控制模式失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
// 再次尝试移动左臂
std::cout << "再次尝试移动左臂..." << std::endl;
res = client.MoveHandEndEffectorV2(
test_posture_left, 2000, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "再次尝试移动左臂仍然失败: error = " << res << std::endl;
return;
}
}
booster::robot::Posture test_posture_right;
test_posture_right.position_ = booster::robot::Position(0.35, -0.2, 0.1);
test_posture_right.orientation_ = booster::robot::Orientation(1.57, -1.57, 0.);
std::cout << "尝试移动右臂到测试位置..." << std::endl;
res = client.MoveHandEndEffectorV2(
test_posture_right, 2000, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "测试右臂移动失败: error = " << res << std::endl;
return;
}
std::cout << "手臂测试移动成功，等待动作完成..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
std::cout << "开始手臂头部组合表演..." << std::endl;
// 序列1: 欢迎动作 - 头部点头配合双手展开
std::cout << "序列1: 欢迎动作 - 头部点头配合双手展开" << std::endl;
// 头部点头
std::cout << "执行头部点头动作..." << std::endl;
res = client.RotateHead(0.5, 0.0);
if (res != 0) {
std::cout << "头部点头失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1500));
res = client.RotateHead(0.0, 0.0);
if (res != 0) {
std::cout << "头部回中失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1500));
// 双手同时展开
std::cout << "执行双手展开动作..." << std::endl;
booster::robot::Posture left_arm_welcome;
left_arm_welcome.position_ = booster::robot::Position(0.4, 0.5, 0.4);
left_arm_welcome.orientation_ = booster::robot::Orientation(-1.57, -0.8, 0.0);
booster::robot::Posture right_arm_welcome;
right_arm_welcome.position_ = booster::robot::Position(0.4, -0.5, 0.4);
right_arm_welcome.orientation_ = booster::robot::Orientation(1.57, -0.8, 0.0);
// 先移动左臂
std::cout << "移动左臂到欢迎姿势..." << std::endl;
res = client.MoveHandEndEffectorV2(left_arm_welcome, 3000, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "左臂欢迎姿势失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(3500));
// 再移动右臂
std::cout << "移动右臂到欢迎姿势..." << std::endl;
res = client.MoveHandEndEffectorV2(right_arm_welcome, 3000, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右臂欢迎姿势失败: error = " << res << std::endl;
return;
}
std::cout << "欢迎动作执行完成，等待动作完成..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(4000));
// 序列2: 头部环绕动作
std::cout << "序列2: 头部环绕动作" << std::endl;
// 头部环绕 - 顺时针
std::cout << "执行头部顺时针环绕..." << std::endl;
// 向右
res = client.RotateHead(0.0, -0.7);
if (res != 0) {
std::cout << "头部右转失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 右下
res = client.RotateHead(0.5, -0.7);
if (res != 0) {
std::cout << "头部右下转失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 下
res = client.RotateHead(0.5, 0.0);
if (res != 0) {
std::cout << "头部下转失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 左下
res = client.RotateHead(0.5, 0.7);
if (res != 0) {
std::cout << "头部左下转失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 左
res = client.RotateHead(0.0, 0.7);
if (res != 0) {
std::cout << "头部左转失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 左上
res = client.RotateHead(-0.3, 0.7);
if (res != 0) {
std::cout << "头部左上转失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 上
res = client.RotateHead(-0.3, 0.0);
if (res != 0) {
std::cout << "头部上转失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 右上
res = client.RotateHead(-0.3, -0.7);
if (res != 0) {
std::cout << "头部右上转失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 回中
res = client.RotateHead(0.0, 0.0);
if (res != 0) {
std::cout << "头部回中失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 序列3: 手臂简化画圈动作
std::cout << "序列3: 手臂简化画圈动作" << std::endl;
// 使用更少的点，形成一个矩形轨迹来模拟圆周运动
std::cout << "左臂开始运动..." << std::endl;
// 定义4个关键点，形成一个小矩形
std::vector<booster::robot::Posture> left_key_points;
// 起始点
booster::robot::Posture left_point1;
left_point1.position_ = booster::robot::Position(0.35, 0.25, 0.3);
left_point1.orientation_ = booster::robot::Orientation(-1.57, -1.57, 0.0);
left_key_points.push_back(left_point1);
// 右上点
booster::robot::Posture left_point2;
left_point2.position_ = booster::robot::Position(0.35, 0.35, 0.3);
left_point2.orientation_ = booster::robot::Orientation(-1.57, -1.57, 0.0);
left_key_points.push_back(left_point2);
// 右下点
booster::robot::Posture left_point3;
left_point3.position_ = booster::robot::Position(0.35, 0.35, 0.2);
left_point3.orientation_ = booster::robot::Orientation(-1.57, -1.57, 0.0);
left_key_points.push_back(left_point3);
// 左下点
booster::robot::Posture left_point4;
left_point4.position_ = booster::robot::Position(0.35, 0.25, 0.2);
left_point4.orientation_ = booster::robot::Orientation(-1.57, -1.57, 0.0);
left_key_points.push_back(left_point4);
// 回到起始点
left_key_points.push_back(left_point1);
// 执行左臂运动
for (const auto& point : left_key_points) {
res = client.MoveHandEndEffector(point, 2000, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "左臂运动失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2500));
}
std::cout << "右臂开始运动..." << std::endl;
// 定义右臂的4个关键点
std::vector<booster::robot::Posture> right_key_points;
// 起始点
booster::robot::Posture right_point1;
right_point1.position_ = booster::robot::Position(0.35, -0.25, 0.3);
right_point1.orientation_ = booster::robot::Orientation(1.57, -1.57, 0.0);
right_key_points.push_back(right_point1);
// 右上点
booster::robot::Posture right_point2;
right_point2.position_ = booster::robot::Position(0.35, -0.35, 0.3);
right_point2.orientation_ = booster::robot::Orientation(1.57, -1.57, 0.0);
right_key_points.push_back(right_point2);
// 右下点
booster::robot::Posture right_point3;
right_point3.position_ = booster::robot::Position(0.35, -0.35, 0.2);
right_point3.orientation_ = booster::robot::Orientation(1.57, -1.57, 0.0);
right_key_points.push_back(right_point3);
// 左下点
booster::robot::Posture right_point4;
right_point4.position_ = booster::robot::Position(0.35, -0.25, 0.2);
right_point4.orientation_ = booster::robot::Orientation(1.57, -1.57, 0.0);
right_key_points.push_back(right_point4);
// 回到起始点
right_key_points.push_back(right_point1);
// 执行右臂运动
for (const auto& point : right_key_points) {
res = client.MoveHandEndEffector(point, 2000, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右臂运动失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2500));
}
// 序列4: 简化的上下运动
std::cout << "序列4: 简化的上下运动" << std::endl;
// 定义简单的上下运动点
std::vector<float> heights = {0.3, 0.35, 0.4, 0.35, 0.3};
for (float height : heights) {
// 左臂运动
booster::robot::Posture left_pos;
left_pos.position_ = booster::robot::Position(0.35, 0.25, height);
left_pos.orientation_ = booster::robot::Orientation(-1.57, -1.57, 0.0);
res = client.MoveHandEndEffector(left_pos, 2000, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "左臂上下运动失败: error = " << res << std::endl;
return;
}
// 右臂运动
booster::robot::Posture right_pos;
right_pos.position_ = booster::robot::Position(0.35, -0.25, height);
right_pos.orientation_ = booster::robot::Orientation(1.57, -1.57, 0.0);
res = client.MoveHandEndEffector(right_pos, 2000, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右臂上下运动失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2500));
}
// 序列5: 结束动作 - 优雅回中
std::cout << "序列5: 结束动作 - 优雅回中" << std::endl;
// 头部缓慢回中
res = client.RotateHead(0.0, 0.0);
if (res != 0) {
std::cout << "头部回中失败: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// 双臂同时回到中立位置
booster::robot::Posture left_arm_neutral;
left_arm_neutral.position_ = booster::robot::Position(0.35, 0.25, 0.3);
left_arm_neutral.orientation_ = booster::robot::Orientation(-1.57, -1.57, 0.0);
booster::robot::Posture right_arm_neutral;
right_arm_neutral.position_ = booster::robot::Position(0.35, -0.25, 0.3);
right_arm_neutral.orientation_ = booster::robot::Orientation(1.57, -1.57, 0.0);
std::cout << "双臂回中..." << std::endl;
res = client.MoveHandEndEffectorV2(left_arm_neutral, 3000, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "左臂回中失败: error = " << res << std::endl;
return;
}
res = client.MoveHandEndEffectorV2(right_arm_neutral, 3000, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "右臂回中失败: error = " << res << std::endl;
return;
}
std::cout << "等待最终动作完成..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(4000));
// 关闭手末端控制模式
std::cout << "关闭手末端控制模式..." << std::endl;
res = client.SwitchHandEndEffectorControlMode(false);
if (res != 0) {
std::cout << "关闭手末端控制模式失败: error = " << res << std::endl;
}
std::cout << "手臂头部组合表演完成!" << std::endl;
}
void RobotDance2(booster::robot::b1::B1LocoClient &client) {
int32_t res = 0;
// First ensure we're in walking mode
res = client.ChangeMode(booster::robot::RobotMode::kWalking);
if (res != 0) {
std::cout << "Failed to change to walking mode: error = " << res << std::endl;
return;
}
std::cout << "Changed to walking mode, waiting for mode change to take effect..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(3000)); // 增加等待时间
// 开启手末端控制模式，必须在行走模式下调用
res = client.SwitchHandEndEffectorControlMode(true);
if (res != 0) {
std::cout << "Failed to enable hand end-effector control mode: error = " << res << std::endl;
return;
}
std::cout << "Enabled hand end-effector control mode, waiting for it to take effect..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(3000)); // 增加等待时间
// Dance sequence
std::cout << "Starting new dance routine..." << std::endl;
// Step 1: 初始姿势 - 双臂高举（更明显）
// 根据坐标系：X向前，Y向右，Z向上
booster::robot::Posture left_arm_up;
left_arm_up.position_ = booster::robot::Position(0.2, 0.5, 0.7); // 更高的Z值，更大的Y值
left_arm_up.orientation_ = booster::robot::Orientation(-1.57, -1.57, 0.);
booster::robot::Posture right_arm_up;
right_arm_up.position_ = booster::robot::Position(0.2, -0.5, 0.7); // 更高的Z值，更大的Y值（负方向）
right_arm_up.orientation_ = booster::robot::Orientation(1.57, -1.57, 0.);
// 尝试使用与mhel相同的参数进行初始化测试
std::cout << "Testing arm movement with known working parameters..." << std::endl;
booster::robot::Posture test_posture_left;
test_posture_left.position_ = booster::robot::Position(0.35, 0.25, 0.1);
test_posture_left.orientation_ = booster::robot::Orientation(-1.57, -1.57, 0.);
res = client.MoveHandEndEffectorV2(
test_posture_left, 2000, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "Test left arm positioning failed: error = " << res << std::endl;
return;
}
booster::robot::Posture test_posture_right;
test_posture_right.position_ = booster::robot::Position(0.35, -0.2, 0.1);
test_posture_right.orientation_ = booster::robot::Orientation(1.57, -1.57, 0.);
res = client.MoveHandEndEffectorV2(
test_posture_right, 2000, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "Test right arm positioning failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(5000)); // 等待测试动作完成
// 继续原来的舞蹈动作
res = client.MoveHandEndEffectorV2(left_arm_up, 3000, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "Left arm positioning failed: error = " << res << std::endl;
return; // 如果左臂移动失败，直接返回
}
res = client.MoveHandEndEffectorV2(right_arm_up, 3000, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "Right arm positioning failed: error = " << res << std::endl;
return; // 如果右臂移动失败，直接返回
}
std::this_thread::sleep_for(std::chrono::milliseconds(4000)); // 增加等待时间确保动作完成
// Step 2: 旋转舞蹈 - 手臂展开，身体旋转
booster::robot::Posture left_arm_out;
left_arm_out.position_ = booster::robot::Position(0.1, 0.7, 0.3); // 更大的Y值
left_arm_out.orientation_ = booster::robot::Orientation(-1.57, 0.0, 0.0);
booster::robot::Posture right_arm_out;
right_arm_out.position_ = booster::robot::Position(0.1, -0.7, 0.3); // 更大的Y值（负方向）
right_arm_out.orientation_ = booster::robot::Orientation(1.57, 0.0, 0.0);
res = client.MoveHandEndEffectorV2(left_arm_out, 3000, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "Left arm out positioning failed: error = " << res << std::endl;
return;
}
res = client.MoveHandEndEffectorV2(right_arm_out, 3000, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "Right arm out positioning failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(4000)); // 增加等待时间确保动作完成
// 顺时针旋转
res = client.Move(0.0, 0.0, 1.5);
if (res != 0) {
std::cout << "Rotation failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
// 停止旋转
res = client.Move(0.0, 0.0, 0.0);
if (res != 0) {
std::cout << "Stop rotation failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 逆时针旋转
res = client.Move(0.0, 0.0, -1.5);
if (res != 0) {
std::cout << "Rotation failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
// 停止旋转
res = client.Move(0.0, 0.0, 0.0);
if (res != 0) {
std::cout << "Stop rotation failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// Step 3: 左右舞蹈 - 身体左右移动，手臂保持展开
// 保持手臂姿势，向左移动
res = client.Move(0.0, 0.6, 0.0);
if (res != 0) {
std::cout << "Left movement failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(4000));
// 停止移动
res = client.Move(0.0, 0.0, 0.0);
if (res != 0) {
std::cout << "Stop movement failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 向右移动
res = client.Move(0.0, -0.6, 0.0);
if (res != 0) {
std::cout << "Right movement failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(4000));
// 停止移动
res = client.Move(0.0, 0.0, 0.0);
if (res != 0) {
std::cout << "Stop movement failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// Step 4: 握手动作
res = client.Handshake(booster::robot::b1::HandAction::kHandOpen);
if (res != 0) {
std::cout << "Handshake open action failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
res = client.Handshake(booster::robot::b1::HandAction::kHandClose);
if (res != 0) {
std::cout << "Handshake close action failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// Step 5: 前后舞蹈 - 手臂前后摆动，身体前后移动
// 手臂前伸（更明显）
booster::robot::Posture left_arm_forward;
left_arm_forward.position_ = booster::robot::Position(0.8, 0.2, 0.4); // 更大的X值
left_arm_forward.orientation_ = booster::robot::Orientation(-1.57, -1.57, 0.0);
booster::robot::Posture right_arm_forward;
right_arm_forward.position_ = booster::robot::Position(0.8, -0.2, 0.4); // 更大的X值
right_arm_forward.orientation_ = booster::robot::Orientation(1.57, -1.57, 0.0);
res = client.MoveHandEndEffectorV2(left_arm_forward, 3000, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "Forward left arm positioning failed: error = " << res << std::endl;
return;
}
res = client.MoveHandEndEffectorV2(right_arm_forward, 3000, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "Forward right arm positioning failed: error = " << res << std::endl;
return;
}
// 前进
res = client.Move(0.6, 0.0, 0.0);
if (res != 0) {
std::cout << "Forward movement failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(4000));
// 停止移动
res = client.Move(0.0, 0.0, 0.0);
if (res != 0) {
std::cout << "Stop movement failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// 手臂后撤（更明显）
booster::robot::Posture left_arm_back;
left_arm_back.position_ = booster::robot::Position(0.1, 0.2, 0.2); // 更小的X值
left_arm_back.orientation_ = booster::robot::Orientation(-1.57, -1.57, 0.0);
booster::robot::Posture right_arm_back;
right_arm_back.position_ = booster::robot::Position(0.1, -0.2, 0.2); // 更小的X值
right_arm_back.orientation_ = booster::robot::Orientation(1.57, -1.57, 0.0);
res = client.MoveHandEndEffectorV2(left_arm_back, 3000, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "Backward left arm positioning failed: error = " << res << std::endl;
return;
}
res = client.MoveHandEndEffectorV2(right_arm_back, 3000, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "Backward right arm positioning failed: error = " << res << std::endl;
return;
}
// 后退
res = client.Move(-0.6, 0.0, 0.0);
if (res != 0) {
std::cout << "Backward movement failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(4000));
// 停止移动
res = client.Move(0.0, 0.0, 0.0);
if (res != 0) {
std::cout << "Stop movement failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// Step 6: 摇头晃脑
res = client.RotateHead(0.0, 0.5); // 向右看
if (res != 0) {
std::cout << "Head rotation failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1500));
res = client.RotateHead(0.0, -0.5); // 向左看
if (res != 0) {
std::cout << "Head rotation failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1500));
res = client.RotateHead(0.0, 0.0);
if (res != 0) {
std::cout << "Head rotation failed: error = " << res << std::endl;
return;
}
// 恢复到中立姿势
booster::robot::Posture left_arm_neutral;
left_arm_neutral.position_ = booster::robot::Position(0.35, 0.25, 0.3);
left_arm_neutral.orientation_ = booster::robot::Orientation(-1.57, -1.57, 0.0);
booster::robot::Posture right_arm_neutral;
right_arm_neutral.position_ = booster::robot::Position(0.35, -0.25, 0.3);
right_arm_neutral.orientation_ = booster::robot::Orientation(1.57, -1.57, 0.0);
res = client.MoveHandEndEffectorV2(left_arm_neutral, 3000, booster::robot::b1::HandIndex::kLeftHand);
if (res != 0) {
std::cout << "Neutral left arm positioning failed: error = " << res << std::endl;
return;
}
res = client.MoveHandEndEffectorV2(right_arm_neutral, 3000, booster::robot::b1::HandIndex::kRightHand);
if (res != 0) {
std::cout << "Neutral right arm positioning failed: error = " << res << std::endl;
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
// 关闭手末端控制模式
res = client.SwitchHandEndEffectorControlMode(false);
if (res != 0) {
std::cout << "Failed to disable hand end-effector control mode: error = " << res << std::endl;
}
std::cout << "Dance routine completed!" << std::endl;
}
int main(int argc, char const *argv[]) {
if (argc < 2) {
std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl;
exit(-1);
}
booster::robot::ChannelFactory::Instance()->Init(0, argv[1]);
booster::robot::b1::B1LocoClient client;
client.Init();
float x, y, z, yaw, pitch;
int32_t res = 0;
std::string input;
int32_t hand_action_count = 0;
while (true) {
bool need_print = false;
std::getline(std::cin, input);
if (!input.empty()) {
if (input == "mp") {
res = client.ChangeMode(booster::robot::RobotMode::kPrepare);
} else if (input == "md") {
res = client.ChangeMode(booster::robot::RobotMode::kDamping);
} else if (input == "mw") {
res = client.ChangeMode(booster::robot::RobotMode::kWalking);
} else if (input == "mc") {
res = client.ChangeMode(booster::robot::RobotMode::kCustom);
} else if (input == "w") {
x = 0.2;
y = 0.0;
z = 0.0;
need_print = true;
res = client.Move(x, y, z);
} else if (input == "ww") {
x = 1.0;
y = 0.0;
z = 0.0;
need_print = true;
res = client.Move(x, y, z);
} else if (input == "l") {
x = 0.0;
y = 0.0;
z = 0.0;
need_print = true;
res = client.Move(x, y, z);
} else if (input == "a") {
x = 0.0;
y = 0.2;
z = 0.0;
need_print = true;
res = client.Move(x, y, z);
} else if (input == "aa") {
x = 0.0;
y = 1.2;
z = 0.0;
need_print = true;
res = client.Move(x, y, z);
}
else if (input == "s") {
x = -0.2;
y = 0.0;
z = 0.0;
need_print = true;
res = client.Move(x, y, z);
} else if (input == "ss") {
x = -1.2;
y = 0.0;
z = 0.0;
need_print = true;
res = client.Move(x, y, z);
}
if (input == "d") {
x = 0.0;
y = -0.2;
z = 0.0;
need_print = true;
res = client.Move(x, y, z);
} else if (input == "dd") {
x == 0.0;
y = -1.2;
z = 0.0;
need_print = true;
res = client.Move(x, y, z);
} else if (input == "q") {
x = 0.0;
y = 0.0;
z = 1.0;
need_print = true;
res = client.Move(x, y, z);
} else if (input == "e") {
x = 0.0;
y = 0.0;
z = -1.0;
need_print = true;
res = client.Move(x, y, z);
} else if (input == "hd") {
yaw = 0.0;
pitch = 1.0;
need_print = true;
res = client.RotateHead(pitch, yaw);
} else if (input == "hu") {
yaw = 0.0;
pitch = -0.3;
need_print = true;
res = client.RotateHead(pitch, yaw);
} else if (input == "hr") {
yaw = -0.785;
pitch = 0.0;
need_print = true;
res = client.RotateHead(pitch, yaw);
} else if (input == "hl") {
yaw = 0.785;
pitch = 0.0;
need_print = true;
res = client.RotateHead(pitch, yaw);
} else if (input == "ho") {
yaw = 0.0;
pitch = 0.0;
need_print = true;
res = client.RotateHead(pitch, yaw);
} else if (input == "wh") {
res = client.WaveHand(booster::robot::b1::HandAction::kHandOpen);
} else if (input == "ch") {
res = client.WaveHand(booster::robot::b1::HandAction::kHandClose);
} else if (input =="hso") {
res = client.Handshake(booster::robot::b1::HandAction::kHandOpen);
} else if (input =="hsc") {
res = client.Handshake(booster::robot::b1::HandAction::kHandClose);
}
else if (input == "ld") {
res = client.LieDown();
} else if (input == "gu") {
res = client.GetUp();
} else if (input == "mhel") {
booster::robot::Posture tar_posture;
tar_posture.position_ = booster::robot::Position(0.35, 0.25, 0.1);
tar_posture.orientation_ = booster::robot::Orientation(-1.57, -1.57, 0.);
res = client.MoveHandEndEffectorV2(
tar_posture, 2000, booster::robot::b1::HandIndex::kLeftHand);
tar_posture.position_ = booster::robot::Position(0.35, -0.2, 0.1);
tar_posture.orientation_ = booster::robot::Orientation(1.57, -1.57, 0.);
res = client.MoveHandEndEffectorV2(
tar_posture, 2000, booster::robot::b1::HandIndex::kRightHand);
} else if (input == "gopenl") {
booster::robot::b1::GripperMotionParameter motion_param;
motion_param.position_ = 500;
motion_param.force_ = 100;
motion_param.speed_ = 100;
res = client.ControlGripper(
motion_param, booster::robot::b1::GripperControlMode::kPosition,
booster::robot::b1::HandIndex::kLeftHand);
} else if (input == "gft") {
booster::robot::Frame src = booster::robot::Frame::kBody;
booster::robot::Frame dst = booster::robot::Frame::kRightHand;
booster::robot::Transform transform;
res = client.GetFrameTransform(src, dst, transform);
if (res == 0) {
std::cout << "pos:" << transform.position_.x_ << " " << transform.position_.y_
<< " " << transform.position_.z_ << std::endl;
std::cout << "ori:" << transform.orientation_.x_ << " " << transform.orientation_.y_
<< " " << transform.orientation_.z_ << " "
<< transform.orientation_.w_ << std::endl;
}
} else if (input == "hcm-start") {
res = client.SwitchHandEndEffectorControlMode(true);
} else if (input == "hcm-stop") {
res = client.SwitchHandEndEffectorControlMode(false);
} else if (input == "hand-down") {
booster::robot::Posture tar_posture;
tar_posture.position_ = booster::robot::Position(0.28, -0.25, 0.05);
tar_posture.orientation_ = booster::robot::Orientation(0., 0., 0.);
res = client.MoveHandEndEffector(
tar_posture, 1000, booster::robot::b1::HandIndex::kRightHand);
std::this_thread::sleep_for(std::chrono::milliseconds(300));
hand_action_count++;
int random = rand() % 3;
if (random == 0) {
HandRock(client);
} else if (random == 1) {
HandScissor(client);
} else {
HandPaper(client);
}
} else if (input == "handl-down") {
booster::robot::Posture tar_posture;
tar_posture.position_ = booster::robot::Position(0.28, 0.25,0.05);
tar_posture.orientation_ = booster::robot::Orientation(0.,0.,0.);
res = client.MoveHandEndEffector(
tar_posture, 1000, booster::robot::b1::HandIndex::kLeftHand);
} else if (input == "hand-up") {
booster::robot::Posture tar_posture;
tar_posture.position_ = booster::robot::Position(0.25, -0.3, 0.25);
tar_posture.orientation_ = booster::robot::Orientation(0., -1.0, 0.);
res = client.MoveHandEndEffector(
tar_posture, 1000, booster::robot::b1::HandIndex::kRightHand);
std::this_thread::sleep_for(std::chrono::milliseconds(300));
HandPaper(client);
} else if (input == "handl-up") {
booster::robot::Posture tar_posture;
tar_posture.position_ = booster::robot::Position(0.25, 0.3, 0.25);
tar_posture.orientation_ = booster::robot::Orientation(0., -1.0, 0.);
res = client.MoveHandEndEffector(
tar_posture, 1000, booster::robot::b1::HandIndex::kLeftHand);
}
else if (input == "combo") {
RobotArmHeadCombo(client);
}
else if (input == "advance") {
AdvanceMove(client);
}
else if (input == "interact") {
RobotInteractiveShow(client);
}
else if (input == "dance") {
RobotDance2(client);
}
else if (input == "grasp") {
HandGrasp(client);
} else if (input == "ok") {
HandOk(client);
} else if (input == "paper") {
HandPaper(client);
} else if (input == "scissor") {
HandScissor(client);
} else if (input == "rock") {
HandRock(client);
}
if (need_print) {
std::cout << "Param: " << x << " " << y << " " << z << std::endl;
std::cout << "Head param: " << pitch << " " << yaw << std::endl;
}
if (res != 0) {
std::cout << "Request failed: error = " << res << std::endl;
}
}
}
return 0;
}
UCLAMP0501P.TCT：超低电容TVS二极管优势尽显
扫一扫
1457
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
