feat:add touch api
This commit is contained in:
parent
4765a69c49
commit
8afee807f7
11
README.md
11
README.md
@ -99,11 +99,12 @@ sudo make install
|
||||
```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 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-utils=2.55.1-0~realsense.12473 -y
|
||||
sudo apt-get install librealsense2-dev=2.55.1-0~realsense.12473 -y
|
||||
sudo apt-get install librealsense2-dbg=2.55.1-0~realsense.12473 -y
|
||||
|
||||
sudo apt-get install librealsense2-dkms=1.3.24-0ubuntu1 -y
|
||||
sudo apt-get install \
|
||||
librealsense2=2.55.1-0~realsense.12473 \
|
||||
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)
|
||||
|
||||
@ -14,15 +14,15 @@
|
||||
<!-- <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"/>-->
|
||||
<!-- <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"/>-->
|
||||
<!-- <RealsenseCamera id="cam6" serial="243122075389" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
|
||||
</Camera>
|
||||
|
||||
<DexHand>
|
||||
<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" />
|
||||
</RH56DFTP>
|
||||
<!-- <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" />-->
|
||||
<!-- </RH56DFTP>-->
|
||||
<!-- <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" />-->
|
||||
<!-- </RH56DFTP>-->
|
||||
@ -32,39 +32,39 @@
|
||||
<!-- <LeftArm id="left_arm" devtype="ti5Robot" />-->
|
||||
<!-- <RightArm />-->
|
||||
<!-- <Neck/>-->
|
||||
<Humanoid id="hc01" dof="14"
|
||||
urdf="/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"
|
||||
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"
|
||||
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"
|
||||
verbose="false">
|
||||
<CanManger id="" devId="">
|
||||
<LeftArmCan id = " " devId = " " channelId ="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="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="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="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
|
||||
</LeftArmCan>
|
||||
<RightArmCan id = " " devId = " " channelId ="1">
|
||||
<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="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="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="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
|
||||
</RightArmCan>
|
||||
<WaistCan id = " " devId = " " channelId ="2">
|
||||
<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"/>
|
||||
</WaistCan>
|
||||
</CanManger>
|
||||
<!-- <Humanoid id="hc01" dof="14"-->
|
||||
<!-- urdf="/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"-->
|
||||
<!-- 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"-->
|
||||
<!-- 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"-->
|
||||
<!-- verbose="false">-->
|
||||
<!-- <CanManger id="" devId="">-->
|
||||
<!-- <LeftArmCan id = " " devId = " " channelId ="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="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="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="1" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
|
||||
<!-- </LeftArmCan>-->
|
||||
<!-- <RightArmCan id = " " devId = " " channelId ="1">-->
|
||||
<!-- <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="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="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="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>-->
|
||||
<!-- </RightArmCan>-->
|
||||
<!-- <WaistCan id = " " devId = " " channelId ="2">-->
|
||||
<!-- <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"/>-->
|
||||
<!-- </WaistCan>-->
|
||||
<!-- </CanManger>-->
|
||||
|
||||
</Humanoid>
|
||||
<!-- </Humanoid>-->
|
||||
</Robot>
|
||||
|
||||
<BioHead>
|
||||
@ -85,7 +85,7 @@
|
||||
|
||||
<Microphone>
|
||||
<!-- <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>
|
||||
|
||||
<Speaker>
|
||||
@ -132,7 +132,7 @@
|
||||
|
||||
</MonitorManager>
|
||||
|
||||
<gRPCServer port="50055">
|
||||
<gRPCServer port="50054">
|
||||
|
||||
</gRPCServer>
|
||||
|
||||
|
||||
@ -574,7 +574,7 @@
|
||||
</link>
|
||||
|
||||
<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"/>
|
||||
<child link="R_FINGER_TIP"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
|
||||
@ -8,9 +8,11 @@
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <pybind11/pybind11.h>
|
||||
#include <pybind11/stl.h>
|
||||
#include <mutex>
|
||||
|
||||
#include "devices/abstract_robot.h"
|
||||
#include "device_manager/device_manager.h"
|
||||
#include "cmvr/msgs/geometry.pb.h"
|
||||
|
||||
namespace py = pybind11;
|
||||
using namespace cmvr::device;
|
||||
@ -19,7 +21,7 @@ class PyRobotWrapper {
|
||||
public:
|
||||
// 构造时只需要传入 config_path 和 robot_name（只在第一次初始化有效）
|
||||
PyRobotWrapper(const std::string& config_path, const std::string& robot_name) {
|
||||
// 如果已经初始化过，则直接返回
|
||||
std::lock_guard<std::mutex> lock(init_mutex); // 线程安全
|
||||
if (!robot_) {
|
||||
const XmlNode config(config_path);
|
||||
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)
|
||||
throw std::runtime_error("Expected 7 joint values");
|
||||
|
||||
std::vector<std::string> joint_names;
|
||||
if (side == "left") {
|
||||
joint_names = {"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
|
||||
"L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R"};
|
||||
} else if (side == "right") {
|
||||
joint_names = {"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
|
||||
"R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
|
||||
} else {
|
||||
throw std::runtime_error("Side must be 'left' or 'right'");
|
||||
}
|
||||
const auto& joint_names = getJointNames(side);
|
||||
|
||||
std::vector<JointPoint> cmd;
|
||||
cmd.push_back({"WAIST_Y", 0});
|
||||
cmd.push_back({"WAIST_P", 0});
|
||||
std::vector<JointPoint> cmd = {{"WAIST_Y", 0}, {"WAIST_P", 0}};
|
||||
for (size_t i = 0; i < 7; ++i)
|
||||
cmd.push_back({joint_names[i], q[i]});
|
||||
|
||||
robot_->moveJ(cmd, 0.8);
|
||||
robot_->moveJ(cmd, speed);
|
||||
}
|
||||
|
||||
|
||||
void torqueOn() {
|
||||
if (!robot_)
|
||||
throw std::runtime_error("Robot not initialized");
|
||||
checkRobotInit();
|
||||
robot_->torqueOn();
|
||||
}
|
||||
|
||||
void torqueOn(const std::string& joint_name) {
|
||||
checkRobotInit();
|
||||
robot_->torqueOn(joint_name);
|
||||
}
|
||||
|
||||
void torqueOff() {
|
||||
if (!robot_)
|
||||
throw std::runtime_error("Robot not initialized");
|
||||
checkRobotInit();
|
||||
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 {
|
||||
if (!robot_)
|
||||
throw std::runtime_error("Robot not initialized");
|
||||
checkRobotInit();
|
||||
|
||||
auto joint_qs = robot_->getJointQ(); // unordered_map<std::string,double>
|
||||
|
||||
std::vector<std::string> joint_names;
|
||||
if (side == "left") {
|
||||
joint_names = {"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
|
||||
"L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R"};
|
||||
} else if (side == "right") {
|
||||
joint_names = {"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
|
||||
"R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
|
||||
} else {
|
||||
throw std::runtime_error("Side must be 'left' or 'right'");
|
||||
}
|
||||
const auto& joint_names = getJointNames(side);
|
||||
|
||||
std::vector<double> values;
|
||||
for (const auto& name : joint_names) {
|
||||
values.push_back(joint_qs[name]);
|
||||
values.push_back(joint_qs.at(name));
|
||||
}
|
||||
|
||||
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:
|
||||
static DeviceManager* dmgr_;
|
||||
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;
|
||||
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) {
|
||||
py::class_<PyRobotWrapper>(m, "Robot")
|
||||
.def(py::init<const std::string&, const std::string&>())
|
||||
.def("moveJ", &PyRobotWrapper::moveJ, py::arg("side"), py::arg("q"))
|
||||
.def("torqueOn", &PyRobotWrapper::torqueOn)
|
||||
.def("torqueOff", &PyRobotWrapper::torqueOff)
|
||||
.def("getJointQ", &PyRobotWrapper::getJointQ, py::arg("side"));
|
||||
.def(py::init<const std::string&, const std::string&>(),
|
||||
py::arg("config_path"), py::arg("robot_name"),
|
||||
"Initialize the robot wrapper with config and robot name")
|
||||
.def("moveJ", &PyRobotWrapper::moveJ, py::arg("side"), py::arg("q"), py::arg("speed")=0.8,
|
||||
"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");
|
||||
}
|
||||
|
||||
@ -11,7 +11,7 @@ constexpr int DOF = 14;
|
||||
using RobotT = cmvr::dyn::Robot<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) {
|
||||
if (argc < 10) {
|
||||
|
||||
@ -87,7 +87,7 @@ int main(int argc, char **argv)
|
||||
|
||||
// 1. 加载机器人模型
|
||||
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");
|
||||
auto robot = std::make_shared<RobotT>(rcfg);
|
||||
|
||||
@ -96,7 +96,7 @@ int main(int argc, char **argv)
|
||||
// 初始关节角度
|
||||
Eigen::Vector<double, DOF> q_init;
|
||||
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);
|
||||
robot->ComputeForwardKinematics(state);
|
||||
|
||||
|
||||
@ -6,6 +6,7 @@
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include "devices/abstract_camera.h"
|
||||
#include "devices/abstract_dexhand.h"
|
||||
#include "devices/abstract_robot.h"
|
||||
#include "cmvr/msgs/geometry.pb.h"
|
||||
@ -79,16 +80,16 @@ namespace cmvr {
|
||||
|
||||
class TouchController {
|
||||
public:
|
||||
TouchController(){};
|
||||
TouchController( std::shared_ptr<device::AbstractRobot> robot,std::shared_ptr<device::AbstractDexHand> hand)
|
||||
:robot_(std::move(robot)),hand_(std::move(hand)),
|
||||
TouchController() {};
|
||||
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)),cam_(std::move(cam)),
|
||||
pid_(std::make_shared<PID>(0.005, 0.001, 0.001, 5000.0, 0.5, 1.0)){}
|
||||
~TouchController()=default;
|
||||
|
||||
|
||||
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( msgs::Pose3d pose, msgs::Pose3d offset,double max_force);
|
||||
@ -96,6 +97,7 @@ namespace cmvr {
|
||||
private:
|
||||
std::shared_ptr<device::AbstractRobot> robot_{nullptr};
|
||||
std::shared_ptr<device::AbstractDexHand> hand_{nullptr};
|
||||
std::shared_ptr<device::AbstractCamera> cam_{nullptr};
|
||||
|
||||
std::shared_ptr<PID> pid_{nullptr};
|
||||
|
||||
|
||||
@ -146,6 +146,13 @@ namespace cmvr::device{
|
||||
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) {
|
||||
|
||||
@ -10,6 +10,8 @@
|
||||
#include "abstract_device.h"
|
||||
#include "utils/controller/cartesian_controller.h"
|
||||
#include "cmvr/msgs/geometry.pb.h"
|
||||
#include "cmvr/msgs/motor.pb.h"
|
||||
|
||||
namespace cmvr::device{
|
||||
|
||||
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 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 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(const std::string &joint_name) { 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"); }
|
||||
|
||||
@ -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 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:
|
||||
int dof_{};
|
||||
RobotState state_{};
|
||||
|
||||
@ -50,6 +50,7 @@ namespace cmvr::device{
|
||||
} RobotCartesian;
|
||||
|
||||
typedef struct {
|
||||
std::string name;
|
||||
bool is_ready;
|
||||
bool power_on;
|
||||
double position;
|
||||
|
||||
@ -19,6 +19,8 @@ namespace cmvr {
|
||||
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 getJointState(grpc::ServerContext *context, const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) override;
|
||||
private:
|
||||
device::DeviceManager& dmgr_;
|
||||
};
|
||||
|
||||
@ -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{
|
||||
rpc torqueOff(CommandHeader.Request) returns (CommandHeader.Feedback);
|
||||
rpc torqueOn(CommandHeader.Request) returns (CommandHeader.Feedback);
|
||||
rpc moveJ(MoveJ.Request) returns (MoveJ.Response);
|
||||
rpc getJointState(JointRequest) returns (JointResponse);
|
||||
}
|
||||
@ -90,7 +90,7 @@ def callback_and_save(frame):
|
||||
js = robot.getJointQ('right')
|
||||
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("请按 p 失能机器人，并手动移动机器人到其他位姿")
|
||||
|
||||
@ -113,28 +113,9 @@ def callback_and_save(frame):
|
||||
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()
|
||||
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):
|
||||
def start_pipeline(serial=TARGET_SERIAL, depth_size=(1280, 720), color_size=(1280, 720), fps: int = 30):
|
||||
ctx = rs.context()
|
||||
devices = ctx.query_devices()
|
||||
if len(devices) == 0:
|
||||
|
||||
@ -40,6 +40,85 @@ YY = data.get("checkerboard_args").get("YY") #标定板的中宽度对应的角
|
||||
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():
|
||||
|
||||
|
||||
@ -113,14 +192,37 @@ def func():
|
||||
|
||||
for i in range(N):
|
||||
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]) # 提取旋转矩阵
|
||||
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__':
|
||||
|
||||
@ -137,3 +239,4 @@ if __name__ == '__main__':
|
||||
logger_.info(f"平移向量是:\n { translation_vector}")
|
||||
|
||||
logger_.info(f"四元数是：\n { quaternion}")
|
||||
|
||||
|
||||
@ -1,46 +1,256 @@
|
||||
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: 旋转矩阵是:
|
||||
2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
|
||||
2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
|
||||
2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
|
||||
2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
|
||||
2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
|
||||
2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
|
||||
2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
|
||||
2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
|
||||
2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
|
||||
2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
|
||||
2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
|
||||
2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
|
||||
2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
|
||||
2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
|
||||
2025-09-08 14:26:55 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
|
||||
2025-09-08 14:26:55 - __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: 平移向量是:
|
||||
2025-09-08 14:26:55 - __main__ - INFO: 平移向量是:
|
||||
[[-0.00797824]
|
||||
[-0.17640328]
|
||||
[ 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]
|
||||
|
||||
46
python/hand_eye_calibration/libs/web-log.2025-08-28.log
Normal file
46
python/hand_eye_calibration/libs/web-log.2025-08-28.log
Normal 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
167
python/hand_eye_calibration/libs/web-log.2025-09-05.log
Normal 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]
|
||||
Binary file not shown.
@ -1,20 +1,290 @@
|
||||
import os
|
||||
os.environ["GLOG_minloglevel"] = "1"
|
||||
|
||||
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 re
|
||||
import time
|
||||
import signal
|
||||
import cv2
|
||||
import subprocess
|
||||
import numpy as np
|
||||
import pyrealsense2 as rs
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
from utils import solve_ik
|
||||
from utils import get_pose
|
||||
from target_position import init_realsense, get_closest_red_point
|
||||
from get_chessboard_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):
|
||||
print("\n收到 Ctrl+C，准备退出...")
|
||||
raise SystemExit
|
||||
@ -35,96 +305,191 @@ def transform_point(T, P):
|
||||
return P_new[:3]
|
||||
|
||||
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)
|
||||
|
||||
# 得到相机在 base 下的位姿
|
||||
T_base_cam = T_base_ee @ T_ee_cam
|
||||
|
||||
# 把目标点从相机系变换到 base 系
|
||||
P_base_target = transform_point(T_base_cam, P_cam_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()
|
||||
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:
|
||||
# 初始化机器人
|
||||
robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
|
||||
|
||||
# 移动到初始关节位姿
|
||||
|
||||
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")
|
||||
if P_cam_target is None:
|
||||
raise RuntimeError("未检测到红点，无法计算目标点")
|
||||
print("红点在相机坐标系下:", P_cam_target)
|
||||
|
||||
# P_cam_target = np.array([0.0716422, 0.0482324, 0.325])
|
||||
|
||||
# "--frame-id",
|
||||
# "R_FINGER_TIP",
|
||||
# "--child-frame-id",
|
||||
# "camera_color_optical_frame",
|
||||
# "--x",
|
||||
# "-0.132714",
|
||||
# "--y",
|
||||
# "0.00161894",
|
||||
# "--z",
|
||||
# "0.0760576",
|
||||
# "--qx",
|
||||
# "0.512312",
|
||||
# "--qy",
|
||||
# "-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:
|
||||
raise RuntimeError("IK 求解失败")
|
||||
|
||||
# 移动到目标点
|
||||
robot.moveJ("right", rq)
|
||||
print("已到达目标点，按 Ctrl+C 回到初始位姿...")
|
||||
|
||||
while True:
|
||||
current_q = robot.getJointQ('right')
|
||||
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)
|
||||
P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png")
|
||||
if P_cam_target is None:
|
||||
print("can not find red point")
|
||||
time.sleep(2)
|
||||
continue
|
||||
time.sleep(1)
|
||||
|
||||
T_cam_target = np.eye(4)
|
||||
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":
|
||||
desire_rpy_base_tool = R.from_matrix(T_base_ee[:3, :3]).as_euler('xyz', degrees=True)
|
||||
desire_pose = list(desire_P_base_tool) + list(desire_rpy_base_tool)
|
||||
ok, _, rq = solve_ik(desire_pose, base_link="PELVIS_S", target_link="R_WRIST_R_S")
|
||||
elif EE_LINK == "CAM":
|
||||
desire_rpy_base_tool = R.from_matrix(T_base_cam[:3, :3]).as_euler('xyz', degrees=True)
|
||||
desire_pose = list(desire_P_base_tool) + list(desire_rpy_base_tool)
|
||||
ok, _, rq = solve_ik(desire_pose, base_link="PELVIS_S", target_link="R_CAM")
|
||||
elif EE_LINK == "FINGER":
|
||||
desire_rpy_base_tool = R.from_matrix(T_base_tool[:3, :3]).as_euler('xyz', degrees=True)
|
||||
desire_pose = list(desire_P_base_tool) + list(desire_rpy_base_tool)
|
||||
ok, _, rq = solve_ik(desire_pose, base_link="PELVIS_S", target_link="R_FINGER_TIP")
|
||||
else:
|
||||
raise RuntimeError("undefined EE_LINK: ", EE_LINK)
|
||||
|
||||
if not ok:
|
||||
print("solve ik failed, pose=", desire_pose)
|
||||
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)
|
||||
time.sleep(5)
|
||||
robot.moveJ("right", init_rq)
|
||||
|
||||
print("Return to init pose, Robot movej: ", init_rq)
|
||||
|
||||
except SystemExit:
|
||||
print("安全退出程序...")
|
||||
except Exception as e:
|
||||
print("捕获异常:", e)
|
||||
|
||||
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()
|
||||
align = None
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
# if __name__ == "__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])
|
||||
|
||||
@ -8,29 +8,38 @@
|
||||
import cv2
|
||||
import numpy as np
|
||||
import pyrealsense2 as rs
|
||||
import sys
|
||||
import time
|
||||
|
||||
# 把.so所在目录加入 Python 路径
|
||||
sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example")
|
||||
|
||||
# 导入机器人控制模块
|
||||
from robot_wrapper import Robot
|
||||
|
||||
|
||||
def init_realsense():
|
||||
"""
|
||||
初始化 RealSense 管道并返回 pipeline 和 align 对象
|
||||
"""
|
||||
"""初始化 RealSense 管道并返回 pipeline 和 align 对象"""
|
||||
pipeline = rs.pipeline()
|
||||
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)
|
||||
profile = pipeline.start(config)
|
||||
align = rs.align(rs.stream.color)
|
||||
return pipeline, align
|
||||
|
||||
def stop_pipeline(pipeline: rs.pipeline):
|
||||
pipeline.stop()
|
||||
|
||||
|
||||
def get_closest_red_point(pipeline, align, vis_path = None,warmup=10):
|
||||
def get_closest_red_point(pipeline, align, vis_path=None, warmup=30,
|
||||
hough_params=None):
|
||||
"""
|
||||
获取最近红色圆点的 3D 坐标。
|
||||
返回 (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):
|
||||
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())
|
||||
depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics
|
||||
|
||||
# 转换到 HSV 并提取红色区域
|
||||
hsv = cv2.cvtColor(color_image, cv2.COLOR_BGR2HSV)
|
||||
lower_red1 = np.array([0, 100, 100])
|
||||
upper_red1 = np.array([10, 255, 255])
|
||||
lower_red2 = np.array([160, 100, 100])
|
||||
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)
|
||||
lower_red1, upper_red1 = np.array([0, 100, 100]), np.array([10, 255, 255])
|
||||
lower_red2, upper_red2 = np.array([160, 100, 100]), np.array([179, 255, 255])
|
||||
mask = cv2.inRange(hsv, lower_red1, upper_red1) | cv2.inRange(hsv, lower_red2, upper_red2)
|
||||
|
||||
# 平滑处理再检测圆
|
||||
mask_blur = cv2.GaussianBlur(mask, (9, 9), 2)
|
||||
circles = cv2.HoughCircles(mask_blur, cv2.HOUGH_GRADIENT, dp=1.2, minDist=20,
|
||||
param1=50, param2=15, minRadius=5, maxRadius=50)
|
||||
circles = cv2.HoughCircles(mask_blur, cv2.HOUGH_GRADIENT, **hough_params)
|
||||
|
||||
closest_point = None
|
||||
min_depth = float('inf')
|
||||
closest_point, min_depth = None, float('inf')
|
||||
|
||||
if circles is not None:
|
||||
circles = np.uint16(np.around(circles))
|
||||
for i in circles[0, :]:
|
||||
u, v, r = i
|
||||
depth = depth_frame.get_distance(u, v)
|
||||
for u, v, r in circles[0, :]:
|
||||
depth = depth_frame.get_distance(int(u), int(v))
|
||||
if 0 < depth < min_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:
|
||||
cv2.circle(color_image, (u, v), r, (0, 255, 0), 2)
|
||||
cv2.circle(color_image, (u, v), 6, (0, 0, 255), -1)
|
||||
|
||||
if vis_path is not None and closest_point is not None:
|
||||
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)
|
||||
|
||||
return closest_point
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
# ========================= 4. 主程序 ========================= #
|
||||
def main(serial="243122075614", avg_window=3):
|
||||
def main(serial="243122075614", avg_window=3, loop=True):
|
||||
pipeline, align = init_realsense()
|
||||
# robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
|
||||
|
||||
try:
|
||||
# 调用获取最近红点
|
||||
point_3d = get_closest_red_point(pipeline, align, vis_path="red_point.png")
|
||||
print("Closest red 3D point:", point_3d)
|
||||
while True:
|
||||
point_3d = get_closest_red_point(pipeline, align, vis_path="red_point.png")
|
||||
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:
|
||||
pipeline.stop()
|
||||
cv2.destroyAllWindows()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
main()
|
||||
|
||||
Binary file not shown.
|
Before Width: | Height: | Size: 1.1 MiB After Width: | Height: | Size: 1.2 MiB |
@ -43,16 +43,27 @@ robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
|
||||
# while True:
|
||||
# for j in JOINT_POSITIONS:
|
||||
# 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.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)
|
||||
# robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364])
|
||||
# robot.torqueOff()
|
||||
|
||||
# robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
|
||||
# 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_R", 1.34062},
|
||||
# // {"R_SHOULDER_Y", 0.0},
|
||||
|
||||
0
python/vision_servo/test.py
Normal file
0
python/vision_servo/test.py
Normal file
@ -112,3 +112,48 @@ bool TouchController::isArrive(double 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);
|
||||
}
|
||||
}
|
||||
|
||||
@ -26,7 +26,7 @@ TEST(TouchControllerTest,MyTest) {
|
||||
|
||||
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
|
||||
auto hand = dmgr.getDevice<AbstractDexHand>("hand1");
|
||||
|
||||
auto cam = dmgr.getDevice<AbstractCamera>("cam4");
|
||||
|
||||
cmvr::msgs::Pose3d pose;
|
||||
|
||||
@ -52,14 +52,12 @@ TEST(TouchControllerTest,MyTest) {
|
||||
delta_pose.mutable_euler()->set_rz(0);
|
||||
|
||||
|
||||
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10000));
|
||||
robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.02);
|
||||
|
||||
Pose3d offset;
|
||||
|
||||
TouchController controller(robot, hand);
|
||||
TouchController controller(robot, hand,cam);
|
||||
|
||||
while (true) {
|
||||
// controller.touch(pose,offset,500);
|
||||
|
||||
@ -7,4 +7,43 @@ add_library(cmvr_es::device::realsense_camera ALIAS realsense_camera) # ✅ 命
|
||||
# 查找 librealsense2
|
||||
find_package(realsense2 REQUIRED)
|
||||
# 链接 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
|
||||
)
|
||||
|
||||
@ -137,12 +137,6 @@ void RealsenseCamera::init() {
|
||||
|
||||
rs_cfg_.enable_device(serial_);
|
||||
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_DEPTH, state_.width, state_.height, RS2_FORMAT_Z16, state_.fps);
|
||||
}
|
||||
@ -191,11 +185,43 @@ void RealsenseCamera::start() {
|
||||
}
|
||||
// 使用相同的同步策略启动
|
||||
profile_ = pipe_.start(rs_cfg_);
|
||||
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();
|
||||
|
||||
|
||||
if (stream_mode_ == RGBD_MODE){
|
||||
auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
|
||||
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;
|
||||
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()));
|
||||
}
|
||||
|
||||
// 对齐深度到彩色坐标系
|
||||
rs2::align align_to(RS2_STREAM_COLOR);
|
||||
frames = align_to.process(frames);
|
||||
|
||||
rs2::video_frame color_frame = frames.get_color_frame();
|
||||
// rs2::frame color_frame = frames.get_color_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) {
|
||||
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;
|
||||
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 point[3];
|
||||
|
||||
@ -106,6 +106,12 @@ namespace cmvr::device{
|
||||
|
||||
cv::Mat latest_depth_;
|
||||
std::mutex depth_mtx_;
|
||||
|
||||
rs2_intrinsics Kd_; // 深度相机内参
|
||||
rs2_intrinsics Kc_; // RGB相机内参
|
||||
rs2_extrinsics Ec2d_; // rgb -> 深度 的外参
|
||||
rs2_extrinsics Ed2c_; // rgb <- 深度 的外参
|
||||
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
@ -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();
|
||||
}
|
||||
@ -42,6 +42,7 @@ namespace cmvr {
|
||||
virtual void torqueOff(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};
|
||||
protected:
|
||||
};
|
||||
|
||||
@ -465,5 +465,12 @@ double Ti5MotorCanopenProtocol::getQ(uint8_t node_id) {
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@ -39,6 +39,7 @@ namespace cmvr {
|
||||
bool reachedTargetQ(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 setQdd(uint8_t node_id, double qdd) override;
|
||||
|
||||
@ -196,6 +196,9 @@ void HumanoidRobot<DOF>::init() {
|
||||
right_task.get();
|
||||
waist_task.get();
|
||||
|
||||
motor_manager_->getMotor("WAIST_Y")->setQ(0);
|
||||
motor_manager_->getMotor("WAIST_P")->setQ(0);
|
||||
|
||||
rsm_.store(ROBOT_ESTOP);
|
||||
LOG(INFO) << "All motors initialized successfully.";
|
||||
}
|
||||
@ -226,13 +229,13 @@ HumanoidRobot<DOF>::~HumanoidRobot() {
|
||||
upd_timer_->stop();
|
||||
|
||||
std::vector<JointPoint> cmd = {
|
||||
{"L_SHOULDER_P", 0.0},
|
||||
{"L_SHOULDER_R", -1.31873},
|
||||
{"L_SHOULDER_Y", 0.0},
|
||||
{"L_ELBOW_R", -0.537621},
|
||||
{"L_WRIST_P", 0.0},
|
||||
{"L_WRIST_Y", 0.000183204},
|
||||
{"L_WRIST_R", 0.0225797},
|
||||
// {"L_SHOULDER_P", 0.0},
|
||||
// {"L_SHOULDER_R", -1.31873},
|
||||
// {"L_SHOULDER_Y", 0.0},
|
||||
// {"L_ELBOW_R", -0.537621},
|
||||
// {"L_WRIST_P", 0.0},
|
||||
// {"L_WRIST_Y", 0.000183204},
|
||||
// {"L_WRIST_R", 0.0225797},
|
||||
|
||||
{"R_SHOULDER_P", -0.0201069},
|
||||
{"R_SHOULDER_R", 1.46698},
|
||||
@ -290,11 +293,31 @@ std::vector<std::string> HumanoidRobot<DOF>::getLinkNames() {
|
||||
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>
|
||||
void HumanoidRobot<DOF>::getState(RobotState &state) {
|
||||
try {
|
||||
lock_guard lock(exec_mtx_);
|
||||
// TODO: copy m_state_ date into state
|
||||
|
||||
} catch (exception &e) {
|
||||
throw runtime_error(e.what());
|
||||
}
|
||||
@ -305,6 +328,17 @@ void HumanoidRobot<DOF>::torqueOn() {
|
||||
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>
|
||||
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_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_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);
|
||||
if (motor != nullptr) {
|
||||
// PPM 模式下 这个实际速度会超30% 左右
|
||||
motor->setQd(vel);
|
||||
|
||||
if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
|
||||
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
|
||||
}
|
||||
motor->setQd(vel);
|
||||
motor->setQ(j.rad);
|
||||
}
|
||||
}
|
||||
@ -467,7 +502,6 @@ void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &
|
||||
} catch (exception &e) {
|
||||
throw runtime_error(e.what());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -884,6 +918,50 @@ Eigen::Vector3d HumanoidRobot<DOF>::rotationMatrixToEulerZYX(const Eigen::Matrix
|
||||
|
||||
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>
|
||||
|
||||
@ -18,6 +18,7 @@
|
||||
#include "cmvr/msgs/robot_detail.pb.h"
|
||||
#include "motor/motor_manager.h"
|
||||
|
||||
|
||||
namespace cmvr::device{
|
||||
|
||||
template<int DOF>
|
||||
@ -49,12 +50,15 @@ namespace cmvr::device{
|
||||
std::unordered_map<std::string,double> getJointQ() 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;
|
||||
math::Pose3d getTransform(std::string &bask_link, std::string &target_link) {throw std::runtime_error("Not implemented");}
|
||||
|
||||
/* torque on and off */
|
||||
void torqueOn() override;
|
||||
void torqueOn(const std::string &joint_name) override;
|
||||
void torqueOff() override;
|
||||
void torqueOff(const std::string &joint_name) override;
|
||||
void calibrateZeroQ(const std::string &joint_name) override;
|
||||
|
||||
/* robot basic command*/
|
||||
@ -89,7 +93,8 @@ namespace cmvr::device{
|
||||
// 将欧拉角(rx, ry, rz)转为旋转矩阵，旋转顺序 Y→Y→Z
|
||||
static Eigen::Matrix3d eulerZYXToRotationMatrix(double rx, double ry, double rz);
|
||||
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:
|
||||
std::string id_;
|
||||
@ -114,9 +119,9 @@ namespace cmvr::device{
|
||||
std::shared_ptr<SPMCRingBuffer<JointCurrentCommand>> CSC_buffer_;
|
||||
|
||||
private:
|
||||
std::vector<XmlNode> l_motors_cfg_;
|
||||
std::vector<XmlNode> r_motors_cfg_;
|
||||
std::vector<XmlNode> waist_motors_cfg_;
|
||||
std::vector<XmlNode> l_motors_cfg_{};
|
||||
std::vector<XmlNode> r_motors_cfg_{};
|
||||
std::vector<XmlNode> waist_motors_cfg_{};
|
||||
|
||||
|
||||
std::shared_ptr<AbstractCanbus> l_can_client_{nullptr};
|
||||
|
||||
@ -5,18 +5,52 @@
|
||||
|
||||
// #include "humanoid_robot.h"
|
||||
|
||||
#include <csignal>
|
||||
#include <atomic>
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include <glog/logging.h>
|
||||
#include "device_manager/device_manager.h"
|
||||
#include <libgen.h>
|
||||
|
||||
#include "cmvr/msgs/can_card_parameter.grpc.pb.h"
|
||||
#include "cmvr/msgs/geometry.pb.h"
|
||||
#include "robot/humanoid_robot/humanoid_robot.h"
|
||||
|
||||
// 定义一个命令行参数 --config_path
|
||||
DEFINE_string(config_path, "../config/cabin_robot.xml", "Path to the robot config XML file");
|
||||
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) {
|
||||
|
||||
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_WRIST_P", 0.0},
|
||||
// {"R_WRIST_Y", 0.0},
|
||||
// {"R_WRIST_R", 0.0},
|
||||
{"R_WRIST_R", 0.0},
|
||||
|
||||
{"WAIST_P" ,0.0},
|
||||
{"WAIST_Y" ,0.0},
|
||||
// {"WAIST_P" ,0.0},
|
||||
// {"WAIST_Y" ,0.0},
|
||||
};
|
||||
robot->moveJ(cmd,0.8);
|
||||
|
||||
@ -345,89 +379,96 @@ TEST(HumanoidRobotTest,MoveDeltaTest) {
|
||||
cmvr::msgs::Pose3d pose;
|
||||
pose.mutable_position()->set_x( 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_ry(0);
|
||||
pose.mutable_euler()->set_rz(0);
|
||||
pose.mutable_euler()->set_rz(-0.7854);
|
||||
|
||||
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->moveDeltaJ("PELVIS_S","R_FINGER_TIP",pose);
|
||||
robot->moveDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.05);
|
||||
}catch (std::exception &e) {
|
||||
LOG(INFO) << e.what();
|
||||
robot->torqueOff();
|
||||
LOG(INFO) << e.what();
|
||||
}
|
||||
}
|
||||
|
||||
TEST(HumanoidRobotTest,AngleToTest) {
|
||||
double rx = M_PI / 6; // 30°
|
||||
double ry = M_PI / 4; // 45°
|
||||
double rz = M_PI / 3; // 60°
|
||||
double rx = 0.9497;
|
||||
double ry = 1.6082;
|
||||
double rz = -2.4832;
|
||||
|
||||
Eigen::Matrix3d R = HumanoidRobot<7>::eulerZYXToRotationMatrix(rx, ry, rz);
|
||||
Eigen::Vector3d euler = HumanoidRobot<7>::rotationMatrixToEulerZYX(R);
|
||||
|
||||
std::cout << "Original Euler angles (rad):\n" << Eigen::Vector3d(rx, ry, rz).transpose() << "\n";
|
||||
std::cout << "Recovered Euler angles (rad):\n" << euler.transpose() << "\n";
|
||||
|
||||
}
|
||||
|
||||
|
||||
TEST(HumanoidRobotTest,MoveIKTest) {
|
||||
std::atomic<bool> running{true};
|
||||
|
||||
// 信号处理函数
|
||||
void signalHandler(int signum) {
|
||||
std::cout << "\nInterrupt signal (" << signum << ") received. Stopping..." << std::endl;
|
||||
running = false;
|
||||
}
|
||||
TEST(HumanoidRobotTest, MoveIKTest) {
|
||||
// 注册信号处理
|
||||
std::signal(SIGINT, signalHandler);
|
||||
|
||||
std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
|
||||
const XmlNode config(config_path);
|
||||
|
||||
if (!config.hasChild("DeviceManager")){
|
||||
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 robot = dmgr.getDevice<AbstractRobot>("hc01");
|
||||
|
||||
auto cur_pose = robot->fk("PELVIS_S","R_FINGER_TIP");
|
||||
|
||||
std::vector<JointPoint> cmd{
|
||||
{"R_SHOULDER_P", -0.08882},
|
||||
{"R_SHOULDER_R", 1.50297},
|
||||
{"R_SHOULDER_Y", 1.85922},
|
||||
{"R_ELBOW_R", 0.214816},
|
||||
{"R_WRIST_P", -1.69504},
|
||||
{"R_WRIST_Y", 0.0363855},
|
||||
{"R_WRIST_R", 0.0519807}
|
||||
};
|
||||
cmvr::msgs::Pose3d pose;
|
||||
pose.mutable_position()->set_x(0.591101);
|
||||
pose.mutable_position()->set_y(-0.159451);
|
||||
pose.mutable_position()->set_z(-0.206554);
|
||||
pose.mutable_euler()->set_rx(0.070589);
|
||||
pose.mutable_euler()->set_ry(0.117212);
|
||||
pose.mutable_euler()->set_rz(0.0916384);
|
||||
|
||||
// 打印关节状态线程
|
||||
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));
|
||||
}
|
||||
});
|
||||
|
||||
robot->moveJ(cmd);
|
||||
// 动作控制线程
|
||||
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();
|
||||
|
||||
// //
|
||||
// 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));
|
||||
// }
|
||||
|
||||
LOG(INFO) << "Test finished gracefully.";
|
||||
}
|
||||
@ -13,7 +13,7 @@ using namespace cmvr::api;
|
||||
|
||||
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);
|
||||
|
||||
|
||||
|
||||
@ -21,8 +21,10 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
|
||||
grpc::Status ret = grpc::Status::OK;
|
||||
|
||||
try {
|
||||
auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
|
||||
ctrl::TouchController touch_controller;
|
||||
std::shared_ptr<AbstractRobot> robot = nullptr;
|
||||
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());
|
||||
response->mutable_header()->set_success(true);
|
||||
response->mutable_header()->set_error_message("");
|
||||
|
||||
@ -45,7 +45,7 @@ TEST(GrpcHumanoidRobotClientTest, MyMoveJTest) {
|
||||
|
||||
|
||||
//
|
||||
std::vector<cmvr::device::JointPositionCmd> cmds{};
|
||||
std::vector<cmvr::device::JointPoint> cmds{};
|
||||
|
||||
|
||||
cmds = {
|
||||
@ -100,3 +100,39 @@ TEST(GrpcHumanoidRobotClientTest, MyMoveJTest) {
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
@ -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;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user