115 lines
3.7 KiB
C++
115 lines
3.7 KiB
C++
//
|
||
// Created by lgv on 2025/8/15.
|
||
//
|
||
#ifdef MAX_ITER
|
||
#undef MAX_ITER
|
||
#endif
|
||
|
||
#include <opencv2/opencv.hpp>
|
||
#include <pybind11/pybind11.h>
|
||
#include <pybind11/stl.h>
|
||
|
||
#include "devices/abstract_robot.h"
|
||
#include "device_manager/device_manager.h"
|
||
|
||
namespace py = pybind11;
|
||
using namespace cmvr::device;
|
||
|
||
class PyRobotWrapper {
|
||
public:
|
||
// 构造时只需要传入 config_path 和 robot_name（只在第一次初始化有效）
|
||
PyRobotWrapper(const std::string& config_path, const std::string& robot_name) {
|
||
// 如果已经初始化过，则直接返回
|
||
if (!robot_) {
|
||
const XmlNode config(config_path);
|
||
if (!config.hasChild("DeviceManager")) {
|
||
throw std::runtime_error("Device Manager node not found");
|
||
}
|
||
auto dmgr_cfg = config.getChild("DeviceManager");
|
||
dmgr_ = &DeviceManager::getInstance(dmgr_cfg);
|
||
robot_ = dmgr_->getDevice<AbstractRobot>(robot_name);
|
||
}
|
||
}
|
||
|
||
void moveJ(const std::string &side, const std::vector<double> &q) {
|
||
if (q.size() != 7)
|
||
throw std::runtime_error("Expected 7 joint values");
|
||
|
||
std::vector<std::string> joint_names;
|
||
if (side == "left") {
|
||
joint_names = {"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
|
||
"L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R"};
|
||
} else if (side == "right") {
|
||
joint_names = {"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
|
||
"R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
|
||
} else {
|
||
throw std::runtime_error("Side must be 'left' or 'right'");
|
||
}
|
||
|
||
std::vector<JointPoint> cmd;
|
||
cmd.push_back({"WAIST_Y", 0});
|
||
cmd.push_back({"WAIST_P", 0});
|
||
for (size_t i = 0; i < 7; ++i)
|
||
cmd.push_back({joint_names[i], q[i]});
|
||
|
||
robot_->moveJ(cmd, 0.8);
|
||
}
|
||
|
||
|
||
void torqueOn() {
|
||
if (!robot_)
|
||
throw std::runtime_error("Robot not initialized");
|
||
robot_->torqueOn();
|
||
}
|
||
|
||
void torqueOff() {
|
||
if (!robot_)
|
||
throw std::runtime_error("Robot not initialized");
|
||
robot_->torqueOff();
|
||
}
|
||
|
||
std::vector<double> getJointQ(const std::string& side) const {
|
||
if (!robot_)
|
||
throw std::runtime_error("Robot not initialized");
|
||
|
||
auto joint_qs = robot_->getJointQ(); // unordered_map<std::string,double>
|
||
|
||
std::vector<std::string> joint_names;
|
||
if (side == "left") {
|
||
joint_names = {"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
|
||
"L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R"};
|
||
} else if (side == "right") {
|
||
joint_names = {"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
|
||
"R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
|
||
} else {
|
||
throw std::runtime_error("Side must be 'left' or 'right'");
|
||
}
|
||
|
||
std::vector<double> values;
|
||
for (const auto& name : joint_names) {
|
||
values.push_back(joint_qs[name]);
|
||
}
|
||
|
||
return values;
|
||
}
|
||
|
||
|
||
|
||
private:
|
||
static DeviceManager* dmgr_;
|
||
static std::shared_ptr<AbstractRobot> robot_;
|
||
};
|
||
|
||
// 静态成员初始化
|
||
DeviceManager* PyRobotWrapper::dmgr_ = nullptr;
|
||
std::shared_ptr<AbstractRobot> PyRobotWrapper::robot_ = nullptr;
|
||
|
||
PYBIND11_MODULE(robot_wrapper, m) {
|
||
py::class_<PyRobotWrapper>(m, "Robot")
|
||
.def(py::init<const std::string&, const std::string&>())
|
||
.def("moveJ", &PyRobotWrapper::moveJ, py::arg("side"), py::arg("q"))
|
||
.def("torqueOn", &PyRobotWrapper::torqueOn)
|
||
.def("torqueOff", &PyRobotWrapper::torqueOff)
|
||
.def("getJointQ", &PyRobotWrapper::getJointQ, py::arg("side"));
|
||
}
|