From 98784a7f88705e8b8ef8ef93e6922d625b76b52a Mon Sep 17 00:00:00 2001 From: Morten Lysgaard Date: Thu, 12 Mar 2026 21:34:41 +0100 Subject: [PATCH 1/3] example/5_replay_trajectory.cpp: Fix bug in parsing of gripper pos and gripper_vel --- .../robot_cpp/example/5_replay_trajectory.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/panthera_cpp/robot_cpp/example/5_replay_trajectory.cpp b/panthera_cpp/robot_cpp/example/5_replay_trajectory.cpp index 2f11d00..b619538 100644 --- a/panthera_cpp/robot_cpp/example/5_replay_trajectory.cpp +++ b/panthera_cpp/robot_cpp/example/5_replay_trajectory.cpp @@ -103,10 +103,10 @@ std::vector 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); @@ -114,11 +114,14 @@ std::vector loadTrajectory(const std::string& filepath) { 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)); } From de8aaa382678b553de1f8a114cbe587c18191814 Mon Sep 17 00:00:00 2001 From: Morten Lysgaard Date: Thu, 12 Mar 2026 21:39:28 +0100 Subject: [PATCH 2/3] Note about using library for json --- panthera_cpp/robot_cpp/example/5_replay_trajectory.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/panthera_cpp/robot_cpp/example/5_replay_trajectory.cpp b/panthera_cpp/robot_cpp/example/5_replay_trajectory.cpp index b619538..1a96ba4 100644 --- a/panthera_cpp/robot_cpp/example/5_replay_trajectory.cpp +++ b/panthera_cpp/robot_cpp/example/5_replay_trajectory.cpp @@ -53,6 +53,7 @@ std::vector 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; From 414618ac556bfd64be868127c5004e3a6156383c Mon Sep 17 00:00:00 2001 From: Morten Lysgaard Date: Thu, 12 Mar 2026 21:40:10 +0100 Subject: [PATCH 3/3] Added gripper support in the 5_record_trajectory.cpp and 5_replay_trajectory.cpp --- .../robot_cpp/example/5_record_trajectory.cpp | 13 ++++++++++--- .../robot_cpp/example/5_replay_trajectory.cpp | 15 +++++++++------ 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/panthera_cpp/robot_cpp/example/5_record_trajectory.cpp b/panthera_cpp/robot_cpp/example/5_record_trajectory.cpp index d6def8a..2e2e398 100644 --- a/panthera_cpp/robot_cpp/example/5_record_trajectory.cpp +++ b/panthera_cpp/robot_cpp/example/5_record_trajectory.cpp @@ -176,6 +176,8 @@ int main(int argc, char** argv) // 获取当前状态 vector positions = robot.getCurrentPos(); vector velocities = robot.getCurrentVel(); + double gripper_pos = robot.getCurrentPosGripper(); + double gripper_vel = robot.getCurrentVelGripper(); // 计算重力补偿力矩（使用 Panthera 类的方法） vector 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; } diff --git a/panthera_cpp/robot_cpp/example/5_replay_trajectory.cpp b/panthera_cpp/robot_cpp/example/5_replay_trajectory.cpp index 1a96ba4..e6fec64 100644 --- a/panthera_cpp/robot_cpp/example/5_replay_trajectory.cpp +++ b/panthera_cpp/robot_cpp/example/5_replay_trajectory.cpp @@ -239,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(); @@ -289,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) {