test:speedL test pass

cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp

@@ -38,6 +38,7 @@ HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
         CSC_buffer_ = make_shared<SPMCRingBuffer<JointCurrentCommand> >(cfg.getAttrDefault("bufferSize", 50));
 
 
+        upd_freq_ = 1000;
         upd_timer_ = make_shared<FDTimer>();
         upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000),
                           [this] { update_state_(); });
@@ -58,7 +59,7 @@ void HumanoidRobot<DOF>::init() {
                                              std::vector<double>(7, 1.0));
 
 
-    ik_solver_ = std::make_shared<PinocchioDlsIKSolver>("/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
+    ik_solver_ = std::make_shared<PinocchioDlsIKSolver>("/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
     "PELVIS_S",
     "R_WRIST_R_S",
     "R_FINGER_TIP_FIXED");
@@ -67,10 +68,10 @@ void HumanoidRobot<DOF>::init() {
     }
 
     speedl_config_.linear_velocity_max = 0.55;
-    speedl_config_.linear_acceleration_max = 0.80;
-    speedl_config_.linear_jerk_max = 3.30;
+    speedl_config_.linear_acceleration_max = 5.00;
+    speedl_config_.linear_jerk_max = 10.0;
     speedl_config_.angular_velocity_max = 1.00;
-    speedl_config_.angular_acceleration_max = 3.00;
+    speedl_config_.angular_acceleration_max = 5.00;
     speedl_config_.angular_jerk_max = 12.0;
     speedl_config_.joint_acceleration_max = std::vector<double>(7, 8.0);
     speedl_config_.linear_target_replan_threshold = 1e-4;
@@ -321,9 +322,8 @@ void HumanoidRobot<DOF>::speedLWorkerLoop_() {
             if (!ik_solver_->speedLStep(target_twist,
                                         dt,
                                         q_now,
-                                        qd_now,
                                         qd_cmd,
-                                        cmvr::CartesianFrame::Tool,
+                                        cmvr::CartesianFrame::Base,
                                         true)) {
                 LOG(ERROR) << "speedL worker: speedLStep() failed";
                 send_zero();

cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp

@@ -261,7 +261,7 @@ TEST(HumanoidRobotTest,speedJTest) {
 }
 
 TEST(HumanoidRobotTest, speedLSmokeTest) {
-    const XmlNode config("/home/lgv/cmvr/0-workspace/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
+    const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
     ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
     auto dmgr_cfg = config.getChild("DeviceManager");
     auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
@@ -270,10 +270,23 @@ TEST(HumanoidRobotTest, speedLSmokeTest) {
     auto robot = std::dynamic_pointer_cast<HumanoidRobot<14>>(robot_abs);
     ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>";
 
-    const std::vector<double> twist_pos = {0.03, 0.0, 0.0, 0.0, 0.0, 0.0};
-    const std::vector<double> twist_neg = {-0.03, 0.0, 0.0, 0.0, 0.0, 0.0};
-    const double acceleration = 0.20;
-    const double segment_time = 2.0;
+    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));
+
+    const std::vector<double> twist_pos = {0.08, 0, 0, 0.00, 0, 0};
+    const std::vector<double> twist_neg = {-0.08, 0, 0, 0.00, 0, 0};
+    const double acceleration = 3.0;
+    const double segment_time = 1.0;
     const double settle_time = 1.0;
     const double sample_dt = 0.02;
 
@@ -304,11 +317,18 @@ TEST(HumanoidRobotTest, speedLSmokeTest) {
         }
     };
 
-    sample_phase(twist_pos, segment_time, "+X");
+    robot->speedL({0.0, 0.0, 0.0, 0.0, 0.00, 0.0}, acceleration, 0.0);
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+
+        sample_phase(twist_pos, segment_time*0.8, "+X");
         sample_phase(twist_neg, segment_time, "-X");
 
+
     LOG(INFO) << "speedLSmokeTest phase: stop";
-    ASSERT_NO_THROW(robot->stopSpeedL());
+    robot->speedL({0.0, 0.0, 0.0, 0.0, 0.00, 0.0}, acceleration, 1);
+    // robot->moveJ(init_cmd,1.0,2.0);
+    // ASSERT_NO_THROW(robot->stopSpeedL());
     const int settle_steps = static_cast<int>(std::ceil(settle_time / sample_dt));
     for (int i = 0; i < settle_steps; ++i) {
         const Eigen::Matrix<double, 6, 1> cmd_twist = robot->getSpeedLCommandTwistBase();
@@ -354,8 +374,9 @@ TEST(HumanoidRobotTest, speedLSmokeTest) {
     ylabel(ax2, "norm [m/s]");
     legend(ax2, {"||command v||"});
     grid(ax2, true);
-
-    show(fig);
+    save("/home/lgv/cmvr/cmvr-es/data/plot1.png");
+    std::cout << "Hello World" << std::endl;
+    // show(fig);
 }
 
 TEST(HumanoidRobotTest, SpeedLOpenLoopPlannerPlot) {

model/xiaoyan_description/dual_arm.urdf

@@ -594,7 +594,7 @@
     <parent link="R_WRIST_Y_S" />
     <child link="R_WRIST_R_S" />
     <axis xyz="1 0 0" />
-    <limit lower="-0.26" upper="1.57" effort="50" velocity="4.71" />
+    <limit lower="-0.57" upper="1.57" effort="50" velocity="4.71" />
   </joint>
 
   <link name="R_FINGER_TIP">