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"));
|
|||
|
|
}
|