diff --git a/CMakeLists.txt b/CMakeLists.txt index d16101c8..01021bbf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -93,6 +93,6 @@ target_link_libraries(cmvr_es PRIVATE cmvr_es::hardware cmvr_es::httpclient cmvr_es::device::canbus - cmvr_es::robot::c701 cmvr_es::device::ti5motor + cmvr_es::ctrl::controller ) \ No newline at end of file diff --git a/config/cabin_robot.xml b/config/cabin_robot.xml index d229e483..84118368 100644 --- a/config/cabin_robot.xml +++ b/config/cabin_robot.xml @@ -20,9 +20,9 @@ - - - + + + @@ -33,11 +33,12 @@ + bufferSize="50" + verbose="false"> @@ -83,12 +84,12 @@ - + - + @@ -131,7 +132,7 @@ - + diff --git a/example/solve_ik.cpp b/example/solve_ik.cpp index 794b6a20..f3aa948a 100644 --- a/example/solve_ik.cpp +++ b/example/solve_ik.cpp @@ -10,6 +10,7 @@ #include #include #include +#include constexpr int DOF = 14; using RobotT = cmvr::dyn::Robot; diff --git a/include/controller/touch_controller.h b/include/controller/touch_controller.h new file mode 100644 index 00000000..1d0c195f --- /dev/null +++ b/include/controller/touch_controller.h @@ -0,0 +1,103 @@ +// +// Created by lgv on 2025/8/24. +// + +#pragma once + +#include + +#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 robot,std::shared_ptr hand) + :robot_(std::move(robot)),hand_(std::move(hand)), + pid_(std::make_shared(0.05, 0.001, 0.01, 50.0, 0.5, 1.0)){} + ~TouchController()=default; + + void touch(std::shared_ptr robot,const msgs::Pose3d pose,const msgs::Pose3d offset); + void touch( msgs::Pose3d pose, msgs::Pose3d offset,double max_force); + + private: + std::shared_ptr robot_{nullptr}; + std::shared_ptr hand_{nullptr}; + + std::shared_ptr pid_{nullptr}; + + const double touch_threshold_ = 5.0; // 触控判定阈值 + + + // 从压阻矩阵提取触控点与压力 + bool extractTouch(const std::vector>& matrix,double& force, int& x, int& y); + + }; + } +} diff --git a/include/devices/abstract_canbus.h b/include/devices/abstract_canbus.h index 21445ba8..3056f191 100644 --- a/include/devices/abstract_canbus.h +++ b/include/devices/abstract_canbus.h @@ -100,5 +100,8 @@ namespace cmvr::device { /// CAN clientstatus cmvr::msgs::ErrorCode status_; + // + bool enable_log_{false}; + }; } diff --git a/include/devices/abstract_robot.h b/include/devices/abstract_robot.h index fbd8c576..c160db13 100644 --- a/include/devices/abstract_robot.h +++ b/include/devices/abstract_robot.h @@ -9,7 +9,7 @@ #include "jsoncpp/json/json.h" #include "abstract_device.h" #include "utils/controller/cartesian_controller.h" - +#include "cmvr/msgs/geometry.pb.h" namespace cmvr::device{ struct JointPoint{ @@ -66,10 +66,12 @@ namespace cmvr::device{ virtual void moveJ(std::vector &joints, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); } virtual void moveJ(std::vector &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(std::string &base_link, std::vector &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 &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"); } diff --git a/include/service/grpc_touch_service.h b/include/service/grpc_touch_service.h new file mode 100644 index 00000000..388ec2dd --- /dev/null +++ b/include/service/grpc_touch_service.h @@ -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_; + }; + + + + } + +} diff --git a/protos/cmvr/api/geometry.proto b/protos/cmvr/api/geometry.proto deleted file mode 100644 index a7ab001b..00000000 --- a/protos/cmvr/api/geometry.proto +++ /dev/null @@ -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; -} diff --git a/protos/cmvr/api/touch_service.proto b/protos/cmvr/api/touch_service.proto new file mode 100644 index 00000000..3b0d6d1c --- /dev/null +++ b/protos/cmvr/api/touch_service.proto @@ -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); +} \ No newline at end of file diff --git a/protos/cmvr/msgs/geometry.proto b/protos/cmvr/msgs/geometry.proto new file mode 100644 index 00000000..dca76637 --- /dev/null +++ b/protos/cmvr/msgs/geometry.proto @@ -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) +} diff --git a/python/hand_eye_calibration/__pycache__/save_poses.cpython-310.pyc b/python/hand_eye_calibration/__pycache__/save_poses.cpython-310.pyc index b8f6b4d8..b90060ed 100644 Binary files a/python/hand_eye_calibration/__pycache__/save_poses.cpython-310.pyc and b/python/hand_eye_calibration/__pycache__/save_poses.cpython-310.pyc differ diff --git a/python/hand_eye_calibration/eye_hand_data/data2025082102/1.jpg b/python/hand_eye_calibration/eye_hand_data/data2025082102/1.jpg deleted file mode 100644 index ec1dda79..00000000 Binary files a/python/hand_eye_calibration/eye_hand_data/data2025082102/1.jpg and /dev/null differ diff --git a/python/hand_eye_calibration/eye_hand_data/data2025082102/RobotToolPose.csv b/python/hand_eye_calibration/eye_hand_data/data2025082102/RobotToolPose.csv deleted file mode 100644 index 59fe6539..00000000 --- a/python/hand_eye_calibration/eye_hand_data/data2025082102/RobotToolPose.csv +++ /dev/null @@ -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 diff --git a/python/hand_eye_calibration/eye_hand_data/data2025082103/1.jpg b/python/hand_eye_calibration/eye_hand_data/data2025082103/1.jpg deleted file mode 100644 index b4910686..00000000 Binary files a/python/hand_eye_calibration/eye_hand_data/data2025082103/1.jpg and /dev/null differ diff --git a/python/hand_eye_calibration/eye_hand_data/data2025082103/RobotToolPose.csv b/python/hand_eye_calibration/eye_hand_data/data2025082103/RobotToolPose.csv deleted file mode 100644 index 8a924c0d..00000000 --- a/python/hand_eye_calibration/eye_hand_data/data2025082103/RobotToolPose.csv +++ /dev/null @@ -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 diff --git a/python/hand_eye_calibration/libs/__pycache__/auxiliary.cpython-310.pyc b/python/hand_eye_calibration/libs/__pycache__/auxiliary.cpython-310.pyc index 06f4ff28..43ab91f6 100644 Binary files a/python/hand_eye_calibration/libs/__pycache__/auxiliary.cpython-310.pyc and b/python/hand_eye_calibration/libs/__pycache__/auxiliary.cpython-310.pyc differ diff --git a/python/hand_eye_calibration/libs/__pycache__/log_setting.cpython-310.pyc b/python/hand_eye_calibration/libs/__pycache__/log_setting.cpython-310.pyc index fba330e8..e992eb09 100644 Binary files a/python/hand_eye_calibration/libs/__pycache__/log_setting.cpython-310.pyc and b/python/hand_eye_calibration/libs/__pycache__/log_setting.cpython-310.pyc differ diff --git a/python/hand_eye_calibration/libs/web-log b/python/hand_eye_calibration/libs/web-log index b0dd5a49..30302d29 100644 --- a/python/hand_eye_calibration/libs/web-log +++ b/python/hand_eye_calibration/libs/web-log @@ -1,704 +1,46 @@ -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: 旋转矩阵是: +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg +2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg +2025-08-23 16:08:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg +2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg +2025-08-23 16:08:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg +2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg +2025-08-23 16:08:46 - __main__ - INFO: 旋转矩阵是: [[-0.99486349 0.07846371 0.06395369] [-0.05884529 0.06577685 -0.9960977 ] [-0.08236419 -0.99474462 -0.06082177]] -2025-08-21 16:26:09 - __main__ - INFO: 平移向量是: +2025-08-23 16:08:46 - __main__ - INFO: 平移向量是: [[-0.00797824] [-0.17640328] [ 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] -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: 相机已关闭，窗口已销毁 diff --git a/python/hand_eye_calibration/libs/web-log.2025-08-22.log b/python/hand_eye_calibration/libs/web-log.2025-08-22.log new file mode 100644 index 00000000..b0dd5a49 --- /dev/null +++ b/python/hand_eye_calibration/libs/web-log.2025-08-22.log @@ -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: 相机已关闭，窗口已销毁 diff --git a/python/math/rpy2rotation.py b/python/math/rpy2rotation.py index 5039208b..4aa12c85 100644 --- a/python/math/rpy2rotation.py +++ b/python/math/rpy2rotation.py @@ -71,9 +71,14 @@ if __name__ == "__main__": # print("R:\n", R) # print("rpy back:", rpy_back) # print("max abs diff:", np.max(np.abs(np.array(rpy) - np.array(rpy_back)))) + """ + 3.54839600e-04, -9.96155522e-01, 8.76016580e-02, 1.49988098e-01, + 9.99999921e-01, 3.37579500e-04, -2.11843500e-04, -0.35, + 1.81456600e-04, 8.76017262e-02, 9.96155563e-01, + """ R = np.array([ - [-1, 0, 0], - [0, -1, 0], - [0, 0, 1] + [3.54839600e-04, -9.96155522e-01, 8.76016580e-02], + [9.99999921e-01, 3.37579500e-04, -2.11843500e-04], + [1.81456600e-04, 8.76017262e-02, 9.96155563e-01] ]) - print(matrix_to_rpy(R)) \ No newline at end of file + print(np.array(matrix_to_rpy(R)) * 180 / np.pi) \ No newline at end of file diff --git a/python/vision_servo/__pycache__/follow_target.cpython-310.pyc b/python/vision_servo/__pycache__/follow_target.cpython-310.pyc index 9888c31a..0ce0e221 100644 Binary files a/python/vision_servo/__pycache__/follow_target.cpython-310.pyc and b/python/vision_servo/__pycache__/follow_target.cpython-310.pyc differ diff --git a/python/vision_servo/__pycache__/get_chessboard_position.cpython-310.pyc b/python/vision_servo/__pycache__/get_chessboard_position.cpython-310.pyc index 587237d2..e8fea3e1 100644 Binary files a/python/vision_servo/__pycache__/get_chessboard_position.cpython-310.pyc and b/python/vision_servo/__pycache__/get_chessboard_position.cpython-310.pyc differ diff --git a/python/vision_servo/follow_target.py b/python/vision_servo/follow_target.py index 39079b71..587088f7 100644 --- a/python/vision_servo/follow_target.py +++ b/python/vision_servo/follow_target.py @@ -327,89 +327,8 @@ def compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target): return P_base_target -if __name__ == "__main__": - pipeline, align = init_realsense() - try: - # 初始化机器人 - robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") - - # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) - - robot.moveJ("right", [-0.344938, 0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996]) - js = robot.getJointQ('right') - print("js = ") - print(js) - T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S") - # T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") - - # P_base_cam = T_base_cam[:3, 3] - - print("T_base_ee =\n", T_base_ee) - # print("T_ee_cam =\n", T_ee_cam) - # print("T_base_cam =\n", T_base_cam) - - - P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png") - # P_cam_target = red_point_position(color_f, depth_f, depth_intr, vis_path="red_point.png") - print("P_cam_target (camera frame) =", P_cam_target) - - # T_cam_target = np.eye(4) - # T_cam_target[:3, 3] = P_cam_target - # T_base_target = T_base_cam @ T_cam_target - # P_base_target = T_base_target[:3, 3] - - # R_ee_cam = np.array([ - # [-1, 0, 0], - # [ 0, 0, -1], - # [ 0, -1, 0] - # ]) - # t_ee_cam = np.array([-0.01212, -0.17655, 0.07506]) - R_ee_cam = np.array( [[-0.99732744 , 0.02734315 , 0.0677519 ], - [-0.06713675 ,0.0228221 , -0.99748274], - [-0.02882056,-0.99936555, -0.02092538]]) - t_ee_cam = np.array([-0.00078482, -0.17419722, 0.06796275]) - # t_ee_cam = np.array([-0.05212, -0.17419722, 0.06796275]) - - # R_ee_cam = np.array( [[-0.99486349, 0.07846371 , 0.06395369], - # [-0.05884529 ,0.06577685, -0.9960977 ], - # [-0.08236419 ,-0.99474462 ,-0.06082177]]) - # - # t_ee_cam = np.array([-0.00797824, -0.17640328, 0.07459845]) - P_base_target = compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target) - - print("P_base_target (base frame) =", P_base_target) - - # 位置：直接用 P_base_target - target_x, target_y, target_z = P_base_target - np.array([0.45, 0, 0]) - target_rx, target_ry, target_rz =R.from_matrix(T_base_ee[:3, :3]).as_euler('xyz', degrees=True) - - # 调 IK - ok, lq, rq = solve_ik(target_x, target_y, target_z, target_rx, target_ry, target_rz) - - - print("rq =", rq) - - if ok: - robot.moveJ("right", rq) - else: - print("solve ik failed") - - - print("已到达目标点，按 Ctrl+C 回到初始位姿...") - while True: - time.sleep(1) - except SystemExit: - print("安全退出程序...") - - finally: - robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) - pipeline.stop() - align = None - depth_intr = None - -# # if __name__ == "__main__": -# +# pipeline, align = init_realsense() # try: # # 初始化机器人 # robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") @@ -420,6 +339,60 @@ if __name__ == "__main__": # js = robot.getJointQ('right') # print("js = ") # print(js) +# T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S") +# # T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") +# +# # P_base_cam = T_base_cam[:3, 3] +# +# print("T_base_ee =\n", T_base_ee) +# # print("T_ee_cam =\n", T_ee_cam) +# # print("T_base_cam =\n", T_base_cam) +# +# +# P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png") +# # P_cam_target = red_point_position(color_f, depth_f, depth_intr, vis_path="red_point.png") +# print("P_cam_target (camera frame) =", P_cam_target) +# +# # T_cam_target = np.eye(4) +# # T_cam_target[:3, 3] = P_cam_target +# # T_base_target = T_base_cam @ T_cam_target +# # P_base_target = T_base_target[:3, 3] +# +# # R_ee_cam = np.array([ +# # [-1, 0, 0], +# # [ 0, 0, -1], +# # [ 0, -1, 0] +# # ]) +# # t_ee_cam = np.array([-0.01212, -0.17655, 0.07506]) +# R_ee_cam = np.array( [[-0.99732744 , 0.02734315 , 0.0677519 ], +# [-0.06713675 ,0.0228221 , -0.99748274], +# [-0.02882056,-0.99936555, -0.02092538]]) +# t_ee_cam = np.array([-0.00078482, -0.17419722, 0.06796275]) +# # t_ee_cam = np.array([-0.05212, -0.17419722, 0.06796275]) +# +# # R_ee_cam = np.array( [[-0.99486349, 0.07846371 , 0.06395369], +# # [-0.05884529 ,0.06577685, -0.9960977 ], +# # [-0.08236419 ,-0.99474462 ,-0.06082177]]) +# # +# # t_ee_cam = np.array([-0.00797824, -0.17640328, 0.07459845]) +# P_base_target = compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target) +# +# print("P_base_target (base frame) =", P_base_target) +# +# # 位置：直接用 P_base_target +# target_x, target_y, target_z = P_base_target - np.array([0.45, 0, 0]) +# target_rx, target_ry, target_rz =R.from_matrix(T_base_ee[:3, :3]).as_euler('xyz', degrees=True) +# +# # 调 IK +# ok, lq, rq = solve_ik(target_x, target_y, target_z, target_rx, target_ry, target_rz) +# +# +# print("rq =", rq) +# +# if ok: +# robot.moveJ("right", rq) +# else: +# print("solve ik failed") # # # print("已到达目标点，按 Ctrl+C 回到初始位姿...") @@ -430,3 +403,33 @@ if __name__ == "__main__": # # finally: # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) +# pipeline.stop() +# align = None +# depth_intr = None + + +if __name__ == "__main__": + + try: + # 初始化机器人 + robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") + + # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) + + robot.moveJ("right", [-0.344938, 0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996]) + js = robot.getJointQ('right') + # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S") + # print("js = ") + # print(js) + # + # print("T_base_ee =\n", T_base_ee) + + + print("已到达目标点，按 Ctrl+C 回到初始位姿...") + while True: + time.sleep(1) + except SystemExit: + print("安全退出程序...") + + finally: + robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) diff --git a/python/vision_servo/robot_warpper_test.py b/python/vision_servo/robot_warpper_test.py index d44ca401..6797c954 100644 --- a/python/vision_servo/robot_warpper_test.py +++ b/python/vision_servo/robot_warpper_test.py @@ -1,8 +1,9 @@ import sys +import os +os.environ["GLOG_minloglevel"] = "1" # 屏蔽 INFO +# from python.vision_servo.follow_target import moveJ import time -from python.vision_servo.follow_target import moveJ - # 把.so所在目录加入 Python 路径 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 = [ - [-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.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.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.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.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.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.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.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.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.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], -] - - -while True: - for j in JOINT_POSITIONS: - robot.moveJ('right', j) -# robot.torqueOn() +# JOINT_POSITIONS = [ +# [-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.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.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.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.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.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.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.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.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.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], +# ] +# +# +# while True: +# for j in JOINT_POSITIONS: +# robot.moveJ('right', j) +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.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) # # -# time.sleep(10) -# robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) -robot.torqueOff() +time.sleep(10) +robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) +# robot.torqueOff() # // {"R_SHOULDER_P", 0.00203898}, # // {"R_SHOULDER_R", 1.34062}, diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 004a23e5..a14da40a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -4,4 +4,5 @@ add_subdirectory(devices) add_subdirectory(monitor) add_subdirectory(device_manager) add_subdirectory(service) -add_subdirectory(http) \ No newline at end of file +add_subdirectory(http) +add_subdirectory(controller) \ No newline at end of file diff --git a/src/controller/CMakeLists.txt b/src/controller/CMakeLists.txt new file mode 100644 index 00000000..c995c881 --- /dev/null +++ b/src/controller/CMakeLists.txt @@ -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} +) diff --git a/src/controller/touch_controller.cpp b/src/controller/touch_controller.cpp new file mode 100644 index 00000000..28dd5cc6 --- /dev/null +++ b/src/controller/touch_controller.cpp @@ -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 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 > &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; + +} + + + diff --git a/src/controller/touch_controller_test.cpp b/src/controller/touch_controller_test.cpp new file mode 100644 index 00000000..02232431 --- /dev/null +++ b/src/controller/touch_controller_test.cpp @@ -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("hc01"); + auto hand = dmgr.getDevice("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)); + } + + +} \ No newline at end of file diff --git a/src/devices/canbus/CMakeLists.txt b/src/devices/canbus/CMakeLists.txt index f9cf48ba..f35cdabf 100644 --- a/src/devices/canbus/CMakeLists.txt +++ b/src/devices/canbus/CMakeLists.txt @@ -13,7 +13,7 @@ add_library(cmvr_es::device::canbus ALIAS canbus) target_link_libraries(canbus PRIVATE - proto-objects +# proto-objects gRPC::grpc++ protobuf::libprotobuf glog::glog diff --git a/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp b/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp index 04444821..9b9d1b64 100644 --- a/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp +++ b/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp @@ -32,9 +32,10 @@ void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response, motor_status->set_position_offset(sdo_response.data()); } - LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_) - << ": command=" << int(sdo_response.cs()) - << ", index=" << sdo_response.index() - << ", subindex=" << int(sdo_response.sub_index()) - << ", data=" << sdo_response.data(); + LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_) + << ": command=" << int(sdo_response.cs()) + << ", index=" << sdo_response.index() + << ", subindex=" << int(sdo_response.sub_index()) + << ", data=" << sdo_response.data(); + } diff --git a/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.cpp b/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.cpp index 5739956a..e4fd5f3c 100644 --- a/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.cpp +++ b/src/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo2.cpp @@ -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_speed(bytes[7] << 24 | bytes[6] << 16 | bytes[5] << 8 | bytes[4]); + double gearRatio = 101.0; double radToDeg = 180.0 / M_PI; auto speed = (motor_status->speed() * 360.0) / (radToDeg * gearRatio * 100.0); 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"; + + + } diff --git a/src/devices/robot/CMakeLists.txt b/src/devices/robot/CMakeLists.txt index e7f9228c..599ad24f 100644 --- a/src/devices/robot/CMakeLists.txt +++ b/src/devices/robot/CMakeLists.txt @@ -1,3 +1,3 @@ #add_subdirectory(ti5_robot) add_subdirectory(humanoid_robot) -add_subdirectory(c701) \ No newline at end of file +#add_subdirectory(c701) \ No newline at end of file diff --git a/src/devices/robot/c701/motor/motor_controller_test.cpp b/src/devices/robot/c701/motor/motor_controller_test.cpp index b01cac00..3e695e17 100644 --- a/src/devices/robot/c701/motor/motor_controller_test.cpp +++ b/src/devices/robot/c701/motor/motor_controller_test.cpp @@ -12,7 +12,7 @@ #include "cmvr/msgs/robot_detail.pb.h" #include "canbus/canopen/sdo_request_protocol.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::ErrorCode; diff --git a/src/devices/robot/humanoid_robot/CMakeLists.txt b/src/devices/robot/humanoid_robot/CMakeLists.txt index 54daad65..fb62ba24 100644 --- a/src/devices/robot/humanoid_robot/CMakeLists.txt +++ b/src/devices/robot/humanoid_robot/CMakeLists.txt @@ -1,3 +1,5 @@ +find_package(protobuf REQUIRED) + add_library(humanoid_robot SHARED humanoid_robot.cpp) 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::device::canbus cmvr_es::device::ti5motor + protobuf::libprotobuf ) @@ -15,7 +18,6 @@ target_link_libraries(humanoid_robot PRIVATE # Unit test # -------------------------------------------------------- find_package(glog REQUIRED) -find_package(protobuf REQUIRED) find_package(PkgConfig REQUIRED) find_package(fcl REQUIRED) find_package(OpenCV REQUIRED) diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index fb4774a1..ace42775 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -355,25 +355,167 @@ void HumanoidRobot::calibrateZeroQ(const std::string &joint_name) { } template -void HumanoidRobot::moveJ_IK(std::string &base_link, std::vector &targets, double vel, - double acc) { +void HumanoidRobot::moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, 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 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 q_cmd; + bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, ctrl::CartesianController::Mode::Position, + q_cmd, 10000, 1e-6); + if (!ok) { + throw runtime_error("solve IK failed"); + } + std::vector 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 +void HumanoidRobot::moveJ_IK(const std::string &base_link, const std::vector &targets, double vel, + double acc) { try { if (rsm_.load() == ROBOT_RUNNING) { flash_cmd_.store(true); 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); - Eigen::Vector q_cmd; - bool ok = m_cctrl_->compute(m_state_, base_link, targets, 1, ctrl::CartesianController::Mode::Position, - q_cmd, 60, 1e-4); + + + // update m_state_ + Eigen::Vector 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 q_cmd; + bool ok = m_cctrl_->compute(m_state_, base_link, targets, 0.002, ctrl::CartesianController::Mode::Position, + q_cmd, 10000, 1e-6); if (!ok) { throw runtime_error("solve IK failed"); } - // TODO: execute joint position command from q_cmd - // 1. set motor vel and acc - // 2. call motor PROFILE POSITION MODE (PPM) function - // 3. wait for completion - // 4. while waiting, check flash_cmd_, if it is true, set it false then exit + std::vector 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"); @@ -561,6 +703,31 @@ bool HumanoidRobot::check_joint_traj_(std::vector > return true; } +template +void HumanoidRobot::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 void HumanoidRobot::update_state_() { std::lock_guard lock(state_mtx_); @@ -570,6 +737,100 @@ void HumanoidRobot::update_state_() { // m_state_->SetQddot(); } +template +Eigen::Matrix3d HumanoidRobot::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 +Eigen::Vector3d HumanoidRobot::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 +cmvr::msgs::Pose3d HumanoidRobot::fk(const std::string &base_link, const std::string &ee_link) { + cmvr::msgs::Pose3d pose; + + try { + // 获取当前关节角度 + Eigen::Vector 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<14>; diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.h b/src/devices/robot/humanoid_robot/humanoid_robot.h index 864896c2..64fb8248 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.h +++ b/src/devices/robot/humanoid_robot/humanoid_robot.h @@ -60,7 +60,9 @@ namespace cmvr::device{ /* robot basic command*/ void eStop() override; void moveJ(std::vector &cmd, double vel, double acc) override; - void moveJ_IK(std::string &base_link, std::vector &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 &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 &targets, 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; @@ -81,6 +83,11 @@ namespace cmvr::device{ int exec_CSP_cmd_(std::vector &ids, std::vector &pos, int dt); int exec_CSV_cmd_(std::vector &ids, std::vector &vel, int dt); int exec_CSC_cmd_(std::vector &ids, std::vector &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: std::string id_; @@ -129,6 +136,8 @@ namespace cmvr::device{ std::shared_ptr motor_manager_{nullptr}; + + }; diff --git a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp index fc1d8752..9c9b7556 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp @@ -10,6 +10,9 @@ #include "device_manager/device_manager.h" #include +#include "cmvr/msgs/can_card_parameter.grpc.pb.h" +#include "cmvr/msgs/geometry.pb.h" +#include "robot/humanoid_robot/humanoid_robot.h" // 定义一个命令行参数 --config_path DEFINE_string(config_path, "../config/cabin_robot.xml", "Path to the robot config XML file"); @@ -322,7 +325,103 @@ TEST(HumanoidRobotTest,FollowJointTrajectoryTest) { 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("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("hc01"); + + + std::vector 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 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)); + // } } \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 72d7aefc..01297461 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -17,6 +17,7 @@ #include "utils/base/logger.h" #include "service/grpc_head_service.h" #include "service/grpc_humanoid_robot_service.h" +#include "service/grpc_touch_service.h" #include "http/httpclient.h" #include "json/json.h" @@ -48,6 +49,7 @@ void runServer(const XmlNode &cfg){ auto dexhand_service = gRPCDexHandServiceImpl(); auto biohand_service = gRPCMBioHeadServiceImpl(); auto humanoid_robot_service = gRPCHumanoidRobotServiceImpl(); + auto touch_service = gRPCTouchServiceImpl(); grpc::ServerBuilder builder; builder.AddListeningPort(address, grpc::InsecureServerCredentials()); @@ -58,6 +60,7 @@ void runServer(const XmlNode &cfg){ builder.RegisterService(&dexhand_service); builder.RegisterService(&biohand_service); builder.RegisterService(&humanoid_robot_service); + builder.RegisterService(&touch_service); // 🔥 关键！启用反射 //grpc::reflection::InitProtoReflectionServerBuilderPlugin(); diff --git a/src/service/CMakeLists.txt b/src/service/CMakeLists.txt index 0a4b1034..28befede 100644 --- a/src/service/CMakeLists.txt +++ b/src/service/CMakeLists.txt @@ -6,6 +6,7 @@ add_library(service grpc_head_service.cpp grpc_dexhand_service.cpp grpc_humanoid_robot_service.cpp + grpc_touch_service.cpp ) target_include_directories(service PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) @@ -15,6 +16,7 @@ target_link_libraries(service PRIVATE gRPC::grpc++ protobuf::libprotobuf cmvr_es::device_manager + cmvr_es::ctrl::controller ) add_library(cmvr_es::service ALIAS service) @@ -58,3 +60,28 @@ target_link_libraries(grpc_humanoid_robot_client_test cmvr_es::device_manager ${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} +) + diff --git a/src/service/grpc_humanoid_robot_client_test.cpp b/src/service/grpc_humanoid_robot_client_test.cpp index 59114dd9..5f8adbfa 100644 --- a/src/service/grpc_humanoid_robot_client_test.cpp +++ b/src/service/grpc_humanoid_robot_client_test.cpp @@ -68,26 +68,7 @@ TEST(GrpcHumanoidRobotClientTest, MyMoveJTest) { {"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; diff --git a/src/service/grpc_touch_client_test.cpp b/src/service/grpc_touch_client_test.cpp new file mode 100644 index 00000000..6d47a0b2 --- /dev/null +++ b/src/service/grpc_touch_client_test.cpp @@ -0,0 +1,58 @@ +// +// Created by lgv on 2025/8/25. +// +#include "gtest/gtest.h" +#include +#include +#include "service/grpc_touch_service.h" +#include "google/protobuf/timestamp.pb.h" +#include +#include + +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; + } +} \ No newline at end of file diff --git a/src/service/grpc_touch_service.cpp b/src/service/grpc_touch_service.cpp new file mode 100644 index 00000000..7e55c657 --- /dev/null +++ b/src/service/grpc_touch_service.cpp @@ -0,0 +1,38 @@ +// +// Created by lgv on 2025/8/25. +// + + +#include "service/grpc_touch_service.h" +#include +#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(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; +} + +