SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat:add touch api

Browse Source
This commit is contained in:
lgv 2025-10-09 16:31:21 +08:00
parent 4765a69c49
commit 8afee807f7
39 changed files with 1732 additions and 376 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
README.md
View File

@ -99,11 +99,12 @@ sudo make install
```shell ```shell
sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u
sudo apt-get install librealsense2-dkms=2.55.1-0~realsense.12473 -y sudo apt-get install librealsense2-dkms=1.3.24-0ubuntu1 -y
sudo apt-get install librealsense2-utils=2.55.1-0~realsense.12473 -y sudo apt-get install \
sudo apt-get install librealsense2-dev=2.55.1-0~realsense.12473 -y librealsense2=2.55.1-0~realsense.12473 \
sudo apt-get install librealsense2-dbg=2.55.1-0~realsense.12473 -y librealsense2-utils=2.55.1-0~realsense.12473 \
librealsense2-dev=2.55.1-0~realsense.12473 \
librealsense2-dbg=2.55.1-0~realsense.12473 -y
``` ```
### 5. OSQPEigen install(build from source) ### 5. OSQPEigen install(build from source)

76
config/cabin_robot.xml
View File

@ -14,15 +14,15 @@
<!-- <UVCCamera id="cam1" serial="/dev/video6" w="640" h="480" fps="30" mode="video" codec="H265"/>--> <!-- <UVCCamera id="cam1" serial="/dev/video6" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
<!-- <UVCCamera id="cam2" serial="/dev/video14" w="640" h="480" fps="30" mode="video" codec="H265"/>--> <!-- <UVCCamera id="cam2" serial="/dev/video14" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
<!-- <RealsenseCamera id="cam3" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>--> <!-- <RealsenseCamera id="cam3" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
<!-- <RealsenseCamera id="cam4" serial="243122075614" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>--> <!-- <RealsenseCamera id="cam4" serial="243122075614" w="1280" h="720" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
<!-- <MechMind id="cam5" ip="10.148.108.111" align="true" _2dtype="color"/>--> <!-- <MechMind id="cam5" ip="10.148.108.111" align="true" _2dtype="color"/>-->
<!-- <RealsenseCamera id="cam6" serial="243122075389" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>--> <!-- <RealsenseCamera id="cam6" serial="243122075389" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
</Camera> </Camera>
<DexHand> <DexHand>
<RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="10.148.108.115" port="6000"> <!-- <RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="10.148.108.115" port="6000">-->
<Freedom order="01" default_force="500" default_speed="500" /> <!-- <Freedom order="01" default_force="500" default_speed="500" />-->
</RH56DFTP> <!-- </RH56DFTP>-->
<!-- <RH56DFTP id="hand2" default_force="500" default_speed="500" ip_address="10.148.108.113" port="6000">--> <!-- <RH56DFTP id="hand2" default_force="500" default_speed="500" ip_address="10.148.108.113" port="6000">-->
<!-- <Freedom order="01" default_force="500" default_speed="500" />--> <!-- <Freedom order="01" default_force="500" default_speed="500" />-->
<!-- </RH56DFTP>--> <!-- </RH56DFTP>-->
@ -32,39 +32,39 @@
<!-- <LeftArm id="left_arm" devtype="ti5Robot" />--> <!-- <LeftArm id="left_arm" devtype="ti5Robot" />-->
<!-- <RightArm />--> <!-- <RightArm />-->
<!-- <Neck/>--> <!-- <Neck/>-->
<Humanoid id="hc01" dof="14" <!-- <Humanoid id="hc01" dof="14"-->
urdf="/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf" <!-- urdf="/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"-->
baseLink="PELVIS_S" <!-- baseLink="PELVIS_S"-->
jointNames="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" <!-- jointNames="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"-->
linkNames="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" <!-- linkNames="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"-->
bufferSize="50" <!-- bufferSize="50"-->
verbose="false"> <!-- verbose="false">-->
<CanManger id="" devId=""> <!-- <CanManger id="" devId="">-->
<LeftArmCan id = " " devId = " " channelId ="0"> <!-- <LeftArmCan id = " " devId = " " channelId ="0">-->
<Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <!-- <Motor id="1" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
</LeftArmCan> <!-- </LeftArmCan>-->
<RightArmCan id = " " devId = " " channelId ="1"> <!-- <RightArmCan id = " " devId = " " channelId ="1">-->
<Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/> <!-- <Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>-->
<Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/> <!-- <Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>-->
</RightArmCan> <!-- </RightArmCan>-->
<WaistCan id = " " devId = " " channelId ="2"> <!-- <WaistCan id = " " devId = " " channelId ="2">-->
<Motor id="14" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="14" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <!-- <Motor id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
</WaistCan> <!-- </WaistCan>-->
</CanManger> <!-- </CanManger>-->
</Humanoid> <!-- </Humanoid>-->
</Robot> </Robot>
<BioHead> <BioHead>
@ -85,7 +85,7 @@
<Microphone> <Microphone>
<!-- <ffmpegMicPhone id="mic1" alsa="hw:0" channels="2" sampleRate="44100" volume="80"/>--> <!-- <ffmpegMicPhone id="mic1" alsa="hw:0" channels="2" sampleRate="44100" volume="80"/>-->
<ffmpegMicPhone id="mic2" alsa="hw:1" channels="1" sampleRate="44100" volume="80"/> <!-- <ffmpegMicPhone id="mic2" alsa="hw:1" channels="1" sampleRate="44100" volume="80"/>-->
</Microphone> </Microphone>
<Speaker> <Speaker>
@ -132,7 +132,7 @@
</MonitorManager> </MonitorManager>
<gRPCServer port="50055"> <gRPCServer port="50054">
</gRPCServer> </gRPCServer>

2
config/robot_description/hc_description/dual_arm.urdf
View File

@ -574,7 +574,7 @@
</link> </link>
<joint name="R_FINGER_TIP_FIXED" type="fixed"> <joint name="R_FINGER_TIP_FIXED" type="fixed">
<origin xyz="-0.002 -0.2851 -0.005" rpy="0 0 -1.5707963267"/> <origin xyz="0 -0.270 -0.005" rpy="0 0 -1.5707963267"/>
<parent link="R_WRIST_R_S"/> <parent link="R_WRIST_R_S"/>
<child link="R_FINGER_TIP"/> <child link="R_FINGER_TIP"/>
<axis xyz="0 0 1"/> <axis xyz="0 0 1"/>

136
example/robot_wrapper.cpp
View File

@ -8,9 +8,11 @@
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include <pybind11/pybind11.h> #include <pybind11/pybind11.h>
#include <pybind11/stl.h> #include <pybind11/stl.h>
#include <mutex>
#include "devices/abstract_robot.h" #include "devices/abstract_robot.h"
#include "device_manager/device_manager.h" #include "device_manager/device_manager.h"
#include "cmvr/msgs/geometry.pb.h"
namespace py = pybind11; namespace py = pybind11;
using namespace cmvr::device; using namespace cmvr::device;
@ -19,7 +21,7 @@ class PyRobotWrapper {
public: public:
// 构造时只需要传入 config_path 和 robot_name（只在第一次初始化有效） // 构造时只需要传入 config_path 和 robot_name（只在第一次初始化有效）
PyRobotWrapper(const std::string& config_path, const std::string& robot_name) { PyRobotWrapper(const std::string& config_path, const std::string& robot_name) {
// 如果已经初始化过，则直接返回 std::lock_guard<std::mutex> lock(init_mutex); // 线程安全
if (!robot_) { if (!robot_) {
const XmlNode config(config_path); const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")) { if (!config.hasChild("DeviceManager")) {
@ -31,84 +33,138 @@ public:
} }
} }
void moveJ(const std::string &side, const std::vector<double> &q) { void moveJ(const std::string &side, const std::vector<double> &q, double speed=0.8) {
if (q.size() != 7) if (q.size() != 7)
throw std::runtime_error("Expected 7 joint values"); throw std::runtime_error("Expected 7 joint values");
std::vector<std::string> joint_names; const auto& joint_names = getJointNames(side);
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; std::vector<JointPoint> cmd = {{"WAIST_Y", 0}, {"WAIST_P", 0}};
cmd.push_back({"WAIST_Y", 0});
cmd.push_back({"WAIST_P", 0});
for (size_t i = 0; i < 7; ++i) for (size_t i = 0; i < 7; ++i)
cmd.push_back({joint_names[i], q[i]}); cmd.push_back({joint_names[i], q[i]});
robot_->moveJ(cmd, 0.8); robot_->moveJ(cmd, speed);
} }
void torqueOn() { void torqueOn() {
if (!robot_) checkRobotInit();
throw std::runtime_error("Robot not initialized");
robot_->torqueOn(); robot_->torqueOn();
} }
void torqueOn(const std::string& joint_name) {
checkRobotInit();
robot_->torqueOn(joint_name);
}
void torqueOff() { void torqueOff() {
if (!robot_) checkRobotInit();
throw std::runtime_error("Robot not initialized");
robot_->torqueOff(); robot_->torqueOff();
} }
void torqueOff(const std::string& joint_name) {
checkRobotInit();
robot_->torqueOff(joint_name);
}
void calibrateZeroQ(const std::string &joint_name) {
checkRobotInit();
robot_->calibrateZeroQ(joint_name);
}
std::vector<double> getJointQ(const std::string& side) const { std::vector<double> getJointQ(const std::string& side) const {
if (!robot_) checkRobotInit();
throw std::runtime_error("Robot not initialized");
auto joint_qs = robot_->getJointQ(); // unordered_map<std::string,double> auto joint_qs = robot_->getJointQ(); // unordered_map<std::string,double>
const auto& joint_names = getJointNames(side);
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; std::vector<double> values;
for (const auto& name : joint_names) { for (const auto& name : joint_names) {
values.push_back(joint_qs[name]); values.push_back(joint_qs.at(name));
} }
return values; return values;
} }
std::vector<double> ik(const std::string& side,
const std::string &base_link,
const std::string &ee_link,
double x, double y, double z,
double rx, double ry, double rz)
{
checkRobotInit();
cmvr::msgs::Pose3d pose;
pose.mutable_position()->set_x(x);
pose.mutable_position()->set_y(y);
pose.mutable_position()->set_z(z);
pose.mutable_euler()->set_rx(rx);
pose.mutable_euler()->set_ry(ry);
pose.mutable_euler()->set_rz(rz);
auto js_cmd = robot_->ik(base_link, ee_link, pose);
if (js_cmd.size() < 14) {
throw std::runtime_error("IK solution size is smaller than expected");
}
if (side == "left") {
return std::vector<double>(js_cmd.begin(), js_cmd.begin() + 7);
} else if (side == "right") {
return std::vector<double>(js_cmd.begin() + 7, js_cmd.begin() + 14);
} else {
throw std::runtime_error("Side must be 'left' or 'right'");
}
}
private: private:
static DeviceManager* dmgr_; static DeviceManager* dmgr_;
static std::shared_ptr<AbstractRobot> robot_; static std::shared_ptr<AbstractRobot> robot_;
static std::mutex init_mutex;
static const std::vector<std::string> LEFT_JOINTS;
static const std::vector<std::string> RIGHT_JOINTS;
void checkRobotInit() const {
if (!robot_)
throw std::runtime_error("Robot not initialized");
}
const std::vector<std::string>& getJointNames(const std::string& side) const {
if (side == "left") return LEFT_JOINTS;
if (side == "right") return RIGHT_JOINTS;
throw std::runtime_error("Side must be 'left' or 'right'");
}
}; };
// 静态成员初始化 // 静态成员初始化
DeviceManager* PyRobotWrapper::dmgr_ = nullptr; DeviceManager* PyRobotWrapper::dmgr_ = nullptr;
std::shared_ptr<AbstractRobot> PyRobotWrapper::robot_ = nullptr; std::shared_ptr<AbstractRobot> PyRobotWrapper::robot_ = nullptr;
std::mutex PyRobotWrapper::init_mutex;
const std::vector<std::string> PyRobotWrapper::LEFT_JOINTS = {"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
"L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R"};
const std::vector<std::string> PyRobotWrapper::RIGHT_JOINTS = {"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
"R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
// Python 绑定
PYBIND11_MODULE(robot_wrapper, m) { PYBIND11_MODULE(robot_wrapper, m) {
py::class_<PyRobotWrapper>(m, "Robot") py::class_<PyRobotWrapper>(m, "Robot")
.def(py::init<const std::string&, const std::string&>()) .def(py::init<const std::string&, const std::string&>(),
.def("moveJ", &PyRobotWrapper::moveJ, py::arg("side"), py::arg("q")) py::arg("config_path"), py::arg("robot_name"),
.def("torqueOn", &PyRobotWrapper::torqueOn) "Initialize the robot wrapper with config and robot name")
.def("torqueOff", &PyRobotWrapper::torqueOff) .def("moveJ", &PyRobotWrapper::moveJ, py::arg("side"), py::arg("q"), py::arg("speed")=0.8,
.def("getJointQ", &PyRobotWrapper::getJointQ, py::arg("side")); "Move robot joints in joint space with optional speed")
.def("torqueOn", py::overload_cast<>(&PyRobotWrapper::torqueOn))
.def("torqueOn", py::overload_cast<const std::string&>(&PyRobotWrapper::torqueOn))
.def("torqueOff", py::overload_cast<>(&PyRobotWrapper::torqueOff))
.def("torqueOff", py::overload_cast<const std::string&>(&PyRobotWrapper::torqueOff))
.def("calibrateZeroQ", &PyRobotWrapper::calibrateZeroQ)
.def("getJointQ", &PyRobotWrapper::getJointQ, py::arg("side"),
"Get joint positions for the specified side ('left' or 'right')")
.def("ik", &PyRobotWrapper::ik,
py::arg("side"), py::arg("base_link"), py::arg("ee_link"),
py::arg("x"), py::arg("y"), py::arg("z"),
py::arg("rx"), py::arg("ry"), py::arg("rz"),
"Compute inverse kinematics and return 7 joint values for the given side");
} }

2
example/solve_fk.cpp
View File

@ -11,7 +11,7 @@ constexpr int DOF = 14;
using RobotT = cmvr::dyn::Robot<DOF>; using RobotT = cmvr::dyn::Robot<DOF>;
using StateT = cmvr::dyn::State<DOF>; using StateT = cmvr::dyn::State<DOF>;
static const char* kUrdfPath = "/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"; static const char* kUrdfPath = "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf";
int main(int argc, char** argv) { int main(int argc, char** argv) {
if (argc < 10) { if (argc < 10) {

4
example/solve_ik.cpp
View File

@ -87,7 +87,7 @@ int main(int argc, char **argv)
// 1. 加载机器人模型 // 1. 加载机器人模型
auto rcfg = LoadRobotFromURDF( auto rcfg = LoadRobotFromURDF(
"/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf", "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
/*root*/ "PELVIS_S"); /*root*/ "PELVIS_S");
auto robot = std::make_shared<RobotT>(rcfg); auto robot = std::make_shared<RobotT>(rcfg);
@ -96,7 +96,7 @@ int main(int argc, char **argv)
// 初始关节角度 // 初始关节角度
Eigen::Vector<double, DOF> q_init; Eigen::Vector<double, DOF> q_init;
q_init << -0.3647738137, -1.4556045962, 1.6057029118, -1.8483036779, 2.9024825461, 0.1850049007, -0.5602506899, q_init << -0.3647738137, -1.4556045962, 1.6057029118, -1.8483036779, 2.9024825461, 0.1850049007, -0.5602506899,
-0.3448998226801712, 0.9351815151808957, 2.2703399775931388, 1.6895896595300133, -2.3279553847104077, 0.46019613485676647, 0.3009592467697589; -0.23905230057945082, 1.2838583858530206, 1.8486727698400174, 1.3036511045462158, -2.824727848601091, -0.02012495463116552, -0.202540520242054;
state->SetQ(q_init); state->SetQ(q_init);
robot->ComputeForwardKinematics(state); robot->ComputeForwardKinematics(state);

8
include/controller/touch_controller.h
View File

@ -6,6 +6,7 @@
#include <utility> #include <utility>
#include "devices/abstract_camera.h"
#include "devices/abstract_dexhand.h" #include "devices/abstract_dexhand.h"
#include "devices/abstract_robot.h" #include "devices/abstract_robot.h"
#include "cmvr/msgs/geometry.pb.h" #include "cmvr/msgs/geometry.pb.h"
@ -80,15 +81,15 @@ namespace cmvr {
class TouchController { class TouchController {
public: public:
TouchController() {}; TouchController() {};
TouchController( std::shared_ptr<device::AbstractRobot> robot,std::shared_ptr<device::AbstractDexHand> hand) TouchController(std::shared_ptr<device::AbstractRobot> robot,std::shared_ptr<device::AbstractDexHand> hand,std::shared_ptr<device::AbstractCamera> cam)
:robot_(std::move(robot)),hand_(std::move(hand)), :robot_(std::move(robot)),hand_(std::move(hand)),cam_(std::move(cam)),
pid_(std::make_shared<PID>(0.005, 0.001, 0.001, 5000.0, 0.5, 1.0)){} pid_(std::make_shared<PID>(0.005, 0.001, 0.001, 5000.0, 0.5, 1.0)){}
~TouchController()=default; ~TouchController()=default;
bool isArrive(double max_force); bool isArrive(double max_force);
void touch(int u,int v ,double max_force){} void touch(int u,int v ,double max_force);
void touch(std::shared_ptr<device::AbstractRobot> robot,const msgs::Pose3d pose,const msgs::Pose3d offset); void touch(std::shared_ptr<device::AbstractRobot> robot,const msgs::Pose3d pose,const msgs::Pose3d offset);
void touch( msgs::Pose3d pose, msgs::Pose3d offset,double max_force); void touch( msgs::Pose3d pose, msgs::Pose3d offset,double max_force);
@ -96,6 +97,7 @@ namespace cmvr {
private: private:
std::shared_ptr<device::AbstractRobot> robot_{nullptr}; std::shared_ptr<device::AbstractRobot> robot_{nullptr};
std::shared_ptr<device::AbstractDexHand> hand_{nullptr}; std::shared_ptr<device::AbstractDexHand> hand_{nullptr};
std::shared_ptr<device::AbstractCamera> cam_{nullptr};
std::shared_ptr<PID> pid_{nullptr}; std::shared_ptr<PID> pid_{nullptr};

7
include/devices/abstract_motor.h
View File

@ -146,6 +146,13 @@ namespace cmvr::device{
return protocol_->getQ(node_id_); return protocol_->getQ(node_id_);
} }
virtual double getQd() {
if (!protocol_) {
throw std::runtime_error("Protocol not set for motor");
}
return protocol_->getQd(node_id_);
}
// 使用的通讯协议 // 使用的通讯协议
virtual void setProtocol(std::shared_ptr<MotorProtocolInterface> protocol) { virtual void setProtocol(std::shared_ptr<MotorProtocolInterface> protocol) {

8
include/devices/abstract_robot.h
View File

@ -10,6 +10,8 @@
#include "abstract_device.h" #include "abstract_device.h"
#include "utils/controller/cartesian_controller.h" #include "utils/controller/cartesian_controller.h"
#include "cmvr/msgs/geometry.pb.h" #include "cmvr/msgs/geometry.pb.h"
#include "cmvr/msgs/motor.pb.h"
namespace cmvr::device{ namespace cmvr::device{
struct JointPoint{ struct JointPoint{
@ -53,13 +55,16 @@ namespace cmvr::device{
virtual void getJointQ(std::unordered_map<std::string,double> &joint_qs) const {throw std::runtime_error("Not implemented");} virtual void getJointQ(std::unordered_map<std::string,double> &joint_qs) const {throw std::runtime_error("Not implemented");}
virtual std::vector<std::string> getLinkNames() { throw std::runtime_error("Not implemented"); } virtual std::vector<std::string> getLinkNames() { throw std::runtime_error("Not implemented"); }
virtual void getJointsState(std::vector<JointState>& states) = 0;
virtual void getState(RobotState &state) { throw std::runtime_error("Not implemented"); } virtual void getState(RobotState &state) { throw std::runtime_error("Not implemented"); }
virtual math::Pose3d getTransform(std::string &bask_link, std::string &target_link) {throw std::runtime_error("Not implemented");} virtual math::Pose3d getTransform(std::string &bask_link, std::string &target_link) {throw std::runtime_error("Not implemented");}
virtual void torqueOn() { throw std::runtime_error("Not implemented"); } virtual void torqueOn() { throw std::runtime_error("Not implemented"); }
virtual void torqueOn(const std::string &joint_name) { throw std::runtime_error("Not implemented"); }
virtual void torqueOff() { throw std::runtime_error("Not implemented"); } virtual void torqueOff() { throw std::runtime_error("Not implemented"); }
virtual void torqueOff(const std::string &joint_name) { throw std::runtime_error("Not implemented"); }
virtual void eStop() { throw std::runtime_error("Not implemented"); } virtual void eStop() { throw std::runtime_error("Not implemented"); }
@ -102,6 +107,9 @@ namespace cmvr::device{
virtual void servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) { throw std::runtime_error("Not implemented"); } virtual void servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) { throw std::runtime_error("Not implemented"); }
virtual void calibrateZeroQ(const std::string &joint_name) = 0; virtual void calibrateZeroQ(const std::string &joint_name) = 0;
virtual msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0;
virtual std::vector<double> ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) = 0;
protected: protected:
int dof_{}; int dof_{};
RobotState state_{}; RobotState state_{};

1
include/devices/state_define.h
View File

@ -50,6 +50,7 @@ namespace cmvr::device{
} RobotCartesian; } RobotCartesian;
typedef struct { typedef struct {
std::string name;
bool is_ready; bool is_ready;
bool power_on; bool power_on;
double position; double position;

2
include/service/grpc_humanoid_robot_service.h
View File

@ -19,6 +19,8 @@ namespace cmvr {
const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) override; const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) override;
grpc::Status moveJ(grpc::ServerContext *context, const cmvr::api::MoveJ_Request *request, cmvr::api::MoveJ_Response *response) override; grpc::Status moveJ(grpc::ServerContext *context, const cmvr::api::MoveJ_Request *request, cmvr::api::MoveJ_Response *response) override;
grpc::Status getJointState(grpc::ServerContext *context, const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) override;
private: private:
device::DeviceManager& dmgr_; device::DeviceManager& dmgr_;
}; };

22
protos/cmvr/api/humanoid_robot.proto
View File

@ -25,8 +25,30 @@ message MoveJ{
} }
// 关节状态消息
message JointState {
repeated string name = 1; // 关节名称
repeated double position = 2; // 关节位置
repeated double velocity = 3; // 可选：关节速度
repeated double effort = 4; // 可选：关节力矩
double timestamp = 5; // 可选：时间戳
}
// 返回消息
message JointResponse {
CommandHeader.Feedback header= 1;
repeated JointState state = 2;
}
// 请求消息（可选）
message JointRequest {
CommandHeader.Request header = 1;
}
service HumanoidRobotService{ service HumanoidRobotService{
rpc torqueOff(CommandHeader.Request) returns (CommandHeader.Feedback); rpc torqueOff(CommandHeader.Request) returns (CommandHeader.Feedback);
rpc torqueOn(CommandHeader.Request) returns (CommandHeader.Feedback); rpc torqueOn(CommandHeader.Request) returns (CommandHeader.Feedback);
rpc moveJ(MoveJ.Request) returns (MoveJ.Response); rpc moveJ(MoveJ.Request) returns (MoveJ.Response);
rpc getJointState(JointRequest) returns (JointResponse);
} }

23
python/hand_eye_calibration/collect_data.py
View File

@ -90,7 +90,7 @@ def callback_and_save(frame):
js = robot.getJointQ('right') js = robot.getJointQ('right')
logger.info(f"当前关节角: {js}") logger.info(f"当前关节角: {js}")
pose = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S") pose = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
logger.info(f"获取位姿成功:\n{pose}") logger.info(f"获取位姿成功:\n{pose}")
logger.info("请按 p 失能机器人，并手动移动机器人到其他位姿") logger.info("请按 p 失能机器人，并手动移动机器人到其他位姿")
@ -113,28 +113,9 @@ def callback_and_save(frame):
return True return True
def init_realsense(serial=TARGET_SERIAL):
"""初始化 RealSense 相机"""
ctx = rs.context()
devices = ctx.query_devices()
if len(devices) == 0:
raise RuntimeError("未检测到任何 RealSense 相机")
if not any(dev.get_info(rs.camera_info.serial_number) == serial for dev in devices):
raise RuntimeError(f"未找到序列号为 {serial} 的 RealSense 相机")
pipeline = rs.pipeline() def start_pipeline(serial=TARGET_SERIAL, depth_size=(1280, 720), color_size=(1280, 720), fps: int = 30):
config = rs.config()
config.enable_device(serial)
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
try:
pipeline.start(config)
except Exception as e:
raise RuntimeError(f"相机连接异常: {e}")
return pipeline
def start_pipeline(serial=TARGET_SERIAL, depth_size=(848, 480), color_size=(1280, 720), fps: int = 30):
ctx = rs.context() ctx = rs.context()
devices = ctx.query_devices() devices = ctx.query_devices()
if len(devices) == 0: if len(devices) == 0:

107
python/hand_eye_calibration/compute_in_hand.py
View File

@ -40,6 +40,85 @@ YY = data.get("checkerboard_args").get("YY") #标定板的中宽度对应的角
L = data.get("checkerboard_args").get("L") #标定板一格的长度 单位为米 L = data.get("checkerboard_args").get("L") #标定板一格的长度 单位为米
import numpy as np
def eulerZYXToRotationMatrix(rx, ry, rz):
# 绕 x 轴的旋转矩阵
R_x = np.array([[1, 0, 0],
[0, np.cos(rx), -np.sin(rx)],
[0, np.sin(rx), np.cos(rx)]])
# 绕 y 轴的旋转矩阵
R_y = np.array([[np.cos(ry), 0, np.sin(ry)],
[0, 1, 0],
[-np.sin(ry), 0, np.cos(ry)]])
# 绕 z 轴的旋转矩阵
R_z = np.array([[np.cos(rz), -np.sin(rz), 0],
[np.sin(rz), np.cos(rz), 0],
[0, 0, 1]])
# 使用 @ 运算符进行矩阵乘法：R = R_x @ R_y @ R_z
return R_x @ R_y @ R_z
def eulerXYZToRotationMatrix(rx, ry, rz):
# 绕 x 轴的旋转矩阵
R_x = np.array([[1, 0, 0],
[0, np.cos(rx), -np.sin(rx)],
[0, np.sin(rx), np.cos(rx)]])
# 绕 y 轴的旋转矩阵
R_y = np.array([[np.cos(ry), 0, np.sin(ry)],
[0, 1, 0],
[-np.sin(ry), 0, np.cos(ry)]])
# 绕 z 轴的旋转矩阵
R_z = np.array([[np.cos(rz), -np.sin(rz), 0],
[np.sin(rz), np.cos(rz), 0],
[0, 0, 1]])
# 使用 @ 运算符进行矩阵乘法：R = R_x @ R_y @ R_z
return R_z @ R_y @ R_x
def rotationMatrixToEulerZYX(R):
# 提取 ry（绕 Y 轴的旋转角度）
ry = np.arcsin(R[0, 2]) # R(0, 2) = sin(ry)
cy = np.cos(ry)
if np.abs(cy) > 1e-6: # 正常情况
rx = np.arctan2(-R[1, 2], R[2, 2])
rz = np.arctan2(-R[0, 1], R[0, 0])
else: # 万向节锁：cy ≈ 0
rx = 0 # 任意选择
if ry > 0:
rz = np.arctan2(R[1, 0], R[1, 1])
else:
rz = np.arctan2(-R[1, 0], R[1, 1])
return np.array([rx, ry, rz])
def rotationMatrixToEulerXYZ(R):
# 提取 ry（绕 Y 轴的旋转角度）
ry = np.arcsin(-R[2, 0]) # R(2, 0) = -sin(ry)
cy = np.cos(ry)
if np.abs(cy) > 1e-6: # 正常情况
rx = np.arctan2(R[2, 1], R[2, 2])
rz = np.arctan2(R[1, 0], R[0, 0])
else: # 万向节锁：cy ≈ 0
rx = 0 # 任意选择
if ry > 0:
rz = np.arctan2(-R[1, 2], R[1, 1])
else:
rz = np.arctan2(R[1, 2], R[1, 1])
return np.array([rx, ry, rz])
def func(): def func():
@ -113,14 +192,37 @@ def func():
for i in range(N): for i in range(N):
mat = tool_pose[4*i:4*i+4, :] # 取第i个4x4矩阵 mat = tool_pose[4*i:4*i+4, :] # 取第i个4x4矩阵
# # 提取旋转矩阵
# R_zyx = mat[0:3, 0:3]
#
# # 将 ZYX 顺序的旋转矩阵转换为欧拉角（ZYX 顺序）
# euler_zyx = rotationMatrixToEulerZYX(R_zyx)
#
# # 将欧拉角转换为 XYZ 顺序的旋转矩阵
# R_xyz = eulerXYZToRotationMatrix(euler_zyx[0], euler_zyx[1], euler_zyx[2])
#
# # 将转换后的旋转矩阵添加到 R_tool
# R_tool.append(R_xyz)
#
# # # 将 ZYX 顺序的旋转矩阵转换为欧拉角（返回值是 rx, ry, rz）
# # euler_zyx = R.from_matrix(R_zyx).as_euler('zyx', degrees=False)
# #
# # # 将欧拉角重新转换为 XYZ 顺序的旋转矩阵
# # R_xyz = R.from_euler('xyz', euler_zyx, degrees=False).as_matrix()
# #
# # # 将转换后的旋转矩阵添加到 R_tool
# # R_tool.append(R_xyz)
R_tool.append(mat[0:3, 0:3]) # 提取旋转矩阵 R_tool.append(mat[0:3, 0:3]) # 提取旋转矩阵
t_tool.append(mat[0:3, 3]) # 提取平移向量 t_tool.append(mat[0:3, 3]) # 提取平移向量
R, t = cv2.calibrateHandEye(R_tool, t_tool, rvecs, tvecs, cv2.CALIB_HAND_EYE_TSAI) R1, t = cv2.calibrateHandEye(R_tool, t_tool, rvecs, tvecs, cv2.CALIB_HAND_EYE_TSAI)
return R,t return R1,t
if __name__ == '__main__': if __name__ == '__main__':
@ -137,3 +239,4 @@ if __name__ == '__main__':
logger_.info(f"平移向量是:\n { translation_vector}") logger_.info(f"平移向量是:\n { translation_vector}")
logger_.info(f"四元数是：\n { quaternion}") logger_.info(f"四元数是：\n { quaternion}")

288
python/hand_eye_calibration/libs/web-log
View File

@ -1,46 +1,256 @@
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg 2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg 2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg 2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg 2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg 2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg 2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg 2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg 2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg 2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg 2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg 2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg 2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg 2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg 2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg 2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg 2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg 2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg 2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg 2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg 2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg 2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg 2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg 2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg 2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg 2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg 2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg 2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg 2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg 2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg 2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg 2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg 2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-08-23 16:08:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg 2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg 2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-08-23 16:08:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg 2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg 2025-09-08 14:26:55 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-08-23 16:08:46 - __main__ - INFO: 旋转矩阵是: 2025-09-08 14:26:55 - __main__ - INFO: 旋转矩阵是:
[[-0.99486349 0.07846371 0.06395369] [[-0.99486349 0.07846371 0.06395369]
[-0.05884529 0.06577685 -0.9960977 ] [-0.05884529 0.06577685 -0.9960977 ]
[-0.08236419 -0.99474462 -0.06082177]] [-0.08236419 -0.99474462 -0.06082177]]
2025-08-23 16:08:46 - __main__ - INFO: 平移向量是: 2025-09-08 14:26:55 - __main__ - INFO: 平移向量是:
[[-0.00797824] [[-0.00797824]
[-0.17640328] [-0.17640328]
[ 0.07459845]] [ 0.07459845]]
2025-08-23 16:08:46 - __main__ - INFO: 四元数是： 2025-09-08 14:26:55 - __main__ - INFO: 四元数是：
[ 0.00673467 0.72826199 -0.68342243 0.05022845]
2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-09-08 14:29:38 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-09-08 14:29:38 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-09-08 14:29:38 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-09-08 14:29:38 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-09-08 14:29:38 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-09-08 14:29:38 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-09-08 14:29:38 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-09-08 14:29:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-09-08 14:30:53 - __main__ - INFO: 旋转矩阵是:
[[ 0.72638143 0.22269568 -0.65021278]
[ 0.420903 0.60375531 0.67699349]
[ 0.54333294 -0.76543201 0.34482337]]
2025-09-08 14:30:53 - __main__ - INFO: 平移向量是:
[[ 0.15849038]
[-0.26275262]
[-0.05082269]]
2025-09-08 14:30:53 - __main__ - INFO: 四元数是：
[-0.44096563 -0.36488029 0.0605942 0.81776526]
2025-09-08 14:32:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-09-08 14:32:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-09-08 14:32:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-09-08 14:32:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-09-08 14:32:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-09-08 14:32:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-09-08 14:32:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-09-08 14:32:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-09-08 14:32:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-09-08 14:32:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-09-08 14:32:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-09-08 14:32:27 - __main__ - INFO: 旋转矩阵是:
[[ 0.05477568 0.03458859 0.99789942]
[-0.99767126 0.04257346 0.0532875 ]
[-0.04064089 -0.99849443 0.03684003]]
2025-09-08 14:32:27 - __main__ - INFO: 平移向量是:
[[-0.14908435]
[-0.19737896]
[-0.05964516]]
2025-09-08 14:32:27 - __main__ - INFO: 四元数是：
[-0.49380213 0.48758531 -0.48463669 0.53249159]
2025-09-08 14:37:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-09-08 14:37:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-09-08 14:37:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-09-08 14:37:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-09-08 14:37:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-09-08 14:37:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-09-08 14:37:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-09-08 14:37:26 - __main__ - INFO: 旋转矩阵是:
[[-0.99486349 0.07846371 0.06395369]
[-0.05884529 0.06577685 -0.9960977 ]
[-0.08236419 -0.99474462 -0.06082177]]
2025-09-08 14:37:26 - __main__ - INFO: 平移向量是:
[[-0.00797824]
[-0.17640328]
[ 0.07459845]]
2025-09-08 14:37:26 - __main__ - INFO: 四元数是：
[ 0.00673467 0.72826199 -0.68342243 0.05022845] [ 0.00673467 0.72826199 -0.68342243 0.05022845]

46
python/hand_eye_calibration/libs/web-log.2025-08-28.log Normal file
View File

@ -0,0 +1,46 @@
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-08-23 16:08:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-08-23 16:08:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-08-23 16:08:46 - __main__ - INFO: 旋转矩阵是:
[[-0.99486349 0.07846371 0.06395369]
[-0.05884529 0.06577685 -0.9960977 ]
[-0.08236419 -0.99474462 -0.06082177]]
2025-08-23 16:08:46 - __main__ - INFO: 平移向量是:
[[-0.00797824]
[-0.17640328]
[ 0.07459845]]
2025-08-23 16:08:46 - __main__ - INFO: 四元数是：
[ 0.00673467 0.72826199 -0.68342243 0.05022845]

167
python/hand_eye_calibration/libs/web-log.2025-09-05.log Normal file
View File

@ -0,0 +1,167 @@
2025-09-05 16:02:35 - libs.auxiliary - INFO: create folder eye_hand_data/data20250905
2025-09-05 16:04:01 - libs.auxiliary - INFO: create folder eye_hand_data/data2025090501
2025-09-05 16:04:01 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-09-05 16:05:28 - libs.auxiliary - INFO: create folder eye_hand_data/data2025090502
2025-09-05 16:05:28 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-09-05 16:05:42 - libs.auxiliary - INFO: create folder eye_hand_data/data2025090503
2025-09-05 16:05:42 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-09-05 16:05:48 - __main__ - INFO: 相机已关闭，窗口已销毁
2025-09-05 16:09:41 - libs.auxiliary - INFO: create folder eye_hand_data/data2025090504
2025-09-05 16:09:41 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-09-05 16:12:46 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-09-05 16:12:53 - __main__ - INFO: 当前关节角: [-0.020066101407867926, 1.46894512840519, 1.4589372327079881, 0.15968113499835065, 0.0808671765475035, -0.13818357132059936, -0.2431388026164853]
2025-09-05 16:12:53 - __main__ - INFO: 获取位姿成功:
[[-0.10992689 0.05250327 -0.99255201 -0.00608509]
[ 0.0426073 -0.99743702 -0.0574805 -0.22011888]
[-0.99302603 -0.04860861 0.10740812 -0.73570586]
[ 0. 0. 0. 1. ]]
2025-09-05 16:12:53 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-09-05 16:12:53 - __main__ - INFO: ===采集第1次数据！ pose 已保存到 eye_hand_data/data2025090504/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025090504/1.jpg
2025-09-05 16:12:56 - __main__ - INFO: robot.torqueOff
2025-09-05 16:25:21 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-09-05 16:25:26 - __main__ - INFO: 当前关节角: [-0.019045662455530632, 0.14962957400644405, 1.3928232302974393, -0.07693350304193537, -1.3815034772680232, 0.03932344561816248, -0.20795691527650662]
2025-09-05 16:25:26 - __main__ - INFO: 获取位姿成功:
[[-0.03644065 0.99920824 0.01596754 -0.01550389]
[-0.99665653 -0.03750756 0.07258755 -0.94854757]
[ 0.07312898 -0.01326902 0.99723422 -0.00291912]
[ 0. 0. 0. 1. ]]
2025-09-05 16:25:26 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-09-05 16:25:26 - __main__ - INFO: ===采集第2次数据！ pose 已保存到 eye_hand_data/data2025090504/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025090504/2.jpg
2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-09-05 17:21:04 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-09-05 17:21:04 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-09-05 17:21:04 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-09-05 17:21:04 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-09-05 17:21:04 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-09-05 17:21:04 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-09-05 17:21:04 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-09-05 17:21:04 - __main__ - INFO: 旋转矩阵是:
[[ 0.05477568 0.03458859 0.99789942]
[-0.99767126 0.04257346 0.0532875 ]
[-0.04064089 -0.99849443 0.03684003]]
2025-09-05 17:21:04 - __main__ - INFO: 平移向量是:
[[-0.14908435]
[-0.19737896]
[-0.05964516]]
2025-09-05 17:21:04 - __main__ - INFO: 四元数是：
[-0.49380213 0.48758531 -0.48463669 0.53249159]
2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-09-05 17:26:28 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-09-05 17:26:28 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-09-05 17:26:28 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-09-05 17:26:28 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-09-05 17:26:28 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-09-05 17:26:28 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-09-05 17:26:28 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-09-05 17:26:28 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-09-05 17:26:28 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-09-05 17:26:28 - __main__ - INFO: 旋转矩阵是:
[[-0.99486349 0.07846371 0.06395369]
[-0.05884529 0.06577685 -0.9960977 ]
[-0.08236419 -0.99474462 -0.06082177]]
2025-09-05 17:26:28 - __main__ - INFO: 平移向量是:
[[-0.00797824]
[-0.17640328]
[ 0.07459845]]
2025-09-05 17:26:28 - __main__ - INFO: 四元数是：
[ 0.00673467 0.72826199 -0.68342243 0.05022845]
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-09-05 17:45:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-09-05 17:45:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-09-05 17:45:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-09-05 17:45:13 - __main__ - INFO: 旋转矩阵是:
[[-0.99486349 0.07846371 0.06395369]
[-0.05884529 0.06577685 -0.9960977 ]
[-0.08236419 -0.99474462 -0.06082177]]
2025-09-05 17:45:13 - __main__ - INFO: 平移向量是:
[[-0.00797824]
[-0.17640328]
[ 0.07459845]]
2025-09-05 17:45:13 - __main__ - INFO: 四元数是：
[ 0.00673467 0.72826199 -0.68342243 0.05022845]

BIN
python/vision_servo/__pycache__/get_chessboard_position.cpython-310.pyc
View File

Binary file not shown.

527
python/vision_servo/follow_target.py
View File

@ -1,20 +1,290 @@
import os import os
os.environ["GLOG_minloglevel"] = "1" import re
import sys
from utils import root_path
sys.path.append(os.path.join(root_path, "cmake-build-debug/example"))
from robot_wrapper import Robot
import time import time
import signal import signal
import cv2
import subprocess
import numpy as np import numpy as np
import pyrealsense2 as rs
from scipy.spatial.transform import Rotation as R from scipy.spatial.transform import Rotation as R
from utils import solve_ik from get_chessboard_position import init_realsense, get_closest_red_point
from utils import get_pose
from target_position import init_realsense, get_closest_red_point import sys
# 把.so所在目录加入 Python 路径
sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example")
# 导入模块
from robot_wrapper import Robot
def get_pose(joint_positions, base_link="PELVIS_S", target_link="R_FINGER_TIP", exec_path="/home/lgv/cmvr/cmvr-es/cmake-build-debug/example/solve_fk"):
# ----------- 1. 构造命令行参数 -----------
joint_str = [str(j) for j in joint_positions]
cmd = [exec_path, base_link, target_link, *joint_str]
# ----------- 2. 运行并捕获输出 -----------
res = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, check=True, text=True)
# ----------- 3. 解析 4×4 变换矩阵 -----------
# 定位起始行（含 “变换矩阵T” 或 “T:”）
lines = res.stdout.splitlines()
start_idx = next((i for i, l in enumerate(lines) if re.search(r"变换矩阵|T\s*:", l)), None)
if start_idx is None or start_idx + 4 >= len(lines):
raise RuntimeError("未在 solve_fk 输出中找到 4×4 变换矩阵:\n" + res.stdout)
try:
mat = np.array([[float(x) for x in lines[start_idx + 1 + r].split()] for r in range(4)], dtype=np.float64)
if mat.shape != (4, 4):
raise ValueError
except Exception:
raise RuntimeError("矩阵解析失败，原始输出:\n" + res.stdout)
return mat
def solve_ik(x, y, z, rx, ry, rz, base_link="PELVIS_S", target_link="R_WRIST_R_S",
exe_path= "/home/lgv/cmvr/cmvr-es/cmake-build-debug/example/solve_ik"):
cmd = [exe_path, base_link, target_link, f"{x}", f"{y}", f"{z}", f"{rx}", f"{ry}", f"{rz}"]
print(" ".join(cmd))
res = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, check=True)
lines = res.stdout.splitlines()
success_line = next((l for l in lines if "[IK Solve]" in l), "")
success = "success=true" in success_line.lower()
if not success:
return False, None, None
def _parse(tag):
pats = rf"^{tag}\s*:\s*(.+)$"
for l in lines:
m = re.match(pats, l.strip())
if m:
return [float(v) for v in m.group(1).split()]
raise RuntimeError(f"success=true 但未找到 {tag}: 行！\n{res.stdout}")
left_q = _parse("left")
right_q = _parse("right")
return True, left_q, right_q
def moveJ(q, side='right'):
cmd = ["moveJ" ,"one" , f"{side}", f"{q[0]}", f"{q[1]}", f"{q[2]}", f"{q[3]}", f"{q[4]}", f"{q[5]}", f"{q[6]}"]
res = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, check=True)
print("movej success")
def rotation_to_degree(mat):
pitch = -np.arcsin(mat[2, 0])
if np.abs(np.cos(pitch)) > 1e-6: # 非奇异
roll = np.arctan2(mat[2, 1], mat[2, 2])
yaw = np.arctan2(mat[1, 0], mat[0, 0])
else: # gimbal lock
roll = 0.0
yaw = np.arctan2(-mat[0, 1], mat[1, 1])
return yaw, pitch, roll
def black_point_position(color_frame, depth_frame, depth_intr, vis_path=None, min_radius=10, avg_window=3):
"""
检测白底黑圆的圆心坐标，返回相机系 (X, Y, Z) [m]
参数
----
color_frame / depth_frame : 对齐后的 RealSense frame
depth_intr : 深度流 intrinsics (rs.intrinsics)
vis_path : 若给定，则保存可视化图片
min_radius : HoughCircles/轮廓的最小半径，像素
avg_window : 深度均值窗口半径（像素）
"""
color_img = np.asanyarray(color_frame.get_data()).copy()
gray = cv2.cvtColor(color_img, cv2.COLOR_BGR2GRAY)
# 1. 二值化（寻找黑色区域）
# Otsu 自动阈值 + 取反 => 黑圆为白，背景为黑
_, mask = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
# 2. 轮廓检测，取面积最大的圆形候选
cnts, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if not cnts:
raise RuntimeError("未检测到任何黑色区域")
# 取最大面积
cnt = max(cnts, key=cv2.contourArea)
(u, v), radius = cv2.minEnclosingCircle(cnt)
if radius < min_radius:
raise RuntimeError(f"检测到圆半径过小 ({radius:.1f}px)，请检查 min_radius 设置或图像质量")
# 3. 取邻域深度中值
width, height = depth_frame.get_width(), depth_frame.get_height()
depths = [
depth_frame.get_distance(int(round(u + du)), int(round(v + dv)))
for du in range(-avg_window, avg_window + 1)
for dv in range(-avg_window, avg_window + 1)
if 0 <= int(round(u + du)) < width and
0 <= int(round(v + dv)) < height
]
depths = [d for d in depths if d > 0]
if not depths:
raise RuntimeError("圆心处深度无效 (0)")
depth = float(np.median(depths))
# 4. 像素 -> 相机坐标
x, y, z = rs.rs2_deproject_pixel_to_point(
depth_intr, [u, v], depth
)
pos = np.array([x, y, z], dtype=np.float32)
# 5. 可视化
if vis_path is not None:
cx, cy = depth_intr.ppx, depth_intr.ppy
cv2.drawMarker(color_img, (int(cx), int(cy)), (0, 255, 0), markerType=cv2.MARKER_CROSS, markerSize=20, thickness=2)
cv2.circle(color_img, (int(u), int(v)), int(radius), (0, 0, 255), 2)
cv2.circle(color_img, (int(u), int(v)), 5, (0, 0, 255), -1)
cv2.imwrite(vis_path, color_img)
print(f"✓ 已保存标记图到 {vis_path}")
return pos
def red_point_position(color_frame, depth_frame, depth_intr, vis_path=None, min_radius=3, avg_window=3):
"""
检测白底红圆的圆心坐标，返回相机系 (X,Y,Z) [m]
"""
# --- 1. 取彩色帧 ----
color_img = np.asanyarray(color_frame.get_data()).copy()
hsv = cv2.cvtColor(color_img, cv2.COLOR_BGR2HSV)
# --- 2. 阈值分割：红色有两个 Hue 区间 (0-10)∪(170-180) ---
lower_red1 = np.array([0, 100, 100])
upper_red1 = np.array([10, 255, 255])
lower_red2 = np.array([170, 100, 100])
upper_red2 = np.array([180, 255, 255])
mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
mask = cv2.bitwise_or(mask1, mask2)
# 可选：形态学开闭运算去噪
kernel = np.ones((5,5), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
# --- 3. 轮廓取最大圆 ---
cnts, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if not cnts:
cv2.imwrite(vis_path, color_img)
raise RuntimeError("未检测到任何红色区域")
cnt = max(cnts, key=cv2.contourArea)
(u, v), radius = cv2.minEnclosingCircle(cnt)
if radius < min_radius:
raise RuntimeError(f"检测到圆半径过小 ({radius:.1f}px)，请检查 min_radius 或图像质量")
# --- 4. 深度中值 ---
width, height = depth_frame.get_width(), depth_frame.get_height()
depths = [
depth_frame.get_distance(int(round(u + du)), int(round(v + dv)))
for du in range(-avg_window, avg_window + 1)
for dv in range(-avg_window, avg_window + 1)
if 0 <= int(round(u + du)) < width and
0 <= int(round(v + dv)) < height
]
depths = [d for d in depths if d > 0]
if not depths:
raise RuntimeError("圆心处深度无效 (0)")
depth = float(np.median(depths))
# --- 5. 反投影到 3-D ---
x, y, z = rs.rs2_deproject_pixel_to_point(depth_intr, [u, v], depth)
pos = np.array([x, y, z], dtype=np.float32)
# --- 6. 可视化保存 ---
if vis_path is not None:
cx, cy = depth_intr.ppx, depth_intr.ppy
cv2.drawMarker(color_img, (int(cx), int(cy)), (0, 255, 0), markerType=cv2.MARKER_CROSS, markerSize=20, thickness=2)
cv2.circle(color_img, (int(u), int(v)), int(radius), (255, 0, 0), 2) # 蓝圈标红圆
cv2.circle(color_img, (int(u), int(v)), 5, (255, 0, 0), -1)
cv2.imwrite(vis_path, color_img)
print(f"✓ 已保存标记图到 {vis_path}")
return pos
# if __name__ == "__main__":
# # 初始化机器人（传入配置文件路径和机器人名称）
#
# pipeline, align, depth_intr = start_pipeline("243122075614")
# robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
# js = robot.getJointQ('right')
# T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S")
# T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
# P_base_cam = T_base_cam[:3, 3]
#
# color_f, depth_f = get_aligned_frames(pipeline, align)
# # P_cam_target = black_point_position(color_f, depth_f, depth_intr, vis_path="black_point.png")
# P_cam_target = red_point_position(color_f, depth_f, depth_intr, vis_path="red_point.png")
#
# P_base_target = np.array([
# P_base_cam[0] + P_cam_target[2],
# P_base_cam[1] + P_cam_target[0],
# P_base_cam[2] - P_cam_target[1],
# ])
# print("P_base_target:", P_base_target)
#
# P_base_tool = P_base_target - np.array([0.45, 0, 0])
# T_base_tool = T_base_ee
# T_base_tool[:3, 3] = P_base_tool
#
# print("T_base_tool:", T_base_tool)
#
# target_x, target_y, target_z = T_base_tool[0, 3], T_base_tool[1, 3], T_base_tool[2, 3]
# target_rx, target_ry, target_rz =R.from_matrix(T_base_tool[:3, :3]).as_euler('xyz', degrees=True)
#
# ok, lq, rq = solve_ik(target_x, target_y, target_z, target_rx, target_ry, target_rz)
# if not ok:
# print("solve ik failed")
# exit(-1)
# # moveJ(rq)
#
# # try:
# # while True:
# # js = get_joint_position('right')
# # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S")
# # T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
# # P_base_cam = T_base_cam[:3, 3]
# #
# # color_f, depth_f = get_aligned_frames(pipeline, align)
# # # P_cam_target = black_point_position(color_f, depth_f, depth_intr, vis_path="black_point.png")
# # P_cam_target = red_point_position(color_f, depth_f, depth_intr, vis_path="red_point.png")
# #
# # P_base_target = np.array([
# # P_base_cam[0] + P_cam_target[2],
# # P_base_cam[1] + P_cam_target[0],
# # P_base_cam[2] - P_cam_target[1],
# # ])
# # print("P_base_target:", P_base_target)
# #
# # P_base_tool = P_base_target - np.array([0.3, 0, 0])
# # T_base_tool = T_base_ee
# # T_base_tool[:3, 3] = P_base_tool
# #
# # print("T_base_tool:", T_base_tool)
# #
# # target_x, target_y, target_z = T_base_tool[0, 3], T_base_tool[1, 3], T_base_tool[2, 3]
# # target_rx, target_ry, target_rz =R.from_matrix(T_base_tool[:3, :3]).as_euler('xyz', degrees=True)
# #
# # ok, lq, rq = solve_ik(target_x, target_y, target_z, target_rx, target_ry, target_rz)
# # if not ok:
# # print("solve ik failed")
# # exit(-1)
# # # moveJ(rq)
# # except Exception as e:
# # print(e)
# ========== Ctrl+C 处理 ==========
def signal_handler(sig, frame): def signal_handler(sig, frame):
print("\n收到 Ctrl+C，准备退出...") print("\n收到 Ctrl+C，准备退出...")
raise SystemExit raise SystemExit
@ -35,96 +305,191 @@ def transform_point(T, P):
return P_new[:3] return P_new[:3]
def compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target): def compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target):
"""
已知:
T_base_ee : 基座->末端 (4x4)
R_ee_cam : 末端->相机的旋转 (3x3)
t_ee_cam : 末端->相机的平移 (3,)
P_cam_target : 目标点在相机下的坐标 (3,)
返回:
P_base_target : 目标点在基座下的坐标 (3,)
"""
# 拼接 T_ee_cam
T_ee_cam = rt_to_transform(R_ee_cam, t_ee_cam) T_ee_cam = rt_to_transform(R_ee_cam, t_ee_cam)
# 得到相机在 base 下的位姿
T_base_cam = T_base_ee @ T_ee_cam T_base_cam = T_base_ee @ T_ee_cam
# 把目标点从相机系变换到 base 系
P_base_target = transform_point(T_base_cam, P_cam_target) P_base_target = transform_point(T_base_cam, P_cam_target)
return P_base_target return P_base_target
IDEL_Q = [-0.05804541534555635, 1.460164607187404, 1.458934384971377, 0.29042831678163805,
-1.498103103566999, 0.039690803641003906, -0.08653132466712826]
INIT_POSE = [0.45, -0.25, -0.15, 0, 0, 0]
# FOLLOW_OFFSET = [-0.3, 0, 0]
# EE_LINK = "EE"
# FOLLOW_OFFSET = [-0.2, 0, 0]
# EE_LINK = "CAM"
FOLLOW_OFFSET = [-0.01, 0, 0]
EE_LINK = "FINGER"
def main():
robot = Robot(os.path.join(root_path, "config/cabin_robot.xml"), "hc01")
current_q = robot.getJointQ('right')
diff = np.array(IDEL_Q) - np.array(current_q)
if np.max(np.abs(diff)) >= 0.01:
robot.moveJ("right", IDEL_Q)
if __name__ == "__main__":
pipeline, align = init_realsense() pipeline, align = init_realsense()
ok, _, init_rq = solve_ik(INIT_POSE, base_link="PELVIS_S", target_link="R_FINGER_TIP")
if not ok:
print("solve ik init_pose failed, init_pose:", INIT_POSE)
return
print("init q:", init_rq)
print("Robot movej: ", init_rq)
robot.moveJ("right", init_rq)
try: try:
while True: # 初始化机器人
current_q = robot.getJointQ('right') robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
T_base_ee = get_pose(current_q, base_link="PELVIS_S", target_link="R_WRIST_R_S")
T_base_cam = get_pose(current_q, base_link="PELVIS_S", target_link="R_CAM") # 移动到初始关节位姿
T_base_tool = get_pose(current_q, base_link="PELVIS_S", target_link="R_FINGER_TIP")
print(T_base_cam) init_joint = [-0.304106 , 1.30538 , 1.4465 ,1.93647 , -2.84955 ,-0.116586 ,0.123911]
# init_joint = [0 ,0 , 0 ,0 , 0 ,0 ,0]
robot.moveJ("right", init_joint)
js = robot.getJointQ('right')
print("当前关节角:", js)
# 获取末端在基座下的位姿
# T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
print("末端位姿 T_base_ee:\n", T_base_ee)
# T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
# print("末端位姿 T_base_ee:\n", T_base_ee)
#
# T_base_ee = get_pose(js, base_link="R_WRIST_R_S", target_link="R_FINGER_TIP")
# print("末端位姿 T_base_ee:\n", T_base_ee)
# while True:
# time.sleep(1)
# 获取红点在相机坐标系下的坐标
P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png") P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png")
if P_cam_target is None: if P_cam_target is None:
print("can not find red point") raise RuntimeError("未检测到红点，无法计算目标点")
time.sleep(2) print("红点在相机坐标系下:", P_cam_target)
continue
T_cam_target = np.eye(4) # P_cam_target = np.array([0.0716422, 0.0482324, 0.325])
T_cam_target[:3, 3] = P_cam_target
T_base_target = T_base_cam @ T_cam_target
P_base_target = T_base_target[:3, 3]
desire_P_base_tool = P_base_target + FOLLOW_OFFSET
if EE_LINK == "EE": # "--frame-id",
desire_rpy_base_tool = R.from_matrix(T_base_ee[:3, :3]).as_euler('xyz', degrees=True) # "R_FINGER_TIP",
desire_pose = list(desire_P_base_tool) + list(desire_rpy_base_tool) # "--child-frame-id",
ok, _, rq = solve_ik(desire_pose, base_link="PELVIS_S", target_link="R_WRIST_R_S") # "camera_color_optical_frame",
elif EE_LINK == "CAM": # "--x",
desire_rpy_base_tool = R.from_matrix(T_base_cam[:3, :3]).as_euler('xyz', degrees=True) # "-0.132714",
desire_pose = list(desire_P_base_tool) + list(desire_rpy_base_tool) # "--y",
ok, _, rq = solve_ik(desire_pose, base_link="PELVIS_S", target_link="R_CAM") # "0.00161894",
elif EE_LINK == "FINGER": # "--z",
desire_rpy_base_tool = R.from_matrix(T_base_tool[:3, :3]).as_euler('xyz', degrees=True) # "0.0760576",
desire_pose = list(desire_P_base_tool) + list(desire_rpy_base_tool) # "--qx",
ok, _, rq = solve_ik(desire_pose, base_link="PELVIS_S", target_link="R_FINGER_TIP") # "0.512312",
else: # "--qy",
raise RuntimeError("undefined EE_LINK: ", EE_LINK) # "-0.503306",
# "--qz",
# "0.493563",
# "--qw",
# "-0.490525",
# # "--roll",
# # "0.18103",
# # "--pitch",
# # "1.60289",
# # "--yaw",
# # "-1.76387",
# 手眼标定结果：四元数和平移
# q_ee_cam = np.array([0.519375,-0.502778, 0.4866,-0.490596])
# t_ee_cam = np.array([-0.0815577, 0.0321285, 0.0746921])
q_ee_cam = np.array([0.512312, -0.503306, 0.493563,-0.490525])
t_ee_cam = np.array([-0.124714, -0.02281894, 0.0800576])
R_ee_cam = R.from_quat(q_ee_cam).as_matrix()
print("末端->相机旋转矩阵 R_ee_cam:\n", R_ee_cam)
# 构造相机相对于 末端的变换矩阵
T_ee_cam = np.eye(4)
T_ee_cam[:3, :3] = R_ee_cam
T_ee_cam[:3, 3] = t_ee_cam
# 相机相对于 基座的 变换矩阵
T_base_cam = T_base_ee @ T_ee_cam
# 相机下红点 -> 基座下红点
P_base_target = np.append(P_cam_target, 1) # 齐次坐标
P_base_target = (T_base_cam @ P_base_target)[:3]
print("红点在基座坐标系下:", P_base_target)
# while True:
# time.sleep(1)
# ----------------- IK 求解 -----------------
# 目标位姿 为 相机 相对于 基座的 姿态
# R_base_target = T_base_ee[:3, :3] @ R_ee_cam
R_base_target = T_base_ee[:3, :3]
rot = R.from_matrix(R_base_target)
target_euler = rot.as_euler('xyz', degrees=True)
d = rot.as_euler('xyz', degrees=False)
target_rx, target_ry, target_rz = target_euler
quat = rot.as_quat()
print("目标旋转欧拉角 (XYZ 度):", target_euler)
print("目标旋转欧拉角 (XYZ 弧度):", d)
print("目标旋转四元数 [x,y,z,w]:", quat)
# while True:
# time.sleep(10)
# IK 求解
ok, lq, rq = solve_ik(*P_base_target, target_rx, target_ry, target_rz,
base_link="PELVIS_S", target_link="R_FINGER_TIP")
if not ok: if not ok:
print("solve ik failed, pose=", desire_pose) raise RuntimeError("IK 求解失败")
time.sleep(1)
else: # 移动到目标点
current_q = robot.getJointQ('right')
diff = np.array(rq) - np.array(current_q)
if np.max(diff) < 0.01:
continue
print("move position:", desire_P_base_tool)
print("Robot movej: ", rq)
input_data = input("move robot? type \"yes\" to move, or type \"no\" to skip \n")
if input_data == "yes":
robot.moveJ("right", rq) robot.moveJ("right", rq)
time.sleep(5) print("已到达目标点，按 Ctrl+C 回到初始位姿...")
robot.moveJ("right", init_rq)
print("Return to init pose, Robot movej: ", init_rq) while True:
time.sleep(1)
except SystemExit: except Exception as e:
print("安全退出程序...") print("捕获异常:", e)
finally: finally:
robot.moveJ("right", IDEL_Q) # 回到初始位姿
robot.moveJ("right", [-0.05804541534555635, 1.460164607187404, 1.458934384971377, 0.29042831678163805,
-1.498103103566999, 0.039690803641003906, -0.08653132466712826])
pipeline.stop() pipeline.stop()
align = None
if __name__ == '__main__': # if __name__ == "__main__":
main() #
# try:
# # 初始化机器人
# robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
# js = robot.getJointQ('right')
# # js = np.zeros(7)
# # js[6] = 0.8
#
# T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S")
# T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP")
#
# print(T_base_cam - T_base_ee)
# T_ee1_ee2 = get_pose(js, base_link="R_FINGER_TIP", target_link="R_WRIST_R_S")
# print(T_ee1_ee2)
#
#
# T_ee2_ee1 = get_pose(js, base_link="R_WRIST_R_S", target_link="R_FINGER_TIP")
# print(T_ee2_ee1)
#
#
# # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364])
#
# # robot.moveJ("right", [-0.344938, 0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996])
# # js = robot.getJointQ('right')
# # print("js = ")
# # print(js)
#
#
# print("已到达目标点，按 Ctrl+C 回到初始位姿...")
# while True:
# time.sleep(1)
# except SystemExit:
# print("安全退出程序...")
# #
# # finally:
# # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364])

85
python/vision_servo/get_chessboard_position.py
View File

@ -8,29 +8,38 @@
import cv2 import cv2
import numpy as np import numpy as np
import pyrealsense2 as rs import pyrealsense2 as rs
import sys
import time import time
# 把.so所在目录加入 Python 路径
sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example")
# 导入机器人控制模块
from robot_wrapper import Robot
def init_realsense(): def init_realsense():
""" """初始化 RealSense 管道并返回 pipeline 和 align 对象"""
初始化 RealSense 管道并返回 pipeline 和 align 对象
"""
pipeline = rs.pipeline() pipeline = rs.pipeline()
config = rs.config() config = rs.config()
config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 30) config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
profile = pipeline.start(config) profile = pipeline.start(config)
align = rs.align(rs.stream.color) align = rs.align(rs.stream.color)
return pipeline, align return pipeline, align
def stop_pipeline(pipeline: rs.pipeline):
pipeline.stop()
def get_closest_red_point(pipeline, align, vis_path=None, warmup=30,
def get_closest_red_point(pipeline, align, vis_path = None,warmup=10): hough_params=None):
""" """
获取最近红色圆点的 3D 坐标。 获取最近红色圆点的 3D 坐标。
返回 (x, y, z) 或 None（如果没检测到）。 返回 (x, y, z) 或 None（如果没检测到）。
""" """
if hough_params is None:
hough_params = dict(dp=1.2, minDist=20,
param1=50, param2=15,
minRadius=5, maxRadius=50)
# 丢掉前几帧，让相机稳定 # 丢掉前几帧，让相机稳定
for _ in range(warmup): for _ in range(warmup):
pipeline.wait_for_frames() pipeline.wait_for_frames()
@ -47,53 +56,61 @@ def get_closest_red_point(pipeline, align, vis_path = None,warmup=10):
color_image = np.asanyarray(color_frame.get_data()) color_image = np.asanyarray(color_frame.get_data())
depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics
# 转换到 HSV 并提取红色区域
hsv = cv2.cvtColor(color_image, cv2.COLOR_BGR2HSV) hsv = cv2.cvtColor(color_image, cv2.COLOR_BGR2HSV)
lower_red1 = np.array([0, 100, 100]) lower_red1, upper_red1 = np.array([0, 100, 100]), np.array([10, 255, 255])
upper_red1 = np.array([10, 255, 255]) lower_red2, upper_red2 = np.array([160, 100, 100]), np.array([179, 255, 255])
lower_red2 = np.array([160, 100, 100]) mask = cv2.inRange(hsv, lower_red1, upper_red1) | cv2.inRange(hsv, lower_red2, upper_red2)
upper_red2 = np.array([179, 255, 255])
mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
mask = cv2.bitwise_or(mask1, mask2)
# 平滑处理再检测圆
mask_blur = cv2.GaussianBlur(mask, (9, 9), 2) mask_blur = cv2.GaussianBlur(mask, (9, 9), 2)
circles = cv2.HoughCircles(mask_blur, cv2.HOUGH_GRADIENT, dp=1.2, minDist=20, circles = cv2.HoughCircles(mask_blur, cv2.HOUGH_GRADIENT, **hough_params)
param1=50, param2=15, minRadius=5, maxRadius=50)
closest_point = None closest_point, min_depth = None, float('inf')
min_depth = float('inf')
if circles is not None: if circles is not None:
circles = np.uint16(np.around(circles)) circles = np.uint16(np.around(circles))
for i in circles[0, :]: for u, v, r in circles[0, :]:
u, v, r = i depth = depth_frame.get_distance(int(u), int(v))
depth = depth_frame.get_distance(u, v)
if 0 < depth < min_depth: if 0 < depth < min_depth:
min_depth = depth min_depth = depth
closest_point = rs.rs2_deproject_pixel_to_point(depth_intrin, [u, v], depth) closest_point = rs.rs2_deproject_pixel_to_point(depth_intrin, [int(u), int(v)], depth)
if vis_path:
if vis_path is not None and closest_point is not None: cv2.circle(color_image, (u, v), r, (0, 255, 0), 2)
cv2.circle(color_image, (u, v), 6, (0, 0, 255), -1) cv2.circle(color_image, (u, v), 6, (0, 0, 255), -1)
if vis_path and closest_point is not None:
cv2.imwrite(vis_path, color_image) cv2.imwrite(vis_path, color_image)
return closest_point return closest_point
def main(serial="243122075614", avg_window=3, loop=True):
# ========================= 4. 主程序 ========================= #
def main(serial="243122075614", avg_window=3):
pipeline, align = init_realsense() pipeline, align = init_realsense()
# robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
try: try:
# 调用获取最近红点 while True:
point_3d = get_closest_red_point(pipeline, align, vis_path="red_point.png") point_3d = get_closest_red_point(pipeline, align, vis_path="red_point.png")
print("Closest red 3D point:", point_3d) if point_3d:
print("Closest red 3D point:", np.round(point_3d, 4))
else:
print("未检测到红点")
if not loop:
break
key = cv2.waitKey(1)
if key == 27: # ESC 键退出
break
except Exception as e:
print("Error:", e)
finally: finally:
pipeline.stop() pipeline.stop()
cv2.destroyAllWindows() cv2.destroyAllWindows()
if __name__ == "__main__": if __name__ == "__main__":
main() main()

BIN
python/vision_servo/red_point.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 1.1 MiB

After

Width:  |  Height:  |  Size: 1.2 MiB

21
python/vision_servo/robot_warpper_test.py
View File

@ -43,16 +43,27 @@ robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
# while True: # while True:
# for j in JOINT_POSITIONS: # for j in JOINT_POSITIONS:
# robot.moveJ('right', j) # robot.moveJ('right', j)
robot.torqueOn() # robot.torqueOn()
# time.sleep(10)
# js = robot.getJointQ('right')
# print(js)
# 控制左臂关节 # 控制左臂关节
# robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) # robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
# # robot.moveJ("right", [-0.344938, 0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996]) # # robot.moveJ("right", [-0.344938, 0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996])
# # robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) # robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
# # # #
# time.sleep(200)
# robot.calibrateZeroQ("R_WRIST_Y")
# robot.calibrateZeroQ("R_WRIST_R")
# # robot.torqueOff("R_WRIST_R")
# time.sleep(10) # time.sleep(10)
# robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) # robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
# robot.torqueOff() # time.sleep(20)
# # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364])
# # robot.torqueOff()
# # time.sleep(500)
# robot.torqueOff("WAIST_P")
# time.sleep(30)
# // {"R_SHOULDER_P", 0.00203898}, # // {"R_SHOULDER_P", 0.00203898},
# // {"R_SHOULDER_R", 1.34062}, # // {"R_SHOULDER_R", 1.34062},
# // {"R_SHOULDER_Y", 0.0}, # // {"R_SHOULDER_Y", 0.0},

0
python/vision_servo/test.py Normal file
View File

45
src/controller/touch_controller.cpp
View File

@ -112,3 +112,48 @@ bool TouchController::isArrive(double max_force) {
return force > max_force; return force > max_force;
} }
void TouchController::touch(int u, int v, double max_force) {
LOG(INFO) << "Touch request at pixel (" << u << ", " << v << ") with max_force=" << max_force;
// 获取目标点和当前位姿
auto target_pose = cam_->get3DPointFromPixel(u, v);
auto cur_pose = robot_->fk("PELVIS_S", "R_FINGER_TIP");
LOG(INFO) << "Target 3D Pose: " << target_pose;
// 定义关键点位
auto pre_touch_pose = cur_pose;
pre_touch_pose.mutable_position()->set_x(target_pose[0] - 0.05);
pre_touch_pose.mutable_position()->set_y(target_pose[1]);
pre_touch_pose.mutable_position()->set_z(target_pose[2]);
auto touch_pose = pre_touch_pose;
touch_pose.mutable_position()->set_x(target_pose[0]);
const auto& retreat_pose = cur_pose;
// retreat_pose.mutable_position()->set_x(target_pose[0] - 0.20);
// 1. 移动到预接触位置
robot_->moveJ("PELVIS_S", "R_FINGER_TIP", pre_touch_pose);
// 2. 向前接触并开启压力监测
robot_->servoJ("PELVIS_S", "R_FINGER_TIP", touch_pose);
// 3. 等待达到最大压力
constexpr int k_sleep_ms = 5;
constexpr int k_timeout_ms = 5000;
int elapsed_ms = 0;
while (!isArrive(max_force) && elapsed_ms < k_timeout_ms) {
std::this_thread::sleep_for(std::chrono::milliseconds(k_sleep_ms));
elapsed_ms += k_sleep_ms;
}
if (elapsed_ms >= k_timeout_ms) {
LOG(WARNING) << "Timeout waiting for force feedback!";
} else {
LOG(INFO) << "Max force reached, retreating.";
robot_->moveJ("PELVIS_S", "R_FINGER_TIP", retreat_pose);
}
}

6
src/controller/touch_controller_test.cpp
View File

@ -26,7 +26,7 @@ TEST(TouchControllerTest,MyTest) {
auto robot = dmgr.getDevice<AbstractRobot>("hc01"); auto robot = dmgr.getDevice<AbstractRobot>("hc01");
auto hand = dmgr.getDevice<AbstractDexHand>("hand1"); auto hand = dmgr.getDevice<AbstractDexHand>("hand1");
auto cam = dmgr.getDevice<AbstractCamera>("cam4");
cmvr::msgs::Pose3d pose; cmvr::msgs::Pose3d pose;
@ -52,14 +52,12 @@ TEST(TouchControllerTest,MyTest) {
delta_pose.mutable_euler()->set_rz(0); delta_pose.mutable_euler()->set_rz(0);
std::this_thread::sleep_for(std::chrono::milliseconds(10000)); std::this_thread::sleep_for(std::chrono::milliseconds(10000));
robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.02); robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.02);
Pose3d offset; Pose3d offset;
TouchController controller(robot, hand); TouchController controller(robot, hand,cam);
while (true) { while (true) {
// controller.touch(pose,offset,500); // controller.touch(pose,offset,500);

39
src/devices/camera/realsense_camera/CMakeLists.txt
View File

@ -8,3 +8,42 @@ add_library(cmvr_es::device::realsense_camera ALIAS realsense_camera) # ✅ 命
find_package(realsense2 REQUIRED) find_package(realsense2 REQUIRED)
# 链接 librealsense2 # 链接 librealsense2
target_link_libraries(realsense_camera PRIVATE realsense2::realsense2) target_link_libraries(realsense_camera PRIVATE realsense2::realsense2)
# --------------------------------------------------------
# Unit test
# --------------------------------------------------------
find_package(glog REQUIRED)
find_package(protobuf REQUIRED)
find_package(PkgConfig REQUIRED)
find_package(fcl REQUIRED)
find_package(OpenCV REQUIRED)
include_directories(
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
)
link_directories(
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
)
add_executable(realsense_camera_test
${CMAKE_CURRENT_SOURCE_DIR}/realsense_camera_test.cpp
)
target_link_libraries(realsense_camera_test
PRIVATE
protobuf::libprotobuf
glog::glog
gtest
gtest_main
pthread
proto-objects
ccd
fcl
${OpenCV_LIBS}
cmvr_es::device::realsense_camera
cmvr_es::device_manager
)

65
src/devices/camera/realsense_camera/realsense_camera.cpp
View File

@ -137,12 +137,6 @@ void RealsenseCamera::init() {
rs_cfg_.enable_device(serial_); rs_cfg_.enable_device(serial_);
if (stream_mode_ == RGBD_MODE){ if (stream_mode_ == RGBD_MODE){
if (align_mode_ == "color") {
align_ = std::make_shared<rs2::align>(RS2_STREAM_COLOR);
}
else if (align_mode_ == "depth") {
align_ = std::make_shared<rs2::align>(RS2_STREAM_DEPTH);
}
rs_cfg_.enable_stream(RS2_STREAM_COLOR, state_.width, state_.height, RS2_FORMAT_BGR8, state_.fps); rs_cfg_.enable_stream(RS2_STREAM_COLOR, state_.width, state_.height, RS2_FORMAT_BGR8, state_.fps);
rs_cfg_.enable_stream(RS2_STREAM_DEPTH, state_.width, state_.height, RS2_FORMAT_Z16, state_.fps); rs_cfg_.enable_stream(RS2_STREAM_DEPTH, state_.width, state_.height, RS2_FORMAT_Z16, state_.fps);
} }
@ -191,11 +185,43 @@ void RealsenseCamera::start() {
} }
// 使用相同的同步策略启动 // 使用相同的同步策略启动
profile_ = pipe_.start(rs_cfg_); profile_ = pipe_.start(rs_cfg_);
auto frames = get_frameset(true);
rs2::frame color_frame = frames.get_color_frame();
auto profile = color_frame.get_profile(); if (stream_mode_ == RGBD_MODE){
auto color_profile = frames.get_color_frame().get_profile(); auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
intrinsics_ = color_profile.as<rs2::video_stream_profile>().get_intrinsics(); auto sp_color = profile_.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
Kd_ = sp_depth.get_intrinsics();
Kc_ = sp_color.get_intrinsics();
Ec2d_ = sp_color.get_extrinsics_to(sp_depth);
Ed2c_ = sp_depth.get_extrinsics_to(sp_color);
intrinsics_ = Kc_;
}
else if (stream_mode_ == COLOR_MODE) {
auto sp_color = profile_.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
Kc_ = sp_color.get_intrinsics();
intrinsics_ = Kc_;
}
else if (stream_mode_ == DEPTH_MODE) {
auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
Kd_ = sp_depth.get_intrinsics();
intrinsics_ = Kd_;
}
if (align_mode_ == "color") {
align_ = std::make_shared<rs2::align>(RS2_STREAM_COLOR);
}
else if (align_mode_ == "depth") {
align_ = std::make_shared<rs2::align>(RS2_STREAM_DEPTH);
}
// auto frames = get_frameset(true);
// rs2::frame color_frame = frames.get_color_frame();
// auto profile = color_frame.get_profile();
// auto color_profile = frames.get_color_frame().get_profile();
// intrinsics_ = color_profile.as<rs2::video_stream_profile>().get_intrinsics();
state_.is_opened = true; state_.is_opened = true;
LOG(INFO) << "realsense start streaming successfully"; LOG(INFO) << "realsense start streaming successfully";
@ -333,10 +359,6 @@ void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsic
throw std::runtime_error("[RealsenseCamera] (getRGBDImages): Failed to get frames: " + std::string(e.what())); throw std::runtime_error("[RealsenseCamera] (getRGBDImages): Failed to get frames: " + std::string(e.what()));
} }
// 对齐深度到彩色坐标系
rs2::align align_to(RS2_STREAM_COLOR);
frames = align_to.process(frames);
rs2::video_frame color_frame = frames.get_color_frame(); rs2::video_frame color_frame = frames.get_color_frame();
// rs2::frame color_frame = frames.get_color_frame(); // rs2::frame color_frame = frames.get_color_frame();
rs2::depth_frame depth_frame = frames.get_depth_frame(); rs2::depth_frame depth_frame = frames.get_depth_frame();
@ -1039,11 +1061,18 @@ void RealsenseCamera::stopStreaming()
Eigen::Vector3f RealsenseCamera::get3DPointFromPixel(int u, int v) { Eigen::Vector3f RealsenseCamera::get3DPointFromPixel(int u, int v) {
std::lock_guard<std::mutex> lock(frame_mtx_); std::lock_guard<std::mutex> lock(frame_mtx_);
if (latest_depth_.empty()) return {0,0,0}; if (latest_depth_.empty()) {
LOG(WARNING) << " depth data empty";
return {0,0,0};
}
// 获取深度值，单位米 // 获取深度值，pixel
float depth_m = latest_depth_.at<uint16_t>(v, u) * 0.001f; float depth_m = latest_depth_.at<uint16_t>(v, u) * 0.001f;
if (depth_m <= 0) return {0,0,0}; if (depth_m <= 0) {
LOG(WARNING) << " depth data is < zero";
return {0,0,0};
}
float pixel[2] = { static_cast<float>(u), static_cast<float>(v) }; float pixel[2] = { static_cast<float>(u), static_cast<float>(v) };
float point[3]; float point[3];

6
src/devices/camera/realsense_camera/realsense_camera.h
View File

@ -106,6 +106,12 @@ namespace cmvr::device{
cv::Mat latest_depth_; cv::Mat latest_depth_;
std::mutex depth_mtx_; std::mutex depth_mtx_;
rs2_intrinsics Kd_; // 深度相机内参
rs2_intrinsics Kc_; // RGB相机内参
rs2_extrinsics Ec2d_; // rgb -> 深度 的外参
rs2_extrinsics Ed2c_; // rgb <- 深度 的外参
}; };
} }

37
src/devices/camera/realsense_camera/realsense_camera_test.cpp Normal file
View File

@ -0,0 +1,37 @@
//
// Created by lgv on 2025/9/1.
//
#include <gtest/gtest.h>
#include "devices/abstract_camera.h"
#include <opencv2/opencv.hpp>
#include "device_manager/device_manager.h"
using namespace cmvr::device;
// 测试: 实时显示 RGB + Depth
TEST(RealsenseCameraRealDeviceTest, SaveFrames) {
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;
}
auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
auto cam = dmgr.getDevice<AbstractCamera>("cam4");
cam->start();
cv::Mat color, depth;
Rs2Intrinsics intrinsics;
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
cam->getRGBDImages(color, depth, intrinsics);
auto pose = cam->get3DPointFromPixel(700,377);
LOG(INFO) << "Pose : " << pose ;
cam->stop();
}

1
src/devices/motor/motor_protocol_interface.h
View File

@ -42,6 +42,7 @@ namespace cmvr {
virtual void torqueOff(uint8_t node_id) = 0; virtual void torqueOff(uint8_t node_id) = 0;
virtual double getQ(uint8_t node_id) = 0; virtual double getQ(uint8_t node_id) = 0;
virtual double getQd(uint8_t node_id) = 0;
CommProto comm_proto{CommProto::CANOPEN}; CommProto comm_proto{CommProto::CANOPEN};
protected: protected:
}; };

7
src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp
View File

@ -465,5 +465,12 @@ double Ti5MotorCanopenProtocol::getQ(uint8_t node_id) {
return (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG); return (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG);
} }
double Ti5MotorCanopenProtocol::getQd(uint8_t node_id) {
auto data_ptr = std::make_unique<msgs::RobotDetail>();
message_manager_->GetSensorData(data_ptr.get());
auto cnt = data_ptr->motors().at(node_id).speed();
return (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG);
}

1
src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h
View File

@ -39,6 +39,7 @@ namespace cmvr {
bool reachedTargetQ(uint8_t node_id) override; bool reachedTargetQ(uint8_t node_id) override;
double getQ(uint8_t node_id) override; double getQ(uint8_t node_id) override;
double getQd(uint8_t node_id) override;
void setQd(uint8_t node_id, double qd) override; void setQd(uint8_t node_id, double qd) override;
void setQdd(uint8_t node_id, double qdd) override; void setQdd(uint8_t node_id, double qdd) override;

98
src/devices/robot/humanoid_robot/humanoid_robot.cpp
View File

@ -196,6 +196,9 @@ void HumanoidRobot<DOF>::init() {
right_task.get(); right_task.get();
waist_task.get(); waist_task.get();
motor_manager_->getMotor("WAIST_Y")->setQ(0);
motor_manager_->getMotor("WAIST_P")->setQ(0);
rsm_.store(ROBOT_ESTOP); rsm_.store(ROBOT_ESTOP);
LOG(INFO) << "All motors initialized successfully."; LOG(INFO) << "All motors initialized successfully.";
} }
@ -226,13 +229,13 @@ HumanoidRobot<DOF>::~HumanoidRobot() {
upd_timer_->stop(); upd_timer_->stop();
std::vector<JointPoint> cmd = { std::vector<JointPoint> cmd = {
{"L_SHOULDER_P", 0.0}, // {"L_SHOULDER_P", 0.0},
{"L_SHOULDER_R", -1.31873}, // {"L_SHOULDER_R", -1.31873},
{"L_SHOULDER_Y", 0.0}, // {"L_SHOULDER_Y", 0.0},
{"L_ELBOW_R", -0.537621}, // {"L_ELBOW_R", -0.537621},
{"L_WRIST_P", 0.0}, // {"L_WRIST_P", 0.0},
{"L_WRIST_Y", 0.000183204}, // {"L_WRIST_Y", 0.000183204},
{"L_WRIST_R", 0.0225797}, // {"L_WRIST_R", 0.0225797},
{"R_SHOULDER_P", -0.0201069}, {"R_SHOULDER_P", -0.0201069},
{"R_SHOULDER_R", 1.46698}, {"R_SHOULDER_R", 1.46698},
@ -290,11 +293,31 @@ std::vector<std::string> HumanoidRobot<DOF>::getLinkNames() {
return link_names_; return link_names_;
} }
template<int DOF>
void HumanoidRobot<DOF>::getJointsState(std::vector<JointState>& states) {
try {
lock_guard lock(exec_mtx_);
states.clear();
JointState state;
for (const auto &pair : motor_manager_->motorsMap()) {
auto motor = pair.second;
state.name = motor->jointName();
state.position = motor->getQ();
state.velocity = motor->getQd();
states.push_back(state);
}
} catch (exception &e) {
throw runtime_error(e.what());
}
}
template<int DOF> template<int DOF>
void HumanoidRobot<DOF>::getState(RobotState &state) { void HumanoidRobot<DOF>::getState(RobotState &state) {
try { try {
lock_guard lock(exec_mtx_); lock_guard lock(exec_mtx_);
// TODO: copy m_state_ date into state // TODO: copy m_state_ date into state
} catch (exception &e) { } catch (exception &e) {
throw runtime_error(e.what()); throw runtime_error(e.what());
} }
@ -305,6 +328,17 @@ void HumanoidRobot<DOF>::torqueOn() {
eStop(); eStop();
} }
template<int DOF>
void HumanoidRobot<DOF>::torqueOn(const std::string &joint_name) {
auto motor = motor_manager_->getMotor(joint_name);
motor->brake();
}
template<int DOF>
void HumanoidRobot<DOF>::torqueOff(const std::string &joint_name) {
auto motor = motor_manager_->getMotor(joint_name);
motor->torqueOff();
}
template<int DOF> template<int DOF>
void HumanoidRobot<DOF>::eStop() { void HumanoidRobot<DOF>::eStop() {
@ -398,7 +432,7 @@ void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &
q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"],
q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"];
LOG(INFO) << "q_init: " << q_init; // LOG(INFO) << "q_init: " << q_init;
m_state_->SetQ(q_init); m_state_->SetQ(q_init);
m_robot_->ComputeForwardKinematics(m_state_); m_robot_->ComputeForwardKinematics(m_state_);
@ -432,10 +466,11 @@ void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &
auto motor = motor_manager_->getMotor(j.joint_name); auto motor = motor_manager_->getMotor(j.joint_name);
if (motor != nullptr) { if (motor != nullptr) {
// PPM 模式下 这个实际速度会超30% 左右 // PPM 模式下 这个实际速度会超30% 左右
motor->setQd(vel);
if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
} }
motor->setQd(vel);
motor->setQ(j.rad); motor->setQ(j.rad);
} }
} }
@ -467,7 +502,6 @@ void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &
} catch (exception &e) { } catch (exception &e) {
throw runtime_error(e.what()); throw runtime_error(e.what());
} }
} }
@ -884,6 +918,50 @@ Eigen::Vector3d HumanoidRobot<DOF>::rotationMatrixToEulerZYX(const Eigen::Matrix
return Eigen::Vector3d(rx, ry, rz); return Eigen::Vector3d(rx, ry, rz);
} }
template<int DOF>
std::vector<double> HumanoidRobot<DOF>::ik(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose) {
// update m_state_
try {
Eigen::Vector<double, DOF> q_init;
auto q_map = getJointQ();
q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"],
q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"],
q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"],
q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"];
LOG(INFO) << "q_init: " << q_init;
m_state_->SetQ(q_init);
m_robot_->ComputeForwardKinematics(m_state_);
Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity();
T_target.block<3,3>(0,0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz()); // 输入为弧度
T_target(0,3) = pose.position().x();
T_target(1,3) = pose.position().y();
T_target(2,3) = pose.position().z();
cmvr::ctrl::PoseTarget target;
target.T_target = T_target;
target.w_posrot = 0.5;
target.weight = 1.0;
target.link_name = ee_link;
// slove ik
Eigen::Vector<double, DOF> q_cmd{};
bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, ctrl::CartesianController<DOF>::Mode::Position,
q_cmd, 10000, 1e-6);
if (!ok) {
throw std::runtime_error("IK solve failed");
} else {
return std::vector<double>(q_cmd.data(), q_cmd.data() + q_cmd.size());
}
}catch (std::exception &e) {
throw runtime_error(e.what());
}
}
template<int DOF> template<int DOF>

13
src/devices/robot/humanoid_robot/humanoid_robot.h
View File

@ -18,6 +18,7 @@
#include "cmvr/msgs/robot_detail.pb.h" #include "cmvr/msgs/robot_detail.pb.h"
#include "motor/motor_manager.h" #include "motor/motor_manager.h"
namespace cmvr::device{ namespace cmvr::device{
template<int DOF> template<int DOF>
@ -49,12 +50,15 @@ namespace cmvr::device{
std::unordered_map<std::string,double> getJointQ() const override; std::unordered_map<std::string,double> getJointQ() const override;
void getJointQ(std::unordered_map<std::string,double> &joint_qs) const override; void getJointQ(std::unordered_map<std::string,double> &joint_qs) const override;
void getJointsState(std::vector<JointState>& states) override;
void getState(RobotState &state) override; void getState(RobotState &state) override;
math::Pose3d getTransform(std::string &bask_link, std::string &target_link) {throw std::runtime_error("Not implemented");} math::Pose3d getTransform(std::string &bask_link, std::string &target_link) {throw std::runtime_error("Not implemented");}
/* torque on and off */ /* torque on and off */
void torqueOn() override; void torqueOn() override;
void torqueOn(const std::string &joint_name) override;
void torqueOff() override; void torqueOff() override;
void torqueOff(const std::string &joint_name) override;
void calibrateZeroQ(const std::string &joint_name) override; void calibrateZeroQ(const std::string &joint_name) override;
/* robot basic command*/ /* robot basic command*/
@ -89,7 +93,8 @@ namespace cmvr::device{
// 将欧拉角(rx, ry, rz)转为旋转矩阵，旋转顺序 Y→Y→Z // 将欧拉角(rx, ry, rz)转为旋转矩阵，旋转顺序 Y→Y→Z
static Eigen::Matrix3d eulerZYXToRotationMatrix(double rx, double ry, double rz); static Eigen::Matrix3d eulerZYXToRotationMatrix(double rx, double ry, double rz);
static Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d &R); static Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d &R);
msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link); msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) override;
std::vector<double> ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) override;
private: private:
std::string id_; std::string id_;
@ -114,9 +119,9 @@ namespace cmvr::device{
std::shared_ptr<SPMCRingBuffer<JointCurrentCommand>> CSC_buffer_; std::shared_ptr<SPMCRingBuffer<JointCurrentCommand>> CSC_buffer_;
private: private:
std::vector<XmlNode> l_motors_cfg_; std::vector<XmlNode> l_motors_cfg_{};
std::vector<XmlNode> r_motors_cfg_; std::vector<XmlNode> r_motors_cfg_{};
std::vector<XmlNode> waist_motors_cfg_; std::vector<XmlNode> waist_motors_cfg_{};
std::shared_ptr<AbstractCanbus> l_can_client_{nullptr}; std::shared_ptr<AbstractCanbus> l_can_client_{nullptr};

149
src/devices/robot/humanoid_robot/humanoid_robot_test.cpp
View File

@ -5,18 +5,52 @@
// #include "humanoid_robot.h" // #include "humanoid_robot.h"
#include <csignal>
#include <atomic>
#include <thread>
#include <chrono>
#include <iostream>
#include "gtest/gtest.h" #include "gtest/gtest.h"
#include <glog/logging.h> #include <glog/logging.h>
#include "device_manager/device_manager.h" #include "device_manager/device_manager.h"
#include <libgen.h> #include <libgen.h>
#include "cmvr/msgs/can_card_parameter.grpc.pb.h" #include "cmvr/msgs/can_card_parameter.grpc.pb.h"
#include "cmvr/msgs/geometry.pb.h"
#include "robot/humanoid_robot/humanoid_robot.h" #include "robot/humanoid_robot/humanoid_robot.h"
// 定义一个命令行参数 --config_path // 定义一个命令行参数 --config_path
DEFINE_string(config_path, "../config/cabin_robot.xml", "Path to the robot config XML file"); DEFINE_string(config_path, "../config/cabin_robot.xml", "Path to the robot config XML file");
using namespace cmvr::device; using namespace cmvr::device;
TEST(HumanoidRobotTest,GetState) {
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";
}
auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
robot->torqueOff();
std::vector<cmvr::device::JointState> joints;
while (true) {
joints.clear();
robot->getJointsState(joints);
for (const auto &js : joints) {
LOG(INFO) << "Joint: " << js.name
<< ", Position: " << js.position
<< ", Velocity: " << js.velocity
<< std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}
}
TEST(HumanoidRobotTest,MyRobotTest) { TEST(HumanoidRobotTest,MyRobotTest) {
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"; std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
@ -53,10 +87,10 @@ TEST(HumanoidRobotTest,MyRobotTest) {
// {"R_ELBOW_R", 0.0}, // {"R_ELBOW_R", 0.0},
// {"R_WRIST_P", 0.0}, // {"R_WRIST_P", 0.0},
// {"R_WRIST_Y", 0.0}, // {"R_WRIST_Y", 0.0},
// {"R_WRIST_R", 0.0}, {"R_WRIST_R", 0.0},
{"WAIST_P" ,0.0}, // {"WAIST_P" ,0.0},
{"WAIST_Y" ,0.0}, // {"WAIST_Y" ,0.0},
}; };
robot->moveJ(cmd,0.8); robot->moveJ(cmd,0.8);
@ -345,89 +379,96 @@ TEST(HumanoidRobotTest,MoveDeltaTest) {
cmvr::msgs::Pose3d pose; cmvr::msgs::Pose3d pose;
pose.mutable_position()->set_x( 0); pose.mutable_position()->set_x( 0);
pose.mutable_position()->set_y(0); pose.mutable_position()->set_y(0);
pose.mutable_position()->set_z(0.01); pose.mutable_position()->set_z(0);
pose.mutable_euler()->set_rx(0); pose.mutable_euler()->set_rx(0);
pose.mutable_euler()->set_ry(0); pose.mutable_euler()->set_ry(0);
pose.mutable_euler()->set_rz(0); pose.mutable_euler()->set_rz(-0.7854);
try { try {
robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.1); // robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.1);
// robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.01); // robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.01);
// robot->moveDeltaJ("PELVIS_S","R_FINGER_TIP",pose); robot->moveDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.05);
}catch (std::exception &e) { }catch (std::exception &e) {
LOG(INFO) << e.what();
robot->torqueOff(); robot->torqueOff();
LOG(INFO) << e.what(); LOG(INFO) << e.what();
} }
} }
TEST(HumanoidRobotTest,AngleToTest) { TEST(HumanoidRobotTest,AngleToTest) {
double rx = M_PI / 6; // 30° double rx = 0.9497;
double ry = M_PI / 4; // 45° double ry = 1.6082;
double rz = M_PI / 3; // 60° double rz = -2.4832;
Eigen::Matrix3d R = HumanoidRobot<7>::eulerZYXToRotationMatrix(rx, ry, rz); Eigen::Matrix3d R = HumanoidRobot<7>::eulerZYXToRotationMatrix(rx, ry, rz);
Eigen::Vector3d euler = HumanoidRobot<7>::rotationMatrixToEulerZYX(R); Eigen::Vector3d euler = HumanoidRobot<7>::rotationMatrixToEulerZYX(R);
std::cout << "Original Euler angles (rad):\n" << Eigen::Vector3d(rx, ry, rz).transpose() << "\n"; std::cout << "Original Euler angles (rad):\n" << Eigen::Vector3d(rx, ry, rz).transpose() << "\n";
std::cout << "Recovered Euler angles (rad):\n" << euler.transpose() << "\n"; std::cout << "Recovered Euler angles (rad):\n" << euler.transpose() << "\n";
} }
std::atomic<bool> running{true};
// 信号处理函数
void signalHandler(int signum) {
std::cout << "\nInterrupt signal (" << signum << ") received. Stopping..." << std::endl;
running = false;
}
TEST(HumanoidRobotTest, MoveIKTest) { TEST(HumanoidRobotTest, MoveIKTest) {
// 注册信号处理
std::signal(SIGINT, signalHandler);
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"; std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path); const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")) { if (!config.hasChild("DeviceManager")) {
LOG(ERROR) << "Device Manager node not found"; LOG(ERROR) << "Device Manager node not found";
return;
} }
auto dmgr_cfg = config.getChild("DeviceManager"); auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(dmgr_cfg); auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01"); auto robot = dmgr.getDevice<AbstractRobot>("hc01");
auto cur_pose = robot->fk("PELVIS_S","R_FINGER_TIP");
std::vector<JointPoint> cmd{ cmvr::msgs::Pose3d pose;
{"R_SHOULDER_P", -0.08882}, pose.mutable_position()->set_x(0.591101);
{"R_SHOULDER_R", 1.50297}, pose.mutable_position()->set_y(-0.159451);
{"R_SHOULDER_Y", 1.85922}, pose.mutable_position()->set_z(-0.206554);
{"R_ELBOW_R", 0.214816}, pose.mutable_euler()->set_rx(0.070589);
{"R_WRIST_P", -1.69504}, pose.mutable_euler()->set_ry(0.117212);
{"R_WRIST_Y", 0.0363855}, pose.mutable_euler()->set_rz(0.0916384);
{"R_WRIST_R", 0.0519807}
};
robot->moveJ(cmd);
// //
// cmvr::ctrl::PoseTarget target;
// std::vector<cmvr::ctrl::PoseTarget> targets;
//
//
// cmvr::msgs::Pose3d pose;
//
// pose.mutable_position()->set_x( 1.49969573e-01);
// pose.mutable_position()->set_y(-4.00100001e-01);
// pose.mutable_position()->set_z(-1.00102800e-01);
//
// pose.mutable_euler()->set_rx(0);
// pose.mutable_euler()->set_ry(0);
// pose.mutable_euler()->set_rz(1.57);
//
//
//
// while (true) {
// robot->moveJ("PELVIS_S","R_WRIST_R_S",pose);
//
//
//
// std::this_thread::sleep_for(std::chrono::milliseconds(2000));
//
//
// robot->moveJ(cmd);
// std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// }
// 打印关节状态线程
std::thread joint_state_thread([&]() {
while (running) {
auto joint_state = robot->getJointQ();
if (auto it = joint_state.find("WAIST_P"); it != joint_state.end()) {
LOG(INFO) << "WAIST_P POS = " << it->second;
} else {
LOG(WARNING) << "WAIST_P not found in joint_state";
}
std::this_thread::sleep_for(std::chrono::milliseconds(5));
}
});
// 动作控制线程
std::thread motion_thread([&]() {
while (running) {
robot->moveJ("PELVIS_S","R_FINGER_TIP",pose,0.5);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
robot->moveJ("PELVIS_S","R_FINGER_TIP",cur_pose,0.5);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}
});
// 等待线程结束
if (motion_thread.joinable()) motion_thread.join();
if (joint_state_thread.joinable()) joint_state_thread.join();
LOG(INFO) << "Test finished gracefully.";
} }

2
src/service/grpc_hlc_client_test.cpp
View File

@ -13,7 +13,7 @@ using namespace cmvr::api;
TEST(GrpcHlcClientTest, MyTest) { TEST(GrpcHlcClientTest, MyTest) {
// 连接服务端 // 连接服务端
auto channel = grpc::CreateChannel("0.0.0.0:50055", grpc::InsecureChannelCredentials()); auto channel = grpc::CreateChannel("0.0.0.0:50052", grpc::InsecureChannelCredentials());
auto stub = cmvr::api::HlcService::NewStub(channel); auto stub = cmvr::api::HlcService::NewStub(channel);

6
src/service/grpc_hlc_service.cpp
View File

@ -21,8 +21,10 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
grpc::Status ret = grpc::Status::OK; grpc::Status ret = grpc::Status::OK;
try { try {
auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id()); std::shared_ptr<AbstractRobot> robot = nullptr;
ctrl::TouchController touch_controller; auto cam = dmgr_.getDevice<AbstractCamera>("cam4");
auto hand = dmgr_.getDevice<AbstractDexHand>("hand1");
ctrl::TouchController touch_controller(robot,hand,cam);;
touch_controller.touch(request->u(),request->v(),request->max_force()); touch_controller.touch(request->u(),request->v(),request->max_force());
response->mutable_header()->set_success(true); response->mutable_header()->set_success(true);
response->mutable_header()->set_error_message(""); response->mutable_header()->set_error_message("");

38
src/service/grpc_humanoid_robot_client_test.cpp
View File

@ -45,7 +45,7 @@ TEST(GrpcHumanoidRobotClientTest, MyMoveJTest) {
// //
std::vector<cmvr::device::JointPositionCmd> cmds{}; std::vector<cmvr::device::JointPoint> cmds{};
cmds = { cmds = {
@ -100,3 +100,39 @@ TEST(GrpcHumanoidRobotClientTest, MyMoveJTest) {
LOG(ERROR) << "RPC failed: " << status.error_message() << std::endl; LOG(ERROR) << "RPC failed: " << status.error_message() << std::endl;
} }
} }
TEST(GrpcHumanoidRobotClientTest, GetJointStateTest) {
// 连接服务端
auto channel = grpc::CreateChannel("0.0.0.0:50052", grpc::InsecureChannelCredentials());
auto stub = cmvr::api::HumanoidRobotService::NewStub(channel);
// 构造请求
cmvr::api::JointRequest request;
request.mutable_header()->set_device_id("hc01");
*request.mutable_header()->mutable_timestamp() = google::protobuf::util::TimeUtil::GetCurrentTime();
cmvr::api::JointResponse response;
grpc::ClientContext context;
// 调用 getJointState
grpc::Status status = stub->getJointState(&context, request, &response);
if (status.ok()) {
LOG(INFO) << "getJointState RPC succeeded.";
LOG(INFO) << "Success: " << response.header().success();
LOG(INFO) << "Error message: " << response.header().error_message();
LOG(INFO) << "Timestamp: " << response.header().timestamp().seconds();
// 输出每个关节状态
for (const auto &js : response.state()) {
for (int i = 0; i < js.name_size(); ++i) {
LOG(INFO) << "Joint: " << js.name(i)
<< ", Position: " << js.position(i)
<< ", Velocity: " << js.velocity(i);
}
}
} else {
LOG(ERROR) << "RPC failed: " << status.error_message();
}
}

32
src/service/grpc_humanoid_robot_service.cpp
View File

@ -81,3 +81,35 @@ grpc::Status gRPCHumanoidRobotServiceImpl::moveJ(grpc::ServerContext *context,
} }
grpc::Status gRPCHumanoidRobotServiceImpl::getJointState(grpc::ServerContext *context,
const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) {
grpc::Status ret = grpc::Status::OK;
try {
auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
std::vector<cmvr::device::JointState> states{};
robot->getJointsState(states);
response->mutable_header()->set_success(true);
response->mutable_header()->set_error_message("");
for (const auto &js : states) {
if (js.name != "WAIST_P" && js.name != "WAIST_Y") {
auto joint_msg = response->add_state();
joint_msg->add_name(js.name);
joint_msg->add_position(js.position);
joint_msg->add_velocity(js.velocity);
}
}
}catch (const std::exception& e) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message(e.what());
ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
}
*response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
return ret;
}