feat:add test

cmvr-es/common/config/cabin_robot.xml

@@ -58,7 +58,7 @@
                             <Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
                             <Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
                         </RightArmCan>
-                        <HeadCan id = " " devId = " " channelId ="2" enable="true">
+                        <HeadCan id = " " devId = " " channelId ="2" enable="false">
                             <Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                             <Motor id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
                             <Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>

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 半边长（米）。

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, 1, 0,
-        0, 0, -1).finished();
+        0, 0, 1).finished();
 
     // AbstractCamera 相机系 -> URDF 相机系
     R_camera_urdf_ = (Eigen::Matrix3d() <<
         1, 0, 0,
-        0, -1, 0,
+        0, 1, 0,
-        0, 0, -1).finished();
+        0, 0, 1).finished();
 
     detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
     updateDepthControlPointInTag();
@@ -338,6 +338,13 @@ bool IbvsController::compute(const std::vector<double>& 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_;

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_)
+        // LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_)
-          << ": command=" << int(sdo_response.cs())
+        //   << ": command=" << int(sdo_response.cs())
-          << ", index=" << sdo_response.index()
+        //   << ", index=" << sdo_response.index()
-          << ", subindex=" << int(sdo_response.sub_index())
+        //   << ", subindex=" << int(sdo_response.sub_index())
-          << ", data=" << sdo_response.data();
+        //   << ", data=" << sdo_response.data();
 
 }

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_)
+    // LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_)
-            << ": command=" << int(sdo_response.cs())
+    //         << ": command=" << int(sdo_response.cs())
-            << ", index=" << sdo_response.index()
+    //         << ", index=" << sdo_response.index()
-            << ", subindex=" << int(sdo_response.sub_index())
+    //         << ", subindex=" << int(sdo_response.sub_index())
-            << ", data=" << sdo_response.data();
+    //         << ", data=" << sdo_response.data();
 }

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<JointPoint> 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<AbstractCamera> 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);
+        auto joint_cmd = buildRightArmJointCmd(q_cmd_next, 0.5);
-        // robot->servoJ(joint_cmd, 0.8, dt);
+        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));