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

fix(touch-screen): move to init pose before touch start

Browse Source
This commit is contained in:
lgv 2026-06-24 17:16:04 +08:00
parent 181e14c7ef
commit 9deb6c5383
2 changed files with 32 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cmvr-es/task/touch_screen_task/include/touch_screen_task.h
View File

@ -118,6 +118,7 @@ private:
void hardStopIbvsMotion(); void hardStopIbvsMotion();
bool holdCurrentControlledPosition() const; bool holdCurrentControlledPosition() const;
bool buildInitJointPositions(std::vector<double>& positions_out) const; bool buildInitJointPositions(std::vector<double>& positions_out) const;
bool moveToInitPositionBeforeStartIfEnabled();
bool moveToInitPositionIfEnabled() const; bool moveToInitPositionIfEnabled() const;
bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const; bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const;
void logTouchingSpeedLState() const; void logTouchingSpeedLState() const;

50
cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp
View File

@ -323,25 +323,6 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
return false; return false;
} }
if (config_.initialization().before_start()) {
std::vector<double> init_positions;
if (!buildInitJointPositions(init_positions)) {
initialized_ = false;
last_status_ = Status::INVALID_CONFIG;
return false;
}
device::JointPositionCommand init_cmd{init_positions};
device::MotionOptions options;
options.velocity = config_.initialization().velocity();
options.acceleration = config_.initialization().acceleration();
const auto result = arm_->moveJ(init_cmd, options);
if (!result.ok()) {
initialized_ = false;
last_status_ = Status::ROBOT_COMMAND_FAILED;
return false;
}
}
if (config_.alignment().ibvs().camera_link().empty() || if (config_.alignment().ibvs().camera_link().empty() ||
config_.alignment().ibvs().control_joint_names().empty()) { config_.alignment().ibvs().control_joint_names().empty()) {
initialized_ = false; initialized_ = false;
@ -425,6 +406,10 @@ bool TouchScreenTask::startFromPixelUnlocked(int u, int v) {
} }
stopUnlocked(); stopUnlocked();
if (!moveToInitPositionBeforeStartIfEnabled()) {
return false;
}
tracker_.clear(); tracker_.clear();
tracker_.resetActiveTagTracking(); tracker_.resetActiveTagTracking();
ibvs_.reset(); ibvs_.reset();
@ -1504,6 +1489,33 @@ bool TouchScreenTask::buildInitJointPositions(std::vector<double>& positions_out
return buildInitJointPositionsFromConfig(config_, positions_out); return buildInitJointPositionsFromConfig(config_, positions_out);
} }
bool TouchScreenTask::moveToInitPositionBeforeStartIfEnabled() {
if (!config_.initialization().before_start()) {
return true;
}
if (!arm_) {
last_status_ = Status::ROBOT_COMMAND_FAILED;
return false;
}
std::vector<double> init_positions;
if (!buildInitJointPositions(init_positions)) {
last_status_ = Status::INVALID_CONFIG;
return false;
}
device::JointPositionCommand init_cmd{init_positions};
device::MotionOptions options;
options.velocity = config_.initialization().velocity();
options.acceleration = config_.initialization().acceleration();
const auto result = arm_->moveJ(init_cmd, options);
if (!result.ok()) {
last_status_ = Status::ROBOT_COMMAND_FAILED;
return false;
}
return true;
}
bool TouchScreenTask::moveToInitPositionIfEnabled() const { bool TouchScreenTask::moveToInitPositionIfEnabled() const {
if (!config_.initialization().after_finish()) { if (!config_.initialization().after_finish()) {
return true; return true;