接入KDL native IK与链路采样

kdl-wasm/CMakeLists.txt

@@ -30,6 +30,7 @@ if(KDL_SOURCE_DIR)
   list(APPEND KDL_WASM_SOURCES
     "${KDL_SOURCE_DIR}/src/chain.cpp"
     "${KDL_SOURCE_DIR}/src/chainfksolverpos_recursive.cpp"
+    "${KDL_SOURCE_DIR}/src/chainiksolverpos_lma.cpp"
     "${KDL_SOURCE_DIR}/src/chainjnttojacsolver.cpp"
     "${KDL_SOURCE_DIR}/src/frames.cpp"
     "${KDL_SOURCE_DIR}/src/jacobian.cpp"

kdl-wasm/bindings/kdl_c_api.cpp

@@ -19,6 +19,7 @@
 #ifdef KDL_WASM_HAS_OROCOS_KDL
 #include "chain.hpp"
 #include "chainfksolverpos_recursive.hpp"
+#include "chainiksolverpos_lma.hpp"
 #include "chainjnttojacsolver.hpp"
 #include "jntarray.hpp"
 #include "segment.hpp"
@@ -286,10 +287,12 @@ struct RobotRecord {
   std::string tip_link;
   std::vector<std::string> active_joint_names;
   std::vector<NativeJoint> joints;
+  std::vector<std::string> link_order;
 #ifdef KDL_WASM_HAS_OROCOS_KDL
   KDL::Chain chain;
   std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver;
   std::unique_ptr<KDL::ChainJntToJacSolver> jac_solver;
+  std::unique_ptr<KDL::ChainIkSolverPos_LMA> ik_solver;
 #endif
 };
 
@@ -337,6 +340,18 @@ std::string json_escape(const std::string& value) {
   return escaped;
 }
 
+#ifdef KDL_WASM_HAS_OROCOS_KDL
+void append_pose_json(std::ostringstream& json, const KDL::Frame& frame) {
+  double x = 0.0;
+  double y = 0.0;
+  double z = 0.0;
+  double w = 1.0;
+  frame.M.GetQuaternion(x, y, z, w);
+  json << "{\"position\":[" << frame.p.x() << "," << frame.p.y() << "," << frame.p.z()
+       << "],\"quaternion\":[" << x << "," << y << "," << z << "," << w << "]}";
+}
+#endif
+
 const JsonValue& required_object_property(const JsonValue& object, const std::string& key) {
   const JsonValue* value = object.get(key);
   if (!value) {
@@ -370,6 +385,25 @@ std::vector<std::string> string_array_property(const JsonValue& object, const st
   return out;
 }
 
+double number_property(const JsonValue& object, const std::string& key) {
+  const JsonValue& value = required_object_property(object, key);
+  if (value.kind != JsonValue::Kind::Number) {
+    throw std::runtime_error("Expected number model property: " + key);
+  }
+  return value.number_value;
+}
+
+double number_property_or(const JsonValue& object, const std::string& key, double fallback) {
+  const JsonValue* value = object.get(key);
+  if (!value) {
+    return fallback;
+  }
+  if (value->kind != JsonValue::Kind::Number) {
+    throw std::runtime_error("Expected numeric property: " + key);
+  }
+  return value->number_value;
+}
+
 void copy_number_triple(const JsonValue& object, const std::string& key, double out[3]) {
   const JsonValue& value = required_object_property(object, key);
   if (value.kind != JsonValue::Kind::Array || value.array_value.size() != 3) {
@@ -438,6 +472,8 @@ KDL::Joint kdl_joint_from_model(const NativeJoint& joint) {
 
 void build_kdl_chain(RobotRecord& record) {
   record.chain = KDL::Chain();
+  record.link_order.clear();
+  record.link_order.push_back(record.base_link);
   for (const NativeJoint& joint : record.joints) {
     const KDL::Frame origin = frame_from_xyz_rpy(joint.xyz, joint.rpy);
     if (joint.xyz[0] != 0.0 || joint.xyz[1] != 0.0 || joint.xyz[2] != 0.0 ||
@@ -445,9 +481,11 @@ void build_kdl_chain(RobotRecord& record) {
       record.chain.addSegment(KDL::Segment(joint.name + "_origin", KDL::Joint(KDL::Joint::Fixed), origin));
     }
     record.chain.addSegment(KDL::Segment(joint.child, kdl_joint_from_model(joint), KDL::Frame::Identity()));
+    record.link_order.push_back(joint.child);
   }
   record.fk_solver = std::make_unique<KDL::ChainFkSolverPos_recursive>(record.chain);
   record.jac_solver = std::make_unique<KDL::ChainJntToJacSolver>(record.chain);
+  record.ik_solver = std::make_unique<KDL::ChainIkSolverPos_LMA>(record.chain);
 }
 
 KDL::JntArray jnt_array_from_input(const RobotRecord& record, const double* joints, int n) {
@@ -473,6 +511,20 @@ void write_pose7(const KDL::Frame& frame, double* out_pose7) {
   out_pose7[2] = frame.p.z();
   frame.M.GetQuaternion(out_pose7[3], out_pose7[4], out_pose7[5], out_pose7[6]);
 }
+
+KDL::Frame frame_from_pose7(const double* pose7) {
+  if (pose7 == nullptr) {
+    throw std::runtime_error("Target pose buffer is required");
+  }
+  const double norm =
+      std::sqrt(pose7[3] * pose7[3] + pose7[4] * pose7[4] + pose7[5] * pose7[5] + pose7[6] * pose7[6]);
+  if (norm <= 0.0) {
+    throw std::runtime_error("Target pose quaternion must be non-zero");
+  }
+  return KDL::Frame(
+      KDL::Rotation::Quaternion(pose7[3] / norm, pose7[4] / norm, pose7[5] / norm, pose7[6] / norm),
+      KDL::Vector(pose7[0], pose7[1], pose7[2]));
+}
 #endif
 
 RobotRecord parse_robot_record(const char* model_json) {
@@ -505,6 +557,189 @@ std::map<int, RobotRecord>::iterator find_robot(int robot_handle) {
   return it;
 }
 
+double distance_for_velocity_change(double from_velocity, double to_velocity, double acceleration) {
+  return std::max(0.0, (to_velocity * to_velocity - from_velocity * from_velocity) / (2.0 * acceleration));
+}
+
+double clamp01(double value) {
+  return std::max(0.0, std::min(1.0, value));
+}
+
+struct TrapProfile {
+  std::string type;
+  double length = 0.0;
+  double sample_time = 0.0;
+  double start_velocity = 0.0;
+  double end_velocity = 0.0;
+  double max_acceleration = 0.0;
+  double duration = 0.0;
+  double t_accel = 0.0;
+  double t_const = 0.0;
+  double t_decel = 0.0;
+  double v_peak = 0.0;
+  bool triangle_diagnostic = false;
+  bool zero_length_diagnostic = false;
+};
+
+struct TrapPoint {
+  double distance = 0.0;
+  double velocity = 0.0;
+  double acceleration = 0.0;
+};
+
+TrapProfile make_trap_profile(double length, const char* options_json) {
+  if (options_json == nullptr) {
+    throw std::runtime_error("Trap profile options JSON is required");
+  }
+  const JsonValue options = JsonParser(options_json).parse();
+  if (options.kind != JsonValue::Kind::Object) {
+    throw std::runtime_error("Trap profile options must be a JSON object");
+  }
+
+  TrapProfile profile;
+  profile.length = length;
+  profile.sample_time = number_property(options, "sampleTime");
+  profile.start_velocity = number_property_or(options, "startVelocity", 0.0);
+  profile.end_velocity = number_property_or(options, "endVelocity", 0.0);
+  const double max_velocity = number_property(options, "maxVelocity");
+  profile.max_acceleration = number_property(options, "maxAcceleration");
+
+  if (!std::isfinite(length) || length < 0.0) {
+    throw std::runtime_error("Trap profile length must be a finite non-negative number");
+  }
+  if (!std::isfinite(max_velocity) || max_velocity <= 0.0) {
+    throw std::runtime_error("maxVelocity must be a finite positive number");
+  }
+  if (!std::isfinite(profile.max_acceleration) || profile.max_acceleration <= 0.0) {
+    throw std::runtime_error("maxAcceleration must be a finite positive number");
+  }
+  if (!std::isfinite(profile.sample_time) || profile.sample_time <= 0.0) {
+    throw std::runtime_error("sampleTime must be a finite positive number");
+  }
+  if (length == 0.0 && (profile.start_velocity > 0.0 || profile.end_velocity > 0.0)) {
+    throw std::runtime_error("Zero-length trap profile requires zero startVelocity and endVelocity");
+  }
+  if (!std::isfinite(profile.start_velocity) || profile.start_velocity < 0.0 ||
+      profile.start_velocity > max_velocity) {
+    throw std::runtime_error("startVelocity must be finite, non-negative, and no greater than maxVelocity");
+  }
+  if (!std::isfinite(profile.end_velocity) || profile.end_velocity < 0.0 ||
+      profile.end_velocity > max_velocity) {
+    throw std::runtime_error("endVelocity must be finite, non-negative, and no greater than maxVelocity");
+  }
+
+  if (length == 0.0) {
+    profile.type = "triangle";
+    profile.zero_length_diagnostic = true;
+    return profile;
+  }
+
+  const double d_accel_to_max =
+      distance_for_velocity_change(profile.start_velocity, max_velocity, profile.max_acceleration);
+  const double d_decel_from_max =
+      distance_for_velocity_change(profile.end_velocity, max_velocity, profile.max_acceleration);
+  profile.type = "trapezoid";
+  profile.v_peak = max_velocity;
+
+  if (d_accel_to_max + d_decel_from_max <= length) {
+    profile.t_const = (length - d_accel_to_max - d_decel_from_max) / max_velocity;
+  } else {
+    profile.type = "triangle";
+    profile.triangle_diagnostic = true;
+    profile.v_peak = std::sqrt(std::max(
+        0.0,
+        profile.max_acceleration * length +
+            (profile.start_velocity * profile.start_velocity +
+             profile.end_velocity * profile.end_velocity) /
+                2.0));
+    if (profile.v_peak + 1e-12 < std::max(profile.start_velocity, profile.end_velocity)) {
+      throw std::runtime_error("Profile length is too short for the requested startVelocity/endVelocity");
+    }
+    profile.t_const = 0.0;
+  }
+
+  profile.t_accel = std::max(0.0, (profile.v_peak - profile.start_velocity) / profile.max_acceleration);
+  profile.t_decel = std::max(0.0, (profile.v_peak - profile.end_velocity) / profile.max_acceleration);
+  profile.duration = profile.t_accel + profile.t_const + profile.t_decel;
+  return profile;
+}
+
+TrapPoint sample_trap_at_time(const TrapProfile& profile, double time) {
+  const double accel_distance =
+      profile.start_velocity * profile.t_accel +
+      0.5 * profile.max_acceleration * profile.t_accel * profile.t_accel;
+  const double const_distance = profile.v_peak * profile.t_const;
+  const double accel_end = profile.t_accel;
+  const double const_end = profile.t_accel + profile.t_const;
+
+  if (time <= accel_end) {
+    return {
+        profile.start_velocity * time + 0.5 * profile.max_acceleration * time * time,
+        profile.start_velocity + profile.max_acceleration * time,
+        profile.max_acceleration};
+  }
+
+  if (time <= const_end) {
+    const double local_time = time - profile.t_accel;
+    return {accel_distance + profile.v_peak * local_time, profile.v_peak, 0.0};
+  }
+
+  const double local_time = std::min(time - const_end, profile.t_decel);
+  return {
+      accel_distance + const_distance + profile.v_peak * local_time -
+          0.5 * profile.max_acceleration * local_time * local_time,
+      std::max(profile.end_velocity, profile.v_peak - profile.max_acceleration * local_time),
+      -profile.max_acceleration};
+}
+
+std::string trap_profile_json(const TrapProfile& profile) {
+  std::ostringstream json;
+  json << "{\"ok\":true,\"type\":\"" << profile.type << "\",\"length\":" << profile.length
+       << ",\"duration\":" << profile.duration << ",\"tAccel\":" << profile.t_accel
+       << ",\"tConst\":" << profile.t_const << ",\"tDecel\":" << profile.t_decel
+       << ",\"vPeak\":" << profile.v_peak << ",\"samples\":[";
+
+  if (profile.duration == 0.0) {
+    json << "{\"index\":0,\"time\":0,\"s\":0,\"sd\":0,\"sdd\":0}";
+  } else {
+    std::vector<double> times;
+    times.push_back(0.0);
+    for (double time = profile.sample_time; time < profile.duration - 1e-12;
+         time += profile.sample_time) {
+      times.push_back(time);
+    }
+    times.push_back(profile.duration);
+    for (std::size_t index = 0; index < times.size(); index += 1) {
+      if (index > 0) {
+        json << ",";
+      }
+      const TrapPoint sample = sample_trap_at_time(profile, times[index]);
+      const double s =
+          index == 0 ? 0.0 : index == times.size() - 1 ? 1.0 : clamp01(sample.distance / profile.length);
+      json << "{\"index\":" << index << ",\"time\":" << times[index] << ",\"s\":" << s
+           << ",\"sd\":" << sample.velocity / profile.length
+           << ",\"sdd\":" << sample.acceleration / profile.length << "}";
+    }
+  }
+
+  json << "],\"diagnostics\":[";
+  bool wrote_diagnostic = false;
+  if (profile.zero_length_diagnostic) {
+    json << "{\"severity\":\"info\",\"code\":\"KDL_TRAP_ZERO_LENGTH\","
+            "\"message\":\"Trap profile length is zero\"}";
+    wrote_diagnostic = true;
+  }
+  if (profile.triangle_diagnostic) {
+    if (wrote_diagnostic) {
+      json << ",";
+    }
+    json << "{\"severity\":\"info\",\"code\":\"KDL_TRAP_TRIANGLE_PROFILE\","
+            "\"message\":\"Trap profile length is too short to reach maxVelocity; using triangle profile\"}";
+  }
+  json << "]}";
+  return json.str();
+}
+
 }  // namespace
 
 extern "C" {
@@ -614,12 +849,54 @@ int kdl_fk(int robot_handle, const double* joints, int n, double* out_pose7) {
 EMSCRIPTEN_KEEPALIVE
 int kdl_fk_all_links(int robot_handle, const double* joints, int n, char* out_json,
                      int out_len) {
+#ifdef KDL_WASM_HAS_OROCOS_KDL
+  try {
+    const auto it = find_robot(robot_handle);
+    if (it == robots.end()) {
+      return -1;
+    }
+    RobotRecord& record = it->second;
+    const KDL::JntArray q = jnt_array_from_input(record, joints, n);
+    std::vector<KDL::Frame> frames(record.chain.getNrOfSegments());
+    const int result = record.fk_solver->JntToCart(q, frames);
+    if (result != 0) {
+      set_last_error("KDL_FK_FAILED", "Native KDL link FK solver failed");
+      return -1;
+    }
+
+    std::ostringstream json;
+    json << "{\"ok\":true,\"linkPoses\":[";
+    json << "{\"link\":\"" << json_escape(record.base_link)
+         << "\",\"pose\":{\"position\":[0,0,0],\"quaternion\":[0,0,0,1]}}";
+    for (std::size_t segment_index = 0; segment_index < frames.size(); segment_index += 1) {
+      const KDL::Segment& segment = record.chain.getSegment(static_cast<unsigned int>(segment_index));
+      const std::string& segment_name = segment.getName();
+      if (segment_name.size() >= 7 &&
+          segment_name.compare(segment_name.size() - 7, 7, "_origin") == 0) {
+        continue;
+      }
+      json << ",{\"link\":\"" << json_escape(segment_name) << "\",\"pose\":";
+      append_pose_json(json, frames[segment_index]);
+      json << "}";
+    }
+    json << "],\"diagnostics\":[]}";
+    const int write_result = write_json(json.str(), out_json, out_len);
+    if (write_result == 0) {
+      clear_last_error();
+    }
+    return write_result;
+  } catch (const std::exception& error) {
+    set_last_error("KDL_FK_FAILED", error.what());
+    return -1;
+  }
+#else
   (void)robot_handle;
   (void)joints;
   (void)n;
   (void)out_json;
   (void)out_len;
-  return not_implemented("kdl_fk_all_links is pending native link-pose serialization");
+  return not_implemented("kdl_fk_all_links requires KDL_SOURCE_DIR");
+#endif
 }
 
 EMSCRIPTEN_KEEPALIVE
@@ -664,13 +941,43 @@ int kdl_jacobian(int robot_handle, const double* joints, int n, double* out_matr
 EMSCRIPTEN_KEEPALIVE
 int kdl_ik(int robot_handle, const double* seed, int n, const double* target_pose7,
            const char* options_json, double* out_joints) {
+#ifdef KDL_WASM_HAS_OROCOS_KDL
+  (void)options_json;
+  try {
+    const auto it = find_robot(robot_handle);
+    if (it == robots.end()) {
+      return -1;
+    }
+    RobotRecord& record = it->second;
+    if (out_joints == nullptr) {
+      throw std::runtime_error("Output joint buffer is required");
+    }
+    const KDL::JntArray q_seed = jnt_array_from_input(record, seed, n);
+    const KDL::Frame target = frame_from_pose7(target_pose7);
+    KDL::JntArray q_out(static_cast<unsigned int>(n));
+    const int result = record.ik_solver->CartToJnt(q_seed, target, q_out);
+    if (result != 0) {
+      set_last_error("KDL_IK_FAILED", record.ik_solver->strError(result));
+      return -1;
+    }
+    for (int index = 0; index < n; index += 1) {
+      out_joints[index] = q_out(static_cast<unsigned int>(index));
+    }
+    clear_last_error();
+    return 0;
+  } catch (const std::exception& error) {
+    set_last_error("KDL_IK_FAILED", error.what());
+    return -1;
+  }
+#else
   (void)robot_handle;
   (void)seed;
   (void)n;
   (void)target_pose7;
   (void)options_json;
   (void)out_joints;
-  return not_implemented("kdl_ik is pending native IK binding");
+  return not_implemented("kdl_ik requires KDL_SOURCE_DIR");
+#endif
 }
 
 EMSCRIPTEN_KEEPALIVE
@@ -716,11 +1023,17 @@ int kdl_plan_path(int robot_handle, const char* request_json, char* out_json,
 EMSCRIPTEN_KEEPALIVE
 int kdl_sample_trap(double length, const char* options_json, char* out_json,
                     int out_len) {
-  (void)length;
+  try {
-  (void)options_json;
+    const TrapProfile profile = make_trap_profile(length, options_json);
-  (void)out_json;
+    const int result = write_json(trap_profile_json(profile), out_json, out_len);
-  (void)out_len;
+    if (result == 0) {
-  return not_implemented("kdl_sample_trap is pending KW-007 native binding");
+      clear_last_error();
+    }
+    return result;
+  } catch (const std::exception& error) {
+    set_last_error("KDL_INVALID_TRAP_PROFILE", error.what());
+    return -1;
+  }
 }
 
 EMSCRIPTEN_KEEPALIVE

kdl-wasm/web/tests/kdl/cAbi.test.ts

@@ -113,7 +113,7 @@ describe("KDL C ABI", () => {
     expect(abi.callNumber("kdl_destroy_robot", ["number"], [handle])).toBe(0);
   });
 
-  it("constructs a native KDL chain and returns real FK and Jacobian data", async () => {
+  it("constructs a native KDL chain and returns real FK, fkAllLinks, Jacobian, and IK data", async () => {
     const native = await loadNativeModule();
     const abi = new KdlNativeAbi(native);
     const model = loadRobotFromUrdfModel(NATIVE_SOLVER_URDF, {
@@ -135,8 +135,11 @@ describe("KDL C ABI", () => {
     const joints = writeFloat64Array(native, [Math.PI / 2, 0.4]);
     const pose = native._malloc?.(7 * Float64Array.BYTES_PER_ELEMENT);
     const jacobian = native._malloc?.(12 * Float64Array.BYTES_PER_ELEMENT);
+    const ikSeed = writeFloat64Array(native, [1.4, 0.3]);
+    const ikOut = native._malloc?.(2 * Float64Array.BYTES_PER_ELEMENT);
     expect(pose).toBeTruthy();
     expect(jacobian).toBeTruthy();
+    expect(ikOut).toBeTruthy();
 
     try {
       expect(abi.callNumber("kdl_fk", ["number", "number", "number", "number"], [handle, joints, 2, pose])).toBe(0);
@@ -147,6 +150,15 @@ describe("KDL C ABI", () => {
       expect(pose7[5]).toBeCloseTo(Math.SQRT1_2);
       expect(pose7[6]).toBeCloseTo(Math.SQRT1_2);
 
+      const links = abi.readJsonCall<{
+        ok: boolean;
+        linkPoses: Array<{ link: string; pose: { position: [number, number, number] } }>;
+      }>("kdl_fk_all_links", ["number", "number", "number"], [handle, joints, 2]);
+      expect(links.ok).toBe(true);
+      expect(links.linkPoses.map((linkPose) => linkPose.link)).toEqual(["base_link", "link_1", "tool0"]);
+      expect(links.linkPoses[2]?.pose.position[0]).toBeCloseTo(0);
+      expect(links.linkPoses[2]?.pose.position[1]).toBeCloseTo(0.4);
+
       expect(abi.callNumber("kdl_jacobian", ["number", "number", "number", "number"], [handle, joints, 2, jacobian])).toBe(0);
       const jac = readFloat64Array(native, jacobian!, 12);
       expect(jac[0]).toBeCloseTo(-0.4, 4);
@@ -154,39 +166,133 @@ describe("KDL C ABI", () => {
       expect(jac[2]).toBeCloseTo(0, 4);
       expect(jac[3]).toBeCloseTo(1, 4);
       expect(jac[10]).toBeCloseTo(1, 4);
+
+      expect(
+        abi.callNumber(
+          "kdl_ik",
+          ["number", "number", "number", "number", "string", "number"],
+          [handle, ikSeed, 2, pose, "{}", ikOut]
+        )
+      ).toBe(0);
+      const ikJoints = readFloat64Array(native, ikOut!, 2);
+      expect(ikJoints[0]).toBeCloseTo(Math.PI / 2, 4);
+      expect(ikJoints[1]).toBeCloseTo(0.4, 4);
     } finally {
       native._free?.(joints);
+      native._free?.(ikSeed);
       if (pose) {
         native._free?.(pose);
       }
       if (jacobian) {
         native._free?.(jacobian);
       }
+      if (ikOut) {
+        native._free?.(ikOut);
+      }
       abi.callNumber("kdl_destroy_robot", ["number"], [handle]);
     }
   });
 
   it("normalizes C ABI failures through kdl_last_error", async () => {
-    const abi = new KdlNativeAbi(await loadNativeModule());
+    const native = await loadNativeModule();
+    const abi = new KdlNativeAbi(native);
+    const seed = writeFloat64Array(native, [0, 0]);
+    const target = writeFloat64Array(native, [0, 0, 0, 0, 0, 0, 1]);
+    const out = native._malloc?.(2 * Float64Array.BYTES_PER_ELEMENT);
 
+    try {
       expect(abi.callNumber("kdl_init", ["string"], ["{}"])).toBe(0);
-    const returnCode = abi.callNumber("kdl_ik", ["number", "number", "number", "number", "string", "number"], [1, 0, 0, 0, "{}", 0]);
+      const returnCode = abi.callNumber(
+        "kdl_ik",
+        ["number", "number", "number", "number", "string", "number"],
+        [404, seed, 2, target, "{}", out]
+      );
 
       expect(returnCode).toBe(-1);
       expect(abi.lastError()).toMatchObject({
-      code: "KDL_NOT_IMPLEMENTED",
+        code: "KDL_INVALID_HANDLE",
         diagnostics: [
           {
             severity: "error",
-          code: "KDL_NOT_IMPLEMENTED"
+            code: "KDL_INVALID_HANDLE"
           }
         ]
       });
       expect(() => abi.checkReturnCode(returnCode)).toThrowError(
         expect.objectContaining({
-        code: "KDL_NOT_IMPLEMENTED"
+          code: "KDL_INVALID_HANDLE"
         })
       );
+    } finally {
+      native._free?.(seed);
+      native._free?.(target);
+      if (out) {
+        native._free?.(out);
+      }
+    }
+  });
+
+  it("returns native trapezoid samples through kdl_sample_trap", async () => {
+    const abi = new KdlNativeAbi(await loadNativeModule());
+
+    expect(abi.callNumber("kdl_init", ["string"], ["{}"])).toBe(0);
+    const profile = abi.readJsonCall<{
+      ok: boolean;
+      type: string;
+      duration: number;
+      samples: Array<{ index: number; time: number; s: number; sd: number; sdd: number }>;
+      diagnostics: Array<{ code: string }>;
+    }>(
+      "kdl_sample_trap",
+      ["number", "string"],
+      [
+        2,
+        JSON.stringify({
+          maxVelocity: 1,
+          maxAcceleration: 1,
+          sampleTime: 0.25
+        })
+      ]
+    );
+
+    expect(profile).toMatchObject({
+      ok: true,
+      type: "trapezoid",
+      duration: 3,
+      diagnostics: []
+    });
+    expect(profile.samples[0]).toMatchObject({ index: 0, time: 0, s: 0 });
+    expect(profile.samples.find((sample) => sample.time === 1)?.s).toBeCloseTo(0.25);
+    expect(profile.samples.find((sample) => sample.time === 1)?.sd).toBeCloseTo(0.5);
+    expect(profile.samples.at(-1)).toMatchObject({ s: 1 });
+
+    const triangle = abi.readJsonCall<{
+      type: string;
+      diagnostics: Array<{ code: string }>;
+    }>(
+      "kdl_sample_trap",
+      ["number", "string"],
+      [
+        0.5,
+        JSON.stringify({
+          maxVelocity: 2,
+          maxAcceleration: 1,
+          sampleTime: 0.1
+        })
+      ]
+    );
+    expect(triangle.type).toBe("triangle");
+    expect(triangle.diagnostics).toContainEqual(
+      expect.objectContaining({
+        code: "KDL_TRAP_TRIANGLE_PROFILE"
+      })
+    );
+
+    const returnCode = abi.callNumber("kdl_sample_trap", ["number", "string", "number", "number"], [1, "{}", 0, 0]);
+    expect(returnCode).toBe(-1);
+    expect(abi.lastError()).toMatchObject({
+      code: "KDL_INVALID_TRAP_PROFILE"
+    });
   });
 
   it("reports JSON output buffer errors without raw strings", async () => {

working/03-推进台账.md

@@ -1185,3 +1185,47 @@
 
 1. 若继续深化 KDL native 主线，优先接入 native `fkAllLinks` JSON 输出和 native IK。
 2. native IK 稳定后，再逐步把 `planMoveL/planMoveC/planPath` 从 TypeScript baseline 切到 native solver。
+
+### 轮次 025
+
+日期：2026-06-27
+关联任务：`KW-003`、`KW-004`、`KW-007`、`KW-013`
+本轮目标：闭合轮次 024 遗留的 native `fkAllLinks` JSON 输出、native IK 和 native sampleTrap C ABI 基线。
+
+做了什么：
+
+1. C ABI 增加 native `kdl_fk_all_links` JSON 序列化，输出 `{ ok, linkPoses, diagnostics }`，并过滤 KDL 内部 origin 辅助段。
+2. CMake 接入 Orocos KDL `ChainIkSolverPos_LMA` 源文件。
+3. `RobotRecord` 持有 native IK solver，随 KDL Chain 生命周期稳定构造。
+4. 实现 native `kdl_ik`，从 pose7 归一化四元数构造 KDL Frame，并输出 joint buffer。
+5. 实现 native `kdl_sample_trap`，输出完整 trapezoid/triangle profile JSON，覆盖 zero length、triangle fallback 和非法输入诊断。
+6. 扩展 `cAbi.test.ts`，覆盖 native FK、fkAllLinks、Jacobian、IK、sampleTrap 和结构化失败路径。
+7. 更新 `README.md`、任务矩阵和验收证据，修正 native IK/fkAllLinks 的旧待办表述。
+
+改了哪些文件：
+
+1. `/home/meswork/kdl_work/kdl-wasm/CMakeLists.txt`
+2. `/home/meswork/kdl_work/kdl-wasm/bindings/kdl_c_api.cpp`
+3. `/home/meswork/kdl_work/kdl-wasm/web/tests/kdl/cAbi.test.ts`
+4. `/home/meswork/kdl_work/working/README.md`
+5. `/home/meswork/kdl_work/working/03-推进台账.md`
+6. `/home/meswork/kdl_work/working/04-任务矩阵.md`
+7. `/home/meswork/kdl_work/working/05-验收证据.md`
+
+验证了什么：
+
+1. `cmake --build kdl-wasm/build-wasm -j16` 通过。
+2. `npm run test -- kdl cAbi trapProfile` 通过，27 个测试文件共 109 个测试通过。
+3. `npm run typecheck` 通过。
+4. `npm test` 通过，27 个测试文件共 109 个测试通过。
+
+问题和风险：
+
+1. Worker runtime 当前仍由 TypeScript registry 管理高层机器人句柄；native C ABI 已覆盖 FK、fkAllLinks、Jacobian、IK 和 sampleTrap，但尚未成为 Worker 默认计算路径。
+2. native `planMoveJ/planMoveL/planMoveC/planPath` 仍保留稳定导出边界，高层轨迹规划继续由 TypeScript runtime 基线承载。
+3. C ABI 轻量 JSON 解析仍只覆盖当前 `NormalizedRobotModel` 和 trap options 必需字段，后续 schema 扩展需要同步增强。
+
+下一步：
+
+1. 若继续深化 KDL native 主线，优先设计 Worker 中 TypeScript handle 与 native RobotHandle 的映射关系。
+2. 映射稳定后，再逐步把 `planMoveJ/planMoveL/planMoveC/planPath` 从 TypeScript runtime 切到 native solver 或 native-assisted solver。

working/04-任务矩阵.md

@@ -72,7 +72,7 @@
 | KW-002.1 | URDF XML 解析 | Done | link/joint/origin/axis/limit 可读取。 |
 | KW-002.2 | 连通性和单位检查 | Done | base/tip 不连通有诊断。 |
 | KW-002.3 | 生成 `NormalizedRobotModel` | Done | activeJointNames 和 limits 稳定。 |
-| KW-002.4 | WASM 创建 KDL Chain | Done | C ABI 根据 `NormalizedRobotModel` 构造 native KDL Chain，FK/Jacobian golden 通过。 |
+| KW-002.4 | WASM 创建 KDL Chain | Done | C ABI 根据 `NormalizedRobotModel` 构造 native KDL Chain，FK/fkAllLinks/Jacobian/IK golden 通过。 |
 | KW-002.5 | RobotHandle 生命周期 | Done | create/getInfo/getLimits/destroy 通过。 |
 
 ### KW-003 到 KW-013：KDL P0 计算接口
@@ -96,7 +96,7 @@
 | KW-011.1 | `planPath` | Done | 多段轨迹合并正确。 |
 | KW-011.2 | `validatePath` | Done | 段报告和诊断正确。 |
 | KW-012.1 | `estimateCycleTime/resampleTrajectory` | Done | 节拍和重采样正确。 |
-| KW-013.1 | C ABI / Embind 稳定导出 | Done | TypeScript 可包装全部 P0 API。 |
+| KW-013.1 | C ABI / Embind 稳定导出 | Done | 稳定导出完整，native FK/fkAllLinks/Jacobian/IK/sampleTrap 已有 C ABI 测试；高层轨迹规划由 TypeScript runtime 包装承载。 |
 | KW-013.2 | TypedArray 和批量性能 | Done | 性能指标有记录。 |
 
 ## 5. GRL 子任务

working/05-验收证据.md

@@ -109,7 +109,7 @@ emcmake/emcc: /home/meswork/emsdk/upstream/emscripten
 3. 产物存在：`kdl-wasm/build-wasm/kdl.js`、`kdl-wasm/build-wasm/kdl.wasm`、`kdl-wasm/build-wasm/kdl.d.ts`。
 4. `npm run test -- kdl rpc` 通过，2 个测试文件共 11 个测试通过。
 5. Node 动态加载 `kdl.js` 并调用 `kdl_init`，输出 `{"rc":0,"hasCwrap":true}`。
-6. 当前 C ABI 在 `KW-013` 已补稳定导出、RobotHandle 生命周期和结构化错误边界；轮次 024 已进一步接入 native KDL Chain、FK 和 Jacobian，native IK 和轨迹计算仍由 TypeScript runtime 基线承载。
+6. 当前 C ABI 在 `KW-013` 已补稳定导出、RobotHandle 生命周期和结构化错误边界；轮次 024/025 已进一步接入 native KDL Chain、FK、fkAllLinks、Jacobian、IK 和 sampleTrap，高层轨迹规划仍由 TypeScript runtime 基线承载。
 
 是否通过：通过。
 
@@ -148,7 +148,7 @@ Vitest 3.2.6
 4. `NormalizedRobotModel` 生成 `activeJointNames`、`limits`、`source.urdfHash`。
 5. base/tip 不连通、unsupported joint type 返回 `KDL_INVALID_MODEL` 结构化诊断。
 6. Worker runtime 支持 `loadRobotFromUrdf/createRobotFromModel/getRobotInfo/getJointLimits/destroyRobot` 的 RobotHandle 生命周期。
-7. Worker runtime 当前仍由 TypeScript registry 管理 RobotHandle；C ABI 已可根据同一 `NormalizedRobotModel` 构造 native KDL Chain 并执行 FK/Jacobian。
+7. Worker runtime 当前仍由 TypeScript registry 管理 RobotHandle；C ABI 已可根据同一 `NormalizedRobotModel` 构造 native KDL Chain 并执行 FK、fkAllLinks、Jacobian、IK 和 sampleTrap。
 
 是否通过：通过。
 
@@ -187,7 +187,7 @@ Vitest 3.2.6
 4. `fk` 返回 `ok/flange/tcp/joints/diagnostics`。
 5. `fkAllLinks` 返回 base-to-tip link pose 顺序。
 6. 覆盖零位 FK、revolute + prismatic 关节、tool TCP 偏移、link 顺序和关节维度结构化诊断。
-7. Worker `fk/fkAllLinks` 当前为 TypeScript 标准模型数值基线；C ABI 已补 native `kdl_fk` golden 测试，`fkAllLinks` native JSON 序列化仍待后续接入。
+7. Worker `fk/fkAllLinks` 当前为 TypeScript 标准模型数值基线；C ABI 已补 native `kdl_fk` pose7 golden 测试和 `kdl_fk_all_links` JSON link pose 顺序测试。
 
 是否通过：通过。
 
@@ -224,7 +224,7 @@ Vitest 3.2.6
 2. `npm run test -- kdl ik` 通过，8 个测试文件共 34 个测试通过。
 3. `ik` 返回 `ok/joints/iterations/residualPosition/residualOrientation/reason/diagnostics`。
 4. 覆盖可达目标 IK、FK 回代误差、`ikBatch` 顺序保持、关节限位失败、unsupported model 的 `invalid_model` reason。
-5. 当前 IK 为 TypeScript 基线几何求解器，支持 single-prismatic 和 Rz+Px 链；native 通用 6 轴迭代 IK 与 Orocos KDL solver 接入后继续扩展。
+5. Worker IK 当前为 TypeScript 基线几何求解器，支持 single-prismatic 和 Rz+Px 链；C ABI 已补 native Orocos KDL `ChainIkSolverPos_LMA` IK 回代测试，后续可继续把 Worker IK 路由切到 native solver。
 
 是否通过：通过。
 
@@ -611,20 +611,23 @@ Emscripten emcc 6.0.0
 
 1. `npm run typecheck` 通过。
 2. `cmake --build kdl-wasm/build-wasm -j16` 通过。
-3. `npm test -- kdl-wasm/web/tests/kdl/cAbi.test.ts` 通过，1 个测试文件共 5 个测试通过。
+3. `npm test -- kdl-wasm/web/tests/kdl/cAbi.test.ts` 通过，1 个测试文件共 6 个测试通过。
-4. `npm run test -- kdl fk jacobian checks c-abi` 通过，27 个测试文件共 108 个测试通过。
+4. `npm run test -- kdl cAbi trapProfile` 通过，27 个测试文件共 109 个测试通过。
-5. `npm test` 通过，27 个测试文件共 108 个测试通过。
+5. `npm test` 通过，27 个测试文件共 109 个测试通过。
 6. C ABI 稳定导出 `kdl_init/create_robot/destroy_robot/get_robot_info/fk/fk_all_links/jacobian/ik/plan_movej/plan_movel/plan_movec/plan_path/sample_trap/last_error`。
 7. C++ ABI 现支持 `kdl_create_robot/kdl_destroy_robot/kdl_get_robot_info` 的 handle 生命周期和 JSON 输出。
 8. WASM wrapper 链接 Orocos KDL 最小源码集，C ABI 可从 `NormalizedRobotModel` 构造 native KDL Chain。
 9. `kdl_fk` 通过 `ChainFkSolverPos_recursive` 输出 pose7，测试覆盖 Rz+Px 链 `[pi/2, 0.4]` 的位置和四元数 golden。
-10. `kdl_jacobian` 通过 `ChainJntToJacSolver` 输出 6 x dof 行主序矩阵，测试覆盖线速度和角速度分量。
+10. `kdl_fk_all_links` 输出 JSON link pose 列表，测试覆盖 `base_link/link_1/tool0` 顺序和 tip 位姿。
-11. 修复 native solver 生命周期：FK/Jacobian solver 在 map 内稳定 `RobotRecord` 上构造，避免持有 move 前 Chain 引用。
+11. `kdl_jacobian` 通过 `ChainJntToJacSolver` 输出 6 x dof 行主序矩阵，测试覆盖线速度和角速度分量。
-12. C ABI 失败统一通过 `kdl_last_error` 返回结构化 `code/message/diagnostics`，覆盖 `KDL_NOT_IMPLEMENTED` 和 `KDL_BUFFER_TOO_SMALL`。
+12. `kdl_ik` 通过 `ChainIkSolverPos_LMA` 求解，测试覆盖 native IK 后 FK 回代到 `[pi/2, 0.4]`。
-13. 新增 `KdlNativeAbi` 封装 C ABI 导出检查、缓冲区读取、返回码检查和错误归一化。
+13. `kdl_sample_trap` 输出完整梯形/三角速度曲线 JSON，测试覆盖 trapezoid、triangle 和非法输入结构化错误。
-14. 新增 `fkPose7` TypedArray 高频 FK 接口，支持复用输出缓冲区并覆盖输出维度错误。
+14. 修复 native solver 生命周期：FK/Jacobian/IK solver 在 map 内稳定 `RobotRecord` 上构造，避免持有 move 前 Chain 引用。
-15. 性能基线已记录 6 轴初始化、TypedArray FK、平面 IK、1000 点批量可达性和 10 秒 4 ms 轨迹采样。
+15. C ABI 失败统一通过 `kdl_last_error` 返回结构化 `code/message/diagnostics`，覆盖 `KDL_INVALID_HANDLE`、`KDL_INVALID_TRAP_PROFILE` 和 `KDL_BUFFER_TOO_SMALL`。
-16. native IK、fkAllLinks JSON 序列化和 native 轨迹规划仍未接入；这些能力继续由 TypeScript baseline 覆盖。
+16. 新增 `KdlNativeAbi` 封装 C ABI 导出检查、缓冲区读取、返回码检查和错误归一化。
+17. 新增 `fkPose7` TypedArray 高频 FK 接口，支持复用输出缓冲区并覆盖输出维度错误。
+18. 性能基线已记录 6 轴初始化、TypedArray FK、平面 IK、1000 点批量可达性和 10 秒 4 ms 轨迹采样。
+19. native `planMoveJ/planMoveL/planMoveC/planPath` 仍由 TypeScript runtime 基线承载；底层 C ABI 已保留稳定导出，后续可逐步切到 native solver。
 
 是否通过：通过。
 

working/README.md

@@ -66,4 +66,4 @@ OPFS、虚拟控制器、UI、报告等内容只在上述两份文档明确要
 
 ## 5. 当前首要任务
 
-`KW-001` 到 `KW-013`、`KW-100` 到 `KW-112` 的文档对标任务已完成并补充验收证据。C ABI 已能从 `NormalizedRobotModel` 构造 native Orocos KDL Chain，并完成 native FK/Jacobian golden 对比；后续首要关注点是继续接入 native IK、fkAllLinks 序列化和轨迹规划。
+`KW-001` 到 `KW-013`、`KW-100` 到 `KW-112` 的文档对标任务已完成并补充验收证据。C ABI 已能从 `NormalizedRobotModel` 构造 native Orocos KDL Chain，并完成 native FK、fkAllLinks、Jacobian、IK 和 sampleTrap golden 对比；高层 `planMoveJ/planMoveL/planMoveC/planPath` 仍由 TypeScript runtime 基线承载，后续若继续深化 native 主线，首要关注点是把这些轨迹规划导出逐步切到 native solver。