SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat:head fun1

Browse Source
This commit is contained in:
lgv 2025-11-13 20:25:19 +08:00
parent aeae44b8a4
commit 3748b59c3d
17 changed files with 888 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -49,7 +49,7 @@ protobuf_generate(
PROTOC_OUT_DIR ${PROTO_BINARY_DIR}
)
target_include_directories(proto-objects PUBLIC ${PROTO_BINARY_DIR} ${PROTO_IMPORT_DIR})
target_link_libraries(proto-objects PUBLIC protobuf::libprotobuf gRPC::grpc++)
target_link_libraries(proto-objects PUBLIC protobuf::libprotobuf gRPC::grpc++ gRPC::grpc++_reflection)
############################################################
# include

32
config/cabin_robot.xml
View File

@ -40,7 +40,7 @@
bufferSize="50"
verbose="false">
<CanManger id="" devId="">
<LeftArmCan id = " " devId = " " channelId ="0" enable="false" toolFrame="L_FINGER_TIP">
<LeftArmCan id = " " devId = " " channelId ="0" enable="true" toolFrame="L_FINGER_TIP">
<Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
@ -73,19 +73,27 @@
</Robot>
<BioHead>
<!-- <esp32 id="bio_head" serial="/dev/ttyUSB0" ctrlFreq="50">-->
<!-- &lt;!&ndash; 配置左眉毛，舵机通道 0~3 &ndash;&gt;-->
<!-- <EyeBrow serial="64:0~3" offest="90 90 90 90"-->
<!-- jLmtUp="170 170 170 170" jLmtLow="10 10 10 10"/>-->
<esp32 id="bio_head" serial="/dev/ttyUSB0" ctrlFreq="50">
<!-- &lt;!&ndash; 配置眼睛，舵机通道 4~9 &ndash;&gt;-->
<!-- <Eye serial="64:4~9" offest="90 90 90 90 90 90"-->
<!-- jLmtUp="170 170 170 170 170 170" jLmtLow="10 10 10 10 10 10"/>-->
<!-- 眉毛 -->
<EyeBrow serial="64:0~3"
offest="90 90 90 90"
jLmtUp="90 170 155 110"
jLmtLow="20 77 90 20"/>
<!-- &lt;!&ndash; 配置嘴巴，舵机通道 0~8 &ndash;&gt;-->
<!-- <Mouth serial="65:0~8" offest="90 90 90 90 90 90 90 90 90"-->
<!-- jLmtUp="170 170 170 170 170 170 170 170 170" jLmtLow="10 10 10 10 10 10 10 10 10"/>-->
<!-- </esp32>-->
<!-- 眼睛 -->
<Eye serial="64:4~9"
offest="90 90 90 90 90 90"
jLmtUp="90 150 165 90 120 115"
jLmtLow="20 90 90 25 70 75"/>
<!-- 嘴巴 -->
<Mouth serial="65:0~9"
offest="90 90 90 90 90 90 90 90 90 90"
jLmtUp="150 110 130 140 100 105 110 125 90 95"
jLmtLow="70 30 80 80 65 55 45 80 85 90"/>
</esp32>
</BioHead >
<Microphone>

10
include/devices/abstract_biohead.h
View File

@ -65,10 +65,20 @@ namespace cmvr::device {
virtual void eStop() {};
virtual void setExpressionPose(FacialExpressionState& expression_state, double vel=0, double acc=0) {}
virtual void streamFacialPose(FacialExpressionState& expression_state, double vel, double acc) {}
virtual void speakstart(){};
virtual void speakstop(){};
virtual void expressionHappy() {}; // 高兴
virtual void expressionSurprised() {}; // 惊讶
virtual void expressionTired() {}; // 困倦（疲惫）
FacialExpressionState expression_state_;
std::atomic<bool> emergency_stop_requested = false;
};
} // namespace cmvr::device

8
include/devices/abstract_motor.h
View File

@ -111,6 +111,14 @@ namespace cmvr::device{
protocol_->setQ(node_id_, q);
}
virtual void set(double q,double qd) {
std::scoped_lock lock(mtx_);
if (!protocol_) {
throw std::runtime_error("Protocol not set for motor");
}
protocol_->set(node_id_,q, qd);
}
virtual bool calibrateZeroQ() {
std::scoped_lock lock(mtx_);
if (!protocol_) {

9
include/service/grpc_head_service.h
View File

@ -26,6 +26,15 @@ namespace cmvr::service
grpc::Status EmergencyStop(grpc::ServerContext* context,
const api::EmergencyStop_Request* request,
api::EmergencyStop_Feedback* response) override;
grpc::Status SpeakStart(grpc::ServerContext* context, const cmvr::api::SpeakStart_Request* request, cmvr::api::SpeakStart_Feedback* response) override;
grpc::Status SpeakStop(grpc::ServerContext* context, const cmvr::api::SpeakStop_Request* request, cmvr::api::SpeakStop_Feedback* response) override;
grpc::Status Happy(grpc::ServerContext* context, const cmvr::api::Happy_Request* request, cmvr::api::Happy_Feedback* response) override;
grpc::Status Surprise(grpc::ServerContext* context, const cmvr::api::Surprise_Request* request, cmvr::api::Surprise_Feedback* response) override;
grpc::Status ExpressionTired(grpc::ServerContext* context, const cmvr::api::ExpressionTired_Request* request, cmvr::api::ExpressionTired_Feedback* response) override;
private:
device::DeviceManager& dmgr_;

57
protos/cmvr/api/biohead_command.proto
View File

@ -174,4 +174,61 @@ message EmergencyStop {
}
}
message SpeakStart {
message Request {
CommandHeader.Request header = 1; // 通用命令头
}
message Feedback {
CommandHeader.Feedback header = 1; // 通用反馈头
}
}
message SpeakStop {
message Request {
CommandHeader.Request header = 1; // 通用命令头
}
message Feedback {
CommandHeader.Feedback header = 1; // 通用反馈头
}
}
// 表情控制 - 高兴
message Happy {
message Request {
CommandHeader.Request header = 1; // 通用命令头
}
message Feedback {
CommandHeader.Feedback header = 1; // 通用反馈头
}
}
// 表情控制 - 惊讶
message Surprise {
message Request {
CommandHeader.Request header = 1; // 通用命令头
}
message Feedback {
CommandHeader.Feedback header = 1; // 通用反馈头
}
}
message ExpressionTired {
message Request {
CommandHeader.Request header = 1;
}
message Feedback {
CommandHeader.Feedback header = 1;
}
}

14
protos/cmvr/api/biohead_service.proto
View File

@ -19,4 +19,18 @@ service BioHeadService {
// 紧急停止
rpc EmergencyStop(EmergencyStop.Request) returns (EmergencyStop.Feedback){};
// 开始说话
rpc SpeakStart(SpeakStart.Request) returns (SpeakStart.Feedback){};
// 停止说话
rpc SpeakStop(SpeakStop.Request) returns (SpeakStop.Feedback){};
rpc Happy(Happy.Request) returns (Happy.Feedback){};
rpc Surprise(Surprise.Request) returns (Surprise.Feedback){};
rpc ExpressionTired(ExpressionTired.Request) returns (ExpressionTired.Feedback){};
}

485
src/devices/biohead/biohead_esp32/biohead_esp32.cpp
View File

@ -227,6 +227,491 @@ void BioHeadRobot::streamFacialPose(FacialExpressionState& expression_state, dou
sendServoCommands(joints, duration);
}
void BioHeadRobot::speakstart() {
if (speak_running_.load()) {
LOG(INFO) << "[BioHeadRobot] speak thread already running.";
return;
}
// 检查 channels 中是否有 65:8 和 65:9
bool found8 = false, found9 = false;
for (size_t i = 0; i < channels_.size(); ++i) {
if (channels_[i].addr == 65 && channels_[i].channel == 8) found8 = true;
if (channels_[i].addr == 65 && channels_[i].channel == 9) found9 = true;
}
if (!found8 || !found9) {
LOG(ERROR) << "[BioHeadRobot] Required servo channels not found (addr 65 ch 8/9). speakstart aborted.";
return;
}
// 启动线程
speak_running_.store(true);
// 清理旧线程（若有）
if (speak_thread_ && speak_thread_->joinable()) {
try {
speak_thread_->join();
} catch (...) {
LOG(WARNING) << "[BioHeadRobot] exception joining previous speak thread.";
}
speak_thread_.reset();
}
speak_thread_ = std::make_shared<std::thread>(&BioHeadRobot::speakthread, this);
LOG(INFO) << "[BioHeadRobot] speak thread started.";
}
void BioHeadRobot::speakstop() {
{
if (!speak_running_.load()) {
LOG(INFO) << "[BioHeadRobot] speak thread not running.";
return;
}
speak_running_.store(false);
}
// 唤醒线程（如果在 wait 中）
// join 并清理线程对象
if (speak_thread_) {
if (speak_thread_->joinable()) {
try {
speak_thread_->join();
} catch (const std::exception &e) {
LOG(WARNING) << "[BioHeadRobot] exception when joining speak thread: " << e.what();
} catch (...) {
LOG(WARNING) << "[BioHeadRobot] unknown exception when joining speak thread.";
}
}
speak_thread_.reset();
}
LOG(INFO) << "[BioHeadRobot] speak thread stopped.";
}
// void BioHeadRobot::speakthread() {
// LOG(INFO) << "[BioHeadRobot] speakthread running.";
//
// // 固定参数
// const double freq = 3.0; // Hz
// const int step_ms = 40; // 下发周期（ms）
// const double closed_angle = 90.0;
// const double open8 = 83.0; // ch8 张嘴角
// const double open9 = 97.0; // ch9 张嘴角
//
// // 找到 channels_ 中对应索引（addr=65 ch=8/9）
// int idx8 = -1, idx9 = -1;
// for (size_t i = 0; i < channels_.size(); ++i) {
// if (channels_[i].addr == 65 && channels_[i].channel == 8) idx8 = static_cast<int>(i);
// if (channels_[i].addr == 65 && channels_[i].channel == 9) idx9 = static_cast<int>(i);
// }
// if (idx8 < 0 || idx9 < 0) {
// LOG(ERROR) << "[BioHeadRobot] speakthread: required channels (65:8/9) not found. Exiting thread.";
// speak_running_.store(false);
// return;
// }
//
// // 以当前角度为基准
// std::vector<double> base = current_joints_;
// if (base.size() != channels_.size()) base.resize(channels_.size(), 90.0);
//
// auto start = std::chrono::steady_clock::now();
//
// while (speak_running_.load()) {
// auto now = std::chrono::steady_clock::now();
// double t = std::chrono::duration_cast<std::chrono::duration<double>>(now - start).count();
//
// // 正弦产生 0..1 区间因子
// double phase = 2.0 * M_PI * freq * t;
// double s = 0.5 * (1.0 + std::sin(phase)); // 0..1
//
// // 计算角度（直接使用度数，不用normalizeToAngle）
// double target8 = closed_angle + (open8 - closed_angle) * s;
// double target9 = closed_angle + (open9 - closed_angle) * s;
//
// // clamp 到限位
// target8 = std::clamp(target8, min_angles_[idx8], max_angles_[idx8]);
// target9 = std::clamp(target9, min_angles_[idx9], max_angles_[idx9]);
//
// // 构建目标角数组（只修改 idx8/idx9）
// std::vector<double> tgt = base;
// tgt[idx8] = target8;
// tgt[idx9] = target9;
//
// // ---- 构造 ESP32 协议帧 ----
// std::vector<uint8_t> raw_data;
// for (int idx : {idx8, idx9}) {
// uint16_t angle_raw = static_cast<uint16_t>(std::round(tgt[idx])); // 直接角度
// raw_data.push_back(channels_[idx].addr); // 地址
// raw_data.push_back(channels_[idx].channel); // 通道
// raw_data.push_back(angle_raw); // 角度（1字节即可）
// raw_data.push_back(step_ms & 0xFF); // 持续时间低字节
// raw_data.push_back((step_ms >> 8) & 0xFF); // 持续时间高字节
// }
//
// // 打印发送数据
// std::string json_output = "{ \"raw_data\": [";
// for (size_t i = 0; i < raw_data.size(); ++i) {
// json_output += std::to_string(raw_data[i]);
// if (i < raw_data.size() - 1) json_output += ", ";
// }
// json_output += "] }";
// std::cout << "Sending data at step: " << json_output << std::endl;
//
// // 下发
// serial_->sendRawServoData(raw_data);
//
// // 控制循环频率
// std::this_thread::sleep_for(std::chrono::milliseconds(step_ms));
// }
//
// // 退出前恢复基准角
// std::vector<uint8_t> restore_data;
// for (int idx : {idx8, idx9}) {
// uint16_t angle_raw = static_cast<uint16_t>(std::round(base[idx]));
// restore_data.push_back(channels_[idx].addr);
// restore_data.push_back(channels_[idx].channel);
// restore_data.push_back(angle_raw);
// restore_data.push_back(200 & 0xFF);
// restore_data.push_back((200 >> 8) & 0xFF);
// }
// serial_->sendRawServoData(restore_data);
// LOG(INFO) << "[BioHeadRobot] speakthread exiting and restored base pose.";
// }
void BioHeadRobot::speakthread() {
LOG(INFO) << "[BioHeadRobot] speakthread running.";
// 固定参数
const double freq = 3.0; // Hz
const int step_ms = 40; // 下发周期（ms）
const double closed_angle = 90.0;
const double open8 = 80.0; // ch8 张嘴角
const double open9 = 100.0; // ch9 张嘴角
// 眼睛舵机参数 (addr=64, channels=4~8)
const std::vector<double> eye_open_angles = {40, 120, 125, 50, 75}; // 睁开角度
const std::vector<double> eye_close_angles = {90, 90, 90, 90, 75}; // 闭合角度
// 找到眼睛舵机索引 (addr=64, channels=4~8)
std::vector<int> eye_indexes;
for (size_t i = 0; i < channels_.size(); ++i) {
if (channels_[i].addr == 64 && channels_[i].channel >= 4 && channels_[i].channel <= 8) {
eye_indexes.push_back(static_cast<int>(i));
}
}
// 找到嘴角舵机索引（addr=65 ch=8/9）
int idx8 = -1, idx9 = -1;
for (size_t i = 0; i < channels_.size(); ++i) {
if (channels_[i].addr == 65 && channels_[i].channel == 8) idx8 = static_cast<int>(i);
if (channels_[i].addr == 65 && channels_[i].channel == 9) idx9 = static_cast<int>(i);
}
if (idx8 < 0 || idx9 < 0) {
LOG(ERROR) << "[BioHeadRobot] speakthread: required channels (65:8/9) not found. Exiting thread.";
speak_running_.store(false);
return;
}
// 以当前角度为基准
std::vector<double> base = current_joints_;
if (base.size() != channels_.size()) {
base.resize(channels_.size(), 90.0);
}
// 简单的伪随机数生成（避免依赖C++11随机数库）
unsigned int seed = static_cast<unsigned int>(std::chrono::system_clock::now().time_since_epoch().count());
// 计时器
auto start = std::chrono::steady_clock::now();
auto last_blink_time = std::chrono::steady_clock::now();
// 使用简单的线性同余生成器
auto simple_rand = [&seed](double min_val, double max_val) -> double {
seed = (seed * 1103515245 + 12345) & 0x7FFFFFFF;
return min_val + (max_val - min_val) * (seed / 2147483647.0);
};
double next_blink_interval = simple_rand(2.0, 8.0); // 2-8秒随机眨眼间隔
// 状态变量
bool is_blinking = false;
auto blink_start_time = std::chrono::steady_clock::now();
double blink_duration = 0.0;
// 眼睛状态（true=睁开，false=闭合）
bool eyes_open = true;
while (speak_running_.load()) {
auto now = std::chrono::steady_clock::now();
double t = std::chrono::duration_cast<std::chrono::duration<double>>(now - start).count();
// 检查是否需要开始眨眼
if (!is_blinking && eyes_open) {
double time_since_last_blink = std::chrono::duration_cast<std::chrono::duration<double>>(now - last_blink_time).count();
if (time_since_last_blink >= next_blink_interval) {
is_blinking = true;
blink_start_time = now;
blink_duration = simple_rand(0.1, 0.3); // 眨眼持续时间0.1-0.3秒
eyes_open = false;
LOG(INFO) << "[BioHeadRobot] Starting blink, duration: " << blink_duration << "s";
}
}
// 处理眨眼状态
if (is_blinking) {
double blink_elapsed = std::chrono::duration_cast<std::chrono::duration<double>>(now - blink_start_time).count();
if (blink_elapsed >= blink_duration) {
// 眨眼结束
is_blinking = false;
eyes_open = true;
last_blink_time = now;
next_blink_interval = simple_rand(2.0, 8.0);
LOG(INFO) << "[BioHeadRobot] Blink completed, next blink in: " << next_blink_interval << "s";
}
}
// 正弦产生 0..1 区间因子（嘴角运动）
double phase = 2.0 * M_PI * freq * t;
double s = 0.5 * (1.0 + std::sin(phase)); // 0..1
// 计算嘴角角度（直接使用度数，不用normalizeToAngle）
double target8 = closed_angle + (open8 - closed_angle) * s;
double target9 = closed_angle + (open9 - closed_angle) * s;
// clamp 到限位
target8 = std::clamp(target8, min_angles_[idx8], max_angles_[idx8]);
target9 = std::clamp(target9, min_angles_[idx9], max_angles_[idx9]);
// 构建目标角数组
std::vector<double> tgt = base;
// 设置嘴角
tgt[idx8] = target8;
tgt[idx9] = target9;
// 设置眼睛状态
if (eye_indexes.size() == 5) {
const std::vector<double>& eye_angles = eyes_open ? eye_open_angles : eye_close_angles;
for (size_t i = 0; i < eye_indexes.size(); ++i) {
if (eye_indexes[i] < tgt.size()) {
tgt[eye_indexes[i]] = eye_angles[i];
}
}
}
// ---- 构造 ESP32 协议帧 ----
std::vector<uint8_t> raw_data;
// 添加嘴角控制
for (int idx : {idx8, idx9}) {
uint8_t angle_raw = static_cast<uint8_t>(std::round(tgt[idx]));
raw_data.push_back(channels_[idx].addr);
raw_data.push_back(channels_[idx].channel);
raw_data.push_back(angle_raw);
raw_data.push_back(step_ms & 0xFF);
raw_data.push_back((step_ms >> 8) & 0xFF);
}
// 添加眼睛控制
if (eye_indexes.size() == 5) {
for (int idx : eye_indexes) {
uint8_t angle_raw = static_cast<uint8_t>(std::round(tgt[idx]));
raw_data.push_back(channels_[idx].addr);
raw_data.push_back(channels_[idx].channel);
raw_data.push_back(angle_raw);
raw_data.push_back(step_ms & 0xFF);
raw_data.push_back((step_ms >> 8) & 0xFF);
}
}
// 打印发送数据（调试用）
if (static_cast<int>(t * 1000) % 100 == 0) { // 每100个周期打印一次，避免过于频繁
std::string json_output = "{ \"raw_data\": [";
for (size_t i = 0; i < raw_data.size(); ++i) {
json_output += std::to_string(raw_data[i]);
if (i < raw_data.size() - 1) json_output += ", ";
}
json_output += "] }";
std::cout << "Sending data: " << json_output << std::endl;
}
// 下发
serial_->sendRawServoData(raw_data);
// 控制循环频率
std::this_thread::sleep_for(std::chrono::milliseconds(step_ms));
}
// 退出前恢复基准角
std::vector<uint8_t> restore_data;
// 恢复嘴角
for (int idx : {idx8, idx9}) {
uint8_t angle_raw = static_cast<uint8_t>(std::round(base[idx]));
restore_data.push_back(channels_[idx].addr);
restore_data.push_back(channels_[idx].channel);
restore_data.push_back(angle_raw);
restore_data.push_back(200 & 0xFF);
restore_data.push_back((200 >> 8) & 0xFF);
}
// 恢复眼睛（确保睁开）
if (eye_indexes.size() == 5) {
for (size_t i = 0; i < eye_indexes.size(); ++i) {
uint8_t angle_raw = static_cast<uint8_t>(std::round(eye_open_angles[i]));
restore_data.push_back(channels_[eye_indexes[i]].addr);
restore_data.push_back(channels_[eye_indexes[i]].channel);
restore_data.push_back(angle_raw);
restore_data.push_back(200 & 0xFF);
restore_data.push_back((200 >> 8) & 0xFF);
}
}
serial_->sendRawServoData(restore_data);
LOG(INFO) << "[BioHeadRobot] speakthread exiting and restored base pose.";
}
void BioHeadRobot::expressionHappy() {
const int step_ms = 200;
// 设备64角度（10通道）
const std::vector<double> device_64_angles = {90, 90, 90, 90, 70, 110, 110, 70, 90, 90};
// 设备65角度（10通道）
const std::vector<double> device_65_angles = {90, 85, 95, 100, 90, 105, 80, 110, 85, 95};
std::vector<uint8_t> raw_data;
// 下发设备64的10个通道（0~9）
for (int ch = 0; ch < 10; ++ch) {
uint8_t angle = static_cast<uint8_t>(std::round(device_64_angles[ch]));
raw_data.push_back(64); // 地址
raw_data.push_back(ch); // 通道号
raw_data.push_back(angle); // 角度
raw_data.push_back(step_ms & 0xFF); // 低字节
raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节
}
// 下发设备65的10个通道（0~9）
for (int ch = 0; ch < 10; ++ch) {
uint8_t angle = static_cast<uint8_t>(std::round(device_65_angles[ch]));
raw_data.push_back(65); // 地址
raw_data.push_back(ch); // 通道号
raw_data.push_back(angle); // 角度
raw_data.push_back(step_ms & 0xFF); // 低字节
raw_data.push_back((step_ms >> 8) & 0xFF); // 高字节
}
serial_->sendRawServoData(raw_data);
}
void BioHeadRobot::expressionSurprised() {
const int step_ms = 200;
const std::vector<double> device_64_angles = {60, 170, 120, 90, 60, 120, 120, 60, 90, 90};
const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 83, 97};
std::vector<uint8_t> raw_data;
for (size_t i = 0; i < 10; ++i) {
int idx = -1;
for (size_t j = 0; j < channels_.size(); ++j) {
if (channels_[j].addr == 64 && channels_[j].channel == 4 + i) {
idx = j;
break;
}
}
if (idx >= 0) {
uint8_t angle = static_cast<uint8_t>(std::round(device_64_angles[i]));
raw_data.push_back(64);
raw_data.push_back(4 + i);
raw_data.push_back(angle);
raw_data.push_back(step_ms & 0xFF);
raw_data.push_back((step_ms >> 8) & 0xFF);
}
}
for (size_t i = 0; i < 10; ++i) {
int idx = -1;
for (size_t j = 0; j < channels_.size(); ++j) {
if (channels_[j].addr == 65 && channels_[j].channel == 8 + i) {
idx = j;
break;
}
}
if (idx >= 0) {
uint8_t angle = static_cast<uint8_t>(std::round(device_65_angles[i]));
raw_data.push_back(65);
raw_data.push_back(8 + i);
raw_data.push_back(angle);
raw_data.push_back(step_ms & 0xFF);
raw_data.push_back((step_ms >> 8) & 0xFF);
}
}
serial_->sendRawServoData(raw_data);
}
void BioHeadRobot::expressionTired() {
const int step_ms = 200;
const std::vector<double> device_64_angles = {90, 90, 90, 90, 75, 105, 105, 75, 90, 90};
const std::vector<double> device_65_angles = {90, 90, 90, 90, 90, 90, 90, 90, 90, 90};
std::vector<uint8_t> raw_data;
for (size_t i = 0; i < 10; ++i) {
int idx = -1;
for (size_t j = 0; j < channels_.size(); ++j) {
if (channels_[j].addr == 64 && channels_[j].channel == 4 + i) {
idx = j;
break;
}
}
if (idx >= 0) {
uint8_t angle = static_cast<uint8_t>(std::round(device_64_angles[i]));
raw_data.push_back(64);
raw_data.push_back(4 + i);
raw_data.push_back(angle);
raw_data.push_back(step_ms & 0xFF);
raw_data.push_back((step_ms >> 8) & 0xFF);
}
}
for (size_t i = 0; i < 10; ++i) {
int idx = -1;
for (size_t j = 0; j < channels_.size(); ++j) {
if (channels_[j].addr == 65 && channels_[j].channel == 8 + i) {
idx = j;
break;
}
}
if (idx >= 0) {
uint8_t angle = static_cast<uint8_t>(std::round(device_65_angles[i]));
raw_data.push_back(65);
raw_data.push_back(8 + i);
raw_data.push_back(angle);
raw_data.push_back(step_ms & 0xFF);
raw_data.push_back((step_ms >> 8) & 0xFF);
}
}
serial_->sendRawServoData(raw_data);
}
void BioHeadRobot::sendServoCommands(const std::vector<double>& targets, uint16_t duration_ms) {
std::vector<uint8_t> addrs, chs;
std::vector<uint16_t> raws;

12
src/devices/biohead/biohead_esp32/biohead_esp32.h
View File

@ -25,6 +25,13 @@ namespace cmvr::device {
void eStop() override;
void setExpressionPose(FacialExpressionState& expression_state, double vel = 0.5, double acc = 0.1) override;
void streamFacialPose(FacialExpressionState& expression_state, double vel, double acc) override;
void speakstart() override;
void speakstop() override;
void speakthread();
void expressionHappy()override;
void expressionSurprised()override;
void expressionTired()override;
private:
@ -34,6 +41,7 @@ namespace cmvr::device {
uint16_t angleToRaw(double angle);
double normalizeToAngle(double normalized, size_t index);
std::shared_ptr<SerialPort> serial_;
std::string port_name_;
@ -48,6 +56,10 @@ namespace cmvr::device {
double angle_deadband_{36};
std::vector<double> last_joints_; // 上次下发角度缓存
std::shared_ptr<std::thread> speak_thread_;
std::atomic<bool> speak_running_{false};
};

1
src/devices/motor/motor_protocol_interface.h
View File

@ -27,6 +27,7 @@ namespace cmvr {
virtual bool initNode(uint8_t node_id) = 0;
virtual void setQ(uint8_t node_id, double angle_rad) = 0;
virtual void set(uint8_t node_id, double angle_rad,double vel) = 0;
virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0;
virtual msgs::RunMode getMode(uint8_t node_id) = 0;
virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0;

14
src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.cpp
View File

@ -15,12 +15,12 @@ void Ti5MotorRPDO1::UpdateData(uint8_t *data) {
data[1] = target_pos_ >> 8 & 0xFF;
data[2] = target_pos_ >> 16 & 0xFF;
data[3] = target_pos_ >> 24 & 0xFF;
//
// data[4] = ctrl_word_ & 0xFF;
// data[5] = ctrl_word_ >> 8 & 0xFF;
data[4] = ctrl_word_ & 0xFF;
data[5] = ctrl_word_ >> 8 & 0xFF;
// data[4] = target_vel_ & 0xFF;
// data[5] = target_vel_ >> 8 & 0xFF;
// data[6] = target_vel_ >> 16 & 0xFF;
// data[7] = target_vel_ >> 24 & 0xFF;
data[4] = target_vel_ & 0xFF;
data[5] = target_vel_ >> 8 & 0xFF;
data[6] = target_vel_ >> 16 & 0xFF;
data[7] = target_vel_ >> 24 & 0xFF;
}

2
src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h
View File

@ -25,7 +25,7 @@ namespace cmvr {
void UpdateData(uint8_t *data) override;
int32_t GetLength() const override {
return 0x06;
return 0x08;
}
uint32_t GetPeriod() const override {

10
src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp
View File

@ -107,6 +107,14 @@ void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad) {
}
}
void Ti5MotorCanopenProtocol::set(uint8_t node_id, double angle_rad, double vel) {
auto pos_cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0;
rpdo1_commands_[node_id]->SetTargetPos(pos_cmd);
rpdo1_commands_[node_id]->SetTargetVel(uint32_t(std::abs(speed)));
can_sender_->Update(rpdo1_commands_[node_id]->ID());
}
void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) {
controlword_t cw = {};
@ -323,7 +331,7 @@ void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool start) {
//6 : 映射控制字
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2,
CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
PROFILE_SPEED_6081 << 16 | SUB_INDEX_0 << 8 | 32);
if (start) {

1
src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h
View File

@ -29,6 +29,7 @@ namespace cmvr {
bool initNode(uint8_t node_id) override;
void setMode(uint8_t node_id, msgs::RunMode mode);
void set(uint8_t node_id, double angle_rad, double vel) override;
void setQ(uint8_t node_id, double angle_rad) override;
void setLimitQ(uint8_t node_id, double ub, double lb) override;

241
src/devices/robot/humanoid_robot/humanoid_robot.cpp
View File

@ -62,7 +62,7 @@ template<int DOF>
void HumanoidRobot<DOF>::torqueOff() {
try {
for (const auto &pair: motor_manager_->motorsMap()) {
if (pair.second->jointName() != "WAIST_Y" && pair.second->jointName() != "WAIST_P")
if (pair.second->jointName() != "R_WRIST_R" && pair.second->jointName() != "WAIST_P")
pair.second->torqueOff();
}
} catch (std::exception &e) {
@ -173,136 +173,139 @@ void HumanoidRobot<DOF>::eStop() {
}
}
template<int DOF>
void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
try {
for (const auto &j: cmd) {
auto motor = motor_manager_->getMotor(j.joint_name);
if (motor != nullptr) {
// PPM 模式下 这个实际速度会超30% 左右
motor->setQd(vel);
if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
}
motor->setQ(j.rad);
}
}
//3. wait for completion
bool completion = true;
do {
completion = true;
for (const auto &j: cmd) {
auto motor = motor_manager_->getMotor(j.joint_name);
if (motor != nullptr) {
if (!motor->reachedTargetQ()) {
completion = false;
break;
}
}
}
// 4. while waiting, check flash_cmd_, if it is true, set it false then exit
if (flash_cmd_.load()) {
flash_cmd_.store(false);
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2));
} while (!completion);
} catch (exception &e) {
throw runtime_error(e.what());
}
}
// template<int DOF>
// void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
// if (cmd.empty()) return;
// try {
// for (const auto &j: cmd) {
// auto motor = motor_manager_->getMotor(j.joint_name);
// if (motor != nullptr) {
// // PPM 模式下 这个实际速度会超30% 左右
// motor->setQd(vel);
//
// std::unordered_map<std::string, double> cur_joints_angle;
// cur_joints_angle.reserve(cmd.size());
//
// std::vector<std::shared_ptr<AbstractMotor> > motors;
// motors.reserve(cmd.size());
//
// for (auto &jp: cmd) {
// auto motor = motor_manager_->getMotor(jp.joint_name);
// motors.push_back(motor);
// // 切 CSP
// if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
// motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
// if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
// motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
// }
// motor->setQ(j.rad);
// }
// // 为 map 预填 key
// cur_joints_angle.emplace(jp.joint_name, 0.0);
// }
//
// getJointQ(cur_joints_angle);
//
// std::vector<double> q_start, q_goal;
// q_start.reserve(cmd.size());
// q_goal.reserve(cmd.size());
// for (auto &jp: cmd) {
// auto it = cur_joints_angle.find(jp.joint_name);
// if (it == cur_joints_angle.end()) {
// LOG(ERROR) << "missing current angle for " << jp.joint_name;
// //3. wait for completion
// bool completion = true;
// do {
// completion = true;
// for (const auto &j: cmd) {
// auto motor = motor_manager_->getMotor(j.joint_name);
// if (motor != nullptr) {
// if (!motor->reachedTargetQ()) {
// completion = false;
// break;
// }
// }
// }
// // 4. while waiting, check flash_cmd_, if it is true, set it false then exit
// if (flash_cmd_.load()) {
// flash_cmd_.store(false);
// return;
// }
// q_start.push_back(it->second); // 当前读到的弧度
// q_goal.push_back(jp.rad); // 目标弧度
// std::this_thread::sleep_for(std::chrono::milliseconds(2));
// } while (!completion);
// } catch (exception &e) {
// throw runtime_error(e.what());
// }
// if (q_start.size() != q_goal.size()) return;
//
//
// TrajPtr traj;
// joint_space_planner_->setPathType(PathType::Quintic);
// joint_space_planner_->setSymmetricLimits(std::vector<double>(cmd.size(), vel),
// std::vector<double>(cmd.size(), acc));
//
//
// bool ok = joint_space_planner_->plan(q_start, q_goal, traj);
// if (!ok) {
// LOG(ERROR) << "planner plan() failed";
// return;
// }
//
//
// // 10ms
// const double dt = 0.01;
// auto samples = joint_space_planner_->sampleTrajectory(traj, dt);
// if (samples.size() < 2) {
// LOG(ERROR) << "trajectory too short";
// return;
// }
//
// const auto t0 = std::chrono::steady_clock::now();
//
// // k=0 开始
// size_t k = 0;
// const size_t K = samples.size();
//
// while (k < K) {
// // // ——根据墙钟计算“应当在的拍次”，用于追赶——
// // auto now = std::chrono::steady_clock::now();
// // double elapsed = std::chrono::duration<double>(now - t0).count();
// // size_t k_should = static_cast<size_t>(std::floor(elapsed / dt)); // 向下取整更保守
// // if (k_should >= K) k_should = K - 1;
// // if (k < k_should) k = k_should; // 追赶，避免越拖越晚
//
// // ——下发第 k 个采样点——
// const auto &s = samples[k];
//
// for (size_t i = 0; i < motors.size(); ++i) {
template<int DOF>
void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
if (cmd.empty()) return;
LOG(INFO) << "vel " << vel << " acc" << acc << " " << cmd.size() << endl;
std::unordered_map<std::string, double> cur_joints_angle;
cur_joints_angle.reserve(cmd.size());
std::vector<std::shared_ptr<AbstractMotor> > motors;
motors.reserve(cmd.size());
for (auto &jp: cmd) {
auto motor = motor_manager_->getMotor(jp.joint_name);
motors.push_back(motor);
// 切 CSP
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
}
// 为 map 预填 key
cur_joints_angle.emplace(jp.joint_name, 0.0);
}
getJointQ(cur_joints_angle);
std::vector<double> q_start, q_goal;
q_start.reserve(cmd.size());
q_goal.reserve(cmd.size());
for (auto &jp: cmd) {
auto it = cur_joints_angle.find(jp.joint_name);
if (it == cur_joints_angle.end()) {
LOG(ERROR) << "missing current angle for " << jp.joint_name;
return;
}
q_start.push_back(it->second); // 当前读到的弧度
q_goal.push_back(jp.rad); // 目标弧度
}
if (q_start.size() != q_goal.size()) return;
TrajPtr traj;
joint_space_planner_->setPathType(PathType::Quintic);
joint_space_planner_->setSymmetricLimits(std::vector<double>(cmd.size(), vel),
std::vector<double>(cmd.size(), acc));
bool ok = joint_space_planner_->plan(q_start, q_goal, traj);
if (!ok) {
LOG(ERROR) << "planner plan() failed";
return;
}
// 10ms
const double dt = 0.001;
auto samples = joint_space_planner_->sampleTrajectory(traj, dt);
if (samples.size() < 2) {
LOG(ERROR) << "trajectory too short";
return;
}
const auto t0 = std::chrono::steady_clock::now();
// k=0 开始
size_t k = 0;
const size_t K = samples.size();
while (k < K) {
// // ——根据墙钟计算“应当在的拍次”，用于追赶——
// auto now = std::chrono::steady_clock::now();
// double elapsed = std::chrono::duration<double>(now - t0).count();
// size_t k_should = static_cast<size_t>(std::floor(elapsed / dt)); // 向下取整更保守
// if (k_should >= K) k_should = K - 1;
// if (k < k_should) k = k_should; // 追赶，避免越拖越晚
// ——下发第 k 个采样点——
const auto &s = samples[k];
for (size_t i = 0; i < motors.size(); ++i) {
// motors[i]->setQd(s.qd[i]);
// motors[i]->setQ(s.q[i]);
// }
//
// ++k;
// if (k < K) {
// auto next_t = t0 + std::chrono::duration<double>(k * dt);
// std::this_thread::sleep_until(next_t);
// }
// }
// }
motors[i]->set(s.q[i],s.qd[i]);
}
++k;
if (k <= K) {
auto next_t = t0 + std::chrono::duration<double>(k * dt);
std::this_thread::sleep_until(next_t);
}
}
}
template<int DOF>
void HumanoidRobot<DOF>::calibrateZeroQ(const std::string &joint_name) {

4
src/main.cpp
View File

@ -21,7 +21,7 @@
#include "http/httpclient.h"
#include "json/json.h"
#include <grpcpp/ext/proto_server_reflection_plugin.h>
void runServer(const XmlNode &cfg){
using namespace cmvr::device;
using namespace cmvr::service;
@ -32,6 +32,8 @@ void runServer(const XmlNode &cfg){
return;
}
try {
// 🔥 关键！启用反射
grpc::reflection::InitProtoReflectionServerBuilderPlugin();
auto dmgr_cfg = cfg.getChild("DeviceManager");
DeviceManager::getInstance(dmgr_cfg);

123
src/service/grpc_head_service.cpp
View File

@ -227,3 +227,126 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop(
return grpc::Status::OK;
}
}
grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, const cmvr::api::SpeakStart_Request* request, cmvr::api::SpeakStart_Feedback* response)
{
{
try {
string dev_id = request->header().device_id();
auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
if (!robot) {
throw runtime_error("Biohead device not found");
}
robot->speakstart(); // kaish开始
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK;
}
catch (const exception& e) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message(e.what());
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK;
}
}
}
grpc::Status gRPCMBioHeadServiceImpl::SpeakStop(grpc::ServerContext* context, const cmvr::api::SpeakStop_Request* request, cmvr::api::SpeakStop_Feedback* response)
{
try {
string dev_id = request->header().device_id();
auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
if (!robot) {
throw runtime_error("Biohead device not found");
}
robot->speakstop(); // 停止执行
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK;
}
catch (const exception& e) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message(e.what());
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK;
}
}
grpc::Status gRPCMBioHeadServiceImpl::Happy(grpc::ServerContext* context, const cmvr::api::Happy_Request* request, cmvr::api::Happy_Feedback* response)
{
try {
string dev_id = request->header().device_id();
auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
if (!robot) {
throw runtime_error("Biohead device not found");
}
robot->expressionHappy(); // 停止执行
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK;
}
catch (const exception& e) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message(e.what());
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK;
}
}
grpc::Status gRPCMBioHeadServiceImpl::Surprise(grpc::ServerContext* context, const cmvr::api::Surprise_Request* request, cmvr::api::Surprise_Feedback* response)
{
try {
string dev_id = request->header().device_id();
auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
if (!robot) {
throw runtime_error("Biohead device not found");
}
robot->expressionSurprised(); //
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK;
}
catch (const exception& e) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message(e.what());
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK;
}
}
grpc::Status gRPCMBioHeadServiceImpl::ExpressionTired(grpc::ServerContext* context, const cmvr::api::ExpressionTired_Request* request, cmvr::api::ExpressionTired_Feedback* response)
{
try {
string dev_id = request->header().device_id();
auto robot = dmgr_.getDevice<AbstractBiohead>(dev_id);
if (!robot) {
throw runtime_error("Biohead device not found");
}
robot->expressionTired(); // 停止执行
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK;
}
catch (const exception& e) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message(e.what());
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK;
}
}