diff --git a/README.md b/README.md index 8f41887a..48727cc6 100644 --- a/README.md +++ b/README.md @@ -99,11 +99,12 @@ sudo make install ```shell sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u -sudo apt-get install librealsense2-dkms=2.55.1-0~realsense.12473 -y -sudo apt-get install librealsense2-utils=2.55.1-0~realsense.12473 -y -sudo apt-get install librealsense2-dev=2.55.1-0~realsense.12473 -y -sudo apt-get install librealsense2-dbg=2.55.1-0~realsense.12473 -y - +sudo apt-get install librealsense2-dkms=1.3.24-0ubuntu1 -y +sudo apt-get install \ +librealsense2=2.55.1-0~realsense.12473 \ +librealsense2-utils=2.55.1-0~realsense.12473 \ +librealsense2-dev=2.55.1-0~realsense.12473 \ +librealsense2-dbg=2.55.1-0~realsense.12473 -y ``` ### 5. OSQPEigen install(build from source) diff --git a/config/cabin_robot.xml b/config/cabin_robot.xml index e1babd54..1fb3350e 100644 --- a/config/cabin_robot.xml +++ b/config/cabin_robot.xml @@ -33,7 +33,7 @@ - + diff --git a/example/robot_wrapper.cpp b/example/robot_wrapper.cpp index fbc1c09f..49f2019f 100644 --- a/example/robot_wrapper.cpp +++ b/example/robot_wrapper.cpp @@ -8,9 +8,11 @@ #include #include #include +#include #include "devices/abstract_robot.h" #include "device_manager/device_manager.h" +#include "cmvr/msgs/geometry.pb.h" namespace py = pybind11; using namespace cmvr::device; @@ -19,7 +21,7 @@ class PyRobotWrapper { public: // 构造时只需要传入 config_path 和 robot_name（只在第一次初始化有效） PyRobotWrapper(const std::string& config_path, const std::string& robot_name) { - // 如果已经初始化过，则直接返回 + std::lock_guard lock(init_mutex); // 线程安全 if (!robot_) { const XmlNode config(config_path); if (!config.hasChild("DeviceManager")) { @@ -31,84 +33,138 @@ public: } } - void moveJ(const std::string &side, const std::vector &q) { + void moveJ(const std::string &side, const std::vector &q, double speed=0.8) { if (q.size() != 7) throw std::runtime_error("Expected 7 joint values"); - std::vector joint_names; - if (side == "left") { - joint_names = {"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", - "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R"}; - } else if (side == "right") { - joint_names = {"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", - "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"}; - } else { - throw std::runtime_error("Side must be 'left' or 'right'"); - } + const auto& joint_names = getJointNames(side); - std::vector cmd; - cmd.push_back({"WAIST_Y", 0}); - cmd.push_back({"WAIST_P", 0}); + std::vector cmd = {{"WAIST_Y", 0}, {"WAIST_P", 0}}; for (size_t i = 0; i < 7; ++i) cmd.push_back({joint_names[i], q[i]}); - robot_->moveJ(cmd, 0.8); + robot_->moveJ(cmd, speed); } - void torqueOn() { - if (!robot_) - throw std::runtime_error("Robot not initialized"); + checkRobotInit(); robot_->torqueOn(); } + void torqueOn(const std::string& joint_name) { + checkRobotInit(); + robot_->torqueOn(joint_name); + } + void torqueOff() { - if (!robot_) - throw std::runtime_error("Robot not initialized"); + checkRobotInit(); robot_->torqueOff(); } + void torqueOff(const std::string& joint_name) { + checkRobotInit(); + robot_->torqueOff(joint_name); + } + + void calibrateZeroQ(const std::string &joint_name) { + checkRobotInit(); + robot_->calibrateZeroQ(joint_name); + } + std::vector getJointQ(const std::string& side) const { - if (!robot_) - throw std::runtime_error("Robot not initialized"); + checkRobotInit(); auto joint_qs = robot_->getJointQ(); // unordered_map - - std::vector joint_names; - if (side == "left") { - joint_names = {"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", - "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R"}; - } else if (side == "right") { - joint_names = {"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", - "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"}; - } else { - throw std::runtime_error("Side must be 'left' or 'right'"); - } + const auto& joint_names = getJointNames(side); std::vector values; for (const auto& name : joint_names) { - values.push_back(joint_qs[name]); + values.push_back(joint_qs.at(name)); } return values; } + std::vector ik(const std::string& side, + const std::string &base_link, + const std::string &ee_link, + double x, double y, double z, + double rx, double ry, double rz) + { + checkRobotInit(); + cmvr::msgs::Pose3d pose; + pose.mutable_position()->set_x(x); + pose.mutable_position()->set_y(y); + pose.mutable_position()->set_z(z); + + pose.mutable_euler()->set_rx(rx); + pose.mutable_euler()->set_ry(ry); + pose.mutable_euler()->set_rz(rz); + + auto js_cmd = robot_->ik(base_link, ee_link, pose); + + if (js_cmd.size() < 14) { + throw std::runtime_error("IK solution size is smaller than expected"); + } + + if (side == "left") { + return std::vector(js_cmd.begin(), js_cmd.begin() + 7); + } else if (side == "right") { + return std::vector(js_cmd.begin() + 7, js_cmd.begin() + 14); + } else { + throw std::runtime_error("Side must be 'left' or 'right'"); + } + } private: static DeviceManager* dmgr_; static std::shared_ptr robot_; + static std::mutex init_mutex; + + static const std::vector LEFT_JOINTS; + static const std::vector RIGHT_JOINTS; + + void checkRobotInit() const { + if (!robot_) + throw std::runtime_error("Robot not initialized"); + } + + const std::vector& getJointNames(const std::string& side) const { + if (side == "left") return LEFT_JOINTS; + if (side == "right") return RIGHT_JOINTS; + throw std::runtime_error("Side must be 'left' or 'right'"); + } }; // 静态成员初始化 DeviceManager* PyRobotWrapper::dmgr_ = nullptr; std::shared_ptr PyRobotWrapper::robot_ = nullptr; +std::mutex PyRobotWrapper::init_mutex; +const std::vector PyRobotWrapper::LEFT_JOINTS = {"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", + "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R"}; +const std::vector PyRobotWrapper::RIGHT_JOINTS = {"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", + "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"}; + +// Python 绑定 PYBIND11_MODULE(robot_wrapper, m) { py::class_(m, "Robot") - .def(py::init()) - .def("moveJ", &PyRobotWrapper::moveJ, py::arg("side"), py::arg("q")) - .def("torqueOn", &PyRobotWrapper::torqueOn) - .def("torqueOff", &PyRobotWrapper::torqueOff) - .def("getJointQ", &PyRobotWrapper::getJointQ, py::arg("side")); + .def(py::init(), + py::arg("config_path"), py::arg("robot_name"), + "Initialize the robot wrapper with config and robot name") + .def("moveJ", &PyRobotWrapper::moveJ, py::arg("side"), py::arg("q"), py::arg("speed")=0.8, + "Move robot joints in joint space with optional speed") + .def("torqueOn", py::overload_cast<>(&PyRobotWrapper::torqueOn)) + .def("torqueOn", py::overload_cast(&PyRobotWrapper::torqueOn)) + .def("torqueOff", py::overload_cast<>(&PyRobotWrapper::torqueOff)) + .def("torqueOff", py::overload_cast(&PyRobotWrapper::torqueOff)) + .def("calibrateZeroQ", &PyRobotWrapper::calibrateZeroQ) + .def("getJointQ", &PyRobotWrapper::getJointQ, py::arg("side"), + "Get joint positions for the specified side ('left' or 'right')") + .def("ik", &PyRobotWrapper::ik, + py::arg("side"), py::arg("base_link"), py::arg("ee_link"), + py::arg("x"), py::arg("y"), py::arg("z"), + py::arg("rx"), py::arg("ry"), py::arg("rz"), + "Compute inverse kinematics and return 7 joint values for the given side"); } diff --git a/example/solve_fk.cpp b/example/solve_fk.cpp index ad52beda..890c634f 100644 --- a/example/solve_fk.cpp +++ b/example/solve_fk.cpp @@ -11,7 +11,7 @@ constexpr int DOF = 14; using RobotT = cmvr::dyn::Robot; using StateT = cmvr::dyn::State; -static const char* kUrdfPath = "/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"; +static const char* kUrdfPath = "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"; int main(int argc, char** argv) { if (argc < 10) { diff --git a/example/solve_ik.cpp b/example/solve_ik.cpp index c0ab6c13..8f679656 100644 --- a/example/solve_ik.cpp +++ b/example/solve_ik.cpp @@ -87,7 +87,7 @@ int main(int argc, char **argv) // 1. 加载机器人模型 auto rcfg = LoadRobotFromURDF( - "/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf", + "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf", /*root*/ "PELVIS_S"); auto robot = std::make_shared(rcfg); @@ -96,7 +96,7 @@ int main(int argc, char **argv) // 初始关节角度 Eigen::Vector q_init; q_init << -0.3647738137, -1.4556045962, 1.6057029118, -1.8483036779, 2.9024825461, 0.1850049007, -0.5602506899, - -0.3448998226801712, 0.9351815151808957, 2.2703399775931388, 1.6895896595300133, -2.3279553847104077, 0.46019613485676647, 0.3009592467697589; + -0.23905230057945082, 1.2838583858530206, 1.8486727698400174, 1.3036511045462158, -2.824727848601091, -0.02012495463116552, -0.202540520242054; state->SetQ(q_init); robot->ComputeForwardKinematics(state); diff --git a/include/controller/touch_controller.h b/include/controller/touch_controller.h index 9fa8dbb9..a92f687a 100644 --- a/include/controller/touch_controller.h +++ b/include/controller/touch_controller.h @@ -6,6 +6,7 @@ #include +#include "devices/abstract_camera.h" #include "devices/abstract_dexhand.h" #include "devices/abstract_robot.h" #include "cmvr/msgs/geometry.pb.h" @@ -79,16 +80,16 @@ namespace cmvr { class TouchController { public: - TouchController(){}; - TouchController( std::shared_ptr robot,std::shared_ptr hand) - :robot_(std::move(robot)),hand_(std::move(hand)), + TouchController() {}; + TouchController(std::shared_ptr robot,std::shared_ptr hand,std::shared_ptr cam) + :robot_(std::move(robot)),hand_(std::move(hand)),cam_(std::move(cam)), pid_(std::make_shared(0.005, 0.001, 0.001, 5000.0, 0.5, 1.0)){} ~TouchController()=default; bool isArrive(double max_force); - void touch(int u,int v ,double max_force){} + void touch(int u,int v ,double max_force); void touch(std::shared_ptr robot,const msgs::Pose3d pose,const msgs::Pose3d offset); void touch( msgs::Pose3d pose, msgs::Pose3d offset,double max_force); @@ -96,6 +97,7 @@ namespace cmvr { private: std::shared_ptr robot_{nullptr}; std::shared_ptr hand_{nullptr}; + std::shared_ptr cam_{nullptr}; std::shared_ptr pid_{nullptr}; diff --git a/include/devices/abstract_motor.h b/include/devices/abstract_motor.h index 84ab9397..d0b8d487 100644 --- a/include/devices/abstract_motor.h +++ b/include/devices/abstract_motor.h @@ -146,6 +146,13 @@ namespace cmvr::device{ return protocol_->getQ(node_id_); } + virtual double getQd() { + if (!protocol_) { + throw std::runtime_error("Protocol not set for motor"); + } + return protocol_->getQd(node_id_); + } + // 使用的通讯协议 virtual void setProtocol(std::shared_ptr protocol) { diff --git a/include/devices/abstract_robot.h b/include/devices/abstract_robot.h index 5cbd5ad0..667eeb67 100644 --- a/include/devices/abstract_robot.h +++ b/include/devices/abstract_robot.h @@ -10,6 +10,8 @@ #include "abstract_device.h" #include "utils/controller/cartesian_controller.h" #include "cmvr/msgs/geometry.pb.h" +#include "cmvr/msgs/motor.pb.h" + namespace cmvr::device{ struct JointPoint{ @@ -53,13 +55,16 @@ namespace cmvr::device{ virtual void getJointQ(std::unordered_map &joint_qs) const {throw std::runtime_error("Not implemented");} virtual std::vector getLinkNames() { throw std::runtime_error("Not implemented"); } + virtual void getJointsState(std::vector& states) = 0; virtual void getState(RobotState &state) { throw std::runtime_error("Not implemented"); } virtual math::Pose3d getTransform(std::string &bask_link, std::string &target_link) {throw std::runtime_error("Not implemented");} virtual void torqueOn() { throw std::runtime_error("Not implemented"); } + virtual void torqueOn(const std::string &joint_name) { throw std::runtime_error("Not implemented"); } virtual void torqueOff() { throw std::runtime_error("Not implemented"); } + virtual void torqueOff(const std::string &joint_name) { throw std::runtime_error("Not implemented"); } virtual void eStop() { throw std::runtime_error("Not implemented"); } @@ -110,6 +115,9 @@ namespace cmvr::device{ { toolFrame_ = toolFrame; } + + virtual msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0; + virtual std::vector ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) = 0; protected: int dof_{}; RobotState state_{}; diff --git a/include/devices/state_define.h b/include/devices/state_define.h index 0835fe65..5c787521 100644 --- a/include/devices/state_define.h +++ b/include/devices/state_define.h @@ -62,6 +62,7 @@ namespace cmvr::device{ } RobotCartesian; typedef struct { + std::string name; bool is_ready; bool power_on; double position; diff --git a/include/service/grpc_humanoid_robot_service.h b/include/service/grpc_humanoid_robot_service.h index cbb3c5c3..498522a1 100644 --- a/include/service/grpc_humanoid_robot_service.h +++ b/include/service/grpc_humanoid_robot_service.h @@ -25,6 +25,8 @@ namespace cmvr { grpc::Status speedJ(grpc::ServerContext* context, const cmvr::api::SpeedJ_Request* request, cmvr::api::SpeedJ_Response* response) override; grpc::Status speedL(grpc::ServerContext* context, const cmvr::api::SpeedL_Request* request, cmvr::api::SpeedL_Response* response) override; + + grpc::Status getJointState(grpc::ServerContext *context, const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) override; private: device::DeviceManager& dmgr_; }; diff --git a/protos/cmvr/api/humanoid_robot.proto b/protos/cmvr/api/humanoid_robot.proto index 910d9053..cc17f220 100644 --- a/protos/cmvr/api/humanoid_robot.proto +++ b/protos/cmvr/api/humanoid_robot.proto @@ -100,6 +100,27 @@ message SpeedL{ } } +// 关节状态消息 +message JointState { + repeated string name = 1; // 关节名称 + repeated double position = 2; // 关节位置 + repeated double velocity = 3; // 可选：关节速度 + repeated double effort = 4; // 可选：关节力矩 + double timestamp = 5; // 可选：时间戳 +} + +// 返回消息 +message JointResponse { + CommandHeader.Feedback header= 1; + repeated JointState state = 2; +} + +// 请求消息（可选） +message JointRequest { + CommandHeader.Request header = 1; +} + + service HumanoidRobotService{ rpc torqueOff(CommandHeader.Request) returns (CommandHeader.Feedback); rpc torqueOn(CommandHeader.Request) returns (CommandHeader.Feedback); @@ -107,4 +128,5 @@ service HumanoidRobotService{ rpc moveL(MoveL.Request) returns (MoveL.Response); rpc speedJ(SpeedJ.Request) returns (SpeedJ.Response); rpc speedL(SpeedL.Request) returns (SpeedL.Response); + rpc getJointState(JointRequest) returns (JointResponse); } diff --git a/python/hand_eye_calibration/collect_data.py b/python/hand_eye_calibration/collect_data.py index 898dd213..9b2a0d2d 100644 --- a/python/hand_eye_calibration/collect_data.py +++ b/python/hand_eye_calibration/collect_data.py @@ -90,7 +90,7 @@ def callback_and_save(frame): js = robot.getJointQ('right') logger.info(f"当前关节角: {js}") - pose = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S") + pose = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") logger.info(f"获取位姿成功:\n{pose}") logger.info("请按 p 失能机器人，并手动移动机器人到其他位姿") @@ -113,28 +113,9 @@ def callback_and_save(frame): return True -def init_realsense(serial=TARGET_SERIAL): - """初始化 RealSense 相机""" - ctx = rs.context() - devices = ctx.query_devices() - if len(devices) == 0: - raise RuntimeError("未检测到任何 RealSense 相机") - if not any(dev.get_info(rs.camera_info.serial_number) == serial for dev in devices): - raise RuntimeError(f"未找到序列号为 {serial} 的 RealSense 相机") - pipeline = rs.pipeline() - config = rs.config() - config.enable_device(serial) - config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30) - - try: - pipeline.start(config) - except Exception as e: - raise RuntimeError(f"相机连接异常: {e}") - return pipeline - -def start_pipeline(serial=TARGET_SERIAL, depth_size=(848, 480), color_size=(1280, 720), fps: int = 30): +def start_pipeline(serial=TARGET_SERIAL, depth_size=(1280, 720), color_size=(1280, 720), fps: int = 30): ctx = rs.context() devices = ctx.query_devices() if len(devices) == 0: diff --git a/python/hand_eye_calibration/compute_in_hand.py b/python/hand_eye_calibration/compute_in_hand.py index fd3c49e6..d733fe79 100644 --- a/python/hand_eye_calibration/compute_in_hand.py +++ b/python/hand_eye_calibration/compute_in_hand.py @@ -40,6 +40,85 @@ YY = data.get("checkerboard_args").get("YY") #标定板的中宽度对应的角 L = data.get("checkerboard_args").get("L") #标定板一格的长度 单位为米 + +import numpy as np + +def eulerZYXToRotationMatrix(rx, ry, rz): + # 绕 x 轴的旋转矩阵 + R_x = np.array([[1, 0, 0], + [0, np.cos(rx), -np.sin(rx)], + [0, np.sin(rx), np.cos(rx)]]) + + # 绕 y 轴的旋转矩阵 + R_y = np.array([[np.cos(ry), 0, np.sin(ry)], + [0, 1, 0], + [-np.sin(ry), 0, np.cos(ry)]]) + + # 绕 z 轴的旋转矩阵 + R_z = np.array([[np.cos(rz), -np.sin(rz), 0], + [np.sin(rz), np.cos(rz), 0], + [0, 0, 1]]) + + # 使用 @ 运算符进行矩阵乘法：R = R_x @ R_y @ R_z + return R_x @ R_y @ R_z + +def eulerXYZToRotationMatrix(rx, ry, rz): + # 绕 x 轴的旋转矩阵 + R_x = np.array([[1, 0, 0], + [0, np.cos(rx), -np.sin(rx)], + [0, np.sin(rx), np.cos(rx)]]) + + # 绕 y 轴的旋转矩阵 + R_y = np.array([[np.cos(ry), 0, np.sin(ry)], + [0, 1, 0], + [-np.sin(ry), 0, np.cos(ry)]]) + + # 绕 z 轴的旋转矩阵 + R_z = np.array([[np.cos(rz), -np.sin(rz), 0], + [np.sin(rz), np.cos(rz), 0], + [0, 0, 1]]) + + # 使用 @ 运算符进行矩阵乘法：R = R_x @ R_y @ R_z + return R_z @ R_y @ R_x + +def rotationMatrixToEulerZYX(R): + # 提取 ry（绕 Y 轴的旋转角度） + ry = np.arcsin(R[0, 2]) # R(0, 2) = sin(ry) + + cy = np.cos(ry) + + if np.abs(cy) > 1e-6: # 正常情况 + rx = np.arctan2(-R[1, 2], R[2, 2]) + rz = np.arctan2(-R[0, 1], R[0, 0]) + else: # 万向节锁：cy ≈ 0 + rx = 0 # 任意选择 + if ry > 0: + rz = np.arctan2(R[1, 0], R[1, 1]) + else: + rz = np.arctan2(-R[1, 0], R[1, 1]) + + return np.array([rx, ry, rz]) + +def rotationMatrixToEulerXYZ(R): + # 提取 ry（绕 Y 轴的旋转角度） + ry = np.arcsin(-R[2, 0]) # R(2, 0) = -sin(ry) + + cy = np.cos(ry) + + if np.abs(cy) > 1e-6: # 正常情况 + rx = np.arctan2(R[2, 1], R[2, 2]) + rz = np.arctan2(R[1, 0], R[0, 0]) + else: # 万向节锁：cy ≈ 0 + rx = 0 # 任意选择 + if ry > 0: + rz = np.arctan2(-R[1, 2], R[1, 1]) + else: + rz = np.arctan2(R[1, 2], R[1, 1]) + + return np.array([rx, ry, rz]) + + + def func(): @@ -113,14 +192,37 @@ def func(): for i in range(N): mat = tool_pose[4*i:4*i+4, :] # 取第i个4x4矩阵 + + # # 提取旋转矩阵 + # R_zyx = mat[0:3, 0:3] + # + # # 将 ZYX 顺序的旋转矩阵转换为欧拉角（ZYX 顺序） + # euler_zyx = rotationMatrixToEulerZYX(R_zyx) + # + # # 将欧拉角转换为 XYZ 顺序的旋转矩阵 + # R_xyz = eulerXYZToRotationMatrix(euler_zyx[0], euler_zyx[1], euler_zyx[2]) + # + # # 将转换后的旋转矩阵添加到 R_tool + # R_tool.append(R_xyz) + # + # # # 将 ZYX 顺序的旋转矩阵转换为欧拉角（返回值是 rx, ry, rz） + # # euler_zyx = R.from_matrix(R_zyx).as_euler('zyx', degrees=False) + # # + # # # 将欧拉角重新转换为 XYZ 顺序的旋转矩阵 + # # R_xyz = R.from_euler('xyz', euler_zyx, degrees=False).as_matrix() + # # + # # # 将转换后的旋转矩阵添加到 R_tool + # # R_tool.append(R_xyz) + + R_tool.append(mat[0:3, 0:3]) # 提取旋转矩阵 t_tool.append(mat[0:3, 3]) # 提取平移向量 - R, t = cv2.calibrateHandEye(R_tool, t_tool, rvecs, tvecs, cv2.CALIB_HAND_EYE_TSAI) + R1, t = cv2.calibrateHandEye(R_tool, t_tool, rvecs, tvecs, cv2.CALIB_HAND_EYE_TSAI) - return R,t + return R1,t if __name__ == '__main__': @@ -137,3 +239,4 @@ if __name__ == '__main__': logger_.info(f"平移向量是:\n { translation_vector}") logger_.info(f"四元数是：\n { quaternion}") + diff --git a/python/hand_eye_calibration/libs/web-log b/python/hand_eye_calibration/libs/web-log index 30302d29..b0db76de 100644 --- a/python/hand_eye_calibration/libs/web-log +++ b/python/hand_eye_calibration/libs/web-log @@ -1,46 +1,256 @@ -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg -2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg -2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg -2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg -2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg -2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg -2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg -2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg -2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg -2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg -2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg -2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg -2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg -2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg -2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg -2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg -2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg -2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg -2025-08-23 16:08:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg -2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg -2025-08-23 16:08:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg -2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg -2025-08-23 16:08:46 - __main__ - INFO: 旋转矩阵是: +2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg +2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg +2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg +2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg +2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg +2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg +2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg +2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg +2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg +2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg +2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg +2025-09-08 14:26:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg +2025-09-08 14:26:53 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg +2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg +2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg +2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg +2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg +2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg +2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg +2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg +2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg +2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg +2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg +2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg +2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg +2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg +2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg +2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg +2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg +2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg +2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg +2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg +2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg +2025-09-08 14:26:54 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg +2025-09-08 14:26:54 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg +2025-09-08 14:26:55 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg +2025-09-08 14:26:55 - __main__ - INFO: 旋转矩阵是: [[-0.99486349 0.07846371 0.06395369] [-0.05884529 0.06577685 -0.9960977 ] [-0.08236419 -0.99474462 -0.06082177]] -2025-08-23 16:08:46 - __main__ - INFO: 平移向量是: +2025-09-08 14:26:55 - __main__ - INFO: 平移向量是: [[-0.00797824] [-0.17640328] [ 0.07459845]] -2025-08-23 16:08:46 - __main__ - INFO: 四元数是： +2025-09-08 14:26:55 - __main__ - INFO: 四元数是： + [ 0.00673467 0.72826199 -0.68342243 0.05022845] +2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg +2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg +2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg +2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg +2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg +2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg +2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg +2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg +2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg +2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg +2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg +2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg +2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg +2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg +2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg +2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg +2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg +2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg +2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg +2025-09-08 14:29:37 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg +2025-09-08 14:29:37 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg +2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg +2025-09-08 14:29:38 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg +2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg +2025-09-08 14:29:38 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg +2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg +2025-09-08 14:29:38 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg +2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg +2025-09-08 14:29:38 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg +2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg +2025-09-08 14:29:38 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg +2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg +2025-09-08 14:29:38 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg +2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg +2025-09-08 14:29:38 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg +2025-09-08 14:29:38 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg +2025-09-08 14:29:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg +2025-09-08 14:29:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg +2025-09-08 14:29:53 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg +2025-09-08 14:30:52 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg +2025-09-08 14:30:52 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg +2025-09-08 14:30:53 - __main__ - INFO: 旋转矩阵是: + [[ 0.72638143 0.22269568 -0.65021278] + [ 0.420903 0.60375531 0.67699349] + [ 0.54333294 -0.76543201 0.34482337]] +2025-09-08 14:30:53 - __main__ - INFO: 平移向量是: + [[ 0.15849038] + [-0.26275262] + [-0.05082269]] +2025-09-08 14:30:53 - __main__ - INFO: 四元数是： + [-0.44096563 -0.36488029 0.0605942 0.81776526] +2025-09-08 14:32:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg +2025-09-08 14:32:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg +2025-09-08 14:32:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg +2025-09-08 14:32:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg +2025-09-08 14:32:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg +2025-09-08 14:32:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg +2025-09-08 14:32:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg +2025-09-08 14:32:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg +2025-09-08 14:32:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg +2025-09-08 14:32:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg +2025-09-08 14:32:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg +2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg +2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg +2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg +2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg +2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg +2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg +2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg +2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg +2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg +2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg +2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg +2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg +2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg +2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg +2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg +2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg +2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg +2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg +2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg +2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg +2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg +2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg +2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg +2025-09-08 14:32:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg +2025-09-08 14:32:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg +2025-09-08 14:32:27 - __main__ - INFO: 旋转矩阵是: + [[ 0.05477568 0.03458859 0.99789942] + [-0.99767126 0.04257346 0.0532875 ] + [-0.04064089 -0.99849443 0.03684003]] +2025-09-08 14:32:27 - __main__ - INFO: 平移向量是: + [[-0.14908435] + [-0.19737896] + [-0.05964516]] +2025-09-08 14:32:27 - __main__ - INFO: 四元数是： + [-0.49380213 0.48758531 -0.48463669 0.53249159] +2025-09-08 14:37:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg +2025-09-08 14:37:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg +2025-09-08 14:37:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg +2025-09-08 14:37:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg +2025-09-08 14:37:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg +2025-09-08 14:37:25 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg +2025-09-08 14:37:25 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg +2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg +2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg +2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg +2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg +2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg +2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg +2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg +2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg +2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg +2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg +2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg +2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg +2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg +2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg +2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg +2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg +2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg +2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg +2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg +2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg +2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg +2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg +2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg +2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg +2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg +2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg +2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg +2025-09-08 14:37:26 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg +2025-09-08 14:37:26 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg +2025-09-08 14:37:26 - __main__ - INFO: 旋转矩阵是: + [[-0.99486349 0.07846371 0.06395369] + [-0.05884529 0.06577685 -0.9960977 ] + [-0.08236419 -0.99474462 -0.06082177]] +2025-09-08 14:37:26 - __main__ - INFO: 平移向量是: + [[-0.00797824] + [-0.17640328] + [ 0.07459845]] +2025-09-08 14:37:26 - __main__ - INFO: 四元数是： [ 0.00673467 0.72826199 -0.68342243 0.05022845] diff --git a/python/hand_eye_calibration/libs/web-log.2025-08-28.log b/python/hand_eye_calibration/libs/web-log.2025-08-28.log new file mode 100644 index 00000000..30302d29 --- /dev/null +++ b/python/hand_eye_calibration/libs/web-log.2025-08-28.log @@ -0,0 +1,46 @@ +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg +2025-08-23 16:08:45 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg +2025-08-23 16:08:45 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg +2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg +2025-08-23 16:08:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg +2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg +2025-08-23 16:08:46 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg +2025-08-23 16:08:46 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg +2025-08-23 16:08:46 - __main__ - INFO: 旋转矩阵是: + [[-0.99486349 0.07846371 0.06395369] + [-0.05884529 0.06577685 -0.9960977 ] + [-0.08236419 -0.99474462 -0.06082177]] +2025-08-23 16:08:46 - __main__ - INFO: 平移向量是: + [[-0.00797824] + [-0.17640328] + [ 0.07459845]] +2025-08-23 16:08:46 - __main__ - INFO: 四元数是： + [ 0.00673467 0.72826199 -0.68342243 0.05022845] diff --git a/python/hand_eye_calibration/libs/web-log.2025-09-05.log b/python/hand_eye_calibration/libs/web-log.2025-09-05.log new file mode 100644 index 00000000..858d0681 --- /dev/null +++ b/python/hand_eye_calibration/libs/web-log.2025-09-05.log @@ -0,0 +1,167 @@ +2025-09-05 16:02:35 - libs.auxiliary - INFO: create folder eye_hand_data/data20250905 +2025-09-05 16:04:01 - libs.auxiliary - INFO: create folder eye_hand_data/data2025090501 +2025-09-05 16:04:01 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0 +2025-09-05 16:05:28 - libs.auxiliary - INFO: create folder eye_hand_data/data2025090502 +2025-09-05 16:05:28 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0 +2025-09-05 16:05:42 - libs.auxiliary - INFO: create folder eye_hand_data/data2025090503 +2025-09-05 16:05:42 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0 +2025-09-05 16:05:48 - __main__ - INFO: 相机已关闭，窗口已销毁 +2025-09-05 16:09:41 - libs.auxiliary - INFO: create folder eye_hand_data/data2025090504 +2025-09-05 16:09:41 - __main__ - INFO: 开始机器人数据采集程序，版本 V1.0.0 +2025-09-05 16:12:46 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人 +2025-09-05 16:12:53 - __main__ - INFO: 当前关节角: [-0.020066101407867926, 1.46894512840519, 1.4589372327079881, 0.15968113499835065, 0.0808671765475035, -0.13818357132059936, -0.2431388026164853] +2025-09-05 16:12:53 - __main__ - INFO: 获取位姿成功: +[[-0.10992689 0.05250327 -0.99255201 -0.00608509] + [ 0.0426073 -0.99743702 -0.0574805 -0.22011888] + [-0.99302603 -0.04860861 0.10740812 -0.73570586] + [ 0. 0. 0. 1. ]] +2025-09-05 16:12:53 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿 +2025-09-05 16:12:53 - __main__ - INFO: ===采集第1次数据！ pose 已保存到 eye_hand_data/data2025090504/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025090504/1.jpg +2025-09-05 16:12:56 - __main__ - INFO: robot.torqueOff +2025-09-05 16:25:21 - __main__ - INFO: robot.torqueOn ，请勿触摸机器人 +2025-09-05 16:25:26 - __main__ - INFO: 当前关节角: [-0.019045662455530632, 0.14962957400644405, 1.3928232302974393, -0.07693350304193537, -1.3815034772680232, 0.03932344561816248, -0.20795691527650662] +2025-09-05 16:25:26 - __main__ - INFO: 获取位姿成功: +[[-0.03644065 0.99920824 0.01596754 -0.01550389] + [-0.99665653 -0.03750756 0.07258755 -0.94854757] + [ 0.07312898 -0.01326902 0.99723422 -0.00291912] + [ 0. 0. 0. 1. ]] +2025-09-05 16:25:26 - __main__ - INFO: 请按 p 失能机器人，并手动移动机器人到其他位姿 +2025-09-05 16:25:26 - __main__ - INFO: ===采集第2次数据！ pose 已保存到 eye_hand_data/data2025090504/RobotToolPose.csv, 图片保存到 eye_hand_data/data2025090504/2.jpg +2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg +2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg +2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg +2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg +2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg +2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg +2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg +2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg +2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg +2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg +2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg +2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg +2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg +2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg +2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg +2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg +2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg +2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg +2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg +2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg +2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg +2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg +2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg +2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg +2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg +2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg +2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg +2025-09-05 17:21:03 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg +2025-09-05 17:21:03 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg +2025-09-05 17:21:04 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg +2025-09-05 17:21:04 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg +2025-09-05 17:21:04 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg +2025-09-05 17:21:04 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg +2025-09-05 17:21:04 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg +2025-09-05 17:21:04 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg +2025-09-05 17:21:04 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg +2025-09-05 17:21:04 - __main__ - INFO: 旋转矩阵是: + [[ 0.05477568 0.03458859 0.99789942] + [-0.99767126 0.04257346 0.0532875 ] + [-0.04064089 -0.99849443 0.03684003]] +2025-09-05 17:21:04 - __main__ - INFO: 平移向量是: + [[-0.14908435] + [-0.19737896] + [-0.05964516]] +2025-09-05 17:21:04 - __main__ - INFO: 四元数是： + [-0.49380213 0.48758531 -0.48463669 0.53249159] +2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg +2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg +2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg +2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg +2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg +2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg +2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg +2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg +2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg +2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg +2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg +2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg +2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg +2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg +2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg +2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg +2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg +2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg +2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg +2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg +2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg +2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg +2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg +2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg +2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg +2025-09-05 17:26:27 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg +2025-09-05 17:26:27 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg +2025-09-05 17:26:28 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg +2025-09-05 17:26:28 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg +2025-09-05 17:26:28 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg +2025-09-05 17:26:28 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg +2025-09-05 17:26:28 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg +2025-09-05 17:26:28 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg +2025-09-05 17:26:28 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg +2025-09-05 17:26:28 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg +2025-09-05 17:26:28 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg +2025-09-05 17:26:28 - __main__ - INFO: 旋转矩阵是: + [[-0.99486349 0.07846371 0.06395369] + [-0.05884529 0.06577685 -0.9960977 ] + [-0.08236419 -0.99474462 -0.06082177]] +2025-09-05 17:26:28 - __main__ - INFO: 平移向量是: + [[-0.00797824] + [-0.17640328] + [ 0.07459845]] +2025-09-05 17:26:28 - __main__ - INFO: 四元数是： + [ 0.00673467 0.72826199 -0.68342243 0.05022845] +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/1.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_1.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/2.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_2.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/3.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_3.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/4.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_4.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/5.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_5.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/6.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_6.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/7.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_7.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/8.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_8.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/9.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_9.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/10.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_10.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/11.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_11.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/12.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_12.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/13.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_13.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/14.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_14.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/15.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_15.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/16.jpg +2025-09-05 17:45:12 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_16.jpg +2025-09-05 17:45:12 - __main__ - INFO: 读 eye_hand_data/data2025082101/17.jpg +2025-09-05 17:45:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_17.jpg +2025-09-05 17:45:13 - __main__ - INFO: 读 eye_hand_data/data2025082101/18.jpg +2025-09-05 17:45:13 - __main__ - INFO: 保存带角点的图片到: eye_hand_data/data2025082101/corner/corner_18.jpg +2025-09-05 17:45:13 - __main__ - INFO: 旋转矩阵是: + [[-0.99486349 0.07846371 0.06395369] + [-0.05884529 0.06577685 -0.9960977 ] + [-0.08236419 -0.99474462 -0.06082177]] +2025-09-05 17:45:13 - __main__ - INFO: 平移向量是: + [[-0.00797824] + [-0.17640328] + [ 0.07459845]] +2025-09-05 17:45:13 - __main__ - INFO: 四元数是： + [ 0.00673467 0.72826199 -0.68342243 0.05022845] 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 e8fea3e1..4ded1209 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 1e908869..3478c4f5 100644 --- a/python/vision_servo/follow_target.py +++ b/python/vision_servo/follow_target.py @@ -1,20 +1,290 @@ import os -os.environ["GLOG_minloglevel"] = "1" - -import sys -from utils import root_path -sys.path.append(os.path.join(root_path, "cmake-build-debug/example")) -from robot_wrapper import Robot - +import re import time import signal +import cv2 +import subprocess import numpy as np +import pyrealsense2 as rs from scipy.spatial.transform import Rotation as R -from utils import solve_ik -from utils import get_pose -from target_position import init_realsense, get_closest_red_point +from get_chessboard_position import init_realsense, get_closest_red_point + +import sys +# 把.so所在目录加入 Python 路径 +sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example") + +# 导入模块 +from robot_wrapper import Robot + + +def get_pose(joint_positions, base_link="PELVIS_S", target_link="R_FINGER_TIP", exec_path="/home/lgv/cmvr/cmvr-es/cmake-build-debug/example/solve_fk"): + # ----------- 1. 构造命令行参数 ----------- + joint_str = [str(j) for j in joint_positions] + cmd = [exec_path, base_link, target_link, *joint_str] + # ----------- 2. 运行并捕获输出 ----------- + res = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, check=True, text=True) + + # ----------- 3. 解析 4×4 变换矩阵 ----------- + # 定位起始行（含 “变换矩阵T” 或 “T:”） + lines = res.stdout.splitlines() + start_idx = next((i for i, l in enumerate(lines) if re.search(r"变换矩阵|T\s*:", l)), None) + + if start_idx is None or start_idx + 4 >= len(lines): + raise RuntimeError("未在 solve_fk 输出中找到 4×4 变换矩阵:\n" + res.stdout) + + try: + mat = np.array([[float(x) for x in lines[start_idx + 1 + r].split()] for r in range(4)], dtype=np.float64) + if mat.shape != (4, 4): + raise ValueError + except Exception: + raise RuntimeError("矩阵解析失败，原始输出:\n" + res.stdout) + return mat + + +def solve_ik(x, y, z, rx, ry, rz, base_link="PELVIS_S", target_link="R_WRIST_R_S", + exe_path= "/home/lgv/cmvr/cmvr-es/cmake-build-debug/example/solve_ik"): + cmd = [exe_path, base_link, target_link, f"{x}", f"{y}", f"{z}", f"{rx}", f"{ry}", f"{rz}"] + print(" ".join(cmd)) + res = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, check=True) + lines = res.stdout.splitlines() + + success_line = next((l for l in lines if "[IK Solve]" in l), "") + success = "success=true" in success_line.lower() + + if not success: + return False, None, None + + def _parse(tag): + pats = rf"^{tag}\s*:\s*(.+)$" + for l in lines: + m = re.match(pats, l.strip()) + if m: + return [float(v) for v in m.group(1).split()] + raise RuntimeError(f"success=true 但未找到 {tag}: 行！\n{res.stdout}") + + left_q = _parse("left") + right_q = _parse("right") + return True, left_q, right_q + + +def moveJ(q, side='right'): + cmd = ["moveJ" ,"one" , f"{side}", f"{q[0]}", f"{q[1]}", f"{q[2]}", f"{q[3]}", f"{q[4]}", f"{q[5]}", f"{q[6]}"] + res = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True, check=True) + print("movej success") + + +def rotation_to_degree(mat): + pitch = -np.arcsin(mat[2, 0]) + if np.abs(np.cos(pitch)) > 1e-6: # 非奇异 + roll = np.arctan2(mat[2, 1], mat[2, 2]) + yaw = np.arctan2(mat[1, 0], mat[0, 0]) + else: # gimbal lock + roll = 0.0 + yaw = np.arctan2(-mat[0, 1], mat[1, 1]) + return yaw, pitch, roll + +def black_point_position(color_frame, depth_frame, depth_intr, vis_path=None, min_radius=10, avg_window=3): + """ + 检测白底黑圆的圆心坐标，返回相机系 (X, Y, Z) [m] + + 参数 + ---- + color_frame / depth_frame : 对齐后的 RealSense frame + depth_intr : 深度流 intrinsics (rs.intrinsics) + vis_path : 若给定，则保存可视化图片 + min_radius : HoughCircles/轮廓的最小半径，像素 + avg_window : 深度均值窗口半径（像素） + """ + color_img = np.asanyarray(color_frame.get_data()).copy() + gray = cv2.cvtColor(color_img, cv2.COLOR_BGR2GRAY) + + # 1. 二值化（寻找黑色区域） + # Otsu 自动阈值 + 取反 => 黑圆为白，背景为黑 + _, mask = cv2.threshold(gray, 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU) + + # 2. 轮廓检测，取面积最大的圆形候选 + cnts, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + if not cnts: + raise RuntimeError("未检测到任何黑色区域") + + # 取最大面积 + cnt = max(cnts, key=cv2.contourArea) + (u, v), radius = cv2.minEnclosingCircle(cnt) + + if radius < min_radius: + raise RuntimeError(f"检测到圆半径过小 ({radius:.1f}px)，请检查 min_radius 设置或图像质量") + + # 3. 取邻域深度中值 + width, height = depth_frame.get_width(), depth_frame.get_height() + depths = [ + depth_frame.get_distance(int(round(u + du)), int(round(v + dv))) + for du in range(-avg_window, avg_window + 1) + for dv in range(-avg_window, avg_window + 1) + if 0 <= int(round(u + du)) < width and + 0 <= int(round(v + dv)) < height + ] + depths = [d for d in depths if d > 0] + if not depths: + raise RuntimeError("圆心处深度无效 (0)") + + depth = float(np.median(depths)) + + # 4. 像素 -> 相机坐标 + x, y, z = rs.rs2_deproject_pixel_to_point( + depth_intr, [u, v], depth + ) + pos = np.array([x, y, z], dtype=np.float32) + + # 5. 可视化 + if vis_path is not None: + cx, cy = depth_intr.ppx, depth_intr.ppy + cv2.drawMarker(color_img, (int(cx), int(cy)), (0, 255, 0), markerType=cv2.MARKER_CROSS, markerSize=20, thickness=2) + cv2.circle(color_img, (int(u), int(v)), int(radius), (0, 0, 255), 2) + cv2.circle(color_img, (int(u), int(v)), 5, (0, 0, 255), -1) + cv2.imwrite(vis_path, color_img) + print(f"✓ 已保存标记图到 {vis_path}") + + return pos + + +def red_point_position(color_frame, depth_frame, depth_intr, vis_path=None, min_radius=3, avg_window=3): + """ + 检测白底红圆的圆心坐标，返回相机系 (X,Y,Z) [m] + """ + # --- 1. 取彩色帧 ---- + color_img = np.asanyarray(color_frame.get_data()).copy() + hsv = cv2.cvtColor(color_img, cv2.COLOR_BGR2HSV) + + # --- 2. 阈值分割：红色有两个 Hue 区间 (0-10)∪(170-180) --- + lower_red1 = np.array([0, 100, 100]) + upper_red1 = np.array([10, 255, 255]) + lower_red2 = np.array([170, 100, 100]) + upper_red2 = np.array([180, 255, 255]) + + mask1 = cv2.inRange(hsv, lower_red1, upper_red1) + mask2 = cv2.inRange(hsv, lower_red2, upper_red2) + mask = cv2.bitwise_or(mask1, mask2) + + # 可选：形态学开闭运算去噪 + kernel = np.ones((5,5), np.uint8) + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel) + mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel) + + # --- 3. 轮廓取最大圆 --- + cnts, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + if not cnts: + cv2.imwrite(vis_path, color_img) + raise RuntimeError("未检测到任何红色区域") + + cnt = max(cnts, key=cv2.contourArea) + (u, v), radius = cv2.minEnclosingCircle(cnt) + if radius < min_radius: + raise RuntimeError(f"检测到圆半径过小 ({radius:.1f}px)，请检查 min_radius 或图像质量") + + # --- 4. 深度中值 --- + width, height = depth_frame.get_width(), depth_frame.get_height() + depths = [ + depth_frame.get_distance(int(round(u + du)), int(round(v + dv))) + for du in range(-avg_window, avg_window + 1) + for dv in range(-avg_window, avg_window + 1) + if 0 <= int(round(u + du)) < width and + 0 <= int(round(v + dv)) < height + ] + depths = [d for d in depths if d > 0] + if not depths: + raise RuntimeError("圆心处深度无效 (0)") + depth = float(np.median(depths)) + + # --- 5. 反投影到 3-D --- + x, y, z = rs.rs2_deproject_pixel_to_point(depth_intr, [u, v], depth) + pos = np.array([x, y, z], dtype=np.float32) + + # --- 6. 可视化保存 --- + if vis_path is not None: + cx, cy = depth_intr.ppx, depth_intr.ppy + cv2.drawMarker(color_img, (int(cx), int(cy)), (0, 255, 0), markerType=cv2.MARKER_CROSS, markerSize=20, thickness=2) + cv2.circle(color_img, (int(u), int(v)), int(radius), (255, 0, 0), 2) # 蓝圈标红圆 + cv2.circle(color_img, (int(u), int(v)), 5, (255, 0, 0), -1) + cv2.imwrite(vis_path, color_img) + print(f"✓ 已保存标记图到 {vis_path}") + + return pos + + +# if __name__ == "__main__": +# # 初始化机器人（传入配置文件路径和机器人名称） +# +# pipeline, align, depth_intr = start_pipeline("243122075614") +# robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") +# js = robot.getJointQ('right') +# T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S") +# T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") +# P_base_cam = T_base_cam[:3, 3] +# +# color_f, depth_f = get_aligned_frames(pipeline, align) +# # P_cam_target = black_point_position(color_f, depth_f, depth_intr, vis_path="black_point.png") +# P_cam_target = red_point_position(color_f, depth_f, depth_intr, vis_path="red_point.png") +# +# P_base_target = np.array([ +# P_base_cam[0] + P_cam_target[2], +# P_base_cam[1] + P_cam_target[0], +# P_base_cam[2] - P_cam_target[1], +# ]) +# print("P_base_target:", P_base_target) +# +# P_base_tool = P_base_target - np.array([0.45, 0, 0]) +# T_base_tool = T_base_ee +# T_base_tool[:3, 3] = P_base_tool +# +# print("T_base_tool:", T_base_tool) +# +# target_x, target_y, target_z = T_base_tool[0, 3], T_base_tool[1, 3], T_base_tool[2, 3] +# target_rx, target_ry, target_rz =R.from_matrix(T_base_tool[:3, :3]).as_euler('xyz', degrees=True) +# +# ok, lq, rq = solve_ik(target_x, target_y, target_z, target_rx, target_ry, target_rz) +# if not ok: +# print("solve ik failed") +# exit(-1) +# # moveJ(rq) +# +# # try: +# # while True: +# # js = get_joint_position('right') +# # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S") +# # T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") +# # P_base_cam = T_base_cam[:3, 3] +# # +# # color_f, depth_f = get_aligned_frames(pipeline, align) +# # # P_cam_target = black_point_position(color_f, depth_f, depth_intr, vis_path="black_point.png") +# # P_cam_target = red_point_position(color_f, depth_f, depth_intr, vis_path="red_point.png") +# # +# # P_base_target = np.array([ +# # P_base_cam[0] + P_cam_target[2], +# # P_base_cam[1] + P_cam_target[0], +# # P_base_cam[2] - P_cam_target[1], +# # ]) +# # print("P_base_target:", P_base_target) +# # +# # P_base_tool = P_base_target - np.array([0.3, 0, 0]) +# # T_base_tool = T_base_ee +# # T_base_tool[:3, 3] = P_base_tool +# # +# # print("T_base_tool:", T_base_tool) +# # +# # target_x, target_y, target_z = T_base_tool[0, 3], T_base_tool[1, 3], T_base_tool[2, 3] +# # target_rx, target_ry, target_rz =R.from_matrix(T_base_tool[:3, :3]).as_euler('xyz', degrees=True) +# # +# # ok, lq, rq = solve_ik(target_x, target_y, target_z, target_rx, target_ry, target_rz) +# # if not ok: +# # print("solve ik failed") +# # exit(-1) +# # # moveJ(rq) +# # except Exception as e: +# # print(e) + +# ========== Ctrl+C 处理 ========== def signal_handler(sig, frame): print("\n收到 Ctrl+C，准备退出...") raise SystemExit @@ -35,96 +305,191 @@ def transform_point(T, P): return P_new[:3] def compute_point_in_base(T_base_ee, R_ee_cam, t_ee_cam, P_cam_target): + """ + 已知: + T_base_ee : 基座->末端 (4x4) + R_ee_cam : 末端->相机的旋转 (3x3) + t_ee_cam : 末端->相机的平移 (3,) + P_cam_target : 目标点在相机下的坐标 (3,) + 返回: + P_base_target : 目标点在基座下的坐标 (3,) + """ + # 拼接 T_ee_cam T_ee_cam = rt_to_transform(R_ee_cam, t_ee_cam) + + # 得到相机在 base 下的位姿 T_base_cam = T_base_ee @ T_ee_cam + + # 把目标点从相机系变换到 base 系 P_base_target = transform_point(T_base_cam, P_cam_target) + return P_base_target -IDEL_Q = [-0.05804541534555635, 1.460164607187404, 1.458934384971377, 0.29042831678163805, - -1.498103103566999, 0.039690803641003906, -0.08653132466712826] -INIT_POSE = [0.45, -0.25, -0.15, 0, 0, 0] - -# FOLLOW_OFFSET = [-0.3, 0, 0] -# EE_LINK = "EE" -# FOLLOW_OFFSET = [-0.2, 0, 0] -# EE_LINK = "CAM" -FOLLOW_OFFSET = [-0.01, 0, 0] -EE_LINK = "FINGER" - -def main(): - robot = Robot(os.path.join(root_path, "config/cabin_robot.xml"), "hc01") - current_q = robot.getJointQ('right') - diff = np.array(IDEL_Q) - np.array(current_q) - if np.max(np.abs(diff)) >= 0.01: - robot.moveJ("right", IDEL_Q) +if __name__ == "__main__": pipeline, align = init_realsense() - ok, _, init_rq = solve_ik(INIT_POSE, base_link="PELVIS_S", target_link="R_FINGER_TIP") - if not ok: - print("solve ik init_pose failed, init_pose:", INIT_POSE) - return - print("init q:", init_rq) - print("Robot movej: ", init_rq) - robot.moveJ("right", init_rq) try: + # 初始化机器人 + robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") + + # 移动到初始关节位姿 + + init_joint = [-0.304106 , 1.30538 , 1.4465 ,1.93647 , -2.84955 ,-0.116586 ,0.123911] + # init_joint = [0 ,0 , 0 ,0 , 0 ,0 ,0] + robot.moveJ("right", init_joint) + + js = robot.getJointQ('right') + print("当前关节角:", js) + + # 获取末端在基座下的位姿 + # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") + T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") + print("末端位姿 T_base_ee:\n", T_base_ee) + # T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") + # print("末端位姿 T_base_ee:\n", T_base_ee) + # + # T_base_ee = get_pose(js, base_link="R_WRIST_R_S", target_link="R_FINGER_TIP") + # print("末端位姿 T_base_ee:\n", T_base_ee) + + # while True: + # time.sleep(1) + + + + # 获取红点在相机坐标系下的坐标 + P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png") + if P_cam_target is None: + raise RuntimeError("未检测到红点，无法计算目标点") + print("红点在相机坐标系下:", P_cam_target) + + # P_cam_target = np.array([0.0716422, 0.0482324, 0.325]) + + # "--frame-id", + # "R_FINGER_TIP", + # "--child-frame-id", + # "camera_color_optical_frame", + # "--x", + # "-0.132714", + # "--y", + # "0.00161894", + # "--z", + # "0.0760576", + # "--qx", + # "0.512312", + # "--qy", + # "-0.503306", + # "--qz", + # "0.493563", + # "--qw", + # "-0.490525", + # # "--roll", + # # "0.18103", + # # "--pitch", + # # "1.60289", + # # "--yaw", + # # "-1.76387", + + # 手眼标定结果：四元数和平移 + # q_ee_cam = np.array([0.519375,-0.502778, 0.4866,-0.490596]) + # t_ee_cam = np.array([-0.0815577, 0.0321285, 0.0746921]) + + q_ee_cam = np.array([0.512312, -0.503306, 0.493563,-0.490525]) + t_ee_cam = np.array([-0.124714, -0.02281894, 0.0800576]) + R_ee_cam = R.from_quat(q_ee_cam).as_matrix() + print("末端->相机旋转矩阵 R_ee_cam:\n", R_ee_cam) + + + # 构造相机相对于 末端的变换矩阵 + T_ee_cam = np.eye(4) + T_ee_cam[:3, :3] = R_ee_cam + T_ee_cam[:3, 3] = t_ee_cam + + # 相机相对于 基座的 变换矩阵 + T_base_cam = T_base_ee @ T_ee_cam + + # 相机下红点 -> 基座下红点 + P_base_target = np.append(P_cam_target, 1) # 齐次坐标 + P_base_target = (T_base_cam @ P_base_target)[:3] + print("红点在基座坐标系下:", P_base_target) + + + # while True: + # time.sleep(1) + + # ----------------- IK 求解 ----------------- + # 目标位姿 为 相机 相对于 基座的 姿态 + # R_base_target = T_base_ee[:3, :3] @ R_ee_cam + R_base_target = T_base_ee[:3, :3] + rot = R.from_matrix(R_base_target) + target_euler = rot.as_euler('xyz', degrees=True) + d = rot.as_euler('xyz', degrees=False) + target_rx, target_ry, target_rz = target_euler + quat = rot.as_quat() + print("目标旋转欧拉角 (XYZ 度):", target_euler) + print("目标旋转欧拉角 (XYZ 弧度):", d) + print("目标旋转四元数 [x,y,z,w]:", quat) + + # while True: + # time.sleep(10) + + # IK 求解 + ok, lq, rq = solve_ik(*P_base_target, target_rx, target_ry, target_rz, + base_link="PELVIS_S", target_link="R_FINGER_TIP") + if not ok: + raise RuntimeError("IK 求解失败") + + # 移动到目标点 + robot.moveJ("right", rq) + print("已到达目标点，按 Ctrl+C 回到初始位姿...") + while True: - current_q = robot.getJointQ('right') - T_base_ee = get_pose(current_q, base_link="PELVIS_S", target_link="R_WRIST_R_S") - T_base_cam = get_pose(current_q, base_link="PELVIS_S", target_link="R_CAM") - T_base_tool = get_pose(current_q, base_link="PELVIS_S", target_link="R_FINGER_TIP") - print(T_base_cam) - P_cam_target = get_closest_red_point(pipeline, align, vis_path="red_point.png") - if P_cam_target is None: - print("can not find red point") - time.sleep(2) - continue + time.sleep(1) - T_cam_target = np.eye(4) - T_cam_target[:3, 3] = P_cam_target - T_base_target = T_base_cam @ T_cam_target - P_base_target = T_base_target[:3, 3] - desire_P_base_tool = P_base_target + FOLLOW_OFFSET - - if EE_LINK == "EE": - desire_rpy_base_tool = R.from_matrix(T_base_ee[:3, :3]).as_euler('xyz', degrees=True) - desire_pose = list(desire_P_base_tool) + list(desire_rpy_base_tool) - ok, _, rq = solve_ik(desire_pose, base_link="PELVIS_S", target_link="R_WRIST_R_S") - elif EE_LINK == "CAM": - desire_rpy_base_tool = R.from_matrix(T_base_cam[:3, :3]).as_euler('xyz', degrees=True) - desire_pose = list(desire_P_base_tool) + list(desire_rpy_base_tool) - ok, _, rq = solve_ik(desire_pose, base_link="PELVIS_S", target_link="R_CAM") - elif EE_LINK == "FINGER": - desire_rpy_base_tool = R.from_matrix(T_base_tool[:3, :3]).as_euler('xyz', degrees=True) - desire_pose = list(desire_P_base_tool) + list(desire_rpy_base_tool) - ok, _, rq = solve_ik(desire_pose, base_link="PELVIS_S", target_link="R_FINGER_TIP") - else: - raise RuntimeError("undefined EE_LINK: ", EE_LINK) - - if not ok: - print("solve ik failed, pose=", desire_pose) - time.sleep(1) - else: - current_q = robot.getJointQ('right') - diff = np.array(rq) - np.array(current_q) - if np.max(diff) < 0.01: - continue - print("move position:", desire_P_base_tool) - print("Robot movej: ", rq) - input_data = input("move robot? type \"yes\" to move, or type \"no\" to skip \n") - if input_data == "yes": - robot.moveJ("right", rq) - time.sleep(5) - robot.moveJ("right", init_rq) - - print("Return to init pose, Robot movej: ", init_rq) - - except SystemExit: - print("安全退出程序...") + except Exception as e: + print("捕获异常:", e) finally: - robot.moveJ("right", IDEL_Q) + # 回到初始位姿 + robot.moveJ("right", [-0.05804541534555635, 1.460164607187404, 1.458934384971377, 0.29042831678163805, + -1.498103103566999, 0.039690803641003906, -0.08653132466712826]) pipeline.stop() + align = None -if __name__ == '__main__': - main() \ No newline at end of file +# if __name__ == "__main__": +# +# try: +# # 初始化机器人 +# robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") +# js = robot.getJointQ('right') +# # js = np.zeros(7) +# # js[6] = 0.8 +# +# T_base_ee = get_pose(js, base_link="PELVIS_S", target_link="R_WRIST_R_S") +# T_base_cam = get_pose(js, base_link="PELVIS_S", target_link="R_FINGER_TIP") +# +# print(T_base_cam - T_base_ee) +# T_ee1_ee2 = get_pose(js, base_link="R_FINGER_TIP", target_link="R_WRIST_R_S") +# print(T_ee1_ee2) +# +# +# T_ee2_ee1 = get_pose(js, base_link="R_WRIST_R_S", target_link="R_FINGER_TIP") +# print(T_ee2_ee1) +# +# +# # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) +# +# # robot.moveJ("right", [-0.344938, 0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996]) +# # js = robot.getJointQ('right') +# # print("js = ") +# # print(js) +# +# +# print("已到达目标点，按 Ctrl+C 回到初始位姿...") +# while True: +# time.sleep(1) +# except SystemExit: +# print("安全退出程序...") +# # +# # finally: +# # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) diff --git a/python/vision_servo/get_chessboard_position.py b/python/vision_servo/get_chessboard_position.py index 845276af..66631c5a 100644 --- a/python/vision_servo/get_chessboard_position.py +++ b/python/vision_servo/get_chessboard_position.py @@ -8,29 +8,38 @@ import cv2 import numpy as np import pyrealsense2 as rs +import sys import time +# 把.so所在目录加入 Python 路径 +sys.path.append("/home/lgv/cmvr/cmvr-es/cmake-build-debug/example") + +# 导入机器人控制模块 +from robot_wrapper import Robot + + def init_realsense(): - """ - 初始化 RealSense 管道并返回 pipeline 和 align 对象 - """ + """初始化 RealSense 管道并返回 pipeline 和 align 对象""" pipeline = rs.pipeline() config = rs.config() - config.enable_stream(rs.stream.depth, 848, 480, rs.format.z16, 30) + config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30) config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30) profile = pipeline.start(config) align = rs.align(rs.stream.color) return pipeline, align -def stop_pipeline(pipeline: rs.pipeline): - pipeline.stop() - -def get_closest_red_point(pipeline, align, vis_path = None,warmup=10): +def get_closest_red_point(pipeline, align, vis_path=None, warmup=30, + hough_params=None): """ 获取最近红色圆点的 3D 坐标。 返回 (x, y, z) 或 None（如果没检测到）。 """ + if hough_params is None: + hough_params = dict(dp=1.2, minDist=20, + param1=50, param2=15, + minRadius=5, maxRadius=50) + # 丢掉前几帧，让相机稳定 for _ in range(warmup): pipeline.wait_for_frames() @@ -47,53 +56,61 @@ def get_closest_red_point(pipeline, align, vis_path = None,warmup=10): color_image = np.asanyarray(color_frame.get_data()) depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics + # 转换到 HSV 并提取红色区域 hsv = cv2.cvtColor(color_image, cv2.COLOR_BGR2HSV) - lower_red1 = np.array([0, 100, 100]) - upper_red1 = np.array([10, 255, 255]) - lower_red2 = np.array([160, 100, 100]) - upper_red2 = np.array([179, 255, 255]) - mask1 = cv2.inRange(hsv, lower_red1, upper_red1) - mask2 = cv2.inRange(hsv, lower_red2, upper_red2) - mask = cv2.bitwise_or(mask1, mask2) + lower_red1, upper_red1 = np.array([0, 100, 100]), np.array([10, 255, 255]) + lower_red2, upper_red2 = np.array([160, 100, 100]), np.array([179, 255, 255]) + mask = cv2.inRange(hsv, lower_red1, upper_red1) | cv2.inRange(hsv, lower_red2, upper_red2) + # 平滑处理再检测圆 mask_blur = cv2.GaussianBlur(mask, (9, 9), 2) - circles = cv2.HoughCircles(mask_blur, cv2.HOUGH_GRADIENT, dp=1.2, minDist=20, - param1=50, param2=15, minRadius=5, maxRadius=50) + circles = cv2.HoughCircles(mask_blur, cv2.HOUGH_GRADIENT, **hough_params) - closest_point = None - min_depth = float('inf') + closest_point, min_depth = None, float('inf') if circles is not None: circles = np.uint16(np.around(circles)) - for i in circles[0, :]: - u, v, r = i - depth = depth_frame.get_distance(u, v) + for u, v, r in circles[0, :]: + depth = depth_frame.get_distance(int(u), int(v)) if 0 < depth < min_depth: min_depth = depth - closest_point = rs.rs2_deproject_pixel_to_point(depth_intrin, [u, v], depth) + closest_point = rs.rs2_deproject_pixel_to_point(depth_intrin, [int(u), int(v)], depth) + if vis_path: + cv2.circle(color_image, (u, v), r, (0, 255, 0), 2) + cv2.circle(color_image, (u, v), 6, (0, 0, 255), -1) - if vis_path is not None and closest_point is not None: - cv2.circle(color_image, (u, v), 6, (0,0,255), -1) + if vis_path and closest_point is not None: cv2.imwrite(vis_path, color_image) + return closest_point - - - - - -# ========================= 4. 主程序 ========================= # -def main(serial="243122075614", avg_window=3): +def main(serial="243122075614", avg_window=3, loop=True): pipeline, align = init_realsense() + # robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") + try: - # 调用获取最近红点 - point_3d = get_closest_red_point(pipeline, align, vis_path="red_point.png") - print("Closest red 3D point:", point_3d) + while True: + point_3d = get_closest_red_point(pipeline, align, vis_path="red_point.png") + if point_3d: + print("Closest red 3D point:", np.round(point_3d, 4)) + else: + print("未检测到红点") + + if not loop: + break + + key = cv2.waitKey(1) + if key == 27: # ESC 键退出 + break + + except Exception as e: + print("Error:", e) finally: pipeline.stop() cv2.destroyAllWindows() + if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/python/vision_servo/red_point.png b/python/vision_servo/red_point.png index c85c560f..371035f6 100644 Binary files a/python/vision_servo/red_point.png and b/python/vision_servo/red_point.png differ diff --git a/python/vision_servo/robot_warpper_test.py b/python/vision_servo/robot_warpper_test.py index ad888b82..3d9fcc86 100644 --- a/python/vision_servo/robot_warpper_test.py +++ b/python/vision_servo/robot_warpper_test.py @@ -43,16 +43,27 @@ robot = Robot("/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml", "hc01") # while True: # for j in JOINT_POSITIONS: # robot.moveJ('right', j) -robot.torqueOn() +# robot.torqueOn() +# time.sleep(10) +# js = robot.getJointQ('right') +# print(js) # 控制左臂关节 # robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) # # robot.moveJ("right", [-0.344938, 0.935147, 2.27031,1.68959, -2.32841,0.460145, 0.300996]) -# # robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) +# robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) # # +# time.sleep(200) +# robot.calibrateZeroQ("R_WRIST_Y") +# robot.calibrateZeroQ("R_WRIST_R") +# # robot.torqueOff("R_WRIST_R") # time.sleep(10) -# robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) -# robot.torqueOff() - +# robot.moveJ("right", [0.0, 0.0, 0.0, 0.0, 0.0,0.0, 0.0]) +# time.sleep(20) +# # robot.moveJ("right", [0.00203898, 1.34062, 0.0,0.322261, 0.0,-0.000210733, -0.0942364]) +# # robot.torqueOff() +# # time.sleep(500) +# robot.torqueOff("WAIST_P") +# time.sleep(30) # // {"R_SHOULDER_P", 0.00203898}, # // {"R_SHOULDER_R", 1.34062}, # // {"R_SHOULDER_Y", 0.0}, diff --git a/python/vision_servo/test.py b/python/vision_servo/test.py new file mode 100644 index 00000000..e69de29b diff --git a/src/controller/touch_controller.cpp b/src/controller/touch_controller.cpp index 504200eb..c4d60872 100644 --- a/src/controller/touch_controller.cpp +++ b/src/controller/touch_controller.cpp @@ -112,3 +112,48 @@ bool TouchController::isArrive(double max_force) { return force > max_force; } + +void TouchController::touch(int u, int v, double max_force) { + LOG(INFO) << "Touch request at pixel (" << u << ", " << v << ") with max_force=" << max_force; + + // 获取目标点和当前位姿 + auto target_pose = cam_->get3DPointFromPixel(u, v); + auto cur_pose = robot_->fk("PELVIS_S", "R_FINGER_TIP"); + + LOG(INFO) << "Target 3D Pose: " << target_pose; + + // 定义关键点位 + auto pre_touch_pose = cur_pose; + pre_touch_pose.mutable_position()->set_x(target_pose[0] - 0.05); + pre_touch_pose.mutable_position()->set_y(target_pose[1]); + pre_touch_pose.mutable_position()->set_z(target_pose[2]); + + auto touch_pose = pre_touch_pose; + touch_pose.mutable_position()->set_x(target_pose[0]); + + const auto& retreat_pose = cur_pose; + // retreat_pose.mutable_position()->set_x(target_pose[0] - 0.20); + + // 1. 移动到预接触位置 + robot_->moveJ("PELVIS_S", "R_FINGER_TIP", pre_touch_pose); + + // 2. 向前接触并开启压力监测 + robot_->servoJ("PELVIS_S", "R_FINGER_TIP", touch_pose); + + // 3. 等待达到最大压力 + constexpr int k_sleep_ms = 5; + constexpr int k_timeout_ms = 5000; + int elapsed_ms = 0; + + while (!isArrive(max_force) && elapsed_ms < k_timeout_ms) { + std::this_thread::sleep_for(std::chrono::milliseconds(k_sleep_ms)); + elapsed_ms += k_sleep_ms; + } + + if (elapsed_ms >= k_timeout_ms) { + LOG(WARNING) << "Timeout waiting for force feedback!"; + } else { + LOG(INFO) << "Max force reached, retreating."; + robot_->moveJ("PELVIS_S", "R_FINGER_TIP", retreat_pose); + } +} diff --git a/src/controller/touch_controller_test.cpp b/src/controller/touch_controller_test.cpp index f00f6e0f..baa283bf 100644 --- a/src/controller/touch_controller_test.cpp +++ b/src/controller/touch_controller_test.cpp @@ -26,7 +26,7 @@ TEST(TouchControllerTest,MyTest) { auto robot = dmgr.getDevice("hc01"); auto hand = dmgr.getDevice("hand1"); - + auto cam = dmgr.getDevice("cam4"); cmvr::msgs::Pose3d pose; @@ -52,14 +52,12 @@ TEST(TouchControllerTest,MyTest) { delta_pose.mutable_euler()->set_rz(0); - - std::this_thread::sleep_for(std::chrono::milliseconds(10000)); robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.02); Pose3d offset; - TouchController controller(robot, hand); + TouchController controller(robot, hand,cam); while (true) { // controller.touch(pose,offset,500); diff --git a/src/devices/camera/realsense_camera/CMakeLists.txt b/src/devices/camera/realsense_camera/CMakeLists.txt index 6d81e2cb..42bfabd1 100644 --- a/src/devices/camera/realsense_camera/CMakeLists.txt +++ b/src/devices/camera/realsense_camera/CMakeLists.txt @@ -7,4 +7,43 @@ add_library(cmvr_es::device::realsense_camera ALIAS realsense_camera) # ✅ 命 # 查找 librealsense2 find_package(realsense2 REQUIRED) # 链接 librealsense2 -target_link_libraries(realsense_camera PRIVATE realsense2::realsense2) \ No newline at end of file +target_link_libraries(realsense_camera PRIVATE realsense2::realsense2) + + +# -------------------------------------------------------- +# Unit test +# -------------------------------------------------------- +find_package(glog REQUIRED) +find_package(protobuf REQUIRED) +find_package(PkgConfig REQUIRED) +find_package(fcl REQUIRED) +find_package(OpenCV REQUIRED) + +include_directories( + ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include +) + +link_directories( + ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib +) + + +add_executable(realsense_camera_test + ${CMAKE_CURRENT_SOURCE_DIR}/realsense_camera_test.cpp +) + + +target_link_libraries(realsense_camera_test + PRIVATE + protobuf::libprotobuf + glog::glog + gtest + gtest_main + pthread + proto-objects + ccd + fcl + ${OpenCV_LIBS} + cmvr_es::device::realsense_camera + cmvr_es::device_manager +) diff --git a/src/devices/camera/realsense_camera/realsense_camera.cpp b/src/devices/camera/realsense_camera/realsense_camera.cpp index b833bb79..1ab767c0 100644 --- a/src/devices/camera/realsense_camera/realsense_camera.cpp +++ b/src/devices/camera/realsense_camera/realsense_camera.cpp @@ -137,12 +137,6 @@ void RealsenseCamera::init() { rs_cfg_.enable_device(serial_); if (stream_mode_ == RGBD_MODE){ - if (align_mode_ == "color") { - align_ = std::make_shared(RS2_STREAM_COLOR); - } - else if (align_mode_ == "depth") { - align_ = std::make_shared(RS2_STREAM_DEPTH); - } rs_cfg_.enable_stream(RS2_STREAM_COLOR, state_.width, state_.height, RS2_FORMAT_BGR8, state_.fps); rs_cfg_.enable_stream(RS2_STREAM_DEPTH, state_.width, state_.height, RS2_FORMAT_Z16, state_.fps); } @@ -191,11 +185,43 @@ void RealsenseCamera::start() { } // 使用相同的同步策略启动 profile_ = pipe_.start(rs_cfg_); - auto frames = get_frameset(true); - rs2::frame color_frame = frames.get_color_frame(); - auto profile = color_frame.get_profile(); - auto color_profile = frames.get_color_frame().get_profile(); - intrinsics_ = color_profile.as().get_intrinsics(); + + + if (stream_mode_ == RGBD_MODE){ + auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as(); + auto sp_color = profile_.get_stream(RS2_STREAM_COLOR).as(); + Kd_ = sp_depth.get_intrinsics(); + Kc_ = sp_color.get_intrinsics(); + Ec2d_ = sp_color.get_extrinsics_to(sp_depth); + Ed2c_ = sp_depth.get_extrinsics_to(sp_color); + + intrinsics_ = Kc_; + } + else if (stream_mode_ == COLOR_MODE) { + auto sp_color = profile_.get_stream(RS2_STREAM_COLOR).as(); + Kc_ = sp_color.get_intrinsics(); + + intrinsics_ = Kc_; + } + else if (stream_mode_ == DEPTH_MODE) { + auto sp_depth = profile_.get_stream(RS2_STREAM_DEPTH).as(); + Kd_ = sp_depth.get_intrinsics(); + + intrinsics_ = Kd_; + } + + if (align_mode_ == "color") { + align_ = std::make_shared(RS2_STREAM_COLOR); + } + else if (align_mode_ == "depth") { + align_ = std::make_shared(RS2_STREAM_DEPTH); + } + + // auto frames = get_frameset(true); + // rs2::frame color_frame = frames.get_color_frame(); + // auto profile = color_frame.get_profile(); + // auto color_profile = frames.get_color_frame().get_profile(); + // intrinsics_ = color_profile.as().get_intrinsics(); state_.is_opened = true; LOG(INFO) << "realsense start streaming successfully"; @@ -333,10 +359,6 @@ void RealsenseCamera::getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsic throw std::runtime_error("[RealsenseCamera] (getRGBDImages): Failed to get frames: " + std::string(e.what())); } - // 对齐深度到彩色坐标系 - rs2::align align_to(RS2_STREAM_COLOR); - frames = align_to.process(frames); - rs2::video_frame color_frame = frames.get_color_frame(); // rs2::frame color_frame = frames.get_color_frame(); rs2::depth_frame depth_frame = frames.get_depth_frame(); @@ -1039,11 +1061,18 @@ void RealsenseCamera::stopStreaming() Eigen::Vector3f RealsenseCamera::get3DPointFromPixel(int u, int v) { std::lock_guard lock(frame_mtx_); - if (latest_depth_.empty()) return {0,0,0}; + if (latest_depth_.empty()) { + LOG(WARNING) << " depth data empty"; + return {0,0,0}; + } - // 获取深度值，单位米 + // 获取深度值，pixel float depth_m = latest_depth_.at(v, u) * 0.001f; - if (depth_m <= 0) return {0,0,0}; + if (depth_m <= 0) { + LOG(WARNING) << " depth data is < zero"; + return {0,0,0}; + } + float pixel[2] = { static_cast(u), static_cast(v) }; float point[3]; diff --git a/src/devices/camera/realsense_camera/realsense_camera.h b/src/devices/camera/realsense_camera/realsense_camera.h index 39b20261..8fc269e5 100644 --- a/src/devices/camera/realsense_camera/realsense_camera.h +++ b/src/devices/camera/realsense_camera/realsense_camera.h @@ -106,6 +106,12 @@ namespace cmvr::device{ cv::Mat latest_depth_; std::mutex depth_mtx_; + + rs2_intrinsics Kd_; // 深度相机内参 + rs2_intrinsics Kc_; // RGB相机内参 + rs2_extrinsics Ec2d_; // rgb -> 深度 的外参 + rs2_extrinsics Ed2c_; // rgb <- 深度 的外参 + }; } diff --git a/src/devices/camera/realsense_camera/realsense_camera_test.cpp b/src/devices/camera/realsense_camera/realsense_camera_test.cpp new file mode 100644 index 00000000..1bc68dee --- /dev/null +++ b/src/devices/camera/realsense_camera/realsense_camera_test.cpp @@ -0,0 +1,37 @@ +// +// Created by lgv on 2025/9/1. +// +#include +#include "devices/abstract_camera.h" +#include +#include "device_manager/device_manager.h" +using namespace cmvr::device; +// 测试: 实时显示 RGB + Depth +TEST(RealsenseCameraRealDeviceTest, SaveFrames) { + + std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"; + const XmlNode config(config_path); + + if (!config.hasChild("DeviceManager")){ + LOG(ERROR) << "Device Manager node not found"; + return; + } + auto dmgr_cfg = config.getChild("DeviceManager"); + auto &dmgr = DeviceManager::getInstance(dmgr_cfg); + + auto cam = dmgr.getDevice("cam4"); + cam->start(); + + cv::Mat color, depth; + Rs2Intrinsics intrinsics; + + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + + cam->getRGBDImages(color, depth, intrinsics); + + auto pose = cam->get3DPointFromPixel(700,377); + + + LOG(INFO) << "Pose : " << pose ; + cam->stop(); +} diff --git a/src/devices/motor/motor_protocol_interface.h b/src/devices/motor/motor_protocol_interface.h index 7d2b11a2..b24dc466 100644 --- a/src/devices/motor/motor_protocol_interface.h +++ b/src/devices/motor/motor_protocol_interface.h @@ -42,6 +42,7 @@ namespace cmvr { virtual void torqueOff(uint8_t node_id) = 0; virtual double getQ(uint8_t node_id) = 0; + virtual double getQd(uint8_t node_id) = 0; CommProto comm_proto{CommProto::CANOPEN}; protected: }; diff --git a/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp b/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp index 69eaf9a2..1c6421c1 100644 --- a/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp +++ b/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp @@ -474,5 +474,12 @@ double Ti5MotorCanopenProtocol::getQ(uint8_t node_id) { return (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG); } +double Ti5MotorCanopenProtocol::getQd(uint8_t node_id) { + auto data_ptr = std::make_unique(); + message_manager_->GetSensorData(data_ptr.get()); + auto cnt = data_ptr->motors().at(node_id).speed(); + return (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG); +} + diff --git a/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h b/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h index f0eb10a8..b6131327 100644 --- a/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h +++ b/src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h @@ -39,6 +39,7 @@ namespace cmvr { bool reachedTargetQ(uint8_t node_id) override; double getQ(uint8_t node_id) override; + double getQd(uint8_t node_id) override; void setQd(uint8_t node_id, double qd) override; void setQdd(uint8_t node_id, double qdd) override; diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index bd12ff94..831fd804 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -196,6 +196,9 @@ void HumanoidRobot::init() { right_task.get(); waist_task.get(); + motor_manager_->getMotor("WAIST_Y")->setQ(0); + motor_manager_->getMotor("WAIST_P")->setQ(0); + rsm_.store(ROBOT_ESTOP); LOG(INFO) << "All motors initialized successfully."; } @@ -226,13 +229,13 @@ HumanoidRobot::~HumanoidRobot() { upd_timer_->stop(); std::vector cmd = { - {"L_SHOULDER_P", 0.0}, - {"L_SHOULDER_R", -1.31873}, - {"L_SHOULDER_Y", 0.0}, - {"L_ELBOW_R", -0.537621}, - {"L_WRIST_P", 0.0}, - {"L_WRIST_Y", 0.000183204}, - {"L_WRIST_R", 0.0225797}, + // {"L_SHOULDER_P", 0.0}, + // {"L_SHOULDER_R", -1.31873}, + // {"L_SHOULDER_Y", 0.0}, + // {"L_ELBOW_R", -0.537621}, + // {"L_WRIST_P", 0.0}, + // {"L_WRIST_Y", 0.000183204}, + // {"L_WRIST_R", 0.0225797}, {"R_SHOULDER_P", -0.0201069}, {"R_SHOULDER_R", 1.46698}, @@ -290,11 +293,31 @@ std::vector HumanoidRobot::getLinkNames() { return link_names_; } +template +void HumanoidRobot::getJointsState(std::vector& states) { + try { + lock_guard lock(exec_mtx_); + + states.clear(); + JointState state; + for (const auto &pair : motor_manager_->motorsMap()) { + auto motor = pair.second; + state.name = motor->jointName(); + state.position = motor->getQ(); + state.velocity = motor->getQd(); + states.push_back(state); + } + } catch (exception &e) { + throw runtime_error(e.what()); + } +} + template void HumanoidRobot::getState(RobotState &state) { try { lock_guard lock(exec_mtx_); // TODO: copy m_state_ date into state + } catch (exception &e) { throw runtime_error(e.what()); } @@ -305,6 +328,17 @@ void HumanoidRobot::torqueOn() { eStop(); } +template +void HumanoidRobot::torqueOn(const std::string &joint_name) { + auto motor = motor_manager_->getMotor(joint_name); + motor->brake(); +} + +template +void HumanoidRobot::torqueOff(const std::string &joint_name) { + auto motor = motor_manager_->getMotor(joint_name); + motor->torqueOff(); +} template void HumanoidRobot::eStop() { @@ -398,7 +432,7 @@ void HumanoidRobot::moveJ(const std::string &base_link, const std::string & q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"], q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"]; - LOG(INFO) << "q_init: " << q_init; + // LOG(INFO) << "q_init: " << q_init; m_state_->SetQ(q_init); m_robot_->ComputeForwardKinematics(m_state_); @@ -432,10 +466,11 @@ void HumanoidRobot::moveJ(const std::string &base_link, const std::string & auto motor = motor_manager_->getMotor(j.joint_name); if (motor != nullptr) { // PPM 模式下 这个实际速度会超30% 左右 - motor->setQd(vel); + if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); } + motor->setQd(vel); motor->setQ(j.rad); } } @@ -467,7 +502,6 @@ void HumanoidRobot::moveJ(const std::string &base_link, const std::string & } catch (exception &e) { throw runtime_error(e.what()); } - } @@ -1610,6 +1644,50 @@ Eigen::Vector3d HumanoidRobot::rotationMatrixToEulerZYX(const Eigen::Matrix return Eigen::Vector3d(rx, ry, rz); } +template +std::vector HumanoidRobot::ik(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose) { + // update m_state_ + + try { + 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 std::runtime_error("IK solve failed"); + } else { + return std::vector(q_cmd.data(), q_cmd.data() + q_cmd.size()); + } + }catch (std::exception &e) { + throw runtime_error(e.what()); + } + + +} template diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.h b/src/devices/robot/humanoid_robot/humanoid_robot.h index c1b2d70e..d122b4cc 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.h +++ b/src/devices/robot/humanoid_robot/humanoid_robot.h @@ -31,6 +31,7 @@ #include #include + namespace cmvr::device{ template @@ -62,12 +63,15 @@ namespace cmvr::device{ std::unordered_map getJointQ() const override; void getJointQ(std::unordered_map &joint_qs) const override; + void getJointsState(std::vector& states) override; void getState(RobotState &state) override; math::Pose3d getTransform(std::string &bask_link, std::string &target_link) override; /* torque on and off */ void torqueOn() override; + void torqueOn(const std::string &joint_name) override; void torqueOff() override; + void torqueOff(const std::string &joint_name) override; void calibrateZeroQ(const std::string &joint_name) override; /* robot basic command*/ @@ -126,6 +130,8 @@ namespace cmvr::device{ // 将欧拉角(rx, ry, rz)转为旋转矩阵，旋转顺序 Y→Y→Z static Eigen::Matrix3d eulerZYXToRotationMatrix(double rx, double ry, double rz); static Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d &R); + msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) override; + std::vector ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) override; double calculateMoveTime(double distance, double vel, double acc); @@ -133,7 +139,7 @@ namespace cmvr::device{ double total_time, size_t num_points, std::vector& time_points, std::vector& distance_ratios); - msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link); + private: std::string id_; @@ -181,9 +187,9 @@ namespace cmvr::device{ private: - std::vector l_motors_cfg_; - std::vector r_motors_cfg_; - std::vector waist_motors_cfg_; + std::vector l_motors_cfg_{}; + std::vector r_motors_cfg_{}; + std::vector waist_motors_cfg_{}; std::shared_ptr l_can_client_{nullptr}; diff --git a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp index dce81562..ad4ab92e 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot_test.cpp @@ -5,13 +5,18 @@ // #include "humanoid_robot.h" +#include +#include +#include +#include +#include + #include "gtest/gtest.h" #include #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" #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" #include "utils/base/abstract_interpolation.h" @@ -19,6 +24,35 @@ // 定义一个命令行参数 --config_path DEFINE_string(config_path, "../config/cabin_robot.xml", "Path to the robot config XML file"); using namespace cmvr::device; + +TEST(HumanoidRobotTest,GetState) { + std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"; + const XmlNode config(config_path); + + if (!config.hasChild("DeviceManager")){ + LOG(ERROR) << "Device Manager node not found"; + } + auto dmgr_cfg = config.getChild("DeviceManager"); + auto &dmgr = DeviceManager::getInstance(dmgr_cfg); + + auto robot = dmgr.getDevice("hc01"); + robot->torqueOff(); + std::vector joints; + + while (true) { + joints.clear(); + robot->getJointsState(joints); + for (const auto &js : joints) { + LOG(INFO) << "Joint: " << js.name + << ", Position: " << js.position + << ", Velocity: " << js.velocity + << std::endl; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } +} + + TEST(HumanoidRobotTest,MyRobotTest) { std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml"; @@ -55,10 +89,10 @@ TEST(HumanoidRobotTest,MyRobotTest) { // {"R_ELBOW_R", 0.0}, // {"R_WRIST_P", 0.0}, // {"R_WRIST_Y", 0.0}, - // {"R_WRIST_R", 0.0}, + {"R_WRIST_R", 0.0}, - {"WAIST_P" ,0.0}, - {"WAIST_Y" ,0.0}, + // {"WAIST_P" ,0.0}, + // {"WAIST_Y" ,0.0}, }; robot->moveJ(cmd,0.8); @@ -347,91 +381,98 @@ TEST(HumanoidRobotTest,MoveDeltaTest) { cmvr::msgs::Pose3d pose; pose.mutable_position()->set_x( 0); pose.mutable_position()->set_y(0); - pose.mutable_position()->set_z(0.01); + pose.mutable_position()->set_z(0); pose.mutable_euler()->set_rx(0); pose.mutable_euler()->set_ry(0); - pose.mutable_euler()->set_rz(0); + pose.mutable_euler()->set_rz(-0.7854); try { - robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.1); + // robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.1); // robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.01); - // robot->moveDeltaJ("PELVIS_S","R_FINGER_TIP",pose); + robot->moveDeltaJ("PELVIS_S","R_FINGER_TIP",pose,0.05); }catch (std::exception &e) { + LOG(INFO) << e.what(); robot->torqueOff(); LOG(INFO) << e.what(); } } TEST(HumanoidRobotTest,AngleToTest) { - double rx = M_PI / 6; // 30° - double ry = M_PI / 4; // 45° - double rz = M_PI / 3; // 60° + double rx = 0.9497; + double ry = 1.6082; + double rz = -2.4832; Eigen::Matrix3d R = HumanoidRobot<7>::eulerZYXToRotationMatrix(rx, ry, rz); Eigen::Vector3d euler = HumanoidRobot<7>::rotationMatrixToEulerZYX(R); std::cout << "Original Euler angles (rad):\n" << Eigen::Vector3d(rx, ry, rz).transpose() << "\n"; std::cout << "Recovered Euler angles (rad):\n" << euler.transpose() << "\n"; + } -TEST(HumanoidRobotTest,MoveIKTest) { +std::atomic running{true}; + +// 信号处理函数 +void signalHandler(int signum) { + std::cout << "\nInterrupt signal (" << signum << ") received. Stopping..." << std::endl; + running = false; +} +TEST(HumanoidRobotTest, MoveIKTest) { + // 注册信号处理 + std::signal(SIGINT, signalHandler); + std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"; const XmlNode config(config_path); - if (!config.hasChild("DeviceManager")){ + if (!config.hasChild("DeviceManager")) { LOG(ERROR) << "Device Manager node not found"; + return; } + auto dmgr_cfg = config.getChild("DeviceManager"); auto &dmgr = DeviceManager::getInstance(dmgr_cfg); - auto robot = dmgr.getDevice("hc01"); + auto cur_pose = robot->fk("PELVIS_S","R_FINGER_TIP"); - 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} - }; + cmvr::msgs::Pose3d pose; + pose.mutable_position()->set_x(0.591101); + pose.mutable_position()->set_y(-0.159451); + pose.mutable_position()->set_z(-0.206554); + pose.mutable_euler()->set_rx(0.070589); + pose.mutable_euler()->set_ry(0.117212); + pose.mutable_euler()->set_rz(0.0916384); + // 打印关节状态线程 + std::thread joint_state_thread([&]() { + while (running) { + auto joint_state = robot->getJointQ(); + if (auto it = joint_state.find("WAIST_P"); it != joint_state.end()) { + LOG(INFO) << "WAIST_P POS = " << it->second; + } else { + LOG(WARNING) << "WAIST_P not found in joint_state"; + } + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + }); - robot->moveJ(cmd); + // 动作控制线程 + std::thread motion_thread([&]() { + while (running) { + robot->moveJ("PELVIS_S","R_FINGER_TIP",pose,0.5); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + robot->moveJ("PELVIS_S","R_FINGER_TIP",cur_pose,0.5); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } + }); + // 等待线程结束 + if (motion_thread.joinable()) motion_thread.join(); + if (joint_state_thread.joinable()) joint_state_thread.join(); - // // - // cmvr::ctrl::PoseTarget target; - // std::vector targets; - // - // - // cmvr::msgs::Pose3d pose; - // - // pose.mutable_position()->set_x( 1.49969573e-01); - // pose.mutable_position()->set_y(-4.00100001e-01); - // pose.mutable_position()->set_z(-1.00102800e-01); - // - // pose.mutable_euler()->set_rx(0); - // pose.mutable_euler()->set_ry(0); - // pose.mutable_euler()->set_rz(1.57); - // - // - // - // while (true) { - // robot->moveJ("PELVIS_S","R_WRIST_R_S",pose); - // - // - // - // std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - // - // - // robot->moveJ(cmd); - // std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - // } - + LOG(INFO) << "Test finished gracefully."; } diff --git a/src/service/grpc_hlc_client_test.cpp b/src/service/grpc_hlc_client_test.cpp index ea3c3194..4fb32bd6 100644 --- a/src/service/grpc_hlc_client_test.cpp +++ b/src/service/grpc_hlc_client_test.cpp @@ -13,7 +13,7 @@ using namespace cmvr::api; TEST(GrpcHlcClientTest, MyTest) { // 连接服务端 - auto channel = grpc::CreateChannel("0.0.0.0:50055", grpc::InsecureChannelCredentials()); + auto channel = grpc::CreateChannel("0.0.0.0:50052", grpc::InsecureChannelCredentials()); auto stub = cmvr::api::HlcService::NewStub(channel); diff --git a/src/service/grpc_hlc_service.cpp b/src/service/grpc_hlc_service.cpp index 95a6edaa..43c9067a 100644 --- a/src/service/grpc_hlc_service.cpp +++ b/src/service/grpc_hlc_service.cpp @@ -21,8 +21,10 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr: grpc::Status ret = grpc::Status::OK; try { - auto robot = dmgr_.getDevice(request->header().device_id()); - ctrl::TouchController touch_controller; + std::shared_ptr robot = nullptr; + auto cam = dmgr_.getDevice("cam4"); + auto hand = dmgr_.getDevice("hand1"); + ctrl::TouchController touch_controller(robot,hand,cam);; touch_controller.touch(request->u(),request->v(),request->max_force()); response->mutable_header()->set_success(true); response->mutable_header()->set_error_message(""); diff --git a/src/service/grpc_humanoid_robot_client_test.cpp b/src/service/grpc_humanoid_robot_client_test.cpp index 5f8adbfa..ba6944f0 100644 --- a/src/service/grpc_humanoid_robot_client_test.cpp +++ b/src/service/grpc_humanoid_robot_client_test.cpp @@ -45,7 +45,7 @@ TEST(GrpcHumanoidRobotClientTest, MyMoveJTest) { // - std::vector cmds{}; + std::vector cmds{}; cmds = { @@ -100,3 +100,39 @@ TEST(GrpcHumanoidRobotClientTest, MyMoveJTest) { LOG(ERROR) << "RPC failed: " << status.error_message() << std::endl; } } + + +TEST(GrpcHumanoidRobotClientTest, GetJointStateTest) { + // 连接服务端 + auto channel = grpc::CreateChannel("0.0.0.0:50052", grpc::InsecureChannelCredentials()); + auto stub = cmvr::api::HumanoidRobotService::NewStub(channel); + + // 构造请求 + cmvr::api::JointRequest request; + request.mutable_header()->set_device_id("hc01"); + *request.mutable_header()->mutable_timestamp() = google::protobuf::util::TimeUtil::GetCurrentTime(); + + cmvr::api::JointResponse response; + grpc::ClientContext context; + + // 调用 getJointState + grpc::Status status = stub->getJointState(&context, request, &response); + + if (status.ok()) { + LOG(INFO) << "getJointState RPC succeeded."; + LOG(INFO) << "Success: " << response.header().success(); + LOG(INFO) << "Error message: " << response.header().error_message(); + LOG(INFO) << "Timestamp: " << response.header().timestamp().seconds(); + + // 输出每个关节状态 + for (const auto &js : response.state()) { + for (int i = 0; i < js.name_size(); ++i) { + LOG(INFO) << "Joint: " << js.name(i) + << ", Position: " << js.position(i) + << ", Velocity: " << js.velocity(i); + } + } + } else { + LOG(ERROR) << "RPC failed: " << status.error_message(); + } +} diff --git a/src/service/grpc_humanoid_robot_service.cpp b/src/service/grpc_humanoid_robot_service.cpp index e2464fdc..b7797557 100644 --- a/src/service/grpc_humanoid_robot_service.cpp +++ b/src/service/grpc_humanoid_robot_service.cpp @@ -151,4 +151,37 @@ grpc::Status gRPCHumanoidRobotServiceImpl::speedL(grpc::ServerContext* context, } *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); return ret; +} + +grpc::Status gRPCHumanoidRobotServiceImpl::getJointState(grpc::ServerContext *context, + const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) { + + grpc::Status ret = grpc::Status::OK; + + try { + auto robot = dmgr_.getDevice(request->header().device_id()); + + std::vector states{}; + robot->getJointsState(states); + + response->mutable_header()->set_success(true); + response->mutable_header()->set_error_message(""); + + for (const auto &js : states) { + + if (js.name != "WAIST_P" && js.name != "WAIST_Y") { + auto joint_msg = response->add_state(); + joint_msg->add_name(js.name); + joint_msg->add_position(js.position); + joint_msg->add_velocity(js.velocity); + } + + } + }catch (const std::exception& e) { + response->mutable_header()->set_success(false); + response->mutable_header()->set_error_message(e.what()); + ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } + *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); + return ret; } \ No newline at end of file