73 lines
1.9 KiB
C++
73 lines
1.9 KiB
C++
|
//
|
||
|
|
// Created by lgv on 2025/8/13.
|
||
|
|
//
|
||
|
|
|
||
|
|
#include <iostream>
|
||
|
|
#include <vector>
|
||
|
|
#include <string>
|
||
|
|
#include <cstdlib> // for std::atof
|
||
|
|
|
||
|
|
#include <glog/logging.h>
|
||
|
|
#include "device_manager/device_manager.h"
|
||
|
|
#include <libgen.h>
|
||
|
|
|
||
|
|
using namespace cmvr::device;
|
||
|
|
|
||
|
|
int main(int argc, char* argv[]) {
|
||
|
|
// 需要传入 side + 7 个关节值
|
||
|
|
if (argc != 9) {
|
||
|
|
LOG(ERROR) << "Usage: " << argv[0] << " <side:left|right> J1 J2 J3 J4 J5 J6 J7" << std::endl;
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
std::string side = argv[1];
|
||
|
|
if (side != "left" && side != "right") {
|
||
|
|
LOG(ERROR) << "side must be 'left' or 'right'" << std::endl;
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
// 输入的 7 个关节
|
||
|
|
std::vector<double> arm_q(7);
|
||
|
|
for (int i = 0; i < 7; ++i) {
|
||
|
|
arm_q[i] = std::atof(argv[i + 2]);
|
||
|
|
}
|
||
|
|
|
||
|
|
// 左右臂关节名称
|
||
|
|
std::vector<std::string> left_joint_names = {
|
||
|
|
"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
|
||
|
|
"L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R"
|
||
|
|
};
|
||
|
|
std::vector<std::string> right_joint_names = {
|
||
|
|
"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
|
||
|
|
"R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"
|
||
|
|
};
|
||
|
|
|
||
|
|
std::vector<JointPositionCmd> cmd;
|
||
|
|
cmd.reserve(7);
|
||
|
|
|
||
|
|
if (side == "left") {
|
||
|
|
for (size_t i = 0; i < 7; ++i)
|
||
|
|
cmd.push_back({left_joint_names[i], arm_q[i]});
|
||
|
|
} else {
|
||
|
|
for (size_t i = 0; i < 7; ++i)
|
||
|
|
cmd.push_back({right_joint_names[i], arm_q[i]});
|
||
|
|
}
|
||
|
|
|
||
|
|
// 获取 robot
|
||
|
|
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
|
||
|
|
const XmlNode config(config_path);
|
||
|
|
|
||
|
|
if (!config.hasChild("DeviceManager")) {
|
||
|
|
LOG(ERROR) << "Device Manager node not found";
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
auto dmgr_cfg = config.getChild("DeviceManager");
|
||
|
|
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
|
||
|
|
|
||
|
|
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
|
||
|
|
|
||
|
|
robot->moveJ(cmd, 0.8);
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|