diff --git a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h index 8319bd1e..6e284d7e 100644 --- a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h +++ b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h @@ -118,6 +118,7 @@ private: void hardStopIbvsMotion(); bool holdCurrentControlledPosition() const; bool buildInitJointPositions(std::vector& positions_out) const; + bool moveToInitPositionBeforeStartIfEnabled(); bool moveToInitPositionIfEnabled() const; bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const; void logTouchingSpeedLState() const; diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp index f0a4f419..1c3d2e6c 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp @@ -323,25 +323,6 @@ bool TouchScreenTask::init(const std::shared_ptr& arm, return false; } - if (config_.initialization().before_start()) { - std::vector 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() || config_.alignment().ibvs().control_joint_names().empty()) { initialized_ = false; @@ -425,6 +406,10 @@ bool TouchScreenTask::startFromPixelUnlocked(int u, int v) { } stopUnlocked(); + if (!moveToInitPositionBeforeStartIfEnabled()) { + return false; + } + tracker_.clear(); tracker_.resetActiveTagTracking(); ibvs_.reset(); @@ -1504,6 +1489,33 @@ bool TouchScreenTask::buildInitJointPositions(std::vector& 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 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 { if (!config_.initialization().after_finish()) { return true;