SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
25ec465e0b
cmvr-es/src/devices/robot/humanoid_robot/humanoid_robot.cpp

1245 lines
51 KiB
C++
Raw Normal View History

feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
//
// 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();
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();
feat:add touch grpc service API
2025-09-01 16:24:08 +08:00
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();
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
}
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>
feat:add move delta J and touch control api
2025-08-28 09:39:59 +08:00
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) {
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
try {
if (rsm_.load() == ROBOT_RUNNING) {
flash_cmd_.store(true);
eStop();
feat:add move delta J and touch control api
2025-08-28 09:39:59 +08:00
}
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
rsm_.store(ROBOT_RUNNING);
feat:add move delta J and touch control api
2025-08-28 09:39:59 +08:00
// 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);
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
if (!ok) {
throw runtime_error("solve IK failed");
}
feat:add move delta J and touch control api
2025-08-28 09:39:59 +08:00
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);
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
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) {
feat:add touch grpc service API
2025-09-01 16:24:08 +08:00
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);
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
}
feat:add touch grpc service API
2025-09-01 16:24:08 +08:00
motor->setQd(vel);
motor->setQ(j.rad);
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
}
}
}
feat:add touch grpc service API
2025-09-01 16:24:08 +08:00
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());
}
}
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
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;
}
feat:add move delta J and touch control api
2025-08-28 09:39:59 +08:00
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());
}
}
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
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();
}
feat:add move delta J and touch control api
2025-08-28 09:39:59 +08:00
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;
}
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
update moveL
2025-09-09 17:03:52 +08:00
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());
// 姿态偏移（弧度，ZYX欧拉角）
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();
// 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.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); // 使用main中的高迭代次数
if (!ik_solvable) {
throw std::runtime_error("moveL: Target pose is unreachable");
}
// 3. 计算位置差值（与main一致，保持姿态不变）
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. 基于路径长度的均匀插值（核心修改点）
// 计算所需的插值点数（根据速度和控制周期计算）
double move_time = total_distance / vel; // 总移动时间
size_t num_points = std::max(2ul, static_cast<size_t>(ceil(move_time / CONTROL_PERIOD)));
double step_distance = total_distance / num_points; // 每个点的距离间隔
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 = static_cast<double>(i) / num_points; // 基于距离的插值系数（0~1）
Eigen::Matrix4d T_interp = T_current; // 复制起点姿态（保持不变）
// 仅位置按比例插值
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::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) << step_distance << "m" << 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::setw(4) << "序号" << " | "
<< std::setw(10) << "x(m)" << " | "
<< std::setw(10) << "y(m)" << " | "
<< std::setw(10) << "z(m)" << " | "
<< std::setw(16) << "到起点距离(m)" << " | "
<< std::setw(16) << "与上一点距离(m)" << std::endl;
std::cout << "-----------------------------------------------------------------------------------------" << std::endl;
double prev_distance = 0.0;
for (size_t idx = 0; idx < cartesian_trajectory.size(); ++idx) {
const auto& T = cartesian_trajectory[idx];
Eigen::Vector3d pos(T(0,3), T(1,3), T(2,3));
Eigen::Vector3d delta_from_start = pos - T_current.block<3,1>(0,3);
double current_distance = delta_from_start.norm();
double segment_distance = (idx == 0) ? current_distance : current_distance - prev_distance;
std::cout << std::setw(4) << 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(16) << current_distance << " | "
<< std::fixed << std::setprecision(6) << std::setw(16) << segment_distance << std::endl;
prev_distance = current_distance;
}
std::cout << "=========================================================================================\n" << std::endl;
// 7. 执行IK求解（与main逻辑一致，逐步更新状态）
auto loop_start_time = std::chrono::high_resolution_clock::now();
for (size_t i = 0; 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求解参数（使用main中的高迭代次数）
int max_iter = 10000;
double tolerance = 1e-6;
// 求解IK，以上一个状态作为初始值（与main一致）
bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD,
ctrl::CartesianController<DOF>::Mode::Position,
q_current, max_iter, tolerance);
// 失败重试机制
if (!ok) {
LOG(WARNING) << "Retrying IK for point " << i;
ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD,
ctrl::CartesianController<DOF>::Mode::Position,
q_current, max_iter * 2, tolerance * 10);
}
if (!ok) {
LOG(ERROR) << "IK failed at point " << i;
LOG(ERROR) << "Target pos: (" << T_interp(0,3) << ","
<< T_interp(1,3) << "," << T_interp(2,3) << ")";
throw std::runtime_error("moveL: IK failed during trajectory execution");
}
// 右臂第二个关节不能超过90°
if (q_current[8] > 1.5708)
q_current[8] = 1.5708;
// 新增：打印IK求解得到的关节角度
std::cout << "\n===================================== 关节角度信息 (点 " << i << ") =====================================" << std::endl;
std::cout << "左手臂关节角度（弧度）：" << std::endl;
std::cout << " L_SHOULDER_P: " << std::fixed << std::setprecision(6) << q_current[0] << std::endl;
std::cout << " L_SHOULDER_R: " << std::fixed << std::setprecision(6) << q_current[1] << std::endl;
std::cout << " L_SHOULDER_Y: " << std::fixed << std::setprecision(6) << q_current[2] << std::endl;
std::cout << " L_ELBOW_R: " << std::fixed << std::setprecision(6) << q_current[3] << std::endl;
std::cout << " L_WRIST_P: " << std::fixed << std::setprecision(6) << q_current[4] << std::endl;
std::cout << " L_WRIST_Y: " << std::fixed << std::setprecision(6) << q_current[5] << std::endl;
std::cout << " L_WRIST_R: " << std::fixed << std::setprecision(6) << q_current[6] << std::endl;
std::cout << "\n右手臂关节角度（弧度）：" << std::endl;
std::cout << " R_SHOULDER_P: " << std::fixed << std::setprecision(6) << q_current[7] << std::endl;
std::cout << " R_SHOULDER_R: " << std::fixed << std::setprecision(6) << q_current[8] << std::endl;
std::cout << " R_SHOULDER_Y: " << std::fixed << std::setprecision(6) << q_current[9] << std::endl;
std::cout << " R_ELBOW_R: " << std::fixed << std::setprecision(6) << q_current[10] << std::endl;
std::cout << " R_WRIST_P: " << std::fixed << std::setprecision(6) << q_current[11] << std::endl;
std::cout << " R_WRIST_Y: " << std::fixed << std::setprecision(6) << q_current[12] << std::endl;
std::cout << " R_WRIST_R: " << std::fixed << std::setprecision(6) << q_current[13] << std::endl;
std::cout << "====================================================================================================\n" << std::endl;
// 更新状态（与main一致，保证连续性）
m_state_->SetQ(q_current);
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_current[j];
jp.vel = vel;
joint_command.push_back(jp);
}
servoJ(joint_command, vel, CONTROL_PERIOD);
// 检查中断
if (flash_cmd_.load()) {
flash_cmd_.store(false);
LOG(INFO) << "moveL: Interrupted by external command";
return;
}
// 控制时间节奏
auto expected_time = loop_start_time + std::chrono::nanoseconds(
static_cast<long long>(i * CONTROL_PERIOD * 1e9)
);
auto now = std::chrono::high_resolution_clock::now();
if (now < expected_time) {
std::this_thread::sleep_until(expected_time);
}
}
// 最终状态更新
m_state_->SetQ(q_current);
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());
}
}
feat:add eye hand calibration and pose follow
2025-08-22 16:57:29 +08:00
template class cmvr::device::HumanoidRobot<7>;
template class cmvr::device::HumanoidRobot<14>;
template class cmvr::device::HumanoidRobot<20>;
Reference in New Issue Copy Permalink