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

fix report arm freedrive control mode

Browse Source
This commit is contained in:
xtkuang 2026-09-23 16:04:01 +08:00
parent 51bdbb9776
commit 9b2f5773e6
4 changed files with 63 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h
View File

@ -34,7 +34,7 @@ public:
CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
RobotMode getRobotMode() const override;
SafetyMode getSafetyMode() const override;
ControlMode getControlMode() const override { return ControlMode::Position; }
ControlMode getControlMode() const override;
bool supportsActionQueueMotion() const noexcept override { return true; }
Result listBaseFrame(std::vector<std::string>& frame_names) const override;
Result listTCPFrame(std::vector<std::string>& frame_names) const override;

33
cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp
View File

@ -733,6 +733,39 @@ SafetyMode AuboArm::getSafetyMode() const
return static_cast<SafetyMode>(hardware_safety_mode_.load());
}
ControlMode AuboArm::getControlMode() const
{
if (!connected_.load()) {
return ControlMode::None;
}
#if defined(CMVR_HAS_AUBO_SDK)
try {
const auto robot_names = sdk_->rpc_client->getRobotNames();
if (robot_names.empty()) {
return ControlMode::Position;
}
const auto robot_interface =
sdk_->rpc_client->getRobotInterface(robot_names.front());
if (!robot_interface) {
return ControlMode::Position;
}
const auto robot_state = robot_interface->getRobotState();
if (robot_state &&
robot_state->getRobotModeType() ==
arcs::common_interface::RobotModeType::BackDrive) {
return ControlMode::Freedrive;
}
} catch (const std::exception&) {
// Keep telemetry available when a controller version does not support
// one of the optional hand-guiding status queries.
}
#endif
return ControlMode::Position;
}
bool AuboArm::isEmergencyStopped() const
{
return emergency_stopped_.load() || hardware_emergency_stopped_.load();

28
cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp
View File

@ -283,6 +283,34 @@ Result HuayanRobot::setSpeedScaling(const double scaling)
return Result::success();
}
ControlMode HuayanRobot::getControlMode() const
{
if (!isConnected()) {
return ControlMode::None;
}
int tri_stage_enabled = 0;
int tri_stage_mode = 1;
int force_control_state = 0;
int tri_stage_result = -1;
int force_state_result = -1;
{
std::lock_guard<std::mutex> lock(mutex_);
tri_stage_result = HRIF_ReadTriStageSwitch(
box_id_, robot_id_, tri_stage_enabled, tri_stage_mode);
force_state_result = HRIF_ReadForceControlState(
box_id_, robot_id_, force_control_state);
}
const bool tri_stage_freedrive =
tri_stage_result == 0 && tri_stage_enabled != 0 && tri_stage_mode == 0;
const bool force_freedrive =
force_state_result == 0 && force_control_state == 3;
if (tri_stage_freedrive || force_freedrive) {
return ControlMode::Freedrive;
}
return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position;
}
bool HuayanRobot::isProtectiveStopped() const
{
const auto state = readHrState_();

2
cmvr-es/devices/arm/huayan_arm/huayan_arm.h
View File

@ -38,7 +38,7 @@ public:
CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
RobotMode getRobotMode() const override;
SafetyMode getSafetyMode() const override;
ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
ControlMode getControlMode() const override;
bool supportsActionQueueMotion() const noexcept override { return true; }
Result listBaseFrame(std::vector<std::string>& frame_names) const override;
Result listTCPFrame(std::vector<std::string>& frame_names) const override;