diff --git a/cmvr-es/algorithms/controllers/CMakeLists.txt b/cmvr-es/algorithms/controllers/CMakeLists.txt index 072d4eb4..1921842a 100644 --- a/cmvr-es/algorithms/controllers/CMakeLists.txt +++ b/cmvr-es/algorithms/controllers/CMakeLists.txt @@ -58,29 +58,3 @@ target_link_libraries(controller PUBLIC add_library(cmvr_es::algorithms::controller ALIAS controller) install(TARGETS controller LIBRARY DESTINATION lib) - - -# -------------------------------------------------------- -# Unit test -# -------------------------------------------------------- -find_package(realsense2 REQUIRED) -add_executable(controller_test - ${CMAKE_CURRENT_SOURCE_DIR}/tests/src/controller_test.cpp -) - -target_link_libraries(controller_test - PRIVATE - cmvr_es::perception - cmvr_es::ik_solver - cmvr_es::base_motion - cmvr_es::proto - cmvr_es::mujoco_viewer - cmvr_es::algorithms::controller - cmvr_es::device::mujoco_camera - gtest - gtest_main - pthread - glog - ${VISP_LIBRARIES} - realsense2::realsense2 -) diff --git a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h index 8d59bcfb..9dd0fc29 100644 --- a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h +++ b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h @@ -24,6 +24,7 @@ public: double stop_twist_norm{1e-9}; double stop_command_velocity_norm{1e-3}; double stop_measured_velocity_norm{1e-2}; + double stop_acceleration{0.5}; }; using ReadStateCallback = std::function& q, std::vector& qd)>; diff --git a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp index c18dddcd..efad63b6 100644 --- a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp +++ b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp @@ -26,6 +26,9 @@ CartesianVelocityController::Config normalizeConfig(CartesianVelocityController: if (config.stop_measured_velocity_norm <= 0.0) { config.stop_measured_velocity_norm = defaults.stop_measured_velocity_norm; } + if (config.stop_acceleration <= 0.0) { + config.stop_acceleration = defaults.stop_acceleration; + } return config; } @@ -104,9 +107,7 @@ Result CartesianVelocityController::stop(const std::optional acceleratio std::lock_guard lock(mutex_); target_twist_ = {}; target_frame_ = FrameType::Base; - if (acceleration.has_value()) { - target_acceleration_ = *acceleration; - } + target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration; command_active_ = true; ++command_version_; } @@ -191,6 +192,13 @@ void CartesianVelocityController::workerLoop_() } if (!planner_->updateSpeedLAcceleration(acceleration)) { + if (twistNorm_(target_twist) < config_.stop_twist_norm && acceleration <= 0.0) { + std::lock_guard lock(mutex_); + command_active_ = false; + sendZero_(); + busy_.store(false); + break; + } CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration=" << acceleration; sendZero_(); diff --git a/cmvr-es/algorithms/controllers/ibvs/include/ibvs_controller.h b/cmvr-es/algorithms/controllers/ibvs/include/ibvs_controller.h index 40063687..67b79a1e 100644 --- a/cmvr-es/algorithms/controllers/ibvs/include/ibvs_controller.h +++ b/cmvr-es/algorithms/controllers/ibvs/include/ibvs_controller.h @@ -209,18 +209,6 @@ public: */ void resetTwistCommandState(); - /** - * @brief 设置关节限位回避参数。 - * @param enable 是否启用。 - * @param gain 回避增益。 - * @param margin_ratio 触发限位回避的边界比例。 - * @param max_push 单关节最大推回速度。 - */ - void setJointLimitAvoidance(bool enable, - double gain = 0.2, - double margin_ratio = 0.05, - double max_push = 0.25); - /** * @brief 设置从 `AbstractCamera` 相机坐标系 `cam` 到 ViSP 相机坐标系 `c` 的旋转矩阵。 * @param R_cv 旋转矩阵。 @@ -316,11 +304,6 @@ private: // 相机 twist 一阶低通滤波系数；默认 1.0 表示不过滤。 double twist_lpf_alpha_{1.0}; - bool limit_avoidance_enabled_{false}; - double limit_avoidance_gain_{0.2}; - double limit_avoidance_margin_ratio_{0.15}; - double limit_avoidance_max_push_{0.25}; - Eigen::Vector2d depth_control_point_tag_{Eigen::Vector2d::Zero()}; Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()}; diff --git a/cmvr-es/algorithms/controllers/ibvs/src/ibvs_controller.cpp b/cmvr-es/algorithms/controllers/ibvs/src/ibvs_controller.cpp index 5c83acab..e4e32da2 100644 --- a/cmvr-es/algorithms/controllers/ibvs/src/ibvs_controller.cpp +++ b/cmvr-es/algorithms/controllers/ibvs/src/ibvs_controller.cpp @@ -114,12 +114,6 @@ bool IbvsController::init(std::shared_ptr solver, camera_frame_name_ = camera_link; initialized_ = solver_ != nullptr && !camera_frame_name_.empty(); if (initialized_) { - if (auto dls_solver = std::dynamic_pointer_cast(solver_)) { - dls_solver->setJointLimitAvoidance(limit_avoidance_enabled_, - limit_avoidance_gain_, - limit_avoidance_margin_ratio_, - limit_avoidance_max_push_); - } has_joint_position_limits_ = solver_->getJointPositionLimits(q_lower_limits_, q_upper_limits_); } else { @@ -555,23 +549,6 @@ void IbvsController::resetTwistCommandState() { has_v_camera_cmd_prev_ = true; } -void IbvsController::setJointLimitAvoidance(bool enable, - double gain, - double margin_ratio, - double max_push) { - limit_avoidance_enabled_ = enable; - limit_avoidance_gain_ = std::max(0.0, gain); - limit_avoidance_margin_ratio_ = std::clamp(margin_ratio, 1e-3, 0.49); - limit_avoidance_max_push_ = max_push; - - if (auto dls_solver = std::dynamic_pointer_cast(solver_)) { - dls_solver->setJointLimitAvoidance(limit_avoidance_enabled_, - limit_avoidance_gain_, - limit_avoidance_margin_ratio_, - limit_avoidance_max_push_); - } -} - void IbvsController::setAlignCameraToVisp(const Eigen::Matrix3d& R_cv) { R_cv_ = R_cv; } diff --git a/cmvr-es/algorithms/controllers/tests/src/controller_test.cpp b/cmvr-es/algorithms/controllers/tests/src/controller_test.cpp deleted file mode 100644 index e3ea8010..00000000 --- a/cmvr-es/algorithms/controllers/tests/src/controller_test.cpp +++ /dev/null @@ -1,869 +0,0 @@ -// -// Created by lgv on 2026/2/10. -// TEST(contrller_test, visp_test){ -#include "gtest/gtest.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -#include "algorithms/controllers/ibvs/include/ibvs_controller.h" -#include "devices/camera/mujoco_camera/include/mujoco_camera.h" -#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" -#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" -using namespace cmvr; - - - - -#include "gtest/gtest.h" - -#include -#include -#include -#include - -#include - -#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" - -// ---- ViSP ---- -#include -#include -#include -#include - -// Created by lgv on 2026/2/10. - -#include "gtest/gtest.h" - -#include -#include -#include -#include - -#include - -#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" - -// ---- ViSP ---- -#include -#include -#include -#include - -using namespace cmvr; - -namespace { - -// clamp helper -static inline double clamp(double x, double lo, double hi) { - return std::max(lo, std::min(hi, x)); -} - -// MuJoCo xmat(9) -> Eigen::Matrix3d (row-major) -static inline Eigen::Matrix3d xmat_to_R(const mjtNum* xmat9) { - Eigen::Matrix3d R; - R << xmat9[0], xmat9[1], xmat9[2], - xmat9[3], xmat9[4], xmat9[5], - xmat9[6], xmat9[7], xmat9[8]; - return R; -} - -// Build vpHomogeneousMatrix from Eigen R,t -static inline vpHomogeneousMatrix make_cMo_from_Eigen(const Eigen::Matrix3d& R, - const Eigen::Vector3d& t) { - vpRotationMatrix vR; - for (int r = 0; r < 3; ++r) - for (int c = 0; c < 3; ++c) - vR[r][c] = R(r, c); - - vpTranslationVector vt(t(0), t(1), t(2)); - vpHomogeneousMatrix cMo(vt, vR); - return cMo; -} - -} // namespace - -class IBVSHomingViewer : public MuJocoViewer { -public: - using MuJocoViewer::MuJocoViewer; - -protected: - enum class Mode { HOMING, IBVS }; - - void initOnce(mjModel* m, mjData* d) override { - // 1) 主视角自由相机 + PiP显示 hand_cam - setupCamera(3.0, -170.0, -40.0); - enablePiPCamera("hand_cam", 405, 1200, 320, 240); - - // 2) 右臂 7DOF actuator/joint - const char* act_names[7] = { - "R_SHOULDER_P_pos", - "R_SHOULDER_R_pos", - "R_SHOULDER_Y_pos", - "R_ELBOW_R_pos", - "R_WRIST_P_pos", - "R_WRIST_Y_pos", - "R_WRIST_R_pos" - }; - const char* jnt_names[7] = { - "R_SHOULDER_P", - "R_SHOULDER_R", - "R_SHOULDER_Y", - "R_ELBOW_R", - "R_WRIST_P", - "R_WRIST_Y", - "R_WRIST_R" - }; - - for (int i = 0; i < 7; ++i) { - act_ids_[i] = mj_name2id(m, mjOBJ_ACTUATOR, act_names[i]); - jnt_ids_[i] = mj_name2id(m, mjOBJ_JOINT, jnt_names[i]); - - if (act_ids_[i] < 0) std::fprintf(stderr, "Cannot find actuator %s\n", act_names[i]); - if (jnt_ids_[i] < 0) std::fprintf(stderr, "Cannot find joint %s\n", jnt_names[i]); - - if (jnt_ids_[i] >= 0) { - qpos_adr_[i] = m->jnt_qposadr[jnt_ids_[i]]; - dof_adr_[i] = m->jnt_dofadr[jnt_ids_[i]]; - } - } - - // 3) 找相机 site + tag - cam_site_id_ = mj_name2id(m, mjOBJ_SITE, "R_CAM_SITE"); - tag_body_id_ = mj_name2id(m, mjOBJ_BODY, "tag_board"); - - std::printf("[IBVS] R_CAM_SITE id=%d, tag_board id=%d\n", - cam_site_id_, tag_body_id_); - - if (cam_site_id_ < 0 || tag_body_id_ < 0) { - std::fprintf(stderr, "[IBVS] Missing R_CAM_SITE or tag_board. Check XML names.\n"); - ready_ = false; - return; - } - - // 4) 初始姿态（保证 PiP 里能看到 tag） - q_home_ = { 0.25, 1.00, M_PI/2 - 0.2, M_PI/2 - 0.2, -M_PI + 0.5, 0.0, 0.0 }; - - // 5) 坐标轴对齐：site相机系 -> ViSP相机系 - // ViSP（+X 右、+Y 下、+Z 前） - // R_CAM_SITE +X 右、+Y 上、+Z 后 - R_cv_ = (Eigen::Matrix3d() << - 1, 0, 0, - 0,-1, 0, - 0, 0, -1).finished(); - - // 6) ViSP IBVS 参数 - tag_half_ = 0.06; // 12cm tag -> half 6cm - Z_des_ = 0.34; - lambda_ = 0.7; - - task_.setServo(vpServo::EYEINHAND_CAMERA); - task_.setInteractionMatrixType(vpServo::CURRENT); - task_.setLambda(lambda_); - - obj_pts_[0].setWorldCoordinates(-tag_half_, -tag_half_, 0.0); - obj_pts_[1].setWorldCoordinates( tag_half_, -tag_half_, 0.0); - obj_pts_[2].setWorldCoordinates( tag_half_, tag_half_, 0.0); - obj_pts_[3].setWorldCoordinates(-tag_half_, tag_half_, 0.0); - - // desired: 正对 + 距离 Z_des - { - vpTranslationVector t_des(0.0, 0.0, Z_des_); - - // tag 在（相机坐标系visp 相机）下的朝向 - vpRotationMatrix R_des; - R_des.buildFrom(M_PI, 0, 0); - vpHomogeneousMatrix cMo_des(t_des, R_des); - - for (int i = 0; i < 4; ++i) { - obj_pts_[i].track(cMo_des); - s_star_[i].buildFrom(obj_pts_[i].get_x(), - obj_pts_[i].get_y(), - obj_pts_[i].get_Z()); - - s_cur_[i].buildFrom(0.0, 0.0, 1.0); - task_.addFeature(s_cur_[i], s_star_[i]); - } - } - - // 初始化 q_cmd 为当前 qpos，避免突变 - for (int i = 0; i < 7; ++i) { - q_cmd_[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0; - } - - mode_ = Mode::HOMING; - home_hold_acc_ = 0.0; - step_count_ = 0; - ready_ = true; - } - - void controlCallback(mjModel* m, mjData* d) override { - if (!ready_) return; - const double dt = m->opt.timestep; - - // ====================== - // A) HOMING - // ====================== - if (mode_ == Mode::HOMING) { - double max_err = 0.0; - for (int i = 0; i < 7; ++i) { - if (act_ids_[i] < 0 || qpos_adr_[i] < 0) continue; - d->ctrl[act_ids_[i]] = q_home_[i]; - const double qi = d->qpos[qpos_adr_[i]]; - max_err = std::max(max_err, std::abs(qi - q_home_[i])); - } - - if (max_err < home_tol_) home_hold_acc_ += dt; - else home_hold_acc_ = 0.0; - - if (home_hold_acc_ > home_hold_time_) { - for (int i = 0; i < 7; ++i) { - if (qpos_adr_[i] >= 0) q_cmd_[i] = d->qpos[qpos_adr_[i]]; - } - mode_ = Mode::IBVS; - std::printf("[IBVS] switch HOMING -> IBVS\n"); - } - return; - } - - // ====================== - // B) IBVS - // ====================== - - // 1) camera(site) pose in world - const mjtNum* pc = d->site_xpos + 3 * cam_site_id_; - const mjtNum* Rc9 = d->site_xmat + 9 * cam_site_id_; - Eigen::Vector3d p_cw(pc[0], pc[1], pc[2]); - Eigen::Matrix3d R_cw = xmat_to_R(Rc9); // site(cam)->world - Eigen::Matrix3d R_wc = R_cw.transpose(); // world->site(cam) - if ((step_count_ % 120) == 0) { - const Eigen::Vector3d x_w = R_cw.col(0); - const Eigen::Vector3d y_w = R_cw.col(1); - const Eigen::Vector3d z_w = R_cw.col(2); - std::printf("[IBVS] site axes(world): x=[%.3f %.3f %.3f] y=[%.3f %.3f %.3f] z=[%.3f %.3f %.3f]\n", - x_w.x(), x_w.y(), x_w.z(), - y_w.x(), y_w.y(), y_w.z(), - z_w.x(), z_w.y(), z_w.z()); - - const int cam_id = mj_name2id(m, mjOBJ_CAMERA, "hand_cam"); - if (cam_id >= 0) { - const mjtNum* Rc9_cam = d->cam_xmat + 9 * cam_id; - Eigen::Matrix3d R_cam_w = xmat_to_R(Rc9_cam); // cam->world - Eigen::Matrix3d R_rel = R_cam_w.transpose() * R_cw; // cam <- site - std::printf("[IBVS] cam<-site:\n" - "%.3f %.3f %.3f\n" - "%.3f %.3f %.3f\n" - "%.3f %.3f %.3f\n", - R_rel(0,0), R_rel(0,1), R_rel(0,2), - R_rel(1,0), R_rel(1,1), R_rel(1,2), - R_rel(2,0), R_rel(2,1), R_rel(2,2)); - } else { - std::printf("[IBVS] cam<-site: hand_cam not found\n"); - } - } - - // 2) tag_board body pose in world - const mjtNum* po = d->xpos + 3 * tag_body_id_; - const mjtNum* Ro9 = d->xmat + 9 * tag_body_id_; - Eigen::Vector3d p_ow(po[0], po[1], po[2]); - Eigen::Matrix3d R_ow = xmat_to_R(Ro9); - - // 3) object->camera(site) - Eigen::Matrix3d R_co_site = R_wc * R_ow; - Eigen::Vector3d t_co_site = R_wc * (p_ow - p_cw); - - // 4) site相机系 -> ViSP相机系（对齐） - Eigen::Matrix3d R_co = R_cv_ * R_co_site; - Eigen::Vector3d t_co = R_cv_ * t_co_site; - - if ((step_count_ % 120) == 0) { - const Eigen::Vector3d z_site = R_co_site.col(2); // tag +Z in site(cam) frame - const Eigen::Vector3d z_visp = R_co.col(2); // tag +Z in ViSP frame - std::printf("[IBVS] tag +Z (site) = [%.3f %.3f %.3f]\n", - z_site.x(), z_site.y(), z_site.z()); - std::printf("[IBVS] tag +Z (visp) = [%.3f %.3f %.3f]\n", - z_visp.x(), z_visp.y(), z_visp.z()); - } - - if ((step_count_ % 60) == 0) { - std::printf("[IBVS] t_co = [%.3f %.3f %.3f]\n", t_co.x(), t_co.y(), t_co.z()); - } - - vpHomogeneousMatrix cMo = make_cMo_from_Eigen(R_co, t_co); - - // 5) current features - for (int i = 0; i < 4; ++i) { - obj_pts_[i].track(cMo); - double x = obj_pts_[i].get_x(); - double y = obj_pts_[i].get_y(); - double Z = std::max(obj_pts_[i].get_Z(), 0.05); - s_cur_[i].buildFrom(x, y, Z); - } - - // 6) ViSP control law -> v_c (ViSP camera frame) - vpColVector v_c = task_.computeControlLaw(); - - // 限幅（关键） - for (int k = 0; k < 6; ++k) { - v_c[k] = clamp(v_c[k], -vmax6_[k], vmax6_[k]); - } - - if ((step_count_ % 60) == 0) { - std::printf("[IBVS] v_c = [%+.3f %+.3f %+.3f %+.3f %+.3f %+.3f]\n", - v_c[0], v_c[1], v_c[2], v_c[3], v_c[4], v_c[5]); - } - - // 7) ViSP相机速度 -> site相机速度（逆对齐） - Eigen::Vector3d v_visp(v_c[0], v_c[1], v_c[2]); - Eigen::Vector3d w_visp(v_c[3], v_c[4], v_c[5]); - - Eigen::Vector3d v_site = R_cv_.transpose() * v_visp; - Eigen::Vector3d w_site = R_cv_.transpose() * w_visp; - - // 8) site相机速度 -> world twist（给 world Jacobian 用） - Eigen::Vector3d v_w = R_cw * v_site; - Eigen::Vector3d w_w = R_cw * w_site; - - Eigen::Matrix twist_w; - twist_w << v_w(0), v_w(1), v_w(2), w_w(0), w_w(1), w_w(2); - - // 9) Jacobian for R_CAM_SITE (world) - std::vector jacp(3 * m->nv); - std::vector jacr(3 * m->nv); - mj_jacSite(m, d, jacp.data(), jacr.data(), cam_site_id_); - - Eigen::Matrix J; - J.setZero(); - for (int j = 0; j < 7; ++j) { - const int dof = dof_adr_[j]; - if (dof < 0) continue; - - J(0,j) = jacp[0*m->nv + dof]; - J(1,j) = jacp[1*m->nv + dof]; - J(2,j) = jacp[2*m->nv + dof]; - J(3,j) = jacr[0*m->nv + dof]; - J(4,j) = jacr[1*m->nv + dof]; - J(5,j) = jacr[2*m->nv + dof]; - } - - // 10) DLS inverse: qdot - Eigen::Matrix A = J * J.transpose(); - A += (mu_*mu_) * Eigen::Matrix::Identity(); - Eigen::Matrix qdot = J.transpose() * A.inverse() * twist_w; - - for (int i = 0; i < 7; ++i) { - qdot(i) = clamp(qdot(i), -qdot_max_, qdot_max_); - } - - // 11) integrate -> position targets - for (int i = 0; i < 7; ++i) { - q_cmd_[i] += qdot(i) * dt; - - if (jnt_ids_[i] >= 0 && m->jnt_limited[jnt_ids_[i]]) { - const double lo = m->jnt_range[2*jnt_ids_[i] + 0]; - const double hi = m->jnt_range[2*jnt_ids_[i] + 1]; - q_cmd_[i] = clamp(q_cmd_[i], lo, hi); - } - - if (act_ids_[i] >= 0) { - d->ctrl[act_ids_[i]] = q_cmd_[i]; - } - } - - // 12) sanity: 看看机械臂是否真的在动 - if ((step_count_ % 120) == 0 && qpos_adr_[0] >= 0 && act_ids_[0] >= 0) { - std::printf("[IBVS] qpos0=%.3f ctrl0=%.3f\n", - d->qpos[qpos_adr_[0]], d->ctrl[act_ids_[0]]); - } - - ++step_count_; - } - - void onReset(mjModel* m, mjData* d) override { - (void)m; - for (int i = 0; i < 7; ++i) { - if (qpos_adr_[i] >= 0) q_cmd_[i] = d->qpos[qpos_adr_[i]]; - } - mode_ = Mode::HOMING; - home_hold_acc_ = 0.0; - step_count_ = 0; - } - -private: - bool ready_{false}; - - std::array act_ids_{}; - std::array jnt_ids_{}; - std::array qpos_adr_{ { -1,-1,-1,-1,-1,-1,-1 } }; - std::array dof_adr_{ { -1,-1,-1,-1,-1,-1,-1 } }; - - int cam_site_id_{-1}; - int tag_body_id_{-1}; - - // frame align - Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()}; - - // homing - Mode mode_{Mode::HOMING}; - std::array q_home_{ {0,0,0,0,0,0,0} }; - double home_tol_{0.02}; - double home_hold_time_{0.3}; - double home_hold_acc_{0.0}; - - // IBVS - double lambda_{0.7}; - double tag_half_{0.06}; - double Z_des_{0.60}; - - double mu_{0.02}; - double qdot_max_{0.6}; - double vmax6_[6] = {0.15, 0.15, 0.20, 0.6, 0.6, 0.6}; - - int step_count_{0}; - - vpServo task_; - vpPoint obj_pts_[4]; - vpFeaturePoint s_cur_[4]; - vpFeaturePoint s_star_[4]; - - std::array q_cmd_{ {0,0,0,0,0,0,0} }; -}; - - - - - - -// ---- ViSP ---- -#include -#include - -#include - - -class IBVSFromMujocoCameraViewer : public MuJocoViewer { -public: - using MuJocoViewer::MuJocoViewer; - - void enableTwistToQdotCheck(bool enable, double tol = 1e-3) { - check_twist_to_qdot_ = enable; - qdot_check_tol_ = tol; - (void)qdot_check_tol_; - } - - bool twistToQdotCheckPassed() const { return !qdot_check_failed_; } - double twistToQdotMaxError() const { return qdot_check_max_err_; } - int twistToQdotCheckSamples() const { return qdot_check_samples_; } - -protected: - enum class Mode { HOMING, IBVS }; - - void initOnce(mjModel* m, mjData* d) override { - setupCamera(3.0, -170.0, -40.0); - enablePiPCamera("hand_cam", 405, 1000, 320, 240); - - const char* act_names[7] = { - "R_SHOULDER_P_pos", - "R_SHOULDER_R_pos", - "R_SHOULDER_Y_pos", - "R_ELBOW_R_pos", - "R_WRIST_P_pos", - "R_WRIST_Y_pos", - "R_WRIST_R_pos" - }; - const char* jnt_names[7] = { - "R_SHOULDER_P", - "R_SHOULDER_R", - "R_SHOULDER_Y", - "R_ELBOW_R", - "R_WRIST_P", - "R_WRIST_Y", - "R_WRIST_R" - }; - - for (int i = 0; i < 7; ++i) { - act_ids_[i] = mj_name2id(m, mjOBJ_ACTUATOR, act_names[i]); - jnt_ids_[i] = mj_name2id(m, mjOBJ_JOINT, jnt_names[i]); - if (act_ids_[i] < 0) { - std::cout << "[IBVS] Cannot find actuator " << act_names[i] << std::endl; - } - if (jnt_ids_[i] < 0) { - std::cout << "[IBVS] Cannot find joint " << jnt_names[i] << std::endl; - } - if (jnt_ids_[i] >= 0) { - qpos_adr_[i] = m->jnt_qposadr[jnt_ids_[i]]; - } - } - - cam_site_id_ = mj_name2id(m, mjOBJ_SITE, "R_CAM_SITE"); - if (cam_site_id_ < 0) { - std::cout << "[IBVS] Missing site R_CAM_SITE in XML" << std::endl; - ready_ = false; - return; - } - - hand_cam_id_ = mj_name2id(m, mjOBJ_CAMERA, "hand_cam"); - if (hand_cam_id_ < 0) { - std::cout << "[IBVS] Missing camera hand_cam in XML" << std::endl; - ready_ = false; - return; - } - mujoco_camera_ = std::make_shared( - [this](std::vector& rgb, - std::vector& depth, - int& width, - int& height, - uint64_t& frame_id) { - return getPiPCameraRGBD(rgb, depth, width, height, frame_id); - }); - mujoco_camera_->setFovyDeg(m->cam_fovy[hand_cam_id_]); - // MuJoCo 渲染线程与控制线程不同步：允许复用最近一帧，避免长时间 no_new_frame。 - mujoco_camera_->setConsumeNewFrameOnly(false); - - q_home_ = {-0.2423, 1.2929,1.61, 1.58, -2.8792, 0.1150,-0.08}; - - ibvs_controller_ = std::make_unique(); - ibvs_controller_->setMu(mu_); - ibvs_controller_->setQdotMax(qdot_max_); - ibvs_controller_->setDepthMode(IbvsController::DepthMode::MONOCULAR); - ibvs_controller_->setDepthZGain(1.0); - ibvs_controller_->setVelocityLimit6(vmax6_); - ibvs_controller_->setTrackedTagId(tracked_tag_id_); - 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); - - const Eigen::Matrix3d R_align = (Eigen::Matrix3d() << - 1, 0, 0, - 0, -1, 0, - 0, 0, -1).finished(); - ibvs_controller_->setAlignCameraToVisp(R_align); - ibvs_controller_->setAlignCameraToUrdf(R_align); - - config::PinocchioDlsIKConfig dls_cfg; - dls_cfg.set_urdf_path(urdf_path_for_check_); - dls_cfg.set_base_frame_name("PELVIS_S"); - dls_cfg.set_flange_frame_name("R_WRIST_R_S"); - dls_cfg.set_tcp_frame_name(camera_frame_name_for_check_); - dls_cfg.set_max_iters(100); - dls_cfg.set_pos_eps(1e-6); - dls_cfg.set_rot_eps(1e-6); - dls_cfg.set_damping(mu_); - auto solver = std::make_shared(dls_cfg); - if (!solver->init()) { - std::cout << "[IBVS] PinocchioDlsIKSolver init failed" << std::endl; - ready_ = false; - return; - } - - if (!ibvs_controller_->init(solver, camera_frame_name_for_check_)) { - std::cout << "[IBVS] IbvsController init failed" << std::endl; - ready_ = false; - return; - } - - perception_ = std::make_shared(mujoco_camera_); - perception_->setTagSize(tag_size_m_); - perception_opt_.detect_tags = true; - perception_opt_.fetch_encoded = false; - perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE; - ibvs_controller_->setPerception(perception_); - - std::vector q_init(7, 0.0); - for (int i = 0; i < 7; ++i) { - q_init[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0; - q_cmd_[i] = q_init[i]; - } - ibvs_controller_->reset(q_init); - - mode_ = Mode::HOMING; - home_hold_acc_ = 0.0; - step_count_ = 0; - qdot_check_max_err_ = 0.0; - qdot_check_samples_ = 0; - qdot_check_failed_ = false; - ready_ = true; - - std::cout << "[IBVS] initOnce OK. cam_site_id=" << cam_site_id_ - << " hand_cam_id=" << hand_cam_id_ << std::endl; - } - - void controlCallback(mjModel* m, mjData* d) override { - if (!ready_) return; - const double dt = m->opt.timestep; - - if (mode_ == Mode::HOMING) { - double max_err = 0.0; - for (int i = 0; i < 7; ++i) { - if (act_ids_[i] < 0 || qpos_adr_[i] < 0) continue; - d->ctrl[act_ids_[i]] = q_home_[i]; - const double qi = d->qpos[qpos_adr_[i]]; - max_err = std::max(max_err, std::abs(qi - q_home_[i])); - } - - if (max_err < home_tol_) home_hold_acc_ += dt; - else home_hold_acc_ = 0.0; - - if (home_hold_acc_ > home_hold_time_) { - std::vector q_now(7, 0.0); - for (int i = 0; i < 7; ++i) { - q_now[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0; - q_cmd_[i] = q_now[i]; - } - ibvs_controller_->reset(q_now); - mode_ = Mode::IBVS; - std::cout << "[IBVS] switch HOMING -> IBVS" << std::endl; - } - return; - } - - std::vector q_now(7, 0.0); - for (int i = 0; i < 7; ++i) { - q_now[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0; - } - - if (perception_) { - switch (ibvs_controller_->depthMode()) { - case IbvsController::DepthMode::MONOCULAR: - perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE; - break; - case IbvsController::DepthMode::PREFER_DEPTH: - perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::PREFER; - break; - case IbvsController::DepthMode::DEPTH_ONLY: - perception_opt_.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::REQUIRE; - break; - } - const bool perception_ok = perception_->update(perception_opt_); - if (!perception_ok && (step_count_ % 60) == 0) { - std::cout << "[IBVS] perception update failed: " - << cmvr::perception::AprilTagPerception::statusToString(perception_->lastStatus()) - << std::endl; - } - } - - std::vector q_cmd_next; - const bool ok = ibvs_controller_->compute(q_now, dt, q_cmd_next); - - if (!ok) { - if ((step_count_ % 60) == 0) { - const auto st = ibvs_controller_->lastComputeStatus(); - std::cout << "[IBVS] compute skipped: " - << IbvsController::statusToString(st) << std::endl; - if (st == IbvsController::ComputeStatus::TAG_MISMATCH && perception_ && perception_->hasTags()) { - std::cout << "[IBVS] visible tag ids:"; - for (const auto& t : perception_->tags()) { - std::cout << " " << t.id; - } - std::cout << std::endl; - } - } - ++step_count_; - return; - } - - if (q_cmd_next.size() == 7) { - for (int i = 0; i < 7; ++i) { - q_cmd_[i] = q_cmd_next[i]; - } - } - - for (int i = 0; i < 7; ++i) { - if (jnt_ids_[i] >= 0 && m->jnt_limited[jnt_ids_[i]]) { - const double lo = m->jnt_range[2 * jnt_ids_[i] + 0]; - const double hi = m->jnt_range[2 * jnt_ids_[i] + 1]; - q_cmd_[i] = clamp(q_cmd_[i], lo, hi); - } - if (act_ids_[i] >= 0) { - d->ctrl[act_ids_[i]] = q_cmd_[i]; - } - } - - if ((step_count_ % 60) == 0) { - const auto& v_c = ibvs_controller_->lastCameraTwistVisp(); - const auto& t_co = ibvs_controller_->lastTagPositionVisp(); - std::cout << "[IBVS] v_c=[" << v_c[0] << " " << v_c[1] << " " << v_c[2] - << " " << v_c[3] << " " << v_c[4] << " " << v_c[5] << "]" << std::endl; - std::cout << "[IBVS] t_co(visp)=[" << t_co.x() << " " << t_co.y() << " " << t_co.z() << "]" << std::endl; - std::cout << "[IBVS] z_source=" - << IbvsController::depthUsageToString(ibvs_controller_->lastDepthUsage()) << std::endl; - } - - ++step_count_; - } - - void onReset(mjModel* m, mjData* d) override { - (void)m; - std::vector q_now(7, 0.0); - for (int i = 0; i < 7; ++i) { - q_now[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0; - q_cmd_[i] = q_now[i]; - } - if (ibvs_controller_) { - ibvs_controller_->reset(q_now); - } - mode_ = Mode::HOMING; - home_hold_acc_ = 0.0; - step_count_ = 0; - qdot_check_max_err_ = 0.0; - qdot_check_samples_ = 0; - qdot_check_failed_ = false; - } - -private: - bool ready_{false}; - - std::array act_ids_{}; - std::array jnt_ids_{}; - std::array qpos_adr_{{-1, -1, -1, -1, -1, -1, -1}}; - int cam_site_id_{-1}; - int hand_cam_id_{-1}; - - Mode mode_{Mode::HOMING}; - std::array q_home_{{0, 0, 0, 0, 0, 0, 0}}; - double home_tol_{0.02}; - double home_hold_time_{0.3}; - double home_hold_acc_{0.0}; - - double mu_{0.02}; - double qdot_max_{0.6}; - std::array vmax6_{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}}; - - bool check_twist_to_qdot_{true}; - double qdot_check_tol_{1e-3}; - double qdot_check_max_err_{0.0}; - int qdot_check_samples_{0}; - bool qdot_check_failed_{false}; - std::string urdf_path_for_check_{ - "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf"}; - std::string camera_frame_name_for_check_{"R_CAM"}; - int tracked_tag_id_{0}; - double tag_size_m_{0.12}; - - std::unique_ptr ibvs_controller_{nullptr}; - std::shared_ptr mujoco_camera_{nullptr}; - std::shared_ptr perception_{nullptr}; - cmvr::perception::AprilTagPerception::Options perception_opt_{}; - - std::array q_cmd_{{0, 0, 0, 0, 0, 0, 0}}; - int step_count_{0}; -}; - - - - - -class ControllerViewer : public MuJocoViewer { -public: - using MuJocoViewer::MuJocoViewer; - - void setTarget(const std::vector &q_target) { - std::lock_guard lock(mtx_); - q_cmd_ = q_target; - } - -protected: - void initOnce(mjModel *m, mjData *d) override - { - (void)d; - setupCamera(3.0, -170.0, -40.0); - // enablePiPCamera("hand_cam", 320, 240, 10); - enablePiPCamera("hand_cam", 405, 1200, 320, 240); - - const char *act_names[7] = { - "R_SHOULDER_P_pos", - "R_SHOULDER_R_pos", - "R_SHOULDER_Y_pos", - "R_ELBOW_R_pos", - "R_WRIST_P_pos", - "R_WRIST_Y_pos", - "R_WRIST_R_pos" - }; - - for (int i = 0; i < 7; ++i) { - act_ids_[i] = mj_name2id(m, mjOBJ_ACTUATOR, act_names[i]); - if (act_ids_[i] < 0) { - std::cout << "Cannot find actuator " << act_names[i] << std::endl; - } - } - - act_ids_inited_ = true; - } - - void controlCallback(mjModel *m, mjData *d) override - { - (void)m; - if (!act_ids_inited_) return; - - std::vector q_local; - { - std::lock_guard lock(mtx_); - q_local = q_cmd_; - } - - for (int i = 0; i < 7; ++i) { - if (act_ids_[i] < 0) continue; - if (i < static_cast(q_local.size())) { - d->ctrl[act_ids_[i]] = q_local[i]; - } - } - } - - void onReset(mjModel *m, mjData *d) override - { - (void)m; - (void)d; - std::lock_guard lock(mtx_); - q_cmd_.assign(7, 0.0); - } - -private: - std::array act_ids_{}; - bool act_ids_inited_{false}; - std::vector q_cmd_{7, 0.0}; - std::mutex mtx_; -}; - -TEST(controller_test, mujoco_viewer_smoke) -{ - ControllerViewer viewer("/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml"); - - std::vector q_seed = { - 0.25, 1.00, M_PI / 2 - 0.2, M_PI / 2 - 0.2, -M_PI+ 0.5 , 0, 0 - }; - viewer.setTarget(q_seed); - viewer.run(); -} - -TEST(controller_test, mujoco_ibvs_sim_gt) -{ - IBVSHomingViewer viewer("/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml"); - viewer.run(); -} - -TEST(controller_test, mujoco_camera_apriltag_ibvs_full) -{ - IBVSFromMujocoCameraViewer viewer( - "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml" - ); - viewer.enableTwistToQdotCheck(true, 1e-3); - viewer.run(); - - // EXPECT_GT(viewer.twistToQdotCheckSamples(), 0) - // << "no valid samples collected for twistToQdot consistency"; - // if (viewer.twistToQdotCheckSamples() > 0) { - // EXPECT_TRUE(viewer.twistToQdotCheckPassed()) - // << "twistToQdot mismatch, max_abs_err=" << viewer.twistToQdotMaxError(); - // } -} diff --git a/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt b/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt index 246aa264..310be1b7 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt +++ b/cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt @@ -25,44 +25,4 @@ target_link_libraries(ik_solver PUBLIC add_library(cmvr_es::ik_solver ALIAS ik_solver) -install(TARGETS ik_solver LIBRARY DESTINATION lib) -# -------------------------------------------------------- -# Unit test -# -------------------------------------------------------- - -add_executable(srs_ik_test - ${CMAKE_CURRENT_SOURCE_DIR}/tests/src/srs_ik_test.cpp -) - - -target_link_libraries(srs_ik_test - PRIVATE - cmvr_es::ik_solver - cmvr_es::base_motion - cmvr_es::arm_motion - cmvr_es::proto - cmvr_es::mujoco_viewer - gtest - gtest_main - pthread - glog - matplot -) - - - - -add_executable(ik_test - ${CMAKE_CURRENT_SOURCE_DIR}/tests/src/ik_test.cpp -) - -target_link_libraries(ik_test - PRIVATE - cmvr_es::ik_solver - cmvr_es::mujoco_viewer - gtest - gtest_main - pthread - glog - cmvr_es::proto -) +install(TARGETS ik_solver LIBRARY DESTINATION lib) \ No newline at end of file diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h index 18fe94f8..338ae1dd 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h @@ -54,11 +54,6 @@ public: std::vector& qdot_out, double qdot_abs_max = std::numeric_limits::infinity()) const override; - void setJointLimitAvoidance(bool enable, - double gain = 0.2, - double margin_ratio = 0.15, - double max_push = 0.25); - void setMaxIters(int iters) { max_iters_ = iters; } void setDamping(double d) { damping_ = d; } void setEps(double pos_eps, double rot_eps) { pos_eps_ = pos_eps; rot_eps_ = rot_eps; } @@ -68,7 +63,7 @@ public: private: Eigen::MatrixXd dampedPseudoInverse(const Eigen::MatrixXd &J, double lambda); - bool refreshJointLimits_(const config::PinocchioDlsIKConfig& cfg); + bool refreshJointLimits_(); Eigen::VectorXd computeJointLimitAvoidanceVelocity(const Eigen::VectorXd& q_chain) const; @@ -77,11 +72,6 @@ private: const Eigen::VectorXd& secondary) const; private: - bool limit_avoidance_enabled_{false}; - double limit_avoidance_gain_{0.2}; - double limit_avoidance_margin_ratio_{0.15}; - double limit_avoidance_max_push_{0.25}; - bool initialized_{false}; int max_iters_; diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h index 68b6e54c..b8c8c928 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h @@ -3,6 +3,7 @@ #include "algorithms/kinematics/ik_solver/common/include/ik_solver.h" #include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h" +#include "cmvr/config/joint_limits_config.pb.h" #include #include @@ -74,6 +75,9 @@ public: int chainDof() const { return chain_q_dof_; } int chainVelocityDof() const { return chain_v_dof_; } + void setJointLimitPolicy(const config::JointLimitPolicyConfig& policy); + const config::JointLimitPolicyConfig& jointLimitPolicy() const { return joint_limit_policy_; } + bool computeJacobianBaseAtQ(const std::vector& q_chain, bool is_tcp, Eigen::MatrixXd& jacobian_base, @@ -153,6 +157,13 @@ protected: Eigen::Matrix3d* base_R_ee_out = nullptr, Eigen::VectorXd* q_full_out = nullptr); + Eigen::VectorXd applyJointSoftLimitsToVelocity(const Eigen::VectorXd& q_chain, + const Eigen::VectorXd& qdot) const; + + bool jointLimitsDisabled() const; + void disableJointLimitCache(); + static double disabledJointLimitAbs(); + /** * @brief 在当前 `model_` 上更新 FK 与 frame placement。 */ @@ -198,6 +209,8 @@ protected: int chain_v_start_{0}; /** @brief 当前链关节速度自由度数量。 */ int chain_v_dof_{0}; + + config::JointLimitPolicyConfig joint_limit_policy_{}; }; } // namespace cmvr diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h index 471cc679..1fa37f8a 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include "cmvr/config/pinocchio_qp_ik_config.pb.h" @@ -51,6 +52,8 @@ public: using IKSolver::update_joints_state; private: + bool refreshJointLimitPolicy_(); + // 配置 config::PinocchioQpIKConfig config_; std::string urdf_path_; diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp index c1408518..188801d2 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp @@ -49,22 +49,16 @@ PinocchioDlsIKSolver::PinocchioDlsIKSolver(const config::PinocchioDlsIKConfig& c , damping_(cfg.damping() > 0.0 ? cfg.damping() : 1e-4) , config_(cfg) { - if (cfg.has_joint_limit_avoidance()) { - const auto& avoidance = cfg.joint_limit_avoidance(); - setJointLimitAvoidance( - avoidance.enable(), - positiveOr(avoidance.gain(), limit_avoidance_gain_), - positiveOr(avoidance.margin_ratio(), limit_avoidance_margin_ratio_), - positiveOr(avoidance.max_push(), limit_avoidance_max_push_)); - } + setJointLimitPolicy(cfg.joint_limit_policy()); } -bool PinocchioDlsIKSolver::refreshJointLimits_(const config::PinocchioDlsIKConfig& cfg) { - const auto source = cfg.has_joint_limits() - ? cfg.joint_limits().source() - : config::JOINT_LIMIT_SOURCE_URDF; - if (source == config::JOINT_LIMIT_SOURCE_UNKNOWN || - source == config::JOINT_LIMIT_SOURCE_URDF) { +bool PinocchioDlsIKSolver::refreshJointLimits_() { + const auto source = joint_limit_policy_.limits().source(); + if (jointLimitsDisabled()) { + disableJointLimitCache(); + return true; + } + if (source == config::JOINT_LIMIT_SOURCE_URDF) { return true; } @@ -80,12 +74,11 @@ bool PinocchioDlsIKSolver::refreshJointLimits_(const config::PinocchioDlsIKConfi } std::unordered_map custom_limits; - if (cfg.has_joint_limits()) { - custom_limits.reserve(static_cast(cfg.joint_limits().joints_size())); - for (const auto& item : cfg.joint_limits().joints()) { - if (!item.joint_name().empty()) { - custom_limits[item.joint_name()] = item; - } + const auto& limits = joint_limit_policy_.limits(); + custom_limits.reserve(static_cast(limits.joints_size())); + for (const auto& item : limits.joints()) { + if (!item.joint_name().empty()) { + custom_limits[item.joint_name()] = item; } } @@ -117,8 +110,9 @@ bool PinocchioDlsIKSolver::refreshJointLimits_(const config::PinocchioDlsIKConfi Eigen::VectorXd PinocchioDlsIKSolver::computeJointLimitAvoidanceVelocity( const Eigen::VectorXd& q_chain) const { - if (!limit_avoidance_enabled_ || - limit_avoidance_gain_ <= 0.0 || + const auto& avoidance = joint_limit_policy_.avoidance(); + if (jointLimitsDisabled() || + !avoidance.enable() || avoidance.gain() <= 0.0 || chain_v_dof_ != chain_q_dof_ || q_chain.size() != chain_q_dof_ || joint_pos_lower_limits_.size() != chain_q_dof_ || @@ -130,10 +124,10 @@ Eigen::VectorXd PinocchioDlsIKSolver::computeJointLimitAvoidanceVelocity( q_chain, joint_pos_lower_limits_, joint_pos_upper_limits_, - limit_avoidance_enabled_, - limit_avoidance_gain_, - limit_avoidance_margin_ratio_, - limit_avoidance_max_push_); + avoidance.enable(), + positiveOr(avoidance.gain(), 0.2), + positiveOr(avoidance.margin_ratio(), 0.15), + positiveOr(avoidance.max_push(), 0.25)); } Eigen::VectorXd PinocchioDlsIKSolver::projectToNullspace(const Eigen::MatrixXd& J_pinv, @@ -154,7 +148,7 @@ bool PinocchioDlsIKSolver::init() { CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] Failed to init pinocchio base: " << err; return false; } - if (!refreshJointLimits_(config_)) { + if (!refreshJointLimits_()) { return false; } @@ -299,16 +293,6 @@ bool PinocchioDlsIKSolver::ik(const std::string& base_link, return true; } -void PinocchioDlsIKSolver::setJointLimitAvoidance(bool enable, - double gain, - double margin_ratio, - double max_push) { - limit_avoidance_enabled_ = enable; - limit_avoidance_gain_ = std::max(0.0, gain); - limit_avoidance_margin_ratio_ = std::clamp(margin_ratio, 1e-3, 0.49); - limit_avoidance_max_push_ = max_push; -} - bool PinocchioDlsIKSolver::ik(const std::string& base_link, const std::string& ee_link, const Eigen::Matrix& target_vel, @@ -417,11 +401,11 @@ bool PinocchioDlsIKSolver::ik(const std::string& base_link, qdot += projectToNullspace(J_pinv, J, qdot_avoid); } + qdot = cmvr::kinematics::scaleToVelocityLimits(qdot, joint_vel_limits_, qdot_abs_max); + qdot = applyJointSoftLimitsToVelocity(q_chain, qdot); joints_vel.resize(chain_v_dof_); - const Eigen::VectorXd qdot_limited = - cmvr::kinematics::scaleToVelocityLimits(qdot, joint_vel_limits_, qdot_abs_max); for (int i = 0; i < chain_v_dof_; ++i) { - joints_vel[i] = qdot_limited[i]; + joints_vel[i] = qdot[i]; } return true; @@ -463,6 +447,7 @@ bool PinocchioDlsIKSolver::solveVelocityBase(const Eigen::MatrixXd& jacobian_bas } qdot = cmvr::kinematics::scaleToVelocityLimits(qdot, joint_vel_limits_, qdot_abs_max); + qdot = applyJointSoftLimitsToVelocity(q_chain, qdot); qdot_out.resize(chain_v_dof_); for (int i = 0; i < chain_v_dof_; ++i) { qdot_out[i] = qdot[i]; diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp index 8b4311c4..a15cbef6 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp @@ -2,13 +2,19 @@ #include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" #include "common/base/logging/logger.h" +#include "common/config/config_files.h" #include #include #include +#include +#include + namespace cmvr { +using cmvr::common::config::positiveOr; + PinocchioIKBase::PinocchioIKBase(const std::string& urdf_path, const std::string& base_frame_name, const std::string& flange_frame_name, @@ -116,6 +122,75 @@ const pinocchio::SE3& PinocchioIKBase::getBasePoseWorld() const { return base_pose_cached_ ? oM_base_cached_ : data_->oMf[base_frame_id_]; } +void PinocchioIKBase::setJointLimitPolicy(const config::JointLimitPolicyConfig& policy) +{ + joint_limit_policy_ = policy; +} + +bool PinocchioIKBase::jointLimitsDisabled() const +{ + return !joint_limit_policy_.limits().enable(); +} + +double PinocchioIKBase::disabledJointLimitAbs() +{ + return 1e6; +} + +void PinocchioIKBase::disableJointLimitCache() +{ + const double big = disabledJointLimitAbs(); + joint_pos_lower_limits_.resize(chain_q_dof_); + joint_pos_upper_limits_.resize(chain_q_dof_); + joint_vel_limits_.resize(chain_v_dof_); + joint_pos_lower_limits_.setConstant(-big); + joint_pos_upper_limits_.setConstant(big); + joint_vel_limits_.setConstant(big); +} + +Eigen::VectorXd PinocchioIKBase::applyJointSoftLimitsToVelocity( + const Eigen::VectorXd& q_chain, + const Eigen::VectorXd& qdot) const +{ + const auto& config = joint_limit_policy_.soft_limit(); + if (jointLimitsDisabled() || !config.enable()) { + return qdot; + } + if (joint_pos_lower_limits_.size() != q_chain.size() || + joint_pos_upper_limits_.size() != q_chain.size() || + qdot.size() != q_chain.size()) { + return qdot; + } + + const double margin_ratio = positiveOr(config.margin_ratio(), 0.08); + const double min_margin_rad = positiveOr(config.min_margin_rad(), 0.02); + Eigen::VectorXd limited = qdot; + for (Eigen::Index i = 0; i < q_chain.size(); ++i) { + const double lower = joint_pos_lower_limits_[i]; + const double upper = joint_pos_upper_limits_[i]; + if (!std::isfinite(lower) || !std::isfinite(upper) || upper <= lower) { + continue; + } + + const double span = upper - lower; + const double margin = std::max(min_margin_rad, margin_ratio * span); + if (limited[i] < 0.0 && q_chain[i] < lower + margin) { + const double ratio = std::clamp((q_chain[i] - lower) / margin, 0.0, 1.0); + limited[i] *= ratio; + if (q_chain[i] <= lower) { + limited[i] = std::max(0.0, limited[i]); + } + } else if (limited[i] > 0.0 && q_chain[i] > upper - margin) { + const double ratio = std::clamp((upper - q_chain[i]) / margin, 0.0, 1.0); + limited[i] *= ratio; + if (q_chain[i] >= upper) { + limited[i] = std::min(0.0, limited[i]); + } + } + } + return limited; +} + void PinocchioIKBase::updateKinematics(const Eigen::VectorXd& q_full) { pinocchio::forwardKinematics(model_, *data_, q_full); pinocchio::updateFramePlacements(model_, *data_); diff --git a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp index 3174b44e..ce7af494 100644 --- a/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp +++ b/cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp @@ -14,6 +14,8 @@ #include // std::clamp, std::max, std::min #include // std::sqrt +#include +#include namespace cmvr { using Eigen::Matrix4d; @@ -36,6 +38,69 @@ namespace cmvr { , qp_time_limit_(positiveOr(config.qp_time_limit(), 1e-2)) , solver_() { + setJointLimitPolicy(config.joint_limit_policy()); + } + + bool PinocchioQpIKSolver::refreshJointLimitPolicy_() + { + const auto& policy = jointLimitPolicy(); + const auto source = policy.limits().source(); + if (jointLimitsDisabled()) { + disableJointLimitCache(); + qdd_max_global_.resize(chain_q_dof_); + qdd_max_global_.setConstant(disabledJointLimitAbs()); + return true; + } + if (source == config::JOINT_LIMIT_SOURCE_URDF) { + return true; + } + if (source != config::JOINT_LIMIT_SOURCE_CUSTOM) { + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] unsupported joint limit source"; + return false; + } + + std::vector joint_names; + if (!getChainJointNames(joint_names) || joint_names.empty()) { + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] failed to get chain joint names for custom limits"; + return false; + } + + std::unordered_map custom_limits; + const auto& limits = policy.limits(); + custom_limits.reserve(static_cast(limits.joints_size())); + for (const auto& item : limits.joints()) { + if (!item.joint_name().empty()) { + custom_limits[item.joint_name()] = item; + } + } + + const auto dof = static_cast(joint_names.size()); + joint_pos_lower_limits_.resize(dof); + joint_pos_upper_limits_.resize(dof); + joint_vel_limits_.resize(dof); + qdd_max_global_.resize(dof); + for (Eigen::Index i = 0; i < dof; ++i) { + const auto& joint_name = joint_names[static_cast(i)]; + const auto it = custom_limits.find(joint_name); + if (it == custom_limits.end()) { + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] missing custom joint limit for " << joint_name; + return false; + } + const auto& limit = it->second; + if (!std::isfinite(limit.q_lb()) || !std::isfinite(limit.q_ub()) || + !std::isfinite(limit.qd()) || !std::isfinite(limit.qdd()) || + limit.q_ub() <= limit.q_lb() || limit.qd() <= 0.0 || + limit.qdd() < 0.0) { + CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] invalid custom joint limit for " + << limit.joint_name(); + return false; + } + joint_pos_lower_limits_[i] = limit.q_lb(); + joint_pos_upper_limits_[i] = limit.q_ub(); + joint_vel_limits_[i] = std::abs(limit.qd()); + qdd_max_global_[i] = std::abs(limit.qdd()); + } + return true; } bool PinocchioQpIKSolver::init() { @@ -89,6 +154,10 @@ namespace cmvr { expected_v += seg.nv; } + if (!refreshJointLimitPolicy_()) { + return false; + } + if (joint_pos_lower_limits_.size() != chain_q_dof_ || joint_pos_upper_limits_.size() != chain_q_dof_) { CMVR_LOG(ERROR) << "[PinocchioQpIKSolver] position limits size mismatch with q_dof."; @@ -100,8 +169,10 @@ namespace cmvr { joint_vel_limits_.resize(chain_v_dof_); joint_vel_limits_.setConstant(big); } - qdd_max_global_.resize(chain_q_dof_); - qdd_max_global_.setConstant(big); + if (qdd_max_global_.size() != chain_q_dof_) { + qdd_max_global_.resize(chain_q_dof_); + qdd_max_global_.setConstant(big); + } // 子链当前关节角（外部 update_joints_state 也会覆盖） if (cur_joints_angle_.empty()) { @@ -316,17 +387,39 @@ namespace cmvr { } const int dof = chain_v_dof_; - MatrixXd cost(6 + dof, dof); - VectorXd target(6 + dof); + const auto& avoidance = jointLimitPolicy().avoidance(); + const bool use_joint_limit_avoidance = + !jointLimitsDisabled() && avoidance.enable() && avoidance.weight() > 0.0; + const int avoidance_rows = use_joint_limit_avoidance ? dof : 0; + + MatrixXd cost(6 + dof + avoidance_rows, dof); + VectorXd target(6 + dof + avoidance_rows); cost.setZero(); target.setZero(); cost.topRows(6) = jacobian_base; target.head(6) = target_twist_base; - cost.bottomRows(dof) = std::sqrt(lambda_) * MatrixXd::Identity(dof, dof); + cost.middleRows(6, dof) = + std::sqrt(lambda_) * MatrixXd::Identity(dof, dof); + target.segment(6, dof).setZero(); VectorXd lower(dof); VectorXd upper(dof); const Eigen::Map q_chain(q_chain_std.data(), dof); + if (use_joint_limit_avoidance) { + const VectorXd qdot_avoid = + cmvr::kinematics::computeJointLimitAvoidanceVelocity( + q_chain, + joint_pos_lower_limits_, + joint_pos_upper_limits_, + true, + positiveOr(avoidance.gain(), 0.2), + positiveOr(avoidance.margin_ratio(), 0.15), + positiveOr(avoidance.max_push(), 0.25)); + const double sqrt_weight = std::sqrt(positiveOr(avoidance.weight(), 0.05)); + cost.middleRows(6 + dof, dof) = + sqrt_weight * MatrixXd::Identity(dof, dof); + target.segment(6 + dof, dof) = sqrt_weight * qdot_avoid; + } for (int i = 0; i < dof; ++i) { double limit = std::numeric_limits::infinity(); if (joint_vel_limits_.size() == dof) { @@ -382,6 +475,7 @@ namespace cmvr { if (qdot.size() != dof) { return false; } + qdot = applyJointSoftLimitsToVelocity(q_chain, qdot); qdot_out.assign(qdot.data(), qdot.data() + qdot.size()); return true; } diff --git a/cmvr-es/algorithms/kinematics/ik_solver/tests/src/ik_test.cpp b/cmvr-es/algorithms/kinematics/ik_solver/tests/src/ik_test.cpp deleted file mode 100644 index 000f9daf..00000000 --- a/cmvr-es/algorithms/kinematics/ik_solver/tests/src/ik_test.cpp +++ /dev/null @@ -1,661 +0,0 @@ -// -// Created by lgv on 11/28/25. -// - - - -#include -#include -#include -#include -#include -#include -#include "pinocchio/multibody/sample-models.hpp" -#include -#include "gtest/gtest.h" -#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" -#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h" -#include "algorithms/kinematics/ik_solver/lawba/include/lawba_ik_solver.h" -#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" -using namespace cmvr; - -namespace { - -constexpr const char* kDefaultUrdf = - "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf"; -constexpr const char* kDefaultBaseFrame = "PELVIS_S"; -constexpr const char* kDefaultFlangeFrame = "R_WRIST_R_S"; -constexpr const char* kDefaultTcpFrame = "R_FINGER_TIP_FIXED"; - -config::PinocchioDlsIKConfig makeDlsConfig() -{ - config::PinocchioDlsIKConfig cfg; - cfg.set_urdf_path(kDefaultUrdf); - cfg.set_base_frame_name(kDefaultBaseFrame); - cfg.set_flange_frame_name(kDefaultFlangeFrame); - cfg.set_tcp_frame_name(kDefaultTcpFrame); - cfg.set_max_iters(100); - cfg.set_pos_eps(1e-6); - cfg.set_rot_eps(1e-6); - cfg.set_damping(1e-4); - return cfg; -} - -config::PinocchioQpIKConfig makeQpConfig() -{ - config::PinocchioQpIKConfig cfg; - cfg.set_urdf_path(kDefaultUrdf); - cfg.set_base_frame_name(kDefaultBaseFrame); - cfg.set_flange_frame_name(kDefaultFlangeFrame); - cfg.set_tcp_frame_name(kDefaultTcpFrame); - cfg.set_lambda(1e-4); - cfg.set_w_posrot(0.5); - cfg.set_max_iters(100); - cfg.set_tol(1e-6); - cfg.set_qp_time_limit(1e-2); - return cfg; -} - -} // namespace - - -class DualArmViewer : public MuJocoViewer { -public: - using Vec7 = std::vector; - - explicit DualArmViewer(const char *model_path) - : MuJocoViewer(model_path), - q_cmd_(7, 0.0) { // 初始 7 维全 0 - } - - // 更新右臂 7 关节的目标角度（rad） - // 顺序： - // [R_SHOULDER_P, R_SHOULDER_R, R_SHOULDER_Y, - // R_ELBOW_R, R_WRIST_P, R_WRIST_Y, R_WRIST_R] - void moveJ(const Vec7 &q_target) { - std::lock_guard lk(mtx_); - q_cmd_ = q_target; // 只更新目标，不直接改 d->ctrl - } - -protected: - // 只在第一次进控制循环时调用 - void initOnce(mjModel *m, mjData *d) override { - UNUSED_VARIABLE(d); - - // actuator 名（已经有） - const char *act_names[7] = { - "R_SHOULDER_P_pos", - "R_SHOULDER_R_pos", - "R_SHOULDER_Y_pos", - "R_ELBOW_R_pos", - "R_WRIST_P_pos", - "R_WRIST_Y_pos", - "R_WRIST_R_pos" - }; - - // 对应的 joint 名 - const char *jnt_names[7] = { - "R_SHOULDER_P", - "R_SHOULDER_R", - "R_SHOULDER_Y", - "R_ELBOW_R", - "R_WRIST_P", - "R_WRIST_Y", - "R_WRIST_R" - }; - - for (int i = 0; i < 7; ++i) { - // 1. 保存 actuator id - int act_id = mj_name2id(m, mjOBJ_ACTUATOR, act_names[i]); - right_act_ids_[i] = act_id; - if (act_id < 0) { - std::cerr << "[DualArmViewer] actuator not found: " - << act_names[i] << std::endl; - } - - // 2. 保存 joint id - int jnt_id = mj_name2id(m, mjOBJ_JOINT, jnt_names[i]); - right_jnt_ids_[i] = jnt_id; - if (jnt_id < 0) { - std::cerr << "[DualArmViewer] joint not found: " - << jnt_names[i] << std::endl; - } - } - - act_ids_inited_ = true; - } - - // 每个 mj_step 前会被 physics 线程调用 - void controlCallback(mjModel *m, mjData *d) override { - if (!act_ids_inited_) return; - - Vec7 q_local(7, 0.0); - { - std::lock_guard lk(mtx_); - q_local = q_cmd_; // 拷贝一份当前目标，避免长时间持锁 - } - - for (int i = 0; i < 7; ++i) { - int act_id = right_act_ids_[i]; - if (act_id < 0) continue; - d->ctrl[act_id] = q_local[i]; - } - - // // 示例：读取第 5 个关节（R_WRIST_P）的实际角度并打印 - // int jnt_id = right_jnt_ids_[4]; - // if (jnt_id >= 0) { - // int qpos_adr = m->jnt_qposadr[jnt_id]; - // double q_actual = d->qpos[qpos_adr]; - // std::cout << "cmd: " << q_local[4] - // << " act: " << q_actual << std::endl; - // } - } - - void onReset(mjModel *m, mjData *d) override { - UNUSED_VARIABLE(m); - UNUSED_VARIABLE(d); - std::lock_guard lk(mtx_); - q_cmd_.assign(7, 0.0); // reset 时把目标清零，保持 7 维 - } - -private: - std::array right_act_ids_{}; // 右臂 7 个 actuator id - std::array right_jnt_ids_{}; // 右臂 7 个 joint id（新加） - bool act_ids_inited_{false}; - - Vec7 q_cmd_; // 当前命令目标角（始终 7 维） - mutable std::mutex mtx_; // 保护 q_cmd_ -}; - - -// ---------- 工具函数：计算均值 + 标准差 ---------- -static void computeMeanStd(const std::vector &data, - double &mean, double &stddev) -{ - if (data.empty()) { - mean = stddev = std::numeric_limits::quiet_NaN(); - return; - } - double sum = 0.0; - for (double x : data) sum += x; - mean = sum / static_cast(data.size()); - - double var = 0.0; - if (data.size() > 1) { - for (double x : data) { - double d = x - mean; - var += d * d; - } - var /= static_cast(data.size() - 1); - } - stddev = std::sqrt(var); -} - -// 姿态误差：两旋转矩阵之间的角度（弧度制） -static double rotationError(const Eigen::Matrix3d &R_des, - const Eigen::Matrix3d &R_cur) -{ - Eigen::Matrix3d R_err = R_des.transpose() * R_cur; - double cos_theta = (R_err.trace() - 1.0) * 0.5; - if (cos_theta > 1.0) cos_theta = 1.0; - if (cos_theta < -1.0) cos_theta = -1.0; - return std::acos(cos_theta); // rad -} - -struct SolverStats { - std::string name; - std::vector times_ms; // 单次成功求解耗时 - std::vector pos_errors; // 末端位置误差 (m) - std::vector ori_errors; // 姿态误差 (rad) - int attempts = 0; // 尝试次数（有 target_pose 就算一次） - int success = 0; // 成功次数（误差在阈值内） -}; - -void benchmarkIkSolversRandomJoints(DualArmViewer &viewer) -{ - // ========== 1. 7 个关节限位，直接用你给的 joints_limits_ 定义 ========== - const std::array, 7> joints_limits_ = {{ - {-0.26, 1.57}, - {-0.78, 1.57}, - {-3.1415, 3.1415}, - { 0.0, 2.05}, - {-3.1415, 3.1415}, - {-0.78, 0.78}, - {-0.26, 1.57}, - }}; - - constexpr int N_SAMPLES = 10; - - // ========== 2. 创建三个求解器实例 ========== - - // 数值优化类 QP IK - PinocchioQpIKSolver qp_solver(makeQpConfig()); - - // 基于广义逆雅可比矩阵的数值增量 IK - PinocchioDlsIKSolver pinv_solver(makeDlsConfig()); - - // 你的专利方法：臂角 ψ + 可行域 + 势场 - LawbaIKSolver psi_solver(config::LawbaIKConfig{}); - - // 统一容器，方便 for 循环 - std::vector solvers = { - &psi_solver, - &pinv_solver, - &qp_solver - }; - - - std::vector stats = { - {"OptPsiLimitBias (Patent)", {}, {}, {}, 0, 0}, - {"Pinocchio Jacobian IK", {}, {}, {}, 0, 0}, - {"Pinocchio QP IK", {}, {}, {}, 0, 0} - }; - - // ========== 3. 初始化（如果类里有 init 就调一下） ========== - for (auto *solver : solvers) { - if (!solver->init()) { - std::cerr << "Warning: solver init() failed.\n"; - } - } - - // ========== 4. 随机数发生器 ========== - std::mt19937 rng(42); // 固定种子，结果可复现 - std::uniform_real_distribution dist01(0.0, 1.0); - - // ========== 5. 主循环：随机关节角 → FK → 三种 IK ========== - // 这里用 PinocchioDlsIKSolver (pinv_solver) 的 FK 作为“真值” - for (int i = 0; i < N_SAMPLES; ++i) { - std::cout << "[ik_compare] sample " << (i + 1) << "/" << N_SAMPLES << std::endl; - - // 5.1 随机生成一组 q_true - std::vector q_true(7); - for (int j = 0; j < 7; ++j) { - double r = dist01(rng); // [0, 1] - double qmin = joints_limits_[j].first; - double qmax = joints_limits_[j].second; - q_true[j] = qmin + r * (qmax - qmin); - } - - // 5.3 为这一组样本生成一个“统一的初始解” q_init（和 q_true 无关） - std::vector q_init(7); - for (int j = 0; j < 7; ++j) { - double r = dist01(rng); - double qmin = joints_limits_[j].first; - double qmax = joints_limits_[j].second; - q_init[j] = qmin + r * (qmax - qmin); - } - - // std::vector q_cur(7, 0.0); - // // q_true = q_cur; - // Eigen::Matrix4d target_pose = Eigen::Matrix4d::Identity(); - // if (!psi_solver.fk(q_true, target_pose, true)) { - // // FK 失败的话，这个样本就跳过 - // continue; - // } - // - // Eigen::Matrix4d target_pose1 = Eigen::Matrix4d::Identity(); - // if (!pinv_solver.fk(q_true, target_pose1, true)) { - // // FK 失败的话，这个样本就跳过 - // continue; - // } - // - // Eigen::Matrix4d target_pose2 = Eigen::Matrix4d::Identity(); - // if (!qp_solver.fk(q_true, target_pose2, true)) { - // // FK 失败的话，这个样本就跳过 - // continue; - // } - - - - // 5.3 三个 IK 分别求解 - for (size_t idx = 0; idx < solvers.size(); ++idx) { - - IKSolver *solver = solvers[idx]; - SolverStats &s = stats[idx]; - s.attempts++; - - // 5.2 用 pinv_solver 的 FK 计算目标末端位姿 target_pose - - Eigen::Matrix4d target_pose = Eigen::Matrix4d::Identity(); - if (!solver->fk(q_true, target_pose, false)) { - // FK 失败的话，这个样本就跳过 - continue; - } - - // 先用真实关节角更新内部状态（按你之前的约定） - // std::vector q_cur(7, 0.0); - solver->update_joints_state(q_init); - - std::vector q_sol; - q_sol.reserve(7); - - auto t0 = std::chrono::steady_clock::now(); - bool ok = solver->ik(target_pose, q_sol, false); // is_tcp = true - auto t1 = std::chrono::steady_clock::now(); - - if (!ok || q_sol.size() != 7) { - // 求解失败，不计入成功统计 - continue; - } - - viewer.moveJ(q_sol); - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - - double dt_ms = std::chrono::duration_cast< - std::chrono::microseconds>(t1 - t0).count() / 1000.0; - - // 5.4 用该求解器自己的 FK 得到实际末端位姿 pose_sol - Eigen::Matrix4d pose_sol = Eigen::Matrix4d::Identity(); - if (!solver->fk(q_sol, pose_sol, false)) { - continue; - } - - // 5.5 计算末端位置误差 + 姿态误差 - Eigen::Vector3d p_des = target_pose.block<3,1>(0,3); - Eigen::Vector3d p_cur = pose_sol.block<3,1>(0,3); - double pos_err = (p_cur - p_des).norm(); // m - - Eigen::Matrix3d R_des = target_pose.block<3,3>(0,0); - Eigen::Matrix3d R_cur = pose_sol.block<3,3>(0,0); - double ori_err = rotationError(R_des, R_cur); // rad - - // 5.6 判定是否“成功解” - // 阈值可以按你机械臂精度需求调 - const double POS_THRESH = 1e-3; // 0.1 mm - const double ORI_THRESH = 1e-3; // ≈ 0.057° - if (pos_err < POS_THRESH && ori_err < ORI_THRESH) { - s.success++; - s.times_ms.push_back(dt_ms); - s.pos_errors.push_back(pos_err); - s.ori_errors.push_back(ori_err); - } else { - // 这里是“收敛但精度不够”的情况，如果你想也可以单独统计 - } - } - - - } - - // ========== 6. 输出统计结果：耗时/误差（均值 + 标准差）+ 成功率 ========== - for (const auto &s : stats) { - std::cout << "========== Solver: " << s.name << " ==========\n"; - std::cout << "Attempts: " << s.attempts - << ", Success: " << s.success; - if (s.attempts > 0) { - double succ_rate = - 100.0 * static_cast(s.success) / - static_cast(s.attempts); - std::cout << " (Success rate: " << succ_rate << "%)\n"; - } else { - std::cout << " (No attempts)\n"; - } - - double mean_t, std_t; - computeMeanStd(s.times_ms, mean_t, std_t); - - double mean_ep, std_ep; - computeMeanStd(s.pos_errors, mean_ep, std_ep); - - double mean_er, std_er; - computeMeanStd(s.ori_errors, mean_er, std_er); - - std::cout << "Time (ms) : mean = " << mean_t - << ", std = " << std_t << "\n"; - std::cout << "PosErr (m) : mean = " << mean_ep - << ", std = " << std_ep << "\n"; - std::cout << "OriErr (rad) : mean = " << mean_er - << ", std = " << std_er << "\n\n"; - } -} - - - - -void benchmarkIkSolversRandomJoints() -{ - // ========== 1. 7 个关节限位，直接用你给的 joints_limits_ 定义 ========== - const std::array, 7> joints_limits_ = {{ - - {-0.26, 1.57}, - {-0.78, 1.57}, - {-M_PI, M_PI}, - { 0.0, 2.05}, - {-M_PI, M_PI}, - {-0.78, 0.78}, - {-0.26, 1.57}, - - }}; - - constexpr int N_SAMPLES = 10; - - // ========== 2. 创建三个求解器实例 ========== - - // 数值优化类 QP IK - PinocchioQpIKSolver qp_solver(makeQpConfig()); - - // 基于广义逆雅可比矩阵的数值增量 IK - PinocchioDlsIKSolver pinv_solver(makeDlsConfig()); - - // 你的专利方法：臂角 ψ + 可行域 + 势场 - LawbaIKSolver psi_solver(config::LawbaIKConfig{}); - - // 统一容器，方便 for 循环 - std::vector solvers = { - &psi_solver, - &pinv_solver, - &qp_solver - }; - - - std::vector stats = { - {"OptPsiLimitBias (Patent)", {}, {}, {}, 0, 0}, - {"Pinocchio Jacobian IK", {}, {}, {}, 0, 0}, - {"Pinocchio QP IK", {}, {}, {}, 0, 0} - }; - - // ========== 3. 初始化（如果类里有 init 就调一下） ========== - for (auto *solver : solvers) { - if (!solver->init()) { - std::cerr << "Warning: solver init() failed.\n"; - } - } - - // ========== 4. 随机数发生器 ========== - std::mt19937 rng(42); // 固定种子，结果可复现 - std::uniform_real_distribution dist01(0.0, 1.0); - - // ========== 5. 主循环：随机关节角 → FK → 三种 IK ========== - // 这里用 PinocchioDlsIKSolver (pinv_solver) 的 FK 作为“真值” - for (int i = 0; i < N_SAMPLES; ++i) { - std::cout << "[ik_compare] sample " << (i + 1) << "/" << N_SAMPLES << std::endl; - - // 5.1 随机生成一组 q_true - std::vector q_true(7); - for (int j = 0; j < 7; ++j) { - double r = dist01(rng); // [0, 1] - double qmin = joints_limits_[j].first; - double qmax = joints_limits_[j].second; - q_true[j] = qmin + r * (qmax - qmin); - } - - // 5.3 为这一组样本生成一个“统一的初始解” q_init（和 q_true 无关） - std::vector q_init(7); - for (int j = 0; j < 7; ++j) { - double r = dist01(rng); - double qmin = joints_limits_[j].first; - double qmax = joints_limits_[j].second; - q_init[j] = qmin + r * (qmax - qmin); - } - - // std::vector q_cur(7, 0.0); - // // q_true = q_cur; - // Eigen::Matrix4d target_pose = Eigen::Matrix4d::Identity(); - // if (!psi_solver.fk(q_true, target_pose, true)) { - // // FK 失败的话，这个样本就跳过 - // continue; - // } - // - // Eigen::Matrix4d target_pose1 = Eigen::Matrix4d::Identity(); - // if (!pinv_solver.fk(q_true, target_pose1, true)) { - // // FK 失败的话，这个样本就跳过 - // continue; - // } - // - // Eigen::Matrix4d target_pose2 = Eigen::Matrix4d::Identity(); - // if (!qp_solver.fk(q_true, target_pose2, true)) { - // // FK 失败的话，这个样本就跳过 - // continue; - // } - - - - // 5.3 三个 IK 分别求解 - for (size_t idx = 0; idx < solvers.size(); ++idx) { - - IKSolver *solver = solvers[idx]; - SolverStats &s = stats[idx]; - s.attempts++; - - // 5.2 用 pinv_solver 的 FK 计算目标末端位姿 target_pose - - Eigen::Matrix4d target_pose = Eigen::Matrix4d::Identity(); - if (!solver->fk(q_true, target_pose, false)) { - // FK 失败的话，这个样本就跳过 - continue; - } - - // 先用真实关节角更新内部状态（按你之前的约定） - // std::vector q_cur(7, 0.0); - solver->update_joints_state(q_init); - - std::vector q_sol; - q_sol.reserve(7); - - auto t0 = std::chrono::steady_clock::now(); - bool ok = solver->ik(target_pose, q_sol, false); // is_tcp = true - auto t1 = std::chrono::steady_clock::now(); - - if (!ok || q_sol.size() != 7) { - // 求解失败，不计入成功统计 - continue; - } - - - double dt_ms = std::chrono::duration_cast< - std::chrono::microseconds>(t1 - t0).count() / 1000.0; - - // 5.4 用该求解器自己的 FK 得到实际末端位姿 pose_sol - Eigen::Matrix4d pose_sol = Eigen::Matrix4d::Identity(); - if (!solver->fk(q_sol, pose_sol, false)) { - continue; - } - - // 5.5 计算末端位置误差 + 姿态误差 - Eigen::Vector3d p_des = target_pose.block<3,1>(0,3); - Eigen::Vector3d p_cur = pose_sol.block<3,1>(0,3); - double pos_err = (p_cur - p_des).norm(); // m - - Eigen::Matrix3d R_des = target_pose.block<3,3>(0,0); - Eigen::Matrix3d R_cur = pose_sol.block<3,3>(0,0); - double ori_err = rotationError(R_des, R_cur); // rad - - // 5.6 判定是否“成功解” - // 阈值可以按你机械臂精度需求调 - const double POS_THRESH = 1e-3; // 0.1 mm - const double ORI_THRESH = 1e-3; // ≈ 0.057° - if (pos_err < POS_THRESH && ori_err < ORI_THRESH) { - s.success++; - s.times_ms.push_back(dt_ms); - s.pos_errors.push_back(pos_err); - s.ori_errors.push_back(ori_err); - } else { - // 这里是“收敛但精度不够”的情况，如果你想也可以单独统计 - } - } - - - } - - // ========== 6. 输出统计结果：耗时/误差（均值 + 标准差）+ 成功率 ========== - for (const auto &s : stats) { - std::cout << "========== Solver: " << s.name << " ==========\n"; - std::cout << "Attempts: " << s.attempts - << ", Success: " << s.success; - if (s.attempts > 0) { - double succ_rate = - 100.0 * static_cast(s.success) / - static_cast(s.attempts); - std::cout << " (Success rate: " << succ_rate << "%)\n"; - } else { - std::cout << " (No attempts)\n"; - } - - double mean_t, std_t; - computeMeanStd(s.times_ms, mean_t, std_t); - - double mean_ep, std_ep; - computeMeanStd(s.pos_errors, mean_ep, std_ep); - - double mean_er, std_er; - computeMeanStd(s.ori_errors, mean_er, std_er); - - std::cout << "Time (ms) : mean = " << mean_t - << ", std = " << std_t << "\n"; - std::cout << "PosErr (m) : mean = " << mean_ep - << ", std = " << std_ep << "\n"; - std::cout << "OriErr (rad) : mean = " << mean_er - << ", std = " << std_er << "\n\n"; - } -} - -TEST(ik_test,pinocchio_lib_test) { - PinocchioQpIKSolver solver(makeQpConfig()); - // PinocchioDlsIKSolver solver(makeDlsConfig()); - ASSERT_TRUE(solver.init()); - - - // 真实关节角（比如从控制器读回来） - std::vector q_cur(7, 0.0); - q_cur[5] = 0.236; - q_cur[4] = -0.236; - q_cur[3] = 0.156; - q_cur[2] = 0.036; - q_cur[1] = 0.236; - solver.update_joints_state(q_cur); - - // 1) 先求当前 TCP 位姿 - Eigen::Matrix4d cur_tcp_pose = Eigen::Matrix4d::Identity(); - ASSERT_TRUE(solver.fk(q_cur, cur_tcp_pose, false)); - std::cout << cur_tcp_pose<< std::endl; - - - Eigen::Matrix4d target_tcp_pose = cur_tcp_pose; - - - std::vector q_target; - ASSERT_TRUE(solver.ik(target_tcp_pose, q_target, false)); - - for (double q: q_target) { - std::cout << q << std::endl; - } - - Eigen::Matrix4d cur_flange_pose = Eigen::Matrix4d::Identity(); - ASSERT_TRUE(solver.fk(q_target, cur_flange_pose, false)); - std::cout << cur_flange_pose<< std::endl; -} - -TEST(ik_test,ik_compare) { - constexpr const char* model_path = - "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml"; - - DualArmViewer viewer(model_path); - std::thread benchmark_thread([&viewer]() { - // viewer.run() 在主线程创建仿真和渲染资源，稍后再开始发送关节目标。 - std::this_thread::sleep_for(std::chrono::seconds(1)); - benchmarkIkSolversRandomJoints(viewer); - std::cout << "[ik_compare] benchmark finished; close the viewer to exit." << std::endl; - }); - - viewer.run(); - benchmark_thread.join(); -} diff --git a/cmvr-es/algorithms/kinematics/ik_solver/tests/src/srs_ik_test.cpp b/cmvr-es/algorithms/kinematics/ik_solver/tests/src/srs_ik_test.cpp deleted file mode 100644 index 4abc96e7..00000000 --- a/cmvr-es/algorithms/kinematics/ik_solver/tests/src/srs_ik_test.cpp +++ /dev/null @@ -1,1898 +0,0 @@ -// -// Created by lgv on 2025/11/3. -// - -#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" -#include "gtest/gtest.h" -#include "algorithms/kinematics/ik_solver/srs/include/srs_ik_solver.h" -#include "algorithms/kinematics/ik_solver/lawba/include/joints_limit_analyzer.h" -#include "algorithms/kinematics/ik_solver/lawba/include/opt_psi_selector.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "algorithms/kinematics/ik_solver/lawba/include/lawba_ik_solver.h" -#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h" -#include "common/math/transform_math.h" -#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h" -#include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h" -#include "common/math/support_functions.h" -#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h" -#include -using namespace cmvr; - -namespace { - -constexpr const char* kDefaultUrdf = - "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf"; -constexpr const char* kDefaultBaseFrame = "PELVIS_S"; -constexpr const char* kDefaultFlangeFrame = "R_WRIST_R_S"; -constexpr const char* kDefaultTcpFrame = "R_FINGER_TIP_FIXED"; - -config::PinocchioDlsIKConfig makeDlsConfig() -{ - config::PinocchioDlsIKConfig cfg; - cfg.set_urdf_path(kDefaultUrdf); - cfg.set_base_frame_name(kDefaultBaseFrame); - cfg.set_flange_frame_name(kDefaultFlangeFrame); - cfg.set_tcp_frame_name(kDefaultTcpFrame); - cfg.set_max_iters(100); - cfg.set_pos_eps(1e-6); - cfg.set_rot_eps(1e-6); - cfg.set_damping(1e-4); - return cfg; -} - -config::PinocchioQpIKConfig makeQpConfig() -{ - config::PinocchioQpIKConfig cfg; - cfg.set_urdf_path(kDefaultUrdf); - cfg.set_base_frame_name(kDefaultBaseFrame); - cfg.set_flange_frame_name(kDefaultFlangeFrame); - cfg.set_tcp_frame_name(kDefaultTcpFrame); - cfg.set_lambda(1e-4); - cfg.set_w_posrot(0.5); - cfg.set_max_iters(100); - cfg.set_tol(1e-6); - cfg.set_qp_time_limit(1e-2); - return cfg; -} - -} // namespace - - -struct IkSample { - double psi; - std::array q; // q1..q7 -}; - - -bool write_ik_samples_csv(const std::string &filepath, - const std::vector &samples, - bool write_header, - int precision) { - std::ofstream ofs(filepath, std::ios::out | std::ios::trunc); - if (!ofs.is_open()) return false; - - // 固定小数点（避免本地化成逗号） - ofs.imbue(std::locale::classic()); - ofs << std::fixed << std::setprecision(precision); - - if (write_header) { - ofs << "psi,q1,q2,q3,q4,q5,q6,q7\n"; - } - for (const auto &s: samples) { - ofs << s.psi; - for (int i = 0; i < 7; ++i) ofs << ',' << s.q[i]; - ofs << '\n'; - } - return true; -} - -static std::filesystem::path find_project_root() { - const auto marker = std::filesystem::path("model/xiaoyan_description/dual_arm.xml"); - auto probe = [&](std::filesystem::path p) -> std::filesystem::path { - for (auto cur = std::move(p); !cur.empty(); cur = cur.parent_path()) { - if (std::filesystem::exists(cur / marker)) { - return cur; - } - const auto parent = cur.parent_path(); - if (parent == cur) { - break; - } - } - return {}; - }; - - auto root = probe(std::filesystem::current_path()); - if (!root.empty()) { - return root; - } - - root = probe(std::filesystem::path(__FILE__).parent_path()); - if (!root.empty()) { - return root; - } - - return std::filesystem::current_path(); -} - -class DualArmViewer : public MuJocoViewer { -public: - using Vec7 = std::vector; - enum class ControlMode { - PositionHold, - VelocityHold - }; - - explicit DualArmViewer(const char *model_path) - : MuJocoViewer(model_path), - q_cmd_(7, 0.0), - qd_cmd_(7, 0.0), - q_meas_(7, 0.0), - qd_meas_(7, 0.0) {} - - void moveJ(const Vec7 &q_target) { - std::lock_guard lk(mtx_); - q_cmd_ = q_target; - qd_cmd_.assign(7, 0.0); - control_mode_ = ControlMode::PositionHold; - } - - void speedJ(const Vec7 &qd_target) { - std::lock_guard lk(mtx_); - qd_cmd_ = qd_target; - control_mode_ = ControlMode::VelocityHold; - } - - Vec7 getQ() const { - std::lock_guard lk(mtx_); - return q_meas_; - } - - Vec7 getQd() const { - std::lock_guard lk(mtx_); - return qd_meas_; - } - - void getJointState(Vec7& q, Vec7& qd) const { - std::lock_guard lk(mtx_); - q = q_meas_; - qd = qd_meas_; - } - - bool getTcpTwistState(Eigen::Vector3d& position, - Eigen::Vector3d& linear_velocity, - Eigen::Vector3d& angular_velocity) const { - std::lock_guard lk(mtx_); - if (tcp_site_id_ < 0) { - return false; - } - position = tcp_pos_meas_; - linear_velocity = tcp_vel_meas_; - angular_velocity = tcp_omega_meas_; - return true; - } - - bool getTcpLinearState(Eigen::Vector3d& position, Eigen::Vector3d& velocity) const { - std::lock_guard lk(mtx_); - if (tcp_site_id_ < 0) { - return false; - } - position = tcp_pos_meas_; - velocity = tcp_vel_meas_; - return true; - } - -protected: - static constexpr int kLeftPositionActuatorGroup = 0; - static constexpr int kLeftVelocityActuatorGroup = 1; - static constexpr int kRightPositionActuatorGroup = 2; - static constexpr int kRightVelocityActuatorGroup = 3; - - void initOnce(mjModel *m, mjData *d) override { - UNUSED_VARIABLE(d); - - const char *pos_act_names[7] = { - "R_SHOULDER_P_pos","R_SHOULDER_R_pos","R_SHOULDER_Y_pos", - "R_ELBOW_R_pos","R_WRIST_P_pos","R_WRIST_Y_pos","R_WRIST_R_pos" - }; - const char *vel_act_names[7] = { - "R_SHOULDER_P_vel","R_SHOULDER_R_vel","R_SHOULDER_Y_vel", - "R_ELBOW_R_vel","R_WRIST_P_vel","R_WRIST_Y_vel","R_WRIST_R_vel" - }; - const char *joint_names[7] = { - "R_SHOULDER_P","R_SHOULDER_R","R_SHOULDER_Y", - "R_ELBOW_R","R_WRIST_P","R_WRIST_Y","R_WRIST_R" - }; - - for (int i = 0; i < 7; ++i) { - const int pos_act_id = mj_name2id(m, mjOBJ_ACTUATOR, pos_act_names[i]); - right_pos_act_ids_[i] = pos_act_id; - if (pos_act_id < 0) { - std::cerr << "[DualArmViewer] position actuator not found: " - << pos_act_names[i] << "\n"; - } - - const int vel_act_id = mj_name2id(m, mjOBJ_ACTUATOR, vel_act_names[i]); - right_vel_act_ids_[i] = vel_act_id; - if (vel_act_id < 0) { - std::cerr << "[DualArmViewer] velocity actuator not found: " - << vel_act_names[i] << "\n"; - } - - const int joint_id = mj_name2id(m, mjOBJ_JOINT, joint_names[i]); - right_qpos_ids_[i] = (joint_id >= 0) ? m->jnt_qposadr[joint_id] : -1; - right_qvel_ids_[i] = (joint_id >= 0) ? m->jnt_dofadr[joint_id] : -1; - if (joint_id < 0) { - std::cerr << "[DualArmViewer] joint not found: " << joint_names[i] << "\n"; - } - } - - tcp_site_id_ = mj_name2id(m, mjOBJ_SITE, "R_FINGER_TIP_SITE"); - if (tcp_site_id_ < 0) { - std::cerr << "[DualArmViewer] tcp site not found: R_FINGER_TIP_SITE\n"; - } - act_ids_inited_ = true; - } - - static inline double clamp(double x, double lo, double hi) { - return std::max(lo, std::min(hi, x)); - } - - void controlCallback(mjModel *m, mjData *d) override { - if (!act_ids_inited_) return; - - Vec7 q_local(7, 0.0); - Vec7 qd_local(7, 0.0); - ControlMode mode_local = ControlMode::PositionHold; - - { - std::lock_guard lk(mtx_); - mode_local = control_mode_; - if (control_mode_ == ControlMode::PositionHold) { - q_local = q_cmd_; - } else { - qd_local = qd_cmd_; - } - } - - int disable_mask = (1 << kLeftVelocityActuatorGroup); - if (mode_local == ControlMode::PositionHold) { - disable_mask |= (1 << kRightVelocityActuatorGroup); - } else { - disable_mask |= (1 << kRightPositionActuatorGroup); - } - m->opt.disableactuator = disable_mask; - - Vec7 q_meas(7, 0.0); - Vec7 qd_meas(7, 0.0); - for (int i = 0; i < 7; ++i) { - const int qpos_id = right_qpos_ids_[i]; - if (qpos_id >= 0) { - q_meas[i] = d->qpos[qpos_id]; - } - const int qvel_id = right_qvel_ids_[i]; - if (qvel_id >= 0) { - qd_meas[i] = d->qvel[qvel_id]; - } - } - - Eigen::Vector3d tcp_pos = Eigen::Vector3d::Zero(); - Eigen::Vector3d tcp_vel = Eigen::Vector3d::Zero(); - Eigen::Vector3d tcp_omega = Eigen::Vector3d::Zero(); - if (tcp_site_id_ >= 0) { - const mjtNum* site_xpos = d->site_xpos + 3 * tcp_site_id_; - tcp_pos = Eigen::Vector3d(site_xpos[0], site_xpos[1], site_xpos[2]); - - mjtNum site_vel[6] = {0, 0, 0, 0, 0, 0}; - mj_objectVelocity(m, d, mjOBJ_SITE, tcp_site_id_, site_vel, 0); - tcp_omega = Eigen::Vector3d(site_vel[0], site_vel[1], site_vel[2]); - tcp_vel = Eigen::Vector3d(site_vel[3], site_vel[4], site_vel[5]); - } - - { - std::lock_guard lk(mtx_); - q_meas_ = q_meas; - qd_meas_ = qd_meas; - tcp_pos_meas_ = tcp_pos; - tcp_vel_meas_ = tcp_vel; - tcp_omega_meas_ = tcp_omega; - } - - for (int i = 0; i < 7; ++i) { - const int pos_act_id = right_pos_act_ids_[i]; - const int vel_act_id = right_vel_act_ids_[i]; - - if (mode_local == ControlMode::PositionHold) { - if (pos_act_id >= 0) { - const double lo = m->actuator_ctrlrange[2 * pos_act_id + 0]; - const double hi = m->actuator_ctrlrange[2 * pos_act_id + 1]; - d->ctrl[pos_act_id] = clamp(q_local[i], lo, hi); - } - if (vel_act_id >= 0) { - const double lo = m->actuator_ctrlrange[2 * vel_act_id + 0]; - const double hi = m->actuator_ctrlrange[2 * vel_act_id + 1]; - d->ctrl[vel_act_id] = clamp(0.0, lo, hi); - } - } else { - if (pos_act_id >= 0) { - const double lo = m->actuator_ctrlrange[2 * pos_act_id + 0]; - const double hi = m->actuator_ctrlrange[2 * pos_act_id + 1]; - d->ctrl[pos_act_id] = clamp(q_meas[i], lo, hi); - } - if (vel_act_id >= 0) { - const double lo = m->actuator_ctrlrange[2 * vel_act_id + 0]; - const double hi = m->actuator_ctrlrange[2 * vel_act_id + 1]; - d->ctrl[vel_act_id] = clamp(qd_local[i], lo, hi); - } - } - } - } - - void onReset(mjModel *m, mjData *d) override { - UNUSED_VARIABLE(m); - UNUSED_VARIABLE(d); - std::lock_guard lk(mtx_); - q_cmd_.assign(7, 0.0); - qd_cmd_.assign(7, 0.0); - q_meas_.assign(7, 0.0); - qd_meas_.assign(7, 0.0); - tcp_pos_meas_.setZero(); - tcp_vel_meas_.setZero(); - tcp_omega_meas_.setZero(); - control_mode_ = ControlMode::PositionHold; - } - -private: - std::array right_pos_act_ids_{}; - std::array right_vel_act_ids_{}; - std::array right_qpos_ids_{}; - std::array right_qvel_ids_{}; - int tcp_site_id_{-1}; - bool act_ids_inited_{false}; - - mutable std::mutex mtx_; - Vec7 q_cmd_; - Vec7 qd_cmd_; - Vec7 q_meas_; - Vec7 qd_meas_; - Eigen::Vector3d tcp_pos_meas_{Eigen::Vector3d::Zero()}; - Eigen::Vector3d tcp_vel_meas_{Eigen::Vector3d::Zero()}; - Eigen::Vector3d tcp_omega_meas_{Eigen::Vector3d::Zero()}; - ControlMode control_mode_{ControlMode::PositionHold}; -}; - - - -// TEST(SRS_IK_TEST, SRS_IK_SLOVER_TEST) { -// using std::cout; -// using std::endl; -// -// // const char *model_path = -// // "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.xml"; -// // -// // DualArmViewer viewer(model_path); -// // std::this_thread::sleep_for(std::chrono::seconds(3)); -// -// int viewer = 0; -// // 把所有 IK 运算 + moveJ 循环放到控制线程里 -// std::thread ctrl_thread([&viewer]() { -// SrsIKSolver slover(config::SrsIKConfig{}); -// std::vector samples; -// samples.reserve(4096); -// -// slover.set_shoulder_config(SrsIKSolver::INWARD); -// -// std::vector joint_angles(7, 0); -// // joint_angles = {0.875, 0.22, 0.2644, M_PI / 2, 1.8, 1.99, 1.56}; -// joint_angles = {-0.013, 1.1537, SupportFunctions::normalize_angle(14.25), -0.522261, SupportFunctions::normalize_angle(-15), -0.000210733, -0.12}; -// joint_angles = {-0.058, 0.748, -1.57, 1.0, 2.42, 0.039, -0.159}; -// joint_angles = {0, 0.22,0,0,0, 0.039, -0.159}; -// joint_angles = {1.1976736612467582, -0.34892824845058407,1.7573503667502743,1.2235428312391159,-0.34034282190211762,-0.62403912395160843, 0.58042546477315315}; -// // joint_angles = {0, 0, 0.0, 0, 0.0, 0, 0}; -// -// // 目标位姿：FK(joint_angles) -// const auto target_pose = slover.calc_total_transform(joint_angles); -// cout << "Target Pose (FK from seed joints):\n" << target_pose << endl; -// -// // 系数矩阵 & ψ 扫描区间 -// Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9); -// slover.cal_coefficient_matrix(target_pose, s_mat, w_mat); -// -// auto res = JointsLimitAnalyzer::estimate_psi_from_joints(s_mat, w_mat, joint_angles, slover.get_shoulder_config(), -// slover.get_elbow_config(), slover.get_wrist_config()); -// -// if (res.ok) { -// std::cout << "res.psi" << res.psi << std::endl; -// } -// auto limits = JointsLimitAnalyzer::calc_arm_angle_limits(s_mat, w_mat, slover.get_joints_limits(), -// slover.get_shoulder_config(), -// slover.get_elbow_config(), slover.get_wrist_config()); -// -// // 误差统计 -// const double kPosTol = 1e-4; // 位置容差（m） -// const double kRotTol = 1e-3; // 姿态容差（rad） -// double max_pos_err = 0.0, max_rot_err = 0.0; -// double sum_pos_err = 0.0, sum_rot_err = 0.0; -// size_t total = 0, bad = 0; -// -// const Eigen::Vector3d p_target = target_pose.block<3, 1>(0, 3); -// const Eigen::Matrix3d R_target = target_pose.block<3, 3>(0, 0); -// -// auto clamp = [](double x, double lo, double hi) { -// return std::max(lo, std::min(hi, x)); -// }; -// -// auto rot_err_rad = [&](const Eigen::Matrix3d &R) -> double { -// Eigen::Matrix3d dR = R_target.transpose() * R; -// double c = clamp((dR.trace() - 1.0) * 0.5, -1.0, 1.0); -// return std::acos(c); // [0, pi] -// }; -// -// cout << "psi(rad), pos_err(m), rot_err(rad), rot_err(deg)\n"; -// -// -// -// -// // 1. 先离散 -// std::vector psi_vals; -// for (const auto &limit: limits) { -// const double psi_lo = limit.first; -// const double psi_hi = limit.second; -// for (double psi = psi_lo; psi <= psi_hi; psi += 0.01) { -// psi_vals.push_back(psi); -// } -// } -// -// -// -// int N = static_cast(psi_vals.size()); -// int L = N - 1; -// -// // k 一直往前加，相当于“时间步” -// long long k = 0; -// -// while (true) { -// // 周期为 2L：0,1,...,L,...,1,0,... -// long long t = k % (2LL * L); -// -// // 这个公式实现：0..L..0 的“来回”索引，完全无 if/else -// int idx = L - std::abs(L - static_cast(t)); -// -// double psi = psi_vals[idx]; -// -// auto q = slover.ikWithPsi(target_pose, psi); -// if (q.size() != 7 || std::any_of(q.begin(), q.end(), -// [](double v) { return !std::isfinite(v); })) { -// ++bad; -// ++total; -// cout << psi << ", nan, nan, nan\n"; -// continue; -// } -// -// // // 可视化：右臂关节位置控制 -// // viewer.moveJ(q); // 更新目标角 -// // std::this_thread::sleep_for(std::chrono::duration(0.01)); -// -// // 用 IK 解做 FK，计算误差 -// const auto T_fk = slover.calc_total_transform(q); -// const Eigen::Vector3d p_fk = T_fk.block<3, 1>(0, 3); -// const Eigen::Matrix3d R_fk = T_fk.block<3, 3>(0, 0); -// -// const double pos_err = (p_fk - p_target).norm(); -// const double rot_err = rot_err_rad(R_fk); -// const double rot_err_deg = rot_err * 180.0 / M_PI; -// -// cout << psi << ", " << pos_err << ", " << rot_err -// << ", " << rot_err_deg << "\n"; -// for (double q1: q) { -// cout << q1 << ", "; -// } -// cout << endl; -// cout <<"con = " << q[0]+q[2] + psi << endl; -// -// SCOPED_TRACE(testing::Message() << "psi=" << psi); -// EXPECT_LT(pos_err, kPosTol); -// EXPECT_LT(rot_err, kRotTol); -// -// max_pos_err = std::max(max_pos_err, pos_err); -// max_rot_err = std::max(max_rot_err, rot_err); -// sum_pos_err += pos_err; -// sum_rot_err += rot_err; -// ++total; -// -// samples.push_back(IkSample{ -// psi, -// {q[0], q[1], q[2], q[3], q[4], q[5], q[6]} -// }); -// -// -// -// -// ++k; -// -// if (k > L) -// break; -// } -// -// // 摘要打印 -// cout << "\nSummary:\n" -// << " total=" << total -// << " bad=" << bad -// << " pos_err_max=" << max_pos_err << " m" -// << " rot_err_max=" << max_rot_err << " rad (" -// << max_rot_err * 180.0 / M_PI << " deg)\n" -// << " pos_err_mean=" << (total ? (sum_pos_err / total) : 0.0) << " m" -// << " rot_err_mean=" << (total ? (sum_rot_err / total) : 0.0) << " rad (" -// << (total ? (sum_rot_err / total) * 180.0 / M_PI : 0.0) << " deg)\n"; -// -// // 文件输出 -// write_ik_samples_csv( -// "/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv", samples, true, 9); -// -// // 最终强约束 -// ASSERT_LT(max_pos_err, 10 * kPosTol) << "Max position error too large."; -// ASSERT_LT(max_rot_err, 10 * kRotTol) << "Max rotation error too large."; -// }); -// -// // ★ MuJoCo / OpenGL 一定在主线程跑 -// // viewer.run(); // 阻塞，直到你关掉窗口 -// ctrl_thread.join(); // 控制线程结束 -// -// // 这里不用再 sleep / join sim_thread 了 -// } - - - - -// -// TEST(SRS_IK_TEST, BEST_PSI_SLOVER_TEST) { -// using std::cout; -// using std::endl; -// -// // std::cout << std::fixed << std::setprecision(7); -// -// SrsIKSolver slover(config::SrsIKConfig{}); -// std::vector samples; -// samples.reserve(4096); -// -// // 1: 当前位姿 -// std::vector joint_angles(7, 0); -// joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364}; -// const auto cur_pose = slover.calc_total_transform(joint_angles); -// Eigen::MatrixXd s_mat(3, 9), w_mat(3, 9); -// slover.cal_coefficient_matrix(cur_pose, s_mat, w_mat); -// auto res = JointsLimitAnalyzer::estimate_psi_from_joints(s_mat, w_mat, joint_angles, slover.get_shoulder_config(), -// slover.get_elbow_config(), slover.get_wrist_config()); -// -// // 2: 目标位姿 -// joint_angles = {0.875, 0.22, 0.2644, M_PI / 2, 1.8, 1.99, 1.56}; -// const auto target_pose = slover.calc_total_transform(joint_angles); -// slover.cal_coefficient_matrix(target_pose, s_mat, w_mat); -// -// // 3； 计算limit -// auto limits = JointsLimitAnalyzer::calc_arm_angle_limits(s_mat, w_mat, slover.get_joints_limits(), -// slover.get_shoulder_config(), -// slover.get_elbow_config(), slover.get_wrist_config()); -// -// // 4； 计算best -// OptPsiSelector opt_psi_selector; -// double best_psi{}; -// opt_psi_selector.update_psi(best_psi,res.psi, limits); -// -// // 误差统计 -// const double kPosTol = 1e-4; // 位置容差（m） -// const double kRotTol = 1e-3; // 姿态容差（rad）≈ 0.0573° -// double max_pos_err = 0.0, max_rot_err = 0.0; -// double sum_pos_err = 0.0, sum_rot_err = 0.0; -// size_t total = 0, bad = 0; -// -// // 便捷引用 -// const Eigen::Vector3d p_target = target_pose.block<3, 1>(0, 3); -// const Eigen::Matrix3d R_target = target_pose.block<3, 3>(0, 0); -// -// auto clamp = [](double x, double lo, double hi) { -// return std::max(lo, std::min(hi, x)); -// }; -// -// auto rot_err_rad = [&](const Eigen::Matrix3d &R) -> double { -// Eigen::Matrix3d dR = R_target.transpose() * R; -// double c = clamp((dR.trace() - 1.0) * 0.5, -1.0, 1.0); -// return std::acos(c); // [0, pi] -// }; -// -// // IK 解 -// auto q = slover.ikWithPsi(target_pose, best_psi); -// -// for (double q1: q) { -// std::cout << q1 << " , "; -// } -// std::cout << std::endl; -// -// // 用 IK 解做 FK，计算误差 -// const auto T_fk = slover.calc_total_transform(q); -// const Eigen::Vector3d p_fk = T_fk.block<3, 1>(0, 3); -// const Eigen::Matrix3d R_fk = T_fk.block<3, 3>(0, 0); -// -// const double pos_err = (p_fk - p_target).norm(); -// const double rot_err = rot_err_rad(R_fk); -// const double rot_err_deg = rot_err * 180.0 / M_PI; -// -// -// // 表头 -// cout << "psi(rad), pos_err(m), rot_err(rad), rot_err(deg)\n"; -// cout << best_psi << ", " << pos_err << ", " << rot_err << ", " << rot_err_deg << "\n"; -// } -// -// -// TEST(SRS_IK_TEST, MOVE_L_SLOVER_TEST) { -// using std::cout; -// using std::endl; -// -// -// std::vector samples; -// samples.reserve(4096); -// -// OptPsiLimitBiasSolver solver; -// -// -// // 1) 当前位姿（估计上一时刻 ψ 用） -// std::vector joint_angles(7, 0); -// joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364}; -// solver.update_joints_state(joint_angles); -// samples.push_back(IkSample{ -// 0.0, { -// joint_angles[0], joint_angles[1], -// joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5], joint_angles[6] -// } -// }); -// -// // 2) 目标位姿（作为直线的起点） -// joint_angles = {0.875, 0.22, 0.2644, M_PI / 2, 1.8, 0.29, 0.59}; -// Eigen::Matrix4d target_pose; -// -// solver.fk(joint_angles,target_pose,true); -// -// // 直线插补参数 —— 从 target_pose 出发沿 X 方向 L 米，共 N 段（N+1 个点，包含起点） -// const int N = 100; // 采样点数（间隔均匀） -// const double L = -0.20; // 直线长度 0.20 m -// Eigen::Vector3d dir = Eigen::Vector3d::UnitX(); -// dir.normalize(); -// -// // 固定姿态（也可以改成对姿态做 Slerp） -// const Eigen::Matrix3d R_fixed = target_pose.block < 3, -// 3 > (0, 0); -// const Eigen::Vector3d p0 = target_pose.block < 3, -// 1 > (0, 3); -// -// // 3) 初始 ψ：用估计得到的 ψ，再根据 target_pose 的可行区间做一次更新 -// std::vector q; -// solver.ik(target_pose,q); -// -// // 4) 误差评估工具 -// const auto clamp = [](double x, double lo, double hi) { -// return std::max(lo, std::min(hi, x)); -// }; -// auto rot_err_rad = [&](const Eigen::Matrix3d &R_goal, const Eigen::Matrix3d &R_fk) -> double { -// Eigen::Matrix3d dR = R_goal.transpose() * R_fk; -// double c = clamp((dR.trace() - 1.0) * 0.5, -1.0, 1.0); -// return std::acos(c); -// }; -// -// samples.push_back(IkSample{0.0, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}}); -// cout << "idx, s(0..1), psi(rad), q1..q7, pos_err(m), rot_err(rad), rot_err(deg)\n"; -// -// // 5) 直线采样 & 每点求 IK（带 ψ 更新） -// for (int k = 0; k <= N; ++k) { -// const double s = static_cast(k) / static_cast(N); // [0,1] -// Eigen::Vector3d p = p0 + s * L * dir; -// -// Eigen::Matrix4d T_goal = Eigen::Matrix4d::Identity(); -// T_goal.block<3, 3>(0, 0) = R_fixed; -// T_goal.block<3, 1>(0, 3) = p; -// -// // 计算当前点的 arm-angle 可行区间，并基于上一时刻 psi_curr 更新一次 -// -// -// // 逆解（带 ψ） -// solver.ik(T_goal,q); -// solver.update_joints_state(q); -// -// // 容错：若 IK 失败（大小不为 7），跳过但打印提示 -// if (q.size() != 7) { -// cout << k << ", " << s << ", " << 0.0 -// << ", IK_FAIL, , , , , , , , ,\n"; -// continue; -// } -// -// // 前向校验 -// Eigen::Matrix4d T_fk; -// solver.fk(q,T_fk,true); -// const Eigen::Vector3d p_fk = T_fk.block < 3, -// 1 > (0, 3); -// const Eigen::Matrix3d R_fk = T_fk.block < 3, -// 3 > (0, 0); -// -// const double pos_err = (p_fk - p).norm(); -// const double rot_err = rot_err_rad(R_fixed, R_fk); -// const double rot_err_deg = rot_err * 180.0 / M_PI; -// -// cout << k << ", " << s << ", " << 0.0 << ", " -// << q[0] << ", " << q[1] << ", " << q[2] << ", " -// << q[3] << ", " << q[4] << ", " << q[5] << ", " << q[6] << ", " -// << pos_err << ", " << rot_err << ", " << rot_err_deg << "\n"; -// -// samples.push_back(IkSample{0.0, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}}); -// } -// write_ik_samples_csv("/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv", samples, true, 9); -// } - - -// TEST(SRS_IK_TEST, MOVE_L_SLOVER_TEST) { -// using std::cout; -// using std::endl; -// -// const char *model_path = -// "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.xml"; -// -// DualArmViewer viewer(model_path); -// -// -// // int viewer = 0; -// // 把所有 IK 运算 + moveJ 循环放到控制线程里 -// std::thread ctrl_thread([&viewer]() { -// std::vector samples; -// std::this_thread::sleep_for(std::chrono::seconds(3)); -// samples.reserve(4096); -// -// OptPsiLimitBiasSolver solver; -// -// -// // 1) 当前位姿（估计上一时刻 ψ 用） -// std::vector joint_angles(7, 0); -// joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364}; -// solver.update_joints_state(joint_angles); -// samples.push_back(IkSample{ -// 0.0, { -// joint_angles[0], joint_angles[1], -// joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5], joint_angles[6] -// } -// }); -// -// // 2) 目标位姿（作为直线的起点） -// joint_angles = {0.25, 1.00, M_PI / 2, M_PI / 2, 0, 0, 0}; -// Eigen::Matrix4d target_pose; -// -// solver.fk(joint_angles,target_pose,true); -// -// // 直线插补参数 —— 从 target_pose 出发沿 X 方向 L 米，共 N 段（N+1 个点，包含起点） -// const int N = 400; // 采样点数（间隔均匀） -// const double L = -0.40; // 直线长度 0.20 m -// Eigen::Vector3d dir = Eigen::Vector3d::UnitX(); -// dir.normalize(); -// -// // 固定姿态（也可以改成对姿态做 Slerp） -// const Eigen::Matrix3d R_fixed = target_pose.block < 3, -// 3 > (0, 0); -// const Eigen::Vector3d p0 = target_pose.block < 3, -// 1 > (0, 3); -// -// // 3) 初始 ψ：用估计得到的 ψ，再根据 target_pose 的可行区间做一次更新 -// std::vector q; -// solver.ik(target_pose,q); -// -// // 4) 误差评估工具 -// const auto clamp = [](double x, double lo, double hi) { -// return std::max(lo, std::min(hi, x)); -// }; -// auto rot_err_rad = [&](const Eigen::Matrix3d &R_goal, const Eigen::Matrix3d &R_fk) -> double { -// Eigen::Matrix3d dR = R_goal.transpose() * R_fk; -// double c = clamp((dR.trace() - 1.0) * 0.5, -1.0, 1.0); -// return std::acos(c); -// }; -// -// samples.push_back(IkSample{0.0, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}}); -// cout << "idx, s(0..1), psi(rad), q1..q7, pos_err(m), rot_err(rad), rot_err(deg)\n"; -// -// // 5) 直线采样 & 每点求 IK（带 ψ 更新） -// for (int k = 0; k <= N; ++k) { -// const double s = static_cast(k) / static_cast(N); // [0,1] -// Eigen::Vector3d p = p0 + s * L * dir; -// -// Eigen::Matrix4d T_goal = Eigen::Matrix4d::Identity(); -// T_goal.block<3, 3>(0, 0) = R_fixed; -// T_goal.block<3, 1>(0, 3) = p; -// -// // 计算当前点的 arm-angle 可行区间，并基于上一时刻 psi_curr 更新一次 -// -// -// // 逆解（带 ψ） -// bool ok = solver.ik(T_goal, q); -// if (!ok) { -// cout << k << ", " << s << ", IK_FAIL\n"; -// break; // 直接跳出循环，看看是在哪个 k 失败的 -// } -// -// solver.update_joints_state(q); -// -// viewer.moveJ(q); // 更新目标角 -// std::this_thread::sleep_for(std::chrono::duration(0.1)); -// -// // 前向校验 -// Eigen::Matrix4d T_fk; -// solver.fk(q,T_fk,true); -// const Eigen::Vector3d p_fk = T_fk.block < 3, -// 1 > (0, 3); -// const Eigen::Matrix3d R_fk = T_fk.block < 3, -// 3 > (0, 0); -// -// const double pos_err = (p_fk - p).norm(); -// const double rot_err = rot_err_rad(R_fixed, R_fk); -// const double rot_err_deg = rot_err * 180.0 / M_PI; -// -// cout << k << ", " << s << ", " << 0.0 << ", " -// << q[0] << ", " << q[1] << ", " << q[2] << ", " -// << q[3] << ", " << q[4] << ", " << q[5] << ", " << q[6] << ", " -// << pos_err << ", " << rot_err << ", " << rot_err_deg << "\n"; -// -// samples.push_back(IkSample{0.0, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}}); -// } -// write_ik_samples_csv("/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv", samples, true, 9); -// }); -// -// // ★ MuJoCo / OpenGL 一定在主线程跑 -// viewer.run(); // 阻塞，直到你关掉窗口 -// ctrl_thread.join(); // 控制线程结束 -// -// // 这里不用再 sleep / join sim_thread 了 -// } -// - -TEST(SRS_IK_TEST, MOVE_L_PLANNER_TEST) { - using std::cout; - using std::endl; - - const auto project_root = find_project_root(); - const auto model_path = (project_root / "model/xiaoyan_description/dual_arm.xml").string(); - const auto ik_csv_path = (project_root / "data/ik_psi_sweep.csv").string(); - const auto traj_csv_path = (project_root / "data/planner/traj.csv").string(); - - DualArmViewer viewer(model_path.c_str()); - - // ★ 把 IK + planner + moveJ 放在控制线程 - std::thread ctrl_thread([&viewer, &ik_csv_path, &traj_csv_path]() { - using namespace std::chrono_literals; - - // 等 MuJoCo / OpenGL 初始化好 - std::this_thread::sleep_for(3s); - - LawbaIKSolver solver(config::LawbaIKConfig{}); - - // =============== 1) 设置初始关节状态 =============== - std::vector joint_angles(7, 0.0); - joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364}; - - solver.update_joints_state(joint_angles); - - // =============== 2) 目标位姿 (直线起点) =============== - joint_angles = {0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0, 0}; - Eigen::Matrix4d target_pose; - solver.fk(joint_angles, target_pose, true); - - // 直线插补：从 target_pose 出发沿 X 方向 L 米，共 N 段 - const int N = 100; - const double L = 0.40; - Eigen::Vector3d dir = Eigen::Vector3d::UnitZ(); - dir.normalize(); - - const Eigen::Matrix3d R_fixed = target_pose.block<3, 3>(0, 0); - const Eigen::Vector3d p0 = target_pose.block<3, 1>(0, 3); - - // =============== 3) 准备容器：IK 路点 + CSV 样本 =============== - std::vector> waypoints; - waypoints.reserve(N + 1); - - std::vector ik_samples; - ik_samples.reserve(N + 1); - - // 用一次 IK 作为起点的 q - std::vector q; - bool ok0 = solver.ik(target_pose, q); - if (!ok0 || q.size() != 7) { - std::cerr << "IK at target_pose failed\n"; - return; - } - solver.update_joints_state(q); - - waypoints.push_back(q); - ik_samples.push_back(IkSample{0.0, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}}); - - // 辅助函数：计算旋转误差 - auto clamp = [](double x, double lo, double hi) { - return std::max(lo, std::min(hi, x)); - }; - auto rot_err_rad = [&](const Eigen::Matrix3d &R_goal, - const Eigen::Matrix3d &R_fk) -> double { - Eigen::Matrix3d dR = R_goal.transpose() * R_fk; - double c = clamp((dR.trace() - 1.0) * 0.5, -1.0, 1.0); - return std::acos(c); - }; - - cout << "=== IK path along straight line ===\n"; - cout << "idx, s(0..1), q1..q7, pos_err(m), rot_err(rad), rot_err(deg)\n"; - - // =============== 4) 直线采样 & 求 IK（只收集，不 moveJ） =============== - for (int k = 0; k <= N; ++k) { - const double s = static_cast(k) / static_cast(N); // [0,1] - Eigen::Vector3d p = p0 + s * L * dir; - - Eigen::Matrix4d T_goal = Eigen::Matrix4d::Identity(); - T_goal.block<3, 3>(0, 0) = R_fixed; - T_goal.block<3, 1>(0, 3) = p; - - bool ok = solver.ik(T_goal, q); - if (!ok) { - cout << k << ", " << s << ", IK_FAIL\n"; - break; - } - - solver.update_joints_state(q); - - // 前向校验一下 IK 误差（方便你确认 IK 本身没问题） - Eigen::Matrix4d T_fk; - solver.fk(q, T_fk, true); - const Eigen::Vector3d p_fk = T_fk.block<3, 1>(0, 3); - const Eigen::Matrix3d R_fk = T_fk.block<3, 3>(0, 0); - - const double pos_err = (p_fk - p).norm(); - const double rot_err = rot_err_rad(R_fixed, R_fk); - const double rot_err_deg = rot_err * 180.0 / M_PI; - - cout << k << ", " << s << ", " - << q[0] << ", " << q[1] << ", " << q[2] << ", " - << q[3] << ", " << q[4] << ", " << q[5] << ", " << q[6] << ", " - << pos_err << ", " << rot_err << ", " << rot_err_deg << "\n"; - - waypoints.push_back(q); - ik_samples.push_back(IkSample{0.0, {q[0], q[1], q[2], q[3], q[4], q[5], q[6]}}); - } - - // 原始 IK 轨迹先写一份 CSV，方便对比 - write_ik_samples_csv( - ik_csv_path, - ik_samples, true, 9); - - if (waypoints.size() < 2) { - std::cerr << "Not enough IK waypoints for planner\n"; - return; - } - - // =============== 5) 使用 JointTrajectoryPlanner 对 IK 路点做时间参数化 =============== - auto planner = std::make_shared(); - planner->setPathType(PathType::Natural); - - // 按自己实际的关节约束改 - planner->setSymmetricLimits( - std::vector(7, 1.5), // vmax - std::vector(7, 3.0) // amax - ); - - TrajPtr traj; - if (!planner->plan(waypoints, traj)) { - std::cerr << "planner.plan(waypoints) failed\n"; - return; - } - - // 采样规划后的轨迹（这里用 0.01 s） - auto plan_samples = planner->sampleTrajectory(traj, 0.01); - - if (plan_samples.empty()) { - std::cerr << "planner.sampleTrajectory returned empty\n"; - return; - } - - // 写一份规划后轨迹的 CSV - planner->writeTrajectoryCsv( - traj_csv_path, - plan_samples); - - cout << "=== Start executing planned trajectory ===\n"; - - // =============== 6) 播放规划后的轨迹到 MuJoCo =============== - for (const auto &smp : plan_samples) { - - const std::vector &q_plan = SupportFunctions::eigen_to_vector(smp.q); - - viewer.moveJ(q_plan); - std::this_thread::sleep_for(10ms); - } - - cout << "=== Planned trajectory finished ===\n"; - }); - - // ★ MuJoCo / OpenGL 一定在主线程跑 - viewer.run(); // 阻塞，直到你关掉窗口 - ctrl_thread.join(); // 控制线程结束 -} - - - - -TEST(SRS_IK_TEST, TR_TEST) { - using std::cout; - using std::endl; - - - PinocchioDlsIKSolver solver(makeDlsConfig()); - solver.init(); - Eigen::Matrix4d T; - T << 9.99998311e-01, -1.78940420e-03, 4.20611000e-04, 3.83367005e-02, - 6.06804000e-05, -1.96560031e-01, -9.80491790e-01, -1.08356411e-01, - 1.83717140e-03, 9.80490159e-01, -1.96559590e-01, -7.19901808e-01, - 0.0, 0.0, 0.0, 1.0; - - std::vector joint_angles = {0.00203898, 1.34062, 0.0, 0.522261, 0.0, -0.000210733, -0.0942364}; - std::vector last_joint_angles = {0.00203898, 1.34062, 0.111, 0.622261, 0.256, -0.0210733, 1.00}; - - - - // 目标位姿：FK(joint_angles) - Eigen::Matrix4d pose; - solver.fk(joint_angles,pose,true); - cout << "Target Pose (FK from seed joints):\n" << pose << endl; - - - - solver.update_joints_state(last_joint_angles); - solver.ik(pose,joint_angles); - - for (double joint_angle: joint_angles) { - cout << joint_angle << endl; - } - - solver.fk(joint_angles,pose,true); - cout << "Target Pose (FK from seed joints):\n" << pose << endl; - - -} - - -#include -#include - -// 计算 TCP 位置/速度/加速度（用中心差分，端点用前向/后向差分） -static bool build_tcp_pva(cmvr::PinocchioDlsIKSolver& solver, - const std::vector& t, - const std::vector>& q, - std::vector& p, - std::vector& v, - std::vector& a, - bool is_tcp=true) -{ - const size_t N = std::min(t.size(), q.size()); - if (N < 3) return false; - - p.assign(N, Eigen::Vector3d::Zero()); - v.assign(N, Eigen::Vector3d::Zero()); - a.assign(N, Eigen::Vector3d::Zero()); - - // p(t) - for (size_t i = 0; i < N; ++i) { - Eigen::Matrix4d T; - if (!solver.fk(q[i], T, is_tcp)) return false; - p[i] = T.block<3,1>(0,3); - } - - auto safe_dt = [&](double dt) { return (dt > 1e-9) ? dt : 1e-9; }; - - // v(t) : 中心差分 - { - double dt0 = safe_dt(t[1] - t[0]); - v[0] = (p[1] - p[0]) / dt0; - - for (size_t i = 1; i + 1 < N; ++i) { - double dt = safe_dt(t[i+1] - t[i-1]); - v[i] = (p[i+1] - p[i-1]) / dt; - } - - double dtn = safe_dt(t[N-1] - t[N-2]); - v[N-1] = (p[N-1] - p[N-2]) / dtn; - } - - // a(t) : 对 v 再中心差分 - { - double dt0 = safe_dt(t[1] - t[0]); - a[0] = (v[1] - v[0]) / dt0; - - for (size_t i = 1; i + 1 < N; ++i) { - double dt = safe_dt(t[i+1] - t[i-1]); - a[i] = (v[i+1] - v[i-1]) / dt; - } - - double dtn = safe_dt(t[N-1] - t[N-2]); - a[N-1] = (v[N-1] - v[N-2]) / dtn; - } - - return true; -} - -static void write_tcp_pva_csv(const std::string& path, - const std::vector& t, - const std::vector& p, - const std::vector& v, - const std::vector& a) -{ - const size_t N = std::min({t.size(), p.size(), v.size(), a.size()}); - std::ofstream os(path); - os << "t,px,py,pz,vx,vy,vz,ax,ay,az,speed,acc\n"; - for (size_t i = 0; i < N; ++i) { - os << std::setprecision(15) << t[i] << "," - << p[i].x() << "," << p[i].y() << "," << p[i].z() << "," - << v[i].x() << "," << v[i].y() << "," << v[i].z() << "," - << a[i].x() << "," << a[i].y() << "," << a[i].z() << "," - << v[i].norm() << "," << a[i].norm() - << "\n"; - } -} -static void write_qtraj_csv(const std::string& path, - const std::vector& t, - const std::vector>& q) { - std::ofstream os(path); - os << "t,q1,q2,q3,q4,q5,q6,q7\n"; - for (size_t i = 0; i < t.size() && i < q.size(); ++i) { - os << std::setprecision(15) << t[i]; - for (int k = 0; k < 7; ++k) os << "," << q[i][k]; - os << "\n"; - } -} - -static void write_twist_trace_csv( - const std::string& path, - const std::vector& t, - const std::vector>& twist) -{ - using Twist = Eigen::Matrix; - const size_t N = std::min(t.size(), twist.size()); - std::vector acceleration(N, Twist::Zero()); - std::vector jerk(N, Twist::Zero()); - - auto safe_dt = [](double dt) { - return (dt > 1e-9) ? dt : 1e-9; - }; - - if (N >= 2) { - const double dt0 = safe_dt(t[1] - t[0]); - acceleration[0] = (twist[1] - twist[0]) / dt0; - - for (size_t i = 1; i + 1 < N; ++i) { - const double dt_c = safe_dt(t[i + 1] - t[i - 1]); - acceleration[i] = (twist[i + 1] - twist[i - 1]) / dt_c; - } - - const double dtn = safe_dt(t[N - 1] - t[N - 2]); - acceleration[N - 1] = (twist[N - 1] - twist[N - 2]) / dtn; - } - - if (N >= 2) { - const double dt0 = safe_dt(t[1] - t[0]); - jerk[0] = (acceleration[1] - acceleration[0]) / dt0; - - for (size_t i = 1; i + 1 < N; ++i) { - const double dt_c = safe_dt(t[i + 1] - t[i - 1]); - jerk[i] = (acceleration[i + 1] - acceleration[i - 1]) / dt_c; - } - - const double dtn = safe_dt(t[N - 1] - t[N - 2]); - jerk[N - 1] = (acceleration[N - 1] - acceleration[N - 2]) / dtn; - } - - std::ofstream os(path); - os << "t," - "vx,vy,vz,wx,wy,wz," - "ax,ay,az,alphax,alphay,alphaz," - "jx,jy,jz,jalphax,jalphay,jalphaz\n"; - for (size_t i = 0; i < N; ++i) { - os << std::setprecision(15) << t[i]; - for (int k = 0; k < 6; ++k) os << "," << twist[i][k]; - for (int k = 0; k < 6; ++k) os << "," << acceleration[i][k]; - for (int k = 0; k < 6; ++k) os << "," << jerk[i][k]; - os << "\n"; - } -} - -static void print_tcp_speed_stats(cmvr::PinocchioDlsIKSolver& solver, - const std::vector& t, - const std::vector>& q, - bool is_tcp=true) { - if (t.size() < 3 || q.size() < 3) return; - - bool has_prev = false; - Eigen::Vector3d p_prev = Eigen::Vector3d::Zero(); - double t_prev = 0.0; - - double vmin = 1e100, vmax = 0.0, vsum = 0.0; - size_t cnt = 0; - - for (size_t i = 0; i < q.size(); ++i) { - Eigen::Matrix4d T; - if (!solver.fk(q[i], T, is_tcp)) continue; - - Eigen::Vector3d p = T.block<3,1>(0,3); - - if (has_prev) { - double dt = t[i] - t_prev; - if (dt > 1e-9) { - double v = (p - p_prev).norm() / dt; - vmin = std::min(vmin, v); - vmax = std::max(vmax, v); - vsum += v; - cnt++; - - // 想看过程就降低频率打印 - if ((cnt % 1) == 0) { - std::cerr << "[tcp] i=" << i << " v=" << v << " m/s\n"; - } - } - } - - p_prev = p; - t_prev = t[i]; - has_prev = true; - } - - if (cnt) { - std::cerr << "TCP speed stats (from t_traj): " - << "min=" << vmin - << " avg=" << (vsum / (double)cnt) - << " max=" << vmax - << " [m/s]\n"; - } -} - -TEST(SRS_IK_TEST, MOVEL_S_CURVE_LOCAL_RUN_MUJOCO) { - // 让输出不被缓冲（gtest/ctest 环境下更容易看到） - std::cerr.setf(std::ios::unitbuf); - - const auto project_root = find_project_root(); - const auto model_path = (project_root / "model/xiaoyan_description/dual_arm.xml").string(); - const auto csv_path = (project_root / "data/ik_movel_scurve.csv").string(); - const auto tcp_csv_path = (project_root / "data/ik_movel_scurve_tcp.csv").string(); - - - DualArmViewer viewer(model_path.c_str()); - - std::thread ctrl_thread([&viewer, &csv_path,&tcp_csv_path]() { - using namespace std::chrono_literals; - std::this_thread::sleep_for(3s); - - auto solver = std::make_shared(makeDlsConfig()); - if (!solver->init()) return; - - // 起点关节 - // std::vector q_start = {-0.424743, 0.759386, 1.80129, 2.03728, -1.34668, 0.0560845, -0.26}; - std::vector q_start = {0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0, 0}; - // std::vector q_start = {0, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0, 0}; - // viewer.moveJ(q_start); - - - // 起点位姿（base下） - Eigen::Matrix4d T0; - if (!solver->fk(q_start, T0, true)) return; - - std::cout << T0 << std::endl; - - // 目标位姿：base X 方向走 0.25m，姿态保持起点 - Eigen::Matrix4d Tg = T0; - // Tg(0,3) += 0.2; - Tg(1,3) -= 0.2; - Eigen::Vector3d dp_check = Tg.block<3,1>(0,3) - T0.block<3,1>(0,3); - std::cerr << "dp(base)=" << dp_check.transpose() << "\n"; - - // 轨迹生成参数（生成 dt 不必极小，1ms~2ms 足够；真正平滑靠 S 曲线 + MuJoCo 伺服滤波） - const double dt_gen = 0.002; - const double v_tcp = 0.2; - const double a_tcp = 5.0; - const double j_tcp = 100.00; - std::vector qd_max(7, 3.0); - - cmvr::device::PinocchioDlsCartesianMotionPlanner planner(solver); - cmvr::config::MoveLPlannerConfig move_l_config; - move_l_config.set_sample_period_s(dt_gen); - move_l_config.set_position_gain(4.0); - move_l_config.set_rotation_gain(4.0); - if (!planner.configureMoveL(move_l_config)) { - std::cerr << "configureMoveL failed\n"; - return; - } - cmvr::device::CartesianJointTrajectory trajectory; - if (!planner.planMoveL(cmvr::common::math::matrixToPose(Tg), - q_start, - qd_max, - v_tcp, - a_tcp, - j_tcp, - cmvr::device::FrameType::Base, - trajectory)) { - std::cerr << "planMoveL failed\n"; - return; - } - const auto& q_traj = trajectory.position; - const auto& t_traj = trajectory.time; - - std::cerr << "traj gen samples=" << q_traj.size() - << " T=" << (t_traj.empty()?0.0:t_traj.back()) << "s\n"; - - write_qtraj_csv(csv_path, t_traj, q_traj); - - std::vector p, v, a; - if (build_tcp_pva(*solver, t_traj, q_traj, p, v, a, true)) { - write_tcp_pva_csv(tcp_csv_path, t_traj, p, v, a); - std::cerr << "saved tcp csv: " << tcp_csv_path << "\n"; - } else { - std::cerr << "build_tcp_pva failed\n"; - } - std::cerr << "saved csv: " << csv_path << "\n"; - - // 正确的 TCP 速度统计（用 t_traj） - print_tcp_speed_stats(*solver, t_traj, q_traj, true); - - // 直接按采样时刻逐点下发位置目标，不在 viewer 内做轨迹缓存和插值。 - const auto t0 = std::chrono::steady_clock::now(); - for (size_t i = 0; i < q_traj.size(); ++i) { - std::this_thread::sleep_until( - t0 + std::chrono::duration_cast( - std::chrono::duration(t_traj[i]))); - viewer.moveJ(q_traj[i]); - } - std::cerr << "trajectory finished in MuJoCo.\n"; - }); - - viewer.run(); - ctrl_thread.join(); -} - - -TEST(SRS_IK_TEST, SPEEDJ_RUN_MUJOCO) { - std::cerr.setf(std::ios::unitbuf); - - const auto project_root = find_project_root(); - const auto model_path = (project_root / "model/xiaoyan_description/dual_arm.xml").string(); - - DualArmViewer viewer(model_path.c_str()); - bool ctrl_ok = true; - std::string failure_msg; - double joint_delta = 0.0; - double settle_drift = 0.0; - - std::thread ctrl_thread([&]() { - using namespace std::chrono_literals; - - auto fail = [&](const std::string& msg) { - ctrl_ok = false; - failure_msg = msg; - std::cerr << msg << "\n"; - }; - - std::this_thread::sleep_for(3s); - - std::vector q_start = {0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0, 0}; - viewer.moveJ(q_start); - std::this_thread::sleep_for(2s); - - const auto q0 = viewer.getQ(); - if (q0.size() != 7) { - fail("invalid q0 size"); - return; - } - - std::vector qd_cmd(7, 0.0); - qd_cmd[0] = 0.2; // 只测 0 号关节 - - viewer.speedJ(qd_cmd); - std::this_thread::sleep_for(2s); - - const auto q1 = viewer.getQ(); - if (q1.size() != 7) { - fail("invalid q1 size"); - return; - } - - viewer.speedJ(std::vector(7, 0.0)); - std::this_thread::sleep_for(1s); - - const auto q2 = viewer.getQ(); - if (q2.size() != 7) { - fail("invalid q2 size"); - return; - } - - viewer.moveJ(q2); // 切回位置保持 - - joint_delta = q1[0] - q0[0]; - settle_drift = std::abs(q2[0] - q1[0]); - - std::cerr << "q0[0]=" << q0[0] - << " q1[0]=" << q1[0] - << " q2[0]=" << q2[0] - << " joint_delta=" << joint_delta - << " settle_drift=" << settle_drift << "\n"; - }); - - viewer.run(); - ctrl_thread.join(); - - ASSERT_TRUE(ctrl_ok) << failure_msg; - EXPECT_GT(joint_delta, 0.1); - EXPECT_LT(settle_drift, 0.05); -} - -TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) { - std::cerr.setf(std::ios::unitbuf); - - const auto project_root = find_project_root(); - const auto model_path = (project_root / "model/xiaoyan_description/dual_arm.xml").string(); - const auto urdf_path = (project_root / "model/xiaoyan_description/dual_arm.urdf").string(); - - DualArmViewer viewer(model_path.c_str()); - bool ctrl_ok = true; - std::string failure_msg; - bool has_exec_samples = false; - double max_exec_linear_yz = 0.0; - double min_exec_vx = std::numeric_limits::infinity(); - double max_exec_vx = -std::numeric_limits::infinity(); - double min_abs_vx_before_reverse = std::numeric_limits::infinity(); - bool saw_negative_after_reverse = false; - double final_exec_linear_speed = std::numeric_limits::infinity(); - std::vector tcp_time; - std::vector tcp_position; - std::vector tcp_velocity; - std::vector tcp_angular_velocity; - std::vector cmd_tcp_position; - std::vector cmd_tcp_velocity; - std::vector cmd_tcp_angular_velocity; - bool tcp_trace_ok = false; - - std::thread ctrl_thread([&]() { - using namespace std::chrono_literals; - std::this_thread::sleep_for(3s); - - auto fail = [&](const std::string& msg) { - ctrl_ok = false; - failure_msg = msg; - std::cerr << msg << "\n"; - }; - - auto dls_cfg = makeDlsConfig(); - dls_cfg.set_urdf_path(urdf_path); - auto solver = std::make_shared(dls_cfg); - if (!solver->init()) { - fail("speedL test: solver.init() failed"); - return; - } - - const double dt = 0.002; - const double segment_time = 1.5; - const double stop_time = 1.0; - const double settle_time = 1.0; - const double total_time = 2.0 * segment_time + stop_time; - const double linear_speed_cmd = 0.15; - const double max_q_ref_tracking_error = 0.15; - - std::vector q_start = {0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0, 0}; - viewer.moveJ(q_start); - std::this_thread::sleep_for(1s); - - const std::vector q_init = viewer.getQ(); - solver->update_joints_state(q_init); - std::vector q_ref = q_init; - - cmvr::config::SpeedLPlannerConfig speedl_config; - speedl_config.set_linear_velocity_max(0.55); - speedl_config.set_linear_acceleration_max(0.80); - speedl_config.set_linear_jerk_max(3.30); - speedl_config.set_angular_velocity_max(1.00); - speedl_config.set_angular_acceleration_max(3.00); - speedl_config.set_angular_jerk_max(12.0); - for (int i = 0; i < 7; ++i) { - speedl_config.add_joint_acceleration_max(8.0); - } - speedl_config.set_linear_target_replan_threshold(1e-4); - speedl_config.set_angular_target_replan_threshold(1e-4); - speedl_config.set_linear_reverse_cos_threshold(-0.8660254037844386); - speedl_config.set_linear_reverse_switch_speed_threshold(1e-3); - cmvr::device::PinocchioDlsCartesianMotionPlanner planner(solver); - if (!planner.configureSpeedL(speedl_config, q_init.size())) { - fail("speedL test: configureSpeedL() failed"); - return; - } - - std::vector> q_traj; - std::vector t_traj; - std::vector> twist_cmd_traj; - std::vector tcp_pos_traj; - std::vector tcp_vel_traj; - std::vector tcp_omega_traj; - - const size_t reserve_count = - static_cast(std::ceil((total_time + settle_time) / dt)) + 8; - q_traj.reserve(reserve_count); - t_traj.reserve(reserve_count); - twist_cmd_traj.reserve(reserve_count); - tcp_pos_traj.reserve(reserve_count); - tcp_vel_traj.reserve(reserve_count); - tcp_omega_traj.reserve(reserve_count); - - auto record_sample = [&](double t_sample, - const std::vector& q_sample, - const std::vector& qd_sample) -> bool { - q_traj.push_back(q_sample); - t_traj.push_back(t_sample); - twist_cmd_traj.push_back( - cmvr::common::math::velocityToVector(planner.getSpeedLCommandTwistBase())); - Eigen::Matrix twist_exec = Eigen::Matrix::Zero(); - if (!solver->computeTwistBaseAtQ(q_sample, qd_sample, true, twist_exec)) { - fail("speedL test: failed to compute executed TCP twist"); - return false; - } - - Eigen::Vector3d tcp_pos = Eigen::Vector3d::Zero(); - Eigen::Vector3d tcp_vel = Eigen::Vector3d::Zero(); - Eigen::Vector3d tcp_omega = Eigen::Vector3d::Zero(); - if (!viewer.getTcpTwistState(tcp_pos, tcp_vel, tcp_omega)) { - fail("speedL test: failed to read TCP state from MuJoCo"); - return false; - } - tcp_pos_traj.push_back(tcp_pos); - tcp_vel_traj.push_back(tcp_vel); - tcp_omega_traj.push_back(tcp_omega); - - has_exec_samples = true; - max_exec_linear_yz = std::max(max_exec_linear_yz, - std::hypot(twist_exec[1], twist_exec[2])); - min_exec_vx = std::min(min_exec_vx, twist_exec[0]); - max_exec_vx = std::max(max_exec_vx, twist_exec[0]); - final_exec_linear_speed = twist_exec.head<3>().norm(); - if (t_sample >= segment_time && !saw_negative_after_reverse) { - min_abs_vx_before_reverse = - std::min(min_abs_vx_before_reverse, std::abs(twist_exec[0])); - if (twist_exec[0] < -1e-4) { - saw_negative_after_reverse = true; - } - } - return true; - }; - - const auto t0 = std::chrono::steady_clock::now(); - const size_t active_steps = static_cast(std::ceil(total_time / dt)); - for (size_t i = 0; i < active_steps; ++i) { - const double t = static_cast(i) * dt; - std::this_thread::sleep_until( - t0 + std::chrono::duration_cast( - std::chrono::duration(t))); - - std::vector q_meas; - std::vector qd_meas; - viewer.getJointState(q_meas, qd_meas); - Eigen::Matrix target_twist = - Eigen::Matrix::Zero(); - if (t < segment_time) { - target_twist[1] = linear_speed_cmd; - target_twist[2] = 0; - } else if (t < 2.0 * segment_time) { - target_twist[1] = -linear_speed_cmd; - target_twist[2] = 0; - } else { - target_twist.setZero(); - } - - std::vector qd_cmd; - if (!planner.speedLStep(cmvr::common::math::vectorToVelocity(target_twist), - dt, - q_meas, - qd_meas, - qd_cmd, - cmvr::device::FrameType::Base)) { - viewer.moveJ(q_meas); - fail("speedL test: speedLStep() failed"); - return; - } - - if (q_ref.size() != q_meas.size()) { - q_ref = q_meas; - } - for (size_t j = 0; j < q_ref.size() && j < qd_cmd.size(); ++j) { - q_ref[j] += qd_cmd[j] * dt; - q_ref[j] = std::max(q_meas[j] - max_q_ref_tracking_error, - std::min(q_ref[j], q_meas[j] + max_q_ref_tracking_error)); - } - viewer.moveJ(q_ref); - if (!record_sample(t, q_meas, qd_meas)) { - return; - } - } - - const size_t settle_steps = static_cast(std::ceil(settle_time / dt)); - for (size_t i = 0; i < settle_steps; ++i) { - const double t = total_time + static_cast(i) * dt; - std::this_thread::sleep_until( - t0 + std::chrono::duration_cast( - std::chrono::duration(t))); - - std::vector q_meas; - std::vector qd_meas; - viewer.getJointState(q_meas, qd_meas); - const Eigen::Matrix target_twist = - Eigen::Matrix::Zero(); - - std::vector qd_cmd; - if (!planner.speedLStep(cmvr::common::math::vectorToVelocity(target_twist), - dt, - q_meas, - qd_meas, - qd_cmd, - cmvr::device::FrameType::Base)) { - viewer.moveJ(q_meas); - fail("speedL test: speedLStep() failed during stop phase"); - return; - } - - if (q_ref.size() != q_meas.size()) { - q_ref = q_meas; - } - for (size_t j = 0; j < q_ref.size() && j < qd_cmd.size(); ++j) { - q_ref[j] += qd_cmd[j] * dt; - q_ref[j] = std::max(q_meas[j] - max_q_ref_tracking_error, - std::min(q_ref[j], q_meas[j] + max_q_ref_tracking_error)); - } - viewer.moveJ(q_ref); - if (!record_sample(t, q_meas, qd_meas)) { - return; - } - } - - viewer.moveJ(viewer.getQ()); - - std::cerr << "speedL samples=" << q_traj.size() - << " T=" << (t_traj.empty() ? 0.0 : t_traj.back()) << "s\n"; - - if (!tcp_pos_traj.empty() && - tcp_pos_traj.size() == t_traj.size() && - tcp_vel_traj.size() == t_traj.size() && - tcp_omega_traj.size() == t_traj.size()) { - tcp_time = t_traj; - tcp_position = std::move(tcp_pos_traj); - tcp_velocity = std::move(tcp_vel_traj); - tcp_angular_velocity = std::move(tcp_omega_traj); - cmd_tcp_position.assign(tcp_time.size(), Eigen::Vector3d::Zero()); - cmd_tcp_velocity.assign(tcp_time.size(), Eigen::Vector3d::Zero()); - cmd_tcp_angular_velocity.assign(tcp_time.size(), Eigen::Vector3d::Zero()); - if (!tcp_time.empty() && twist_cmd_traj.size() == tcp_time.size()) { - cmd_tcp_position[0] = tcp_position[0]; - cmd_tcp_velocity[0] = twist_cmd_traj[0].head<3>(); - cmd_tcp_angular_velocity[0] = twist_cmd_traj[0].tail<3>(); - for (size_t i = 1; i < tcp_time.size(); ++i) { - const double dt_sample = std::max(1e-9, tcp_time[i] - tcp_time[i - 1]); - cmd_tcp_velocity[i] = twist_cmd_traj[i].head<3>(); - cmd_tcp_angular_velocity[i] = twist_cmd_traj[i].tail<3>(); - cmd_tcp_position[i] = cmd_tcp_position[i - 1] + - 0.5 * (cmd_tcp_velocity[i - 1] + - cmd_tcp_velocity[i]) * dt_sample; - } - } - tcp_trace_ok = true; - } else { - std::cerr << "failed to collect TCP state from MuJoCo\n"; - } - - std::cerr << "speedL exec stats: max|vyz|=" << max_exec_linear_yz - << " max_vx=" << max_exec_vx - << " min_vx=" << min_exec_vx << "\n"; - - std::cerr << "speedL finished in MuJoCo.\n"; - }); - - viewer.run(); - ctrl_thread.join(); - if (tcp_trace_ok && - !tcp_time.empty() && - tcp_position.size() == tcp_time.size() && - tcp_velocity.size() == tcp_time.size() && - tcp_angular_velocity.size() == tcp_time.size() && - cmd_tcp_angular_velocity.size() == tcp_time.size()) { - std::vector tcp_x; - std::vector tcp_y; - std::vector tcp_z; - std::vector tcp_vx; - std::vector tcp_vy; - std::vector tcp_vz; - std::vector tcp_wx; - std::vector tcp_wy; - std::vector tcp_wz; - std::vector tcp_speed; - std::vector cmd_x; - std::vector cmd_y; - std::vector cmd_z; - std::vector cmd_vx; - std::vector cmd_vy; - std::vector cmd_vz; - std::vector cmd_wx; - std::vector cmd_wy; - std::vector cmd_wz; - std::vector cmd_speed; - tcp_x.reserve(tcp_time.size()); - tcp_y.reserve(tcp_time.size()); - tcp_z.reserve(tcp_time.size()); - tcp_vx.reserve(tcp_time.size()); - tcp_vy.reserve(tcp_time.size()); - tcp_vz.reserve(tcp_time.size()); - tcp_wx.reserve(tcp_time.size()); - tcp_wy.reserve(tcp_time.size()); - tcp_wz.reserve(tcp_time.size()); - tcp_speed.reserve(tcp_time.size()); - cmd_x.reserve(tcp_time.size()); - cmd_y.reserve(tcp_time.size()); - cmd_z.reserve(tcp_time.size()); - cmd_vx.reserve(tcp_time.size()); - cmd_vy.reserve(tcp_time.size()); - cmd_vz.reserve(tcp_time.size()); - cmd_wx.reserve(tcp_time.size()); - cmd_wy.reserve(tcp_time.size()); - cmd_wz.reserve(tcp_time.size()); - cmd_speed.reserve(tcp_time.size()); - for (size_t i = 0; i < tcp_time.size(); ++i) { - tcp_x.push_back(tcp_position[i].x()); - tcp_y.push_back(tcp_position[i].y()); - tcp_z.push_back(tcp_position[i].z()); - tcp_vx.push_back(tcp_velocity[i].x()); - tcp_vy.push_back(tcp_velocity[i].y()); - tcp_vz.push_back(tcp_velocity[i].z()); - tcp_wx.push_back(tcp_angular_velocity[i].x()); - tcp_wy.push_back(tcp_angular_velocity[i].y()); - tcp_wz.push_back(tcp_angular_velocity[i].z()); - tcp_speed.push_back(tcp_velocity[i].norm()); - cmd_x.push_back(cmd_tcp_position[i].x()); - cmd_y.push_back(cmd_tcp_position[i].y()); - cmd_z.push_back(cmd_tcp_position[i].z()); - cmd_vx.push_back(cmd_tcp_velocity[i].x()); - cmd_vy.push_back(cmd_tcp_velocity[i].y()); - cmd_vz.push_back(cmd_tcp_velocity[i].z()); - cmd_wx.push_back(cmd_tcp_angular_velocity[i].x()); - cmd_wy.push_back(cmd_tcp_angular_velocity[i].y()); - cmd_wz.push_back(cmd_tcp_angular_velocity[i].z()); - cmd_speed.push_back(cmd_tcp_velocity[i].norm()); - } - - using namespace matplot; - auto fig = figure(true); - fig->size(1600, 1450); - fig->font_size(16); - - auto ax_pos = subplot(fig, std::array{0.08f, 0.75f, 0.88f, 0.17f}); - hold(ax_pos, on); - auto line_x = plot(ax_pos, tcp_time, tcp_x); - line_x->line_width(2.0); - line_x->display_name("tcp x"); - auto line_y = plot(ax_pos, tcp_time, tcp_y); - line_y->line_width(2.0); - line_y->display_name("tcp y"); - auto line_z = plot(ax_pos, tcp_time, tcp_z); - line_z->line_width(2.0); - line_z->display_name("tcp z"); - auto cmd_line_x = plot(ax_pos, tcp_time, cmd_x); - cmd_line_x->line_width(2.0); - cmd_line_x->line_style("--"); - cmd_line_x->display_name("cmd x"); - auto cmd_line_y = plot(ax_pos, tcp_time, cmd_y); - cmd_line_y->line_width(2.0); - cmd_line_y->line_style("--"); - cmd_line_y->display_name("cmd y"); - auto cmd_line_z = plot(ax_pos, tcp_time, cmd_z); - cmd_line_z->line_width(2.0); - cmd_line_z->line_style("--"); - cmd_line_z->display_name("cmd z"); - title(ax_pos, "TCP Position"); - xlabel(ax_pos, "time [s]"); - ylabel(ax_pos, "position [m]"); - legend(ax_pos, std::vector{"tcp x", "tcp y", "tcp z", "cmd x", "cmd y", "cmd z"}); - grid(ax_pos, on); - - auto ax_vel = subplot(fig, std::array{0.08f, 0.52f, 0.88f, 0.17f}); - hold(ax_vel, on); - auto line_vx = plot(ax_vel, tcp_time, tcp_vx); - line_vx->line_width(2.0); - line_vx->display_name("tcp vx"); - auto line_vy = plot(ax_vel, tcp_time, tcp_vy); - line_vy->line_width(2.0); - line_vy->display_name("tcp vy"); - auto line_vz = plot(ax_vel, tcp_time, tcp_vz); - line_vz->line_width(2.0); - line_vz->display_name("tcp vz"); - auto cmd_line_vx = plot(ax_vel, tcp_time, cmd_vx); - cmd_line_vx->line_width(2.0); - cmd_line_vx->line_style("--"); - cmd_line_vx->display_name("cmd vx"); - auto cmd_line_vy = plot(ax_vel, tcp_time, cmd_vy); - cmd_line_vy->line_width(2.0); - cmd_line_vy->line_style("--"); - cmd_line_vy->display_name("cmd vy"); - auto cmd_line_vz = plot(ax_vel, tcp_time, cmd_vz); - cmd_line_vz->line_width(2.0); - cmd_line_vz->line_style("--"); - cmd_line_vz->display_name("cmd vz"); - title(ax_vel, "TCP Velocity"); - xlabel(ax_vel, "time [s]"); - ylabel(ax_vel, "velocity [m/s]"); - legend(ax_vel, std::vector{"tcp vx", "tcp vy", "tcp vz", "cmd vx", "cmd vy", "cmd vz"}); - grid(ax_vel, on); - - auto ax_ang = subplot(fig, std::array{0.08f, 0.29f, 0.88f, 0.17f}); - hold(ax_ang, on); - auto line_wx = plot(ax_ang, tcp_time, tcp_wx); - line_wx->line_width(2.0); - line_wx->display_name("tcp wx"); - auto line_wy = plot(ax_ang, tcp_time, tcp_wy); - line_wy->line_width(2.0); - line_wy->display_name("tcp wy"); - auto line_wz = plot(ax_ang, tcp_time, tcp_wz); - line_wz->line_width(2.0); - line_wz->display_name("tcp wz"); - auto cmd_line_wx = plot(ax_ang, tcp_time, cmd_wx); - cmd_line_wx->line_width(2.0); - cmd_line_wx->line_style("--"); - cmd_line_wx->display_name("cmd wx"); - auto cmd_line_wy = plot(ax_ang, tcp_time, cmd_wy); - cmd_line_wy->line_width(2.0); - cmd_line_wy->line_style("--"); - cmd_line_wy->display_name("cmd wy"); - auto cmd_line_wz = plot(ax_ang, tcp_time, cmd_wz); - cmd_line_wz->line_width(2.0); - cmd_line_wz->line_style("--"); - cmd_line_wz->display_name("cmd wz"); - title(ax_ang, "TCP Angular Velocity"); - xlabel(ax_ang, "time [s]"); - ylabel(ax_ang, "omega [rad/s]"); - legend(ax_ang, std::vector{"tcp wx", "tcp wy", "tcp wz", "cmd wx", "cmd wy", "cmd wz"}); - grid(ax_ang, on); - - auto ax_speed = subplot(fig, std::array{0.08f, 0.06f, 0.88f, 0.17f}); - hold(ax_speed, on); - auto line_speed = plot(ax_speed, tcp_time, tcp_speed); - line_speed->line_width(2.0); - line_speed->display_name("speed norm"); - auto cmd_line_speed = plot(ax_speed, tcp_time, cmd_speed); - cmd_line_speed->line_width(2.0); - cmd_line_speed->line_style("--"); - cmd_line_speed->display_name("cmd speed norm"); - title(ax_speed, "TCP Speed Norm"); - xlabel(ax_speed, "time [s]"); - ylabel(ax_speed, "speed [m/s]"); - legend(ax_speed, std::vector{"speed norm", "cmd speed norm"}); - grid(ax_speed, on); - - show(fig); - } - - ASSERT_TRUE(ctrl_ok) << failure_msg; - ASSERT_TRUE(has_exec_samples) << "speedL test did not record any executed twist sample"; - ASSERT_TRUE(tcp_trace_ok) << "speedL test failed to build TCP trace"; - EXPECT_GT(max_exec_vx, 0.03) << "speedL never accelerated to a meaningful +X command"; - EXPECT_LT(min_exec_vx, -0.03) << "speedL never switched to a meaningful -X command"; - EXPECT_LT(max_exec_linear_yz, 1e-3) << "speedL introduced unintended lateral linear twist"; - ASSERT_TRUE(std::isfinite(min_abs_vx_before_reverse)) - << "speedL never reached the reverse phase"; - EXPECT_LT(min_abs_vx_before_reverse, 0.01) - << "speedL changed sign before decelerating close enough to zero"; - EXPECT_LT(final_exec_linear_speed, 0.02) - << "speedL did not settle close enough to zero during stop phase"; -} diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt b/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt index 8136c07e..eb26409f 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt +++ b/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt @@ -1,6 +1,5 @@ add_library(arm_motion SHARED - cartesian_motion/pinocchio_dls/src/pinocchio_dls_cartesian_motion_planner.cpp - cartesian_motion/pinocchio_qp/src/pinocchio_qp_cartesian_motion_planner.cpp + cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp joint_motion/toppra/src/toppra_joint_motion_planner.cpp ) diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h index 29153eea..42a553c3 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h @@ -1,6 +1,7 @@ #ifndef CMVR_ES_CARTESIAN_MOTION_PLANNER_H #define CMVR_ES_CARTESIAN_MOTION_PLANNER_H +#include #include #include "cmvr/config/arm_config/arm_config.pb.h" @@ -12,6 +13,10 @@ struct CartesianJointTrajectory { std::vector> position; std::vector> velocity; std::vector time; + double planned_path_length{0.0}; + double executable_path_length{0.0}; + bool truncated{false}; + std::string truncation_reason; }; class CartesianMotionPlanner { diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner_factory.h b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner_factory.h index 0c4eb769..ec527c68 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner_factory.h +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner_factory.h @@ -3,10 +3,8 @@ #include -#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" #include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" -#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h" -#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h" +#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h" #include "algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h" #include "cmvr/config/arm_config/arm_config.pb.h" @@ -23,21 +21,12 @@ public: return nullptr; } switch (move_l.algorithm_case()) { - case config::MoveLConfig::kPinocchioQpCartesianMotionPlanner: + case config::MoveLConfig::kPinocchioCartesianMotionPlanner: if (speed_l.algorithm_case() != - config::SpeedLConfig::kPinocchioQpCartesianMotionPlanner) { + config::SpeedLConfig::kPinocchioCartesianMotionPlanner) { return nullptr; } - return std::make_shared(solver); - case config::MoveLConfig::kPinocchioDlsCartesianMotionPlanner: - if (speed_l.algorithm_case() != - config::SpeedLConfig::kPinocchioDlsCartesianMotionPlanner) { - return nullptr; - } - if (auto dls_solver = std::dynamic_pointer_cast(solver)) { - return std::make_shared(dls_solver); - } - return nullptr; + return std::make_shared(solver); case config::MoveLConfig::ALGORITHM_NOT_SET: default: return nullptr; @@ -48,10 +37,8 @@ public: const config::SpeedLConfig& cfg) { switch (cfg.algorithm_case()) { - case config::SpeedLConfig::kPinocchioQpCartesianMotionPlanner: - return &cfg.pinocchio_qp_cartesian_motion_planner(); - case config::SpeedLConfig::kPinocchioDlsCartesianMotionPlanner: - return &cfg.pinocchio_dls_cartesian_motion_planner(); + case config::SpeedLConfig::kPinocchioCartesianMotionPlanner: + return &cfg.pinocchio_cartesian_motion_planner(); case config::SpeedLConfig::ALGORITHM_NOT_SET: default: return nullptr; @@ -62,10 +49,8 @@ public: const config::MoveLConfig& cfg) { switch (cfg.algorithm_case()) { - case config::MoveLConfig::kPinocchioQpCartesianMotionPlanner: - return &cfg.pinocchio_qp_cartesian_motion_planner(); - case config::MoveLConfig::kPinocchioDlsCartesianMotionPlanner: - return &cfg.pinocchio_dls_cartesian_motion_planner(); + case config::MoveLConfig::kPinocchioCartesianMotionPlanner: + return &cfg.pinocchio_cartesian_motion_planner(); case config::MoveLConfig::ALGORITHM_NOT_SET: default: return nullptr; diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h new file mode 100644 index 00000000..9d6f070e --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h @@ -0,0 +1,107 @@ +#ifndef CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H +#define CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H + +#include + +#include +#include + +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" +#include "../../cartesian_motion_planner.h" +#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h" + +namespace cmvr::device { + +class PinocchioCartesianMotionPlanner final : public CartesianMotionPlanner { +public: + explicit PinocchioCartesianMotionPlanner(std::shared_ptr solver); + + bool configureSpeedL(const config::SpeedLPlannerConfig& config, + std::size_t dof) override; + bool configureMoveL(const config::MoveLPlannerConfig& config) override; + + bool planMoveL(const CartesianPose& target, + const std::vector& q_start, + const std::vector& qd_max, + double velocity, + double acceleration, + double jerk, + FrameType frame, + CartesianJointTrajectory& trajectory) override; + + bool speedLStep(const CartesianVelocity& target_velocity, + double dt, + const std::vector& q_measured, + const std::vector& qd_measured, + std::vector& qd_command, + FrameType frame) override; + + bool updateSpeedLAcceleration(double acceleration) override; + CartesianVelocity getSpeedLCommandTwistBase() const override; + +private: + bool refreshJointAccelerationLimits_(); + Eigen::VectorXd applyJointAccelerationLimits_(const Eigen::VectorXd& qdot, + const Eigen::VectorXd& reference, + double dt) const; + bool checkMoveLPlanLineDeviation_(const Eigen::Vector3d& p_start, + const Eigen::Vector3d& line_direction, + const Eigen::Vector3d& p_current, + double traveled, + bool& deviation_warned, + bool& direction_warned, + std::string& stop_reason) const; + bool checkMoveLPlanJointContinuity_(const Eigen::VectorXd& q_current, + const Eigen::VectorXd& q_next, + const Eigen::VectorXd& qdot, + const Eigen::VectorXd& prev_qdot, + double dt, + double path_position, + std::string& stop_reason) const; + bool checkMoveLPlanJointPositionLimits_(const Eigen::VectorXd& q_current, + const Eigen::VectorXd& q_next, + const Eigen::VectorXd& qdot, + double dt, + double path_position, + std::string& stop_reason) const; + bool checkMoveLPlanCartesianStepFeasibility_( + const Eigen::Matrix& desired_twist, + const Eigen::Matrix& achieved_twist, + double path_position, + std::string& stop_reason) const; + bool updateAndValidateSpeedLLineDeviation_(const std::vector& q_measured, + bool is_stop_command, + const Eigen::Vector3d& command_linear_base); + bool validateSpeedLJointVelocityCommand_(const Eigen::VectorXd& qdot, + const Eigen::VectorXd& reference, + double dt) const; + bool validateSpeedLPredictedJointPositionLimits_(const std::vector& q_measured, + const Eigen::VectorXd& qdot, + double dt) const; + bool validateSpeedLCartesianVelocityFeasibility_( + const Eigen::Matrix& desired_twist, + const Eigen::Matrix& achieved_twist, + const Eigen::VectorXd& q_current, + const Eigen::VectorXd& qdot) const; + std::string describeJointLimitCandidates_(const Eigen::VectorXd& q_current, + const Eigen::VectorXd& qdot) const; + + std::shared_ptr solver_{nullptr}; + config::MoveLPlannerConfig movel_config_{}; + config::SpeedLPlannerConfig speedl_config_{}; + cmvr::CartesianTwistLimiter twist_limiter_{}; + Eigen::VectorXd joint_acceleration_limits_; + std::vector prev_qdot_command_; + Eigen::Matrix speedl_command_twist_base_{Eigen::Matrix::Zero()}; + Eigen::Vector3d speedl_line_start_tcp_base_{Eigen::Vector3d::Zero()}; + Eigen::Vector3d speedl_line_direction_base_{Eigen::Vector3d::Zero()}; + double speedl_applied_acceleration_{0.25}; + bool speedl_line_check_active_{false}; + bool speedl_line_deviation_warned_{false}; + bool speedl_line_direction_warned_{false}; + bool speedl_configured_{false}; +}; + +} // namespace cmvr::device + +#endif // CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp new file mode 100644 index 00000000..ab29231d --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp @@ -0,0 +1,1126 @@ +#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h" + +#include +#include +#include +#include +#include +#include +#include + +#include "algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h" +#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve.h" +#include "common/base/logging/logger.h" +#include "common/math/cartesian_motion_math.h" +#include "common/config/config_files.h" +#include "common/math/transform_math.h" + +namespace cmvr::device { + +using cmvr::common::config::positiveOr; +using cmvr::device::cartesian_motion::clamp; +using cmvr::device::cartesian_motion::directionDeviationDeg; +using cmvr::device::cartesian_motion::lateralDistanceToLine; +using cmvr::device::cartesian_motion::rotationVector; +using cmvr::device::cartesian_motion::toEigenVector; +using cmvr::device::cartesian_motion::toStdVector; + +namespace { + +double lineDirectionDeviationDeg(const Eigen::Vector3d& expected, + const Eigen::Vector3d& actual) +{ + return directionDeviationDeg(expected, actual); +} + +} // namespace + +PinocchioCartesianMotionPlanner::PinocchioCartesianMotionPlanner( + std::shared_ptr solver) + : solver_(std::move(solver)) +{ +} + +bool PinocchioCartesianMotionPlanner::refreshJointAccelerationLimits_() +{ + joint_acceleration_limits_.resize(0); + if (!solver_) { + return false; + } + const auto& limits = solver_->jointLimitPolicy().limits(); + if (limits.source() != config::JOINT_LIMIT_SOURCE_CUSTOM) { + return true; + } + + std::vector joint_names; + if (!solver_->getChainJointNames(joint_names) || joint_names.empty()) { + return false; + } + + std::unordered_map custom_limits; + custom_limits.reserve(static_cast(limits.joints_size())); + for (const auto& item : limits.joints()) { + if (!item.joint_name().empty()) { + custom_limits[item.joint_name()] = item; + } + } + + joint_acceleration_limits_.resize(static_cast(joint_names.size())); + for (Eigen::Index i = 0; i < joint_acceleration_limits_.size(); ++i) { + const auto& joint_name = joint_names[static_cast(i)]; + const auto it = custom_limits.find(joint_name); + if (it == custom_limits.end() || !std::isfinite(it->second.qdd()) || it->second.qdd() < 0.0) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner] invalid custom qdd for " + << joint_name; + return false; + } + joint_acceleration_limits_[i] = std::abs(it->second.qdd()); + } + return true; +} + +Eigen::VectorXd PinocchioCartesianMotionPlanner::applyJointAccelerationLimits_( + const Eigen::VectorXd& qdot, + const Eigen::VectorXd& reference, + const double dt) const +{ + if (reference.size() != qdot.size() || dt <= 0.0) { + return qdot; + } + + Eigen::VectorXd limited = qdot; + for (Eigen::Index i = 0; i < qdot.size(); ++i) { + double acc_limit = 8.0; + if (joint_acceleration_limits_.size() == qdot.size() && + joint_acceleration_limits_[i] > 0.0) { + acc_limit = joint_acceleration_limits_[i]; + } + + const double delta_max = acc_limit * dt; + const double delta = clamp(qdot[i] - reference[i], -delta_max, delta_max); + limited[i] = reference[i] + delta; + } + return limited; +} + +bool PinocchioCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlannerConfig& config, + const std::size_t dof) +{ + if (!solver_ || dof == 0) { + return false; + } + const auto solver_dof = static_cast(std::max(0, solver_->chainVelocityDof())); + if (solver_dof != 0 && solver_dof != dof) { + return false; + } + speedl_config_ = config; + + if (!refreshJointAccelerationLimits_()) { + return false; + } + + cartesian_motion::configureTwistLimiterFromSpeedLConfig(twist_limiter_, speedl_config_); + + prev_qdot_command_.assign(dof, 0.0); + speedl_command_twist_base_.setZero(); + speedl_line_check_active_ = false; + speedl_line_deviation_warned_ = false; + speedl_line_direction_warned_ = false; + speedl_applied_acceleration_ = positiveOr(speedl_config_.linear_acceleration_max(), 5.0); + speedl_configured_ = true; + return true; +} + +bool PinocchioCartesianMotionPlanner::configureMoveL( + const config::MoveLPlannerConfig& config) +{ + if (!std::isfinite(config.sample_period_s()) || + !std::isfinite(config.position_gain()) || + !std::isfinite(config.rotation_gain())) { + return false; + } + movel_config_ = config; + return true; +} + +bool PinocchioCartesianMotionPlanner::planMoveL(const CartesianPose& target, + const std::vector& q_start, + const std::vector& qd_max, + const double velocity, + const double acceleration, + const double jerk, + const FrameType frame, + CartesianJointTrajectory& trajectory) +{ + trajectory = {}; + const double dt = positiveOr(movel_config_.sample_period_s(), 0.001); + if (!solver_ || q_start.empty() || + velocity <= 0.0 || acceleration <= 0.0 || jerk <= 0.0) { + return false; + } + if (static_cast(q_start.size()) != solver_->chainDof()) { + return false; + } + if (!qd_max.empty() && qd_max.size() != q_start.size()) { + return false; + } + + Eigen::Matrix4d start_pose_base = Eigen::Matrix4d::Identity(); + if (!solver_->fk(q_start, start_pose_base, true)) { + return false; + } + + const Eigen::Matrix4d target_pose_input = common::math::poseToMatrix(target); + const Eigen::Matrix4d target_pose_base = + frame == FrameType::Tool ? start_pose_base * target_pose_input : target_pose_input; + + const Eigen::Vector3d p_start = start_pose_base.block<3, 1>(0, 3); + const Eigen::Vector3d p_target = target_pose_base.block<3, 1>(0, 3); + const Eigen::Vector3d dp = p_target - p_start; + const double linear_distance = dp.norm(); + + const Eigen::Matrix3d R_start = start_pose_base.block<3, 3>(0, 0); + const Eigen::Matrix3d R_target = target_pose_base.block<3, 3>(0, 0); + const Eigen::Vector3d total_rotation_vector = rotationVector(R_target * R_start.transpose()); + const double angular_distance = total_rotation_vector.norm(); + + const double path_length = linear_distance > 1e-9 ? linear_distance : angular_distance; + trajectory.planned_path_length = path_length; + trajectory.executable_path_length = 0.0; + trajectory.position.push_back(q_start); + trajectory.velocity.push_back(std::vector(q_start.size(), 0.0)); + trajectory.time.push_back(0.0); + if (path_length <= 1e-9) { + return true; + } + + cmvr::SCurve curve(velocity, acceleration, jerk); + const cmvr::SCurveProfile profile = curve.calculateProfile(0.0, path_length, 0.0, 0.0); + if (profile.total_time <= 0.0) { + return false; + } + + Eigen::VectorXd q_current = toEigenVector(q_start); + Eigen::Vector3d linear_direction = Eigen::Vector3d::Zero(); + if (linear_distance > 1e-9) { + linear_direction = dp / linear_distance; + } + const Eigen::Quaterniond q_start_rot(R_start); + const Eigen::Quaterniond q_target_rot(R_target); + const double position_gain = positiveOr(movel_config_.position_gain(), 4.0); + const double rotation_gain = positiveOr(movel_config_.rotation_gain(), 4.0); + bool line_deviation_warned = false; + bool line_direction_warned = false; + double executable_path_length = 0.0; + Eigen::VectorXd prev_qdot = Eigen::VectorXd::Zero(q_current.size()); + + double previous_time = 0.0; + for (double t = std::min(dt, profile.total_time); + t <= profile.total_time + 1e-9; + t = std::min(t + dt, profile.total_time)) { + const double step_dt = std::max(1e-6, t - previous_time); + previous_time = t; + + const double s = clamp(curve.getPositionAtTime(profile, t), 0.0, path_length); + const double sd = std::max(0.0, curve.getVelocityAtTime(profile, t)); + const double ratio = clamp(s / path_length, 0.0, 1.0); + + const std::vector q_std = toStdVector(q_current); + Eigen::Matrix4d current_pose_base = Eigen::Matrix4d::Identity(); + if (!solver_->fk(q_std, current_pose_base, true)) { + return false; + } + + const Eigen::Vector3d p_current = current_pose_base.block<3, 1>(0, 3); + const Eigen::Vector3d p_desired = p_start + ratio * dp; + std::string stop_reason; + if (!checkMoveLPlanLineDeviation_(p_start, + linear_direction, + p_current, + (p_current - p_start).norm(), + line_deviation_warned, + line_direction_warned, + stop_reason)) { + trajectory.truncated = true; + trajectory.truncation_reason = std::move(stop_reason); + trajectory.executable_path_length = executable_path_length; + break; + } + Eigen::Matrix target_twist_base = Eigen::Matrix::Zero(); + target_twist_base.head<3>() = + linear_direction * sd + position_gain * (p_desired - p_current); + + if (angular_distance > 1e-9) { + const Eigen::Matrix3d R_current = current_pose_base.block<3, 3>(0, 0); + const Eigen::Matrix3d R_desired = + q_start_rot.slerp(ratio, q_target_rot).toRotationMatrix(); + const Eigen::Vector3d rotation_error = + rotationVector(R_desired * R_current.transpose()); + target_twist_base.tail<3>() = + (total_rotation_vector / path_length) * sd + rotation_gain * rotation_error; + } + + Eigen::MatrixXd jacobian_base; + Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); + if (!solver_->computeJacobianBaseAtQ(q_std, true, jacobian_base, base_R_tool)) { + return false; + } + + std::vector qdot_std; + if (!solver_->solveVelocityBase(jacobian_base, + target_twist_base, + q_std, + qdot_std, + std::numeric_limits::infinity())) { + return false; + } + + Eigen::VectorXd qdot = toEigenVector(qdot_std); + if (!qd_max.empty()) { + double scale = 1.0; + for (Eigen::Index i = 0; i < qdot.size(); ++i) { + const double limit = std::abs(qd_max[static_cast(i)]); + if (limit <= 0.0 || !std::isfinite(limit)) { + continue; + } + const double value = std::abs(qdot[i]); + if (value > limit) { + scale = std::min(scale, limit / value); + } + } + qdot *= scale; + } + const Eigen::Matrix achieved_twist_base = jacobian_base * qdot; + if (!checkMoveLPlanCartesianStepFeasibility_(target_twist_base, + achieved_twist_base, + s, + stop_reason)) { + trajectory.truncated = true; + trajectory.truncation_reason = std::move(stop_reason); + trajectory.executable_path_length = executable_path_length; + break; + } + const Eigen::VectorXd q_next = q_current + qdot * step_dt; + if (!checkMoveLPlanJointContinuity_(q_current, + q_next, + qdot, + prev_qdot, + step_dt, + s, + stop_reason)) { + trajectory.truncated = true; + trajectory.truncation_reason = std::move(stop_reason); + trajectory.executable_path_length = executable_path_length; + break; + } + if (!checkMoveLPlanJointPositionLimits_(q_current, + q_next, + qdot, + step_dt, + s, + stop_reason)) { + trajectory.truncated = true; + trajectory.truncation_reason = std::move(stop_reason); + trajectory.executable_path_length = executable_path_length; + break; + } + + const std::vector q_next_std = toStdVector(q_next); + Eigen::Matrix4d next_pose_base = Eigen::Matrix4d::Identity(); + if (!solver_->fk(q_next_std, next_pose_base, true)) { + return false; + } + const Eigen::Vector3d p_next = next_pose_base.block<3, 1>(0, 3); + if (!checkMoveLPlanLineDeviation_(p_start, + linear_direction, + p_next, + (p_next - p_start).norm(), + line_deviation_warned, + line_direction_warned, + stop_reason)) { + trajectory.truncated = true; + trajectory.truncation_reason = std::move(stop_reason); + trajectory.executable_path_length = executable_path_length; + break; + } + + q_current = q_next; + prev_qdot = qdot; + + trajectory.position.push_back(toStdVector(q_current)); + trajectory.velocity.push_back(toStdVector(qdot)); + trajectory.time.push_back(t); + executable_path_length = s; + trajectory.executable_path_length = executable_path_length; + + if (t >= profile.total_time - 1e-9) { + break; + } + } + if (!trajectory.truncated) { + trajectory.executable_path_length = path_length; + } + + return true; +} + +bool PinocchioCartesianMotionPlanner::checkMoveLPlanJointContinuity_( + const Eigen::VectorXd& q_current, + const Eigen::VectorXd& q_next, + const Eigen::VectorXd& qdot, + const Eigen::VectorXd& prev_qdot, + const double dt, + const double path_position, + std::string& stop_reason) const +{ + const auto& config = movel_config_.joint_continuity_check(); + if (!config.enable()) { + return true; + } + if (q_current.size() != q_next.size() || + q_current.size() != qdot.size() || + qdot.size() != prev_qdot.size() || + dt <= 0.0 || !std::isfinite(dt)) { + stop_reason = "joint continuity invalid state"; + CMVR_LOG(WARNING) << "[PinocchioCartesianMotionPlanner][moveL][plan] joint continuity invalid state"; + return false; + } + + for (Eigen::Index i = 0; i < q_next.size(); ++i) { + if (!std::isfinite(q_current[i]) || + !std::isfinite(q_next[i]) || + !std::isfinite(qdot[i]) || + !std::isfinite(prev_qdot[i])) { + std::ostringstream oss; + oss << "joint continuity non-finite value: joint_index=" << i + << ", path_position_m=" << path_position; + stop_reason = oss.str(); + CMVR_LOG(WARNING) << "[PinocchioCartesianMotionPlanner][moveL][plan] " + << stop_reason; + return false; + } + } + + const double max_delta = (q_next - q_current).cwiseAbs().maxCoeff(); + const double max_velocity = qdot.cwiseAbs().maxCoeff(); + const double max_acceleration = ((qdot - prev_qdot) / dt).cwiseAbs().maxCoeff(); + const double delta_limit = positiveOr(config.max_joint_delta_rad(), 0.05); + const double velocity_limit = positiveOr(config.max_joint_velocity_rad_s(), 10.0); + const double acceleration_limit = positiveOr(config.max_joint_acceleration_rad_s2(), 5000.0); + + if (max_delta > delta_limit || + max_velocity > velocity_limit || + max_acceleration > acceleration_limit) { + CMVR_LOG(WARNING) << "[PinocchioCartesianMotionPlanner][moveL][plan] joint continuity exceeds stop threshold: max_delta_rad=" + << max_delta << ", delta_limit_rad=" << delta_limit + << ", max_velocity_rad_s=" << max_velocity + << ", velocity_limit_rad_s=" << velocity_limit + << ", max_acceleration_rad_s2=" << max_acceleration + << ", acceleration_limit_rad_s2=" << acceleration_limit + << ", path_position_m=" << path_position; + std::ostringstream oss; + oss << "joint continuity too large: max_delta_rad=" << max_delta + << ", delta_limit_rad=" << delta_limit + << ", max_velocity_rad_s=" << max_velocity + << ", velocity_limit_rad_s=" << velocity_limit + << ", max_acceleration_rad_s2=" << max_acceleration + << ", acceleration_limit_rad_s2=" << acceleration_limit + << ", path_position_m=" << path_position; + stop_reason = oss.str(); + return false; + } + return true; +} + +bool PinocchioCartesianMotionPlanner::checkMoveLPlanJointPositionLimits_( + const Eigen::VectorXd& q_current, + const Eigen::VectorXd& q_next, + const Eigen::VectorXd& qdot, + const double dt, + const double path_position, + std::string& stop_reason) const +{ + if (!solver_ || !solver_->jointLimitPolicy().limits().enable()) { + return true; + } + if (q_current.size() != q_next.size() || + q_current.size() != qdot.size()) { + stop_reason = "joint position limit check size mismatch"; + CMVR_LOG(WARNING) << "[PinocchioCartesianMotionPlanner][moveL][plan] " + << stop_reason; + return false; + } + + Eigen::VectorXd lower; + Eigen::VectorXd upper; + if (!solver_->getJointPositionLimits(lower, upper) || + lower.size() != q_next.size() || + upper.size() != q_next.size()) { + return true; + } + + std::vector joint_names; + (void)solver_->getChainJointNames(joint_names); + constexpr double kLimitEps = 1e-6; + for (Eigen::Index i = 0; i < q_next.size(); ++i) { + const double q = q_current[i]; + const double qn = q_next[i]; + const double lo = lower[i]; + const double hi = upper[i]; + if (!std::isfinite(q) || !std::isfinite(qn) || + !std::isfinite(lo) || !std::isfinite(hi) || hi <= lo) { + continue; + } + + const bool violates_lower = qn < lo - kLimitEps; + const bool violates_upper = qn > hi + kLimitEps; + const bool moves_out_lower = q <= lo + kLimitEps && qdot[i] < -kLimitEps; + const bool moves_out_upper = q >= hi - kLimitEps && qdot[i] > kLimitEps; + if (violates_lower || violates_upper || moves_out_lower || moves_out_upper) { + const std::string joint_name = + static_cast(i) < joint_names.size() + ? joint_names[static_cast(i)] + : std::to_string(i); + CMVR_LOG(WARNING) << "[PinocchioCartesianMotionPlanner][moveL][plan] predicted joint position limit exceeded: joint=" + << joint_name << ", q=" << q + << ", q_next=" << qn + << ", lower=" << lo + << ", upper=" << hi + << ", qdot=" << qdot[i] + << ", dt=" << dt + << ", path_position_m=" << path_position; + std::ostringstream oss; + oss << "joint position limit exceeded: joint=" << joint_name + << ", q=" << q + << ", q_next=" << qn + << ", lower=" << lo + << ", upper=" << hi + << ", qdot=" << qdot[i] + << ", dt=" << dt + << ", path_position_m=" << path_position; + stop_reason = oss.str(); + return false; + } + } + return true; +} + +bool PinocchioCartesianMotionPlanner::checkMoveLPlanCartesianStepFeasibility_( + const Eigen::Matrix& desired_twist, + const Eigen::Matrix& achieved_twist, + const double path_position, + std::string& stop_reason) const +{ + const auto& config = movel_config_.cartesian_step_feasibility_check(); + if (!config.enable()) { + return true; + } + + const Eigen::Vector3d desired_linear = desired_twist.head<3>(); + const Eigen::Vector3d achieved_linear = achieved_twist.head<3>(); + const double desired_linear_norm = desired_linear.norm(); + const double achieved_linear_norm = achieved_linear.norm(); + const double min_desired_linear_speed = + positiveOr(config.min_desired_linear_speed(), 1e-4); + if (desired_linear_norm >= min_desired_linear_speed) { + const double speed_ratio = achieved_linear_norm / desired_linear_norm; + const double min_ratio = positiveOr(config.min_linear_speed_ratio(), 0.2); + const double deviation_deg = + achieved_linear_norm > 1e-9 + ? directionDeviationDeg(desired_linear, achieved_linear) + : 180.0; + const double max_deviation_deg = + positiveOr(config.max_linear_direction_deviation_deg(), 45.0); + if (speed_ratio < min_ratio || deviation_deg > max_deviation_deg) { + CMVR_LOG(WARNING) << "[PinocchioCartesianMotionPlanner][moveL][plan] cartesian step infeasible after joint limits: desired_linear=[" + << desired_linear.transpose() << "], achieved_linear=[" + << achieved_linear.transpose() << "], speed_ratio=" + << speed_ratio << ", min_ratio=" << min_ratio + << ", deviation_deg=" << deviation_deg + << ", max_deviation_deg=" << max_deviation_deg + << ", path_position_m=" << path_position; + std::ostringstream oss; + oss << "cartesian step infeasible after joint limits: linear_speed_ratio=" + << speed_ratio << ", min_ratio=" << min_ratio + << ", linear_deviation_deg=" << deviation_deg + << ", max_deviation_deg=" << max_deviation_deg + << ", path_position_m=" << path_position; + stop_reason = oss.str(); + return false; + } + } + + const Eigen::Vector3d desired_angular = desired_twist.tail<3>(); + const Eigen::Vector3d achieved_angular = achieved_twist.tail<3>(); + const double desired_angular_norm = desired_angular.norm(); + const double achieved_angular_norm = achieved_angular.norm(); + const double min_desired_angular_speed = + positiveOr(config.min_desired_angular_speed(), 1e-4); + if (desired_angular_norm >= min_desired_angular_speed) { + const double speed_ratio = achieved_angular_norm / desired_angular_norm; + const double min_ratio = positiveOr(config.min_angular_speed_ratio(), 0.2); + const double deviation_deg = + achieved_angular_norm > 1e-9 + ? directionDeviationDeg(desired_angular, achieved_angular) + : 180.0; + const double max_deviation_deg = + positiveOr(config.max_angular_direction_deviation_deg(), 45.0); + if (speed_ratio < min_ratio || deviation_deg > max_deviation_deg) { + CMVR_LOG(WARNING) << "[PinocchioCartesianMotionPlanner][moveL][plan] cartesian angular step infeasible after joint limits: desired_angular=[" + << desired_angular.transpose() << "], achieved_angular=[" + << achieved_angular.transpose() << "], speed_ratio=" + << speed_ratio << ", min_ratio=" << min_ratio + << ", deviation_deg=" << deviation_deg + << ", max_deviation_deg=" << max_deviation_deg + << ", path_position_m=" << path_position; + std::ostringstream oss; + oss << "cartesian angular step infeasible after joint limits: angular_speed_ratio=" + << speed_ratio << ", min_ratio=" << min_ratio + << ", angular_deviation_deg=" << deviation_deg + << ", max_deviation_deg=" << max_deviation_deg + << ", path_position_m=" << path_position; + stop_reason = oss.str(); + return false; + } + } + + return true; +} + +bool PinocchioCartesianMotionPlanner::checkMoveLPlanLineDeviation_( + const Eigen::Vector3d& p_start, + const Eigen::Vector3d& line_direction, + const Eigen::Vector3d& p_current, + const double traveled, + bool& deviation_warned, + bool& direction_warned, + std::string& stop_reason) const +{ + const auto& config = movel_config_.line_deviation_check(); + if (!config.enable() || line_direction.squaredNorm() <= 1e-12) { + return true; + } + + const double min_distance = positiveOr(config.line_check_min_distance_m(), 0.01); + if (traveled < min_distance) { + return true; + } + + const double lateral_error = lateralDistanceToLine(p_start, line_direction, p_current); + const double stop_m = positiveOr(config.line_deviation_stop_m(), 0.03); + if (lateral_error >= stop_m) { + CMVR_LOG(WARNING) << "[PinocchioCartesianMotionPlanner][moveL][plan] line deviation exceeds stop threshold: lateral_error_m=" + << lateral_error << ", stop_m=" << stop_m + << ", traveled_m=" << traveled; + deviation_warned = true; + std::ostringstream oss; + oss << "line deviation too large: lateral_error_m=" << lateral_error + << ", stop_m=" << stop_m + << ", traveled_m=" << traveled; + stop_reason = oss.str(); + return false; + } + + const double warn_m = positiveOr(config.line_deviation_warn_m(), 0.01); + if (!deviation_warned && lateral_error >= warn_m) { + CMVR_LOG(WARNING) << "[PinocchioCartesianMotionPlanner][moveL][plan] line deviation warning: lateral_error_m=" + << lateral_error << ", warn_m=" << warn_m + << ", traveled_m=" << traveled; + deviation_warned = true; + } + + const Eigen::Vector3d actual_direction = (p_current - p_start) / traveled; + const double deviation_deg = lineDirectionDeviationDeg(line_direction, actual_direction); + const double stop_deg = positiveOr(config.line_direction_stop_deg(), 45.0); + if (deviation_deg >= stop_deg) { + CMVR_LOG(WARNING) << "[PinocchioCartesianMotionPlanner][moveL][plan] line direction deviation exceeds stop threshold: deviation_deg=" + << deviation_deg << ", stop_deg=" << stop_deg + << ", traveled_m=" << traveled; + direction_warned = true; + std::ostringstream oss; + oss << "line direction deviation too large: deviation_deg=" << deviation_deg + << ", stop_deg=" << stop_deg + << ", traveled_m=" << traveled; + stop_reason = oss.str(); + return false; + } + + const double warn_deg = positiveOr(config.line_direction_warn_deg(), 20.0); + if (!direction_warned && deviation_deg >= warn_deg) { + CMVR_LOG(WARNING) << "[PinocchioCartesianMotionPlanner][moveL][plan] line direction deviation warning: deviation_deg=" + << deviation_deg << ", warn_deg=" << warn_deg + << ", traveled_m=" << traveled; + direction_warned = true; + } + return true; +} + +bool PinocchioCartesianMotionPlanner::updateAndValidateSpeedLLineDeviation_( + const std::vector& q_measured, + const bool is_stop_command, + const Eigen::Vector3d& command_linear_base) +{ + const auto& config = speedl_config_.line_deviation_check(); + if (!config.enable()) { + return true; + } + + const double command_norm = command_linear_base.norm(); + if (is_stop_command || command_norm <= 1e-9) { + speedl_line_check_active_ = false; + speedl_line_deviation_warned_ = false; + speedl_line_direction_warned_ = false; + return true; + } + + Eigen::Matrix4d tcp_pose_base = Eigen::Matrix4d::Identity(); + if (!solver_->fk(q_measured, tcp_pose_base, true)) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] line check FK failed"; + return false; + } + const Eigen::Vector3d tcp_position = tcp_pose_base.block<3, 1>(0, 3); + const Eigen::Vector3d command_direction = command_linear_base / command_norm; + + bool reset_line = !speedl_line_check_active_; + if (!reset_line && speedl_line_direction_base_.squaredNorm() > 1e-12) { + const double reset_deg = positiveOr(config.line_direction_reset_deg(), 10.0); + const double command_change_deg = + lineDirectionDeviationDeg(speedl_line_direction_base_, command_direction); + reset_line = command_change_deg >= reset_deg; + } + + if (reset_line) { + speedl_line_start_tcp_base_ = tcp_position; + speedl_line_direction_base_ = command_direction; + speedl_line_check_active_ = true; + speedl_line_deviation_warned_ = false; + speedl_line_direction_warned_ = false; + return true; + } + + const Eigen::Vector3d delta = tcp_position - speedl_line_start_tcp_base_; + const double traveled = delta.norm(); + const double min_distance = positiveOr(config.line_check_min_distance_m(), 0.01); + if (traveled < min_distance) { + return true; + } + + const double lateral_error = + lateralDistanceToLine(speedl_line_start_tcp_base_, speedl_line_direction_base_, tcp_position); + const double stop_m = positiveOr(config.line_deviation_stop_m(), 0.03); + if (lateral_error >= stop_m) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] line deviation too large: lateral_error_m=" + << lateral_error << ", stop_m=" << stop_m + << ", traveled_m=" << traveled; + return false; + } + + const double warn_m = positiveOr(config.line_deviation_warn_m(), 0.01); + if (!speedl_line_deviation_warned_ && lateral_error >= warn_m) { + CMVR_LOG(WARNING) << "[PinocchioCartesianMotionPlanner][speedL] line deviation warning: lateral_error_m=" + << lateral_error << ", warn_m=" << warn_m + << ", traveled_m=" << traveled; + speedl_line_deviation_warned_ = true; + } + + const Eigen::Vector3d actual_direction = delta / traveled; + const double deviation_deg = + lineDirectionDeviationDeg(speedl_line_direction_base_, actual_direction); + const double stop_deg = positiveOr(config.line_direction_stop_deg(), 45.0); + if (deviation_deg >= stop_deg) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] line direction deviation too large: deviation_deg=" + << deviation_deg << ", stop_deg=" << stop_deg + << ", traveled_m=" << traveled; + return false; + } + + const double warn_deg = positiveOr(config.line_direction_warn_deg(), 20.0); + if (!speedl_line_direction_warned_ && deviation_deg >= warn_deg) { + CMVR_LOG(WARNING) << "[PinocchioCartesianMotionPlanner][speedL] line direction deviation warning: deviation_deg=" + << deviation_deg << ", warn_deg=" << warn_deg + << ", traveled_m=" << traveled; + speedl_line_direction_warned_ = true; + } + + return true; +} + +bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target_velocity, + const double dt, + const std::vector& q_measured, + const std::vector& qd_measured, + std::vector& qd_command, + const FrameType frame) +{ + qd_command.clear(); + if (!solver_ || !speedl_configured_ || dt <= 0.0) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] invalid state: solver=" + << (solver_ ? 1 : 0) + << ", configured=" << (speedl_configured_ ? 1 : 0) + << ", dt=" << dt; + return false; + } + if (static_cast(q_measured.size()) != solver_->chainDof() || + static_cast(qd_measured.size()) != solver_->chainVelocityDof()) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] state size mismatch: q=" + << q_measured.size() << "/" << solver_->chainDof() + << ", qd=" << qd_measured.size() << "/" << solver_->chainVelocityDof(); + return false; + } + + Eigen::Matrix measured_twist_base = Eigen::Matrix::Zero(); + Eigen::MatrixXd jacobian_base; + Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); + if (!solver_->computeTwistBaseAtQ(q_measured, + qd_measured, + true, + measured_twist_base, + &jacobian_base, + &base_R_tool)) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] computeTwistBaseAtQ failed"; + return false; + } + + const Eigen::Matrix target_twist = common::math::velocityToVector(target_velocity); + const bool is_stop_command = target_twist.squaredNorm() <= 1e-12; + if (is_stop_command) { + twist_limiter_.synchronize(measured_twist_base, dt, true); + } else if (speedl_command_twist_base_.squaredNorm() <= 1e-12) { + twist_limiter_.initialize(Eigen::Matrix::Zero()); + } + twist_limiter_.setTargetTwist(target_twist, common::math::toPlannerFrame(frame)); + speedl_command_twist_base_ = twist_limiter_.update(dt, base_R_tool); + if (!updateAndValidateSpeedLLineDeviation_(q_measured, + is_stop_command, + speedl_command_twist_base_.head<3>())) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] line deviation check failed"; + return false; + } + + Eigen::VectorXd reference = toEigenVector(qd_measured); + if (prev_qdot_command_.size() == q_measured.size()) { + reference = toEigenVector(prev_qdot_command_); + } + + const bool enforce_acceleration_limits = + !speedl_config_.has_enforce_joint_acceleration_limits() || + speedl_config_.enforce_joint_acceleration_limits(); + std::vector qdot_std; + if (!solver_->solveVelocityBase(jacobian_base, + speedl_command_twist_base_, + q_measured, + qdot_std, + std::numeric_limits::infinity())) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] solveVelocityBase failed"; + return false; + } + Eigen::VectorXd qdot = toEigenVector(qdot_std); + if (enforce_acceleration_limits) { + qdot = applyJointAccelerationLimits_(qdot, reference, dt); + } + const Eigen::Matrix achieved_twist_base = jacobian_base * qdot; + if (!validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_, + achieved_twist_base, + toEigenVector(q_measured), + qdot)) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] cartesian velocity feasibility check failed"; + return false; + } + if (!validateSpeedLJointVelocityCommand_(qdot, reference, dt)) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] joint velocity check failed"; + return false; + } + if (!validateSpeedLPredictedJointPositionLimits_(q_measured, qdot, dt)) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] predicted joint position limit check failed"; + return false; + } + const Eigen::VectorXd q_predicted = toEigenVector(q_measured) + qdot * dt; + if (!updateAndValidateSpeedLLineDeviation_(toStdVector(q_predicted), + is_stop_command, + speedl_command_twist_base_.head<3>())) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] predicted line deviation check failed"; + return false; + } + + qd_command = toStdVector(qdot); + prev_qdot_command_ = qd_command; + return true; +} + +bool PinocchioCartesianMotionPlanner::validateSpeedLJointVelocityCommand_( + const Eigen::VectorXd& qdot, + const Eigen::VectorXd& reference, + const double dt) const +{ + const auto& config = speedl_config_.joint_velocity_check(); + if (!config.enable()) { + return true; + } + if (qdot.size() <= 0 || qdot.size() != reference.size() || + dt <= 0.0 || !std::isfinite(dt)) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] joint velocity check invalid state: qdot_size=" + << qdot.size() << ", reference_size=" << reference.size() + << ", dt=" << dt; + return false; + } + + for (Eigen::Index i = 0; i < qdot.size(); ++i) { + if (!std::isfinite(qdot[i]) || !std::isfinite(reference[i])) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] joint velocity command non-finite: joint_index=" + << i << ", qdot=" << qdot[i] + << ", reference=" << reference[i]; + return false; + } + } + + const double max_velocity = qdot.cwiseAbs().maxCoeff(); + const double max_acceleration = ((qdot - reference) / dt).cwiseAbs().maxCoeff(); + const double velocity_limit = positiveOr(config.max_joint_velocity_rad_s(), 30.0); + const double acceleration_limit = positiveOr(config.max_joint_acceleration_rad_s2(), 10000.0); + if (max_velocity > velocity_limit || max_acceleration > acceleration_limit) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] joint velocity command exceeds threshold: max_joint_velocity_rad_s=" + << max_velocity << ", velocity_limit_rad_s=" << velocity_limit + << ", max_joint_acceleration_rad_s2=" << max_acceleration + << ", acceleration_limit_rad_s2=" << acceleration_limit + << ", dt=" << dt; + return false; + } + return true; +} + +bool PinocchioCartesianMotionPlanner::validateSpeedLPredictedJointPositionLimits_( + const std::vector& q_measured, + const Eigen::VectorXd& qdot, + const double dt) const +{ + if (!solver_ || !solver_->jointLimitPolicy().limits().enable()) { + return true; + } + if (static_cast(q_measured.size()) != qdot.size()) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] joint position limit check size mismatch: q=" + << q_measured.size() << ", qdot=" << qdot.size(); + return false; + } + + Eigen::VectorXd lower; + Eigen::VectorXd upper; + if (!solver_->getJointPositionLimits(lower, upper) || + lower.size() != qdot.size() || + upper.size() != qdot.size()) { + return true; + } + + std::vector joint_names; + (void)solver_->getChainJointNames(joint_names); + constexpr double kLimitEps = 1e-6; + for (Eigen::Index i = 0; i < qdot.size(); ++i) { + const double q = q_measured[static_cast(i)]; + const double q_next = q + qdot[i] * dt; + const double lo = lower[i]; + const double hi = upper[i]; + if (!std::isfinite(q) || !std::isfinite(q_next) || + !std::isfinite(lo) || !std::isfinite(hi) || hi <= lo) { + continue; + } + + const bool violates_lower = q_next < lo - kLimitEps; + const bool violates_upper = q_next > hi + kLimitEps; + const bool moves_out_lower = q <= lo + kLimitEps && qdot[i] < -kLimitEps; + const bool moves_out_upper = q >= hi - kLimitEps && qdot[i] > kLimitEps; + if (violates_lower || violates_upper || moves_out_lower || moves_out_upper) { + const std::string joint_name = + static_cast(i) < joint_names.size() + ? joint_names[static_cast(i)] + : std::to_string(i); + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] predicted joint position limit exceeded: joint=" + << joint_name << ", q=" << q + << ", q_next=" << q_next + << ", lower=" << lo + << ", upper=" << hi + << ", qdot=" << qdot[i] + << ", dt=" << dt; + return false; + } + } + return true; +} + +bool PinocchioCartesianMotionPlanner::validateSpeedLCartesianVelocityFeasibility_( + const Eigen::Matrix& desired_twist, + const Eigen::Matrix& achieved_twist, + const Eigen::VectorXd& q_current, + const Eigen::VectorXd& qdot) const +{ + const auto& config = speedl_config_.cartesian_velocity_feasibility_check(); + if (!config.enable()) { + return true; + } + + const Eigen::Vector3d desired_linear = desired_twist.head<3>(); + const Eigen::Vector3d achieved_linear = achieved_twist.head<3>(); + const double desired_linear_norm = desired_linear.norm(); + const double achieved_linear_norm = achieved_linear.norm(); + const double min_desired_linear_speed = + positiveOr(config.min_desired_linear_speed(), 1e-4); + if (desired_linear_norm >= min_desired_linear_speed) { + const double speed_ratio = achieved_linear_norm / desired_linear_norm; + const double min_ratio = positiveOr(config.min_linear_speed_ratio(), 0.2); + const double deviation_deg = + achieved_linear_norm > 1e-9 + ? directionDeviationDeg(desired_linear, achieved_linear) + : 180.0; + const double max_deviation_deg = + positiveOr(config.max_linear_direction_deviation_deg(), 45.0); + if (speed_ratio < min_ratio || deviation_deg > max_deviation_deg) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] cartesian velocity infeasible after joint limits " << "limit_candidates=[" + << describeJointLimitCandidates_(q_current, qdot) << "]: desired_linear=[" + << desired_linear.transpose() << "], achieved_linear=[" + << achieved_linear.transpose() << "], speed_ratio=" + << speed_ratio << ", min_ratio=" << min_ratio + << ", deviation_deg=" << deviation_deg + << ", max_deviation_deg=" << max_deviation_deg + ; + return false; + } + } + + const Eigen::Vector3d desired_angular = desired_twist.tail<3>(); + const Eigen::Vector3d achieved_angular = achieved_twist.tail<3>(); + const double desired_angular_norm = desired_angular.norm(); + const double achieved_angular_norm = achieved_angular.norm(); + const double min_desired_angular_speed = + positiveOr(config.min_desired_angular_speed(), 1e-4); + if (desired_angular_norm >= min_desired_angular_speed) { + const double speed_ratio = achieved_angular_norm / desired_angular_norm; + const double min_ratio = positiveOr(config.min_angular_speed_ratio(), 0.2); + const double deviation_deg = + achieved_angular_norm > 1e-9 + ? directionDeviationDeg(desired_angular, achieved_angular) + : 180.0; + const double max_deviation_deg = + positiveOr(config.max_angular_direction_deviation_deg(), 45.0); + if (speed_ratio < min_ratio || deviation_deg > max_deviation_deg) { + CMVR_LOG(ERROR) << "[PinocchioCartesianMotionPlanner][speedL] cartesian angular velocity infeasible after joint limits: desired_angular=[" + << desired_angular.transpose() << "], achieved_angular=[" + << achieved_angular.transpose() << "], speed_ratio=" + << speed_ratio << ", min_ratio=" << min_ratio + << ", deviation_deg=" << deviation_deg + << ", max_deviation_deg=" << max_deviation_deg + << ", limit_candidates=[" + << describeJointLimitCandidates_(q_current, qdot) << "]"; + return false; + } + } + + return true; +} + +std::string PinocchioCartesianMotionPlanner::describeJointLimitCandidates_( + const Eigen::VectorXd& q_current, + const Eigen::VectorXd& qdot) const +{ + if (!solver_ || q_current.size() != qdot.size()) { + return "none"; + } + + std::vector joint_names; + (void)solver_->getChainJointNames(joint_names); + + Eigen::VectorXd qd_limits; + const bool has_qd_limits = + solver_->getJointVelocityLimits(qd_limits) && qd_limits.size() == qdot.size(); + Eigen::VectorXd lower; + Eigen::VectorXd upper; + const bool has_q_limits = + solver_->getJointPositionLimits(lower, upper) && + lower.size() == q_current.size() && + upper.size() == q_current.size(); + + std::vector candidates; + constexpr double kVelocityRatio = 0.98; + for (Eigen::Index i = 0; i < qdot.size(); ++i) { + const std::string joint_name = + static_cast(i) < joint_names.size() + ? joint_names[static_cast(i)] + : std::to_string(i); + + std::vector reasons; + if (has_qd_limits) { + const double qd_limit = std::abs(qd_limits[i]); + if (std::isfinite(qd_limit) && qd_limit > 0.0 && + std::abs(qdot[i]) >= kVelocityRatio * qd_limit) { + std::ostringstream oss; + oss << "qd=" << qdot[i] << "/" << qd_limit; + reasons.push_back(oss.str()); + } + } + + if (has_q_limits) { + const double q = q_current[i]; + const double lo = lower[i]; + const double hi = upper[i]; + if (std::isfinite(q) && std::isfinite(lo) && std::isfinite(hi) && hi > lo) { + const double margin = std::max(0.02, 0.02 * (hi - lo)); + if (q <= lo + margin) { + std::ostringstream oss; + oss << "near_lower q=" << q << "/" << lo; + reasons.push_back(oss.str()); + } + if (q >= hi - margin) { + std::ostringstream oss; + oss << "near_upper q=" << q << "/" << hi; + reasons.push_back(oss.str()); + } + } + } + + if (!reasons.empty()) { + std::ostringstream item; + item << joint_name << "("; + for (std::size_t j = 0; j < reasons.size(); ++j) { + if (j > 0) { + item << "; "; + } + item << reasons[j]; + } + item << ")"; + candidates.push_back(item.str()); + } + } + + if (candidates.empty()) { + return "none"; + } + std::ostringstream oss; + for (std::size_t i = 0; i < candidates.size(); ++i) { + if (i > 0) { + oss << ", "; + } + oss << candidates[i]; + } + return oss.str(); +} + +bool PinocchioCartesianMotionPlanner::updateSpeedLAcceleration(const double acceleration) +{ + if (!speedl_configured_ || acceleration <= 0.0) { + return false; + } + if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9) { + return true; + } + + cartesian_motion::updateTwistLimiterAcceleration( + twist_limiter_, + speedl_config_, + acceleration); + speedl_applied_acceleration_ = acceleration; + return true; +} + +CartesianVelocity PinocchioCartesianMotionPlanner::getSpeedLCommandTwistBase() const +{ + return common::math::vectorToVelocity(speedl_command_twist_base_); +} + +} // namespace cmvr::device diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h deleted file mode 100644 index 87730db6..00000000 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h +++ /dev/null @@ -1,66 +0,0 @@ -#ifndef CMVR_ES_PINOCCHIO_DLS_CARTESIAN_MOTION_PLANNER_H -#define CMVR_ES_PINOCCHIO_DLS_CARTESIAN_MOTION_PLANNER_H - -#include - -#include -#include - -#include "../../cartesian_motion_planner.h" -#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" -#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h" - -namespace cmvr::device { - -class PinocchioDlsCartesianMotionPlanner final : public CartesianMotionPlanner { -public: - explicit PinocchioDlsCartesianMotionPlanner(std::shared_ptr solver); - - bool configureSpeedL(const config::SpeedLPlannerConfig& config, - std::size_t dof) override; - bool configureMoveL(const config::MoveLPlannerConfig& config) override; - - bool planMoveL(const CartesianPose& target, - const std::vector& q_start, - const std::vector& qd_max, - double velocity, - double acceleration, - double jerk, - FrameType frame, - CartesianJointTrajectory& trajectory) override; - - bool speedLStep(const CartesianVelocity& target_velocity, - double dt, - const std::vector& q_measured, - const std::vector& qd_measured, - std::vector& qd_command, - FrameType frame) override; - - bool updateSpeedLAcceleration(double acceleration) override; - CartesianVelocity getSpeedLCommandTwistBase() const override; - -private: - bool refreshJointLimits_(); - Eigen::VectorXd applyJointVelocityLimits_(const Eigen::VectorXd& qdot) const; - Eigen::VectorXd applyJointSoftLimits_(const Eigen::VectorXd& q, - const Eigen::VectorXd& qdot); - Eigen::VectorXd applyJointAccelerationLimits_(const Eigen::VectorXd& qdot, - const Eigen::VectorXd& reference, - double dt) const; - - std::shared_ptr solver_{nullptr}; - config::MoveLPlannerConfig movel_config_{}; - config::SpeedLPlannerConfig speedl_config_{}; - cmvr::CartesianTwistLimiter twist_limiter_{}; - Eigen::VectorXd joint_lower_limits_; - Eigen::VectorXd joint_upper_limits_; - Eigen::VectorXd joint_velocity_limits_; - std::vector prev_qdot_command_; - Eigen::Matrix speedl_command_twist_base_{Eigen::Matrix::Zero()}; - double speedl_applied_acceleration_{0.25}; - bool speedl_configured_{false}; -}; - -} // namespace cmvr::device - -#endif // CMVR_ES_PINOCCHIO_DLS_CARTESIAN_MOTION_PLANNER_H diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/src/pinocchio_dls_cartesian_motion_planner.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/src/pinocchio_dls_cartesian_motion_planner.cpp deleted file mode 100644 index 12a1c988..00000000 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/src/pinocchio_dls_cartesian_motion_planner.cpp +++ /dev/null @@ -1,408 +0,0 @@ -#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h" - -#include - -#include -#include -#include -#include - -#include "algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h" -#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve.h" -#include "common/math/cartesian_motion_math.h" -#include "common/math/joint_limits.h" -#include "common/config/config_files.h" -#include "common/math/transform_math.h" - -namespace cmvr::device { - -using cmvr::common::config::positiveOr; -using cmvr::device::cartesian_motion::clamp; -using cmvr::device::cartesian_motion::directionDeviationDeg; -using cmvr::device::cartesian_motion::rotationVector; -using cmvr::device::cartesian_motion::toEigenVector; -using cmvr::device::cartesian_motion::toStdVector; - -PinocchioDlsCartesianMotionPlanner::PinocchioDlsCartesianMotionPlanner( - std::shared_ptr solver) - : solver_(std::move(solver)) -{ -} - -bool PinocchioDlsCartesianMotionPlanner::refreshJointLimits_() -{ - if (!solver_) { - return false; - } - if (!solver_->getJointPositionLimits(joint_lower_limits_, joint_upper_limits_)) { - return false; - } - if (!solver_->getJointVelocityLimits(joint_velocity_limits_)) { - return false; - } - return true; -} - -Eigen::VectorXd PinocchioDlsCartesianMotionPlanner::applyJointVelocityLimits_( - const Eigen::VectorXd& qdot) const -{ - return cmvr::kinematics::scaleToVelocityLimits(qdot, joint_velocity_limits_); -} - -Eigen::VectorXd PinocchioDlsCartesianMotionPlanner::applyJointSoftLimits_( - const Eigen::VectorXd& q, - const Eigen::VectorXd& qdot) -{ - if (joint_lower_limits_.size() != q.size() || - joint_upper_limits_.size() != q.size() || - qdot.size() != q.size()) { - return qdot; - } - - Eigen::VectorXd limited = qdot; - for (Eigen::Index i = 0; i < q.size(); ++i) { - const double lower = joint_lower_limits_[i]; - const double upper = joint_upper_limits_[i]; - if (!std::isfinite(lower) || !std::isfinite(upper) || upper <= lower) { - continue; - } - - const double span = upper - lower; - const double margin = std::max(0.02, 0.08 * span); - if (limited[i] < 0.0 && q[i] < lower + margin) { - const double ratio = clamp((q[i] - lower) / margin, 0.0, 1.0); - limited[i] *= ratio; - if (q[i] <= lower) { - limited[i] = std::max(0.0, limited[i]); - } - } else if (limited[i] > 0.0 && q[i] > upper - margin) { - const double ratio = clamp((upper - q[i]) / margin, 0.0, 1.0); - limited[i] *= ratio; - if (q[i] >= upper) { - limited[i] = std::min(0.0, limited[i]); - } - } - } - return limited; -} - -Eigen::VectorXd PinocchioDlsCartesianMotionPlanner::applyJointAccelerationLimits_( - const Eigen::VectorXd& qdot, - const Eigen::VectorXd& reference, - const double dt) const -{ - if (reference.size() != qdot.size() || dt <= 0.0) { - return qdot; - } - - Eigen::VectorXd limited = qdot; - for (Eigen::Index i = 0; i < qdot.size(); ++i) { - double acc_limit = 8.0; - if (i < speedl_config_.joint_acceleration_max_size() && - speedl_config_.joint_acceleration_max(static_cast(i)) > 0.0) { - acc_limit = speedl_config_.joint_acceleration_max(static_cast(i)); - } - - const double delta_max = acc_limit * dt; - const double delta = clamp(qdot[i] - reference[i], -delta_max, delta_max); - limited[i] = reference[i] + delta; - } - return limited; -} - -bool PinocchioDlsCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlannerConfig& config, - const std::size_t dof) -{ - if (!solver_ || dof == 0) { - return false; - } - - const auto solver_dof = static_cast(std::max(0, solver_->chainVelocityDof())); - if (solver_dof != 0 && solver_dof != dof) { - return false; - } - - speedl_config_ = config; - - if (!refreshJointLimits_()) { - return false; - } - - cartesian_motion::configureTwistLimiterFromSpeedLConfig(twist_limiter_, speedl_config_); - - prev_qdot_command_.assign(dof, 0.0); - speedl_command_twist_base_.setZero(); - speedl_applied_acceleration_ = positiveOr(speedl_config_.linear_acceleration_max(), 5.0); - speedl_configured_ = true; - return true; -} - -bool PinocchioDlsCartesianMotionPlanner::configureMoveL( - const config::MoveLPlannerConfig& config) -{ - if (!std::isfinite(config.sample_period_s()) || - !std::isfinite(config.position_gain()) || - !std::isfinite(config.rotation_gain())) { - return false; - } - movel_config_ = config; - return true; -} - -bool PinocchioDlsCartesianMotionPlanner::planMoveL(const CartesianPose& target, - const std::vector& q_start, - const std::vector& qd_max, - const double velocity, - const double acceleration, - const double jerk, - const FrameType frame, - CartesianJointTrajectory& trajectory) -{ - trajectory = {}; - const double dt = positiveOr(movel_config_.sample_period_s(), 0.001); - if (!solver_ || q_start.empty() || - velocity <= 0.0 || acceleration <= 0.0 || jerk <= 0.0) { - return false; - } - if (static_cast(q_start.size()) != solver_->chainDof()) { - return false; - } - if (!qd_max.empty() && qd_max.size() != q_start.size()) { - return false; - } - if (!refreshJointLimits_()) { - return false; - } - - Eigen::Matrix4d start_pose_base = Eigen::Matrix4d::Identity(); - if (!solver_->fk(q_start, start_pose_base, true)) { - return false; - } - - const Eigen::Matrix4d target_pose_input = common::math::poseToMatrix(target); - const Eigen::Matrix4d target_pose_base = - frame == FrameType::Tool ? start_pose_base * target_pose_input : target_pose_input; - - const Eigen::Vector3d p_start = start_pose_base.block<3, 1>(0, 3); - const Eigen::Vector3d p_target = target_pose_base.block<3, 1>(0, 3); - const Eigen::Vector3d dp = p_target - p_start; - const double linear_distance = dp.norm(); - - const Eigen::Matrix3d R_start = start_pose_base.block<3, 3>(0, 0); - const Eigen::Matrix3d R_target = target_pose_base.block<3, 3>(0, 0); - const Eigen::Vector3d total_rotation_vector = rotationVector(R_target * R_start.transpose()); - const double angular_distance = total_rotation_vector.norm(); - - const double path_length = linear_distance > 1e-9 ? linear_distance : angular_distance; - trajectory.position.push_back(q_start); - trajectory.velocity.push_back(std::vector(q_start.size(), 0.0)); - trajectory.time.push_back(0.0); - if (path_length <= 1e-9) { - return true; - } - - cmvr::SCurve curve(velocity, acceleration, jerk); - const cmvr::SCurveProfile profile = curve.calculateProfile(0.0, path_length, 0.0, 0.0); - if (profile.total_time <= 0.0) { - return false; - } - - Eigen::VectorXd q_current = toEigenVector(q_start); - Eigen::Vector3d linear_direction = Eigen::Vector3d::Zero(); - if (linear_distance > 1e-9) { - linear_direction = dp / linear_distance; - } - const Eigen::Quaterniond q_start_rot(R_start); - const Eigen::Quaterniond q_target_rot(R_target); - const double position_gain = positiveOr(movel_config_.position_gain(), 4.0); - const double rotation_gain = positiveOr(movel_config_.rotation_gain(), 4.0); - - double previous_time = 0.0; - for (double t = std::min(dt, profile.total_time); - t <= profile.total_time + 1e-9; - t = std::min(t + dt, profile.total_time)) { - const double step_dt = std::max(1e-6, t - previous_time); - previous_time = t; - - const double s = clamp(curve.getPositionAtTime(profile, t), 0.0, path_length); - const double sd = std::max(0.0, curve.getVelocityAtTime(profile, t)); - const double ratio = clamp(s / path_length, 0.0, 1.0); - - const std::vector q_std = toStdVector(q_current); - Eigen::Matrix4d current_pose_base = Eigen::Matrix4d::Identity(); - if (!solver_->fk(q_std, current_pose_base, true)) { - return false; - } - - const Eigen::Vector3d p_current = current_pose_base.block<3, 1>(0, 3); - const Eigen::Vector3d p_desired = p_start + ratio * dp; - Eigen::Matrix target_twist_base = Eigen::Matrix::Zero(); - target_twist_base.head<3>() = - linear_direction * sd + position_gain * (p_desired - p_current); - - if (angular_distance > 1e-9) { - const Eigen::Matrix3d R_current = current_pose_base.block<3, 3>(0, 0); - const Eigen::Matrix3d R_desired = - q_start_rot.slerp(ratio, q_target_rot).toRotationMatrix(); - const Eigen::Vector3d rotation_error = rotationVector(R_desired * R_current.transpose()); - target_twist_base.tail<3>() = - (total_rotation_vector / path_length) * sd + rotation_gain * rotation_error; - } - - Eigen::MatrixXd jacobian_base; - Eigen::Matrix3d base_R_tool; - if (!solver_->computeJacobianBaseAtQ(q_std, true, jacobian_base, base_R_tool)) { - return false; - } - - std::vector qdot_std; - if (!solver_->solveVelocityBase(jacobian_base, - target_twist_base, - q_std, - qdot_std, - std::numeric_limits::infinity())) { - return false; - } - - Eigen::VectorXd qdot = applyJointVelocityLimits_(toEigenVector(qdot_std)); - if (!qd_max.empty()) { - double scale = 1.0; - for (Eigen::Index i = 0; i < qdot.size(); ++i) { - const double limit = std::abs(qd_max[static_cast(i)]); - if (limit <= 0.0 || !std::isfinite(limit)) { - continue; - } - const double value = std::abs(qdot[i]); - if (value > limit) { - scale = std::min(scale, limit / value); - } - } - qdot *= scale; - } - qdot = applyJointSoftLimits_(q_current, qdot); - q_current += qdot * step_dt; - - if (joint_lower_limits_.size() == q_current.size() && - joint_upper_limits_.size() == q_current.size()) { - q_current = q_current.cwiseMax(joint_lower_limits_).cwiseMin(joint_upper_limits_); - } - - trajectory.position.push_back(toStdVector(q_current)); - trajectory.velocity.push_back(toStdVector(qdot)); - trajectory.time.push_back(t); - - if (t >= profile.total_time - 1e-9) { - break; - } - } - - return true; -} - -bool PinocchioDlsCartesianMotionPlanner::speedLStep(const CartesianVelocity& target_velocity, - const double dt, - const std::vector& q_measured, - const std::vector& qd_measured, - std::vector& qd_command, - const FrameType frame) -{ - qd_command.clear(); - if (!solver_ || !speedl_configured_ || dt <= 0.0) { - return false; - } - if (static_cast(q_measured.size()) != solver_->chainDof() || - static_cast(qd_measured.size()) != solver_->chainVelocityDof()) { - return false; - } - - Eigen::Matrix measured_twist_base = Eigen::Matrix::Zero(); - Eigen::MatrixXd jacobian_base; - Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); - if (!solver_->computeTwistBaseAtQ(q_measured, - qd_measured, - true, - measured_twist_base, - &jacobian_base, - &base_R_tool)) { - return false; - } - - const Eigen::Matrix target_twist = common::math::velocityToVector(target_velocity); - if (target_twist.squaredNorm() <= 1e-12) { - twist_limiter_.synchronize(measured_twist_base, dt, true); - } else if (speedl_command_twist_base_.squaredNorm() <= 1e-12) { - twist_limiter_.initialize(Eigen::Matrix::Zero()); - } - twist_limiter_.setTargetTwist(target_twist, common::math::toPlannerFrame(frame)); - speedl_command_twist_base_ = twist_limiter_.update(dt, base_R_tool); - - std::vector qdot_std; - if (!solver_->solveVelocityBase(jacobian_base, - speedl_command_twist_base_, - q_measured, - qdot_std, - std::numeric_limits::infinity())) { - return false; - } - - Eigen::VectorXd qdot = applyJointVelocityLimits_(toEigenVector(qdot_std)); - qdot = applyJointSoftLimits_(toEigenVector(q_measured), qdot); - - Eigen::VectorXd reference = toEigenVector(qd_measured); - if (prev_qdot_command_.size() == qdot.size()) { - reference = toEigenVector(prev_qdot_command_); - } - qdot = applyJointAccelerationLimits_(qdot, reference, dt); - - const Eigen::Matrix achieved_twist_base = jacobian_base * qdot; - const Eigen::Vector3d desired_linear = speedl_command_twist_base_.head<3>(); - const Eigen::Vector3d achieved_linear = achieved_twist_base.head<3>(); - const double desired_linear_norm = desired_linear.norm(); - const double achieved_linear_norm = achieved_linear.norm(); - const double direction_check_min_speed = - std::max(1e-4, positiveOr(speedl_config_.linear_reverse_switch_speed_threshold(), 1e-3)); - if (desired_linear_norm > direction_check_min_speed) { - const double linear_min_speed_ratio = - clamp(positiveOr(speedl_config_.linear_min_speed_ratio(), 0.2), 0.0, 1.0); - const double speed_ratio = achieved_linear_norm / desired_linear_norm; - if (speed_ratio < linear_min_speed_ratio) { - return false; - } - if (achieved_linear_norm > direction_check_min_speed) { - const double deviation_deg = directionDeviationDeg(desired_linear, achieved_linear); - const double severe_direction_deviation_deg = - positiveOr(speedl_config_.severe_direction_deviation_deg(), 45.0); - if (deviation_deg >= severe_direction_deviation_deg) { - return false; - } - } - } - - qd_command = toStdVector(qdot); - prev_qdot_command_ = qd_command; - return true; -} - -bool PinocchioDlsCartesianMotionPlanner::updateSpeedLAcceleration(const double acceleration) -{ - if (!speedl_configured_ || acceleration <= 0.0) { - return false; - } - if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9) { - return true; - } - - cartesian_motion::updateTwistLimiterAcceleration( - twist_limiter_, - speedl_config_, - acceleration); - speedl_applied_acceleration_ = acceleration; - return true; -} - -CartesianVelocity PinocchioDlsCartesianMotionPlanner::getSpeedLCommandTwistBase() const -{ - return common::math::vectorToVelocity(speedl_command_twist_base_); -} - -} // namespace cmvr::device diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h deleted file mode 100644 index 1d0d2d6e..00000000 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h +++ /dev/null @@ -1,76 +0,0 @@ -#ifndef CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H -#define CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H - -#include - -#include -#include - -#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h" -#include "../../cartesian_motion_planner.h" -#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h" -#include "common/math/qp_solver.h" - -namespace cmvr::device { - -class PinocchioQpCartesianMotionPlanner final : public CartesianMotionPlanner { -public: - explicit PinocchioQpCartesianMotionPlanner(std::shared_ptr solver); - - bool configureSpeedL(const config::SpeedLPlannerConfig& config, - std::size_t dof) override; - bool configureMoveL(const config::MoveLPlannerConfig& config) override; - - bool planMoveL(const CartesianPose& target, - const std::vector& q_start, - const std::vector& qd_max, - double velocity, - double acceleration, - double jerk, - FrameType frame, - CartesianJointTrajectory& trajectory) override; - - bool speedLStep(const CartesianVelocity& target_velocity, - double dt, - const std::vector& q_measured, - const std::vector& qd_measured, - std::vector& qd_command, - FrameType frame) override; - - bool updateSpeedLAcceleration(double acceleration) override; - CartesianVelocity getSpeedLCommandTwistBase() const override; - -private: - bool refreshJointLimits_(const config::CartesianVelocityQpConfig& config); - bool configureQpSolver_(Eigen::Index dof, double solver_eps); - bool solveVelocityQp_(const Eigen::MatrixXd& jacobian_base, - const Eigen::Matrix& target_twist_base, - const Eigen::VectorXd& q_measured, - const Eigen::VectorXd& qd_reference, - double dt, - const std::vector& qd_max, - bool enforce_acceleration_limits, - const config::CartesianVelocityQpConfig& qp_config, - Eigen::VectorXd& qdot); - bool validateAchievedLinearTwist_(const Eigen::MatrixXd& jacobian_base, - const Eigen::VectorXd& qdot) const; - - std::shared_ptr solver_{nullptr}; - config::MoveLPlannerConfig movel_config_{}; - config::SpeedLPlannerConfig speedl_config_{}; - cmvr::CartesianTwistLimiter twist_limiter_{}; - cmvr::QPSolver qp_solver_; - int qp_solver_dof_{0}; - double qp_solver_eps_{0.0}; - Eigen::VectorXd joint_lower_limits_; - Eigen::VectorXd joint_upper_limits_; - Eigen::VectorXd joint_velocity_limits_; - std::vector prev_qdot_command_; - Eigen::Matrix speedl_command_twist_base_{Eigen::Matrix::Zero()}; - double speedl_applied_acceleration_{0.25}; - bool speedl_configured_{false}; -}; - -} // namespace cmvr::device - -#endif // CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/src/pinocchio_qp_cartesian_motion_planner.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/src/pinocchio_qp_cartesian_motion_planner.cpp deleted file mode 100644 index 9ed250c2..00000000 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/src/pinocchio_qp_cartesian_motion_planner.cpp +++ /dev/null @@ -1,590 +0,0 @@ -#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h" - -#include -#include -#include -#include -#include -#include - -#include "algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h" -#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve.h" -#include "common/base/logging/logger.h" -#include "common/math/cartesian_motion_math.h" -#include "common/math/joint_limits.h" -#include "common/config/config_files.h" -#include "common/math/proto_geometry.h" -#include "common/math/transform_math.h" - -namespace cmvr::device { - -using cmvr::common::config::positiveOr; -using cmvr::device::cartesian_motion::clamp; -using cmvr::device::cartesian_motion::directionDeviationDeg; -using cmvr::device::cartesian_motion::rotationVector; -using cmvr::device::cartesian_motion::toEigenVector; -using cmvr::device::cartesian_motion::toStdVector; - -namespace { - -const config::CartesianVelocityQpConfig& qpConfigOrDefault( - const config::CartesianVelocityQpConfig& config) -{ - static const config::CartesianVelocityQpConfig defaults; - return config.ByteSizeLong() > 0 ? config : defaults; -} - -Eigen::Matrix twistTrackingWeightOrDefault( - const cmvr::common::Vec6& value) -{ - Eigen::Matrix defaults; - defaults << 1.0, 1.0, 1.0, 0.5, 0.5, 0.5; - Eigen::Matrix weight = - cmvr::common::math::toEigenVec6(value, defaults); - for (int i = 0; i < weight.size(); ++i) { - if (!std::isfinite(weight[i]) || weight[i] <= 0.0) { - weight[i] = defaults[i]; - } - } - return weight; -} - -} // namespace - -PinocchioQpCartesianMotionPlanner::PinocchioQpCartesianMotionPlanner( - std::shared_ptr solver) - : solver_(std::move(solver)) -{ -} - -bool PinocchioQpCartesianMotionPlanner::refreshJointLimits_( - const config::CartesianVelocityQpConfig& config) -{ - if (!solver_) { - return false; - } - const auto source = config.has_joint_limits() - ? config.joint_limits().source() - : config::JOINT_LIMIT_SOURCE_URDF; - if (source == config::JOINT_LIMIT_SOURCE_CUSTOM) { - std::vector joint_names; - if (!solver_->getChainJointNames(joint_names) || joint_names.empty()) { - return false; - } - std::unordered_map custom_limits; - if (config.has_joint_limits()) { - custom_limits.reserve( - static_cast(config.joint_limits().joints_size())); - for (const auto& item : config.joint_limits().joints()) { - if (!item.joint_name().empty()) { - custom_limits[item.joint_name()] = item; - } - } - } - - const auto dof = static_cast(joint_names.size()); - joint_lower_limits_.resize(dof); - joint_upper_limits_.resize(dof); - joint_velocity_limits_.resize(dof); - for (Eigen::Index i = 0; i < dof; ++i) { - const auto it = custom_limits.find(joint_names[static_cast(i)]); - if (it == custom_limits.end()) { - CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] missing custom joint limit for " - << joint_names[static_cast(i)]; - return false; - } - const auto& limit = it->second; - if (!std::isfinite(limit.q_lb()) || !std::isfinite(limit.q_ub()) || - !std::isfinite(limit.qd()) || limit.q_ub() <= limit.q_lb() || - limit.qd() <= 0.0) { - CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] invalid custom joint limit for " - << limit.joint_name(); - return false; - } - joint_lower_limits_[i] = limit.q_lb(); - joint_upper_limits_[i] = limit.q_ub(); - joint_velocity_limits_[i] = std::abs(limit.qd()); - } - return true; - } - - if (!solver_->getJointPositionLimits(joint_lower_limits_, joint_upper_limits_)) { - return false; - } - if (!solver_->getJointVelocityLimits(joint_velocity_limits_)) { - return false; - } - return true; -} - -bool PinocchioQpCartesianMotionPlanner::configureQpSolver_(const Eigen::Index dof, - const double solver_eps) -{ - if (dof <= 0) { - return false; - } - const double eps = solver_eps > 0.0 ? solver_eps : 1e-3; - if (qp_solver_dof_ != static_cast(dof) || std::abs(qp_solver_eps_ - eps) > 1e-12) { - qp_solver_.Setup(static_cast(dof), static_cast(dof), eps); - qp_solver_.ResetIsFirst(); - qp_solver_dof_ = static_cast(dof); - qp_solver_eps_ = eps; - } - return true; -} - -bool PinocchioQpCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlannerConfig& config, - const std::size_t dof) -{ - if (!solver_ || dof == 0) { - return false; - } - const auto solver_dof = static_cast(std::max(0, solver_->chainVelocityDof())); - if (solver_dof != 0 && solver_dof != dof) { - return false; - } - speedl_config_ = config; - - const auto& qp_config = qpConfigOrDefault(speedl_config_.qp()); - if (!refreshJointLimits_(qp_config)) { - return false; - } - - cartesian_motion::configureTwistLimiterFromSpeedLConfig(twist_limiter_, speedl_config_); - - prev_qdot_command_.assign(dof, 0.0); - speedl_command_twist_base_.setZero(); - speedl_applied_acceleration_ = positiveOr(speedl_config_.linear_acceleration_max(), 5.0); - if (!configureQpSolver_(static_cast(dof), - positiveOr(qp_config.solver_eps(), 1e-3))) { - return false; - } - speedl_configured_ = true; - return true; -} - -bool PinocchioQpCartesianMotionPlanner::configureMoveL( - const config::MoveLPlannerConfig& config) -{ - if (!std::isfinite(config.sample_period_s()) || - !std::isfinite(config.position_gain()) || - !std::isfinite(config.rotation_gain())) { - return false; - } - movel_config_ = config; - return true; -} - -bool PinocchioQpCartesianMotionPlanner::planMoveL(const CartesianPose& target, - const std::vector& q_start, - const std::vector& qd_max, - const double velocity, - const double acceleration, - const double jerk, - const FrameType frame, - CartesianJointTrajectory& trajectory) -{ - trajectory = {}; - const double dt = positiveOr(movel_config_.sample_period_s(), 0.001); - if (!solver_ || q_start.empty() || - velocity <= 0.0 || acceleration <= 0.0 || jerk <= 0.0) { - return false; - } - if (static_cast(q_start.size()) != solver_->chainDof()) { - return false; - } - if (!qd_max.empty() && qd_max.size() != q_start.size()) { - return false; - } - const auto& qp_config = qpConfigOrDefault(movel_config_.qp()); - if (!refreshJointLimits_(qp_config)) { - return false; - } - - Eigen::Matrix4d start_pose_base = Eigen::Matrix4d::Identity(); - if (!solver_->fk(q_start, start_pose_base, true)) { - return false; - } - - const Eigen::Matrix4d target_pose_input = common::math::poseToMatrix(target); - const Eigen::Matrix4d target_pose_base = - frame == FrameType::Tool ? start_pose_base * target_pose_input : target_pose_input; - - const Eigen::Vector3d p_start = start_pose_base.block<3, 1>(0, 3); - const Eigen::Vector3d p_target = target_pose_base.block<3, 1>(0, 3); - const Eigen::Vector3d dp = p_target - p_start; - const double linear_distance = dp.norm(); - - const Eigen::Matrix3d R_start = start_pose_base.block<3, 3>(0, 0); - const Eigen::Matrix3d R_target = target_pose_base.block<3, 3>(0, 0); - const Eigen::Vector3d total_rotation_vector = rotationVector(R_target * R_start.transpose()); - const double angular_distance = total_rotation_vector.norm(); - - const double path_length = linear_distance > 1e-9 ? linear_distance : angular_distance; - trajectory.position.push_back(q_start); - trajectory.velocity.push_back(std::vector(q_start.size(), 0.0)); - trajectory.time.push_back(0.0); - if (path_length <= 1e-9) { - return true; - } - - cmvr::SCurve curve(velocity, acceleration, jerk); - const cmvr::SCurveProfile profile = curve.calculateProfile(0.0, path_length, 0.0, 0.0); - if (profile.total_time <= 0.0) { - return false; - } - - Eigen::VectorXd q_current = toEigenVector(q_start); - Eigen::VectorXd qdot_previous = Eigen::VectorXd::Zero(static_cast(q_start.size())); - Eigen::Vector3d linear_direction = Eigen::Vector3d::Zero(); - if (linear_distance > 1e-9) { - linear_direction = dp / linear_distance; - } - const Eigen::Quaterniond q_start_rot(R_start); - const Eigen::Quaterniond q_target_rot(R_target); - const double position_gain = positiveOr(movel_config_.position_gain(), 4.0); - const double rotation_gain = positiveOr(movel_config_.rotation_gain(), 4.0); - - qp_solver_.ResetIsFirst(); - double previous_time = 0.0; - for (double t = std::min(dt, profile.total_time); - t <= profile.total_time + 1e-9; - t = std::min(t + dt, profile.total_time)) { - const double step_dt = std::max(1e-6, t - previous_time); - previous_time = t; - - const double s = clamp(curve.getPositionAtTime(profile, t), 0.0, path_length); - const double sd = std::max(0.0, curve.getVelocityAtTime(profile, t)); - const double ratio = clamp(s / path_length, 0.0, 1.0); - - const std::vector q_std = toStdVector(q_current); - Eigen::Matrix4d current_pose_base = Eigen::Matrix4d::Identity(); - if (!solver_->fk(q_std, current_pose_base, true)) { - return false; - } - - const Eigen::Vector3d p_current = current_pose_base.block<3, 1>(0, 3); - const Eigen::Vector3d p_desired = p_start + ratio * dp; - Eigen::Matrix target_twist_base = Eigen::Matrix::Zero(); - target_twist_base.head<3>() = - linear_direction * sd + position_gain * (p_desired - p_current); - - if (angular_distance > 1e-9) { - const Eigen::Matrix3d R_current = current_pose_base.block<3, 3>(0, 0); - const Eigen::Matrix3d R_desired = - q_start_rot.slerp(ratio, q_target_rot).toRotationMatrix(); - const Eigen::Vector3d rotation_error = - rotationVector(R_desired * R_current.transpose()); - target_twist_base.tail<3>() = - (total_rotation_vector / path_length) * sd + rotation_gain * rotation_error; - } - - Eigen::MatrixXd jacobian_base; - Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); - if (!solver_->computeJacobianBaseAtQ(q_std, true, jacobian_base, base_R_tool)) { - return false; - } - - Eigen::VectorXd qdot; - if (!solveVelocityQp_(jacobian_base, - target_twist_base, - q_current, - qdot_previous, - step_dt, - qd_max, - false, - qp_config, - qdot)) { - return false; - } - - q_current += qdot * step_dt; - if (joint_lower_limits_.size() == q_current.size() && - joint_upper_limits_.size() == q_current.size()) { - q_current = q_current.cwiseMax(joint_lower_limits_).cwiseMin(joint_upper_limits_); - } - - trajectory.position.push_back(toStdVector(q_current)); - trajectory.velocity.push_back(toStdVector(qdot)); - trajectory.time.push_back(t); - qdot_previous = qdot; - - if (t >= profile.total_time - 1e-9) { - break; - } - } - - return true; -} - -bool PinocchioQpCartesianMotionPlanner::solveVelocityQp_( - const Eigen::MatrixXd& jacobian_base, - const Eigen::Matrix& target_twist_base, - const Eigen::VectorXd& q_measured, - const Eigen::VectorXd& qd_reference, - const double dt, - const std::vector& qd_max, - const bool enforce_acceleration_limits, - const config::CartesianVelocityQpConfig& qp_config, - Eigen::VectorXd& qdot) -{ - const Eigen::Index dof = q_measured.size(); - if (jacobian_base.rows() != 6 || jacobian_base.cols() != dof || - qd_reference.size() != dof || dt <= 0.0) { - return false; - } - - if (!configureQpSolver_(dof, positiveOr(qp_config.solver_eps(), 1e-3))) { - return false; - } - - const Eigen::Matrix twist_weight = - twistTrackingWeightOrDefault(qp_config.twist_tracking_weight()); - Eigen::Matrix task_weight = Eigen::Matrix::Identity(); - for (int i = 0; i < 6; ++i) { - task_weight(i, i) = twist_weight[i]; - } - const double qdot_regularization = - positiveOr(qp_config.qdot_regularization(), 1e-4); - const double prev_qdot_regularization = - positiveOr(qp_config.prev_qdot_regularization(), - enforce_acceleration_limits ? 2e-2 : 1e-4); - const bool use_joint_limit_avoidance = - qp_config.has_joint_limit_avoidance() && - qp_config.joint_limit_avoidance().enable() && - qp_config.joint_limit_avoidance().weight() > 0.0; - const int avoidance_rows = use_joint_limit_avoidance ? static_cast(dof) : 0; - - Eigen::MatrixXd cost(6 + 2 * dof + avoidance_rows, dof); - Eigen::VectorXd target(6 + 2 * dof + avoidance_rows); - cost.topRows(6) = task_weight * jacobian_base; - target.head(6) = task_weight * target_twist_base; - cost.middleRows(6, dof) = std::sqrt(qdot_regularization) * Eigen::MatrixXd::Identity(dof, dof); - target.segment(6, dof).setZero(); - cost.middleRows(6 + dof, dof) = - std::sqrt(prev_qdot_regularization) * Eigen::MatrixXd::Identity(dof, dof); - target.segment(6 + dof, dof) = std::sqrt(prev_qdot_regularization) * qd_reference; - if (use_joint_limit_avoidance) { - const auto& avoidance = qp_config.joint_limit_avoidance(); - const Eigen::VectorXd qdot_avoid = - cmvr::kinematics::computeJointLimitAvoidanceVelocity( - q_measured, - joint_lower_limits_, - joint_upper_limits_, - true, - positiveOr(avoidance.gain(), 0.2), - positiveOr(avoidance.margin_ratio(), 0.15), - positiveOr(avoidance.max_push(), 0.25)); - const double sqrt_weight = std::sqrt( - positiveOr(qp_config.joint_limit_avoidance().weight(), 0.05)); - cost.middleRows(6 + 2 * dof, dof) = - sqrt_weight * Eigen::MatrixXd::Identity(dof, dof); - target.segment(6 + 2 * dof, dof) = sqrt_weight * qdot_avoid; - } - - Eigen::VectorXd lower(dof); - Eigen::VectorXd upper(dof); - for (Eigen::Index i = 0; i < dof; ++i) { - double velocity_limit = std::numeric_limits::infinity(); - if (joint_velocity_limits_.size() == dof && joint_velocity_limits_[i] > 0.0) { - velocity_limit = std::abs(joint_velocity_limits_[i]); - } - if (qd_max.size() == static_cast(dof)) { - const double requested_limit = std::abs(qd_max[static_cast(i)]); - if (std::isfinite(requested_limit) && requested_limit > 0.0) { - velocity_limit = std::min(velocity_limit, requested_limit); - } - } - - double lb = -velocity_limit; - double ub = velocity_limit; - if (enforce_acceleration_limits) { - double acc_limit = 8.0; - if (i < speedl_config_.joint_acceleration_max_size() && - speedl_config_.joint_acceleration_max(static_cast(i)) > 0.0) { - acc_limit = speedl_config_.joint_acceleration_max(static_cast(i)); - } - lb = std::max(lb, qd_reference[i] - acc_limit * dt); - ub = std::min(ub, qd_reference[i] + acc_limit * dt); - } - - if (joint_lower_limits_.size() == dof && joint_upper_limits_.size() == dof) { - lb = std::max(lb, (joint_lower_limits_[i] - q_measured[i]) / dt); - ub = std::min(ub, (joint_upper_limits_[i] - q_measured[i]) / dt); - } - - if (lb > ub) { - CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] velocity bound infeasible at joint " - << i << ": lb=" << lb << ", ub=" << ub - << ", q=" << q_measured[i] - << ", qd_ref=" << qd_reference[i] - << ", dt=" << dt; - return false; - } - lower[i] = lb; - upper[i] = ub; - } - - qp_solver_.SetCostFunction(cost, target); - qp_solver_.SetConstraintsFunction(Eigen::MatrixXd::Identity(dof, dof), lower, upper); - qp_solver_.SetPrimalVariable(qd_reference); - - try { - qdot = qp_solver_.Solve(); - } catch (const cmvr::QPSolverException& error) { - CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] QP failed: " - << error.what() << " (code=" << error.code() << ")"; - return false; - } catch (const std::exception& error) { - CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] QP failed: " - << error.what(); - return false; - } - return qdot.size() == dof; -} - -bool PinocchioQpCartesianMotionPlanner::validateAchievedLinearTwist_( - const Eigen::MatrixXd& jacobian_base, - const Eigen::VectorXd& qdot) const -{ - const Eigen::Matrix achieved_twist_base = jacobian_base * qdot; - const Eigen::Vector3d desired_linear = speedl_command_twist_base_.head<3>(); - const Eigen::Vector3d achieved_linear = achieved_twist_base.head<3>(); - const double desired_linear_norm = desired_linear.norm(); - const double achieved_linear_norm = achieved_linear.norm(); - const double direction_check_min_speed = - std::max(1e-4, positiveOr(speedl_config_.linear_reverse_switch_speed_threshold(), 1e-3)); - if (desired_linear_norm <= direction_check_min_speed) { - return true; - } - - const double linear_min_speed_ratio = - clamp(positiveOr(speedl_config_.linear_min_speed_ratio(), 0.2), 0.0, 1.0); - const double speed_ratio = achieved_linear_norm / desired_linear_norm; - if (speed_ratio < linear_min_speed_ratio) { - CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] achieved speed too low: desired_linear=[" - << desired_linear.x() << ", " << desired_linear.y() << ", " << desired_linear.z() - << "], achieved_linear=[" << achieved_linear.x() << ", " - << achieved_linear.y() << ", " << achieved_linear.z() - << "], desired_norm=" << desired_linear_norm - << ", achieved_norm=" << achieved_linear_norm - << ", speed_ratio=" << speed_ratio - << ", min_ratio=" << linear_min_speed_ratio - << ", direction_check_min_speed=" << direction_check_min_speed; - return false; - } - if (achieved_linear_norm <= direction_check_min_speed) { - return true; - } - const double deviation_deg = directionDeviationDeg(desired_linear, achieved_linear); - const double severe_direction_deviation_deg = - positiveOr(speedl_config_.severe_direction_deviation_deg(), 45.0); - if (deviation_deg >= severe_direction_deviation_deg) { - CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] direction deviation too large: desired_linear=[" - << desired_linear.x() << ", " << desired_linear.y() << ", " << desired_linear.z() - << "], achieved_linear=[" << achieved_linear.x() << ", " - << achieved_linear.y() << ", " << achieved_linear.z() - << "], deviation_deg=" << deviation_deg - << ", severe_threshold_deg=" << severe_direction_deviation_deg - << ", direction_check_min_speed=" << direction_check_min_speed; - return false; - } - return true; -} - -bool PinocchioQpCartesianMotionPlanner::speedLStep(const CartesianVelocity& target_velocity, - const double dt, - const std::vector& q_measured, - const std::vector& qd_measured, - std::vector& qd_command, - const FrameType frame) -{ - qd_command.clear(); - if (!solver_ || !speedl_configured_ || dt <= 0.0) { - CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] invalid state: solver=" - << (solver_ ? 1 : 0) - << ", configured=" << (speedl_configured_ ? 1 : 0) - << ", dt=" << dt; - return false; - } - if (static_cast(q_measured.size()) != solver_->chainDof() || - static_cast(qd_measured.size()) != solver_->chainVelocityDof()) { - CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] state size mismatch: q=" - << q_measured.size() << "/" << solver_->chainDof() - << ", qd=" << qd_measured.size() << "/" << solver_->chainVelocityDof(); - return false; - } - - Eigen::Matrix measured_twist_base = Eigen::Matrix::Zero(); - Eigen::MatrixXd jacobian_base; - Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); - if (!solver_->computeTwistBaseAtQ(q_measured, - qd_measured, - true, - measured_twist_base, - &jacobian_base, - &base_R_tool)) { - CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] computeTwistBaseAtQ failed"; - return false; - } - - const Eigen::Matrix target_twist = common::math::velocityToVector(target_velocity); - if (target_twist.squaredNorm() <= 1e-12) { - twist_limiter_.synchronize(measured_twist_base, dt, true); - } else if (speedl_command_twist_base_.squaredNorm() <= 1e-12) { - twist_limiter_.initialize(Eigen::Matrix::Zero()); - } - twist_limiter_.setTargetTwist(target_twist, common::math::toPlannerFrame(frame)); - speedl_command_twist_base_ = twist_limiter_.update(dt, base_R_tool); - - Eigen::VectorXd reference = toEigenVector(qd_measured); - if (prev_qdot_command_.size() == q_measured.size()) { - reference = toEigenVector(prev_qdot_command_); - } - - Eigen::VectorXd qdot; - if (!solveVelocityQp_(jacobian_base, - speedl_command_twist_base_, - toEigenVector(q_measured), - reference, - dt, - {}, - true, - qpConfigOrDefault(speedl_config_.qp()), - qdot)) { - CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] solveVelocityQp failed"; - return false; - } - if (!validateAchievedLinearTwist_(jacobian_base, qdot)) { - CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner][speedL] validateAchievedLinearTwist failed"; - return false; - } - - qd_command = toStdVector(qdot); - prev_qdot_command_ = qd_command; - return true; -} - -bool PinocchioQpCartesianMotionPlanner::updateSpeedLAcceleration(const double acceleration) -{ - if (!speedl_configured_ || acceleration <= 0.0) { - return false; - } - if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9) { - return true; - } - - cartesian_motion::updateTwistLimiterAcceleration( - twist_limiter_, - speedl_config_, - acceleration); - speedl_applied_acceleration_ = acceleration; - return true; -} - -CartesianVelocity PinocchioQpCartesianMotionPlanner::getSpeedLCommandTwistBase() const -{ - return common::math::vectorToVelocity(speedl_command_twist_base_); -} - -} // namespace cmvr::device diff --git a/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt b/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt index 52df3e28..bd84d4b9 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt +++ b/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt @@ -20,46 +20,4 @@ target_link_libraries(base_motion PUBLIC ) add_library(cmvr_es::base_motion ALIAS base_motion) -install(TARGETS base_motion LIBRARY DESTINATION lib) - - -# -------------------------------------------------------- -# Unit test -# -------------------------------------------------------- - - - - -add_executable(toppra_joint_trajectory_planner_test - ${CMAKE_CURRENT_SOURCE_DIR}/joint_trajectory/toppra/src/toppra_joint_trajectory_planner_test.cpp -) - - -target_link_libraries(toppra_joint_trajectory_planner_test - PRIVATE - cmvr_es::base_motion - gtest - gtest_main - pthread - glog - cmvr_es::proto - ccd - fcl - OsqpEigen - -) - -add_executable(cartesian_twist_limiter_test - ${CMAKE_CURRENT_SOURCE_DIR}/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter_test.cpp -) - -target_link_libraries(cartesian_twist_limiter_test - PRIVATE - cmvr_es::base_motion - gtest - gtest_main - pthread - glog - cmvr_es::proto - matplot -) +install(TARGETS base_motion LIBRARY DESTINATION lib) \ No newline at end of file diff --git a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter_test.cpp b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter_test.cpp deleted file mode 100644 index f40d07ca..00000000 --- a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter_test.cpp +++ /dev/null @@ -1,732 +0,0 @@ -// -// Created by lgv on 2026/3/10. -// - -#include "gtest/gtest.h" - -#include -#include - -#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h" - -namespace cmvr -{ -namespace -{ - -using Twist = CartesianTwistLimiter::Twist; - -constexpr double kDt = 0.01; -constexpr double kComponentEps = 1e-8; -constexpr double kDirectionEps = 1e-6; -constexpr double kSwitchSpeedUpperBound = 0.05; - -Twist makeTwist(double vx, double vy, double vz, double wx, double wy, double wz) -{ - Twist twist = Twist::Zero(); - twist << vx, vy, vz, wx, wy, wz; - return twist; -} - -Eigen::Vector3d normalizedOrZero(const Eigen::Vector3d& value) -{ - const double norm = value.norm(); - if (norm <= kComponentEps) { - return Eigen::Vector3d::Zero(); - } - return value / norm; -} - -CartesianTwistLimiter makeLimiter() -{ - CartesianTwistLimiter limiter; - limiter.setLinearConstraints(1.0, 1.0, 5.0); - limiter.setAngularConstraints(1.0, 1.0, 5.0); - limiter.initialize(); - return limiter; -} - -Twist linearTargetAtTime(double t) -{ - if (t < 1.0) { - return makeTwist(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - } - if (t < 3.5) { - return makeTwist(0.8, 0.0, 0.0, 0.0, 0.0, 0.0); - } - if (t < 6.0) { - return makeTwist(-0.8, 0.6, 0.2, 0.0, 0.0, 0.0); - } - if (t < 8.5) { - return makeTwist(-0.7, 0.0, 0.0, 0.0, 0.0, 0.0); - } - return makeTwist(-0.7, 0.0, 0.0, 0.0, 0.0, 0.0); -} - -Twist angularTargetAtTime(double t) -{ - if (t < 1.0) { - return makeTwist(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - } - if (t < 3.5) { - return makeTwist(0.0, 0.0, 0.0, 0.8, 0.0, 0.0); - } - if (t < 6.0) { - return makeTwist(0.0, 0.0, 0.0, -0.8, 0, 0); - } - if (t < 8.5) { - return makeTwist(0.0, 0.0, 0.0, -0.6, 0.0, 0.0); - } - return makeTwist(0.0, 0.0, 0.0, -0.6, 0.0, 0.0); -} - -Twist synchronizeTargetAtTime(double t) -{ - if (t < 1.0) { - return makeTwist(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - } - if (t < 3.5) { - return makeTwist(0.8, 0.0, 0.0, 0.0, 0.0, 0.0); - } - if (t < 6.0) { - return makeTwist(0.0, 0.7, 0.0, 0.0, 0.0, 0.0); - } - if (t < 8.0) { - return makeTwist(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - } - return makeTwist(-0.6, 0.0, 0.0, 0.0, 0.0, 0.0); -} - -} // namespace - -TEST(CARTESIAN_TWIST_LIMITER_TEST, LinearDirectionRemainsLockedForFixedTarget) -{ - CartesianTwistLimiter limiter = makeLimiter(); - const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); - - const Eigen::Vector3d expected_dir = normalizedOrZero(Eigen::Vector3d(0.8, 0.6, 0.0)); - limiter.setTargetTwist(makeTwist(0.8, 0.6, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base); - - double max_speed = 0.0; - for (int i = 0; i < 400; ++i) { - const Twist twist = limiter.update(kDt, base_R_tool); - const Eigen::Vector3d linear = twist.head<3>(); - max_speed = std::max(max_speed, linear.norm()); - - if (linear.norm() > kComponentEps) { - const Eigen::Vector3d dir = linear.normalized(); - EXPECT_LT(dir.cross(expected_dir).norm(), kDirectionEps) << "step=" << i; - } - } - - EXPECT_GT(max_speed, 0.5); -} - -TEST(CARTESIAN_TWIST_LIMITER_TEST, LinearReverseStopsBeforeSwitchingDirection) -{ - CartesianTwistLimiter limiter = makeLimiter(); - const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); - - limiter.setTargetTwist(makeTwist(0.8, 0.0, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base); - - double speed_before_switch = 0.0; - for (int i = 0; i < 200; ++i) { - const Twist twist = limiter.update(kDt, base_R_tool); - if (twist.head<3>().norm() > 0.2) { - speed_before_switch = twist.head<3>().norm(); - break; - } - } - ASSERT_GT(speed_before_switch, 0.2); - - limiter.setTargetTwist(makeTwist(-0.8, 0.0, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base); - - bool negative_x_seen = false; - double speed_when_negative_x_appears = 0.0; - for (int i = 0; i < 500; ++i) { - const Twist twist = limiter.update(kDt, base_R_tool); - EXPECT_NEAR(twist.y(), 0.0, kComponentEps); - EXPECT_NEAR(twist.z(), 0.0, kComponentEps); - - if (!negative_x_seen && twist.x() < -kComponentEps) { - negative_x_seen = true; - speed_when_negative_x_appears = twist.head<3>().norm(); - break; - } - - if (!negative_x_seen) { - EXPECT_GE(twist.x(), -kComponentEps); - } - } - - ASSERT_TRUE(negative_x_seen); - EXPECT_LT(speed_when_negative_x_appears, kSwitchSpeedUpperBound); -} - -TEST(CARTESIAN_TWIST_LIMITER_TEST, LinearNonCollinearSwitchStopsBeforeChangingAxis) -{ - CartesianTwistLimiter limiter = makeLimiter(); - const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); - - limiter.setTargetTwist(makeTwist(0.8, 0.0, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base); - - for (int i = 0; i < 200; ++i) { - if (limiter.update(kDt, base_R_tool).head<3>().norm() > 0.2) { - break; - } - } - - limiter.setTargetTwist(makeTwist(0.0, 0.8, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base); - - bool y_seen = false; - double speed_when_y_appears = 0.0; - for (int i = 0; i < 500; ++i) { - const Twist twist = limiter.update(kDt, base_R_tool); - EXPECT_NEAR(twist.z(), 0.0, kComponentEps); - - if (!y_seen && std::abs(twist.y()) > kComponentEps) { - y_seen = true; - speed_when_y_appears = twist.head<3>().norm(); - break; - } - - if (!y_seen) { - EXPECT_NEAR(twist.y(), 0.0, kComponentEps); - EXPECT_GE(twist.x(), -kComponentEps); - } - } - - ASSERT_TRUE(y_seen); - EXPECT_LT(speed_when_y_appears, kSwitchSpeedUpperBound); -} - -TEST(CARTESIAN_TWIST_LIMITER_TEST, AngularNonCollinearSwitchStopsBeforeChangingAxis) -{ - CartesianTwistLimiter limiter = makeLimiter(); - const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); - - limiter.setTargetTwist(makeTwist(0.0, 0.0, 0.0, 0.8, 0.0, 0.0), CartesianFrame::Base); - - for (int i = 0; i < 200; ++i) { - if (limiter.update(kDt, base_R_tool).tail<3>().norm() > 0.2) { - break; - } - } - - limiter.setTargetTwist(makeTwist(0.0, 0.0, 0.0, 0.0, 0.0, 0.8), CartesianFrame::Base); - - bool z_seen = false; - double speed_when_z_appears = 0.0; - for (int i = 0; i < 500; ++i) { - const Twist twist = limiter.update(kDt, base_R_tool); - EXPECT_NEAR(twist(4), 0.0, kComponentEps); - - if (!z_seen && std::abs(twist(5)) > kComponentEps) { - z_seen = true; - speed_when_z_appears = twist.tail<3>().norm(); - break; - } - - if (!z_seen) { - EXPECT_NEAR(twist(5), 0.0, kComponentEps); - EXPECT_GE(twist(3), -kComponentEps); - } - } - - ASSERT_TRUE(z_seen); - EXPECT_LT(speed_when_z_appears, kSwitchSpeedUpperBound); -} - -TEST(CARTESIAN_TWIST_LIMITER_TEST, StopKeepsCurrentLinearDirection) -{ - CartesianTwistLimiter limiter = makeLimiter(); - const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); - - limiter.setTargetTwist(makeTwist(0.8, 0.6, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base); - - Eigen::Vector3d moving_dir = Eigen::Vector3d::Zero(); - for (int i = 0; i < 300; ++i) { - const Twist twist = limiter.update(kDt, base_R_tool); - if (twist.head<3>().norm() > 0.2) { - moving_dir = twist.head<3>().normalized(); - break; - } - } - ASSERT_GT(moving_dir.norm(), 0.5); - - limiter.stop(); - - double final_speed = 0.0; - for (int i = 0; i < 400; ++i) { - const Twist twist = limiter.update(kDt, base_R_tool); - final_speed = twist.head<3>().norm(); - if (final_speed > kComponentEps) { - EXPECT_LT(twist.head<3>().normalized().cross(moving_dir).norm(), kDirectionEps) - << "step=" << i; - } - } - - EXPECT_LT(final_speed, 1e-4); -} - -TEST(CARTESIAN_TWIST_LIMITER_TEST, SynchronizeWithoutKeepingTargetClearsOldCommand) -{ - CartesianTwistLimiter limiter = makeLimiter(); - const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); - - limiter.setTargetTwist(makeTwist(0.8, 0.0, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base); - for (int i = 0; i < 80; ++i) { - limiter.update(kDt, base_R_tool); - } - - limiter.synchronize(Twist::Zero(), kDt, false); - - EXPECT_NEAR(limiter.getTargetTwistBase().norm(), 0.0, kComponentEps); - - for (int i = 0; i < 50; ++i) { - const Twist twist = limiter.update(kDt, base_R_tool); - EXPECT_NEAR(twist.norm(), 0.0, kComponentEps) << "step=" << i; - } - - EXPECT_FALSE(limiter.isMoving()); -} - -TEST(CARTESIAN_TWIST_LIMITER_TEST, SynchronizeKeepingTargetContinuesTowardExistingCommand) -{ - CartesianTwistLimiter limiter = makeLimiter(); - const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); - - limiter.setTargetTwist(makeTwist(0.8, 0.0, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base); - for (int i = 0; i < 120; ++i) { - limiter.update(kDt, base_R_tool); - } - - ASSERT_GT(limiter.getTwistBase().x(), 0.2); - - const Twist measured_twist = makeTwist(0.15, 0.0, 0.0, 0.0, 0.0, 0.0); - limiter.synchronize(measured_twist, kDt, true); - - EXPECT_NEAR(limiter.getTwistBase().x(), 0.15, kComponentEps); - EXPECT_NEAR(limiter.getTwistBase().y(), 0.0, kComponentEps); - EXPECT_NEAR(limiter.getTwistBase().z(), 0.0, kComponentEps); - - double max_resumed_speed = measured_twist.x(); - for (int i = 0; i < 200; ++i) { - const Twist twist = limiter.update(kDt, base_R_tool); - EXPECT_NEAR(twist.y(), 0.0, kComponentEps); - EXPECT_NEAR(twist.z(), 0.0, kComponentEps); - max_resumed_speed = std::max(max_resumed_speed, twist.head<3>().norm()); - if (max_resumed_speed > 0.4) { - break; - } - } - - EXPECT_GT(max_resumed_speed, 0.4); - EXPECT_GT(limiter.getTargetTwistBase().x(), 0.7); -} - -TEST(CARTESIAN_TWIST_LIMITER_TEST, EmergencyStopStopsFasterThanNormalStopAndAcceptsNewTarget) -{ - CartesianTwistLimiter normal_limiter = makeLimiter(); - CartesianTwistLimiter emergency_limiter = makeLimiter(); - const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); - - const Twist forward_twist = makeTwist(0.8, 0.0, 0.0, 0.0, 0.0, 0.0); - normal_limiter.setTargetTwist(forward_twist, CartesianFrame::Base); - emergency_limiter.setTargetTwist(forward_twist, CartesianFrame::Base); - - for (int i = 0; i < 250; ++i) { - normal_limiter.update(kDt, base_R_tool); - emergency_limiter.update(kDt, base_R_tool); - } - - const double start_speed = normal_limiter.getTwistBase().head<3>().norm(); - ASSERT_GT(start_speed, 0.2); - ASSERT_NEAR(emergency_limiter.getTwistBase().head<3>().norm(), start_speed, 1e-3); - - normal_limiter.stop(); - emergency_limiter.emergencyStop(3.0, 20.0); - - int normal_stop_steps = -1; - int emergency_stop_steps = -1; - for (int i = 0; i < 500; ++i) { - const double normal_speed = normal_limiter.update(kDt, base_R_tool).head<3>().norm(); - const double emergency_speed = emergency_limiter.update(kDt, base_R_tool).head<3>().norm(); - - if (normal_stop_steps < 0 && normal_speed < 1e-3) { - normal_stop_steps = i; - } - if (emergency_stop_steps < 0 && emergency_speed < 1e-3) { - emergency_stop_steps = i; - } - - if (normal_stop_steps >= 0 && emergency_stop_steps >= 0) { - break; - } - } - - ASSERT_GE(normal_stop_steps, 0); - ASSERT_GE(emergency_stop_steps, 0); - EXPECT_LT(emergency_stop_steps, normal_stop_steps); - - const Twist restart_twist = makeTwist(-0.4, 0.0, 0.0, 0.0, 0.0, 0.0); - emergency_limiter.setTargetTwist(restart_twist, CartesianFrame::Base); - - double restarted_speed = 0.0; - double restarted_x = 0.0; - for (int i = 0; i < 300; ++i) { - const Twist twist = emergency_limiter.update(kDt, base_R_tool); - restarted_speed = twist.head<3>().norm(); - restarted_x = twist.x(); - if (restarted_speed > 0.2) { - break; - } - } - - EXPECT_GT(restarted_speed, 0.2); - EXPECT_LT(restarted_x, -0.2); -} - -TEST(CARTESIAN_TWIST_LIMITER_TEST, PlotLinearVelocityComponentsAndNorm) -{ - CartesianTwistLimiter limiter = makeLimiter(); - const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); - - std::vector time; - std::vector vx; - std::vector vy; - std::vector vz; - std::vector speed; - std::vector target_speed; - std::vector ax; - std::vector ay; - std::vector az; - std::vector accel; - std::vector jx; - std::vector jy; - std::vector jz; - std::vector jerk; - - constexpr double kEmergencyTime = 8.5; - constexpr double kRestartTime = 10.0; - constexpr double kTotalTime = 14.0; - const int steps = static_cast(kTotalTime / kDt); - - time.reserve(steps + 1); - vx.reserve(steps + 1); - vy.reserve(steps + 1); - vz.reserve(steps + 1); - speed.reserve(steps + 1); - target_speed.reserve(steps + 1); - ax.reserve(steps + 1); - ay.reserve(steps + 1); - az.reserve(steps + 1); - accel.reserve(steps + 1); - jx.reserve(steps + 1); - jy.reserve(steps + 1); - jz.reserve(steps + 1); - jerk.reserve(steps + 1); - - bool emergency_stop_called = false; - bool restart_called = false; - for (int i = 0; i <= steps; ++i) { - const double t = i * kDt; - if (t < kEmergencyTime) { - limiter.setTargetTwist(linearTargetAtTime(t), CartesianFrame::Base); - } else if (!emergency_stop_called) { - limiter.emergencyStop(10.0, 50.0); - emergency_stop_called = true; - } else if (t >= kRestartTime && !restart_called) { - limiter.setTargetTwist(makeTwist(0.0, -0.6, 0.0, 0.0, 0.0, 0.0), CartesianFrame::Base); - restart_called = true; - } - - const Twist twist = limiter.update(kDt, base_R_tool); - const Twist target = limiter.getTargetTwistBase(); - const Twist acceleration = limiter.getAccelerationBase(); - const Twist current_jerk = limiter.getJerkBase(); - - time.push_back(t); - vx.push_back(twist.x()); - vy.push_back(twist.y()); - vz.push_back(twist.z()); - speed.push_back(twist.head<3>().norm()); - target_speed.push_back(target.head<3>().norm()); - ax.push_back(acceleration.x()); - ay.push_back(acceleration.y()); - az.push_back(acceleration.z()); - accel.push_back(acceleration.head<3>().norm()); - jx.push_back(current_jerk.x()); - jy.push_back(current_jerk.y()); - jz.push_back(current_jerk.z()); - jerk.push_back(current_jerk.head<3>().norm()); - } - - using namespace matplot; - - auto configure_axes = [](const axes_handle& axes) { - axes->line_width(1.5f); - grid(axes, on); - }; - - auto style_line = [](const auto& line, std::initializer_list color, double width) { - line->line_width(width); - line->color(color); - }; - - auto fig = figure(true); - fig->size(1600, 1200); - fig->font_size(16); - - auto ax_v = subplot(fig, std::array{0.07f, 0.69f, 0.88f, 0.24f}); - hold(ax_v, on); - style_line(plot(ax_v, time, vx), {0.86f, 0.16f, 0.16f}, 3.0); - style_line(plot(ax_v, time, vy), {0.16f, 0.47f, 0.80f}, 3.0); - style_line(plot(ax_v, time, vz), {0.12f, 0.62f, 0.42f}, 3.0); - style_line(plot(ax_v, time, speed), {0.10f, 0.10f, 0.10f}, 3.5); - auto line_target_speed = plot(ax_v, time, target_speed); - line_target_speed->line_width(2.5); - line_target_speed->line_style("--"); - line_target_speed->color({0.80f, 0.55f, 0.10f}); - title(ax_v, "CartesianTwistLimiter Linear Velocity With Emergency Stop And Restart"); - xlabel(ax_v, "time [s]"); - ylabel(ax_v, "velocity [m/s]"); - legend(ax_v, {"vx", "vy", "vz", "speed", "target speed"}); - configure_axes(ax_v); - - auto ax_a = subplot(fig, std::array{0.07f, 0.38f, 0.88f, 0.24f}); - hold(ax_a, on); - style_line(plot(ax_a, time, ax), {0.86f, 0.16f, 0.16f}, 3.0); - style_line(plot(ax_a, time, ay), {0.16f, 0.47f, 0.80f}, 3.0); - style_line(plot(ax_a, time, az), {0.12f, 0.62f, 0.42f}, 3.0); - style_line(plot(ax_a, time, accel), {0.10f, 0.10f, 0.10f}, 3.5); - title(ax_a, "CartesianTwistLimiter Linear Acceleration"); - xlabel(ax_a, "time [s]"); - ylabel(ax_a, "acceleration [m/s^2]"); - legend(ax_a, {"ax", "ay", "az", "acc norm"}); - configure_axes(ax_a); - - auto ax_j = subplot(fig, std::array{0.07f, 0.07f, 0.88f, 0.24f}); - hold(ax_j, on); - style_line(plot(ax_j, time, jx), {0.86f, 0.16f, 0.16f}, 3.0); - style_line(plot(ax_j, time, jy), {0.16f, 0.47f, 0.80f}, 3.0); - style_line(plot(ax_j, time, jz), {0.12f, 0.62f, 0.42f}, 3.0); - style_line(plot(ax_j, time, jerk), {0.10f, 0.10f, 0.10f}, 3.5); - title(ax_j, "CartesianTwistLimiter Linear Jerk"); - xlabel(ax_j, "time [s]"); - ylabel(ax_j, "jerk [m/s^3]"); - legend(ax_j, {"jx", "jy", "jz", "jerk norm"}); - configure_axes(ax_j); - - show(fig); - cla(); -} - -TEST(CARTESIAN_TWIST_LIMITER_TEST, PlotAngularVelocityComponentsAndNorm) -{ - CartesianTwistLimiter limiter = makeLimiter(); - const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); - - std::vector time; - std::vector wx; - std::vector wy; - std::vector wz; - std::vector omega; - std::vector target_omega; - - constexpr double kEmergencyTime = 8.5; - constexpr double kRestartTime = 10.0; - constexpr double kTotalTime = 14.0; - const int steps = static_cast(kTotalTime / kDt); - - time.reserve(steps + 1); - wx.reserve(steps + 1); - wy.reserve(steps + 1); - wz.reserve(steps + 1); - omega.reserve(steps + 1); - target_omega.reserve(steps + 1); - - bool emergency_stop_called = false; - bool restart_called = false; - for (int i = 0; i <= steps; ++i) { - const double t = i * kDt; - if (t < kEmergencyTime) { - limiter.setTargetTwist(angularTargetAtTime(t), CartesianFrame::Base); - } else if (!emergency_stop_called) { - limiter.emergencyStop(10.0, 50.0); - emergency_stop_called = true; - } else if (t >= kRestartTime && !restart_called) { - limiter.setTargetTwist(makeTwist(0.0, 0.0, 0.0, 0.0, 0.0, -0.5), CartesianFrame::Base); - restart_called = true; - } - - const Twist twist = limiter.update(kDt, base_R_tool); - const Twist target = limiter.getTargetTwistBase(); - - time.push_back(t); - wx.push_back(twist(3)); - wy.push_back(twist(4)); - wz.push_back(twist(5)); - omega.push_back(twist.tail<3>().norm()); - target_omega.push_back(target.tail<3>().norm()); - } - - using namespace matplot; - - auto configure_axes = [](const axes_handle& axes) { - axes->line_width(1.5f); - grid(axes, on); - }; - - auto style_line = [](const auto& line, std::initializer_list color, double width) { - line->line_width(width); - line->color(color); - }; - - auto fig = figure(true); - fig->size(1600, 520); - fig->font_size(16); - - auto ax = subplot(fig, std::array{0.07f, 0.14f, 0.88f, 0.74f}); - hold(ax, on); - style_line(plot(ax, time, wx), {0.86f, 0.16f, 0.16f}, 3.0); - style_line(plot(ax, time, wy), {0.16f, 0.47f, 0.80f}, 3.0); - style_line(plot(ax, time, wz), {0.12f, 0.62f, 0.42f}, 3.0); - style_line(plot(ax, time, omega), {0.10f, 0.10f, 0.10f}, 3.5); - auto line_target_omega = plot(ax, time, target_omega); - line_target_omega->line_width(2.5); - line_target_omega->line_style("--"); - line_target_omega->color({0.80f, 0.55f, 0.10f}); - title(ax, "CartesianTwistLimiter Angular Velocity With Emergency Stop And Restart"); - xlabel(ax, "time [s]"); - ylabel(ax, "angular velocity [rad/s]"); - legend(ax, {"wx", "wy", "wz", "omega", "target omega"}); - configure_axes(ax); - - show(fig); - cla(); -} - -TEST(CARTESIAN_TWIST_LIMITER_TEST, PlotSynchronizeTrackingWithMeasuredTwist) -{ - CartesianTwistLimiter limiter = makeLimiter(); - const Eigen::Matrix3d base_R_tool = Eigen::Matrix3d::Identity(); - - Twist measured_twist = Twist::Zero(); - - std::vector time; - std::vector target_speed; - std::vector commanded_speed; - std::vector measured_speed; - std::vector commanded_vx; - std::vector commanded_vy; - std::vector measured_vx; - std::vector measured_vy; - std::vector tracking_error; - std::vector acc_norm; - std::vector jerk_norm; - - constexpr double kTotalTime = 10.0; - constexpr double kTrackingTau = 0.08; - const double alpha = kDt / (kTrackingTau + kDt); - const int steps = static_cast(kTotalTime / kDt); - - time.reserve(steps + 1); - target_speed.reserve(steps + 1); - commanded_speed.reserve(steps + 1); - measured_speed.reserve(steps + 1); - commanded_vx.reserve(steps + 1); - commanded_vy.reserve(steps + 1); - measured_vx.reserve(steps + 1); - measured_vy.reserve(steps + 1); - tracking_error.reserve(steps + 1); - acc_norm.reserve(steps + 1); - jerk_norm.reserve(steps + 1); - - for (int i = 0; i <= steps; ++i) { - const double t = i * kDt; - const Twist target = synchronizeTargetAtTime(t); - - limiter.synchronize(measured_twist, kDt, true); - limiter.setTargetTwist(target, CartesianFrame::Base); - const Twist commanded = limiter.update(kDt, base_R_tool); - - measured_twist += alpha * (commanded - measured_twist); - - time.push_back(t); - target_speed.push_back(target.head<3>().norm()); - commanded_speed.push_back(commanded.head<3>().norm()); - measured_speed.push_back(measured_twist.head<3>().norm()); - commanded_vx.push_back(commanded.x()); - commanded_vy.push_back(commanded.y()); - measured_vx.push_back(measured_twist.x()); - measured_vy.push_back(measured_twist.y()); - tracking_error.push_back((commanded.head<3>() - measured_twist.head<3>()).norm()); - acc_norm.push_back(limiter.getAccelerationBase().head<3>().norm()); - jerk_norm.push_back(limiter.getJerkBase().head<3>().norm()); - } - - using namespace matplot; - - auto configure_axes = [](const axes_handle& axes) { - axes->line_width(1.5f); - grid(axes, on); - }; - - auto style_line = [](const auto& line, std::initializer_list color, double width) { - line->line_width(width); - line->color(color); - }; - - auto fig = figure(true); - fig->size(1600, 1200); - fig->font_size(16); - - auto ax_speed = subplot(fig, std::array{0.07f, 0.69f, 0.88f, 0.24f}); - hold(ax_speed, on); - auto line_target = plot(ax_speed, time, target_speed); - line_target->line_width(2.5); - line_target->line_style("--"); - line_target->color({0.80f, 0.55f, 0.10f}); - style_line(plot(ax_speed, time, commanded_speed), {0.10f, 0.10f, 0.10f}, 3.5); - style_line(plot(ax_speed, time, measured_speed), {0.55f, 0.12f, 0.72f}, 3.0); - title(ax_speed, "Synchronize: target / commanded / measured speed"); - xlabel(ax_speed, "time [s]"); - ylabel(ax_speed, "speed [m/s]"); - legend(ax_speed, {"target speed", "commanded speed", "measured speed"}); - configure_axes(ax_speed); - - auto ax_components = subplot(fig, std::array{0.07f, 0.38f, 0.88f, 0.24f}); - hold(ax_components, on); - style_line(plot(ax_components, time, commanded_vx), {0.86f, 0.16f, 0.16f}, 3.0); - auto line_measured_vx = plot(ax_components, time, measured_vx); - line_measured_vx->line_width(2.5); - line_measured_vx->line_style("--"); - line_measured_vx->color({0.86f, 0.16f, 0.16f}); - style_line(plot(ax_components, time, commanded_vy), {0.16f, 0.47f, 0.80f}, 3.0); - auto line_measured_vy = plot(ax_components, time, measured_vy); - line_measured_vy->line_width(2.5); - line_measured_vy->line_style("--"); - line_measured_vy->color({0.16f, 0.47f, 0.80f}); - title(ax_components, "Synchronize: commanded vs measured components"); - xlabel(ax_components, "time [s]"); - ylabel(ax_components, "velocity [m/s]"); - legend(ax_components, {"cmd vx", "meas vx", "cmd vy", "meas vy"}); - configure_axes(ax_components); - - auto ax_dynamics = subplot(fig, std::array{0.07f, 0.07f, 0.88f, 0.24f}); - hold(ax_dynamics, on); - style_line(plot(ax_dynamics, time, tracking_error), {0.10f, 0.10f, 0.10f}, 3.5); - style_line(plot(ax_dynamics, time, acc_norm), {0.12f, 0.62f, 0.42f}, 3.0); - style_line(plot(ax_dynamics, time, jerk_norm), {0.55f, 0.12f, 0.72f}, 3.0); - title(ax_dynamics, "Synchronize: tracking error / acceleration / jerk"); - xlabel(ax_dynamics, "time [s]"); - ylabel(ax_dynamics, "norm"); - legend(ax_dynamics, {"|cmd - meas|", "acc norm", "jerk norm"}); - configure_axes(ax_dynamics); - - show(fig); - cla(); -} - -} // namespace cmvr diff --git a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner_test.cpp b/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner_test.cpp deleted file mode 100644 index 91e907b6..00000000 --- a/cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner_test.cpp +++ /dev/null @@ -1,88 +0,0 @@ -// -// Created by lgv on 11/11/25. -// - -#include "gtest/gtest.h" -#include "algorithms/motion_planner/base_motion/joint_trajectory/toppra/include/toppra_joint_trajectory_planner.h" - -#include - -using namespace cmvr; -TEST(TOPPRA_JOINT_TRAJECTORY_PLANNER_TEST,TOPPRA_TEST) { - - auto planner = std::make_shared(); - planner->setPathType(PathType::Quintic); - - planner->setSymmetricLimits(std::vector(7, 1.5), - std::vector(7, 3.0)); - TrajPtr traj; - std::vector q0{0.0,-0.5,0.8,0.0,0.2,-0.3,0.1}; - std::vector q1{1.2,0.2,-0.6,0.7,-0.4,0.5,-0.2}; - if (!planner->plan(q0, q1, traj)) { std::cerr << "plan failed\n"; } - - // 2) 采样 0.01 s - auto samples = planner->sampleTrajectory(traj, 0.01); - - // 3) 写 CSV - if (!planner->writeTrajectoryCsv("/home/lgv/cmvr/cmvr-es/data/planner/traj.csv", samples)) { - std::cerr << "write csv failed\n"; - } - - std::cout << "CSV saved: traj.csv\n"; - -} - -TEST(TOPPRA_JOINT_TRAJECTORY_PLANNER_TEST, TOPPRA_WAYPOINTS_TEST) { - - auto planner = std::make_shared(); - planner->setPathType(PathType::Quintic); - - // 7 自由度对称速度 / 加速度约束 - planner->setSymmetricLimits(std::vector(7, 1.5), - std::vector(7, 3.0)); - - // ------- 1) 构造多个 q 路点 ------- - std::vector q0 { 0.0, -0.5, 0.8, 0.0, 0.2, -0.3, 0.1}; - std::vector q1 { 0.5, -0.2, 0.4, 0.3, -0.1, 0.1, 0.0}; - std::vector q2 { 0.9, 0.1, -0.3, 0.5, -0.3, 0.3, -0.1}; - std::vector q3 { 1.2, 0.2, -0.6, 0.7, -0.4, 0.5, -0.2}; // 终点 - - std::vector> waypoints; - waypoints.push_back(q0); - waypoints.push_back(q1); - waypoints.push_back(q2); - waypoints.push_back(q3); - - // ------- 2) 调多路点 plan ------- - TrajPtr traj; - if (!planner->plan(waypoints, traj)) { - std::cerr << "multi-waypoints plan failed\n"; - FAIL(); // GTest 标记失败 - } - - // ------- 3) 采样并简单校验 ------- - // 0.01 s 采样 - auto samples = planner->sampleTrajectory(traj, 0.01); - ASSERT_FALSE(samples.empty()); - - // （下面假设 TrajSample 里有 q / pos 这样的关节角向量字段， - // 你按自己的结构名改一下就行） - const auto &q_start = samples.front().q; - const auto &q_end = samples.back().q; - - ASSERT_EQ(q_start.size(), q0.size()); - ASSERT_EQ(q_end.size(), q3.size()); - - for (size_t i = 0; i < q0.size(); ++i) { - EXPECT_NEAR(q_start[i], q0[i], 1e-4); - EXPECT_NEAR(q_end[i], q3[i], 1e-4); - } - - // ------- 4) 写 CSV 看一下轨迹 ------- - if (!planner->writeTrajectoryCsv( - "/home/lgv/cmvr/cmvr-es/data/planner/traj.csv", samples)) { - std::cerr << "write csv failed\n"; - } else { - std::cout << "CSV saved: traj.csv\n"; - } -} diff --git a/cmvr-es/algorithms/perception/CMakeLists.txt b/cmvr-es/algorithms/perception/CMakeLists.txt index 6eef84ee..98eddf21 100644 --- a/cmvr-es/algorithms/perception/CMakeLists.txt +++ b/cmvr-es/algorithms/perception/CMakeLists.txt @@ -31,18 +31,3 @@ target_link_libraries(perception PUBLIC add_library(cmvr_es::perception ALIAS perception) install(TARGETS perception LIBRARY DESTINATION lib) - -add_executable(tag_relative_target_3d_test - apriltag/src/tag_relative_target_3d_test.cpp -) - -target_link_libraries(tag_relative_target_3d_test - PRIVATE - cmvr_es::perception - cmvr_es::device::realsense_camera - cmvr_es::proto - glog - gtest - gtest_main - pthread -) diff --git a/cmvr-es/algorithms/perception/apriltag/src/tag_relative_target_3d_test.cpp b/cmvr-es/algorithms/perception/apriltag/src/tag_relative_target_3d_test.cpp deleted file mode 100644 index d895251e..00000000 --- a/cmvr-es/algorithms/perception/apriltag/src/tag_relative_target_3d_test.cpp +++ /dev/null @@ -1,711 +0,0 @@ -// -// Created by lgv on 2026/2/26. -// - -#include "algorithms/perception/apriltag/include/tag_relative_target_3d.h" -#include "devices/camera/realsense_camera/include/realsense_camera.h" - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace { - -// 直接在这里改测试参数。 -constexpr const char* kRsSerial = "243122074587"; -// constexpr const char* kRsSerial = "243122075389"; -// constexpr const char* kRsSerial = "f1421544"; -// Target pixel. Negative means image center. -constexpr int kTargetU = -1; -constexpr int kTargetV = -1; -constexpr double kTagSize = 0.01975 ; -// <= 0 means loop until user abort. -constexpr int kTries = -1; -constexpr int kWidth = 1280; -constexpr int kHeight = 720; -constexpr int kFps = 30; -constexpr const char* kWindowName = "TagRelativeTarget3DRealSenseTest"; -constexpr int kWarmupMaxTries = 60; -constexpr int kWarmupSleepMs = 50; -constexpr int kFrameRetryMax = 3; -constexpr int kFrameRetrySleepMs = 25; -constexpr int kLogEveryNFrames = 1; - -struct FramePack { - cv::Mat color; - cv::Mat depth; - cmvr::device::Rs2Intrinsics intrinsics{}; -}; - -bool fetchRGBFrame(const std::shared_ptr& camera, - FramePack& frame, - std::string& error_out) { - error_out.clear(); - for (int retry = 0; retry < kFrameRetryMax; ++retry) { - try { - camera->getRGBImage(frame.color, frame.intrinsics); - if (!frame.color.empty()) { - return true; - } - error_out = "empty_frame"; - } catch (const std::exception& e) { - error_out = e.what(); - } - if (retry + 1 < kFrameRetryMax) { - std::this_thread::sleep_for(std::chrono::milliseconds(kFrameRetrySleepMs)); - } - } - return false; -} - -bool fetchRGBDFrame(const std::shared_ptr& camera, - FramePack& frame, - std::string& error_out) { - error_out.clear(); - for (int retry = 0; retry < kFrameRetryMax; ++retry) { - try { - camera->getRGBDImages(frame.color, frame.depth, frame.intrinsics); - if (!frame.color.empty() && !frame.depth.empty()) { - return true; - } - error_out = "empty_rgbd_frame"; - } catch (const std::exception& e) { - error_out = e.what(); - } - if (retry + 1 < kFrameRetryMax) { - std::this_thread::sleep_for(std::chrono::milliseconds(kFrameRetrySleepMs)); - } - } - return false; -} - -bool projectPoint(const Eigen::Vector3d& p_c, - const cmvr::device::Rs2Intrinsics& K, - cv::Point& uv) { - if (!std::isfinite(p_c.x()) || !std::isfinite(p_c.y()) || !std::isfinite(p_c.z())) { - return false; - } - if (p_c.z() <= 1e-6) { - return false; - } - if (!std::isfinite(K.fx) || !std::isfinite(K.fy) || - !std::isfinite(K.cx) || !std::isfinite(K.cy) || - K.fx <= 0.0f || K.fy <= 0.0f) { - return false; - } - const double u = static_cast(K.fx) * (p_c.x() / p_c.z()) + static_cast(K.cx); - const double v = static_cast(K.fy) * (p_c.y() / p_c.z()) + static_cast(K.cy); - uv.x = static_cast(std::lround(u)); - uv.y = static_cast(std::lround(v)); - return true; -} - -bool sampleDepthMetersAtPixel(const cv::Mat& depth, int u, int v, double& z_m) { - if (depth.empty() || depth.type() != CV_16UC1) { - return false; - } - if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows) { - return false; - } - - std::vector vals; - vals.reserve(9); - constexpr int kRadius = 1; // 3x3 - for (int yy = std::max(0, v - kRadius); yy <= std::min(depth.rows - 1, v + kRadius); ++yy) { - for (int xx = std::max(0, u - kRadius); xx <= std::min(depth.cols - 1, u + kRadius); ++xx) { - const uint16_t raw = depth.at(yy, xx); - if (raw == 0) { - continue; - } - const double z = static_cast(raw) * 1e-3; // mm -> m - if (!std::isfinite(z) || z <= 1e-4 || z >= 50.0) { - continue; - } - vals.push_back(z); - } - } - if (vals.empty()) { - return false; - } - - const size_t mid = vals.size() / 2; - std::nth_element(vals.begin(), vals.begin() + mid, vals.end()); - z_m = vals[mid]; - return std::isfinite(z_m) && z_m > 0.0; -} - -void drawTagAxes(cv::Mat& img, - const Eigen::Matrix4d& T_c_t, - const cmvr::device::Rs2Intrinsics& K, - double axis_len, - int tag_id) { - const Eigen::Matrix3d R = T_c_t.block<3,3>(0,0); - const Eigen::Vector3d t = T_c_t.block<3,1>(0,3); - const Eigen::Vector3d o_c = t; - const Eigen::Vector3d x_c = R * Eigen::Vector3d(axis_len, 0, 0) + t; - const Eigen::Vector3d y_c = R * Eigen::Vector3d(0, axis_len, 0) + t; - const Eigen::Vector3d z_c = R * Eigen::Vector3d(0, 0, axis_len) + t; - - cv::Point o, x, y, z; - if (!projectPoint(o_c, K, o)) return; - if (projectPoint(x_c, K, x)) { - cv::line(img, o, x, cv::Scalar(0, 0, 255), 2); // X red - } - if (projectPoint(y_c, K, y)) { - cv::line(img, o, y, cv::Scalar(0, 255, 0), 2); // Y green - } - if (projectPoint(z_c, K, z)) { - cv::line(img, o, z, cv::Scalar(255, 0, 0), 2); // Z blue - } - cv::circle(img, o, 3, cv::Scalar(255, 255, 255), -1); - cv::putText(img, - "id=" + std::to_string(tag_id), - cv::Point(o.x + 6, o.y - 6), - cv::FONT_HERSHEY_SIMPLEX, - 0.45, - cv::Scalar(255, 255, 255), - 1); -} - -struct DisplayTagPose { - int tag_id{-1}; - Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()}; -}; - -bool detectTagPosesForDisplay(const cv::Mat& color, - const cmvr::device::Rs2Intrinsics& intrinsics, - double tag_size_m, - std::vector& out_tags) { - out_tags.clear(); - if (color.empty()) { - return false; - } - if (color.channels() != 1 && color.channels() != 3 && color.channels() != 4) { - return false; - } - if (!std::isfinite(intrinsics.fx) || !std::isfinite(intrinsics.fy) || - !std::isfinite(intrinsics.cx) || !std::isfinite(intrinsics.cy) || - intrinsics.fx <= 0.0f || intrinsics.fy <= 0.0f) { - return false; - } - - cv::Mat gray; - if (color.channels() == 1) { - gray = color; - } else if (color.channels() == 3) { - cv::cvtColor(color, gray, cv::COLOR_BGR2GRAY); - } else { - cv::cvtColor(color, gray, cv::COLOR_BGRA2GRAY); - } - if (gray.empty() || gray.type() != CV_8UC1) { - return false; - } - if (!gray.isContinuous()) { - gray = gray.clone(); - } - - vpCameraParameters cam; - cam.initPersProjWithoutDistortion( - static_cast(intrinsics.fx), - static_cast(intrinsics.fy), - static_cast(intrinsics.cx), - static_cast(intrinsics.cy)); - - vpImage I(gray.rows, gray.cols); - for (int y = 0; y < gray.rows; ++y) { - std::memcpy(I[y], gray.ptr(y), static_cast(gray.cols)); - } - - static vpDetectorAprilTag detector(vpDetectorAprilTag::TAG_36h11); - static bool detector_init = false; - if (!detector_init) { - detector.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS); - detector_init = true; - } - - std::vector cMo_vec; - const bool detected = detector.detect(I, tag_size_m, cam, cMo_vec); - if (!detected || cMo_vec.empty()) { - return false; - } - - const std::vector ids = detector.getTagsId(); - const size_t n = std::min(ids.size(), cMo_vec.size()); - out_tags.reserve(n); - for (size_t i = 0; i < n; ++i) { - Eigen::Matrix4d T = Eigen::Matrix4d::Identity(); - const auto& cMo = cMo_vec[i]; - for (int r = 0; r < 3; ++r) { - for (int c = 0; c < 3; ++c) { - T(r, c) = cMo[r][c]; - } - } - T(0, 3) = cMo[0][3]; - T(1, 3) = cMo[1][3]; - T(2, 3) = cMo[2][3]; - out_tags.push_back(DisplayTagPose{ids[i], T}); - } - return !out_tags.empty(); -} - -std::string formatVec3(const Eigen::Vector3d& p, int precision = 6) { - std::ostringstream oss; - oss << std::fixed << std::setprecision(precision) - << "[" << p.x() << ", " << p.y() << ", " << p.z() << "]"; - return oss.str(); -} - -} // namespace - - -TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointInDetectedTags) { - const std::string serial = kRsSerial; - if (serial.empty()) { - GTEST_SKIP() << "kRsSerial is empty, please set it in tag_relative_target_3d_test.cpp"; - } - - const double tag_size = kTagSize; - const int tries = kTries; - const bool infinite = (tries <= 0); - const int width = kWidth; - const int height = kHeight; - const int fps = kFps; - - std::cout << "[TagRelativeTarget3DTest] serial=" << serial - << " target_uv=[" << kTargetU << ", " << kTargetV << "]" - << " tag_size=" << tag_size - << " tries=" << tries << "\n"; - - cmvr::config::RealSenseCameraConfig cam_cfg; - cam_cfg.set_id("tag_relative_target_3d_test"); - cam_cfg.set_serialnumber(serial); - cam_cfg.set_width(width); - cam_cfg.set_height(height); - cam_cfg.set_fps(fps); - cam_cfg.set_codec("H265"); - cam_cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO); - cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD); - cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR); - cam_cfg.set_buffer_size(30); - cam_cfg.set_sync(true); - - auto camera = std::make_shared(cam_cfg); - - auto perception = std::make_shared(camera); - perception->setTagSize(tag_size); - cmvr::perception::TagRelativeTarget3D tracker(perception); - - cmvr::perception::AprilTagPerception::Options opt; - opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::REQUIRE; - opt.detect_tags = true; - tracker.setActiveTagSwitchPolicy(4, 1.2); - tracker.setTrackingCandidateScoreWeights(1.0, 2.0, 0.08); - tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE); - - ASSERT_NO_THROW(camera->init()); - ASSERT_NO_THROW(camera->start()); - struct CameraStopGuard { - std::shared_ptr cam; - ~CameraStopGuard() { - if (!cam) return; - try { - cam->stop(); - } catch (...) { - } - } - } stop_guard{camera}; - - bool window_enabled = true; - try { - cv::namedWindow(kWindowName, cv::WINDOW_NORMAL); - cv::resizeWindow(kWindowName, width, height); - cv::imshow(kWindowName, cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20))); - cv::waitKey(1); - } catch (const cv::Exception& e) { - window_enabled = false; - std::cout << "[TagRelativeTarget3DTest] window disabled: " << e.what() << "\n"; - } - if (!window_enabled) { - GTEST_SKIP() << "OpenCV highgui is not available, skip windowed test."; - } - - bool ok = false; - bool user_abort = false; - int frame_fail_count = 0; - int plane_valid_count = 0; - int lock_success_count = 0; - int track_success_count = 0; - int track_fail_count = 0; - int tag_switch_count = 0; - int printed_count = 0; - bool target_uv_initialized = false; - int target_u = -1; - int target_v = -1; - bool target_locked = false; - FramePack last_good_frame; - last_good_frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20)); - bool has_last_good_frame = false; - const auto t_loop_start = std::chrono::steady_clock::now(); - - // Warmup: try to get RGB frames after pipeline start. - bool warmup_ok = false; - bool warmup_abort = false; - for (int i = 0; i < kWarmupMaxTries; ++i) { - FramePack warm_frame; - std::string warm_error; - warmup_ok = fetchRGBFrame(camera, warm_frame, warm_error); - if (warmup_ok) { - break; - } - const int key = cv::waitKey(1); - if (key == 27 || key == 'q' || key == 'Q') { - warmup_abort = true; - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(kWarmupSleepMs)); - } - if (warmup_abort) { - GTEST_SKIP() << "User aborted during warmup"; - } - if (!warmup_ok) { - std::cout << "[TagRelativeTarget3DTest] warmup failed, continue running and retry per frame\n"; - } - - for (int i = 0; infinite || i < tries; ++i) { - FramePack frame; - frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20)); - std::string frame_error; - const bool frame_ok = fetchRGBFrame(camera, frame, frame_error); - if (!frame_ok) { - ++frame_fail_count; - if (has_last_good_frame) { - frame.color = last_good_frame.color.clone(); - frame.intrinsics = last_good_frame.intrinsics; - } else { - frame.color = cv::Mat(height, width, CV_8UC3, cv::Scalar(20, 20, 20)); - } - } else { - last_good_frame.color = frame.color.clone(); - last_good_frame.intrinsics = frame.intrinsics; - has_last_good_frame = true; - } - std::vector vis_tags; - const bool vis_tag_detected = detectTagPosesForDisplay(frame.color, frame.intrinsics, tag_size, vis_tags); - - if (!target_uv_initialized) { - target_u = (kTargetU >= 0) ? kTargetU : (frame.color.cols / 2); - target_v = (kTargetV >= 0) ? kTargetV : (frame.color.rows / 2); - target_uv_initialized = true; - } - - Eigen::Vector3d p_c_target = Eigen::Vector3d::Zero(); - int used_tag_id = -1; - double spread_m = 0.0; - bool target_valid = false; - std::string phase = target_locked ? "track" : "lock"; - - if (!target_locked) { - perception->update(opt); - if (tracker.startTrackingFromPixel(target_u, target_v)) { - p_c_target = tracker.lastTargetInCamera(); - used_tag_id = tracker.lastUsedTagId(); - spread_m = tracker.lastSpread(); - target_locked = true; - phase = "track"; - target_valid = true; - ok = true; - ++plane_valid_count; - ++lock_success_count; - ++printed_count; - - std::cout << "[TagRelativeTarget3DTest][LOCK] target_p_c=" - << formatVec3(p_c_target) - << " used_tag=" << used_tag_id - << " active_tag=" << tracker.activeTagId() - << " spread=" << spread_m - << "\n"; - std::cout << "[TagRelativeTarget3DTest] anchors=" << tracker.anchorCount() << "\n"; - } else if (((i + 1) % kLogEveryNFrames) == 0) { - std::cout << "[TagRelativeTarget3DTest] startTracking failed: " - << cmvr::perception::TagRelativeTarget3D::statusToString(tracker.lastStatus()) - << "\n"; - } - } else { - const int old_active_tag = tracker.activeTagId(); - perception->update(opt); - if (tracker.track()) { - p_c_target = tracker.lastTargetInCamera(); - used_tag_id = tracker.lastUsedTagId(); - spread_m = tracker.lastSpread(); - const bool switched = tracker.lastSwitched(); - - target_valid = true; - ok = true; - ++track_success_count; - ++printed_count; - if (switched) { - ++tag_switch_count; - std::cout << "[TagRelativeTarget3DTest][SWITCH] active_tag " - << old_active_tag << " -> " << tracker.activeTagId() << "\n"; - } - std::cout << "[TagRelativeTarget3DTest][TRACK] target_p_c=" - << formatVec3(p_c_target) - << " used_tag=" << used_tag_id - << " active_tag=" << tracker.activeTagId() - << " spread=" << spread_m - << "anchorCount=" << tracker.anchorCount() - << std::endl; - } else { - ++track_fail_count; - if (((i + 1) % kLogEveryNFrames) == 0) { - std::cout << "[TagRelativeTarget3DTest] track failed: " - << cmvr::perception::TagRelativeTarget3D::statusToString(tracker.lastStatus()) - << "\n"; - } - } - } - const auto status = tracker.lastStatus(); - const char* status_str = cmvr::perception::TagRelativeTarget3D::statusToString(status); - - cv::Mat vis = frame.color.clone(); - cv::drawMarker(vis, - cv::Point(target_u, target_v), - cv::Scalar(0, 255, 255), - cv::MARKER_CROSS, - 16, - 2); - cv::putText(vis, - std::string("status: ") + status_str, - cv::Point(12, 28), - cv::FONT_HERSHEY_SIMPLEX, - 0.7, - cv::Scalar(0, 255, 0), - 2); - cv::putText(vis, - "press q / Esc to quit", - cv::Point(12, 56), - cv::FONT_HERSHEY_SIMPLEX, - 0.6, - cv::Scalar(0, 255, 255), - 2); - cv::putText(vis, - "phase=" + phase + " locked=" + std::to_string(target_locked ? 1 : 0) + - " active_tag=" + std::to_string(tracker.activeTagId()) + - " miss=" + std::to_string(tracker.activeTagMissingCount()) + - " anchors=" + std::to_string(tracker.anchorCount()) + - " vis_tags=" + std::to_string(vis_tag_detected ? vis_tags.size() : 0) + - " frame_fail=" + std::to_string(frame_fail_count), - cv::Point(12, 84), - cv::FONT_HERSHEY_SIMPLEX, - 0.5, - cv::Scalar(200, 200, 255), - 1); - if (!frame_error.empty()) { - cv::putText(vis, - "frame error: " + frame_error, - cv::Point(12, 106), - cv::FONT_HERSHEY_SIMPLEX, - 0.45, - cv::Scalar(0, 0, 255), - 1); - } - cv::putText(vis, - std::string("target3d: ") + (target_valid ? "valid" : "invalid") + - " used_tag=" + std::to_string(used_tag_id) + - " spread=" + std::to_string(spread_m), - cv::Point(12, 128), - cv::FONT_HERSHEY_SIMPLEX, - 0.45, - target_valid ? cv::Scalar(0, 255, 255) : cv::Scalar(0, 0, 255), - 1); - cv::putText(vis, - "target_p_c=" + (target_valid ? formatVec3(p_c_target) : "invalid"), - cv::Point(12, 150), - cv::FONT_HERSHEY_SIMPLEX, - 0.45, - target_valid ? cv::Scalar(0, 255, 255) : cv::Scalar(0, 0, 255), - 1); - - if (vis_tag_detected) { - const double axis_len = tag_size * 0.5; - for (const auto& tag : vis_tags) { - drawTagAxes(vis, tag.T_c_t, frame.intrinsics, axis_len, tag.tag_id); - } - } - - if (target_valid && std::isfinite(frame.intrinsics.fx) && std::isfinite(frame.intrinsics.fy) && - std::isfinite(frame.intrinsics.cx) && std::isfinite(frame.intrinsics.cy) && - p_c_target.z() > 1e-6) { - const double u = frame.intrinsics.fx * (p_c_target.x() / p_c_target.z()) + frame.intrinsics.cx; - const double v = frame.intrinsics.fy * (p_c_target.y() / p_c_target.z()) + frame.intrinsics.cy; - const int ui = static_cast(std::lround(u)); - const int vi = static_cast(std::lround(v)); - if (ui >= 0 && ui < vis.cols && vi >= 0 && vi < vis.rows) { - cv::circle(vis, cv::Point(ui, vi), 6, cv::Scalar(0, 255, 255), 2); - cv::putText(vis, - "target", - cv::Point(ui + 8, vi + 16), - cv::FONT_HERSHEY_SIMPLEX, - 0.5, - cv::Scalar(0, 255, 255), - 1); - } - } - - try { - cv::imshow(kWindowName, vis); - const int key = cv::waitKey(1); - if (key == 27 || key == 'q' || key == 'Q') { - user_abort = true; - break; - } - } catch (const cv::Exception& e) { - std::cout << "[TagRelativeTarget3DTest] window error: " << e.what() << "\n"; - user_abort = true; - break; - } - - std::this_thread::sleep_for(std::chrono::milliseconds(30)); - } - - try { - cv::destroyWindow(kWindowName); - } catch (...) { - // ignore window destroy errors in headless environments - } - - if (!user_abort && !infinite) { - EXPECT_TRUE(ok) << "No valid tag detected in " << tries << " tries"; - } - const auto t_loop_end = std::chrono::steady_clock::now(); - const double total_s = std::chrono::duration_cast>(t_loop_end - t_loop_start).count(); - std::cout << "[TagRelativeTarget3DTest] summary " - << " plane_valid=" << plane_valid_count - << " lock_ok=" << lock_success_count - << " track_ok=" << track_success_count - << " track_fail=" << track_fail_count - << " switch=" << tag_switch_count - << " printed=" << printed_count - << " frame_fail=" << frame_fail_count - << " duration_s=" << total_s - << "\n"; -} - -// TEST(TagRelativeTarget3DRealSenseTest, PrintTargetPointNoDisplayMinimal) { -// const std::string serial = kRsSerial; -// if (serial.empty()) { -// GTEST_SKIP() << "kRsSerial is empty, please set it in tag_relative_target_3d_test.cpp"; -// } -// -// cmvr::config::RealSenseCameraConfig cam_cfg; -// cam_cfg.set_id("tag_relative_target_3d_test_no_display"); -// cam_cfg.set_serialnumber(serial); -// cam_cfg.set_width(kWidth); -// cam_cfg.set_height(kHeight); -// cam_cfg.set_fps(kFps); -// cam_cfg.set_codec("H265"); -// cam_cfg.set_camera_mode(cmvr::config::CAMERA_MODE_PHOTO); -// cam_cfg.set_stream_mode(cmvr::config::STREAM_MODE_RGBD); -// cam_cfg.set_align_mode(cmvr::config::ALIGN_MODE_COLOR); -// cam_cfg.set_buffer_size(30); -// cam_cfg.set_sync(true); -// -// auto camera = std::make_shared(cam_cfg); -// cmvr::perception::TagRelativeTarget3D tracker(camera); -// ASSERT_NO_THROW(camera->init()); -// ASSERT_NO_THROW(camera->start()); -// struct CameraStopGuard { -// std::shared_ptr cam; -// ~CameraStopGuard() { -// if (!cam) return; -// try { -// cam->stop(); -// } catch (...) { -// } -// } -// } stop_guard{camera}; -// tracker.setTagSize(kTagSize); -// tracker.setActiveTagSwitchPolicy(4, 1.2); -// tracker.setTrackingCandidateScoreWeights(1.0, 1.0, 0.08); -// // 深度法采样：5x5 中值 + MAD 离群剔除。 -// tracker.setDepthSamplingConfig(5, true, 2.5, 0.003); -// const int tries = kTries; -// const bool infinite = (tries <= 0); -// int plane_fail_count = 0; -// int depth_fail_count = 0; -// int plane_ok_count = 0; -// int depth_ok_count = 0; -// int both_ok_count = 0; -// int printed_count = 0; -// bool got_target = false; -// const int target_u = (kTargetU >= 0) ? kTargetU : (kWidth / 2); -// const int target_v = (kTargetV >= 0) ? kTargetV : (kHeight / 2); -// -// for (int i = 0; infinite || i < tries; ++i) { -// // 用公开接口分别求解两种方法（内部自行抓帧/缓存）。 -// tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE); -// const bool plane_ok = tracker.solveFromPixel(target_u, target_v); -// const Eigen::Vector3d p_c_plane = plane_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero(); -// const int plane_used_tag = plane_ok ? tracker.lastUsedTagId() : -1; -// const double plane_spread = plane_ok ? tracker.lastSpread() : 0.0; -// if (plane_ok) { -// ++plane_ok_count; -// } else { -// ++plane_fail_count; -// } -// -// tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::DEPTH_IMAGE); -// const bool depth_ok = tracker.solveFromPixel(target_u, target_v); -// const Eigen::Vector3d p_c_depth = depth_ok ? tracker.lastTargetInCamera() : Eigen::Vector3d::Zero(); -// if (depth_ok) { -// ++depth_ok_count; -// } else { -// ++depth_fail_count; -// } -// -// if (plane_ok && depth_ok) { -// ++both_ok_count; -// } -// if (plane_ok || depth_ok) { -// got_target = true; -// } -// -// std::cout << "[TagRelativeTarget3DNoDisplay][COMPARE] " -// << "uv=[" << target_u << "," << target_v << "] " -// << "plane_ok=" << plane_ok -// << " plane_p_c=" << (plane_ok ? formatVec3(p_c_plane) : "[invalid]") -// << " used_tag=" << plane_used_tag -// << " spread=" << plane_spread -// << " | depth_ok=" << depth_ok -// << " depth_p_c=" << (depth_ok ? formatVec3(p_c_depth) : "[invalid]"); -// if (plane_ok && depth_ok) { -// std::cout << " | diff_norm=" << (p_c_plane - p_c_depth).norm(); -// } -// std::cout << std::endl; -// -// ++printed_count; -// std::this_thread::sleep_for(std::chrono::milliseconds(30)); -// } -// -// EXPECT_TRUE(got_target) -// << "Failed to solve target point in camera frame. " -// << " plane_fail=" << plane_fail_count -// << " depth_fail=" << depth_fail_count -// << " plane_ok=" << plane_ok_count -// << " depth_ok=" << depth_ok_count -// << " both_ok=" << both_ok_count -// << " printed=" << printed_count; -// } diff --git a/cmvr-es/common/math/cartesian_motion_math.h b/cmvr-es/common/math/cartesian_motion_math.h index 8f5cb8ca..2d0f6b45 100644 --- a/cmvr-es/common/math/cartesian_motion_math.h +++ b/cmvr-es/common/math/cartesian_motion_math.h @@ -43,6 +43,15 @@ inline double directionDeviationDeg(const Eigen::Vector3d& desired, return std::acos(direction_cos) * rad_to_deg; } +inline double lateralDistanceToLine(const Eigen::Vector3d& start, + const Eigen::Vector3d& direction, + const Eigen::Vector3d& point) +{ + const Eigen::Vector3d delta = point - start; + const Eigen::Vector3d lateral = delta - delta.dot(direction) * direction; + return lateral.norm(); +} + inline Eigen::Vector3d rotationVector(const Eigen::Matrix3d& rotation) { Eigen::AngleAxisd angle_axis(rotation); diff --git a/cmvr-es/config/devices/arm/arm.pb.txt b/cmvr-es/config/devices/arm/arm.pb.txt index 147f1a1f..714c5c9b 100644 --- a/cmvr-es/config/devices/arm/arm.pb.txt +++ b/cmvr-es/config/devices/arm/arm.pb.txt @@ -29,14 +29,30 @@ arm { pos_eps: 1e-6 rot_eps: 1e-6 damping: 1e-6 - joint_limits { - source: JOINT_LIMIT_SOURCE_URDF - } - joint_limit_avoidance { - enable: false - gain: 0.2 - margin_ratio: 0.15 - max_push: 0.25 + joint_limit_policy { + limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 } + } + soft_limit { + enable: true + margin_ratio: 0.01 + min_margin_rad: 0.01 + } + avoidance { + enable: false + gain: 0.2 + margin_ratio: 0.15 + max_push: 0.25 + weight: 0.05 + } } } } @@ -52,62 +68,73 @@ arm { } move_l { - pinocchio_qp_cartesian_motion_planner { + pinocchio_cartesian_motion_planner { sample_period_s: 0.001 position_gain: 4.0 rotation_gain: 4.0 - qp { - joint_limits { - source: JOINT_LIMIT_SOURCE_URDF - } - twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } - qdot_regularization: 1e-4 - prev_qdot_regularization: 1e-4 - solver_eps: 1e-3 - joint_limit_avoidance { - enable: false - margin_ratio: 0.15 - gain: 0.2 - max_push: 0.25 - weight: 0.05 - } + line_deviation_check { + enable: true + line_deviation_warn_m: 0.01 + line_deviation_stop_m: 0.03 + line_direction_warn_deg: 20.0 + line_direction_stop_deg: 45.0 + line_direction_reset_deg: 10.0 + line_check_min_distance_m: 0.01 + } + joint_continuity_check { + enable: true + max_joint_delta_rad: 0.05 + max_joint_velocity_rad_s: 10.0 + max_joint_acceleration_rad_s2: 5000.0 + } + cartesian_step_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 45.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 45.0 + min_desired_linear_speed: 1e-4 + min_desired_angular_speed: 1e-4 } } } speed_l { - pinocchio_qp_cartesian_motion_planner { - qp { - joint_limits { - source: JOINT_LIMIT_SOURCE_URDF - } - twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } - qdot_regularization: 1e-4 - prev_qdot_regularization: 2e-2 - solver_eps: 1e-3 - joint_limit_avoidance { - enable: false - margin_ratio: 0.15 - gain: 0.2 - max_push: 0.25 - weight: 0.05 - } - } + pinocchio_cartesian_motion_planner { linear_velocity_max: 0.55 linear_acceleration_max: 5.0 linear_jerk_max: 10.0 angular_velocity_max: 1.0 angular_acceleration_max: 5.0 angular_jerk_max: 12.0 - joint_acceleration_max: 8.0 linear_target_replan_threshold: 1e-4 angular_target_replan_threshold: 1e-4 linear_reverse_cos_threshold: -0.8660254037844386 linear_reverse_switch_speed_threshold: 1e-3 - normal_direction_deviation_deg: 10.0 - mild_direction_deviation_deg: 25.0 - severe_direction_deviation_deg: 45.0 - linear_min_speed_ratio: 0.2 + enforce_joint_acceleration_limits: true + line_deviation_check { + enable: true + line_deviation_warn_m: 0.01 + line_deviation_stop_m: 0.03 + line_direction_warn_deg: 20.0 + line_direction_stop_deg: 45.0 + line_direction_reset_deg: 10.0 + line_check_min_distance_m: 0.01 + } + joint_velocity_check { + enable: true + max_joint_velocity_rad_s: 30.0 + max_joint_acceleration_rad_s2: 10000.0 + } + cartesian_velocity_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 5.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 5.0 + min_desired_linear_speed: 1e-4 + min_desired_angular_speed: 1e-4 + } } speed_l_controller { @@ -116,6 +143,7 @@ arm { stop_twist_norm: 1e-9 stop_command_velocity_norm: 1e-3 stop_measured_velocity_norm: 1e-2 + stop_acceleration: 10 } } } diff --git a/cmvr-es/config/devices/arm/arm_dls_ik_dls_motion.pb.txt b/cmvr-es/config/devices/arm/arm_dls_ik_dls_motion.pb.txt deleted file mode 100644 index 8cf2cdb4..00000000 --- a/cmvr-es/config/devices/arm/arm_dls_ik_dls_motion.pb.txt +++ /dev/null @@ -1,92 +0,0 @@ -arm { - robot_arms { - id: "right_arm" - - motor { - motor_system_id: "ti5_motors" - motor_group_ids: "right_arm_can" - dof: 7 - joint_names: "R_SHOULDER_P" - joint_names: "R_SHOULDER_R" - joint_names: "R_SHOULDER_Y" - joint_names: "R_ELBOW_R" - joint_names: "R_WRIST_P" - joint_names: "R_WRIST_Y" - joint_names: "R_WRIST_R" - upd_freq: 1000 - buffer_size: 50 - default_vel: 1.0 - default_acc: 2.0 - } - - kinematics { - pinocchio_dls_ik_solver { - urdf_path: "model/xiaoyan_description/dual_arm.urdf" - base_frame_name: "PELVIS_S" - flange_frame_name: "R_WRIST_R_S" - tcp_frame_name: "R_FINGER_TIP_FIXED" - max_iters: 100 - pos_eps: 1e-6 - rot_eps: 1e-6 - damping: 1e-6 - joint_limits { - source: JOINT_LIMIT_SOURCE_URDF - } - joint_limit_avoidance { - enable: false - gain: 0.2 - margin_ratio: 0.15 - max_push: 0.25 - } - } - } - - motion { - move_j { - toppra_joint_motion_planner { - path_type: TOPPRA_PATH_TYPE_QUINTIC - sample_period_s: 0.001 - grid_size: 150 - high_grid_size: 300 - } - } - - move_l { - pinocchio_dls_cartesian_motion_planner { - sample_period_s: 0.001 - position_gain: 4.0 - rotation_gain: 4.0 - } - } - - speed_l { - pinocchio_dls_cartesian_motion_planner { - linear_velocity_max: 0.55 - linear_acceleration_max: 5.0 - linear_jerk_max: 10.0 - angular_velocity_max: 1.0 - angular_acceleration_max: 5.0 - angular_jerk_max: 12.0 - joint_acceleration_max: 8.0 - linear_target_replan_threshold: 1e-4 - angular_target_replan_threshold: 1e-4 - linear_reverse_cos_threshold: -0.8660254037844386 - linear_reverse_switch_speed_threshold: 1e-3 - normal_direction_deviation_deg: 10.0 - mild_direction_deviation_deg: 25.0 - severe_direction_deviation_deg: 45.0 - linear_min_speed_ratio: 0.2 - } - - speed_l_controller { - cartesian_velocity_controller { - control_period_s: 0.001 - stop_twist_norm: 1e-9 - stop_command_velocity_norm: 1e-3 - stop_measured_velocity_norm: 1e-2 - } - } - } - } - } -} diff --git a/cmvr-es/config/devices/arm/arm_dls_ik_qp_motion.pb.txt b/cmvr-es/config/devices/arm/arm_dls_ik_qp_motion.pb.txt deleted file mode 100644 index 9943fd8a..00000000 --- a/cmvr-es/config/devices/arm/arm_dls_ik_qp_motion.pb.txt +++ /dev/null @@ -1,138 +0,0 @@ -arm { - robot_arms { - id: "right_arm" - - motor { - motor_system_id: "ti5_motors" - motor_group_ids: "right_arm_can" - dof: 7 - joint_names: "R_SHOULDER_P" - joint_names: "R_SHOULDER_R" - joint_names: "R_SHOULDER_Y" - joint_names: "R_ELBOW_R" - joint_names: "R_WRIST_P" - joint_names: "R_WRIST_Y" - joint_names: "R_WRIST_R" - upd_freq: 1000 - buffer_size: 50 - default_vel: 1.0 - default_acc: 2.0 - } - - kinematics { - pinocchio_dls_ik_solver { - urdf_path: "model/xiaoyan_description/dual_arm.urdf" - base_frame_name: "PELVIS_S" - flange_frame_name: "R_WRIST_R_S" - tcp_frame_name: "R_FINGER_TIP_FIXED" - max_iters: 100 - pos_eps: 1e-6 - rot_eps: 1e-6 - damping: 1e-6 - joint_limits { - source: JOINT_LIMIT_SOURCE_URDF - } - joint_limit_avoidance { - enable: false - gain: 0.2 - margin_ratio: 0.15 - max_push: 0.25 - } - } - } - - motion { - move_j { - toppra_joint_motion_planner { - path_type: TOPPRA_PATH_TYPE_QUINTIC - sample_period_s: 0.001 - grid_size: 150 - high_grid_size: 300 - } - } - - move_l { - pinocchio_qp_cartesian_motion_planner { - sample_period_s: 0.001 - position_gain: 4.0 - rotation_gain: 4.0 - qp { - joint_limits { - source: JOINT_LIMIT_SOURCE_URDF - } - twist_tracking_weight { - x: 1.0 - y: 1.0 - z: 1.0 - rx: 0.5 - ry: 0.5 - rz: 0.5 - } - qdot_regularization: 1e-4 - prev_qdot_regularization: 1e-4 - solver_eps: 1e-3 - joint_limit_avoidance { - enable: false - margin_ratio: 0.15 - gain: 0.2 - max_push: 0.25 - weight: 0.05 - } - } - } - } - - speed_l { - pinocchio_qp_cartesian_motion_planner { - qp { - joint_limits { - source: JOINT_LIMIT_SOURCE_URDF - } - twist_tracking_weight { - x: 1.0 - y: 1.0 - z: 1.0 - rx: 0.5 - ry: 0.5 - rz: 0.5 - } - qdot_regularization: 1e-4 - prev_qdot_regularization: 2e-2 - solver_eps: 1e-3 - joint_limit_avoidance { - enable: false - margin_ratio: 0.15 - gain: 0.2 - max_push: 0.25 - weight: 0.05 - } - } - linear_velocity_max: 0.55 - linear_acceleration_max: 5.0 - linear_jerk_max: 10.0 - angular_velocity_max: 1.0 - angular_acceleration_max: 5.0 - angular_jerk_max: 12.0 - joint_acceleration_max: 8.0 - linear_target_replan_threshold: 1e-4 - angular_target_replan_threshold: 1e-4 - linear_reverse_cos_threshold: -0.8660254037844386 - linear_reverse_switch_speed_threshold: 1e-3 - normal_direction_deviation_deg: 10.0 - mild_direction_deviation_deg: 25.0 - severe_direction_deviation_deg: 45.0 - linear_min_speed_ratio: 0.2 - } - - speed_l_controller { - cartesian_velocity_controller { - control_period_s: 0.001 - stop_twist_norm: 1e-9 - stop_command_velocity_norm: 1e-3 - stop_measured_velocity_norm: 1e-2 - } - } - } - } - } -} diff --git a/cmvr-es/config/devices/arm/arm_mujoco.pb.txt b/cmvr-es/config/devices/arm/arm_mujoco.pb.txt index a9cd606f..0d5604bd 100644 --- a/cmvr-es/config/devices/arm/arm_mujoco.pb.txt +++ b/cmvr-es/config/devices/arm/arm_mujoco.pb.txt @@ -1,10 +1,10 @@ arm { robot_arms { - id: "right_arm_mujoco" + id: "mujoco_right_arm" motor { motor_system_id: "mujoco_motors" - motor_group_ids: "right_arm_mujoco" + motor_group_ids: "mujoco_right_arm" dof: 7 joint_names: "R_SHOULDER_P" joint_names: "R_SHOULDER_R" @@ -29,14 +29,30 @@ arm { pos_eps: 1e-6 rot_eps: 1e-6 damping: 1e-6 - joint_limits { - source: JOINT_LIMIT_SOURCE_URDF - } - joint_limit_avoidance { - enable: true - gain: 0.2 - margin_ratio: 0.15 - max_push: 0.25 + joint_limit_policy { + limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 100.0 } + joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 100.0 } + joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 100.0 } + joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 100.0 } + joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 100.0 } + joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 100.0 } + joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 100.0 } + } + soft_limit { + enable: true + margin_ratio: 0.01 + min_margin_rad: 0.01 + } + avoidance { + enable: false + gain: 0.2 + margin_ratio: 0.15 + max_push: 0.25 + weight: 2.0 + } } } } @@ -52,62 +68,73 @@ arm { } move_l { - pinocchio_qp_cartesian_motion_planner { + pinocchio_cartesian_motion_planner { sample_period_s: 0.001 position_gain: 4.0 rotation_gain: 4.0 - qp { - joint_limits { - source: JOINT_LIMIT_SOURCE_URDF - } - twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } - qdot_regularization: 1e-4 - prev_qdot_regularization: 1e-4 - solver_eps: 1e-3 - joint_limit_avoidance { - enable: false - margin_ratio: 0.15 - gain: 0.2 - max_push: 0.25 - weight: 0.05 - } + line_deviation_check { + enable: true + line_deviation_warn_m: 0.01 + line_deviation_stop_m: 0.03 + line_direction_warn_deg: 20.0 + line_direction_stop_deg: 45.0 + line_direction_reset_deg: 10.0 + line_check_min_distance_m: 0.01 + } + joint_continuity_check { + enable: true + max_joint_delta_rad: 0.05 + max_joint_velocity_rad_s: 10.0 + max_joint_acceleration_rad_s2: 5000.0 + } + cartesian_step_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 5.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 5.0 + min_desired_linear_speed: 1e-4 + min_desired_angular_speed: 1e-4 } } } speed_l { - pinocchio_qp_cartesian_motion_planner { - qp { - joint_limits { - source: JOINT_LIMIT_SOURCE_URDF - } - twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } - qdot_regularization: 1e-4 - prev_qdot_regularization: 2e-2 - solver_eps: 1e-3 - joint_limit_avoidance { - enable: false - margin_ratio: 0.15 - gain: 0.2 - max_push: 0.25 - weight: 0.05 - } - } - linear_velocity_max: 0.55 - linear_acceleration_max: 5.0 - linear_jerk_max: 10.0 + pinocchio_cartesian_motion_planner { + linear_velocity_max: 2.0 + linear_acceleration_max: 15.0 + linear_jerk_max: 60.0 angular_velocity_max: 1.0 angular_acceleration_max: 5.0 angular_jerk_max: 12.0 - joint_acceleration_max: 8.0 linear_target_replan_threshold: 1e-4 angular_target_replan_threshold: 1e-4 linear_reverse_cos_threshold: -0.8660254037844386 linear_reverse_switch_speed_threshold: 1e-3 - normal_direction_deviation_deg: 10.0 - mild_direction_deviation_deg: 25.0 - severe_direction_deviation_deg: 45.0 - linear_min_speed_ratio: 0.2 + enforce_joint_acceleration_limits: true + line_deviation_check { + enable: true + line_deviation_warn_m: 0.01 + line_deviation_stop_m: 0.03 + line_direction_warn_deg: 20.0 + line_direction_stop_deg: 45.0 + line_direction_reset_deg: 10.0 + line_check_min_distance_m: 0.01 + } + joint_velocity_check { + enable: true + max_joint_velocity_rad_s: 10.0 + max_joint_acceleration_rad_s2: 5000.0 + } + cartesian_velocity_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 5.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 5.0 + min_desired_linear_speed: 1e-4 + min_desired_angular_speed: 1e-4 + } } speed_l_controller { @@ -116,6 +143,7 @@ arm { stop_twist_norm: 1e-9 stop_command_velocity_norm: 1e-3 stop_measured_velocity_norm: 1e-2 + stop_acceleration: 10 } } } diff --git a/cmvr-es/config/devices/arm/arm_mujoco_dls_ik_dls_motion.pb.txt b/cmvr-es/config/devices/arm/arm_mujoco_dls_ik_dls_motion.pb.txt deleted file mode 100644 index 1d7ea131..00000000 --- a/cmvr-es/config/devices/arm/arm_mujoco_dls_ik_dls_motion.pb.txt +++ /dev/null @@ -1,92 +0,0 @@ -arm { - robot_arms { - id: "right_arm_mujoco" - - motor { - motor_system_id: "mujoco_motors" - motor_group_ids: "right_arm_mujoco" - dof: 7 - joint_names: "R_SHOULDER_P" - joint_names: "R_SHOULDER_R" - joint_names: "R_SHOULDER_Y" - joint_names: "R_ELBOW_R" - joint_names: "R_WRIST_P" - joint_names: "R_WRIST_Y" - joint_names: "R_WRIST_R" - upd_freq: 1000 - buffer_size: 50 - default_vel: 1.0 - default_acc: 2.0 - } - - kinematics { - pinocchio_dls_ik_solver { - urdf_path: "model/xiaoyan_description/dual_arm.urdf" - base_frame_name: "PELVIS_S" - flange_frame_name: "R_WRIST_R_S" - tcp_frame_name: "R_FINGER_TIP_FIXED" - max_iters: 100 - pos_eps: 1e-6 - rot_eps: 1e-6 - damping: 1e-6 - joint_limits { - source: JOINT_LIMIT_SOURCE_URDF - } - joint_limit_avoidance { - enable: false - gain: 0.2 - margin_ratio: 0.15 - max_push: 0.25 - } - } - } - - motion { - move_j { - toppra_joint_motion_planner { - path_type: TOPPRA_PATH_TYPE_QUINTIC - sample_period_s: 0.001 - grid_size: 150 - high_grid_size: 300 - } - } - - move_l { - pinocchio_dls_cartesian_motion_planner { - sample_period_s: 0.001 - position_gain: 4.0 - rotation_gain: 4.0 - } - } - - speed_l { - pinocchio_dls_cartesian_motion_planner { - linear_velocity_max: 0.55 - linear_acceleration_max: 5.0 - linear_jerk_max: 10.0 - angular_velocity_max: 1.0 - angular_acceleration_max: 5.0 - angular_jerk_max: 12.0 - joint_acceleration_max: 8.0 - linear_target_replan_threshold: 1e-4 - angular_target_replan_threshold: 1e-4 - linear_reverse_cos_threshold: -0.8660254037844386 - linear_reverse_switch_speed_threshold: 1e-3 - normal_direction_deviation_deg: 10.0 - mild_direction_deviation_deg: 25.0 - severe_direction_deviation_deg: 45.0 - linear_min_speed_ratio: 0.2 - } - - speed_l_controller { - cartesian_velocity_controller { - control_period_s: 0.001 - stop_twist_norm: 1e-9 - stop_command_velocity_norm: 1e-3 - stop_measured_velocity_norm: 1e-2 - } - } - } - } - } -} diff --git a/cmvr-es/config/devices/arm/arm_mujoco_dls_ik_qp_motion.pb.txt b/cmvr-es/config/devices/arm/arm_mujoco_dls_ik_qp_motion.pb.txt deleted file mode 100644 index 6c1f310e..00000000 --- a/cmvr-es/config/devices/arm/arm_mujoco_dls_ik_qp_motion.pb.txt +++ /dev/null @@ -1,120 +0,0 @@ -arm { - robot_arms { - id: "right_arm_mujoco" - - motor { - motor_system_id: "mujoco_motors" - motor_group_ids: "right_arm_mujoco" - dof: 7 - joint_names: "R_SHOULDER_P" - joint_names: "R_SHOULDER_R" - joint_names: "R_SHOULDER_Y" - joint_names: "R_ELBOW_R" - joint_names: "R_WRIST_P" - joint_names: "R_WRIST_Y" - joint_names: "R_WRIST_R" - upd_freq: 1000 - buffer_size: 50 - default_vel: 1.0 - default_acc: 2.0 - } - - kinematics { - pinocchio_dls_ik_solver { - urdf_path: "model/xiaoyan_description/dual_arm.urdf" - base_frame_name: "PELVIS_S" - flange_frame_name: "R_WRIST_R_S" - tcp_frame_name: "R_FINGER_TIP_FIXED" - max_iters: 100 - pos_eps: 1e-6 - rot_eps: 1e-6 - damping: 1e-6 - joint_limits { - source: JOINT_LIMIT_SOURCE_URDF - } - joint_limit_avoidance { - enable: false - gain: 0.2 - margin_ratio: 0.15 - max_push: 0.25 - } - } - } - - motion { - move_j { - toppra_joint_motion_planner { - path_type: TOPPRA_PATH_TYPE_QUINTIC - sample_period_s: 0.001 - grid_size: 150 - high_grid_size: 300 - } - } - - move_l { - pinocchio_qp_cartesian_motion_planner { - sample_period_s: 0.001 - position_gain: 4.0 - rotation_gain: 4.0 - qp { - joint_limits { source: JOINT_LIMIT_SOURCE_URDF } - twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } - qdot_regularization: 1e-4 - prev_qdot_regularization: 1e-4 - solver_eps: 1e-3 - joint_limit_avoidance { - enable: false - margin_ratio: 0.15 - gain: 0.2 - max_push: 0.25 - weight: 0.05 - } - } - } - } - - speed_l { - pinocchio_qp_cartesian_motion_planner { - qp { - joint_limits { source: JOINT_LIMIT_SOURCE_URDF } - twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } - qdot_regularization: 1e-4 - prev_qdot_regularization: 2e-2 - solver_eps: 1e-3 - joint_limit_avoidance { - enable: false - margin_ratio: 0.15 - gain: 0.2 - max_push: 0.25 - weight: 0.05 - } - } - linear_velocity_max: 0.55 - linear_acceleration_max: 5.0 - linear_jerk_max: 10.0 - angular_velocity_max: 1.0 - angular_acceleration_max: 5.0 - angular_jerk_max: 12.0 - joint_acceleration_max: 8.0 - linear_target_replan_threshold: 1e-4 - angular_target_replan_threshold: 1e-4 - linear_reverse_cos_threshold: -0.8660254037844386 - linear_reverse_switch_speed_threshold: 1e-3 - normal_direction_deviation_deg: 10.0 - mild_direction_deviation_deg: 25.0 - severe_direction_deviation_deg: 45.0 - linear_min_speed_ratio: 0.2 - } - - speed_l_controller { - cartesian_velocity_controller { - control_period_s: 0.001 - stop_twist_norm: 1e-9 - stop_command_velocity_norm: 1e-3 - stop_measured_velocity_norm: 1e-2 - } - } - } - } - } -} diff --git a/cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt b/cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt new file mode 100644 index 00000000..ef729d58 --- /dev/null +++ b/cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt @@ -0,0 +1,153 @@ +arm { + robot_arms { + id: "mujoco_right_arm" + + motor { + motor_system_id: "mujoco_motors" + motor_group_ids: "mujoco_right_arm" + dof: 7 + joint_names: "R_SHOULDER_P" + joint_names: "R_SHOULDER_R" + joint_names: "R_SHOULDER_Y" + joint_names: "R_ELBOW_R" + joint_names: "R_WRIST_P" + joint_names: "R_WRIST_Y" + joint_names: "R_WRIST_R" + upd_freq: 1000 + buffer_size: 50 + default_vel: 1.0 + default_acc: 2.0 + } + + kinematics { + pinocchio_qp_ik_solver { + urdf_path: "model/xiaoyan_description/dual_arm.urdf" + base_frame_name: "PELVIS_S" + flange_frame_name: "R_WRIST_R_S" + tcp_frame_name: "R_FINGER_TIP_FIXED" + lambda: 1e-4 + w_posrot: 0.5 + max_iters: 100 + tol: 1e-6 + qp_time_limit: 1e-2 + joint_limit_policy { + limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 100.0 } + joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 100.0 } + joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 100.0 } + joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 100.0 } + joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 100.0 } + joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 100.0 } + joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 100.0 } + } + soft_limit { + enable: true + margin_ratio: 0.01 + min_margin_rad: 0.01 + } + avoidance { + enable: false + gain: 0.2 + margin_ratio: 0.01 + max_push: 0.02 + weight: 0.05 + } + } + } + } + + motion { + move_j { + toppra_joint_motion_planner { + path_type: TOPPRA_PATH_TYPE_QUINTIC + sample_period_s: 0.001 + grid_size: 150 + high_grid_size: 300 + } + } + + move_l { + pinocchio_cartesian_motion_planner { + sample_period_s: 0.001 + position_gain: 4.0 + rotation_gain: 4.0 + line_deviation_check { + enable: true + line_deviation_warn_m: 0.01 + line_deviation_stop_m: 0.03 + line_direction_warn_deg: 20.0 + line_direction_stop_deg: 45.0 + line_direction_reset_deg: 10.0 + line_check_min_distance_m: 0.01 + } + joint_continuity_check { + enable: true + max_joint_delta_rad: 0.05 + max_joint_velocity_rad_s: 10.0 + max_joint_acceleration_rad_s2: 5000.0 + } + cartesian_step_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 5.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 5.0 + min_desired_linear_speed: 1e-4 + min_desired_angular_speed: 1e-4 + } + } + } + + speed_l { + pinocchio_cartesian_motion_planner { + linear_velocity_max: 2.0 + linear_acceleration_max: 15.0 + linear_jerk_max: 60.0 + angular_velocity_max: 1.0 + angular_acceleration_max: 5.0 + angular_jerk_max: 12.0 + linear_target_replan_threshold: 1e-4 + angular_target_replan_threshold: 1e-4 + linear_reverse_cos_threshold: -0.8660254037844386 + linear_reverse_switch_speed_threshold: 1e-3 + enforce_joint_acceleration_limits: true + line_deviation_check { + enable: true + line_deviation_warn_m: 0.005 + line_deviation_stop_m: 0.005 + line_direction_warn_deg: 20.0 + line_direction_stop_deg: 45.0 + line_direction_reset_deg: 10.0 + line_check_min_distance_m: 0.01 + } + joint_velocity_check { + enable: true + max_joint_velocity_rad_s: 10.0 + max_joint_acceleration_rad_s2: 5000.0 + } + cartesian_velocity_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 20.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 20.0 + min_desired_linear_speed: 1e-4 + min_desired_angular_speed: 1e-4 + } + } + + speed_l_controller { + cartesian_velocity_controller { + control_period_s: 0.001 + stop_twist_norm: 1e-9 + stop_command_velocity_norm: 1e-3 + stop_measured_velocity_norm: 1e-2 + stop_acceleration: 5 + } + } + } + } + } +} diff --git a/cmvr-es/config/devices/arm/arm_mujoco_qp_ik_qp_motion.pb.txt b/cmvr-es/config/devices/arm/arm_mujoco_qp_ik_qp_motion.pb.txt deleted file mode 100644 index 12ec39f9..00000000 --- a/cmvr-es/config/devices/arm/arm_mujoco_qp_ik_qp_motion.pb.txt +++ /dev/null @@ -1,112 +0,0 @@ -arm { - robot_arms { - id: "right_arm_mujoco" - - motor { - motor_system_id: "mujoco_motors" - motor_group_ids: "right_arm_mujoco" - dof: 7 - joint_names: "R_SHOULDER_P" - joint_names: "R_SHOULDER_R" - joint_names: "R_SHOULDER_Y" - joint_names: "R_ELBOW_R" - joint_names: "R_WRIST_P" - joint_names: "R_WRIST_Y" - joint_names: "R_WRIST_R" - upd_freq: 1000 - buffer_size: 50 - default_vel: 1.0 - default_acc: 2.0 - } - - kinematics { - pinocchio_qp_ik_solver { - urdf_path: "model/xiaoyan_description/dual_arm.urdf" - base_frame_name: "PELVIS_S" - flange_frame_name: "R_WRIST_R_S" - tcp_frame_name: "R_FINGER_TIP_FIXED" - lambda: 1e-4 - w_posrot: 0.5 - max_iters: 100 - tol: 1e-6 - qp_time_limit: 1e-2 - } - } - - motion { - move_j { - toppra_joint_motion_planner { - path_type: TOPPRA_PATH_TYPE_QUINTIC - sample_period_s: 0.001 - grid_size: 150 - high_grid_size: 300 - } - } - - move_l { - pinocchio_qp_cartesian_motion_planner { - sample_period_s: 0.001 - position_gain: 4.0 - rotation_gain: 4.0 - qp { - joint_limits { source: JOINT_LIMIT_SOURCE_URDF } - twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } - qdot_regularization: 1e-4 - prev_qdot_regularization: 1e-4 - solver_eps: 1e-3 - joint_limit_avoidance { - enable: false - margin_ratio: 0.15 - gain: 0.2 - max_push: 0.25 - weight: 0.05 - } - } - } - } - - speed_l { - pinocchio_qp_cartesian_motion_planner { - qp { - joint_limits { source: JOINT_LIMIT_SOURCE_URDF } - twist_tracking_weight { x: 1.0 y: 1.0 z: 1.0 rx: 0.5 ry: 0.5 rz: 0.5 } - qdot_regularization: 1e-4 - prev_qdot_regularization: 2e-2 - solver_eps: 1e-3 - joint_limit_avoidance { - enable: false - margin_ratio: 0.15 - gain: 0.2 - max_push: 0.25 - weight: 0.05 - } - } - linear_velocity_max: 0.55 - linear_acceleration_max: 5.0 - linear_jerk_max: 10.0 - angular_velocity_max: 1.0 - angular_acceleration_max: 5.0 - angular_jerk_max: 12.0 - joint_acceleration_max: 8.0 - linear_target_replan_threshold: 1e-4 - angular_target_replan_threshold: 1e-4 - linear_reverse_cos_threshold: -0.8660254037844386 - linear_reverse_switch_speed_threshold: 1e-3 - normal_direction_deviation_deg: 10.0 - mild_direction_deviation_deg: 25.0 - severe_direction_deviation_deg: 45.0 - linear_min_speed_ratio: 0.2 - } - - speed_l_controller { - cartesian_velocity_controller { - control_period_s: 0.001 - stop_twist_norm: 1e-9 - stop_command_velocity_norm: 1e-3 - stop_measured_velocity_norm: 1e-2 - } - } - } - } - } -} diff --git a/cmvr-es/config/devices/arm/arm_qp.pb.txt b/cmvr-es/config/devices/arm/arm_qp.pb.txt new file mode 100644 index 00000000..9e25391f --- /dev/null +++ b/cmvr-es/config/devices/arm/arm_qp.pb.txt @@ -0,0 +1,153 @@ +arm { + robot_arms { + id: "right_arm" + + motor { + motor_system_id: "ti5_motors" + motor_group_ids: "right_arm_can" + dof: 7 + joint_names: "R_SHOULDER_P" + joint_names: "R_SHOULDER_R" + joint_names: "R_SHOULDER_Y" + joint_names: "R_ELBOW_R" + joint_names: "R_WRIST_P" + joint_names: "R_WRIST_Y" + joint_names: "R_WRIST_R" + upd_freq: 1000 + buffer_size: 50 + default_vel: 1.0 + default_acc: 2.0 + } + + kinematics { + pinocchio_qp_ik_solver { + urdf_path: "model/xiaoyan_description/dual_arm.urdf" + base_frame_name: "PELVIS_S" + flange_frame_name: "R_WRIST_R_S" + tcp_frame_name: "R_FINGER_TIP_FIXED" + lambda: 1e-4 + w_posrot: 0.5 + max_iters: 100 + tol: 1e-6 + qp_time_limit: 1e-2 + joint_limit_policy { + limits { + enable: true + source: JOINT_LIMIT_SOURCE_CUSTOM + joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 100.0 } + joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 } + joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 } + } + soft_limit { + enable: true + margin_ratio: 0.01 + min_margin_rad: 0.01 + } + avoidance { + enable: false + gain: 0.2 + margin_ratio: 0.01 + max_push: 0.02 + weight: 0.05 + } + } + } + } + + motion { + move_j { + toppra_joint_motion_planner { + path_type: TOPPRA_PATH_TYPE_QUINTIC + sample_period_s: 0.001 + grid_size: 150 + high_grid_size: 300 + } + } + + move_l { + pinocchio_cartesian_motion_planner { + sample_period_s: 0.001 + position_gain: 4.0 + rotation_gain: 4.0 + line_deviation_check { + enable: true + line_deviation_warn_m: 0.01 + line_deviation_stop_m: 0.03 + line_direction_warn_deg: 20.0 + line_direction_stop_deg: 45.0 + line_direction_reset_deg: 10.0 + line_check_min_distance_m: 0.01 + } + joint_continuity_check { + enable: true + max_joint_delta_rad: 0.05 + max_joint_velocity_rad_s: 10.0 + max_joint_acceleration_rad_s2: 5000.0 + } + cartesian_step_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 45.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 45.0 + min_desired_linear_speed: 1e-4 + min_desired_angular_speed: 1e-4 + } + } + } + + speed_l { + pinocchio_cartesian_motion_planner { + linear_velocity_max: 0.55 + linear_acceleration_max: 5.0 + linear_jerk_max: 10.0 + angular_velocity_max: 1.0 + angular_acceleration_max: 5.0 + angular_jerk_max: 12.0 + linear_target_replan_threshold: 1e-4 + angular_target_replan_threshold: 1e-4 + linear_reverse_cos_threshold: -0.8660254037844386 + linear_reverse_switch_speed_threshold: 1e-3 + enforce_joint_acceleration_limits: true + line_deviation_check { + enable: true + line_deviation_warn_m: 0.01 + line_deviation_stop_m: 0.03 + line_direction_warn_deg: 20.0 + line_direction_stop_deg: 45.0 + line_direction_reset_deg: 10.0 + line_check_min_distance_m: 0.01 + } + joint_velocity_check { + enable: true + max_joint_velocity_rad_s: 30.0 + max_joint_acceleration_rad_s2: 10000.0 + } + cartesian_velocity_feasibility_check { + enable: true + min_linear_speed_ratio: 0.2 + max_linear_direction_deviation_deg: 5.0 + min_angular_speed_ratio: 0.2 + max_angular_direction_deviation_deg: 5.0 + min_desired_linear_speed: 1e-4 + min_desired_angular_speed: 1e-4 + } + } + + speed_l_controller { + cartesian_velocity_controller { + control_period_s: 0.001 + stop_twist_norm: 1e-9 + stop_command_velocity_norm: 1e-3 + stop_measured_velocity_norm: 1e-2 + stop_acceleration: 0.5 + } + } + } + } + } +} diff --git a/cmvr-es/config/devices/arm/arm_qp_ik_qp_motion.pb.txt b/cmvr-es/config/devices/arm/arm_qp_ik_qp_motion.pb.txt deleted file mode 100644 index e8f7dd20..00000000 --- a/cmvr-es/config/devices/arm/arm_qp_ik_qp_motion.pb.txt +++ /dev/null @@ -1,130 +0,0 @@ -arm { - robot_arms { - id: "right_arm" - - motor { - motor_system_id: "ti5_motors" - motor_group_ids: "right_arm_can" - dof: 7 - joint_names: "R_SHOULDER_P" - joint_names: "R_SHOULDER_R" - joint_names: "R_SHOULDER_Y" - joint_names: "R_ELBOW_R" - joint_names: "R_WRIST_P" - joint_names: "R_WRIST_Y" - joint_names: "R_WRIST_R" - upd_freq: 1000 - buffer_size: 50 - default_vel: 1.0 - default_acc: 2.0 - } - - kinematics { - pinocchio_qp_ik_solver { - urdf_path: "model/xiaoyan_description/dual_arm.urdf" - base_frame_name: "PELVIS_S" - flange_frame_name: "R_WRIST_R_S" - tcp_frame_name: "R_FINGER_TIP_FIXED" - lambda: 1e-4 - w_posrot: 0.5 - max_iters: 100 - tol: 1e-6 - qp_time_limit: 1e-2 - } - } - - motion { - move_j { - toppra_joint_motion_planner { - path_type: TOPPRA_PATH_TYPE_QUINTIC - sample_period_s: 0.001 - grid_size: 150 - high_grid_size: 300 - } - } - - move_l { - pinocchio_qp_cartesian_motion_planner { - sample_period_s: 0.001 - position_gain: 4.0 - rotation_gain: 4.0 - qp { - joint_limits { - source: JOINT_LIMIT_SOURCE_URDF - } - twist_tracking_weight { - x: 1.0 - y: 1.0 - z: 1.0 - rx: 0.5 - ry: 0.5 - rz: 0.5 - } - qdot_regularization: 1e-4 - prev_qdot_regularization: 1e-4 - solver_eps: 1e-3 - joint_limit_avoidance { - enable: false - margin_ratio: 0.15 - gain: 0.2 - max_push: 0.25 - weight: 0.05 - } - } - } - } - - speed_l { - pinocchio_qp_cartesian_motion_planner { - qp { - joint_limits { - source: JOINT_LIMIT_SOURCE_URDF - } - twist_tracking_weight { - x: 1.0 - y: 1.0 - z: 1.0 - rx: 0.5 - ry: 0.5 - rz: 0.5 - } - qdot_regularization: 1e-4 - prev_qdot_regularization: 2e-2 - solver_eps: 1e-3 - joint_limit_avoidance { - enable: false - margin_ratio: 0.15 - gain: 0.2 - max_push: 0.25 - weight: 0.05 - } - } - linear_velocity_max: 0.55 - linear_acceleration_max: 5.0 - linear_jerk_max: 10.0 - angular_velocity_max: 1.0 - angular_acceleration_max: 5.0 - angular_jerk_max: 12.0 - joint_acceleration_max: 8.0 - linear_target_replan_threshold: 1e-4 - angular_target_replan_threshold: 1e-4 - linear_reverse_cos_threshold: -0.8660254037844386 - linear_reverse_switch_speed_threshold: 1e-3 - normal_direction_deviation_deg: 10.0 - mild_direction_deviation_deg: 25.0 - severe_direction_deviation_deg: 45.0 - linear_min_speed_ratio: 0.2 - } - - speed_l_controller { - cartesian_velocity_controller { - control_period_s: 0.001 - stop_twist_norm: 1e-9 - stop_command_velocity_norm: 1e-3 - stop_measured_velocity_norm: 1e-2 - } - } - } - } - } -} diff --git a/protos/cmvr/config/cartesian_motion_validation_config.proto b/protos/cmvr/config/cartesian_motion_validation_config.proto new file mode 100644 index 00000000..a15d2204 --- /dev/null +++ b/protos/cmvr/config/cartesian_motion_validation_config.proto @@ -0,0 +1,46 @@ +syntax = "proto3"; + +package cmvr.config; + +message CartesianLineDeviationCheckConfig { + bool enable = 1; + double line_deviation_warn_m = 2; + double line_deviation_stop_m = 3; + double line_direction_warn_deg = 4; + double line_direction_stop_deg = 5; + double line_direction_reset_deg = 6; + double line_check_min_distance_m = 7; +} + +message JointContinuityCheckConfig { + bool enable = 1; + double max_joint_delta_rad = 2; + double max_joint_velocity_rad_s = 3; + double max_joint_acceleration_rad_s2 = 4; +} + +message JointVelocityCheckConfig { + bool enable = 1; + double max_joint_velocity_rad_s = 2; + double max_joint_acceleration_rad_s2 = 3; +} + +message CartesianStepFeasibilityCheckConfig { + bool enable = 1; + double min_linear_speed_ratio = 2; + double max_linear_direction_deviation_deg = 3; + double min_angular_speed_ratio = 4; + double max_angular_direction_deviation_deg = 5; + double min_desired_linear_speed = 6; + double min_desired_angular_speed = 7; +} + +message CartesianVelocityFeasibilityCheckConfig { + bool enable = 1; + double min_linear_speed_ratio = 2; + double max_linear_direction_deviation_deg = 3; + double min_angular_speed_ratio = 4; + double max_angular_direction_deviation_deg = 5; + double min_desired_linear_speed = 6; + double min_desired_angular_speed = 7; +} diff --git a/protos/cmvr/config/pinocchio_dls_ik_config.proto b/protos/cmvr/config/pinocchio_dls_ik_config.proto index 4eb2ed0f..534a4d4e 100644 --- a/protos/cmvr/config/pinocchio_dls_ik_config.proto +++ b/protos/cmvr/config/pinocchio_dls_ik_config.proto @@ -14,13 +14,5 @@ message PinocchioDlsIKConfig { double pos_eps = 6; double rot_eps = 7; double damping = 8; - JointLimitAvoidanceConfig joint_limit_avoidance = 9; - JointLimitsConfig joint_limits = 10; -} - -message JointLimitAvoidanceConfig { - bool enable = 1; - double gain = 2; - double margin_ratio = 3; - double max_push = 4; + JointLimitPolicyConfig joint_limit_policy = 9; } diff --git a/protos/cmvr/config/pinocchio_qp_ik_config.proto b/protos/cmvr/config/pinocchio_qp_ik_config.proto index 45cbc106..4fbbace8 100644 --- a/protos/cmvr/config/pinocchio_qp_ik_config.proto +++ b/protos/cmvr/config/pinocchio_qp_ik_config.proto @@ -2,6 +2,8 @@ syntax = "proto3"; package cmvr.config; +import "cmvr/config/joint_limits_config.proto"; + // 只管 QP 这个 IK 的参数 message PinocchioQpIKConfig { string urdf_path = 1; @@ -14,4 +16,5 @@ message PinocchioQpIKConfig { int32 max_iters = 7; double tol = 8; double qp_time_limit = 9; + JointLimitPolicyConfig joint_limit_policy = 10; }