From 2c97319c9be45f07d2934275ea64923345c31d82 Mon Sep 17 00:00:00 2001
From: linbo <1034003879@qq.com>
Date: Wed, 22 Oct 2025 11:01:27 +0800
Subject: [PATCH] update moveJ limitQ
---
config/cabin_robot.xml | 32 +++++++++----------
.../robot/humanoid_robot/humanoid_robot.cpp | 26 +++++++++++++--
2 files changed, 40 insertions(+), 18 deletions(-)
diff --git a/config/cabin_robot.xml b/config/cabin_robot.xml
index 54ae96de..ce6e0a77 100644
--- a/config/cabin_robot.xml
+++ b/config/cabin_robot.xml
@@ -41,31 +41,31 @@
verbose="false">
-
-
-
-
-
-
-
+
+
+
+
+
+
+
-
-
-
-
+
+
+
+
-
-
-
+
+
+
-
-
+
+
diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp
index 54a66a79..2c7933a9 100644
--- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp
+++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp
@@ -3,6 +3,9 @@
//
#include "humanoid_robot.h"
+
+#include
+
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
#include "motor/ti5_motor/ti5_motor.h"
#include "utils/base/abstract_interpolation.h"
@@ -315,7 +318,16 @@ void HumanoidRobot::moveJ(std::vector &cmd, double vel, double
if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
}
- motor->setQ(j.rad);
+ auto cfg = motor->getConfig();
+ double limitQLb = cfg.getAttrDefault("limitQLb",(float)3.14);
+ double limitQUb = cfg.getAttrDefault("limitQUb",(float)3.14);
+ double clamped_rad = j.rad;
+ if (clamped_rad > limitQUb) {
+ clamped_rad = limitQUb; // 正向超限，限制到正向最大
+ } else if (clamped_rad < limitQLb) {
+ clamped_rad = limitQLb; // 反向超限，限制到反向最大
+ }
+ motor->setQ(clamped_rad);
}
}
@@ -403,7 +415,17 @@ void HumanoidRobot::moveJ(const std::string &base_link, const std::string &
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
}
motor->setQd(vel);
- motor->setQ(j.rad);
+
+ auto cfg = motor->getConfig();
+ double limitQLb = cfg.getAttrDefault("limitQLb",(float)3.14);
+ double limitQUb = cfg.getAttrDefault("limitQUb",(float)3.14);
+ double clamped_rad = j.rad;
+ if (clamped_rad > limitQUb) {
+ clamped_rad = limitQUb; // 正向超限，限制到正向最大
+ } else if (clamped_rad < limitQLb) {
+ clamped_rad = limitQLb; // 反向超限，限制到反向最大
+ }
+ motor->setQ(clamped_rad);
}
}