diff --git a/kdl-wasm/CMakeLists.txt b/kdl-wasm/CMakeLists.txt index da54f15..131170b 100644 --- a/kdl-wasm/CMakeLists.txt +++ b/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" diff --git a/kdl-wasm/bindings/kdl_c_api.cpp b/kdl-wasm/bindings/kdl_c_api.cpp index 810e201..a61378f 100644 --- a/kdl-wasm/bindings/kdl_c_api.cpp +++ b/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 active_joint_names; std::vector joints; + std::vector link_order; #ifdef KDL_WASM_HAS_OROCOS_KDL KDL::Chain chain; std::unique_ptr fk_solver; std::unique_ptr jac_solver; + std::unique_ptr 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 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(record.chain); record.jac_solver = std::make_unique(record.chain); + record.ik_solver = std::make_unique(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::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 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 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(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(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(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; - (void)options_json; - (void)out_json; - (void)out_len; - return not_implemented("kdl_sample_trap is pending KW-007 native binding"); + try { + const TrapProfile profile = make_trap_profile(length, options_json); + const int result = write_json(trap_profile_json(profile), out_json, out_len); + if (result == 0) { + clear_last_error(); + } + return result; + } catch (const std::exception& error) { + set_last_error("KDL_INVALID_TRAP_PROFILE", error.what()); + return -1; + } } EMSCRIPTEN_KEEPALIVE diff --git a/kdl-wasm/web/tests/kdl/cAbi.test.ts b/kdl-wasm/web/tests/kdl/cAbi.test.ts index e064e44..4416d07 100644 --- a/kdl-wasm/web/tests/kdl/cAbi.test.ts +++ b/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 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"], + [404, seed, 2, target, "{}", out] + ); + + expect(returnCode).toBe(-1); + expect(abi.lastError()).toMatchObject({ + code: "KDL_INVALID_HANDLE", + diagnostics: [ + { + severity: "error", + code: "KDL_INVALID_HANDLE" + } + ] + }); + expect(() => abi.checkReturnCode(returnCode)).toThrowError( + expect.objectContaining({ + 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 returnCode = abi.callNumber("kdl_ik", ["number", "number", "number", "number", "string", "number"], [1, 0, 0, 0, "{}", 0]); - - expect(returnCode).toBe(-1); - expect(abi.lastError()).toMatchObject({ - code: "KDL_NOT_IMPLEMENTED", - diagnostics: [ - { - severity: "error", - code: "KDL_NOT_IMPLEMENTED" - } + 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(() => abi.checkReturnCode(returnCode)).toThrowError( + 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_NOT_IMPLEMENTED" + 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 () => { diff --git a/working/03-推进台账.md b/working/03-推进台账.md index 1c33223..62cb13c 100644 --- a/working/03-推进台账.md +++ b/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。 diff --git a/working/04-任务矩阵.md b/working/04-任务矩阵.md index e5013a1..19615f3 100644 --- a/working/04-任务矩阵.md +++ b/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 子任务 diff --git a/working/05-验收证据.md b/working/05-验收证据.md index e6d70c2..d6bf4b4 100644 --- a/working/05-验收证据.md +++ b/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 个测试通过。 -4. `npm run test -- kdl fk jacobian checks c-abi` 通过，27 个测试文件共 108 个测试通过。 -5. `npm test` 通过，27 个测试文件共 108 个测试通过。 +3. `npm test -- kdl-wasm/web/tests/kdl/cAbi.test.ts` 通过，1 个测试文件共 6 个测试通过。 +4. `npm run test -- kdl cAbi trapProfile` 通过，27 个测试文件共 109 个测试通过。 +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 行主序矩阵，测试覆盖线速度和角速度分量。 -11. 修复 native solver 生命周期：FK/Jacobian solver 在 map 内稳定 `RobotRecord` 上构造，避免持有 move 前 Chain 引用。 -12. C ABI 失败统一通过 `kdl_last_error` 返回结构化 `code/message/diagnostics`，覆盖 `KDL_NOT_IMPLEMENTED` 和 `KDL_BUFFER_TOO_SMALL`。 -13. 新增 `KdlNativeAbi` 封装 C ABI 导出检查、缓冲区读取、返回码检查和错误归一化。 -14. 新增 `fkPose7` TypedArray 高频 FK 接口，支持复用输出缓冲区并覆盖输出维度错误。 -15. 性能基线已记录 6 轴初始化、TypedArray FK、平面 IK、1000 点批量可达性和 10 秒 4 ms 轨迹采样。 -16. native IK、fkAllLinks JSON 序列化和 native 轨迹规划仍未接入；这些能力继续由 TypeScript baseline 覆盖。 +10. `kdl_fk_all_links` 输出 JSON link pose 列表，测试覆盖 `base_link/link_1/tool0` 顺序和 tip 位姿。 +11. `kdl_jacobian` 通过 `ChainJntToJacSolver` 输出 6 x dof 行主序矩阵，测试覆盖线速度和角速度分量。 +12. `kdl_ik` 通过 `ChainIkSolverPos_LMA` 求解，测试覆盖 native IK 后 FK 回代到 `[pi/2, 0.4]`。 +13. `kdl_sample_trap` 输出完整梯形/三角速度曲线 JSON，测试覆盖 trapezoid、triangle 和非法输入结构化错误。 +14. 修复 native solver 生命周期：FK/Jacobian/IK solver 在 map 内稳定 `RobotRecord` 上构造，避免持有 move 前 Chain 引用。 +15. C ABI 失败统一通过 `kdl_last_error` 返回结构化 `code/message/diagnostics`，覆盖 `KDL_INVALID_HANDLE`、`KDL_INVALID_TRAP_PROFILE` 和 `KDL_BUFFER_TOO_SMALL`。 +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。 是否通过：通过。 diff --git a/working/README.md b/working/README.md index 92088c1..d8bf4da 100644 --- a/working/README.md +++ b/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。