2170 lines
79 KiB
C++
2170 lines
79 KiB
C++
#include "task/touch_screen_task/include/touch_screen_task.h"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <exception>
|
|
#include <limits>
|
|
#include <sstream>
|
|
#include <unordered_map>
|
|
|
|
#include "common/base/logging/logger.h"
|
|
#include "common/math/proto_geometry.h"
|
|
#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h"
|
|
#include "cmvr/config/touch_screen_algorithm_config.pb.h"
|
|
#include "manager/device_manager/include/device_manager.h"
|
|
#include "service/grpc/server/include/camera_operational_activity_registry.h"
|
|
#include "service/grpc/stop_all/include/stop_all_admission_gate.h"
|
|
#include <visp3/core/vpRotationMatrix.h>
|
|
|
|
namespace cmvr::task {
|
|
|
|
namespace {
|
|
|
|
device::CartesianVelocity toCartesianVelocity(const Eigen::Matrix<double, 6, 1>& twist) {
|
|
return {twist[0], twist[1], twist[2], twist[3], twist[4], twist[5]};
|
|
}
|
|
|
|
Eigen::Matrix<double, 6, 1> toEigen6(const device::CartesianVelocity& twist) {
|
|
Eigen::Matrix<double, 6, 1> out;
|
|
out << twist.vx, twist.vy, twist.vz, twist.wx, twist.wy, twist.wz;
|
|
return out;
|
|
}
|
|
|
|
bool computeLinearMoveDeltaTool(const Eigen::Matrix<double, 6, 1>& twist_base,
|
|
const double move_length,
|
|
Eigen::Vector3d& delta_out) {
|
|
if (!std::isfinite(move_length) || move_length <= 0.0) {
|
|
return false;
|
|
}
|
|
|
|
const Eigen::Vector3d linear = twist_base.head<3>();
|
|
if (!linear.allFinite()) {
|
|
return false;
|
|
}
|
|
|
|
const double linear_norm = linear.norm();
|
|
if (!std::isfinite(linear_norm) || linear_norm <= 1e-9) {
|
|
return false;
|
|
}
|
|
|
|
delta_out = linear / linear_norm * move_length;
|
|
return delta_out.allFinite();
|
|
}
|
|
|
|
using TouchScreenTaskConfig = cmvr::config::TouchScreenTaskConfig;
|
|
|
|
std::array<double, 6> toArray6(const Eigen::Matrix<double, 6, 1>& value) {
|
|
return {{value[0], value[1], value[2], value[3], value[4], value[5]}};
|
|
}
|
|
|
|
|
|
|
|
std::vector<std::string> controlJointNames(const TouchScreenTaskConfig& config) {
|
|
const auto& names = config.alignment().ibvs().control_joint_names();
|
|
return {names.begin(), names.end()};
|
|
}
|
|
|
|
bool buildInitJointPositionsFromConfig(const TouchScreenTaskConfig& config,
|
|
std::vector<double>& positions_out) {
|
|
std::unordered_map<std::string, double> q_map;
|
|
q_map.reserve(static_cast<size_t>(config.initialization().joint_positions_size()));
|
|
for (const auto& joint : config.initialization().joint_positions()) {
|
|
if (!joint.has_joint_name() || joint.joint_name().empty() || !joint.has_rad() ||
|
|
!std::isfinite(joint.rad())) {
|
|
return false;
|
|
}
|
|
q_map[joint.joint_name()] = joint.rad();
|
|
}
|
|
|
|
positions_out.clear();
|
|
positions_out.reserve(static_cast<size_t>(
|
|
config.alignment().ibvs().control_joint_names_size()));
|
|
for (const auto& name : config.alignment().ibvs().control_joint_names()) {
|
|
const auto it = q_map.find(name);
|
|
if (it == q_map.end()) {
|
|
return false;
|
|
}
|
|
positions_out.push_back(it->second);
|
|
}
|
|
return !positions_out.empty();
|
|
}
|
|
|
|
bool isTouchTriggered(const TouchScreenTaskConfig& config,
|
|
const double resultant_force_value) {
|
|
return resultant_force_value >= config.touch().tactile().force_threshold();
|
|
}
|
|
|
|
double tactileForceValue(const device::AbstractDexHand::TactilePoint& point,
|
|
const cmvr::config::TouchScreenTactileCriterion criterion) {
|
|
switch (criterion) {
|
|
case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_FZ:
|
|
return static_cast<double>(point.fz);
|
|
case cmvr::config::TOUCH_SCREEN_TACTILE_CRITERION_MAGNITUDE:
|
|
return point.magnitude();
|
|
}
|
|
return static_cast<double>(point.fz);
|
|
}
|
|
|
|
Eigen::Matrix3d rotationFromTargetRotvec(double rx, double ry, double rz) {
|
|
vpRotationMatrix R_visp;
|
|
R_visp.buildFrom(rx, ry, rz);
|
|
|
|
Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
|
|
for (int r = 0; r < 3; ++r) {
|
|
for (int c = 0; c < 3; ++c) {
|
|
R(r, c) = R_visp[r][c];
|
|
}
|
|
}
|
|
return R;
|
|
}
|
|
|
|
double rotationErrorRad(const Eigen::Matrix3d& R_current,
|
|
const Eigen::Matrix3d& R_target) {
|
|
if (!R_current.allFinite() || !R_target.allFinite()) {
|
|
return std::numeric_limits<double>::infinity();
|
|
}
|
|
|
|
const Eigen::Matrix3d R_err = R_current * R_target.transpose();
|
|
const double cos_angle = std::clamp(0.5 * (R_err.trace() - 1.0), -1.0, 1.0);
|
|
return std::acos(cos_angle);
|
|
}
|
|
|
|
Eigen::Vector3d rotvecFromRotationMatrix(const Eigen::Matrix3d& R) {
|
|
const Eigen::AngleAxisd aa(R);
|
|
if (!std::isfinite(aa.angle()) || !aa.axis().allFinite() || std::abs(aa.angle()) <= 1e-12) {
|
|
return Eigen::Vector3d::Zero();
|
|
}
|
|
return aa.axis() * aa.angle();
|
|
}
|
|
|
|
bool extractProjectedYawAboutTargetNormal(const Eigen::Matrix3d& R_target,
|
|
const Eigen::Matrix3d& R_current,
|
|
double& yaw_rad_out) {
|
|
if (!R_target.allFinite() || !R_current.allFinite()) {
|
|
return false;
|
|
}
|
|
|
|
const Eigen::Vector3d z_ref = R_target.col(2);
|
|
const Eigen::Vector3d x_ref = R_target.col(0);
|
|
const Eigen::Vector3d y_ref = R_target.col(1);
|
|
|
|
Eigen::Vector3d in_plane = R_current.col(0) - z_ref * z_ref.dot(R_current.col(0));
|
|
if (in_plane.norm() <= 1e-9) {
|
|
in_plane = R_current.col(1) - z_ref * z_ref.dot(R_current.col(1));
|
|
}
|
|
const double in_plane_norm = in_plane.norm();
|
|
if (!std::isfinite(in_plane_norm) || in_plane_norm <= 1e-9) {
|
|
return false;
|
|
}
|
|
|
|
in_plane /= in_plane_norm;
|
|
yaw_rad_out = std::atan2(y_ref.dot(in_plane), x_ref.dot(in_plane));
|
|
return std::isfinite(yaw_rad_out);
|
|
}
|
|
|
|
bool appendRequestedTactileRegions(
|
|
const device::AbstractDexHand::FingerType finger,
|
|
const device::AbstractDexHand::TactileRegion region,
|
|
std::vector<device::AbstractDexHand::TactileRegionKey>& regions_out) {
|
|
using DeviceTactileRegion = device::AbstractDexHand::TactileRegion;
|
|
|
|
switch (region) {
|
|
case device::AbstractDexHand::TactileRegion::TIP:
|
|
regions_out.emplace_back(finger, DeviceTactileRegion::TIP);
|
|
return true;
|
|
case device::AbstractDexHand::TactileRegion::FINGER:
|
|
regions_out.emplace_back(finger, DeviceTactileRegion::FINGER);
|
|
return true;
|
|
case device::AbstractDexHand::TactileRegion::PAD:
|
|
regions_out.emplace_back(finger, DeviceTactileRegion::PAD);
|
|
return true;
|
|
case device::AbstractDexHand::TactileRegion::THUMB_MIDDLE:
|
|
if (finger != device::AbstractDexHand::FingerType::THUMB) {
|
|
return false;
|
|
}
|
|
regions_out.emplace_back(finger, DeviceTactileRegion::THUMB_MIDDLE);
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
perception::AprilTagPerception::DepthPolicy toDepthPolicy(
|
|
const cmvr::config::TouchScreenDepthPolicy policy) {
|
|
switch (policy) {
|
|
case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_NONE:
|
|
return perception::AprilTagPerception::DepthPolicy::NONE;
|
|
case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_PREFER:
|
|
return perception::AprilTagPerception::DepthPolicy::PREFER;
|
|
case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_REQUIRE:
|
|
return perception::AprilTagPerception::DepthPolicy::REQUIRE;
|
|
}
|
|
return perception::AprilTagPerception::DepthPolicy::NONE;
|
|
}
|
|
|
|
perception::TagRelativeTarget3D::TargetPointMethod toTargetPointMethod(
|
|
const cmvr::config::TouchScreenTargetPointMethod method) {
|
|
switch (method) {
|
|
case cmvr::config::TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE:
|
|
return perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE;
|
|
case cmvr::config::TOUCH_SCREEN_TARGET_POINT_METHOD_DEPTH_IMAGE:
|
|
return perception::TagRelativeTarget3D::TargetPointMethod::DEPTH_IMAGE;
|
|
}
|
|
return perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE;
|
|
}
|
|
|
|
device::AbstractDexHand::FingerType toFingerType(const cmvr::config::TouchScreenFingerType finger) {
|
|
switch (finger) {
|
|
case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_PINKY:
|
|
return device::AbstractDexHand::FingerType::PINKY;
|
|
case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_RING:
|
|
return device::AbstractDexHand::FingerType::RING;
|
|
case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_MIDDLE:
|
|
return device::AbstractDexHand::FingerType::MIDDLE;
|
|
case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_INDEX:
|
|
return device::AbstractDexHand::FingerType::INDEX;
|
|
case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_THUMB:
|
|
return device::AbstractDexHand::FingerType::THUMB;
|
|
}
|
|
return device::AbstractDexHand::FingerType::INDEX;
|
|
}
|
|
|
|
const char* fingerTypeToString(const device::AbstractDexHand::FingerType finger) {
|
|
switch (finger) {
|
|
case device::AbstractDexHand::FingerType::PINKY: return "PINKY";
|
|
case device::AbstractDexHand::FingerType::RING: return "RING";
|
|
case device::AbstractDexHand::FingerType::MIDDLE: return "MIDDLE";
|
|
case device::AbstractDexHand::FingerType::INDEX: return "INDEX";
|
|
case device::AbstractDexHand::FingerType::THUMB: return "THUMB";
|
|
case device::AbstractDexHand::FingerType::PALM: return "PALM";
|
|
}
|
|
return "UNKNOWN";
|
|
}
|
|
|
|
const char* tactileRegionToString(const device::AbstractDexHand::TactileRegion region) {
|
|
switch (region) {
|
|
case device::AbstractDexHand::TactileRegion::TIP: return "TIP";
|
|
case device::AbstractDexHand::TactileRegion::FINGER: return "FINGER";
|
|
case device::AbstractDexHand::TactileRegion::PAD: return "PAD";
|
|
case device::AbstractDexHand::TactileRegion::THUMB_MIDDLE: return "THUMB_MIDDLE";
|
|
case device::AbstractDexHand::TactileRegion::PALM_PAD: return "PALM_PAD";
|
|
}
|
|
return "UNKNOWN";
|
|
}
|
|
|
|
device::AbstractDexHand::TactileRegion toTactileRegion(
|
|
const cmvr::config::TouchScreenTactileRegion region) {
|
|
switch (region) {
|
|
case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_TIP:
|
|
return device::AbstractDexHand::TactileRegion::TIP;
|
|
case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_FINGER:
|
|
return device::AbstractDexHand::TactileRegion::FINGER;
|
|
case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_PAD:
|
|
return device::AbstractDexHand::TactileRegion::PAD;
|
|
case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE:
|
|
return device::AbstractDexHand::TactileRegion::THUMB_MIDDLE;
|
|
}
|
|
return device::AbstractDexHand::TactileRegion::TIP;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
TouchScreenTask::TouchScreenTask(const cmvr::config::TouchScreenTaskConfig& cfg)
|
|
: id_(cfg.id()),
|
|
tracker_(nullptr),
|
|
config_(cfg) {
|
|
config_valid_ = validateConfig(config_);
|
|
if (!config_valid_) {
|
|
last_status_ = Status::INVALID_CONFIG;
|
|
}
|
|
}
|
|
|
|
bool TouchScreenTask::init() {
|
|
auto& admission_gate = service::globalStopAllAdmissionGate();
|
|
std::uint64_t admission_generation = 0U;
|
|
{
|
|
auto admission = admission_gate.lockAdmission();
|
|
if (!admission.accepting()) {
|
|
last_status_ = Status::NOT_INITIALIZED;
|
|
return false;
|
|
}
|
|
admission_generation = admission.generation();
|
|
}
|
|
|
|
if (!config_valid_) {
|
|
last_status_ = Status::INVALID_CONFIG;
|
|
return false;
|
|
}
|
|
|
|
const auto& devices = config_.devices();
|
|
if (!devices.has_arm_id() || devices.arm_id().empty() ||
|
|
!devices.has_camera_id() || devices.camera_id().empty() ||
|
|
!devices.has_dexhand_id() || devices.dexhand_id().empty()) {
|
|
last_status_ = Status::INVALID_CONFIG;
|
|
return false;
|
|
}
|
|
|
|
auto& dm = device::DeviceManager::getInstance();
|
|
auto arm = dm.getDevice<device::RobotArm>(devices.arm_id());
|
|
auto dexhand = dm.getDevice<device::AbstractDexHand>(devices.dexhand_id());
|
|
auto camera = dm.getDevice<device::AbstractCamera>(devices.camera_id());
|
|
if (!camera) {
|
|
CMVR_LOG(ERROR) << "[TouchScreenTask] Failed to start camera: " << devices.camera_id();
|
|
last_status_ = Status::NOT_INITIALIZED;
|
|
return false;
|
|
}
|
|
|
|
auto& camera_registry =
|
|
service::globalCameraOperationalActivityRegistry();
|
|
service::CameraOperationalActivityRegistry::ActivityToken camera_token;
|
|
service::CameraOperationalActivityRegistry::DispatchResult camera_start;
|
|
try {
|
|
camera_start = camera_registry.start(
|
|
devices.camera_id(), camera, &camera_token);
|
|
} catch (...) {
|
|
last_status_ = Status::NOT_INITIALIZED;
|
|
return false;
|
|
}
|
|
if (camera_start != service::CameraOperationalActivityRegistry::
|
|
DispatchResult::Success) {
|
|
CMVR_LOG(ERROR) << "[TouchScreenTask] Failed to start camera: "
|
|
<< devices.camera_id();
|
|
last_status_ = Status::NOT_INITIALIZED;
|
|
return false;
|
|
}
|
|
|
|
const auto admission_current = [&] {
|
|
auto admission = admission_gate.lockAdmission();
|
|
return admission.accepting() &&
|
|
admission.generation() == admission_generation;
|
|
};
|
|
const auto rollback_camera = [&] {
|
|
if (camera_registry.stopIfCurrent(camera_token)) {
|
|
return;
|
|
}
|
|
const auto ticket = admission_gate.beginStopAll();
|
|
(void)admission_gate.finishStopAll(ticket, false);
|
|
};
|
|
const auto mark_interrupted = [this] {
|
|
std::lock_guard lock(mutex_);
|
|
initialized_ = false;
|
|
last_status_ = Status::NOT_INITIALIZED;
|
|
};
|
|
|
|
if (!admission_current()) {
|
|
rollback_camera();
|
|
mark_interrupted();
|
|
return false;
|
|
}
|
|
|
|
bool initialized = false;
|
|
try {
|
|
initialized = init(arm, dexhand, camera);
|
|
} catch (...) {
|
|
rollback_camera();
|
|
throw;
|
|
}
|
|
if (!initialized) {
|
|
rollback_camera();
|
|
return false;
|
|
}
|
|
if (admission_current()) {
|
|
return true;
|
|
}
|
|
|
|
rollback_camera();
|
|
mark_interrupted();
|
|
return false;
|
|
}
|
|
|
|
bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
|
|
const std::shared_ptr<device::AbstractDexHand>& dexhand,
|
|
const std::shared_ptr<device::AbstractCamera>& camera) {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
arm_ = arm;
|
|
{
|
|
std::lock_guard arm_lock(activity_arm_mutex_);
|
|
activity_arm_ = arm;
|
|
}
|
|
dexhand_ = dexhand;
|
|
camera_ = camera;
|
|
|
|
if (!config_valid_ || !arm_ || !camera_) {
|
|
initialized_ = false;
|
|
last_status_ = Status::INVALID_CONFIG;
|
|
return false;
|
|
}
|
|
|
|
if ((config_.initialization().before_start() || config_.initialization().after_finish()) &&
|
|
config_.initialization().joint_positions().empty()) {
|
|
initialized_ = false;
|
|
last_status_ = Status::INVALID_CONFIG;
|
|
return false;
|
|
}
|
|
|
|
if (config_.alignment().ibvs().camera_link().empty() ||
|
|
config_.alignment().ibvs().control_joint_names().empty()) {
|
|
initialized_ = false;
|
|
last_status_ = Status::INVALID_CONFIG;
|
|
return false;
|
|
}
|
|
|
|
perception_ = std::make_shared<perception::AprilTagPerception>(camera_);
|
|
perception_->setTagSize(config_.perception().apriltag().tag_size_m());
|
|
|
|
tracker_.setPerception(perception_);
|
|
tracker_.setTargetPointMethod(toTargetPointMethod(
|
|
config_.perception().apriltag().target_point_method()));
|
|
|
|
auto pinocchio_solver =
|
|
std::dynamic_pointer_cast<cmvr::PinocchioIKBase>(arm_->kinematicsSolver());
|
|
if (!pinocchio_solver ||
|
|
!ibvs_.init(pinocchio_solver,
|
|
config_.alignment().ibvs().camera_link())) {
|
|
initialized_ = false;
|
|
last_status_ = Status::INVALID_CONFIG;
|
|
return false;
|
|
}
|
|
|
|
ibvs_.setPerception(perception_);
|
|
if (!validateControlJointNames()) {
|
|
initialized_ = false;
|
|
last_status_ = Status::CONTROL_JOINT_MISMATCH;
|
|
return false;
|
|
}
|
|
|
|
initialized_ = applyConfig();
|
|
if (initialized_) {
|
|
tracker_.clear();
|
|
tracker_.resetActiveTagTracking();
|
|
ibvs_.reset();
|
|
phase_ = Phase::IDLE;
|
|
phase_after_retract_ = Phase::DONE;
|
|
final_status_after_retract_ = Status::DONE;
|
|
target_locked_ = false;
|
|
ibvs_target_initialized_ = false;
|
|
touch_command_started_ = false;
|
|
retract_command_started_ = false;
|
|
align_stable_count_ = 0;
|
|
last_active_tag_id_ = -1;
|
|
last_touch_pressure_sum_ = 0.0;
|
|
last_touch_nonzero_count_ = 0;
|
|
last_align_error_camera_.setZero();
|
|
touch_start_position_valid_ = false;
|
|
touch_start_position_base_.setZero();
|
|
retract_start_position_valid_ = false;
|
|
retract_start_position_base_.setZero();
|
|
last_status_ = Status::IDLE;
|
|
} else {
|
|
last_status_ = Status::INVALID_CONFIG;
|
|
}
|
|
return initialized_;
|
|
}
|
|
|
|
bool TouchScreenTask::touch(const int u, const int v) {
|
|
return touchIfCurrent(u, v, [] { return true; });
|
|
}
|
|
|
|
bool TouchScreenTask::touchIfCurrent(
|
|
const int u,
|
|
const int v,
|
|
const std::function<bool()>& still_admitted,
|
|
SafetyHooks safety_hooks)
|
|
{
|
|
auto& admission_gate = service::globalStopAllAdmissionGate();
|
|
std::uint64_t admission_generation = 0U;
|
|
{
|
|
auto admission = admission_gate.lockAdmission();
|
|
if (!admission.accepting()) {
|
|
return false;
|
|
}
|
|
admission_generation = admission.generation();
|
|
}
|
|
if (stop_requested_.load(std::memory_order_acquire)) {
|
|
return false;
|
|
}
|
|
const auto activity_generation =
|
|
activity_generation_.load(std::memory_order_acquire);
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
if (!still_admitted || !still_admitted()) {
|
|
return false;
|
|
}
|
|
if (!initialized_ || !camera_ || camera_->id().empty()) {
|
|
last_status_ = Status::NOT_INITIALIZED;
|
|
return false;
|
|
}
|
|
|
|
auto& camera_registry =
|
|
service::globalCameraOperationalActivityRegistry();
|
|
service::CameraOperationalActivityRegistry::ActivityToken camera_token;
|
|
service::CameraOperationalActivityRegistry::DispatchResult camera_start;
|
|
try {
|
|
camera_start = camera_registry.start(
|
|
camera_->id(), camera_, &camera_token);
|
|
} catch (...) {
|
|
last_status_ = Status::NOT_INITIALIZED;
|
|
return false;
|
|
}
|
|
if (camera_start != service::CameraOperationalActivityRegistry::
|
|
DispatchResult::Success) {
|
|
last_status_ = Status::NOT_INITIALIZED;
|
|
return false;
|
|
}
|
|
|
|
const auto rollback_camera = [&] {
|
|
if (camera_registry.stopIfCurrent(camera_token)) {
|
|
return;
|
|
}
|
|
const auto ticket = admission_gate.beginStopAll();
|
|
(void)admission_gate.finishStopAll(ticket, false);
|
|
};
|
|
|
|
bool admission_current = false;
|
|
bool admitted = false;
|
|
{
|
|
// The arm lease and activity marker are the publication point. Holding
|
|
// admission here makes that point linearizable with beginStopAll().
|
|
auto admission = admission_gate.lockAdmission();
|
|
admission_current = admission.accepting() &&
|
|
admission.generation() == admission_generation;
|
|
if (admission_current) {
|
|
admitted = beginActivityIfCurrent(activity_generation);
|
|
}
|
|
}
|
|
if (!admission_current || !admitted) {
|
|
rollback_camera();
|
|
return false;
|
|
}
|
|
|
|
resetActivityUnlocked();
|
|
activity_safety_hooks_ = std::move(safety_hooks);
|
|
if (!activitySafetyCurrent()) {
|
|
last_status_ = Status::SAFETY_ADMISSION_REVOKED;
|
|
activity_active_.store(false, std::memory_order_release);
|
|
clearActivitySafetyHooksUnlocked();
|
|
releaseActivityControlUnlocked();
|
|
rollback_camera();
|
|
return false;
|
|
}
|
|
|
|
const bool started = startFromPixelUnlocked(u, v);
|
|
if (!started) {
|
|
activity_active_.store(false, std::memory_order_release);
|
|
clearActivitySafetyHooksUnlocked();
|
|
releaseActivityControlUnlocked();
|
|
rollback_camera();
|
|
return false;
|
|
}
|
|
|
|
// startFromPixelUnlocked() may perform an interruptible initialization
|
|
// move. Do not retain the global gate across device work; reject and roll
|
|
// back if StopAll changed the generation while that work was in flight.
|
|
admission_current = false;
|
|
{
|
|
auto admission = admission_gate.lockAdmission();
|
|
admission_current = admission.accepting() &&
|
|
admission.generation() == admission_generation;
|
|
}
|
|
if (admission_current) {
|
|
return true;
|
|
}
|
|
activity_active_.store(false, std::memory_order_release);
|
|
resetActivityUnlocked();
|
|
clearActivitySafetyHooksUnlocked();
|
|
releaseActivityControlUnlocked();
|
|
rollback_camera();
|
|
return false;
|
|
}
|
|
|
|
bool TouchScreenTask::beginActivityIfCurrent(
|
|
const std::uint64_t activity_generation)
|
|
{
|
|
if (stop_requested_.load(std::memory_order_acquire) ||
|
|
activity_generation_.load(std::memory_order_acquire) !=
|
|
activity_generation) {
|
|
return false;
|
|
}
|
|
if (isBusyUnlocked()) {
|
|
last_status_ = Status::TASK_BUSY;
|
|
return false;
|
|
}
|
|
if (!acquireActivityControlUnlocked()) {
|
|
last_status_ = Status::TASK_BUSY;
|
|
return false;
|
|
}
|
|
if (stop_requested_.load(std::memory_order_acquire) ||
|
|
activity_generation_.load(std::memory_order_acquire) !=
|
|
activity_generation) {
|
|
releaseActivityControlUnlocked();
|
|
return false;
|
|
}
|
|
activity_active_.store(true, std::memory_order_release);
|
|
return true;
|
|
}
|
|
|
|
bool TouchScreenTask::acquireActivityControlUnlocked()
|
|
{
|
|
if (!arm_ || arm_->id().empty() || activityControlToken().valid()) {
|
|
return false;
|
|
}
|
|
static std::atomic<std::uint64_t> sequence{0U};
|
|
const auto acquired = control::ControlAuthorityManager::instance()
|
|
.tryAcquire(
|
|
arm_->id(),
|
|
"touch-screen:" + id_ + ":" +
|
|
std::to_string(
|
|
sequence.fetch_add(1U, std::memory_order_relaxed) + 1U),
|
|
std::chrono::duration_cast<
|
|
control::ControlAuthorityManager::Duration>(
|
|
std::chrono::hours(24)));
|
|
if (!acquired.acquired) {
|
|
return false;
|
|
}
|
|
{
|
|
std::lock_guard lock(activity_control_mutex_);
|
|
activity_control_token_ = acquired.token;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
control::ControlLeaseToken TouchScreenTask::activityControlToken() const
|
|
{
|
|
std::lock_guard lock(activity_control_mutex_);
|
|
return activity_control_token_;
|
|
}
|
|
|
|
bool TouchScreenTask::activityControlCurrent() const
|
|
{
|
|
const auto token = activityControlToken();
|
|
return token.valid() &&
|
|
control::ControlAuthorityManager::instance().validate(
|
|
token);
|
|
}
|
|
|
|
std::function<bool()> TouchScreenTask::activityCancellationRequested() const
|
|
{
|
|
const auto token = activityControlToken();
|
|
return [this, token] {
|
|
return stop_requested_.load(std::memory_order_acquire) ||
|
|
!control::ControlAuthorityManager::instance().validate(token);
|
|
};
|
|
}
|
|
|
|
control::ControlDispatchGuard
|
|
TouchScreenTask::tryBeginActivityDispatch() const
|
|
{
|
|
const auto token = activityControlToken();
|
|
return control::ControlAuthorityManager::instance().tryBeginDispatch(
|
|
token);
|
|
}
|
|
|
|
bool TouchScreenTask::activitySafetyCurrent() const
|
|
{
|
|
if (!activity_safety_hooks_.revalidate) {
|
|
return true;
|
|
}
|
|
try {
|
|
return activity_safety_hooks_.revalidate();
|
|
} catch (...) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool TouchScreenTask::runArmActuationIfCurrent(
|
|
const SafetyHooks::HardwareOperation& operation) const
|
|
{
|
|
if (!operation || !activitySafetyCurrent()) {
|
|
return false;
|
|
}
|
|
auto authority_dispatch = tryBeginActivityDispatch();
|
|
if (!authority_dispatch.acquired()) {
|
|
return false;
|
|
}
|
|
try {
|
|
return activity_safety_hooks_.dispatch_actuation
|
|
? activity_safety_hooks_.dispatch_actuation(operation)
|
|
: operation();
|
|
} catch (...) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool TouchScreenTask::runArmStopIfCurrent(
|
|
const SafetyHooks::HardwareOperation& operation) const
|
|
{
|
|
if (!operation) {
|
|
return false;
|
|
}
|
|
auto authority_dispatch = tryBeginActivityDispatch();
|
|
if (!authority_dispatch.acquired()) {
|
|
return false;
|
|
}
|
|
try {
|
|
return activity_safety_hooks_.dispatch_stop
|
|
? activity_safety_hooks_.dispatch_stop(operation)
|
|
: operation();
|
|
} catch (...) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void TouchScreenTask::clearActivitySafetyHooksUnlocked() noexcept
|
|
{
|
|
activity_safety_hooks_ = {};
|
|
}
|
|
|
|
void TouchScreenTask::releaseActivityControlUnlocked() noexcept
|
|
{
|
|
control::ControlLeaseToken token;
|
|
{
|
|
std::lock_guard lock(activity_control_mutex_);
|
|
token = std::move(activity_control_token_);
|
|
activity_control_token_ = {};
|
|
}
|
|
control::ControlAuthorityManager::instance().release(token);
|
|
}
|
|
|
|
void TouchScreenTask::finishActivityUnlocked(
|
|
const Phase phase,
|
|
const Status status) noexcept
|
|
{
|
|
phase_ = phase;
|
|
last_status_ = status;
|
|
touch_command_started_ = false;
|
|
retract_command_started_ = false;
|
|
activity_active_.store(false, std::memory_order_release);
|
|
clearActivitySafetyHooksUnlocked();
|
|
releaseActivityControlUnlocked();
|
|
}
|
|
|
|
bool TouchScreenTask::startFromPixel(const int u, const int v) {
|
|
return touchIfCurrent(u, v, [] { return true; });
|
|
}
|
|
|
|
bool TouchScreenTask::startFromPixelUnlocked(int u, int v) {
|
|
if (!initialized_) {
|
|
last_status_ = Status::NOT_INITIALIZED;
|
|
return false;
|
|
}
|
|
if (u < 0 || v < 0) {
|
|
last_status_ = Status::INVALID_CONFIG;
|
|
return false;
|
|
}
|
|
|
|
if (!moveToInitPositionBeforeStartIfEnabled()) {
|
|
return false;
|
|
}
|
|
|
|
tracker_.clear();
|
|
tracker_.resetActiveTagTracking();
|
|
ibvs_.reset();
|
|
|
|
target_u_ = u;
|
|
target_v_ = v;
|
|
target_locked_ = false;
|
|
ibvs_target_initialized_ = false;
|
|
touch_command_started_ = false;
|
|
retract_command_started_ = false;
|
|
align_stable_count_ = 0;
|
|
align_debug_count_ = 0;
|
|
last_touch_pressure_sum_ = 0.0;
|
|
last_touch_nonzero_count_ = 0;
|
|
last_active_tag_id_ = -1;
|
|
last_align_error_camera_.setZero();
|
|
locked_target_rotation_valid_ = false;
|
|
locked_target_rotation_.setIdentity();
|
|
touch_start_position_valid_ = false;
|
|
touch_start_position_base_.setZero();
|
|
retract_start_position_valid_ = false;
|
|
retract_start_position_base_.setZero();
|
|
phase_ = Phase::ALIGNING;
|
|
phase_after_retract_ = Phase::DONE;
|
|
final_status_after_retract_ = Status::DONE;
|
|
phase_start_time_ = Clock::now();
|
|
last_status_ = Status::ALIGN_WAITING_TRACK;
|
|
return true;
|
|
}
|
|
|
|
bool TouchScreenTask::step(const double dt) {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
if (stop_requested_.load(std::memory_order_acquire)) {
|
|
return true;
|
|
}
|
|
if (activity_active_.load(std::memory_order_acquire) &&
|
|
!activityControlCurrent()) {
|
|
activity_active_.store(false, std::memory_order_release);
|
|
resetActivityUnlocked();
|
|
clearActivitySafetyHooksUnlocked();
|
|
releaseActivityControlUnlocked();
|
|
return true;
|
|
}
|
|
if (activity_active_.load(std::memory_order_acquire) &&
|
|
!activitySafetyCurrent()) {
|
|
(void)runArmStopIfCurrent([this] {
|
|
return arm_ && arm_->stopMotion().ok();
|
|
});
|
|
finishActivityUnlocked(
|
|
Phase::FAILED, Status::SAFETY_ADMISSION_REVOKED);
|
|
return false;
|
|
}
|
|
if (!initialized_) {
|
|
last_status_ = Status::NOT_INITIALIZED;
|
|
return false;
|
|
}
|
|
if (!std::isfinite(dt) || dt <= 0.0) {
|
|
enterFailed(Status::INVALID_CONFIG);
|
|
return false;
|
|
}
|
|
|
|
if (dexhand_) {
|
|
const bool tactile_ok = updateTouchPressure();
|
|
// std::cout << "[TouchScreenTask][TACTILE] finger="
|
|
// << fingerTypeToString(toFingerType(config_.touch().tactile().finger()))
|
|
// << ", region=" << tactileRegionToString(toTactileRegion(config_.touch().tactile().region()))
|
|
// << ", ok=" << (tactile_ok ? 1 : 0)
|
|
// << ", nonzero_count=" << last_touch_nonzero_count_
|
|
// << ", pressure_sum=" << last_touch_pressure_sum_
|
|
// << std::endl;
|
|
}
|
|
|
|
switch (phase_) {
|
|
case Phase::IDLE:
|
|
last_status_ = Status::IDLE;
|
|
return true;
|
|
case Phase::ALIGNING:
|
|
return stepAligning(dt);
|
|
case Phase::ALIGN_REACHED:
|
|
last_status_ = Status::ALIGN_REACHED;
|
|
if (config_.alignment().pause_when_reached()) {
|
|
return true;
|
|
}
|
|
if (!startTouchPhase()) {
|
|
enterFailed(last_status_ == Status::TACTILE_UNAVAILABLE ||
|
|
last_status_ == Status::INVALID_CONFIG ||
|
|
last_status_ == Status::ROBOT_STATE_FAILED
|
|
? last_status_
|
|
: Status::ROBOT_COMMAND_FAILED);
|
|
return false;
|
|
}
|
|
return true;
|
|
case Phase::TOUCHING:
|
|
return stepTouching();
|
|
case Phase::DWELLING:
|
|
return stepDwelling();
|
|
case Phase::RETRACTING:
|
|
return stepRetracting();
|
|
case Phase::DONE:
|
|
last_status_ = Status::DONE;
|
|
return true;
|
|
case Phase::FAILED:
|
|
return false;
|
|
}
|
|
enterFailed(Status::INVALID_CONFIG);
|
|
return false;
|
|
}
|
|
|
|
void TouchScreenTask::stop() {
|
|
(void)stopActivity();
|
|
}
|
|
|
|
bool TouchScreenTask::stopActivity() {
|
|
activity_generation_.fetch_add(1U, std::memory_order_acq_rel);
|
|
stop_requested_.store(true, std::memory_order_release);
|
|
|
|
std::shared_ptr<device::RobotArm> arm;
|
|
{
|
|
std::lock_guard lock(activity_arm_mutex_);
|
|
arm = activity_arm_;
|
|
}
|
|
|
|
static std::atomic<std::uint64_t> stop_sequence{0U};
|
|
auto& authority = control::ControlAuthorityManager::instance();
|
|
const auto expected_token = activityControlToken();
|
|
control::ControlAcquireResult stop_barrier;
|
|
bool barrier_error = false;
|
|
if (arm && expected_token.valid()) {
|
|
try {
|
|
stop_barrier = authority.preemptAcquireIfCurrent(
|
|
expected_token,
|
|
"touch-screen-stop:" + id_ + ":" +
|
|
std::to_string(
|
|
stop_sequence.fetch_add(
|
|
1U, std::memory_order_relaxed) + 1U),
|
|
std::chrono::duration_cast<
|
|
control::ControlAuthorityManager::Duration>(
|
|
std::chrono::hours(24)));
|
|
} catch (...) {
|
|
barrier_error = true;
|
|
(void)authority.quarantineIfCurrent(expected_token);
|
|
}
|
|
}
|
|
|
|
// Only the caller which atomically converted this task's exact lease may
|
|
// touch the driver. If StopAll already owns the safety barrier, its arm
|
|
// stop runs independently while this task only drains its old step.
|
|
if (stop_barrier.acquired) {
|
|
try {
|
|
(void)arm->stopMotion();
|
|
} catch (...) {
|
|
}
|
|
}
|
|
|
|
bool was_active = false;
|
|
{
|
|
// A step holds this mutex through all of its arm submissions. Taking
|
|
// it here proves that the old step has exited before state is reset.
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
was_active = activity_active_.exchange(
|
|
false, std::memory_order_acq_rel);
|
|
if (was_active || activityControlToken().valid() ||
|
|
isBusyUnlocked()) {
|
|
resetActivityUnlocked();
|
|
}
|
|
clearActivitySafetyHooksUnlocked();
|
|
releaseActivityControlUnlocked();
|
|
}
|
|
|
|
bool stopped = !barrier_error;
|
|
if (stop_barrier.acquired) {
|
|
const bool handler_released = authority.waitForPreemptedRelease(
|
|
stop_barrier.token,
|
|
control::ControlAuthorityManager::Duration::zero());
|
|
if (handler_released) {
|
|
// A backend may have allowed the cancellation request to return
|
|
// without fully quiescing. Confirm once more after the old task
|
|
// step and every guarded dispatch have drained.
|
|
try {
|
|
stopped = arm->stopMotion().ok();
|
|
} catch (...) {
|
|
stopped = false;
|
|
}
|
|
} else {
|
|
stopped = false;
|
|
}
|
|
|
|
if (stopped) {
|
|
authority.release(stop_barrier.token);
|
|
} else {
|
|
(void)authority.retireSafetyHolder(stop_barrier.token);
|
|
}
|
|
}
|
|
|
|
stop_requested_.store(false, std::memory_order_release);
|
|
return stopped;
|
|
}
|
|
|
|
void TouchScreenTask::resetActivityUnlocked() {
|
|
ibvs_.resetTwistCommandState();
|
|
|
|
phase_ = Phase::IDLE;
|
|
phase_after_retract_ = Phase::DONE;
|
|
final_status_after_retract_ = Status::DONE;
|
|
target_locked_ = false;
|
|
ibvs_target_initialized_ = false;
|
|
touch_command_started_ = false;
|
|
retract_command_started_ = false;
|
|
align_stable_count_ = 0;
|
|
last_active_tag_id_ = -1;
|
|
last_touch_pressure_sum_ = 0.0;
|
|
last_touch_nonzero_count_ = 0;
|
|
last_align_error_camera_.setZero();
|
|
locked_target_rotation_valid_ = false;
|
|
locked_target_rotation_.setIdentity();
|
|
touch_start_position_valid_ = false;
|
|
touch_start_position_base_.setZero();
|
|
retract_start_position_valid_ = false;
|
|
retract_start_position_base_.setZero();
|
|
last_status_ = Status::STOPPED;
|
|
}
|
|
|
|
TouchScreenTask::Phase TouchScreenTask::phase() const {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return phase_;
|
|
}
|
|
|
|
TouchScreenTask::Status TouchScreenTask::lastStatus() const {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return last_status_;
|
|
}
|
|
|
|
bool TouchScreenTask::isBusyUnlocked() const {
|
|
return phase_ == Phase::ALIGNING || phase_ == Phase::ALIGN_REACHED ||
|
|
phase_ == Phase::TOUCHING || phase_ == Phase::DWELLING ||
|
|
phase_ == Phase::RETRACTING;
|
|
}
|
|
|
|
bool TouchScreenTask::isBusy() const {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return isBusyUnlocked();
|
|
}
|
|
|
|
TaskState TouchScreenTask::state() const {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
if (!initialized_) {
|
|
return TaskState::UNINITIALIZED;
|
|
}
|
|
if (isBusyUnlocked()) {
|
|
return TaskState::RUNNING;
|
|
}
|
|
if (phase_ == Phase::DONE) {
|
|
return TaskState::SUCCEEDED;
|
|
}
|
|
if (phase_ == Phase::FAILED) {
|
|
return TaskState::FAILED;
|
|
}
|
|
if (last_status_ == Status::STOPPED) {
|
|
return TaskState::STOPPED;
|
|
}
|
|
return TaskState::IDLE;
|
|
}
|
|
|
|
bool TouchScreenTask::isFinished() const {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return phase_ == Phase::DONE;
|
|
}
|
|
|
|
bool TouchScreenTask::isFailed() const {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return phase_ == Phase::FAILED;
|
|
}
|
|
|
|
int TouchScreenTask::targetU() const {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return target_u_;
|
|
}
|
|
|
|
int TouchScreenTask::targetV() const {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return target_v_;
|
|
}
|
|
|
|
double TouchScreenTask::lastTouchPressureSum() const {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return last_touch_pressure_sum_;
|
|
}
|
|
|
|
int TouchScreenTask::lastTouchNonzeroCount() const {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return last_touch_nonzero_count_;
|
|
}
|
|
|
|
int TouchScreenTask::lastActiveTagId() const {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return last_active_tag_id_;
|
|
}
|
|
|
|
Eigen::Vector3d TouchScreenTask::lastAlignErrorCamera() const {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return last_align_error_camera_;
|
|
}
|
|
|
|
std::string TouchScreenTask::controlDeviceId() const {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
if (arm_ && !arm_->id().empty()) {
|
|
return arm_->id();
|
|
}
|
|
return config_.devices().arm_id();
|
|
}
|
|
|
|
std::string TouchScreenTask::stateString() const {
|
|
return taskStateToString(state());
|
|
}
|
|
|
|
std::string TouchScreenTask::detailStatusString() const {
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return std::string(phaseToString(phase_)) + "/" + statusToString(last_status_);
|
|
}
|
|
|
|
const char* TouchScreenTask::phaseToString(const Phase phase) {
|
|
switch (phase) {
|
|
case Phase::IDLE: return "IDLE";
|
|
case Phase::ALIGNING: return "ALIGNING";
|
|
case Phase::ALIGN_REACHED: return "ALIGN_REACHED";
|
|
case Phase::TOUCHING: return "TOUCHING";
|
|
case Phase::DWELLING: return "DWELLING";
|
|
case Phase::RETRACTING: return "RETRACTING";
|
|
case Phase::DONE: return "DONE";
|
|
case Phase::FAILED: return "FAILED";
|
|
}
|
|
return "UNKNOWN";
|
|
}
|
|
|
|
const char* TouchScreenTask::statusToString(const Status status) {
|
|
switch (status) {
|
|
case Status::IDLE: return "IDLE";
|
|
case Status::NOT_INITIALIZED: return "NOT_INITIALIZED";
|
|
case Status::INVALID_CONFIG: return "INVALID_CONFIG";
|
|
case Status::CONTROL_JOINT_MISMATCH: return "CONTROL_JOINT_MISMATCH";
|
|
case Status::ALIGN_WAITING_PERCEPTION: return "ALIGN_WAITING_PERCEPTION";
|
|
case Status::ALIGN_WAITING_TRACK: return "ALIGN_WAITING_TRACK";
|
|
case Status::ALIGN_TARGET_SETUP_FAILED: return "ALIGN_TARGET_SETUP_FAILED";
|
|
case Status::ALIGN_COMPUTE_FAILED: return "ALIGN_COMPUTE_FAILED";
|
|
case Status::ALIGN_TIMEOUT: return "ALIGN_TIMEOUT";
|
|
case Status::ALIGNING: return "ALIGNING";
|
|
case Status::ALIGN_REACHED: return "ALIGN_REACHED";
|
|
case Status::TOUCHING: return "TOUCHING";
|
|
case Status::TACTILE_UNAVAILABLE: return "TACTILE_UNAVAILABLE";
|
|
case Status::TOUCH_TRIGGERED: return "TOUCH_TRIGGERED";
|
|
case Status::TOUCH_FORWARD_TIMEOUT: return "TOUCH_FORWARD_TIMEOUT";
|
|
case Status::RETRACTING: return "RETRACTING";
|
|
case Status::DONE: return "DONE";
|
|
case Status::STOPPED: return "STOPPED";
|
|
case Status::SAFETY_ADMISSION_REVOKED: return "SAFETY_ADMISSION_REVOKED";
|
|
case Status::ROBOT_STATE_FAILED: return "ROBOT_STATE_FAILED";
|
|
case Status::ROBOT_COMMAND_FAILED: return "ROBOT_COMMAND_FAILED";
|
|
case Status::TASK_BUSY: return "TASK_BUSY";
|
|
}
|
|
return "UNKNOWN";
|
|
}
|
|
|
|
bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig& config) {
|
|
if (!config.has_id() || config.id().empty() ||
|
|
!config.has_devices() ||
|
|
!config.devices().has_arm_id() || config.devices().arm_id().empty() ||
|
|
!config.devices().has_dexhand_id() || config.devices().dexhand_id().empty() ||
|
|
!config.devices().has_camera_id() || config.devices().camera_id().empty() ||
|
|
!config.has_initialization() ||
|
|
!config.initialization().has_before_start() ||
|
|
!config.initialization().has_after_finish() ||
|
|
!config.initialization().has_velocity() ||
|
|
!config.initialization().has_acceleration() ||
|
|
!config.has_perception() ||
|
|
!config.perception().has_apriltag() ||
|
|
!config.has_alignment() ||
|
|
!config.alignment().has_ibvs() ||
|
|
!config.alignment().has_target() ||
|
|
!config.alignment().has_error_threshold() ||
|
|
!config.alignment().has_stable_frames() ||
|
|
!config.alignment().has_timeout_s() ||
|
|
!config.alignment().has_pause_when_reached() ||
|
|
!config.has_touch() ||
|
|
!config.touch().has_tactile() ||
|
|
!config.touch().has_dwell_time_s() ||
|
|
!config.has_retract() ||
|
|
!config.retract().has_twist_tool() ||
|
|
!config.retract().has_acceleration() ||
|
|
!config.retract().has_duration_s()) {
|
|
return false;
|
|
}
|
|
|
|
const auto& apriltag = config.perception().apriltag();
|
|
const auto& alignment = config.alignment();
|
|
const auto& ibvs = alignment.ibvs();
|
|
const auto& target = alignment.target();
|
|
const auto& touch = config.touch();
|
|
const auto& tactile = touch.tactile();
|
|
const auto& retract = config.retract();
|
|
|
|
if (!apriltag.has_tag_size_m() ||
|
|
!apriltag.has_depth_policy() ||
|
|
!apriltag.has_target_point_method() ||
|
|
!target.has_position_in_camera() ||
|
|
!hasVec3(target.position_in_camera()) ||
|
|
!target.has_rotation_vector() ||
|
|
!hasVec3(target.rotation_vector()) ||
|
|
!target.has_mode() ||
|
|
!hasVec6(alignment.error_threshold()) ||
|
|
!ibvs.has_camera_link() ||
|
|
!ibvs.has_lambda() ||
|
|
!ibvs.has_mu() ||
|
|
!ibvs.has_qdot_max() ||
|
|
!ibvs.has_vmax6() ||
|
|
!hasVec6(ibvs.vmax6()) ||
|
|
!ibvs.has_amax6() ||
|
|
!hasVec6(ibvs.amax6()) ||
|
|
!ibvs.has_twist_filter_alpha() ||
|
|
!ibvs.has_r_camera_to_visp() ||
|
|
!hasMat3(ibvs.r_camera_to_visp()) ||
|
|
!ibvs.has_r_camera_to_urdf() ||
|
|
!hasMat3(ibvs.r_camera_to_urdf()) ||
|
|
!tactile.has_finger() ||
|
|
!tactile.has_region() ||
|
|
!tactile.has_criterion() ||
|
|
!tactile.has_force_threshold() ||
|
|
!hasVec6(retract.twist_tool())) {
|
|
return false;
|
|
}
|
|
|
|
if (ibvs.camera_link().empty() ||
|
|
ibvs.control_joint_names().empty()) {
|
|
return false;
|
|
}
|
|
|
|
const Eigen::Vector3d target_position =
|
|
cmvr::common::math::toEigenVec3(alignment.target().position_in_camera());
|
|
const Eigen::Vector3d target_rotation =
|
|
cmvr::common::math::toEigenVec3(alignment.target().rotation_vector());
|
|
const Eigen::Matrix<double, 6, 1> error_threshold =
|
|
cmvr::common::math::toEigenVec6(alignment.error_threshold());
|
|
const Eigen::Matrix3d camera_to_visp =
|
|
cmvr::common::math::toEigenMat3(ibvs.r_camera_to_visp());
|
|
const Eigen::Matrix3d camera_to_urdf =
|
|
cmvr::common::math::toEigenMat3(ibvs.r_camera_to_urdf());
|
|
|
|
if (!std::isfinite(apriltag.tag_size_m()) || apriltag.tag_size_m() <= 0.0 ||
|
|
!target_position.allFinite() ||
|
|
!target_rotation.allFinite() ||
|
|
alignment.stable_frames() <= 0 ||
|
|
!std::isfinite(alignment.timeout_s()) || alignment.timeout_s() <= 0.0 ||
|
|
!std::isfinite(tactile.force_threshold()) || tactile.force_threshold() < 0.0 ||
|
|
!std::isfinite(touch.dwell_time_s()) || touch.dwell_time_s() < 0.0 ||
|
|
!camera_to_visp.allFinite() || !camera_to_urdf.allFinite() ||
|
|
!cmvr::common::math::toEigenVec6(retract.twist_tool()).allFinite() ||
|
|
!std::isfinite(retract.acceleration()) || retract.acceleration() <= 0.0 ||
|
|
!std::isfinite(retract.duration_s()) || retract.duration_s() < 0.0) {
|
|
return false;
|
|
}
|
|
for (int i = 0; i < error_threshold.size(); ++i) {
|
|
if (!std::isfinite(error_threshold[i]) || error_threshold[i] < 0.0) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
if (!std::isfinite(config.initialization().velocity()) ||
|
|
config.initialization().velocity() <= 0.0 ||
|
|
!std::isfinite(config.initialization().acceleration()) ||
|
|
config.initialization().acceleration() <= 0.0) {
|
|
return false;
|
|
}
|
|
|
|
if (config.initialization().before_start() || config.initialization().after_finish()) {
|
|
std::vector<double> init_positions;
|
|
if (!buildInitJointPositionsFromConfig(config, init_positions)) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
std::vector<device::AbstractDexHand::TactileRegionKey> tactile_regions;
|
|
if (!appendRequestedTactileRegions(toFingerType(tactile.finger()),
|
|
toTactileRegion(tactile.region()),
|
|
tactile_regions)) {
|
|
return false;
|
|
}
|
|
|
|
switch (touch.motion_case()) {
|
|
case cmvr::config::TouchScreenTaskTouchConfig::kSpeedL: {
|
|
const auto& speed_l = touch.speed_l();
|
|
if (!speed_l.has_twist_tool() ||
|
|
!hasVec6(speed_l.twist_tool()) ||
|
|
!speed_l.has_acceleration() ||
|
|
!speed_l.has_max_distance_m()) {
|
|
return false;
|
|
}
|
|
const Eigen::Matrix<double, 6, 1> twist =
|
|
cmvr::common::math::toEigenVec6(speed_l.twist_tool());
|
|
return twist.allFinite() &&
|
|
std::isfinite(speed_l.acceleration()) &&
|
|
speed_l.acceleration() > 0.0 &&
|
|
std::isfinite(speed_l.max_distance_m()) &&
|
|
speed_l.max_distance_m() >= 0.0;
|
|
}
|
|
case cmvr::config::TouchScreenTaskTouchConfig::kMoveL: {
|
|
const auto& move_l = touch.move_l();
|
|
if (!move_l.has_direction_tool() ||
|
|
!hasVec6(move_l.direction_tool()) ||
|
|
!move_l.has_distance_m() ||
|
|
!move_l.has_velocity() ||
|
|
!move_l.has_acceleration() ||
|
|
!move_l.has_jerk()) {
|
|
return false;
|
|
}
|
|
const Eigen::Matrix<double, 6, 1> direction =
|
|
cmvr::common::math::toEigenVec6(move_l.direction_tool());
|
|
Eigen::Vector3d touch_forward_delta = Eigen::Vector3d::Zero();
|
|
if (!computeLinearMoveDeltaTool(direction, move_l.distance_m(), touch_forward_delta) ||
|
|
!std::isfinite(move_l.velocity()) || move_l.velocity() <= 0.0 ||
|
|
!std::isfinite(move_l.acceleration()) || move_l.acceleration() <= 0.0 ||
|
|
!std::isfinite(move_l.jerk()) || move_l.jerk() <= 0.0 ||
|
|
move_l.joint_velocity_limits_size() != ibvs.control_joint_names_size()) {
|
|
return false;
|
|
}
|
|
for (const double qd_max_i : move_l.joint_velocity_limits()) {
|
|
if (!std::isfinite(qd_max_i) || qd_max_i <= 0.0) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
case cmvr::config::TouchScreenTaskTouchConfig::MOTION_NOT_SET:
|
|
default:
|
|
return false;
|
|
}
|
|
}
|
|
|
|
bool TouchScreenTask::applyConfig() {
|
|
if (!perception_) {
|
|
return false;
|
|
}
|
|
if (!validateConfig(config_)) {
|
|
return false;
|
|
}
|
|
|
|
if (dexhand_) {
|
|
const auto& tactile = config_.touch().tactile();
|
|
std::vector<device::AbstractDexHand::TactileRegionKey> tactile_regions;
|
|
if (!appendRequestedTactileRegions(toFingerType(tactile.finger()),
|
|
toTactileRegion(tactile.region()),
|
|
tactile_regions)) {
|
|
return false;
|
|
}
|
|
try {
|
|
dexhand_->setTactilePollingRegions(tactile_regions);
|
|
} catch (...) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
const auto& apriltag = config_.perception().apriltag();
|
|
const auto& ibvs = config_.alignment().ibvs();
|
|
const auto target_position_in_camera =
|
|
cmvr::common::math::toEigenVec3(config_.alignment().target().position_in_camera());
|
|
const Eigen::Matrix3d r_camera_to_visp =
|
|
cmvr::common::math::toEigenMat3(ibvs.r_camera_to_visp());
|
|
const Eigen::Matrix3d r_camera_to_urdf =
|
|
cmvr::common::math::toEigenMat3(ibvs.r_camera_to_urdf());
|
|
perception_->setTagSize(apriltag.tag_size_m());
|
|
tracker_.setTargetPointMethod(toTargetPointMethod(apriltag.target_point_method()));
|
|
ibvs_.setLambda(ibvs.lambda());
|
|
ibvs_.setMu(ibvs.mu());
|
|
ibvs_.setQdotMax(ibvs.qdot_max());
|
|
ibvs_.setVelocityLimit6(toArray6(cmvr::common::math::toEigenVec6(ibvs.vmax6())));
|
|
ibvs_.setAccelerationLimit6(toArray6(cmvr::common::math::toEigenVec6(ibvs.amax6())));
|
|
ibvs_.setTwistFilterAlpha(ibvs.twist_filter_alpha());
|
|
ibvs_.setAlignCameraToVisp(r_camera_to_visp);
|
|
ibvs_.setAlignCameraToUrdf(r_camera_to_urdf);
|
|
CMVR_LOG(DEBUG) << "[TouchScreenTask] Apply config id=" << config_.id()
|
|
<< ", arm_id=" << config_.devices().arm_id()
|
|
<< ", camera_id=" << config_.devices().camera_id()
|
|
<< ", tag_size_m=" << apriltag.tag_size_m()
|
|
<< ", target_position_in_camera=[" << target_position_in_camera.x()
|
|
<< ", " << target_position_in_camera.y()
|
|
<< ", " << target_position_in_camera.z() << "]"
|
|
<< ", r_camera_to_visp=[" << r_camera_to_visp(0, 0)
|
|
<< ", " << r_camera_to_visp(0, 1)
|
|
<< ", " << r_camera_to_visp(0, 2)
|
|
<< "; " << r_camera_to_visp(1, 0)
|
|
<< ", " << r_camera_to_visp(1, 1)
|
|
<< ", " << r_camera_to_visp(1, 2)
|
|
<< "; " << r_camera_to_visp(2, 0)
|
|
<< ", " << r_camera_to_visp(2, 1)
|
|
<< ", " << r_camera_to_visp(2, 2) << "]"
|
|
<< ", r_camera_to_urdf=[" << r_camera_to_urdf(0, 0)
|
|
<< ", " << r_camera_to_urdf(0, 1)
|
|
<< ", " << r_camera_to_urdf(0, 2)
|
|
<< "; " << r_camera_to_urdf(1, 0)
|
|
<< ", " << r_camera_to_urdf(1, 1)
|
|
<< ", " << r_camera_to_urdf(1, 2)
|
|
<< "; " << r_camera_to_urdf(2, 0)
|
|
<< ", " << r_camera_to_urdf(2, 1)
|
|
<< ", " << r_camera_to_urdf(2, 2) << "]";
|
|
return true;
|
|
}
|
|
|
|
bool TouchScreenTask::validateControlJointNames() const {
|
|
std::vector<std::string> solver_joint_names;
|
|
if (!ibvs_.getChainJointNames(solver_joint_names)) {
|
|
return false;
|
|
}
|
|
const std::vector<std::string> task_joint_names = controlJointNames(config_);
|
|
if (solver_joint_names == task_joint_names) {
|
|
return true;
|
|
}
|
|
|
|
std::ostringstream mismatch;
|
|
mismatch << "[TouchScreenTask] control_joint_names mismatch with IbvsController IK chain"
|
|
<< ", task joints=[";
|
|
for (const auto& name : task_joint_names) {
|
|
mismatch << name << ' ';
|
|
}
|
|
mismatch << "], solver joints=[";
|
|
for (const auto& name : solver_joint_names) {
|
|
mismatch << name << ' ';
|
|
}
|
|
mismatch << ']';
|
|
CMVR_LOG(ERROR) << mismatch.str();
|
|
return false;
|
|
}
|
|
|
|
bool TouchScreenTask::stepAligning(const double dt) {
|
|
const auto& alignment = config_.alignment();
|
|
const auto target_position_in_camera =
|
|
cmvr::common::math::toEigenVec3(alignment.target().position_in_camera());
|
|
const auto target_rotation_vector =
|
|
cmvr::common::math::toEigenVec3(alignment.target().rotation_vector());
|
|
const auto error_threshold =
|
|
cmvr::common::math::toEigenVec6(alignment.error_threshold());
|
|
|
|
const auto now = Clock::now();
|
|
const double elapsed = std::chrono::duration<double>(now - phase_start_time_).count();
|
|
if (elapsed > alignment.timeout_s()) {
|
|
enterFailed(Status::ALIGN_TIMEOUT);
|
|
return false;
|
|
}
|
|
const double ibvs_dt = std::clamp(dt, 0.005, 0.05);
|
|
|
|
perception::AprilTagPerception::Options perception_options;
|
|
perception_options.depth_policy =
|
|
toDepthPolicy(config_.perception().apriltag().depth_policy());
|
|
perception_options.detect_tags = true;
|
|
perception_options.fetch_encoded = false;
|
|
if (!perception_->update(perception_options)) {
|
|
hardStopIbvsMotion();
|
|
last_status_ = Status::ALIGN_WAITING_PERCEPTION;
|
|
return true;
|
|
}
|
|
|
|
bool tracking_ok = false;
|
|
if (!target_locked_) {
|
|
tracking_ok = tracker_.startTrackingFromPixel(target_u_, target_v_);
|
|
if (tracking_ok) {
|
|
target_locked_ = true;
|
|
ibvs_target_initialized_ = false;
|
|
align_stable_count_ = 0;
|
|
}
|
|
} else {
|
|
tracking_ok = tracker_.track();
|
|
}
|
|
|
|
if (!tracking_ok) {
|
|
hardStopIbvsMotion();
|
|
align_stable_count_ = 0;
|
|
last_status_ = Status::ALIGN_WAITING_TRACK;
|
|
return true;
|
|
}
|
|
|
|
const int tag_id = tracker_.activeTagId();
|
|
last_active_tag_id_ = tag_id;
|
|
if (tag_id < 0) {
|
|
hardStopIbvsMotion();
|
|
align_stable_count_ = 0;
|
|
last_status_ = Status::ALIGN_WAITING_TRACK;
|
|
return true;
|
|
}
|
|
|
|
Eigen::Vector3d p_t_target = Eigen::Vector3d::Zero();
|
|
if (!tracker_.getAnchorInTag(tag_id, p_t_target)) {
|
|
hardStopIbvsMotion();
|
|
align_stable_count_ = 0;
|
|
last_status_ = Status::ALIGN_WAITING_TRACK;
|
|
return true;
|
|
}
|
|
|
|
const auto* current_tag = perception_->findTag(tag_id);
|
|
if (!current_tag) {
|
|
hardStopIbvsMotion();
|
|
align_stable_count_ = 0;
|
|
last_status_ = Status::ALIGN_WAITING_TRACK;
|
|
return true;
|
|
}
|
|
|
|
Eigen::Matrix3d R_target = rotationFromTargetRotvec(target_rotation_vector.x(),
|
|
target_rotation_vector.y(),
|
|
target_rotation_vector.z());
|
|
const Eigen::Matrix3d R_current = current_tag->T_c_t.block<3, 3>(0, 0);
|
|
switch (alignment.target().mode()) {
|
|
case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION:
|
|
break;
|
|
case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION:
|
|
if (!locked_target_rotation_valid_ || tracker_.lastSwitched()) {
|
|
double locked_yaw_rad = 0.0;
|
|
if (!extractProjectedYawAboutTargetNormal(R_target, R_current, locked_yaw_rad)) {
|
|
enterFailed(Status::ALIGN_TARGET_SETUP_FAILED);
|
|
return false;
|
|
}
|
|
const Eigen::Matrix3d Rz_locked =
|
|
Eigen::AngleAxisd(locked_yaw_rad, Eigen::Vector3d::UnitZ()).toRotationMatrix();
|
|
locked_target_rotation_ = R_target * Rz_locked;
|
|
locked_target_rotation_valid_ = true;
|
|
}
|
|
R_target = locked_target_rotation_;
|
|
break;
|
|
case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY:
|
|
// True position-only mode: keep the desired orientation equal to the
|
|
// current tag orientation every frame so IBVS does not actively try to
|
|
// correct rotational error.
|
|
R_target = R_current;
|
|
break;
|
|
}
|
|
const Eigen::Vector3d target_rotvec = rotvecFromRotationMatrix(R_target);
|
|
|
|
ibvs_.setTrackedTagId(tag_id);
|
|
const bool refresh_target = !ibvs_target_initialized_ || tracker_.lastSwitched() ||
|
|
alignment.target().mode() ==
|
|
cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY;
|
|
if (refresh_target) {
|
|
if (!ibvs_.setTargetFromPointInTag(p_t_target,
|
|
target_position_in_camera,
|
|
target_rotvec.x(),
|
|
target_rotvec.y(),
|
|
target_rotvec.z())) {
|
|
enterFailed(Status::ALIGN_TARGET_SETUP_FAILED);
|
|
return false;
|
|
}
|
|
ibvs_target_initialized_ = true;
|
|
}
|
|
|
|
std::vector<double> q_now;
|
|
if (!readControlledJointPositions(q_now)) {
|
|
enterFailed(Status::ROBOT_STATE_FAILED);
|
|
return false;
|
|
}
|
|
|
|
std::vector<double> qdot_cmd;
|
|
if (!ibvs_.computeQdot(q_now, ibvs_dt, qdot_cmd)) {
|
|
switch (ibvs_.lastComputeStatus()) {
|
|
case IbvsController::ComputeStatus::NO_NEW_FRAME:
|
|
case IbvsController::ComputeStatus::NO_TAG:
|
|
case IbvsController::ComputeStatus::TAG_MISMATCH:
|
|
case IbvsController::ComputeStatus::NO_DEPTH:
|
|
hardStopIbvsMotion();
|
|
align_stable_count_ = 0;
|
|
last_status_ = Status::ALIGN_WAITING_TRACK;
|
|
return true;
|
|
case IbvsController::ComputeStatus::OK:
|
|
case IbvsController::ComputeStatus::NOT_READY:
|
|
case IbvsController::ComputeStatus::BAD_IMAGE:
|
|
case IbvsController::ComputeStatus::INVALID_INPUT:
|
|
case IbvsController::ComputeStatus::IK_FAILED:
|
|
default:
|
|
enterFailed(Status::ALIGN_COMPUTE_FAILED);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
if ((align_debug_count_++ % 20) == 0) {
|
|
Eigen::Matrix<double, 6, 1> achieved_twist_base =
|
|
Eigen::Matrix<double, 6, 1>::Zero();
|
|
bool achieved_ok = false;
|
|
auto pinocchio_solver =
|
|
arm_ ? std::dynamic_pointer_cast<cmvr::PinocchioIKBase>(arm_->kinematicsSolver()) : nullptr;
|
|
if (pinocchio_solver) {
|
|
achieved_ok = pinocchio_solver->computeTwistBaseAtQ(
|
|
q_now,
|
|
qdot_cmd,
|
|
config_.alignment().ibvs().camera_link(),
|
|
achieved_twist_base);
|
|
}
|
|
|
|
const auto& target_c = tracker_.lastTargetInCamera();
|
|
const Eigen::Vector3d err_c = target_c - target_position_in_camera;
|
|
const auto& v_visp = ibvs_.lastCameraTwistVisp();
|
|
const double qdot_norm =
|
|
qdot_cmd.empty()
|
|
? 0.0
|
|
: Eigen::Map<const Eigen::VectorXd>(
|
|
qdot_cmd.data(),
|
|
static_cast<Eigen::Index>(qdot_cmd.size())).norm();
|
|
CMVR_LOG(DEBUG) << "[TouchScreenTask][ALIGN_DEBUG]"
|
|
<< " target_c=[" << target_c.x() << ", " << target_c.y()
|
|
<< ", " << target_c.z() << "]"
|
|
<< ", err_c=[" << err_c.x() << ", " << err_c.y()
|
|
<< ", " << err_c.z() << "]"
|
|
<< ", v_visp=[" << v_visp[0] << ", " << v_visp[1]
|
|
<< ", " << v_visp[2] << ", " << v_visp[3]
|
|
<< ", " << v_visp[4] << ", " << v_visp[5] << "]"
|
|
<< ", qdot0=" << (qdot_cmd.empty() ? 0.0 : qdot_cmd.front())
|
|
<< ", qdot_norm=" << qdot_norm
|
|
<< ", achieved_ok=" << achieved_ok
|
|
<< ", achieved_twist_base=[" << achieved_twist_base[0]
|
|
<< ", " << achieved_twist_base[1]
|
|
<< ", " << achieved_twist_base[2]
|
|
<< ", " << achieved_twist_base[3]
|
|
<< ", " << achieved_twist_base[4]
|
|
<< ", " << achieved_twist_base[5] << "]";
|
|
}
|
|
|
|
if (!sendJointVelocity(qdot_cmd)) {
|
|
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
|
return false;
|
|
}
|
|
|
|
last_align_error_camera_ = tracker_.lastTargetInCamera() - target_position_in_camera;
|
|
const Eigen::Vector3d rot_error_vec =
|
|
rotvecFromRotationMatrix(R_target.transpose() * R_current);
|
|
bool align_ok =
|
|
std::abs(last_align_error_camera_.x()) <= error_threshold[0] &&
|
|
std::abs(last_align_error_camera_.y()) <= error_threshold[1] &&
|
|
std::abs(last_align_error_camera_.z()) <= error_threshold[2];
|
|
switch (alignment.target().mode()) {
|
|
case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION:
|
|
align_ok = align_ok &&
|
|
std::abs(rot_error_vec.x()) <= error_threshold[3] &&
|
|
std::abs(rot_error_vec.y()) <= error_threshold[4] &&
|
|
std::abs(rot_error_vec.z()) <= error_threshold[5];
|
|
break;
|
|
case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION:
|
|
align_ok = align_ok &&
|
|
std::abs(rot_error_vec.x()) <= error_threshold[3] &&
|
|
std::abs(rot_error_vec.y()) <= error_threshold[4];
|
|
break;
|
|
case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY:
|
|
break;
|
|
}
|
|
if (align_ok) {
|
|
++align_stable_count_;
|
|
} else {
|
|
align_stable_count_ = 0;
|
|
}
|
|
|
|
if (align_stable_count_ >= alignment.stable_frames()) {
|
|
CMVR_LOG(INFO) << "[TouchScreenTask][ALIGN_REACHED] tag_id=" << tag_id
|
|
<< ", err_xyz=[" << last_align_error_camera_.x() << ", "
|
|
<< last_align_error_camera_.y() << ", "
|
|
<< last_align_error_camera_.z() << "]"
|
|
<< ", err_rxyz=[" << rot_error_vec.x() << ", "
|
|
<< rot_error_vec.y() << ", "
|
|
<< rot_error_vec.z() << "]";
|
|
hardStopIbvsMotion();
|
|
phase_ = Phase::ALIGN_REACHED;
|
|
phase_start_time_ = Clock::now();
|
|
touch_command_started_ = false;
|
|
last_status_ = Status::ALIGN_REACHED;
|
|
return true;
|
|
}
|
|
|
|
last_status_ = Status::ALIGNING;
|
|
return true;
|
|
}
|
|
|
|
bool TouchScreenTask::stepTouching() {
|
|
if (!touch_command_started_) {
|
|
if (!startTouchPhase()) {
|
|
enterFailed(last_status_ == Status::TACTILE_UNAVAILABLE ||
|
|
last_status_ == Status::INVALID_CONFIG ||
|
|
last_status_ == Status::ROBOT_STATE_FAILED
|
|
? last_status_
|
|
: Status::ROBOT_COMMAND_FAILED);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
if (!dexhand_) {
|
|
enterFailed(Status::TACTILE_UNAVAILABLE);
|
|
return false;
|
|
}
|
|
|
|
if (isTouchTriggered(config_, last_touch_pressure_sum_)) {
|
|
if (config_.touch().motion_case() == cmvr::config::TouchScreenTaskTouchConfig::kSpeedL) {
|
|
logTouchingSpeedLState();
|
|
}
|
|
if (!handleTouchTriggered(true)) {
|
|
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
if (config_.touch().motion_case() == cmvr::config::TouchScreenTaskTouchConfig::kMoveL) {
|
|
last_status_ = Status::TOUCHING;
|
|
return true;
|
|
}
|
|
|
|
const double speed_l_max_distance_m = config_.touch().speed_l().max_distance_m();
|
|
if (speed_l_max_distance_m > 0.0) {
|
|
if (!touch_start_position_valid_) {
|
|
enterFailed(Status::ROBOT_STATE_FAILED);
|
|
return false;
|
|
}
|
|
|
|
Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero();
|
|
if (!readCurrentTouchPointPositionBase(current_position_base)) {
|
|
enterFailed(Status::ROBOT_STATE_FAILED);
|
|
return false;
|
|
}
|
|
|
|
const double traveled_distance =
|
|
(current_position_base - touch_start_position_base_).norm();
|
|
if (traveled_distance >= speed_l_max_distance_m) {
|
|
logTouchingSpeedLState();
|
|
if (!updateTouchPressure()) {
|
|
enterFailed(Status::TACTILE_UNAVAILABLE);
|
|
return false;
|
|
}
|
|
if (isTouchTriggered(config_, last_touch_pressure_sum_)) {
|
|
if (!handleTouchTriggered(true)) {
|
|
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
if (!startRetractPhase(Phase::FAILED, Status::TOUCH_FORWARD_TIMEOUT)) {
|
|
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
}
|
|
|
|
last_status_ = Status::TOUCHING;
|
|
return true;
|
|
}
|
|
|
|
bool TouchScreenTask::stepDwelling() {
|
|
const double elapsed = std::chrono::duration<double>(Clock::now() - phase_start_time_).count();
|
|
if (elapsed < config_.touch().dwell_time_s()) {
|
|
last_status_ = Status::TOUCH_TRIGGERED;
|
|
return true;
|
|
}
|
|
|
|
if (!startRetractPhase(Phase::DONE, Status::DONE)) {
|
|
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool TouchScreenTask::stepRetracting() {
|
|
if (!retract_command_started_) {
|
|
if (!startRetractPhase(phase_after_retract_, final_status_after_retract_)) {
|
|
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
|
return false;
|
|
}
|
|
}
|
|
|
|
const auto now = Clock::now();
|
|
const double elapsed = std::chrono::duration<double>(now - phase_start_time_).count();
|
|
const double retract_duration_s = config_.retract().duration_s();
|
|
if (elapsed < retract_duration_s) {
|
|
if (std::chrono::duration<double>(now - last_retract_log_time_).count() >= 0.2) {
|
|
last_retract_log_time_ = now;
|
|
const auto cmd_base = arm_ ? arm_->getSpeedLCommandTwistBase() : device::CartesianVelocity{};
|
|
Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero();
|
|
const bool have_current = readCurrentTouchPointPositionBase(current_position_base);
|
|
const bool have_delta = retract_start_position_valid_ && have_current;
|
|
Eigen::Vector3d delta_base = Eigen::Vector3d::Zero();
|
|
if (have_delta) {
|
|
delta_base = current_position_base - retract_start_position_base_;
|
|
}
|
|
if (have_delta) {
|
|
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed
|
|
<< "/" << retract_duration_s
|
|
<< ", cmd_base=[" << cmd_base.vx << ", "
|
|
<< cmd_base.vy << ", " << cmd_base.vz << ", "
|
|
<< cmd_base.wx << ", " << cmd_base.wy << ", "
|
|
<< cmd_base.wz << "]"
|
|
<< ", tcp_delta_base=[" << delta_base.x() << ", "
|
|
<< delta_base.y() << ", " << delta_base.z()
|
|
<< "], tcp_dist=" << delta_base.norm();
|
|
} else {
|
|
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed
|
|
<< "/" << retract_duration_s
|
|
<< ", cmd_base=[" << cmd_base.vx << ", "
|
|
<< cmd_base.vy << ", " << cmd_base.vz << ", "
|
|
<< cmd_base.wx << ", " << cmd_base.wy << ", "
|
|
<< cmd_base.wz << "]"
|
|
<< ", tcp_delta_base=unavailable";
|
|
}
|
|
}
|
|
last_status_ = Status::RETRACTING;
|
|
return true;
|
|
}
|
|
|
|
Eigen::Vector3d final_position_base = Eigen::Vector3d::Zero();
|
|
const bool have_final_position = readCurrentTouchPointPositionBase(final_position_base);
|
|
Eigen::Vector3d final_delta_base = Eigen::Vector3d::Zero();
|
|
const bool have_final_delta = retract_start_position_valid_ && have_final_position;
|
|
if (have_final_delta) {
|
|
final_delta_base = final_position_base - retract_start_position_base_;
|
|
}
|
|
if (have_final_delta) {
|
|
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed
|
|
<< ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0)
|
|
<< ", final_tcp_delta_base=[" << final_delta_base.x() << ", "
|
|
<< final_delta_base.y() << ", " << final_delta_base.z()
|
|
<< "], final_tcp_dist=" << final_delta_base.norm();
|
|
} else {
|
|
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed
|
|
<< ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0)
|
|
<< ", final_tcp_delta_base=unavailable";
|
|
}
|
|
|
|
if (!runArmStopIfCurrent([this] {
|
|
return arm_ && arm_->stopL().ok();
|
|
})) {
|
|
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
|
return false;
|
|
}
|
|
holdCurrentControlledPosition();
|
|
if ((phase_after_retract_ == Phase::DONE || phase_after_retract_ == Phase::FAILED) &&
|
|
!moveToInitPositionIfEnabled()) {
|
|
finishActivityUnlocked(
|
|
Phase::FAILED, Status::ROBOT_COMMAND_FAILED);
|
|
return false;
|
|
}
|
|
const auto completed_phase = phase_after_retract_;
|
|
const auto completed_status = final_status_after_retract_;
|
|
finishActivityUnlocked(completed_phase, completed_status);
|
|
return phase_ != Phase::FAILED;
|
|
}
|
|
|
|
bool TouchScreenTask::readControlledJointPositions(std::vector<double>& q_out) const {
|
|
if (!arm_) {
|
|
return false;
|
|
}
|
|
|
|
const auto state = arm_->getJointState();
|
|
const auto model = arm_->getRobotModel();
|
|
std::unordered_map<std::string, double> q_map;
|
|
q_map.reserve(model.joint_names.size());
|
|
for (size_t i = 0; i < model.joint_names.size() && i < state.position.size(); ++i) {
|
|
q_map[model.joint_names[i]] = state.position[i];
|
|
}
|
|
|
|
const auto& control_joint_names = config_.alignment().ibvs().control_joint_names();
|
|
q_out.resize(static_cast<size_t>(control_joint_names.size()));
|
|
for (int i = 0; i < control_joint_names.size(); ++i) {
|
|
const auto it = q_map.find(control_joint_names[i]);
|
|
if (it == q_map.end()) {
|
|
return false;
|
|
}
|
|
q_out[static_cast<size_t>(i)] = it->second;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool TouchScreenTask::sendJointVelocity(const std::vector<double>& qdot) const {
|
|
if (!arm_ || qdot.size() != static_cast<size_t>(
|
|
config_.alignment().ibvs().control_joint_names_size())) {
|
|
return false;
|
|
}
|
|
|
|
device::JointVelocityCommand cmd;
|
|
cmd.velocity = qdot;
|
|
return runArmActuationIfCurrent([this, &cmd] {
|
|
return arm_ && arm_->speedJ(cmd, 0.0, 0.0).ok();
|
|
});
|
|
}
|
|
|
|
bool TouchScreenTask::sendZeroJointVelocity() const {
|
|
std::vector<double> zero(
|
|
static_cast<size_t>(config_.alignment().ibvs().control_joint_names_size()), 0.0);
|
|
return sendJointVelocity(zero);
|
|
}
|
|
|
|
void TouchScreenTask::hardStopIbvsMotion() {
|
|
sendZeroJointVelocity();
|
|
ibvs_.resetTwistCommandState();
|
|
}
|
|
|
|
bool TouchScreenTask::readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const {
|
|
if (!arm_) {
|
|
return false;
|
|
}
|
|
|
|
try {
|
|
const auto pose = arm_->fk(true);
|
|
p_out << pose.x, pose.y, pose.z;
|
|
return p_out.allFinite();
|
|
} catch (...) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void TouchScreenTask::logTouchingSpeedLState() const {
|
|
if (!arm_ || config_.touch().motion_case() != cmvr::config::TouchScreenTaskTouchConfig::kSpeedL ||
|
|
phase_ != Phase::TOUCHING) {
|
|
return;
|
|
}
|
|
|
|
const Eigen::Matrix<double, 6, 1> cmd_twist_base = toEigen6(arm_->getSpeedLCommandTwistBase());
|
|
Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero();
|
|
const bool have_current_position = readCurrentTouchPointPositionBase(current_position_base);
|
|
const bool have_delta = touch_start_position_valid_ && have_current_position;
|
|
Eigen::Vector3d cumulative_delta_base = Eigen::Vector3d::Zero();
|
|
if (have_delta) {
|
|
cumulative_delta_base = current_position_base - touch_start_position_base_;
|
|
}
|
|
|
|
std::ostringstream state_log;
|
|
state_log << "[TouchScreenTask][TOUCHING] speedl_cmd_base=["
|
|
<< cmd_twist_base[0] << ", "
|
|
<< cmd_twist_base[1] << ", "
|
|
<< cmd_twist_base[2] << ", "
|
|
<< cmd_twist_base[3] << ", "
|
|
<< cmd_twist_base[4] << ", "
|
|
<< cmd_twist_base[5] << "]";
|
|
if (have_delta) {
|
|
state_log << ", cum_tcp_delta_base=["
|
|
<< cumulative_delta_base.x() << ", "
|
|
<< cumulative_delta_base.y() << ", "
|
|
<< cumulative_delta_base.z() << "]"
|
|
<< ", cum_tcp_dist=" << cumulative_delta_base.norm();
|
|
} else {
|
|
state_log << ", cum_tcp_delta_base=[unavailable]";
|
|
}
|
|
CMVR_LOG(DEBUG) << state_log.str();
|
|
}
|
|
|
|
bool TouchScreenTask::holdCurrentControlledPosition() const {
|
|
if (!arm_) {
|
|
return false;
|
|
}
|
|
|
|
const auto state = arm_->getJointState();
|
|
const auto model = arm_->getRobotModel();
|
|
std::unordered_map<std::string, double> q_map;
|
|
q_map.reserve(model.joint_names.size());
|
|
for (size_t i = 0; i < model.joint_names.size() && i < state.position.size(); ++i) {
|
|
q_map[model.joint_names[i]] = state.position[i];
|
|
}
|
|
|
|
device::JointPositionCommand joints;
|
|
joints.position.reserve(static_cast<size_t>(
|
|
config_.alignment().ibvs().control_joint_names_size()));
|
|
for (const auto& name : config_.alignment().ibvs().control_joint_names()) {
|
|
const auto it = q_map.find(name);
|
|
if (it == q_map.end()) {
|
|
return false;
|
|
}
|
|
joints.position.push_back(it->second);
|
|
}
|
|
|
|
return runArmActuationIfCurrent([this, &joints] {
|
|
return arm_ && arm_->servoJ(joints).ok();
|
|
});
|
|
}
|
|
|
|
bool TouchScreenTask::buildInitJointPositions(std::vector<double>& positions_out) const {
|
|
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();
|
|
options.cancellation_requested = activityCancellationRequested();
|
|
if (!runArmActuationIfCurrent([this, &init_cmd, &options] {
|
|
return arm_ && arm_->moveJ(init_cmd, options).ok();
|
|
})) {
|
|
last_status_ = Status::ROBOT_COMMAND_FAILED;
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool TouchScreenTask::moveToInitPositionIfEnabled() const {
|
|
if (!config_.initialization().after_finish()) {
|
|
return true;
|
|
}
|
|
std::vector<double> init_positions;
|
|
if (!arm_ || !buildInitJointPositions(init_positions)) {
|
|
return false;
|
|
}
|
|
|
|
device::JointPositionCommand init_cmd{init_positions};
|
|
device::MotionOptions options;
|
|
options.velocity = config_.initialization().velocity();
|
|
options.acceleration = config_.initialization().acceleration();
|
|
options.cancellation_requested = activityCancellationRequested();
|
|
return runArmActuationIfCurrent([this, &init_cmd, &options] {
|
|
return arm_ && arm_->moveJ(init_cmd, options).ok();
|
|
});
|
|
}
|
|
|
|
bool TouchScreenTask::handleTouchTriggered(const bool stop_forward_motion) {
|
|
if (stop_forward_motion) {
|
|
if (!runArmStopIfCurrent([this] {
|
|
return arm_ && arm_->stopL().ok();
|
|
})) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
phase_ = Phase::DWELLING;
|
|
phase_start_time_ = Clock::now();
|
|
last_status_ = Status::TOUCH_TRIGGERED;
|
|
return true;
|
|
}
|
|
|
|
bool TouchScreenTask::startTouchPhase() {
|
|
if (!arm_) {
|
|
last_status_ = Status::ROBOT_COMMAND_FAILED;
|
|
return false;
|
|
}
|
|
|
|
phase_ = Phase::TOUCHING;
|
|
phase_start_time_ = Clock::now();
|
|
touch_command_started_ = true;
|
|
retract_command_started_ = false;
|
|
retract_start_position_valid_ = false;
|
|
retract_start_position_base_.setZero();
|
|
last_status_ = Status::TOUCHING;
|
|
|
|
if (config_.touch().motion_case() == cmvr::config::TouchScreenTaskTouchConfig::kSpeedL) {
|
|
touch_start_position_valid_ = readCurrentTouchPointPositionBase(touch_start_position_base_);
|
|
const auto& speed_l = config_.touch().speed_l();
|
|
if (speed_l.max_distance_m() > 0.0 && !touch_start_position_valid_) {
|
|
CMVR_LOG(ERROR) << "[TouchScreenTask] startTouchPhase failed: cannot read touch start pose "
|
|
<< "for speedL distance-based touching";
|
|
last_status_ = Status::ROBOT_STATE_FAILED;
|
|
return false;
|
|
}
|
|
const auto velocity = toCartesianVelocity(
|
|
cmvr::common::math::toEigenVec6(speed_l.twist_tool()));
|
|
if (!runArmActuationIfCurrent([this, velocity, &speed_l] {
|
|
return arm_ && arm_->speedL(
|
|
velocity,
|
|
speed_l.acceleration(),
|
|
0.0,
|
|
device::FrameType::Tool).ok();
|
|
})) {
|
|
last_status_ = Status::ROBOT_COMMAND_FAILED;
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
Eigen::Vector3d touch_forward_delta = Eigen::Vector3d::Zero();
|
|
const auto& move_l = config_.touch().move_l();
|
|
if (!computeLinearMoveDeltaTool(cmvr::common::math::toEigenVec6(move_l.direction_tool()),
|
|
move_l.distance_m(),
|
|
touch_forward_delta)) {
|
|
last_status_ = Status::INVALID_CONFIG;
|
|
return false;
|
|
}
|
|
|
|
device::CartesianPose pose_cmd;
|
|
pose_cmd.x = touch_forward_delta.x();
|
|
pose_cmd.y = touch_forward_delta.y();
|
|
pose_cmd.z = touch_forward_delta.z();
|
|
device::MotionOptions options;
|
|
options.velocity = move_l.velocity();
|
|
options.acceleration = move_l.acceleration();
|
|
options.jerk = move_l.jerk();
|
|
options.joint_velocity_limits.assign(move_l.joint_velocity_limits().begin(),
|
|
move_l.joint_velocity_limits().end());
|
|
options.cancellation_requested = activityCancellationRequested();
|
|
if (!runArmActuationIfCurrent([this, &pose_cmd, &options] {
|
|
return arm_ && arm_->moveL(
|
|
pose_cmd, options, device::FrameType::Tool).ok();
|
|
})) {
|
|
last_status_ = Status::ROBOT_COMMAND_FAILED;
|
|
return false;
|
|
}
|
|
|
|
if (!moveToInitPositionIfEnabled()) {
|
|
last_status_ = Status::ROBOT_COMMAND_FAILED;
|
|
return false;
|
|
}
|
|
|
|
finishActivityUnlocked(Phase::DONE, Status::DONE);
|
|
retract_start_position_valid_ = false;
|
|
retract_start_position_base_.setZero();
|
|
return true;
|
|
}
|
|
|
|
bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract,
|
|
const Status final_status_after_retract) {
|
|
if (!arm_) {
|
|
return false;
|
|
}
|
|
|
|
const auto& retract = config_.retract();
|
|
retract_start_position_valid_ = readCurrentTouchPointPositionBase(retract_start_position_base_);
|
|
const auto retract_cmd = toCartesianVelocity(
|
|
cmvr::common::math::toEigenVec6(retract.twist_tool()));
|
|
if (retract_start_position_valid_) {
|
|
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=["
|
|
<< retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz
|
|
<< ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz
|
|
<< "], acceleration=" << retract.acceleration()
|
|
<< ", duration_s=" << retract.duration_s()
|
|
<< ", start_tcp_base=[" << retract_start_position_base_.x() << ", "
|
|
<< retract_start_position_base_.y() << ", "
|
|
<< retract_start_position_base_.z() << "]";
|
|
} else {
|
|
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=["
|
|
<< retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz
|
|
<< ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz
|
|
<< "], acceleration=" << retract.acceleration()
|
|
<< ", duration_s=" << retract.duration_s()
|
|
<< ", start_tcp_base=unavailable";
|
|
}
|
|
|
|
if (!runArmActuationIfCurrent([this, &retract_cmd, &retract] {
|
|
return arm_ && arm_->speedL(
|
|
retract_cmd,
|
|
retract.acceleration(),
|
|
0.0,
|
|
device::FrameType::Tool).ok();
|
|
})) {
|
|
CMVR_LOG(ERROR) << "[TouchScreenTask][RETRACT_START] speedL failed";
|
|
return false;
|
|
}
|
|
|
|
phase_ = Phase::RETRACTING;
|
|
phase_after_retract_ = next_phase_after_retract;
|
|
final_status_after_retract_ = final_status_after_retract;
|
|
phase_start_time_ = Clock::now();
|
|
last_retract_log_time_ = phase_start_time_;
|
|
retract_command_started_ = true;
|
|
last_status_ = Status::RETRACTING;
|
|
return true;
|
|
}
|
|
|
|
void TouchScreenTask::enterFailed(const Status status) {
|
|
(void)runArmStopIfCurrent([this] {
|
|
return arm_ && arm_->stopL().ok();
|
|
});
|
|
hardStopIbvsMotion();
|
|
holdCurrentControlledPosition();
|
|
const auto final_status = moveToInitPositionIfEnabled()
|
|
? status
|
|
: Status::ROBOT_COMMAND_FAILED;
|
|
finishActivityUnlocked(Phase::FAILED, final_status);
|
|
}
|
|
|
|
bool TouchScreenTask::updateTouchPressure() {
|
|
last_touch_pressure_sum_ = 0.0;
|
|
last_touch_nonzero_count_ = 0;
|
|
if (!dexhand_) {
|
|
return false;
|
|
}
|
|
|
|
const auto& tactile = config_.touch().tactile();
|
|
std::vector<device::AbstractDexHand::TactileRegionKey> tactile_regions;
|
|
if (!appendRequestedTactileRegions(toFingerType(tactile.finger()),
|
|
toTactileRegion(tactile.region()),
|
|
tactile_regions)) {
|
|
return false;
|
|
}
|
|
|
|
double resultant_value = 0.0;
|
|
double resultant_fz = 0.0;
|
|
try {
|
|
for (const auto& tactile_region : tactile_regions) {
|
|
const auto resultant_force = dexhand_->getResultantForce(tactile_region.first, tactile_region.second);
|
|
resultant_value += tactileForceValue(resultant_force, tactile.criterion());
|
|
resultant_fz += static_cast<double>(resultant_force.fz);
|
|
}
|
|
} catch (...) {
|
|
return false;
|
|
}
|
|
|
|
last_touch_nonzero_count_ = std::abs(resultant_value) > 1e-9 ? 1 : 0;
|
|
last_touch_pressure_sum_ = resultant_value;
|
|
if (phase_ == Phase::TOUCHING) {
|
|
CMVR_LOG(DEBUG) << "[TouchScreenTask][TOUCHING][TACTILE] fz=" << resultant_fz
|
|
<< ", criterion_value=" << resultant_value
|
|
<< ", threshold=" << tactile.force_threshold()
|
|
;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
} // namespace cmvr::task
|