2077 lines
88 KiB
C++
2077 lines
88 KiB
C++
//
|
||
// Created by xtkuang on 2025/7/24.
|
||
//
|
||
|
||
#include "humanoid_robot.h"
|
||
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
|
||
#include "motor/ti5_motor/ti5_motor.h"
|
||
#include "utils/base/abstract_interpolation.h"
|
||
|
||
|
||
using namespace std;
|
||
using namespace cmvr::device;
|
||
|
||
template<int DOF>
|
||
HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
|
||
try {
|
||
id_ = cfg.getAttrString("id");
|
||
|
||
dof_ = DOF;
|
||
if (!pathExists(cfg.getAttrString("urdf"))) {
|
||
throw runtime_error("urdf file does not exist");
|
||
}
|
||
auto rcfg = cmvr::dyn::LoadRobotFromURDF(
|
||
cfg.getAttrString("urdf"), cfg.getAttrString("baseLink"));
|
||
m_robot_ = std::make_shared<cmvr::dyn::Robot<DOF> >(rcfg);
|
||
joint_names_ = splitString(cfg.getAttrString("jointNames"), ",");
|
||
link_names_ = splitString(cfg.getAttrString("linkNames"), ",");
|
||
if (joint_names_.size() != dof_) {
|
||
throw runtime_error("joint names size mismatched with dof");
|
||
}
|
||
|
||
m_state_ = m_robot_->MakeState(link_names_, joint_names_);
|
||
m_cctrl_ = make_shared<ctrl::CartesianController<DOF> >(m_robot_);
|
||
upd_freq_ = cfg.getAttrDefault("updFreq", 500);
|
||
CSP_buffer_ = make_shared<SPMCRingBuffer<JointPoint> >(cfg.getAttrDefault("bufferSize", 50));
|
||
CSV_buffer_ = make_shared<SPMCRingBuffer<JointVelocityCommand> >(cfg.getAttrDefault("bufferSize", 50));
|
||
CSC_buffer_ = make_shared<SPMCRingBuffer<JointCurrentCommand> >(cfg.getAttrDefault("bufferSize", 50));
|
||
|
||
|
||
auto can_cfg = cfg.getChild("CanManger");
|
||
|
||
auto l_can_cfg = can_cfg.getChild("LeftArmCan");
|
||
l_motors_cfg_ = l_can_cfg.getChildren("Motor");
|
||
l_can_client_ = std::make_shared<SocketCanClientRaw>(l_can_cfg);
|
||
l_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
|
||
l_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
|
||
l_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
|
||
|
||
auto r_can_cfg = can_cfg.getChild("RightArmCan");
|
||
r_motors_cfg_ = r_can_cfg.getChildren("Motor");
|
||
r_can_client_ = std::make_shared<SocketCanClientRaw>(r_can_cfg);
|
||
r_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
|
||
r_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
|
||
r_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
|
||
|
||
auto waist_can_cfg = can_cfg.getChild("WaistCan");
|
||
waist_motors_cfg_ = waist_can_cfg.getChildren("Motor");
|
||
waist_can_client_ = std::make_shared<SocketCanClientRaw>(waist_can_cfg);
|
||
waist_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
|
||
waist_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
|
||
waist_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
|
||
|
||
upd_timer_ = make_shared<FDTimer>();
|
||
upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000),
|
||
[this] { update_state_(); });
|
||
rsm_.store(ROBOT_READY);
|
||
} catch (exception &e) {
|
||
LOG(ERROR) << "HumanoidRobot init failed, id=" << id_;
|
||
throw runtime_error(e.what());
|
||
}
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::init() {
|
||
// 1 === 初始化公共组件 ===
|
||
l_can_client_->init();
|
||
r_can_client_->init();
|
||
waist_can_client_->init();
|
||
auto ret = l_can_sender_->Init(l_can_client_.get(), false);
|
||
if (ret != ErrorCode::OK) {
|
||
LOG(ERROR) << "Failed to init can sender.";
|
||
}
|
||
ret = r_can_sender_->Init(r_can_client_.get(), false);
|
||
if (ret != ErrorCode::OK) {
|
||
LOG(ERROR) << "Failed to init can sender.";
|
||
}
|
||
ret = waist_can_sender_->Init(waist_can_client_.get(), false);
|
||
if (ret != ErrorCode::OK) {
|
||
LOG(ERROR) << "Failed to init can sender.";
|
||
}
|
||
|
||
|
||
ret = l_can_receiver_->Init(l_can_client_.get(), l_message_manager_.get(), false);
|
||
if (ret != ErrorCode::OK) {
|
||
LOG(ERROR) << "Failed to init can receiver.";
|
||
}
|
||
ret = r_can_receiver_->Init(r_can_client_.get(), r_message_manager_.get(), false);
|
||
if (ret != ErrorCode::OK) {
|
||
LOG(ERROR) << "Failed to init can receiver.";
|
||
}
|
||
ret = waist_can_receiver_->Init(waist_can_client_.get(), waist_message_manager_.get(), false);
|
||
if (ret != ErrorCode::OK) {
|
||
LOG(ERROR) << "Failed to init can receiver.";
|
||
}
|
||
|
||
|
||
// 2 === 启动通讯 ===
|
||
l_can_client_->start();
|
||
ret = l_can_sender_->Start();
|
||
if (ret != ErrorCode::OK) {
|
||
LOG(ERROR) << "Failed to start can sender.";
|
||
}
|
||
|
||
r_can_client_->start();
|
||
ret = r_can_sender_->Start();
|
||
if (ret != ErrorCode::OK) {
|
||
LOG(ERROR) << "Failed to start can sender.";
|
||
}
|
||
|
||
waist_can_client_->start();
|
||
ret = waist_can_sender_->Start();
|
||
if (ret != ErrorCode::OK) {
|
||
LOG(ERROR) << "Failed to start can sender.";
|
||
}
|
||
|
||
ret = l_can_receiver_->Start();
|
||
if (ret != ErrorCode::OK) {
|
||
LOG(ERROR) << "Failed to start can receiver.";
|
||
}
|
||
|
||
ret = r_can_receiver_->Start();
|
||
if (ret != ErrorCode::OK) {
|
||
LOG(ERROR) << "Failed to start can receiver.";
|
||
}
|
||
|
||
ret = waist_can_receiver_->Start();
|
||
if (ret != ErrorCode::OK) {
|
||
LOG(ERROR) << "Failed to start can receiver.";
|
||
}
|
||
|
||
// 3 == 创建协议 ===
|
||
auto l_canopen_protocol = std::make_shared<Ti5MotorCanopenProtocol>(l_can_sender_, l_message_manager_);
|
||
auto r_canopen_protocol = std::make_shared<Ti5MotorCanopenProtocol>(r_can_sender_, r_message_manager_);
|
||
auto waist_canopen_protocol = std::make_shared<Ti5MotorCanopenProtocol>(waist_can_sender_, waist_message_manager_);
|
||
|
||
// 4 === 创建 MotorManager ===
|
||
motor_manager_ = std::make_shared<MotorManager>();
|
||
|
||
// for (const auto& cfg : r_motors_cfg_) {
|
||
// auto motor = std::make_shared<Ti5Motor>(cfg);
|
||
// motor->setProtocol(r_canopen_protocol);
|
||
// motor->init(); // 耗时操作
|
||
// motor_manager_->addMotor(motor);
|
||
// }
|
||
//
|
||
// for (const auto& cfg : l_motors_cfg_) {
|
||
// auto motor = std::make_shared<Ti5Motor>(cfg);
|
||
// motor->setProtocol(l_canopen_protocol);
|
||
// motor->init(); // 耗时操作
|
||
// motor_manager_->addMotor(motor);
|
||
// }
|
||
|
||
// 5 === 并行创建电机 ===
|
||
auto left_task = std::async(std::launch::async, [&] {
|
||
LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing LEFT motors...";
|
||
for (const auto &cfg: l_motors_cfg_) {
|
||
auto motor = std::make_shared<Ti5Motor>(cfg);
|
||
motor->setProtocol(l_canopen_protocol);
|
||
motor->init();
|
||
motor_manager_->addMotor(motor);
|
||
}
|
||
});
|
||
|
||
auto right_task = std::async(std::launch::async, [&] {
|
||
LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing RIGHT motors...";
|
||
for (const auto &cfg: r_motors_cfg_) {
|
||
auto motor = std::make_shared<Ti5Motor>(cfg);
|
||
motor->setProtocol(r_canopen_protocol);
|
||
motor->init();
|
||
motor_manager_->addMotor(motor);
|
||
}
|
||
});
|
||
|
||
auto waist_task = std::async(std::launch::async, [&] {
|
||
LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing waist motors...";
|
||
for (const auto &cfg: waist_motors_cfg_) {
|
||
auto motor = std::make_shared<Ti5Motor>(cfg);
|
||
motor->setProtocol(waist_canopen_protocol);
|
||
motor->init();
|
||
motor_manager_->addMotor(motor);
|
||
}
|
||
});
|
||
|
||
// 等待两个线程完成
|
||
left_task.get();
|
||
right_task.get();
|
||
waist_task.get();
|
||
|
||
rsm_.store(ROBOT_ESTOP);
|
||
LOG(INFO) << "All motors initialized successfully.";
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::torqueOff() {
|
||
try {
|
||
if (rsm_.load() == ROBOT_RUNNING) {
|
||
throw runtime_error("robot is running");
|
||
}
|
||
|
||
if (rsm_.load() != ROBOT_TOROFF) {
|
||
for (const auto &pair: motor_manager_->motorsMap()) {
|
||
if (pair.second->jointName() != "WAIST_Y" && pair.second->jointName() != "WAIST_P" )
|
||
pair.second->torqueOff();
|
||
}
|
||
rsm_.store(ROBOT_TOROFF);
|
||
}
|
||
} catch (std::exception &e) {
|
||
throw runtime_error(e.what());
|
||
}
|
||
}
|
||
|
||
|
||
template<int DOF>
|
||
HumanoidRobot<DOF>::~HumanoidRobot() {
|
||
// TODO: close can interfaces
|
||
upd_timer_->stop();
|
||
|
||
std::vector<JointPoint> cmd = {
|
||
{"L_SHOULDER_P", 0.0},
|
||
{"L_SHOULDER_R", -1.31873},
|
||
{"L_SHOULDER_Y", 0.0},
|
||
{"L_ELBOW_R", -0.537621},
|
||
{"L_WRIST_P", 0.0},
|
||
{"L_WRIST_Y", 0.000183204},
|
||
{"L_WRIST_R", 0.0225797},
|
||
|
||
{"R_SHOULDER_P", -0.0201069},
|
||
{"R_SHOULDER_R", 1.46698},
|
||
{"R_SHOULDER_Y", 1.45894},
|
||
{"R_ELBOW_R", 0.159681},
|
||
{"R_WRIST_P", 0.0808349},
|
||
{"R_WRIST_Y", -0.138279},
|
||
{"R_WRIST_R", -0.243169},
|
||
|
||
{"WAIST_Y", 0},
|
||
{"WAIST_P", 0}
|
||
};
|
||
|
||
// this->moveJ(cmd,0.8);
|
||
this->torqueOff();
|
||
}
|
||
|
||
template<int DOF>
|
||
int HumanoidRobot<DOF>::getDOF() {
|
||
return dof_;
|
||
}
|
||
|
||
template<int DOF>
|
||
std::vector<std::string> HumanoidRobot<DOF>::getJointNames() {
|
||
return joint_names_;
|
||
}
|
||
|
||
template<int DOF>
|
||
std::unordered_map<std::string, double> HumanoidRobot<DOF>::getJointQ() const{
|
||
std::unordered_map<std::string, double> joint_qs;
|
||
|
||
for (const auto &pair : motor_manager_->motorsMap()) {
|
||
auto motor = pair.second;
|
||
joint_qs[motor->jointName()] = motor->getQ();
|
||
}
|
||
return joint_qs;
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::getJointQ(std::unordered_map<std::string, double> &joint_qs) const {
|
||
for (auto &pair : joint_qs) {
|
||
auto motor = motor_manager_->getMotor(pair.first);
|
||
if (motor) {
|
||
pair.second = motor->getQ();
|
||
} else {
|
||
pair.second = 0.0;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
template<int DOF>
|
||
std::vector<std::string> HumanoidRobot<DOF>::getLinkNames() {
|
||
return link_names_;
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::getState(RobotState &state) {
|
||
try {
|
||
lock_guard lock(exec_mtx_);
|
||
// TODO: copy m_state_ date into state
|
||
} catch (exception &e) {
|
||
throw runtime_error(e.what());
|
||
}
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::torqueOn() {
|
||
eStop();
|
||
}
|
||
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::eStop() {
|
||
if (rsm_.load() != ROBOT_ESTOP) {
|
||
CSP_buffer_->clear();
|
||
CSV_buffer_->clear();
|
||
CSC_buffer_->clear();
|
||
for (const auto &pair: motor_manager_->motorsMap()) {
|
||
pair.second->brake();
|
||
}
|
||
rsm_.store(ROBOT_ESTOP);
|
||
}
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
|
||
try {
|
||
if (rsm_.load() == ROBOT_RUNNING) {
|
||
flash_cmd_.store(true);
|
||
eStop();
|
||
}
|
||
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
|
||
rsm_.store(ROBOT_RUNNING);
|
||
|
||
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);
|
||
|
||
rsm_.store(ROBOT_ESTOP);
|
||
} else {
|
||
throw runtime_error("rsm invalid");
|
||
}
|
||
} catch (exception &e) {
|
||
throw runtime_error(e.what());
|
||
}
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::calibrateZeroQ(const std::string &joint_name) {
|
||
auto motor = motor_manager_->getMotor(joint_name);
|
||
motor->calibrateZeroQ();
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, double acc) {
|
||
|
||
try {
|
||
if (rsm_.load() == ROBOT_RUNNING) {
|
||
flash_cmd_.store(true);
|
||
eStop();
|
||
}
|
||
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
|
||
rsm_.store(ROBOT_RUNNING);
|
||
|
||
|
||
// update m_state_
|
||
Eigen::Vector<double, DOF> q_init;
|
||
auto q_map = getJointQ();
|
||
q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"],
|
||
q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"],
|
||
q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"],
|
||
q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"];
|
||
|
||
LOG(INFO) << "q_init: " << q_init;
|
||
m_state_->SetQ(q_init);
|
||
m_robot_->ComputeForwardKinematics(m_state_);
|
||
|
||
|
||
Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity();
|
||
T_target.block<3,3>(0,0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz()); // 输入为弧度
|
||
T_target(0,3) = pose.position().x();
|
||
T_target(1,3) = pose.position().y();
|
||
T_target(2,3) = pose.position().z();
|
||
|
||
cmvr::ctrl::PoseTarget target;
|
||
target.T_target = T_target;
|
||
target.w_posrot = 0.5;
|
||
target.weight = 1.0;
|
||
target.link_name = ee_link;
|
||
|
||
// slove ik
|
||
Eigen::Vector<double, DOF> q_cmd;
|
||
bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, ctrl::CartesianController<DOF>::Mode::Position,
|
||
q_cmd, 10000, 1e-6);
|
||
if (!ok) {
|
||
throw runtime_error("solve IK failed");
|
||
}
|
||
std::vector<JointPoint> joint_points{
|
||
{"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]},
|
||
{"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]},
|
||
{"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]},
|
||
{"R_WRIST_R", q_cmd[13]}
|
||
};
|
||
for (const auto &j: joint_points) {
|
||
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: joint_points) {
|
||
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);
|
||
rsm_.store(ROBOT_READY);
|
||
} else {
|
||
throw runtime_error("rsm invalid");
|
||
}
|
||
} catch (exception &e) {
|
||
throw runtime_error(e.what());
|
||
}
|
||
|
||
}
|
||
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel,
|
||
double acc) {
|
||
try {
|
||
if (rsm_.load() == ROBOT_RUNNING) {
|
||
flash_cmd_.store(true);
|
||
eStop();
|
||
}
|
||
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
|
||
rsm_.store(ROBOT_RUNNING);
|
||
|
||
|
||
// update m_state_
|
||
Eigen::Vector<double, DOF> q_init;
|
||
auto q_map = getJointQ();
|
||
q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"],
|
||
q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"],
|
||
q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"],
|
||
q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"];
|
||
|
||
LOG(INFO) << "q_init: " << q_init;
|
||
m_state_->SetQ(q_init);
|
||
m_robot_->ComputeForwardKinematics(m_state_);
|
||
|
||
// slove ik
|
||
Eigen::Vector<double, DOF> q_cmd;
|
||
bool ok = m_cctrl_->compute(m_state_, base_link, targets, 0.002, ctrl::CartesianController<DOF>::Mode::Position,
|
||
q_cmd, 10000, 1e-6);
|
||
if (!ok) {
|
||
throw runtime_error("solve IK failed");
|
||
}
|
||
std::vector<JointPoint> joint_points{
|
||
{"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]},
|
||
{"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]},
|
||
{"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]},
|
||
{"R_WRIST_R", q_cmd[13]}
|
||
};
|
||
// for (const auto &j: joint_points) {
|
||
// 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: joint_points) {
|
||
// 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);
|
||
rsm_.store(ROBOT_READY);
|
||
} else {
|
||
throw runtime_error("rsm invalid");
|
||
}
|
||
} catch (exception &e) {
|
||
throw runtime_error(e.what());
|
||
}
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) {
|
||
try {
|
||
if (rsm_.load() == ROBOT_RUNNING) {
|
||
flash_cmd_.store(true);
|
||
eStop();
|
||
return;
|
||
}
|
||
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
|
||
rsm_.store(ROBOT_RUNNING);
|
||
|
||
// 获取当前关节状态
|
||
Eigen::Vector<double, DOF> q_init;
|
||
auto q_map = getJointQ();
|
||
q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"],
|
||
q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"],
|
||
q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"],
|
||
q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"];
|
||
|
||
LOG(INFO) << "q_init: " << q_init;
|
||
m_state_->SetQ(q_init);
|
||
m_robot_->ComputeForwardKinematics(m_state_);
|
||
|
||
// 获取基座链接索引
|
||
auto base_idx = m_robot_->GetLinkIdx(base_link);
|
||
|
||
// 获取当前末端位姿 - 使用前向运动学计算
|
||
std::vector<Eigen::Matrix4d> current_poses;
|
||
for (const auto& target : targets) {
|
||
auto ee_idx = m_robot_->GetLinkIdx(target.link_name);
|
||
|
||
// 使用正向运动学计算当前位姿
|
||
Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx);
|
||
current_poses.push_back(T);
|
||
|
||
// 打印当前末端执行器的 XYZ 和欧拉角
|
||
if (&target == &targets.front()) {
|
||
Eigen::Vector3d position = T.block<3, 1>(0, 3);
|
||
Eigen::Matrix3d rotation = T.block<3, 3>(0, 0);
|
||
Eigen::Vector3d euler = rotationMatrixToEulerZYX(rotation);
|
||
|
||
LOG(INFO) << "Starting point (Initial position): "
|
||
<< "X: " << position[0] << ", Y: " << position[1] << ", Z: " << position[2];
|
||
LOG(INFO) << "Starting orientation (Euler angles): "
|
||
<< "RX: " << euler[0] << ", RY: " << euler[1] << ", RZ: " << euler[2];
|
||
}
|
||
}
|
||
|
||
// 计算最大距离和插值点数
|
||
double max_distance = 0.0;
|
||
for (size_t i = 0; i < targets.size(); i++) {
|
||
Eigen::Vector3d current_pos = current_poses[i].block<3, 1>(0, 3);
|
||
Eigen::Vector3d target_pos = targets[i].T_target.block<3, 1>(0, 3);
|
||
double distance = (target_pos - current_pos).norm();
|
||
max_distance = std::max(max_distance, distance);
|
||
}
|
||
|
||
// 基于速度和距离计算插值点数
|
||
double move_time = max_distance / vel;
|
||
int num_points = static_cast<int>(move_time * 100); // 100Hz控制频率
|
||
|
||
// 存储所有插值点的关节角度
|
||
std::vector<Eigen::Vector<double, DOF>> joint_trajectory;
|
||
joint_trajectory.reserve(num_points + 1);
|
||
|
||
// 记录上一次成功的关节角度
|
||
Eigen::Vector<double, DOF> last_success_q = q_init;
|
||
|
||
// 预先计算所有插值点的逆运动学
|
||
for (int i = 0; i <= num_points; i++) {
|
||
if (flash_cmd_.load()) {
|
||
flash_cmd_.store(false);
|
||
rsm_.store(ROBOT_READY);
|
||
return;
|
||
}
|
||
|
||
double t = static_cast<double>(i) / num_points;
|
||
|
||
// 创建插值后的目标（只做位置插值，旋转保持不变）
|
||
std::vector<cmvr::ctrl::PoseTarget> interpolated_targets = targets;
|
||
for (size_t j = 0; j < targets.size(); j++) {
|
||
// 位置线性插值
|
||
Eigen::Vector3d current_pos = current_poses[j].block<3, 1>(0, 3);
|
||
Eigen::Vector3d target_pos = targets[j].T_target.block<3, 1>(0, 3);
|
||
Eigen::Vector3d interp_pos = current_pos + t * (target_pos - current_pos);
|
||
|
||
// 保持旋转不变
|
||
Eigen::Matrix3d current_rot_matrix = current_poses[j].block<3, 3>(0, 0);
|
||
interpolated_targets[j].T_target.setIdentity();
|
||
interpolated_targets[j].T_target.block<3, 3>(0, 0) = current_rot_matrix;
|
||
interpolated_targets[j].T_target.block<3, 1>(0, 3) = interp_pos;
|
||
}
|
||
|
||
// 求解逆运动学
|
||
Eigen::Vector<double, DOF> q_cmd;
|
||
bool ok = m_cctrl_->compute(m_state_, base_link, interpolated_targets, 0.002,
|
||
ctrl::CartesianController<DOF>::Mode::Position,
|
||
q_cmd, 10000, 1e-6);
|
||
|
||
if (!ok) {
|
||
LOG(WARNING) << "IK failed at point " << i << ", using last successful configuration";
|
||
q_cmd = last_success_q;
|
||
} else {
|
||
last_success_q = q_cmd;
|
||
}
|
||
|
||
joint_trajectory.push_back(q_cmd);
|
||
|
||
// 获取当前末端执行器的位置 (通过正向运动学)
|
||
m_state_->SetQ(q_cmd);
|
||
m_robot_->ComputeForwardKinematics(m_state_);
|
||
|
||
// 获取当前末端执行器的位姿 (变换矩阵 T)
|
||
auto ee_idx = m_robot_->GetLinkIdx(targets[0].link_name);
|
||
Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx);
|
||
|
||
// 从变换矩阵中提取 XYZ 坐标
|
||
Eigen::Vector3d end_effector_pos = T.block<3, 1>(0, 3);
|
||
|
||
// 打印 IK 解算出的 XYZ 位置
|
||
if (i % 10 == 0) { // 每10个点打印一次，避免日志过多
|
||
LOG(INFO) << "IK solution at point " << i << " : "
|
||
<< "X: " << end_effector_pos[0] << ", Y: " << end_effector_pos[1] << ", Z: " << end_effector_pos[2];
|
||
}
|
||
}
|
||
|
||
// 执行轨迹
|
||
for (int i = 0; i <= num_points; i++) {
|
||
if (flash_cmd_.load()) {
|
||
flash_cmd_.store(false);
|
||
break;
|
||
}
|
||
|
||
// 获取当前时间点的关节角度
|
||
Eigen::Vector<double, DOF> q_cmd = joint_trajectory[i];
|
||
|
||
// 发送关节命令
|
||
std::vector<JointPoint> joint_points{
|
||
{"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]},
|
||
{"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]},
|
||
{"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]},
|
||
{"R_WRIST_R", q_cmd[13]}
|
||
};
|
||
|
||
// 设置每个关节的速度和位置
|
||
for (size_t j = 0; j < joint_points.size(); j++) {
|
||
auto& joint_point = joint_points[j];
|
||
auto motor = motor_manager_->getMotor(joint_point.joint_name);
|
||
if (motor != nullptr) {
|
||
motor->setQ(joint_point.rad);
|
||
}
|
||
}
|
||
|
||
// 等待一段时间，控制频率
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||
}
|
||
|
||
// 等待最终位置到达 - 检查所有关节
|
||
bool completion = true;
|
||
do {
|
||
completion = true;
|
||
for (const auto& name : joint_names_) {
|
||
auto motor = motor_manager_->getMotor(name);
|
||
if (motor != nullptr && !motor->reachedTargetQ()) {
|
||
completion = false;
|
||
break;
|
||
}
|
||
}
|
||
if (flash_cmd_.load()) {
|
||
flash_cmd_.store(false);
|
||
return;
|
||
}
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(2));
|
||
} while (!completion);
|
||
|
||
rsm_.store(ROBOT_READY);
|
||
} else {
|
||
throw std::runtime_error("rsm invalid");
|
||
}
|
||
} catch (std::exception &e) {
|
||
rsm_.store(ROBOT_ESTOP);
|
||
throw std::runtime_error(e.what());
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) {
|
||
try {
|
||
if (rsm_.load() == ROBOT_RUNNING) {
|
||
flash_cmd_.store(true);
|
||
eStop();
|
||
} else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
|
||
rsm_.store(ROBOT_RUNNING);
|
||
|
||
// 获取电机控制对象
|
||
// 这里的控制函数需要根据你的实际实现来进行填充
|
||
// 获取目标关节的电机
|
||
auto motor = motor_manager_->getMotor(joint_name);
|
||
if (motor == nullptr) {
|
||
throw runtime_error("Motor not found for joint: " + joint_name);
|
||
}
|
||
|
||
|
||
// 设置电机的运行模式为速度模式
|
||
if (motor->getMode() != msgs::RUN_MODE_PROFILE_VELOCITY) {
|
||
motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
|
||
}
|
||
|
||
// 设置加速度和目标速度
|
||
motor->setQd(vel);
|
||
|
||
|
||
LOG(INFO) << "开始在关节 " << joint_name << " 上进行速度控制，速度：" << vel << " rad/s，加速度：" << acc << " rad/s²";
|
||
std::this_thread::sleep_for(std::chrono::seconds(10));
|
||
motor->setQd(0);
|
||
LOG(INFO) << "速度控制完成，电机已停止。";
|
||
|
||
|
||
|
||
|
||
// 运动完成后不立即将 rsm_ 置为 READY，防止误操作
|
||
} else {
|
||
throw runtime_error("无效的机器人状态，无法进行速度控制");
|
||
}
|
||
} catch (exception &e) {
|
||
rsm_.store(ROBOT_ESTOP); // 出错时，设置为紧急停止状态
|
||
LOG(ERROR) << "speedJ 控制失败: " << e.what();
|
||
throw runtime_error("speedJ 控制失败: " + string(e.what()));
|
||
}
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) {
|
||
if (vel <= 0 || acc <= 0) {
|
||
throw std::runtime_error("speedL: vel and acc must be positive");
|
||
}
|
||
|
||
try {
|
||
const double CONTROL_PERIOD = 1.0 / 100; // 控制周期保持不变
|
||
const size_t MAX_QUEUE_SIZE = 10; // 队列最大缓存的轨迹点数量，防止内存溢出
|
||
|
||
// 定义基座和末端执行器链接
|
||
std::string base_link = "PELVIS_S";
|
||
std::string ee_link = toolFrame_;
|
||
|
||
msgs::Pose3d current_pose = fk(base_link, ee_link);
|
||
|
||
// 初始化当前位姿矩阵
|
||
Eigen::Matrix4d T_current = Eigen::Matrix4d::Identity();
|
||
T_current.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(
|
||
current_pose.euler().rx(), current_pose.euler().ry(), current_pose.euler().rz()
|
||
);
|
||
T_current(0, 3) = current_pose.position().x();
|
||
T_current(1, 3) = current_pose.position().y();
|
||
T_current(2, 3) = current_pose.position().z();
|
||
|
||
// 获取目标笛卡尔速度方向
|
||
Eigen::Vector3d direction = Eigen::Vector3d::Zero();
|
||
bool is_rotation = false;
|
||
|
||
// 根据方向设置笛卡尔速度
|
||
switch (dir) {
|
||
case RobotJointIndexDirection::X_POSITIVE:
|
||
direction.x() = 1.0;
|
||
break;
|
||
case RobotJointIndexDirection::X_NEGATIVE:
|
||
direction.x() = -1.0;
|
||
break;
|
||
case RobotJointIndexDirection::Y_POSITIVE:
|
||
direction.y() = 1.0;
|
||
break;
|
||
case RobotJointIndexDirection::Y_NEGATIVE:
|
||
direction.y() = -1.0;
|
||
break;
|
||
case RobotJointIndexDirection::Z_POSITIVE:
|
||
direction.z() = 1.0;
|
||
break;
|
||
case RobotJointIndexDirection::Z_NEGATIVE:
|
||
direction.z() = -1.0;
|
||
break;
|
||
case RobotJointIndexDirection::ROTATE_X:
|
||
direction.x() = 1.0;
|
||
is_rotation = true;
|
||
break;
|
||
case RobotJointIndexDirection::ROTATE_Y:
|
||
direction.y() = 1.0;
|
||
is_rotation = true;
|
||
break;
|
||
case RobotJointIndexDirection::ROTATE_Z:
|
||
direction.z() = 1.0;
|
||
is_rotation = true;
|
||
break;
|
||
case RobotJointIndexDirection::FORWARD:
|
||
if (cart == RobotCartesian::X) direction.x() = 1.0;
|
||
else if (cart == RobotCartesian::Y) direction.y() = 1.0;
|
||
else if (cart == RobotCartesian::Z) direction.z() = 1.0;
|
||
break;
|
||
case RobotJointIndexDirection::BACKWARD:
|
||
if (cart == RobotCartesian::X) direction.x() = -1.0;
|
||
else if (cart == RobotCartesian::Y) direction.y() = -1.0;
|
||
else if (cart == RobotCartesian::Z) direction.z() = -1.0;
|
||
break;
|
||
default:
|
||
throw std::runtime_error("speedL: unknown direction");
|
||
}
|
||
|
||
// 计算末端执行器的总运动时间和轨迹点数量
|
||
double move_time = calculateMoveTime(direction.norm(), vel, acc);
|
||
size_t num_points = std::max(2ul, static_cast<size_t>(ceil(move_time / CONTROL_PERIOD)));
|
||
|
||
// 生成S曲线速度规划的时间点和距离比例（主线程预计算）
|
||
std::vector<double> time_points;
|
||
std::vector<double> distance_ratios;
|
||
generateSTrapezoidalProfile(direction.norm(), vel, acc, move_time, num_points,
|
||
time_points, distance_ratios);
|
||
|
||
LOG(INFO) << "speedL: Planning trajectory - points=" << num_points
|
||
<< ", total distance=" << direction.norm() << "m, move time=" << move_time << "s";
|
||
|
||
// 创建轨迹队列及同步机制
|
||
std::queue<std::pair<Eigen::Vector<double, DOF>, Eigen::Vector<double, DOF>>> trajectory_queue;
|
||
std::mutex queue_mutex;
|
||
std::condition_variable queue_cv;
|
||
std::atomic<bool> planning_completed{false}; // 规划是否完成
|
||
std::atomic<bool> execution_failed{false}; // 执行是否失败
|
||
std::atomic<size_t> planned_points{0}; // 已规划的点数
|
||
std::atomic<size_t> executed_points{0}; // 已执行的点数
|
||
|
||
// 获取当前关节位置（初始点）
|
||
auto q_map_current = getJointQ();
|
||
Eigen::Vector<double, DOF> q_current;
|
||
for (int i = 0; i < DOF; ++i) {
|
||
q_current[i] = q_map_current[joint_names_[i]];
|
||
}
|
||
|
||
// 先将初始点加入队列
|
||
{
|
||
std::lock_guard<std::mutex> lock(queue_mutex);
|
||
trajectory_queue.push({q_current, Eigen::Vector<double, DOF>::Zero()});
|
||
planned_points.store(planned_points.load() + 1); // 使用store和load操作原子变量
|
||
}
|
||
|
||
// 启动控制执行子线程（先启动子线程）
|
||
std::thread control_thread([&]() {
|
||
try {
|
||
auto start_time = std::chrono::high_resolution_clock::now();
|
||
LOG(INFO) << "控制执行线程已启动";
|
||
|
||
// 循环条件使用load()读取原子变量
|
||
while (!planning_completed.load() || !trajectory_queue.empty() && !execution_failed.load()) {
|
||
// 从队列中获取轨迹点
|
||
std::pair<Eigen::Vector<double, DOF>, Eigen::Vector<double, DOF>> point;
|
||
bool has_point = false;
|
||
|
||
{
|
||
std::unique_lock<std::mutex> lock(queue_mutex);
|
||
// 等待队列中有数据或规划完成，使用load()读取原子变量
|
||
if (queue_cv.wait_for(lock, std::chrono::milliseconds(500),
|
||
[&] { return !trajectory_queue.empty() || planning_completed.load() || execution_failed.load(); })) {
|
||
|
||
if (!trajectory_queue.empty()) {
|
||
point = trajectory_queue.front();
|
||
trajectory_queue.pop();
|
||
has_point = true;
|
||
executed_points.store(executed_points.load() + 1); // 使用store和load操作原子变量
|
||
}
|
||
} else {
|
||
// 超时，可能规划线程出现问题
|
||
LOG(WARNING) << "控制线程等待轨迹点超时";
|
||
execution_failed.store(true); // 使用store设置原子变量
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (has_point) {
|
||
// 计算当前点的期望执行时间，确保按时间规划执行
|
||
auto current_time = std::chrono::high_resolution_clock::now();
|
||
std::chrono::duration<double> elapsed = current_time - start_time;
|
||
double expected_time = (executed_points.load() - 1) * CONTROL_PERIOD; // 使用load读取原子变量
|
||
|
||
// 如果执行过快，等待到期望时间
|
||
if (elapsed.count() < expected_time) {
|
||
std::this_thread::sleep_for(std::chrono::duration<double>(expected_time - elapsed.count()));
|
||
}
|
||
|
||
// 更新关节命令
|
||
m_state_->SetQ(point.first);
|
||
m_robot_->ComputeForwardKinematics(m_state_);
|
||
|
||
// 发送关节命令
|
||
for (int j = 0; j < DOF; ++j) {
|
||
auto motor = motor_manager_->getMotor(joint_names_[j]);
|
||
if (motor != nullptr) {
|
||
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
|
||
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
|
||
}
|
||
motor->setQd(point.second[j]);
|
||
motor->setQ(point.first[j]);
|
||
|
||
LOG(INFO) << "执行点 " << executed_points.load() // 使用load读取原子变量
|
||
<< ": joint[" << joint_names_[j] << "] = "
|
||
<< point.first[j];
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
if (execution_failed.load()) { // 使用load读取原子变量
|
||
LOG(ERROR) << "控制执行线程异常退出";
|
||
rsm_.store(ROBOT_ERROR);
|
||
} else {
|
||
LOG(INFO) << "控制执行线程完成，共执行 " << executed_points.load() // 使用load读取原子变量
|
||
<< " 个轨迹点";
|
||
rsm_.store(ROBOT_READY);
|
||
}
|
||
} catch (const std::exception &e) {
|
||
LOG(ERROR) << "控制线程错误: " << e.what();
|
||
execution_failed.store(true); // 使用store设置原子变量
|
||
rsm_.store(ROBOT_ERROR);
|
||
}
|
||
});
|
||
|
||
// 主线程开始进行IK逆解和轨迹点规划（边规划边放入队列）
|
||
try {
|
||
Eigen::Vector<double, DOF> prev_q = q_current; // 上一个关节位置
|
||
double prev_time = 0.0;
|
||
|
||
// 生成并规划轨迹点（从1开始，因为0已经作为初始点）
|
||
for (size_t i = 1; i <= num_points; ++i) {
|
||
// 检查执行线程是否失败，如果失败则停止规划，使用load读取原子变量
|
||
if (execution_failed.load()) {
|
||
LOG(WARNING) << "执行线程失败，停止轨迹规划";
|
||
break;
|
||
}
|
||
|
||
// 生成笛卡尔空间轨迹点
|
||
double s = distance_ratios[i];
|
||
Eigen::Matrix4d T_interp = Eigen::Matrix4d::Identity();
|
||
|
||
if (is_rotation) {
|
||
// 旋转运动
|
||
T_interp.block<3, 1>(0, 3) = T_current.block<3, 1>(0, 3);
|
||
double angle = s * direction.norm();
|
||
Eigen::AngleAxisd rotation(angle, direction.normalized());
|
||
Eigen::Matrix3d R_current = T_current.block<3, 3>(0, 0);
|
||
Eigen::Matrix3d R_interp = rotation * R_current;
|
||
T_interp.block<3, 3>(0, 0) = R_interp;
|
||
} else {
|
||
// 平移运动
|
||
T_interp.block<3, 3>(0, 0) = T_current.block<3, 3>(0, 0);
|
||
T_interp(0, 3) = T_current(0, 3) + s * direction.x();
|
||
T_interp(1, 3) = T_current(1, 3) + s * direction.y();
|
||
T_interp(2, 3) = T_current(2, 3) + s * direction.z();
|
||
}
|
||
|
||
// IK逆解计算
|
||
cmvr::ctrl::PoseTarget current_target;
|
||
current_target.T_target = T_interp;
|
||
current_target.link_name = ee_link;
|
||
current_target.w_posrot = 0.5;
|
||
current_target.weight = 1.0;
|
||
|
||
Eigen::Vector<double, DOF> q_next;
|
||
bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD,
|
||
ctrl::CartesianController<DOF>::Mode::Position,
|
||
q_next, 10000, 1e-6);
|
||
|
||
if (!ok) {
|
||
LOG(WARNING) << "轨迹点 " << i << " IK求解失败，停止规划";
|
||
execution_failed.store(true); // 使用store设置原子变量
|
||
break;
|
||
}
|
||
|
||
// 计算时间差和关节速度
|
||
double dt = time_points[i] - prev_time;
|
||
Eigen::Vector<double, DOF> q_vel;
|
||
if (dt > 0) {
|
||
q_vel = (q_next - prev_q) / dt;
|
||
} else {
|
||
q_vel = Eigen::Vector<double, DOF>::Zero();
|
||
}
|
||
|
||
// 将计算好的轨迹点放入队列，如果队列满了则等待
|
||
{
|
||
std::unique_lock<std::mutex> lock(queue_mutex);
|
||
// 等待队列有空间，使用load读取原子变量
|
||
queue_cv.wait(lock, [&] {
|
||
return trajectory_queue.size() < MAX_QUEUE_SIZE || execution_failed.load();
|
||
});
|
||
|
||
if (execution_failed.load()) { // 使用load读取原子变量
|
||
break;
|
||
}
|
||
|
||
trajectory_queue.push({q_next, q_vel});
|
||
planned_points.store(planned_points.load() + 1); // 使用store和load操作原子变量
|
||
prev_q = q_next;
|
||
prev_time = time_points[i];
|
||
|
||
LOG(INFO) << "规划点 " << i << " 已加入队列，当前队列大小: " << trajectory_queue.size();
|
||
}
|
||
queue_cv.notify_one(); // 通知控制线程有新数据
|
||
|
||
// 简单的速率控制，避免规划过快，使用load读取原子变量
|
||
if (planned_points.load() - executed_points.load() > MAX_QUEUE_SIZE / 2) {
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||
}
|
||
}
|
||
} catch (const std::exception &e) {
|
||
LOG(ERROR) << "轨迹规划错误: " << e.what();
|
||
execution_failed.store(true); // 使用store设置原子变量
|
||
}
|
||
|
||
// 规划完成，通知控制线程，使用store设置原子变量
|
||
planning_completed.store(true);
|
||
queue_cv.notify_one();
|
||
LOG(INFO) << "轨迹规划完成，共规划 " << planned_points.load() // 使用load读取原子变量
|
||
<< " 个轨迹点";
|
||
|
||
// 等待控制线程完成
|
||
if (control_thread.joinable()) {
|
||
control_thread.join();
|
||
}
|
||
|
||
if (execution_failed.load()) { // 使用load读取原子变量
|
||
LOG(ERROR) << "speedL执行失败";
|
||
rsm_.store(ROBOT_ERROR);
|
||
throw std::runtime_error("speedL execution failed");
|
||
} else {
|
||
LOG(INFO) << "speedL轨迹执行成功完成";
|
||
}
|
||
|
||
} catch (const std::exception &e) {
|
||
LOG(ERROR) << "speedL失败: " << e.what();
|
||
rsm_.store(ROBOT_ERROR);
|
||
throw std::runtime_error(std::string("speedL error: ") + e.what());
|
||
}
|
||
}
|
||
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::followJointTrajectory(std::vector<std::vector<JointPoint> > &traj, double dt) {
|
||
try {
|
||
// 1. 状态机检查：仅允许在 ESTOP/READY/TOROFF 状态启动
|
||
if (rsm_.load() != ROBOT_ESTOP && rsm_.load() != ROBOT_READY && rsm_.load() != ROBOT_TOROFF) {
|
||
throw runtime_error("followJointTrajectory: invalid robot state (" + std::to_string(rsm_.load()) + ")");
|
||
}
|
||
|
||
// 2. 轨迹合法性检查
|
||
if (!check_joint_traj_(traj, dt)) {
|
||
throw runtime_error("followJointTrajectory: invalid trajectory");
|
||
}
|
||
|
||
// 3. 初始化：切换电机模式为CSP，清空缓冲，更新状态机
|
||
std::lock_guard<std::mutex> exec_lock(exec_mtx_); // 防止多线程指令冲突
|
||
CSP_buffer_->clear(); // 清空CSP模式缓冲
|
||
rsm_.store(ROBOT_RUNNING);
|
||
|
||
// 3.1 预配置所有电机为CSP模式（避免轨迹执行中切换模式导致延迟）
|
||
for (const auto& motor_pair : motor_manager_->motorsMap()) {
|
||
auto motor = motor_pair.second;
|
||
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
|
||
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
|
||
LOG(INFO) << "Motor " << motor->jointName() << " switched to CSP mode";
|
||
}
|
||
}
|
||
|
||
// 4. 轨迹执行：使用定时器按dt间隔发送轨迹点
|
||
std::shared_ptr<FDTimer> traj_timer = std::make_shared<FDTimer>();
|
||
std::atomic<size_t> waypoint_idx(0); // 当前执行的轨迹点索引（原子变量防线程竞争）
|
||
std::atomic<bool> traj_completed(false); // 轨迹是否完成
|
||
|
||
// 4.1 定时器回调：发送当前轨迹点
|
||
traj_timer->start(
|
||
std::chrono::nanoseconds(static_cast<long long>(dt * 1e9)), // dt转换为纳秒
|
||
[this, &traj, &waypoint_idx, &traj_completed, traj_timer]() {
|
||
// 检查轨迹中断（外部指令触发）
|
||
if (flash_cmd_.load()) {
|
||
flash_cmd_.store(false);
|
||
traj_timer->stop();
|
||
traj_completed.store(true);
|
||
rsm_.store(ROBOT_ESTOP);
|
||
LOG(INFO) << "followJointTrajectory: interrupted by external command";
|
||
return;
|
||
}
|
||
|
||
// 检查轨迹是否完成
|
||
size_t current_idx = waypoint_idx.load();
|
||
if (current_idx >= traj.size()) {
|
||
traj_timer->stop();
|
||
traj_completed.store(true);
|
||
rsm_.store(ROBOT_ESTOP);
|
||
LOG(INFO) << "followJointTrajectory: trajectory completed";
|
||
return;
|
||
}
|
||
|
||
// 4.2 发送当前轨迹点的关节指令
|
||
const auto& current_waypoint = traj[current_idx];
|
||
for (const auto& joint : current_waypoint) {
|
||
auto motor = motor_manager_->getMotor(joint.joint_name);
|
||
if (motor) {
|
||
// 优先使用轨迹点中的速度，若无则用默认速度（0.5 rad/s）
|
||
double target_vel = (joint.vel > 0) ? joint.vel : 0.5;
|
||
motor->setQd(target_vel); // 设置关节速度
|
||
motor->setQ(joint.rad); // 设置关节目标位置
|
||
LOG(INFO)<< "Joint " << joint.joint_name
|
||
<< " -> pos=" << joint.rad << " rad, vel=" << target_vel << " rad/s";
|
||
}
|
||
}
|
||
|
||
// 4.3 推进轨迹点索引
|
||
waypoint_idx.fetch_add(1);
|
||
}
|
||
);
|
||
|
||
// 4.4 等待轨迹完成或中断
|
||
while (!traj_completed.load()) {
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(1)); // 降低CPU占用
|
||
}
|
||
|
||
} catch (std::exception &e) {
|
||
// 异常处理：停止轨迹，重置状态机
|
||
rsm_.store(ROBOT_ESTOP);
|
||
LOG(ERROR) << "followJointTrajectory failed: " << e.what();
|
||
throw runtime_error("followJointTrajectory error: " + std::string(e.what()));
|
||
}
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link,
|
||
std::vector<std::vector<cmvr::ctrl::PoseTarget> > &targets, double dt) {
|
||
try {
|
||
// 1. 基础校验：状态机与轨迹合法性
|
||
if (rsm_.load() == ROBOT_RUNNING) {
|
||
flash_cmd_.store(true);
|
||
eStop(); // 中断当前运动
|
||
throw runtime_error("followPoseTrajectory: robot is running, interrupted");
|
||
}
|
||
if (rsm_.load() != ROBOT_ESTOP && rsm_.load() != ROBOT_READY) {
|
||
throw runtime_error("followPoseTrajectory: invalid robot state (" + std::to_string(rsm_.load()) + ")");
|
||
}
|
||
if (targets.empty()) {
|
||
throw runtime_error("followPoseTrajectory: pose trajectory is empty");
|
||
}
|
||
if (dt <= 0 || dt > 0.1) {
|
||
throw runtime_error("followPoseTrajectory: invalid dt=" + std::to_string(dt) + " (must be 0 < dt ≤ 0.1)");
|
||
}
|
||
// // 检查基座链接有效性（依赖机器人模型接口）
|
||
// int base_link_idx = m_robot_->GetLinkIdx(base_link);
|
||
// if (base_link_idx == -1) {
|
||
// throw runtime_error("followPoseTrajectory: invalid base link: " + base_link);
|
||
// }
|
||
|
||
|
||
// 2. 关键步骤1：获取当前关节状态，解算“轨迹第一个点”的关节配置（作为后续IK基准）
|
||
std::lock_guard<std::mutex> exec_lock(exec_mtx_);
|
||
CSP_buffer_->clear(); // 清空CSP缓冲，避免指令冲突
|
||
|
||
// 2.1 获取当前关节角度（初始化机器人状态）
|
||
Eigen::Vector<double, DOF> q_current;
|
||
auto q_map_current = getJointQ();
|
||
q_current << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], q_map_current["L_ELBOW_R"],
|
||
q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"],
|
||
q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], q_map_current["R_ELBOW_R"],
|
||
q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"];
|
||
m_state_->SetQ(q_current);
|
||
m_robot_->ComputeForwardKinematics(m_state_); // 更新当前正运动学状态
|
||
|
||
|
||
// 2.2 解算“轨迹第一个点”的关节配置（q_first，作为后续所有IK的初始值）
|
||
const auto& first_pose_targets = targets[0]; // 轨迹第一个点的位姿目标
|
||
Eigen::Vector<double, DOF> q_first; // 轨迹第一个点的关节配置（IK基准）
|
||
bool ik_first_ok = m_cctrl_->compute(
|
||
m_state_, // 当前机器人状态（作为IK初始值）
|
||
base_link, // 基座链接
|
||
first_pose_targets, // 第一个点的位姿目标
|
||
dt, // 控制周期（用于速度限制）
|
||
ctrl::CartesianController<DOF>::Mode::Position, // 位置控制模式
|
||
q_first, // 输出：第一个点的关节配置
|
||
10000, // IK最大迭代次数（确保精度）
|
||
1e-6 // IK位置精度（1mm/0.001°）
|
||
);
|
||
if (!ik_first_ok) {
|
||
throw runtime_error("followPoseTrajectory: IK failed for the FIRST waypoint (unreachable target)");
|
||
}
|
||
LOG(INFO) << "followPoseTrajectory: first waypoint IK solved successfully, q_first=" << q_first.transpose();
|
||
|
||
|
||
// 3. 关键步骤2：从当前位置移动到“轨迹第一个点”（过渡运动）
|
||
rsm_.store(ROBOT_RUNNING); // 切换状态为运行中
|
||
LOG(INFO) << "followPoseTrajectory: moving from current position to first waypoint...";
|
||
|
||
// 3.1 配置电机为CSP模式（用于过渡运动和后续轨迹）
|
||
for (const auto& motor_pair : motor_manager_->motorsMap()) {
|
||
auto motor = motor_pair.second;
|
||
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
|
||
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
|
||
LOG(INFO) << "followPoseTrajectory: motor " << motor->jointName() << " switched to CSP mode";
|
||
}
|
||
}
|
||
|
||
// 3.2 执行“当前→第一个点”的过渡运动（匀速逼近，避免冲击）
|
||
const double TRANSITION_VEL = 0.5; // 过渡运动速度（1rad/s，可根据需求调整）
|
||
bool transition_completed = false;
|
||
auto transition_start_time = std::chrono::high_resolution_clock::now();
|
||
|
||
while (!transition_completed && !flash_cmd_.load()) {
|
||
// 3.2.1 计算当前应到达的关节位置（匀速插值）
|
||
auto now = std::chrono::high_resolution_clock::now();
|
||
double elapsed = std::chrono::duration<double>(now - transition_start_time).count();
|
||
Eigen::Vector<double, DOF> q_transition = q_current + (q_first - q_current) * std::min(elapsed * TRANSITION_VEL / (q_first - q_current).norm(), 1.0);
|
||
|
||
// 3.2.2 发送过渡运动关节指令
|
||
for (size_t i = 0; i < DOF; ++i) {
|
||
const std::string& joint_name = joint_names_[i];
|
||
auto motor = motor_manager_->getMotor(joint_name);
|
||
if (motor) {
|
||
motor->setQd(TRANSITION_VEL); // 过渡运动速度
|
||
motor->setQ(q_transition[i]); // 当前过渡位置
|
||
}
|
||
}
|
||
|
||
// 3.2.3 检查过渡运动是否完成（所有关节到达目标）
|
||
transition_completed = true;
|
||
for (size_t i = 0; i < DOF; ++i) {
|
||
const std::string& joint_name = joint_names_[i];
|
||
auto motor = motor_manager_->getMotor(joint_name);
|
||
if (motor && !motor->reachedTargetQ()) { // 精度阈值：0.0001rad（≈0.0057°）
|
||
transition_completed = false;
|
||
break;
|
||
}
|
||
}
|
||
|
||
// 3.2.4 控制过渡运动频率（与后续轨迹一致）
|
||
std::this_thread::sleep_for(std::chrono::nanoseconds(static_cast<long long>(dt * 1e9)));
|
||
}
|
||
|
||
// 3.2.5 过渡运动中断处理
|
||
if (flash_cmd_.load()) {
|
||
flash_cmd_.store(false);
|
||
rsm_.store(ROBOT_ESTOP);
|
||
throw runtime_error("followPoseTrajectory: transition to first waypoint interrupted");
|
||
}
|
||
LOG(INFO) << "followPoseTrajectory: reached first waypoint, start trajectory execution";
|
||
|
||
|
||
// 4. 关键步骤3：执行轨迹（所有点的IK均以q_first为初始值）
|
||
std::shared_ptr<FDTimer> traj_timer = std::make_shared<FDTimer>();
|
||
std::atomic<size_t> waypoint_idx(0); // 当前执行的轨迹点索引（从0开始，即第一个点）
|
||
std::atomic<bool> traj_completed(false); // 轨迹是否完成
|
||
Eigen::Vector<double, DOF> last_valid_q = q_first; // 上一次有效的关节配置（容错用）
|
||
|
||
// 4.1 初始化机器人状态为第一个点（确保轨迹起始状态正确）
|
||
m_state_->SetQ(q_first);
|
||
m_robot_->ComputeForwardKinematics(m_state_);
|
||
|
||
// 4.2 定时器回调：按dt间隔解算IK并发送指令（IK初始值固定为q_first）
|
||
traj_timer->start(
|
||
std::chrono::nanoseconds(static_cast<long long>(dt * 1e9)), // 定时器周期=控制周期dt
|
||
[this, &base_link, &targets, &waypoint_idx, &traj_completed, &last_valid_q, &q_first, traj_timer, dt]() {
|
||
// 4.2.1 检查外部中断
|
||
if (flash_cmd_.load()) {
|
||
flash_cmd_.store(false);
|
||
traj_timer->stop();
|
||
traj_completed.store(true);
|
||
rsm_.store(ROBOT_ESTOP);
|
||
LOG(INFO) << "followPoseTrajectory: trajectory interrupted by external command";
|
||
return;
|
||
}
|
||
|
||
// 4.2.2 检查轨迹是否完成
|
||
size_t current_idx = waypoint_idx.load();
|
||
if (current_idx >= targets.size()) {
|
||
traj_timer->stop();
|
||
traj_completed.store(true);
|
||
rsm_.store(ROBOT_ESTOP);
|
||
LOG(INFO) << "followPoseTrajectory: trajectory executed completely";
|
||
return;
|
||
}
|
||
|
||
// 4.2.3 解算当前轨迹点的IK（关键：初始值固定为q_first）
|
||
const auto& current_pose_targets = targets[current_idx];
|
||
Eigen::Vector<double, DOF> q_cmd; // 当前点的关节目标
|
||
|
||
// 临时更新机器人状态为q_first（确保IK初始值固定）
|
||
m_state_->SetQ(q_first);
|
||
m_robot_->ComputeForwardKinematics(m_state_);
|
||
|
||
bool ik_ok = m_cctrl_->compute(
|
||
m_state_, // IK初始值：固定为q_first
|
||
base_link, // 基座链接
|
||
current_pose_targets, // 当前点的位姿目标
|
||
dt, // 控制周期
|
||
ctrl::CartesianController<DOF>::Mode::Position,
|
||
q_cmd, // 输出：当前点的关节配置
|
||
5000, // 减少迭代次数（平衡精度与速度）
|
||
5e-4 // IK精度：0.5mm/0.028°（轨迹执行可适当放宽）
|
||
);
|
||
|
||
// 4.2.4 IK容错：失败时使用上一次有效配置
|
||
if (!ik_ok) {
|
||
LOG(WARNING) << "followPoseTrajectory: IK failed at waypoint " << current_idx
|
||
<< ", use last valid config (q_last_valid=" << last_valid_q.transpose() << ")";
|
||
q_cmd = last_valid_q;
|
||
} else {
|
||
last_valid_q = q_cmd; // 更新有效配置
|
||
}
|
||
|
||
// 4.2.5 发送当前点的关节指令（固定速度，可根据需求调整）
|
||
const double TRAJ_VEL = 1.0; // 轨迹执行速度（1rad/s）
|
||
for (size_t i = 0; i < DOF; ++i) {
|
||
const std::string& joint_name = joint_names_[i];
|
||
auto motor = motor_manager_->getMotor(joint_name);
|
||
if (motor) {
|
||
motor->setQd(TRAJ_VEL); // 轨迹执行速度
|
||
motor->setQ(q_cmd[i]); // 关节目标位置
|
||
LOG(INFO) << "followPoseTrajectory: waypoint " << current_idx
|
||
<< ", joint " << joint_name << " -> pos=" << q_cmd[i] << " rad";
|
||
}
|
||
}
|
||
|
||
// 4.2.6 推进轨迹点索引
|
||
waypoint_idx.fetch_add(1);
|
||
}
|
||
);
|
||
|
||
// 4.3 等待轨迹执行完成
|
||
while (!traj_completed.load()) {
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(1)); // 降低CPU占用
|
||
}
|
||
|
||
} catch (std::exception &e) {
|
||
// 异常处理：重置状态机，确保机器人安全
|
||
rsm_.store(ROBOT_ESTOP);
|
||
LOG(ERROR) << "followPoseTrajectory failed: " << e.what();
|
||
throw runtime_error("followPoseTrajectory error: " + std::string(e.what()));
|
||
}
|
||
}
|
||
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double dt) {
|
||
for (const auto &j: joints) {
|
||
auto motor = motor_manager_->getMotor(j.joint_name);
|
||
if (motor != nullptr) {
|
||
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
|
||
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
|
||
}
|
||
motor->setQd(j.vel);
|
||
motor->setQ(j.rad);
|
||
}
|
||
}
|
||
rsm_.store(ROBOT_READY);
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double vel, double dt) {
|
||
for (const auto &j: joints) {
|
||
auto motor = motor_manager_->getMotor(j.joint_name);
|
||
if (motor != nullptr) {
|
||
|
||
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
|
||
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
|
||
}
|
||
motor->setQd(vel);
|
||
motor->setQ(j.rad);
|
||
}
|
||
}
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel,
|
||
double acc) {
|
||
try {
|
||
// update m_state_
|
||
Eigen::Vector<double, DOF> q_init;
|
||
auto q_map = getJointQ();
|
||
q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"],
|
||
q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"],
|
||
q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"],
|
||
q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"];
|
||
|
||
LOG(INFO) << "q_init: " << q_init;
|
||
m_state_->SetQ(q_init);
|
||
m_robot_->ComputeForwardKinematics(m_state_);
|
||
|
||
|
||
Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity();
|
||
T_target.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz());
|
||
// 输入为弧度
|
||
T_target(0, 3) = pose.position().x();
|
||
T_target(1, 3) = pose.position().y();
|
||
T_target(2, 3) = pose.position().z();
|
||
|
||
cmvr::ctrl::PoseTarget target;
|
||
target.T_target = T_target;
|
||
target.w_posrot = 0.5;
|
||
target.weight = 1.0;
|
||
target.link_name = ee_link;
|
||
|
||
// slove ik
|
||
Eigen::Vector<double, DOF> q_cmd;
|
||
bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002,
|
||
ctrl::CartesianController<DOF>::Mode::Position,
|
||
q_cmd, 10000, 1e-6);
|
||
if (!ok) {
|
||
throw runtime_error("solve IK failed");
|
||
}
|
||
std::vector<JointPoint> joint_points{
|
||
{"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]},
|
||
{"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]},
|
||
{"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]},
|
||
{"R_WRIST_R", q_cmd[13]}
|
||
};
|
||
servoJ(joint_points, vel, 0.1);
|
||
} catch (exception &e) {
|
||
throw runtime_error(e.what());
|
||
}
|
||
}
|
||
|
||
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) {
|
||
try {
|
||
// 1 : 计算当前位姿
|
||
auto cur_pose = fk(base_link, ee_link);
|
||
|
||
// 2 : 计算目标角度 target pos = cur_pose + delta_pose
|
||
cmvr::msgs::Pose3d target_pose;
|
||
target_pose.mutable_position()->set_x(cur_pose.position().x() + delta_pose.position().x());
|
||
target_pose.mutable_position()->set_y(cur_pose.position().y() + delta_pose.position().y());
|
||
target_pose.mutable_position()->set_z(cur_pose.position().z() + delta_pose.position().z());
|
||
|
||
target_pose.mutable_euler()->set_rx(cur_pose.euler().rx() + delta_pose.euler().rx());
|
||
target_pose.mutable_euler()->set_ry(cur_pose.euler().ry() + delta_pose.euler().ry());
|
||
target_pose.mutable_euler()->set_rz(cur_pose.euler().rz() + delta_pose.euler().rz());
|
||
|
||
//3 :
|
||
servoJ(base_link, ee_link, target_pose, vel, acc);
|
||
} catch (exception &e) {
|
||
throw runtime_error(e.what());
|
||
}
|
||
}
|
||
|
||
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) {
|
||
try {
|
||
Eigen::Vector<double, DOF> q_cmd;
|
||
bool ok = m_cctrl_->compute(m_state_, base_link, targets, 1, ctrl::CartesianController<DOF>::Mode::Position,
|
||
q_cmd, 60, 1e-4);
|
||
if (!ok) {
|
||
LOG(WARNING) << "[HumanoidRobot] (servoL): solve IK failed, id=" << id_;
|
||
throw runtime_error("IK failed");
|
||
}
|
||
std::vector<JointPoint> joints(dof_);
|
||
for (size_t i = 0; i < dof_; i++) {
|
||
joints[i].joint_name = joint_names_[i];
|
||
joints[i].rad = q_cmd[i];
|
||
}
|
||
servoJ(joints, dt);
|
||
} catch (exception &e) {
|
||
throw runtime_error(e.what());
|
||
}
|
||
}
|
||
|
||
template<int DOF>
|
||
bool HumanoidRobot<DOF>::check_joint_traj_(std::vector<std::vector<JointPoint> > &traj, double dt) {
|
||
// TODO: to be implemented
|
||
return true;
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,
|
||
double vel, double acc) {
|
||
try {
|
||
// 1 : 计算当前位姿
|
||
auto cur_pose = fk(base_link, ee_link);
|
||
|
||
// 2 : 计算目标角度 target pos = cur_pose + delta_pose
|
||
cmvr::msgs::Pose3d target_pose;
|
||
target_pose.mutable_position()->set_x(cur_pose.position().x() + delta_pose.position().x());
|
||
target_pose.mutable_position()->set_y(cur_pose.position().y() + delta_pose.position().y());
|
||
target_pose.mutable_position()->set_z(cur_pose.position().z() + delta_pose.position().z());
|
||
|
||
target_pose.mutable_euler()->set_rx(cur_pose.euler().rx() + delta_pose.euler().rx());
|
||
target_pose.mutable_euler()->set_ry(cur_pose.euler().ry() + delta_pose.euler().ry());
|
||
target_pose.mutable_euler()->set_rz(cur_pose.euler().rz() + delta_pose.euler().rz());
|
||
|
||
//3 :
|
||
moveJ(base_link, ee_link, target_pose, vel, acc);
|
||
} catch (exception &e) {
|
||
throw runtime_error(e.what());
|
||
}
|
||
}
|
||
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::update_state_() {
|
||
std::lock_guard lock(state_mtx_);
|
||
// TODO: set m_state_
|
||
// m_state_->SetQ();
|
||
// m_state_->SetQdot();
|
||
// m_state_->SetQddot();
|
||
}
|
||
|
||
template<int DOF>
|
||
Eigen::Matrix3d HumanoidRobot<DOF>::eulerZYXToRotationMatrix(double rx, double ry, double rz) {
|
||
Eigen::Matrix3d R_x;
|
||
R_x << 1, 0, 0,
|
||
0, cos(rx), -sin(rx),
|
||
0, sin(rx), cos(rx);
|
||
|
||
Eigen::Matrix3d R_y;
|
||
R_y << cos(ry), 0, sin(ry),
|
||
0, 1, 0,
|
||
-sin(ry), 0, cos(ry);
|
||
|
||
Eigen::Matrix3d R_z;
|
||
R_z << cos(rz), -sin(rz), 0,
|
||
sin(rz), cos(rz), 0,
|
||
0, 0, 1;
|
||
|
||
return R_x * R_y * R_z;
|
||
}
|
||
|
||
template<int DOF>
|
||
Eigen::Vector3d HumanoidRobot<DOF>::rotationMatrixToEulerZYX(const Eigen::Matrix3d &R) {
|
||
double rx, ry, rz;
|
||
|
||
// 根据 R = R_x * R_y * R_z
|
||
// R = | cy*cz -cy*sz sy |
|
||
// | sx*sy*cz + cx*sz -sx*sy*sz + cx*cz -sx*cy |
|
||
// | -cx*sy*cz + sx*sz cx*sy*sz + sx*cz cx*cy |
|
||
|
||
// 提取 ry（绕 Y 的角度）
|
||
ry = std::asin(R(0,2)); // R(0,2) = sin(ry)
|
||
|
||
double cy = std::cos(ry);
|
||
|
||
if (std::abs(cy) > 1e-6) {
|
||
// 正常情况
|
||
rx = std::atan2(-R(1,2), R(2,2));
|
||
rz = std::atan2(-R(0,1), R(0,0));
|
||
} else {
|
||
// 万向节锁：cy ≈ 0
|
||
rx = 0; // 任意选择
|
||
if (ry > 0) {
|
||
rz = std::atan2(R(1,0), R(1,1));
|
||
} else {
|
||
rz = std::atan2(-R(1,0), R(1,1));
|
||
}
|
||
}
|
||
|
||
return Eigen::Vector3d(rx, ry, rz);
|
||
}
|
||
|
||
|
||
template<int DOF>
|
||
cmvr::msgs::Pose3d HumanoidRobot<DOF>::fk(const std::string &base_link, const std::string &ee_link) {
|
||
cmvr::msgs::Pose3d pose;
|
||
|
||
try {
|
||
// 获取当前关节角度
|
||
Eigen::Vector<double, DOF> q;
|
||
auto q_map = getJointQ(); // 类似 moveJ 中获取关节角度
|
||
q << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"],
|
||
q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"],
|
||
q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"],
|
||
q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"];
|
||
|
||
// 更新状态并计算前向运动学
|
||
m_state_->SetQ(q);
|
||
m_robot_->ComputeForwardKinematics(m_state_);
|
||
|
||
// 获取基座和末端索引
|
||
auto base_idx = m_robot_->GetLinkIdx(base_link);
|
||
auto ee_idx = m_robot_->GetLinkIdx(ee_link);
|
||
|
||
// 获取变换矩阵
|
||
Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx);
|
||
|
||
// 填充 Pose3d
|
||
pose.mutable_position()->set_x(T(0,3));
|
||
pose.mutable_position()->set_y(T(1,3));
|
||
pose.mutable_position()->set_z(T(2,3));
|
||
|
||
// 将旋转矩阵转换为欧拉角
|
||
Eigen::Matrix3d R = T.block<3,3>(0,0);
|
||
Eigen::Vector3d euler = rotationMatrixToEulerZYX(R); // 你需要实现或已有此工具函数
|
||
pose.mutable_euler()->set_rx(euler(0));
|
||
pose.mutable_euler()->set_ry(euler(1));
|
||
pose.mutable_euler()->set_rz(euler(2));
|
||
|
||
} catch (const std::exception &e) {
|
||
throw std::runtime_error(std::string("FK计算失败: ") + e.what());
|
||
}
|
||
|
||
return pose;
|
||
}
|
||
|
||
|
||
void printTrajectoryInfo(
|
||
const std::vector<Eigen::Matrix4d>& trajectory,
|
||
const std::vector<double>& times,
|
||
const std::vector<double>& velocities,
|
||
double total_distance) {
|
||
|
||
std::cout << "\n===================================== 轨迹详细信息 =====================================" << std::endl;
|
||
std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl;
|
||
std::cout << "总运动时间: " << std::fixed << std::setprecision(3) << times.back() << "s" << std::endl;
|
||
std::cout << "轨迹点总数: " << trajectory.size() << " 个" << std::endl;
|
||
std::cout << "-----------------------------------------------------------------------------------------" << std::endl;
|
||
std::cout << std::setw(4) << "序号" << " | "
|
||
<< std::setw(8) << "时间(s)" << " | "
|
||
<< std::setw(10) << "x(m)" << " | "
|
||
<< std::setw(10) << "y(m)" << " | "
|
||
<< std::setw(10) << "z(m)" << " | "
|
||
<< std::setw(12) << "速度(m/s)" << " | "
|
||
<< std::setw(16) << "到起点距离(m)" << std::endl;
|
||
std::cout << "-----------------------------------------------------------------------------------------" << std::endl;
|
||
|
||
Eigen::Vector3d start_pos(trajectory[0](0,3), trajectory[0](1,3), trajectory[0](2,3));
|
||
for (size_t idx = 0; idx < trajectory.size(); ++idx) {
|
||
const auto& T = trajectory[idx];
|
||
Eigen::Vector3d pos(T(0,3), T(1,3), T(2,3));
|
||
double dist_from_start = (pos - start_pos).norm();
|
||
|
||
std::cout << std::setw(4) << idx << " | "
|
||
<< std::fixed << std::setprecision(3) << std::setw(8) << times[idx] << " | "
|
||
<< std::fixed << std::setprecision(6) << std::setw(10) << pos.x() << " | "
|
||
<< std::fixed << std::setprecision(6) << std::setw(10) << pos.y() << " | "
|
||
<< std::fixed << std::setprecision(6) << std::setw(10) << pos.z() << " | "
|
||
<< std::fixed << std::setprecision(6) << std::setw(12) << velocities[idx] << " | "
|
||
<< std::fixed << std::setprecision(6) << std::setw(16) << dist_from_start << std::endl;
|
||
}
|
||
std::cout << "=========================================================================================\n" << std::endl;
|
||
}
|
||
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::moveDeltaL(const std::string &base_link, const std::string &ee_link,
|
||
msgs::Pose3d delta_pose, double vel, double acc) {
|
||
try {
|
||
Eigen::Vector<double, DOF> q_current_for_ik;
|
||
auto q_map_current = getJointQ();
|
||
q_current_for_ik << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], q_map_current["L_ELBOW_R"],
|
||
q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"],
|
||
q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], q_map_current["R_ELBOW_R"],
|
||
q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"];
|
||
|
||
LOG(INFO) << "q_current_for_ik: " << q_current_for_ik;
|
||
|
||
// 1. 计算末端当前位姿（通过FK）
|
||
msgs::Pose3d current_pose = fk(base_link, ee_link);
|
||
|
||
LOG(INFO) << current_pose.mutable_position()->x() << " " << current_pose.mutable_position()->y() << " "
|
||
<< current_pose.mutable_position()->z() << " " << current_pose.mutable_euler()->rx() << " "
|
||
<< current_pose.mutable_euler()->ry() << " " << current_pose.mutable_euler()->rz();
|
||
|
||
// 2. 计算目标位姿 = 当前位姿 + 相对偏移（位置/姿态分别叠加）
|
||
msgs::Pose3d target_pose;
|
||
target_pose.mutable_position()->set_x(current_pose.position().x() + delta_pose.position().x());
|
||
target_pose.mutable_position()->set_y(current_pose.position().y() + delta_pose.position().y());
|
||
target_pose.mutable_position()->set_z(current_pose.position().z() + delta_pose.position().z());
|
||
target_pose.mutable_euler()->set_rx(current_pose.euler().rx() + delta_pose.euler().rx());
|
||
target_pose.mutable_euler()->set_ry(current_pose.euler().ry() + delta_pose.euler().ry());
|
||
target_pose.mutable_euler()->set_rz(current_pose.euler().rz() + delta_pose.euler().rz());
|
||
|
||
// 3. 调用moveL执行直线运动到目标位姿
|
||
moveL(base_link, ee_link, target_pose, vel, acc);
|
||
|
||
} catch (const std::exception &e) {
|
||
LOG(ERROR) << "moveDeltaL failed: " << e.what();
|
||
throw std::runtime_error(std::string("moveDeltaL error: ") + e.what());
|
||
}
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &ee_link,
|
||
msgs::Pose3d target_pose, double vel, double acc) {
|
||
if (vel <= 0 || acc <= 0) {
|
||
throw std::runtime_error("moveL: vel and acc must be positive");
|
||
}
|
||
|
||
try {
|
||
const double CONTROL_PERIOD = 1.0 / 50.0; // 控制周期保持不变
|
||
msgs::Pose3d current_pose = fk(base_link, ee_link);
|
||
|
||
// 1. 初始化当前和目标位姿矩阵
|
||
Eigen::Matrix4d T_current = Eigen::Matrix4d::Identity();
|
||
T_current.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(
|
||
current_pose.euler().rx(), current_pose.euler().ry(), current_pose.euler().rz()
|
||
);
|
||
T_current(0, 3) = current_pose.position().x();
|
||
T_current(1, 3) = current_pose.position().y();
|
||
T_current(2, 3) = current_pose.position().z();
|
||
|
||
Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity();
|
||
T_target.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(
|
||
target_pose.euler().rx(), target_pose.euler().ry(), target_pose.euler().rz()
|
||
);
|
||
T_target(0, 3) = target_pose.position().x();
|
||
T_target(1, 3) = target_pose.position().y();
|
||
T_target(2, 3) = target_pose.position().z();
|
||
|
||
// 保存起始姿态，确保整个运动过程中姿态保持不变
|
||
Eigen::Matrix3d start_orientation = T_current.block<3, 3>(0, 0);
|
||
|
||
// 2. 获取当前关节配置并验证目标可达性
|
||
Eigen::Vector<double, DOF> q_current;
|
||
auto q_map_current = getJointQ();
|
||
q_current << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], q_map_current["L_ELBOW_R"],
|
||
q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"],
|
||
q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], q_map_current["R_ELBOW_R"],
|
||
q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"];
|
||
|
||
LOG(INFO) << "Current joint configuration: " << q_current;
|
||
m_state_->SetQ(q_current);
|
||
m_robot_->ComputeForwardKinematics(m_state_);
|
||
|
||
// 验证目标点可达性 - 使用起始姿态，确保姿态不变
|
||
cmvr::ctrl::PoseTarget target_ik_check;
|
||
target_ik_check.T_target = T_target;
|
||
target_ik_check.T_target.block<3, 3>(0, 0) = start_orientation; // 使用起始姿态
|
||
target_ik_check.w_posrot = 0.5;
|
||
target_ik_check.weight = 1.0;
|
||
target_ik_check.link_name = ee_link;
|
||
|
||
Eigen::Vector<double, DOF> q_cmd_check;
|
||
bool ik_solvable = m_cctrl_->compute(m_state_, base_link, {target_ik_check}, 0.002,
|
||
ctrl::CartesianController<DOF>::Mode::Position,
|
||
q_cmd_check, 10000, 1e-6);
|
||
if (!ik_solvable) {
|
||
throw std::runtime_error("moveL: Target pose is unreachable with constant orientation");
|
||
}
|
||
|
||
// 3. 计算位置差值（保持姿态不变）
|
||
Eigen::Vector3d delta_pos = T_target.block<3, 1>(0, 3) - T_current.block<3, 1>(0, 3);
|
||
double total_distance = delta_pos.norm();
|
||
|
||
if (total_distance < 1e-6) {
|
||
LOG(INFO) << "moveL: Target is already reached";
|
||
return;
|
||
}
|
||
|
||
// 4. 基于S曲线速度规划的时间规划
|
||
// 计算总时间和插值点数
|
||
double move_time = calculateMoveTime(total_distance, vel, acc);
|
||
size_t num_points = std::max(2ul, static_cast<size_t>(ceil(move_time / CONTROL_PERIOD)));
|
||
|
||
// 生成时间轴和距离比例
|
||
std::vector<double> time_points;
|
||
std::vector<double> distance_ratios;
|
||
generateSTrapezoidalProfile(total_distance, vel, acc, move_time, num_points,
|
||
time_points, distance_ratios);
|
||
|
||
LOG(INFO) << "moveL: Planning trajectory - points=" << num_points
|
||
<< ", total distance=" << total_distance << "m, move time=" << move_time << "s";
|
||
|
||
// 5. 生成轨迹点（位置线性插值，姿态保持不变）
|
||
std::vector<Eigen::Matrix4d> cartesian_trajectory;
|
||
for (size_t i = 0; i <= num_points; ++i) {
|
||
double s = distance_ratios[i]; // 使用S曲线规划的距离比例
|
||
|
||
Eigen::Matrix4d T_interp = Eigen::Matrix4d::Identity();
|
||
T_interp.block<3, 3>(0, 0) = start_orientation; // 保持起始姿态不变
|
||
// 仅位置按比例插值
|
||
T_interp(0, 3) = T_current(0, 3) + s * delta_pos.x();
|
||
T_interp(1, 3) = T_current(1, 3) + s * delta_pos.y();
|
||
T_interp(2, 3) = T_current(2, 3) + s * delta_pos.z();
|
||
|
||
cartesian_trajectory.push_back(T_interp);
|
||
}
|
||
|
||
// 6. 预先计算所有轨迹点的关节位置
|
||
std::vector<Eigen::Vector<double, DOF>> joint_positions;
|
||
joint_positions.push_back(q_current); // 起始位置
|
||
|
||
// 预先计算所有关节位置
|
||
for (size_t i = 1; i < cartesian_trajectory.size(); ++i) {
|
||
const auto& T_interp = cartesian_trajectory[i];
|
||
|
||
// 构造当前目标
|
||
cmvr::ctrl::PoseTarget current_target;
|
||
current_target.T_target = T_interp;
|
||
current_target.link_name = ee_link;
|
||
current_target.w_posrot = 0.5;
|
||
current_target.weight = 1.0;
|
||
|
||
// 使用前一点的位置作为初始值求解IK
|
||
Eigen::Vector<double, DOF> q_next;
|
||
bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD,
|
||
ctrl::CartesianController<DOF>::Mode::Position,
|
||
q_next, 10000, 1e-6);
|
||
|
||
if (!ok) {
|
||
throw std::runtime_error("Pre-computation IK failed");
|
||
// LOG(WARNING) << "Pre-computation IK failed at point " << i << ", using previous point";
|
||
// q_next = joint_positions.back();
|
||
}
|
||
|
||
joint_positions.push_back(q_next);
|
||
}
|
||
|
||
// 7. 计算每个点的关节速度
|
||
std::vector<Eigen::Vector<double, DOF>> joint_velocities;
|
||
joint_velocities.push_back(Eigen::Vector<double, DOF>::Zero()); // 起始速度为零
|
||
|
||
for (size_t i = 1; i < joint_positions.size(); ++i) {
|
||
double dt = time_points[i] - time_points[i-1];
|
||
Eigen::Vector<double, DOF> vel = (joint_positions[i] - joint_positions[i-1]) / dt;
|
||
joint_velocities.push_back(vel);
|
||
}
|
||
|
||
// 8. 打印轨迹信息
|
||
std::cout << "\n===================================== 轨迹规划信息 =====================================" << std::endl;
|
||
std::cout << "轨迹点总数: " << cartesian_trajectory.size() << " 个" << std::endl;
|
||
std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl;
|
||
std::cout << "最大速度: " << std::fixed << std::setprecision(6) << vel << "m/s" << std::endl;
|
||
std::cout << "加速度: " << std::fixed << std::setprecision(6) << acc << "m/s²" << std::endl;
|
||
std::cout << "总时间: " << std::fixed << std::setprecision(6) << move_time << "s" << std::endl;
|
||
std::cout << "起点位置: (x=" << T_current(0,3) << ", y=" << T_current(1,3) << ", z=" << T_current(2,3) << ")" << std::endl;
|
||
std::cout << "终点位置: (x=" << T_target(0,3) << ", y=" << T_target(1,3) << ", z=" << T_target(2,3) << ")" << std::endl;
|
||
std::cout << "保持姿态不变" << std::endl;
|
||
std::cout << "-----------------------------------------------------------------------------------------" << std::endl;
|
||
|
||
// 9. 执行轨迹
|
||
auto loop_start_time = std::chrono::high_resolution_clock::now();
|
||
|
||
for (size_t i = 0; i < cartesian_trajectory.size(); ++i) {
|
||
// 获取当前点的关节位置和速度
|
||
Eigen::Vector<double, DOF> q_cmd = joint_positions[i];
|
||
Eigen::Vector<double, DOF> q_vel = joint_velocities[i];
|
||
|
||
// 更新状态
|
||
m_state_->SetQ(q_cmd);
|
||
m_robot_->ComputeForwardKinematics(m_state_);
|
||
|
||
// 发送关节命令 - 为每个电机单独设置位置和速度
|
||
std::vector<JointPoint> joint_command;
|
||
for (size_t j = 0; j < DOF; ++j) {
|
||
JointPoint jp;
|
||
jp.joint_name = joint_names_[j];
|
||
jp.rad = q_cmd[j];
|
||
jp.vel = std::abs(q_vel[j]); // 使用计算出的关节速度
|
||
joint_command.push_back(jp);
|
||
}
|
||
|
||
// 计算当前点应该执行的时间
|
||
double expected_time = time_points[i];
|
||
// servoJ(joint_command, vel, expected_time);
|
||
for (const auto &j: joint_command) {
|
||
auto motor = motor_manager_->getMotor(j.joint_name);
|
||
if (motor != nullptr) {
|
||
|
||
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
|
||
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
|
||
}
|
||
motor->setQd(j.vel);
|
||
motor->setQ(j.rad);
|
||
}
|
||
}
|
||
|
||
// 检查中断
|
||
if (flash_cmd_.load()) {
|
||
flash_cmd_.store(false);
|
||
LOG(INFO) << "moveL: Interrupted by external command";
|
||
return;
|
||
}
|
||
|
||
// 控制时间节奏 - 使用精确的时间规划
|
||
auto expected_time_point = loop_start_time + std::chrono::nanoseconds(
|
||
static_cast<long long>(expected_time * 1e9)
|
||
);
|
||
auto now = std::chrono::high_resolution_clock::now();
|
||
if (now < expected_time_point) {
|
||
std::this_thread::sleep_until(expected_time_point);
|
||
} else {
|
||
LOG(WARNING) << "moveL: Behind schedule at point " << i
|
||
<< " by " << std::chrono::duration_cast<std::chrono::milliseconds>(now - expected_time_point).count() << "ms";
|
||
}
|
||
}
|
||
|
||
// 最终状态更新
|
||
m_state_->SetQ(joint_positions.back());
|
||
m_robot_->ComputeForwardKinematics(m_state_);
|
||
rsm_.store(ROBOT_READY);
|
||
LOG(INFO) << "moveL: Trajectory completed successfully";
|
||
|
||
} catch (const std::exception &e) {
|
||
LOG(ERROR) << "moveL failed: " << e.what();
|
||
rsm_.store(ROBOT_ERROR);
|
||
throw std::runtime_error(std::string("moveL error: ") + e.what());
|
||
}
|
||
}
|
||
|
||
|
||
// 辅助函数：计算运动时间
|
||
template<int DOF>
|
||
double HumanoidRobot<DOF>::calculateMoveTime(double distance, double vel, double acc) {
|
||
// 计算加速和减速所需的时间和距离
|
||
double acc_time = vel / acc;
|
||
double acc_distance = 0.5 * acc * acc_time * acc_time;
|
||
|
||
// 如果加速距离超过总距离的一半，需要调整最大速度
|
||
if (2 * acc_distance > distance) {
|
||
// 三角形速度曲线：加速然后直接减速
|
||
double max_reachable_vel = std::sqrt(acc * distance);
|
||
return 2 * max_reachable_vel / acc;
|
||
} else {
|
||
// 梯形速度曲线：加速-匀速-减速
|
||
double constant_distance = distance - 2 * acc_distance;
|
||
double constant_time = constant_distance / vel;
|
||
return 2 * acc_time + constant_time;
|
||
}
|
||
}
|
||
|
||
// 辅助函数：生成S曲线轨迹规划
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::generateSTrapezoidalProfile(double total_distance, double max_vel, double max_acc,
|
||
double total_time, size_t num_points,
|
||
std::vector<double>& time_points,
|
||
std::vector<double>& distance_ratios) {
|
||
time_points.clear();
|
||
distance_ratios.clear();
|
||
|
||
// 计算加速和减速阶段的时间
|
||
double acc_time = max_vel / max_acc;
|
||
double acc_distance = 0.5 * max_acc * acc_time * acc_time;
|
||
|
||
// 确定实际的速度曲线形状
|
||
if (2 * acc_distance > total_distance) {
|
||
// 三角形速度曲线
|
||
double actual_max_vel = std::sqrt(max_acc * total_distance);
|
||
acc_time = actual_max_vel / max_acc;
|
||
acc_distance = 0.5 * max_acc * acc_time * acc_time;
|
||
|
||
double dec_time = acc_time;
|
||
|
||
// 生成时间点和距离比例
|
||
for (size_t i = 0; i <= num_points; ++i) {
|
||
double t = static_cast<double>(i) / num_points * total_time;
|
||
time_points.push_back(t);
|
||
|
||
if (t <= acc_time) {
|
||
// 加速阶段
|
||
double s = 0.5 * max_acc * t * t;
|
||
distance_ratios.push_back(s / total_distance);
|
||
} else {
|
||
// 减速阶段
|
||
double dec_start_time = total_time - dec_time;
|
||
double dec_elapsed = t - dec_start_time;
|
||
double s = acc_distance + actual_max_vel * dec_elapsed - 0.5 * max_acc * dec_elapsed * dec_elapsed;
|
||
distance_ratios.push_back(s / total_distance);
|
||
}
|
||
}
|
||
} else {
|
||
// 梯形速度曲线
|
||
double constant_time = (total_distance - 2 * acc_distance) / max_vel;
|
||
double dec_time = acc_time;
|
||
|
||
// 生成时间点和距离比例
|
||
for (size_t i = 0; i <= num_points; ++i) {
|
||
double t = static_cast<double>(i) / num_points * total_time;
|
||
time_points.push_back(t);
|
||
|
||
if (t <= acc_time) {
|
||
// 加速阶段
|
||
double s = 0.5 * max_acc * t * t;
|
||
distance_ratios.push_back(s / total_distance);
|
||
} else if (t <= acc_time + constant_time) {
|
||
// 匀速阶段
|
||
double s = acc_distance + max_vel * (t - acc_time);
|
||
distance_ratios.push_back(s / total_distance);
|
||
} else {
|
||
// 减速阶段
|
||
double dec_start_time = acc_time + constant_time;
|
||
double dec_elapsed = t - dec_start_time;
|
||
double s = acc_distance + max_vel * constant_time +
|
||
max_vel * dec_elapsed - 0.5 * max_acc * dec_elapsed * dec_elapsed;
|
||
distance_ratios.push_back(s / total_distance);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
template<int DOF>
|
||
cmvr::math::Pose3d HumanoidRobot<DOF>::getTransform(std::string &base_link, std::string &target_link)
|
||
{
|
||
// 获取gRPC生成的Pose3d消息
|
||
auto grpc_pose = fk(base_link, target_link);
|
||
|
||
// 转换为cmvr::math::Pose3d
|
||
cmvr::math::Pose3d math_pose;
|
||
|
||
// 转换位置信息
|
||
math_pose.position.x = grpc_pose.position().x();
|
||
math_pose.position.y = grpc_pose.position().y();
|
||
math_pose.position.z = grpc_pose.position().z();
|
||
|
||
// 转换四元数
|
||
math_pose.quaternion.w = grpc_pose.quaternion().w();
|
||
math_pose.quaternion.x = grpc_pose.quaternion().x();
|
||
math_pose.quaternion.y = grpc_pose.quaternion().y();
|
||
math_pose.quaternion.z = grpc_pose.quaternion().z();
|
||
|
||
// 转换欧拉角
|
||
math_pose.euler.rx = grpc_pose.euler().rx();
|
||
math_pose.euler.ry = grpc_pose.euler().ry();
|
||
math_pose.euler.rz = grpc_pose.euler().rz();
|
||
|
||
return math_pose;
|
||
}
|
||
|
||
template class cmvr::device::HumanoidRobot<7>;
|
||
template class cmvr::device::HumanoidRobot<14>;
|
||
template class cmvr::device::HumanoidRobot<20>;
|
||
|
||
|