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

Merge branch 'lgv_dev_robot' into dev

Browse Source
This commit is contained in:
lgv 2025-08-28 09:46:20 +08:00
commit f0b5dfc72d
39 changed files with 1752 additions and 882 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -93,6 +93,6 @@ target_link_libraries(cmvr_es PRIVATE
cmvr_es::hardware cmvr_es::hardware
cmvr_es::httpclient cmvr_es::httpclient
cmvr_es::device::canbus cmvr_es::device::canbus
cmvr_es::robot::c701
cmvr_es::device::ti5motor cmvr_es::device::ti5motor
cmvr_es::ctrl::controller
) )

63
config/cabin_robot.xml
View File

@ -32,38 +32,39 @@
<!-- <LeftArm id="left_arm" devtype="ti5Robot" />--> <!-- <LeftArm id="left_arm" devtype="ti5Robot" />-->
<!-- <RightArm />--> <!-- <RightArm />-->
<!-- <Neck/>--> <!-- <Neck/>-->
<!-- <Humanoid id="hc01" dof="14"--> <Humanoid id="hc01" dof="14"
<!-- urdf="/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"--> urdf="/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"
<!-- baseLink="PELVIS_S"--> baseLink="PELVIS_S"
<!-- jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"--> jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
<!-- linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S"--> linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S"
<!-- bufferSize="50">--> bufferSize="50"
<!-- <CanManger id="" devId="">--> verbose="false">
<!-- <LeftArmCan id = " " devId = " " channelId ="0">--> <CanManger id="" devId="">
<!-- <Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <LeftArmCan id = " " devId = " " channelId ="0">
<!-- <Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="25" jointName="L_SHOULDER_Y" 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="26" jointName="L_ELBOW_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="27" jointName="L_WRIST_P" 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="28" jointName="L_WRIST_Y" 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"/>--> <!-- <Motor id="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- </LeftArmCan>--> </LeftArmCan>
<!-- <RightArmCan id = " " devId = " " channelId ="1">--> <RightArmCan id = " " devId = " " channelId ="1">
<!-- <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>--> <Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
<!-- <Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>--> <Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
<!-- </RightArmCan>--> </RightArmCan>
<!-- <WaistCan id = " " devId = " " channelId ="2">--> <WaistCan id = " " devId = " " channelId ="2">
<!-- <Motor id="14" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="14" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>--> <Motor id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- </WaistCan>--> </WaistCan>
<!-- </CanManger>--> </CanManger>
<!-- </Humanoid>--> </Humanoid>
</Robot> </Robot>
<BioHead> <BioHead>
@ -131,7 +132,7 @@
</MonitorManager> </MonitorManager>
<gRPCServer port="50060"> <gRPCServer port="50055">
</gRPCServer> </gRPCServer>

1
example/solve_ik.cpp
View File

@ -10,6 +10,7 @@
#include <string> #include <string>
#include <vector> #include <vector>
#include <cmath> #include <cmath>
#include <glog/logging.h>
constexpr int DOF = 14; constexpr int DOF = 14;
using RobotT = cmvr::dyn::Robot<DOF>; using RobotT = cmvr::dyn::Robot<DOF>;

103
include/controller/touch_controller.h Normal file
View File

@ -0,0 +1,103 @@
//
// Created by lgv on 2025/8/24.
//
#pragma once
#include <utility>
#include "devices/abstract_dexhand.h"
#include "devices/abstract_robot.h"
#include "cmvr/msgs/geometry.pb.h"
namespace cmvr {
namespace ctrl {
// 简易版 PID 控制器，带死区、积分限幅、输出限幅和输出斜率限制
class PID {
public:
PID(double kp, double ki, double kd,
double i_max,
double output_max_pos, double output_max_neg,
double delta_max = 0.0) // 输出变化最大值，0 表示不限制
: kp_(kp), ki_(ki), kd_(kd),
i_max_(i_max),
output_max_pos_(output_max_pos),
output_max_neg_(output_max_neg),
delta_max_(delta_max),
prev_error_(0), integral_(0), prev_output_(0) {}
double compute(double target, double current, double dt, double deadband = 0.0) {
double error = target - current;
// 死区处理
if (fabs(error) <= deadband) {
error = 0.0;
}
// 积分累加限幅
integral_ += error * dt;
if (integral_ > i_max_) integral_ = i_max_;
if (integral_ < -i_max_) integral_ = -i_max_;
// 微分
double derivative = (error - prev_error_) / dt;
prev_error_ = error;
// PID 输出
double output = kp_ * error + ki_ * integral_ + kd_ * derivative;
// 输出限幅
if (output > output_max_pos_) output = output_max_pos_;
if (output < -output_max_neg_) output = -output_max_neg_;
// 输出斜率限制
if (delta_max_ > 0.0) {
double delta = output - prev_output_;
if (delta > delta_max_) output = prev_output_ + delta_max_;
else if (delta < -delta_max_) output = prev_output_ - delta_max_;
}
prev_output_ = output;
return output;
}
private:
double kp_, ki_, kd_;
double prev_error_;
double integral_;
double i_max_; // 积分限幅
double output_max_pos_; // 向下按的最大输出
double output_max_neg_; // 向上抬的最大输出
double delta_max_; // 输出斜率限制
double prev_output_;
};
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)),
pid_(std::make_shared<PID>(0.05, 0.001, 0.01, 50.0, 0.5, 1.0)){}
~TouchController()=default;
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);
private:
std::shared_ptr<device::AbstractRobot> robot_{nullptr};
std::shared_ptr<device::AbstractDexHand> hand_{nullptr};
std::shared_ptr<PID> pid_{nullptr};
const double touch_threshold_ = 5.0; // 触控判定阈值
// 从压阻矩阵提取触控点与压力
bool extractTouch(const std::vector<std::vector<uint16_t>>& matrix,double& force, int& x, int& y);
};
}
}

3
include/devices/abstract_canbus.h
View File

@ -100,5 +100,8 @@ namespace cmvr::device {
/// CAN clientstatus /// CAN clientstatus
cmvr::msgs::ErrorCode status_; cmvr::msgs::ErrorCode status_;
//
bool enable_log_{false};
}; };
} }

6
include/devices/abstract_robot.h
View File

@ -9,7 +9,7 @@
#include "jsoncpp/json/json.h" #include "jsoncpp/json/json.h"
#include "abstract_device.h" #include "abstract_device.h"
#include "utils/controller/cartesian_controller.h" #include "utils/controller/cartesian_controller.h"
#include "cmvr/msgs/geometry.pb.h"
namespace cmvr::device{ namespace cmvr::device{
struct JointPoint{ struct JointPoint{
@ -66,10 +66,12 @@ namespace cmvr::device{
virtual void moveJ(std::vector<double> &joints, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); } virtual void moveJ(std::vector<double> &joints, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
virtual void moveJ(std::vector<JointPoint> &cmd, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); } virtual void moveJ(std::vector<JointPoint> &cmd, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
virtual void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
virtual void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
virtual void moveJ_IK(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); } virtual void moveJ_IK(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
virtual void moveJ_IK(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); } virtual void moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
virtual void moveL(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); } virtual void moveL(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }

25
include/service/grpc_touch_service.h Normal file
View File

@ -0,0 +1,25 @@
//
// Created by lgv on 2025/8/25.
//
#pragma once
#include "device_manager/device_manager.h"
#include "cmvr/api/touch_service.grpc.pb.h"
namespace cmvr {
namespace service {
class gRPCTouchServiceImpl final : public api::TouchService::Service {
public:
gRPCTouchServiceImpl();
~gRPCTouchServiceImpl() = default;
grpc::Status touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) override;
private:
device::DeviceManager& dmgr_;
};
}
}

67
protos/cmvr/api/geometry.proto
View File

@ -1,67 +0,0 @@
syntax = "proto3";
package cmvr.api;
message Vec2 {
double x = 1;
double y = 2;
}
message Vec3 {
double x = 1;
double y = 2;
double z = 3;
}
message SE2Pose {
Vec2 position = 1; // (m)
double angle = 2; // (rad)
}
message SE2Velocity {
Vec2 linear = 1; // (m/s)
double angular = 2; // (rad/s)
}
message Quaternion {
double x = 1;
double y = 2;
double z = 3;
double w = 4;
}
message EulerAngleZYX {
double z = 1;
double y = 2;
double x = 3;
}
message SE3Pose {
Vec3 position = 1; // (m)
oneof rotation {
Quaternion quaternion = 2;
EulerAngleZYX euler = 3;
}
}
message Inertial {
// Mass (kg)
double mass = 1;
// Center of mass (m)
Vec3 center_of_mass = 2;
// Inertia tensor
Inertia inertia = 3;
}
// Inertia tensor components (kg*m^2)
message Inertia {
double ixx = 1;
double iyy = 2;
double izz = 3;
double ixy = 4;
double ixz = 5;
double iyz = 6;
}

23
protos/cmvr/api/touch_service.proto Normal file
View File

@ -0,0 +1,23 @@
syntax = "proto3";
package cmvr.api;
import "cmvr/api/common.proto";
import "cmvr/msgs/geometry.proto";
message Touch{
message Request{
CommandHeader.Request header = 1;
cmvr.msgs.Pose3d pose = 2;
cmvr.msgs.Pose3d offset = 3;
}
message Response{
CommandHeader.Feedback header= 1;
}
}
service TouchService{
rpc touch(Touch.Request) returns (Touch.Response);
}

51
protos/cmvr/msgs/geometry.proto Normal file
View File

@ -0,0 +1,51 @@
syntax = "proto3";
package cmvr.msgs;
message Vec2 {
double x = 1;
double y = 2;
}
message Vec3 {
double x = 1;
double y = 2;
double z = 3;
}
// ===== 四元数 =====
message Quat {
double w = 1;
double x = 2;
double y = 3;
double z = 4;
}
// ===== 位置（3D）=====
message Position {
double x = 1; // unit: m
double y = 2;
double z = 3;
}
// ===== 欧拉角 =====
message Euler {
double rx = 1; // unit: rad (roll)
double ry = 2; // unit: rad (pitch)
double rz = 3; // unit: rad (yaw)
}
// ===== 位姿（3D）=====
message Pose3d {
Position position = 1; // (m)
Quat quaternion = 2; // 四元数
Euler euler = 3; // 欧拉角 (rad)
}
// ===== 位姿（2D）=====
message Pose2d {
double x = 1; // (m)
double y = 2; // (m)
double theta = 3; // (rad)
}

BIN
python/hand_eye_calibration/__pycache__/save_poses.cpython-310.pyc
View File

Binary file not shown.

BIN
python/hand_eye_calibration/eye_hand_data/data2025082102/1.jpg
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 148 KiB

4
python/hand_eye_calibration/eye_hand_data/data2025082102/RobotToolPose.csv
View File

@ -1,4 +0,0 @@
0.9999928544,-0.0037748262,-0.0002045763,0.0309996779
-0.0001987754,0.0015370337,-0.999998799,-0.2050560322
0.0037751361,0.9999916941,0.0015362724,-0.4492433164
0.0,0.0,0.0,1.0
1 0.9999928544 -0.0037748262 -0.0002045763 0.0309996779
2 -0.0001987754 0.0015370337 -0.999998799 -0.2050560322
3 0.0037751361 0.9999916941 0.0015362724 -0.4492433164
4 0.0 0.0 0.0 1.0

BIN
python/hand_eye_calibration/eye_hand_data/data2025082103/1.jpg
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 121 KiB

4
python/hand_eye_calibration/eye_hand_data/data2025082103/RobotToolPose.csv
View File

@ -1,4 +0,0 @@
-0.1731632498,-0.8956300844,0.4097087267,0.3589373459
0.7907069601,-0.3744371788,-0.4843338749,-0.3095923731
0.587194169,0.2400907141,0.7730197002,-0.2279086867
0.0,0.0,0.0,1.0
1 -0.1731632498 -0.8956300844 0.4097087267 0.3589373459
2 0.7907069601 -0.3744371788 -0.4843338749 -0.3095923731
3 0.587194169 0.2400907141 0.7730197002 -0.2279086867
4 0.0 0.0 0.0 1.0

BIN
python/hand_eye_calibration/libs/__pycache__/auxiliary.cpython-310.pyc
View File

Binary file not shown.

BIN
python/hand_eye_calibration/libs/__pycache__/log_setting.cpython-310.pyc
View File

Binary file not shown.

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

@ -1,704 +1,46 @@
2025-08-21 09:52:37 - __main__ - INFO: 读 eye_hand_data/data2025082010/1.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/1.jpg 2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-08-21 09:54:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_1.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/2.jpg 2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-08-21 09:54:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_2.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/3.jpg 2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-08-21 09:54:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_3.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/4.jpg 2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-08-21 09:54:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_4.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/5.jpg 2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-08-21 09:54:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_5.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/6.jpg 2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-08-21 09:54:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_6.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/7.jpg 2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-08-21 09:54:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_7.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/8.jpg 2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_8.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/9.jpg 2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_9.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/10.jpg 2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_10.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/11.jpg 2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_11.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/12.jpg 2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_12.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/13.jpg 2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_13.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/14.jpg 2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_14.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/15.jpg 2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_15.jpg 2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/16.jpg 2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_16.jpg 2025-08-23 16:08:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/17.jpg 2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_17.jpg 2025-08-23 16:08:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/18.jpg 2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_18.jpg 2025-08-23 16:08:46 - __main__ - INFO: 旋转矩阵是:
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/19.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_19.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/20.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_20.jpg
2025-08-21 09:54:47 - __main__ - INFO: 旋转矩阵是:
[[-0.8747942 0.22946188 0.42671109]
[-0.45426317 -0.08221912 -0.88706538]
[-0.16846387 -0.96983878 0.17616089]]
2025-08-21 09:54:47 - __main__ - INFO: 平移向量是:
[[-0.12651951]
[-0.21061809]
[ 0.0697177 ]]
2025-08-21 09:54:47 - __main__ - INFO: 四元数是：
[ 0.08840819 -0.63569139 0.73026951 -0.234066 ]
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/1.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_1.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/2.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_2.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/3.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_3.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/4.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_4.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/5.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_5.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/6.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_6.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/7.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_7.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/8.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_8.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/9.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_9.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/10.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_10.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/11.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_11.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/12.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_12.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/13.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_13.jpg
2025-08-21 09:55:22 - __main__ - INFO: 读 eye_hand_data/data2025082009/14.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_14.jpg
2025-08-21 09:55:22 - __main__ - INFO: 读 eye_hand_data/data2025082009/15.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_15.jpg
2025-08-21 09:55:22 - __main__ - INFO: 读 eye_hand_data/data2025082009/16.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_16.jpg
2025-08-21 09:55:22 - __main__ - INFO: 读 eye_hand_data/data2025082009/17.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_17.jpg
2025-08-21 09:55:22 - __main__ - INFO: 读 eye_hand_data/data2025082009/18.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_18.jpg
2025-08-21 09:55:22 - __main__ - INFO: 读 eye_hand_data/data2025082009/19.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_19.jpg
2025-08-21 09:55:22 - __main__ - INFO: 读 eye_hand_data/data2025082009/20.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_20.jpg
2025-08-21 09:55:22 - __main__ - INFO: 旋转矩阵是:
[[-0.87479368 0.22946254 0.4267118 ]
[-0.45426401 -0.08221905 -0.88706496]
[-0.16846434 -0.96983863 0.17616128]]
2025-08-21 09:55:22 - __main__ - INFO: 平移向量是:
[[-0.12651976]
[-0.21061824]
[ 0.0697178 ]]
2025-08-21 09:55:22 - __main__ - INFO: 四元数是：
[ 0.08840828 -0.63569123 0.73026947 -0.23406652]
2025-08-21 11:27:09 - libs.auxiliary - INFO: create folder eye_hand_data/data20250821
2025-08-21 11:27:09 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-08-21 11:27:20 - __main__ - INFO: robot.torqueOff
2025-08-21 11:28:46 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:28:51 - __main__ - INFO: 当前关节角: [0.8253898778736996, 0.2562934472690343, -0.0472638845356522, 0.982365663091435, -0.811866925952772, 1.0474525818308873, 0.016616543126199347]
2025-08-21 11:28:51 - __main__ - INFO: 获取位姿成功:
[[-0.25318109 -0.89031678 0.37846449 0.20513955]
[ 0.6294954 -0.44867523 -0.63437061 -0.47519781]
[ 0.73459844 0.07763101 0.6740464 -0.14564497]
[ 0. 0. 0. 1. ]]
2025-08-21 11:28:51 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:28:51 - __main__ - INFO: ===采集第1次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/1.jpg
2025-08-21 11:28:53 - __main__ - INFO: robot.torqueOff
2025-08-21 11:29:00 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:29:04 - __main__ - INFO: 当前关节角: [0.8917611258247912, 0.3193556252779419, 0.0663351766206834, 0.6812782681741733, -1.1216997200029486, 1.03807878215244, 0.016623187844958753]
2025-08-21 11:29:04 - __main__ - INFO: 获取位姿成功:
[[-0.25224294 -0.90609294 0.33966025 0.22289984]
[ 0.8456558 -0.37703157 -0.37777436 -0.51750052]
[ 0.47036131 0.19194474 0.8613463 -0.11653364]
[ 0. 0. 0. 1. ]]
2025-08-21 11:29:04 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:29:04 - __main__ - INFO: ===采集第2次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/2.jpg
2025-08-21 11:29:05 - __main__ - INFO: robot.torqueOff
2025-08-21 11:29:14 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:29:18 - __main__ - INFO: 当前关节角: [1.1896438678089452, 0.30651423165262204, 0.10917083072596027, 0.6814301474601026, -1.6521096487448172, 1.0335860030255448, 0.016608949161902885]
2025-08-21 11:29:18 - __main__ - INFO: 获取位姿成功:
[[-0.42170444 -0.84445498 0.33024408 0.2558707 ]
[ 0.9003429 -0.43313682 0.04213264 -0.51967065]
[ 0.10746175 0.31510043 0.94295477 -0.04397596]
[ 0. 0. 0. 1. ]]
2025-08-21 11:29:18 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:29:18 - __main__ - INFO: ===采集第3次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/3.jpg
2025-08-21 11:29:19 - __main__ - INFO: robot.torqueOff
2025-08-21 11:29:28 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:29:34 - __main__ - INFO: 当前关节角: [1.4559585002127002, 0.30681324399679527, 0.17970546960258804, 0.6812127702321163, -2.1080019043365628, 1.0236910675472528, 0.016632680300329335]
2025-08-21 11:29:34 - __main__ - INFO: 获取位姿成功:
[[-0.44997072 -0.85803562 0.24759084 0.26886029]
[ 0.85847564 -0.33920789 0.38465255 -0.52136803]
[-0.24606082 0.38563309 0.8892363 0.03026231]
[ 0. 0. 0. 1. ]]
2025-08-21 11:29:34 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:29:34 - __main__ - INFO: ===采集第4次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/4.jpg
2025-08-21 11:29:36 - __main__ - INFO: robot.torqueOff
2025-08-21 11:29:46 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:29:51 - __main__ - INFO: 当前关节角: [1.6850703015280803, 0.3357206183368209, 0.14558294028196603, 0.7301084078459711, -2.380654649191254, 1.0414419590902364, 0.016624137090495813]
2025-08-21 11:29:51 - __main__ - INFO: 获取位姿成功:
[[-0.48372144 -0.85725737 0.17641818 0.27941969]
[ 0.74091476 -0.29378779 0.60393216 -0.50585554]
[-0.46589579 0.42284577 0.77726609 0.08600871]
[ 0. 0. 0. 1. ]]
2025-08-21 11:29:51 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:29:51 - __main__ - INFO: ===采集第5次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/5.jpg
2025-08-21 11:29:53 - __main__ - INFO: robot.torqueOff
2025-08-21 11:30:03 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:30:07 - __main__ - INFO: 当前关节角: [1.9511694551949232, 0.7207327096764498, -0.06620038375442117, 0.8133790740933053, -1.3808665335126573, 0.21355366696300027, 1.1411364716197472]
2025-08-21 11:30:07 - __main__ - INFO: 获取位姿成功:
[[-0.21216612 -0.68156647 0.70032327 0.38204138]
[ 0.97416126 -0.20429083 0.09630729 -0.3389997 ]
[ 0.07742981 0.70266095 0.70729924 0.18673545]
[ 0. 0. 0. 1. ]]
2025-08-21 11:30:07 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:30:07 - __main__ - INFO: ===采集第6次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/6.jpg
2025-08-21 11:30:08 - __main__ - INFO: robot.torqueOff
2025-08-21 11:30:17 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:30:21 - __main__ - INFO: 当前关节角: [1.8940381633015537, 0.4051379952163314, -0.12349589512570175, 0.6297399309851491, -1.2283436591092607, 0.9395015316200465, 1.0908577832583988]
2025-08-21 11:30:21 - __main__ - INFO: 获取位姿成功:
[[-0.4310179 -0.70629928 0.5615736 0.28826997]
[ 0.89989504 -0.38227375 0.20989447 -0.50135244]
[ 0.06642653 0.59582557 0.80036205 0.12584165]
[ 0. 0. 0. 1. ]]
2025-08-21 11:30:21 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:30:21 - __main__ - INFO: ===采集第7次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/7.jpg
2025-08-21 11:30:22 - __main__ - INFO: robot.torqueOff
2025-08-21 11:30:35 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:30:42 - __main__ - INFO: 当前关节角: [1.8361265915767224, 1.1135570917860655, -0.08976350572081057, 0.5308864500014717, -1.5194516356954322, 0.07328175546087345, 0.9682465849732351]
2025-08-21 11:30:42 - __main__ - INFO: 获取位姿成功:
[[-0.13468827 -0.72882711 0.67131968 0.45481058]
[ 0.98906271 -0.05778144 0.13570654 -0.23383011]
[-0.06011679 0.68225534 0.7286382 0.15317249]
[ 0. 0. 0. 1. ]]
2025-08-21 11:30:42 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:30:42 - __main__ - INFO: ===采集第8次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/8.jpg
2025-08-21 11:30:43 - __main__ - INFO: robot.torqueOff
2025-08-21 11:31:04 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:31:08 - __main__ - INFO: 当前关节角: [1.773997521930743, 0.605477215057948, 0.028329283807957347, 1.2173817716473154, -1.7268484961050745, 0.15453242720534835, 0.8873870023900281]
2025-08-21 11:31:08 - __main__ - INFO: 获取位姿成功:
[[-0.35110132 -0.76965787 0.53324913 0.36358769]
[ 0.90927708 -0.14433419 0.39036245 -0.28723159]
[-0.22347945 0.62192798 0.75050818 0.11913745]
[ 0. 0. 0. 1. ]]
2025-08-21 11:31:08 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:31:08 - __main__ - INFO: ===采集第9次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/9.jpg
2025-08-21 11:31:09 - __main__ - INFO: robot.torqueOff
2025-08-21 11:31:26 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:31:30 - __main__ - INFO: 当前关节角: [1.4005130683931457, 1.4388303137420266, 0.15982067409229817, 0.8202088957324373, -1.891621384164204, -1.0446542059876405, 0.5873542192644378]
2025-08-21 11:31:30 - __main__ - INFO: 获取位姿成功:
[[ 0.36091887 -0.80430791 0.47204487 0.44921615]
[ 0.93025175 0.34636313 -0.12109612 -0.02012862]
[-0.06610037 0.48282644 0.87321782 -0.00595984]
[ 0. 0. 0. 1. ]]
2025-08-21 11:31:30 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:31:30 - __main__ - INFO: ===采集第10次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/10.jpg
2025-08-21 11:31:32 - __main__ - INFO: robot.torqueOff
2025-08-21 11:31:51 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:31:56 - __main__ - INFO: robot.torqueOff
2025-08-21 11:32:05 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:32:09 - __main__ - INFO: 当前关节角: [1.4349839708258678, 0.8371567255510698, -0.01699624134102253, 1.6177536050220789, -2.016514568720455, -0.8374870629979659, 0.5931132919377683]
2025-08-21 11:32:09 - __main__ - INFO: 获取位姿成功:
[[-0.0401021 -0.92276134 0.3832797 0.33858263]
[ 0.95373667 0.07903515 0.29006864 -0.11724388]
[-0.2979567 0.37718027 0.87690185 -0.01763812]
[ 0. 0. 0. 1. ]]
2025-08-21 11:32:09 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:32:09 - __main__ - INFO: ===采集第11次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/11.jpg
2025-08-21 11:32:12 - __main__ - INFO: robot.torqueOff
2025-08-21 11:32:48 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:32:52 - __main__ - INFO: 当前关节角: [0.7093417632317545, 0.9039997485340793, 0.3698402999208317, 1.4349972602633867, -1.868800572207794, -0.677916040481914, -0.3356152520822075]
2025-08-21 11:32:52 - __main__ - INFO: 获取位姿成功:
[[ 6.72744280e-03 -9.95813278e-01 9.11628002e-02 3.05739589e-01]
[ 9.97824272e-01 7.05770400e-04 -6.59259053e-02 -1.35775630e-01]
[ 6.55855519e-02 9.14079675e-02 9.93651407e-01 -1.75063848e-01]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
2025-08-21 11:32:52 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:32:52 - __main__ - INFO: ===采集第12次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/12.jpg
2025-08-21 11:32:53 - __main__ - INFO: robot.torqueOff
2025-08-21 11:34:07 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:34:12 - __main__ - INFO: 当前关节角: [0.5287221701769795, 0.9609924505790383, 0.5908911081355157, 1.1136415746388637, -2.4692230547809086, -0.6455401229494787, -0.3356162013277445]
2025-08-21 11:34:12 - __main__ - INFO: 获取位姿成功:
[[ 0.12520332 -0.99186645 -0.02291465 0.31366201]
[ 0.90598349 0.10488766 0.41011278 -0.19783819]
[-0.40437364 -0.07210778 0.91174691 -0.25384818]
[ 0. 0. 0. 1. ]]
2025-08-21 11:34:12 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:34:12 - __main__ - INFO: ===采集第13次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/13.jpg
2025-08-21 11:34:13 - __main__ - INFO: robot.torqueOff
2025-08-21 11:34:31 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:34:37 - __main__ - INFO: 当前关节角: [0.39919666739988574, 1.3179505393164566, 0.9448201543641821, 1.2761780402126846, -2.03749479358051, -0.385342428786525, -0.3356180998188186]
2025-08-21 11:34:37 - __main__ - INFO: 获取位姿成功:
[[ 0.03301101 -0.99908595 0.02715761 0.313764 ]
[ 0.83829597 0.01288215 -0.54506323 -0.0913168 ]
[ 0.54421516 0.0407592 0.83795498 -0.18631875]
[ 0. 0. 0. 1. ]]
2025-08-21 11:34:37 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:34:37 - __main__ - INFO: ===采集第14次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/14.jpg
2025-08-21 11:34:39 - __main__ - INFO: robot.torqueOff
2025-08-21 11:35:06 - __main__ - INFO: 检测到 'q'，退出程序
2025-08-21 11:35:07 - __main__ - INFO: 相机已关闭，窗口已销毁
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/1.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_1.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/2.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_2.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/3.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_3.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/4.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_4.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/5.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_5.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/6.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_6.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/7.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_7.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/8.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_8.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/9.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_9.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/10.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_10.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/11.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_11.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/12.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_12.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/13.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_13.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/14.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_14.jpg
2025-08-21 11:35:46 - __main__ - INFO: 旋转矩阵是:
[[-0.99732744 0.02734315 0.0677519 ]
[-0.06713675 0.0228221 -0.99748274]
[-0.02882056 -0.99936555 -0.02092538]]
2025-08-21 11:35:46 - __main__ - INFO: 平移向量是:
[[-0.00078482]
[-0.17419722]
[ 0.06796275]]
2025-08-21 11:35:46 - __main__ - INFO: 四元数是：
[-0.01392688 0.71433097 -0.69885263 0.03379822]
2025-08-21 11:38:53 - __main__ - INFO: 读 eye_hand_data/data20250821/1.jpg
2025-08-21 11:38:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_1.jpg
2025-08-21 11:38:53 - __main__ - INFO: 读 eye_hand_data/data20250821/2.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_2.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/3.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_3.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/4.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_4.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/5.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_5.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/6.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_6.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/7.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_7.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/8.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_8.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/9.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_9.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/10.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_10.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/11.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_11.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/12.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_12.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/13.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_13.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/14.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_14.jpg
2025-08-21 11:38:54 - __main__ - INFO: 旋转矩阵是:
[[-0.99732744 0.02734315 0.0677519 ]
[-0.06713675 0.0228221 -0.99748274]
[-0.02882056 -0.99936555 -0.02092538]]
2025-08-21 11:38:54 - __main__ - INFO: 平移向量是:
[[-0.00078482]
[-0.17419722]
[ 0.06796275]]
2025-08-21 11:38:54 - __main__ - INFO: 四元数是：
[-0.01392688 0.71433097 -0.69885263 0.03379822]
2025-08-21 16:14:42 - libs.auxiliary - INFO: create folder eye_hand_data/data2025082101
2025-08-21 16:14:42 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-08-21 16:15:03 - __main__ - INFO: robot.torqueOff
2025-08-21 16:15:13 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:15:27 - __main__ - INFO: robot.torqueOff
2025-08-21 16:16:00 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:16:07 - __main__ - INFO: 当前关节角: [0.1881774860208302, 1.5359324367098321, 2.101302129336008, 1.237947176207676, -3.239618392465156, 0.660369236729398, -0.22447188913024077]
2025-08-21 16:16:07 - __main__ - INFO: 获取位姿成功:
[[-0.13936557 -0.98748272 -0.0738588 0.25639562]
[ 0.98789482 -0.13351676 -0.07897536 -0.27191259]
[ 0.06812542 -0.08397117 0.9941367 -0.23921664]
[ 0. 0. 0. 1. ]]
2025-08-21 16:16:07 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:16:07 - __main__ - INFO: ===采集第1次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/1.jpg
2025-08-21 16:16:11 - __main__ - INFO: robot.torqueOff
2025-08-21 16:16:20 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:16:25 - __main__ - INFO: 当前关节角: [0.1847488111409769, 1.3553365747934838, 2.1088087630430623, 1.2244802297734347, -3.265897305913068, 0.6804733079587482, -0.22435892891133088]
2025-08-21 16:16:25 - __main__ - INFO: 获取位姿成功:
[[-0.12214152 -0.99148995 -0.04504583 0.24935823]
[ 0.98590508 -0.12643169 0.10957282 -0.3301866 ]
[-0.11433557 -0.03102752 0.99295754 -0.22048319]
[ 0. 0. 0. 1. ]]
2025-08-21 16:16:25 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:16:25 - __main__ - INFO: ===采集第2次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/2.jpg
2025-08-21 16:16:28 - __main__ - INFO: robot.torqueOff
2025-08-21 16:16:36 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:16:40 - __main__ - INFO: 当前关节角: [0.18216591403464222, 1.3093807506078976, 2.108801169078766, 1.237752580872579, -3.386807456195824, 0.674951546669682, -0.22420894811647574]
2025-08-21 16:16:40 - __main__ - INFO: 获取位姿成功:
[[-0.12765968 -0.99174931 0.01167535 0.24725332]
[ 0.95778442 -0.12021331 0.26114702 -0.34440277]
[-0.25758884 0.04452042 0.96522843 -0.21331776]
[ 0. 0. 0. 1. ]]
2025-08-21 16:16:40 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:16:40 - __main__ - INFO: ===采集第3次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/3.jpg
2025-08-21 16:16:43 - __main__ - INFO: robot.torqueOff
2025-08-21 16:16:52 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:16:57 - __main__ - INFO: 当前关节角: [0.15805412814783334, 1.218028207858052, 2.1077446587960202, 1.3528049369461503, -3.398763203735069, 0.6851996014877597, -0.2241823692414381]
2025-08-21 16:16:57 - __main__ - INFO: 获取位姿成功:
[[-0.12061637 -0.99148511 -0.04908122 0.24079104]
[ 0.90170295 -0.13010493 0.41231601 -0.36562399]
[-0.41519089 0.00547538 0.90971784 -0.17673377]
[ 0. 0. 0. 1. ]]
2025-08-21 16:16:57 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:16:57 - __main__ - INFO: ===采集第4次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/4.jpg
2025-08-21 16:16:59 - __main__ - INFO: robot.torqueOff
2025-08-21 16:17:08 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:17:14 - __main__ - INFO: 当前关节角: [0.15782630921893942, 1.148865228782473, 2.0802051472748952, 1.4294517678358942, -3.4739330085691504, 0.6850638593759604, -0.22417762301375282]
2025-08-21 16:17:14 - __main__ - INFO: 获取位姿成功:
[[-0.13323657 -0.98863605 -0.06961879 0.24168424]
[ 0.81161017 -0.14915447 0.56483792 -0.37307124]
[-0.56880309 0.01875375 0.8222599 -0.14946229]
[ 0. 0. 0. 1. ]]
2025-08-21 16:17:14 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:17:14 - __main__ - INFO: ===采集第5次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/5.jpg
2025-08-21 16:17:21 - __main__ - INFO: robot.torqueOff
2025-08-21 16:17:38 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:17:43 - __main__ - INFO: 当前关节角: [0.3590416826912605, 1.1394164387065981, 2.0769226562077487, 1.5773176436344847, -2.5660726784358565, 0.5330820549286868, 0.8458689010901876]
2025-08-21 16:17:43 - __main__ - INFO: 获取位姿成功:
[[-0.1100018 -0.97171944 0.20895198 0.26213488]
[ 0.9937792 -0.11120581 0.00601407 -0.35469134]
[ 0.01739269 0.20831369 0.97790741 -0.04493756]
[ 0. 0. 0. 1. ]]
2025-08-21 16:17:43 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:17:43 - __main__ - INFO: ===采集第6次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/6.jpg
2025-08-21 16:17:46 - __main__ - INFO: robot.torqueOff
2025-08-21 16:17:53 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:17:59 - __main__ - INFO: 当前关节角: [0.3520818144135517, 1.1556029736045101, 2.017053740185504, 1.6291274650471073, -2.566324228503177, 0.5650403044248166, 0.8458594086348171]
2025-08-21 16:17:59 - __main__ - INFO: 获取位姿成功:
[[-0.20049553 -0.96061353 0.19241411 0.26384736]
[ 0.97904311 -0.18929837 0.07510466 -0.33478381]
[-0.03572288 0.20343986 0.97843554 -0.04338287]
[ 0. 0. 0. 1. ]]
2025-08-21 16:17:59 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:17:59 - __main__ - INFO: ===采集第7次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/7.jpg
2025-08-21 16:18:01 - __main__ - INFO: robot.torqueOff
2025-08-21 16:18:09 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:18:13 - __main__ - INFO: 当前关节角: [0.3172653866053403, 1.313845052368681, 1.8904177408232146, 1.7188083864061938, -2.6905567381656357, 0.28883548277045484, 0.8458527639160577]
2025-08-21 16:18:13 - __main__ - INFO: 获取位姿成功:
[[-0.09067794 -0.96156996 0.25915385 0.27763283]
[ 0.99082305 -0.1133011 -0.0737058 -0.27031831]
[ 0.1002357 0.25009212 0.96301959 -0.05935151]
[ 0. 0. 0. 1. ]]
2025-08-21 16:18:13 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:18:13 - __main__ - INFO: ===采集第8次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/8.jpg
2025-08-21 16:18:16 - __main__ - INFO: robot.torqueOff
2025-08-21 16:18:25 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:18:30 - __main__ - INFO: 当前关节角: [0.3113658255925252, 1.3320734144168047, 1.7972094230484208, 1.807990953858325, -2.9523187889736606, 0.29634021798643495, 0.8458480176883724]
2025-08-21 16:18:30 - __main__ - INFO: 获取位姿成功:
[[-0.06182316 -0.96333913 0.26106631 0.27572117]
[ 0.985666 -0.01779069 0.16776779 -0.23950089]
[-0.15697273 0.26769612 0.9506305 -0.05533822]
[ 0. 0. 0. 1. ]]
2025-08-21 16:18:30 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:18:30 - __main__ - INFO: ===采集第9次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/9.jpg
2025-08-21 16:18:33 - __main__ - INFO: robot.torqueOff
2025-08-21 16:18:44 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:18:49 - __main__ - INFO: 当前关节角: [0.3174068241903619, 1.4675260063272884, 1.5487007384287987, 1.7904982571014212, -3.2589716104746933, 0.07981446324690623, 0.8458337790053165]
2025-08-21 16:18:49 - __main__ - INFO: 获取位姿成功:
[[ 0.00204587 -0.95045805 0.31084613 0.291817 ]
[ 0.96975598 0.0777536 0.2313606 -0.15674434]
[-0.24406795 0.30097156 0.92187144 -0.07498582]
[ 0. 0. 0. 1. ]]
2025-08-21 16:18:49 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:18:49 - __main__ - INFO: ===采集第10次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/10.jpg
2025-08-21 16:18:52 - __main__ - INFO: robot.torqueOff
2025-08-21 16:19:08 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:19:14 - __main__ - INFO: robot.torqueOff
2025-08-21 16:19:25 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:19:30 - __main__ - INFO: 当前关节角: [0.27753946087946524, 1.6105355419582903, 1.2276915274253017, 1.6875801067280616, -3.5108406242955006, -0.6567573574608925, 0.8458072001302789]
2025-08-21 16:19:30 - __main__ - INFO: 获取位姿成功:
[[ 0.38872691 -0.8780319 0.27919772 0.29523427]
[ 0.91848265 0.39319638 -0.0422638 -0.05153552]
[-0.07267057 0.27286734 0.95930308 -0.10555432]
[ 0. 0. 0. 1. ]]
2025-08-21 16:19:30 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:19:30 - __main__ - INFO: ===采集第11次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/11.jpg
2025-08-21 16:19:33 - __main__ - INFO: robot.torqueOff
2025-08-21 16:20:00 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:20:06 - __main__ - INFO: 当前关节角: [0.19953615811726558, 1.5134979676870048, 1.3755934745543004, 1.2340486247869789, -3.235004109909517, -0.39607934505618747, -0.18381760126912305]
2025-08-21 16:20:06 - __main__ - INFO: 获取位姿成功:
[[ 0.2004248 -0.97752505 -0.065381 0.29826073]
[ 0.95269085 0.17889858 0.24571414 -0.11593768]
[-0.22849516 -0.11153509 0.96713489 -0.24686201]
[ 0. 0. 0. 1. ]]
2025-08-21 16:20:06 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:20:06 - __main__ - INFO: ===采集第12次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/12.jpg
2025-08-21 16:20:08 - __main__ - INFO: robot.torqueOff
2025-08-21 16:20:19 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:20:24 - __main__ - INFO: 当前关节角: [0.14909894575122867, 1.5289374463472523, 1.327669864370393, 1.130913097185633, -3.1925177781618777, -0.44474526525007424, -0.2899508462764979]
2025-08-21 16:20:24 - __main__ - INFO: 获取位姿成功:
[[ 0.1795737 -0.98355917 -0.01909571 0.27480921]
[ 0.95378635 0.16931837 0.24823956 -0.10309542]
[-0.24092504 -0.06279052 0.96851044 -0.28676362]
[ 0. 0. 0. 1. ]]
2025-08-21 16:20:24 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:20:24 - __main__ - INFO: ===采集第13次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/13.jpg
2025-08-21 16:20:26 - __main__ - INFO: robot.torqueOff
2025-08-21 16:20:38 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:20:43 - __main__ - INFO: 当前关节角: [0.16214537641255308, 1.3656235486785808, 1.5120835918367883, 1.2035123451053618, -2.9271913103622, -0.014777854520918107, -0.2900476693212778]
2025-08-21 16:20:43 - __main__ - INFO: 获取位姿成功:
[[ 0.03397924 -0.99615678 -0.08072841 0.28406157]
[ 0.99940914 0.03344943 0.00790666 -0.19681757]
[-0.00517596 -0.08094937 0.99670478 -0.25137906]
[ 0. 0. 0. 1. ]]
2025-08-21 16:20:43 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:20:43 - __main__ - INFO: ===采集第14次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/14.jpg
2025-08-21 16:20:45 - __main__ - INFO: robot.torqueOff
2025-08-21 16:21:17 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:21:26 - __main__ - INFO: 当前关节角: [0.23122007565318572, 1.628535135831983, 1.828089329614453, 1.4320631423083405, -3.0376977295588006, 0.19928270955887112, 0.14386290736881704]
2025-08-21 16:21:26 - __main__ - INFO: 获取位姿成功:
[[ 0.02777164 -0.99933259 0.02373016 0.29887982]
[ 0.98010716 0.02255505 -0.19718324 -0.18783112]
[ 0.1965164 0.0287342 0.98007941 -0.17737878]
[ 0. 0. 0. 1. ]]
2025-08-21 16:21:26 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:21:26 - __main__ - INFO: ===采集第15次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/15.jpg
2025-08-21 16:21:29 - __main__ - INFO: robot.torqueOff
2025-08-21 16:21:42 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:21:47 - __main__ - INFO: 当前关节角: [0.22977437470024648, 1.339187060471517, 2.1365172402697836, 1.3177511977536744, -3.033028390762013, 0.8488428873577902, 0.1439046741724476]
2025-08-21 16:21:47 - __main__ - INFO: 获取位姿成功:
[[-0.24782493 -0.96680534 0.06221126 0.25321104]
[ 0.96857382 -0.24585225 0.03770167 -0.33940743]
[-0.02115539 0.06959961 0.99735066 -0.17940936]
[ 0. 0. 0. 1. ]]
2025-08-21 16:21:47 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:21:47 - __main__ - INFO: ===采集第16次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/16.jpg
2025-08-21 16:21:50 - __main__ - INFO: robot.torqueOff
2025-08-21 16:22:00 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:22:05 - __main__ - INFO: 当前关节角: [0.2123139522916026, 1.6156225487913838, 1.9870955510359554, 1.1415721753212567, -3.261262139955614, 0.40399890057186194, -0.2904045856432116]
2025-08-21 16:22:05 - __main__ - INFO: 获取位姿成功:
[[-0.02238853 -0.99787522 -0.06118662 0.27706052]
[ 0.9941035 -0.01572528 -0.10728906 -0.21679508]
[ 0.10609892 -0.06322787 0.99234331 -0.26052986]
[ 0. 0. 0. 1. ]]
2025-08-21 16:22:05 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:22:05 - __main__ - INFO: ===采集第17次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/17.jpg
2025-08-21 16:22:08 - __main__ - INFO: robot.torqueOff
2025-08-21 16:22:20 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:22:25 - __main__ - INFO: 当前关节角: [0.7920960399069291, 1.588399136034099, 1.9284805883681642, 1.3829188023637766, -3.1986537013134204, 0.2975713894479991, 1.133828230229938]
2025-08-21 16:22:25 - __main__ - INFO: 获取位姿成功:
[[ 0.02514396 -0.85857888 0.51206454 0.37379129]
[ 0.99831944 -0.00518728 -0.05771811 -0.22146041]
[ 0.05221177 0.51265524 0.85700556 -0.00260762]
[ 0. 0. 0. 1. ]]
2025-08-21 16:22:25 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:22:25 - __main__ - INFO: ===采集第18次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/18.jpg
2025-08-21 16:22:28 - __main__ - INFO: robot.torqueOff
2025-08-21 16:22:38 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:22:42 - __main__ - INFO: 当前关节角: [0.5278507627739604, 1.75270689378007, 1.4620754384535017, 1.2688669510862645, -3.519189238793925, -0.41788541353348263, 0.5962609901386524]
2025-08-21 16:22:42 - __main__ - INFO: 获取位姿成功:
[[ 0.30642283 -0.90783743 0.28624507 0.37152839]
[ 0.94130348 0.33372329 0.05075938 -0.05876817]
[-0.14160791 0.25388965 0.95681098 -0.11450045]
[ 0. 0. 0. 1. ]]
2025-08-21 16:22:42 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:22:42 - __main__ - INFO: ===采集第19次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/19.jpg
2025-08-21 16:22:45 - __main__ - INFO: robot.torqueOff
2025-08-21 16:22:51 - __main__ - INFO: 检测到 'q'，退出程序
2025-08-21 16:22:52 - __main__ - INFO: 相机已关闭，窗口已销毁
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/19.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_19.jpg
2025-08-21 16:23:13 - __main__ - INFO: 旋转矩阵是:
[[-0.99574619 0.06505242 0.06525112]
[-0.06118047 0.06271804 -0.9961543 ]
[-0.06889468 -0.99590895 -0.05847131]]
2025-08-21 16:23:13 - __main__ - INFO: 平移向量是:
[[-0.00932961]
[-0.17733044]
[ 0.07299168]]
2025-08-21 16:23:13 - __main__ - INFO: 四元数是：
[ 0.0013306 0.72748463 -0.68457228 0.04609919]
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-08-21 16:25:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-08-21 16:25:47 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-08-21 16:25:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-08-21 16:25:47 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-08-21 16:25:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-08-21 16:25:47 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-08-21 16:25:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-08-21 16:25:47 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-08-21 16:25:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-08-21 16:25:47 - __main__ - INFO: 读 eye_hand_data/data2025082101/19.jpg
2025-08-21 16:25:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_19.jpg
2025-08-21 16:25:47 - __main__ - INFO: 旋转矩阵是:
[[-0.99574619 0.06505242 0.06525112]
[-0.06118047 0.06271804 -0.9961543 ]
[-0.06889468 -0.99590895 -0.05847131]]
2025-08-21 16:25:47 - __main__ - INFO: 平移向量是:
[[-0.00932961]
[-0.17733044]
[ 0.07299168]]
2025-08-21 16:25:47 - __main__ - INFO: 四元数是：
[ 0.0013306 0.72748463 -0.68457228 0.04609919]
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-08-21 16:26:09 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-08-21 16:26:09 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-08-21 16:26:09 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-08-21 16:26:09 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-08-21 16:26:09 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-08-21 16:26:09 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-08-21 16:26:09 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-08-21 16:26:09 - __main__ - INFO: 旋转矩阵是:
[[-0.99486349 0.07846371 0.06395369] [[-0.99486349 0.07846371 0.06395369]
[-0.05884529 0.06577685 -0.9960977 ] [-0.05884529 0.06577685 -0.9960977 ]
[-0.08236419 -0.99474462 -0.06082177]] [-0.08236419 -0.99474462 -0.06082177]]
2025-08-21 16:26:09 - __main__ - INFO: 平移向量是: 2025-08-23 16:08:46 - __main__ - INFO: 平移向量是:
[[-0.00797824] [[-0.00797824]
[-0.17640328] [-0.17640328]
[ 0.07459845]] [ 0.07459845]]
2025-08-21 16:26:09 - __main__ - INFO: 四元数是： 2025-08-23 16:08:46 - __main__ - INFO: 四元数是：
[ 0.00673467 0.72826199 -0.68342243 0.05022845] [ 0.00673467 0.72826199 -0.68342243 0.05022845]
2025-08-21 17:18:15 - libs.auxiliary - INFO: create folder eye_hand_data/data2025082102
2025-08-21 17:18:15 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-08-21 17:18:34 - __main__ - INFO: 当前关节角: [0.0020332839403781207, 1.3408007778845155, 1.0441700907637408e-05, 0.3222679105856357, 2.752812057468044e-05, 0.001749459524797795, -0.09380919019975154]
2025-08-21 17:18:34 - __main__ - INFO: 获取位姿成功:
[[ 9.99992854e-01 -3.77482620e-03 -2.04576300e-04 3.09996779e-02]
[-1.98775400e-04 1.53703370e-03 -9.99998799e-01 -2.05056032e-01]
[ 3.77513610e-03 9.99991694e-01 1.53627240e-03 -4.49243316e-01]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
2025-08-21 17:18:34 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 17:18:34 - __main__ - INFO: ===采集第1次数据！ pose 已保存到 eye_hand_data/data2025082102/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082102/1.jpg
2025-08-21 17:18:36 - __main__ - INFO: 检测到 'q'，退出程序
2025-08-21 17:18:36 - __main__ - INFO: 相机已关闭，窗口已销毁
2025-08-21 17:19:10 - libs.auxiliary - INFO: create folder eye_hand_data/data2025082103
2025-08-21 17:19:10 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-08-21 17:19:15 - __main__ - INFO: robot.torqueOff
2025-08-21 17:19:46 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 17:19:50 - __main__ - INFO: 当前关节角: [0.8979217293602972, 1.0614919233239744, 0.02469082566441424, 0.3463407774054252, -1.1223005924279064, 0.6689039033530859, -0.09368958526208224]
2025-08-21 17:19:50 - __main__ - INFO: 获取位姿成功:
[[-0.17316325 -0.89563008 0.40970873 0.35893735]
[ 0.79070696 -0.37443718 -0.48433387 -0.30959237]
[ 0.58719417 0.24009071 0.7730197 -0.22790869]
[ 0. 0. 0. 1. ]]
2025-08-21 17:19:50 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 17:19:50 - __main__ - INFO: ===采集第1次数据！ pose 已保存到 eye_hand_data/data2025082103/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082103/1.jpg
2025-08-21 17:19:53 - __main__ - INFO: robot.torqueOff
2025-08-21 17:21:50 - __main__ - INFO: 检测到 'q'，退出程序
2025-08-21 17:21:51 - __main__ - INFO: 相机已关闭，窗口已销毁
2025-08-21 17:21:57 - libs.auxiliary - INFO: create folder eye_hand_data/data2025082104
2025-08-21 17:21:57 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-08-21 17:22:07 - __main__ - INFO: robot.torqueOff
2025-08-21 17:22:16 - __main__ - INFO: 相机已关闭，窗口已销毁
2025-08-21 18:00:33 - libs.auxiliary - INFO: create folder eye_hand_data/data2025082105
2025-08-21 18:00:33 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-08-21 18:00:45 - __main__ - INFO: 检测到 'q'，退出程序
2025-08-21 18:00:45 - __main__ - INFO: 相机已关闭，窗口已销毁
2025-08-21 18:01:14 - libs.auxiliary - INFO: create folder eye_hand_data/data2025082106
2025-08-21 18:01:14 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-08-21 18:01:32 - __main__ - INFO: 检测到 'q'，退出程序
2025-08-21 18:01:32 - __main__ - INFO: 相机已关闭，窗口已销毁

704
python/hand_eye_calibration/libs/web-log.2025-08-22.log Normal file
View File

@ -0,0 +1,704 @@
2025-08-21 09:52:37 - __main__ - INFO: 读 eye_hand_data/data2025082010/1.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/1.jpg
2025-08-21 09:54:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_1.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/2.jpg
2025-08-21 09:54:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_2.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/3.jpg
2025-08-21 09:54:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_3.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/4.jpg
2025-08-21 09:54:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_4.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/5.jpg
2025-08-21 09:54:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_5.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/6.jpg
2025-08-21 09:54:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_6.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/7.jpg
2025-08-21 09:54:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_7.jpg
2025-08-21 09:54:46 - __main__ - INFO: 读 eye_hand_data/data2025082009/8.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_8.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/9.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_9.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/10.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_10.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/11.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_11.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/12.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_12.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/13.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_13.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/14.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_14.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/15.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_15.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/16.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_16.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/17.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_17.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/18.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_18.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/19.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_19.jpg
2025-08-21 09:54:47 - __main__ - INFO: 读 eye_hand_data/data2025082009/20.jpg
2025-08-21 09:54:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_20.jpg
2025-08-21 09:54:47 - __main__ - INFO: 旋转矩阵是:
[[-0.8747942 0.22946188 0.42671109]
[-0.45426317 -0.08221912 -0.88706538]
[-0.16846387 -0.96983878 0.17616089]]
2025-08-21 09:54:47 - __main__ - INFO: 平移向量是:
[[-0.12651951]
[-0.21061809]
[ 0.0697177 ]]
2025-08-21 09:54:47 - __main__ - INFO: 四元数是：
[ 0.08840819 -0.63569139 0.73026951 -0.234066 ]
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/1.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_1.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/2.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_2.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/3.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_3.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/4.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_4.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/5.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_5.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/6.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_6.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/7.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_7.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/8.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_8.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/9.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_9.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/10.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_10.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/11.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_11.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/12.jpg
2025-08-21 09:55:21 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_12.jpg
2025-08-21 09:55:21 - __main__ - INFO: 读 eye_hand_data/data2025082009/13.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_13.jpg
2025-08-21 09:55:22 - __main__ - INFO: 读 eye_hand_data/data2025082009/14.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_14.jpg
2025-08-21 09:55:22 - __main__ - INFO: 读 eye_hand_data/data2025082009/15.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_15.jpg
2025-08-21 09:55:22 - __main__ - INFO: 读 eye_hand_data/data2025082009/16.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_16.jpg
2025-08-21 09:55:22 - __main__ - INFO: 读 eye_hand_data/data2025082009/17.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_17.jpg
2025-08-21 09:55:22 - __main__ - INFO: 读 eye_hand_data/data2025082009/18.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_18.jpg
2025-08-21 09:55:22 - __main__ - INFO: 读 eye_hand_data/data2025082009/19.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_19.jpg
2025-08-21 09:55:22 - __main__ - INFO: 读 eye_hand_data/data2025082009/20.jpg
2025-08-21 09:55:22 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082009/corner/corner_20.jpg
2025-08-21 09:55:22 - __main__ - INFO: 旋转矩阵是:
[[-0.87479368 0.22946254 0.4267118 ]
[-0.45426401 -0.08221905 -0.88706496]
[-0.16846434 -0.96983863 0.17616128]]
2025-08-21 09:55:22 - __main__ - INFO: 平移向量是:
[[-0.12651976]
[-0.21061824]
[ 0.0697178 ]]
2025-08-21 09:55:22 - __main__ - INFO: 四元数是：
[ 0.08840828 -0.63569123 0.73026947 -0.23406652]
2025-08-21 11:27:09 - libs.auxiliary - INFO: create folder eye_hand_data/data20250821
2025-08-21 11:27:09 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-08-21 11:27:20 - __main__ - INFO: robot.torqueOff
2025-08-21 11:28:46 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:28:51 - __main__ - INFO: 当前关节角: [0.8253898778736996, 0.2562934472690343, -0.0472638845356522, 0.982365663091435, -0.811866925952772, 1.0474525818308873, 0.016616543126199347]
2025-08-21 11:28:51 - __main__ - INFO: 获取位姿成功:
[[-0.25318109 -0.89031678 0.37846449 0.20513955]
[ 0.6294954 -0.44867523 -0.63437061 -0.47519781]
[ 0.73459844 0.07763101 0.6740464 -0.14564497]
[ 0. 0. 0. 1. ]]
2025-08-21 11:28:51 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:28:51 - __main__ - INFO: ===采集第1次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/1.jpg
2025-08-21 11:28:53 - __main__ - INFO: robot.torqueOff
2025-08-21 11:29:00 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:29:04 - __main__ - INFO: 当前关节角: [0.8917611258247912, 0.3193556252779419, 0.0663351766206834, 0.6812782681741733, -1.1216997200029486, 1.03807878215244, 0.016623187844958753]
2025-08-21 11:29:04 - __main__ - INFO: 获取位姿成功:
[[-0.25224294 -0.90609294 0.33966025 0.22289984]
[ 0.8456558 -0.37703157 -0.37777436 -0.51750052]
[ 0.47036131 0.19194474 0.8613463 -0.11653364]
[ 0. 0. 0. 1. ]]
2025-08-21 11:29:04 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:29:04 - __main__ - INFO: ===采集第2次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/2.jpg
2025-08-21 11:29:05 - __main__ - INFO: robot.torqueOff
2025-08-21 11:29:14 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:29:18 - __main__ - INFO: 当前关节角: [1.1896438678089452, 0.30651423165262204, 0.10917083072596027, 0.6814301474601026, -1.6521096487448172, 1.0335860030255448, 0.016608949161902885]
2025-08-21 11:29:18 - __main__ - INFO: 获取位姿成功:
[[-0.42170444 -0.84445498 0.33024408 0.2558707 ]
[ 0.9003429 -0.43313682 0.04213264 -0.51967065]
[ 0.10746175 0.31510043 0.94295477 -0.04397596]
[ 0. 0. 0. 1. ]]
2025-08-21 11:29:18 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:29:18 - __main__ - INFO: ===采集第3次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/3.jpg
2025-08-21 11:29:19 - __main__ - INFO: robot.torqueOff
2025-08-21 11:29:28 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:29:34 - __main__ - INFO: 当前关节角: [1.4559585002127002, 0.30681324399679527, 0.17970546960258804, 0.6812127702321163, -2.1080019043365628, 1.0236910675472528, 0.016632680300329335]
2025-08-21 11:29:34 - __main__ - INFO: 获取位姿成功:
[[-0.44997072 -0.85803562 0.24759084 0.26886029]
[ 0.85847564 -0.33920789 0.38465255 -0.52136803]
[-0.24606082 0.38563309 0.8892363 0.03026231]
[ 0. 0. 0. 1. ]]
2025-08-21 11:29:34 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:29:34 - __main__ - INFO: ===采集第4次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/4.jpg
2025-08-21 11:29:36 - __main__ - INFO: robot.torqueOff
2025-08-21 11:29:46 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:29:51 - __main__ - INFO: 当前关节角: [1.6850703015280803, 0.3357206183368209, 0.14558294028196603, 0.7301084078459711, -2.380654649191254, 1.0414419590902364, 0.016624137090495813]
2025-08-21 11:29:51 - __main__ - INFO: 获取位姿成功:
[[-0.48372144 -0.85725737 0.17641818 0.27941969]
[ 0.74091476 -0.29378779 0.60393216 -0.50585554]
[-0.46589579 0.42284577 0.77726609 0.08600871]
[ 0. 0. 0. 1. ]]
2025-08-21 11:29:51 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:29:51 - __main__ - INFO: ===采集第5次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/5.jpg
2025-08-21 11:29:53 - __main__ - INFO: robot.torqueOff
2025-08-21 11:30:03 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:30:07 - __main__ - INFO: 当前关节角: [1.9511694551949232, 0.7207327096764498, -0.06620038375442117, 0.8133790740933053, -1.3808665335126573, 0.21355366696300027, 1.1411364716197472]
2025-08-21 11:30:07 - __main__ - INFO: 获取位姿成功:
[[-0.21216612 -0.68156647 0.70032327 0.38204138]
[ 0.97416126 -0.20429083 0.09630729 -0.3389997 ]
[ 0.07742981 0.70266095 0.70729924 0.18673545]
[ 0. 0. 0. 1. ]]
2025-08-21 11:30:07 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:30:07 - __main__ - INFO: ===采集第6次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/6.jpg
2025-08-21 11:30:08 - __main__ - INFO: robot.torqueOff
2025-08-21 11:30:17 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:30:21 - __main__ - INFO: 当前关节角: [1.8940381633015537, 0.4051379952163314, -0.12349589512570175, 0.6297399309851491, -1.2283436591092607, 0.9395015316200465, 1.0908577832583988]
2025-08-21 11:30:21 - __main__ - INFO: 获取位姿成功:
[[-0.4310179 -0.70629928 0.5615736 0.28826997]
[ 0.89989504 -0.38227375 0.20989447 -0.50135244]
[ 0.06642653 0.59582557 0.80036205 0.12584165]
[ 0. 0. 0. 1. ]]
2025-08-21 11:30:21 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:30:21 - __main__ - INFO: ===采集第7次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/7.jpg
2025-08-21 11:30:22 - __main__ - INFO: robot.torqueOff
2025-08-21 11:30:35 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:30:42 - __main__ - INFO: 当前关节角: [1.8361265915767224, 1.1135570917860655, -0.08976350572081057, 0.5308864500014717, -1.5194516356954322, 0.07328175546087345, 0.9682465849732351]
2025-08-21 11:30:42 - __main__ - INFO: 获取位姿成功:
[[-0.13468827 -0.72882711 0.67131968 0.45481058]
[ 0.98906271 -0.05778144 0.13570654 -0.23383011]
[-0.06011679 0.68225534 0.7286382 0.15317249]
[ 0. 0. 0. 1. ]]
2025-08-21 11:30:42 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:30:42 - __main__ - INFO: ===采集第8次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/8.jpg
2025-08-21 11:30:43 - __main__ - INFO: robot.torqueOff
2025-08-21 11:31:04 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:31:08 - __main__ - INFO: 当前关节角: [1.773997521930743, 0.605477215057948, 0.028329283807957347, 1.2173817716473154, -1.7268484961050745, 0.15453242720534835, 0.8873870023900281]
2025-08-21 11:31:08 - __main__ - INFO: 获取位姿成功:
[[-0.35110132 -0.76965787 0.53324913 0.36358769]
[ 0.90927708 -0.14433419 0.39036245 -0.28723159]
[-0.22347945 0.62192798 0.75050818 0.11913745]
[ 0. 0. 0. 1. ]]
2025-08-21 11:31:08 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:31:08 - __main__ - INFO: ===采集第9次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/9.jpg
2025-08-21 11:31:09 - __main__ - INFO: robot.torqueOff
2025-08-21 11:31:26 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:31:30 - __main__ - INFO: 当前关节角: [1.4005130683931457, 1.4388303137420266, 0.15982067409229817, 0.8202088957324373, -1.891621384164204, -1.0446542059876405, 0.5873542192644378]
2025-08-21 11:31:30 - __main__ - INFO: 获取位姿成功:
[[ 0.36091887 -0.80430791 0.47204487 0.44921615]
[ 0.93025175 0.34636313 -0.12109612 -0.02012862]
[-0.06610037 0.48282644 0.87321782 -0.00595984]
[ 0. 0. 0. 1. ]]
2025-08-21 11:31:30 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:31:30 - __main__ - INFO: ===采集第10次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/10.jpg
2025-08-21 11:31:32 - __main__ - INFO: robot.torqueOff
2025-08-21 11:31:51 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:31:56 - __main__ - INFO: robot.torqueOff
2025-08-21 11:32:05 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:32:09 - __main__ - INFO: 当前关节角: [1.4349839708258678, 0.8371567255510698, -0.01699624134102253, 1.6177536050220789, -2.016514568720455, -0.8374870629979659, 0.5931132919377683]
2025-08-21 11:32:09 - __main__ - INFO: 获取位姿成功:
[[-0.0401021 -0.92276134 0.3832797 0.33858263]
[ 0.95373667 0.07903515 0.29006864 -0.11724388]
[-0.2979567 0.37718027 0.87690185 -0.01763812]
[ 0. 0. 0. 1. ]]
2025-08-21 11:32:09 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:32:09 - __main__ - INFO: ===采集第11次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/11.jpg
2025-08-21 11:32:12 - __main__ - INFO: robot.torqueOff
2025-08-21 11:32:48 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:32:52 - __main__ - INFO: 当前关节角: [0.7093417632317545, 0.9039997485340793, 0.3698402999208317, 1.4349972602633867, -1.868800572207794, -0.677916040481914, -0.3356152520822075]
2025-08-21 11:32:52 - __main__ - INFO: 获取位姿成功:
[[ 6.72744280e-03 -9.95813278e-01 9.11628002e-02 3.05739589e-01]
[ 9.97824272e-01 7.05770400e-04 -6.59259053e-02 -1.35775630e-01]
[ 6.55855519e-02 9.14079675e-02 9.93651407e-01 -1.75063848e-01]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
2025-08-21 11:32:52 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:32:52 - __main__ - INFO: ===采集第12次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/12.jpg
2025-08-21 11:32:53 - __main__ - INFO: robot.torqueOff
2025-08-21 11:34:07 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:34:12 - __main__ - INFO: 当前关节角: [0.5287221701769795, 0.9609924505790383, 0.5908911081355157, 1.1136415746388637, -2.4692230547809086, -0.6455401229494787, -0.3356162013277445]
2025-08-21 11:34:12 - __main__ - INFO: 获取位姿成功:
[[ 0.12520332 -0.99186645 -0.02291465 0.31366201]
[ 0.90598349 0.10488766 0.41011278 -0.19783819]
[-0.40437364 -0.07210778 0.91174691 -0.25384818]
[ 0. 0. 0. 1. ]]
2025-08-21 11:34:12 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:34:12 - __main__ - INFO: ===采集第13次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/13.jpg
2025-08-21 11:34:13 - __main__ - INFO: robot.torqueOff
2025-08-21 11:34:31 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 11:34:37 - __main__ - INFO: 当前关节角: [0.39919666739988574, 1.3179505393164566, 0.9448201543641821, 1.2761780402126846, -2.03749479358051, -0.385342428786525, -0.3356180998188186]
2025-08-21 11:34:37 - __main__ - INFO: 获取位姿成功:
[[ 0.03301101 -0.99908595 0.02715761 0.313764 ]
[ 0.83829597 0.01288215 -0.54506323 -0.0913168 ]
[ 0.54421516 0.0407592 0.83795498 -0.18631875]
[ 0. 0. 0. 1. ]]
2025-08-21 11:34:37 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 11:34:37 - __main__ - INFO: ===采集第14次数据！ pose 已保存到 eye_hand_data/data20250821/RobotToolPose.csv, 图片保存到 eye_hand_data/data20250821/14.jpg
2025-08-21 11:34:39 - __main__ - INFO: robot.torqueOff
2025-08-21 11:35:06 - __main__ - INFO: 检测到 'q'，退出程序
2025-08-21 11:35:07 - __main__ - INFO: 相机已关闭，窗口已销毁
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/1.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_1.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/2.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_2.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/3.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_3.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/4.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_4.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/5.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_5.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/6.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_6.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/7.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_7.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/8.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_8.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/9.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_9.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/10.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_10.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/11.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_11.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/12.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_12.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/13.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_13.jpg
2025-08-21 11:35:46 - __main__ - INFO: 读 eye_hand_data/data20250821/14.jpg
2025-08-21 11:35:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_14.jpg
2025-08-21 11:35:46 - __main__ - INFO: 旋转矩阵是:
[[-0.99732744 0.02734315 0.0677519 ]
[-0.06713675 0.0228221 -0.99748274]
[-0.02882056 -0.99936555 -0.02092538]]
2025-08-21 11:35:46 - __main__ - INFO: 平移向量是:
[[-0.00078482]
[-0.17419722]
[ 0.06796275]]
2025-08-21 11:35:46 - __main__ - INFO: 四元数是：
[-0.01392688 0.71433097 -0.69885263 0.03379822]
2025-08-21 11:38:53 - __main__ - INFO: 读 eye_hand_data/data20250821/1.jpg
2025-08-21 11:38:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_1.jpg
2025-08-21 11:38:53 - __main__ - INFO: 读 eye_hand_data/data20250821/2.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_2.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/3.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_3.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/4.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_4.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/5.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_5.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/6.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_6.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/7.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_7.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/8.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_8.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/9.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_9.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/10.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_10.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/11.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_11.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/12.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_12.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/13.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_13.jpg
2025-08-21 11:38:54 - __main__ - INFO: 读 eye_hand_data/data20250821/14.jpg
2025-08-21 11:38:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data20250821/corner/corner_14.jpg
2025-08-21 11:38:54 - __main__ - INFO: 旋转矩阵是:
[[-0.99732744 0.02734315 0.0677519 ]
[-0.06713675 0.0228221 -0.99748274]
[-0.02882056 -0.99936555 -0.02092538]]
2025-08-21 11:38:54 - __main__ - INFO: 平移向量是:
[[-0.00078482]
[-0.17419722]
[ 0.06796275]]
2025-08-21 11:38:54 - __main__ - INFO: 四元数是：
[-0.01392688 0.71433097 -0.69885263 0.03379822]
2025-08-21 16:14:42 - libs.auxiliary - INFO: create folder eye_hand_data/data2025082101
2025-08-21 16:14:42 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-08-21 16:15:03 - __main__ - INFO: robot.torqueOff
2025-08-21 16:15:13 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:15:27 - __main__ - INFO: robot.torqueOff
2025-08-21 16:16:00 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:16:07 - __main__ - INFO: 当前关节角: [0.1881774860208302, 1.5359324367098321, 2.101302129336008, 1.237947176207676, -3.239618392465156, 0.660369236729398, -0.22447188913024077]
2025-08-21 16:16:07 - __main__ - INFO: 获取位姿成功:
[[-0.13936557 -0.98748272 -0.0738588 0.25639562]
[ 0.98789482 -0.13351676 -0.07897536 -0.27191259]
[ 0.06812542 -0.08397117 0.9941367 -0.23921664]
[ 0. 0. 0. 1. ]]
2025-08-21 16:16:07 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:16:07 - __main__ - INFO: ===采集第1次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/1.jpg
2025-08-21 16:16:11 - __main__ - INFO: robot.torqueOff
2025-08-21 16:16:20 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:16:25 - __main__ - INFO: 当前关节角: [0.1847488111409769, 1.3553365747934838, 2.1088087630430623, 1.2244802297734347, -3.265897305913068, 0.6804733079587482, -0.22435892891133088]
2025-08-21 16:16:25 - __main__ - INFO: 获取位姿成功:
[[-0.12214152 -0.99148995 -0.04504583 0.24935823]
[ 0.98590508 -0.12643169 0.10957282 -0.3301866 ]
[-0.11433557 -0.03102752 0.99295754 -0.22048319]
[ 0. 0. 0. 1. ]]
2025-08-21 16:16:25 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:16:25 - __main__ - INFO: ===采集第2次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/2.jpg
2025-08-21 16:16:28 - __main__ - INFO: robot.torqueOff
2025-08-21 16:16:36 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:16:40 - __main__ - INFO: 当前关节角: [0.18216591403464222, 1.3093807506078976, 2.108801169078766, 1.237752580872579, -3.386807456195824, 0.674951546669682, -0.22420894811647574]
2025-08-21 16:16:40 - __main__ - INFO: 获取位姿成功:
[[-0.12765968 -0.99174931 0.01167535 0.24725332]
[ 0.95778442 -0.12021331 0.26114702 -0.34440277]
[-0.25758884 0.04452042 0.96522843 -0.21331776]
[ 0. 0. 0. 1. ]]
2025-08-21 16:16:40 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:16:40 - __main__ - INFO: ===采集第3次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/3.jpg
2025-08-21 16:16:43 - __main__ - INFO: robot.torqueOff
2025-08-21 16:16:52 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:16:57 - __main__ - INFO: 当前关节角: [0.15805412814783334, 1.218028207858052, 2.1077446587960202, 1.3528049369461503, -3.398763203735069, 0.6851996014877597, -0.2241823692414381]
2025-08-21 16:16:57 - __main__ - INFO: 获取位姿成功:
[[-0.12061637 -0.99148511 -0.04908122 0.24079104]
[ 0.90170295 -0.13010493 0.41231601 -0.36562399]
[-0.41519089 0.00547538 0.90971784 -0.17673377]
[ 0. 0. 0. 1. ]]
2025-08-21 16:16:57 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:16:57 - __main__ - INFO: ===采集第4次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/4.jpg
2025-08-21 16:16:59 - __main__ - INFO: robot.torqueOff
2025-08-21 16:17:08 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:17:14 - __main__ - INFO: 当前关节角: [0.15782630921893942, 1.148865228782473, 2.0802051472748952, 1.4294517678358942, -3.4739330085691504, 0.6850638593759604, -0.22417762301375282]
2025-08-21 16:17:14 - __main__ - INFO: 获取位姿成功:
[[-0.13323657 -0.98863605 -0.06961879 0.24168424]
[ 0.81161017 -0.14915447 0.56483792 -0.37307124]
[-0.56880309 0.01875375 0.8222599 -0.14946229]
[ 0. 0. 0. 1. ]]
2025-08-21 16:17:14 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:17:14 - __main__ - INFO: ===采集第5次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/5.jpg
2025-08-21 16:17:21 - __main__ - INFO: robot.torqueOff
2025-08-21 16:17:38 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:17:43 - __main__ - INFO: 当前关节角: [0.3590416826912605, 1.1394164387065981, 2.0769226562077487, 1.5773176436344847, -2.5660726784358565, 0.5330820549286868, 0.8458689010901876]
2025-08-21 16:17:43 - __main__ - INFO: 获取位姿成功:
[[-0.1100018 -0.97171944 0.20895198 0.26213488]
[ 0.9937792 -0.11120581 0.00601407 -0.35469134]
[ 0.01739269 0.20831369 0.97790741 -0.04493756]
[ 0. 0. 0. 1. ]]
2025-08-21 16:17:43 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:17:43 - __main__ - INFO: ===采集第6次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/6.jpg
2025-08-21 16:17:46 - __main__ - INFO: robot.torqueOff
2025-08-21 16:17:53 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:17:59 - __main__ - INFO: 当前关节角: [0.3520818144135517, 1.1556029736045101, 2.017053740185504, 1.6291274650471073, -2.566324228503177, 0.5650403044248166, 0.8458594086348171]
2025-08-21 16:17:59 - __main__ - INFO: 获取位姿成功:
[[-0.20049553 -0.96061353 0.19241411 0.26384736]
[ 0.97904311 -0.18929837 0.07510466 -0.33478381]
[-0.03572288 0.20343986 0.97843554 -0.04338287]
[ 0. 0. 0. 1. ]]
2025-08-21 16:17:59 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:17:59 - __main__ - INFO: ===采集第7次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/7.jpg
2025-08-21 16:18:01 - __main__ - INFO: robot.torqueOff
2025-08-21 16:18:09 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:18:13 - __main__ - INFO: 当前关节角: [0.3172653866053403, 1.313845052368681, 1.8904177408232146, 1.7188083864061938, -2.6905567381656357, 0.28883548277045484, 0.8458527639160577]
2025-08-21 16:18:13 - __main__ - INFO: 获取位姿成功:
[[-0.09067794 -0.96156996 0.25915385 0.27763283]
[ 0.99082305 -0.1133011 -0.0737058 -0.27031831]
[ 0.1002357 0.25009212 0.96301959 -0.05935151]
[ 0. 0. 0. 1. ]]
2025-08-21 16:18:13 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:18:13 - __main__ - INFO: ===采集第8次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/8.jpg
2025-08-21 16:18:16 - __main__ - INFO: robot.torqueOff
2025-08-21 16:18:25 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:18:30 - __main__ - INFO: 当前关节角: [0.3113658255925252, 1.3320734144168047, 1.7972094230484208, 1.807990953858325, -2.9523187889736606, 0.29634021798643495, 0.8458480176883724]
2025-08-21 16:18:30 - __main__ - INFO: 获取位姿成功:
[[-0.06182316 -0.96333913 0.26106631 0.27572117]
[ 0.985666 -0.01779069 0.16776779 -0.23950089]
[-0.15697273 0.26769612 0.9506305 -0.05533822]
[ 0. 0. 0. 1. ]]
2025-08-21 16:18:30 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:18:30 - __main__ - INFO: ===采集第9次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/9.jpg
2025-08-21 16:18:33 - __main__ - INFO: robot.torqueOff
2025-08-21 16:18:44 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:18:49 - __main__ - INFO: 当前关节角: [0.3174068241903619, 1.4675260063272884, 1.5487007384287987, 1.7904982571014212, -3.2589716104746933, 0.07981446324690623, 0.8458337790053165]
2025-08-21 16:18:49 - __main__ - INFO: 获取位姿成功:
[[ 0.00204587 -0.95045805 0.31084613 0.291817 ]
[ 0.96975598 0.0777536 0.2313606 -0.15674434]
[-0.24406795 0.30097156 0.92187144 -0.07498582]
[ 0. 0. 0. 1. ]]
2025-08-21 16:18:49 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:18:49 - __main__ - INFO: ===采集第10次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/10.jpg
2025-08-21 16:18:52 - __main__ - INFO: robot.torqueOff
2025-08-21 16:19:08 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:19:14 - __main__ - INFO: robot.torqueOff
2025-08-21 16:19:25 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:19:30 - __main__ - INFO: 当前关节角: [0.27753946087946524, 1.6105355419582903, 1.2276915274253017, 1.6875801067280616, -3.5108406242955006, -0.6567573574608925, 0.8458072001302789]
2025-08-21 16:19:30 - __main__ - INFO: 获取位姿成功:
[[ 0.38872691 -0.8780319 0.27919772 0.29523427]
[ 0.91848265 0.39319638 -0.0422638 -0.05153552]
[-0.07267057 0.27286734 0.95930308 -0.10555432]
[ 0. 0. 0. 1. ]]
2025-08-21 16:19:30 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:19:30 - __main__ - INFO: ===采集第11次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/11.jpg
2025-08-21 16:19:33 - __main__ - INFO: robot.torqueOff
2025-08-21 16:20:00 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:20:06 - __main__ - INFO: 当前关节角: [0.19953615811726558, 1.5134979676870048, 1.3755934745543004, 1.2340486247869789, -3.235004109909517, -0.39607934505618747, -0.18381760126912305]
2025-08-21 16:20:06 - __main__ - INFO: 获取位姿成功:
[[ 0.2004248 -0.97752505 -0.065381 0.29826073]
[ 0.95269085 0.17889858 0.24571414 -0.11593768]
[-0.22849516 -0.11153509 0.96713489 -0.24686201]
[ 0. 0. 0. 1. ]]
2025-08-21 16:20:06 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:20:06 - __main__ - INFO: ===采集第12次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/12.jpg
2025-08-21 16:20:08 - __main__ - INFO: robot.torqueOff
2025-08-21 16:20:19 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:20:24 - __main__ - INFO: 当前关节角: [0.14909894575122867, 1.5289374463472523, 1.327669864370393, 1.130913097185633, -3.1925177781618777, -0.44474526525007424, -0.2899508462764979]
2025-08-21 16:20:24 - __main__ - INFO: 获取位姿成功:
[[ 0.1795737 -0.98355917 -0.01909571 0.27480921]
[ 0.95378635 0.16931837 0.24823956 -0.10309542]
[-0.24092504 -0.06279052 0.96851044 -0.28676362]
[ 0. 0. 0. 1. ]]
2025-08-21 16:20:24 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:20:24 - __main__ - INFO: ===采集第13次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/13.jpg
2025-08-21 16:20:26 - __main__ - INFO: robot.torqueOff
2025-08-21 16:20:38 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:20:43 - __main__ - INFO: 当前关节角: [0.16214537641255308, 1.3656235486785808, 1.5120835918367883, 1.2035123451053618, -2.9271913103622, -0.014777854520918107, -0.2900476693212778]
2025-08-21 16:20:43 - __main__ - INFO: 获取位姿成功:
[[ 0.03397924 -0.99615678 -0.08072841 0.28406157]
[ 0.99940914 0.03344943 0.00790666 -0.19681757]
[-0.00517596 -0.08094937 0.99670478 -0.25137906]
[ 0. 0. 0. 1. ]]
2025-08-21 16:20:43 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:20:43 - __main__ - INFO: ===采集第14次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/14.jpg
2025-08-21 16:20:45 - __main__ - INFO: robot.torqueOff
2025-08-21 16:21:17 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:21:26 - __main__ - INFO: 当前关节角: [0.23122007565318572, 1.628535135831983, 1.828089329614453, 1.4320631423083405, -3.0376977295588006, 0.19928270955887112, 0.14386290736881704]
2025-08-21 16:21:26 - __main__ - INFO: 获取位姿成功:
[[ 0.02777164 -0.99933259 0.02373016 0.29887982]
[ 0.98010716 0.02255505 -0.19718324 -0.18783112]
[ 0.1965164 0.0287342 0.98007941 -0.17737878]
[ 0. 0. 0. 1. ]]
2025-08-21 16:21:26 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:21:26 - __main__ - INFO: ===采集第15次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/15.jpg
2025-08-21 16:21:29 - __main__ - INFO: robot.torqueOff
2025-08-21 16:21:42 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:21:47 - __main__ - INFO: 当前关节角: [0.22977437470024648, 1.339187060471517, 2.1365172402697836, 1.3177511977536744, -3.033028390762013, 0.8488428873577902, 0.1439046741724476]
2025-08-21 16:21:47 - __main__ - INFO: 获取位姿成功:
[[-0.24782493 -0.96680534 0.06221126 0.25321104]
[ 0.96857382 -0.24585225 0.03770167 -0.33940743]
[-0.02115539 0.06959961 0.99735066 -0.17940936]
[ 0. 0. 0. 1. ]]
2025-08-21 16:21:47 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:21:47 - __main__ - INFO: ===采集第16次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/16.jpg
2025-08-21 16:21:50 - __main__ - INFO: robot.torqueOff
2025-08-21 16:22:00 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:22:05 - __main__ - INFO: 当前关节角: [0.2123139522916026, 1.6156225487913838, 1.9870955510359554, 1.1415721753212567, -3.261262139955614, 0.40399890057186194, -0.2904045856432116]
2025-08-21 16:22:05 - __main__ - INFO: 获取位姿成功:
[[-0.02238853 -0.99787522 -0.06118662 0.27706052]
[ 0.9941035 -0.01572528 -0.10728906 -0.21679508]
[ 0.10609892 -0.06322787 0.99234331 -0.26052986]
[ 0. 0. 0. 1. ]]
2025-08-21 16:22:05 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:22:05 - __main__ - INFO: ===采集第17次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/17.jpg
2025-08-21 16:22:08 - __main__ - INFO: robot.torqueOff
2025-08-21 16:22:20 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:22:25 - __main__ - INFO: 当前关节角: [0.7920960399069291, 1.588399136034099, 1.9284805883681642, 1.3829188023637766, -3.1986537013134204, 0.2975713894479991, 1.133828230229938]
2025-08-21 16:22:25 - __main__ - INFO: 获取位姿成功:
[[ 0.02514396 -0.85857888 0.51206454 0.37379129]
[ 0.99831944 -0.00518728 -0.05771811 -0.22146041]
[ 0.05221177 0.51265524 0.85700556 -0.00260762]
[ 0. 0. 0. 1. ]]
2025-08-21 16:22:25 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:22:25 - __main__ - INFO: ===采集第18次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/18.jpg
2025-08-21 16:22:28 - __main__ - INFO: robot.torqueOff
2025-08-21 16:22:38 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 16:22:42 - __main__ - INFO: 当前关节角: [0.5278507627739604, 1.75270689378007, 1.4620754384535017, 1.2688669510862645, -3.519189238793925, -0.41788541353348263, 0.5962609901386524]
2025-08-21 16:22:42 - __main__ - INFO: 获取位姿成功:
[[ 0.30642283 -0.90783743 0.28624507 0.37152839]
[ 0.94130348 0.33372329 0.05075938 -0.05876817]
[-0.14160791 0.25388965 0.95681098 -0.11450045]
[ 0. 0. 0. 1. ]]
2025-08-21 16:22:42 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 16:22:42 - __main__ - INFO: ===采集第19次数据！ pose 已保存到 eye_hand_data/data2025082101/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082101/19.jpg
2025-08-21 16:22:45 - __main__ - INFO: robot.torqueOff
2025-08-21 16:22:51 - __main__ - INFO: 检测到 'q'，退出程序
2025-08-21 16:22:52 - __main__ - INFO: 相机已关闭，窗口已销毁
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-08-21 16:23:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-08-21 16:23:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-08-21 16:23:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/19.jpg
2025-08-21 16:23:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_19.jpg
2025-08-21 16:23:13 - __main__ - INFO: 旋转矩阵是:
[[-0.99574619 0.06505242 0.06525112]
[-0.06118047 0.06271804 -0.9961543 ]
[-0.06889468 -0.99590895 -0.05847131]]
2025-08-21 16:23:13 - __main__ - INFO: 平移向量是:
[[-0.00932961]
[-0.17733044]
[ 0.07299168]]
2025-08-21 16:23:13 - __main__ - INFO: 四元数是：
[ 0.0013306 0.72748463 -0.68457228 0.04609919]
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-08-21 16:25:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-08-21 16:25:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-08-21 16:25:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-08-21 16:25:47 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-08-21 16:25:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-08-21 16:25:47 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-08-21 16:25:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-08-21 16:25:47 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-08-21 16:25:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-08-21 16:25:47 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-08-21 16:25:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-08-21 16:25:47 - __main__ - INFO: 读 eye_hand_data/data2025082101/19.jpg
2025-08-21 16:25:47 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_19.jpg
2025-08-21 16:25:47 - __main__ - INFO: 旋转矩阵是:
[[-0.99574619 0.06505242 0.06525112]
[-0.06118047 0.06271804 -0.9961543 ]
[-0.06889468 -0.99590895 -0.05847131]]
2025-08-21 16:25:47 - __main__ - INFO: 平移向量是:
[[-0.00932961]
[-0.17733044]
[ 0.07299168]]
2025-08-21 16:25:47 - __main__ - INFO: 四元数是：
[ 0.0013306 0.72748463 -0.68457228 0.04609919]
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg
2025-08-21 16:26:08 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg
2025-08-21 16:26:08 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg
2025-08-21 16:26:09 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg
2025-08-21 16:26:09 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg
2025-08-21 16:26:09 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg
2025-08-21 16:26:09 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg
2025-08-21 16:26:09 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg
2025-08-21 16:26:09 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg
2025-08-21 16:26:09 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg
2025-08-21 16:26:09 - __main__ - INFO: 旋转矩阵是:
[[-0.99486349 0.07846371 0.06395369]
[-0.05884529 0.06577685 -0.9960977 ]
[-0.08236419 -0.99474462 -0.06082177]]
2025-08-21 16:26:09 - __main__ - INFO: 平移向量是:
[[-0.00797824]
[-0.17640328]
[ 0.07459845]]
2025-08-21 16:26:09 - __main__ - INFO: 四元数是：
[ 0.00673467 0.72826199 -0.68342243 0.05022845]
2025-08-21 17:18:15 - libs.auxiliary - INFO: create folder eye_hand_data/data2025082102
2025-08-21 17:18:15 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-08-21 17:18:34 - __main__ - INFO: 当前关节角: [0.0020332839403781207, 1.3408007778845155, 1.0441700907637408e-05, 0.3222679105856357, 2.752812057468044e-05, 0.001749459524797795, -0.09380919019975154]
2025-08-21 17:18:34 - __main__ - INFO: 获取位姿成功:
[[ 9.99992854e-01 -3.77482620e-03 -2.04576300e-04 3.09996779e-02]
[-1.98775400e-04 1.53703370e-03 -9.99998799e-01 -2.05056032e-01]
[ 3.77513610e-03 9.99991694e-01 1.53627240e-03 -4.49243316e-01]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
2025-08-21 17:18:34 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 17:18:34 - __main__ - INFO: ===采集第1次数据！ pose 已保存到 eye_hand_data/data2025082102/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082102/1.jpg
2025-08-21 17:18:36 - __main__ - INFO: 检测到 'q'，退出程序
2025-08-21 17:18:36 - __main__ - INFO: 相机已关闭，窗口已销毁
2025-08-21 17:19:10 - libs.auxiliary - INFO: create folder eye_hand_data/data2025082103
2025-08-21 17:19:10 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-08-21 17:19:15 - __main__ - INFO: robot.torqueOff
2025-08-21 17:19:46 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人
2025-08-21 17:19:50 - __main__ - INFO: 当前关节角: [0.8979217293602972, 1.0614919233239744, 0.02469082566441424, 0.3463407774054252, -1.1223005924279064, 0.6689039033530859, -0.09368958526208224]
2025-08-21 17:19:50 - __main__ - INFO: 获取位姿成功:
[[-0.17316325 -0.89563008 0.40970873 0.35893735]
[ 0.79070696 -0.37443718 -0.48433387 -0.30959237]
[ 0.58719417 0.24009071 0.7730197 -0.22790869]
[ 0. 0. 0. 1. ]]
2025-08-21 17:19:50 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿
2025-08-21 17:19:50 - __main__ - INFO: ===采集第1次数据！ pose 已保存到 eye_hand_data/data2025082103/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025082103/1.jpg
2025-08-21 17:19:53 - __main__ - INFO: robot.torqueOff
2025-08-21 17:21:50 - __main__ - INFO: 检测到 'q'，退出程序
2025-08-21 17:21:51 - __main__ - INFO: 相机已关闭，窗口已销毁
2025-08-21 17:21:57 - libs.auxiliary - INFO: create folder eye_hand_data/data2025082104
2025-08-21 17:21:57 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-08-21 17:22:07 - __main__ - INFO: robot.torqueOff
2025-08-21 17:22:16 - __main__ - INFO: 相机已关闭，窗口已销毁
2025-08-21 18:00:33 - libs.auxiliary - INFO: create folder eye_hand_data/data2025082105
2025-08-21 18:00:33 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-08-21 18:00:45 - __main__ - INFO: 检测到 'q'，退出程序
2025-08-21 18:00:45 - __main__ - INFO: 相机已关闭，窗口已销毁
2025-08-21 18:01:14 - libs.auxiliary - INFO: create folder eye_hand_data/data2025082106
2025-08-21 18:01:14 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0
2025-08-21 18:01:32 - __main__ - INFO: 检测到 'q'，退出程序
2025-08-21 18:01:32 - __main__ - INFO: 相机已关闭，窗口已销毁

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

Binary file not shown.

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

Binary file not shown.

69
python/vision_servo/robot_warpper_test.py
View File

@ -1,8 +1,9 @@
import sys import sys
import os
os.environ["GLOG_minloglevel"] = "1" # 屏蔽 INFO
# from python.vision_servo.follow_target import moveJ
import time import time
from python.vision_servo.follow_target import moveJ
# 把.so所在目录加入 Python 路径 # 把.so所在目录加入 Python 路径
sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example") sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example")
@ -15,42 +16,42 @@ robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01")
JOINT_POSITIONS = [ # JOINT_POSITIONS = [
[-0.1756037771702, 1.1800090074539, 1.8522456884384, 1.5761110782623, -2.7331359386444, 0.2563314139843, -0.0606026798487], # [-0.1756037771702, 1.1800090074539, 1.8522456884384, 1.5761110782623, -2.7331359386444, 0.2563314139843, -0.0606026798487],
[-0.1923076510429, 1.1099290847778, 1.8832376003265, 1.7514026165009, -2.6219944953918, 0.1934144645929, 0.2889180481434], # [-0.1923076510429, 1.1099290847778, 1.8832376003265, 1.7514026165009, -2.6219944953918, 0.1934144645929, 0.2889180481434],
[0.2704087197781, 0.9557962417603, 1.7136577367783, 1.6711657047272, -2.5147924423218, 0.0584972538054, 0.783962905407], # [0.2704087197781, 0.9557962417603, 1.7136577367783, 1.6711657047272, -2.5147924423218, 0.0584972538054, 0.783962905407],
[0.383531242609, 0.7108500599861, 1.7350907325745, 1.8691725730896, -2.2797479629517, -0.1232690215111, 1.2245988845825], # [0.383531242609, 0.7108500599861, 1.7350907325745, 1.8691725730896, -2.2797479629517, -0.1232690215111, 1.2245988845825],
[0.3922453224659, 0.7737803459167, 1.8068689107895, 1.6336191892624, -2.1118586063385, -0.0338453501463, 1.1426001787186], # [0.3922453224659, 0.7737803459167, 1.8068689107895, 1.6336191892624, -2.1118586063385, -0.0338453501463, 1.1426001787186],
[0.2443187087774, 0.7818641066551, 1.9731577634811, 1.5233805179596, -1.872123837471, 0.0405574627221, 1.1054704189301], # [0.2443187087774, 0.7818641066551, 1.9731577634811, 1.5233805179596, -1.872123837471, 0.0405574627221, 1.1054704189301],
[0.1276659220457, 0.9156659245491, 2.0081288814545, 1.4481414556503, -1.8519257307053, 0.2532368600368, 0.8394842743874], # [0.1276659220457, 0.9156659245491, 2.0081288814545, 1.4481414556503, -1.8519257307053, 0.2532368600368, 0.8394842743874],
[0.1861536949873, 1.0141937732697, 1.8355123996735, 1.1459348201752, -2.2625010013580, 0.5969776511192, 0.2645072638988], # [0.1861536949873, 1.0141937732697, 1.8355123996735, 1.1459348201752, -2.2625010013580, 0.5969776511192, 0.2645072638988],
[-0.0192848723382, 0.9415585398674, 1.8732609748840, 1.1809715032578, -2.6499755382538, 0.6354467868805, -0.2003904730082], # [-0.0192848723382, 0.9415585398674, 1.8732609748840, 1.1809715032578, -2.6499755382538, 0.6354467868805, -0.2003904730082],
[-0.1747295111418, 1.0453623533249, 1.8939573764801, 1.4446462392807, -2.6536157131195, 0.5147834420204, -0.1335265636444], # [-0.1747295111418, 1.0453623533249, 1.8939573764801, 1.4446462392807, -2.6536157131195, 0.5147834420204, -0.1335265636444],
[-0.0191576723009, 1.0618592500687, 1.8731262683868, 1.4747705459595, -2.4312825202942, 0.3830471336842, 0.2654716968536], # [-0.0191576723009, 1.0618592500687, 1.8731262683868, 1.4747705459595, -2.4312825202942, 0.3830471336842, 0.2654716968536],
[0.1436645090580, 0.8961057662964, 1.9278227090836, 1.5483351945877, -2.1520030498505, 0.2300240248442, 0.7717508077621], # [0.1436645090580, 0.8961057662964, 1.9278227090836, 1.5483351945877, -2.1520030498505, 0.2300240248442, 0.7717508077621],
[-0.2458887547255, 1.3182551860809, 1.8035455942154, 1.7302685976028, -2.8433899879456, 0.0447540767491, -0.0295310281217], # [-0.2458887547255, 1.3182551860809, 1.8035455942154, 1.7302685976028, -2.8433899879456, 0.0447540767491, -0.0295310281217],
[-0.1628440171480, 1.1346882581711, 1.7724473476410, 1.8686875104904, -2.8621222972870, 0.0011371960863, 0.3351055085659], # [-0.1628440171480, 1.1346882581711, 1.7724473476410, 1.8686875104904, -2.8621222972870, 0.0011371960863, 0.3351055085659],
[0.2227641940117, 0.9846931695938, 1.5547094345093, 1.9238842725754, -2.8571910858154, -0.1052865162492, 0.8961741328239], # [0.2227641940117, 0.9846931695938, 1.5547094345093, 1.9238842725754, -2.8571910858154, -0.1052865162492, 0.8961741328239],
[0.4523145258427, 0.7738097310066, 1.5341449975967, 1.9425786733627, -2.5897336006165, -0.1147941574454, 1.2599183320999], # [0.4523145258427, 0.7738097310066, 1.5341449975967, 1.9425786733627, -2.5897336006165, -0.1147941574454, 1.2599183320999],
[0.6323996782303, 0.7300637960434, 1.3266047239304, 2.0460503101349, -2.7566838264465, -0.1049704179168, 1.4412622451782], # [0.6323996782303, 0.7300637960434, 1.3266047239304, 2.0460503101349, -2.7566838264465, -0.1049704179168, 1.4412622451782],
[0.6435400247574, 0.7136778831482, 1.3097128868103, 2.0483627319336, -2.7511403560638, -0.1215622797608, 1.5219746828079], # [0.6435400247574, 0.7136778831482, 1.3097128868103, 2.0483627319336, -2.7511403560638, -0.1215622797608, 1.5219746828079],
[0.0131992585957, 1.1347451210022, 1.4958723783493, 1.7922105789185, -3.1400320529938, -0.2474720925093, 0.4145260155201], # [0.0131992585957, 1.1347451210022, 1.4958723783493, 1.7922105789185, -3.1400320529938, -0.2474720925093, 0.4145260155201],
[-0.1823624074459, 1.4246475696564, 1.7136197090149, 1.6952623128891, -2.9659118652344, -0.1499162465334, -0.0364842526615], # [-0.1823624074459, 1.4246475696564, 1.7136197090149, 1.6952623128891, -2.9659118652344, -0.1499162465334, -0.0364842526615],
] # ]
#
#
while True: # while True:
for j in JOINT_POSITIONS: # for j in JOINT_POSITIONS:
robot.moveJ('right', j) # robot.moveJ('right', j)
# robot.torqueOn() robot.torqueOn()
# 控制左臂关节 # 控制左臂关节
# robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
# # robot.moveJ("right", [-0.344938, 0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996]) # # robot.moveJ("right", [-0.344938, 0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996])
# # robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) # # robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0])
# # # #
# time.sleep(10) time.sleep(10)
# robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364])
robot.torqueOff() # robot.torqueOff()
# // {"R_SHOULDER_P", 0.00203898}, # // {"R_SHOULDER_P", 0.00203898},
# // {"R_SHOULDER_R", 1.34062}, # // {"R_SHOULDER_R", 1.34062},

3
src/CMakeLists.txt
View File

@ -4,4 +4,5 @@ add_subdirectory(devices)
add_subdirectory(monitor) add_subdirectory(monitor)
add_subdirectory(device_manager) add_subdirectory(device_manager)
add_subdirectory(service) add_subdirectory(service)
add_subdirectory(http) add_subdirectory(http)
add_subdirectory(controller)

58
src/controller/CMakeLists.txt Normal file
View File

@ -0,0 +1,58 @@
find_package(glog REQUIRED)
find_package(protobuf REQUIRED)
add_library(controller SHARED
${CMAKE_CURRENT_SOURCE_DIR}/touch_controller.cpp
)
target_include_directories(controller PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(controller PRIVATE
protobuf::libprotobuf
glog::glog
cmvr_es::device::humanoid_robot
)
add_library(cmvr_es::ctrl::controller ALIAS controller)
# --------------------------------------------------------
# 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(touch_controller_test
${CMAKE_CURRENT_SOURCE_DIR}/touch_controller_test.cpp
)
target_link_libraries(touch_controller_test
PRIVATE
protobuf::libprotobuf
glog::glog
cmvr_es::device::humanoid_robot
cmvr_es::ctrl::controller
gtest
gtest_main
pthread
glog::glog
proto-objects
ccd
fcl
cmvr_es::device_manager
${OpenCV_LIBS}
)

94
src/controller/touch_controller.cpp Normal file
View File

@ -0,0 +1,94 @@
//
// Created by lgv on 2025/8/24.
//
#include "controller/touch_controller.h"
using namespace cmvr::ctrl;
using namespace cmvr::msgs;
void TouchController::touch(std::shared_ptr<device::AbstractRobot> robot, const msgs::Pose3d pose, const msgs::Pose3d offset) {
msgs::Position target_position;
target_position.set_x(pose.position().x() - offset.position().x());
target_position.set_y(pose.position().y() - offset.position().y());
target_position.set_z(pose.position().z() - offset.position().z());
msgs::Euler target_euler;
target_euler.set_rx(pose.euler().rx() - offset.euler().rx());
target_euler.set_ry(pose.euler().ry() - offset.euler().ry());
target_euler.set_rz(pose.euler().rz() - offset.euler().rz());
msgs::Pose3d target_pose;
*target_pose.mutable_position() = target_position;
*target_pose.mutable_euler() = target_euler;
robot->moveJ("PELVIS_S","R_WRIST_R_S",target_pose);
}
void TouchController::touch(msgs::Pose3d pose, msgs::Pose3d offset, double max_force) {
auto hand_data = hand_->getSensorData();
double force = 0;
int x ,y;
extractTouch(hand_data.index.tip.data,force,x,y);
double dz = pid_->compute(max_force, force, 0.01,10);
std::cout << dz << std::endl;
// LOG(INFO) << "Force : " << force << " dz : " << dz;
// // 输出 3x3 数组
// LOG(INFO) << "Tip data (3x3):";
// for (size_t i = 0; i < hand_data.index.tip.data.size(); ++i) {
// std::stringstream ss;
// for (size_t j = 0; j < hand_data.index.tip.data[i].size(); ++j) {
// ss << hand_data.index.tip.data[i][j] << "\t";
// }
// LOG(INFO) << ss.str();
// }
}
bool TouchController::extractTouch(const std::vector<std::vector<uint16_t> > &matrix, double &force, int &x, int &y) {
int rows = matrix.size();
int cols = matrix[0].size();
double maxVal = 0;
int maxX = -1, maxY = -1;
double total = 0;
for (int i = 0; i < rows; i++) {
for (int j = 0; j < cols; j++) {
double val = matrix[i][j];
total += val;
if (val > maxVal) {
maxVal = val;
maxX = i;
maxY = j;
}
}
}
if (maxVal > touch_threshold_) {
force = total;
x = maxX;
y = maxY;
return true;
}
return false;
}

53
src/controller/touch_controller_test.cpp Normal file
View File

@ -0,0 +1,53 @@
//
// Created by lgv on 2025/8/27.
//
#include "device_manager/device_manager.h"
#include "gtest/gtest.h"
#include "controller/touch_controller.h"
#include "cmvr/msgs/geometry.pb.h"
using namespace cmvr::device;
using namespace cmvr::msgs;
using namespace cmvr::ctrl;
TEST(TouchControllerTest,MyTest) {
//
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");
auto hand = dmgr.getDevice<AbstractDexHand>("hand1");
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);
robot->moveJ("PELVIS_S","R_WRIST_R_S",pose);
Pose3d offset;
TouchController controller(robot, hand);
while (true) {
controller.touch(pose,offset,500);
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}

11
src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp
View File

@ -32,9 +32,10 @@ void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response,
motor_status->set_position_offset(sdo_response.data()); motor_status->set_position_offset(sdo_response.data());
} }
LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_) LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_)
<< ": command=" << int(sdo_response.cs()) << ": command=" << int(sdo_response.cs())
<< ", index=" << sdo_response.index() << ", index=" << sdo_response.index()
<< ", subindex=" << int(sdo_response.sub_index()) << ", subindex=" << int(sdo_response.sub_index())
<< ", data=" << sdo_response.data(); << ", data=" << sdo_response.data();
} }

6
src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.cpp
View File

@ -20,11 +20,15 @@ void Ti5MotorTPDO2::Parse(const std::uint8_t *bytes, int32_t length, msgs::Robot
motor_status->set_position(bytes[3] << 24 | bytes[2] << 16 | bytes[1] << 8 | bytes[0]); motor_status->set_position(bytes[3] << 24 | bytes[2] << 16 | bytes[1] << 8 | bytes[0]);
motor_status->set_speed(bytes[7] << 24 | bytes[6] << 16 | bytes[5] << 8 | bytes[4]); motor_status->set_speed(bytes[7] << 24 | bytes[6] << 16 | bytes[5] << 8 | bytes[4]);
double gearRatio = 101.0; double gearRatio = 101.0;
double radToDeg = 180.0 / M_PI; double radToDeg = 180.0 / M_PI;
auto speed = (motor_status->speed() * 360.0) / (radToDeg * gearRatio * 100.0); auto speed = (motor_status->speed() * 360.0) / (radToDeg * gearRatio * 100.0);
auto angle_rad = (motor_status->position() * 360.0) / (gearRatio * 65536.0 * radToDeg); auto angle_rad = (motor_status->position() * 360.0) / (gearRatio * 65536.0 * radToDeg);
LOG(INFO) << " Motor ID " << int(this->node_id_) << " pos = " << angle_rad << " rad speed = " << speed << " rad/s"; // LOG(INFO) << " Motor ID " << int(this->node_id_) << " pos = " << angle_rad << " rad speed = " << speed << " rad/s";
} }

2
src/devices/robot/CMakeLists.txt
View File

@ -1,3 +1,3 @@
#add_subdirectory(ti5_robot) #add_subdirectory(ti5_robot)
add_subdirectory(humanoid_robot) add_subdirectory(humanoid_robot)
add_subdirectory(c701) #add_subdirectory(c701)

2
src/devices/robot/c701/motor/motor_controller_test.cpp
View File

@ -12,7 +12,7 @@
#include "cmvr/msgs/robot_detail.pb.h" #include "cmvr/msgs/robot_detail.pb.h"
#include "canbus/canopen/sdo_request_protocol.h" #include "canbus/canopen/sdo_request_protocol.h"
#include "canbus/can_client/socket/socket_can_client_raw.h" #include "canbus/can_client/socket/socket_can_client_raw.h"
#include "cmvr/msgs/ti5_motor.pb.h" // #include "cmvr/msgs/ti5_motor.pb.h"
using cmvr::msgs::RobotDetail; using cmvr::msgs::RobotDetail;
using cmvr::msgs::ErrorCode; using cmvr::msgs::ErrorCode;

4
src/devices/robot/humanoid_robot/CMakeLists.txt
View File

@ -1,3 +1,5 @@
find_package(protobuf REQUIRED)
add_library(humanoid_robot SHARED humanoid_robot.cpp) add_library(humanoid_robot SHARED humanoid_robot.cpp)
target_include_directories(humanoid_robot PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_include_directories(humanoid_robot PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@ -8,6 +10,7 @@ target_link_libraries(humanoid_robot PRIVATE
cmvr_es::utils cmvr_es::utils
cmvr_es::device::canbus cmvr_es::device::canbus
cmvr_es::device::ti5motor cmvr_es::device::ti5motor
protobuf::libprotobuf
) )
@ -15,7 +18,6 @@ target_link_libraries(humanoid_robot PRIVATE
# Unit test # Unit test
# -------------------------------------------------------- # --------------------------------------------------------
find_package(glog REQUIRED) find_package(glog REQUIRED)
find_package(protobuf REQUIRED)
find_package(PkgConfig REQUIRED) find_package(PkgConfig REQUIRED)
find_package(fcl REQUIRED) find_package(fcl REQUIRED)
find_package(OpenCV REQUIRED) find_package(OpenCV REQUIRED)

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

@ -355,25 +355,167 @@ void HumanoidRobot<DOF>::calibrateZeroQ(const std::string &joint_name) {
} }
template<int DOF> template<int DOF>
void HumanoidRobot<DOF>::moveJ_IK(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, double acc) {
double acc) {
try {
if (rsm_.load() == ROBOT_RUNNING) {
flash_cmd_.store(true);
eStop();
}
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
rsm_.store(ROBOT_RUNNING);
// update m_state_
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 runtime_error("solve IK failed");
}
std::vector<JointPoint> joint_points{
{"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]},
{"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]},
{"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]},
{"R_WRIST_R", q_cmd[13]}
};
for (const auto &j: joint_points) {
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->setQ(j.rad);
}
}
//3. wait for completion
bool completion = true;
do {
completion = true;
for (const auto &j: joint_points) {
auto motor = motor_manager_->getMotor(j.joint_name);
if (motor != nullptr) {
if (!motor->reachedTargetQ()) {
completion = false;
break;
}
}
}
// 4. while waiting, check flash_cmd_, if it is true, set it false then exit
if (flash_cmd_.load()) {
flash_cmd_.store(false);
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2));
} while (!completion);
rsm_.store(ROBOT_READY);
} else {
throw runtime_error("rsm invalid");
}
} catch (exception &e) {
throw runtime_error(e.what());
}
}
template<int DOF>
void HumanoidRobot<DOF>::moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel,
double acc) {
try { try {
if (rsm_.load() == ROBOT_RUNNING) { if (rsm_.load() == ROBOT_RUNNING) {
flash_cmd_.store(true); flash_cmd_.store(true);
eStop(); eStop();
} else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) { }
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
rsm_.store(ROBOT_RUNNING); rsm_.store(ROBOT_RUNNING);
Eigen::Vector<double, DOF> q_cmd;
bool ok = m_cctrl_->compute(m_state_, base_link, targets, 1, ctrl::CartesianController<DOF>::Mode::Position,
q_cmd, 60, 1e-4); // update m_state_
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_);
// slove ik
Eigen::Vector<double, DOF> q_cmd;
bool ok = m_cctrl_->compute(m_state_, base_link, targets, 0.002, ctrl::CartesianController<DOF>::Mode::Position,
q_cmd, 10000, 1e-6);
if (!ok) { if (!ok) {
throw runtime_error("solve IK failed"); throw runtime_error("solve IK failed");
} }
// TODO: execute joint position command from q_cmd std::vector<JointPoint> joint_points{
// 1. set motor vel and acc {"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]},
// 2. call motor PROFILE POSITION MODE (PPM) function {"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]},
// 3. wait for completion {"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]},
// 4. while waiting, check flash_cmd_, if it is true, set it false then exit {"R_WRIST_R", q_cmd[13]}
};
// for (const auto &j: joint_points) {
// 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->setQ(j.rad);
// }
// }
//
// //3. wait for completion
// bool completion = true;
// do {
// completion = true;
// for (const auto &j: joint_points) {
// auto motor = motor_manager_->getMotor(j.joint_name);
// if (motor != nullptr) {
// if (!motor->reachedTargetQ()) {
// completion = false;
// break;
// }
// }
// }
// // 4. while waiting, check flash_cmd_, if it is true, set it false then exit
// if (flash_cmd_.load()) {
// flash_cmd_.store(false);
// return;
// }
// std::this_thread::sleep_for(std::chrono::milliseconds(2));
// } while (!completion);
rsm_.store(ROBOT_READY); rsm_.store(ROBOT_READY);
} else { } else {
throw runtime_error("rsm invalid"); throw runtime_error("rsm invalid");
@ -561,6 +703,31 @@ bool HumanoidRobot<DOF>::check_joint_traj_(std::vector<std::vector<JointPoint> >
return true; return true;
} }
template<int DOF>
void HumanoidRobot<DOF>::moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,
double vel, double acc) {
try {
// 1 : 计算当前位姿
auto cur_pose = fk(base_link, ee_link);
// 2 : 计算目标角度 target pos = cur_pose + delta_pose
cmvr::msgs::Pose3d target_pose;
target_pose.mutable_position()->set_x(cur_pose.position().x() + delta_pose.position().x());
target_pose.mutable_position()->set_y(cur_pose.position().y() + delta_pose.position().y());
target_pose.mutable_position()->set_z(cur_pose.position().z() + delta_pose.position().z());
target_pose.mutable_euler()->set_rx(cur_pose.euler().rx() + delta_pose.euler().rx());
target_pose.mutable_euler()->set_ry(cur_pose.euler().ry() + delta_pose.euler().ry());
target_pose.mutable_euler()->set_rz(cur_pose.euler().rz() + delta_pose.euler().rz());
//3 :
moveJ(base_link, ee_link, target_pose, vel, acc);
} catch (exception &e) {
throw runtime_error(e.what());
}
}
template<int DOF> template<int DOF>
void HumanoidRobot<DOF>::update_state_() { void HumanoidRobot<DOF>::update_state_() {
std::lock_guard lock(state_mtx_); std::lock_guard lock(state_mtx_);
@ -570,6 +737,100 @@ void HumanoidRobot<DOF>::update_state_() {
// m_state_->SetQddot(); // m_state_->SetQddot();
} }
template<int DOF>
Eigen::Matrix3d HumanoidRobot<DOF>::eulerZYXToRotationMatrix(double rx, double ry, double rz) {
Eigen::Matrix3d R_x;
R_x << 1, 0, 0,
0, cos(rx), -sin(rx),
0, sin(rx), cos(rx);
Eigen::Matrix3d R_y;
R_y << cos(ry), 0, sin(ry),
0, 1, 0,
-sin(ry), 0, cos(ry);
Eigen::Matrix3d R_z;
R_z << cos(rz), -sin(rz), 0,
sin(rz), cos(rz), 0,
0, 0, 1;
return R_x * R_y * R_z;
}
template<int DOF>
Eigen::Vector3d HumanoidRobot<DOF>::rotationMatrixToEulerZYX(const Eigen::Matrix3d &R) {
double rx, ry, rz;
// 根据 R = R_x * R_y * R_z
// R = | cy*cz -cy*sz sy |
// | sx*sy*cz + cx*sz -sx*sy*sz + cx*cz -sx*cy |
// | -cx*sy*cz + sx*sz cx*sy*sz + sx*cz cx*cy |
// 提取 ry（绕 Y 的角度）
ry = std::asin(R(0,2)); // R(0,2) = sin(ry)
double cy = std::cos(ry);
if (std::abs(cy) > 1e-6) {
// 正常情况
rx = std::atan2(-R(1,2), R(2,2));
rz = std::atan2(-R(0,1), R(0,0));
} else {
// 万向节锁：cy ≈ 0
rx = 0; // 任意选择
if (ry > 0) {
rz = std::atan2(R(1,0), R(1,1));
} else {
rz = std::atan2(-R(1,0), R(1,1));
}
}
return Eigen::Vector3d(rx, ry, rz);
}
template<int DOF>
cmvr::msgs::Pose3d HumanoidRobot<DOF>::fk(const std::string &base_link, const std::string &ee_link) {
cmvr::msgs::Pose3d pose;
try {
// 获取当前关节角度
Eigen::Vector<double, DOF> q;
auto q_map = getJointQ(); // 类似 moveJ 中获取关节角度
q << 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"];
// 更新状态并计算前向运动学
m_state_->SetQ(q);
m_robot_->ComputeForwardKinematics(m_state_);
// 获取基座和末端索引
auto base_idx = m_robot_->GetLinkIdx(base_link);
auto ee_idx = m_robot_->GetLinkIdx(ee_link);
// 获取变换矩阵
Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx);
// 填充 Pose3d
pose.mutable_position()->set_x(T(0,3));
pose.mutable_position()->set_y(T(1,3));
pose.mutable_position()->set_z(T(2,3));
// 将旋转矩阵转换为欧拉角
Eigen::Matrix3d R = T.block<3,3>(0,0);
Eigen::Vector3d euler = rotationMatrixToEulerZYX(R); // 你需要实现或已有此工具函数
pose.mutable_euler()->set_rx(euler(0));
pose.mutable_euler()->set_ry(euler(1));
pose.mutable_euler()->set_rz(euler(2));
} catch (const std::exception &e) {
throw std::runtime_error(std::string("FK计算失败: ") + e.what());
}
return pose;
}
template class cmvr::device::HumanoidRobot<7>; template class cmvr::device::HumanoidRobot<7>;
template class cmvr::device::HumanoidRobot<14>; template class cmvr::device::HumanoidRobot<14>;

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

@ -60,7 +60,9 @@ namespace cmvr::device{
/* robot basic command*/ /* robot basic command*/
void eStop() override; void eStop() override;
void moveJ(std::vector<JointPoint> &cmd, double vel, double acc) override; void moveJ(std::vector<JointPoint> &cmd, double vel, double acc) override;
void moveJ_IK(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) override; void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel, double acc) override;
void moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) override ;
void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override;
void moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) override; void moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) override;
void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) override; void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) override;
void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) override; void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) override;
@ -81,6 +83,11 @@ namespace cmvr::device{
int exec_CSP_cmd_(std::vector<int> &ids, std::vector<double> &pos, int dt); int exec_CSP_cmd_(std::vector<int> &ids, std::vector<double> &pos, int dt);
int exec_CSV_cmd_(std::vector<int> &ids, std::vector<double> &vel, int dt); int exec_CSV_cmd_(std::vector<int> &ids, std::vector<double> &vel, int dt);
int exec_CSC_cmd_(std::vector<int> &ids, std::vector<double> &cur, int dt); int exec_CSC_cmd_(std::vector<int> &ids, std::vector<double> &cur, int dt);
public:
// 将欧拉角(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);
private: private:
std::string id_; std::string id_;
@ -129,6 +136,8 @@ namespace cmvr::device{
std::shared_ptr<MotorManager> motor_manager_{nullptr}; std::shared_ptr<MotorManager> motor_manager_{nullptr};
}; };

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

@ -10,6 +10,9 @@
#include "device_manager/device_manager.h" #include "device_manager/device_manager.h"
#include <libgen.h> #include <libgen.h>
#include "cmvr/msgs/can_card_parameter.grpc.pb.h"
#include "cmvr/msgs/geometry.pb.h"
#include "robot/humanoid_robot/humanoid_robot.h"
// 定义一个命令行参数 --config_path // 定义一个命令行参数 --config_path
DEFINE_string(config_path, "../config/cabin_robot.xml", "Path to the robot config XML file"); DEFINE_string(config_path, "../config/cabin_robot.xml", "Path to the robot config XML file");
@ -324,7 +327,103 @@ TEST(HumanoidRobotTest,FollowJointTrajectoryTest) {
std::this_thread::sleep_for(std::chrono::milliseconds(500)); std::this_thread::sleep_for(std::chrono::milliseconds(500));
} }
}
TEST(HumanoidRobotTest,MoveDeltaTest) {
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");
cmvr::msgs::Pose3d pose;
pose.mutable_position()->set_x( 0);
pose.mutable_position()->set_y(0);
pose.mutable_position()->set_z(0.03);
pose.mutable_euler()->set_rx(0);
pose.mutable_euler()->set_ry(0);
pose.mutable_euler()->set_rz(0);
robot->moveDeltaJ("PELVIS_S","R_WRIST_R_S",pose);
}
TEST(HumanoidRobotTest,AngleToTest) {
double rx = M_PI / 6; // 30°
double ry = M_PI / 4; // 45°
double rz = M_PI / 3; // 60°
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::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");
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}
};
robot->moveJ(cmd);
// //
// cmvr::ctrl::PoseTarget target;
// std::vector<cmvr::ctrl::PoseTarget> targets;
//
//
// cmvr::msgs::Pose3d pose;
//
// pose.mutable_position()->set_x( 1.49969573e-01);
// pose.mutable_position()->set_y(-4.00100001e-01);
// pose.mutable_position()->set_z(-1.00102800e-01);
//
// pose.mutable_euler()->set_rx(0);
// pose.mutable_euler()->set_ry(0);
// pose.mutable_euler()->set_rz(1.57);
//
//
//
// while (true) {
// robot->moveJ("PELVIS_S","R_WRIST_R_S",pose);
//
//
//
// std::this_thread::sleep_for(std::chrono::milliseconds(2000));
//
//
// robot->moveJ(cmd);
// std::this_thread::sleep_for(std::chrono::milliseconds(2000));
// }
} }

3
src/main.cpp
View File

@ -17,6 +17,7 @@
#include "utils/base/logger.h" #include "utils/base/logger.h"
#include "service/grpc_head_service.h" #include "service/grpc_head_service.h"
#include "service/grpc_humanoid_robot_service.h" #include "service/grpc_humanoid_robot_service.h"
#include "service/grpc_touch_service.h"
#include "http/httpclient.h" #include "http/httpclient.h"
#include "json/json.h" #include "json/json.h"
@ -48,6 +49,7 @@ void runServer(const XmlNode &cfg){
auto dexhand_service = gRPCDexHandServiceImpl(); auto dexhand_service = gRPCDexHandServiceImpl();
auto biohand_service = gRPCMBioHeadServiceImpl(); auto biohand_service = gRPCMBioHeadServiceImpl();
auto humanoid_robot_service = gRPCHumanoidRobotServiceImpl(); auto humanoid_robot_service = gRPCHumanoidRobotServiceImpl();
auto touch_service = gRPCTouchServiceImpl();
grpc::ServerBuilder builder; grpc::ServerBuilder builder;
builder.AddListeningPort(address, grpc::InsecureServerCredentials()); builder.AddListeningPort(address, grpc::InsecureServerCredentials());
@ -58,6 +60,7 @@ void runServer(const XmlNode &cfg){
builder.RegisterService(&dexhand_service); builder.RegisterService(&dexhand_service);
builder.RegisterService(&biohand_service); builder.RegisterService(&biohand_service);
builder.RegisterService(&humanoid_robot_service); builder.RegisterService(&humanoid_robot_service);
builder.RegisterService(&touch_service);
// 🔥 关键！启用反射 // 🔥 关键！启用反射
//grpc::reflection::InitProtoReflectionServerBuilderPlugin(); //grpc::reflection::InitProtoReflectionServerBuilderPlugin();

27
src/service/CMakeLists.txt
View File

@ -6,6 +6,7 @@ add_library(service
grpc_head_service.cpp grpc_head_service.cpp
grpc_dexhand_service.cpp grpc_dexhand_service.cpp
grpc_humanoid_robot_service.cpp grpc_humanoid_robot_service.cpp
grpc_touch_service.cpp
) )
target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@ -15,6 +16,7 @@ target_link_libraries(service PRIVATE
gRPC::grpc++ gRPC::grpc++
protobuf::libprotobuf protobuf::libprotobuf
cmvr_es::device_manager cmvr_es::device_manager
cmvr_es::ctrl::controller
) )
add_library(cmvr_es::service ALIAS service) add_library(cmvr_es::service ALIAS service)
@ -58,3 +60,28 @@ target_link_libraries(grpc_humanoid_robot_client_test
cmvr_es::device_manager cmvr_es::device_manager
${OpenCV_LIBS} ${OpenCV_LIBS}
) )
add_executable(grpc_touch_client_test
${CMAKE_CURRENT_SOURCE_DIR}/grpc_touch_client_test.cpp
)
target_link_libraries(grpc_touch_client_test
PRIVATE
cmvr_es::device::canbus
cmvr_es::device::ti5motor
cmvr_es::device::humanoid_robot
gtest
gtest_main
pthread
glog::glog
proto-objects
ccd
fcl
cmvr_es::device_manager
${OpenCV_LIBS}
)

21
src/service/grpc_humanoid_robot_client_test.cpp
View File

@ -68,26 +68,7 @@ TEST(GrpcHumanoidRobotClientTest, MyMoveJTest) {
{"WAIST_Y" ,0.0}, {"WAIST_Y" ,0.0},
}; };
// cmds = {
// {"L_SHOULDER_P", 0.0},
// {"L_SHOULDER_R", 0.0},
// {"L_SHOULDER_Y", 0.0},
// {"L_ELBOW_R", 0.0},
// {"L_WRIST_P", 0.0},
// {"L_WRIST_Y", 0.0},
// {"L_WRIST_R", 0.0},
//
// {"R_SHOULDER_P", 0.0},
// {"R_SHOULDER_R", 0.0},
// {"R_SHOULDER_Y", 0.0},
// {"R_ELBOW_R", 0.0},
// {"R_WRIST_P", 0.0},
// {"R_WRIST_Y", 0.0},
// {"R_WRIST_R", 0.0},
//
// {"WAIST_P" ,0.0},
// {"WAIST_Y" ,0.0},
// };
// 构造请求 // 构造请求
cmvr::api::MoveJ_Request request; cmvr::api::MoveJ_Request request;

58
src/service/grpc_touch_client_test.cpp Normal file
View File

@ -0,0 +1,58 @@
//
// Created by lgv on 2025/8/25.
//
#include "gtest/gtest.h"
#include <glog/logging.h>
#include <grpcpp/grpcpp.h>
#include "service/grpc_touch_service.h"
#include "google/protobuf/timestamp.pb.h"
#include <iostream>
#include <google/protobuf/util/time_util.h>
using namespace cmvr::api;
TEST(GrpcTouchClientTest, MyTest) {
// 连接服务端
auto channel = grpc::CreateChannel("0.0.0.0:50052", grpc::InsecureChannelCredentials());
auto stub = cmvr::api::TouchService::NewStub(channel);
grpc::ClientContext context;
cmvr::api::Touch_Request request;
cmvr::api::Touch_Response response;
request.mutable_header()->set_device_id("hc01");
*request.mutable_header()->mutable_timestamp() = google::protobuf::util::TimeUtil::GetCurrentTime();
// 目标位姿
request.mutable_pose()->mutable_position()->set_x(0.15);
request.mutable_pose()->mutable_position()->set_y(-0.4);
request.mutable_pose()->mutable_position()->set_z(0);
request.mutable_pose()->mutable_euler()->set_rx(0);
request.mutable_pose()->mutable_euler()->set_ry(0);
request.mutable_pose()->mutable_euler()->set_rz(1.57);
// 偏移位姿
request.mutable_offset()->mutable_position()->set_x(0);
request.mutable_offset()->mutable_position()->set_y(0);
request.mutable_offset()->mutable_position()->set_z(0);
request.mutable_offset()->mutable_euler()->set_rx(0);
request.mutable_offset()->mutable_euler()->set_ry(0);
request.mutable_offset()->mutable_euler()->set_rz(0);
// 调用
grpc::Status status = stub->touch(&context, request, &response);
if (status.ok()) {
LOG(INFO) << "Touch RPC succeeded." << std::endl;
LOG(INFO) << "Success: " << response.mutable_header()->success() << std::endl;
LOG(INFO) << "Error message: " << response.mutable_header()->error_message() << std::endl;
LOG(INFO) << "Timestamp: " << response.mutable_header()->timestamp().seconds() << std::endl;
} else {
LOG(ERROR) << "Touch RPC failed: " << status.error_message() << std::endl;
}
}

38
src/service/grpc_touch_service.cpp Normal file
View File

@ -0,0 +1,38 @@
//
// Created by lgv on 2025/8/25.
//
#include "service/grpc_touch_service.h"
#include <google/protobuf/util/time_util.h>
#include "robot/humanoid_robot/humanoid_robot.h"
#include "controller/touch_controller.h"
using namespace cmvr::service;
using namespace cmvr::device;
using namespace cmvr::api;
using google::protobuf::util::TimeUtil;
gRPCTouchServiceImpl::gRPCTouchServiceImpl():dmgr_(DeviceManager::getInstance()){}
grpc::Status gRPCTouchServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) {
grpc::Status ret = grpc::Status::OK;
try {
auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
ctrl::TouchController touch_controller;
touch_controller.touch(robot,request->pose(),request->offset());
response->mutable_header()->set_success(true);
response->mutable_header()->set_error_message("");
}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;
}