Fix json gripper trajectory replay

panthera_cpp/robot_cpp/example/5_record_trajectory.cpp

@@ -176,6 +176,8 @@ int main(int argc, char** argv)
             // 获取当前状态
             vector<double> positions = robot.getCurrentPos();
             vector<double> velocities = robot.getCurrentVel();
+            double gripper_pos = robot.getCurrentPosGripper();
+            double gripper_vel = robot.getCurrentVelGripper();
 
             // 计算重力补偿力矩（使用 Panthera 类的方法）
             vector<double> gravity_torque = robot.getGravity(positions);
@@ -195,9 +197,12 @@ int main(int argc, char** argv)
             // 零刚度零阻尼控制（纯重力补偿模式，可自由拖动）
             robot.posVelTorqueKpKd(zero_pos, zero_vel, total_torque, zero_kp, zero_kd);
 
-            // 记录关节位置速度 + 夹爪（暂时用0代替）
+            // Gripper compliance (freely draggable during recording)
+            robot.gripperControlMIT(0, 0, 0, 0, 0);
+
+            // Record joint position, velocity + gripper
             if (DO_RECORD && rec && rec->isOpen()) {
-                rec->log(positions, velocities, 0.0, 0.0);
+                rec->log(positions, velocities, gripper_pos, gripper_vel);
             }
 
             // 打印状态（每0.1秒）
@@ -211,7 +216,9 @@ int main(int argc, char** argv)
                               << setw(6) << positions[i] << "rad "
                               << setw(6) << velocities[i] << "rad/s | ";
                 }
-                cout << "夹爪: 0.000rad 0.000rad/s   " << flush;
+                cout << "Gripper: " << fixed << setprecision(3)
+                     << setw(6) << gripper_pos << "rad "
+                     << setw(6) << gripper_vel << "rad/s   " << flush;
                 last_print_time = current_time;
             }
 

panthera_cpp/robot_cpp/example/5_replay_trajectory.cpp

@@ -53,6 +53,7 @@ std::vector<TrajectoryPoint> loadTrajectory(const std::string& filepath) {
         return trajectory;
     }
 
+    // TODO XXX FIXME HACK: This hand-coded json-parser should be replaced with a library
     std::string line;
     while (std::getline(file, line)) {
         if (line.empty()) continue;
@@ -103,22 +104,25 @@ std::vector<TrajectoryPoint> loadTrajectory(const std::string& filepath) {
             }
         }
 
-        // 提取夹爪位置 "gripper_pos"
+        // Extract gripper position "gripper_pos" (14 chars)
         pos = line.find("\"gripper_pos\":");
         if (pos != std::string::npos) {
-            size_t start = pos + 15;
+            size_t start = pos + 14;
             size_t end = line.find(",", start);
             if (end == std::string::npos) {
                 end = line.find("}", start);
             }
             point.gripper_position = std::stod(line.substr(start, end - start));
         }
 
-        // 提取夹爪速度 "gripper_vel"
+        // Extract gripper velocity "gripper_vel" (14 chars)
         pos = line.find("\"gripper_vel\":");
         if (pos != std::string::npos) {
-            size_t start = pos + 15;
-            size_t end = line.find("}", start);
+            size_t start = pos + 14;
+            size_t end = line.find(",", start);
+            if (end == std::string::npos) {
+                end = line.find("}", start);
+            }
             point.gripper_velocity = std::stod(line.substr(start, end - start));
         }
 
@@ -235,8 +239,13 @@ int main(int argc, char** argv)
         robot.posVelMaxTorque(start_pos, move_vel, max_torque, true);
         std::this_thread::sleep_for(std::chrono::seconds(2));
 
-        std::cout << "[Player] 已到达起点，开始回放..." << std::endl;
-        std::cout << "\n按 Ctrl+C 停止" << std::endl;
+        // Move gripper slowly to initial state (0.3 rad/s, max torque 0.5 Nm)
+        std::cout << "[Player] Moving gripper to start position..." << std::endl;
+        robot.gripperControl(first_frame.gripper_position, 0.3, 0.5);
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+
+        std::cout << "[Player] Reached start, beginning replay..." << std::endl;
+        std::cout << "\nPress Ctrl+C to stop" << std::endl;
         std::cout << std::string(60, '=') << "\n" << std::endl;
 
         auto start_time = std::chrono::steady_clock::now();
@@ -285,10 +294,8 @@ int main(int argc, char** argv)
             // 反馈力矩 = PD 控制（用于轨迹跟踪）
             robot.posVelTorqueKpKd(point.positions, point.velocities, total_torque, kp_play, kd_play);
 
-            // 夹爪控制（如果有夹爪数据）
-            if (point.gripper_position != 0.0 || point.gripper_velocity != 0.0) {
-                robot.gripperControlMIT(point.gripper_position, point.gripper_velocity, 0.0, gripper_kp, gripper_kd);
-            }
+            // Gripper control (always follow log)
+            robot.gripperControlMIT(point.gripper_position, point.gripper_velocity, 0.0, gripper_kp, gripper_kd);
 
             // 打印进度（每10个点）
             if (i % 10 == 0) {