feat: add touch screen app class
This commit is contained in:
parent
a7ed28e2be
commit
245ac5c3c8
@ -121,6 +121,7 @@ target_link_libraries(cmvr_es PRIVATE
|
||||
cmvr_es::planner
|
||||
cmvr_es::device::humanoid_robot
|
||||
cmvr_es::common
|
||||
cmvr_es::applications
|
||||
)
|
||||
|
||||
install(TARGETS cmvr_es RUNTIME DESTINATION bin)
|
||||
|
||||
@ -11,5 +11,6 @@ add_subdirectory(planner)
|
||||
|
||||
add_subdirectory(ik_solver)
|
||||
add_subdirectory(data_center)
|
||||
add_subdirectory(applications)
|
||||
add_subdirectory(simulate)
|
||||
add_subdirectory(common)
|
||||
|
||||
25
cmvr-es/applications/CMakeLists.txt
Normal file
25
cmvr-es/applications/CMakeLists.txt
Normal file
@ -0,0 +1,25 @@
|
||||
add_library(applications
|
||||
src/touch_screen_app.cpp
|
||||
)
|
||||
|
||||
target_include_directories(applications PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
target_link_libraries(applications PUBLIC
|
||||
cmvr_es::controller
|
||||
)
|
||||
|
||||
add_library(cmvr_es::applications ALIAS applications)
|
||||
install(TARGETS applications LIBRARY DESTINATION lib)
|
||||
|
||||
add_executable(touch_screen_app_test
|
||||
src/touch_screen_app_test.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(touch_screen_app_test PRIVATE
|
||||
cmvr_es::applications
|
||||
cmvr_es::device_manager
|
||||
gtest
|
||||
gtest_main
|
||||
pthread
|
||||
glog
|
||||
)
|
||||
225
cmvr-es/applications/include/touch_screen_app.h
Normal file
225
cmvr-es/applications/include/touch_screen_app.h
Normal file
@ -0,0 +1,225 @@
|
||||
#pragma once
|
||||
|
||||
#ifndef CMVR_ES_TOUCH_SCREEN_APP_H
|
||||
#define CMVR_ES_TOUCH_SCREEN_APP_H
|
||||
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <Eigen/Dense>
|
||||
|
||||
#include "controller/include/ibvs_controller.h"
|
||||
#include "devices/camera/abstract_camera.h"
|
||||
#include "devices/dexhand/abstract_dexhand.h"
|
||||
#include "devices/robot/abstract_robot.h"
|
||||
#include "perception/include/apriltag_perception.h"
|
||||
#include "perception/include/tag_relative_target_3d.h"
|
||||
|
||||
namespace cmvr::app {
|
||||
|
||||
class TouchScreenApp {
|
||||
public:
|
||||
enum class Phase {
|
||||
IDLE = 0,
|
||||
ALIGNING,
|
||||
ALIGN_REACHED,
|
||||
TOUCHING,
|
||||
DWELLING,
|
||||
RETRACTING,
|
||||
DONE,
|
||||
FAILED
|
||||
};
|
||||
|
||||
enum class Status {
|
||||
IDLE = 0,
|
||||
NOT_INITIALIZED,
|
||||
INVALID_CONFIG,
|
||||
CONTROL_JOINT_MISMATCH,
|
||||
ALIGN_WAITING_PERCEPTION,
|
||||
ALIGN_WAITING_TRACK,
|
||||
ALIGN_TARGET_SETUP_FAILED,
|
||||
ALIGN_COMPUTE_FAILED,
|
||||
ALIGN_TIMEOUT,
|
||||
ALIGNING,
|
||||
ALIGN_REACHED,
|
||||
TOUCHING,
|
||||
TOUCH_TRIGGERED,
|
||||
TOUCH_TIMEOUT,
|
||||
RETRACTING,
|
||||
DONE,
|
||||
STOPPED,
|
||||
ROBOT_STATE_FAILED,
|
||||
ROBOT_COMMAND_FAILED
|
||||
};
|
||||
|
||||
enum class TactileRegion {
|
||||
TIP = 0,
|
||||
FINGER,
|
||||
PAD,
|
||||
TIP_AND_FINGER,
|
||||
THUMB_MIDDLE
|
||||
};
|
||||
|
||||
struct Options {
|
||||
// IBVS / IK 初始化参数。
|
||||
std::string urdf_path;
|
||||
std::string base_link{"PELVIS_S"};
|
||||
std::string flange_link{"R_WRIST_R_S"};
|
||||
std::string camera_link;
|
||||
|
||||
// 视觉感知参数。
|
||||
double tag_size_m{0.12};
|
||||
perception::AprilTagPerception::DepthPolicy depth_policy{
|
||||
perception::AprilTagPerception::DepthPolicy::NONE};
|
||||
perception::TagRelativeTarget3D::TargetPointMethod target_point_method{
|
||||
perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE};
|
||||
|
||||
// 视觉阶段目标：触控点在相机坐标系中的 hover 位置。
|
||||
Eigen::Vector3d hover_target_in_camera{0.0, 0.0, 0.12};
|
||||
double target_rx{3.14159265358979323846};
|
||||
double target_ry{0.0};
|
||||
double target_rz{0.0};
|
||||
|
||||
// IBVS 参数。
|
||||
double ibvs_lambda{0.7};
|
||||
double ibvs_mu{0.02};
|
||||
double ibvs_qdot_max{0.6};
|
||||
std::array<double, 6> ibvs_vmax6{{0.4, 0.4, 0.4, 0.4, 0.4, 0.4}};
|
||||
bool enable_joint_limit_avoidance{true};
|
||||
double joint_limit_avoidance_gain{0.2};
|
||||
double joint_limit_avoidance_margin_ratio{0.05};
|
||||
double joint_limit_avoidance_max_push{0.25};
|
||||
Eigen::Matrix3d R_camera_to_visp{Eigen::Matrix3d::Identity()};
|
||||
Eigen::Matrix3d R_camera_to_urdf{Eigen::Matrix3d::Identity()};
|
||||
|
||||
// 关节控制链，默认右臂 7 轴。
|
||||
std::vector<std::string> control_joint_names{
|
||||
"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
|
||||
"R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
|
||||
|
||||
// 视觉对准收敛判据。
|
||||
double align_xy_threshold_m{0.003};
|
||||
double align_z_threshold_m{0.010};
|
||||
int align_stable_frames{5};
|
||||
double align_timeout_s{10.0};
|
||||
bool pause_after_align_reached{false};
|
||||
|
||||
// 触控阶段：speedL 目标 twist（base_link 系）。
|
||||
Eigen::Matrix<double, 6, 1> touch_twist_base{
|
||||
(Eigen::Matrix<double, 6, 1>() << 0.0, 0.0, -0.02, 0.0, 0.0, 0.0).finished()};
|
||||
double touch_acceleration{0.6};
|
||||
double touch_timeout_s{2.0};
|
||||
|
||||
// 接触后停留与回退。
|
||||
double dwell_time_s{0.05};
|
||||
Eigen::Matrix<double, 6, 1> retract_twist_base{
|
||||
(Eigen::Matrix<double, 6, 1>() << 0.0, 0.0, 0.03, 0.0, 0.0, 0.0).finished()};
|
||||
double retract_acceleration{0.8};
|
||||
double retract_duration_s{0.20};
|
||||
|
||||
// 指尖触觉判据。
|
||||
device::FingerType tactile_finger{device::FingerType::INDEX};
|
||||
TactileRegion tactile_region{TactileRegion::FINGER};
|
||||
double tactile_pressure_sum_threshold{3000.0};
|
||||
double tactile_pressure_peak_threshold{0.0};
|
||||
};
|
||||
|
||||
TouchScreenApp();
|
||||
~TouchScreenApp() = default;
|
||||
|
||||
bool init(const std::shared_ptr<device::AbstractRobot>& robot,
|
||||
const std::shared_ptr<device::AbstractDexHand>& dexhand,
|
||||
const std::shared_ptr<device::AbstractCamera>& camera,
|
||||
const Options& options);
|
||||
|
||||
void setOptions(const Options& options);
|
||||
const Options& options() const { return options_; }
|
||||
|
||||
bool startFromPixel(int u, int v);
|
||||
bool step();
|
||||
void stop();
|
||||
|
||||
Phase phase() const { return phase_; }
|
||||
Status lastStatus() const { return last_status_; }
|
||||
static const char* phaseToString(Phase phase);
|
||||
static const char* statusToString(Status status);
|
||||
|
||||
bool isBusy() const { return phase_ == Phase::ALIGNING || phase_ == Phase::ALIGN_REACHED ||
|
||||
phase_ == Phase::TOUCHING || phase_ == Phase::DWELLING ||
|
||||
phase_ == Phase::RETRACTING; }
|
||||
bool isFinished() const { return phase_ == Phase::DONE; }
|
||||
bool isFailed() const { return phase_ == Phase::FAILED; }
|
||||
|
||||
int targetU() const { return target_u_; }
|
||||
int targetV() const { return target_v_; }
|
||||
double lastTouchPressureSum() const { return last_touch_pressure_sum_; }
|
||||
double lastTouchPressurePeak() const { return last_touch_pressure_peak_; }
|
||||
int lastActiveTagId() const { return last_active_tag_id_; }
|
||||
const Eigen::Vector3d& lastAlignErrorCamera() const { return last_align_error_camera_; }
|
||||
|
||||
const std::shared_ptr<perception::AprilTagPerception>& perception() const { return perception_; }
|
||||
const perception::TagRelativeTarget3D& tracker() const { return tracker_; }
|
||||
const IbvsController& ibvs() const { return ibvs_; }
|
||||
|
||||
private:
|
||||
using Clock = std::chrono::steady_clock;
|
||||
|
||||
bool applyOptions();
|
||||
bool validateControlJointNames() const;
|
||||
bool stepAligning();
|
||||
bool stepTouching();
|
||||
bool stepDwelling();
|
||||
bool stepRetracting();
|
||||
|
||||
bool readControlledJointPositions(std::vector<double>& q_out) const;
|
||||
bool sendJointVelocity(const std::vector<double>& qdot) const;
|
||||
bool sendZeroJointVelocity() const;
|
||||
bool holdCurrentControlledPosition() const;
|
||||
|
||||
bool startTouchPhase();
|
||||
bool startRetractPhase(Phase next_phase_after_retract, Status final_status_after_retract);
|
||||
void enterFailed(Status status);
|
||||
|
||||
bool updateTouchPressure();
|
||||
|
||||
private:
|
||||
std::shared_ptr<device::AbstractRobot> robot_{nullptr};
|
||||
std::shared_ptr<device::AbstractDexHand> dexhand_{nullptr};
|
||||
std::shared_ptr<device::AbstractCamera> camera_{nullptr};
|
||||
|
||||
std::shared_ptr<perception::AprilTagPerception> perception_{nullptr};
|
||||
perception::TagRelativeTarget3D tracker_;
|
||||
IbvsController ibvs_;
|
||||
Options options_{};
|
||||
|
||||
Phase phase_{Phase::IDLE};
|
||||
Phase phase_after_retract_{Phase::DONE};
|
||||
Status last_status_{Status::NOT_INITIALIZED};
|
||||
|
||||
bool initialized_{false};
|
||||
bool target_locked_{false};
|
||||
bool ibvs_target_initialized_{false};
|
||||
bool touch_command_started_{false};
|
||||
bool retract_command_started_{false};
|
||||
|
||||
int target_u_{-1};
|
||||
int target_v_{-1};
|
||||
int align_stable_count_{0};
|
||||
int last_active_tag_id_{-1};
|
||||
|
||||
double last_touch_pressure_sum_{0.0};
|
||||
double last_touch_pressure_peak_{0.0};
|
||||
Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()};
|
||||
|
||||
Clock::time_point phase_start_time_{};
|
||||
Status final_status_after_retract_{Status::DONE};
|
||||
};
|
||||
|
||||
using touch_screen_app = TouchScreenApp;
|
||||
|
||||
} // namespace cmvr::app
|
||||
|
||||
#endif // CMVR_ES_TOUCH_SCREEN_APP_H
|
||||
704
cmvr-es/applications/src/touch_screen_app.cpp
Normal file
704
cmvr-es/applications/src/touch_screen_app.cpp
Normal file
@ -0,0 +1,704 @@
|
||||
#include "applications/include/touch_screen_app.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <exception>
|
||||
#include <unordered_map>
|
||||
|
||||
namespace cmvr::app {
|
||||
|
||||
namespace {
|
||||
|
||||
std::vector<double> toStdVector6(const Eigen::Matrix<double, 6, 1>& twist) {
|
||||
std::vector<double> out(6, 0.0);
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
out[static_cast<size_t>(i)] = twist[i];
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
void accumulateMatrixStats(const std::vector<std::vector<device::TactilePoint>>& matrix,
|
||||
double& sum_out,
|
||||
double& peak_out) {
|
||||
for (const auto& row : matrix) {
|
||||
for (const auto value : row) {
|
||||
const double v = static_cast<double>(value);
|
||||
sum_out += v;
|
||||
peak_out = std::max(peak_out, v);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TouchScreenApp::TouchScreenApp()
|
||||
: tracker_(nullptr) {}
|
||||
|
||||
bool TouchScreenApp::init(const std::shared_ptr<device::AbstractRobot>& robot,
|
||||
const std::shared_ptr<device::AbstractDexHand>& dexhand,
|
||||
const std::shared_ptr<device::AbstractCamera>& camera,
|
||||
const Options& options) {
|
||||
robot_ = robot;
|
||||
dexhand_ = dexhand;
|
||||
camera_ = camera;
|
||||
options_ = options;
|
||||
|
||||
if (!robot_ || !dexhand_ || !camera_) {
|
||||
initialized_ = false;
|
||||
last_status_ = Status::INVALID_CONFIG;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (options_.urdf_path.empty() || options_.camera_link.empty() || options_.control_joint_names.empty()) {
|
||||
initialized_ = false;
|
||||
last_status_ = Status::INVALID_CONFIG;
|
||||
return false;
|
||||
}
|
||||
|
||||
perception_ = std::make_shared<perception::AprilTagPerception>(camera_);
|
||||
perception_->setTagSize(options_.tag_size_m);
|
||||
|
||||
tracker_.setPerception(perception_);
|
||||
tracker_.setTargetPointMethod(options_.target_point_method);
|
||||
|
||||
if (!ibvs_.init(options_.urdf_path,
|
||||
options_.base_link,
|
||||
options_.flange_link,
|
||||
options_.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_ = applyOptions();
|
||||
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_pressure_peak_ = 0.0;
|
||||
last_align_error_camera_.setZero();
|
||||
last_status_ = Status::IDLE;
|
||||
} else {
|
||||
last_status_ = Status::INVALID_CONFIG;
|
||||
}
|
||||
return initialized_;
|
||||
}
|
||||
|
||||
void TouchScreenApp::setOptions(const Options& options) {
|
||||
options_ = options;
|
||||
if (initialized_) {
|
||||
applyOptions();
|
||||
}
|
||||
}
|
||||
|
||||
bool TouchScreenApp::startFromPixel(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;
|
||||
}
|
||||
|
||||
stop();
|
||||
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;
|
||||
last_touch_pressure_sum_ = 0.0;
|
||||
last_touch_pressure_peak_ = 0.0;
|
||||
last_active_tag_id_ = -1;
|
||||
last_align_error_camera_.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 TouchScreenApp::step() {
|
||||
if (!initialized_) {
|
||||
last_status_ = Status::NOT_INITIALIZED;
|
||||
return false;
|
||||
}
|
||||
|
||||
switch (phase_) {
|
||||
case Phase::IDLE:
|
||||
last_status_ = Status::IDLE;
|
||||
return true;
|
||||
case Phase::ALIGNING:
|
||||
return stepAligning();
|
||||
case Phase::ALIGN_REACHED:
|
||||
last_status_ = Status::ALIGN_REACHED;
|
||||
if (options_.pause_after_align_reached) {
|
||||
return true;
|
||||
}
|
||||
if (!startTouchPhase()) {
|
||||
enterFailed(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;
|
||||
}
|
||||
last_status_ = Status::INVALID_CONFIG;
|
||||
return false;
|
||||
}
|
||||
|
||||
void TouchScreenApp::stop() {
|
||||
if (robot_) {
|
||||
try {
|
||||
robot_->stopSpeedL();
|
||||
} catch (...) {
|
||||
}
|
||||
}
|
||||
|
||||
sendZeroJointVelocity();
|
||||
holdCurrentControlledPosition();
|
||||
|
||||
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_pressure_peak_ = 0.0;
|
||||
last_align_error_camera_.setZero();
|
||||
last_status_ = Status::STOPPED;
|
||||
}
|
||||
|
||||
const char* TouchScreenApp::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* TouchScreenApp::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::TOUCH_TRIGGERED: return "TOUCH_TRIGGERED";
|
||||
case Status::TOUCH_TIMEOUT: return "TOUCH_TIMEOUT";
|
||||
case Status::RETRACTING: return "RETRACTING";
|
||||
case Status::DONE: return "DONE";
|
||||
case Status::STOPPED: return "STOPPED";
|
||||
case Status::ROBOT_STATE_FAILED: return "ROBOT_STATE_FAILED";
|
||||
case Status::ROBOT_COMMAND_FAILED: return "ROBOT_COMMAND_FAILED";
|
||||
}
|
||||
return "UNKNOWN";
|
||||
}
|
||||
|
||||
bool TouchScreenApp::applyOptions() {
|
||||
if (!perception_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
perception_->setTagSize(options_.tag_size_m);
|
||||
tracker_.setTargetPointMethod(options_.target_point_method);
|
||||
ibvs_.setLambda(options_.ibvs_lambda);
|
||||
ibvs_.setMu(options_.ibvs_mu);
|
||||
ibvs_.setQdotMax(options_.ibvs_qdot_max);
|
||||
ibvs_.setVelocityLimit6(options_.ibvs_vmax6);
|
||||
ibvs_.setJointLimitAvoidance(options_.enable_joint_limit_avoidance,
|
||||
options_.joint_limit_avoidance_gain,
|
||||
options_.joint_limit_avoidance_margin_ratio,
|
||||
options_.joint_limit_avoidance_max_push);
|
||||
ibvs_.setAlignCameraToVisp(options_.R_camera_to_visp);
|
||||
ibvs_.setAlignCameraToUrdf(options_.R_camera_to_urdf);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool TouchScreenApp::validateControlJointNames() const {
|
||||
std::vector<std::string> solver_joint_names;
|
||||
if (!ibvs_.getChainJointNames(solver_joint_names)) {
|
||||
return false;
|
||||
}
|
||||
if (solver_joint_names == options_.control_joint_names) {
|
||||
return true;
|
||||
}
|
||||
|
||||
std::cerr << "[TouchScreenApp] control_joint_names mismatch with IbvsController IK chain\n";
|
||||
std::cerr << " app joints :";
|
||||
for (const auto& name : options_.control_joint_names) {
|
||||
std::cerr << " " << name;
|
||||
}
|
||||
std::cerr << "\n solver joints:";
|
||||
for (const auto& name : solver_joint_names) {
|
||||
std::cerr << " " << name;
|
||||
}
|
||||
std::cerr << '\n';
|
||||
return false;
|
||||
}
|
||||
|
||||
bool TouchScreenApp::stepAligning() {
|
||||
const auto now = Clock::now();
|
||||
const double elapsed = std::chrono::duration<double>(now - phase_start_time_).count();
|
||||
if (elapsed > options_.align_timeout_s) {
|
||||
enterFailed(Status::ALIGN_TIMEOUT);
|
||||
return false;
|
||||
}
|
||||
|
||||
perception::AprilTagPerception::Options perception_options;
|
||||
perception_options.depth_policy = options_.depth_policy;
|
||||
perception_options.detect_tags = true;
|
||||
perception_options.fetch_encoded = false;
|
||||
if (!perception_->update(perception_options)) {
|
||||
sendZeroJointVelocity();
|
||||
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) {
|
||||
sendZeroJointVelocity();
|
||||
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) {
|
||||
sendZeroJointVelocity();
|
||||
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)) {
|
||||
sendZeroJointVelocity();
|
||||
align_stable_count_ = 0;
|
||||
last_status_ = Status::ALIGN_WAITING_TRACK;
|
||||
return true;
|
||||
}
|
||||
|
||||
ibvs_.setTrackedTagId(tag_id);
|
||||
if (!ibvs_target_initialized_ || tracker_.lastSwitched()) {
|
||||
if (!ibvs_.setTargetFromPointInTag(p_t_target,
|
||||
options_.hover_target_in_camera,
|
||||
options_.target_rx,
|
||||
options_.target_ry,
|
||||
options_.target_rz)) {
|
||||
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_.compute(q_now, 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:
|
||||
sendZeroJointVelocity();
|
||||
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 (!sendJointVelocity(qdot_cmd)) {
|
||||
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
||||
return false;
|
||||
}
|
||||
|
||||
last_align_error_camera_ = tracker_.lastTargetInCamera() - options_.hover_target_in_camera;
|
||||
if (std::abs(last_align_error_camera_.x()) <= options_.align_xy_threshold_m &&
|
||||
std::abs(last_align_error_camera_.y()) <= options_.align_xy_threshold_m &&
|
||||
std::abs(last_align_error_camera_.z()) <= options_.align_z_threshold_m) {
|
||||
++align_stable_count_;
|
||||
} else {
|
||||
align_stable_count_ = 0;
|
||||
}
|
||||
|
||||
if (align_stable_count_ >= options_.align_stable_frames) {
|
||||
sendZeroJointVelocity();
|
||||
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 TouchScreenApp::stepTouching() {
|
||||
if (!touch_command_started_) {
|
||||
if (!startTouchPhase()) {
|
||||
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
updateTouchPressure();
|
||||
if (last_touch_pressure_sum_ >= options_.tactile_pressure_sum_threshold ||
|
||||
(options_.tactile_pressure_peak_threshold > 0.0 &&
|
||||
last_touch_pressure_peak_ >= options_.tactile_pressure_peak_threshold)) {
|
||||
try {
|
||||
robot_->stopSpeedL();
|
||||
} catch (...) {
|
||||
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
||||
return false;
|
||||
}
|
||||
phase_ = Phase::DWELLING;
|
||||
phase_start_time_ = Clock::now();
|
||||
last_status_ = Status::TOUCH_TRIGGERED;
|
||||
return true;
|
||||
}
|
||||
|
||||
const double elapsed = std::chrono::duration<double>(Clock::now() - phase_start_time_).count();
|
||||
if (elapsed > options_.touch_timeout_s) {
|
||||
try {
|
||||
robot_->stopSpeedL();
|
||||
} catch (...) {
|
||||
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
||||
return false;
|
||||
}
|
||||
if (!startRetractPhase(Phase::FAILED, Status::TOUCH_TIMEOUT)) {
|
||||
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
last_status_ = Status::TOUCHING;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool TouchScreenApp::stepDwelling() {
|
||||
const double elapsed = std::chrono::duration<double>(Clock::now() - phase_start_time_).count();
|
||||
if (elapsed < options_.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 TouchScreenApp::stepRetracting() {
|
||||
if (!retract_command_started_) {
|
||||
if (!startRetractPhase(phase_after_retract_, final_status_after_retract_)) {
|
||||
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
const double elapsed = std::chrono::duration<double>(Clock::now() - phase_start_time_).count();
|
||||
if (elapsed < options_.retract_duration_s) {
|
||||
last_status_ = Status::RETRACTING;
|
||||
return true;
|
||||
}
|
||||
|
||||
try {
|
||||
robot_->stopSpeedL();
|
||||
} catch (...) {
|
||||
enterFailed(Status::ROBOT_COMMAND_FAILED);
|
||||
return false;
|
||||
}
|
||||
holdCurrentControlledPosition();
|
||||
phase_ = phase_after_retract_;
|
||||
last_status_ = final_status_after_retract_;
|
||||
return phase_ != Phase::FAILED;
|
||||
}
|
||||
|
||||
bool TouchScreenApp::readControlledJointPositions(std::vector<double>& q_out) const {
|
||||
if (!robot_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::vector<device::JointState> states;
|
||||
robot_->getJointsState(states);
|
||||
std::unordered_map<std::string, double> q_map;
|
||||
q_map.reserve(states.size());
|
||||
for (const auto& state : states) {
|
||||
q_map[state.name] = state.position;
|
||||
}
|
||||
|
||||
q_out.resize(options_.control_joint_names.size());
|
||||
for (size_t i = 0; i < options_.control_joint_names.size(); ++i) {
|
||||
const auto it = q_map.find(options_.control_joint_names[i]);
|
||||
if (it == q_map.end()) {
|
||||
return false;
|
||||
}
|
||||
q_out[i] = it->second;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool TouchScreenApp::sendJointVelocity(const std::vector<double>& qdot) const {
|
||||
if (!robot_ || qdot.size() != options_.control_joint_names.size()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::vector<device::JointVelocityCommand> cmd;
|
||||
cmd.reserve(qdot.size());
|
||||
for (size_t i = 0; i < qdot.size(); ++i) {
|
||||
cmd.push_back({options_.control_joint_names[i], qdot[i]});
|
||||
}
|
||||
|
||||
try {
|
||||
robot_->speedJ(cmd);
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool TouchScreenApp::sendZeroJointVelocity() const {
|
||||
std::vector<double> zero(options_.control_joint_names.size(), 0.0);
|
||||
return sendJointVelocity(zero);
|
||||
}
|
||||
|
||||
bool TouchScreenApp::holdCurrentControlledPosition() const {
|
||||
if (!robot_) {
|
||||
return false;
|
||||
}
|
||||
|
||||
std::vector<device::JointState> states;
|
||||
robot_->getJointsState(states);
|
||||
std::unordered_map<std::string, double> q_map;
|
||||
q_map.reserve(states.size());
|
||||
for (const auto& state : states) {
|
||||
q_map[state.name] = state.position;
|
||||
}
|
||||
|
||||
std::vector<device::JointPoint> joints;
|
||||
joints.reserve(options_.control_joint_names.size());
|
||||
for (const auto& name : options_.control_joint_names) {
|
||||
const auto it = q_map.find(name);
|
||||
if (it == q_map.end()) {
|
||||
return false;
|
||||
}
|
||||
joints.emplace_back(name, it->second, 0.0);
|
||||
}
|
||||
|
||||
try {
|
||||
robot_->servoJ(joints, 0.02);
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool TouchScreenApp::startTouchPhase() {
|
||||
if (!robot_) {
|
||||
return false;
|
||||
}
|
||||
try {
|
||||
if (!robot_->speedL(toStdVector6(options_.touch_twist_base),
|
||||
options_.touch_acceleration,
|
||||
0.0)) {
|
||||
return false;
|
||||
}
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
phase_ = Phase::TOUCHING;
|
||||
phase_start_time_ = Clock::now();
|
||||
touch_command_started_ = true;
|
||||
retract_command_started_ = false;
|
||||
last_status_ = Status::TOUCHING;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool TouchScreenApp::startRetractPhase(const Phase next_phase_after_retract,
|
||||
const Status final_status_after_retract) {
|
||||
if (!robot_) {
|
||||
return false;
|
||||
}
|
||||
try {
|
||||
if (!robot_->speedL(toStdVector6(options_.retract_twist_base),
|
||||
options_.retract_acceleration,
|
||||
0.0)) {
|
||||
return false;
|
||||
}
|
||||
} catch (...) {
|
||||
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();
|
||||
retract_command_started_ = true;
|
||||
last_status_ = Status::RETRACTING;
|
||||
return true;
|
||||
}
|
||||
|
||||
void TouchScreenApp::enterFailed(const Status status) {
|
||||
try {
|
||||
if (robot_) {
|
||||
robot_->stopSpeedL();
|
||||
}
|
||||
} catch (...) {
|
||||
}
|
||||
sendZeroJointVelocity();
|
||||
holdCurrentControlledPosition();
|
||||
phase_ = Phase::FAILED;
|
||||
touch_command_started_ = false;
|
||||
retract_command_started_ = false;
|
||||
last_status_ = status;
|
||||
}
|
||||
|
||||
bool TouchScreenApp::updateTouchPressure() {
|
||||
if (!dexhand_) {
|
||||
last_touch_pressure_sum_ = 0.0;
|
||||
last_touch_pressure_peak_ = 0.0;
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto& sensors = dexhand_->getSensorData();
|
||||
double sum = 0.0;
|
||||
double peak = 0.0;
|
||||
|
||||
auto accumulate_finger = [&](const auto& finger_sensor) {
|
||||
switch (options_.tactile_region) {
|
||||
case TactileRegion::TIP:
|
||||
accumulateMatrixStats(finger_sensor.tip.data, sum, peak);
|
||||
break;
|
||||
case TactileRegion::FINGER:
|
||||
accumulateMatrixStats(finger_sensor.finger.data, sum, peak);
|
||||
break;
|
||||
case TactileRegion::PAD:
|
||||
accumulateMatrixStats(finger_sensor.pad.data, sum, peak);
|
||||
break;
|
||||
case TactileRegion::TIP_AND_FINGER:
|
||||
accumulateMatrixStats(finger_sensor.tip.data, sum, peak);
|
||||
accumulateMatrixStats(finger_sensor.finger.data, sum, peak);
|
||||
break;
|
||||
case TactileRegion::THUMB_MIDDLE:
|
||||
break;
|
||||
}
|
||||
};
|
||||
|
||||
switch (options_.tactile_finger) {
|
||||
case device::FingerType::PINKY:
|
||||
accumulate_finger(sensors.pinky);
|
||||
break;
|
||||
case device::FingerType::RING:
|
||||
accumulate_finger(sensors.ring);
|
||||
break;
|
||||
case device::FingerType::MIDDLE:
|
||||
accumulate_finger(sensors.middle);
|
||||
break;
|
||||
case device::FingerType::INDEX:
|
||||
accumulate_finger(sensors.index);
|
||||
break;
|
||||
case device::FingerType::THUMB:
|
||||
switch (options_.tactile_region) {
|
||||
case TactileRegion::TIP:
|
||||
accumulateMatrixStats(sensors.thumb.tip.data, sum, peak);
|
||||
break;
|
||||
case TactileRegion::FINGER:
|
||||
accumulateMatrixStats(sensors.thumb.finger.data, sum, peak);
|
||||
break;
|
||||
case TactileRegion::PAD:
|
||||
accumulateMatrixStats(sensors.thumb.pad.data, sum, peak);
|
||||
break;
|
||||
case TactileRegion::TIP_AND_FINGER:
|
||||
accumulateMatrixStats(sensors.thumb.tip.data, sum, peak);
|
||||
accumulateMatrixStats(sensors.thumb.finger.data, sum, peak);
|
||||
break;
|
||||
case TactileRegion::THUMB_MIDDLE:
|
||||
accumulateMatrixStats(sensors.thumb.middle.data, sum, peak);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
last_touch_pressure_sum_ = sum;
|
||||
last_touch_pressure_peak_ = peak;
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace cmvr::app
|
||||
112
cmvr-es/applications/src/touch_screen_app_test.cpp
Normal file
112
cmvr-es/applications/src/touch_screen_app_test.cpp
Normal file
@ -0,0 +1,112 @@
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
|
||||
#include "applications/include/touch_screen_app.h"
|
||||
#include "device_manager/include/device_manager.h"
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr const char* kConfigPath =
|
||||
"/home/lgv/cmvr/0-workspace/cmvr-es/cmvr-es/common/config/cabin_robot.xml";
|
||||
// 这些 id 需要与现场配置一致；保持为示例调用中的写法。
|
||||
constexpr const char* kRobotId = "hc01";
|
||||
constexpr const char* kDexhandId = "dexhand1";
|
||||
constexpr const char* kCameraId = "cam1";
|
||||
constexpr const char* kUrdfPath =
|
||||
"/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf";
|
||||
constexpr const char* kBaseLink = "PELVIS_S";
|
||||
constexpr const char* kFlangeLink = "R_WRIST_R_S";
|
||||
constexpr const char* kCameraLink = "R_CAM";
|
||||
constexpr int kTargetU = 320;
|
||||
constexpr int kTargetV = 240;
|
||||
|
||||
void run_touch_once(int u, int v) {
|
||||
const XmlNode config(kConfigPath);
|
||||
if (!config.hasChild("DeviceManager")) {
|
||||
std::cerr << "DeviceManager node not found\n";
|
||||
return;
|
||||
}
|
||||
|
||||
auto& dm = cmvr::device::DeviceManager::getInstance(config.getChild("DeviceManager"));
|
||||
|
||||
auto robot = dm.getDevice<cmvr::device::AbstractRobot>(kRobotId);
|
||||
auto dexhand = dm.getDevice<cmvr::device::AbstractDexHand>(kDexhandId);
|
||||
auto camera = dm.getDevice<cmvr::device::AbstractCamera>(kCameraId);
|
||||
|
||||
|
||||
cmvr::app::TouchScreenApp app;
|
||||
cmvr::app::TouchScreenApp::Options opt;
|
||||
|
||||
opt.urdf_path = kUrdfPath;
|
||||
opt.base_link = kBaseLink;
|
||||
opt.flange_link = kFlangeLink;
|
||||
opt.camera_link = kCameraLink;
|
||||
opt.tag_size_m = 0.02;
|
||||
|
||||
opt.hover_target_in_camera = Eigen::Vector3d(0.0, 0.0, 0.12);
|
||||
opt.pause_after_align_reached = true;
|
||||
|
||||
// 只测试视觉对齐阶段：禁止进入真实下压。
|
||||
opt.touch_twist_base <<
|
||||
0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0;
|
||||
opt.touch_acceleration = 0.6;
|
||||
|
||||
opt.retract_twist_base <<
|
||||
0.0, 0.0, 0.03,
|
||||
0.0, 0.0, 0.0;
|
||||
opt.retract_duration_s = 0.20;
|
||||
|
||||
opt.tactile_finger = cmvr::device::FingerType::INDEX;
|
||||
opt.tactile_region = cmvr::app::TouchScreenApp::TactileRegion::FINGER;
|
||||
opt.tactile_pressure_sum_threshold = 1e12;
|
||||
|
||||
if (!app.init(robot, dexhand, camera, opt)) {
|
||||
std::cerr << "TouchScreenApp init failed\n";
|
||||
return;
|
||||
}
|
||||
|
||||
if (!app.startFromPixel(u, v)) {
|
||||
std::cerr << "startFromPixel failed\n";
|
||||
return;
|
||||
}
|
||||
|
||||
while (app.isBusy()) {
|
||||
if (!app.step()) {
|
||||
std::cerr << "touch failed, status="
|
||||
<< cmvr::app::TouchScreenApp::statusToString(app.lastStatus())
|
||||
<< "\n";
|
||||
break;
|
||||
}
|
||||
std::cout << "phase=" << cmvr::app::TouchScreenApp::phaseToString(app.phase())
|
||||
<< ", status=" << cmvr::app::TouchScreenApp::statusToString(app.lastStatus())
|
||||
<< ", active_tag=" << app.lastActiveTagId()
|
||||
<< ", err_c=[" << app.lastAlignErrorCamera().x() << ", "
|
||||
<< app.lastAlignErrorCamera().y() << ", "
|
||||
<< app.lastAlignErrorCamera().z() << "]\n";
|
||||
|
||||
if (app.lastStatus() == cmvr::app::TouchScreenApp::Status::ALIGN_REACHED) {
|
||||
std::cout << "align reached\n";
|
||||
app.stop();
|
||||
break;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
|
||||
if (app.isFinished()) {
|
||||
std::cout << "touch done\n";
|
||||
} else if (app.isFailed()) {
|
||||
std::cout << "touch failed: "
|
||||
<< cmvr::app::TouchScreenApp::statusToString(app.lastStatus())
|
||||
<< "\n";
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(TouchScreenAppTest, RunTouchOnceOnRealRobot) {
|
||||
run_touch_once(kTargetU, kTargetV);
|
||||
}
|
||||
@ -241,6 +241,13 @@ public:
|
||||
// 最近一次输出的相机 twist，位于 ViSP 相机坐标系 `c`。
|
||||
const Eigen::Matrix<double, 6, 1>& lastCameraTwistVisp() const { return last_v_camera_visp_; }
|
||||
|
||||
/**
|
||||
* @brief 获取当前 IK 链的关节名称（base->camera 顺序）。
|
||||
* @param joint_names 输出关节名称列表。
|
||||
* @return 已初始化且成功读取时返回 `true`。
|
||||
*/
|
||||
bool getChainJointNames(std::vector<std::string>& joint_names) const;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief 核心控制链路：由感知缓存和当前关节状态计算关节速度命令。
|
||||
|
||||
@ -540,7 +540,7 @@ protected:
|
||||
ibvs_controller_->setDepthZGain(1.0);
|
||||
ibvs_controller_->setVelocityLimit6(vmax6_);
|
||||
ibvs_controller_->setTrackedTagId(tracked_tag_id_);
|
||||
ibvs_controller_->setTargetFromPointInTag(Eigen::Vector3d(0.08, 0.0, 0),
|
||||
ibvs_controller_->setTargetFromPointInTag(Eigen::Vector3d(0.08, 0.05, 0),
|
||||
Eigen::Vector3d(0.0, 0.0, 0.4));
|
||||
ibvs_controller_->setJointLimitAvoidance(true, 0.2, 0.15, 0.25);
|
||||
|
||||
|
||||
@ -146,6 +146,14 @@ bool IbvsController::compute(const std::vector<double>& joints_angle,
|
||||
return computeInternal(joints_angle, qdot_out);
|
||||
}
|
||||
|
||||
bool IbvsController::getChainJointNames(std::vector<std::string>& joint_names) const {
|
||||
if (!dls_solver_) {
|
||||
joint_names.clear();
|
||||
return false;
|
||||
}
|
||||
return dls_solver_->getChainJointNames(joint_names);
|
||||
}
|
||||
|
||||
bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
|
||||
std::vector<double>& qdot_out) {
|
||||
last_depth_usage_ = DepthUsage::NONE;
|
||||
@ -284,9 +292,15 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
|
||||
return false;
|
||||
}
|
||||
|
||||
Eigen::Map<const Eigen::VectorXd> q_chain(joints_angle.data(), static_cast<Eigen::Index>(joints_angle.size()));
|
||||
Eigen::Map<const Eigen::VectorXd> qdot_vec(qdot.data(), static_cast<Eigen::Index>(qdot.size()));
|
||||
const Eigen::VectorXd qdot_soft_limited = dls_solver_->applyJointSoftLimitVelocity(q_chain, qdot_vec);
|
||||
|
||||
qdot_out.resize(qdot.size());
|
||||
for (size_t i = 0; i < qdot.size(); ++i) {
|
||||
qdot_out[i] = SupportFunctions::clamp(qdot[i], -qdot_max_, qdot_max_);
|
||||
qdot_out[i] = SupportFunctions::clamp(qdot_soft_limited[static_cast<Eigen::Index>(i)],
|
||||
-qdot_max_,
|
||||
qdot_max_);
|
||||
}
|
||||
|
||||
last_compute_status_ = ComputeStatus::OK;
|
||||
|
||||
@ -90,6 +90,22 @@ public:
|
||||
double damping = -1.0,
|
||||
double qdot_abs_max = std::numeric_limits<double>::infinity());
|
||||
|
||||
/**
|
||||
* @brief 对靠近关节位置限位且仍继续向外运动的关节速度做软压缩。
|
||||
*
|
||||
* 该接口不改变主任务的求解方式，只在得到关节速度命令后做逐轴后处理。
|
||||
* 软/硬边界参数来自当前 speedL 配置中的：
|
||||
* - `joint_soft_limit_margin`
|
||||
* - `joint_hard_limit_margin`
|
||||
* - `enable_joint_soft_limit_velocity`
|
||||
*
|
||||
* @param q_chain 当前链关节位置，size=chain_dof_，单位 rad。
|
||||
* @param qdot_des 待后处理的关节速度命令，size=chain_v_dof_，单位 rad/s。
|
||||
* @return 经过软限位速度压缩后的关节速度命令。
|
||||
*/
|
||||
Eigen::VectorXd applyJointSoftLimitVelocity(const Eigen::VectorXd& q_chain,
|
||||
const Eigen::VectorXd& qdot_des);
|
||||
|
||||
void setJointLimitAvoidance(bool enable,
|
||||
double gain = 0.2,
|
||||
double margin_ratio = 0.15,
|
||||
@ -245,8 +261,6 @@ private:
|
||||
Eigen::VectorXd* q_full_out = nullptr);
|
||||
|
||||
Eigen::VectorXd applyJointVelocityLimits(const Eigen::VectorXd& qdot_des) const;
|
||||
Eigen::VectorXd applyJointSoftLimitVelocity(const Eigen::VectorXd& q_chain,
|
||||
const Eigen::VectorXd& qdot_des);
|
||||
|
||||
Eigen::VectorXd applyJointAccelerationLimits(const Eigen::VectorXd& qdot_des,
|
||||
const Eigen::VectorXd& qdot_reference,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user