1013 lines
36 KiB
C++
1013 lines
36 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"
|
||
|
||
|
||
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();
|
||
|
||
motor_manager_->getMotor("WAIST_Y")->setQ(0);
|
||
motor_manager_->getMotor("WAIST_P")->setQ(0);
|
||
|
||
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>::getJointsState(std::vector<JointState>& states) {
|
||
try {
|
||
lock_guard lock(exec_mtx_);
|
||
|
||
states.clear();
|
||
JointState state;
|
||
for (const auto &pair : motor_manager_->motorsMap()) {
|
||
auto motor = pair.second;
|
||
state.name = motor->jointName();
|
||
state.position = motor->getQ();
|
||
state.velocity = motor->getQd();
|
||
states.push_back(state);
|
||
}
|
||
} catch (exception &e) {
|
||
throw runtime_error(e.what());
|
||
}
|
||
}
|
||
|
||
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>::torqueOn(const std::string &joint_name) {
|
||
auto motor = motor_manager_->getMotor(joint_name);
|
||
motor->brake();
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::torqueOff(const std::string &joint_name) {
|
||
auto motor = motor_manager_->getMotor(joint_name);
|
||
motor->torqueOff();
|
||
}
|
||
|
||
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% 左右
|
||
|
||
if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
|
||
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
|
||
}
|
||
motor->setQd(vel);
|
||
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();
|
||
} else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
|
||
rsm_.store(ROBOT_RUNNING);
|
||
// TODO:
|
||
// 1. interpolate line waypoint by vel and acc
|
||
// 2. for each waypoint, call cartesian controller to solve joint positions
|
||
// 3. for each waypoint, call motor Cyclic Synchronous Position (CSP) command with Timer
|
||
// 4. in the loop, check flash_cmd_, if it is true, set it false then exit
|
||
// Eigen::Vector<double, DOF> q_cmd;
|
||
// m_state_->SetQ(state_.joint_positions);
|
||
// bool ok = m_cctrl_.compute(m_state_, base_link, targets, 1, ctrl::CartesianController<DOF>::Mode::Position, q_cmd, 60, 1e-4);
|
||
// if (!ok) {
|
||
// throw runtime_error("solve IK failed");
|
||
// }
|
||
rsm_.store(ROBOT_READY);
|
||
} else {
|
||
throw runtime_error("rsm invalid");
|
||
}
|
||
} catch (exception &e) {
|
||
throw 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);
|
||
// TODO:
|
||
// 1. set joint speed and acc
|
||
// 2. set joint speed by PROFILE VELOCITY MODE (PVM)
|
||
// rsm_.store(ROBOT_READY); -> should not set rsm_ to ready because motor is running
|
||
} else {
|
||
throw runtime_error("rsm invalid");
|
||
}
|
||
} catch (exception &e) {
|
||
throw runtime_error(e.what());
|
||
}
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::speedL(RobotCartesian cart, 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);
|
||
// TODO: ???
|
||
// rsm_.store(ROBOT_READY); -> should not set rsm_ to ready because motor is running
|
||
} else {
|
||
throw runtime_error("rsm invalid");
|
||
}
|
||
} catch (exception &e) {
|
||
throw runtime_error(e.what());
|
||
}
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::followJointTrajectory(std::vector<std::vector<JointPoint> > &traj, double dt) {
|
||
try {
|
||
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
|
||
auto ok = check_joint_traj_(traj, dt);
|
||
if (!ok) { throw runtime_error("joint traj invalid"); }
|
||
|
||
rsm_.store(ROBOT_RUNNING);
|
||
// TODO: need to optimize callback loop
|
||
for (auto i = 0; i < traj.size(); i++) {
|
||
if (flash_cmd_.load()) {
|
||
flash_cmd_.store(false);
|
||
LOG(INFO) << "followJointTrajectory is canceled";
|
||
return;
|
||
}
|
||
servoJ(traj[i], dt);
|
||
this_thread::sleep_for(chrono::milliseconds((int) dt));
|
||
}
|
||
rsm_.store(ROBOT_ESTOP);
|
||
} else {
|
||
throw runtime_error("rsm invalid");
|
||
}
|
||
} catch (exception &e) {
|
||
throw runtime_error(e.what());
|
||
}
|
||
}
|
||
|
||
template<int DOF>
|
||
void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link,
|
||
std::vector<std::vector<cmvr::ctrl::PoseTarget> > &targets, double dt) {
|
||
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);
|
||
// TODO:
|
||
// 1. set Timer(dt)
|
||
// 2. for each timestamp, use Cyclic Synchronous Position (CSP) Mode to set joint position
|
||
// 3. if flash_cmd_ is set, set it to false and exit
|
||
// 3. join timer
|
||
rsm_.store(ROBOT_READY);
|
||
} else {
|
||
throw runtime_error("rsm invalid");
|
||
}
|
||
} catch (exception &e) {
|
||
throw runtime_error(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>
|
||
std::vector<double> HumanoidRobot<DOF>::ik(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose) {
|
||
// update m_state_
|
||
|
||
try {
|
||
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 std::runtime_error("IK solve failed");
|
||
} else {
|
||
return std::vector<double>(q_cmd.data(), q_cmd.data() + q_cmd.size());
|
||
}
|
||
}catch (std::exception &e) {
|
||
throw runtime_error(e.what());
|
||
}
|
||
|
||
|
||
}
|
||
|
||
|
||
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;
|
||
}
|
||
|
||
template class cmvr::device::HumanoidRobot<7>;
|
||
template class cmvr::device::HumanoidRobot<14>;
|
||
template class cmvr::device::HumanoidRobot<20>;
|