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

update controller

Browse Source
This commit is contained in:
linbo 2025-11-07 15:29:07 +08:00
parent a76a92aa9b
commit 68756bd57a
7 changed files with 82 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
include/utils/solver/qp_solver.h
View File

@ -40,7 +40,7 @@ namespace cmvr::ctrl{
// CartesianController declaration (implementation in .cpp) // CartesianController declaration (implementation in .cpp)
// ----------------------------------------------------------------------------- // -----------------------------------------------------------------------------
template<int DOF> template<int DOF>
class CartesianController { class QPSolver {
public: public:
using RobotT = cmvr::dyn::Robot<DOF>; using RobotT = cmvr::dyn::Robot<DOF>;
using VecD = Eigen::Vector<double, DOF>; using VecD = Eigen::Vector<double, DOF>;
@ -48,7 +48,7 @@ namespace cmvr::ctrl{
enum class Mode { Position, Velocity, Torque }; enum class Mode { Position, Velocity, Torque };
CartesianController(std::shared_ptr<RobotT> robot, double dsafe = 0.05, double lambda = 1e-2); QPSolver(std::shared_ptr<RobotT> robot, double dsafe = 0.05, double lambda = 1e-2);
/* ------------------------- configuration ----------------------------- */ /* ------------------------- configuration ----------------------------- */
void setCollisionPairs(const std::vector<CollisionPair>& pairs); void setCollisionPairs(const std::vector<CollisionPair>& pairs);
@ -110,9 +110,9 @@ namespace cmvr::ctrl{
} }
// ------------------------------ explicit instantiation ----------------------- // ------------------------------ explicit instantiation -----------------------
extern template class cmvr::ctrl::CartesianController<7>; extern template class cmvr::ctrl::QPSolver<7>;
extern template class cmvr::ctrl::CartesianController<14>; extern template class cmvr::ctrl::QPSolver<14>;
extern template class cmvr::ctrl::CartesianController<20>; extern template class cmvr::ctrl::QPSolver<20>;
#endif //CMVR_ES_CARTESIAN_CONTROLLER_H #endif //CMVR_ES_CARTESIAN_CONTROLLER_H

48
src/devices/robot/controller/cartesiancontroller.cpp
View File

@ -8,17 +8,59 @@ using namespace std;
using namespace cmvr::device; using namespace cmvr::device;
CartesianController::CartesianController(const XmlNode& cfg):AbstractController(cfg) 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) void CartesianController::call(const Json::Value& json)
{ {
if (state_ != ControllerState_Idle) try
return; {
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<std::string> 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() void CartesianController::interrupt()

4
src/devices/robot/controller/cartesiancontroller.h
View File

@ -4,6 +4,7 @@
#pragma once #pragma once
#include "abstractcontroller.h" #include "abstractcontroller.h"
#include "utils/solver/qp_solver.h"
namespace cmvr::device namespace cmvr::device
{ {
@ -15,6 +16,7 @@ namespace cmvr::device
void interrupt() override; void interrupt() override;
void stop() override; void stop() override;
private: private:
std::string base_link_;
std::string ee_link_;
}; };
} }

5
src/devices/robot/controller/jointpositioncontroller.cpp
View File

@ -11,7 +11,8 @@ using namespace cmvr::hardware;
JointPositionController::JointPositionController(const XmlNode& cfg):AbstractController(cfg) 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) 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) catch (const std::exception& e)
{ {

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

@ -30,7 +30,7 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
} }
m_state_ = m_robot_->MakeState(link_names_, joint_names_); m_state_ = m_robot_->MakeState(link_names_, joint_names_);
m_cctrl_ = make_shared<ctrl::CartesianController<DOF> >(m_robot_); m_cctrl_ = make_shared<ctrl::QPSolver<DOF> >(m_robot_);
upd_freq_ = cfg.getAttrDefault("updFreq", 500); upd_freq_ = cfg.getAttrDefault("updFreq", 500);
CSP_buffer_ = make_shared<SPMCRingBuffer<JointPoint> >(cfg.getAttrDefault("bufferSize", 50)); CSP_buffer_ = make_shared<SPMCRingBuffer<JointPoint> >(cfg.getAttrDefault("bufferSize", 50));
CSV_buffer_ = make_shared<SPMCRingBuffer<JointVelocityCommand> >(cfg.getAttrDefault("bufferSize", 50)); CSV_buffer_ = make_shared<SPMCRingBuffer<JointVelocityCommand> >(cfg.getAttrDefault("bufferSize", 50));
@ -397,7 +397,7 @@ void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &
// slove ik // slove ik
Eigen::Vector<double, DOF> q_cmd; Eigen::Vector<double, DOF> q_cmd;
bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002,
ctrl::CartesianController<DOF>::Mode::Position, ctrl::QPSolver<DOF>::Mode::Position,
q_cmd, 10000, 1e-6); q_cmd, 10000, 1e-6);
if (!ok) { if (!ok) {
throw runtime_error("solve IK failed"); throw runtime_error("solve IK failed");
@ -466,7 +466,7 @@ void HumanoidRobot<DOF>::moveJ_IK(const std::string &base_link, const std::vecto
// slove ik // slove ik
Eigen::Vector<double, DOF> q_cmd; Eigen::Vector<double, DOF> q_cmd;
bool ok = m_cctrl_->compute(m_state_, base_link, targets, 0.002, ctrl::CartesianController<DOF>::Mode::Position, bool ok = m_cctrl_->compute(m_state_, base_link, targets, 0.002, ctrl::QPSolver<DOF>::Mode::Position,
q_cmd, 10000, 1e-6); q_cmd, 10000, 1e-6);
if (!ok) { if (!ok) {
throw runtime_error("solve IK failed"); throw runtime_error("solve IK failed");
@ -603,7 +603,7 @@ void HumanoidRobot<DOF>::moveL(std::string &base_link, std::vector<cmvr::ctrl::P
// 求解逆运动学 // 求解逆运动学
Eigen::Vector<double, DOF> q_cmd; Eigen::Vector<double, DOF> q_cmd;
bool ok = m_cctrl_->compute(m_state_, base_link, interpolated_targets, 0.002, bool ok = m_cctrl_->compute(m_state_, base_link, interpolated_targets, 0.002,
ctrl::CartesianController<DOF>::Mode::Position, ctrl::QPSolver<DOF>::Mode::Position,
q_cmd, 10000, 1e-6); q_cmd, 10000, 1e-6);
if (!ok) { if (!ok) {
@ -959,7 +959,7 @@ void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection di
Eigen::Vector<double, DOF> q_next; Eigen::Vector<double, DOF> q_next;
bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD, bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD,
ctrl::CartesianController<DOF>::Mode::Position, ctrl::QPSolver<DOF>::Mode::Position,
q_next, 10000, 1e-6); q_next, 10000, 1e-6);
if (!ok) { if (!ok) {
@ -1160,7 +1160,7 @@ void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link,
base_link, // 基座链接 base_link, // 基座链接
first_pose_targets, // 第一个点的位姿目标 first_pose_targets, // 第一个点的位姿目标
dt, // 控制周期（用于速度限制） dt, // 控制周期（用于速度限制）
ctrl::CartesianController<DOF>::Mode::Position, // 位置控制模式 ctrl::QPSolver<DOF>::Mode::Position, // 位置控制模式
q_first, // 输出：第一个点的关节配置 q_first, // 输出：第一个点的关节配置
10000, // IK最大迭代次数（确保精度） 10000, // IK最大迭代次数（确保精度）
1e-6 // IK位置精度（1mm/0.001°） 1e-6 // IK位置精度（1mm/0.001°）
@ -1278,7 +1278,7 @@ void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link,
base_link, // 基座链接 base_link, // 基座链接
current_pose_targets, // 当前点的位姿目标 current_pose_targets, // 当前点的位姿目标
dt, // 控制周期 dt, // 控制周期
ctrl::CartesianController<DOF>::Mode::Position, ctrl::QPSolver<DOF>::Mode::Position,
q_cmd, // 输出：当前点的关节配置 q_cmd, // 输出：当前点的关节配置
5000, // 减少迭代次数（平衡精度与速度） 5000, // 减少迭代次数（平衡精度与速度）
5e-4 // IK精度：0.5mm/0.028°（轨迹执行可适当放宽） 5e-4 // IK精度：0.5mm/0.028°（轨迹执行可适当放宽）
@ -1386,7 +1386,7 @@ void HumanoidRobot<DOF>::servoJ(const std::string &base_link, const std::string
// slove ik // slove ik
Eigen::Vector<double, DOF> q_cmd; Eigen::Vector<double, DOF> q_cmd;
bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002,
ctrl::CartesianController<DOF>::Mode::Position, ctrl::QPSolver<DOF>::Mode::Position,
q_cmd, 10000, 1e-6); q_cmd, 10000, 1e-6);
if (!ok) { if (!ok) {
throw runtime_error("solve IK failed"); throw runtime_error("solve IK failed");
@ -1433,7 +1433,7 @@ template<int DOF>
void HumanoidRobot<DOF>::servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) { void HumanoidRobot<DOF>::servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) {
try { try {
Eigen::Vector<double, DOF> q_cmd; Eigen::Vector<double, DOF> q_cmd;
bool ok = m_cctrl_->compute(m_state_, base_link, targets, 1, ctrl::CartesianController<DOF>::Mode::Position, bool ok = m_cctrl_->compute(m_state_, base_link, targets, 1, ctrl::QPSolver<DOF>::Mode::Position,
q_cmd, 60, 1e-4); q_cmd, 60, 1e-4);
if (!ok) { if (!ok) {
LOG(WARNING) << "[HumanoidRobot] (servoL): solve IK failed, id=" << id_; LOG(WARNING) << "[HumanoidRobot] (servoL): solve IK failed, id=" << id_;
@ -1575,7 +1575,7 @@ std::vector<double> HumanoidRobot<
// slove ik // slove ik
Eigen::Vector<double, DOF> q_cmd{}; Eigen::Vector<double, DOF> q_cmd{};
bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002,
ctrl::CartesianController<DOF>::Mode::Position, ctrl::QPSolver<DOF>::Mode::Position,
q_cmd, 10000, 1e-6); q_cmd, 10000, 1e-6);
if (!ok) { if (!ok) {
throw std::runtime_error("IK solve failed"); throw std::runtime_error("IK solve failed");
@ -1765,7 +1765,7 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
Eigen::Vector<double, DOF> q_cmd_check; Eigen::Vector<double, DOF> q_cmd_check;
bool ik_solvable = m_cctrl_->compute(m_state_, base_link, {target_ik_check}, 0.002, bool ik_solvable = m_cctrl_->compute(m_state_, base_link, {target_ik_check}, 0.002,
ctrl::CartesianController<DOF>::Mode::Position, ctrl::QPSolver<DOF>::Mode::Position,
q_cmd_check, 10000, 1e-6); q_cmd_check, 10000, 1e-6);
if (!ik_solvable) { if (!ik_solvable) {
throw std::runtime_error("moveL: Target pose is unreachable with constant orientation"); throw std::runtime_error("moveL: Target pose is unreachable with constant orientation");
@ -1827,7 +1827,7 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
// 使用前一点的位置作为初始值求解IK // 使用前一点的位置作为初始值求解IK
Eigen::Vector<double, DOF> q_next; Eigen::Vector<double, DOF> q_next;
bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD, bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD,
ctrl::CartesianController<DOF>::Mode::Position, ctrl::QPSolver<DOF>::Mode::Position,
q_next, 10000, 1e-6); q_next, 10000, 1e-6);
if (!ok) { if (!ok) {

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

@ -151,7 +151,7 @@ namespace cmvr::device{
std::shared_ptr<cmvr::dyn::State<DOF>> m_state_; std::shared_ptr<cmvr::dyn::State<DOF>> m_state_;
std::shared_ptr<cmvr::dyn::Robot<DOF>> m_robot_; std::shared_ptr<cmvr::dyn::Robot<DOF>> m_robot_;
std::shared_ptr<cmvr::ctrl::CartesianController<DOF>> m_cctrl_; std::shared_ptr<cmvr::ctrl::QPSolver<DOF>> m_cctrl_;
std::vector<std::string> joint_names_; std::vector<std::string> joint_names_;
std::vector<std::string> link_names_; std::vector<std::string> link_names_;

26
src/utils/solver/qp_solver.cpp
View File

@ -3,7 +3,7 @@
using namespace cmvr::ctrl; using namespace cmvr::ctrl;
template<int DOF> template<int DOF>
CartesianController<DOF>::CartesianController(std::shared_ptr<RobotT> robot, double dsafe, double lambda) QPSolver<DOF>::QPSolver(std::shared_ptr<RobotT> robot, double dsafe, double lambda)
: robot_(std::move(robot)), d_safe_(dsafe), lambda_(lambda), solver_() : robot_(std::move(robot)), d_safe_(dsafe), lambda_(lambda), solver_()
{ {
dist_req_.enable_nearest_points = true; dist_req_.enable_nearest_points = true;
@ -11,13 +11,13 @@ CartesianController<DOF>::CartesianController(std::shared_ptr<RobotT> robot, dou
// === configuration setters ================================================== // === configuration setters ==================================================
template<int DOF> template<int DOF>
void CartesianController<DOF>::setCollisionPairs(const std::vector<CollisionPair>& p) void QPSolver<DOF>::setCollisionPairs(const std::vector<CollisionPair>& p)
{ {
pairs_ = p; pairs_ = p;
} }
template<int DOF> template<int DOF>
void CartesianController<DOF>::setPositionLimits(const std::vector<std::optional<double>>& qmin, void QPSolver<DOF>::setPositionLimits(const std::vector<std::optional<double>>& qmin,
const std::vector<std::optional<double>>& qmax) const std::vector<std::optional<double>>& qmax)
{ {
if (qmin.size()==DOF) q_min_ = qmin; if (qmin.size()==DOF) q_min_ = qmin;
@ -25,13 +25,13 @@ void CartesianController<DOF>::setPositionLimits(const std::vector<std::optional
} }
template<int DOF> template<int DOF>
void CartesianController<DOF>::setVelocityLimits(const std::vector<std::optional<double>>& qdmax) void QPSolver<DOF>::setVelocityLimits(const std::vector<std::optional<double>>& qdmax)
{ {
if (qdmax.size()==DOF) qd_max_ = qdmax; if (qdmax.size()==DOF) qd_max_ = qdmax;
} }
template<int DOF> template<int DOF>
void CartesianController<DOF>::setAccelerationLimits(const std::vector<std::optional<double>>& qddmax) void QPSolver<DOF>::setAccelerationLimits(const std::vector<std::optional<double>>& qddmax)
{ {
if (qddmax.size()==DOF) qdd_max_ = qddmax; if (qddmax.size()==DOF) qdd_max_ = qddmax;
} }
@ -39,7 +39,7 @@ void CartesianController<DOF>::setAccelerationLimits(const std::vector<std::opti
// === helpers =============================================================== // === helpers ===============================================================
template<int DOF> template<int DOF>
inline void CartesianController<DOF>::clampVec( inline void QPSolver<DOF>::clampVec(
VecD& v, const std::vector<std::optional<double>>& lo,const std::vector<std::optional<double>>& hi) const VecD& v, const std::vector<std::optional<double>>& lo,const std::vector<std::optional<double>>& hi) const
{ {
for (int i=0;i<DOF;++i) { for (int i=0;i<DOF;++i) {
@ -51,7 +51,7 @@ inline void CartesianController<DOF>::clampVec(
// === main compute =========================================================== // === main compute ===========================================================
template<int DOF> template<int DOF>
bool CartesianController<DOF>::compute( bool QPSolver<DOF>::compute(
std::shared_ptr<cmvr::dyn::State<DOF>>& state, std::shared_ptr<cmvr::dyn::State<DOF>>& state,
const std::string& base_link, const std::string& base_link,
const std::vector<PoseTarget>& targets, const std::vector<PoseTarget>& targets,
@ -114,7 +114,7 @@ bool CartesianController<DOF>::compute(
// === solveIK ============================================================== // === solveIK ==============================================================
template<int DOF> template<int DOF>
bool CartesianController<DOF>::solveIK( bool QPSolver<DOF>::solveIK(
std::shared_ptr<cmvr::dyn::State<DOF>>& state, std::shared_ptr<cmvr::dyn::State<DOF>>& state,
const std::string& base_link, const std::string& base_link,
const std::vector<PoseTarget>& targets, const std::vector<PoseTarget>& targets,
@ -198,7 +198,7 @@ bool CartesianController<DOF>::solveIK(
try { try {
dq = solver_.Solve(); dq = solver_.Solve();
} catch (const std::exception& e) { } 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; return false;
} }
@ -218,7 +218,7 @@ bool CartesianController<DOF>::solveIK(
// === distanceConstraint ===================================================== // === distanceConstraint =====================================================
template<int DOF> template<int DOF>
bool CartesianController<DOF>::distanceConstraint( bool QPSolver<DOF>::distanceConstraint(
const CollisionPair& cp, const CollisionPair& cp,
std::shared_ptr<cmvr::dyn::State<DOF>>& state, std::shared_ptr<cmvr::dyn::State<DOF>>& state,
const std::vector<std::shared_ptr<fcl::CollisionObjectd>>& objs, const std::vector<std::shared_ptr<fcl::CollisionObjectd>>& objs,
@ -258,6 +258,6 @@ bool CartesianController<DOF>::distanceConstraint(
return true; // 需要加入不等式 return true; // 需要加入不等式
} }
template class cmvr::ctrl::CartesianController<7>; template class cmvr::ctrl::QPSolver<7>;
template class cmvr::ctrl::CartesianController<14>; template class cmvr::ctrl::QPSolver<14>;
template class cmvr::ctrl::CartesianController<20>; template class cmvr::ctrl::QPSolver<20>;