From 68756bd57aa6480a973fffcd808ecb6b5f2db78c Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Fri, 7 Nov 2025 15:29:07 +0800 Subject: [PATCH] update controller --- include/utils/solver/qp_solver.h | 10 ++-- .../robot/controller/cartesiancontroller.cpp | 48 +++++++++++++++++-- .../robot/controller/cartesiancontroller.h | 4 +- .../controller/jointpositioncontroller.cpp | 5 +- .../robot/humanoid_robot/humanoid_robot.cpp | 24 +++++----- .../robot/humanoid_robot/humanoid_robot.h | 2 +- src/utils/solver/qp_solver.cpp | 26 +++++----- 7 files changed, 82 insertions(+), 37 deletions(-) diff --git a/include/utils/solver/qp_solver.h b/include/utils/solver/qp_solver.h index 72fd524c..d84a3c57 100644 --- a/include/utils/solver/qp_solver.h +++ b/include/utils/solver/qp_solver.h @@ -40,7 +40,7 @@ namespace cmvr::ctrl{ // CartesianController declaration (implementation in .cpp) // ----------------------------------------------------------------------------- template - class CartesianController { + class QPSolver { public: using RobotT = cmvr::dyn::Robot; using VecD = Eigen::Vector; @@ -48,7 +48,7 @@ namespace cmvr::ctrl{ enum class Mode { Position, Velocity, Torque }; - CartesianController(std::shared_ptr robot, double dsafe = 0.05, double lambda = 1e-2); + QPSolver(std::shared_ptr robot, double dsafe = 0.05, double lambda = 1e-2); /* ------------------------- configuration ----------------------------- */ void setCollisionPairs(const std::vector& pairs); @@ -110,9 +110,9 @@ namespace cmvr::ctrl{ } // ------------------------------ explicit instantiation ----------------------- -extern template class cmvr::ctrl::CartesianController<7>; -extern template class cmvr::ctrl::CartesianController<14>; -extern template class cmvr::ctrl::CartesianController<20>; +extern template class cmvr::ctrl::QPSolver<7>; +extern template class cmvr::ctrl::QPSolver<14>; +extern template class cmvr::ctrl::QPSolver<20>; #endif //CMVR_ES_CARTESIAN_CONTROLLER_H diff --git a/src/devices/robot/controller/cartesiancontroller.cpp b/src/devices/robot/controller/cartesiancontroller.cpp index 64fdc41c..f27fb4a9 100644 --- a/src/devices/robot/controller/cartesiancontroller.cpp +++ b/src/devices/robot/controller/cartesiancontroller.cpp @@ -8,17 +8,59 @@ using namespace std; using namespace cmvr::device; CartesianController::CartesianController(const XmlNode& cfg):AbstractController(cfg) { + try + { + defaultSpeed_ = cfg.getAttrDefault("defaultSpeed",0.5f); + defaultAcc_ = cfg.getAttrDefault("defaultAcc",0.5f); + base_link_ = cfg.getAttrString("base_link"); + ee_link_ = cfg.getAttrString("ee_link"); + } + catch(const exception& e) + { + + } } void CartesianController::call(const Json::Value& json) { - if (state_ != ControllerState_Idle) - return; + try + { + if (state_ != ControllerState_Idle) + return; + //检测是否传入了base_link和ee_link，如果未指定，则使用成员变量中默认的值 + std::string baseLink = base_link_; + std::string eeLink = ee_link_; + if (json.isMember("params")) + { + std::vector canGroupIds; + const Json::Value& params = json["params"]; + if (params.isMember("base_link")) + { + baseLink = params["base_link"].asString(); + } + if (params.isMember("eeLink")) + { + eeLink = params["eeLink"].asString(); + } + //通过canGroupId去获取当前需要控制的电机的实际位置 + if (params.isMember("canGroupId")) + { + for (const auto & canGroupId : params["canGroupId"]) + { + canGroupIds.emplace_back(canGroupId.asString()); + } + } + } + state_ = ControllerState_Executing; + } + catch(const std::exception& e) + { + + } - state_ = ControllerState_Executing; } void CartesianController::interrupt() diff --git a/src/devices/robot/controller/cartesiancontroller.h b/src/devices/robot/controller/cartesiancontroller.h index 1546655d..babe74d5 100644 --- a/src/devices/robot/controller/cartesiancontroller.h +++ b/src/devices/robot/controller/cartesiancontroller.h @@ -4,6 +4,7 @@ #pragma once #include "abstractcontroller.h" +#include "utils/solver/qp_solver.h" namespace cmvr::device { @@ -15,6 +16,7 @@ namespace cmvr::device void interrupt() override; void stop() override; private: - + std::string base_link_; + std::string ee_link_; }; } diff --git a/src/devices/robot/controller/jointpositioncontroller.cpp b/src/devices/robot/controller/jointpositioncontroller.cpp index c186748c..d0acdee4 100644 --- a/src/devices/robot/controller/jointpositioncontroller.cpp +++ b/src/devices/robot/controller/jointpositioncontroller.cpp @@ -11,7 +11,8 @@ using namespace cmvr::hardware; JointPositionController::JointPositionController(const XmlNode& cfg):AbstractController(cfg) { - + defaultSpeed_ = cfg.getAttrDefault("defaultSpeed",0.5f); + defaultAcc_ = cfg.getAttrDefault("defaultAcc",0.5f); } void JointPositionController::call(const Json::Value& json) @@ -85,7 +86,7 @@ void JointPositionController::call(const Json::Value& json) } } } - + state_ = ControllerState_Executing; } catch (const std::exception& e) { diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index d3a47110..4a29bc00 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -30,7 +30,7 @@ HumanoidRobot::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { } m_state_ = m_robot_->MakeState(link_names_, joint_names_); - m_cctrl_ = make_shared >(m_robot_); + m_cctrl_ = make_shared >(m_robot_); upd_freq_ = cfg.getAttrDefault("updFreq", 500); CSP_buffer_ = make_shared >(cfg.getAttrDefault("bufferSize", 50)); CSV_buffer_ = make_shared >(cfg.getAttrDefault("bufferSize", 50)); @@ -397,7 +397,7 @@ void HumanoidRobot::moveJ(const std::string &base_link, const std::string & // slove ik Eigen::Vector q_cmd; bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, - ctrl::CartesianController::Mode::Position, + ctrl::QPSolver::Mode::Position, q_cmd, 10000, 1e-6); if (!ok) { throw runtime_error("solve IK failed"); @@ -466,7 +466,7 @@ void HumanoidRobot::moveJ_IK(const std::string &base_link, const std::vecto // slove ik Eigen::Vector q_cmd; - bool ok = m_cctrl_->compute(m_state_, base_link, targets, 0.002, ctrl::CartesianController::Mode::Position, + bool ok = m_cctrl_->compute(m_state_, base_link, targets, 0.002, ctrl::QPSolver::Mode::Position, q_cmd, 10000, 1e-6); if (!ok) { throw runtime_error("solve IK failed"); @@ -603,7 +603,7 @@ void HumanoidRobot::moveL(std::string &base_link, std::vector q_cmd; bool ok = m_cctrl_->compute(m_state_, base_link, interpolated_targets, 0.002, - ctrl::CartesianController::Mode::Position, + ctrl::QPSolver::Mode::Position, q_cmd, 10000, 1e-6); if (!ok) { @@ -959,7 +959,7 @@ void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection di Eigen::Vector q_next; bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD, - ctrl::CartesianController::Mode::Position, + ctrl::QPSolver::Mode::Position, q_next, 10000, 1e-6); if (!ok) { @@ -1160,7 +1160,7 @@ void HumanoidRobot::followPoseTrajectory(std::string &base_link, base_link, // 基座链接 first_pose_targets, // 第一个点的位姿目标 dt, // 控制周期（用于速度限制） - ctrl::CartesianController::Mode::Position, // 位置控制模式 + ctrl::QPSolver::Mode::Position, // 位置控制模式 q_first, // 输出：第一个点的关节配置 10000, // IK最大迭代次数（确保精度） 1e-6 // IK位置精度（1mm/0.001°） @@ -1278,7 +1278,7 @@ void HumanoidRobot::followPoseTrajectory(std::string &base_link, base_link, // 基座链接 current_pose_targets, // 当前点的位姿目标 dt, // 控制周期 - ctrl::CartesianController::Mode::Position, + ctrl::QPSolver::Mode::Position, q_cmd, // 输出：当前点的关节配置 5000, // 减少迭代次数（平衡精度与速度） 5e-4 // IK精度：0.5mm/0.028°（轨迹执行可适当放宽） @@ -1386,7 +1386,7 @@ void HumanoidRobot::servoJ(const std::string &base_link, const std::string // slove ik Eigen::Vector q_cmd; bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, - ctrl::CartesianController::Mode::Position, + ctrl::QPSolver::Mode::Position, q_cmd, 10000, 1e-6); if (!ok) { throw runtime_error("solve IK failed"); @@ -1433,7 +1433,7 @@ template void HumanoidRobot::servoL(std::string &base_link, std::vector &targets, double dt) { try { Eigen::Vector q_cmd; - bool ok = m_cctrl_->compute(m_state_, base_link, targets, 1, ctrl::CartesianController::Mode::Position, + bool ok = m_cctrl_->compute(m_state_, base_link, targets, 1, ctrl::QPSolver::Mode::Position, q_cmd, 60, 1e-4); if (!ok) { LOG(WARNING) << "[HumanoidRobot] (servoL): solve IK failed, id=" << id_; @@ -1575,7 +1575,7 @@ std::vector HumanoidRobot< // slove ik Eigen::Vector q_cmd{}; bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, - ctrl::CartesianController::Mode::Position, + ctrl::QPSolver::Mode::Position, q_cmd, 10000, 1e-6); if (!ok) { throw std::runtime_error("IK solve failed"); @@ -1765,7 +1765,7 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & Eigen::Vector q_cmd_check; bool ik_solvable = m_cctrl_->compute(m_state_, base_link, {target_ik_check}, 0.002, - ctrl::CartesianController::Mode::Position, + ctrl::QPSolver::Mode::Position, q_cmd_check, 10000, 1e-6); if (!ik_solvable) { throw std::runtime_error("moveL: Target pose is unreachable with constant orientation"); @@ -1827,7 +1827,7 @@ void HumanoidRobot::moveL(const std::string &base_link, const std::string & // 使用前一点的位置作为初始值求解IK Eigen::Vector q_next; bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD, - ctrl::CartesianController::Mode::Position, + ctrl::QPSolver::Mode::Position, q_next, 10000, 1e-6); if (!ok) { diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.h b/src/devices/robot/humanoid_robot/humanoid_robot.h index 3933a541..e68e8e5a 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.h +++ b/src/devices/robot/humanoid_robot/humanoid_robot.h @@ -151,7 +151,7 @@ namespace cmvr::device{ std::shared_ptr> m_state_; std::shared_ptr> m_robot_; - std::shared_ptr> m_cctrl_; + std::shared_ptr> m_cctrl_; std::vector joint_names_; std::vector link_names_; diff --git a/src/utils/solver/qp_solver.cpp b/src/utils/solver/qp_solver.cpp index e669c8c4..415cad89 100644 --- a/src/utils/solver/qp_solver.cpp +++ b/src/utils/solver/qp_solver.cpp @@ -3,7 +3,7 @@ using namespace cmvr::ctrl; template -CartesianController::CartesianController(std::shared_ptr robot, double dsafe, double lambda) +QPSolver::QPSolver(std::shared_ptr robot, double dsafe, double lambda) : robot_(std::move(robot)), d_safe_(dsafe), lambda_(lambda), solver_() { dist_req_.enable_nearest_points = true; @@ -11,13 +11,13 @@ CartesianController::CartesianController(std::shared_ptr robot, dou // === configuration setters ================================================== template -void CartesianController::setCollisionPairs(const std::vector& p) +void QPSolver::setCollisionPairs(const std::vector& p) { pairs_ = p; } template -void CartesianController::setPositionLimits(const std::vector>& qmin, +void QPSolver::setPositionLimits(const std::vector>& qmin, const std::vector>& qmax) { if (qmin.size()==DOF) q_min_ = qmin; @@ -25,13 +25,13 @@ void CartesianController::setPositionLimits(const std::vector -void CartesianController::setVelocityLimits(const std::vector>& qdmax) +void QPSolver::setVelocityLimits(const std::vector>& qdmax) { if (qdmax.size()==DOF) qd_max_ = qdmax; } template -void CartesianController::setAccelerationLimits(const std::vector>& qddmax) +void QPSolver::setAccelerationLimits(const std::vector>& qddmax) { if (qddmax.size()==DOF) qdd_max_ = qddmax; } @@ -39,7 +39,7 @@ void CartesianController::setAccelerationLimits(const std::vector -inline void CartesianController::clampVec( +inline void QPSolver::clampVec( VecD& v, const std::vector>& lo,const std::vector>& hi) const { for (int i=0;i::clampVec( // === main compute =========================================================== template -bool CartesianController::compute( +bool QPSolver::compute( std::shared_ptr>& state, const std::string& base_link, const std::vector& targets, @@ -114,7 +114,7 @@ bool CartesianController::compute( // === solveIK ============================================================== template -bool CartesianController::solveIK( +bool QPSolver::solveIK( std::shared_ptr>& state, const std::string& base_link, const std::vector& targets, @@ -198,7 +198,7 @@ bool CartesianController::solveIK( try { dq = solver_.Solve(); } catch (const std::exception& e) { - std::cerr << "[CartesianController] QP failed: " << e.what() << std::endl; + std::cerr << "[QPSolver] QP failed: " << e.what() << std::endl; return false; } @@ -218,7 +218,7 @@ bool CartesianController::solveIK( // === distanceConstraint ===================================================== template -bool CartesianController::distanceConstraint( +bool QPSolver::distanceConstraint( const CollisionPair& cp, std::shared_ptr>& state, const std::vector>& objs, @@ -258,6 +258,6 @@ bool CartesianController::distanceConstraint( return true; // 需要加入不等式 } -template class cmvr::ctrl::CartesianController<7>; -template class cmvr::ctrl::CartesianController<14>; -template class cmvr::ctrl::CartesianController<20>; \ No newline at end of file +template class cmvr::ctrl::QPSolver<7>; +template class cmvr::ctrl::QPSolver<14>; +template class cmvr::ctrl::QPSolver<20>; \ No newline at end of file