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

refactor(motion): consolidate IK and motion planners

Browse Source
This commit is contained in:
lgv 2026-06-30 16:05:25 +08:00
parent ca19449545
commit 7e803452e1
43 changed files with 1995 additions and 7138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
cmvr-es/algorithms/controllers/CMakeLists.txt
View File

@ -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
)

1
cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h
View File

@ -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<bool(std::vector<double>& q, std::vector<double>& qd)>;

14
cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp
View File

@ -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<double> acceleratio
std::lock_guard<std::mutex> 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<std::mutex> lock(mutex_);
command_active_ = false;
sendZero_();
busy_.store(false);
break;
}
CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration="
<< acceleration;
sendZero_();

17
cmvr-es/algorithms/controllers/ibvs/include/ibvs_controller.h
View File

@ -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()};

23
cmvr-es/algorithms/controllers/ibvs/src/ibvs_controller.cpp
View File

@ -114,12 +114,6 @@ bool IbvsController::init(std::shared_ptr<cmvr::PinocchioIKBase> solver,
camera_frame_name_ = camera_link;
initialized_ = solver_ != nullptr && !camera_frame_name_.empty();
if (initialized_) {
if (auto dls_solver = std::dynamic_pointer_cast<PinocchioDlsIKSolver>(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<PinocchioDlsIKSolver>(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;
}

869
cmvr-es/algorithms/controllers/tests/src/controller_test.cpp
View File

@ -1,869 +0,0 @@
//
// Created by lgv on 2026/2/10.
// TEST(contrller_test, visp_test){
#include "gtest/gtest.h"
#include <algorithm>
#include <array>
#include <atomic>
#include <chrono>
#include <cmath>
#include <limits>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
#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 <array>
#include <cmath>
#include <cstdio>
#include <vector>
#include <Eigen/Dense>
#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
// ---- ViSP ----
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpPoint.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
// Created by lgv on 2026/2/10.
#include "gtest/gtest.h"
#include <array>
#include <cmath>
#include <cstdio>
#include <vector>
#include <Eigen/Dense>
#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
// ---- ViSP ----
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpPoint.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
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<double,6,1> 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<mjtNum> jacp(3 * m->nv);
std::vector<mjtNum> jacr(3 * m->nv);
mj_jacSite(m, d, jacp.data(), jacr.data(), cam_site_id_);
Eigen::Matrix<double,6,7> 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<double,6,6> A = J * J.transpose();
A += (mu_*mu_) * Eigen::Matrix<double,6,6>::Identity();
Eigen::Matrix<double,7,1> 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<int,7> act_ids_{};
std::array<int,7> jnt_ids_{};
std::array<int,7> qpos_adr_{ { -1,-1,-1,-1,-1,-1,-1 } };
std::array<int,7> 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<double,7> 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<double,7> q_cmd_{ {0,0,0,0,0,0,0} };
};
// ---- ViSP ----
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpImage.h>
#include <visp3/detection/vpDetectorAprilTag.h>
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<device::MujocoCamera>(
[this](std::vector<unsigned char>& rgb,
std::vector<float>& 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<IbvsController>();
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<PinocchioDlsIKSolver>(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<cmvr::perception::AprilTagPerception>(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<double> 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<double> 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<double> 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<double> 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<double> 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<int, 7> act_ids_{};
std::array<int, 7> jnt_ids_{};
std::array<int, 7> 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<double, 7> 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<double, 6> 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<IbvsController> ibvs_controller_{nullptr};
std::shared_ptr<device::MujocoCamera> mujoco_camera_{nullptr};
std::shared_ptr<cmvr::perception::AprilTagPerception> perception_{nullptr};
cmvr::perception::AprilTagPerception::Options perception_opt_{};
std::array<double, 7> 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<double> &q_target) {
std::lock_guard<std::mutex> 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<double> q_local;
{
std::lock_guard<std::mutex> lock(mtx_);
q_local = q_cmd_;
}
for (int i = 0; i < 7; ++i) {
if (act_ids_[i] < 0) continue;
if (i < static_cast<int>(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<std::mutex> lock(mtx_);
q_cmd_.assign(7, 0.0);
}
private:
std::array<int, 7> act_ids_{};
bool act_ids_inited_{false};
std::vector<double> 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<double> 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();
// }
}

40
cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt
View File

@ -26,43 +26,3 @@ 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
)

12
cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h
View File

@ -54,11 +54,6 @@ public:
std::vector<double>& qdot_out,
double qdot_abs_max = std::numeric_limits<double>::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_;

13
cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h
View File

@ -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 <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/data.hpp>
@ -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<double>& 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

3
cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h
View File

@ -14,6 +14,7 @@
#include <limits>
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
#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_;

61
cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp
View File

@ -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,14 +74,13 @@ bool PinocchioDlsIKSolver::refreshJointLimits_(const config::PinocchioDlsIKConfi
}
std::unordered_map<std::string, config::JointLimitConfig> custom_limits;
if (cfg.has_joint_limits()) {
custom_limits.reserve(static_cast<std::size_t>(cfg.joint_limits().joints_size()));
for (const auto& item : cfg.joint_limits().joints()) {
const auto& limits = joint_limit_policy_.limits();
custom_limits.reserve(static_cast<std::size_t>(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<Eigen::Index>(joint_names.size());
joint_pos_lower_limits_.resize(dof);
@ -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<double,6,1>& 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];

75
cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp
View File

@ -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 <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <algorithm>
#include <cmath>
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_);

100
cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp
View File

@ -14,6 +14,8 @@
#include <algorithm> // std::clamp, std::max, std::min
#include <cmath> // std::sqrt
#include <limits>
#include <unordered_map>
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<std::string> 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<std::string, config::JointLimitConfig> custom_limits;
const auto& limits = policy.limits();
custom_limits.reserve(static_cast<std::size_t>(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<Eigen::Index>(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<std::size_t>(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);
}
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<const VectorXd> 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<double>::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;
}

661
cmvr-es/algorithms/kinematics/ik_solver/tests/src/ik_test.cpp
View File

@ -1,661 +0,0 @@
//
// Created by lgv on 11/28/25.
//
#include <iostream>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/model.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include "pinocchio/multibody/sample-models.hpp"
#include <Eigen/Dense>
#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<double>;
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<std::mutex> 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<std::mutex> 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<std::mutex> lk(mtx_);
q_cmd_.assign(7, 0.0); // reset 时把目标清零，保持 7 维
}
private:
std::array<int, 7> right_act_ids_{}; // 右臂 7 个 actuator id
std::array<int, 7> 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<double> &data,
double &mean, double &stddev)
{
if (data.empty()) {
mean = stddev = std::numeric_limits<double>::quiet_NaN();
return;
}
double sum = 0.0;
for (double x : data) sum += x;
mean = sum / static_cast<double>(data.size());
double var = 0.0;
if (data.size() > 1) {
for (double x : data) {
double d = x - mean;
var += d * d;
}
var /= static_cast<double>(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<double> times_ms; // 单次成功求解耗时
std::vector<double> pos_errors; // 末端位置误差 (m)
std::vector<double> ori_errors; // 姿态误差 (rad)
int attempts = 0; // 尝试次数（有 target_pose 就算一次）
int success = 0; // 成功次数（误差在阈值内）
};
void benchmarkIkSolversRandomJoints(DualArmViewer &viewer)
{
// ========== 1. 7 个关节限位，直接用你给的 joints_limits_ 定义 ==========
const std::array<std::pair<double,double>, 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<IKSolver*> solvers = {
&psi_solver,
&pinv_solver,
&qp_solver
};
std::vector<SolverStats> 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<double> 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<double> 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<double> 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<double> 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<double> q_cur(7, 0.0);
solver->update_joints_state(q_init);
std::vector<double> 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<double>(s.success) /
static_cast<double>(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<std::pair<double,double>, 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<IKSolver*> solvers = {
&psi_solver,
&pinv_solver,
&qp_solver
};
std::vector<SolverStats> 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<double> 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<double> 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<double> 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<double> 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<double> q_cur(7, 0.0);
solver->update_joints_state(q_init);
std::vector<double> 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<double>(s.success) /
static_cast<double>(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<double> 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<double> 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();
}

1898
cmvr-es/algorithms/kinematics/ik_solver/tests/src/srs_ik_test.cpp
View File

File diff suppressed because it is too large Load Diff

3
cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt
View File

@ -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
)

5
cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h
View File

@ -1,6 +1,7 @@
#ifndef CMVR_ES_CARTESIAN_MOTION_PLANNER_H
#define CMVR_ES_CARTESIAN_MOTION_PLANNER_H
#include <string>
#include <vector>
#include "cmvr/config/arm_config/arm_config.pb.h"
@ -12,6 +13,10 @@ struct CartesianJointTrajectory {
std::vector<std::vector<double>> position;
std::vector<std::vector<double>> velocity;
std::vector<double> time;
double planned_path_length{0.0};
double executable_path_length{0.0};
bool truncated{false};
std::string truncation_reason;
};
class CartesianMotionPlanner {

31
cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner_factory.h
View File

@ -3,10 +3,8 @@
#include <memory>
#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<PinocchioQpCartesianMotionPlanner>(solver);
case config::MoveLConfig::kPinocchioDlsCartesianMotionPlanner:
if (speed_l.algorithm_case() !=
config::SpeedLConfig::kPinocchioDlsCartesianMotionPlanner) {
return nullptr;
}
if (auto dls_solver = std::dynamic_pointer_cast<cmvr::PinocchioDlsIKSolver>(solver)) {
return std::make_shared<PinocchioDlsCartesianMotionPlanner>(dls_solver);
}
return nullptr;
return std::make_shared<PinocchioCartesianMotionPlanner>(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;

107
cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h Normal file
View File

@ -0,0 +1,107 @@
#ifndef CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H
#define CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H
#include <Eigen/Core>
#include <memory>
#include <vector>
#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<cmvr::PinocchioIKBase> 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<double>& q_start,
const std::vector<double>& qd_max,
double velocity,
double acceleration,
double jerk,
FrameType frame,
CartesianJointTrajectory& trajectory) override;
bool speedLStep(const CartesianVelocity& target_velocity,
double dt,
const std::vector<double>& q_measured,
const std::vector<double>& qd_measured,
std::vector<double>& 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<double, 6, 1>& desired_twist,
const Eigen::Matrix<double, 6, 1>& achieved_twist,
double path_position,
std::string& stop_reason) const;
bool updateAndValidateSpeedLLineDeviation_(const std::vector<double>& 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<double>& q_measured,
const Eigen::VectorXd& qdot,
double dt) const;
bool validateSpeedLCartesianVelocityFeasibility_(
const Eigen::Matrix<double, 6, 1>& desired_twist,
const Eigen::Matrix<double, 6, 1>& 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<cmvr::PinocchioIKBase> solver_{nullptr};
config::MoveLPlannerConfig movel_config_{};
config::SpeedLPlannerConfig speedl_config_{};
cmvr::CartesianTwistLimiter twist_limiter_{};
Eigen::VectorXd joint_acceleration_limits_;
std::vector<double> prev_qdot_command_;
Eigen::Matrix<double, 6, 1> speedl_command_twist_base_{Eigen::Matrix<double, 6, 1>::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

1126
cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

66
cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h
View File

@ -1,66 +0,0 @@
#ifndef CMVR_ES_PINOCCHIO_DLS_CARTESIAN_MOTION_PLANNER_H
#define CMVR_ES_PINOCCHIO_DLS_CARTESIAN_MOTION_PLANNER_H
#include <Eigen/Core>
#include <memory>
#include <vector>
#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<cmvr::PinocchioDlsIKSolver> 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<double>& q_start,
const std::vector<double>& qd_max,
double velocity,
double acceleration,
double jerk,
FrameType frame,
CartesianJointTrajectory& trajectory) override;
bool speedLStep(const CartesianVelocity& target_velocity,
double dt,
const std::vector<double>& q_measured,
const std::vector<double>& qd_measured,
std::vector<double>& 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<cmvr::PinocchioDlsIKSolver> 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<double> prev_qdot_command_;
Eigen::Matrix<double, 6, 1> speedl_command_twist_base_{Eigen::Matrix<double, 6, 1>::Zero()};
double speedl_applied_acceleration_{0.25};
bool speedl_configured_{false};
};
} // namespace cmvr::device
#endif // CMVR_ES_PINOCCHIO_DLS_CARTESIAN_MOTION_PLANNER_H

408
cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/src/pinocchio_dls_cartesian_motion_planner.cpp
View File

@ -1,408 +0,0 @@
#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_dls/include/pinocchio_dls_cartesian_motion_planner.h"
#include <Eigen/Geometry>
#include <algorithm>
#include <cmath>
#include <limits>
#include <utility>
#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<cmvr::PinocchioDlsIKSolver> 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<int>(i)) > 0.0) {
acc_limit = speedl_config_.joint_acceleration_max(static_cast<int>(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::size_t>(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<double>& q_start,
const std::vector<double>& 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<int>(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<double>(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<double> 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<double, 6, 1> target_twist_base = Eigen::Matrix<double, 6, 1>::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<double> qdot_std;
if (!solver_->solveVelocityBase(jacobian_base,
target_twist_base,
q_std,
qdot_std,
std::numeric_limits<double>::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<std::size_t>(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<double>& q_measured,
const std::vector<double>& qd_measured,
std::vector<double>& qd_command,
const FrameType frame)
{
qd_command.clear();
if (!solver_ || !speedl_configured_ || dt <= 0.0) {
return false;
}
if (static_cast<int>(q_measured.size()) != solver_->chainDof() ||
static_cast<int>(qd_measured.size()) != solver_->chainVelocityDof()) {
return false;
}
Eigen::Matrix<double, 6, 1> measured_twist_base = Eigen::Matrix<double, 6, 1>::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<double, 6, 1> 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<double, 6, 1>::Zero());
}
twist_limiter_.setTargetTwist(target_twist, common::math::toPlannerFrame(frame));
speedl_command_twist_base_ = twist_limiter_.update(dt, base_R_tool);
std::vector<double> qdot_std;
if (!solver_->solveVelocityBase(jacobian_base,
speedl_command_twist_base_,
q_measured,
qdot_std,
std::numeric_limits<double>::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<double, 6, 1> 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

76
cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h
View File

@ -1,76 +0,0 @@
#ifndef CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H
#define CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H
#include <Eigen/Core>
#include <memory>
#include <vector>
#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<cmvr::PinocchioIKBase> 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<double>& q_start,
const std::vector<double>& qd_max,
double velocity,
double acceleration,
double jerk,
FrameType frame,
CartesianJointTrajectory& trajectory) override;
bool speedLStep(const CartesianVelocity& target_velocity,
double dt,
const std::vector<double>& q_measured,
const std::vector<double>& qd_measured,
std::vector<double>& 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<double, 6, 1>& target_twist_base,
const Eigen::VectorXd& q_measured,
const Eigen::VectorXd& qd_reference,
double dt,
const std::vector<double>& 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<cmvr::PinocchioIKBase> 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<double> prev_qdot_command_;
Eigen::Matrix<double, 6, 1> speedl_command_twist_base_{Eigen::Matrix<double, 6, 1>::Zero()};
double speedl_applied_acceleration_{0.25};
bool speedl_configured_{false};
};
} // namespace cmvr::device
#endif // CMVR_ES_PINOCCHIO_QP_CARTESIAN_MOTION_PLANNER_H

590
cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/src/pinocchio_qp_cartesian_motion_planner.cpp
View File

@ -1,590 +0,0 @@
#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/include/pinocchio_qp_cartesian_motion_planner.h"
#include <algorithm>
#include <cmath>
#include <Eigen/Geometry>
#include <limits>
#include <unordered_map>
#include <utility>
#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<double, 6, 1> twistTrackingWeightOrDefault(
const cmvr::common::Vec6& value)
{
Eigen::Matrix<double, 6, 1> defaults;
defaults << 1.0, 1.0, 1.0, 0.5, 0.5, 0.5;
Eigen::Matrix<double, 6, 1> 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<cmvr::PinocchioIKBase> 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<std::string> joint_names;
if (!solver_->getChainJointNames(joint_names) || joint_names.empty()) {
return false;
}
std::unordered_map<std::string, config::JointLimitConfig> custom_limits;
if (config.has_joint_limits()) {
custom_limits.reserve(
static_cast<std::size_t>(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<Eigen::Index>(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<std::size_t>(i)]);
if (it == custom_limits.end()) {
CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] missing custom joint limit for "
<< joint_names[static_cast<std::size_t>(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<int>(dof) || std::abs(qp_solver_eps_ - eps) > 1e-12) {
qp_solver_.Setup(static_cast<int>(dof), static_cast<int>(dof), eps);
qp_solver_.ResetIsFirst();
qp_solver_dof_ = static_cast<int>(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::size_t>(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<Eigen::Index>(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<double>& q_start,
const std::vector<double>& 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<int>(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<double>(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<Eigen::Index>(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<double> 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<double, 6, 1> target_twist_base = Eigen::Matrix<double, 6, 1>::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<double, 6, 1>& target_twist_base,
const Eigen::VectorXd& q_measured,
const Eigen::VectorXd& qd_reference,
const double dt,
const std::vector<double>& 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<double, 6, 1> twist_weight =
twistTrackingWeightOrDefault(qp_config.twist_tracking_weight());
Eigen::Matrix<double, 6, 6> task_weight = Eigen::Matrix<double, 6, 6>::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<int>(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<double>::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<std::size_t>(dof)) {
const double requested_limit = std::abs(qd_max[static_cast<std::size_t>(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<int>(i)) > 0.0) {
acc_limit = speedl_config_.joint_acceleration_max(static_cast<int>(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<double, 6, 1> 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<double>& q_measured,
const std::vector<double>& qd_measured,
std::vector<double>& 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<int>(q_measured.size()) != solver_->chainDof() ||
static_cast<int>(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<double, 6, 1> measured_twist_base = Eigen::Matrix<double, 6, 1>::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<double, 6, 1> 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<double, 6, 1>::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

42
cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt
View File

@ -21,45 +21,3 @@ 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
)

732
cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter_test.cpp
View File

@ -1,732 +0,0 @@
//
// Created by lgv on 2026/3/10.
//
#include "gtest/gtest.h"
#include <Eigen/Core>
#include <matplot/matplot.h>
#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<double> time;
std::vector<double> vx;
std::vector<double> vy;
std::vector<double> vz;
std::vector<double> speed;
std::vector<double> target_speed;
std::vector<double> ax;
std::vector<double> ay;
std::vector<double> az;
std::vector<double> accel;
std::vector<double> jx;
std::vector<double> jy;
std::vector<double> jz;
std::vector<double> jerk;
constexpr double kEmergencyTime = 8.5;
constexpr double kRestartTime = 10.0;
constexpr double kTotalTime = 14.0;
const int steps = static_cast<int>(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<float> 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<float, 4>{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<float, 4>{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<float, 4>{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<double> time;
std::vector<double> wx;
std::vector<double> wy;
std::vector<double> wz;
std::vector<double> omega;
std::vector<double> target_omega;
constexpr double kEmergencyTime = 8.5;
constexpr double kRestartTime = 10.0;
constexpr double kTotalTime = 14.0;
const int steps = static_cast<int>(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<float> 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<float, 4>{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<double> time;
std::vector<double> target_speed;
std::vector<double> commanded_speed;
std::vector<double> measured_speed;
std::vector<double> commanded_vx;
std::vector<double> commanded_vy;
std::vector<double> measured_vx;
std::vector<double> measured_vy;
std::vector<double> tracking_error;
std::vector<double> acc_norm;
std::vector<double> jerk_norm;
constexpr double kTotalTime = 10.0;
constexpr double kTrackingTau = 0.08;
const double alpha = kDt / (kTrackingTau + kDt);
const int steps = static_cast<int>(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<float> 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<float, 4>{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<float, 4>{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<float, 4>{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

88
cmvr-es/algorithms/motion_planner/base_motion/joint_trajectory/toppra/src/toppra_joint_trajectory_planner_test.cpp
View File

@ -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 <memory>
using namespace cmvr;
TEST(TOPPRA_JOINT_TRAJECTORY_PLANNER_TEST,TOPPRA_TEST) {
auto planner = std::make_shared<ToppraJointTrajectoryPlanner>();
planner->setPathType(PathType::Quintic);
planner->setSymmetricLimits(std::vector<double>(7, 1.5),
std::vector<double>(7, 3.0));
TrajPtr traj;
std::vector<double> q0{0.0,-0.5,0.8,0.0,0.2,-0.3,0.1};
std::vector<double> 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<ToppraJointTrajectoryPlanner>();
planner->setPathType(PathType::Quintic);
// 7 自由度对称速度 / 加速度约束
planner->setSymmetricLimits(std::vector<double>(7, 1.5),
std::vector<double>(7, 3.0));
// ------- 1) 构造多个 q 路点 -------
std::vector<double> q0 { 0.0, -0.5, 0.8, 0.0, 0.2, -0.3, 0.1};
std::vector<double> q1 { 0.5, -0.2, 0.4, 0.3, -0.1, 0.1, 0.0};
std::vector<double> q2 { 0.9, 0.1, -0.3, 0.5, -0.3, 0.3, -0.1};
std::vector<double> q3 { 1.2, 0.2, -0.6, 0.7, -0.4, 0.5, -0.2}; // 终点
std::vector<std::vector<double>> 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";
}
}

15
cmvr-es/algorithms/perception/CMakeLists.txt
View File

@ -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
)

711
cmvr-es/algorithms/perception/apriltag/src/tag_relative_target_3d_test.cpp
View File

@ -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 <gtest/gtest.h>
#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstring>
#include <cstdint>
#include <iomanip>
#include <iostream>
#include <limits>
#include <sstream>
#include <thread>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpImage.h>
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<cmvr::device::RealsenseCamera>& 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<cmvr::device::RealsenseCamera>& 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<double>(K.fx) * (p_c.x() / p_c.z()) + static_cast<double>(K.cx);
const double v = static_cast<double>(K.fy) * (p_c.y() / p_c.z()) + static_cast<double>(K.cy);
uv.x = static_cast<int>(std::lround(u));
uv.y = static_cast<int>(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<double> 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<uint16_t>(yy, xx);
if (raw == 0) {
continue;
}
const double z = static_cast<double>(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<DisplayTagPose>& 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<double>(intrinsics.fx),
static_cast<double>(intrinsics.fy),
static_cast<double>(intrinsics.cx),
static_cast<double>(intrinsics.cy));
vpImage<unsigned char> I(gray.rows, gray.cols);
for (int y = 0; y < gray.rows; ++y) {
std::memcpy(I[y], gray.ptr<unsigned char>(y), static_cast<size_t>(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<vpHomogeneousMatrix> cMo_vec;
const bool detected = detector.detect(I, tag_size_m, cam, cMo_vec);
if (!detected || cMo_vec.empty()) {
return false;
}
const std::vector<int> 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<cmvr::device::RealsenseCamera>(cam_cfg);
auto perception = std::make_shared<cmvr::perception::AprilTagPerception>(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<cmvr::device::RealsenseCamera> 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<DisplayTagPose> 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<int>(std::lround(u));
const int vi = static_cast<int>(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<std::chrono::duration<double>>(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<cmvr::device::RealsenseCamera>(cam_cfg);
// cmvr::perception::TagRelativeTarget3D tracker(camera);
// ASSERT_NO_THROW(camera->init());
// ASSERT_NO_THROW(camera->start());
// struct CameraStopGuard {
// std::shared_ptr<cmvr::device::RealsenseCamera> 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;
// }

9
cmvr-es/common/math/cartesian_motion_math.h
View File

@ -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);

106
cmvr-es/config/devices/arm/arm.pb.txt
View File

@ -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_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 }
}
joint_limit_avoidance {
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
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
}
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
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
}
}
}

92
cmvr-es/config/devices/arm/arm_dls_ik_dls_motion.pb.txt
View File

@ -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
}
}
}
}
}
}

138
cmvr-es/config/devices/arm/arm_dls_ik_qp_motion.pb.txt
View File

@ -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
}
}
}
}
}
}

118
cmvr-es/config/devices/arm/arm_mujoco.pb.txt
View File

@ -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 {
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
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
}
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
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
}
}
}

92
cmvr-es/config/devices/arm/arm_mujoco_dls_ik_dls_motion.pb.txt
View File

@ -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
}
}
}
}
}
}

120
cmvr-es/config/devices/arm/arm_mujoco_dls_ik_qp_motion.pb.txt
View File

@ -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
}
}
}
}
}
}

153
cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt Normal file
View File

@ -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
}
}
}
}
}
}

112
cmvr-es/config/devices/arm/arm_mujoco_qp_ik_qp_motion.pb.txt
View File

@ -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
}
}
}
}
}
}

153
cmvr-es/config/devices/arm/arm_qp.pb.txt Normal file
View File

@ -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
}
}
}
}
}
}

130
cmvr-es/config/devices/arm/arm_qp_ik_qp_motion.pb.txt
View File

@ -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
}
}
}
}
}
}

46
protos/cmvr/config/cartesian_motion_validation_config.proto Normal file
View File

@ -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;
}

10
protos/cmvr/config/pinocchio_dls_ik_config.proto
View File

@ -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;
}

3
protos/cmvr/config/pinocchio_qp_ik_config.proto
View File

@ -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;
}