131 lines
4.1 KiB
C++
131 lines
4.1 KiB
C++
//
|
|
// Created by xtkuang on 2025/7/31.
|
|
//
|
|
|
|
#include "utils/dynamics/robot.h"
|
|
#include "utils/controller/cartesian_controller.h"
|
|
#include <Eigen/Dense>
|
|
#include <iostream>
|
|
#include <memory>
|
|
#include <string>
|
|
#include <vector>
|
|
#include <cmath>
|
|
#include <glog/logging.h>
|
|
|
|
constexpr int DOF = 14;
|
|
using RobotT = cmvr::dyn::Robot<DOF>;
|
|
using ControllerT = cmvr::ctrl::CartesianController<DOF>;
|
|
|
|
// 将欧拉角(rx, ry, rz)转为旋转矩阵，旋转顺序 Z→Y→X
|
|
Eigen::Matrix3d eulerToRotationMatrix(double rx, double ry, double rz)
|
|
{
|
|
Eigen::Matrix3d R_x;
|
|
R_x << 1, 0, 0,
|
|
0, cos(rx), -sin(rx),
|
|
0, sin(rx), cos(rx);
|
|
|
|
Eigen::Matrix3d R_y;
|
|
R_y << cos(ry), 0, sin(ry),
|
|
0, 1, 0,
|
|
-sin(ry), 0, cos(ry);
|
|
|
|
Eigen::Matrix3d R_z;
|
|
R_z << cos(rz), -sin(rz), 0,
|
|
sin(rz), cos(rz), 0,
|
|
0, 0, 1;
|
|
|
|
// ✅ 按照输入顺序 X → Y → Z 旋转
|
|
return R_x * R_y * R_z;
|
|
}
|
|
|
|
int main(int argc, char **argv)
|
|
{
|
|
if (argc != 9)
|
|
{
|
|
std::cerr << "用法: " << argv[0] << " <base link> <target link> <x> <y> <z> <rx> <ry> <rz>\n";
|
|
return -1;
|
|
}
|
|
|
|
std::vector<std::string> link_names = {
|
|
"PELVIS_S",
|
|
"L_SHOULDER_P_S", "L_SHOULDER_R_S", "L_SHOULDER_Y_S", "L_ELBOW_R_S", "L_WRIST_P_S", "L_WRIST_Y_S", "L_WRIST_R_S",
|
|
"R_SHOULDER_P_S", "R_SHOULDER_R_S", "R_SHOULDER_Y_S", "R_ELBOW_R_S", "R_WRIST_P_S", "R_WRIST_Y_S", "R_WRIST_R_S", "R_CAM", "R_FINGER_TIP"
|
|
};
|
|
|
|
std::vector<std::string> joint_names = {
|
|
"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
|
|
"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R",
|
|
};
|
|
|
|
std::string BASE_LINK = argv[1];
|
|
std::string target_link = argv[2];
|
|
bool find_base_link = false;
|
|
bool find_target_link = false;
|
|
for (auto link_name : link_names) {
|
|
if (BASE_LINK == link_name) {
|
|
find_base_link = true;
|
|
}
|
|
if (target_link == link_name) {
|
|
find_target_link = true;
|
|
}
|
|
}
|
|
if (!find_base_link) {
|
|
std::cerr << "base link 无效" << std::endl;
|
|
return -1;
|
|
}
|
|
if (!find_target_link) {
|
|
std::cerr << "target link 无效" << std::endl;
|
|
return -1;
|
|
}
|
|
|
|
double x = std::stod(argv[3]);
|
|
double y = std::stod(argv[4]);
|
|
double z = std::stod(argv[5]);
|
|
double rx = std::stod(argv[6]) * M_PI / 180.0;
|
|
double ry = std::stod(argv[7]) * M_PI / 180.0;
|
|
double rz = std::stod(argv[8]) * M_PI / 180.0;
|
|
|
|
// 1. 加载机器人模型
|
|
auto rcfg = LoadRobotFromURDF(
|
|
"/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
|
|
/*root*/ "PELVIS_S");
|
|
auto robot = std::make_shared<RobotT>(rcfg);
|
|
|
|
auto state = robot->MakeState(link_names, joint_names);
|
|
|
|
// 初始关节角度
|
|
Eigen::Vector<double, DOF> q_init;
|
|
q_init << -0.3647738137, -1.4556045962, 1.6057029118, -1.8483036779, 2.9024825461, 0.1850049007, -0.5602506899,
|
|
-0.23905230057945082, 1.2838583858530206, 1.8486727698400174, 1.3036511045462158, -2.824727848601091, -0.02012495463116552, -0.202540520242054;
|
|
state->SetQ(q_init);
|
|
robot->ComputeForwardKinematics(state);
|
|
|
|
// 2. 构造目标位姿矩阵
|
|
Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity();
|
|
T_target.block<3,3>(0,0) = eulerToRotationMatrix(rx, ry, rz); // 输入为弧度
|
|
T_target(0,3) = x;
|
|
T_target(1,3) = y;
|
|
T_target(2,3) = z;
|
|
|
|
// 3. 创建控制器
|
|
ControllerT ctrl(robot);
|
|
constexpr double DT = 0.002; // 2ms
|
|
|
|
std::vector<cmvr::ctrl::PoseTarget> targets = {
|
|
{target_link, T_target, 0.5, 1.0}
|
|
};
|
|
|
|
Eigen::Vector<double, DOF> q_cmd;
|
|
bool ok = ctrl.compute(state, BASE_LINK, targets, DT, ControllerT::Mode::Position, q_cmd, 10000, 1e-6);
|
|
|
|
// ✅ 格式化输出
|
|
std::cout << "[IK Solve] success=" << std::boolalpha << ok << "\n";
|
|
std::cout << "left: ";
|
|
for(int i=0; i<7; ++i) std::cout << q_cmd[i] << (i<6?" ":"");
|
|
std::cout << "\nright: ";
|
|
for(int i=7; i<14; ++i) std::cout << q_cmd[i] << (i<13?" ":"");
|
|
std::cout << std::endl;
|
|
|
|
return 0;
|
|
}
|