diff --git a/config/cabin_robot.xml b/config/cabin_robot.xml index 13dd1646..e1babd54 100644 --- a/config/cabin_robot.xml +++ b/config/cabin_robot.xml @@ -40,7 +40,7 @@ bufferSize="50" verbose="false"> - + diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index 46540788..4b5b8bd9 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -15,6 +15,7 @@ template HumanoidRobot::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::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { l_can_sender_ = std::make_shared >(); l_can_receiver_ = std::make_shared >(); l_message_manager_ = std::make_shared >(); + left_toolFrame_ = l_can_cfg.getAttrString("toolFrame"); auto r_can_cfg = can_cfg.getChild("RightArmCan"); @@ -52,7 +54,7 @@ HumanoidRobot::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { r_can_sender_ = std::make_shared >(); r_can_receiver_ = std::make_shared >(); r_message_manager_ = std::make_shared >(); - + 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::speedJ(std::string &joint_name, RobotJointIndexDirectio } } -// template -// void HumanoidRobot::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> velocity_queue; -// std::mutex queue_mutex; -// std::condition_variable queue_cv; -// std::atomic ik_completed{false}; -// std::atomic stop_requested{false}; -// -// // IK计算线程（使用CartesianController的Velocity模式） -// std::thread ik_thread([&]() { -// try { -// // 获取当前关节位置 -// auto q_map_current = getJointQ(); -// Eigen::Vector 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 joint_velocities; -// bool ok = m_cctrl_->compute(m_state_, base_link, {target}, CONTROL_PERIOD, -// ctrl::CartesianController::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 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(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 joint_velocities; -// bool has_velocity = false; -// -// // 从队列中获取关节速度 -// { -// std::unique_lock 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 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 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 void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) { if (vel <= 0 || acc <= 0) { @@ -1027,7 +797,7 @@ void HumanoidRobot::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); diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.h b/src/devices/robot/humanoid_robot/humanoid_robot.h index c1b2d70e..cd9bafd9 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.h +++ b/src/devices/robot/humanoid_robot/humanoid_robot.h @@ -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 upd_timer_; std::atomic rsm_{ROBOT_DISABLED};