89 lines
2.2 KiB
C
89 lines
2.2 KiB
C
|
//
|
||
|
|
// Created by lgv on 2025/11/3.
|
||
|
|
//
|
||
|
|
|
||
|
|
#ifndef CMVR_ES_SRS_IK_SLOVER_H
|
||
|
|
#define CMVR_ES_SRS_IK_SLOVER_H
|
||
|
|
|
||
|
|
|
||
|
|
#include <Eigen/Dense>
|
||
|
|
#include "srs_ik/ik_limit_analyzer.h"
|
||
|
|
namespace cmvr {
|
||
|
|
namespace utils {
|
||
|
|
|
||
|
|
class SRSIkSlover {
|
||
|
|
public:
|
||
|
|
enum ConfigDirection {
|
||
|
|
OUTWARD = 1, // 向外
|
||
|
|
INWARD = -1 // 向内
|
||
|
|
};
|
||
|
|
|
||
|
|
public:
|
||
|
|
SRSIkSlover();
|
||
|
|
~SRSIkSlover(){};
|
||
|
|
|
||
|
|
std::vector<double> inverse_kinematics(const Eigen::MatrixXd& pose, double psi);
|
||
|
|
|
||
|
|
Eigen::Matrix4d calc_total_transform(const std::vector<double>& joint_angles);
|
||
|
|
|
||
|
|
bool cal_coefficient_matrix(const Eigen::MatrixXd& pose,Eigen::MatrixXd& s_mat , Eigen::MatrixXd& w_mat);
|
||
|
|
std::vector<std::pair<double, double>> calc_arm_angle_limits(const Eigen::MatrixXd& s_mat ,const Eigen::MatrixXd& w_mat);
|
||
|
|
|
||
|
|
void set_shoulder_config(ConfigDirection value) {
|
||
|
|
shoulder_config_ = value;
|
||
|
|
}
|
||
|
|
|
||
|
|
void set_elbow_config(ConfigDirection value) {
|
||
|
|
elbow_config_ = value;
|
||
|
|
}
|
||
|
|
|
||
|
|
void set_wrist_config(ConfigDirection value) {
|
||
|
|
wrist_config_ = value;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
private:
|
||
|
|
|
||
|
|
|
||
|
|
ConfigDirection shoulder_config_{OUTWARD};
|
||
|
|
ConfigDirection elbow_config_{OUTWARD};
|
||
|
|
ConfigDirection wrist_config_{OUTWARD};
|
||
|
|
// DH参数
|
||
|
|
Eigen::VectorXd link_lengths_;
|
||
|
|
Eigen::MatrixXd dh_params_;
|
||
|
|
|
||
|
|
double d_bs_, d_se_, d_ew_, d_wt_;
|
||
|
|
|
||
|
|
// 关节物理限位
|
||
|
|
// first : min second : max
|
||
|
|
std::vector<std::pair<double, double>> joints_limits_{};
|
||
|
|
|
||
|
|
IkLimitAnalyzer ik_limit_analyzer_;
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
// 计算参考平面相对于基坐标系的旋转矩阵
|
||
|
|
Eigen::Matrix3d reference_plane(const Eigen::Vector3d& S, const Eigen::Vector3d& W);
|
||
|
|
|
||
|
|
// 罗德里格斯公式
|
||
|
|
Eigen::Matrix3d calc_rotation_matrix(const Eigen::Vector3d& rotation_axis, double rotation_angle);
|
||
|
|
|
||
|
|
|
||
|
|
// 使用 DH 参数计算变换矩阵
|
||
|
|
Eigen::Matrix4d calc_dh(double d, double alpha, double a, double theta);
|
||
|
|
|
||
|
|
// 将角度归一化到 [-π, π] 范围内
|
||
|
|
double normalize_angle(const double angle);
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
};
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
#endif //CMVR_ES_SRS_IK_SLOVER_H
|