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

update

Browse Source
This commit is contained in:
linbo 2025-09-19 15:23:14 +08:00
parent ee9a8022d0
commit 83c52ac19b
3 changed files with 7 additions and 235 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
config/cabin_robot.xml
View File

@ -40,7 +40,7 @@
bufferSize="50"
verbose="false">
<CanManger id="" devId="">
<LeftArmCan id = " " devId = " " channelId ="0">
<LeftArmCan id = " " devId = " " channelId ="0" toolFrame="L_FINGER_TIP">
<!-- <Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->

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

@ -15,6 +15,7 @@ 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");
@ -44,6 +45,7 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(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> >();
left_toolFrame_ = l_can_cfg.getAttrString("toolFrame");
auto r_can_cfg = can_cfg.getChild("RightArmCan");
@ -52,7 +54,7 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(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> >();
right_toolFrame_ = r_can_cfg.getAttrString("toolFrame");
auto waist_can_cfg = can_cfg.getChild("WaistCan");
waist_motors_cfg_ = waist_can_cfg.getChildren("Motor");
@ -784,238 +786,6 @@ void HumanoidRobot<DOF>::speedJ(std::string &joint_name, RobotJointIndexDirectio
}
}
// 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");
// }
// rsm_.store(ROBOT_RUNNING);
// try {
// const double CONTROL_PERIOD = 1.0 / 50.0; // 控制周期保持不变
//
// // 定义基座和末端执行器链接
// std::string base_link = "PELVIS_S";
// std::string ee_link = "R_WRIST_R_S";
//
// // 获取当前末端执行器位姿
// 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();
//
// // 根据方向设置笛卡尔速度
// if (cart == RobotCartesian::X) {
// direction.x() = (dir == RobotJointIndexDirection::FORWARD) ? 1.0 : -1.0;
// } else if (cart == RobotCartesian::Y) {
// direction.y() = (dir == RobotJointIndexDirection::FORWARD) ? 1.0 : -1.0;
// } else if (cart == RobotCartesian::Z) {
// direction.z() = (dir == RobotJointIndexDirection::FORWARD) ? 1.0 : -1.0;
// }
//
// // 规范化方向向量
// direction.normalize();
//
// // 计算目标位姿（基于当前位姿和方向）
// Eigen::Matrix4d T_target = T_current;
// T_target(0, 3) += direction.x() * vel * CONTROL_PERIOD; // 小位移
// T_target(1, 3) += direction.y() * vel * CONTROL_PERIOD;
// T_target(2, 3) += direction.z() * vel * CONTROL_PERIOD;
//
// // 创建目标位姿
// cmvr::ctrl::PoseTarget target;
// target.T_target = T_target;
// target.link_name = ee_link;
// target.w_posrot = 0.5; // 位置和旋转权重
// target.weight = 1.0;
//
// // 创建队列用于存储关节速度
// std::queue<Eigen::Vector<double, DOF>> velocity_queue;
// std::mutex queue_mutex;
// std::condition_variable queue_cv;
// std::atomic<bool> ik_completed{false};
// std::atomic<bool> stop_requested{false};
//
// // IK计算线程（使用CartesianController的Velocity模式）
// std::thread ik_thread([&]() {
// try {
// // 获取当前关节位置
// auto q_map_current = getJointQ();
// Eigen::Vector<double, DOF> current_joint_positions;
// for (int i = 0; i < DOF; ++i) {
// current_joint_positions[i] = q_map_current[joint_names_[i]];
// }
//
// // 设置初始状态
// m_state_->SetQ(current_joint_positions);
// m_robot_->ComputeForwardKinematics(m_state_);
//
// LOG(INFO) << "Current robot state: " << rsm_.load();
// LOG(INFO) << "stop_requested: " << stop_requested.load();
//
// // IK计算循环
// while (rsm_.load() == ROBOT_RUNNING ) {
// // 使用CartesianController计算关节速度
// Eigen::Vector<double, DOF> joint_velocities;
// bool ok = m_cctrl_->compute(m_state_, base_link, {target}, CONTROL_PERIOD,
// ctrl::CartesianController<DOF>::Mode::Velocity,
// joint_velocities, 10000, 1e-6);
//
// if (!ok) {
// LOG(WARNING) << "IK计算失败，使用上一个有效速度";
// // 可以使用上一个有效的速度或者零速度
// continue;
// }
//
// LOG(INFO) << "IK计算得到的关节速度：";
// for (int i = 0; i < DOF; ++i) {
// LOG(INFO) << "关节 " << joint_names_[i] << " 速度: " << joint_velocities[i];
// }
//
// // 将关节速度加入队列
// {
// std::lock_guard<std::mutex> lock(queue_mutex);
// velocity_queue.push(joint_velocities);
// }
// queue_cv.notify_one();
//
// // 更新目标位姿（基于当前位姿和方向）
// msgs::Pose3d current_pose = fk(base_link, ee_link);
// 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();
//
// T_target = T_current;
// T_target(0, 3) += direction.x() * vel * CONTROL_PERIOD;
// T_target(1, 3) += direction.y() * vel * CONTROL_PERIOD;
// T_target(2, 3) += direction.z() * vel * CONTROL_PERIOD;
//
// target.T_target = T_target;
//
// // 等待控制周期
// std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(CONTROL_PERIOD * 1000)));
// }
//
// ik_completed.store(true);
// queue_cv.notify_one();
//
// } catch (const std::exception& e) {
// LOG(ERROR) << "IK thread error: " << e.what();
// ik_completed.store(true);
// queue_cv.notify_one();
// }
// });
//
// // 控制执行线程（周期同步速度模式）
// std::thread control_thread([&]() {
// try {
// // 设置所有电机为周期同步速度模式
// for (int i = 0; i < DOF; ++i) {
// auto motor = motor_manager_->getMotor(joint_names_[i]);
// if (motor != nullptr) {
// if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) {
// motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
// }
// }
// }
//
// // 控制循环
// while (rsm_.load() == ROBOT_RUNNING && !stop_requested.load()) {
// Eigen::Vector<double, DOF> joint_velocities;
// bool has_velocity = false;
//
// // 从队列中获取关节速度
// {
// std::unique_lock<std::mutex> lock(queue_mutex);
// if (queue_cv.wait_for(lock, std::chrono::milliseconds(100),
// [&] { return !velocity_queue.empty() || ik_completed.load(); })) {
// if (!velocity_queue.empty()) {
// joint_velocities = velocity_queue.front();
// velocity_queue.pop();
// has_velocity = true;
// }
// }
// }
//
// if (has_velocity) {
// // 发送关节速度命令
// for (int j = 0; j < DOF; ++j) {
// auto motor = motor_manager_->getMotor(joint_names_[j]);
// // if (motor != nullptr) {
// // motor->setQd(joint_velocities[j]);
// // }
// }
// }
//
// // 检查是否完成
// {
// std::lock_guard<std::mutex> lock(queue_mutex);
// if (ik_completed.load() && velocity_queue.empty()) {
// break;
// }
// }
//
// // 控制时间节奏
// std::this_thread::sleep_for(std::chrono::milliseconds(10));
// }
//
// // 停止所有关节
// for (int i = 0; i < DOF; ++i) {
// auto motor = motor_manager_->getMotor(joint_names_[i]);
// if (motor != nullptr) {
// motor->setQd(0);
// }
// }
//
// } catch (const std::exception& e) {
// LOG(ERROR) << "Control thread error: " << e.what();
// }
// });
//
// LOG(INFO) << "开始在笛卡尔空间进行速度控制，方向：" << dir << "，速度：" << vel;
//
// // 等待停止命令
// while (rsm_.load() == ROBOT_RUNNING && !flash_cmd_.load()) {
// std::this_thread::sleep_for(std::chrono::milliseconds(100));
// }
//
// // 通知线程停止
// stop_requested.store(true);
// {
// std::lock_guard<std::mutex> lock(queue_mutex);
// ik_completed.store(true);
// }
// queue_cv.notify_all();
//
// // 等待线程结束
// if (ik_thread.joinable()) {
// ik_thread.join();
// }
// if (control_thread.joinable()) {
// control_thread.join();
// }
//
// LOG(INFO) << "speedL速度控制完成，所有电机已停止。";
//
// } catch (const std::exception &e) {
// LOG(ERROR) << "speedL failed: " << e.what();
// rsm_.store(ROBOT_ERROR);
// throw std::runtime_error(std::string("speedL error: ") + e.what());
// }
// }
template<int DOF>
void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) {
if (vel <= 0 || acc <= 0) {
@ -1027,7 +797,7 @@ void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection di
// 定义基座和末端执行器链接
std::string base_link = "PELVIS_S";
std::string ee_link = "R_WRIST_R_S";
std::string ee_link = right_toolFrame_;
msgs::Pose3d current_pose = fk(base_link, ee_link);

2
src/devices/robot/humanoid_robot/humanoid_robot.h
View File

@ -137,6 +137,8 @@ namespace cmvr::device{
private:
std::string id_;
std::string right_toolFrame_;
std::string left_toolFrame_;
int upd_freq_;
std::shared_ptr<FDTimer> upd_timer_;
std::atomic<RobotStateMachine> rsm_{ROBOT_DISABLED};