From e37073f4933efd9b5c13a8faee1060506f690c76 Mon Sep 17 00:00:00 2001 From: lgv Date: Sat, 28 Feb 2026 16:03:09 +0800 Subject: [PATCH] feat:add test --- cmvr-es/common/config/cabin_robot.xml | 2 +- cmvr-es/controller/include/ibvs_controller.h | 2 +- cmvr-es/controller/src/ibvs_controller.cpp | 15 ++++-- .../protocol/ti5_motor_sdo_response.cpp | 12 ++--- .../motor/protocol/ti5_motor_sdo_response.cpp | 10 ++-- .../src/humanoid_robot_test.cpp | 50 +++++++++++++++++-- 6 files changed, 70 insertions(+), 21 deletions(-) diff --git a/cmvr-es/common/config/cabin_robot.xml b/cmvr-es/common/config/cabin_robot.xml index 18a107c9..fa833a13 100644 --- a/cmvr-es/common/config/cabin_robot.xml +++ b/cmvr-es/common/config/cabin_robot.xml @@ -58,7 +58,7 @@ - + diff --git a/cmvr-es/controller/include/ibvs_controller.h b/cmvr-es/controller/include/ibvs_controller.h index 656c7857..d4a97fa7 100644 --- a/cmvr-es/controller/include/ibvs_controller.h +++ b/cmvr-es/controller/include/ibvs_controller.h @@ -253,7 +253,7 @@ private: // 参数 // ViSP 控制增益。 - double lambda_{1.7}; + double lambda_{0.7}; // tag 边长（米）。 double tag_size_m_{0.12}; // tag 半边长（米）。 diff --git a/cmvr-es/controller/src/ibvs_controller.cpp b/cmvr-es/controller/src/ibvs_controller.cpp index f3551d53..40ef367f 100644 --- a/cmvr-es/controller/src/ibvs_controller.cpp +++ b/cmvr-es/controller/src/ibvs_controller.cpp @@ -51,14 +51,14 @@ IbvsController::IbvsController() // AbstractCamera 相机系 -> ViSP 相机系 R_cv_ = (Eigen::Matrix3d() << 1, 0, 0, - 0, -1, 0, - 0, 0, -1).finished(); + 0, 1, 0, + 0, 0, 1).finished(); // AbstractCamera 相机系 -> URDF 相机系 R_camera_urdf_ = (Eigen::Matrix3d() << 1, 0, 0, - 0, -1, 0, - 0, 0, -1).finished(); + 0, 1, 0, + 0, 0, 1).finished(); detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS); updateDepthControlPointInTag(); @@ -338,6 +338,13 @@ bool IbvsController::compute(const std::vector& joints_angle, // q_{k+1} = q_k + qdot * dt q_cmd_[i] += qdot[i] * dt; } + + + std::cout << "b_cmd: " ; + for (const auto& it : q_cmd_) { + std::cout << it << " "; + } + std::cout << std::endl; clampJointCommandInPlace(q_cmd_); q_cmd_out = q_cmd_; diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp index 9b9d1b64..e90f28a9 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp +++ b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp @@ -31,11 +31,11 @@ void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response, case msgs::POSITION_OFFSET_2008: motor_status->set_position_offset(sdo_response.data()); } - - LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_) - << ": command=" << int(sdo_response.cs()) - << ", index=" << sdo_response.index() - << ", subindex=" << int(sdo_response.sub_index()) - << ", data=" << sdo_response.data(); + // + // LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_) + // << ": command=" << int(sdo_response.cs()) + // << ", index=" << sdo_response.index() + // << ", subindex=" << int(sdo_response.sub_index()) + // << ", data=" << sdo_response.data(); } diff --git a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp index d1fbc3dd..84d97bd2 100644 --- a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp +++ b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp @@ -31,9 +31,9 @@ void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response, break; } - LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_) - << ": command=" << int(sdo_response.cs()) - << ", index=" << sdo_response.index() - << ", subindex=" << int(sdo_response.sub_index()) - << ", data=" << sdo_response.data(); + // LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_) + // << ": command=" << int(sdo_response.cs()) + // << ", index=" << sdo_response.index() + // << ", subindex=" << int(sdo_response.sub_index()) + // << ", data=" << sdo_response.data(); } diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp index 7d1186d5..b4914c69 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp @@ -127,6 +127,11 @@ TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) { return true; }; + + + + + ASSERT_TRUE(refreshRightArmQ()) << "Failed to read right-arm joint q"; @@ -159,6 +164,19 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { ASSERT_NE(robot, nullptr); ASSERT_NE(camera, nullptr); + std::vector init_cmd{}; + init_cmd = { + {"R_SHOULDER_P", -0.2423}, + {"R_SHOULDER_R", 1.2929}, + {"R_SHOULDER_Y", 1.61}, + {"R_ELBOW_R", 1.58}, + {"R_WRIST_P", -2.8792}, + {"R_WRIST_Y", 0.1150}, + {"R_WRIST_R", -0.08}, + }; + robot->moveJ(init_cmd,1.0,2.0); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + ASSERT_NO_THROW(camera->start()); struct RuntimeGuard { std::shared_ptr camera; @@ -181,12 +199,30 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { cmvr::IbvsController ibvs_controller; ibvs_controller.setMu(0.1); + ibvs_controller.setLambda(0.6); ibvs_controller.setQdotMax(0.6); ibvs_controller.setTagSize(0.12); ibvs_controller.setTrackedTagId(0); - ibvs_controller.setTarget(0.0, 0.0, 0.35, 3.14159265358979323846, 0.0, 0.0); + ibvs_controller.setTarget(0.0, 0.0, 0.40); ibvs_controller.setDepthMode(cmvr::IbvsController::DepthMode::MONOCULAR); ibvs_controller.setDepthZGain(1.0); + Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); + // ibvs_controller.setAlignCameraToVisp(I); // RealSense optical -> ViSP + // ibvs_controller.setAlignCameraToUrdf(I); // optical -> URDF 的 R_CAM + Eigen::Matrix3d RxPi; + RxPi << 1,0,0, + 0,-1,0, + 0,0,-1; + ibvs_controller.setAlignCameraToUrdf(RxPi); + ibvs_controller.setAlignCameraToVisp(RxPi); + + + + // ibvs_controller.setVelocityLimit6({0.15, 0.15, 0.20, 0, 0, 0}); + // ibvs_controller.setVelocityLimit6({0.03,0.03,0.03,0.005,0.005,0.005}); + ibvs_controller.setQdotMax(0.15); + + ASSERT_TRUE(ibvs_controller.init(camera, "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf", @@ -205,6 +241,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { return false; } q_now[i] = it->second; + // std::cout << kRightArmJointNames[i] << " " << q_now[i] << std::endl; } return true; }; @@ -215,7 +252,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { const int max_steps = 30000; const int log_every = 1; - const int cycle_ms = 33; + const int cycle_ms = 1; int ok_steps = 0; int fail_steps = 0; @@ -251,8 +288,8 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { continue; } - auto joint_cmd = buildRightArmJointCmd(q_cmd_next, 0.8); - // robot->servoJ(joint_cmd, 0.8, dt); + auto joint_cmd = buildRightArmJointCmd(q_cmd_next, 0.5); + robot->servoJ(joint_cmd, 0.5, 0.5); ++ok_steps; if ((step % log_every) == 0) { @@ -264,6 +301,11 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { << ", " << v_c[3] << ", " << v_c[4] << ", " << v_c[5] << "]" << " z_source=" << cmvr::IbvsController::depthUsageToString(ibvs_controller.lastDepthUsage()) << std::endl; + std::cout << "q_cmd: " ; + for (const auto& it : q_cmd_next) { + std::cout << it << " "; + } + std::cout << std::endl; } std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));