diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ede28bd --- /dev/null +++ b/.gitignore @@ -0,0 +1,8 @@ +node_modules/ +dist/ +coverage/ +kdl-wasm/build*/ +kdl_install/ +work/working1/ +git.txt +*.log diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..d0193c9 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "orocos_kinematics_dynamics"] + path = orocos_kinematics_dynamics + url = https://github.com/orocos/orocos_kinematics_dynamics.git diff --git a/kdl-wasm/CMakeLists.txt b/kdl-wasm/CMakeLists.txt new file mode 100644 index 0000000..da54f15 --- /dev/null +++ b/kdl-wasm/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.16) + +project(kdl_wasm_wrapper LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +set(KDL_SOURCE_DIR "" CACHE PATH "Path to the Orocos KDL source tree") +if(KDL_SOURCE_DIR AND NOT EXISTS "${KDL_SOURCE_DIR}") + message(FATAL_ERROR "KDL_SOURCE_DIR does not exist: ${KDL_SOURCE_DIR}") +endif() + +set(EIGEN3_INCLUDE_DIR "" CACHE PATH "Path to the Eigen3 include directory") +if(KDL_SOURCE_DIR AND NOT EIGEN3_INCLUDE_DIR) + find_path(EIGEN3_INCLUDE_DIR Eigen/Core PATH_SUFFIXES eigen3) +endif() +if(KDL_SOURCE_DIR AND NOT EIGEN3_INCLUDE_DIR AND EXISTS "/usr/include/eigen3/Eigen/Core") + set(EIGEN3_INCLUDE_DIR "/usr/include/eigen3" CACHE PATH "Path to the Eigen3 include directory" FORCE) +endif() +if(KDL_SOURCE_DIR AND NOT EIGEN3_INCLUDE_DIR) + message(FATAL_ERROR "Eigen3 include directory was not found") +endif() + +set(KDL_WASM_SOURCES + bindings/kdl_c_api.cpp +) + +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/chainjnttojacsolver.cpp" + "${KDL_SOURCE_DIR}/src/frames.cpp" + "${KDL_SOURCE_DIR}/src/jacobian.cpp" + "${KDL_SOURCE_DIR}/src/jntarray.cpp" + "${KDL_SOURCE_DIR}/src/joint.cpp" + "${KDL_SOURCE_DIR}/src/rigidbodyinertia.cpp" + "${KDL_SOURCE_DIR}/src/rotationalinertia.cpp" + "${KDL_SOURCE_DIR}/src/segment.cpp" + "${KDL_SOURCE_DIR}/src/utilities/utility.cxx" + ) +endif() + +if(EMSCRIPTEN) + add_executable(kdl ${KDL_WASM_SOURCES}) + target_compile_options(kdl PRIVATE "-fexceptions") + if(KDL_SOURCE_DIR) + target_include_directories(kdl PRIVATE "${KDL_SOURCE_DIR}/src" "${EIGEN3_INCLUDE_DIR}") + target_compile_definitions(kdl PRIVATE KDL_WASM_HAS_OROCOS_KDL=1) + endif() + set_target_properties(kdl PROPERTIES OUTPUT_NAME "kdl" SUFFIX ".js") + target_link_options(kdl PRIVATE + "--no-entry" + "-sMODULARIZE=1" + "-sEXPORT_ES6=1" + "-sEXPORT_NAME=createKdlModule" + "-sENVIRONMENT=web,worker,node" + "-sALLOW_MEMORY_GROWTH=1" + "-fexceptions" + "-sDISABLE_EXCEPTION_CATCHING=0" + "-sEXPORTED_RUNTIME_METHODS=ccall,cwrap,UTF8ToString,stringToUTF8,lengthBytesUTF8,HEAPF64" + "-sEXPORTED_FUNCTIONS=['_malloc','_free','_kdl_init','_kdl_create_robot','_kdl_destroy_robot','_kdl_get_robot_info','_kdl_fk','_kdl_fk_all_links','_kdl_jacobian','_kdl_ik','_kdl_plan_movej','_kdl_plan_movel','_kdl_plan_movec','_kdl_plan_path','_kdl_sample_trap','_kdl_last_error']" + ) + add_custom_command(TARGET kdl POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "${CMAKE_CURRENT_SOURCE_DIR}/bindings/kdl.d.ts" + "${CMAKE_CURRENT_BINARY_DIR}/kdl.d.ts" + ) +else() + add_library(kdl SHARED ${KDL_WASM_SOURCES}) + if(KDL_SOURCE_DIR) + target_include_directories(kdl PRIVATE "${KDL_SOURCE_DIR}/src" "${EIGEN3_INCLUDE_DIR}") + target_compile_definitions(kdl PRIVATE KDL_WASM_HAS_OROCOS_KDL=1) + endif() +endif() diff --git a/kdl-wasm/bindings/kdl.d.ts b/kdl-wasm/bindings/kdl.d.ts new file mode 100644 index 0000000..ad7f794 --- /dev/null +++ b/kdl-wasm/bindings/kdl.d.ts @@ -0,0 +1,26 @@ +export interface KdlModule { + ccall: ( + ident: string, + returnType: string | null, + argTypes: Array, + args: unknown[] + ) => unknown; + cwrap: ( + ident: string, + returnType: string | null, + argTypes: Array + ) => (...args: unknown[]) => unknown; + UTF8ToString: (ptr: number) => string; + stringToUTF8: (value: string, outPtr: number, maxBytesToWrite: number) => void; + lengthBytesUTF8: (value: string) => number; + _malloc: (size: number) => number; + _free: (ptr: number) => void; +} + +export interface KdlModuleFactoryOptions { + locateFile?: (path: string, prefix: string) => string; + print?: (text: string) => void; + printErr?: (text: string) => void; +} + +export default function createKdlModule(options?: KdlModuleFactoryOptions): Promise; diff --git a/kdl-wasm/bindings/kdl_c_api.cpp b/kdl-wasm/bindings/kdl_c_api.cpp new file mode 100644 index 0000000..810e201 --- /dev/null +++ b/kdl-wasm/bindings/kdl_c_api.cpp @@ -0,0 +1,731 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef __EMSCRIPTEN__ +#include +#else +#define EMSCRIPTEN_KEEPALIVE +#endif + +#ifdef KDL_WASM_HAS_OROCOS_KDL +#include "chain.hpp" +#include "chainfksolverpos_recursive.hpp" +#include "chainjnttojacsolver.hpp" +#include "jntarray.hpp" +#include "segment.hpp" +#endif + +namespace { + +std::string last_error = + R"({"code":"KDL_OK","message":"No error","diagnostics":[]})"; +int next_robot_handle = 1; + +struct JsonValue { + enum class Kind { Null, Bool, Number, String, Array, Object }; + + Kind kind = Kind::Null; + bool bool_value = false; + double number_value = 0.0; + std::string string_value; + std::vector array_value; + std::map object_value; + + const JsonValue* get(const std::string& key) const { + if (kind != Kind::Object) { + return nullptr; + } + const auto it = object_value.find(key); + return it == object_value.end() ? nullptr : &it->second; + } +}; + +class JsonParser { + public: + explicit JsonParser(const std::string& input) : input_(input) {} + + JsonValue parse() { + JsonValue value = parse_value(); + skip_ws(); + if (pos_ != input_.size()) { + throw std::runtime_error("Unexpected trailing JSON content"); + } + return value; + } + + private: + const std::string& input_; + std::size_t pos_ = 0; + + JsonValue parse_value() { + skip_ws(); + if (pos_ >= input_.size()) { + throw std::runtime_error("Unexpected end of JSON"); + } + const char ch = input_[pos_]; + if (ch == '{') { + return parse_object(); + } + if (ch == '[') { + return parse_array(); + } + if (ch == '"') { + JsonValue value; + value.kind = JsonValue::Kind::String; + value.string_value = parse_string(); + return value; + } + if (ch == 't' || ch == 'f') { + return parse_bool(); + } + if (ch == 'n') { + expect_literal("null"); + return {}; + } + return parse_number(); + } + + JsonValue parse_object() { + consume('{'); + JsonValue value; + value.kind = JsonValue::Kind::Object; + skip_ws(); + if (match('}')) { + return value; + } + + while (true) { + skip_ws(); + const std::string key = parse_string(); + skip_ws(); + consume(':'); + value.object_value.emplace(key, parse_value()); + skip_ws(); + if (match('}')) { + return value; + } + consume(','); + } + } + + JsonValue parse_array() { + consume('['); + JsonValue value; + value.kind = JsonValue::Kind::Array; + skip_ws(); + if (match(']')) { + return value; + } + + while (true) { + value.array_value.push_back(parse_value()); + skip_ws(); + if (match(']')) { + return value; + } + consume(','); + } + } + + std::string parse_string() { + consume('"'); + std::string out; + while (pos_ < input_.size()) { + const char ch = input_[pos_++]; + if (ch == '"') { + return out; + } + if (ch != '\\') { + out.push_back(ch); + continue; + } + if (pos_ >= input_.size()) { + throw std::runtime_error("Invalid JSON string escape"); + } + const char escaped = input_[pos_++]; + switch (escaped) { + case '"': + case '\\': + case '/': + out.push_back(escaped); + break; + case 'b': + out.push_back('\b'); + break; + case 'f': + out.push_back('\f'); + break; + case 'n': + out.push_back('\n'); + break; + case 'r': + out.push_back('\r'); + break; + case 't': + out.push_back('\t'); + break; + case 'u': + if (pos_ + 4 > input_.size()) { + throw std::runtime_error("Invalid JSON unicode escape"); + } + out.push_back('?'); + pos_ += 4; + break; + default: + throw std::runtime_error("Invalid JSON string escape"); + } + } + throw std::runtime_error("Unterminated JSON string"); + } + + JsonValue parse_bool() { + JsonValue value; + value.kind = JsonValue::Kind::Bool; + if (starts_with("true")) { + value.bool_value = true; + pos_ += 4; + return value; + } + if (starts_with("false")) { + value.bool_value = false; + pos_ += 5; + return value; + } + throw std::runtime_error("Invalid JSON boolean"); + } + + JsonValue parse_number() { + const std::size_t start = pos_; + if (input_[pos_] == '-') { + pos_ += 1; + } + while (pos_ < input_.size() && std::isdigit(static_cast(input_[pos_]))) { + pos_ += 1; + } + if (pos_ < input_.size() && input_[pos_] == '.') { + pos_ += 1; + while (pos_ < input_.size() && std::isdigit(static_cast(input_[pos_]))) { + pos_ += 1; + } + } + if (pos_ < input_.size() && (input_[pos_] == 'e' || input_[pos_] == 'E')) { + pos_ += 1; + if (pos_ < input_.size() && (input_[pos_] == '+' || input_[pos_] == '-')) { + pos_ += 1; + } + while (pos_ < input_.size() && std::isdigit(static_cast(input_[pos_]))) { + pos_ += 1; + } + } + if (start == pos_) { + throw std::runtime_error("Expected JSON number"); + } + JsonValue value; + value.kind = JsonValue::Kind::Number; + value.number_value = std::stod(input_.substr(start, pos_ - start)); + return value; + } + + void expect_literal(const char* literal) { + if (!starts_with(literal)) { + throw std::runtime_error(std::string("Expected JSON literal ") + literal); + } + pos_ += std::strlen(literal); + } + + bool starts_with(const char* literal) const { + const std::size_t len = std::strlen(literal); + return input_.compare(pos_, len, literal) == 0; + } + + void skip_ws() { + while (pos_ < input_.size() && std::isspace(static_cast(input_[pos_]))) { + pos_ += 1; + } + } + + bool match(char expected) { + if (pos_ < input_.size() && input_[pos_] == expected) { + pos_ += 1; + return true; + } + return false; + } + + void consume(char expected) { + if (!match(expected)) { + throw std::runtime_error(std::string("Expected JSON character ") + expected); + } + } +}; + +struct NativeJoint { + std::string name; + std::string type; + std::string child; + double xyz[3] = {0.0, 0.0, 0.0}; + double rpy[3] = {0.0, 0.0, 0.0}; + double axis[3] = {1.0, 0.0, 0.0}; + bool active = false; +}; + +struct RobotRecord { + std::string model_json; + std::string robot_id; + std::string name; + std::string base_link; + std::string tip_link; + std::vector active_joint_names; + std::vector joints; +#ifdef KDL_WASM_HAS_OROCOS_KDL + KDL::Chain chain; + std::unique_ptr fk_solver; + std::unique_ptr jac_solver; +#endif +}; + +std::map robots; + +void set_last_error(const char* code, const char* message) { + last_error = std::string("{\"code\":\"") + code + "\",\"message\":\"" + + message + "\",\"diagnostics\":[{\"severity\":\"error\",\"code\":\"" + + code + "\",\"message\":\"" + message + "\"}]}"; +} + +void clear_last_error() { + last_error = R"({"code":"KDL_OK","message":"No error","diagnostics":[]})"; +} + +int write_json(const std::string& json, char* out_json, int out_len) { + if (out_json == nullptr || out_len <= 0) { + set_last_error("KDL_BUFFER_TOO_SMALL", "Output buffer is not writable"); + return -1; + } + + const int required = static_cast(json.size()) + 1; + if (out_len < required) { + set_last_error("KDL_BUFFER_TOO_SMALL", "Output buffer is too small"); + return -1; + } + + std::memcpy(out_json, json.c_str(), static_cast(required)); + return 0; +} + +int not_implemented(const char* function_name) { + set_last_error("KDL_NOT_IMPLEMENTED", function_name); + return -1; +} + +std::string json_escape(const std::string& value) { + std::string escaped; + for (const char ch : value) { + if (ch == '"' || ch == '\\') { + escaped.push_back('\\'); + } + escaped.push_back(ch); + } + return escaped; +} + +const JsonValue& required_object_property(const JsonValue& object, const std::string& key) { + const JsonValue* value = object.get(key); + if (!value) { + throw std::runtime_error("Missing model property: " + key); + } + return *value; +} + +std::string string_property(const JsonValue& object, const std::string& key) { + const JsonValue& value = required_object_property(object, key); + if (value.kind != JsonValue::Kind::String) { + throw std::runtime_error("Expected string model property: " + key); + } + return value.string_value; +} + +std::vector string_array_property(const JsonValue& object, const std::string& key) { + const JsonValue& value = required_object_property(object, key); + if (value.kind != JsonValue::Kind::Array) { + throw std::runtime_error("Expected string array model property: " + key); + } + + std::vector out; + out.reserve(value.array_value.size()); + for (const JsonValue& item : value.array_value) { + if (item.kind != JsonValue::Kind::String) { + throw std::runtime_error("Expected string item in model property: " + key); + } + out.push_back(item.string_value); + } + return out; +} + +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) { + throw std::runtime_error("Expected numeric triple model property: " + key); + } + for (std::size_t index = 0; index < 3; index += 1) { + if (value.array_value[index].kind != JsonValue::Kind::Number) { + throw std::runtime_error("Expected numeric triple model property: " + key); + } + out[index] = value.array_value[index].number_value; + } +} + +std::vector parse_joints(const JsonValue& model, + const std::vector& active_joint_names) { + const JsonValue& joints_value = required_object_property(model, "joints"); + if (joints_value.kind != JsonValue::Kind::Array) { + throw std::runtime_error("Expected joints array"); + } + + std::vector joints; + joints.reserve(joints_value.array_value.size()); + for (const JsonValue& joint_value : joints_value.array_value) { + if (joint_value.kind != JsonValue::Kind::Object) { + throw std::runtime_error("Expected joint object"); + } + NativeJoint joint; + joint.name = string_property(joint_value, "name"); + joint.type = string_property(joint_value, "type"); + joint.child = string_property(joint_value, "child"); + + const JsonValue& origin = required_object_property(joint_value, "origin"); + if (origin.kind != JsonValue::Kind::Object) { + throw std::runtime_error("Expected joint origin object"); + } + copy_number_triple(origin, "xyz", joint.xyz); + copy_number_triple(origin, "rpy", joint.rpy); + copy_number_triple(joint_value, "axis", joint.axis); + joint.active = std::find(active_joint_names.begin(), active_joint_names.end(), joint.name) != + active_joint_names.end(); + joints.push_back(joint); + } + return joints; +} + +#ifdef KDL_WASM_HAS_OROCOS_KDL +KDL::Frame frame_from_xyz_rpy(const double xyz[3], const double rpy[3]) { + return KDL::Frame( + KDL::Rotation::RPY(rpy[0], rpy[1], rpy[2]), + KDL::Vector(xyz[0], xyz[1], xyz[2])); +} + +KDL::Joint kdl_joint_from_model(const NativeJoint& joint) { + if (joint.type == "fixed") { + return KDL::Joint(joint.name, KDL::Joint::Fixed); + } + const KDL::Vector axis(joint.axis[0], joint.axis[1], joint.axis[2]); + if (joint.type == "revolute" || joint.type == "continuous") { + return KDL::Joint(joint.name, KDL::Vector(0.0, 0.0, 0.0), axis, KDL::Joint::RotAxis); + } + if (joint.type == "prismatic") { + return KDL::Joint(joint.name, KDL::Vector(0.0, 0.0, 0.0), axis, KDL::Joint::TransAxis); + } + throw std::runtime_error("Unsupported KDL joint type: " + joint.type); +} + +void build_kdl_chain(RobotRecord& record) { + record.chain = KDL::Chain(); + 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 || + joint.rpy[0] != 0.0 || joint.rpy[1] != 0.0 || joint.rpy[2] != 0.0) { + 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.fk_solver = std::make_unique(record.chain); + record.jac_solver = std::make_unique(record.chain); +} + +KDL::JntArray jnt_array_from_input(const RobotRecord& record, const double* joints, int n) { + if (joints == nullptr) { + throw std::runtime_error("Joint input is required"); + } + if (n != static_cast(record.active_joint_names.size())) { + throw std::runtime_error("Joint vector dimension does not match robot DOF"); + } + KDL::JntArray q(static_cast(n)); + for (int index = 0; index < n; index += 1) { + q(static_cast(index)) = joints[index]; + } + return q; +} + +void write_pose7(const KDL::Frame& frame, double* out_pose7) { + if (out_pose7 == nullptr) { + throw std::runtime_error("Output pose buffer is required"); + } + out_pose7[0] = frame.p.x(); + out_pose7[1] = frame.p.y(); + out_pose7[2] = frame.p.z(); + frame.M.GetQuaternion(out_pose7[3], out_pose7[4], out_pose7[5], out_pose7[6]); +} +#endif + +RobotRecord parse_robot_record(const char* model_json) { + if (model_json == nullptr) { + throw std::runtime_error("Robot model JSON is required"); + } + + RobotRecord record; + record.model_json = model_json; + const JsonValue model = JsonParser(record.model_json).parse(); + if (model.kind != JsonValue::Kind::Object) { + throw std::runtime_error("Robot model must be a JSON object"); + } + + record.robot_id = string_property(model, "robotId"); + record.name = string_property(model, "name"); + record.base_link = string_property(model, "baseLink"); + record.tip_link = string_property(model, "tipLink"); + record.active_joint_names = string_array_property(model, "activeJointNames"); + record.joints = parse_joints(model, record.active_joint_names); + + return record; +} + +std::map::iterator find_robot(int robot_handle) { + const auto it = robots.find(robot_handle); + if (it == robots.end()) { + set_last_error("KDL_INVALID_HANDLE", "RobotHandle does not exist"); + } + return it; +} + +} // namespace + +extern "C" { + +EMSCRIPTEN_KEEPALIVE +int kdl_init(const char* options_json) { + (void)options_json; + robots.clear(); + next_robot_handle = 1; + clear_last_error(); + return 0; +} + +EMSCRIPTEN_KEEPALIVE +int kdl_create_robot(const char* model_json) { + try { + RobotRecord record = parse_robot_record(model_json); + const int handle = next_robot_handle++; + auto [it, inserted] = robots.emplace(handle, std::move(record)); + (void)inserted; +#ifdef KDL_WASM_HAS_OROCOS_KDL + build_kdl_chain(it->second); +#endif + clear_last_error(); + return handle; + } catch (const std::exception& error) { + set_last_error("KDL_INVALID_MODEL", error.what()); + return -1; + } +} + +EMSCRIPTEN_KEEPALIVE +int kdl_destroy_robot(int robot_handle) { + if (robots.erase(robot_handle) == 0) { + set_last_error("KDL_INVALID_HANDLE", "RobotHandle does not exist"); + return -1; + } + clear_last_error(); + return 0; +} + +EMSCRIPTEN_KEEPALIVE +int kdl_get_robot_info(int robot_handle, char* out_json, int out_len) { + const auto it = find_robot(robot_handle); + if (it == robots.end()) { + return -1; + } + const RobotRecord& record = it->second; + + std::ostringstream json; + json << "{\"handle\":" << robot_handle << ",\"robotId\":\"" << json_escape(record.robot_id) + << "\",\"name\":\"" << json_escape(record.name) << "\",\"baseLink\":\"" + << json_escape(record.base_link) << "\",\"tipLink\":\"" << json_escape(record.tip_link) + << "\",\"dof\":" << record.active_joint_names.size() << ",\"jointNames\":["; + for (std::size_t index = 0; index < record.active_joint_names.size(); index += 1) { + if (index > 0) { + json << ","; + } + json << "\"" << json_escape(record.active_joint_names[index]) << "\""; + } + json << "],\"limits\":[],\"nativeState\":\"" +#ifdef KDL_WASM_HAS_OROCOS_KDL + << "kdl_chain" +#else + << "model_cached" +#endif + << "\"}"; + const int result = write_json(json.str(), out_json, out_len); + if (result == 0) { + clear_last_error(); + } + return result; +} + +EMSCRIPTEN_KEEPALIVE +int kdl_fk(int robot_handle, const double* joints, int n, double* out_pose7) { +#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); + KDL::Frame frame; + const int result = record.fk_solver->JntToCart(q, frame); + if (result != 0) { + set_last_error("KDL_FK_FAILED", "Native KDL FK solver failed"); + return -1; + } + write_pose7(frame, out_pose7); + clear_last_error(); + return 0; + } 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_pose7; + return not_implemented("kdl_fk requires KDL_SOURCE_DIR"); +#endif +} + +EMSCRIPTEN_KEEPALIVE +int kdl_fk_all_links(int robot_handle, const double* joints, int n, char* out_json, + int out_len) { + (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"); +} + +EMSCRIPTEN_KEEPALIVE +int kdl_jacobian(int robot_handle, const double* joints, int n, double* out_matrix) { +#ifdef KDL_WASM_HAS_OROCOS_KDL + try { + const auto it = find_robot(robot_handle); + if (it == robots.end()) { + return -1; + } + RobotRecord& record = it->second; + if (out_matrix == nullptr) { + throw std::runtime_error("Output Jacobian buffer is required"); + } + const KDL::JntArray q = jnt_array_from_input(record, joints, n); + KDL::Jacobian jac(static_cast(n)); + const int result = record.jac_solver->JntToJac(q, jac); + if (result != 0) { + set_last_error("KDL_JACOBIAN_FAILED", "Native KDL Jacobian solver failed"); + return -1; + } + for (int col = 0; col < n; col += 1) { + for (int row = 0; row < 6; row += 1) { + out_matrix[row * n + col] = jac(static_cast(row), static_cast(col)); + } + } + clear_last_error(); + return 0; + } catch (const std::exception& error) { + set_last_error("KDL_JACOBIAN_FAILED", error.what()); + return -1; + } +#else + (void)robot_handle; + (void)joints; + (void)n; + (void)out_matrix; + return not_implemented("kdl_jacobian requires KDL_SOURCE_DIR"); +#endif +} + +EMSCRIPTEN_KEEPALIVE +int kdl_ik(int robot_handle, const double* seed, int n, const double* target_pose7, + const char* options_json, double* out_joints) { + (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"); +} + +EMSCRIPTEN_KEEPALIVE +int kdl_plan_movej(int robot_handle, const char* request_json, char* out_json, + int out_len) { + (void)robot_handle; + (void)request_json; + (void)out_json; + (void)out_len; + return not_implemented("kdl_plan_movej is pending KW-008 native binding"); +} + +EMSCRIPTEN_KEEPALIVE +int kdl_plan_movel(int robot_handle, const char* request_json, char* out_json, + int out_len) { + (void)robot_handle; + (void)request_json; + (void)out_json; + (void)out_len; + return not_implemented("kdl_plan_movel is pending KW-009 native binding"); +} + +EMSCRIPTEN_KEEPALIVE +int kdl_plan_movec(int robot_handle, const char* request_json, char* out_json, + int out_len) { + (void)robot_handle; + (void)request_json; + (void)out_json; + (void)out_len; + return not_implemented("kdl_plan_movec is pending KW-010 native binding"); +} + +EMSCRIPTEN_KEEPALIVE +int kdl_plan_path(int robot_handle, const char* request_json, char* out_json, + int out_len) { + (void)robot_handle; + (void)request_json; + (void)out_json; + (void)out_len; + return not_implemented("kdl_plan_path is pending KW-011 native binding"); +} + +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"); +} + +EMSCRIPTEN_KEEPALIVE +int kdl_last_error(char* out_json, int out_len) { + return write_json(last_error, out_json, out_len); +} + +} diff --git a/kdl-wasm/web/src/grl/ast/ast.ts b/kdl-wasm/web/src/grl/ast/ast.ts new file mode 100644 index 0000000..51aa25b --- /dev/null +++ b/kdl-wasm/web/src/grl/ast/ast.ts @@ -0,0 +1,239 @@ +import type { GrlSourceRange, GrlToken } from "../lexer/index.js"; + +export type GrlAstNodeKind = + | "Program" + | "LanguageDeclaration" + | "ModuleDeclaration" + | "ImportDeclaration" + | "DataDeclaration" + | "TargetDeclaration" + | "PathDeclaration" + | "PathDefaultsBlock" + | "PathSourceBlock" + | "PathPoint" + | "PathEvent" + | "OperationDeclaration" + | "OperationProcessBlock" + | "OperationActionBlock" + | "ProcedureDeclaration" + | "FunctionDeclaration" + | "RawTopLevelDeclaration" + | "IdentifierExpression" + | "NumberLiteral" + | "StringLiteral" + | "BooleanLiteral" + | "ArrayExpression" + | "CallExpression" + | "ObjectExpression" + | "OffsetExpression"; + +export interface GrlAstNode { + kind: GrlAstNodeKind; + range: GrlSourceRange; +} + +export interface GrlLanguageDeclaration extends GrlAstNode { + kind: "LanguageDeclaration"; + language: "grl"; + version: string; +} + +export interface GrlImportDeclaration extends GrlAstNode { + kind: "ImportDeclaration"; + moduleName: string; +} + +export type GrlDeclarationStorage = "persistent" | "const" | "var"; + +export interface GrlDataDeclaration extends GrlAstNode { + kind: "DataDeclaration"; + storage: GrlDeclarationStorage; + typeName: string; + name: string; + initializer: GrlExpression; +} + +export interface GrlTargetDeclaration extends GrlAstNode { + kind: "TargetDeclaration"; + name: string; + target: GrlExpression; +} + +export interface GrlPathProperty { + key: string; + value: GrlExpression; + range: GrlSourceRange; +} + +export interface GrlPathDefaultsBlock extends GrlAstNode { + kind: "PathDefaultsBlock"; + properties: GrlPathProperty[]; +} + +export interface GrlPathSourceBlock extends GrlAstNode { + kind: "PathSourceBlock"; + properties: GrlPathProperty[]; +} + +export interface GrlPathPoint extends GrlAstNode { + kind: "PathPoint"; + id: string; + motionTokens: GrlToken[]; +} + +export interface GrlPathEvent extends GrlAstNode { + kind: "PathEvent"; + timing: "before" | "after" | "at"; + pointId: string; + distance?: GrlNumberLiteral; + actionTokens: GrlToken[]; +} + +export type GrlPathItem = + | GrlPathDefaultsBlock + | GrlPathSourceBlock + | GrlPathPoint + | GrlPathEvent; + +export interface GrlPathDeclaration extends GrlAstNode { + kind: "PathDeclaration"; + name: string; + items: GrlPathItem[]; +} + +export interface GrlOperationProcessBlock extends GrlAstNode { + kind: "OperationProcessBlock"; + properties: GrlPathProperty[]; +} + +export interface GrlOperationActionBlock extends GrlAstNode { + kind: "OperationActionBlock"; + actionKind: "start_action" | "end_action"; + actionTokens: GrlToken[]; +} + +export type GrlOperationItem = GrlOperationProcessBlock | GrlOperationActionBlock; + +export interface GrlOperationDeclaration extends GrlAstNode { + kind: "OperationDeclaration"; + name: string; + operationKind: string; + pathName: string; + items: GrlOperationItem[]; +} + +export interface GrlProcedureDeclaration extends GrlAstNode { + kind: "ProcedureDeclaration"; + name: string; + params: GrlToken[]; + bodyTokens: GrlToken[]; +} + +export interface GrlFunctionDeclaration extends GrlAstNode { + kind: "FunctionDeclaration"; + returnType: string; + name: string; + params: GrlToken[]; + bodyTokens: GrlToken[]; +} + +export interface GrlRawTopLevelDeclaration extends GrlAstNode { + kind: "RawTopLevelDeclaration"; + declarationType: string; + tokens: GrlToken[]; +} + +export type GrlTopLevelDeclaration = + | GrlImportDeclaration + | GrlDataDeclaration + | GrlTargetDeclaration + | GrlPathDeclaration + | GrlOperationDeclaration + | GrlProcedureDeclaration + | GrlFunctionDeclaration + | GrlRawTopLevelDeclaration; + +export interface GrlModuleDeclaration extends GrlAstNode { + kind: "ModuleDeclaration"; + name: string; + declarations: GrlTopLevelDeclaration[]; +} + +export interface GrlProgram extends GrlAstNode { + kind: "Program"; + language?: GrlLanguageDeclaration; + module: GrlModuleDeclaration; +} + +export interface GrlIdentifierExpression extends GrlAstNode { + kind: "IdentifierExpression"; + name: string; +} + +export interface GrlNumberLiteral extends GrlAstNode { + kind: "NumberLiteral"; + value: number; + raw: string; + unit?: { + raw: string; + kind: string; + siUnit: string; + normalizedValue: number; + }; +} + +export interface GrlStringLiteral extends GrlAstNode { + kind: "StringLiteral"; + value: string; +} + +export interface GrlBooleanLiteral extends GrlAstNode { + kind: "BooleanLiteral"; + value: boolean; +} + +export interface GrlArrayExpression extends GrlAstNode { + kind: "ArrayExpression"; + elements: GrlExpression[]; +} + +export interface GrlCallExpression extends GrlAstNode { + kind: "CallExpression"; + callee: string; + args: GrlExpression[]; +} + +export interface GrlObjectProperty { + key: string; + value: GrlExpression; + range: GrlSourceRange; +} + +export interface GrlObjectExpression extends GrlAstNode { + kind: "ObjectExpression"; + typeName: string; + properties: GrlObjectProperty[]; +} + +export interface GrlOffsetAxis { + axis: "x" | "y" | "z"; + value: GrlNumberLiteral; +} + +export interface GrlOffsetExpression extends GrlAstNode { + kind: "OffsetExpression"; + base: GrlExpression; + mode: "frame" | "tool"; + frameName?: string; + axes: GrlOffsetAxis[]; +} + +export type GrlExpression = + | GrlIdentifierExpression + | GrlNumberLiteral + | GrlStringLiteral + | GrlBooleanLiteral + | GrlArrayExpression + | GrlCallExpression + | GrlObjectExpression + | GrlOffsetExpression; diff --git a/kdl-wasm/web/src/grl/ast/index.ts b/kdl-wasm/web/src/grl/ast/index.ts new file mode 100644 index 0000000..1fb11b2 --- /dev/null +++ b/kdl-wasm/web/src/grl/ast/index.ts @@ -0,0 +1,37 @@ +export type { + GrlAstNode, + GrlAstNodeKind, + GrlArrayExpression, + GrlBooleanLiteral, + GrlCallExpression, + GrlDataDeclaration, + GrlDeclarationStorage, + GrlExpression, + GrlFunctionDeclaration, + GrlIdentifierExpression, + GrlImportDeclaration, + GrlLanguageDeclaration, + GrlModuleDeclaration, + GrlNumberLiteral, + GrlObjectExpression, + GrlObjectProperty, + GrlOffsetAxis, + GrlOffsetExpression, + GrlOperationActionBlock, + GrlOperationDeclaration, + GrlOperationItem, + GrlOperationProcessBlock, + GrlPathDeclaration, + GrlPathDefaultsBlock, + GrlPathEvent, + GrlPathItem, + GrlPathPoint, + GrlPathProperty, + GrlPathSourceBlock, + GrlProcedureDeclaration, + GrlProgram, + GrlRawTopLevelDeclaration, + GrlStringLiteral, + GrlTargetDeclaration, + GrlTopLevelDeclaration +} from "./ast.js"; diff --git a/kdl-wasm/web/src/grl/generator/generator.ts b/kdl-wasm/web/src/grl/generator/generator.ts new file mode 100644 index 0000000..62b3361 --- /dev/null +++ b/kdl-wasm/web/src/grl/generator/generator.ts @@ -0,0 +1,210 @@ +export type GrlGeneratorStyle = "expanded" | "compact"; + +export interface GeneratedTargetSpec { + name: string; + kind: "joint" | "pose"; + values: number[]; +} + +export interface GeneratedPathPointSpec { + id?: string; + motion: "movej" | "movel" | "movec"; + target: string; + via?: string; + speed?: string; + zone?: string; +} + +export interface GeneratedPathSpec { + name: string; + source?: Record; + defaults: { + speed: string; + zone: string; + }; + points: GeneratedPathPointSpec[]; +} + +export interface GeneratedOperationSpec { + name: string; + kind: string; + path: string; + startAction?: string; + endAction?: string; +} + +export interface GrlProgramGenerationSpec { + moduleName: string; + speeds?: Record; + zones?: Record; + targets: GeneratedTargetSpec[]; + path: GeneratedPathSpec; + operation: GeneratedOperationSpec; +} + +export interface GeneratedGrlProgram { + text: string; + stableIds: { + targets: string[]; + points: string[]; + path: string; + operation: string; + }; +} + +export function generateGrlProgram(spec: GrlProgramGenerationSpec, style: GrlGeneratorStyle = "expanded"): GeneratedGrlProgram { + const normalized = normalizeSpec(spec); + const text = style === "compact" ? renderCompact(normalized) : renderExpanded(normalized); + return { + text, + stableIds: { + targets: normalized.targets.map((target) => target.name), + points: normalized.path.points.map((point) => point.id!), + path: normalized.path.name, + operation: normalized.operation.name + } + }; +} + +function normalizeSpec(spec: GrlProgramGenerationSpec): GrlProgramGenerationSpec { + return { + ...spec, + speeds: sortRecord(spec.speeds ?? {}), + zones: sortRecord(spec.zones ?? {}), + targets: [...spec.targets].sort((left, right) => left.name.localeCompare(right.name)), + path: { + ...spec.path, + ...(spec.path.source ? { source: sortRecord(spec.path.source) } : {}), + points: spec.path.points.map((point, index) => ({ + ...point, + id: point.id ?? `p${String(index).padStart(2, "0")}` + })) + } + }; +} + +function renderExpanded(spec: GrlProgramGenerationSpec): string { + const lines: string[] = [`language grl 0.1`, `module ${spec.moduleName}`]; + for (const [name, expression] of Object.entries(spec.speeds ?? {})) { + lines.push(` const speed ${name} = ${expression}`); + } + for (const [name, expression] of Object.entries(spec.zones ?? {})) { + lines.push(` const zone ${name} = ${expression}`); + } + for (const target of spec.targets) { + lines.push(...renderTargetExpanded(target)); + } + lines.push(` path ${spec.path.name} {`); + if (spec.path.source && Object.keys(spec.path.source).length > 0) { + lines.push(` source {`); + for (const [key, value] of Object.entries(spec.path.source)) { + lines.push(` ${key}: ${formatSourceLiteral(key, value)}`); + } + lines.push(` }`); + } + lines.push(` defaults {`); + lines.push(` speed: ${spec.path.defaults.speed}`); + lines.push(` zone: ${spec.path.defaults.zone}`); + lines.push(` }`); + for (const point of spec.path.points) { + lines.push(` ${renderPoint(point)}`); + } + lines.push(` }`); + lines.push(...renderOperationExpanded(spec.operation)); + lines.push(` proc main()`); + lines.push(` run_operation ${spec.operation.name}`); + lines.push(` end`); + lines.push(`end`); + return lines.join("\n"); +} + +function renderCompact(spec: GrlProgramGenerationSpec): string { + const lines: string[] = [`language grl 0.1`, `module ${spec.moduleName}`]; + for (const [name, expression] of Object.entries(spec.speeds ?? {})) { + lines.push(` const speed ${name} = ${expression}`); + } + for (const [name, expression] of Object.entries(spec.zones ?? {})) { + lines.push(` const zone ${name} = ${expression}`); + } + for (const target of spec.targets) { + lines.push(` ${renderTargetCompact(target)}`); + } + const source = spec.path.source && Object.keys(spec.path.source).length > 0 + ? ` source { ${Object.entries(spec.path.source).map(([key, value]) => `${key}: ${formatSourceLiteral(key, value)}`).join(" ")} }` + : ""; + lines.push(` path ${spec.path.name} {${source} defaults { speed: ${spec.path.defaults.speed} zone: ${spec.path.defaults.zone} } ${spec.path.points.map(renderPoint).join(" ")} }`); + lines.push(` operation ${spec.operation.name} { kind: ${spec.operation.kind} path: ${spec.operation.path}${spec.operation.startAction ? ` start_action: ${spec.operation.startAction}` : ""}${spec.operation.endAction ? ` end_action: ${spec.operation.endAction}` : ""} }`); + lines.push(` proc main()`); + lines.push(` run_operation ${spec.operation.name}`); + lines.push(` end`); + lines.push(`end`); + return lines.join("\n"); +} + +function renderTargetExpanded(target: GeneratedTargetSpec): string[] { + if (target.kind === "joint") { + return [ + ` target ${target.name} = joint_target {`, + ` joints: [${target.values.map((value) => `${value} deg`).join(", ")}]`, + ` }` + ]; + } + return [ + ` target ${target.name} = pose_target {`, + ` pose: pose(${target.values.map((value, index) => `${value} ${index < 3 ? "mm" : "deg"}`).join(", ")})`, + ` }` + ]; +} + +function renderTargetCompact(target: GeneratedTargetSpec): string { + if (target.kind === "joint") { + return `target ${target.name} = joint_target { joints: [${target.values.map((value) => `${value} deg`).join(", ")}] }`; + } + return `target ${target.name} = pose_target { pose: pose(${target.values.map((value, index) => `${value} ${index < 3 ? "mm" : "deg"}`).join(", ")}) }`; +} + +function renderPoint(point: GeneratedPathPointSpec): string { + const params = [ + `point ${point.id} ${point.motion}`, + point.motion === "movec" ? `via ${point.via}` : undefined, + point.motion === "movec" ? `target ${point.target}` : point.target, + point.speed ? `speed ${point.speed}` : undefined, + point.zone ? `zone ${point.zone}` : undefined + ].filter(Boolean); + return params.join(" "); +} + +function renderOperationExpanded(operation: GeneratedOperationSpec): string[] { + const lines = [` operation ${operation.name} {`, ` kind: ${operation.kind}`, ` path: ${operation.path}`]; + if (operation.startAction) { + lines.push(` start_action:`); + lines.push(` ${operation.startAction}`); + } + if (operation.endAction) { + lines.push(` end_action:`); + lines.push(` ${operation.endAction}`); + } + lines.push(` }`); + return lines; +} + +function sortRecord(record: Record): Record { + return Object.fromEntries(Object.entries(record).sort(([left], [right]) => left.localeCompare(right))); +} + +function formatLiteral(value: string | number | boolean): string { + if (typeof value === "string") { + return /^[A-Za-z_][A-Za-z0-9_]*$/.test(value) ? value : JSON.stringify(value); + } + return String(value); +} + +function formatSourceLiteral(key: string, value: string | number | boolean): string { + if (typeof value !== "string") { + return String(value); + } + if (key === "type" && /^[A-Za-z_][A-Za-z0-9_]*$/.test(value)) { + return value; + } + return JSON.stringify(value); +} diff --git a/kdl-wasm/web/src/grl/generator/index.ts b/kdl-wasm/web/src/grl/generator/index.ts new file mode 100644 index 0000000..577f3f7 --- /dev/null +++ b/kdl-wasm/web/src/grl/generator/index.ts @@ -0,0 +1,10 @@ +export { + generateGrlProgram, + type GeneratedGrlProgram, + type GeneratedOperationSpec, + type GeneratedPathPointSpec, + type GeneratedPathSpec, + type GeneratedTargetSpec, + type GrlGeneratorStyle, + type GrlProgramGenerationSpec +} from "./generator.js"; diff --git a/kdl-wasm/web/src/grl/ir/index.ts b/kdl-wasm/web/src/grl/ir/index.ts new file mode 100644 index 0000000..13c4bcf --- /dev/null +++ b/kdl-wasm/web/src/grl/ir/index.ts @@ -0,0 +1,61 @@ +export type { + CompiledMotionRequest, + CompiledOperation, + CompiledPath, + AlarmInstruction, + BreakInstruction, + CallInstruction, + CatchInstruction, + ContinueInstruction, + ControlExpression, + ControlFlowInstruction, + ExceptionFlowInstruction, + ExceptionInstruction, + ExecutableBranch, + ExecutableControlInstruction, + ExecutableForInstruction, + ExecutableIfInstruction, + ExecutableInstruction, + ExecutableProcedure, + ExecutableSwitchCase, + ExecutableSwitchInstruction, + ExecutableWhileInstruction, + FinallyInstruction, + ForInstruction, + FunctionSignature, + IfBranch, + IfInstruction, + IoDomain, + IoFlowInstruction, + IoReference, + IoWriteInstruction, + JumpInstruction, + KdlBridgeRequests, + LabelInstruction, + MotionInstruction, + MotionKind, + OperationActionInstruction, + OperationExecutionStep, + PathEventInstruction, + ProcedureFlowInstruction, + ProcedureSignature, + ProcFunctionAnalysis, + PulseInstruction, + RaiseInstruction, + RawProcedureStatement, + ReturnInstruction, + RoutineParameter, + RoutineParameterDirection, + SemanticProgramIr, + SemanticSourceMapEntry, + SemanticSymbol, + SemanticSymbolKind, + TryInstruction, + UnsupportedRuntimeInstruction, + RunOperationInstruction, + RunPathInstruction, + SwitchCaseInstruction, + SwitchInstruction, + WhileInstruction, + WaitInstruction +} from "./motion.js"; diff --git a/kdl-wasm/web/src/grl/ir/motion.ts b/kdl-wasm/web/src/grl/ir/motion.ts new file mode 100644 index 0000000..868b89f --- /dev/null +++ b/kdl-wasm/web/src/grl/ir/motion.ts @@ -0,0 +1,428 @@ +import type { + JointTarget, + MoveCRequest, + MoveJRequest, + MoveLRequest, + MotionDiagnostic, + MotionSourceMap, + PathEventRequest, + PathPlanRequest, + Pose, + PoseTarget, + SpeedSpec, + ZoneSpec +} from "../../kdl/types.js"; + +export type MotionKind = "MOVEJ" | "MOVEL" | "MOVEC"; + +export interface MotionInstruction { + id?: string; + kind: MotionKind; + target?: JointTarget | PoseTarget; + via?: PoseTarget; + speed: SpeedSpec; + zone: ZoneSpec; + tool?: Pose; + frame?: Pose; + sourceMap?: MotionSourceMap; + pathId?: string; + pointId?: string; + source?: Record; +} + +export type CompiledMotionRequest = MoveJRequest | MoveLRequest | MoveCRequest; + +export interface PathEventInstruction { + id?: string; + timing: PathEventRequest["timing"]; + pointId: string; + distance?: number; + kind: string; + sourceMap?: MotionSourceMap; + data?: Record; +} + +export interface CompiledPath { + pathId: string; + request: PathPlanRequest; + motions: MotionInstruction[]; + events: PathEventInstruction[]; +} + +export interface RunPathInstruction { + kind: "RUN_PATH"; + pathId: string; + sourceMap?: MotionSourceMap; +} + +export interface OperationActionInstruction { + kind: "ACTION"; + actionKind: "start_action" | "end_action"; + operationId: string; + statement: string; + tokens?: unknown[]; + sourceMap?: MotionSourceMap; +} + +export interface CompiledOperation { + operationId: string; + kind: string; + pathId: string; + process: Record; + startActions: OperationActionInstruction[]; + endActions: OperationActionInstruction[]; +} + +export interface RunOperationInstruction { + kind: "RUN_OPERATION"; + operationId: string; + sourceMap?: MotionSourceMap; +} + +export type OperationExecutionStep = + | OperationActionInstruction + | RunPathInstruction; + +export type IoDomain = "di" | "do" | "ai" | "ao" | "gi" | "go" | "ri" | "ro" | "alias"; + +export interface IoReference { + domain: IoDomain; + index?: number; + alias?: string; + raw: string; +} + +export interface IoWriteInstruction { + kind: "IO_WRITE"; + target: IoReference; + value: boolean | number | string; + sourceMap?: MotionSourceMap; +} + +export interface WaitInstruction { + kind: "WAIT"; + condition: string; + timeout?: number; + onTimeout?: { + kind: "alarm" | "call"; + value: string; + }; + sourceMap?: MotionSourceMap; +} + +export interface PulseInstruction { + kind: "PULSE"; + target: IoReference; + duration: number; + trace: Array<{ + time: number; + action: "set" | "reset"; + target: IoReference; + value: boolean; + }>; + sourceMap?: MotionSourceMap; +} + +export type IoFlowInstruction = IoWriteInstruction | WaitInstruction | PulseInstruction; + +export interface ControlExpression { + text: string; + tokens?: unknown[]; + sourceMap?: MotionSourceMap; +} + +export interface RawProcedureStatement { + kind: "RAW_STATEMENT"; + text: string; + tokens?: unknown[]; + sourceMap?: MotionSourceMap; +} + +export interface IfBranch { + branchKind: "if" | "elseif" | "else"; + condition?: ControlExpression; + body: ProcedureFlowInstruction[]; + sourceMap?: MotionSourceMap; +} + +export interface IfInstruction { + kind: "IF"; + branches: IfBranch[]; + sourceMap?: MotionSourceMap; +} + +export interface WhileInstruction { + kind: "WHILE"; + condition: ControlExpression; + body: ProcedureFlowInstruction[]; + sourceMap?: MotionSourceMap; +} + +export interface ForInstruction { + kind: "FOR"; + iterator: string; + from: ControlExpression; + to: ControlExpression; + step?: ControlExpression; + body: ProcedureFlowInstruction[]; + sourceMap?: MotionSourceMap; +} + +export interface SwitchCaseInstruction { + caseKind: "case" | "default"; + value?: string | number | boolean; + raw?: string; + body: ProcedureFlowInstruction[]; + sourceMap?: MotionSourceMap; +} + +export interface SwitchInstruction { + kind: "SWITCH"; + expression: ControlExpression; + cases: SwitchCaseInstruction[]; + sourceMap?: MotionSourceMap; +} + +export interface BreakInstruction { + kind: "BREAK"; + sourceMap?: MotionSourceMap; +} + +export interface ContinueInstruction { + kind: "CONTINUE"; + sourceMap?: MotionSourceMap; +} + +export interface LabelInstruction { + kind: "LABEL"; + name: string; + scopePath: string[]; + sourceMap?: MotionSourceMap; +} + +export interface JumpInstruction { + kind: "JUMP"; + label: string; + scopePath: string[]; + sourceMap?: MotionSourceMap; +} + +export type ControlFlowInstruction = + | IfInstruction + | WhileInstruction + | ForInstruction + | SwitchInstruction + | BreakInstruction + | ContinueInstruction + | LabelInstruction + | JumpInstruction; + +export type ProcedureFlowInstruction = ControlFlowInstruction | RawProcedureStatement; + +export type RoutineParameterDirection = "in" | "out" | "inout"; + +export interface RoutineParameter { + name: string; + typeName: string; + direction: RoutineParameterDirection; + sourceMap?: MotionSourceMap; +} + +export interface ProcedureSignature { + kind: "PROC_SIGNATURE"; + name: string; + parameters: RoutineParameter[]; + sourceMap?: MotionSourceMap; +} + +export interface FunctionSignature { + kind: "FUNC_SIGNATURE"; + name: string; + returnType: string; + parameters: RoutineParameter[]; + sourceMap?: MotionSourceMap; +} + +export interface CallInstruction { + kind: "CALL"; + target: string; + args: ControlExpression[]; + sourceMap?: MotionSourceMap; +} + +export interface ReturnInstruction { + kind: "RETURN"; + value?: ControlExpression; + sourceMap?: MotionSourceMap; +} + +export interface ProcFunctionAnalysis { + procedures: ProcedureSignature[]; + functions: FunctionSignature[]; + calls: CallInstruction[]; + returns: ReturnInstruction[]; + diagnostics: MotionDiagnostic[]; +} + +export interface AlarmInstruction { + kind: "ALARM"; + alarmId: string; + message?: string; + severity?: string; + sourceMap?: MotionSourceMap; +} + +export interface RaiseInstruction { + kind: "RAISE"; + alarmId: string; + sourceMap?: MotionSourceMap; +} + +export interface CatchInstruction { + alarmId?: string; + body: ExceptionFlowInstruction[]; + sourceMap?: MotionSourceMap; +} + +export interface FinallyInstruction { + body: ExceptionFlowInstruction[]; + sourceMap?: MotionSourceMap; +} + +export interface TryInstruction { + kind: "TRY"; + body: ExceptionFlowInstruction[]; + catches: CatchInstruction[]; + finally?: FinallyInstruction; + sourceMap?: MotionSourceMap; +} + +export interface UnsupportedRuntimeInstruction { + kind: "UNSUPPORTED_RUNTIME"; + feature: "trap" | "interrupt" | "task"; + message: string; + sourceMap?: MotionSourceMap; +} + +export type ExceptionInstruction = + | AlarmInstruction + | RaiseInstruction + | TryInstruction + | UnsupportedRuntimeInstruction; + +export type ExceptionFlowInstruction = ExceptionInstruction | RawProcedureStatement; + +export type SemanticSymbolKind = + | "data" + | "target" + | "path" + | "operation" + | "procedure" + | "function" + | "raw"; + +export interface SemanticSymbol { + kind: SemanticSymbolKind; + name: string; + typeName?: string; + sourceMap?: MotionSourceMap; +} + +export interface SemanticSourceMapEntry { + kind: string; + id: string; + sourceMap: MotionSourceMap; + pathId?: string; + pointId?: string; + operationId?: string; + procedureId?: string; +} + +export interface ExecutableBranch { + branchKind: "if" | "elseif" | "else"; + condition?: ControlExpression; + body: ExecutableInstruction[]; + sourceMap?: MotionSourceMap; +} + +export interface ExecutableIfInstruction { + kind: "EXEC_IF"; + branches: ExecutableBranch[]; + sourceMap?: MotionSourceMap; +} + +export interface ExecutableWhileInstruction { + kind: "EXEC_WHILE"; + condition: ControlExpression; + body: ExecutableInstruction[]; + sourceMap?: MotionSourceMap; +} + +export interface ExecutableForInstruction { + kind: "EXEC_FOR"; + iterator: string; + from: ControlExpression; + to: ControlExpression; + step?: ControlExpression; + body: ExecutableInstruction[]; + sourceMap?: MotionSourceMap; +} + +export interface ExecutableSwitchCase { + caseKind: "case" | "default"; + value?: string | number | boolean; + raw?: string; + body: ExecutableInstruction[]; + sourceMap?: MotionSourceMap; +} + +export interface ExecutableSwitchInstruction { + kind: "EXEC_SWITCH"; + expression: ControlExpression; + cases: ExecutableSwitchCase[]; + sourceMap?: MotionSourceMap; +} + +export type ExecutableControlInstruction = + | ExecutableIfInstruction + | ExecutableWhileInstruction + | ExecutableForInstruction + | ExecutableSwitchInstruction; + +export type ExecutableInstruction = + | MotionInstruction + | IoFlowInstruction + | RunPathInstruction + | RunOperationInstruction + | CallInstruction + | ReturnInstruction + | BreakInstruction + | ContinueInstruction + | AlarmInstruction + | RaiseInstruction + | UnsupportedRuntimeInstruction + | ExecutableControlInstruction + | RawProcedureStatement; + +export interface ExecutableProcedure { + name: string; + instructions: ExecutableInstruction[]; + sourceMap?: MotionSourceMap; +} + +export interface KdlBridgeRequests { + motionRequests: CompiledMotionRequest[]; + pathRequests: PathPlanRequest[]; +} + +export interface SemanticProgramIr { + moduleName: string; + symbols: SemanticSymbol[]; + semanticChecks: string[]; + procedures: ExecutableProcedure[]; + paths: CompiledPath[]; + operations: CompiledOperation[]; + diagnostics: MotionDiagnostic[]; + sourceMap: SemanticSourceMapEntry[]; + kdlBridge: KdlBridgeRequests; +} diff --git a/kdl-wasm/web/src/grl/lexer/index.ts b/kdl-wasm/web/src/grl/lexer/index.ts new file mode 100644 index 0000000..a269527 --- /dev/null +++ b/kdl-wasm/web/src/grl/lexer/index.ts @@ -0,0 +1,15 @@ +export { GRL_KEYWORDS, isGrlKeyword, type GrlKeyword } from "./keywords.js"; +export { lexGrl, type GrlLexerOptions } from "./lexer.js"; +export { isGrlUnitLiteral, normalizeUnitLiteral, normalizeUnitValue, type UnitKind } from "./units.js"; +export type { + GrlCommentToken, + GrlEofToken, + GrlIdentifierToken, + GrlKeywordToken, + GrlNumberToken, + GrlSourcePosition, + GrlSourceRange, + GrlStringToken, + GrlToken, + GrlTokenKind +} from "./tokens.js"; diff --git a/kdl-wasm/web/src/grl/lexer/keywords.ts b/kdl-wasm/web/src/grl/lexer/keywords.ts new file mode 100644 index 0000000..1bfd4b8 --- /dev/null +++ b/kdl-wasm/web/src/grl/lexer/keywords.ts @@ -0,0 +1,95 @@ +export const GRL_KEYWORDS = [ + "language", + "module", + "import", + "end", + "persistent", + "const", + "var", + "robot", + "tool", + "frame", + "load", + "target", + "speed", + "zone", + "path", + "operation", + "process", + "proc", + "func", + "return", + "call", + "if", + "elseif", + "else", + "switch", + "case", + "default", + "while", + "for", + "to", + "step", + "break", + "continue", + "label", + "jump", + "movej", + "movel", + "movec", + "run_path", + "run_operation", + "set_tool", + "set_frame", + "set_speed", + "set_zone", + "wait", + "pulse", + "timer", + "io", + "true", + "false", + "all", + "any", + "rising", + "falling", + "changed", + "trap", + "interrupt", + "enable", + "disable", + "raise", + "alarm", + "try", + "catch", + "finally", + "task", + "sync", + "post_hint", + "source", + "defaults", + "point", + "event", + "before", + "after", + "at", + "joint_target", + "pose_target", + "pose", + "poseq", + "joints", + "robot_config", + "ext_axis", + "fine", + "continuous", + "cnt", + "z" +] as const; + +export type GrlKeyword = (typeof GRL_KEYWORDS)[number]; + +const KEYWORD_SET = new Set(GRL_KEYWORDS); + +export function isGrlKeyword(value: string): value is GrlKeyword { + return KEYWORD_SET.has(value); +} diff --git a/kdl-wasm/web/src/grl/lexer/lexer.ts b/kdl-wasm/web/src/grl/lexer/lexer.ts new file mode 100644 index 0000000..f119b10 --- /dev/null +++ b/kdl-wasm/web/src/grl/lexer/lexer.ts @@ -0,0 +1,429 @@ +import { isGrlKeyword } from "./keywords.js"; +import { isGrlUnitLiteral, normalizeUnitLiteral } from "./units.js"; +import type { + GrlCommentToken, + GrlEofToken, + GrlIdentifierToken, + GrlKeywordToken, + GrlNumberToken, + GrlOperatorToken, + GrlPunctuationToken, + GrlSourcePosition, + GrlToken +} from "./tokens.js"; + +export interface GrlLexerOptions { + preserveComments?: boolean; +} + +interface ScannerState { + index: number; + line: number; + column: number; +} + +const TWO_CHAR_OPERATORS = new Set(["==", "!=", "<=", ">=", "&&", "||", "->", ":="]); +const PUNCTUATION = new Set(["(", ")", "{", "}", "[", "]", ",", ":", ";", "."]); +const OPERATORS = new Set(["+", "-", "*", "/", "=", "<", ">", "!"]); + +export function lexGrl(source: string, options: GrlLexerOptions = {}): GrlToken[] { + const scanner = new GrlScanner(source, options); + return scanner.scanTokens(); +} + +class GrlScanner { + private index = 0; + private line = 1; + private column = 1; + private readonly tokens: GrlToken[] = []; + private readonly preserveComments: boolean; + + constructor( + private readonly source: string, + options: GrlLexerOptions + ) { + this.preserveComments = options.preserveComments ?? true; + } + + scanTokens(): GrlToken[] { + while (!this.isAtEnd()) { + const char = this.peek(); + + if (this.isWhitespace(char)) { + this.advance(); + continue; + } + + if (char === "/" && this.peek(1) === "/") { + this.scanLineComment(); + continue; + } + + if (char === "/" && this.peek(1) === "*") { + this.scanBlockComment(); + continue; + } + + if (char === "\"") { + this.scanString(); + continue; + } + + if (this.isIdentifierStart(char)) { + this.scanIdentifierOrKeyword(); + continue; + } + + if (this.isNumberStart(char)) { + this.scanNumber(); + continue; + } + + this.scanPunctuationOrOperator(); + } + + const start = this.position(); + const token: GrlEofToken = { + kind: "eof", + raw: "", + value: "", + range: { start, end: start } + }; + this.tokens.push(token); + return this.tokens; + } + + private scanLineComment(): void { + const start = this.position(); + this.advance(); + this.advance(); + + const contentStart = this.index; + while (!this.isAtEnd() && this.peek() !== "\n") { + this.advance(); + } + + if (this.preserveComments) { + const value = this.source.slice(contentStart, this.index); + const token: GrlCommentToken = { + kind: "comment", + style: "line", + raw: this.source.slice(start.offset, this.index), + value, + range: { start, end: this.position() } + }; + this.tokens.push(token); + } + } + + private scanBlockComment(): void { + const start = this.position(); + this.advance(); + this.advance(); + + const contentStart = this.index; + while (!this.isAtEnd()) { + if (this.peek() === "*" && this.peek(1) === "/") { + const value = this.source.slice(contentStart, this.index); + this.advance(); + this.advance(); + + if (this.preserveComments) { + const token: GrlCommentToken = { + kind: "comment", + style: "block", + raw: this.source.slice(start.offset, this.index), + value, + range: { start, end: this.position() } + }; + this.tokens.push(token); + } + return; + } + + this.advance(); + } + + throw this.error(start, "Unterminated block comment"); + } + + private scanString(): void { + const start = this.position(); + this.advance(); + let value = ""; + + while (!this.isAtEnd()) { + const char = this.peek(); + if (char === "\"") { + this.advance(); + const token: GrlToken = { + kind: "string", + raw: this.source.slice(start.offset, this.index), + value, + range: { start, end: this.position() } + }; + this.tokens.push(token); + return; + } + + if (char === "\\") { + this.advance(); + value += this.readEscapedCharacter(start); + continue; + } + + value += this.advance(); + } + + throw this.error(start, "Unterminated string literal"); + } + + private readEscapedCharacter(start: GrlSourcePosition): string { + if (this.isAtEnd()) { + throw this.error(start, "Unterminated string escape"); + } + + const escaped = this.advance(); + switch (escaped) { + case "n": + return "\n"; + case "r": + return "\r"; + case "t": + return "\t"; + case "\\": + case "\"": + return escaped; + default: + return escaped; + } + } + + private scanIdentifierOrKeyword(): void { + const start = this.position(); + + while (!this.isAtEnd() && this.isIdentifierPart(this.peek())) { + this.advance(); + } + + const raw = this.source.slice(start.offset, this.index); + if (isGrlKeyword(raw)) { + const token: GrlKeywordToken = { + kind: "keyword", + raw, + value: raw, + range: { start, end: this.position() } + }; + this.tokens.push(token); + return; + } + + const token: GrlIdentifierToken = { + kind: "identifier", + raw, + value: raw, + range: { start, end: this.position() } + }; + this.tokens.push(token); + } + + private scanNumber(): void { + const start = this.position(); + + if (this.peek() === ".") { + this.advance(); + } + + while (!this.isAtEnd() && this.isDigit(this.peek())) { + this.advance(); + } + + if (this.peek() === "." && this.isDigit(this.peek(1))) { + this.advance(); + while (!this.isAtEnd() && this.isDigit(this.peek())) { + this.advance(); + } + } + + if ((this.peek() === "e" || this.peek() === "E") && this.isExponentStart()) { + this.advance(); + if (this.peek() === "+" || this.peek() === "-") { + this.advance(); + } + while (!this.isAtEnd() && this.isDigit(this.peek())) { + this.advance(); + } + } + + const numericEnd = this.position(); + const numericRaw = this.source.slice(start.offset, numericEnd.offset); + const value = Number(numericRaw); + if (!Number.isFinite(value)) { + throw this.error(start, `Invalid number literal: ${numericRaw}`); + } + + const unit = this.tryScanUnitAfterNumber(); + const token: GrlNumberToken = { + kind: "number", + raw: this.source.slice(start.offset, this.index), + value, + range: { start, end: this.position() }, + ...(unit + ? { + unit: { + raw: unit.literal, + kind: unit.kind, + siUnit: unit.siUnit, + normalizedValue: value * unit.factor + } + } + : {}) + }; + this.tokens.push(token); + } + + private tryScanUnitAfterNumber(): ReturnType | undefined { + const state = this.save(); + + while (!this.isAtEnd() && (this.peek() === " " || this.peek() === "\t")) { + this.advance(); + } + + const unitStart = this.index; + + if (this.peek() === "%") { + this.advance(); + } else { + while (!this.isAtEnd() && /[A-Za-z0-9/^]/.test(this.peek())) { + this.advance(); + } + } + + const literal = this.source.slice(unitStart, this.index); + if (!literal || !isGrlUnitLiteral(literal)) { + this.restore(state); + return undefined; + } + + return normalizeUnitLiteral(literal); + } + + private scanPunctuationOrOperator(): void { + const start = this.position(); + const two = `${this.peek()}${this.peek(1)}`; + + if (TWO_CHAR_OPERATORS.has(two)) { + this.advance(); + this.advance(); + const token: GrlOperatorToken = { + kind: "operator", + raw: two, + value: two, + range: { start, end: this.position() } + }; + this.tokens.push(token); + return; + } + + const char = this.advance(); + if (PUNCTUATION.has(char)) { + const token: GrlPunctuationToken = { + kind: "punctuation", + raw: char, + value: char, + range: { start, end: this.position() } + }; + this.tokens.push(token); + return; + } + + if (OPERATORS.has(char)) { + const token: GrlOperatorToken = { + kind: "operator", + raw: char, + value: char, + range: { start, end: this.position() } + }; + this.tokens.push(token); + return; + } + + throw this.error(start, `Unexpected character: ${char}`); + } + + private isExponentStart(): boolean { + const next = this.peek(1); + if (this.isDigit(next)) { + return true; + } + return (next === "+" || next === "-") && this.isDigit(this.peek(2)); + } + + private isNumberStart(char: string): boolean { + return this.isDigit(char) || (char === "." && this.isDigit(this.peek(1))); + } + + private isIdentifierStart(char: string): boolean { + return /[A-Za-z_]/.test(char); + } + + private isIdentifierPart(char: string): boolean { + return /[A-Za-z0-9_]/.test(char); + } + + private isDigit(char: string): boolean { + return /[0-9]/.test(char); + } + + private isWhitespace(char: string): boolean { + return char === " " || char === "\t" || char === "\r" || char === "\n"; + } + + private advance(): string { + const char = this.source[this.index] ?? ""; + this.index += 1; + + if (char === "\n") { + this.line += 1; + this.column = 1; + } else { + this.column += 1; + } + + return char; + } + + private peek(distance = 0): string { + return this.source[this.index + distance] ?? ""; + } + + private isAtEnd(): boolean { + return this.index >= this.source.length; + } + + private position(): GrlSourcePosition { + return { + offset: this.index, + line: this.line, + column: this.column + }; + } + + private save(): ScannerState { + return { + index: this.index, + line: this.line, + column: this.column + }; + } + + private restore(state: ScannerState): void { + this.index = state.index; + this.line = state.line; + this.column = state.column; + } + + private error(position: GrlSourcePosition, message: string): Error { + return new Error(`${message} at ${position.line}:${position.column}`); + } +} diff --git a/kdl-wasm/web/src/grl/lexer/tokens.ts b/kdl-wasm/web/src/grl/lexer/tokens.ts new file mode 100644 index 0000000..30179fb --- /dev/null +++ b/kdl-wasm/web/src/grl/lexer/tokens.ts @@ -0,0 +1,86 @@ +import type { GrlKeyword } from "./keywords.js"; +import type { UnitKind } from "./units.js"; + +export type GrlTokenKind = + | "keyword" + | "identifier" + | "number" + | "string" + | "comment" + | "punctuation" + | "operator" + | "eof"; + +export interface GrlSourcePosition { + offset: number; + line: number; + column: number; +} + +export interface GrlSourceRange { + start: GrlSourcePosition; + end: GrlSourcePosition; +} + +export interface GrlBaseToken { + kind: GrlTokenKind; + raw: string; + range: GrlSourceRange; +} + +export interface GrlKeywordToken extends GrlBaseToken { + kind: "keyword"; + value: GrlKeyword; +} + +export interface GrlIdentifierToken extends GrlBaseToken { + kind: "identifier"; + value: string; +} + +export interface GrlNumberToken extends GrlBaseToken { + kind: "number"; + value: number; + unit?: { + raw: string; + kind: UnitKind; + siUnit: string; + normalizedValue: number; + }; +} + +export interface GrlStringToken extends GrlBaseToken { + kind: "string"; + value: string; +} + +export interface GrlCommentToken extends GrlBaseToken { + kind: "comment"; + style: "line" | "block"; + value: string; +} + +export interface GrlPunctuationToken extends GrlBaseToken { + kind: "punctuation"; + value: string; +} + +export interface GrlOperatorToken extends GrlBaseToken { + kind: "operator"; + value: string; +} + +export interface GrlEofToken extends GrlBaseToken { + kind: "eof"; + value: ""; +} + +export type GrlToken = + | GrlKeywordToken + | GrlIdentifierToken + | GrlNumberToken + | GrlStringToken + | GrlCommentToken + | GrlPunctuationToken + | GrlOperatorToken + | GrlEofToken; diff --git a/kdl-wasm/web/src/grl/lexer/units.ts b/kdl-wasm/web/src/grl/lexer/units.ts new file mode 100644 index 0000000..b93b6a1 --- /dev/null +++ b/kdl-wasm/web/src/grl/lexer/units.ts @@ -0,0 +1,59 @@ +export type UnitKind = + | "length" + | "angle" + | "time" + | "mass" + | "linear_velocity" + | "angular_velocity" + | "linear_acceleration" + | "angular_acceleration" + | "percent"; + +export interface UnitDefinition { + literal: string; + kind: UnitKind; + siUnit: string; + factor: number; +} + +const UNIT_DEFINITIONS: UnitDefinition[] = [ + { literal: "m", kind: "length", siUnit: "m", factor: 1 }, + { literal: "mm", kind: "length", siUnit: "m", factor: 0.001 }, + { literal: "rad", kind: "angle", siUnit: "rad", factor: 1 }, + { literal: "deg", kind: "angle", siUnit: "rad", factor: Math.PI / 180 }, + { literal: "s", kind: "time", siUnit: "s", factor: 1 }, + { literal: "ms", kind: "time", siUnit: "s", factor: 0.001 }, + { literal: "kg", kind: "mass", siUnit: "kg", factor: 1 }, + { literal: "m/s", kind: "linear_velocity", siUnit: "m/s", factor: 1 }, + { literal: "mm/s", kind: "linear_velocity", siUnit: "m/s", factor: 0.001 }, + { literal: "rad/s", kind: "angular_velocity", siUnit: "rad/s", factor: 1 }, + { literal: "deg/s", kind: "angular_velocity", siUnit: "rad/s", factor: Math.PI / 180 }, + { literal: "m/s2", kind: "linear_acceleration", siUnit: "m/s2", factor: 1 }, + { literal: "m/s^2", kind: "linear_acceleration", siUnit: "m/s2", factor: 1 }, + { literal: "mm/s2", kind: "linear_acceleration", siUnit: "m/s2", factor: 0.001 }, + { literal: "mm/s^2", kind: "linear_acceleration", siUnit: "m/s2", factor: 0.001 }, + { literal: "rad/s2", kind: "angular_acceleration", siUnit: "rad/s2", factor: 1 }, + { literal: "rad/s^2", kind: "angular_acceleration", siUnit: "rad/s2", factor: 1 }, + { literal: "deg/s2", kind: "angular_acceleration", siUnit: "rad/s2", factor: Math.PI / 180 }, + { literal: "deg/s^2", kind: "angular_acceleration", siUnit: "rad/s2", factor: Math.PI / 180 }, + { literal: "%", kind: "percent", siUnit: "ratio", factor: 0.01 } +]; + +const UNITS = new Map(UNIT_DEFINITIONS.map((definition) => [definition.literal, definition])); + +export function normalizeUnitLiteral(literal: string): UnitDefinition { + const definition = UNITS.get(literal); + if (!definition) { + throw new Error(`Unknown GRL unit: ${literal}`); + } + return definition; +} + +export function isGrlUnitLiteral(literal: string): boolean { + return UNITS.has(literal); +} + +export function normalizeUnitValue(value: number, unitLiteral: string): number { + const unit = normalizeUnitLiteral(unitLiteral); + return value * unit.factor; +} diff --git a/kdl-wasm/web/src/grl/parser/errors.ts b/kdl-wasm/web/src/grl/parser/errors.ts new file mode 100644 index 0000000..9af2210 --- /dev/null +++ b/kdl-wasm/web/src/grl/parser/errors.ts @@ -0,0 +1,11 @@ +import type { GrlToken } from "../lexer/index.js"; + +export class GrlParseError extends Error { + constructor( + message: string, + readonly token: GrlToken + ) { + super(`${message} at ${token.range.start.line}:${token.range.start.column}`); + this.name = "GrlParseError"; + } +} diff --git a/kdl-wasm/web/src/grl/parser/expressionParser.ts b/kdl-wasm/web/src/grl/parser/expressionParser.ts new file mode 100644 index 0000000..be796fd --- /dev/null +++ b/kdl-wasm/web/src/grl/parser/expressionParser.ts @@ -0,0 +1,351 @@ +import type { + GrlArrayExpression, + GrlBooleanLiteral, + GrlCallExpression, + GrlExpression, + GrlIdentifierExpression, + GrlNumberLiteral, + GrlObjectExpression, + GrlObjectProperty, + GrlOffsetAxis, + GrlOffsetExpression, + GrlStringLiteral +} from "../ast/index.js"; +import type { GrlToken } from "../lexer/index.js"; +import { GrlParseError } from "./errors.js"; + +export function parseGrlExpression(tokens: GrlToken[]): GrlExpression { + const parser = new GrlExpressionParser(tokens); + return parser.parse(); +} + +class GrlExpressionParser { + private current = 0; + + constructor(private readonly tokens: GrlToken[]) {} + + parse(): GrlExpression { + const expression = this.parseOffsetExpression(); + if (!this.isAtEnd()) { + throw new GrlParseError("Unexpected token after expression", this.peek()); + } + return expression; + } + + private parseOffsetExpression(): GrlExpression { + const base = this.parsePrimary(); + if (this.matchKeyword("offset")) { + return this.finishOffsetExpression(base, "frame"); + } + if (this.matchKeyword("offset_in")) { + if (this.matchKeyword("tool")) { + return this.finishOffsetExpression(base, "tool"); + } + this.consumeKeyword("frame", "Expected tool or frame after offset_in"); + const frameName = this.consumeIdentifierLike("Expected frame name after offset_in frame"); + return this.finishOffsetExpression(base, "frame", frameName.raw); + } + return base; + } + + private finishOffsetExpression( + base: GrlExpression, + mode: "frame" | "tool", + frameName?: string + ): GrlOffsetExpression { + const axes: GrlOffsetAxis[] = []; + while (!this.isAtEnd()) { + const axis = this.consumeAxis(); + const valueExpression = this.parsePrimary(); + if (valueExpression.kind !== "NumberLiteral") { + throw new GrlParseError(`Expected length value after offset ${axis}`, valueExpression.range ? this.previous() : this.peek()); + } + const value = valueExpression; + axes.push({ axis, value }); + } + + if (axes.length === 0) { + throw new GrlParseError("Expected at least one offset axis", this.peek()); + } + + return { + kind: "OffsetExpression", + base, + mode, + ...(frameName ? { frameName } : {}), + axes, + range: { + start: base.range.start, + end: axes.at(-1)?.value.range.end ?? base.range.end + } + }; + } + + private parsePrimary(): GrlExpression { + const token = this.peek(); + + if (token.kind === "operator" && token.raw === "-") { + return this.parseNegativeNumber(); + } + + if (token.kind === "number") { + this.advance(); + return numberLiteralFromToken(token); + } + + if (token.kind === "string") { + this.advance(); + const literal: GrlStringLiteral = { + kind: "StringLiteral", + value: token.value, + range: token.range + }; + return literal; + } + + if (token.kind === "keyword" && (token.raw === "true" || token.raw === "false")) { + this.advance(); + const literal: GrlBooleanLiteral = { + kind: "BooleanLiteral", + value: token.raw === "true", + range: token.range + }; + return literal; + } + + if (token.kind === "punctuation" && token.raw === "[") { + return this.parseArrayExpression(); + } + + if (token.kind === "punctuation" && token.raw === "(") { + this.advance(); + const expression = this.parseOffsetExpression(); + this.consumePunctuation(")", "Expected ) after expression"); + return expression; + } + + if (token.kind === "identifier" || token.kind === "keyword") { + const name = this.advance(); + if (this.matchPunctuation("(")) { + return this.finishCallExpression(name); + } + if (this.matchPunctuation("{")) { + return this.finishObjectExpression(name); + } + const expression: GrlIdentifierExpression = { + kind: "IdentifierExpression", + name: name.raw, + range: name.range + }; + return expression; + } + + throw new GrlParseError("Expected expression", token); + } + + private parseArrayExpression(): GrlArrayExpression { + const start = this.consumePunctuation("[", "Expected ["); + const elements: GrlExpression[] = []; + while (!this.checkPunctuation("]") && !this.isAtEnd()) { + elements.push(this.parseOffsetExpression()); + this.matchPunctuation(","); + } + const end = this.consumePunctuation("]", "Expected ] after array expression"); + + return { + kind: "ArrayExpression", + elements, + range: { + start: start.range.start, + end: end.range.end + } + }; + } + + private parseNegativeNumber(): GrlNumberLiteral { + const minus = this.advance(); + const number = this.consumeNumberLiteral("Expected number after -"); + return { + ...number, + value: -number.value, + raw: `${minus.raw}${number.raw}`, + ...(number.unit + ? { + unit: { + ...number.unit, + normalizedValue: -number.unit.normalizedValue + } + } + : {}), + range: { + start: minus.range.start, + end: number.range.end + } + }; + } + + private finishCallExpression(callee: GrlToken): GrlCallExpression { + const args: GrlExpression[] = []; + while (!this.checkPunctuation(")") && !this.isAtEnd()) { + args.push(this.parseOffsetExpression()); + this.matchPunctuation(","); + } + const end = this.consumePunctuation(")", "Expected ) after call expression"); + + return { + kind: "CallExpression", + callee: callee.raw, + args, + range: { + start: callee.range.start, + end: end.range.end + } + }; + } + + private finishObjectExpression(typeName: GrlToken): GrlObjectExpression { + const properties: GrlObjectProperty[] = []; + while (!this.checkPunctuation("}") && !this.isAtEnd()) { + const key = this.consumeIdentifierLike("Expected object property name"); + this.consumePunctuation(":", "Expected : after object property name"); + const value = this.parseOffsetExpression(); + properties.push({ + key: key.raw, + value, + range: { + start: key.range.start, + end: value.range.end + } + }); + this.matchPunctuation(","); + } + const end = this.consumePunctuation("}", "Expected } after object expression"); + + return { + kind: "ObjectExpression", + typeName: typeName.raw, + properties, + range: { + start: typeName.range.start, + end: end.range.end + } + }; + } + + private consumeAxis(): "x" | "y" | "z" { + const token = this.consumeIdentifierLike("Expected offset axis"); + if (token.raw === "x" || token.raw === "y" || token.raw === "z") { + return token.raw; + } + throw new GrlParseError("Expected offset axis x, y, or z", token); + } + + private consumeNumberLiteral(message: string): GrlNumberLiteral { + const token = this.consume("number", message); + if (token.kind !== "number") { + throw new GrlParseError(message, token); + } + return numberLiteralFromToken(token); + } + + private consumeIdentifierLike(message: string): GrlToken { + const token = this.peek(); + if (token.kind === "identifier" || token.kind === "keyword") { + return this.advance(); + } + throw new GrlParseError(message, token); + } + + private consume(kind: GrlToken["kind"], message: string): GrlToken { + if (this.check(kind)) { + return this.advance(); + } + throw new GrlParseError(message, this.peek()); + } + + private consumeKeyword(keyword: string, message: string): GrlToken { + if (this.checkKeyword(keyword)) { + return this.advance(); + } + throw new GrlParseError(message, this.peek()); + } + + private consumePunctuation(value: string, message: string): GrlToken { + if (this.checkPunctuation(value)) { + return this.advance(); + } + throw new GrlParseError(message, this.peek()); + } + + private matchKeyword(keyword: string): boolean { + if (this.checkKeyword(keyword)) { + this.advance(); + return true; + } + return false; + } + + private matchPunctuation(value: string): boolean { + if (this.checkPunctuation(value)) { + this.advance(); + return true; + } + return false; + } + + private check(kind: GrlToken["kind"]): boolean { + return !this.isAtEnd() && this.peek().kind === kind; + } + + private checkKeyword(keyword: string): boolean { + const token = this.peek(); + return (token.kind === "keyword" || token.kind === "identifier") && token.raw === keyword; + } + + private checkPunctuation(value: string): boolean { + const token = this.peek(); + return token.kind === "punctuation" && token.raw === value; + } + + private advance(): GrlToken { + if (!this.isAtEnd()) { + this.current += 1; + } + return this.previous(); + } + + private isAtEnd(): boolean { + return this.current >= this.tokens.length; + } + + private peek(): GrlToken { + return this.tokens[this.current] ?? this.tokens[this.tokens.length - 1]!; + } + + private previous(): GrlToken { + return this.tokens[this.current - 1] ?? this.tokens[0]!; + } +} + +function numberLiteralFromToken(token: GrlToken): GrlNumberLiteral { + if (token.kind !== "number") { + throw new GrlParseError("Expected number literal", token); + } + + return { + kind: "NumberLiteral", + value: token.value, + raw: token.raw, + ...(token.unit + ? { + unit: { + raw: token.unit.raw, + kind: token.unit.kind, + siUnit: token.unit.siUnit, + normalizedValue: token.unit.normalizedValue + } + } + : {}), + range: token.range + }; +} diff --git a/kdl-wasm/web/src/grl/parser/index.ts b/kdl-wasm/web/src/grl/parser/index.ts new file mode 100644 index 0000000..82a8e2b --- /dev/null +++ b/kdl-wasm/web/src/grl/parser/index.ts @@ -0,0 +1,3 @@ +export { GrlParseError } from "./errors.js"; +export { parseGrl } from "./parser.js"; +export { parseGrlExpression } from "./expressionParser.js"; diff --git a/kdl-wasm/web/src/grl/parser/parser.ts b/kdl-wasm/web/src/grl/parser/parser.ts new file mode 100644 index 0000000..2d5023b --- /dev/null +++ b/kdl-wasm/web/src/grl/parser/parser.ts @@ -0,0 +1,859 @@ +import { lexGrl, type GrlToken } from "../lexer/index.js"; +import type { + GrlDataDeclaration, + GrlDeclarationStorage, + GrlFunctionDeclaration, + GrlImportDeclaration, + GrlLanguageDeclaration, + GrlModuleDeclaration, + GrlOperationActionBlock, + GrlOperationDeclaration, + GrlOperationItem, + GrlOperationProcessBlock, + GrlPathDeclaration, + GrlPathDefaultsBlock, + GrlPathEvent, + GrlPathItem, + GrlPathPoint, + GrlPathProperty, + GrlPathSourceBlock, + GrlProcedureDeclaration, + GrlProgram, + GrlRawTopLevelDeclaration, + GrlTargetDeclaration, + GrlTopLevelDeclaration +} from "../ast/index.js"; +import { GrlParseError } from "./errors.js"; +import { parseGrlExpression } from "./expressionParser.js"; + +const RAW_TOP_LEVEL_KEYWORDS = new Set([ + "trap", + "task", + "post_hint" +]); + +export function parseGrl(source: string): GrlProgram { + const tokens = lexGrl(source).filter((token) => token.kind !== "comment"); + return new GrlParser(tokens).parseProgram(); +} + +class GrlParser { + private current = 0; + + constructor(private readonly tokens: GrlToken[]) {} + + parseProgram(): GrlProgram { + const first = this.peek(); + const language = this.matchKeyword("language") ? this.finishLanguageDeclaration(this.previous()) : undefined; + const module = this.parseModuleDeclaration(); + const eof = this.consume("eof", "Expected end of file after module declaration"); + + return { + kind: "Program", + ...(language ? { language } : {}), + module, + range: { + start: (language?.range ?? module.range).start, + end: eof.range.end + } + }; + } + + private finishLanguageDeclaration(languageToken: GrlToken): GrlLanguageDeclaration { + const languageName = this.consumeIdentifierLike("Expected language name after language"); + if (languageName.raw !== "grl") { + throw new GrlParseError("Only language grl is supported", languageName); + } + + const version = this.consume("number", "Expected GRL language version"); + return { + kind: "LanguageDeclaration", + language: "grl", + version: version.raw, + range: { + start: languageToken.range.start, + end: version.range.end + } + }; + } + + private parseModuleDeclaration(): GrlModuleDeclaration { + const moduleToken = this.consumeKeyword("module", "Expected module declaration"); + const name = this.consume("identifier", "Expected module name"); + const declarations: GrlTopLevelDeclaration[] = []; + + while (!this.checkKeyword("end") && !this.isAtEnd()) { + declarations.push(this.parseTopLevelDeclaration()); + } + + const end = this.consumeKeyword("end", "Expected end after module declaration"); + return { + kind: "ModuleDeclaration", + name: name.raw, + declarations, + range: { + start: moduleToken.range.start, + end: end.range.end + } + }; + } + + private parseTopLevelDeclaration(): GrlTopLevelDeclaration { + if (this.matchKeyword("import")) { + return this.finishImportDeclaration(this.previous()); + } + + if (this.matchKeyword("proc")) { + return this.finishProcedureDeclaration(this.previous()); + } + + if (this.matchKeyword("func")) { + return this.finishFunctionDeclaration(this.previous()); + } + + if (this.checkDataDeclarationStart()) { + return this.parseDataDeclaration(); + } + + if (this.matchKeyword("target")) { + return this.finishTargetDeclaration(this.previous()); + } + + if (this.matchKeyword("path")) { + return this.finishPathDeclaration(this.previous()); + } + + if (this.matchKeyword("operation")) { + return this.finishOperationDeclaration(this.previous()); + } + + const token = this.peek(); + if (token.kind === "keyword" && RAW_TOP_LEVEL_KEYWORDS.has(token.raw)) { + return this.parseRawTopLevelDeclaration(); + } + + throw new GrlParseError("Expected top-level declaration", token); + } + + private finishImportDeclaration(importToken: GrlToken): GrlImportDeclaration { + const moduleName = this.consume("identifier", "Expected imported module name"); + return { + kind: "ImportDeclaration", + moduleName: moduleName.raw, + range: { + start: importToken.range.start, + end: moduleName.range.end + } + }; + } + + private parseDataDeclaration(): GrlDataDeclaration { + const storageToken = this.advance(); + const storage = storageToken.raw as GrlDeclarationStorage; + const typeName = this.consumeIdentifierLike("Expected type name in data declaration"); + const name = this.consume("identifier", "Expected variable name in data declaration"); + this.consumeOperator("=", "Expected = in data declaration"); + const initializerTokens = this.collectFlatExpressionTokens(); + const initializer = parseGrlExpression(initializerTokens); + + return { + kind: "DataDeclaration", + storage, + typeName: typeName.raw, + name: name.raw, + initializer, + range: { + start: storageToken.range.start, + end: initializer.range.end + } + }; + } + + private finishTargetDeclaration(targetToken: GrlToken): GrlTargetDeclaration { + const name = this.consume("identifier", "Expected target name"); + this.consumeOperator("=", "Expected = in target declaration"); + const targetTokens = this.collectFlatExpressionTokens(); + const target = parseGrlExpression(targetTokens); + + return { + kind: "TargetDeclaration", + name: name.raw, + target, + range: { + start: targetToken.range.start, + end: target.range.end + } + }; + } + + private finishPathDeclaration(pathToken: GrlToken): GrlPathDeclaration { + const name = this.consume("identifier", "Expected path name"); + this.consumePunctuation("{", "Expected { after path name"); + const items: GrlPathItem[] = []; + + while (!this.checkPunctuation("}") && !this.isAtEnd()) { + if (this.matchKeyword("defaults")) { + items.push(this.finishPathDefaultsBlock(this.previous())); + continue; + } + if (this.matchKeyword("source")) { + items.push(this.finishPathSourceBlock(this.previous())); + continue; + } + if (this.matchKeyword("point")) { + items.push(this.finishPathPoint(this.previous())); + continue; + } + if (this.matchKeyword("event")) { + items.push(this.finishPathEvent(this.previous())); + continue; + } + throw new GrlParseError("Expected path item", this.peek()); + } + + const end = this.consumePunctuation("}", "Expected } after path declaration"); + return { + kind: "PathDeclaration", + name: name.raw, + items, + range: { + start: pathToken.range.start, + end: end.range.end + } + }; + } + + private finishPathDefaultsBlock(start: GrlToken): GrlPathDefaultsBlock { + const { properties, end } = this.parsePathPropertyBlock("defaults"); + return { + kind: "PathDefaultsBlock", + properties, + range: { + start: start.range.start, + end: end.range.end + } + }; + } + + private finishPathSourceBlock(start: GrlToken): GrlPathSourceBlock { + const { properties, end } = this.parsePathPropertyBlock("source"); + return { + kind: "PathSourceBlock", + properties, + range: { + start: start.range.start, + end: end.range.end + } + }; + } + + private parsePathPropertyBlock(blockName: string): { properties: GrlPathProperty[]; end: GrlToken } { + this.consumePunctuation("{", `Expected { after path ${blockName}`); + const properties: GrlPathProperty[] = []; + + while (!this.checkPunctuation("}") && !this.isAtEnd()) { + const key = this.consumeIdentifierLike(`Expected ${blockName} property name`); + this.consumePunctuation(":", `Expected : after ${blockName} property name`); + const valueTokens = this.collectPathPropertyValueTokens(); + const value = parseGrlExpression(valueTokens); + properties.push({ + key: key.raw, + value, + range: { + start: key.range.start, + end: value.range.end + } + }); + this.matchPunctuation(","); + } + + const end = this.consumePunctuation("}", `Expected } after path ${blockName}`); + return { properties, end }; + } + + private finishPathPoint(start: GrlToken): GrlPathPoint { + const id = this.consume("identifier", "Expected path point id"); + const motionTokens = this.collectPathMotionTokens(); + if (motionTokens.length === 0) { + throw new GrlParseError("Expected path point motion statement", this.peek()); + } + return { + kind: "PathPoint", + id: id.raw, + motionTokens, + range: { + start: start.range.start, + end: motionTokens.at(-1)?.range.end ?? id.range.end + } + }; + } + + private finishPathEvent(start: GrlToken): GrlPathEvent { + const timing = this.consumeIdentifierLike("Expected before, after, or at after event"); + if (timing.raw !== "before" && timing.raw !== "after" && timing.raw !== "at") { + throw new GrlParseError("Expected before, after, or at after event", timing); + } + const point = this.consume("identifier", "Expected event point id"); + let distance: GrlPathEvent["distance"]; + if (timing.raw === "at") { + this.consumeIdentifierValue("distance", "Expected distance in event at"); + const distanceToken = this.peek(); + const distanceTokens = this.collectSignedNumberTokens(); + const distanceExpression = parseGrlExpression(distanceTokens); + if (distanceExpression.kind !== "NumberLiteral") { + throw new GrlParseError("Expected numeric event distance", distanceToken); + } + distance = distanceExpression; + } + const actionTokens = this.collectPathEventActionTokens(); + if (actionTokens.length === 0) { + throw new GrlParseError("Expected path event action", this.peek()); + } + + return { + kind: "PathEvent", + timing: timing.raw, + pointId: point.raw, + ...(distance ? { distance } : {}), + actionTokens, + range: { + start: start.range.start, + end: actionTokens.at(-1)?.range.end ?? point.range.end + } + }; + } + + private finishOperationDeclaration(operationToken: GrlToken): GrlOperationDeclaration { + const name = this.consume("identifier", "Expected operation name"); + this.consumePunctuation("{", "Expected { after operation name"); + let operationKind: string | undefined; + let pathName: string | undefined; + const items: GrlOperationItem[] = []; + + while (!this.checkPunctuation("}") && !this.isAtEnd()) { + if (this.matchIdentifierValue("kind")) { + this.consumePunctuation(":", "Expected : after operation kind"); + operationKind = this.consumeIdentifierLike("Expected operation kind").raw; + this.matchPunctuation(","); + continue; + } + if (this.matchIdentifierValue("path")) { + this.consumePunctuation(":", "Expected : after operation path"); + pathName = this.consumeIdentifierLike("Expected operation path name").raw; + this.matchPunctuation(","); + continue; + } + if (this.matchIdentifierValue("process")) { + items.push(this.finishOperationProcessBlock(this.previous())); + continue; + } + if (this.matchIdentifierValue("start_action")) { + items.push(this.finishOperationActionBlock(this.previous(), "start_action")); + continue; + } + if (this.matchIdentifierValue("end_action")) { + items.push(this.finishOperationActionBlock(this.previous(), "end_action")); + continue; + } + throw new GrlParseError("Expected operation item", this.peek()); + } + + const end = this.consumePunctuation("}", "Expected } after operation declaration"); + if (!operationKind) { + throw new GrlParseError("Operation requires kind", end); + } + if (!pathName) { + throw new GrlParseError("Operation requires path", end); + } + + return { + kind: "OperationDeclaration", + name: name.raw, + operationKind, + pathName, + items, + range: { + start: operationToken.range.start, + end: end.range.end + } + }; + } + + private finishOperationProcessBlock(start: GrlToken): GrlOperationProcessBlock { + const { properties, end } = this.parsePathPropertyBlock("process"); + return { + kind: "OperationProcessBlock", + properties, + range: { + start: start.range.start, + end: end.range.end + } + }; + } + + private finishOperationActionBlock( + start: GrlToken, + actionKind: "start_action" | "end_action" + ): GrlOperationActionBlock { + this.consumePunctuation(":", `Expected : after ${actionKind}`); + const actionTokens = this.collectOperationActionTokens(); + if (actionTokens.length === 0) { + throw new GrlParseError(`Expected ${actionKind} action`, this.peek()); + } + return { + kind: "OperationActionBlock", + actionKind, + actionTokens, + range: { + start: start.range.start, + end: actionTokens.at(-1)?.range.end ?? start.range.end + } + }; + } + + private finishProcedureDeclaration(procToken: GrlToken): GrlProcedureDeclaration { + const name = this.consume("identifier", "Expected procedure name"); + this.consumePunctuation("(", "Expected ( after procedure name"); + + const params: GrlToken[] = []; + while (!this.checkPunctuation(")") && !this.isAtEnd()) { + params.push(this.advance()); + } + this.consumePunctuation(")", "Expected ) after procedure parameters"); + + const bodyTokens: GrlToken[] = []; + let nestedBlocks = 0; + while (!this.isAtEnd()) { + if (this.checkKeyword("end") && nestedBlocks === 0) { + break; + } + + const token = this.advance(); + bodyTokens.push(token); + + if (token.kind === "keyword" && token.raw === "end" && nestedBlocks > 0) { + nestedBlocks -= 1; + } else if (token.kind === "keyword" && ["if", "while", "for", "switch", "try"].includes(token.raw)) { + nestedBlocks += 1; + } + } + + const end = this.consumeKeyword("end", "Expected end after procedure declaration"); + + return { + kind: "ProcedureDeclaration", + name: name.raw, + params, + bodyTokens, + range: { + start: procToken.range.start, + end: end.range.end + } + }; + } + + private finishFunctionDeclaration(funcToken: GrlToken): GrlFunctionDeclaration { + const returnType = this.consumeIdentifierLike("Expected function return type"); + const name = this.consume("identifier", "Expected function name"); + this.consumePunctuation("(", "Expected ( after function name"); + + const params: GrlToken[] = []; + while (!this.checkPunctuation(")") && !this.isAtEnd()) { + params.push(this.advance()); + } + this.consumePunctuation(")", "Expected ) after function parameters"); + + const bodyTokens: GrlToken[] = []; + let nestedBlocks = 0; + while (!this.isAtEnd()) { + if (this.checkKeyword("end") && nestedBlocks === 0) { + break; + } + + const token = this.advance(); + bodyTokens.push(token); + + if (token.kind === "keyword" && token.raw === "end" && nestedBlocks > 0) { + nestedBlocks -= 1; + } else if (token.kind === "keyword" && ["if", "while", "for", "switch", "try"].includes(token.raw)) { + nestedBlocks += 1; + } + } + + const end = this.consumeKeyword("end", "Expected end after function declaration"); + + return { + kind: "FunctionDeclaration", + returnType: returnType.raw, + name: name.raw, + params, + bodyTokens, + range: { + start: funcToken.range.start, + end: end.range.end + } + }; + } + + private parseRawTopLevelDeclaration(): GrlRawTopLevelDeclaration { + const first = this.advance(); + const tokens: GrlToken[] = [first]; + + if (["path", "operation"].includes(first.raw)) { + this.collectBalancedBlock(tokens); + } else if (["trap", "task"].includes(first.raw)) { + this.collectUntilMatchingEnd(tokens); + } else { + this.collectFlatDeclaration(tokens); + } + + return { + kind: "RawTopLevelDeclaration", + declarationType: first.raw, + tokens, + range: { + start: first.range.start, + end: tokens.at(-1)?.range.end ?? first.range.end + } + }; + } + + private collectBalancedBlock(tokens: GrlToken[]): void { + let braceDepth = 0; + while (!this.isAtEnd()) { + const token = this.advance(); + tokens.push(token); + if (token.kind === "punctuation" && token.raw === "{") { + braceDepth += 1; + } else if (token.kind === "punctuation" && token.raw === "}") { + braceDepth -= 1; + if (braceDepth === 0) { + return; + } + } + } + } + + private collectUntilMatchingEnd(tokens: GrlToken[]): void { + let nestedBlocks = 0; + while (!this.isAtEnd()) { + const token = this.advance(); + tokens.push(token); + if (token.kind === "keyword" && token.raw === "end") { + if (nestedBlocks === 0) { + return; + } + nestedBlocks -= 1; + } else if (token.kind === "keyword" && ["if", "while", "for", "switch", "try"].includes(token.raw)) { + nestedBlocks += 1; + } + } + } + + private collectFlatDeclaration(tokens: GrlToken[]): void { + while (!this.isAtEnd()) { + if (this.checkKeyword("end") || this.startsTopLevelDeclaration(this.peek())) { + return; + } + tokens.push(this.advance()); + } + } + + private collectFlatExpressionTokens(): GrlToken[] { + const tokens: GrlToken[] = []; + let braceDepth = 0; + let bracketDepth = 0; + let parenDepth = 0; + + while (!this.isAtEnd()) { + if ( + braceDepth === 0 && + bracketDepth === 0 && + parenDepth === 0 && + (this.checkKeyword("end") || this.startsTopLevelDeclaration(this.peek()) || this.startsNextDataDeclaration()) + ) { + break; + } + + const token = this.advance(); + tokens.push(token); + + if (token.kind === "punctuation") { + if (token.raw === "{") { + braceDepth += 1; + } else if (token.raw === "}") { + braceDepth -= 1; + } else if (token.raw === "[") { + bracketDepth += 1; + } else if (token.raw === "]") { + bracketDepth -= 1; + } else if (token.raw === "(") { + parenDepth += 1; + } else if (token.raw === ")") { + parenDepth -= 1; + } + } + } + + if (tokens.length === 0) { + throw new GrlParseError("Expected expression", this.peek()); + } + return tokens; + } + + private collectPathPropertyValueTokens(): GrlToken[] { + return this.collectUntil((token, depth) => + depth.brace === 0 && + depth.bracket === 0 && + depth.paren === 0 && + ((token.kind === "punctuation" && (token.raw === "," || token.raw === "}")) || + this.isPathItemStart(token) || + this.isPathPropertyStart()) + ); + } + + private collectPathMotionTokens(): GrlToken[] { + return this.collectUntil((token, depth) => + depth.brace === 0 && + depth.bracket === 0 && + depth.paren === 0 && + ((token.kind === "punctuation" && token.raw === "}") || this.isPathItemStart(token)) + ); + } + + private collectPathEventActionTokens(): GrlToken[] { + return this.collectUntil((token, depth) => + depth.brace === 0 && + depth.bracket === 0 && + depth.paren === 0 && + ((token.kind === "punctuation" && token.raw === "}") || this.isPathItemStart(token)) + ); + } + + private collectOperationActionTokens(): GrlToken[] { + return this.collectUntil((token, depth) => + depth.brace === 0 && + depth.bracket === 0 && + depth.paren === 0 && + ((token.kind === "punctuation" && token.raw === "}") || this.isOperationItemStart(token)) + ); + } + + private collectSignedNumberTokens(): GrlToken[] { + const tokens: GrlToken[] = []; + if (this.peek().kind === "operator" && this.peek().raw === "-") { + tokens.push(this.advance()); + } + tokens.push(this.consume("number", "Expected numeric value")); + return tokens; + } + + private collectUntil( + shouldStop: ( + token: GrlToken, + depth: { brace: number; bracket: number; paren: number } + ) => boolean + ): GrlToken[] { + const tokens: GrlToken[] = []; + const depth = { brace: 0, bracket: 0, paren: 0 }; + + while (!this.isAtEnd() && !shouldStop(this.peek(), depth)) { + const token = this.advance(); + tokens.push(token); + + if (token.kind === "punctuation") { + if (token.raw === "{") { + depth.brace += 1; + } else if (token.raw === "}") { + depth.brace -= 1; + } else if (token.raw === "[") { + depth.bracket += 1; + } else if (token.raw === "]") { + depth.bracket -= 1; + } else if (token.raw === "(") { + depth.paren += 1; + } else if (token.raw === ")") { + depth.paren -= 1; + } + } + } + + if (tokens.length === 0) { + throw new GrlParseError("Expected expression", this.peek()); + } + return tokens; + } + + private isPathItemStart(token: GrlToken): boolean { + return ( + token.kind === "keyword" && + (token.raw === "defaults" || token.raw === "source" || token.raw === "point" || token.raw === "event") + ); + } + + private isOperationItemStart(token: GrlToken): boolean { + return ( + ((token.kind === "keyword" || token.kind === "identifier") && + (token.raw === "kind" || token.raw === "path" || token.raw === "process")) || + ((token.kind === "identifier" || token.kind === "keyword") && + (token.raw === "start_action" || token.raw === "end_action")) + ); + } + + private isPathPropertyStart(): boolean { + const token = this.peek(); + const next = this.peek(1); + return ( + (token.kind === "keyword" || token.kind === "identifier") && + next.kind === "punctuation" && + next.raw === ":" + ); + } + + private startsNextDataDeclaration(): boolean { + const current = this.peek(); + const next = this.peekNext(); + const following = this.peek(2); + + if (current.kind !== "identifier" && current.kind !== "keyword") { + return false; + } + if (next.kind !== "identifier" && next.kind !== "keyword") { + return false; + } + return following.kind === "operator" && following.raw === "="; + } + + private startsTopLevelDeclaration(token: GrlToken): boolean { + if (token.kind !== "keyword") { + return false; + } + if (token.raw === "target") { + const name = this.peek(1); + const equals = this.peek(2); + return name.kind === "identifier" && equals.kind === "operator" && equals.raw === "="; + } + + return ( + token.raw === "import" || + token.raw === "proc" || + token.raw === "path" || + token.raw === "operation" || + token.raw === "persistent" || + token.raw === "const" || + token.raw === "var" || + RAW_TOP_LEVEL_KEYWORDS.has(token.raw) + ); + } + + private checkDataDeclarationStart(): boolean { + return this.checkKeyword("persistent") || this.checkKeyword("const") || this.checkKeyword("var"); + } + + private consumeIdentifierLike(message: string): GrlToken { + const token = this.peek(); + if (token.kind === "identifier" || token.kind === "keyword") { + return this.advance(); + } + throw new GrlParseError(message, token); + } + + private consume(kind: GrlToken["kind"], message: string): GrlToken { + if (this.check(kind)) { + return this.advance(); + } + throw new GrlParseError(message, this.peek()); + } + + private consumeKeyword(keyword: string, message: string): GrlToken { + if (this.checkKeyword(keyword)) { + return this.advance(); + } + throw new GrlParseError(message, this.peek()); + } + + private consumeIdentifierValue(value: string, message: string): GrlToken { + const token = this.peek(); + if ((token.kind === "keyword" || token.kind === "identifier") && token.raw === value) { + return this.advance(); + } + throw new GrlParseError(message, token); + } + + private consumePunctuation(value: string, message: string): GrlToken { + if (this.checkPunctuation(value)) { + return this.advance(); + } + throw new GrlParseError(message, this.peek()); + } + + private consumeOperator(value: string, message: string): GrlToken { + const token = this.peek(); + if (token.kind === "operator" && token.raw === value) { + return this.advance(); + } + throw new GrlParseError(message, token); + } + + private matchKeyword(keyword: string): boolean { + if (this.checkKeyword(keyword)) { + this.advance(); + return true; + } + return false; + } + + private matchIdentifierValue(value: string): boolean { + const token = this.peek(); + if ((token.kind === "keyword" || token.kind === "identifier") && token.raw === value) { + this.advance(); + return true; + } + return false; + } + + private matchPunctuation(value: string): boolean { + if (this.checkPunctuation(value)) { + this.advance(); + return true; + } + return false; + } + + private check(kind: GrlToken["kind"]): boolean { + return this.peek().kind === kind; + } + + private checkKeyword(keyword: string): boolean { + const token = this.peek(); + return token.kind === "keyword" && token.raw === keyword; + } + + private checkPunctuation(value: string): boolean { + const token = this.peek(); + return token.kind === "punctuation" && token.raw === value; + } + + private advance(): GrlToken { + if (!this.isAtEnd()) { + this.current += 1; + } + return this.previous(); + } + + private isAtEnd(): boolean { + return this.peek().kind === "eof"; + } + + private peek(distance = 0): GrlToken { + return this.tokens[this.current + distance] ?? this.tokens[this.tokens.length - 1]!; + } + + private peekNext(): GrlToken { + return this.peek(1); + } + + private previous(): GrlToken { + return this.tokens[this.current - 1] ?? this.tokens[0]!; + } +} diff --git a/kdl-wasm/web/src/grl/post/index.ts b/kdl-wasm/web/src/grl/post/index.ts new file mode 100644 index 0000000..7898fe8 --- /dev/null +++ b/kdl-wasm/web/src/grl/post/index.ts @@ -0,0 +1,8 @@ +export { + postProcessAllBrands, + postProcessBrand, + type MultiBrandPostResult, + type PostBrand, + type PostIssue, + type PostResult +} from "./postProcessor.js"; diff --git a/kdl-wasm/web/src/grl/post/postProcessor.ts b/kdl-wasm/web/src/grl/post/postProcessor.ts new file mode 100644 index 0000000..864aea0 --- /dev/null +++ b/kdl-wasm/web/src/grl/post/postProcessor.ts @@ -0,0 +1,232 @@ +import type { MotionDiagnostic, SpeedSpec, ZoneSpec } from "../../kdl/types.js"; +import type { + ExecutableInstruction, + MotionInstruction, + SemanticProgramIr +} from "../ir/index.js"; + +export type PostBrand = "abb" | "fanuc" | "kuka"; + +export interface PostIssue { + severity: MotionDiagnostic["severity"]; + code: string; + message: string; + brand?: PostBrand; +} + +export interface PostResult { + brand: PostBrand; + filename: string; + text: string; + report: PostIssue[]; +} + +export interface MultiBrandPostResult { + outputs: Record; + report: PostIssue[]; +} + +export function postProcessAllBrands(ir: SemanticProgramIr): MultiBrandPostResult { + const abb = postProcessBrand(ir, "abb"); + const fanuc = postProcessBrand(ir, "fanuc"); + const kuka = postProcessBrand(ir, "kuka"); + return { + outputs: { abb, fanuc, kuka }, + report: [...abb.report, ...fanuc.report, ...kuka.report] + }; +} + +export function postProcessBrand(ir: SemanticProgramIr, brand: PostBrand): PostResult { + const report: PostIssue[] = collectBrandHintIssues(ir, brand); + const text = renderBrandProgram(ir, brand, report); + return { + brand, + filename: filenameFor(ir.moduleName, brand), + text, + report + }; +} + +function renderBrandProgram(ir: SemanticProgramIr, brand: PostBrand, report: PostIssue[]): string { + switch (brand) { + case "abb": + return renderAbb(ir, report); + case "fanuc": + return renderFanuc(ir, report); + case "kuka": + return renderKuka(ir, report); + } +} + +function renderAbb(ir: SemanticProgramIr, report: PostIssue[]): string { + const lines = [`MODULE ${ir.moduleName}`, " PROC main()"]; + for (const instruction of mainInstructions(ir)) { + lines.push(` ${renderAbbInstruction(instruction, report)}`); + } + lines.push(" ENDPROC", "ENDMODULE"); + return lines.join("\n"); +} + +function renderFanuc(ir: SemanticProgramIr, report: PostIssue[]): string { + const lines = ["/PROG MAIN", "/MN"]; + mainInstructions(ir).forEach((instruction, index) => { + lines.push(` ${index + 1}: ${renderFanucInstruction(instruction, report)} ;`); + }); + lines.push("/END"); + return lines.join("\n"); +} + +function renderKuka(ir: SemanticProgramIr, report: PostIssue[]): string { + const lines = ["DEF Main()"]; + for (const instruction of mainInstructions(ir)) { + lines.push(` ${renderKukaInstruction(instruction, report)}`); + } + lines.push("END"); + return lines.join("\n"); +} + +function renderAbbInstruction(instruction: ExecutableInstruction, report: PostIssue[]): string { + if (isMotion(instruction)) { + const target = motionTargetName(instruction); + const zone = abbZone(instruction.zone); + const speed = abbSpeed(instruction.speed); + if (instruction.kind === "MOVEJ") return `MoveJ ${target},${speed},${zone},tool0;`; + if (instruction.kind === "MOVEL") return `MoveL ${target},${speed},${zone},tool0;`; + return `MoveC ${motionViaName(instruction)},${target},${speed},${zone},tool0;`; + } + if (instruction.kind === "IO_WRITE") return `SetDO ${instruction.target.raw},${formatValue(instruction.value)};`; + if (instruction.kind === "WAIT") return `WaitUntil ${instruction.condition};`; + if (instruction.kind === "PULSE") return `PulseDO ${instruction.target.raw},${instruction.duration.toFixed(3)};`; + return unsupportedLine("abb", instruction.kind, report); +} + +function renderFanucInstruction(instruction: ExecutableInstruction, report: PostIssue[]): string { + if (isMotion(instruction)) { + const target = motionTargetName(instruction); + const speed = fanucSpeed(instruction.speed); + const zone = fanucZone(instruction.zone); + if (instruction.kind === "MOVEJ") return `J ${target} ${speed} ${zone}`; + if (instruction.kind === "MOVEL") return `L ${target} ${speed} ${zone}`; + return `C ${motionViaName(instruction)} ${target} ${speed} ${zone}`; + } + if (instruction.kind === "IO_WRITE") return `${fanucIo(instruction.target.raw)}=${formatValue(instruction.value)}`; + if (instruction.kind === "WAIT") return `WAIT (${instruction.condition})`; + if (instruction.kind === "PULSE") return `PULSE ${fanucIo(instruction.target.raw)} ${Math.round(instruction.duration * 1000)}ms`; + return unsupportedLine("fanuc", instruction.kind, report); +} + +function renderKukaInstruction(instruction: ExecutableInstruction, report: PostIssue[]): string { + if (isMotion(instruction)) { + const target = motionTargetName(instruction); + const speed = kukaSpeed(instruction.speed); + const zone = kukaZone(instruction.zone); + if (instruction.kind === "MOVEJ") return `PTP ${target} ${speed}${zone}`; + if (instruction.kind === "MOVEL") return `LIN ${target} ${speed}${zone}`; + return `CIRC ${motionViaName(instruction)}, ${target} ${speed}${zone}`; + } + if (instruction.kind === "IO_WRITE") return `${kukaIo(instruction.target.raw)} = ${formatValue(instruction.value)}`; + if (instruction.kind === "WAIT") return `WAIT FOR ${instruction.condition}`; + if (instruction.kind === "PULSE") return `PULSE ${kukaIo(instruction.target.raw)} ${instruction.duration.toFixed(3)}`; + return unsupportedLine("kuka", instruction.kind, report); +} + +function mainInstructions(ir: SemanticProgramIr): ExecutableInstruction[] { + return ir.procedures.find((procedure) => procedure.name === "main")?.instructions ?? []; +} + +function isMotion(instruction: ExecutableInstruction): instruction is MotionInstruction { + return instruction.kind === "MOVEJ" || instruction.kind === "MOVEL" || instruction.kind === "MOVEC"; +} + +function motionTargetName(instruction: MotionInstruction): string { + const target = instruction.target; + if (target && "id" in target && target.id) { + return target.id; + } + return instruction.pointId ?? instruction.id ?? "p_auto"; +} + +function motionViaName(instruction: MotionInstruction): string { + const via = instruction.via; + if (via && "id" in via && via.id) { + return via.id; + } + return "via_auto"; +} + +function abbSpeed(speed: SpeedSpec): string { + if (speed.kind === "joint_percent") return `v${Math.round(speed.value * 100)}`; + if (speed.kind === "linear") return `v${Math.round(speed.velocity * 1000)}`; + return "v100"; +} + +function fanucSpeed(speed: SpeedSpec): string { + if (speed.kind === "joint_percent") return `${Math.round(speed.value * 100)}%`; + if (speed.kind === "linear") return `${Math.round(speed.velocity * 1000)}mm/sec`; + return "100mm/sec"; +} + +function kukaSpeed(speed: SpeedSpec): string { + if (speed.kind === "joint_percent") return `Vel=${Math.round(speed.value * 100)}%`; + if (speed.kind === "linear") return `Vel=${speed.velocity.toFixed(3)}m/s`; + return "Vel=0.100m/s"; +} + +function abbZone(zone: ZoneSpec): string { + if (zone.kind === "fine") return "fine"; + if (zone.kind === "distance") return `z${Math.round(zone.value * 1000)}`; + if (zone.kind === "cnt") return `z${Math.round(zone.value * 100)}`; + return "z10"; +} + +function fanucZone(zone: ZoneSpec): string { + if (zone.kind === "fine") return "FINE"; + if (zone.kind === "cnt") return `CNT${Math.round(zone.value * 100)}`; + if (zone.kind === "distance") return `CNT${Math.max(1, Math.round(zone.value * 1000))}`; + return "CNT10"; +} + +function kukaZone(zone: ZoneSpec): string { + return zone.kind === "fine" ? "" : " C_DIS"; +} + +function fanucIo(raw: string): string { + return raw.replace("io.do", "DO").replace("io.di", "DI").replace("[", "[").replace("]", "]"); +} + +function kukaIo(raw: string): string { + return raw.replace("io.do", "$OUT").replace("io.di", "$IN"); +} + +function formatValue(value: boolean | number | string): string { + if (typeof value === "boolean") return value ? "TRUE" : "FALSE"; + return String(value); +} + +function unsupportedLine(brand: PostBrand, kind: string, report: PostIssue[]): string { + report.push({ + severity: "warning", + code: "GRL_POST_UNSUPPORTED", + message: `${kind} is not supported by ${brand} prototype postprocessor`, + brand + }); + return `! unsupported ${kind}`; +} + +function collectBrandHintIssues(ir: SemanticProgramIr, brand: PostBrand): PostIssue[] { + return ir.symbols + .filter((symbol) => symbol.kind === "raw" && symbol.typeName === "post_hint") + .filter((symbol) => !symbol.name.includes(brand)) + .map((symbol) => ({ + severity: "info" as const, + code: "GRL_POST_HINT_IGNORED", + message: `post_hint ${symbol.name} ignored for ${brand}`, + brand + })); +} + +function filenameFor(moduleName: string, brand: PostBrand): string { + if (brand === "abb") return `${moduleName}.mod`; + if (brand === "fanuc") return `${moduleName}.ls`; + return `${moduleName}.src`; +} diff --git a/kdl-wasm/web/src/grl/semantic/compileControlFlow.ts b/kdl-wasm/web/src/grl/semantic/compileControlFlow.ts new file mode 100644 index 0000000..b0befad --- /dev/null +++ b/kdl-wasm/web/src/grl/semantic/compileControlFlow.ts @@ -0,0 +1,576 @@ +import { KdlStructuredError } from "../../kdl/rpc.js"; +import type { MotionSourceMap } from "../../kdl/types.js"; +import type { GrlProcedureDeclaration } from "../ast/index.js"; +import type { + ControlExpression, + ControlFlowInstruction, + ForInstruction, + IfInstruction, + JumpInstruction, + LabelInstruction, + ProcedureFlowInstruction, + RawProcedureStatement, + SwitchCaseInstruction, + SwitchInstruction, + WhileInstruction +} from "../ir/index.js"; +import type { GrlToken } from "../lexer/index.js"; + +type StopKeyword = "elseif" | "else" | "case" | "default" | "end"; + +interface ParseContext { + scopePath: string[]; + loopDepth: number; + switchDepth: number; +} + +export function parseProcedureControlFlow(procedure: GrlProcedureDeclaration): ProcedureFlowInstruction[] { + return parseControlFlowStatements(procedure.bodyTokens); +} + +export function parseControlFlowStatements(tokens: GrlToken[]): ProcedureFlowInstruction[] { + const parser = new ControlFlowParser(tokens); + const flow = parser.parseRoot(); + validateJumps(flow); + return flow; +} + +class ControlFlowParser { + private current = 0; + + constructor(private readonly tokens: GrlToken[]) {} + + parseRoot(): ProcedureFlowInstruction[] { + return this.parseBlock(new Set(), { + scopePath: [], + loopDepth: 0, + switchDepth: 0 + }); + } + + private parseBlock(stopKeywords: Set, context: ParseContext): ProcedureFlowInstruction[] { + const instructions: ProcedureFlowInstruction[] = []; + while (!this.isAtEnd()) { + if (this.isStopKeyword(stopKeywords)) { + break; + } + + if (this.matchKeyword("if")) { + instructions.push(this.finishIf(this.previous(), context)); + continue; + } + if (this.matchKeyword("while")) { + instructions.push(this.finishWhile(this.previous(), context)); + continue; + } + if (this.matchKeyword("for")) { + instructions.push(this.finishFor(this.previous(), context)); + continue; + } + if (this.matchKeyword("switch")) { + instructions.push(this.finishSwitch(this.previous(), context)); + continue; + } + if (this.matchKeyword("break")) { + instructions.push(this.finishBreak(this.previous(), context)); + continue; + } + if (this.matchKeyword("continue")) { + instructions.push(this.finishContinue(this.previous(), context)); + continue; + } + if (this.matchKeyword("label")) { + instructions.push(this.finishLabel(this.previous(), context)); + continue; + } + if (this.matchKeyword("jump")) { + instructions.push(this.finishJump(this.previous(), context)); + continue; + } + if (this.checkKeyword("end")) { + throw controlError("GRL_CONTROL_UNEXPECTED_END", "Unexpected end in procedure body", this.peek()); + } + + instructions.push(this.finishRawStatement()); + } + return instructions; + } + + private finishIf(start: GrlToken, context: ParseContext): IfInstruction { + const condition = this.parseBooleanLineExpression(start); + const branches: IfInstruction["branches"] = [ + { + branchKind: "if", + condition, + body: this.parseBlock(new Set(["elseif", "else", "end"]), { + ...context, + scopePath: [...context.scopePath, scopeId(start, "if")] + }), + sourceMap: tokenSourceMap(start) + } + ]; + + while (this.matchKeyword("elseif")) { + const branchStart = this.previous(); + const branchCondition = this.parseBooleanLineExpression(branchStart); + branches.push({ + branchKind: "elseif", + condition: branchCondition, + body: this.parseBlock(new Set(["elseif", "else", "end"]), { + ...context, + scopePath: [...context.scopePath, scopeId(branchStart, `elseif${branches.length}`)] + }), + sourceMap: tokenSourceMap(branchStart) + }); + } + + if (this.matchKeyword("else")) { + const branchStart = this.previous(); + branches.push({ + branchKind: "else", + body: this.parseBlock(new Set(["end"]), { + ...context, + scopePath: [...context.scopePath, scopeId(branchStart, "else")] + }), + sourceMap: tokenSourceMap(branchStart) + }); + } + + this.consumeKeyword("end", "Expected end after if block"); + return { + kind: "IF", + branches, + sourceMap: tokenSourceMap(start) + }; + } + + private finishWhile(start: GrlToken, context: ParseContext): WhileInstruction { + const condition = this.parseBooleanLineExpression(start); + const body = this.parseBlock(new Set(["end"]), { + scopePath: [...context.scopePath, scopeId(start, "while")], + loopDepth: context.loopDepth + 1, + switchDepth: context.switchDepth + }); + this.consumeKeyword("end", "Expected end after while block"); + return { + kind: "WHILE", + condition, + body, + sourceMap: tokenSourceMap(start) + }; + } + + private finishFor(start: GrlToken, context: ParseContext): ForInstruction { + const iterator = this.consumeIdentifier("Expected loop variable after for"); + if (!this.matchOperator("=") && !this.matchOperator(":=")) { + throw controlError("GRL_FOR_ASSIGNMENT_EXPECTED", "Expected = after for loop variable", this.peek()); + } + + const from = this.parseLineExpressionUntil(start, ["to"]); + this.consumeKeyword("to", "Expected to in for loop"); + const to = this.parseLineExpressionUntil(start, ["step"]); + const step = this.matchKeyword("step") ? this.parseLineExpressionUntil(start, []) : undefined; + const body = this.parseBlock(new Set(["end"]), { + scopePath: [...context.scopePath, scopeId(start, "for")], + loopDepth: context.loopDepth + 1, + switchDepth: context.switchDepth + }); + this.consumeKeyword("end", "Expected end after for block"); + + return { + kind: "FOR", + iterator: iterator.raw, + from, + to, + ...(step ? { step } : {}), + body, + sourceMap: tokenSourceMap(start) + }; + } + + private finishSwitch(start: GrlToken, context: ParseContext): SwitchInstruction { + const expression = this.parseLineExpressionUntil(start, []); + const cases: SwitchCaseInstruction[] = []; + const seenCases = new Set(); + let seenDefault = false; + + while (!this.isAtEnd() && !this.checkKeyword("end")) { + if (this.matchKeyword("case")) { + const caseStart = this.previous(); + const valueTokens = this.collectLineExpressionTokens(caseStart, []); + if (valueTokens.length === 0) { + throw controlError("GRL_SWITCH_CASE_VALUE_MISSING", "Expected case value", caseStart); + } + const constant = parseCaseConstant(valueTokens); + const key = `${typeof constant.value}:${String(constant.value)}`; + if (seenCases.has(key)) { + throw controlError("GRL_SWITCH_CASE_DUPLICATE", `Duplicate switch case ${constant.raw}`, caseStart); + } + seenCases.add(key); + cases.push({ + caseKind: "case", + value: constant.value, + raw: constant.raw, + body: this.parseBlock(new Set(["case", "default", "end"]), { + scopePath: [...context.scopePath, scopeId(caseStart, `case:${constant.raw}`)], + loopDepth: context.loopDepth, + switchDepth: context.switchDepth + 1 + }), + sourceMap: tokenSourceMap(caseStart) + }); + continue; + } + + if (this.matchKeyword("default")) { + const defaultStart = this.previous(); + if (seenDefault) { + throw controlError("GRL_SWITCH_DEFAULT_DUPLICATE", "Duplicate switch default case", defaultStart); + } + seenDefault = true; + cases.push({ + caseKind: "default", + body: this.parseBlock(new Set(["case", "default", "end"]), { + scopePath: [...context.scopePath, scopeId(defaultStart, "default")], + loopDepth: context.loopDepth, + switchDepth: context.switchDepth + 1 + }), + sourceMap: tokenSourceMap(defaultStart) + }); + continue; + } + + throw controlError("GRL_SWITCH_CASE_EXPECTED", "Expected case, default, or end in switch", this.peek()); + } + + this.consumeKeyword("end", "Expected end after switch block"); + return { + kind: "SWITCH", + expression, + cases, + sourceMap: tokenSourceMap(start) + }; + } + + private finishBreak(start: GrlToken, context: ParseContext): ControlFlowInstruction { + if (context.loopDepth === 0 && context.switchDepth === 0) { + throw controlError("GRL_BREAK_OUTSIDE_FLOW", "break is only valid inside loop or switch", start); + } + return { + kind: "BREAK", + sourceMap: tokenSourceMap(start) + }; + } + + private finishContinue(start: GrlToken, context: ParseContext): ControlFlowInstruction { + if (context.loopDepth === 0) { + throw controlError("GRL_CONTINUE_OUTSIDE_LOOP", "continue is only valid inside loop", start); + } + return { + kind: "CONTINUE", + sourceMap: tokenSourceMap(start) + }; + } + + private finishLabel(start: GrlToken, context: ParseContext): LabelInstruction { + const label = this.consumeIdentifier("Expected label name"); + return { + kind: "LABEL", + name: label.raw, + scopePath: [...context.scopePath], + sourceMap: tokenSourceMap(start) + }; + } + + private finishJump(start: GrlToken, context: ParseContext): JumpInstruction { + const label = this.consumeIdentifier("Expected label name after jump"); + return { + kind: "JUMP", + label: label.raw, + scopePath: [...context.scopePath], + sourceMap: tokenSourceMap(start) + }; + } + + private finishRawStatement(): RawProcedureStatement { + const start = this.peek(); + const tokens = this.collectLineExpressionTokens(start, []); + if (tokens.length === 0) { + const token = this.advance(); + return { + kind: "RAW_STATEMENT", + text: token.raw, + tokens: [token], + sourceMap: tokenSourceMap(token) + }; + } + return { + kind: "RAW_STATEMENT", + text: stringifyTokens(tokens), + tokens, + sourceMap: tokenSourceMap(start) + }; + } + + private parseBooleanLineExpression(start: GrlToken): ControlExpression { + const expression = this.parseLineExpressionUntil(start, []); + if (!isBooleanCondition(expression.tokens as GrlToken[])) { + throw controlError("GRL_CONTROL_CONDITION_NOT_BOOL", "Control condition must be boolean", start); + } + return expression; + } + + private parseLineExpressionUntil(start: GrlToken, stopKeywords: string[]): ControlExpression { + const tokens = this.collectLineExpressionTokens(start, stopKeywords); + if (tokens.length === 0) { + throw controlError("GRL_CONTROL_EXPRESSION_MISSING", "Expected control expression", start); + } + return { + text: stringifyTokens(tokens), + tokens, + sourceMap: tokenSourceMap(tokens[0]!) + }; + } + + private collectLineExpressionTokens(start: GrlToken, stopKeywords: string[]): GrlToken[] { + const tokens: GrlToken[] = []; + let parenDepth = 0; + let bracketDepth = 0; + let braceDepth = 0; + + while (!this.isAtEnd()) { + const token = this.peek(); + if (token.range.start.line !== start.range.start.line) { + break; + } + if ( + parenDepth === 0 && + bracketDepth === 0 && + braceDepth === 0 && + (token.kind === "keyword" || token.kind === "identifier") && + stopKeywords.includes(token.raw) + ) { + break; + } + + const consumed = this.advance(); + tokens.push(consumed); + if (consumed.kind === "punctuation") { + if (consumed.raw === "(") parenDepth += 1; + if (consumed.raw === ")") parenDepth = Math.max(0, parenDepth - 1); + if (consumed.raw === "[") bracketDepth += 1; + if (consumed.raw === "]") bracketDepth = Math.max(0, bracketDepth - 1); + if (consumed.raw === "{") braceDepth += 1; + if (consumed.raw === "}") braceDepth = Math.max(0, braceDepth - 1); + } + } + return tokens; + } + + private isStopKeyword(stopKeywords: Set): boolean { + if (stopKeywords.size === 0) { + return false; + } + const token = this.peek(); + return (token.kind === "keyword" || token.kind === "identifier") && stopKeywords.has(token.raw as StopKeyword); + } + + private matchKeyword(keyword: string): boolean { + if (this.checkKeyword(keyword)) { + this.advance(); + return true; + } + return false; + } + + private checkKeyword(keyword: string): boolean { + const token = this.peek(); + return (token.kind === "keyword" || token.kind === "identifier") && token.raw === keyword; + } + + private consumeKeyword(keyword: string, message: string): GrlToken { + if (this.checkKeyword(keyword)) { + return this.advance(); + } + throw controlError("GRL_KEYWORD_EXPECTED", message, this.peek()); + } + + private consumeIdentifier(message: string): GrlToken { + const token = this.peek(); + if (token.kind === "identifier" || token.kind === "keyword") { + return this.advance(); + } + throw controlError("GRL_IDENTIFIER_EXPECTED", message, token); + } + + private matchOperator(operator: string): boolean { + const token = this.peek(); + if (token.kind === "operator" && token.raw === operator) { + this.advance(); + return true; + } + return false; + } + + private advance(): GrlToken { + this.current += 1; + return this.previous(); + } + + private previous(): GrlToken { + return this.tokens[this.current - 1]!; + } + + private peek(): GrlToken { + return this.tokens[this.current]!; + } + + private isAtEnd(): boolean { + return this.current >= this.tokens.length; + } +} + +function validateJumps(flow: ProcedureFlowInstruction[]): void { + const labels = new Map(); + const jumps: JumpInstruction[] = []; + + visitFlow(flow, (instruction) => { + if (instruction.kind === "LABEL") { + if (labels.has(instruction.name)) { + throw controlError("GRL_LABEL_DUPLICATE", `Duplicate label ${instruction.name}`, undefined, instruction.sourceMap); + } + labels.set(instruction.name, instruction); + } else if (instruction.kind === "JUMP") { + jumps.push(instruction); + } + }); + + for (const jump of jumps) { + const label = labels.get(jump.label); + if (!label) { + throw controlError("GRL_LABEL_NOT_FOUND", `Unknown label ${jump.label}`, undefined, jump.sourceMap); + } + if (!isPrefix(label.scopePath, jump.scopePath)) { + throw controlError( + "GRL_JUMP_INTO_BLOCK", + `jump ${jump.label} cannot enter a nested or sibling block`, + undefined, + jump.sourceMap + ); + } + } +} + +function visitFlow(flow: ProcedureFlowInstruction[], visit: (instruction: ProcedureFlowInstruction) => void): void { + for (const instruction of flow) { + visit(instruction); + if (instruction.kind === "IF") { + for (const branch of instruction.branches) { + visitFlow(branch.body, visit); + } + } else if (instruction.kind === "WHILE" || instruction.kind === "FOR") { + visitFlow(instruction.body, visit); + } else if (instruction.kind === "SWITCH") { + for (const switchCase of instruction.cases) { + visitFlow(switchCase.body, visit); + } + } + } +} + +function isPrefix(prefix: string[], value: string[]): boolean { + return prefix.length <= value.length && prefix.every((part, index) => value[index] === part); +} + +function isBooleanCondition(tokens: GrlToken[]): boolean { + if (tokens.length === 0) { + return false; + } + if (tokens.length === 1) { + const [token] = tokens; + if (!token) return false; + if (token.kind === "number" || token.kind === "string") { + return false; + } + return token.kind === "identifier" || token.kind === "keyword"; + } + if (tokens.length === 2 && tokens[0]?.raw === "-" && tokens[1]?.kind === "number") { + return false; + } + if (tokens.some((token) => token.kind === "operator" && ["==", "!=", "<", ">", "<=", ">=", "&&", "||", "!"].includes(token.raw))) { + return true; + } + if (tokens.some((token) => token.kind === "keyword" && (token.raw === "true" || token.raw === "false"))) { + return true; + } + const first = tokens[0]; + return Boolean( + first && + (first.kind === "keyword" || first.kind === "identifier") && + ["all", "any", "rising", "falling", "changed"].includes(first.raw) + ); +} + +function parseCaseConstant(tokens: GrlToken[]): { value: string | number | boolean; raw: string } { + if (tokens.length === 2 && tokens[0]?.kind === "operator" && tokens[0].raw === "-" && tokens[1]?.kind === "number") { + const value = -(tokens[1].unit?.normalizedValue ?? tokens[1].value); + return { value, raw: stringifyTokens(tokens) }; + } + + if (tokens.length !== 1) { + throw controlError("GRL_SWITCH_CASE_NOT_CONSTANT", "switch case must be a constant expression", tokens[0]); + } + + const token = tokens[0]!; + if (token.kind === "number") { + return { value: token.unit?.normalizedValue ?? token.value, raw: token.raw }; + } + if (token.kind === "string") { + return { value: token.value, raw: token.raw }; + } + if (token.kind === "keyword" && (token.raw === "true" || token.raw === "false")) { + return { value: token.raw === "true", raw: token.raw }; + } + if (token.kind === "identifier" || token.kind === "keyword") { + return { value: token.raw, raw: token.raw }; + } + + throw controlError("GRL_SWITCH_CASE_NOT_CONSTANT", "switch case must be a constant expression", token); +} + +function stringifyTokens(tokens: GrlToken[]): string { + return tokens.map((token) => token.raw).join(" "); +} + +function scopeId(token: GrlToken, kind: string): string { + return `${kind}@${token.range.start.line}:${token.range.start.column}`; +} + +function tokenSourceMap(token: GrlToken): MotionSourceMap { + return { + line: token.range.start.line, + column: token.range.start.column + }; +} + +function controlError( + code: string, + message: string, + token?: GrlToken, + sourceMap?: MotionSourceMap +): KdlStructuredError { + const map = sourceMap ?? (token ? tokenSourceMap(token) : undefined); + return new KdlStructuredError( + code, + message, + map + ? [ + { + severity: "error", + code, + message, + sourceMap: map + } + ] + : undefined + ); +} diff --git a/kdl-wasm/web/src/grl/semantic/compileData.ts b/kdl-wasm/web/src/grl/semantic/compileData.ts new file mode 100644 index 0000000..4a684d9 --- /dev/null +++ b/kdl-wasm/web/src/grl/semantic/compileData.ts @@ -0,0 +1,296 @@ +import { rpyToQuaternion } from "../../math/poseMath.js"; +import type { JointTarget, OffsetSpec, Pose, PoseTarget, SpeedSpec, ZoneSpec } from "../../kdl/types.js"; +import { KdlStructuredError } from "../../kdl/rpc.js"; +import type { + GrlArrayExpression, + GrlCallExpression, + GrlDataDeclaration, + GrlExpression, + GrlNumberLiteral, + GrlObjectExpression, + GrlOffsetExpression, + GrlTargetDeclaration +} from "../ast/index.js"; + +export type CompiledGrlDataValue = + | Pose + | JointTarget + | PoseTarget + | SpeedSpec + | ZoneSpec + | OffsetSpec + | Record + | string + | number + | boolean + | number[]; + +export interface CompiledGrlDataDeclaration { + name: string; + storage: GrlDataDeclaration["storage"]; + typeName: string; + value: CompiledGrlDataValue; +} + +export interface CompiledGrlTargetDeclaration { + name: string; + target: JointTarget | PoseTarget; +} + +export function compileGrlDataDeclaration(declaration: GrlDataDeclaration): CompiledGrlDataDeclaration { + return { + name: declaration.name, + storage: declaration.storage, + typeName: declaration.typeName, + value: compileByType(declaration.typeName, declaration.initializer) + }; +} + +export function compileGrlTargetDeclaration(declaration: GrlTargetDeclaration): CompiledGrlTargetDeclaration { + const target = compileTargetExpression(declaration.target); + return { + name: declaration.name, + target: { + ...target, + id: target.id ?? declaration.name + } + }; +} + +export function compileTargetExpression(expression: GrlExpression): JointTarget | PoseTarget { + if (isObjectExpression(expression, "joint_target")) { + return { + joints: compileNumberArray(requiredProperty(expression, "joints")) + }; + } + + if (isObjectExpression(expression, "pose_target")) { + const pose = compilePoseExpression(requiredProperty(expression, "pose")); + const configExpression = findProperty(expression, "config"); + const config = configExpression ? compileRobotConfig(configExpression) : undefined; + return { + pose, + ...(config ? { config } : {}), + ...(findIdentifierName(expression, "tool") ? { tool: { id: findIdentifierName(expression, "tool") } as unknown as Pose } : {}), + ...(findIdentifierName(expression, "frame") ? { frame: { id: findIdentifierName(expression, "frame") } as unknown as Pose } : {}) + }; + } + + throw compileError("GRL_UNSUPPORTED_TARGET", "Expected joint_target or pose_target expression"); +} + +export function compileSpeedExpression(expression: GrlExpression): SpeedSpec { + if (!isCallExpression(expression)) { + throw compileError("GRL_INVALID_SPEED", "Speed expression must be a call"); + } + + const first = expression.args[0]; + if (!first || first.kind !== "NumberLiteral") { + throw compileError("GRL_INVALID_SPEED", "Speed expression requires a numeric value"); + } + + if (expression.callee === "joint") { + if (first.unit?.kind === "percent") { + return { kind: "joint_percent", value: first.unit.normalizedValue }; + } + return { kind: "joint_abs", velocity: normalizedNumber(first) }; + } + + if (expression.callee === "linear") { + return { kind: "linear", velocity: normalizedNumber(first), ...compileAcceleration(expression) }; + } + + if (expression.callee === "angular") { + return { kind: "linear", velocity: 0, angularVelocity: normalizedNumber(first), ...compileAcceleration(expression) }; + } + + throw compileError("GRL_INVALID_SPEED", `Unsupported speed expression: ${expression.callee}`); +} + +export function compileZoneExpression(expression: GrlExpression): ZoneSpec { + if (expression.kind === "IdentifierExpression") { + if (expression.name === "fine") { + return { kind: "fine" }; + } + if (expression.name === "continuous") { + return { kind: "continuous" }; + } + } + + if (isCallExpression(expression) && expression.callee === "z") { + const first = expression.args[0]; + if (!first || first.kind !== "NumberLiteral") { + throw compileError("GRL_INVALID_ZONE", "z(...) requires a distance"); + } + return { kind: "distance", value: normalizedNumber(first) }; + } + + if (isCallExpression(expression) && expression.callee === "cnt") { + const first = expression.args[0]; + if (!first || first.kind !== "NumberLiteral") { + throw compileError("GRL_INVALID_ZONE", "cnt(...) requires a percent value"); + } + return { kind: "cnt", value: normalizedNumber(first) }; + } + + throw compileError("GRL_INVALID_ZONE", "Unsupported zone expression"); +} + +export function compileOffsetExpression(expression: GrlOffsetExpression): OffsetSpec { + const xyz: [number, number, number] = [0, 0, 0]; + for (const axis of expression.axes) { + const index = axis.axis === "x" ? 0 : axis.axis === "y" ? 1 : 2; + xyz[index] = normalizedNumber(axis.value); + } + + return { + mode: expression.mode, + ...(expression.frameName ? { frameId: expression.frameName } : {}), + xyz + }; +} + +function compileByType(typeName: string, expression: GrlExpression): CompiledGrlDataValue { + if (typeName === "speed") { + return compileSpeedExpression(expression); + } + if (typeName === "zone") { + return compileZoneExpression(expression); + } + if (typeName === "pose") { + return compilePoseExpression(expression); + } + if (typeName === "pose_target" || typeName === "joint_target") { + return compileTargetExpression(expression); + } + if (expression.kind === "OffsetExpression") { + return compileOffsetExpression(expression); + } + if (typeName === "tool" && isObjectExpression(expression, "tool")) { + return { + tcp: compilePoseExpression(requiredProperty(expression, "tcp")), + ...(findProperty(expression, "mass") ? { mass: normalizedNumber(findProperty(expression, "mass") as GrlNumberLiteral) } : {}), + ...(findProperty(expression, "cog") ? { cog: compileNumberArray(findProperty(expression, "cog")!) } : {}) + }; + } + if (typeName === "frame" && isObjectExpression(expression, "frame")) { + return { + origin: compilePoseExpression(requiredProperty(expression, "origin")) + }; + } + if (expression.kind === "NumberLiteral") { + return normalizedNumber(expression); + } + if (expression.kind === "StringLiteral" || expression.kind === "BooleanLiteral") { + return expression.value; + } + if (expression.kind === "ArrayExpression") { + return compileNumberArray(expression); + } + + return { kind: expression.kind }; +} + +function compilePoseExpression(expression: GrlExpression): Pose { + if (!isCallExpression(expression) || (expression.callee !== "pose" && expression.callee !== "poseq")) { + throw compileError("GRL_INVALID_POSE", "Expected pose(...) or poseq(...) expression"); + } + + const values = expression.args.map((arg) => { + if (arg.kind !== "NumberLiteral") { + throw compileError("GRL_INVALID_POSE", "Pose arguments must be numeric"); + } + return normalizedNumber(arg); + }); + + if (expression.callee === "pose") { + if (values.length !== 6) { + throw compileError("GRL_INVALID_POSE", "pose(...) requires 6 arguments"); + } + return { + position: [values[0]!, values[1]!, values[2]!], + quaternion: rpyToQuaternion([values[3]!, values[4]!, values[5]!]) + }; + } + + if (values.length !== 7) { + throw compileError("GRL_INVALID_POSE", "poseq(...) requires 7 arguments"); + } + return { + position: [values[0]!, values[1]!, values[2]!], + quaternion: [values[3]!, values[4]!, values[5]!, values[6]!] + }; +} + +function compileRobotConfig(expression: GrlExpression) { + if (!isCallExpression(expression) || expression.callee !== "robot_config") { + throw compileError("GRL_INVALID_CONFIG", "Expected robot_config(...)"); + } + const values = expression.args.map((arg) => { + if (arg.kind !== "NumberLiteral") { + throw compileError("GRL_INVALID_CONFIG", "robot_config arguments must be numeric"); + } + return arg.value as -1 | 0 | 1; + }); + return { + ...(values[0] !== undefined ? { shoulder: values[0] } : {}), + ...(values[1] !== undefined ? { elbow: values[1] } : {}), + ...(values[2] !== undefined ? { wrist: values[2] } : {}) + }; +} + +function compileNumberArray(expression: GrlExpression): number[] { + if (expression.kind !== "ArrayExpression") { + throw compileError("GRL_INVALID_ARRAY", "Expected numeric array"); + } + return expression.elements.map((element) => { + if (element.kind !== "NumberLiteral") { + throw compileError("GRL_INVALID_ARRAY", "Array elements must be numeric"); + } + return normalizedNumber(element); + }); +} + +function compileAcceleration(expression: GrlCallExpression): { acceleration?: number } { + for (let index = 1; index < expression.args.length; index += 1) { + const marker = expression.args[index]; + const value = expression.args[index + 1]; + if (marker?.kind === "IdentifierExpression" && marker.name === "acc" && value?.kind === "NumberLiteral") { + return { acceleration: normalizedNumber(value) }; + } + } + return {}; +} + +function normalizedNumber(expression: GrlNumberLiteral): number { + return expression.unit?.normalizedValue ?? expression.value; +} + +function requiredProperty(expression: GrlObjectExpression, key: string): GrlExpression { + const property = findProperty(expression, key); + if (!property) { + throw compileError("GRL_MISSING_PROPERTY", `${expression.typeName} is missing ${key}`); + } + return property; +} + +function findProperty(expression: GrlObjectExpression, key: string): GrlExpression | undefined { + return expression.properties.find((property) => property.key === key)?.value; +} + +function findIdentifierName(expression: GrlObjectExpression, key: string): string | undefined { + const value = findProperty(expression, key); + return value?.kind === "IdentifierExpression" ? value.name : undefined; +} + +function isObjectExpression(expression: GrlExpression, typeName: string): expression is GrlObjectExpression { + return expression.kind === "ObjectExpression" && expression.typeName === typeName; +} + +function isCallExpression(expression: GrlExpression): expression is GrlCallExpression { + return expression.kind === "CallExpression"; +} + +function compileError(code: string, message: string): KdlStructuredError { + return new KdlStructuredError(code, message); +} diff --git a/kdl-wasm/web/src/grl/semantic/compileException.ts b/kdl-wasm/web/src/grl/semantic/compileException.ts new file mode 100644 index 0000000..bed4c3f --- /dev/null +++ b/kdl-wasm/web/src/grl/semantic/compileException.ts @@ -0,0 +1,321 @@ +import { KdlStructuredError } from "../../kdl/rpc.js"; +import type { MotionDiagnostic, MotionSourceMap } from "../../kdl/types.js"; +import type { GrlProcedureDeclaration, GrlRawTopLevelDeclaration, GrlTopLevelDeclaration } from "../ast/index.js"; +import type { + AlarmInstruction, + ExceptionFlowInstruction, + RaiseInstruction, + RawProcedureStatement, + TryInstruction, + UnsupportedRuntimeInstruction +} from "../ir/index.js"; +import type { GrlToken } from "../lexer/index.js"; + +type StopKeyword = "catch" | "finally" | "end"; + +export interface ExceptionAnalysis { + procedures: Record; + unsupported: UnsupportedRuntimeInstruction[]; + diagnostics: MotionDiagnostic[]; +} + +export function analyzeExceptionSemantics(declarations: GrlTopLevelDeclaration[]): ExceptionAnalysis { + const diagnostics: MotionDiagnostic[] = []; + const procedures: Record = {}; + const unsupported: UnsupportedRuntimeInstruction[] = []; + + for (const declaration of declarations) { + if (declaration.kind === "ProcedureDeclaration") { + procedures[declaration.name] = parseProcedureExceptionFlow(declaration); + continue; + } + if (declaration.kind === "RawTopLevelDeclaration") { + const instruction = compileUnsupportedTopLevel(declaration); + if (instruction) { + unsupported.push(instruction); + diagnostics.push(diagnostic("warning", "GRL_P1_UNIMPLEMENTED", instruction.message, instruction.sourceMap)); + } + } + } + + return { procedures, unsupported, diagnostics }; +} + +export function parseProcedureExceptionFlow(procedure: GrlProcedureDeclaration): ExceptionFlowInstruction[] { + return parseExceptionFlowStatements(procedure.bodyTokens); +} + +export function parseExceptionFlowStatements(tokens: GrlToken[]): ExceptionFlowInstruction[] { + return new ExceptionFlowParser(tokens).parseRoot(); +} + +class ExceptionFlowParser { + private current = 0; + + constructor(private readonly tokens: GrlToken[]) {} + + parseRoot(): ExceptionFlowInstruction[] { + return this.parseBlock(new Set()); + } + + private parseBlock(stopKeywords: Set): ExceptionFlowInstruction[] { + const instructions: ExceptionFlowInstruction[] = []; + while (!this.isAtEnd()) { + if (this.isStopKeyword(stopKeywords)) { + break; + } + if (this.matchKeyword("alarm")) { + instructions.push(this.finishAlarm(this.previous())); + continue; + } + if (this.matchKeyword("raise")) { + instructions.push(this.finishRaise(this.previous())); + continue; + } + if (this.matchKeyword("try")) { + instructions.push(this.finishTry(this.previous())); + continue; + } + if (this.matchKeyword("enable") || this.matchKeyword("disable")) { + instructions.push(this.finishUnsupportedInterrupt(this.previous())); + continue; + } + instructions.push(this.finishRawStatement()); + } + return instructions; + } + + private finishAlarm(start: GrlToken): AlarmInstruction { + const tokens = this.collectLineTokens(start); + const id = tokens[0]; + if (!id || !isIdentifierLike(id)) { + throw exceptionError("GRL_ALARM_ID_MISSING", "alarm requires an alarm id", tokenSourceMap(start)); + } + const message = tokens.find((token) => token.kind === "string"); + const severityIndex = tokens.findIndex((token) => token.raw === "severity"); + const severity = severityIndex >= 0 ? tokens[severityIndex + 1] : undefined; + return { + kind: "ALARM", + alarmId: id.raw, + ...(message?.kind === "string" ? { message: message.value } : {}), + ...(severity && isIdentifierLike(severity) ? { severity: severity.raw } : {}), + sourceMap: tokenSourceMap(start) + }; + } + + private finishRaise(start: GrlToken): RaiseInstruction { + const tokens = this.collectLineTokens(start); + const id = tokens[0]; + if (!id || !isIdentifierLike(id)) { + throw exceptionError("GRL_RAISE_ID_MISSING", "raise requires an alarm id", tokenSourceMap(start)); + } + return { + kind: "RAISE", + alarmId: id.raw, + sourceMap: tokenSourceMap(start) + }; + } + + private finishTry(start: GrlToken): TryInstruction { + const body = this.parseBlock(new Set(["catch", "finally", "end"])); + const catches: TryInstruction["catches"] = []; + let finallyBlock: TryInstruction["finally"]; + + while (this.matchKeyword("catch")) { + const catchStart = this.previous(); + const header = this.collectLineTokens(catchStart); + const alarmId = header[0] && isIdentifierLike(header[0]) ? header[0].raw : undefined; + catches.push({ + ...(alarmId ? { alarmId } : {}), + body: this.parseBlock(new Set(["catch", "finally", "end"])), + sourceMap: tokenSourceMap(catchStart) + }); + } + + if (this.matchKeyword("finally")) { + const finallyStart = this.previous(); + finallyBlock = { + body: this.parseBlock(new Set(["end"])), + sourceMap: tokenSourceMap(finallyStart) + }; + } + + this.consumeKeyword("end", "Expected end after try block"); + if (catches.length === 0 && !finallyBlock) { + throw exceptionError("GRL_TRY_HANDLER_MISSING", "try requires catch or finally", tokenSourceMap(start)); + } + + return { + kind: "TRY", + body, + catches, + ...(finallyBlock ? { finally: finallyBlock } : {}), + sourceMap: tokenSourceMap(start) + }; + } + + private finishUnsupportedInterrupt(start: GrlToken): UnsupportedRuntimeInstruction { + const tokens = this.collectLineTokens(start); + const hasInterrupt = tokens.some((token) => token.raw === "interrupt"); + return { + kind: "UNSUPPORTED_RUNTIME", + feature: "interrupt", + message: hasInterrupt + ? `${start.raw} interrupt is parsed but not executable in P0` + : `${start.raw} is parsed but not executable in P0`, + sourceMap: tokenSourceMap(start) + }; + } + + private finishRawStatement(): RawProcedureStatement { + const start = this.peek(); + const tokens = this.collectLineTokens(start); + if (tokens.length === 0) { + const token = this.advance(); + return { + kind: "RAW_STATEMENT", + text: token.raw, + tokens: [token], + sourceMap: tokenSourceMap(token) + }; + } + return { + kind: "RAW_STATEMENT", + text: stringifyTokens(tokens), + tokens, + sourceMap: tokenSourceMap(start) + }; + } + + private collectLineTokens(start: GrlToken): GrlToken[] { + const tokens: GrlToken[] = []; + let parenDepth = 0; + let bracketDepth = 0; + let braceDepth = 0; + while (!this.isAtEnd()) { + const token = this.peek(); + if (token.range.start.line !== start.range.start.line) { + break; + } + const consumed = this.advance(); + tokens.push(consumed); + if (consumed.kind === "punctuation") { + if (consumed.raw === "(") parenDepth += 1; + if (consumed.raw === ")") parenDepth = Math.max(0, parenDepth - 1); + if (consumed.raw === "[") bracketDepth += 1; + if (consumed.raw === "]") bracketDepth = Math.max(0, bracketDepth - 1); + if (consumed.raw === "{") braceDepth += 1; + if (consumed.raw === "}") braceDepth = Math.max(0, braceDepth - 1); + } + if (parenDepth === 0 && bracketDepth === 0 && braceDepth === 0) { + continue; + } + } + return tokens; + } + + private isStopKeyword(stopKeywords: Set): boolean { + if (stopKeywords.size === 0) { + return false; + } + const token = this.peek(); + return (token.kind === "keyword" || token.kind === "identifier") && stopKeywords.has(token.raw as StopKeyword); + } + + private matchKeyword(keyword: string): boolean { + if (this.checkKeyword(keyword)) { + this.advance(); + return true; + } + return false; + } + + private checkKeyword(keyword: string): boolean { + const token = this.peek(); + return (token.kind === "keyword" || token.kind === "identifier") && token.raw === keyword; + } + + private consumeKeyword(keyword: string, message: string): GrlToken { + if (this.checkKeyword(keyword)) { + return this.advance(); + } + throw exceptionError("GRL_KEYWORD_EXPECTED", message, tokenSourceMap(this.peek())); + } + + private advance(): GrlToken { + this.current += 1; + return this.previous(); + } + + private previous(): GrlToken { + return this.tokens[this.current - 1]!; + } + + private peek(): GrlToken { + return this.tokens[this.current]!; + } + + private isAtEnd(): boolean { + return this.current >= this.tokens.length; + } +} + +function compileUnsupportedTopLevel(declaration: GrlRawTopLevelDeclaration): UnsupportedRuntimeInstruction | undefined { + if (declaration.declarationType === "trap") { + return { + kind: "UNSUPPORTED_RUNTIME", + feature: "trap", + message: `trap ${declaration.tokens[1]?.raw ?? ""}`.trim() + " is parsed but not executable in P0", + sourceMap: rangeSourceMap(declaration.range.start) + }; + } + if (declaration.declarationType === "task") { + return { + kind: "UNSUPPORTED_RUNTIME", + feature: "task", + message: `task ${declaration.tokens[1]?.raw ?? ""}`.trim() + " is parsed but not executable in P0", + sourceMap: rangeSourceMap(declaration.range.start) + }; + } + return undefined; +} + +function isIdentifierLike(token: GrlToken): boolean { + return token.kind === "identifier" || token.kind === "keyword"; +} + +function stringifyTokens(tokens: GrlToken[]): string { + return tokens.map((token) => token.raw).join(" "); +} + +function tokenSourceMap(token: GrlToken): MotionSourceMap { + return { + line: token.range.start.line, + column: token.range.start.column + }; +} + +function rangeSourceMap(position: { line: number; column: number }): MotionSourceMap { + return { + line: position.line, + column: position.column + }; +} + +function diagnostic( + severity: MotionDiagnostic["severity"], + code: string, + message: string, + sourceMap?: MotionSourceMap +): MotionDiagnostic { + return { + severity, + code, + message, + ...(sourceMap ? { sourceMap } : {}) + }; +} + +function exceptionError(code: string, message: string, sourceMap: MotionSourceMap): KdlStructuredError { + return new KdlStructuredError(code, message, [diagnostic("error", code, message, sourceMap)]); +} diff --git a/kdl-wasm/web/src/grl/semantic/compileIo.ts b/kdl-wasm/web/src/grl/semantic/compileIo.ts new file mode 100644 index 0000000..05439da --- /dev/null +++ b/kdl-wasm/web/src/grl/semantic/compileIo.ts @@ -0,0 +1,380 @@ +import { KdlStructuredError } from "../../kdl/rpc.js"; +import type { MotionSourceMap } from "../../kdl/types.js"; +import type { + IoFlowInstruction, + IoReference, + IoWriteInstruction, + OperationActionInstruction, + PathEventInstruction, + PulseInstruction, + WaitInstruction +} from "../ir/index.js"; +import { lexGrl, type GrlToken } from "../lexer/index.js"; + +export interface IoMap { + aliases?: Record; + allowedRanges?: Partial>; +} + +export function parseIoFlowStatements(tokens: GrlToken[], ioMap: IoMap = {}): IoFlowInstruction[] { + const parser = new IoStatementParser(tokens, ioMap); + return parser.parseAll(); +} + +export function compilePathEventIo(event: PathEventInstruction, ioMap: IoMap = {}): IoFlowInstruction[] { + const tokens = event.data?.tokens; + if (Array.isArray(tokens)) { + return parseIoFlowStatements(tokens as GrlToken[], ioMap); + } + const statement = event.data?.statement; + if (typeof statement !== "string") { + return []; + } + return compileStatementString(statement, event.sourceMap, ioMap); +} + +export function compileOperationActionIo( + action: OperationActionInstruction, + ioMap: IoMap = {} +): IoFlowInstruction[] { + if (Array.isArray(action.tokens)) { + return parseIoFlowStatements(action.tokens as GrlToken[], ioMap); + } + return compileStatementString(action.statement, action.sourceMap, ioMap); +} + +class IoStatementParser { + private current = 0; + + constructor( + private readonly tokens: GrlToken[], + private readonly ioMap: IoMap + ) {} + + parseAll(): IoFlowInstruction[] { + const instructions: IoFlowInstruction[] = []; + while (!this.isAtEnd()) { + if (this.checkKeyword("wait")) { + instructions.push(this.finishWait(this.advance())); + continue; + } + if (this.checkKeyword("pulse")) { + instructions.push(this.finishPulse(this.advance())); + continue; + } + if (this.checkIoStart()) { + instructions.push(this.finishIoWrite(this.peek())); + continue; + } + this.advance(); + } + return instructions; + } + + private finishIoWrite(start: GrlToken): IoWriteInstruction { + const target = this.parseIoReference(); + this.consumeOperator("=", "Expected = in IO assignment"); + const value = this.parseValue(); + return { + kind: "IO_WRITE", + target, + value, + sourceMap: tokenSourceMap(start) + }; + } + + private finishWait(start: GrlToken): WaitInstruction { + const conditionTokens = this.collectUntilKeyword(["timeout", "on_timeout"]); + if (conditionTokens.length === 0) { + throw ioError("GRL_WAIT_CONDITION_MISSING", "wait requires a condition"); + } + + let timeout: number | undefined; + let onTimeout: WaitInstruction["onTimeout"]; + if (this.matchKeyword("timeout")) { + timeout = this.parseDuration(); + } + if (this.matchKeyword("on_timeout")) { + const kind = this.consumeIdentifier("Expected on_timeout action").raw; + if (kind === "alarm") { + const message = this.consumeString("Expected alarm message"); + onTimeout = { kind: "alarm", value: message.value }; + } else if (kind === "call") { + onTimeout = { kind: "call", value: this.collectRest().map((token) => token.raw).join(" ") }; + } else { + throw ioError("GRL_WAIT_TIMEOUT_ACTION_INVALID", `Unsupported on_timeout action ${kind}`); + } + } + + return { + kind: "WAIT", + condition: conditionTokens.map((token) => token.raw).join(" "), + ...(timeout !== undefined ? { timeout } : {}), + ...(onTimeout ? { onTimeout } : {}), + sourceMap: tokenSourceMap(start) + }; + } + + private finishPulse(start: GrlToken): PulseInstruction { + const target = this.parseIoReference(); + this.consumeKeyword("duration", "Expected duration in pulse"); + const duration = this.parseDuration(); + return { + kind: "PULSE", + target, + duration, + trace: [ + { time: 0, action: "set", target, value: true }, + { time: duration, action: "reset", target, value: false } + ], + sourceMap: tokenSourceMap(start) + }; + } + + private parseIoReference(): IoReference { + const io = this.consumeKeyword("io", "Expected io reference"); + this.consumePunctuation(".", "Expected . after io"); + const domain = this.consumeIdentifier("Expected IO domain").raw as IoReference["domain"]; + if (!["di", "do", "ai", "ao", "gi", "go", "ri", "ro", "alias"].includes(domain)) { + throw ioError("GRL_IO_DOMAIN_INVALID", `Unsupported IO domain ${domain}`); + } + + if (domain === "alias") { + this.consumePunctuation(".", "Expected . after io.alias"); + const alias = this.consumeIdentifier("Expected IO alias").raw; + const mapped = this.ioMap.aliases?.[alias]; + return mapped ?? { domain, alias, raw: `io.alias.${alias}` }; + } + + this.consumePunctuation("[", "Expected [ after IO domain"); + const indexToken = this.consumeNumber("Expected IO index"); + this.consumePunctuation("]", "Expected ] after IO index"); + const index = indexToken.value; + this.validateIoIndex(domain, index, io); + return { + domain, + index, + raw: `io.${domain}[${index}]` + }; + } + + private validateIoIndex(domain: IoReference["domain"], index: number, token: GrlToken): void { + if (!Number.isInteger(index) || index < 0) { + throw ioError("GRL_IO_INDEX_INVALID", `Invalid IO index ${index}`); + } + const range = this.ioMap.allowedRanges?.[domain]; + if (range && (index < range.min || index > range.max)) { + throw new KdlStructuredError( + "GRL_IO_ADDRESS_NOT_FOUND", + `IO address io.${domain}[${index}] is outside [${range.min}, ${range.max}]`, + [ + { + severity: "error", + code: "GRL_IO_ADDRESS_NOT_FOUND", + message: `IO address io.${domain}[${index}] is outside [${range.min}, ${range.max}]`, + sourceMap: tokenSourceMap(token) + } + ] + ); + } + } + + private parseValue(): boolean | number | string { + const token = this.advance(); + if (token.kind === "keyword" && (token.raw === "true" || token.raw === "false")) { + return token.raw === "true"; + } + if (token.kind === "number") { + return normalizedNumber(token); + } + if (token.kind === "string") { + return token.value; + } + if (token.kind === "identifier" || token.kind === "keyword") { + return token.raw; + } + throw ioError("GRL_IO_VALUE_INVALID", "Unsupported IO assignment value"); + } + + private parseDuration(): number { + const token = this.consumeNumber("Expected duration"); + return normalizedNumber(token); + } + + private collectUntilKeyword(keywords: string[]): GrlToken[] { + const tokens: GrlToken[] = []; + let parenDepth = 0; + while (!this.isAtEnd()) { + const token = this.peek(); + if (parenDepth === 0) { + if ((token.kind === "keyword" || token.kind === "identifier") && keywords.includes(token.raw)) { + break; + } + if (tokens.length > 0 && this.isCurrentStatementStartAfter(tokens)) { + break; + } + } + const consumed = this.advance(); + tokens.push(consumed); + if (consumed.kind === "punctuation" && consumed.raw === "(") { + parenDepth += 1; + } else if (consumed.kind === "punctuation" && consumed.raw === ")") { + parenDepth = Math.max(0, parenDepth - 1); + } + } + return tokens; + } + + private collectRest(): GrlToken[] { + const tokens: GrlToken[] = []; + while (!this.isAtEnd()) { + if (tokens.length > 0 && this.isCurrentStatementStartAfter(tokens)) { + break; + } + tokens.push(this.advance()); + } + return tokens; + } + + private checkIoStart(): boolean { + return this.isIoStartAtCurrent(); + } + + private isCurrentStatementStartAfter(tokens: GrlToken[]): boolean { + const token = this.peek(); + if (this.isKeywordLike(token, "wait") || this.isKeywordLike(token, "pulse")) { + return true; + } + if (!this.isIoStartAtCurrent()) { + return false; + } + const previous = tokens.at(-1); + return previous ? token.range.start.line > previous.range.end.line : true; + } + + private isIoStartAtCurrent(): boolean { + return this.peek().raw === "io" && this.maybePeek(1)?.raw === "."; + } + + private isKeywordLike(token: GrlToken, keyword: string): boolean { + return (token.kind === "keyword" || token.kind === "identifier") && token.raw === keyword; + } + + private matchKeyword(keyword: string): boolean { + const token = this.peek(); + if ((token.kind === "keyword" || token.kind === "identifier") && token.raw === keyword) { + this.advance(); + return true; + } + return false; + } + + private checkKeyword(keyword: string): boolean { + const token = this.peek(); + return (token.kind === "keyword" || token.kind === "identifier") && token.raw === keyword; + } + + private consumeKeyword(keyword: string, message: string): GrlToken { + if (this.checkKeyword(keyword)) { + return this.advance(); + } + throw ioError("GRL_KEYWORD_EXPECTED", message); + } + + private consumeIdentifier(message: string): GrlToken { + const token = this.peek(); + if (token.kind === "identifier" || token.kind === "keyword") { + return this.advance(); + } + throw ioError("GRL_IDENTIFIER_EXPECTED", message); + } + + private consumePunctuation(value: string, message: string): GrlToken { + const token = this.peek(); + if (token.kind === "punctuation" && token.raw === value) { + return this.advance(); + } + throw ioError("GRL_PUNCTUATION_EXPECTED", message); + } + + private consumeOperator(value: string, message: string): GrlToken { + const token = this.peek(); + if (token.kind === "operator" && token.raw === value) { + return this.advance(); + } + throw ioError("GRL_OPERATOR_EXPECTED", message); + } + + private consume(kind: GrlToken["kind"], message: string): GrlToken { + if (this.peek().kind === kind) { + return this.advance(); + } + throw ioError("GRL_TOKEN_EXPECTED", message); + } + + private consumeNumber(message: string): Extract { + const token = this.peek(); + if (token.kind === "number") { + return this.advance() as Extract; + } + throw ioError("GRL_TOKEN_EXPECTED", message); + } + + private consumeString(message: string): Extract { + const token = this.peek(); + if (token.kind === "string") { + return this.advance() as Extract; + } + throw ioError("GRL_TOKEN_EXPECTED", message); + } + + private advance(): GrlToken { + this.current += 1; + return this.previous(); + } + + private previous(): GrlToken { + return this.tokens[this.current - 1]!; + } + + private peek(): GrlToken { + return this.tokens[this.current]!; + } + + private maybePeek(distance = 0): GrlToken | undefined { + return this.tokens[this.current + distance]; + } + + private isAtEnd(): boolean { + return this.current >= this.tokens.length; + } +} + +function compileStatementString( + statement: string, + _sourceMap: MotionSourceMap | undefined, + ioMap: IoMap +): IoFlowInstruction[] { + const tokens = lexGrl(statement, { preserveComments: false }).filter( + (token) => token.kind !== "eof" && token.kind !== "comment" + ); + return parseIoFlowStatements(tokens, ioMap); +} + +function normalizedNumber(token: GrlToken): number { + if (token.kind !== "number") { + throw ioError("GRL_NUMBER_EXPECTED", "Expected number"); + } + return token.unit?.normalizedValue ?? token.value; +} + +function tokenSourceMap(token: GrlToken): MotionSourceMap { + return { + line: token.range.start.line, + column: token.range.start.column + }; +} + +function ioError(code: string, message: string): KdlStructuredError { + return new KdlStructuredError(code, message); +} diff --git a/kdl-wasm/web/src/grl/semantic/compileMotion.ts b/kdl-wasm/web/src/grl/semantic/compileMotion.ts new file mode 100644 index 0000000..58a0004 --- /dev/null +++ b/kdl-wasm/web/src/grl/semantic/compileMotion.ts @@ -0,0 +1,978 @@ +import { KdlStructuredError } from "../../kdl/rpc.js"; +import { applyOffset } from "../../kdl/poseApi.js"; +import type { + JointTarget, + JsonObject, + MoveCRequest, + MoveJRequest, + MoveLRequest, + MotionSegmentRequest, + PathEventRequest, + PathPlanRequest, + Pose, + PoseTarget, + SpeedSpec, + ZoneSpec +} from "../../kdl/types.js"; +import type { + CompiledOperation, + CompiledPath, + MotionInstruction, + OperationActionInstruction, + OperationExecutionStep, + PathEventInstruction, + RunOperationInstruction, + RunPathInstruction +} from "../ir/index.js"; +import { parseGrlExpression } from "../parser/index.js"; +import type { + GrlExpression, + GrlOperationActionBlock, + GrlOperationDeclaration, + GrlOperationProcessBlock, + GrlPathDeclaration, + GrlPathDefaultsBlock, + GrlPathEvent, + GrlPathPoint, + GrlPathProperty, + GrlPathSourceBlock, + GrlProcedureDeclaration +} from "../ast/index.js"; +import type { GrlToken } from "../lexer/index.js"; +import { + compileOffsetExpression, + compileGrlDataDeclaration, + compileGrlTargetDeclaration, + compileSpeedExpression, + compileTargetExpression, + compileZoneExpression, + type CompiledGrlDataValue +} from "./compileData.js"; +import type { GrlDataDeclaration, GrlTargetDeclaration } from "../ast/index.js"; + +export interface GrlMotionContext { + targets: Map; + speeds: Map; + zones: Map; + tools: Map; + frames: Map; + currentSpeed?: SpeedSpec; + currentZone?: ZoneSpec; + currentTool?: Pose; + currentFrame?: Pose; +} + +export interface MotionRequestOptions { + startJoints: number[]; + sampleTime: number; +} + +interface PathDefaults { + speed?: SpeedSpec; + zone?: ZoneSpec; + tool?: Pose; + frame?: Pose; +} + +export interface PathCompileOptions extends MotionRequestOptions { + speedOverride?: number; + stopOnError?: boolean; +} + +export function buildMotionContext(declarations: Array): GrlMotionContext { + const context: GrlMotionContext = { + targets: new Map(), + speeds: new Map(), + zones: new Map(), + tools: new Map(), + frames: new Map() + }; + + for (const declaration of declarations) { + if (declaration.kind === "TargetDeclaration") { + const compiled = compileGrlTargetDeclaration(declaration); + context.targets.set(compiled.name, compiled.target); + continue; + } + + const compiled = compileGrlDataDeclaration(declaration); + addCompiledData(context, compiled.name, compiled.typeName, compiled.value); + } + + return context; +} + +export function parseProcedureMotionInstructions( + procedure: GrlProcedureDeclaration, + context: GrlMotionContext +): MotionInstruction[] { + const parser = new MotionStatementParser(procedure.bodyTokens, context); + return parser.parseAll(); +} + +export function parseProcedureRunPathStatements(procedure: GrlProcedureDeclaration): RunPathInstruction[] { + const parser = new RunPathStatementParser(procedure.bodyTokens); + return parser.parseAll(); +} + +export function parseProcedureRunOperationStatements(procedure: GrlProcedureDeclaration): RunOperationInstruction[] { + const parser = new RunOperationStatementParser(procedure.bodyTokens); + return parser.parseAll(); +} + +export function compilePathToPlanRequest( + path: GrlPathDeclaration, + context: GrlMotionContext, + options: PathCompileOptions +): CompiledPath { + const defaults = compilePathDefaults(path.items.find((item): item is GrlPathDefaultsBlock => item.kind === "PathDefaultsBlock"), context); + const source = compilePathSource(path.items.find((item): item is GrlPathSourceBlock => item.kind === "PathSourceBlock")); + const points = path.items.filter((item): item is GrlPathPoint => item.kind === "PathPoint"); + const events = path.items.filter((item): item is GrlPathEvent => item.kind === "PathEvent"); + + if (points.length === 0) { + throw motionError("GRL_PATH_EMPTY", `Path ${path.name} must contain at least one point`); + } + + const pointIds = new Set(); + const motions: MotionInstruction[] = []; + const segments: MotionSegmentRequest[] = []; + + for (const point of points) { + if (pointIds.has(point.id)) { + throw motionError("GRL_PATH_POINT_DUPLICATE", `Path ${path.name} contains duplicate point ${point.id}`); + } + pointIds.add(point.id); + + const pointContext = cloneMotionContext(context); + applyPathDefaults(pointContext, defaults); + const [motion] = new MotionStatementParser(point.motionTokens, pointContext).parseAll(); + if (!motion) { + throw motionError("GRL_PATH_POINT_MOTION_MISSING", `Path point ${point.id} has no motion`); + } + + const instruction: MotionInstruction = { + ...motion, + id: point.id, + pathId: path.name, + pointId: point.id, + ...(source ? { source } : {}) + }; + motions.push(instruction); + segments.push(motionToSegment(instruction, path.name, source)); + } + + const compiledEvents = events.map((event, index) => compilePathEvent(event, index, pointIds)); + const request: PathPlanRequest = { + pathId: path.name, + startJoints: options.startJoints, + segments, + ...(compiledEvents.length > 0 ? { events: compiledEvents } : {}), + sampleTime: options.sampleTime, + ...(options.speedOverride !== undefined ? { speedOverride: options.speedOverride } : {}), + ...(options.stopOnError !== undefined ? { stopOnError: options.stopOnError } : {}), + ...(source ? { source } : {}) + }; + + return { + pathId: path.name, + request, + motions, + events: compiledEvents + }; +} + +export function compileOperation( + operation: GrlOperationDeclaration, + paths: Map +): CompiledOperation { + if (!paths.has(operation.pathName)) { + throw motionError( + "GRL_OPERATION_PATH_NOT_FOUND", + `Operation ${operation.name} references unknown path ${operation.pathName}` + ); + } + + const processBlock = operation.items.find( + (item): item is GrlOperationProcessBlock => item.kind === "OperationProcessBlock" + ); + const actionBlocks = operation.items.filter( + (item): item is GrlOperationActionBlock => item.kind === "OperationActionBlock" + ); + + return { + operationId: operation.name, + kind: operation.operationKind, + pathId: operation.pathName, + process: processBlock ? compileProcessBlock(processBlock) : {}, + startActions: actionBlocks + .filter((item) => item.actionKind === "start_action") + .map((item) => compileOperationAction(operation.name, item)), + endActions: actionBlocks + .filter((item) => item.actionKind === "end_action") + .map((item) => compileOperationAction(operation.name, item)) + }; +} + +export function expandRunOperation( + run: RunOperationInstruction, + operations: Map +): OperationExecutionStep[] { + const operation = operations.get(run.operationId); + if (!operation) { + throw motionError("GRL_OPERATION_NOT_FOUND", `Unknown operation ${run.operationId}`); + } + return [ + ...operation.startActions, + { + kind: "RUN_PATH", + pathId: operation.pathId, + ...(run.sourceMap ? { sourceMap: run.sourceMap } : {}) + }, + ...operation.endActions + ]; +} + +export function compileMotionToKdlRequest( + instruction: MotionInstruction, + options: MotionRequestOptions +): MoveJRequest | MoveLRequest | MoveCRequest { + if (instruction.kind === "MOVEJ") { + if (!instruction.target) { + throw motionError("GRL_MOTION_TARGET_MISSING", "MOVEJ requires target"); + } + return { + startJoints: options.startJoints, + target: instruction.target, + speed: instruction.speed, + zone: instruction.zone, + ...(instruction.tool ? { tool: instruction.tool } : {}), + ...(instruction.frame ? { frame: instruction.frame } : {}), + sampleTime: options.sampleTime, + ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}) + }; + } + + if (instruction.kind === "MOVEL") { + if (!instruction.target || !isPoseTarget(instruction.target)) { + throw motionError("GRL_MOTION_TARGET_TYPE", "MOVEL requires PoseTarget"); + } + return { + startJoints: options.startJoints, + target: instruction.target, + speed: instruction.speed, + zone: instruction.zone, + ...(instruction.tool ? { tool: instruction.tool } : {}), + ...(instruction.frame ? { frame: instruction.frame } : {}), + sampleTime: options.sampleTime, + ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}) + }; + } + + if (!instruction.via || !instruction.target || !isPoseTarget(instruction.target)) { + throw motionError("GRL_MOTION_TARGET_TYPE", "MOVEC requires via and target PoseTarget"); + } + + return { + startJoints: options.startJoints, + via: instruction.via, + target: instruction.target, + speed: instruction.speed, + zone: instruction.zone, + ...(instruction.tool ? { tool: instruction.tool } : {}), + ...(instruction.frame ? { frame: instruction.frame } : {}), + sampleTime: options.sampleTime, + ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}) + }; +} + +function motionToSegment( + instruction: MotionInstruction, + pathId: string, + source?: Record +): MotionSegmentRequest { + if (instruction.kind === "MOVEJ") { + if (!instruction.target) { + throw motionError("GRL_MOTION_TARGET_MISSING", "MOVEJ requires target"); + } + const targetId = targetIdOf(instruction.target); + return { + id: instruction.pointId ?? instruction.id ?? `${pathId}_${instruction.kind.toLowerCase()}`, + motion: "MOVEJ", + target: instruction.target, + ...(targetId ? { targetId } : {}), + speed: instruction.speed, + zone: instruction.zone, + ...(instruction.tool ? { tool: instruction.tool } : {}), + ...(instruction.frame ? { frame: instruction.frame } : {}), + ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}), + ...(source ? { source } : {}) + }; + } + + if (instruction.kind === "MOVEL") { + if (!instruction.target || !isPoseTarget(instruction.target)) { + throw motionError("GRL_MOTION_TARGET_TYPE", "MOVEL requires PoseTarget"); + } + const targetId = targetIdOf(instruction.target); + return { + id: instruction.pointId ?? instruction.id ?? `${pathId}_${instruction.kind.toLowerCase()}`, + motion: "MOVEL", + target: instruction.target, + ...(targetId ? { targetId } : {}), + speed: instruction.speed, + zone: instruction.zone, + ...(instruction.tool ? { tool: instruction.tool } : {}), + ...(instruction.frame ? { frame: instruction.frame } : {}), + ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}), + ...(source ? { source } : {}) + }; + } + + if (!instruction.via || !instruction.target || !isPoseTarget(instruction.target)) { + throw motionError("GRL_MOTION_TARGET_TYPE", "MOVEC requires via and target PoseTarget"); + } + const targetId = targetIdOf(instruction.target); + return { + id: instruction.pointId ?? instruction.id ?? `${pathId}_${instruction.kind.toLowerCase()}`, + motion: "MOVEC", + via: instruction.via, + target: instruction.target, + ...(targetId ? { targetId } : {}), + speed: instruction.speed, + zone: instruction.zone, + ...(instruction.tool ? { tool: instruction.tool } : {}), + ...(instruction.frame ? { frame: instruction.frame } : {}), + ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}), + ...(source ? { source } : {}) + }; +} + +function addCompiledData( + context: GrlMotionContext, + name: string, + typeName: string, + value: CompiledGrlDataValue +): void { + if (typeName === "speed") { + context.speeds.set(name, value as SpeedSpec); + } else if (typeName === "zone") { + context.zones.set(name, value as ZoneSpec); + } else if (typeName === "tool") { + context.tools.set(name, (value as { tcp: Pose }).tcp); + } else if (typeName === "frame") { + context.frames.set(name, (value as { origin: Pose }).origin); + } +} + +class MotionStatementParser { + private current = 0; + + constructor( + private readonly tokens: GrlToken[], + private readonly context: GrlMotionContext + ) {} + + parseAll(): MotionInstruction[] { + const instructions: MotionInstruction[] = []; + while (!this.isAtEnd()) { + if (this.matchKeyword("set_tool")) { + this.context.currentTool = this.resolveNamedPose(this.consumeIdentifier("Expected tool name"), "tool"); + continue; + } + if (this.matchKeyword("set_frame")) { + this.context.currentFrame = this.resolveNamedPose(this.consumeIdentifier("Expected frame name"), "frame"); + continue; + } + if (this.matchKeyword("set_speed")) { + this.context.currentSpeed = this.parseSpeedArgument(); + continue; + } + if (this.matchKeyword("set_zone")) { + this.context.currentZone = this.parseZoneArgument(); + continue; + } + if (this.matchKeyword("movej")) { + instructions.push(this.finishMoveJ(this.previous())); + continue; + } + if (this.matchKeyword("movel")) { + instructions.push(this.finishMoveL(this.previous())); + continue; + } + if (this.matchKeyword("movec")) { + instructions.push(this.finishMoveC(this.previous())); + continue; + } + this.advance(); + } + return instructions; + } + + private finishMoveJ(start: GrlToken): MotionInstruction { + const target = this.parseTargetArgument(); + const params = this.parseMotionParams(); + return this.withDefaults({ + kind: "MOVEJ", + target, + ...params, + sourceMap: tokenSourceMap(start) + }); + } + + private finishMoveL(start: GrlToken): MotionInstruction { + const target = this.parseTargetArgument(); + if (!isPoseTarget(target)) { + throw motionError("GRL_MOTION_TARGET_TYPE", "MOVEL requires PoseTarget"); + } + const params = this.parseMotionParams(); + return this.withDefaults({ + kind: "MOVEL", + target, + ...params, + sourceMap: tokenSourceMap(start) + }); + } + + private finishMoveC(start: GrlToken): MotionInstruction { + this.consumeKeyword("via", "Expected via in MOVEC"); + const via = this.parseTargetArgument(); + if (!isPoseTarget(via)) { + throw motionError("GRL_MOTION_TARGET_TYPE", "MOVEC via requires PoseTarget"); + } + this.consumeKeyword("target", "Expected target in MOVEC"); + const target = this.parseTargetArgument(); + if (!isPoseTarget(target)) { + throw motionError("GRL_MOTION_TARGET_TYPE", "MOVEC target requires PoseTarget"); + } + const params = this.parseMotionParams(); + return this.withDefaults({ + kind: "MOVEC", + via, + target, + ...params, + sourceMap: tokenSourceMap(start) + }); + } + + private parseMotionParams(): Partial { + const params: Partial = {}; + while (!this.isAtEnd() && !this.isMotionStart(this.peek())) { + if (this.matchKeyword("speed")) { + params.speed = this.parseSpeedArgument(); + } else if (this.matchKeyword("zone")) { + params.zone = this.parseZoneArgument(); + } else if (this.matchKeyword("tool")) { + params.tool = this.resolveNamedPose(this.consumeIdentifier("Expected tool name"), "tool"); + } else if (this.matchKeyword("frame")) { + params.frame = this.resolveNamedPose(this.consumeIdentifier("Expected frame name"), "frame"); + } else { + break; + } + } + return params; + } + + private parseTargetArgument(): JointTarget | PoseTarget { + const expressionTokens = this.collectExpressionUntilParamKeyword(); + const expression = parseGrlExpression(expressionTokens); + return this.resolveTargetExpression(expression); + } + + private parseSpeedArgument(): SpeedSpec { + const token = this.peek(); + if ((token.kind === "identifier" || token.kind === "keyword") && this.context.speeds.has(token.raw)) { + this.advance(); + return this.context.speeds.get(token.raw)!; + } + return compileSpeedExpression(parseGrlExpression(this.collectExpressionUntilParamKeyword())); + } + + private parseZoneArgument(): ZoneSpec { + const token = this.peek(); + if ((token.kind === "identifier" || token.kind === "keyword") && this.context.zones.has(token.raw)) { + this.advance(); + return this.context.zones.get(token.raw)!; + } + return compileZoneExpression(parseGrlExpression(this.collectExpressionUntilParamKeyword())); + } + + private resolveTargetExpression(expression: GrlExpression): JointTarget | PoseTarget { + if (expression.kind === "OffsetExpression") { + const base = this.resolveTargetExpression(expression.base); + if (!isPoseTarget(base)) { + throw motionError("GRL_MOTION_TARGET_TYPE", "offset target requires PoseTarget"); + } + return applyOffset(base, compileOffsetExpression(expression)); + } + if (expression.kind === "IdentifierExpression") { + const target = this.context.targets.get(expression.name); + if (!target) { + throw motionError("GRL_TARGET_NOT_FOUND", `Unknown target ${expression.name}`); + } + return target; + } + return compileTargetExpression(expression); + } + + private withDefaults(instruction: Partial & Pick): MotionInstruction { + const speed = instruction.speed ?? this.context.currentSpeed; + const zone = instruction.zone ?? this.context.currentZone; + if (!speed) { + throw motionError("GRL_SPEED_UNRESOLVED", `${instruction.kind} has no speed`); + } + if (!zone) { + throw motionError("GRL_ZONE_UNRESOLVED", `${instruction.kind} has no zone`); + } + return { + kind: instruction.kind, + ...(instruction.target ? { target: instruction.target } : {}), + ...(instruction.via ? { via: instruction.via } : {}), + speed, + zone, + ...(instruction.tool ?? this.context.currentTool ? { tool: instruction.tool ?? this.context.currentTool } : {}), + ...(instruction.frame ?? this.context.currentFrame ? { frame: instruction.frame ?? this.context.currentFrame } : {}), + ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}) + }; + } + + private resolveNamedPose(name: string, kind: "tool" | "frame"): Pose { + const source = kind === "tool" ? this.context.tools : this.context.frames; + const pose = source.get(name); + if (!pose) { + throw motionError(kind === "tool" ? "GRL_TOOL_NOT_FOUND" : "GRL_FRAME_NOT_FOUND", `Unknown ${kind} ${name}`); + } + return pose; + } + + private collectExpressionUntilParamKeyword(): GrlToken[] { + const tokens: GrlToken[] = []; + let parenDepth = 0; + let bracketDepth = 0; + let braceDepth = 0; + const startLine = this.peek().range.start.line; + + while (!this.isAtEnd()) { + const token = this.peek(); + if (tokens.length > 0 && token.range.start.line > startLine && this.isStatementStart(token)) { + break; + } + if ( + parenDepth === 0 && + bracketDepth === 0 && + braceDepth === 0 && + this.isExpressionTerminator(token) + ) { + break; + } + + const consumed = this.advance(); + tokens.push(consumed); + if (consumed.kind === "punctuation") { + if (consumed.raw === "(") parenDepth += 1; + if (consumed.raw === ")") parenDepth -= 1; + if (consumed.raw === "[") bracketDepth += 1; + if (consumed.raw === "]") bracketDepth -= 1; + if (consumed.raw === "{") braceDepth += 1; + if (consumed.raw === "}") braceDepth -= 1; + } + } + + if (tokens.length === 0) { + throw motionError("GRL_EXPRESSION_MISSING", "Expected motion expression"); + } + return tokens; + } + + private isExpressionTerminator(token: GrlToken): boolean { + return ( + this.isMotionStart(token) || + ((token.kind === "keyword" || token.kind === "identifier") && + ["speed", "zone", "tool", "frame", "via", "target"].includes(token.raw)) + ); + } + + private isMotionStart(token: GrlToken): boolean { + return ( + (token.kind === "keyword" || token.kind === "identifier") && + ["movej", "movel", "movec", "set_tool", "set_frame", "set_speed", "set_zone"].includes(token.raw) + ); + } + + private isStatementStart(token: GrlToken): boolean { + return ( + (token.kind === "keyword" || token.kind === "identifier") && + [ + "movej", + "movel", + "movec", + "set_tool", + "set_frame", + "set_speed", + "set_zone", + "io", + "wait", + "pulse", + "run_path", + "run_operation", + "if", + "elseif", + "else", + "while", + "for", + "switch", + "case", + "default", + "break", + "continue", + "label", + "jump", + "call", + "return", + "alarm", + "raise", + "try", + "catch", + "finally", + "enable", + "disable", + "end" + ].includes(token.raw) + ); + } + + private consumeIdentifier(message: string): string { + const token = this.peek(); + if (token.kind === "identifier" || token.kind === "keyword") { + this.advance(); + return token.raw; + } + throw motionError("GRL_IDENTIFIER_EXPECTED", message); + } + + private consumeKeyword(keyword: string, message: string): void { + if (!this.matchKeyword(keyword)) { + throw motionError("GRL_KEYWORD_EXPECTED", message); + } + } + + private matchKeyword(keyword: string): boolean { + const token = this.peek(); + if ((token.kind === "keyword" || token.kind === "identifier") && token.raw === keyword) { + this.advance(); + return true; + } + return false; + } + + private advance(): GrlToken { + this.current += 1; + return this.previous(); + } + + private previous(): GrlToken { + return this.tokens[this.current - 1]!; + } + + private peek(): GrlToken { + return this.tokens[this.current]!; + } + + private isAtEnd(): boolean { + return this.current >= this.tokens.length; + } +} + +class RunPathStatementParser { + private current = 0; + + constructor(private readonly tokens: GrlToken[]) {} + + parseAll(): RunPathInstruction[] { + const instructions: RunPathInstruction[] = []; + while (!this.isAtEnd()) { + if (this.matchKeyword("run_path")) { + const start = this.previous(); + const path = this.consumeIdentifier("Expected path name after run_path"); + instructions.push({ + kind: "RUN_PATH", + pathId: path.raw, + sourceMap: tokenSourceMap(start) + }); + continue; + } + this.advance(); + } + return instructions; + } + + private consumeIdentifier(message: string): GrlToken { + const token = this.peek(); + if (token.kind === "identifier" || token.kind === "keyword") { + return this.advance(); + } + throw motionError("GRL_IDENTIFIER_EXPECTED", message); + } + + private matchKeyword(keyword: string): boolean { + const token = this.peek(); + if ((token.kind === "keyword" || token.kind === "identifier") && token.raw === keyword) { + this.advance(); + return true; + } + return false; + } + + private advance(): GrlToken { + this.current += 1; + return this.previous(); + } + + private previous(): GrlToken { + return this.tokens[this.current - 1]!; + } + + private peek(): GrlToken { + return this.tokens[this.current]!; + } + + private isAtEnd(): boolean { + return this.current >= this.tokens.length; + } +} + +class RunOperationStatementParser { + private current = 0; + + constructor(private readonly tokens: GrlToken[]) {} + + parseAll(): RunOperationInstruction[] { + const instructions: RunOperationInstruction[] = []; + while (!this.isAtEnd()) { + if (this.matchKeyword("run_operation")) { + const start = this.previous(); + const operation = this.consumeIdentifier("Expected operation name after run_operation"); + instructions.push({ + kind: "RUN_OPERATION", + operationId: operation.raw, + sourceMap: tokenSourceMap(start) + }); + continue; + } + this.advance(); + } + return instructions; + } + + private consumeIdentifier(message: string): GrlToken { + const token = this.peek(); + if (token.kind === "identifier" || token.kind === "keyword") { + return this.advance(); + } + throw motionError("GRL_IDENTIFIER_EXPECTED", message); + } + + private matchKeyword(keyword: string): boolean { + const token = this.peek(); + if ((token.kind === "keyword" || token.kind === "identifier") && token.raw === keyword) { + this.advance(); + return true; + } + return false; + } + + private advance(): GrlToken { + this.current += 1; + return this.previous(); + } + + private previous(): GrlToken { + return this.tokens[this.current - 1]!; + } + + private peek(): GrlToken { + return this.tokens[this.current]!; + } + + private isAtEnd(): boolean { + return this.current >= this.tokens.length; + } +} + +function compileProcessBlock(block: GrlOperationProcessBlock): Record { + return Object.fromEntries(block.properties.map((property) => [property.key, compileLiteralValue(property.value)])); +} + +function compileOperationAction( + operationId: string, + block: GrlOperationActionBlock +): OperationActionInstruction { + const first = block.actionTokens[0] ?? block.actionTokens[block.actionTokens.length - 1]!; + return { + kind: "ACTION", + actionKind: block.actionKind, + operationId, + statement: block.actionTokens.map((token) => token.raw).join(" "), + tokens: block.actionTokens, + sourceMap: tokenSourceMap(first) + }; +} + +function compilePathDefaults(block: GrlPathDefaultsBlock | undefined, context: GrlMotionContext): PathDefaults { + const defaults: PathDefaults = {}; + if (!block) { + return defaults; + } + + for (const property of block.properties) { + if (property.key === "speed") { + defaults.speed = resolveSpeed(property.value, context); + } else if (property.key === "zone") { + defaults.zone = resolveZone(property.value, context); + } else if (property.key === "tool") { + defaults.tool = resolveNamedPoseFromExpression(property.value, context, "tool"); + } else if (property.key === "frame") { + defaults.frame = resolveNamedPoseFromExpression(property.value, context, "frame"); + } + } + return defaults; +} + +function compilePathSource(block: GrlPathSourceBlock | undefined): JsonObject | undefined { + if (!block) { + return undefined; + } + return Object.fromEntries(block.properties.map((property) => [property.key, compileLiteralValue(property.value)])); +} + +function compilePathEvent( + event: GrlPathEvent, + index: number, + pointIds: Set +): PathEventInstruction & PathEventRequest { + if (!pointIds.has(event.pointId)) { + throw motionError("GRL_PATH_EVENT_POINT_NOT_FOUND", `Path event references unknown point ${event.pointId}`); + } + + return { + id: `event_${index}`, + timing: event.timing, + pointId: event.pointId, + ...(event.distance ? { distance: normalizedNumber(event.distance) } : {}), + kind: event.actionTokens[0]?.raw ?? "statement", + sourceMap: tokenSourceMap(event.actionTokens[0] ?? event.actionTokens[event.actionTokens.length - 1]!), + data: { + statement: event.actionTokens.map((token) => token.raw).join(" "), + tokens: event.actionTokens + } + }; +} + +function cloneMotionContext(context: GrlMotionContext): GrlMotionContext { + return { + targets: context.targets, + speeds: context.speeds, + zones: context.zones, + tools: context.tools, + frames: context.frames, + ...(context.currentSpeed ? { currentSpeed: context.currentSpeed } : {}), + ...(context.currentZone ? { currentZone: context.currentZone } : {}), + ...(context.currentTool ? { currentTool: context.currentTool } : {}), + ...(context.currentFrame ? { currentFrame: context.currentFrame } : {}) + }; +} + +function applyPathDefaults(context: GrlMotionContext, defaults: PathDefaults): void { + if (defaults.speed) { + context.currentSpeed = defaults.speed; + } + if (defaults.zone) { + context.currentZone = defaults.zone; + } + if (defaults.tool) { + context.currentTool = defaults.tool; + } + if (defaults.frame) { + context.currentFrame = defaults.frame; + } +} + +function resolveSpeed(expression: GrlExpression, context: GrlMotionContext): SpeedSpec { + if (expression.kind === "IdentifierExpression" && context.speeds.has(expression.name)) { + return context.speeds.get(expression.name)!; + } + return compileSpeedExpression(expression); +} + +function resolveZone(expression: GrlExpression, context: GrlMotionContext): ZoneSpec { + if (expression.kind === "IdentifierExpression" && context.zones.has(expression.name)) { + return context.zones.get(expression.name)!; + } + return compileZoneExpression(expression); +} + +function resolveNamedPoseFromExpression( + expression: GrlExpression, + context: GrlMotionContext, + kind: "tool" | "frame" +): Pose { + if (expression.kind !== "IdentifierExpression") { + throw motionError(kind === "tool" ? "GRL_TOOL_NOT_FOUND" : "GRL_FRAME_NOT_FOUND", `Path ${kind} must reference a named ${kind}`); + } + const source = kind === "tool" ? context.tools : context.frames; + const pose = source.get(expression.name); + if (!pose) { + throw motionError(kind === "tool" ? "GRL_TOOL_NOT_FOUND" : "GRL_FRAME_NOT_FOUND", `Unknown ${kind} ${expression.name}`); + } + return pose; +} + +function compileLiteralValue(expression: GrlExpression): unknown { + if (expression.kind === "NumberLiteral") { + return normalizedNumber(expression); + } + if (expression.kind === "StringLiteral" || expression.kind === "BooleanLiteral") { + return expression.value; + } + if (expression.kind === "IdentifierExpression") { + return expression.name; + } + if (expression.kind === "ArrayExpression") { + return expression.elements.map(compileLiteralValue); + } + if (expression.kind === "ObjectExpression") { + return Object.fromEntries(expression.properties.map((property) => [property.key, compileLiteralValue(property.value)])); + } + if (expression.kind === "CallExpression") { + return { + callee: expression.callee, + args: expression.args.map(compileLiteralValue) + }; + } + return { + kind: expression.kind + }; +} + +function tokenSourceMap(token: GrlToken) { + return { + line: token.range.start.line, + column: token.range.start.column + }; +} + +function isPoseTarget(target: JointTarget | PoseTarget): target is PoseTarget { + return "pose" in target; +} + +function targetIdOf(target: JointTarget | PoseTarget): string | undefined { + return target.id; +} + +function normalizedNumber(expression: { value: number; unit?: { normalizedValue: number } }): number { + return expression.unit?.normalizedValue ?? expression.value; +} + +function motionError(code: string, message: string): KdlStructuredError { + return new KdlStructuredError(code, message); +} diff --git a/kdl-wasm/web/src/grl/semantic/compileProcFunction.ts b/kdl-wasm/web/src/grl/semantic/compileProcFunction.ts new file mode 100644 index 0000000..df1022b --- /dev/null +++ b/kdl-wasm/web/src/grl/semantic/compileProcFunction.ts @@ -0,0 +1,647 @@ +import { KdlStructuredError } from "../../kdl/rpc.js"; +import type { MotionDiagnostic, MotionSourceMap } from "../../kdl/types.js"; +import type { + GrlDataDeclaration, + GrlFunctionDeclaration, + GrlProcedureDeclaration, + GrlTargetDeclaration, + GrlTopLevelDeclaration +} from "../ast/index.js"; +import type { + CallInstruction, + ControlExpression, + FunctionSignature, + ProcedureFlowInstruction, + ProcedureSignature, + ProcFunctionAnalysis, + ReturnInstruction, + RoutineParameter, + RoutineParameterDirection +} from "../ir/index.js"; +import type { GrlToken } from "../lexer/index.js"; +import { parseControlFlowStatements } from "./compileControlFlow.js"; + +type RoutineDeclaration = GrlProcedureDeclaration | GrlFunctionDeclaration; +type RoutineSignature = ProcedureSignature | FunctionSignature; +type RoutineKind = "proc" | "func"; +type InferredType = string | "unknown"; + +interface AnalysisContext { + routineName: string; + routineKind: RoutineKind; + returnType?: string; + parameters: RoutineParameter[]; + symbols: Map; + outerNames: Set; + signatures: Map; + diagnostics: MotionDiagnostic[]; + calls: CallInstruction[]; + returns: ReturnInstruction[]; +} + +interface FlowResult { + normalExits: Set[]; + returnExits: Set[]; +} + +export function analyzeProcFunctionSemantics(declarations: GrlTopLevelDeclaration[]): ProcFunctionAnalysis { + const procedures = declarations.filter( + (decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration" + ); + const functions = declarations.filter( + (decl): decl is GrlFunctionDeclaration => decl.kind === "FunctionDeclaration" + ); + const globalNames = collectGlobalNames(declarations); + const diagnostics: MotionDiagnostic[] = []; + const signatures = new Map(); + const procedureSignatures = procedures.map((procedure) => compileProcedureSignature(procedure, globalNames, diagnostics)); + const functionSignatures = functions.map((func) => compileFunctionSignature(func, globalNames, diagnostics)); + + for (const signature of [...procedureSignatures, ...functionSignatures]) { + if (signatures.has(signature.name)) { + throw routineError("GRL_ROUTINE_DUPLICATE", `Duplicate routine ${signature.name}`, signature.sourceMap); + } + signatures.set(signature.name, signature); + } + + const calls: CallInstruction[] = []; + const returns: ReturnInstruction[] = []; + for (const procedure of procedures) { + analyzeRoutineBody(procedure, "proc", undefined, signatures, globalNames, diagnostics, calls, returns); + } + for (const func of functions) { + analyzeRoutineBody(func, "func", func.returnType, signatures, globalNames, diagnostics, calls, returns); + } + + return { + procedures: procedureSignatures, + functions: functionSignatures, + calls, + returns, + diagnostics + }; +} + +export function compileProcedureSignature( + procedure: GrlProcedureDeclaration, + globalNames: Set = new Set(), + diagnostics: MotionDiagnostic[] = [] +): ProcedureSignature { + return { + kind: "PROC_SIGNATURE", + name: procedure.name, + parameters: parseRoutineParameters(procedure.params, procedure.name, globalNames, diagnostics), + sourceMap: rangeSourceMap(procedure.range.start) + }; +} + +export function compileFunctionSignature( + func: GrlFunctionDeclaration, + globalNames: Set = new Set(), + diagnostics: MotionDiagnostic[] = [] +): FunctionSignature { + return { + kind: "FUNC_SIGNATURE", + name: func.name, + returnType: func.returnType, + parameters: parseRoutineParameters(func.params, func.name, globalNames, diagnostics), + sourceMap: rangeSourceMap(func.range.start) + }; +} + +function analyzeRoutineBody( + declaration: RoutineDeclaration, + routineKind: RoutineKind, + returnType: string | undefined, + signatures: Map, + globalNames: Set, + diagnostics: MotionDiagnostic[], + calls: CallInstruction[], + returns: ReturnInstruction[] +): void { + const signature = signatures.get(declaration.name); + if (!signature) { + throw routineError("GRL_ROUTINE_NOT_FOUND", `Missing routine signature ${declaration.name}`); + } + + const symbols = new Map(signature.parameters.map((parameter) => [parameter.name, parameter.typeName])); + const context: AnalysisContext = { + routineName: declaration.name, + routineKind, + ...(returnType ? { returnType } : {}), + parameters: signature.parameters, + symbols, + outerNames: globalNames, + signatures, + diagnostics, + calls, + returns + }; + const flow = parseControlFlowStatements(declaration.bodyTokens); + const result = analyzeFlow(flow, new Set(), context); + const outParameters = signature.parameters.filter((parameter) => parameter.direction === "out"); + + for (const exit of [...result.normalExits, ...result.returnExits]) { + for (const parameter of outParameters) { + if (!exit.has(parameter.name)) { + throw routineError( + "GRL_OUT_PARAM_NOT_ASSIGNED", + `out parameter ${parameter.name} is not assigned on all normal return paths`, + parameter.sourceMap + ); + } + } + } + + if (routineKind === "func" && returnType !== "void" && result.normalExits.length > 0) { + throw routineError("GRL_FUNC_MISSING_RETURN", `Function ${declaration.name} does not return on all normal paths`, rangeSourceMap(declaration.range.start)); + } +} + +function analyzeFlow(flow: ProcedureFlowInstruction[], incoming: Set, context: AnalysisContext): FlowResult { + let normalStates: Set[] = [new Set(incoming)]; + const returnStates: Set[] = []; + + for (const instruction of flow) { + const nextNormalStates: Set[] = []; + for (const state of normalStates) { + const result = analyzeInstruction(instruction, state, context); + nextNormalStates.push(...result.normalExits); + returnStates.push(...result.returnExits); + } + normalStates = nextNormalStates; + if (normalStates.length === 0) { + break; + } + } + + return { + normalExits: normalStates, + returnExits: returnStates + }; +} + +function analyzeInstruction( + instruction: ProcedureFlowInstruction, + incoming: Set, + context: AnalysisContext +): FlowResult { + if (instruction.kind === "RAW_STATEMENT") { + return analyzeRawStatement(instruction.tokens as GrlToken[] | undefined, incoming, context); + } + + if (instruction.kind === "IF") { + const normalExits: Set[] = []; + const returnExits: Set[] = []; + for (const branch of instruction.branches) { + const result = analyzeFlow(branch.body, new Set(incoming), context); + normalExits.push(...result.normalExits); + returnExits.push(...result.returnExits); + } + if (!instruction.branches.some((branch) => branch.branchKind === "else")) { + normalExits.push(new Set(incoming)); + } + return { normalExits, returnExits }; + } + + if (instruction.kind === "WHILE" || instruction.kind === "FOR") { + const body = analyzeFlow(instruction.body, new Set(incoming), context); + return { + normalExits: [new Set(incoming), ...body.normalExits], + returnExits: body.returnExits + }; + } + + if (instruction.kind === "SWITCH") { + const normalExits: Set[] = []; + const returnExits: Set[] = []; + for (const switchCase of instruction.cases) { + const result = analyzeFlow(switchCase.body, new Set(incoming), context); + normalExits.push(...result.normalExits); + returnExits.push(...result.returnExits); + } + if (!instruction.cases.some((switchCase) => switchCase.caseKind === "default")) { + normalExits.push(new Set(incoming)); + } + return { normalExits, returnExits }; + } + + return { + normalExits: [new Set(incoming)], + returnExits: [] + }; +} + +function analyzeRawStatement( + tokens: GrlToken[] | undefined, + incoming: Set, + context: AnalysisContext +): FlowResult { + if (!tokens || tokens.length === 0) { + return { normalExits: [new Set(incoming)], returnExits: [] }; + } + + checkFunctionSideEffects(tokens, context); + const assigned = new Set(incoming); + const declaration = parseLocalDeclaration(tokens); + if (declaration) { + if (context.symbols.has(declaration.name) || context.outerNames.has(declaration.name)) { + context.diagnostics.push(diagnostic("warning", "GRL_NAME_SHADOWS_OUTER_SCOPE", `Local ${declaration.name} shadows an outer name`, declaration.sourceMap)); + } + context.symbols.set(declaration.name, declaration.typeName); + assigned.add(declaration.name); + return { normalExits: [assigned], returnExits: [] }; + } + + const assignment = parseAssignment(tokens); + if (assignment) { + assigned.add(assignment.name); + } + + const call = parseCallStatement(tokens); + if (call) { + validateCall(call, context); + applyCallAssignments(call, assigned, context); + context.calls.push(call); + if (call.target === context.routineName) { + context.diagnostics.push(diagnostic("warning", "GRL_RECURSIVE_CALL", `Routine ${context.routineName} calls itself`, call.sourceMap)); + } + return { normalExits: [assigned], returnExits: [] }; + } + + const returnInstruction = parseReturnStatement(tokens); + if (returnInstruction) { + validateReturn(returnInstruction, context); + context.returns.push(returnInstruction); + return { normalExits: [], returnExits: [assigned] }; + } + + return { normalExits: [assigned], returnExits: [] }; +} + +function parseRoutineParameters( + tokens: GrlToken[], + routineName: string, + globalNames: Set, + diagnostics: MotionDiagnostic[] +): RoutineParameter[] { + const parameters: RoutineParameter[] = []; + const seen = new Set(); + for (const group of splitTopLevel(tokens, ",")) { + if (group.length === 0) { + continue; + } + let offset = 0; + let direction: RoutineParameterDirection = "in"; + const first = group[0]!; + if (isDirection(first)) { + direction = first.raw as RoutineParameterDirection; + offset = 1; + } + const typeName = group[offset]; + const name = group[offset + 1]; + if (!typeName || !name || !isIdentifierLike(typeName) || !isIdentifierLike(name)) { + throw routineError("GRL_PARAMETER_INVALID", `Invalid parameter list for ${routineName}`, tokenSourceMap(first)); + } + if (seen.has(name.raw)) { + throw routineError("GRL_PARAMETER_DUPLICATE", `Duplicate parameter ${name.raw}`, tokenSourceMap(name)); + } + seen.add(name.raw); + if (globalNames.has(name.raw)) { + diagnostics.push(diagnostic("warning", "GRL_NAME_SHADOWS_OUTER_SCOPE", `Parameter ${name.raw} shadows an outer name`, tokenSourceMap(name))); + } + parameters.push({ + name: name.raw, + typeName: typeName.raw, + direction, + sourceMap: tokenSourceMap(name) + }); + } + return parameters; +} + +function parseLocalDeclaration(tokens: GrlToken[]): { name: string; typeName: string; sourceMap: MotionSourceMap } | undefined { + const storage = tokens[0]; + if (!storage || !["var", "const", "persistent"].includes(storage.raw)) { + return undefined; + } + const typeName = tokens[1]; + const name = tokens[2]; + if (!typeName || !name || !isIdentifierLike(typeName) || !isIdentifierLike(name)) { + return undefined; + } + return { + name: name.raw, + typeName: typeName.raw, + sourceMap: tokenSourceMap(name) + }; +} + +function parseAssignment(tokens: GrlToken[]): { name: string; sourceMap: MotionSourceMap } | undefined { + const name = tokens[0]; + const operator = tokens[1]; + if (!name || !operator || !isIdentifierLike(name) || operator.kind !== "operator" || (operator.raw !== "=" && operator.raw !== ":=")) { + return undefined; + } + return { + name: name.raw, + sourceMap: tokenSourceMap(name) + }; +} + +function parseCallStatement(tokens: GrlToken[]): CallInstruction | undefined { + const start = tokens[0]; + const target = tokens[1]; + if (!start || start.raw !== "call" || !target || !isIdentifierLike(target)) { + return undefined; + } + const argTokens = tokens.slice(2); + const args = parseCallArgs(argTokens); + return { + kind: "CALL", + target: target.raw, + args, + sourceMap: tokenSourceMap(start) + }; +} + +function parseReturnStatement(tokens: GrlToken[]): ReturnInstruction | undefined { + const start = tokens[0]; + if (!start || start.raw !== "return") { + return undefined; + } + const valueTokens = tokens.slice(1); + return { + kind: "RETURN", + ...(valueTokens.length > 0 ? { value: expressionFromTokens(valueTokens) } : {}), + sourceMap: tokenSourceMap(start) + }; +} + +function parseCallArgs(tokens: GrlToken[]): ControlExpression[] { + if (tokens[0]?.raw === "(" && tokens.at(-1)?.raw === ")") { + return splitTopLevel(tokens.slice(1, -1), ",").filter((group) => group.length > 0).map(expressionFromTokens); + } + return splitTopLevel(tokens, ",").filter((group) => group.length > 0).map(expressionFromTokens); +} + +function validateCall(call: CallInstruction, context: AnalysisContext): void { + const signature = context.signatures.get(call.target); + if (!signature) { + throw routineError("GRL_CALL_TARGET_NOT_FOUND", `Unknown call target ${call.target}`, call.sourceMap); + } + if (call.args.length !== signature.parameters.length) { + throw routineError("GRL_CALL_ARITY_MISMATCH", `Call ${call.target} expects ${signature.parameters.length} arguments`, call.sourceMap); + } + if (context.routineKind === "func" && signature.kind === "PROC_SIGNATURE") { + throw routineError("GRL_FUNC_SIDE_EFFECT", `Function ${context.routineName} cannot call procedure ${call.target}`, call.sourceMap); + } + + for (let index = 0; index < signature.parameters.length; index += 1) { + const parameter = signature.parameters[index]!; + const arg = call.args[index]!; + const argTokens = arg.tokens as GrlToken[] | undefined; + if ((parameter.direction === "out" || parameter.direction === "inout") && (!argTokens || !isLValueExpression(argTokens))) { + throw routineError("GRL_ARGUMENT_NOT_LVALUE", `${parameter.direction} argument ${parameter.name} must be a writable lvalue`, arg.sourceMap); + } + const actualType = inferExpressionType(argTokens ?? [], context); + if (!isTypeCompatible(parameter.typeName, actualType)) { + throw routineError("GRL_CALL_ARGUMENT_TYPE", `Argument ${index + 1} for ${call.target} is not compatible with ${parameter.typeName}`, arg.sourceMap); + } + } +} + +function applyCallAssignments(call: CallInstruction, assigned: Set, context: AnalysisContext): void { + const signature = context.signatures.get(call.target); + if (!signature) { + return; + } + for (let index = 0; index < signature.parameters.length; index += 1) { + const parameter = signature.parameters[index]!; + if (parameter.direction !== "out" && parameter.direction !== "inout") { + continue; + } + const argTokens = call.args[index]?.tokens as GrlToken[] | undefined; + const target = argTokens?.[0]; + if (target && isIdentifierLike(target)) { + assigned.add(target.raw); + } + } +} + +function validateReturn(returnInstruction: ReturnInstruction, context: AnalysisContext): void { + if (context.routineKind === "proc") { + if (returnInstruction.value) { + throw routineError("GRL_RETURN_VALUE_IN_PROC", "proc return cannot include a value", returnInstruction.sourceMap); + } + return; + } + + if (context.returnType === "void") { + if (returnInstruction.value) { + throw routineError("GRL_RETURN_TYPE_MISMATCH", "void function cannot return a value", returnInstruction.sourceMap); + } + return; + } + + if (!returnInstruction.value) { + throw routineError("GRL_RETURN_VALUE_MISSING", `Function ${context.routineName} must return ${context.returnType}`, returnInstruction.sourceMap); + } + + const actualType = inferExpressionType(returnInstruction.value.tokens as GrlToken[] | undefined ?? [], context); + if (!isTypeCompatible(context.returnType ?? "unknown", actualType)) { + throw routineError("GRL_RETURN_TYPE_MISMATCH", `Return value is not compatible with ${context.returnType}`, returnInstruction.value.sourceMap); + } +} + +function checkFunctionSideEffects(tokens: GrlToken[], context: AnalysisContext): void { + if (context.routineKind !== "func") { + return; + } + const first = tokens[0]; + if (!first) { + return; + } + if (["movej", "movel", "movec", "wait", "pulse", "run_path", "run_operation"].includes(first.raw)) { + throw routineError("GRL_FUNC_SIDE_EFFECT", `Function ${context.routineName} cannot execute ${first.raw}`, tokenSourceMap(first)); + } +} + +function inferExpressionType(tokens: GrlToken[], context: AnalysisContext): InferredType { + if (tokens.length === 0) { + return "unknown"; + } + if (tokens.some((token) => token.kind === "operator" && ["==", "!=", "<", ">", "<=", ">=", "&&", "||", "!"].includes(token.raw))) { + return "bool"; + } + if (tokens.length === 1) { + return inferSingleTokenType(tokens[0]!, context); + } + if (tokens[0] && isIdentifierLike(tokens[0]) && tokens[1]?.raw === "(") { + const signature = context.signatures.get(tokens[0].raw); + if (signature?.kind === "FUNC_SIGNATURE") { + return signature.returnType; + } + } + const operandTypes = tokens + .filter((token) => token.kind !== "operator" && token.kind !== "punctuation") + .map((token) => inferSingleTokenType(token, context)) + .filter((type) => type !== "unknown"); + if (operandTypes.length > 0 && operandTypes.every((type) => ["int", "real"].includes(type))) { + return operandTypes.includes("real") ? "real" : "int"; + } + return "unknown"; +} + +function inferSingleTokenType(token: GrlToken, context: AnalysisContext): InferredType { + if (token.kind === "number") { + if (token.unit?.kind === "time") return "time"; + if (token.unit?.kind === "length") return "length"; + if (token.unit?.kind === "angle") return "angle"; + if (token.unit?.kind === "percent") return "percent"; + return Number.isInteger(token.value) ? "int" : "real"; + } + if (token.kind === "string") { + return "string"; + } + if (token.kind === "keyword" && (token.raw === "true" || token.raw === "false")) { + return "bool"; + } + if (isIdentifierLike(token)) { + return context.symbols.get(token.raw) ?? "unknown"; + } + return "unknown"; +} + +function isTypeCompatible(expected: string, actual: InferredType): boolean { + if (expected === "unknown" || actual === "unknown") { + return true; + } + if (expected === actual) { + return true; + } + return expected === "real" && actual === "int"; +} + +function isLValueExpression(tokens: GrlToken[]): boolean { + const first = tokens[0]; + if (!first || !isIdentifierLike(first) || ["true", "false"].includes(first.raw)) { + return false; + } + return !tokens.some((token) => token.kind === "operator"); +} + +function expressionFromTokens(tokens: GrlToken[]): ControlExpression { + return { + text: stringifyTokens(tokens), + tokens, + ...(tokens[0] ? { sourceMap: tokenSourceMap(tokens[0]) } : {}) + }; +} + +function splitTopLevel(tokens: GrlToken[], separator: string): GrlToken[][] { + const groups: GrlToken[][] = []; + let current: GrlToken[] = []; + let parenDepth = 0; + let bracketDepth = 0; + let braceDepth = 0; + + for (const token of tokens) { + if ( + token.kind === "punctuation" && + token.raw === separator && + parenDepth === 0 && + bracketDepth === 0 && + braceDepth === 0 + ) { + groups.push(current); + current = []; + continue; + } + current.push(token); + if (token.kind === "punctuation") { + if (token.raw === "(") parenDepth += 1; + if (token.raw === ")") parenDepth = Math.max(0, parenDepth - 1); + if (token.raw === "[") bracketDepth += 1; + if (token.raw === "]") bracketDepth = Math.max(0, bracketDepth - 1); + if (token.raw === "{") braceDepth += 1; + if (token.raw === "}") braceDepth = Math.max(0, braceDepth - 1); + } + } + groups.push(current); + return groups; +} + +function collectGlobalNames(declarations: GrlTopLevelDeclaration[]): Set { + const names = new Set(); + for (const declaration of declarations) { + if ( + declaration.kind === "DataDeclaration" || + declaration.kind === "TargetDeclaration" || + declaration.kind === "PathDeclaration" || + declaration.kind === "OperationDeclaration" || + declaration.kind === "ProcedureDeclaration" || + declaration.kind === "FunctionDeclaration" + ) { + names.add(declaration.name); + } + } + return names; +} + +function isDirection(token: GrlToken | undefined): boolean { + return Boolean(token && (token.raw === "in" || token.raw === "out" || token.raw === "inout")); +} + +function isIdentifierLike(token: GrlToken): boolean { + return token.kind === "identifier" || token.kind === "keyword"; +} + +function stringifyTokens(tokens: GrlToken[]): string { + return tokens.map((token) => token.raw).join(" "); +} + +function tokenSourceMap(token: GrlToken): MotionSourceMap { + return { + line: token.range.start.line, + column: token.range.start.column + }; +} + +function rangeSourceMap(position: { line: number; column: number }): MotionSourceMap { + return { + line: position.line, + column: position.column + }; +} + +function diagnostic( + severity: MotionDiagnostic["severity"], + code: string, + message: string, + sourceMap?: MotionSourceMap +): MotionDiagnostic { + return { + severity, + code, + message, + ...(sourceMap ? { sourceMap } : {}) + }; +} + +function routineError(code: string, message: string, sourceMap?: MotionSourceMap): KdlStructuredError { + return new KdlStructuredError( + code, + message, + sourceMap + ? [ + { + severity: "error", + code, + message, + sourceMap + } + ] + : undefined + ); +} diff --git a/kdl-wasm/web/src/grl/semantic/compileSemantic.ts b/kdl-wasm/web/src/grl/semantic/compileSemantic.ts new file mode 100644 index 0000000..5ef9548 --- /dev/null +++ b/kdl-wasm/web/src/grl/semantic/compileSemantic.ts @@ -0,0 +1,549 @@ +import type { MotionDiagnostic, MotionSourceMap } from "../../kdl/types.js"; +import type { + GrlDataDeclaration, + GrlFunctionDeclaration, + GrlOperationDeclaration, + GrlPathDeclaration, + GrlProcedureDeclaration, + GrlProgram, + GrlRawTopLevelDeclaration, + GrlTargetDeclaration, + GrlTopLevelDeclaration +} from "../ast/index.js"; +import type { + AlarmInstruction, + BreakInstruction, + CallInstruction, + ContinueInstruction, + ControlFlowInstruction, + ExecutableBranch, + ExecutableInstruction, + ExecutableProcedure, + ExecutableSwitchCase, + IoFlowInstruction, + ProcedureFlowInstruction, + RawProcedureStatement, + ReturnInstruction, + SemanticProgramIr, + SemanticSourceMapEntry, + SemanticSymbol, + UnsupportedRuntimeInstruction +} from "../ir/index.js"; +import { + analyzeExceptionSemantics, + parseExceptionFlowStatements +} from "./compileException.js"; +import { analyzeProcFunctionSemantics } from "./compileProcFunction.js"; +import { + buildMotionContext, + compileMotionToKdlRequest, + compileOperation, + compilePathToPlanRequest, + parseProcedureMotionInstructions, + parseProcedureRunOperationStatements, + parseProcedureRunPathStatements +} from "./compileMotion.js"; +import { + parseControlFlowStatements +} from "./compileControlFlow.js"; +import { parseIoFlowStatements } from "./compileIo.js"; + +export interface SemanticCompileOptions { + startJoints: number[]; + sampleTime: number; +} + +const SEMANTIC_CHECKS = [ + "language/module/proc", + "const/var/persistent symbols", + "tool/frame/speed/zone", + "joint_target/pose_target", + "movej/movel/movec", + "path/point/event/run_path", + "operation/run_operation", + "io/wait/pulse", + "if/elseif/else/while/for/switch", + "call/return/break/continue", + "proc parameter directions", + "func returns and side effects", + "alarm/raise/try/catch", + "source map propagation", + "KDL motion request bridge", + "KDL path request bridge", + "duplicate symbol diagnostics", + "missing reference diagnostics", + "P1 unsupported diagnostics", + "operation action expansion", + "path event expansion", + "raw statement preservation" +] as const; + +export function compileSemanticProgram(program: GrlProgram, options: SemanticCompileOptions): SemanticProgramIr { + const declarations = program.module.declarations; + const diagnostics: MotionDiagnostic[] = []; + const sourceMap: SemanticSourceMapEntry[] = []; + const symbols = buildSemanticSymbols(declarations, diagnostics); + const motionContext = buildMotionContext( + declarations.filter( + (decl): decl is GrlDataDeclaration | GrlTargetDeclaration => + decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration" + ) + ); + const paths = declarations + .filter((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration") + .map((path) => compilePathToPlanRequest(path, motionContext, options)); + const pathDeclarations = new Map( + declarations + .filter((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration") + .map((path) => [path.name, path]) + ); + const operations = declarations + .filter((decl): decl is GrlOperationDeclaration => decl.kind === "OperationDeclaration") + .map((operation) => compileOperation(operation, pathDeclarations)); + const procFunction = analyzeProcFunctionSemantics(declarations); + const exceptionAnalysis = analyzeExceptionSemantics(declarations); + diagnostics.push(...procFunction.diagnostics, ...exceptionAnalysis.diagnostics); + const procedures = declarations + .filter((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration") + .map((procedure) => compileExecutableProcedure(procedure, motionContext, diagnostics)); + + for (const path of paths) { + collectPathSourceMaps(path, sourceMap); + } + for (const operation of operations) { + collectOperationSourceMaps(operation, sourceMap); + } + for (const procedure of procedures) { + collectExecutableSourceMaps(procedure.instructions, sourceMap, { procedureId: procedure.name }); + } + + return { + moduleName: program.module.name, + symbols, + semanticChecks: [...SEMANTIC_CHECKS], + procedures, + paths, + operations, + diagnostics, + sourceMap, + kdlBridge: { + motionRequests: procedures.flatMap((procedure) => + procedure.instructions.flatMap((instruction) => + instruction.kind === "MOVEJ" || instruction.kind === "MOVEL" || instruction.kind === "MOVEC" + ? [compileMotionToKdlRequest(instruction, options)] + : [] + ) + ), + pathRequests: paths.map((path) => path.request) + } + }; +} + +function compileExecutableProcedure( + procedure: GrlProcedureDeclaration, + motionContext: ReturnType, + diagnostics: MotionDiagnostic[] +): ExecutableProcedure { + const motion = parseProcedureMotionInstructions(procedure, cloneMotionContextForSemantic(motionContext)); + const io = parseIoFlowStatements(procedure.bodyTokens); + const runPaths = parseProcedureRunPathStatements(procedure); + const runOperations = parseProcedureRunOperationStatements(procedure); + const controls = parseControlFlowStatements(procedure.bodyTokens); + const exceptions = parseExceptionFlowStatements(procedure.bodyTokens); + const nestedControlLines = collectNestedControlLines(controls); + const topLevelMotion = motion.filter((instruction) => !isNestedInstruction(instruction, nestedControlLines)); + const topLevelIo = io.filter((instruction) => !isNestedInstruction(instruction, nestedControlLines)); + const topLevelRunPaths = runPaths.filter((instruction) => !isNestedInstruction(instruction, nestedControlLines)); + const topLevelRunOperations = runOperations.filter((instruction) => !isNestedInstruction(instruction, nestedControlLines)); + const topLevelExceptions = exceptions.filter( + (instruction): instruction is AlarmInstruction | UnsupportedRuntimeInstruction => + (instruction.kind === "ALARM" || instruction.kind === "UNSUPPORTED_RUNTIME") && + !isNestedInstruction(instruction, nestedControlLines) + ); + const topLevelStructuredLines = new Set([ + ...topLevelMotion, + ...topLevelIo, + ...topLevelRunPaths, + ...topLevelRunOperations, + ...topLevelExceptions + ].map((instruction) => instruction.sourceMap?.line).filter((line): line is number => line !== undefined)); + const instructions = mergeExecutableInstructions( + procedure.bodyTokens, + [ + ...topLevelMotion, + ...topLevelIo, + ...topLevelRunPaths, + ...topLevelRunOperations, + ...topLevelExceptions, + ...flattenControlInstructions(controls, topLevelStructuredLines), + ...extractRawCallsAndReturns(controls, topLevelStructuredLines) + ], + diagnostics + ); + + return { + name: procedure.name, + instructions, + sourceMap: rangeSourceMap(procedure.range.start) + }; +} + +function mergeExecutableInstructions( + tokens: GrlProcedureDeclaration["bodyTokens"], + instructions: ExecutableInstruction[], + diagnostics: MotionDiagnostic[] +): ExecutableInstruction[] { + const sorted = [...instructions].sort((left, right) => sourceOrder(left.sourceMap, right.sourceMap)); + const seen = new Set(); + const merged: ExecutableInstruction[] = []; + for (const instruction of sorted) { + const key = instructionKey(instruction); + if (key && seen.has(key)) { + continue; + } + if (key) { + seen.add(key); + } + merged.push(instruction); + } + for (const token of tokens) { + if (["catch", "finally", "end"].includes(token.raw)) { + continue; + } + if (!merged.some((instruction) => instruction.sourceMap?.line === token.range.start.line)) { + diagnostics.push(diagnostic("info", "GRL_RAW_STATEMENT_PRESERVED", `Statement ${token.raw} preserved as raw IR`, tokenSourceMap(token))); + } + } + return merged; +} + +function flattenControlInstructions(instructions: ProcedureFlowInstruction[], excludedRawLines = new Set()): ExecutableInstruction[] { + return instructions.flatMap((instruction): ExecutableInstruction[] => { + if (instruction.kind === "RAW_STATEMENT") { + return rawStatementToExecutable(instruction, excludedRawLines); + } + if (instruction.kind === "IF") { + return [ + { + kind: "EXEC_IF", + branches: instruction.branches.map((branch): ExecutableBranch => ({ + branchKind: branch.branchKind, + ...(branch.condition ? { condition: branch.condition } : {}), + body: flattenProcedureFlow(branch.body), + ...(branch.sourceMap ? { sourceMap: branch.sourceMap } : {}) + })), + ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}) + } + ]; + } + if (instruction.kind === "WHILE") { + return [ + { + kind: "EXEC_WHILE", + condition: instruction.condition, + body: flattenProcedureFlow(instruction.body), + ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}) + } + ]; + } + if (instruction.kind === "FOR") { + return [ + { + kind: "EXEC_FOR", + iterator: instruction.iterator, + from: instruction.from, + to: instruction.to, + ...(instruction.step ? { step: instruction.step } : {}), + body: flattenProcedureFlow(instruction.body), + ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}) + } + ]; + } + if (instruction.kind === "SWITCH") { + return [ + { + kind: "EXEC_SWITCH", + expression: instruction.expression, + cases: instruction.cases.map((switchCase): ExecutableSwitchCase => ({ + caseKind: switchCase.caseKind, + ...(switchCase.value !== undefined ? { value: switchCase.value } : {}), + ...(switchCase.raw ? { raw: switchCase.raw } : {}), + body: flattenProcedureFlow(switchCase.body), + ...(switchCase.sourceMap ? { sourceMap: switchCase.sourceMap } : {}) + })), + ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}) + } + ]; + } + if (instruction.kind === "BREAK" || instruction.kind === "CONTINUE") { + return [instruction as BreakInstruction | ContinueInstruction]; + } + return []; + }); +} + +function flattenProcedureFlow(instructions: ProcedureFlowInstruction[]): ExecutableInstruction[] { + const controls = instructions.filter((instruction): instruction is ControlFlowInstruction => instruction.kind !== "RAW_STATEMENT"); + const raw = instructions.filter((instruction): instruction is RawProcedureStatement => instruction.kind === "RAW_STATEMENT"); + return [...flattenControlInstructions(controls), ...extractRawCallsAndReturns(raw)]; +} + +function extractRawCallsAndReturns( + instructions: Array, + excludedRawLines = new Set() +): ExecutableInstruction[] { + const extracted: ExecutableInstruction[] = []; + for (const instruction of instructions) { + if (instruction.kind !== "RAW_STATEMENT") { + continue; + } + extracted.push(...rawStatementToExecutable(instruction, excludedRawLines)); + } + return extracted; +} + +function rawStatementToExecutable( + instruction: RawProcedureStatement, + excludedRawLines = new Set() +): ExecutableInstruction[] { + if (instruction.sourceMap?.line && excludedRawLines.has(instruction.sourceMap.line)) { + return []; + } + const tokens = instruction.tokens as { raw: string }[] | undefined; + const first = tokens?.[0]?.raw; + const second = tokens?.[1]?.raw; + if (first && ["set_tool", "set_frame", "set_speed", "set_zone"].includes(first)) { + return []; + } + const typedTokens = instruction.tokens as GrlProcedureDeclaration["bodyTokens"] | undefined; + if (typedTokens && (first === "io" || first === "wait" || first === "pulse")) { + return parseIoFlowStatements(typedTokens); + } + if (typedTokens && (first === "alarm" || first === "raise" || first === "enable" || first === "disable")) { + return parseExceptionFlowStatements(typedTokens).filter( + (item): item is AlarmInstruction | UnsupportedRuntimeInstruction => item.kind === "ALARM" || item.kind === "UNSUPPORTED_RUNTIME" + ); + } + if (first === "call" && second) { + return [{ + kind: "CALL" as const, + target: second, + args: [], + ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}) + }]; + } + if (first === "return") { + return [{ + kind: "RETURN" as const, + ...(instruction.sourceMap ? { sourceMap: instruction.sourceMap } : {}) + }]; + } + return [instruction]; +} + +function collectNestedControlLines(instructions: ProcedureFlowInstruction[]): Set { + const lines = new Set(); + for (const instruction of instructions) { + if (instruction.kind === "RAW_STATEMENT") { + continue; + } + if (instruction.kind === "IF") { + for (const branch of instruction.branches) { + collectFlowLines(branch.body, lines); + } + } else if (instruction.kind === "WHILE" || instruction.kind === "FOR") { + collectFlowLines(instruction.body, lines); + } else if (instruction.kind === "SWITCH") { + for (const switchCase of instruction.cases) { + collectFlowLines(switchCase.body, lines); + } + } + } + return lines; +} + +function collectFlowLines(instructions: ProcedureFlowInstruction[], lines: Set): void { + for (const instruction of instructions) { + if (instruction.sourceMap?.line) { + lines.add(instruction.sourceMap.line); + } + if (instruction.kind === "IF") { + for (const branch of instruction.branches) { + collectFlowLines(branch.body, lines); + } + } else if (instruction.kind === "WHILE" || instruction.kind === "FOR") { + collectFlowLines(instruction.body, lines); + } else if (instruction.kind === "SWITCH") { + for (const switchCase of instruction.cases) { + collectFlowLines(switchCase.body, lines); + } + } + } +} + +function isNestedInstruction(instruction: { sourceMap?: MotionSourceMap }, nestedLines: Set): boolean { + return Boolean(instruction.sourceMap?.line && nestedLines.has(instruction.sourceMap.line)); +} + +function buildSemanticSymbols(declarations: GrlTopLevelDeclaration[], diagnostics: MotionDiagnostic[]): SemanticSymbol[] { + const symbols: SemanticSymbol[] = []; + const seen = new Map(); + for (const declaration of declarations) { + const symbol = symbolFromDeclaration(declaration); + if (!symbol) { + continue; + } + const previous = seen.get(symbol.name); + if (previous) { + diagnostics.push(diagnostic("error", "GRL_SYMBOL_DUPLICATE", `Duplicate symbol ${symbol.name}`, symbol.sourceMap)); + } + seen.set(symbol.name, symbol); + symbols.push(symbol); + } + return symbols; +} + +function symbolFromDeclaration(declaration: GrlTopLevelDeclaration): SemanticSymbol | undefined { + if (declaration.kind === "DataDeclaration") { + return { kind: "data", name: declaration.name, typeName: declaration.typeName, sourceMap: rangeSourceMap(declaration.range.start) }; + } + if (declaration.kind === "TargetDeclaration") { + return { kind: "target", name: declaration.name, sourceMap: rangeSourceMap(declaration.range.start) }; + } + if (declaration.kind === "PathDeclaration") { + return { kind: "path", name: declaration.name, sourceMap: rangeSourceMap(declaration.range.start) }; + } + if (declaration.kind === "OperationDeclaration") { + return { kind: "operation", name: declaration.name, sourceMap: rangeSourceMap(declaration.range.start) }; + } + if (declaration.kind === "ProcedureDeclaration") { + return { kind: "procedure", name: declaration.name, sourceMap: rangeSourceMap(declaration.range.start) }; + } + if (declaration.kind === "FunctionDeclaration") { + return { kind: "function", name: declaration.name, typeName: declaration.returnType, sourceMap: rangeSourceMap(declaration.range.start) }; + } + if (declaration.kind === "RawTopLevelDeclaration") { + const name = declaration.tokens[1]?.raw ?? declaration.declarationType; + return { kind: "raw", name, typeName: declaration.declarationType, sourceMap: rangeSourceMap(declaration.range.start) }; + } + return undefined; +} + +function collectPathSourceMaps(path: SemanticProgramIr["paths"][number], sourceMap: SemanticSourceMapEntry[]): void { + for (const segment of path.request.segments) { + if (segment.sourceMap) { + sourceMap.push({ + kind: "path_point", + id: segment.id ?? `${path.pathId}:${segment.motion}`, + pathId: path.pathId, + ...(segment.id ? { pointId: segment.id } : {}), + sourceMap: segment.sourceMap + }); + } + } + for (const event of path.events) { + if (event.sourceMap) { + sourceMap.push({ + kind: "path_event", + id: event.id ?? `${path.pathId}:event`, + pathId: path.pathId, + pointId: event.pointId, + sourceMap: event.sourceMap + }); + } + } +} + +function collectOperationSourceMaps(operation: SemanticProgramIr["operations"][number], sourceMap: SemanticSourceMapEntry[]): void { + for (const action of [...operation.startActions, ...operation.endActions]) { + if (action.sourceMap) { + sourceMap.push({ + kind: "operation_action", + id: `${operation.operationId}:${action.actionKind}`, + operationId: operation.operationId, + sourceMap: action.sourceMap + }); + } + } +} + +function collectExecutableSourceMaps( + instructions: ExecutableInstruction[], + sourceMap: SemanticSourceMapEntry[], + context: { procedureId: string } +): void { + for (const instruction of instructions) { + if (instruction.sourceMap) { + sourceMap.push({ + kind: instruction.kind, + id: `${context.procedureId}:${instruction.kind}:${instruction.sourceMap.line ?? 0}:${instruction.sourceMap.column ?? 0}`, + procedureId: context.procedureId, + sourceMap: instruction.sourceMap + }); + } + if (instruction.kind === "EXEC_IF") { + for (const branch of instruction.branches) { + collectExecutableSourceMaps(branch.body, sourceMap, context); + } + } else if (instruction.kind === "EXEC_WHILE" || instruction.kind === "EXEC_FOR") { + collectExecutableSourceMaps(instruction.body, sourceMap, context); + } else if (instruction.kind === "EXEC_SWITCH") { + for (const switchCase of instruction.cases) { + collectExecutableSourceMaps(switchCase.body, sourceMap, context); + } + } + } +} + +function cloneMotionContextForSemantic(context: ReturnType): ReturnType { + return { + targets: context.targets, + speeds: context.speeds, + zones: context.zones, + tools: context.tools, + frames: context.frames, + ...(context.currentSpeed ? { currentSpeed: context.currentSpeed } : {}), + ...(context.currentZone ? { currentZone: context.currentZone } : {}), + ...(context.currentTool ? { currentTool: context.currentTool } : {}), + ...(context.currentFrame ? { currentFrame: context.currentFrame } : {}) + }; +} + +function sourceOrder(left: MotionSourceMap | undefined, right: MotionSourceMap | undefined): number { + return (left?.line ?? Number.MAX_SAFE_INTEGER) - (right?.line ?? Number.MAX_SAFE_INTEGER) || + (left?.column ?? Number.MAX_SAFE_INTEGER) - (right?.column ?? Number.MAX_SAFE_INTEGER); +} + +function instructionKey(instruction: ExecutableInstruction): string | undefined { + const line = instruction.sourceMap?.line; + const column = instruction.sourceMap?.column; + return line ? `${instruction.kind}:${line}:${column ?? 0}` : undefined; +} + +function tokenSourceMap(token: GrlProcedureDeclaration["bodyTokens"][number]): MotionSourceMap { + return { + line: token.range.start.line, + column: token.range.start.column + }; +} + +function rangeSourceMap(position: { line: number; column: number }): MotionSourceMap { + return { + line: position.line, + column: position.column + }; +} + +function diagnostic( + severity: MotionDiagnostic["severity"], + code: string, + message: string, + sourceMap?: MotionSourceMap +): MotionDiagnostic { + return { + severity, + code, + message, + ...(sourceMap ? { sourceMap } : {}) + }; +} diff --git a/kdl-wasm/web/src/grl/semantic/index.ts b/kdl-wasm/web/src/grl/semantic/index.ts new file mode 100644 index 0000000..d5c619c --- /dev/null +++ b/kdl-wasm/web/src/grl/semantic/index.ts @@ -0,0 +1,49 @@ +export { + compileGrlDataDeclaration, + compileGrlTargetDeclaration, + compileOffsetExpression, + compileSpeedExpression, + compileTargetExpression, + compileZoneExpression, + type CompiledGrlDataDeclaration, + type CompiledGrlDataValue, + type CompiledGrlTargetDeclaration +} from "./compileData.js"; +export { + buildMotionContext, + compileOperation, + compilePathToPlanRequest, + compileMotionToKdlRequest, + expandRunOperation, + parseProcedureMotionInstructions, + parseProcedureRunOperationStatements, + parseProcedureRunPathStatements, + type GrlMotionContext, + type PathCompileOptions, + type MotionRequestOptions +} from "./compileMotion.js"; +export { + compileOperationActionIo, + compilePathEventIo, + parseIoFlowStatements, + type IoMap +} from "./compileIo.js"; +export { + parseControlFlowStatements, + parseProcedureControlFlow +} from "./compileControlFlow.js"; +export { + analyzeProcFunctionSemantics, + compileFunctionSignature, + compileProcedureSignature +} from "./compileProcFunction.js"; +export { + analyzeExceptionSemantics, + parseExceptionFlowStatements, + parseProcedureExceptionFlow, + type ExceptionAnalysis +} from "./compileException.js"; +export { + compileSemanticProgram, + type SemanticCompileOptions +} from "./compileSemantic.js"; diff --git a/kdl-wasm/web/src/kdl/kdl.worker.ts b/kdl-wasm/web/src/kdl/kdl.worker.ts new file mode 100644 index 0000000..8f942b8 --- /dev/null +++ b/kdl-wasm/web/src/kdl/kdl.worker.ts @@ -0,0 +1,16 @@ +import { createDefaultNativeKdlModuleLoader } from "./nativeModule.js"; +import { createKdlWorkerRuntime } from "./runtime.js"; +import { dispatchKdlRpcRequest } from "./workerRpc.js"; +import type { KdlRpcRequest } from "./rpc.js"; + +const runtime = createKdlWorkerRuntime(createDefaultNativeKdlModuleLoader()); +const workerScope = globalThis as unknown as { + onmessage: ((event: MessageEvent>) => void) | null; + postMessage: (message: unknown, transfer?: Transferable[]) => void; +}; + +workerScope.onmessage = (event) => { + void dispatchKdlRpcRequest(runtime, event.data).then((response) => { + workerScope.postMessage(response); + }); +}; diff --git a/kdl-wasm/web/src/kdl/kdlClient.ts b/kdl-wasm/web/src/kdl/kdlClient.ts new file mode 100644 index 0000000..d831da7 --- /dev/null +++ b/kdl-wasm/web/src/kdl/kdlClient.ts @@ -0,0 +1,318 @@ +import { + KdlStructuredError, + rpcErrorToException, + type KdlRpcRequest, + type KdlRpcResponse +} from "./rpc.js"; +import type { + CycleTimeResult, + JointLimits, + FkOptions, + FkResult, + IkOptions, + IkResult, + JacobianOptions, + JacobianResult, + KdlApiMethod, + KdlInitOptions, + KdlRuntimeInfo, + KdlWasmApi, + LinkPoseResult, + LimitCheckResult, + MoveCRequest, + MoveJRequest, + MoveLRequest, + NormalizedRobotModel, + OffsetSpec, + PathPlanRequest, + PathPlanResult, + PathValidationResult, + Pose, + PoseLike, + PoseNormalizeOptions, + PoseTarget, + ReachabilityResult, + RobotHandle, + RobotInfo, + SingularityResult, + TrapProfileOptions, + TrapProfileResult, + TrapSample, + TrajectoryResult, + UrdfLoadOptions +} from "./types.js"; + +export interface KdlWorkerLike { + postMessage(message: KdlRpcRequest, transfer?: Transferable[]): void; + terminate?: () => void; + addEventListener(type: "message", listener: (event: MessageEvent) => void): void; + addEventListener(type: "error", listener: (event: ErrorEvent) => void): void; + removeEventListener(type: "message", listener: (event: MessageEvent) => void): void; + removeEventListener(type: "error", listener: (event: ErrorEvent) => void): void; +} + +interface PendingCall { + method: KdlApiMethod; + resolve: (value: unknown) => void; + reject: (reason?: unknown) => void; +} + +export class KdlWorkerClient + implements + Pick< + KdlWasmApi, + | "init" + | "dispose" + | "loadRobotFromUrdf" + | "createRobotFromModel" + | "destroyRobot" + | "getRobotInfo" + | "getJointLimits" + | "normalizePose" + | "composePose" + | "inversePose" + | "applyToolAndFrame" + | "applyOffset" + | "makeTrapProfile" + | "sampleTrapProfile" + | "fk" + | "fkPose7" + | "fkAllLinks" + | "ik" + | "ikBatch" + | "jacobian" + | "checkSingularity" + | "checkJointLimits" + | "checkVelocityLimits" + | "checkReachability" + | "checkReachabilityBatch" + | "planMoveJ" + | "planMoveL" + | "planMoveC" + | "planPath" + | "validatePath" + | "estimateCycleTime" + | "resampleTrajectory" + > +{ + private nextId = 1; + private worker: KdlWorkerLike | undefined; + private readonly pending = new Map(); + private readonly handleMessage = (event: MessageEvent) => { + this.acceptResponse(event.data); + }; + private readonly handleError = (event: ErrorEvent) => { + this.failWorker(event.error instanceof Error ? event.error : new Error(event.message)); + }; + + constructor(private readonly createWorker: () => KdlWorkerLike) {} + + async init(options?: KdlInitOptions): Promise { + return this.call("init", options) as Promise; + } + + async dispose(): Promise { + if (!this.worker) { + return; + } + + try { + await this.call("dispose"); + } finally { + this.detachWorker(); + } + } + + async loadRobotFromUrdf(urdfXml: string, options: UrdfLoadOptions): Promise { + return this.call("loadRobotFromUrdf", urdfXml, options) as Promise; + } + + async createRobotFromModel(model: NormalizedRobotModel): Promise { + return this.call("createRobotFromModel", model) as Promise; + } + + async destroyRobot(handle: RobotHandle): Promise { + await this.call("destroyRobot", handle); + } + + async getRobotInfo(handle: RobotHandle): Promise { + return this.call("getRobotInfo", handle) as Promise; + } + + async getJointLimits(handle: RobotHandle): Promise { + return this.call("getJointLimits", handle) as Promise; + } + + async normalizePose(input: PoseLike, options?: PoseNormalizeOptions): Promise { + return this.call("normalizePose", input, options) as Promise; + } + + async composePose(a: Pose, b: Pose): Promise { + return this.call("composePose", a, b) as Promise; + } + + async inversePose(pose: Pose): Promise { + return this.call("inversePose", pose) as Promise; + } + + async applyToolAndFrame(target: PoseTarget, tool: Pose, frame: Pose): Promise { + return this.call("applyToolAndFrame", target, tool, frame) as Promise; + } + + async applyOffset(target: PoseTarget, offset: OffsetSpec): Promise { + return this.call("applyOffset", target, offset) as Promise; + } + + async makeTrapProfile(length: number, options: TrapProfileOptions): Promise { + return this.call("makeTrapProfile", length, options) as Promise; + } + + async sampleTrapProfile(length: number, options: TrapProfileOptions): Promise { + return this.call("sampleTrapProfile", length, options) as Promise; + } + + async fk(handle: RobotHandle, joints: Float64Array, options?: FkOptions): Promise { + return this.call("fk", handle, joints, options) as Promise; + } + + async fkPose7(handle: RobotHandle, joints: Float64Array, out?: Float64Array, options?: FkOptions): Promise { + return this.call("fkPose7", handle, joints, out, options) as Promise; + } + + async fkAllLinks(handle: RobotHandle, joints: Float64Array, options?: FkOptions): Promise { + return this.call("fkAllLinks", handle, joints, options) as Promise; + } + + async ik(handle: RobotHandle, seed: Float64Array, target: Pose, options?: IkOptions): Promise { + return this.call("ik", handle, seed, target, options) as Promise; + } + + async ikBatch(handle: RobotHandle, seeds: Float64Array[], targets: Pose[], options?: IkOptions): Promise { + return this.call("ikBatch", handle, seeds, targets, options) as Promise; + } + + async jacobian(handle: RobotHandle, joints: Float64Array, options?: JacobianOptions): Promise { + return this.call("jacobian", handle, joints, options) as Promise; + } + + async checkSingularity(handle: RobotHandle, joints: Float64Array): Promise { + return this.call("checkSingularity", handle, joints) as Promise; + } + + async checkJointLimits(handle: RobotHandle, joints: Float64Array): Promise { + return this.call("checkJointLimits", handle, joints) as Promise; + } + + async checkVelocityLimits(handle: RobotHandle, trajectory: TrajectoryResult): Promise { + return this.call("checkVelocityLimits", handle, trajectory) as Promise; + } + + async checkReachability(handle: RobotHandle, target: PoseTarget, options?: IkOptions): Promise { + return this.call("checkReachability", handle, target, options) as Promise; + } + + async checkReachabilityBatch( + handle: RobotHandle, + targets: PoseTarget[], + options?: IkOptions + ): Promise { + return this.call("checkReachabilityBatch", handle, targets, options) as Promise; + } + + async planMoveJ(handle: RobotHandle, request: MoveJRequest): Promise { + return this.call("planMoveJ", handle, request) as Promise; + } + + async planMoveL(handle: RobotHandle, request: MoveLRequest): Promise { + return this.call("planMoveL", handle, request) as Promise; + } + + async planMoveC(handle: RobotHandle, request: MoveCRequest): Promise { + return this.call("planMoveC", handle, request) as Promise; + } + + async planPath(handle: RobotHandle, request: PathPlanRequest): Promise { + return this.call("planPath", handle, request) as Promise; + } + + async validatePath(handle: RobotHandle, request: PathPlanRequest): Promise { + return this.call("validatePath", handle, request) as Promise; + } + + async estimateCycleTime(input: TrajectoryResult | PathPlanResult): Promise { + return this.call("estimateCycleTime", input) as Promise; + } + + async resampleTrajectory(trajectory: TrajectoryResult, sampleTime: number): Promise { + return this.call("resampleTrajectory", trajectory, sampleTime) as Promise; + } + + async call(method: KdlApiMethod, ...args: unknown[]): Promise { + const worker = this.ensureWorker(); + const id = this.nextId++; + const request: KdlRpcRequest = { + id, + method, + payload: args + }; + + return new Promise((resolve, reject) => { + this.pending.set(id, { method, resolve, reject }); + worker.postMessage(request); + }); + } + + private ensureWorker(): KdlWorkerLike { + if (this.worker) { + return this.worker; + } + + const worker = this.createWorker(); + worker.addEventListener("message", this.handleMessage); + worker.addEventListener("error", this.handleError); + this.worker = worker; + return worker; + } + + private acceptResponse(response: KdlRpcResponse): void { + const pending = this.pending.get(response.id); + if (!pending) { + return; + } + + this.pending.delete(response.id); + + if (response.ok) { + pending.resolve(response.result); + } else { + pending.reject( + rpcErrorToException( + response.error ?? { + code: "KDL_RPC_MISSING_ERROR", + message: `KDL worker returned a failed response for ${pending.method} without error details` + } + ) + ); + } + } + + private failWorker(error: Error): void { + const structured = new KdlStructuredError("KDL_WORKER_CRASHED", error.message); + for (const pending of this.pending.values()) { + pending.reject(structured); + } + this.pending.clear(); + this.detachWorker(); + } + + private detachWorker(): void { + if (!this.worker) { + return; + } + + this.worker.removeEventListener("message", this.handleMessage); + this.worker.removeEventListener("error", this.handleError); + this.worker.terminate?.(); + this.worker = undefined; + } +} diff --git a/kdl-wasm/web/src/kdl/nativeAbi.ts b/kdl-wasm/web/src/kdl/nativeAbi.ts new file mode 100644 index 0000000..fc0fc36 --- /dev/null +++ b/kdl-wasm/web/src/kdl/nativeAbi.ts @@ -0,0 +1,118 @@ +import { KdlStructuredError } from "./rpc.js"; +import type { KdlError } from "./types.js"; +import type { NativeKdlModule } from "./nativeModule.js"; + +export const KDL_C_ABI_EXPORTS = [ + "kdl_init", + "kdl_create_robot", + "kdl_destroy_robot", + "kdl_get_robot_info", + "kdl_fk", + "kdl_fk_all_links", + "kdl_jacobian", + "kdl_ik", + "kdl_plan_movej", + "kdl_plan_movel", + "kdl_plan_movec", + "kdl_plan_path", + "kdl_sample_trap", + "kdl_last_error" +] as const; + +export type KdlCAbiExport = (typeof KDL_C_ABI_EXPORTS)[number]; + +export class KdlNativeAbi { + constructor(private readonly module: NativeKdlModule) { + this.assertRuntimeMethods(); + } + + assertExports(names: readonly KdlCAbiExport[] = KDL_C_ABI_EXPORTS): void { + for (const name of names) { + try { + if (typeof this.module.cwrap?.(name, "number", []) === "function") { + continue; + } + } catch { + // Normalized below. + } + { + throw new KdlStructuredError("KDL_C_ABI_EXPORT_MISSING", `Missing C ABI export: ${name}`); + } + } + } + + callNumber(ident: KdlCAbiExport, argTypes: Array, args: unknown[]): number { + return Number(this.module.ccall(ident, "number", argTypes, args)); + } + + checkReturnCode(returnCode: number): void { + if (returnCode >= 0) { + return; + } + + const error = this.lastError(); + throw new KdlStructuredError(error.code, error.message, error.diagnostics); + } + + readJsonCall(ident: KdlCAbiExport, argTypes: Array, args: unknown[], bytes = 16_384): T { + const ptr = this.malloc(bytes); + try { + const returnCode = this.callNumber(ident, [...argTypes, "number", "number"], [...args, ptr, bytes]); + this.checkReturnCode(returnCode); + return JSON.parse(this.module.UTF8ToString?.(ptr) ?? "") as T; + } finally { + this.free(ptr); + } + } + + lastError(bytes = 16_384): KdlError { + const ptr = this.malloc(bytes); + try { + const returnCode = this.callNumber("kdl_last_error", ["number", "number"], [ptr, bytes]); + if (returnCode < 0) { + return { + code: "KDL_LAST_ERROR_FAILED", + message: "kdl_last_error failed", + diagnostics: [ + { + severity: "error", + code: "KDL_LAST_ERROR_FAILED", + message: "kdl_last_error failed" + } + ] + }; + } + return JSON.parse(this.module.UTF8ToString?.(ptr) ?? "") as KdlError; + } finally { + this.free(ptr); + } + } + + private assertRuntimeMethods(): void { + const missing = [ + ["cwrap", this.module.cwrap], + ["UTF8ToString", this.module.UTF8ToString], + ["_malloc", this.module._malloc], + ["_free", this.module._free] + ].flatMap(([name, value]) => (typeof value === "function" ? [] : [name as string])); + + if (missing.length > 0) { + throw new KdlStructuredError( + "KDL_C_ABI_RUNTIME_MISSING", + `KDL native module is missing runtime methods: ${missing.join(", ")}` + ); + } + } + + private malloc(bytes: number): number { + const ptr = this.module._malloc?.(bytes); + if (!ptr) { + throw new KdlStructuredError("KDL_WASM_ALLOC_FAILED", `Failed to allocate ${bytes} bytes`); + } + return ptr; + } + + private free(ptr: number): void { + this.module._free?.(ptr); + } +} diff --git a/kdl-wasm/web/src/kdl/nativeModule.ts b/kdl-wasm/web/src/kdl/nativeModule.ts new file mode 100644 index 0000000..36fb904 --- /dev/null +++ b/kdl-wasm/web/src/kdl/nativeModule.ts @@ -0,0 +1,58 @@ +import type { KdlInitOptions } from "./types.js"; + +export interface NativeKdlModule { + ccall: ( + ident: string, + returnType: string | null, + argTypes: Array, + args: unknown[] + ) => unknown; + cwrap?: ( + ident: string, + returnType: string | null, + argTypes: Array + ) => (...args: unknown[]) => unknown; + UTF8ToString?: (ptr: number) => string; + stringToUTF8?: (value: string, outPtr: number, maxBytesToWrite: number) => void; + lengthBytesUTF8?: (value: string) => number; + _malloc?: (size: number) => number; + _free?: (ptr: number) => void; + HEAPF64?: Float64Array; +} + +export type NativeKdlModuleLoader = (options: KdlInitOptions) => Promise; + +interface NativeKdlModuleFactoryOptions { + locateFile?: (path: string, prefix: string) => string; + print?: (text: string) => void; + printErr?: (text: string) => void; +} + +type NativeKdlModuleFactory = ( + options?: NativeKdlModuleFactoryOptions +) => Promise; + +export function createDefaultNativeKdlModuleLoader(defaultWrapperUrl?: string): NativeKdlModuleLoader { + return async (options) => { + const wrapperUrl = + options.wrapperUrl ?? defaultWrapperUrl ?? new URL("../../../build-wasm/kdl.js", import.meta.url).href; + const imported = (await import(/* @vite-ignore */ wrapperUrl)) as { + default?: NativeKdlModuleFactory; + createKdlModule?: NativeKdlModuleFactory; + }; + const factory = imported.default ?? imported.createKdlModule; + + if (typeof factory !== "function") { + throw new Error(`KDL WASM wrapper did not export a module factory: ${wrapperUrl}`); + } + + return factory({ + locateFile: (path, prefix) => { + if (path.endsWith(".wasm") && options.wasmUrl) { + return options.wasmUrl; + } + return new URL(path, prefix || wrapperUrl).href; + } + }); + }; +} diff --git a/kdl-wasm/web/src/kdl/performanceBaseline.ts b/kdl-wasm/web/src/kdl/performanceBaseline.ts new file mode 100644 index 0000000..26e1355 --- /dev/null +++ b/kdl-wasm/web/src/kdl/performanceBaseline.ts @@ -0,0 +1,256 @@ +import { KdlWorkerRuntime } from "./runtime.js"; +import type { PerformanceBaselineResult, PerformanceMetric, Pose, TrajectoryResult } from "./types.js"; + +const SIX_AXIS_URDF = ` + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +`; + +const PLANAR_URDF = ` + + + + + + + + + + + + + + + + + +`; + +export interface PerformanceBaselineOptions { + fkIterations?: number; + ikIterations?: number; + reachabilityTargets?: number; + sampleTime?: number; + trajectorySeconds?: number; +} + +export async function runPerformanceBaseline( + options: PerformanceBaselineOptions = {} +): Promise { + const fkIterations = options.fkIterations ?? 1_000; + const ikIterations = options.ikIterations ?? 200; + const reachabilityTargets = options.reachabilityTargets ?? 1_000; + const sampleTime = options.sampleTime ?? 0.004; + const trajectorySeconds = options.trajectorySeconds ?? 10; + const runtime = new KdlWorkerRuntime(); + const metrics: PerformanceMetric[] = []; + + const initStart = performance.now(); + await runtime.init({ wasmBuild: "performance-baseline" }); + const sixAxisHandle = await runtime.loadRobotFromUrdf(SIX_AXIS_URDF, { + robotId: "performance_6_axis", + baseLink: "base_link", + tipLink: "tool0" + }); + metrics.push(singleMetric("robot_init_6_axis", performance.now() - initStart, 1_000)); + + const fkInput = new Float64Array([0.1, -0.2, 0.15, 0.05, -0.1, 0.2]); + const fkOutput = new Float64Array(7); + metrics.push(await repeatedMetric("fk_pose7_typed_array", fkIterations, 1, () => { + return runtime.fkPose7(sixAxisHandle, fkInput, fkOutput); + })); + + const planarHandle = await runtime.loadRobotFromUrdf(PLANAR_URDF, { + robotId: "performance_planar", + baseLink: "base_link", + tipLink: "tool0" + }); + + const ikTargets = makeTargets(ikIterations, 0.15, 0.65); + metrics.push(await repeatedMetric("ik_planar_average", ikIterations, 10, (index) => { + return runtime.ik(planarHandle, new Float64Array([0, 0.2]), ikTargets[index]!, { + positionTolerance: 1e-9 + }); + })); + + const reachability = makePoseTargets(reachabilityTargets, 0.05, 0.95); + const reachabilityStart = performance.now(); + const reachabilityResult = await runtime.checkReachabilityBatch(planarHandle, reachability); + const reachabilityMs = performance.now() - reachabilityStart; + metrics.push({ + name: "reachability_batch_1000", + iterations: 1, + totalMs: reachabilityMs, + averageMs: reachabilityMs, + thresholdMs: 500, + points: reachabilityTargets, + ok: reachabilityMs <= 500 && reachabilityResult.length === reachabilityTargets + }); + + const trajectoryStart = performance.now(); + const trajectory = await runtime.planMoveJ(planarHandle, { + startJoints: [0, 0], + target: { + id: "ten_second_goal", + joints: [0, 1] + }, + speed: { + kind: "joint_abs", + velocity: 1 / trajectorySeconds, + acceleration: 1 + }, + zone: { + kind: "fine" + }, + sampleTime + }); + const trajectoryMs = performance.now() - trajectoryStart; + metrics.push(trajectoryMetric("trajectory_10s_4ms", trajectory, trajectoryMs, 500)); + + await runtime.dispose(); + return { + ok: metrics.every((metric) => metric.ok), + metrics, + diagnostics: metrics.flatMap((metric) => + metric.ok + ? [] + : [ + { + severity: "warning" as const, + code: "KDL_PERFORMANCE_BASELINE_MISS", + message: `${metric.name} exceeded ${metric.thresholdMs ?? "unbounded"} ms`, + data: { + metric + } + } + ] + ) + }; +} + +function singleMetric(name: string, totalMs: number, thresholdMs: number): PerformanceMetric { + return { + name, + iterations: 1, + totalMs, + averageMs: totalMs, + thresholdMs, + ok: totalMs <= thresholdMs + }; +} + +async function repeatedMetric( + name: string, + iterations: number, + thresholdMs: number, + fn: (index: number) => unknown | Promise +): Promise { + let maxMs = 0; + const start = performance.now(); + for (let index = 0; index < iterations; index += 1) { + const before = performance.now(); + await fn(index); + maxMs = Math.max(maxMs, performance.now() - before); + } + const totalMs = performance.now() - start; + const averageMs = totalMs / iterations; + return { + name, + iterations, + totalMs, + averageMs, + maxMs, + thresholdMs, + ok: averageMs <= thresholdMs + }; +} + +function trajectoryMetric( + name: string, + trajectory: TrajectoryResult, + totalMs: number, + thresholdMs: number +): PerformanceMetric { + const points = trajectory.points.length; + return { + name, + iterations: 1, + totalMs, + averageMs: totalMs, + thresholdMs, + points, + ok: trajectory.ok && points >= 2_500 && totalMs <= thresholdMs + }; +} + +function makeTargets(count: number, minRadius: number, maxRadius: number): Pose[] { + return Array.from({ length: count }, (_, index) => { + const ratio = count <= 1 ? 0 : index / (count - 1); + const angle = ratio * Math.PI * 2; + const radius = minRadius + (maxRadius - minRadius) * ((index % 97) / 96); + return pose(Math.cos(angle) * radius, Math.sin(angle) * radius); + }); +} + +function makePoseTargets(count: number, minRadius: number, maxRadius: number) { + return makeTargets(count, minRadius, maxRadius).map((poseValue, index) => ({ + id: `target_${index}`, + pose: poseValue + })); +} + +function pose(x: number, y: number): Pose { + return { + position: [x, y, 0], + quaternion: [0, 0, 0, 1] + }; +} diff --git a/kdl-wasm/web/src/kdl/poseApi.ts b/kdl-wasm/web/src/kdl/poseApi.ts new file mode 100644 index 0000000..702597c --- /dev/null +++ b/kdl-wasm/web/src/kdl/poseApi.ts @@ -0,0 +1,99 @@ +import { KdlStructuredError } from "./rpc.js"; +import { + composePose as composePoseMath, + inversePose as inversePoseMath, + normalizeQuaternion, + rpyToQuaternion +} from "../math/poseMath.js"; +import type { OffsetSpec, Pose, PoseLike, PoseTarget } from "./types.js"; + +export function normalizePose(input: PoseLike): Pose { + if (!input || typeof input !== "object") { + throw new KdlStructuredError("KDL_INVALID_POSE", "Pose input must be an object"); + } + + if ("position" in input && "quaternion" in input) { + return { + position: validateVector3(input.position, "position"), + quaternion: normalizeQuaternion(validateVector4(input.quaternion, "quaternion")) + }; + } + + if ("xyz" in input && "rpy" in input) { + return { + position: validateVector3(input.xyz, "xyz"), + quaternion: rpyToQuaternion(validateVector3(input.rpy, "rpy")) + }; + } + + if ("xyz" in input && "quat" in input) { + return { + position: validateVector3(input.xyz, "xyz"), + quaternion: normalizeQuaternion(validateVector4(input.quat, "quat")) + }; + } + + throw new KdlStructuredError("KDL_INVALID_POSE", "Pose input must contain position/quaternion, xyz/rpy, or xyz/quat"); +} + +export function composePose(a: Pose, b: Pose): Pose { + return composePoseMath(normalizePose(a), normalizePose(b)); +} + +export function inversePose(pose: Pose): Pose { + return inversePoseMath(normalizePose(pose)); +} + +export function applyToolAndFrame(target: PoseTarget, tool: Pose, frame: Pose): Pose { + return composePose(composePose(normalizePose(frame), normalizePose(target.pose)), normalizePose(tool)); +} + +export function applyOffset(target: PoseTarget, offset: OffsetSpec): PoseTarget { + const offsetPose = offsetToPose(offset); + const pose = offset.mode === "tool" ? composePose(target.pose, offsetPose) : composePose(offsetPose, target.pose); + + return { + ...target, + pose + }; +} + +function offsetToPose(offset: OffsetSpec): Pose { + const xyz: [number, number, number] = offset.xyz ? validateVector3(offset.xyz, "offset.xyz") : [0, 0, 0]; + if (offset.rpy && offset.quaternion) { + throw new KdlStructuredError("KDL_INVALID_OFFSET", "Offset cannot specify both rpy and quaternion"); + } + + if (offset.rpy) { + return { + position: xyz, + quaternion: rpyToQuaternion(validateVector3(offset.rpy, "offset.rpy")) + }; + } + + if (offset.quaternion) { + return { + position: xyz, + quaternion: normalizeQuaternion(validateVector4(offset.quaternion, "offset.quaternion")) + }; + } + + return { + position: xyz, + quaternion: [0, 0, 0, 1] + }; +} + +function validateVector3(value: unknown, field: string): [number, number, number] { + if (!Array.isArray(value) || value.length !== 3 || value.some((entry) => !Number.isFinite(entry))) { + throw new KdlStructuredError("KDL_INVALID_POSE", `${field} must contain 3 finite numbers`); + } + return [value[0]!, value[1]!, value[2]!]; +} + +function validateVector4(value: unknown, field: string): [number, number, number, number] { + if (!Array.isArray(value) || value.length !== 4 || value.some((entry) => !Number.isFinite(entry))) { + throw new KdlStructuredError("KDL_INVALID_POSE", `${field} must contain 4 finite numbers`); + } + return [value[0]!, value[1]!, value[2]!, value[3]!]; +} diff --git a/kdl-wasm/web/src/kdl/rpc.ts b/kdl-wasm/web/src/kdl/rpc.ts new file mode 100644 index 0000000..52dadbe --- /dev/null +++ b/kdl-wasm/web/src/kdl/rpc.ts @@ -0,0 +1,78 @@ +import type { KdlApiMethod, KdlError, KdlWasmApi, MotionDiagnostic } from "./types.js"; + +export interface KdlRpcRequest { + id: number; + method: KdlApiMethod; + payload: T; +} + +export interface KdlRpcErrorPayload { + code: string; + message: string; + diagnostics?: MotionDiagnostic[]; +} + +export interface KdlRpcResponse { + id: number; + ok: boolean; + result?: T; + error?: KdlRpcErrorPayload; +} + +export class KdlStructuredError extends Error implements KdlError { + readonly code: string; + readonly diagnostics: MotionDiagnostic[]; + + constructor(code: string, message: string, diagnostics?: MotionDiagnostic[]) { + super(message); + this.name = "KdlStructuredError"; + this.code = code; + this.diagnostics = diagnostics ?? [ + { + severity: "error", + code, + message + } + ]; + } +} + +export type KdlRuntimeHandlers = Partial<{ + [Method in keyof KdlWasmApi]: (...args: unknown[]) => Promise | unknown; +}>; + +export function createRpcError( + code: string, + message: string, + diagnostics?: MotionDiagnostic[] +): KdlRpcErrorPayload { + return { + code, + message, + diagnostics: + diagnostics ?? + [ + { + severity: "error", + code, + message + } + ] + }; +} + +export function normalizeThrownError(error: unknown): KdlRpcErrorPayload { + if (error instanceof KdlStructuredError) { + return createRpcError(error.code, error.message, error.diagnostics); + } + + if (error instanceof Error) { + return createRpcError("KDL_WORKER_ERROR", error.message); + } + + return createRpcError("KDL_WORKER_ERROR", String(error)); +} + +export function rpcErrorToException(error: KdlRpcErrorPayload): KdlStructuredError { + return new KdlStructuredError(error.code, error.message, error.diagnostics); +} diff --git a/kdl-wasm/web/src/kdl/runtime.ts b/kdl-wasm/web/src/kdl/runtime.ts new file mode 100644 index 0000000..34ecc8a --- /dev/null +++ b/kdl-wasm/web/src/kdl/runtime.ts @@ -0,0 +1,329 @@ +import { KdlStructuredError, type KdlRuntimeHandlers } from "./rpc.js"; +import type { NativeKdlModule, NativeKdlModuleLoader } from "./nativeModule.js"; +import { + applyOffset as applyOffsetToTarget, + applyToolAndFrame as applyToolAndFrameToTarget, + composePose as composeRuntimePose, + inversePose as inverseRuntimePose, + normalizePose as normalizeRuntimePose +} from "./poseApi.js"; +import { + makeTrapProfile as makeRuntimeTrapProfile, + sampleTrapProfile as sampleRuntimeTrapProfile +} from "./trapProfile.js"; +import { + estimateCycleTime as estimateRuntimeCycleTime, + resampleTrajectory as resampleRuntimeTrajectory +} from "./trajectoryUtils.js"; +import { loadRobotFromUrdfModel } from "../robot/urdfParser.js"; +import { RobotModelRegistry } from "../robot/normalizedRobotModel.js"; +import type { + FkOptions, + IkOptions, + JacobianOptions, + KdlInitOptions, + KdlRuntimeInfo, + MoveCRequest, + MoveJRequest, + MoveLRequest, + NormalizedRobotModel, + OffsetSpec, + PathPlanRequest, + PathPlanResult, + Pose, + PoseLike, + PoseNormalizeOptions, + PoseTarget, + RobotHandle, + TrapProfileOptions, + TrajectoryResult, + UrdfLoadOptions +} from "./types.js"; + +export class KdlWorkerRuntime { + private initialized = false; + private nativeModule: NativeKdlModule | undefined; + private readonly robots = new RobotModelRegistry(); + + constructor(private readonly loadNativeModule?: NativeKdlModuleLoader) {} + + async init(options?: KdlInitOptions): Promise { + if (this.loadNativeModule) { + await this.initNativeModule(options ?? {}); + } + + this.initialized = true; + + return { + version: "0.1.0", + wasmBuild: options?.wasmBuild ?? (this.nativeModule ? "wasm" : "stub"), + supportsThreads: options?.useThreads ?? false, + supportsWasmFs: false + }; + } + + async dispose(): Promise { + this.initialized = false; + this.nativeModule = undefined; + } + + assertInitialized(method: string): void { + if (!this.initialized) { + throw new KdlStructuredError( + "KDL_NOT_INITIALIZED", + `KDL runtime must be initialized before calling ${method}` + ); + } + } + + async loadRobotFromUrdf(urdfXml: string, options: UrdfLoadOptions): Promise { + this.assertInitialized("loadRobotFromUrdf"); + const model = loadRobotFromUrdfModel(urdfXml, options); + return this.createRobotFromModel(model); + } + + async createRobotFromModel(model: NormalizedRobotModel): Promise { + this.assertInitialized("createRobotFromModel"); + return this.robots.create(model); + } + + async destroyRobot(handle: RobotHandle): Promise { + this.assertInitialized("destroyRobot"); + this.robots.destroy(handle); + } + + async getRobotInfo(handle: RobotHandle) { + this.assertInitialized("getRobotInfo"); + return this.robots.getInfo(handle); + } + + async getJointLimits(handle: RobotHandle) { + this.assertInitialized("getJointLimits"); + return this.robots.getJointLimits(handle); + } + + async normalizePose(input: PoseLike, _options?: PoseNormalizeOptions) { + this.assertInitialized("normalizePose"); + return normalizeRuntimePose(input); + } + + async composePose(a: Pose, b: Pose) { + this.assertInitialized("composePose"); + return composeRuntimePose(a, b); + } + + async inversePose(pose: Pose) { + this.assertInitialized("inversePose"); + return inverseRuntimePose(pose); + } + + async applyToolAndFrame(target: PoseTarget, tool: Pose, frame: Pose) { + this.assertInitialized("applyToolAndFrame"); + return applyToolAndFrameToTarget(target, tool, frame); + } + + async applyOffset(target: PoseTarget, offset: OffsetSpec) { + this.assertInitialized("applyOffset"); + return applyOffsetToTarget(target, offset); + } + + async makeTrapProfile(length: number, options: TrapProfileOptions) { + this.assertInitialized("makeTrapProfile"); + return makeRuntimeTrapProfile(length, options); + } + + async sampleTrapProfile(length: number, options: TrapProfileOptions) { + this.assertInitialized("sampleTrapProfile"); + return sampleRuntimeTrapProfile(length, options); + } + + async fk(handle: RobotHandle, joints: Float64Array | number[], options?: FkOptions) { + this.assertInitialized("fk"); + return this.robots.fk(handle, joints, options); + } + + async fkPose7(handle: RobotHandle, joints: Float64Array | number[], out?: Float64Array, options?: FkOptions) { + this.assertInitialized("fkPose7"); + return this.robots.fkPose7(handle, joints, out, options); + } + + async fkAllLinks(handle: RobotHandle, joints: Float64Array | number[], options?: FkOptions) { + this.assertInitialized("fkAllLinks"); + return this.robots.fkAllLinks(handle, joints, options); + } + + async ik(handle: RobotHandle, seed: Float64Array | number[], target: Pose, options?: IkOptions) { + this.assertInitialized("ik"); + return this.robots.ik(handle, seed, target, options); + } + + async ikBatch( + handle: RobotHandle, + seeds: Array, + targets: Pose[], + options?: IkOptions + ) { + this.assertInitialized("ikBatch"); + return this.robots.ikBatch(handle, seeds, targets, options); + } + + async jacobian(handle: RobotHandle, joints: Float64Array | number[], options?: JacobianOptions) { + this.assertInitialized("jacobian"); + return this.robots.jacobian(handle, joints, options); + } + + async checkSingularity(handle: RobotHandle, joints: Float64Array | number[]) { + this.assertInitialized("checkSingularity"); + return this.robots.checkSingularity(handle, joints); + } + + async checkJointLimits(handle: RobotHandle, joints: Float64Array | number[]) { + this.assertInitialized("checkJointLimits"); + return this.robots.checkJointLimits(handle, joints); + } + + async checkVelocityLimits(handle: RobotHandle, trajectory: TrajectoryResult) { + this.assertInitialized("checkVelocityLimits"); + return this.robots.checkVelocityLimits(handle, trajectory); + } + + async checkReachability(handle: RobotHandle, target: PoseTarget, options?: IkOptions) { + this.assertInitialized("checkReachability"); + return this.robots.checkReachability(handle, target, options); + } + + async checkReachabilityBatch(handle: RobotHandle, targets: PoseTarget[], options?: IkOptions) { + this.assertInitialized("checkReachabilityBatch"); + return this.robots.checkReachabilityBatch(handle, targets, options); + } + + async planMoveJ(handle: RobotHandle, request: MoveJRequest) { + this.assertInitialized("planMoveJ"); + return this.robots.planMoveJ(handle, request); + } + + async planMoveL(handle: RobotHandle, request: MoveLRequest) { + this.assertInitialized("planMoveL"); + return this.robots.planMoveL(handle, request); + } + + async planMoveC(handle: RobotHandle, request: MoveCRequest) { + this.assertInitialized("planMoveC"); + return this.robots.planMoveC(handle, request); + } + + async planPath(handle: RobotHandle, request: PathPlanRequest) { + this.assertInitialized("planPath"); + return this.robots.planPath(handle, request); + } + + async validatePath(handle: RobotHandle, request: PathPlanRequest) { + this.assertInitialized("validatePath"); + return this.robots.validatePath(handle, request); + } + + async estimateCycleTime(input: TrajectoryResult | PathPlanResult) { + this.assertInitialized("estimateCycleTime"); + return estimateRuntimeCycleTime(input); + } + + async resampleTrajectory(trajectory: TrajectoryResult, sampleTime: number) { + this.assertInitialized("resampleTrajectory"); + return resampleRuntimeTrajectory(trajectory, sampleTime); + } + + private async initNativeModule(options: KdlInitOptions): Promise { + try { + const nativeModule = await this.loadNativeModule?.(options); + if (!nativeModule) { + throw new Error("No KDL native module was returned"); + } + + const result = nativeModule.ccall("kdl_init", "number", ["string"], [JSON.stringify(options)]); + if (Number(result) !== 0) { + throw new Error(`kdl_init returned ${String(result)}`); + } + + this.nativeModule = nativeModule; + } catch (error) { + const message = error instanceof Error ? error.message : String(error); + throw new KdlStructuredError( + "KDL_WASM_INIT_FAILED", + `Failed to initialize KDL WASM runtime: ${message}` + ); + } + } +} + +export function createKdlWorkerRuntime(loadNativeModule?: NativeKdlModuleLoader): KdlRuntimeHandlers { + const runtime = new KdlWorkerRuntime(loadNativeModule); + + return { + init: (options?: unknown) => runtime.init(options as KdlInitOptions | undefined), + dispose: () => runtime.dispose(), + loadRobotFromUrdf: (urdfXml: unknown, options: unknown) => + runtime.loadRobotFromUrdf(urdfXml as string, options as UrdfLoadOptions), + createRobotFromModel: (model: unknown) => runtime.createRobotFromModel(model as NormalizedRobotModel), + destroyRobot: (handle: unknown) => runtime.destroyRobot(handle as RobotHandle), + getRobotInfo: (handle: unknown) => runtime.getRobotInfo(handle as RobotHandle), + getJointLimits: (handle: unknown) => runtime.getJointLimits(handle as RobotHandle), + normalizePose: (input: unknown, options: unknown) => + runtime.normalizePose(input as PoseLike, options as PoseNormalizeOptions | undefined), + composePose: (a: unknown, b: unknown) => runtime.composePose(a as Pose, b as Pose), + inversePose: (pose: unknown) => runtime.inversePose(pose as Pose), + applyToolAndFrame: (target: unknown, tool: unknown, frame: unknown) => + runtime.applyToolAndFrame(target as PoseTarget, tool as Pose, frame as Pose), + applyOffset: (target: unknown, offset: unknown) => + runtime.applyOffset(target as PoseTarget, offset as OffsetSpec), + makeTrapProfile: (length: unknown, options: unknown) => + runtime.makeTrapProfile(length as number, options as TrapProfileOptions), + sampleTrapProfile: (length: unknown, options: unknown) => + runtime.sampleTrapProfile(length as number, options as TrapProfileOptions), + fk: (handle: unknown, joints: unknown, options: unknown) => + runtime.fk(handle as RobotHandle, joints as Float64Array | number[], options as FkOptions | undefined), + fkPose7: (handle: unknown, joints: unknown, out: unknown, options: unknown) => + runtime.fkPose7( + handle as RobotHandle, + joints as Float64Array | number[], + out as Float64Array | undefined, + options as FkOptions | undefined + ), + fkAllLinks: (handle: unknown, joints: unknown, options: unknown) => + runtime.fkAllLinks(handle as RobotHandle, joints as Float64Array | number[], options as FkOptions | undefined), + ik: (handle: unknown, seed: unknown, target: unknown, options: unknown) => + runtime.ik(handle as RobotHandle, seed as Float64Array | number[], target as Pose, options as IkOptions | undefined), + ikBatch: (handle: unknown, seeds: unknown, targets: unknown, options: unknown) => + runtime.ikBatch( + handle as RobotHandle, + seeds as Array, + targets as Pose[], + options as IkOptions | undefined + ), + jacobian: (handle: unknown, joints: unknown, options: unknown) => + runtime.jacobian(handle as RobotHandle, joints as Float64Array | number[], options as JacobianOptions | undefined), + checkSingularity: (handle: unknown, joints: unknown) => + runtime.checkSingularity(handle as RobotHandle, joints as Float64Array | number[]), + checkJointLimits: (handle: unknown, joints: unknown) => + runtime.checkJointLimits(handle as RobotHandle, joints as Float64Array | number[]), + checkVelocityLimits: (handle: unknown, trajectory: unknown) => + runtime.checkVelocityLimits(handle as RobotHandle, trajectory as TrajectoryResult), + checkReachability: (handle: unknown, target: unknown, options: unknown) => + runtime.checkReachability(handle as RobotHandle, target as PoseTarget, options as IkOptions | undefined), + checkReachabilityBatch: (handle: unknown, targets: unknown, options: unknown) => + runtime.checkReachabilityBatch(handle as RobotHandle, targets as PoseTarget[], options as IkOptions | undefined), + planMoveJ: (handle: unknown, request: unknown) => + runtime.planMoveJ(handle as RobotHandle, request as MoveJRequest), + planMoveL: (handle: unknown, request: unknown) => + runtime.planMoveL(handle as RobotHandle, request as MoveLRequest), + planMoveC: (handle: unknown, request: unknown) => + runtime.planMoveC(handle as RobotHandle, request as MoveCRequest), + planPath: (handle: unknown, request: unknown) => + runtime.planPath(handle as RobotHandle, request as PathPlanRequest), + validatePath: (handle: unknown, request: unknown) => + runtime.validatePath(handle as RobotHandle, request as PathPlanRequest), + estimateCycleTime: (input: unknown) => + runtime.estimateCycleTime(input as TrajectoryResult | PathPlanResult), + resampleTrajectory: (trajectory: unknown, sampleTime: unknown) => + runtime.resampleTrajectory(trajectory as TrajectoryResult, sampleTime as number) + }; +} diff --git a/kdl-wasm/web/src/kdl/trajectoryUtils.ts b/kdl-wasm/web/src/kdl/trajectoryUtils.ts new file mode 100644 index 0000000..71d0cb7 --- /dev/null +++ b/kdl-wasm/web/src/kdl/trajectoryUtils.ts @@ -0,0 +1,163 @@ +import { KdlStructuredError } from "./rpc.js"; +import type { + CycleTimeResult, + MotionDiagnostic, + PathPlanResult, + Pose, + TrajectoryPoint, + TrajectoryResult +} from "./types.js"; + +export function estimateCycleTime(input: TrajectoryResult | PathPlanResult): CycleTimeResult { + const diagnostics: MotionDiagnostic[] = input.diagnostics ?? []; + if (isPathPlanResult(input)) { + const segmentTimes = input.segments.map((segment) => + cycleTimeSegment(segment.motion, segment.duration, segment.points[0]?.segmentId) + ); + return { + ok: input.ok, + motionTime: input.duration, + totalTime: input.duration, + segmentTimes, + diagnostics + }; + } + + return { + ok: input.ok, + motionTime: input.duration, + totalTime: input.duration, + segmentTimes: [cycleTimeSegment(input.motion, input.duration, input.points[0]?.segmentId)], + diagnostics + }; +} + +function cycleTimeSegment( + motion: TrajectoryResult["motion"], + duration: number, + segmentId?: string +): CycleTimeResult["segmentTimes"][number] { + return { + motion, + duration, + ...(segmentId ? { segmentId } : {}) + }; +} + +export function resampleTrajectory(trajectory: TrajectoryResult, sampleTime: number): TrajectoryResult { + if (!Number.isFinite(sampleTime) || sampleTime <= 0) { + throw new KdlStructuredError("KDL_INVALID_SAMPLE_TIME", "sampleTime must be a finite positive number"); + } + if (trajectory.points.length === 0) { + return { + ...trajectory, + sampleTime, + diagnostics: [ + ...trajectory.diagnostics, + { + severity: "warning", + code: "KDL_RESAMPLE_EMPTY_TRAJECTORY", + message: "Cannot resample a trajectory without points" + } + ] + }; + } + + const duration = trajectory.duration; + const times = duration === 0 ? [0] : sampleTimes(duration, sampleTime); + const points = times.map((time, index) => { + const source = interpolatePoint(trajectory.points, time); + const previous = index > 0 ? times[index - 1]! : time; + return { + ...source, + index, + time, + dt: index === 0 ? 0 : time - previous + }; + }); + + return { + ...trajectory, + sampleTime, + points, + diagnostics: [ + ...trajectory.diagnostics, + { + severity: "info", + code: "KDL_TRAJECTORY_RESAMPLED", + message: `Trajectory was resampled to ${sampleTime}s` + } + ] + }; +} + +function isPathPlanResult(input: TrajectoryResult | PathPlanResult): input is PathPlanResult { + return "segments" in input; +} + +function sampleTimes(duration: number, sampleTime: number): number[] { + const times: number[] = [0]; + for (let time = sampleTime; time < duration - 1e-12; time += sampleTime) { + times.push(time); + } + times.push(duration); + return times; +} + +function interpolatePoint(points: TrajectoryPoint[], time: number): TrajectoryPoint { + if (time <= points[0]!.time) { + return { + ...points[0]!, + joints: [...points[0]!.joints], + jointVelocity: [...points[0]!.jointVelocity], + jointAcceleration: [...points[0]!.jointAcceleration] + }; + } + const last = points.at(-1)!; + if (time >= last.time) { + return { + ...last, + joints: [...last.joints], + jointVelocity: [...last.jointVelocity], + jointAcceleration: [...last.jointAcceleration] + }; + } + + const nextIndex = points.findIndex((point) => point.time >= time); + const next = points[nextIndex]!; + const prev = points[nextIndex - 1]!; + const ratio = (time - prev.time) / (next.time - prev.time); + return { + ...next, + time, + s: lerp(prev.s, next.s, ratio), + sd: lerp(prev.sd, next.sd, ratio), + sdd: lerp(prev.sdd, next.sdd, ratio), + joints: lerpArray(prev.joints, next.joints, ratio), + jointVelocity: lerpArray(prev.jointVelocity, next.jointVelocity, ratio), + jointAcceleration: lerpArray(prev.jointAcceleration, next.jointAcceleration, ratio), + flange: lerpPose(prev.flange, next.flange, ratio), + tcp: lerpPose(prev.tcp, next.tcp, ratio), + diagnostics: [] + }; +} + +function lerp(a: number, b: number, ratio: number): number { + return a + (b - a) * ratio; +} + +function lerpArray(a: number[], b: number[], ratio: number): number[] { + const length = Math.max(a.length, b.length); + return Array.from({ length }, (_, index) => lerp(a[index] ?? 0, b[index] ?? 0, ratio)); +} + +function lerpPose(a: Pose, b: Pose, ratio: number): Pose { + return { + position: [ + lerp(a.position[0], b.position[0], ratio), + lerp(a.position[1], b.position[1], ratio), + lerp(a.position[2], b.position[2], ratio) + ], + quaternion: ratio < 0.5 ? a.quaternion : b.quaternion + }; +} diff --git a/kdl-wasm/web/src/kdl/trapProfile.ts b/kdl-wasm/web/src/kdl/trapProfile.ts new file mode 100644 index 0000000..01dc4da --- /dev/null +++ b/kdl-wasm/web/src/kdl/trapProfile.ts @@ -0,0 +1,222 @@ +import { KdlStructuredError } from "./rpc.js"; +import type { MotionDiagnostic, TrapProfileOptions, TrapProfileResult, TrapSample } from "./types.js"; + +export function makeTrapProfile(length: number, options: TrapProfileOptions): TrapProfileResult { + validateTrapInputs(length, options); + + if (length === 0) { + return { + ok: true, + type: "triangle", + length, + duration: 0, + tAccel: 0, + tConst: 0, + tDecel: 0, + vPeak: 0, + samples: [ + { + index: 0, + time: 0, + s: 0, + sd: 0, + sdd: 0 + } + ], + diagnostics: [ + { + severity: "info", + code: "KDL_TRAP_ZERO_LENGTH", + message: "Trap profile length is zero" + } + ] + }; + } + + const startVelocity = options.startVelocity ?? 0; + const endVelocity = options.endVelocity ?? 0; + const maxVelocity = options.maxVelocity; + const maxAcceleration = options.maxAcceleration; + const dAccelToMax = distanceForVelocityChange(startVelocity, maxVelocity, maxAcceleration); + const dDecelFromMax = distanceForVelocityChange(endVelocity, maxVelocity, maxAcceleration); + const diagnostics: MotionDiagnostic[] = []; + let type: TrapProfileResult["type"] = "trapezoid"; + let vPeak = maxVelocity; + let tConst = 0; + + if (dAccelToMax + dDecelFromMax <= length) { + tConst = (length - dAccelToMax - dDecelFromMax) / maxVelocity; + } else { + type = "triangle"; + vPeak = Math.sqrt(Math.max(0, maxAcceleration * length + (startVelocity ** 2 + endVelocity ** 2) / 2)); + if (vPeak + 1e-12 < Math.max(startVelocity, endVelocity)) { + throw new KdlStructuredError( + "KDL_INVALID_TRAP_PROFILE", + "Profile length is too short for the requested startVelocity/endVelocity" + ); + } + tConst = 0; + diagnostics.push({ + severity: "info", + code: "KDL_TRAP_TRIANGLE_PROFILE", + message: "Trap profile length is too short to reach maxVelocity; using triangle profile" + }); + } + + const tAccel = Math.max(0, (vPeak - startVelocity) / maxAcceleration); + const tDecel = Math.max(0, (vPeak - endVelocity) / maxAcceleration); + const duration = tAccel + tConst + tDecel; + const samples = sampleProfile({ + length, + sampleTime: options.sampleTime, + startVelocity, + endVelocity, + maxAcceleration, + tAccel, + tConst, + tDecel, + vPeak, + duration + }); + + return { + ok: true, + type, + length, + duration, + tAccel, + tConst, + tDecel, + vPeak, + samples, + diagnostics + }; +} + +export function sampleTrapProfile(length: number, options: TrapProfileOptions): TrapSample[] { + return makeTrapProfile(length, options).samples; +} + +interface ProfileSegments { + length: number; + sampleTime: number; + startVelocity: number; + endVelocity: number; + maxAcceleration: number; + tAccel: number; + tConst: number; + tDecel: number; + vPeak: number; + duration: number; +} + +function validateTrapInputs(length: number, options: TrapProfileOptions): void { + if (!Number.isFinite(length) || length < 0) { + throw new KdlStructuredError("KDL_INVALID_TRAP_PROFILE", "Trap profile length must be a finite non-negative number"); + } + if (!Number.isFinite(options.maxVelocity) || options.maxVelocity <= 0) { + throw new KdlStructuredError("KDL_INVALID_TRAP_PROFILE", "maxVelocity must be a finite positive number"); + } + if (!Number.isFinite(options.maxAcceleration) || options.maxAcceleration <= 0) { + throw new KdlStructuredError("KDL_INVALID_TRAP_PROFILE", "maxAcceleration must be a finite positive number"); + } + if (!Number.isFinite(options.sampleTime) || options.sampleTime <= 0) { + throw new KdlStructuredError("KDL_INVALID_TRAP_PROFILE", "sampleTime must be a finite positive number"); + } + + const startVelocity = options.startVelocity ?? 0; + const endVelocity = options.endVelocity ?? 0; + if (length === 0 && (startVelocity > 0 || endVelocity > 0)) { + throw new KdlStructuredError( + "KDL_INVALID_TRAP_PROFILE", + "Zero-length trap profile requires zero startVelocity and endVelocity" + ); + } + if (!Number.isFinite(startVelocity) || startVelocity < 0 || startVelocity > options.maxVelocity) { + throw new KdlStructuredError( + "KDL_INVALID_TRAP_PROFILE", + "startVelocity must be finite, non-negative, and no greater than maxVelocity" + ); + } + if (!Number.isFinite(endVelocity) || endVelocity < 0 || endVelocity > options.maxVelocity) { + throw new KdlStructuredError( + "KDL_INVALID_TRAP_PROFILE", + "endVelocity must be finite, non-negative, and no greater than maxVelocity" + ); + } +} + +function distanceForVelocityChange(fromVelocity: number, toVelocity: number, acceleration: number): number { + return Math.max(0, (toVelocity ** 2 - fromVelocity ** 2) / (2 * acceleration)); +} + +function sampleProfile(profile: ProfileSegments): TrapSample[] { + if (profile.duration === 0) { + return [ + { + index: 0, + time: 0, + s: 0, + sd: 0, + sdd: 0 + } + ]; + } + + const times: number[] = [0]; + for (let time = profile.sampleTime; time < profile.duration - 1e-12; time += profile.sampleTime) { + times.push(time); + } + times.push(profile.duration); + + return times.map((time, index) => { + const sample = sampleAtTime(profile, time); + return { + index, + time, + s: index === 0 ? 0 : index === times.length - 1 ? 1 : clamp01(sample.distance / profile.length), + sd: sample.velocity / profile.length, + sdd: sample.acceleration / profile.length + }; + }); +} + +function sampleAtTime(profile: ProfileSegments, time: number): { + distance: number; + velocity: number; + acceleration: number; +} { + const accelDistance = + profile.startVelocity * profile.tAccel + 0.5 * profile.maxAcceleration * profile.tAccel ** 2; + const constDistance = profile.vPeak * profile.tConst; + const accelEnd = profile.tAccel; + const constEnd = profile.tAccel + profile.tConst; + + if (time <= accelEnd) { + return { + distance: profile.startVelocity * time + 0.5 * profile.maxAcceleration * time ** 2, + velocity: profile.startVelocity + profile.maxAcceleration * time, + acceleration: profile.maxAcceleration + }; + } + + if (time <= constEnd) { + const localTime = time - profile.tAccel; + return { + distance: accelDistance + profile.vPeak * localTime, + velocity: profile.vPeak, + acceleration: 0 + }; + } + + const localTime = Math.min(time - constEnd, profile.tDecel); + return { + distance: accelDistance + constDistance + profile.vPeak * localTime - 0.5 * profile.maxAcceleration * localTime ** 2, + velocity: Math.max(profile.endVelocity, profile.vPeak - profile.maxAcceleration * localTime), + acceleration: -profile.maxAcceleration + }; +} + +function clamp01(value: number): number { + return Math.min(1, Math.max(0, value)); +} diff --git a/kdl-wasm/web/src/kdl/types.ts b/kdl-wasm/web/src/kdl/types.ts new file mode 100644 index 0000000..3fb5349 --- /dev/null +++ b/kdl-wasm/web/src/kdl/types.ts @@ -0,0 +1,494 @@ +export type RobotHandle = number; + +export interface MotionSourceMap { + file?: string; + line?: number; + column?: number; + module?: string; +} + +export interface MotionDiagnostic { + severity: "info" | "warning" | "error"; + code: string; + message: string; + time?: number; + pointIndex?: number; + segmentId?: string; + targetId?: string; + sourceMap?: MotionSourceMap; + data?: Record; +} + +export interface KdlError { + code: string; + message: string; + diagnostics: MotionDiagnostic[]; +} + +export interface KdlInitOptions { + wrapperUrl?: string; + wasmUrl?: string; + useThreads?: boolean; + wasmBuild?: string; +} + +export interface KdlRuntimeInfo { + version: string; + kdlVersion?: string; + wasmBuild: string; + supportsThreads: boolean; + supportsWasmFs: boolean; +} + +export interface Pose { + position: [number, number, number]; + quaternion: [number, number, number, number]; +} + +export type PoseLike = + | Pose + | { xyz: [number, number, number]; rpy: [number, number, number] } + | { xyz: [number, number, number]; quat: [number, number, number, number] }; + +export interface RobotConfiguration { + shoulder?: -1 | 0 | 1; + elbow?: -1 | 0 | 1; + wrist?: -1 | 0 | 1; + turnNumbers?: number[]; +} + +export interface PoseTarget { + id?: string; + pose: Pose; + config?: RobotConfiguration; + tool?: Pose; + frame?: Pose; + extAxis?: number[]; + sourceMap?: MotionSourceMap; +} + +export interface JointTarget { + id?: string; + joints: number[]; + extAxis?: number[]; + sourceMap?: MotionSourceMap; +} + +export type SpeedSpec = + | { kind: "joint_percent"; value: number } + | { kind: "joint_abs"; velocity: number; acceleration?: number } + | { kind: "linear"; velocity: number; acceleration?: number; angularVelocity?: number }; + +export type ZoneSpec = + | { kind: "fine" } + | { kind: "distance"; value: number } + | { kind: "cnt"; value: number } + | { kind: "continuous" }; + +export interface JointLimits { + name: string; + lower: number; + upper: number; + velocity: number; + acceleration: number; + jerk?: number; +} + +export interface RobotInfo { + handle: RobotHandle; + robotId: string; + name: string; + baseLink: string; + tipLink: string; + dof: number; + jointNames: string[]; + limits: JointLimits[]; +} + +export type JointType = "revolute" | "continuous" | "prismatic" | "fixed"; + +export interface LinkModel { + name: string; +} + +export interface JointModel { + name: string; + type: JointType; + parent: string; + child: string; + origin: { + xyz: [number, number, number]; + rpy: [number, number, number]; + }; + axis: [number, number, number]; + limit?: JointLimits; +} + +export interface NormalizedRobotModel { + robotId: string; + baseLink: string; + tipLink: string; + name: string; + links: LinkModel[]; + joints: JointModel[]; + activeJointNames: string[]; + limits: JointLimits[]; + source: { + type: "urdf"; + urdfHash: string; + }; +} + +export type JsonObject = Record; +export interface JointLimitOverride { + name: string; + lower?: number; + upper?: number; + velocity?: number; + acceleration?: number; + jerk?: number; +} + +export interface UrdfLoadOptions { + robotId: string; + baseLink: string; + tipLink: string; + tool?: Pose; + base?: Pose; + jointOrder?: string[]; + overrideLimits?: JointLimitOverride[]; +} +export type PoseNormalizeOptions = JsonObject; +export interface OffsetSpec { + mode: "frame" | "tool" | "world"; + frameId?: string; + xyz?: [number, number, number]; + rpy?: [number, number, number]; + quaternion?: [number, number, number, number]; +} +export interface FkOptions { + tool?: Pose; + frame?: Pose; + includeFlange?: boolean; +} + +export interface FkResult { + ok: boolean; + flange: Pose; + tcp: Pose; + joints: number[]; + diagnostics: MotionDiagnostic[]; +} + +export type Pose7Array = Float64Array | [number, number, number, number, number, number, number] | number[]; + +export interface LinkPoseResult { + ok: boolean; + linkPoses: Array<{ link: string; pose: Pose }>; + diagnostics: MotionDiagnostic[]; +} +export type JacobianOptions = JsonObject; +export interface JacobianResult { + ok: boolean; + rows: number; + cols: number; + data: Float64Array | number[]; + diagnostics: MotionDiagnostic[]; +} +export interface IkOptions { + tool?: Pose; + frame?: Pose; + qMin?: number[]; + qMax?: number[]; + maxIterations?: number; + positionTolerance?: number; + orientationTolerance?: number; + seeds?: number[][]; + preferredConfig?: RobotConfiguration; + allowApproximate?: boolean; +} + +export interface IkResult { + ok: boolean; + joints?: number[]; + iterations: number; + residualPosition?: number; + residualOrientation?: number; + configuration?: RobotConfiguration; + reason?: "unreachable" | "joint_limit" | "singularity" | "max_iteration" | "invalid_model"; + diagnostics: MotionDiagnostic[]; +} +export interface LimitCheckResult { + ok: boolean; + diagnostics: MotionDiagnostic[]; + maxJointVelocityRatio?: number; + maxJointAccelerationRatio?: number; +} + +export interface SingularityResult { + ok: boolean; + nearSingularity: boolean; + manipulability?: number; + conditionNumber?: number; + diagnostics: MotionDiagnostic[]; +} + +export interface ReachabilityResult { + ok: boolean; + reachable: boolean; + targetId?: string; + joints?: number[]; + residualPosition?: number; + residualOrientation?: number; + nearestPose?: Pose; + diagnostics: MotionDiagnostic[]; +} +export interface TrapProfileOptions { + maxVelocity: number; + maxAcceleration: number; + sampleTime: number; + startVelocity?: number; + endVelocity?: number; +} + +export interface TrapSample { + index: number; + time: number; + s: number; + sd: number; + sdd: number; +} + +export interface TrapProfileResult { + ok: boolean; + type: "trapezoid" | "triangle"; + length: number; + duration: number; + tAccel: number; + tConst: number; + tDecel: number; + vPeak: number; + samples: TrapSample[]; + diagnostics: MotionDiagnostic[]; +} +export interface MoveJRequest { + startJoints: number[]; + target: JointTarget | PoseTarget; + speed: SpeedSpec; + zone: ZoneSpec; + tool?: Pose; + frame?: Pose; + sampleTime: number; + speedOverride?: number; + sourceMap?: MotionSourceMap; +} + +export interface MoveLRequest { + startJoints: number[]; + target: PoseTarget; + speed: SpeedSpec; + zone: ZoneSpec; + tool?: Pose; + frame?: Pose; + sampleTime: number; + orientationMode?: "fixed" | "slerp" | "tool_z_lock"; + ik?: IkOptions; + speedOverride?: number; + sourceMap?: MotionSourceMap; +} + +export interface MoveCRequest { + startJoints: number[]; + via: PoseTarget; + target: PoseTarget; + speed: SpeedSpec; + zone: ZoneSpec; + tool?: Pose; + frame?: Pose; + sampleTime: number; + orientationMode?: "fixed" | "slerp"; + arcMode?: "via" | "center" | "radius"; + circleDirection?: "short" | "long" | "cw" | "ccw"; + ik?: IkOptions; + speedOverride?: number; + sourceMap?: MotionSourceMap; +} +export type MotionKind = "MOVEJ" | "MOVEL" | "MOVEC"; + +export interface TrajectoryEvent { + id?: string; + time: number; + pointIndex: number; + kind: string; + sourceMap?: MotionSourceMap; + data?: JsonObject; +} + +export interface TrajectoryPoint { + index: number; + time: number; + dt: number; + s: number; + sd: number; + sdd: number; + joints: number[]; + jointVelocity: number[]; + jointAcceleration: number[]; + flange: Pose; + tcp: Pose; + tcpVelocity?: [number, number, number, number, number, number]; + tcpAcceleration?: [number, number, number, number, number, number]; + motion: MotionKind; + segmentId?: string; + targetId?: string; + sourceMap?: MotionSourceMap; + diagnostics: MotionDiagnostic[]; +} + +export interface TrajectoryResult { + ok: boolean; + motion: MotionKind; + duration: number; + sampleTime: number; + points: TrajectoryPoint[]; + events: TrajectoryEvent[]; + diagnostics: MotionDiagnostic[]; + meta?: JsonObject; +} +export interface MotionSegmentRequest { + id: string; + motion: MotionKind; + target?: JointTarget | PoseTarget; + via?: PoseTarget; + targetId?: string; + speed: SpeedSpec; + zone: ZoneSpec; + tool?: Pose; + frame?: Pose; + sourceMap?: MotionSourceMap; + source?: JsonObject; +} + +export interface PathEventRequest { + id?: string; + timing: "before" | "after" | "at"; + pointId: string; + distance?: number; + kind: string; + sourceMap?: MotionSourceMap; + data?: JsonObject; +} + +export interface PathPlanRequest { + pathId?: string; + startJoints: number[]; + segments: MotionSegmentRequest[]; + events?: PathEventRequest[]; + sampleTime: number; + speedOverride?: number; + stopOnError?: boolean; + source?: JsonObject; +} + +export interface PathPlanResult { + ok: boolean; + duration: number; + segments: TrajectoryResult[]; + points: TrajectoryPoint[]; + diagnostics: MotionDiagnostic[]; +} + +export interface SegmentValidationReport { + segmentId: string; + ok: boolean; + motion: MotionKind; + duration?: number; + maxJointVelocityRatio?: number; + maxJointAccelerationRatio?: number; + maxCartesianError?: number; + diagnostics: MotionDiagnostic[]; +} + +export interface PathValidationResult { + ok: boolean; + reachable: boolean; + cycleTime?: number; + segmentReports: SegmentValidationReport[]; + diagnostics: MotionDiagnostic[]; +} +export interface CycleTimeSegment { + segmentId?: string; + motion: MotionKind; + duration: number; +} + +export interface CycleTimeResult { + ok: boolean; + motionTime: number; + waitTime?: number; + ioTime?: number; + totalTime: number; + segmentTimes: CycleTimeSegment[]; + diagnostics: MotionDiagnostic[]; +} + +export interface PerformanceMetric { + name: string; + iterations: number; + totalMs: number; + averageMs: number; + thresholdMs?: number; + maxMs?: number; + points?: number; + ok: boolean; +} + +export interface PerformanceBaselineResult { + ok: boolean; + metrics: PerformanceMetric[]; + diagnostics: MotionDiagnostic[]; +} + +export interface KdlWasmApi { + init(options?: KdlInitOptions): Promise; + dispose(): Promise; + + loadRobotFromUrdf(urdfXml: string, options: UrdfLoadOptions): Promise; + createRobotFromModel(model: NormalizedRobotModel): Promise; + destroyRobot(handle: RobotHandle): Promise; + getRobotInfo(handle: RobotHandle): Promise; + getJointLimits(handle: RobotHandle): Promise; + + normalizePose(input: PoseLike, options?: PoseNormalizeOptions): Promise; + composePose(a: Pose, b: Pose): Promise; + inversePose(pose: Pose): Promise; + applyToolAndFrame(target: PoseTarget, tool: Pose, frame: Pose): Promise; + applyOffset(target: PoseTarget, offset: OffsetSpec): Promise; + + fk(handle: RobotHandle, joints: Float64Array, options?: FkOptions): Promise; + fkPose7(handle: RobotHandle, joints: Float64Array, out?: Float64Array, options?: FkOptions): Promise; + fkAllLinks(handle: RobotHandle, joints: Float64Array, options?: FkOptions): Promise; + jacobian(handle: RobotHandle, joints: Float64Array, options?: JacobianOptions): Promise; + ik(handle: RobotHandle, seed: Float64Array, target: Pose, options?: IkOptions): Promise; + ikBatch(handle: RobotHandle, seeds: Float64Array[], targets: Pose[], options?: IkOptions): Promise; + + checkJointLimits(handle: RobotHandle, joints: Float64Array): Promise; + checkVelocityLimits(handle: RobotHandle, trajectory: TrajectoryResult): Promise; + checkSingularity(handle: RobotHandle, joints: Float64Array): Promise; + checkReachability(handle: RobotHandle, target: PoseTarget, options?: IkOptions): Promise; + checkReachabilityBatch(handle: RobotHandle, targets: PoseTarget[], options?: IkOptions): Promise; + + makeTrapProfile(length: number, options: TrapProfileOptions): Promise; + sampleTrapProfile(length: number, options: TrapProfileOptions): Promise; + + planMoveJ(handle: RobotHandle, request: MoveJRequest): Promise; + planMoveL(handle: RobotHandle, request: MoveLRequest): Promise; + planMoveC(handle: RobotHandle, request: MoveCRequest): Promise; + + planPath(handle: RobotHandle, request: PathPlanRequest): Promise; + validatePath(handle: RobotHandle, request: PathPlanRequest): Promise; + estimateCycleTime(input: TrajectoryResult | PathPlanResult): Promise; + resampleTrajectory(trajectory: TrajectoryResult, sampleTime: number): Promise; +} + +export type KdlApiMethod = keyof KdlWasmApi; diff --git a/kdl-wasm/web/src/kdl/workerRpc.ts b/kdl-wasm/web/src/kdl/workerRpc.ts new file mode 100644 index 0000000..d5b02ed --- /dev/null +++ b/kdl-wasm/web/src/kdl/workerRpc.ts @@ -0,0 +1,49 @@ +import { + createRpcError, + normalizeThrownError, + type KdlRpcRequest, + type KdlRpcResponse, + type KdlRuntimeHandlers +} from "./rpc.js"; + +export async function dispatchKdlRpcRequest( + runtime: KdlRuntimeHandlers, + request: KdlRpcRequest +): Promise { + if (!Number.isInteger(request.id)) { + return { + id: Number.isFinite(request.id) ? request.id : -1, + ok: false, + error: createRpcError("KDL_RPC_INVALID_ID", "RPC request id must be an integer") + }; + } + + const handler = runtime[request.method]; + if (typeof handler !== "function") { + return { + id: request.id, + ok: false, + error: createRpcError( + "KDL_METHOD_NOT_IMPLEMENTED", + `${String(request.method)} is not implemented by the KDL worker runtime` + ) + }; + } + + const args = Array.isArray(request.payload) ? request.payload : [request.payload]; + + try { + const result = await handler(...args); + return { + id: request.id, + ok: true, + result + }; + } catch (error) { + return { + id: request.id, + ok: false, + error: normalizeThrownError(error) + }; + } +} diff --git a/kdl-wasm/web/src/math/poseMath.ts b/kdl-wasm/web/src/math/poseMath.ts new file mode 100644 index 0000000..afce1fb --- /dev/null +++ b/kdl-wasm/web/src/math/poseMath.ts @@ -0,0 +1,238 @@ +import type { Pose } from "../kdl/types.js"; + +export type Mat4 = [ + number, + number, + number, + number, + number, + number, + number, + number, + number, + number, + number, + number, + number, + number, + number, + number +]; + +export function identityMat4(): Mat4 { + return [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]; +} + +export function multiplyMat4(a: Mat4, b: Mat4): Mat4 { + const out = new Array(16).fill(0) as Mat4; + for (let row = 0; row < 4; row += 1) { + for (let col = 0; col < 4; col += 1) { + out[row * 4 + col] = + a[row * 4 + 0]! * b[col + 0]! + + a[row * 4 + 1]! * b[col + 4]! + + a[row * 4 + 2]! * b[col + 8]! + + a[row * 4 + 3]! * b[col + 12]!; + } + } + return out; +} + +export function translationMat4(xyz: [number, number, number]): Mat4 { + const [x, y, z] = xyz; + return [1, 0, 0, x, 0, 1, 0, y, 0, 0, 1, z, 0, 0, 0, 1]; +} + +export function rotationFromRpyMat4(rpy: [number, number, number]): Mat4 { + const [roll, pitch, yaw] = rpy; + const cr = Math.cos(roll); + const sr = Math.sin(roll); + const cp = Math.cos(pitch); + const sp = Math.sin(pitch); + const cy = Math.cos(yaw); + const sy = Math.sin(yaw); + + return [ + cy * cp, + cy * sp * sr - sy * cr, + cy * sp * cr + sy * sr, + 0, + sy * cp, + sy * sp * sr + cy * cr, + sy * sp * cr - cy * sr, + 0, + -sp, + cp * sr, + cp * cr, + 0, + 0, + 0, + 0, + 1 + ]; +} + +export function axisAngleMat4(axis: [number, number, number], angle: number): Mat4 { + const [nx, ny, nz] = normalizeVector(axis); + const c = Math.cos(angle); + const s = Math.sin(angle); + const t = 1 - c; + + return [ + t * nx * nx + c, + t * nx * ny - s * nz, + t * nx * nz + s * ny, + 0, + t * nx * ny + s * nz, + t * ny * ny + c, + t * ny * nz - s * nx, + 0, + t * nx * nz - s * ny, + t * ny * nz + s * nx, + t * nz * nz + c, + 0, + 0, + 0, + 0, + 1 + ]; +} + +export function mat4ToPose(matrix: Mat4): Pose { + return { + position: [matrix[3], matrix[7], matrix[11]], + quaternion: normalizeQuaternion(rotationMat4ToQuaternion(matrix)) + }; +} + +export function poseToMat4(pose: Pose): Mat4 { + const [x, y, z, w] = normalizeQuaternion(pose.quaternion); + const xx = x * x; + const yy = y * y; + const zz = z * z; + const xy = x * y; + const xz = x * z; + const yz = y * z; + const wx = w * x; + const wy = w * y; + const wz = w * z; + const [px, py, pz] = pose.position; + + return [ + 1 - 2 * (yy + zz), + 2 * (xy - wz), + 2 * (xz + wy), + px, + 2 * (xy + wz), + 1 - 2 * (xx + zz), + 2 * (yz - wx), + py, + 2 * (xz - wy), + 2 * (yz + wx), + 1 - 2 * (xx + yy), + pz, + 0, + 0, + 0, + 1 + ]; +} + +export function composePose(a: Pose, b: Pose): Pose { + return mat4ToPose(multiplyMat4(poseToMat4(a), poseToMat4(b))); +} + +export function inversePose(pose: Pose): Pose { + const matrix = poseToMat4(pose); + const r00 = matrix[0]; + const r01 = matrix[1]; + const r02 = matrix[2]; + const tx = matrix[3]; + const r10 = matrix[4]; + const r11 = matrix[5]; + const r12 = matrix[6]; + const ty = matrix[7]; + const r20 = matrix[8]; + const r21 = matrix[9]; + const r22 = matrix[10]; + const tz = matrix[11]; + + return mat4ToPose([ + r00, + r10, + r20, + -(r00 * tx + r10 * ty + r20 * tz), + r01, + r11, + r21, + -(r01 * tx + r11 * ty + r21 * tz), + r02, + r12, + r22, + -(r02 * tx + r12 * ty + r22 * tz), + 0, + 0, + 0, + 1 + ]); +} + +export function rpyToQuaternion(rpy: [number, number, number]): [number, number, number, number] { + return normalizeQuaternion(rotationMat4ToQuaternion(rotationFromRpyMat4(rpy))); +} + +export function normalizeQuaternion(input: [number, number, number, number]): [number, number, number, number] { + const [x, y, z, w] = input; + const length = Math.hypot(x, y, z, w); + if (length === 0) { + return [0, 0, 0, 1]; + } + return [x / length, y / length, z / length, w / length]; +} + +export function jointMotionMat4(type: string, axis: [number, number, number], value: number): Mat4 { + if (type === "revolute" || type === "continuous") { + return axisAngleMat4(axis, value); + } + if (type === "prismatic") { + const [x, y, z] = normalizeVector(axis); + return translationMat4([x * value, y * value, z * value]); + } + return identityMat4(); +} + +function rotationMat4ToQuaternion(matrix: Mat4): [number, number, number, number] { + const m00 = matrix[0]; + const m01 = matrix[1]; + const m02 = matrix[2]; + const m10 = matrix[4]; + const m11 = matrix[5]; + const m12 = matrix[6]; + const m20 = matrix[8]; + const m21 = matrix[9]; + const m22 = matrix[10]; + const trace = m00 + m11 + m22; + + if (trace > 0) { + const s = Math.sqrt(trace + 1) * 2; + return [(m21 - m12) / s, (m02 - m20) / s, (m10 - m01) / s, 0.25 * s]; + } + if (m00 > m11 && m00 > m22) { + const s = Math.sqrt(1 + m00 - m11 - m22) * 2; + return [0.25 * s, (m01 + m10) / s, (m02 + m20) / s, (m21 - m12) / s]; + } + if (m11 > m22) { + const s = Math.sqrt(1 + m11 - m00 - m22) * 2; + return [(m01 + m10) / s, 0.25 * s, (m12 + m21) / s, (m02 - m20) / s]; + } + const s = Math.sqrt(1 + m22 - m00 - m11) * 2; + return [(m02 + m20) / s, (m12 + m21) / s, 0.25 * s, (m10 - m01) / s]; +} + +function normalizeVector(axis: [number, number, number]): [number, number, number] { + const [x, y, z] = axis; + const length = Math.hypot(x, y, z); + if (length === 0) { + return [1, 0, 0]; + } + return [x / length, y / length, z / length]; +} diff --git a/kdl-wasm/web/src/robot/index.ts b/kdl-wasm/web/src/robot/index.ts new file mode 100644 index 0000000..60d9bb8 --- /dev/null +++ b/kdl-wasm/web/src/robot/index.ts @@ -0,0 +1,2 @@ +export { RobotModelRegistry, validateNormalizedRobotModel, type RobotRegistryRecord } from "./normalizedRobotModel.js"; +export { loadRobotFromUrdfModel } from "./urdfParser.js"; diff --git a/kdl-wasm/web/src/robot/normalizedRobotModel.ts b/kdl-wasm/web/src/robot/normalizedRobotModel.ts new file mode 100644 index 0000000..1c13a7e --- /dev/null +++ b/kdl-wasm/web/src/robot/normalizedRobotModel.ts @@ -0,0 +1,1781 @@ +import { KdlStructuredError } from "../kdl/rpc.js"; +import { makeTrapProfile } from "../kdl/trapProfile.js"; +import { + identityMat4, + jointMotionMat4, + type Mat4, + mat4ToPose, + multiplyMat4, + poseToMat4, + rotationFromRpyMat4, + translationMat4 +} from "../math/poseMath.js"; +import type { + FkOptions, + FkResult, + IkOptions, + IkResult, + JacobianOptions, + JacobianResult, + JointTarget, + JointLimits, + LinkPoseResult, + LimitCheckResult, + MotionDiagnostic, + MoveCRequest, + MoveJRequest, + MoveLRequest, + NormalizedRobotModel, + PathPlanRequest, + PathPlanResult, + PathValidationResult, + Pose, + PoseTarget, + ReachabilityResult, + RobotHandle, + RobotInfo, + SegmentValidationReport, + SpeedSpec, + SingularityResult, + TrajectoryPoint, + TrajectoryResult +} from "../kdl/types.js"; + +export interface RobotRegistryRecord { + handle: RobotHandle; + model: NormalizedRobotModel; +} + +export class RobotModelRegistry { + private nextHandle = 1; + private readonly robots = new Map(); + + create(model: NormalizedRobotModel): RobotHandle { + validateNormalizedRobotModel(model); + const handle = this.nextHandle++; + this.robots.set(handle, { handle, model }); + return handle; + } + + destroy(handle: RobotHandle): void { + this.ensure(handle); + this.robots.delete(handle); + } + + getInfo(handle: RobotHandle): RobotInfo { + const record = this.ensure(handle); + const model = record.model; + + return { + handle, + robotId: model.robotId, + name: model.name, + baseLink: model.baseLink, + tipLink: model.tipLink, + jointNames: model.activeJointNames, + dof: model.activeJointNames.length, + limits: model.limits + }; + } + + getJointLimits(handle: RobotHandle): JointLimits[] { + return this.ensure(handle).model.limits; + } + + fk(handle: RobotHandle, joints: Float64Array | number[], options: FkOptions = {}): FkResult { + const record = this.ensure(handle); + const jointValues = Array.from(joints); + validateJointVector(record.model, jointValues); + const linkPoses = computeLinkPoses(record.model, jointValues); + const flange = linkPoses.at(-1)?.pose ?? identityPose(); + const tcp = options.tool ? composeFkPose(flange, options.tool) : flange; + + return { + ok: true, + flange, + tcp: options.frame ? composeFkPose(options.frame, tcp) : tcp, + joints: jointValues, + diagnostics: [] + }; + } + + fkPose7( + handle: RobotHandle, + joints: Float64Array | number[], + out?: Float64Array, + options: FkOptions = {} + ): Float64Array { + const output = out ?? new Float64Array(7); + if (output.length < 7) { + throw new KdlStructuredError("KDL_OUTPUT_DIMENSION_MISMATCH", "fkPose7 output buffer must have length >= 7"); + } + + const result = this.fk(handle, joints, options); + output[0] = result.tcp.position[0]; + output[1] = result.tcp.position[1]; + output[2] = result.tcp.position[2]; + output[3] = result.tcp.quaternion[0]; + output[4] = result.tcp.quaternion[1]; + output[5] = result.tcp.quaternion[2]; + output[6] = result.tcp.quaternion[3]; + return output; + } + + fkAllLinks(handle: RobotHandle, joints: Float64Array | number[], options: FkOptions = {}): LinkPoseResult { + const record = this.ensure(handle); + const jointValues = Array.from(joints); + validateJointVector(record.model, jointValues); + const linkPoses = computeLinkPoses(record.model, jointValues); + + return { + ok: true, + linkPoses: options.includeFlange === false ? linkPoses.slice(0, -1) : linkPoses, + diagnostics: [] + }; + } + + ik(handle: RobotHandle, seed: Float64Array | number[], target: Pose, options: IkOptions = {}): IkResult { + const record = this.ensure(handle); + const seedValues = Array.from(seed); + validateJointVector(record.model, seedValues); + return solveIk(record.model, seedValues, target, options); + } + + ikBatch( + handle: RobotHandle, + seeds: Array, + targets: Pose[], + options: IkOptions = {} + ): IkResult[] { + const record = this.ensure(handle); + if (seeds.length !== targets.length) { + throw new KdlStructuredError( + "KDL_IK_BATCH_DIMENSION_MISMATCH", + `Expected ${targets.length} seeds, got ${seeds.length}` + ); + } + + let lastSuccessfulSeed: number[] | undefined; + return targets.map((target, index) => { + const seed = lastSuccessfulSeed ?? Array.from(seeds[index] ?? []); + validateJointVector(record.model, seed); + const result = solveIk(record.model, seed, target, options); + if (result.ok && result.joints) { + lastSuccessfulSeed = result.joints; + } + return result; + }); + } + + jacobian( + handle: RobotHandle, + joints: Float64Array | number[], + _options: JacobianOptions = {} + ): JacobianResult { + const record = this.ensure(handle); + const jointValues = Array.from(joints); + validateJointVector(record.model, jointValues); + return computeJacobian(record.model, jointValues); + } + + checkSingularity(handle: RobotHandle, joints: Float64Array | number[]): SingularityResult { + const jacobian = this.jacobian(handle, joints); + const rows = matrixRows(jacobian.data as number[], jacobian.rows, jacobian.cols); + const linearRows = rows.slice(0, 3); + const gram = multiplyMatrix(linearRows, transpose(linearRows)); + const manipulability = Math.sqrt(Math.max(0, determinant3(gram))); + const conditionNumber = estimateConditionNumber(linearRows); + const nearSingularity = manipulability < 1e-6 || conditionNumber > 1e6; + + return { + ok: true, + nearSingularity, + manipulability, + conditionNumber, + diagnostics: nearSingularity + ? [ + { + severity: "warning", + code: "KDL_SINGULARITY", + message: "Jacobian is near singular" + } + ] + : [] + }; + } + + checkJointLimits(handle: RobotHandle, joints: Float64Array | number[]): LimitCheckResult { + const record = this.ensure(handle); + const jointValues = Array.from(joints); + validateJointVector(record.model, jointValues); + const diagnostics = jointValues.flatMap((value, index) => { + const limits = record.model.limits[index]; + if (!limits || (value >= limits.lower && value <= limits.upper)) { + return []; + } + return [ + { + severity: "error" as const, + code: "KDL_JOINT_LIMIT", + message: `Joint ${limits.name}=${value} is outside [${limits.lower}, ${limits.upper}]` + } + ]; + }); + + return { + ok: diagnostics.length === 0, + diagnostics + }; + } + + checkVelocityLimits(handle: RobotHandle, trajectory: TrajectoryResult): LimitCheckResult { + const record = this.ensure(handle); + const points = trajectory.points; + const diagnostics = []; + let maxJointVelocityRatio = 0; + let maxJointAccelerationRatio = 0; + + for (const [pointIndex, point] of points.entries()) { + for (let index = 0; index < record.model.limits.length; index += 1) { + const limit = record.model.limits[index]!; + const velocityRatio = limitRatio(point.jointVelocity[index] ?? 0, limit.velocity); + const accelerationRatio = limitRatio(point.jointAcceleration[index] ?? 0, limit.acceleration); + maxJointVelocityRatio = Math.max(maxJointVelocityRatio, velocityRatio); + maxJointAccelerationRatio = Math.max(maxJointAccelerationRatio, accelerationRatio); + if (velocityRatio > 1) { + diagnostics.push({ + severity: "error" as const, + code: "KDL_VELOCITY_LIMIT", + message: `Point ${pointIndex} joint ${limit.name} velocity exceeds limit`, + pointIndex + }); + } + if (accelerationRatio > 1) { + diagnostics.push({ + severity: "error" as const, + code: "KDL_ACCEL_LIMIT", + message: `Point ${pointIndex} joint ${limit.name} acceleration exceeds limit`, + pointIndex + }); + } + } + } + + return { + ok: diagnostics.length === 0, + diagnostics, + maxJointVelocityRatio, + maxJointAccelerationRatio + }; + } + + checkReachability(handle: RobotHandle, target: PoseTarget, options: IkOptions = {}): ReachabilityResult { + const record = this.ensure(handle); + const seed = options.seeds?.[0] ?? record.model.activeJointNames.map(() => 0); + const result = solveIk(record.model, seed, target.pose, options); + + return { + ok: result.ok, + reachable: result.ok, + ...(target.id ? { targetId: target.id } : {}), + ...(result.joints ? { joints: result.joints } : {}), + ...(result.residualPosition !== undefined ? { residualPosition: result.residualPosition } : {}), + ...(result.residualOrientation !== undefined ? { residualOrientation: result.residualOrientation } : {}), + diagnostics: result.diagnostics + }; + } + + checkReachabilityBatch(handle: RobotHandle, targets: PoseTarget[], options: IkOptions = {}): ReachabilityResult[] { + return targets.map((target, index) => { + const seed = options.seeds?.[index]; + return this.checkReachability(handle, target, { + ...options, + ...(seed ? { seeds: [seed] } : {}) + }); + }); + } + + planMoveJ(handle: RobotHandle, request: MoveJRequest): TrajectoryResult { + const record = this.ensure(handle); + const startJoints = [...request.startJoints]; + validateJointVector(record.model, startJoints); + + const targetResolution = resolveMoveJTarget(record.model, startJoints, request.target); + if (!targetResolution.ok) { + return emptyMoveJResult(request, targetResolution.diagnostics); + } + + const endJoints = targetResolution.joints; + const endpointDiagnostics = [ + ...jointLimitDiagnostics(record.model, startJoints), + ...jointLimitDiagnostics(record.model, endJoints) + ]; + if (endpointDiagnostics.length > 0) { + return emptyMoveJResult(request, endpointDiagnostics); + } + + const dq = endJoints.map((end, index) => end - startJoints[index]!); + const motionScale = Math.max(...dq.map((value) => Math.abs(value)), 0); + const profile = makeTrapProfile(motionScale === 0 ? 0 : 1, { + maxVelocity: normalizedMoveJVelocity(record.model, dq, request.speed, request.speedOverride), + maxAcceleration: normalizedMoveJAcceleration(record.model, dq, request.speed, request.speedOverride), + sampleTime: request.sampleTime + }); + const diagnostics: MotionDiagnostic[] = [ + ...moveJSpeedDiagnostics(request.speed), + ...zoneApproximationDiagnostics(request.zone), + ...profile.diagnostics + ]; + const fkOptions = moveJTargetFkOptions(request); + const targetId = targetIdOf(request.target); + const points: TrajectoryPoint[] = profile.samples.map((sample, index) => { + const joints = dq.map((delta, jointIndex) => startJoints[jointIndex]! + delta * sample.s); + const jointVelocity = dq.map((delta) => delta * sample.sd); + const jointAcceleration = dq.map((delta) => delta * sample.sdd); + const linkPoses = computeLinkPoses(record.model, joints); + const flange = linkPoses.at(-1)?.pose ?? identityPose(); + const tcpWithTool = fkOptions.tool ? composeFkPose(flange, fkOptions.tool) : flange; + const tcp = fkOptions.frame ? composeFkPose(fkOptions.frame, tcpWithTool) : tcpWithTool; + const pointDiagnostics = singularityDiagnostics(record.model, joints); + + return { + index, + time: sample.time, + dt: index === 0 ? 0 : sample.time - profile.samples[index - 1]!.time, + s: sample.s, + sd: sample.sd, + sdd: sample.sdd, + joints, + jointVelocity, + jointAcceleration, + flange, + tcp, + motion: "MOVEJ", + ...(targetId ? { targetId } : {}), + ...(request.sourceMap ? { sourceMap: request.sourceMap } : {}), + diagnostics: pointDiagnostics + }; + }); + + const velocityDiagnostics = velocityLimitDiagnostics(record.model, points); + diagnostics.push(...velocityDiagnostics); + for (const point of points) { + diagnostics.push(...point.diagnostics); + } + + return { + ok: diagnostics.every((diagnostic) => diagnostic.severity !== "error"), + motion: "MOVEJ", + duration: profile.duration, + sampleTime: request.sampleTime, + points, + events: [], + diagnostics, + meta: { + targetType: isJointTarget(request.target) ? "joint" : "pose", + qStart: startJoints, + qEnd: endJoints, + profileType: profile.type, + zone: request.zone + } + }; + } + + planMoveL(handle: RobotHandle, request: MoveLRequest): TrajectoryResult { + const record = this.ensure(handle); + const startJoints = [...request.startJoints]; + validateJointVector(record.model, startJoints); + + const startFk = this.fk(handle, startJoints, moveLFkOptions(request)); + const startPose = startFk.tcp; + const targetPose = request.target.pose; + const length = distance(startPose.position, targetPose.position); + const profile = makeTrapProfile(length, { + maxVelocity: linearSpeedValue(request.speed, request.speedOverride), + maxAcceleration: linearAccelerationValue(request.speed, request.speedOverride), + sampleTime: request.sampleTime + }); + const diagnostics: MotionDiagnostic[] = [ + ...moveLSpeedDiagnostics(request.speed), + ...zoneApproximationDiagnostics(request.zone), + ...profile.diagnostics + ]; + const points: TrajectoryPoint[] = []; + let seed = startJoints; + + for (const [index, sample] of profile.samples.entries()) { + const targetSamplePose = interpolateLinearPose(startPose, targetPose, sample.s); + const ikResult = solveIk(record.model, seed, targetSamplePose, { + ...request.ik, + seeds: [seed] + }); + if (!ikResult.ok || !ikResult.joints) { + return { + ok: false, + motion: "MOVEL", + duration: sample.time, + sampleTime: request.sampleTime, + points, + events: [], + diagnostics: [ + ...diagnostics, + ...ikResult.diagnostics.map((diagnostic) => ({ + ...diagnostic, + pointIndex: index + })) + ], + meta: { + length, + failedPointIndex: index, + targetId: request.target.id + } + }; + } + + const joints = ikResult.joints; + const previous = points.at(-1); + const dt = index === 0 ? 0 : sample.time - profile.samples[index - 1]!.time; + const jointVelocity = previous && dt > 0 + ? joints.map((value, jointIndex) => (value - previous.joints[jointIndex]!) / dt) + : joints.map(() => 0); + const jointAcceleration = previous && dt > 0 + ? jointVelocity.map((value, jointIndex) => (value - previous.jointVelocity[jointIndex]!) / dt) + : joints.map(() => 0); + const fk = this.fk(handle, joints, moveLFkOptions(request)); + const pointDiagnostics = [ + ...singularityDiagnostics(record.model, joints), + ...linearErrorDiagnostics(index, fk.tcp, targetSamplePose) + ]; + + points.push({ + index, + time: sample.time, + dt, + s: sample.s, + sd: sample.sd, + sdd: sample.sdd, + joints, + jointVelocity, + jointAcceleration, + flange: fk.flange, + tcp: fk.tcp, + motion: "MOVEL", + ...(request.target.id ? { targetId: request.target.id } : {}), + ...(request.sourceMap ? { sourceMap: request.sourceMap } : {}), + diagnostics: pointDiagnostics + }); + seed = joints; + } + + const velocityDiagnostics = velocityLimitDiagnostics(record.model, points); + diagnostics.push(...velocityDiagnostics); + for (const point of points) { + diagnostics.push(...point.diagnostics); + } + + return { + ok: diagnostics.every((diagnostic) => diagnostic.severity !== "error"), + motion: "MOVEL", + duration: profile.duration, + sampleTime: request.sampleTime, + points, + events: [], + diagnostics, + meta: { + length, + targetId: request.target.id, + profileType: profile.type, + orientationMode: request.orientationMode ?? "fixed" + } + }; + } + + planMoveC(handle: RobotHandle, request: MoveCRequest): TrajectoryResult { + const record = this.ensure(handle); + const startJoints = [...request.startJoints]; + validateJointVector(record.model, startJoints); + + const startFk = this.fk(handle, startJoints, moveCFkOptions(request)); + const arc = computeCircleArc(startFk.tcp, request.via.pose, request.target.pose); + if (!arc.ok) { + return { + ok: false, + motion: "MOVEC", + duration: 0, + sampleTime: request.sampleTime, + points: [], + events: [], + diagnostics: arc.diagnostics, + meta: { + viaId: request.via.id, + targetId: request.target.id + } + }; + } + + const profile = makeTrapProfile(arc.meta.length, { + maxVelocity: linearSpeedValue(request.speed, request.speedOverride), + maxAcceleration: linearAccelerationValue(request.speed, request.speedOverride), + sampleTime: request.sampleTime + }); + const diagnostics: MotionDiagnostic[] = [ + ...moveCSpeedDiagnostics(request.speed), + ...zoneApproximationDiagnostics(request.zone), + ...profile.diagnostics + ]; + const points: TrajectoryPoint[] = []; + let seed = startJoints; + let maxArcError = 0; + + for (const [index, sample] of profile.samples.entries()) { + const targetSamplePose = sampleCirclePose(arc, sample.s, request.target.pose.quaternion); + const ikResult = solveIk(record.model, seed, targetSamplePose, { + ...request.ik, + seeds: [seed] + }); + if (!ikResult.ok || !ikResult.joints) { + return { + ok: false, + motion: "MOVEC", + duration: sample.time, + sampleTime: request.sampleTime, + points, + events: [], + diagnostics: [ + ...diagnostics, + ...ikResult.diagnostics.map((diagnostic) => ({ + ...diagnostic, + pointIndex: index + })) + ], + meta: { + circle: { + ...arc.meta, + maxArcError + }, + failedPointIndex: index, + viaId: request.via.id, + targetId: request.target.id + } + }; + } + + const joints = ikResult.joints; + const previous = points.at(-1); + const dt = index === 0 ? 0 : sample.time - profile.samples[index - 1]!.time; + const jointVelocity = previous && dt > 0 + ? joints.map((value, jointIndex) => (value - previous.joints[jointIndex]!) / dt) + : joints.map(() => 0); + const jointAcceleration = previous && dt > 0 + ? jointVelocity.map((value, jointIndex) => (value - previous.jointVelocity[jointIndex]!) / dt) + : joints.map(() => 0); + const fk = this.fk(handle, joints, moveCFkOptions(request)); + const arcError = distance(fk.tcp.position, targetSamplePose.position); + maxArcError = Math.max(maxArcError, arcError); + const pointDiagnostics = [ + ...singularityDiagnostics(record.model, joints), + ...arcErrorDiagnostics(index, arcError) + ]; + + points.push({ + index, + time: sample.time, + dt, + s: sample.s, + sd: sample.sd, + sdd: sample.sdd, + joints, + jointVelocity, + jointAcceleration, + flange: fk.flange, + tcp: fk.tcp, + motion: "MOVEC", + ...(request.target.id ? { targetId: request.target.id } : {}), + ...(request.sourceMap ? { sourceMap: request.sourceMap } : {}), + diagnostics: pointDiagnostics + }); + seed = joints; + } + + const velocityDiagnostics = velocityLimitDiagnostics(record.model, points); + diagnostics.push(...velocityDiagnostics); + for (const point of points) { + diagnostics.push(...point.diagnostics); + } + + return { + ok: diagnostics.every((diagnostic) => diagnostic.severity !== "error"), + motion: "MOVEC", + duration: profile.duration, + sampleTime: request.sampleTime, + points, + events: [], + diagnostics, + meta: { + circle: { + ...arc.meta, + maxArcError + }, + viaId: request.via.id, + targetId: request.target.id, + profileType: profile.type, + orientationMode: request.orientationMode ?? "fixed" + } + }; + } + + planPath(handle: RobotHandle, request: PathPlanRequest): PathPlanResult { + validateJointVector(this.ensure(handle).model, request.startJoints); + if (request.segments.length === 0) { + const diagnostics = [pathEmptyDiagnostic()]; + return { + ok: false, + duration: 0, + segments: [], + points: [], + diagnostics + }; + } + + const segments: TrajectoryResult[] = []; + const points: TrajectoryPoint[] = []; + const diagnostics: MotionDiagnostic[] = []; + let currentJoints = [...request.startJoints]; + let timeOffset = 0; + + for (const segment of request.segments) { + const result = this.planPathSegment(handle, request, segment, currentJoints); + const segmentDiagnostics = result.diagnostics.map((diagnostic) => ({ + ...diagnostic, + segmentId: diagnostic.segmentId ?? segment.id, + ...(diagnostic.sourceMap ?? segment.sourceMap ? { sourceMap: diagnostic.sourceMap ?? segment.sourceMap } : {}) + })); + const segmentPoints = result.points.map((point) => ({ + ...point, + segmentId: point.segmentId ?? segment.id, + ...(point.sourceMap ?? segment.sourceMap ? { sourceMap: point.sourceMap ?? segment.sourceMap } : {}), + diagnostics: point.diagnostics.map((diagnostic) => ({ + ...diagnostic, + segmentId: diagnostic.segmentId ?? segment.id, + ...(diagnostic.sourceMap ?? segment.sourceMap ? { sourceMap: diagnostic.sourceMap ?? segment.sourceMap } : {}) + })) + })); + const segmentResult = { + ...result, + points: segmentPoints, + diagnostics: segmentDiagnostics + }; + segments.push(segmentResult); + diagnostics.push(...segmentDiagnostics); + + for (const point of segmentPoints) { + if (points.length > 0 && point.index === 0) { + continue; + } + const previous = points.at(-1); + const time = point.time + timeOffset; + points.push({ + ...point, + index: points.length, + time, + dt: previous ? time - previous.time : 0, + segmentId: segment.id, + ...(point.targetId ? { targetId: point.targetId } : {}), + ...(point.sourceMap ?? segment.sourceMap ? { sourceMap: point.sourceMap ?? segment.sourceMap } : {}), + diagnostics: point.diagnostics.map((diagnostic) => ({ + ...diagnostic, + segmentId: diagnostic.segmentId ?? segment.id, + ...(diagnostic.sourceMap ?? segment.sourceMap ? { sourceMap: diagnostic.sourceMap ?? segment.sourceMap } : {}) + })) + }); + } + + timeOffset += result.duration; + if (result.ok && result.points.at(-1)) { + currentJoints = [...result.points.at(-1)!.joints]; + } + if (!result.ok && request.stopOnError !== false) { + break; + } + } + + return { + ok: segments.length === request.segments.length && segments.every((segment) => segment.ok), + duration: timeOffset, + segments, + points, + diagnostics + }; + } + + validatePath(handle: RobotHandle, request: PathPlanRequest): PathValidationResult { + const plan = this.planPath(handle, request); + const segmentReports: SegmentValidationReport[] = plan.segments.map((segment, index) => { + const segmentRequest = request.segments[index]!; + const velocityCheck = this.checkVelocityLimits(handle, segment); + return { + segmentId: segmentRequest.id, + ok: segment.ok && velocityCheck.ok, + motion: segment.motion, + duration: segment.duration, + maxJointVelocityRatio: velocityCheck.maxJointVelocityRatio ?? 0, + maxJointAccelerationRatio: velocityCheck.maxJointAccelerationRatio ?? 0, + maxCartesianError: maxCartesianError(segment), + diagnostics: [ + ...segment.diagnostics, + ...velocityCheck.diagnostics.map((diagnostic) => ({ + ...diagnostic, + segmentId: diagnostic.segmentId ?? segmentRequest.id + })) + ] + }; + }); + const diagnostics = [ + ...plan.diagnostics, + ...segmentReports.flatMap((report) => report.diagnostics) + ]; + return { + ok: plan.ok && segmentReports.every((report) => report.ok), + reachable: plan.ok && segmentReports.every((report) => report.ok), + ...(plan.ok ? { cycleTime: plan.duration } : {}), + segmentReports, + diagnostics + }; + } + + private ensure(handle: RobotHandle): RobotRegistryRecord { + const record = this.robots.get(handle); + if (!record) { + throw new KdlStructuredError("KDL_INVALID_HANDLE", `RobotHandle ${handle} does not exist`); + } + return record; + } + + private planPathSegment( + handle: RobotHandle, + request: PathPlanRequest, + segment: PathPlanRequest["segments"][number], + startJoints: number[] + ): TrajectoryResult { + const common = { + startJoints, + speed: segment.speed, + zone: segment.zone, + sampleTime: request.sampleTime, + ...(segment.tool ? { tool: segment.tool } : {}), + ...(segment.frame ? { frame: segment.frame } : {}), + ...(request.speedOverride ? { speedOverride: request.speedOverride } : {}), + ...(segment.sourceMap ? { sourceMap: segment.sourceMap } : {}) + }; + + if (segment.motion === "MOVEJ" && segment.target) { + return this.planMoveJ(handle, { + ...common, + target: segment.target + }); + } + if (segment.motion === "MOVEL" && segment.target && isPoseTarget(segment.target)) { + return this.planMoveL(handle, { + ...common, + target: segment.target + }); + } + if (segment.motion === "MOVEC" && segment.via && segment.target && isPoseTarget(segment.target)) { + return this.planMoveC(handle, { + ...common, + via: segment.via, + target: segment.target + }); + } + + return { + ok: false, + motion: segment.motion, + duration: 0, + sampleTime: request.sampleTime, + points: [], + events: [], + diagnostics: [ + { + severity: "error", + code: "KDL_PATH_SEGMENT_INVALID", + message: `Path segment ${segment.id} has invalid target/via for ${segment.motion}`, + segmentId: segment.id, + ...(segment.sourceMap ? { sourceMap: segment.sourceMap } : {}) + } + ] + }; + } +} + +export function validateNormalizedRobotModel(model: NormalizedRobotModel): void { + const linkNames = new Set(model.links.map((link) => link.name)); + if (!linkNames.has(model.baseLink)) { + throw new KdlStructuredError("KDL_INVALID_MODEL", `baseLink does not exist: ${model.baseLink}`); + } + if (!linkNames.has(model.tipLink)) { + throw new KdlStructuredError("KDL_INVALID_MODEL", `tipLink does not exist: ${model.tipLink}`); + } + + const jointNames = new Set(); + for (const joint of model.joints) { + if (jointNames.has(joint.name)) { + throw new KdlStructuredError("KDL_INVALID_MODEL", `Duplicate joint name: ${joint.name}`); + } + jointNames.add(joint.name); + if (!linkNames.has(joint.parent) || !linkNames.has(joint.child)) { + throw new KdlStructuredError("KDL_INVALID_MODEL", `Joint ${joint.name} references an unknown link`); + } + } + + for (const activeJoint of model.activeJointNames) { + if (!jointNames.has(activeJoint)) { + throw new KdlStructuredError("KDL_INVALID_MODEL", `activeJointNames contains an unknown joint: ${activeJoint}`); + } + } +} + +function validateJointVector(model: NormalizedRobotModel, joints: number[]): void { + if (joints.length !== model.activeJointNames.length) { + throw new KdlStructuredError( + "KDL_JOINT_DIMENSION_MISMATCH", + `Expected ${model.activeJointNames.length} joints, got ${joints.length}` + ); + } + + for (let index = 0; index < joints.length; index += 1) { + if (!Number.isFinite(joints[index])) { + throw new KdlStructuredError("KDL_INVALID_JOINTS", `Joint value at index ${index} is not finite`); + } + } +} + +function pathEmptyDiagnostic(): MotionDiagnostic { + return { + severity: "error", + code: "KDL_PATH_EMPTY", + message: "PathPlanRequest.segments must contain at least one segment" + }; +} + +function maxCartesianError(segment: TrajectoryResult): number { + const circle = segment.meta?.circle; + if (isRecord(circle) && typeof circle.maxArcError === "number") { + return circle.maxArcError; + } + + let maxError = 0; + for (const diagnostic of segment.diagnostics) { + const data = diagnostic.data; + if (!data) { + continue; + } + if (typeof data.positionError === "number") { + maxError = Math.max(maxError, data.positionError); + } + if (typeof data.arcError === "number") { + maxError = Math.max(maxError, data.arcError); + } + } + return maxError; +} + +function isRecord(value: unknown): value is Record { + return !!value && typeof value === "object"; +} + +function resolveMoveJTarget( + model: NormalizedRobotModel, + startJoints: number[], + target: JointTarget | PoseTarget +): { ok: true; joints: number[] } | { ok: false; diagnostics: MotionDiagnostic[] } { + if (isJointTarget(target)) { + const joints = [...target.joints]; + validateJointVector(model, joints); + return { + ok: true, + joints + }; + } + + const result = solveIk(model, startJoints, target.pose, { + seeds: [startJoints] + }); + if (result.ok && result.joints) { + return { + ok: true, + joints: result.joints + }; + } + + return { + ok: false, + diagnostics: result.diagnostics.length > 0 + ? result.diagnostics + : [ + { + severity: "error", + code: "KDL_MOVEJ_IK_FAILED", + message: "MOVEJ pose target IK failed" + } + ] + }; +} + +function emptyMoveJResult(request: MoveJRequest, diagnostics: MotionDiagnostic[]): TrajectoryResult { + return { + ok: false, + motion: "MOVEJ", + duration: 0, + sampleTime: request.sampleTime, + points: [], + events: [], + diagnostics, + meta: { + targetType: isJointTarget(request.target) ? "joint" : "pose" + } + }; +} + +function jointLimitDiagnostics(model: NormalizedRobotModel, joints: number[]): MotionDiagnostic[] { + return joints.flatMap((value, index) => { + const limit = model.limits[index]; + if (!limit || (value >= limit.lower && value <= limit.upper)) { + return []; + } + return [ + { + severity: "error" as const, + code: "KDL_JOINT_LIMIT", + message: `Joint ${limit.name}=${value} is outside [${limit.lower}, ${limit.upper}]`, + data: { + joint: limit.name, + value, + lower: limit.lower, + upper: limit.upper + } + } + ]; + }); +} + +function velocityLimitDiagnostics(model: NormalizedRobotModel, points: TrajectoryPoint[]): MotionDiagnostic[] { + return points.flatMap((point) => + model.limits.flatMap((limit, index) => { + const diagnostics: MotionDiagnostic[] = []; + const velocityRatio = limitRatio(point.jointVelocity[index] ?? 0, limit.velocity); + const accelerationRatio = limitRatio(point.jointAcceleration[index] ?? 0, limit.acceleration); + if (velocityRatio > 1 + 1e-9) { + diagnostics.push({ + severity: "error", + code: "KDL_VELOCITY_LIMIT", + message: `Point ${point.index} joint ${limit.name} velocity exceeds limit`, + pointIndex: point.index + }); + } + if (accelerationRatio > 1 + 1e-9) { + diagnostics.push({ + severity: "error", + code: "KDL_ACCEL_LIMIT", + message: `Point ${point.index} joint ${limit.name} acceleration exceeds limit`, + pointIndex: point.index + }); + } + return diagnostics; + }) + ); +} + +function singularityDiagnostics(model: NormalizedRobotModel, joints: number[]): MotionDiagnostic[] { + const jacobian = computeJacobian(model, joints); + const rows = matrixRows(jacobian.data as number[], jacobian.rows, jacobian.cols); + const linearRows = rows.slice(0, 3); + const gram = multiplyMatrix(linearRows, transpose(linearRows)); + const manipulability = Math.sqrt(Math.max(0, determinant3(gram))); + const conditionNumber = estimateConditionNumber(linearRows); + if (manipulability >= 1e-6 && conditionNumber <= 1e6) { + return []; + } + return [ + { + severity: "warning", + code: "KDL_SINGULARITY", + message: "MOVEJ sample is near singular", + data: { + manipulability, + conditionNumber + } + } + ]; +} + +function normalizedMoveJVelocity( + model: NormalizedRobotModel, + dq: number[], + speed: SpeedSpec, + speedOverride = 1 +): number { + const override = validateSpeedOverride(speedOverride); + if (speed.kind === "joint_percent") { + return Math.max(1e-9, speed.value * override); + } + + const requested = speed.kind === "joint_abs" ? speed.velocity : speed.velocity; + const byRequest = normalizedLimitByScalar(dq, requested * override); + const byJointLimits = normalizedLimitByJointLimits(model, dq, "velocity"); + return Math.max(1e-9, Math.min(byRequest, byJointLimits)); +} + +function normalizedMoveJAcceleration( + model: NormalizedRobotModel, + dq: number[], + speed: SpeedSpec, + speedOverride = 1 +): number { + const override = validateSpeedOverride(speedOverride); + const requested = speed.kind === "joint_abs" ? speed.acceleration : undefined; + const byRequest = requested ? normalizedLimitByScalar(dq, requested * override) : Infinity; + const byJointLimits = normalizedLimitByJointLimits(model, dq, "acceleration"); + return Math.max(1e-9, Math.min(byRequest, byJointLimits, 1)); +} + +function normalizedLimitByScalar(dq: number[], scalar: number): number { + if (!Number.isFinite(scalar) || scalar <= 0) { + throw new KdlStructuredError("KDL_INVALID_SPEED", "MOVEJ speed must be a finite positive number"); + } + const maxDelta = Math.max(...dq.map((value) => Math.abs(value)), 0); + if (maxDelta === 0) { + return scalar; + } + return scalar / maxDelta; +} + +function normalizedLimitByJointLimits( + model: NormalizedRobotModel, + dq: number[], + field: "velocity" | "acceleration" +): number { + const ratios = dq.flatMap((delta, index) => { + const absDelta = Math.abs(delta); + const limit = model.limits[index]?.[field] ?? Infinity; + if (absDelta <= 1e-12 || !Number.isFinite(limit)) { + return []; + } + return [limit / absDelta]; + }); + return ratios.length === 0 ? 1 : Math.max(1e-9, Math.min(...ratios)); +} + +function validateSpeedOverride(speedOverride: number): number { + if (!Number.isFinite(speedOverride) || speedOverride <= 0) { + throw new KdlStructuredError("KDL_INVALID_SPEED", "speedOverride must be a finite positive number"); + } + return speedOverride; +} + +function moveJSpeedDiagnostics(speed: SpeedSpec): MotionDiagnostic[] { + if (speed.kind === "linear") { + return [ + { + severity: "warning", + code: "KDL_MOVEJ_LINEAR_SPEED_APPROX", + message: "MOVEJ received linear speed; using its velocity as a joint-space scalar" + } + ]; + } + return []; +} + +function zoneApproximationDiagnostics(zone: MoveJRequest["zone"]): MotionDiagnostic[] { + if (zone.kind === "fine") { + return []; + } + return [ + { + severity: "warning", + code: "KDL_ZONE_APPROX_FINE", + message: "Motion zone is approximated as fine in P0" + } + ]; +} + +function moveLSpeedDiagnostics(speed: SpeedSpec): MotionDiagnostic[] { + if (speed.kind === "linear") { + return []; + } + return [ + { + severity: "warning", + code: "KDL_MOVEL_JOINT_SPEED_APPROX", + message: "MOVEL received joint speed; using its velocity as a linear scalar" + } + ]; +} + +function moveCSpeedDiagnostics(speed: SpeedSpec): MotionDiagnostic[] { + if (speed.kind === "linear") { + return []; + } + return [ + { + severity: "warning", + code: "KDL_MOVEC_JOINT_SPEED_APPROX", + message: "MOVEC received joint speed; using its velocity as a linear scalar" + } + ]; +} + +function linearSpeedValue(speed: SpeedSpec, speedOverride = 1): number { + const override = validateSpeedOverride(speedOverride); + const value = speed.kind === "joint_percent" ? speed.value : speed.velocity; + if (!Number.isFinite(value) || value <= 0) { + throw new KdlStructuredError("KDL_INVALID_SPEED", "MOVEL speed must be a finite positive number"); + } + return Math.max(1e-9, value * override); +} + +function linearAccelerationValue(speed: SpeedSpec, speedOverride = 1): number { + const override = validateSpeedOverride(speedOverride); + const value = speed.kind === "linear" || speed.kind === "joint_abs" ? speed.acceleration : undefined; + if (value !== undefined && (!Number.isFinite(value) || value <= 0)) { + throw new KdlStructuredError("KDL_INVALID_SPEED", "MOVEL acceleration must be a finite positive number"); + } + return Math.max(1e-9, (value ?? linearSpeedValue(speed, 1)) * override); +} + +function moveLFkOptions(request: MoveLRequest): FkOptions { + const options: FkOptions = {}; + const tool = request.tool ?? request.target.tool; + const frame = request.frame ?? request.target.frame; + if (tool) { + options.tool = tool; + } + if (frame) { + options.frame = frame; + } + return options; +} + +function moveCFkOptions(request: MoveCRequest): FkOptions { + const options: FkOptions = {}; + const tool = request.tool ?? request.target.tool; + const frame = request.frame ?? request.target.frame; + if (tool) { + options.tool = tool; + } + if (frame) { + options.frame = frame; + } + return options; +} + +function interpolateLinearPose(start: Pose, target: Pose, s: number): Pose { + return { + position: [ + start.position[0] + (target.position[0] - start.position[0]) * s, + start.position[1] + (target.position[1] - start.position[1]) * s, + start.position[2] + (target.position[2] - start.position[2]) * s + ], + quaternion: target.quaternion + }; +} + +function linearErrorDiagnostics(index: number, actual: Pose, expected: Pose): MotionDiagnostic[] { + const positionError = distance(actual.position, expected.position); + if (positionError <= 1e-6) { + return []; + } + return [ + { + severity: "error", + code: "KDL_TCP_LINE_ERROR", + message: `MOVEL TCP line error ${positionError} exceeds tolerance`, + pointIndex: index, + data: { + positionError + } + } + ]; +} + +interface CircleArc { + start: [number, number, number]; + center: [number, number, number]; + radius: number; + normal: [number, number, number]; + startAngle: number; + angle: number; + meta: { + center: [number, number, number]; + radius: number; + normal: [number, number, number]; + angle: number; + length: number; + direction: "cw" | "ccw"; + }; +} + +function computeCircleArc( + startPose: Pose, + viaPose: Pose, + targetPose: Pose +): { ok: true; meta: CircleArc["meta"]; arc: CircleArc } | { ok: false; diagnostics: MotionDiagnostic[] } { + const start = startPose.position; + const via = viaPose.position; + const target = targetPose.position; + const a = sub3(via, start); + const b = sub3(target, start); + const normalVector = cross3(a, b); + const normalLength = norm3(normalVector); + if (distance(start, via) < 1e-9 || distance(via, target) < 1e-9 || distance(start, target) < 1e-9 || normalLength < 1e-9) { + return { + ok: false, + diagnostics: [ + { + severity: "error", + code: "KDL_ARC_DEGENERATE", + message: "MOVEC requires three distinct non-collinear points" + } + ] + }; + } + + const center = circleCenter3(start, via, target, normalVector); + const radius = distance(center, start); + if (!Number.isFinite(radius) || radius < 1e-9) { + return { + ok: false, + diagnostics: [ + { + severity: "error", + code: "KDL_ARC_DEGENERATE", + message: "MOVEC arc radius is too small" + } + ] + }; + } + + const normal = normalize3(normalVector); + const u = normalize3(sub3(start, center)); + const v = cross3(normal, u); + const startAngle = 0; + const viaAngle = positiveAngle(pointAngle(center, u, v, via)); + const targetAngle = positiveAngle(pointAngle(center, u, v, target)); + const angle = viaAngle <= targetAngle ? targetAngle : targetAngle + Math.PI * 2; + const length = radius * angle; + if (length < 1e-9) { + return { + ok: false, + diagnostics: [ + { + severity: "error", + code: "KDL_ARC_DEGENERATE", + message: "MOVEC arc length is too short" + } + ] + }; + } + + const meta = { + center, + radius, + normal, + angle, + length, + direction: normal[2] >= 0 ? "ccw" as const : "cw" as const + }; + return { + ok: true, + meta, + arc: { + start, + center, + radius, + normal, + startAngle, + angle, + meta + } + }; +} + +function sampleCirclePose( + arcResult: { arc: CircleArc }, + s: number, + quaternion: [number, number, number, number] +): Pose { + const arc = arcResult.arc; + const u = normalize3(sub3(arc.start, arc.center)); + const v = cross3(arc.normal, u); + const angle = arc.startAngle + arc.angle * s; + return { + position: add3( + arc.center, + add3(scale3(u, Math.cos(angle) * arc.radius), scale3(v, Math.sin(angle) * arc.radius)) + ), + quaternion + }; +} + +function arcErrorDiagnostics(index: number, arcError: number): MotionDiagnostic[] { + if (arcError <= 1e-6) { + return []; + } + return [ + { + severity: "error", + code: "KDL_ARC_ERROR", + message: `MOVEC arc error ${arcError} exceeds tolerance`, + pointIndex: index, + data: { + arcError + } + } + ]; +} + +function circleCenter3( + p1: [number, number, number], + p2: [number, number, number], + p3: [number, number, number], + normal: [number, number, number] +): [number, number, number] { + const a = sub3(p2, p1); + const b = sub3(p3, p1); + const a2 = dot3(a, a); + const b2 = dot3(b, b); + const cross = cross3(a, b); + const denom = 2 * dot3(cross, cross); + if (Math.abs(denom) < 1e-18) { + return [NaN, NaN, NaN]; + } + const termA = scale3(cross3(b, normal), a2); + const termB = scale3(cross3(normal, a), b2); + return add3(p1, scale3(add3(termA, termB), 1 / denom)); +} + +function pointAngle( + center: [number, number, number], + u: [number, number, number], + v: [number, number, number], + point: [number, number, number] +): number { + const radial = sub3(point, center); + return Math.atan2(dot3(radial, v), dot3(radial, u)); +} + +function positiveAngle(angle: number): number { + const twoPi = Math.PI * 2; + let result = angle % twoPi; + if (result < 0) { + result += twoPi; + } + return result; +} + +function add3(a: [number, number, number], b: [number, number, number]): [number, number, number] { + return [a[0] + b[0], a[1] + b[1], a[2] + b[2]]; +} + +function sub3(a: [number, number, number], b: [number, number, number]): [number, number, number] { + return [a[0] - b[0], a[1] - b[1], a[2] - b[2]]; +} + +function scale3(a: [number, number, number], scale: number): [number, number, number] { + return [a[0] * scale, a[1] * scale, a[2] * scale]; +} + +function dot3(a: [number, number, number], b: [number, number, number]): number { + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; +} + +function cross3(a: [number, number, number], b: [number, number, number]): [number, number, number] { + return [ + a[1] * b[2] - a[2] * b[1], + a[2] * b[0] - a[0] * b[2], + a[0] * b[1] - a[1] * b[0] + ]; +} + +function norm3(a: [number, number, number]): number { + return Math.hypot(a[0], a[1], a[2]); +} + +function normalize3(a: [number, number, number]): [number, number, number] { + const length = norm3(a); + if (length === 0) { + return [0, 0, 0]; + } + return [a[0] / length, a[1] / length, a[2] / length]; +} + +function moveJTargetFkOptions(request: MoveJRequest): FkOptions { + const target = isPoseTarget(request.target) ? request.target : undefined; + const options: FkOptions = {}; + const tool = request.tool ?? target?.tool; + const frame = request.frame ?? target?.frame; + if (tool) { + options.tool = tool; + } + if (frame) { + options.frame = frame; + } + return options; +} + +function targetIdOf(target: JointTarget | PoseTarget): string | undefined { + return target.id; +} + +function isJointTarget(target: JointTarget | PoseTarget): target is JointTarget { + return "joints" in target; +} + +function isPoseTarget(target: JointTarget | PoseTarget): target is PoseTarget { + return "pose" in target; +} + +function limitRatio(value: number, limit: number): number { + if (!Number.isFinite(limit) || limit <= 0) { + return 0; + } + return Math.abs(value) / limit; +} + +function computeLinkPoses( + model: NormalizedRobotModel, + joints: number[] +): Array<{ link: string; pose: Pose }> { + const poses = new Map(); + poses.set(model.baseLink, identityMat4()); + const activeJointIndex = new Map(model.activeJointNames.map((name, index) => [name, index])); + + let progressed = true; + while (progressed) { + progressed = false; + for (const joint of model.joints) { + if (!poses.has(joint.parent) || poses.has(joint.child)) { + continue; + } + + const parentPose = poses.get(joint.parent)!; + const origin = multiplyMat4(translationMat4(joint.origin.xyz), rotationFromRpyMat4(joint.origin.rpy)); + const activeIndex = activeJointIndex.get(joint.name); + const jointValue = activeIndex === undefined ? 0 : joints[activeIndex]!; + const motion = jointMotionMat4(joint.type, joint.axis, jointValue); + poses.set(joint.child, multiplyMat4(parentPose, multiplyMat4(origin, motion))); + progressed = true; + } + } + + const orderedLinks = chainLinkOrder(model); + return orderedLinks.map((link) => ({ + link, + pose: mat4ToPose(poses.get(link) ?? identityMat4()) + })); +} + +function computeJacobian(model: NormalizedRobotModel, joints: number[]): JacobianResult { + const basePose = computeLinkPoses(model, joints).at(-1)?.pose ?? identityPose(); + const rows = 6; + const cols = joints.length; + const data = new Array(rows * cols).fill(0); + const epsilon = 1e-6; + + for (let col = 0; col < cols; col += 1) { + const perturbed = [...joints]; + perturbed[col] = (perturbed[col] ?? 0) + epsilon; + const pose = computeLinkPoses(model, perturbed).at(-1)?.pose ?? identityPose(); + data[col] = (pose.position[0] - basePose.position[0]) / epsilon; + data[cols + col] = (pose.position[1] - basePose.position[1]) / epsilon; + data[cols * 2 + col] = (pose.position[2] - basePose.position[2]) / epsilon; + data[cols * 3 + col] = (pose.quaternion[0] - basePose.quaternion[0]) / epsilon; + data[cols * 4 + col] = (pose.quaternion[1] - basePose.quaternion[1]) / epsilon; + data[cols * 5 + col] = (pose.quaternion[2] - basePose.quaternion[2]) / epsilon; + } + + return { + ok: true, + rows, + cols, + data, + diagnostics: [] + }; +} + +function solveIk( + model: NormalizedRobotModel, + seed: number[], + target: Pose, + options: IkOptions +): IkResult { + const chainJoints = model.joints.filter((joint) => model.activeJointNames.includes(joint.name)); + + if (chainJoints.length === 1 && chainJoints[0]?.type === "prismatic") { + return solveSinglePrismaticIk(model, chainJoints[0], seed, target, options); + } + + if ( + chainJoints.length === 2 && + (chainJoints[0]?.type === "revolute" || chainJoints[0]?.type === "continuous") && + chainJoints[1]?.type === "prismatic" && + isAxis(chainJoints[0].axis, [0, 0, 1]) && + isAxis(chainJoints[1].axis, [1, 0, 0]) + ) { + return solvePlanarRzPxIk(model, seed, target, options); + } + + return { + ok: false, + iterations: 0, + reason: "invalid_model", + diagnostics: [ + { + severity: "error", + code: "KDL_IK_UNSUPPORTED_MODEL", + message: "Current TypeScript IK baseline supports only single-prismatic or Rz+Px chains" + } + ] + }; +} + +function solveSinglePrismaticIk( + model: NormalizedRobotModel, + joint: NormalizedRobotModel["joints"][number], + seed: number[], + target: Pose, + options: IkOptions +): IkResult { + const zero = computeLinkPoses(model, [0]).at(-1)?.pose.position ?? [0, 0, 0]; + const axisIndex = dominantAxisIndex(joint.axis); + const q = target.position[axisIndex]! - zero[axisIndex]!; + return finishIkCandidate(model, seed, [q], target, options); +} + +function solvePlanarRzPxIk( + model: NormalizedRobotModel, + seed: number[], + target: Pose, + options: IkOptions +): IkResult { + const [x, y] = target.position; + const q2 = Math.hypot(x, y); + const q1Candidates = [Math.atan2(y, x), Math.atan2(y, x) + Math.PI]; + const candidates = q1Candidates.map((q1, index) => [normalizeAngleNear(q1, seed[0] ?? 0), index === 0 ? q2 : -q2]); + + let best: IkResult | undefined; + for (const candidate of candidates) { + const result = finishIkCandidate(model, seed, candidate, target, options); + if (result.ok) { + return result; + } + if (!best || (result.residualPosition ?? Infinity) < (best.residualPosition ?? Infinity)) { + best = result; + } + } + + return best ?? { + ok: false, + iterations: 1, + reason: "unreachable", + diagnostics: [ + { + severity: "error", + code: "KDL_TARGET_UNREACHABLE", + message: "No IK candidate found" + } + ] + }; +} + +function finishIkCandidate( + model: NormalizedRobotModel, + seed: number[], + candidate: number[], + target: Pose, + options: IkOptions +): IkResult { + const limited = applyOptionLimits(model, candidate, options); + if (!limited.ok) { + return limited.result; + } + + const fkPose = computeLinkPoses(model, candidate).at(-1)?.pose ?? identityPose(); + const residualPosition = distance(fkPose.position, target.position); + const tolerance = options.positionTolerance ?? 1e-6; + const baseResult = { + joints: candidate, + iterations: 1, + residualPosition, + residualOrientation: quaternionDistance(fkPose.quaternion, target.quaternion), + diagnostics: [] + }; + + if (residualPosition <= tolerance || options.allowApproximate) { + return { + ok: true, + ...baseResult + }; + } + + return { + ok: false, + ...baseResult, + reason: "unreachable", + diagnostics: [ + { + severity: "error", + code: "KDL_TARGET_UNREACHABLE", + message: `IK residual ${residualPosition} exceeds tolerance ${tolerance}` + } + ] + }; +} + +function applyOptionLimits( + model: NormalizedRobotModel, + candidate: number[], + options: IkOptions +): { ok: true } | { ok: false; result: IkResult } { + for (let index = 0; index < candidate.length; index += 1) { + const value = candidate[index]!; + const limits = model.limits[index]; + const lower = options.qMin?.[index] ?? limits?.lower ?? -Infinity; + const upper = options.qMax?.[index] ?? limits?.upper ?? Infinity; + if (value < lower || value > upper) { + return { + ok: false, + result: { + ok: false, + iterations: 1, + joints: candidate, + reason: "joint_limit", + diagnostics: [ + { + severity: "error", + code: "KDL_JOINT_LIMIT", + message: `IK candidate joint ${index}=${value} is outside [${lower}, ${upper}]` + } + ] + } + }; + } + } + return { ok: true }; +} + +function chainLinkOrder(model: NormalizedRobotModel): string[] { + const order = [model.baseLink]; + let current = model.baseLink; + const visited = new Set([current]); + + while (current !== model.tipLink) { + const nextJoint = model.joints.find((joint) => joint.parent === current && !visited.has(joint.child)); + if (!nextJoint) { + break; + } + current = nextJoint.child; + visited.add(current); + order.push(current); + } + + return order; +} + +function composeFkPose(a: Pose, b: Pose): Pose { + return mat4ToPose(multiplyMat4(poseToMat4(a), poseToMat4(b))); +} + +function identityPose(): Pose { + return { + position: [0, 0, 0], + quaternion: [0, 0, 0, 1] + }; +} + +function isAxis(actual: [number, number, number], expected: [number, number, number]): boolean { + return actual.every((value, index) => Math.abs(value - expected[index]!) < 1e-9); +} + +function dominantAxisIndex(axis: [number, number, number]): 0 | 1 | 2 { + const absolute = axis.map((value) => Math.abs(value)); + if (absolute[1]! > absolute[0]! && absolute[1]! >= absolute[2]!) { + return 1; + } + if (absolute[2]! > absolute[0]! && absolute[2]! > absolute[1]!) { + return 2; + } + return 0; +} + +function normalizeAngleNear(angle: number, seed: number): number { + const twoPi = Math.PI * 2; + let normalized = angle; + while (normalized - seed > Math.PI) { + normalized -= twoPi; + } + while (normalized - seed < -Math.PI) { + normalized += twoPi; + } + return normalized; +} + +function distance(a: [number, number, number], b: [number, number, number]): number { + return Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2]); +} + +function quaternionDistance(a: [number, number, number, number], b: [number, number, number, number]): number { + return Math.min( + Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2], a[3] - b[3]), + Math.hypot(a[0] + b[0], a[1] + b[1], a[2] + b[2], a[3] + b[3]) + ); +} + +function matrixRows(data: number[], rows: number, cols: number): number[][] { + return Array.from({ length: rows }, (_, row) => + Array.from({ length: cols }, (_, col) => data[row * cols + col] ?? 0) + ); +} + +function transpose(matrix: number[][]): number[][] { + const rows = matrix.length; + const cols = matrix[0]?.length ?? 0; + return Array.from({ length: cols }, (_, col) => Array.from({ length: rows }, (_, row) => matrix[row]?.[col] ?? 0)); +} + +function multiplyMatrix(a: number[][], b: number[][]): number[][] { + const rows = a.length; + const cols = b[0]?.length ?? 0; + const inner = b.length; + return Array.from({ length: rows }, (_, row) => + Array.from({ length: cols }, (_, col) => { + let sum = 0; + for (let index = 0; index < inner; index += 1) { + sum += (a[row]?.[index] ?? 0) * (b[index]?.[col] ?? 0); + } + return sum; + }) + ); +} + +function determinant3(matrix: number[][]): number { + const a = matrix[0]?.[0] ?? 0; + const b = matrix[0]?.[1] ?? 0; + const c = matrix[0]?.[2] ?? 0; + const d = matrix[1]?.[0] ?? 0; + const e = matrix[1]?.[1] ?? 0; + const f = matrix[1]?.[2] ?? 0; + const g = matrix[2]?.[0] ?? 0; + const h = matrix[2]?.[1] ?? 0; + const i = matrix[2]?.[2] ?? 0; + return a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g); +} + +function estimateConditionNumber(matrix: number[][]): number { + const columnNorms = transpose(matrix).map((column) => Math.hypot(...column)); + const nonZero = columnNorms.filter((value) => value > 1e-12); + if (nonZero.length === 0) { + return Infinity; + } + return Math.max(...nonZero) / Math.min(...nonZero); +} diff --git a/kdl-wasm/web/src/robot/urdfParser.ts b/kdl-wasm/web/src/robot/urdfParser.ts new file mode 100644 index 0000000..51d8f45 --- /dev/null +++ b/kdl-wasm/web/src/robot/urdfParser.ts @@ -0,0 +1,277 @@ +import { createHash } from "node:crypto"; +import { XMLParser } from "fast-xml-parser"; +import { KdlStructuredError } from "../kdl/rpc.js"; +import type { + JointLimitOverride, + JointLimits, + JointModel, + JointType, + LinkModel, + NormalizedRobotModel, + UrdfLoadOptions +} from "../kdl/types.js"; + +type XmlNode = Record; + +const SUPPORTED_JOINT_TYPES = new Set(["revolute", "continuous", "prismatic", "fixed"]); + +export function loadRobotFromUrdfModel(urdfXml: string, options: UrdfLoadOptions): NormalizedRobotModel { + const robot = parseUrdfRoot(urdfXml); + const robotName = stringAttr(robot["@_name"]) ?? options.robotId; + const links = asArray(robot.link).map(parseLink); + const joints = asArray(robot.joint).map(parseJoint); + + const linkNames = new Set(links.map((link) => link.name)); + if (!linkNames.has(options.baseLink)) { + throw invalidModel(`URDF baseLink does not exist: ${options.baseLink}`); + } + if (!linkNames.has(options.tipLink)) { + throw invalidModel(`URDF tipLink does not exist: ${options.tipLink}`); + } + + const chain = buildChain(joints, options.baseLink, options.tipLink); + const activeJointNames = resolveActiveJointOrder(chain, options.jointOrder); + const limits = activeJointNames.map((name) => { + const joint = joints.find((candidate) => candidate.name === name); + if (!joint) { + throw invalidModel(`Joint order references an unknown joint: ${name}`); + } + return limitForJoint(joint, options.overrideLimits ?? []); + }); + + return { + robotId: options.robotId, + name: robotName, + baseLink: options.baseLink, + tipLink: options.tipLink, + links, + joints, + activeJointNames, + limits, + source: { + type: "urdf", + urdfHash: createHash("sha256").update(urdfXml).digest("hex") + } + }; +} + +function parseUrdfRoot(urdfXml: string): XmlNode { + const parser = new XMLParser({ + ignoreAttributes: false, + attributeNamePrefix: "@_", + trimValues: true, + parseAttributeValue: false, + parseTagValue: false, + allowBooleanAttributes: true + }); + const parsed = parser.parse(urdfXml) as XmlNode; + const robot = parsed.robot; + + if (!isObject(robot)) { + throw invalidModel("URDF document must contain a robot root element"); + } + + return robot; +} + +function parseLink(node: XmlNode): LinkModel { + const name = stringAttr(node["@_name"]); + if (!name) { + throw invalidModel("URDF link is missing name"); + } + return { name }; +} + +function parseJoint(node: XmlNode): JointModel { + const name = stringAttr(node["@_name"]); + const type = stringAttr(node["@_type"]); + if (!name || !type) { + throw invalidModel("URDF joint is missing name or type"); + } + if (!SUPPORTED_JOINT_TYPES.has(type as JointType)) { + throw invalidModel(`Unsupported joint type for ${name}: ${type}`); + } + + const parent = parseLinkRef(node.parent, "parent", name); + const child = parseLinkRef(node.child, "child", name); + const originNode = isObject(node.origin) ? node.origin : {}; + const axisNode = isObject(node.axis) ? node.axis : {}; + const jointType = type as JointType; + + return { + name, + type: jointType, + parent, + child, + origin: { + xyz: parseTriple(stringAttr(originNode["@_xyz"]), [0, 0, 0], `joint ${name} origin xyz`), + rpy: parseTriple(stringAttr(originNode["@_rpy"]), [0, 0, 0], `joint ${name} origin rpy`) + }, + axis: parseTriple(stringAttr(axisNode["@_xyz"]), [1, 0, 0], `joint ${name} axis xyz`), + ...(jointType === "fixed" ? {} : { limit: parseJointLimit(node.limit, name, jointType) }) + }; +} + +function parseJointLimit(node: unknown, jointName: string, jointType: JointType): JointLimits { + const limitNode = isObject(node) ? node : {}; + const continuous = jointType === "continuous"; + const lower = continuous ? -Infinity : numberAttr(limitNode["@_lower"], `joint ${jointName} lower limit`); + const upper = continuous ? Infinity : numberAttr(limitNode["@_upper"], `joint ${jointName} upper limit`); + + return { + name: jointName, + lower, + upper, + velocity: optionalNumberAttr(limitNode["@_velocity"], `joint ${jointName} velocity limit`) ?? Infinity, + acceleration: optionalNumberAttr(limitNode["@_acceleration"], `joint ${jointName} acceleration limit`) ?? Infinity, + ...optionalJerk(limitNode, jointName) + }; +} + +function parseLinkRef(node: unknown, field: "parent" | "child", jointName: string): string { + if (!isObject(node)) { + throw invalidModel(`URDF joint ${jointName} is missing ${field}`); + } + const link = stringAttr(node["@_link"]); + if (!link) { + throw invalidModel(`URDF joint ${jointName} ${field} is missing link`); + } + return link; +} + +function buildChain(joints: JointModel[], baseLink: string, tipLink: string): JointModel[] { + const byParent = new Map(); + for (const joint of joints) { + const children = byParent.get(joint.parent) ?? []; + children.push(joint); + byParent.set(joint.parent, children); + } + + const queue: Array<{ link: string; chain: JointModel[] }> = [{ link: baseLink, chain: [] }]; + const visited = new Set([baseLink]); + + while (queue.length > 0) { + const current = queue.shift(); + if (!current) { + break; + } + if (current.link === tipLink) { + return current.chain; + } + + for (const joint of byParent.get(current.link) ?? []) { + if (visited.has(joint.child)) { + continue; + } + visited.add(joint.child); + queue.push({ link: joint.child, chain: [...current.chain, joint] }); + } + } + + throw invalidModel(`URDF baseLink ${baseLink} is not connected to tipLink ${tipLink}`); +} + +function resolveActiveJointOrder(chain: JointModel[], jointOrder?: string[]): string[] { + const defaultOrder = chain + .filter((joint) => joint.type !== "fixed") + .map((joint) => joint.name); + + if (!jointOrder || jointOrder.length === 0) { + return defaultOrder; + } + + const chainActive = new Set(defaultOrder); + for (const jointName of jointOrder) { + if (!chainActive.has(jointName)) { + throw invalidModel(`jointOrder contains a joint outside the base-tip chain: ${jointName}`); + } + } + + if (jointOrder.length !== defaultOrder.length) { + throw invalidModel("jointOrder must contain every active joint in the base-tip chain exactly once"); + } + + return [...jointOrder]; +} + +function limitForJoint(joint: JointModel, overrides: JointLimitOverride[]): JointLimits { + const base = joint.limit; + if (!base) { + throw invalidModel(`Active joint ${joint.name} is missing limits`); + } + const override = overrides.find((candidate) => candidate.name === joint.name); + if (!override) { + return base; + } + + return { + name: joint.name, + lower: override.lower ?? base.lower, + upper: override.upper ?? base.upper, + velocity: override.velocity ?? base.velocity, + acceleration: override.acceleration ?? base.acceleration, + ...mergedOptionalJerk(override, base) + }; +} + +function optionalJerk(limitNode: XmlNode, jointName: string): Pick | Record { + const jerk = optionalNumberAttr(limitNode["@_jerk"], `joint ${jointName} jerk limit`); + return jerk === undefined ? {} : { jerk }; +} + +function mergedOptionalJerk( + override: JointLimitOverride, + base: JointLimits +): Pick | Record { + const jerk = override.jerk ?? base.jerk; + return jerk === undefined ? {} : { jerk }; +} + +function parseTriple(value: string | undefined, fallback: [number, number, number], context: string): [number, number, number] { + if (!value) { + return fallback; + } + const parts = value.trim().split(/\s+/).map((part) => Number(part)); + if (parts.length !== 3 || parts.some((part) => !Number.isFinite(part))) { + throw invalidModel(`Invalid ${context}: ${value}`); + } + return [parts[0] ?? 0, parts[1] ?? 0, parts[2] ?? 0]; +} + +function numberAttr(value: unknown, context: string): number { + const numberValue = optionalNumberAttr(value, context); + if (numberValue === undefined) { + throw invalidModel(`Missing ${context}`); + } + return numberValue; +} + +function optionalNumberAttr(value: unknown, context: string): number | undefined { + if (value === undefined || value === null || value === "") { + return undefined; + } + const numberValue = Number(value); + if (!Number.isFinite(numberValue)) { + throw invalidModel(`Invalid ${context}: ${String(value)}`); + } + return numberValue; +} + +function stringAttr(value: unknown): string | undefined { + return typeof value === "string" && value.length > 0 ? value : undefined; +} + +function asArray(value: unknown): T[] { + if (value === undefined || value === null) { + return []; + } + return Array.isArray(value) ? (value as T[]) : [value as T]; +} + +function isObject(value: unknown): value is XmlNode { + return typeof value === "object" && value !== null && !Array.isArray(value); +} + +function invalidModel(message: string): KdlStructuredError { + return new KdlStructuredError("KDL_INVALID_MODEL", message); +} diff --git a/kdl-wasm/web/tests/grl/controlFlow.test.ts b/kdl-wasm/web/tests/grl/controlFlow.test.ts new file mode 100644 index 0000000..a9d4be4 --- /dev/null +++ b/kdl-wasm/web/tests/grl/controlFlow.test.ts @@ -0,0 +1,197 @@ +import { describe, expect, it } from "vitest"; +import type { GrlProcedureDeclaration } from "../../src/grl/ast/index.js"; +import { parseGrl } from "../../src/grl/parser/index.js"; +import { parseProcedureControlFlow } from "../../src/grl/semantic/index.js"; + +function procedure(source: string): GrlProcedureDeclaration { + return parseGrl(source).module.declarations.find( + (decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration" + )!; +} + +describe("GRL control-flow compilation", () => { + it("compiles if, elseif, else, while, for, switch, labels, and jumps", () => { + const proc = procedure(`language grl 0.1 +module Main + proc main() + label retry + if ready == true + wait io.di[1] == true + elseif fault == true + jump recovery + else + jump retry + end + while all(io.di[1] == true, io.di[2] == false) + continue + end + for i = 1 to 3 step 1 + movej home + end + switch mode + case 1 + break + case 2 + jump done + default + jump recovery + end + label recovery + label done + end +end +`); + + const flow = parseProcedureControlFlow(proc); + + expect(flow).toEqual([ + expect.objectContaining({ kind: "LABEL", name: "retry", scopePath: [] }), + expect.objectContaining({ + kind: "IF", + branches: [ + expect.objectContaining({ + branchKind: "if", + condition: expect.objectContaining({ text: "ready == true" }), + body: [expect.objectContaining({ kind: "RAW_STATEMENT", text: "wait io . di [ 1 ] == true" })] + }), + expect.objectContaining({ + branchKind: "elseif", + condition: expect.objectContaining({ text: "fault == true" }), + body: [expect.objectContaining({ kind: "JUMP", label: "recovery" })] + }), + expect.objectContaining({ + branchKind: "else", + body: [expect.objectContaining({ kind: "JUMP", label: "retry" })] + }) + ] + }), + expect.objectContaining({ + kind: "WHILE", + condition: expect.objectContaining({ + text: "all ( io . di [ 1 ] == true , io . di [ 2 ] == false )" + }), + body: [expect.objectContaining({ kind: "CONTINUE" })] + }), + expect.objectContaining({ + kind: "FOR", + iterator: "i", + from: expect.objectContaining({ text: "1" }), + to: expect.objectContaining({ text: "3" }), + step: expect.objectContaining({ text: "1" }), + body: [expect.objectContaining({ kind: "RAW_STATEMENT", text: "movej home" })] + }), + expect.objectContaining({ + kind: "SWITCH", + expression: expect.objectContaining({ text: "mode" }), + cases: [ + expect.objectContaining({ caseKind: "case", value: 1, body: [expect.objectContaining({ kind: "BREAK" })] }), + expect.objectContaining({ caseKind: "case", value: 2, body: [expect.objectContaining({ kind: "JUMP", label: "done" })] }), + expect.objectContaining({ caseKind: "default", body: [expect.objectContaining({ kind: "JUMP", label: "recovery" })] }) + ] + }), + expect.objectContaining({ kind: "LABEL", name: "recovery", scopePath: [] }), + expect.objectContaining({ kind: "LABEL", name: "done", scopePath: [] }) + ]); + }); + + it("reports non-boolean control conditions", () => { + const proc = procedure(`language grl 0.1 +module Main + proc main() + if 1 + end + end +end +`); + + expect(() => parseProcedureControlFlow(proc)).toThrowError( + expect.objectContaining({ code: "GRL_CONTROL_CONDITION_NOT_BOOL" }) + ); + }); + + it("reports break and continue outside valid blocks", () => { + const breakProc = procedure(`language grl 0.1 +module Main + proc main() + break + end +end +`); + const continueProc = procedure(`language grl 0.1 +module Main + proc main() + switch mode + case 1 + continue + end + end +end +`); + + expect(() => parseProcedureControlFlow(breakProc)).toThrowError( + expect.objectContaining({ code: "GRL_BREAK_OUTSIDE_FLOW" }) + ); + expect(() => parseProcedureControlFlow(continueProc)).toThrowError( + expect.objectContaining({ code: "GRL_CONTINUE_OUTSIDE_LOOP" }) + ); + }); + + it("reports duplicate or non-constant switch cases", () => { + const duplicateProc = procedure(`language grl 0.1 +module Main + proc main() + switch mode + case 1 + break + case 1 + break + end + end +end +`); + const nonConstantProc = procedure(`language grl 0.1 +module Main + proc main() + switch mode + case mode + 1 + break + end + end +end +`); + + expect(() => parseProcedureControlFlow(duplicateProc)).toThrowError( + expect.objectContaining({ code: "GRL_SWITCH_CASE_DUPLICATE" }) + ); + expect(() => parseProcedureControlFlow(nonConstantProc)).toThrowError( + expect.objectContaining({ code: "GRL_SWITCH_CASE_NOT_CONSTANT" }) + ); + }); + + it("reports labels that cannot be reached by jump", () => { + const intoBlockProc = procedure(`language grl 0.1 +module Main + proc main() + jump inner + if ready == true + label inner + end + end +end +`); + const missingLabelProc = procedure(`language grl 0.1 +module Main + proc main() + jump missing + end +end +`); + + expect(() => parseProcedureControlFlow(intoBlockProc)).toThrowError( + expect.objectContaining({ code: "GRL_JUMP_INTO_BLOCK" }) + ); + expect(() => parseProcedureControlFlow(missingLabelProc)).toThrowError( + expect.objectContaining({ code: "GRL_LABEL_NOT_FOUND" }) + ); + }); +}); diff --git a/kdl-wasm/web/tests/grl/dataDeclarations.test.ts b/kdl-wasm/web/tests/grl/dataDeclarations.test.ts new file mode 100644 index 0000000..040e8ab --- /dev/null +++ b/kdl-wasm/web/tests/grl/dataDeclarations.test.ts @@ -0,0 +1,151 @@ +import { describe, expect, it } from "vitest"; +import type { GrlDataDeclaration, GrlTargetDeclaration } from "../../src/grl/ast/index.js"; +import { parseGrl } from "../../src/grl/parser/index.js"; +import { + compileGrlDataDeclaration, + compileGrlTargetDeclaration, + compileOffsetExpression +} from "../../src/grl/semantic/index.js"; + +function dataDeclarations(source: string): GrlDataDeclaration[] { + return parseGrl(source).module.declarations.filter( + (declaration): declaration is GrlDataDeclaration => declaration.kind === "DataDeclaration" + ); +} + +function targetDeclarations(source: string): GrlTargetDeclaration[] { + return parseGrl(source).module.declarations.filter( + (declaration): declaration is GrlTargetDeclaration => declaration.kind === "TargetDeclaration" + ); +} + +describe("GRL data declarations and target compilation", () => { + it("compiles tool and frame declarations into shared structures", () => { + const [toolDecl, frameDecl] = dataDeclarations(`language grl 0.1 +module Main + persistent tool gripper = tool { + tcp: pose(0 mm, 0 mm, 180 mm, 0 deg, 0 deg, 0 deg), + mass: 2.5 kg, + cog: [0 mm, 0 mm, 80 mm] + } + persistent frame fixture = frame { + origin: pose(800 mm, 0 mm, 200 mm, 0 deg, 0 deg, 0 deg) + } +end +`); + + expect(toolDecl).toMatchObject({ + storage: "persistent", + typeName: "tool", + name: "gripper", + initializer: { kind: "ObjectExpression", typeName: "tool" } + }); + expect(compileGrlDataDeclaration(toolDecl!)).toMatchObject({ + name: "gripper", + value: { + tcp: { + position: [0, 0, 0.18], + quaternion: [0, 0, 0, 1] + }, + mass: 2.5, + cog: [0, 0, 0.08] + } + }); + expect(compileGrlDataDeclaration(frameDecl!)).toMatchObject({ + name: "fixture", + value: { + origin: { + position: [0.8, 0, 0.2], + quaternion: [0, 0, 0, 1] + } + } + }); + }); + + it("compiles speed and zone declarations", () => { + const declarations = dataDeclarations(`language grl 0.1 +module Main + const speed v_joint = joint(80 %) + const speed v_pick = linear(300 mm/s) + const speed v_slow = linear(100 mm/s, acc 500 mm/s2) + const zone z_fine = fine + const zone z10 = z(10 mm) + const zone z_cnt = cnt(30) + const zone z_cont = continuous +end +`); + const compiled = declarations.map(compileGrlDataDeclaration); + + expect(compiled).toMatchObject([ + { name: "v_joint", value: { kind: "joint_percent", value: 0.8 } }, + { name: "v_pick", value: { kind: "linear", velocity: 0.3 } }, + { name: "v_slow", value: { kind: "linear", velocity: 0.1, acceleration: 0.5 } }, + { name: "z_fine", value: { kind: "fine" } }, + { name: "z10", value: { kind: "distance", value: 0.01 } }, + { name: "z_cnt", value: { kind: "cnt", value: 30 } }, + { name: "z_cont", value: { kind: "continuous" } } + ]); + }); + + it("compiles joint_target and pose_target declarations", () => { + const [home, pick] = targetDeclarations(`language grl 0.1 +module Main + target home = joint_target { + joints: [0 deg, -30 deg, 60 deg, 0 deg, 60 deg, 0 deg] + } + target pick = pose_target { + pose: pose(500 mm, 120 mm, 300 mm, 180 deg, 0 deg, 90 deg), + config: robot_config(0, 0, 1), + tool: gripper, + frame: fixture + } +end +`); + + expect(compileGrlTargetDeclaration(home!)).toMatchObject({ + name: "home", + target: { + joints: [0, -Math.PI / 6, Math.PI / 3, 0, Math.PI / 3, 0] + } + }); + + const compiledPick = compileGrlTargetDeclaration(pick!); + expect(compiledPick.name).toBe("pick"); + expect("pose" in compiledPick.target).toBe(true); + if ("pose" in compiledPick.target) { + expect(compiledPick.target.pose.position).toEqual([0.5, 0.12, 0.3]); + expect(compiledPick.target.config).toEqual({ shoulder: 0, elbow: 0, wrist: 1 }); + } + }); + + it("parses and compiles offset expressions", () => { + const [declFrame, declTool] = dataDeclarations(`language grl 0.1 +module Main + var pose_target p2 = pick offset x 20 mm y -10 mm z 50 mm + var pose_target p3 = pick offset_in tool z -50 mm +end +`); + + expect(declFrame?.initializer).toMatchObject({ + kind: "OffsetExpression", + mode: "frame", + axes: [ + { axis: "x" }, + { axis: "y" }, + { axis: "z" } + ] + }); + if (declFrame?.initializer.kind === "OffsetExpression") { + expect(compileOffsetExpression(declFrame.initializer)).toEqual({ + mode: "frame", + xyz: [0.02, -0.01, 0.05] + }); + } + if (declTool?.initializer.kind === "OffsetExpression") { + expect(compileOffsetExpression(declTool.initializer)).toEqual({ + mode: "tool", + xyz: [0, 0, -0.05] + }); + } + }); +}); diff --git a/kdl-wasm/web/tests/grl/exceptionCompile.test.ts b/kdl-wasm/web/tests/grl/exceptionCompile.test.ts new file mode 100644 index 0000000..b595d4a --- /dev/null +++ b/kdl-wasm/web/tests/grl/exceptionCompile.test.ts @@ -0,0 +1,125 @@ +import { describe, expect, it } from "vitest"; +import type { GrlProcedureDeclaration, GrlRawTopLevelDeclaration } from "../../src/grl/ast/index.js"; +import { parseGrl } from "../../src/grl/parser/index.js"; +import { + analyzeExceptionSemantics, + parseProcedureExceptionFlow +} from "../../src/grl/semantic/index.js"; + +const PROGRAM = `language grl 0.1 +module Main + trap recover_trap() + raise E_STOP + end + task background cycle 10 ms + call monitor() + end + proc main() + alarm E_STOP "Emergency stop" severity fatal + try + raise E_STOP + catch E_STOP + alarm RECOVER "Recovering" severity warning + finally + alarm CLEANUP "Cleanup" + end + enable interrupt guard + disable interrupt guard + end +end +`; + +function declarations() { + return parseGrl(PROGRAM).module.declarations; +} + +describe("GRL alarm, raise, try/catch, interrupt, and task semantics", () => { + it("keeps trap and task as parsed raw declarations for P1 diagnostics", () => { + const raw = declarations().filter( + (decl): decl is GrlRawTopLevelDeclaration => decl.kind === "RawTopLevelDeclaration" + ); + + expect(raw[0]?.declarationType).toBe("trap"); + expect(raw[0]?.tokens[0]).toMatchObject({ raw: "trap" }); + expect(raw[0]?.tokens[1]).toMatchObject({ raw: "recover_trap" }); + expect(raw[1]?.declarationType).toBe("task"); + expect(raw[1]?.tokens[0]).toMatchObject({ raw: "task" }); + expect(raw[1]?.tokens[1]).toMatchObject({ raw: "background" }); + }); + + it("compiles alarm, raise, try/catch/finally, and interrupt diagnostics from procedure body", () => { + const procedure = declarations().find( + (decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration" + )!; + + expect(parseProcedureExceptionFlow(procedure)).toEqual([ + expect.objectContaining({ + kind: "ALARM", + alarmId: "E_STOP", + message: "Emergency stop", + severity: "fatal" + }), + expect.objectContaining({ + kind: "TRY", + body: [expect.objectContaining({ kind: "RAISE", alarmId: "E_STOP" })], + catches: [ + expect.objectContaining({ + alarmId: "E_STOP", + body: [ + expect.objectContaining({ + kind: "ALARM", + alarmId: "RECOVER", + message: "Recovering", + severity: "warning" + }) + ] + }) + ], + finally: expect.objectContaining({ + body: [expect.objectContaining({ kind: "ALARM", alarmId: "CLEANUP", message: "Cleanup" })] + }) + }), + expect.objectContaining({ kind: "UNSUPPORTED_RUNTIME", feature: "interrupt" }), + expect.objectContaining({ kind: "UNSUPPORTED_RUNTIME", feature: "interrupt" }) + ]); + }); + + it("reports P1 trap/task semantics as explicit unsupported diagnostics", () => { + const analysis = analyzeExceptionSemantics(declarations()); + + expect(analysis.unsupported).toEqual([ + expect.objectContaining({ kind: "UNSUPPORTED_RUNTIME", feature: "trap" }), + expect.objectContaining({ kind: "UNSUPPORTED_RUNTIME", feature: "task" }) + ]); + expect(analysis.diagnostics).toEqual([ + expect.objectContaining({ severity: "warning", code: "GRL_P1_UNIMPLEMENTED" }), + expect.objectContaining({ severity: "warning", code: "GRL_P1_UNIMPLEMENTED" }) + ]); + }); + + it("reports missing alarm ids and try blocks without handlers", () => { + const missingAlarmId = parseGrl(`language grl 0.1 +module Main + proc main() + alarm + end +end +`).module.declarations.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!; + const tryWithoutHandler = parseGrl(`language grl 0.1 +module Main + proc main() + try + raise E_STOP + end + end +end +`).module.declarations.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!; + + expect(() => parseProcedureExceptionFlow(missingAlarmId)).toThrowError( + expect.objectContaining({ code: "GRL_ALARM_ID_MISSING" }) + ); + expect(() => parseProcedureExceptionFlow(tryWithoutHandler)).toThrowError( + expect.objectContaining({ code: "GRL_TRY_HANDLER_MISSING" }) + ); + }); +}); diff --git a/kdl-wasm/web/tests/grl/generator/roundtrip.test.ts b/kdl-wasm/web/tests/grl/generator/roundtrip.test.ts new file mode 100644 index 0000000..fefe791 --- /dev/null +++ b/kdl-wasm/web/tests/grl/generator/roundtrip.test.ts @@ -0,0 +1,149 @@ +import { describe, expect, it } from "vitest"; +import { generateGrlProgram, type GrlProgramGenerationSpec } from "../../../src/grl/generator/index.js"; +import { postProcessAllBrands } from "../../../src/grl/post/index.js"; +import type { GrlOperationDeclaration, GrlPathDeclaration, GrlTargetDeclaration } from "../../../src/grl/ast/index.js"; +import { parseGrl } from "../../../src/grl/parser/index.js"; +import { compileSemanticProgram } from "../../../src/grl/semantic/index.js"; + +const SPEC: GrlProgramGenerationSpec = { + moduleName: "GeneratedCell", + speeds: { + v_linear: "linear(200 mm/s)", + v_joint: "joint(50 %)" + }, + zones: { + z10: "z(10 mm)", + zf: "fine" + }, + targets: [ + { name: "pick", kind: "pose", values: [500, 0, 0, 0, 0, 0] }, + { name: "home", kind: "joint", values: [0] }, + { name: "place", kind: "pose", values: [600, 0, 0, 0, 0, 0] } + ], + path: { + name: "generated_path", + source: { + type: "cad_curve", + id: "edge_001", + sample_distance: 5 + }, + defaults: { + speed: "v_linear", + zone: "z10" + }, + points: [ + { motion: "movej", target: "home", speed: "v_joint", zone: "zf" }, + { motion: "movel", target: "pick" }, + { id: "place_point", motion: "movel", target: "place", zone: "zf" } + ] + }, + operation: { + name: "generated_op", + kind: "handling", + path: "generated_path", + startAction: "io.do[1] = true", + endAction: "io.do[1] = false" + } +}; + +describe("GRL generator and roundtrip", () => { + it("generates stable expanded GRL with target/path/operation first", () => { + const first = generateGrlProgram(SPEC, "expanded"); + const second = generateGrlProgram(SPEC, "expanded"); + + expect(first).toEqual(second); + expect(first.stableIds).toEqual({ + targets: ["home", "pick", "place"], + points: ["p00", "p01", "place_point"], + path: "generated_path", + operation: "generated_op" + }); + expect(first.text).toBe(`language grl 0.1 +module GeneratedCell + const speed v_joint = joint(50 %) + const speed v_linear = linear(200 mm/s) + const zone z10 = z(10 mm) + const zone zf = fine + target home = joint_target { + joints: [0 deg] + } + target pick = pose_target { + pose: pose(500 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg) + } + target place = pose_target { + pose: pose(600 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg) + } + path generated_path { + source { + id: "edge_001" + sample_distance: 5 + type: cad_curve + } + defaults { + speed: v_linear + zone: z10 + } + point p00 movej home speed v_joint zone zf + point p01 movel pick + point place_point movel place zone zf + } + operation generated_op { + kind: handling + path: generated_path + start_action: + io.do[1] = true + end_action: + io.do[1] = false + } + proc main() + run_operation generated_op + end +end`); + }); + + it("supports compact output that remains parseable", () => { + const compact = generateGrlProgram(SPEC, "compact"); + + expect(compact.text).toContain("path generated_path { source { id: \"edge_001\""); + expect(parseGrl(compact.text).module.name).toBe("GeneratedCell"); + }); + + it("roundtrips through parser, semantic IR, and postprocessors", () => { + const generated = generateGrlProgram(SPEC, "expanded"); + const ast = parseGrl(generated.text); + const declarations = ast.module.declarations; + const targets = declarations.filter((decl): decl is GrlTargetDeclaration => decl.kind === "TargetDeclaration"); + const path = declarations.find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!; + const operation = declarations.find((decl): decl is GrlOperationDeclaration => decl.kind === "OperationDeclaration")!; + const ir = compileSemanticProgram(ast, { + startJoints: [0], + sampleTime: 0.004 + }); + const post = postProcessAllBrands(ir); + + expect(targets.map((target) => target.name)).toEqual(["home", "pick", "place"]); + expect(path.items).toEqual( + expect.arrayContaining([ + expect.objectContaining({ kind: "PathSourceBlock" }), + expect.objectContaining({ kind: "PathDefaultsBlock" }), + expect.objectContaining({ kind: "PathPoint", id: "p00" }), + expect.objectContaining({ kind: "PathPoint", id: "p01" }), + expect.objectContaining({ kind: "PathPoint", id: "place_point" }) + ]) + ); + expect(operation).toMatchObject({ + name: "generated_op", + operationKind: "handling", + pathName: "generated_path" + }); + expect(ir.symbols).toEqual( + expect.arrayContaining([ + expect.objectContaining({ kind: "path", name: "generated_path" }), + expect.objectContaining({ kind: "operation", name: "generated_op" }) + ]) + ); + expect(post.outputs.abb.text).toContain("MODULE GeneratedCell"); + expect(post.outputs.fanuc.text).toContain("/PROG MAIN"); + expect(post.outputs.kuka.text).toContain("DEF Main()"); + }); +}); diff --git a/kdl-wasm/web/tests/grl/ioCompile.test.ts b/kdl-wasm/web/tests/grl/ioCompile.test.ts new file mode 100644 index 0000000..d95b0dd --- /dev/null +++ b/kdl-wasm/web/tests/grl/ioCompile.test.ts @@ -0,0 +1,205 @@ +import { describe, expect, it } from "vitest"; +import type { + GrlDataDeclaration, + GrlOperationDeclaration, + GrlPathDeclaration, + GrlProcedureDeclaration, + GrlTargetDeclaration +} from "../../src/grl/ast/index.js"; +import type { PathEventInstruction } from "../../src/grl/ir/index.js"; +import { parseGrl } from "../../src/grl/parser/index.js"; +import { + buildMotionContext, + compileOperation, + compileOperationActionIo, + compilePathEventIo, + compilePathToPlanRequest, + parseIoFlowStatements, + type IoMap +} from "../../src/grl/semantic/index.js"; + +const PROGRAM = `language grl 0.1 +module Main + const speed v = joint(50 %) + const zone zf = fine + target home = joint_target { joints: [0 deg] } + path io_path { + defaults { speed: v, zone: zf } + point p0 movej home + event at p0 distance 0 mm pulse io.do[20] duration 100 ms + } + operation io_op { + kind: handling + path: io_path + start_action: + wait io.di[4] == true timeout 500 ms on_timeout alarm "part missing" + end_action: + pulse io.do[5] duration 250 ms + } + proc main() + io.do[1] = true + io.go[2] = 16 + io.alias.grip_close = false + wait all(io.di[1] == true, io.di[2] == false) timeout 2 s on_timeout alarm "Clamp close timeout" + wait any(rising(io.di[3]), falling(io.di[4]), changed(io.ai[1])) + wait io.di[5] == true timeout 1 s on_timeout call recover + pulse io.do[3] duration 200 ms + end +end +`; + +const IO_MAP: IoMap = { + aliases: { + grip_close: { domain: "do", index: 6, raw: "io.do[6]" } + }, + allowedRanges: { + ai: { min: 1, max: 8 }, + di: { min: 1, max: 16 }, + do: { min: 1, max: 32 }, + go: { min: 1, max: 4 } + } +}; + +function declarations() { + return parseGrl(PROGRAM).module.declarations; +} + +function motionContext(decls = declarations()) { + return buildMotionContext( + decls.filter( + (decl): decl is GrlDataDeclaration | GrlTargetDeclaration => + decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration" + ) + ); +} + +function pathsByName(paths: GrlPathDeclaration[]) { + return new Map(paths.map((path) => [path.name, path])); +} + +describe("GRL IO, wait, and pulse compilation", () => { + it("compiles procedure IO writes, wait conditions, timeout actions, and pulse traces", () => { + const procedure = declarations().find( + (decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration" + )!; + + const instructions = parseIoFlowStatements(procedure.bodyTokens, IO_MAP); + + expect(instructions).toHaveLength(7); + expect(instructions[0]).toMatchObject({ + kind: "IO_WRITE", + target: { domain: "do", index: 1, raw: "io.do[1]" }, + value: true + }); + expect(instructions[1]).toMatchObject({ + kind: "IO_WRITE", + target: { domain: "go", index: 2, raw: "io.go[2]" }, + value: 16 + }); + expect(instructions[2]).toMatchObject({ + kind: "IO_WRITE", + target: { domain: "do", index: 6, raw: "io.do[6]" }, + value: false + }); + expect(instructions[3]).toMatchObject({ + kind: "WAIT", + condition: "all ( io . di [ 1 ] == true , io . di [ 2 ] == false )", + timeout: 2, + onTimeout: { kind: "alarm", value: "Clamp close timeout" } + }); + expect(instructions[4]).toMatchObject({ + kind: "WAIT", + condition: "any ( rising ( io . di [ 3 ] ) , falling ( io . di [ 4 ] ) , changed ( io . ai [ 1 ] ) )" + }); + expect(instructions[5]).toMatchObject({ + kind: "WAIT", + condition: "io . di [ 5 ] == true", + timeout: 1, + onTimeout: { kind: "call", value: "recover" } + }); + expect(instructions[6]).toMatchObject({ + kind: "PULSE", + target: { domain: "do", index: 3, raw: "io.do[3]" }, + duration: 0.2, + trace: [ + { time: 0, action: "set", target: { domain: "do", index: 3 }, value: true }, + { time: 0.2, action: "reset", target: { domain: "do", index: 3 }, value: false } + ] + }); + }); + + it("validates IO addresses against configured ranges", () => { + const procedure = parseGrl(`language grl 0.1 +module Main + proc main() + io.do[99] = true + end +end +`).module.declarations.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!; + + expect(() => + parseIoFlowStatements(procedure.bodyTokens, { allowedRanges: { do: { min: 1, max: 16 } } }) + ).toThrowError(expect.objectContaining({ code: "GRL_IO_ADDRESS_NOT_FOUND" })); + }); + + it("expands path event IO metadata into pulse IR without entering KDL motion segments", () => { + const decls = declarations(); + const path = decls.find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!; + const compiled = compilePathToPlanRequest(path, motionContext(decls), { + startJoints: [0], + sampleTime: 0.004 + }); + + expect(compiled.request.segments).toHaveLength(1); + expect(compiled.request.events).toHaveLength(1); + expect(compilePathEventIo(compiled.events[0]!, IO_MAP)).toEqual([ + expect.objectContaining({ + kind: "PULSE", + target: { domain: "do", index: 20, raw: "io.do[20]" }, + duration: 0.1 + }) + ]); + }); + + it("expands operation action metadata into wait and pulse IR with preserved units", () => { + const decls = declarations(); + const operation = decls.find( + (decl): decl is GrlOperationDeclaration => decl.kind === "OperationDeclaration" + )!; + const paths = decls.filter((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration"); + const compiled = compileOperation(operation, pathsByName(paths)); + + expect(compileOperationActionIo(compiled.startActions[0]!, IO_MAP)).toEqual([ + expect.objectContaining({ + kind: "WAIT", + condition: "io . di [ 4 ] == true", + timeout: 0.5, + onTimeout: { kind: "alarm", value: "part missing" } + }) + ]); + expect(compileOperationActionIo(compiled.endActions[0]!, IO_MAP)).toEqual([ + expect.objectContaining({ + kind: "PULSE", + target: { domain: "do", index: 5, raw: "io.do[5]" }, + duration: 0.25 + }) + ]); + }); + + it("lexes statement fallback metadata so unit literals and booleans remain typed", () => { + const event: PathEventInstruction = { + timing: "at", + pointId: "p0", + kind: "pulse", + data: { statement: "pulse io.do[7] duration 125 ms" } + }; + + expect(compilePathEventIo(event, IO_MAP)).toEqual([ + expect.objectContaining({ + kind: "PULSE", + target: { domain: "do", index: 7, raw: "io.do[7]" }, + duration: 0.125 + }) + ]); + }); +}); diff --git a/kdl-wasm/web/tests/grl/lexer.test.ts b/kdl-wasm/web/tests/grl/lexer.test.ts new file mode 100644 index 0000000..a0351b2 --- /dev/null +++ b/kdl-wasm/web/tests/grl/lexer.test.ts @@ -0,0 +1,94 @@ +import { describe, expect, it } from "vitest"; +import { GRL_KEYWORDS, lexGrl, normalizeUnitLiteral, normalizeUnitValue } from "../../src/grl/lexer/index.js"; +import type { GrlNumberToken } from "../../src/grl/lexer/index.js"; + +function numbers(source: string): GrlNumberToken[] { + return lexGrl(source).filter((token): token is GrlNumberToken => token.kind === "number"); +} + +describe("GRL lexer", () => { + it("recognizes comments, keywords, identifiers, and source positions", () => { + const tokens = lexGrl(`// generated\nlanguage grl 0.1\nmodule Main\n proc main()\n end\nend\n`); + + expect(tokens[0]).toMatchObject({ + kind: "comment", + style: "line", + value: " generated", + range: { + start: { line: 1, column: 1 }, + end: { line: 1, column: 13 } + } + }); + expect(tokens.filter((token) => token.kind === "keyword").map((token) => token.raw)).toEqual([ + "language", + "module", + "proc", + "end", + "end" + ]); + expect(tokens.find((token) => token.raw === "Main")).toMatchObject({ + kind: "identifier", + range: { + start: { line: 3, column: 8 } + } + }); + expect(tokens.at(-1)).toMatchObject({ kind: "eof" }); + }); + + it("normalizes numeric literals with GRL units into SI values", () => { + const found = numbers("100 mm 0.25 m 180 deg 3.14159 rad 300 mm/s 50 % 200 ms 2.5 kg 500 mm/s2"); + + expect(found.map((token) => token.unit?.raw)).toEqual([ + "mm", + "m", + "deg", + "rad", + "mm/s", + "%", + "ms", + "kg", + "mm/s2" + ]); + expect(found[0]?.unit?.normalizedValue).toBeCloseTo(0.1); + expect(found[1]?.unit?.normalizedValue).toBeCloseTo(0.25); + expect(found[2]?.unit?.normalizedValue).toBeCloseTo(Math.PI); + expect(found[3]?.unit?.normalizedValue).toBeCloseTo(3.14159); + expect(found[4]?.unit?.normalizedValue).toBeCloseTo(0.3); + expect(found[5]?.unit?.normalizedValue).toBeCloseTo(0.5); + expect(found[6]?.unit?.normalizedValue).toBeCloseTo(0.2); + expect(found[7]?.unit?.normalizedValue).toBeCloseTo(2.5); + expect(found[8]?.unit?.normalizedValue).toBeCloseTo(0.5); + }); + + it("keeps unit raw text on number tokens", () => { + const [token] = numbers("linear(300 mm/s)"); + + expect(token).toMatchObject({ + kind: "number", + raw: "300 mm/s", + value: 300, + unit: { + raw: "mm/s", + kind: "linear_velocity", + siUnit: "m/s" + } + }); + }); + + it("exposes the full reserved keyword set from the specification", () => { + expect(GRL_KEYWORDS).toContain("movej"); + expect(GRL_KEYWORDS).toContain("run_operation"); + expect(GRL_KEYWORDS).toContain("post_hint"); + expect(GRL_KEYWORDS).toContain("continuous"); + expect(GRL_KEYWORDS).toHaveLength(85); + }); + + it("provides direct unit helpers for parser and semantic layers", () => { + expect(normalizeUnitLiteral("deg/s")).toMatchObject({ + kind: "angular_velocity", + siUnit: "rad/s" + }); + expect(normalizeUnitValue(90, "deg/s")).toBeCloseTo(Math.PI / 2); + expect(() => normalizeUnitLiteral("inch")).toThrow("Unknown GRL unit"); + }); +}); diff --git a/kdl-wasm/web/tests/grl/motionCompile.test.ts b/kdl-wasm/web/tests/grl/motionCompile.test.ts new file mode 100644 index 0000000..5ed31e4 --- /dev/null +++ b/kdl-wasm/web/tests/grl/motionCompile.test.ts @@ -0,0 +1,124 @@ +import { describe, expect, it } from "vitest"; +import type { GrlDataDeclaration, GrlProcedureDeclaration, GrlTargetDeclaration } from "../../src/grl/ast/index.js"; +import { parseGrl } from "../../src/grl/parser/index.js"; +import { + buildMotionContext, + compileMotionToKdlRequest, + parseProcedureMotionInstructions +} from "../../src/grl/semantic/index.js"; + +const PROGRAM = `language grl 0.1 +module Main + persistent tool gripper = tool { + tcp: pose(0 mm, 0 mm, 100 mm, 0 deg, 0 deg, 0 deg), + mass: 1 kg + } + persistent frame fixture = frame { + origin: pose(800 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg) + } + const speed v_joint = joint(60 %) + const speed v_linear = linear(300 mm/s) + const zone z10 = z(10 mm) + target home = joint_target { + joints: [0 deg, 0 deg] + } + target pick = pose_target { + pose: pose(500 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg), + tool: gripper, + frame: fixture + } + target mid = pose_target { + pose: pose(550 mm, 50 mm, 0 mm, 0 deg, 0 deg, 0 deg) + } + target arc_end = pose_target { + pose: pose(600 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg) + } + proc main() + set_tool gripper + set_frame fixture + set_speed v_linear + set_zone z10 + movej home speed v_joint zone fine + movel pick + movec via mid target arc_end speed linear(150 mm/s) zone fine + end +end +`; + +function declarations() { + return parseGrl(PROGRAM).module.declarations; +} + +describe("GRL motion instruction compilation", () => { + it("parses movej, movel, and movec from procedure body tokens", () => { + const decls = declarations(); + const context = buildMotionContext( + decls.filter( + (decl): decl is GrlDataDeclaration | GrlTargetDeclaration => + decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration" + ) + ); + const procedure = decls.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!; + const instructions = parseProcedureMotionInstructions(procedure, context); + + expect(instructions.map((instruction) => instruction.kind)).toEqual(["MOVEJ", "MOVEL", "MOVEC"]); + expect(instructions[0]).toMatchObject({ + kind: "MOVEJ", + speed: { kind: "joint_percent", value: 0.6 }, + zone: { kind: "fine" }, + target: { joints: [0, 0] }, + sourceMap: { line: 32 } + }); + expect(instructions[1]).toMatchObject({ + kind: "MOVEL", + speed: { kind: "linear", velocity: 0.3 }, + zone: { kind: "distance", value: 0.01 } + }); + expect(instructions[1]?.tool?.position).toEqual([0, 0, 0.1]); + expect(instructions[1]?.frame?.position).toEqual([0.8, 0, 0]); + expect(instructions[2]).toMatchObject({ + kind: "MOVEC", + speed: { kind: "linear", velocity: 0.15 }, + zone: { kind: "fine" } + }); + }); + + it("compiles motion instructions to KDL request shapes", () => { + const decls = declarations(); + const context = buildMotionContext( + decls.filter( + (decl): decl is GrlDataDeclaration | GrlTargetDeclaration => + decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration" + ) + ); + const procedure = decls.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!; + const [movej, movel, movec] = parseProcedureMotionInstructions(procedure, context); + + expect(compileMotionToKdlRequest(movej!, { startJoints: [0, 0], sampleTime: 0.004 })).toMatchObject({ + startJoints: [0, 0], + target: { joints: [0, 0] }, + speed: { kind: "joint_percent", value: 0.6 }, + zone: { kind: "fine" }, + sampleTime: 0.004 + }); + + expect(compileMotionToKdlRequest(movel!, { startJoints: [0, 0], sampleTime: 0.004 })).toMatchObject({ + startJoints: [0, 0], + target: { pose: { position: [0.5, 0, 0] } }, + speed: { kind: "linear", velocity: 0.3 }, + zone: { kind: "distance", value: 0.01 }, + tool: { position: [0, 0, 0.1] }, + frame: { position: [0.8, 0, 0] }, + sampleTime: 0.004 + }); + + expect(compileMotionToKdlRequest(movec!, { startJoints: [0, 0], sampleTime: 0.004 })).toMatchObject({ + startJoints: [0, 0], + via: { pose: { position: [0.55, 0.05, 0] } }, + target: { pose: { position: [0.6, 0, 0] } }, + speed: { kind: "linear", velocity: 0.15 }, + zone: { kind: "fine" }, + sampleTime: 0.004 + }); + }); +}); diff --git a/kdl-wasm/web/tests/grl/operationCompile.test.ts b/kdl-wasm/web/tests/grl/operationCompile.test.ts new file mode 100644 index 0000000..c01c12f --- /dev/null +++ b/kdl-wasm/web/tests/grl/operationCompile.test.ts @@ -0,0 +1,153 @@ +import { describe, expect, it } from "vitest"; +import type { + GrlOperationDeclaration, + GrlPathDeclaration, + GrlProcedureDeclaration +} from "../../src/grl/ast/index.js"; +import { parseGrl } from "../../src/grl/parser/index.js"; +import { + compileOperation, + expandRunOperation, + parseProcedureRunOperationStatements +} from "../../src/grl/semantic/index.js"; + +const PROGRAM = `language grl 0.1 +module Main + const speed v = joint(50 %) + const zone zf = fine + target home = joint_target { joints: [0 deg] } + path weld_path { + defaults { speed: v, zone: zf } + point p0 movej home + } + operation weld_op_01 { + kind: arc_welding + path: weld_path + process { + weld_id: "WELD_1" + voltage: 24.0 + current: 180.0 + weave: none + } + start_action: + io.do[20] = true + end_action: + io.do[20] = false + } + proc main() + run_operation weld_op_01 + end +end +`; + +function declarations() { + return parseGrl(PROGRAM).module.declarations; +} + +function pathsByName(paths: GrlPathDeclaration[]) { + return new Map(paths.map((path) => [path.name, path])); +} + +describe("GRL operation compilation", () => { + it("parses operation kind, path, process, and action blocks", () => { + const operation = declarations().find( + (decl): decl is GrlOperationDeclaration => decl.kind === "OperationDeclaration" + )!; + + expect(operation).toMatchObject({ + kind: "OperationDeclaration", + name: "weld_op_01", + operationKind: "arc_welding", + pathName: "weld_path", + items: [ + { kind: "OperationProcessBlock" }, + { kind: "OperationActionBlock", actionKind: "start_action" }, + { kind: "OperationActionBlock", actionKind: "end_action" } + ] + }); + }); + + it("compiles operation process metadata and action statements", () => { + const decls = declarations(); + const operation = decls.find((decl): decl is GrlOperationDeclaration => decl.kind === "OperationDeclaration")!; + const paths = decls.filter((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration"); + const compiled = compileOperation(operation, pathsByName(paths)); + + expect(compiled).toMatchObject({ + operationId: "weld_op_01", + kind: "arc_welding", + pathId: "weld_path", + process: { + weld_id: "WELD_1", + voltage: 24, + current: 180, + weave: "none" + }, + startActions: [ + { + kind: "ACTION", + actionKind: "start_action", + operationId: "weld_op_01", + statement: "io . do [ 20 ] = true" + } + ], + endActions: [ + { + kind: "ACTION", + actionKind: "end_action", + operationId: "weld_op_01", + statement: "io . do [ 20 ] = false" + } + ] + }); + }); + + it("extracts run_operation and expands to start action, path, and end action", () => { + const decls = declarations(); + const procedure = decls.find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!; + const operation = decls.find((decl): decl is GrlOperationDeclaration => decl.kind === "OperationDeclaration")!; + const paths = decls.filter((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration"); + const compiled = compileOperation(operation, pathsByName(paths)); + const [run] = parseProcedureRunOperationStatements(procedure); + + expect(run).toEqual({ + kind: "RUN_OPERATION", + operationId: "weld_op_01", + sourceMap: { + line: 25, + column: 5 + } + }); + expect(expandRunOperation(run!, new Map([[compiled.operationId, compiled]]))).toEqual([ + expect.objectContaining({ kind: "ACTION", actionKind: "start_action" }), + { + kind: "RUN_PATH", + pathId: "weld_path", + sourceMap: { + line: 25, + column: 5 + } + }, + expect.objectContaining({ kind: "ACTION", actionKind: "end_action" }) + ]); + }); + + it("reports operations that reference missing paths and missing run_operation targets", () => { + const missingPathOperation = parseGrl(`language grl 0.1 +module Main + operation bad_op { + kind: handling + path: missing_path + } +end +`).module.declarations.find((decl): decl is GrlOperationDeclaration => decl.kind === "OperationDeclaration")!; + + expect(() => compileOperation(missingPathOperation, new Map())).toThrowError( + expect.objectContaining({ code: "GRL_OPERATION_PATH_NOT_FOUND" }) + ); + + expect(() => + expandRunOperation({ kind: "RUN_OPERATION", operationId: "missing_op" }, new Map()) + ).toThrowError(expect.objectContaining({ code: "GRL_OPERATION_NOT_FOUND" })); + }); +}); diff --git a/kdl-wasm/web/tests/grl/parser.test.ts b/kdl-wasm/web/tests/grl/parser.test.ts new file mode 100644 index 0000000..63f2d93 --- /dev/null +++ b/kdl-wasm/web/tests/grl/parser.test.ts @@ -0,0 +1,103 @@ +import { describe, expect, it } from "vitest"; +import { GrlParseError, parseGrl } from "../../src/grl/parser/index.js"; + +describe("GRL parser", () => { + it("parses the minimal language/module/proc skeleton with source ranges", () => { + const ast = parseGrl(`language grl 0.1 + +module Main + proc main() + // body comment should not affect parser + end +end +`); + + expect(ast).toMatchObject({ + kind: "Program", + language: { + kind: "LanguageDeclaration", + language: "grl", + version: "0.1", + range: { + start: { line: 1, column: 1 }, + end: { line: 1, column: 17 } + } + }, + module: { + kind: "ModuleDeclaration", + name: "Main", + declarations: [ + { + kind: "ProcedureDeclaration", + name: "main", + params: [] + } + ] + } + }); + expect(ast.module.range.start).toMatchObject({ line: 3, column: 1 }); + expect(ast.module.range.end).toMatchObject({ line: 7, column: 4 }); + }); + + it("parses imports, data declarations, targets, and procedure body tokens", () => { + const ast = parseGrl(`language grl 0.1 +module Main + import CommonTools + const speed v_pick = linear(300 mm/s) + target home = joint_target { + joints: [0 deg, 0 deg] + } + proc main() + movej home + end +end +`); + + expect(ast.module.declarations.map((decl) => decl.kind)).toEqual([ + "ImportDeclaration", + "DataDeclaration", + "TargetDeclaration", + "ProcedureDeclaration" + ]); + expect(ast.module.declarations[0]).toMatchObject({ + kind: "ImportDeclaration", + moduleName: "CommonTools" + }); + expect(ast.module.declarations[1]).toMatchObject({ + kind: "DataDeclaration", + storage: "const", + typeName: "speed", + name: "v_pick", + initializer: { + kind: "CallExpression", + callee: "linear" + } + }); + expect(ast.module.declarations[2]).toMatchObject({ + kind: "TargetDeclaration", + name: "home", + target: { + kind: "ObjectExpression", + typeName: "joint_target" + } + }); + expect(ast.module.declarations[3]).toMatchObject({ + kind: "ProcedureDeclaration", + bodyTokens: [ + { + kind: "keyword", + raw: "movej" + }, + { + kind: "identifier", + raw: "home" + } + ] + }); + }); + + it("reports stable line and column on invalid syntax", () => { + expect(() => parseGrl("language grl\nmodule Main\nend\n")).toThrow(GrlParseError); + expect(() => parseGrl("language grl\nmodule Main\nend\n")).toThrow("Expected GRL language version at 2:1"); + }); +}); diff --git a/kdl-wasm/web/tests/grl/pathCompile.test.ts b/kdl-wasm/web/tests/grl/pathCompile.test.ts new file mode 100644 index 0000000..ff39212 --- /dev/null +++ b/kdl-wasm/web/tests/grl/pathCompile.test.ts @@ -0,0 +1,251 @@ +import { describe, expect, it } from "vitest"; +import type { + GrlDataDeclaration, + GrlPathDeclaration, + GrlProcedureDeclaration, + GrlTargetDeclaration +} from "../../src/grl/ast/index.js"; +import { parseGrl } from "../../src/grl/parser/index.js"; +import { + buildMotionContext, + compilePathToPlanRequest, + parseProcedureRunPathStatements +} from "../../src/grl/semantic/index.js"; + +const PROGRAM = `language grl 0.1 +module Main + persistent tool gripper = tool { + tcp: pose(0 mm, 0 mm, 100 mm, 0 deg, 0 deg, 0 deg) + } + persistent frame fixture = frame { + origin: pose(800 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg) + } + const speed v_joint = joint(60 %) + const speed v_linear = linear(300 mm/s) + const zone z10 = z(10 mm) + target home = joint_target { + joints: [0 deg, 0 deg] + } + target pick = pose_target { + pose: pose(500 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg) + } + target mid = pose_target { + pose: pose(550 mm, 50 mm, 0 mm, 0 deg, 0 deg, 0 deg) + } + target arc_end = pose_target { + pose: pose(600 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg) + } + path pick_path { + source { + type: cad_curve + id: "edge_032" + sample_distance: 5 mm + } + defaults { + tool: gripper, + frame: fixture, + speed: v_linear, + zone: z10 + } + point approach movej home speed v_joint zone fine + point p1 movel pick offset z 100 mm + point p2 movec via mid target arc_end speed linear(150 mm/s) zone fine + event before p1 io.do[10] = true + event after p2 io.do[10] = false + event at p1 distance -20 mm pulse io.do[20] duration 100 ms + } + proc main() + run_path pick_path + end +end +`; + +function declarations() { + return parseGrl(PROGRAM).module.declarations; +} + +function motionContext(decls = declarations()) { + return buildMotionContext( + decls.filter( + (decl): decl is GrlDataDeclaration | GrlTargetDeclaration => + decl.kind === "DataDeclaration" || decl.kind === "TargetDeclaration" + ) + ); +} + +describe("GRL path compilation", () => { + it("parses path defaults, source metadata, points, and events as AST nodes", () => { + const path = declarations().find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!; + + expect(path).toMatchObject({ + kind: "PathDeclaration", + name: "pick_path", + items: [ + { kind: "PathSourceBlock" }, + { kind: "PathDefaultsBlock" }, + { kind: "PathPoint", id: "approach" }, + { kind: "PathPoint", id: "p1" }, + { kind: "PathPoint", id: "p2" }, + { kind: "PathEvent", timing: "before", pointId: "p1" }, + { kind: "PathEvent", timing: "after", pointId: "p2" }, + { kind: "PathEvent", timing: "at", pointId: "p1" } + ] + }); + expect(path.items[0]).toMatchObject({ + properties: [ + { key: "type", value: { kind: "IdentifierExpression", name: "cad_curve" } }, + { key: "id", value: { kind: "StringLiteral", value: "edge_032" } }, + { key: "sample_distance", value: { kind: "NumberLiteral" } } + ] + }); + }); + + it("compiles a path to PathPlanRequest with defaults, source map, source metadata, and events", () => { + const decls = declarations(); + const path = decls.find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!; + const compiled = compilePathToPlanRequest(path, motionContext(decls), { + startJoints: [0, 0], + sampleTime: 0.004 + }); + + expect(compiled.pathId).toBe("pick_path"); + expect(compiled.request).toMatchObject({ + pathId: "pick_path", + startJoints: [0, 0], + sampleTime: 0.004, + source: { + type: "cad_curve", + id: "edge_032", + sample_distance: 0.005 + }, + segments: [ + { + id: "approach", + motion: "MOVEJ", + targetId: "home", + speed: { kind: "joint_percent", value: 0.6 }, + zone: { kind: "fine" } + }, + { + id: "p1", + motion: "MOVEL", + targetId: "pick", + speed: { kind: "linear", velocity: 0.3 }, + zone: { kind: "distance", value: 0.01 }, + tool: { position: [0, 0, 0.1] }, + frame: { position: [0.8, 0, 0] }, + sourceMap: { line: 37 } + }, + { + id: "p2", + motion: "MOVEC", + targetId: "arc_end", + speed: { kind: "linear", velocity: 0.15 }, + zone: { kind: "fine" } + } + ], + events: [ + { + timing: "before", + pointId: "p1", + kind: "io", + data: { statement: "io . do [ 10 ] = true" } + }, + { + timing: "after", + pointId: "p2", + kind: "io" + }, + { + timing: "at", + pointId: "p1", + distance: -0.02, + kind: "pulse" + } + ] + }); + expect(compiled.request.segments[1]?.target).toMatchObject({ + pose: { + position: [0.5, 0, 0.1] + } + }); + }); + + it("extracts run_path statements from procedure body tokens", () => { + const procedure = declarations().find((decl): decl is GrlProcedureDeclaration => decl.kind === "ProcedureDeclaration")!; + + expect(parseProcedureRunPathStatements(procedure)).toEqual([ + { + kind: "RUN_PATH", + pathId: "pick_path", + sourceMap: { + line: 44, + column: 5 + } + } + ]); + }); + + it("reports empty paths and duplicate point names", () => { + const emptyPath = parseGrl(`language grl 0.1 +module Main + path empty_path { + } +end +`).module.declarations.find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!; + + expect(() => + compilePathToPlanRequest(emptyPath, motionContext([]), { startJoints: [], sampleTime: 0.004 }) + ).toThrowError(expect.objectContaining({ code: "GRL_PATH_EMPTY" })); + + const duplicatePath = parseGrl(`language grl 0.1 +module Main + const speed v = joint(50 %) + const zone zf = fine + target home = joint_target { joints: [0 deg] } + path dup_path { + defaults { speed: v, zone: zf } + point p movej home + point p movej home + } +end +`).module.declarations.find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!; + const duplicateContext = motionContext(parseGrl(`language grl 0.1 +module Main + const speed v = joint(50 %) + const zone zf = fine + target home = joint_target { joints: [0 deg] } +end +`).module.declarations); + + expect(() => + compilePathToPlanRequest(duplicatePath, duplicateContext, { startJoints: [0], sampleTime: 0.004 }) + ).toThrowError(expect.objectContaining({ code: "GRL_PATH_POINT_DUPLICATE" })); + }); + + it("reports events that reference missing points", () => { + const path = parseGrl(`language grl 0.1 +module Main + const speed v = joint(50 %) + const zone zf = fine + target home = joint_target { joints: [0 deg] } + path bad_event { + defaults { speed: v, zone: zf } + point p movej home + event after missing io.do[1] = true + } +end +`).module.declarations.find((decl): decl is GrlPathDeclaration => decl.kind === "PathDeclaration")!; + const context = motionContext(parseGrl(`language grl 0.1 +module Main + const speed v = joint(50 %) + const zone zf = fine + target home = joint_target { joints: [0 deg] } +end +`).module.declarations); + + expect(() => + compilePathToPlanRequest(path, context, { startJoints: [0], sampleTime: 0.004 }) + ).toThrowError(expect.objectContaining({ code: "GRL_PATH_EVENT_POINT_NOT_FOUND" })); + }); +}); diff --git a/kdl-wasm/web/tests/grl/procFunction.test.ts b/kdl-wasm/web/tests/grl/procFunction.test.ts new file mode 100644 index 0000000..32476aa --- /dev/null +++ b/kdl-wasm/web/tests/grl/procFunction.test.ts @@ -0,0 +1,201 @@ +import { describe, expect, it } from "vitest"; +import type { GrlFunctionDeclaration, GrlProcedureDeclaration } from "../../src/grl/ast/index.js"; +import { parseGrl } from "../../src/grl/parser/index.js"; +import { analyzeProcFunctionSemantics } from "../../src/grl/semantic/index.js"; + +function declarations(source: string) { + return parseGrl(source).module.declarations; +} + +describe("GRL proc, func, call, return, and scope semantics", () => { + it("parses function declarations and analyzes proc/func signatures, calls, returns, and warnings", () => { + const decls = declarations(`language grl 0.1 +module Main + var int global_count = 0 + proc read_sensor(out bool ok) + ok = true + end + proc main(in bool start, out bool done, inout int count) + call read_sensor(done) + call helper(count) + call self_check() + return + end + proc self_check() + call self_check() + end + func int helper(inout int value) + var int global_count = 1 + return value + end +end +`); + const func = decls.find((decl): decl is GrlFunctionDeclaration => decl.kind === "FunctionDeclaration")!; + + expect(func).toMatchObject({ + kind: "FunctionDeclaration", + returnType: "int", + name: "helper", + bodyTokens: [ + { raw: "var" }, + { raw: "int" }, + { raw: "global_count" }, + { raw: "=" }, + { raw: "1" }, + { raw: "return" }, + { raw: "value" } + ] + }); + + const analysis = analyzeProcFunctionSemantics(decls); + + expect(analysis.procedures).toEqual([ + expect.objectContaining({ + name: "read_sensor", + parameters: [expect.objectContaining({ name: "ok", typeName: "bool", direction: "out" })] + }), + expect.objectContaining({ + name: "main", + parameters: [ + expect.objectContaining({ name: "start", typeName: "bool", direction: "in" }), + expect.objectContaining({ name: "done", typeName: "bool", direction: "out" }), + expect.objectContaining({ name: "count", typeName: "int", direction: "inout" }) + ] + }), + expect.objectContaining({ name: "self_check", parameters: [] }) + ]); + expect(analysis.functions).toEqual([ + expect.objectContaining({ + name: "helper", + returnType: "int", + parameters: [expect.objectContaining({ name: "value", typeName: "int", direction: "inout" })] + }) + ]); + expect(analysis.calls).toEqual([ + expect.objectContaining({ kind: "CALL", target: "read_sensor", args: [expect.objectContaining({ text: "done" })] }), + expect.objectContaining({ kind: "CALL", target: "helper", args: [expect.objectContaining({ text: "count" })] }), + expect.objectContaining({ kind: "CALL", target: "self_check", args: [] }), + expect.objectContaining({ kind: "CALL", target: "self_check", args: [] }) + ]); + expect(analysis.returns).toEqual([ + expect.objectContaining({ kind: "RETURN" }), + expect.objectContaining({ kind: "RETURN", value: expect.objectContaining({ text: "value" }) }) + ]); + expect(analysis.diagnostics).toEqual([ + expect.objectContaining({ severity: "warning", code: "GRL_RECURSIVE_CALL" }), + expect.objectContaining({ severity: "warning", code: "GRL_NAME_SHADOWS_OUTER_SCOPE" }) + ]); + }); + + it("reports out parameters that are not assigned on all normal return paths", () => { + const decls = declarations(`language grl 0.1 +module Main + proc main(out bool done) + if ready == true + done = true + end + return + end +end +`); + + expect(() => analyzeProcFunctionSemantics(decls)).toThrowError( + expect.objectContaining({ code: "GRL_OUT_PARAM_NOT_ASSIGNED" }) + ); + }); + + it("reports out and inout call arguments that are not lvalues", () => { + const decls = declarations(`language grl 0.1 +module Main + proc set_done(out bool done) + done = true + end + proc main() + call set_done(true) + end +end +`); + + expect(() => analyzeProcFunctionSemantics(decls)).toThrowError( + expect.objectContaining({ code: "GRL_ARGUMENT_NOT_LVALUE" }) + ); + }); + + it("reports missing or incompatible function returns", () => { + const missingReturn = declarations(`language grl 0.1 +module Main + func int bad(in bool ready) + if ready == true + return 1 + end + end +end +`); + const wrongReturn = declarations(`language grl 0.1 +module Main + func bool bad() + return 1 + end +end +`); + + expect(() => analyzeProcFunctionSemantics(missingReturn)).toThrowError( + expect.objectContaining({ code: "GRL_FUNC_MISSING_RETURN" }) + ); + expect(() => analyzeProcFunctionSemantics(wrongReturn)).toThrowError( + expect.objectContaining({ code: "GRL_RETURN_TYPE_MISMATCH" }) + ); + }); + + it("reports illegal function side effects and procedure return values", () => { + const functionSideEffect = declarations(`language grl 0.1 +module Main + func bool bad() + wait io.di[1] == true + return true + end +end +`); + const procedureReturnValue = declarations(`language grl 0.1 +module Main + proc main() + return true + end +end +`); + + expect(() => analyzeProcFunctionSemantics(functionSideEffect)).toThrowError( + expect.objectContaining({ code: "GRL_FUNC_SIDE_EFFECT" }) + ); + expect(() => analyzeProcFunctionSemantics(procedureReturnValue)).toThrowError( + expect.objectContaining({ code: "GRL_RETURN_VALUE_IN_PROC" }) + ); + }); + + it("reports call target and argument type errors", () => { + const missingCall = declarations(`language grl 0.1 +module Main + proc main() + call missing() + end +end +`); + const typeMismatch = declarations(`language grl 0.1 +module Main + proc expects_int(in int value) + return + end + proc main() + call expects_int("bad") + end +end +`); + + expect(() => analyzeProcFunctionSemantics(missingCall)).toThrowError( + expect.objectContaining({ code: "GRL_CALL_TARGET_NOT_FOUND" }) + ); + expect(() => analyzeProcFunctionSemantics(typeMismatch)).toThrowError( + expect.objectContaining({ code: "GRL_CALL_ARGUMENT_TYPE" }) + ); + }); +}); diff --git a/kdl-wasm/web/tests/grl/semanticIr.test.ts b/kdl-wasm/web/tests/grl/semanticIr.test.ts new file mode 100644 index 0000000..41f0686 --- /dev/null +++ b/kdl-wasm/web/tests/grl/semanticIr.test.ts @@ -0,0 +1,148 @@ +import { describe, expect, it } from "vitest"; +import { parseGrl } from "../../src/grl/parser/index.js"; +import { compileSemanticProgram } from "../../src/grl/semantic/index.js"; + +const PROGRAM = `language grl 0.1 +module Main + const speed vj = joint(50 %) + const speed vl = linear(200 mm/s) + const zone zf = fine + target home = joint_target { joints: [0 deg] } + target pick = pose_target { pose: pose(500 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg) } + path pick_path { + defaults { speed: vl, zone: zf } + point p0 movej home speed vj zone fine + point p1 movel pick + event before p1 io.do[1] = true + } + operation pick_op { + kind: handling + path: pick_path + start_action: + io.do[2] = true + end_action: + io.do[2] = false + } + proc set_done(out bool done) + done = true + end + proc main(out bool done) + set_speed vl + set_zone zf + movej home speed vj zone fine + io.do[3] = true + wait io.di[1] == true timeout 1 s + if done == false + pulse io.do[4] duration 100 ms + else + alarm DONE "Already done" + end + call set_done(done) + run_path pick_path + run_operation pick_op + return + end +end +`; + +describe("GRL semantic analyzer, executable IR, and source map", () => { + it("compiles a complete program into unified executable IR and KDL bridge requests", () => { + const ir = compileSemanticProgram(parseGrl(PROGRAM), { + startJoints: [0], + sampleTime: 0.004 + }); + + expect(ir.moduleName).toBe("Main"); + expect(ir.semanticChecks).toHaveLength(22); + expect(ir.symbols).toEqual([ + expect.objectContaining({ kind: "data", name: "vj", typeName: "speed" }), + expect.objectContaining({ kind: "data", name: "vl", typeName: "speed" }), + expect.objectContaining({ kind: "data", name: "zf", typeName: "zone" }), + expect.objectContaining({ kind: "target", name: "home" }), + expect.objectContaining({ kind: "target", name: "pick" }), + expect.objectContaining({ kind: "path", name: "pick_path" }), + expect.objectContaining({ kind: "operation", name: "pick_op" }), + expect.objectContaining({ kind: "procedure", name: "set_done" }), + expect.objectContaining({ kind: "procedure", name: "main" }) + ]); + expect(ir.paths).toHaveLength(1); + expect(ir.operations).toHaveLength(1); + expect(ir.kdlBridge.pathRequests).toEqual([ + expect.objectContaining({ + pathId: "pick_path", + segments: [ + expect.objectContaining({ id: "p0", motion: "MOVEJ" }), + expect.objectContaining({ id: "p1", motion: "MOVEL" }) + ] + }) + ]); + expect(ir.kdlBridge.motionRequests).toEqual([ + expect.objectContaining({ + startJoints: [0], + speed: { kind: "joint_percent", value: 0.5 }, + zone: { kind: "fine" }, + sampleTime: 0.004 + }) + ]); + + const main = ir.procedures.find((procedure) => procedure.name === "main")!; + expect(main.instructions).toEqual([ + expect.objectContaining({ kind: "MOVEJ" }), + expect.objectContaining({ kind: "IO_WRITE", target: expect.objectContaining({ domain: "do", index: 3 }) }), + expect.objectContaining({ kind: "WAIT", timeout: 1 }), + expect.objectContaining({ + kind: "EXEC_IF", + branches: [ + expect.objectContaining({ + branchKind: "if", + body: expect.arrayContaining([expect.objectContaining({ kind: "PULSE", duration: 0.1 })]) + }), + expect.objectContaining({ + branchKind: "else", + body: expect.arrayContaining([expect.objectContaining({ kind: "ALARM", alarmId: "DONE" })]) + }) + ] + }), + expect.objectContaining({ kind: "CALL", target: "set_done" }), + expect.objectContaining({ kind: "RUN_PATH", pathId: "pick_path" }), + expect.objectContaining({ kind: "RUN_OPERATION", operationId: "pick_op" }), + expect.objectContaining({ kind: "RETURN" }) + ]); + }); + + it("exposes source map entries for GRL procedure lines, path points, and operation actions", () => { + const ir = compileSemanticProgram(parseGrl(PROGRAM), { + startJoints: [0], + sampleTime: 0.004 + }); + + expect(ir.sourceMap).toEqual( + expect.arrayContaining([ + expect.objectContaining({ kind: "path_point", pathId: "pick_path", pointId: "p0" }), + expect.objectContaining({ kind: "path_point", pathId: "pick_path", pointId: "p1" }), + expect.objectContaining({ kind: "operation_action", operationId: "pick_op" }), + expect.objectContaining({ kind: "MOVEJ", procedureId: "main", sourceMap: expect.objectContaining({ line: 28 }) }), + expect.objectContaining({ kind: "EXEC_IF", procedureId: "main" }), + expect.objectContaining({ kind: "RUN_OPERATION", procedureId: "main" }) + ]) + ); + }); + + it("reports duplicate symbols through semantic diagnostics", () => { + const ir = compileSemanticProgram(parseGrl(`language grl 0.1 +module Main + const speed v = joint(10 %) + const speed v = joint(20 %) + proc main() + end +end +`), { + startJoints: [], + sampleTime: 0.004 + }); + + expect(ir.diagnostics).toEqual([ + expect.objectContaining({ severity: "error", code: "GRL_SYMBOL_DUPLICATE" }) + ]); + }); +}); diff --git a/kdl-wasm/web/tests/kdl/cAbi.test.ts b/kdl-wasm/web/tests/kdl/cAbi.test.ts new file mode 100644 index 0000000..e064e44 --- /dev/null +++ b/kdl-wasm/web/tests/kdl/cAbi.test.ts @@ -0,0 +1,224 @@ +import { access } from "node:fs/promises"; +import { fileURLToPath } from "node:url"; +import { describe, expect, it } from "vitest"; +import { KDL_C_ABI_EXPORTS, KdlNativeAbi } from "../../src/kdl/nativeAbi.js"; +import type { NativeKdlModule } from "../../src/kdl/nativeModule.js"; +import { loadRobotFromUrdfModel } from "../../src/robot/urdfParser.js"; + +const BUILD_DIR = new URL("../../../build-wasm/", import.meta.url); +const WRAPPER_URL = new URL("kdl.js", BUILD_DIR); + +type NativeFactory = (options?: { + locateFile?: (path: string, prefix: string) => string; +}) => Promise; + +async function loadNativeModule(): Promise { + await access(fileURLToPath(WRAPPER_URL)); + const imported = (await import(/* @vite-ignore */ WRAPPER_URL.href)) as { + default?: NativeFactory; + createKdlModule?: NativeFactory; + }; + const factory = imported.default ?? imported.createKdlModule; + if (!factory) { + throw new Error("kdl.js did not export createKdlModule"); + } + return factory({ + locateFile: (path) => fileURLToPath(new URL(path, BUILD_DIR)) + }); +} + +const NATIVE_SOLVER_URDF = ` + + + + + + + + + + + + + + + + + +`; + +function writeFloat64Array(native: NativeKdlModule, values: number[]): number { + const bytes = values.length * Float64Array.BYTES_PER_ELEMENT; + const ptr = native._malloc?.(bytes); + if (!ptr) { + throw new Error(`Failed to allocate ${bytes} bytes`); + } + native.HEAPF64?.set(values, ptr / Float64Array.BYTES_PER_ELEMENT); + return ptr; +} + +function readFloat64Array(native: NativeKdlModule, ptr: number, length: number): number[] { + return Array.from(native.HEAPF64?.subarray( + ptr / Float64Array.BYTES_PER_ELEMENT, + ptr / Float64Array.BYTES_PER_ELEMENT + length + ) ?? []); +} + +describe("KDL C ABI", () => { + it("exports the stable P0 ABI names", async () => { + const abi = new KdlNativeAbi(await loadNativeModule()); + + expect(() => abi.assertExports()).not.toThrow(); + expect(KDL_C_ABI_EXPORTS).toEqual([ + "kdl_init", + "kdl_create_robot", + "kdl_destroy_robot", + "kdl_get_robot_info", + "kdl_fk", + "kdl_fk_all_links", + "kdl_jacobian", + "kdl_ik", + "kdl_plan_movej", + "kdl_plan_movel", + "kdl_plan_movec", + "kdl_plan_path", + "kdl_sample_trap", + "kdl_last_error" + ]); + }); + + it("initializes, caches model handles, returns JSON info, and destroys handles", async () => { + const abi = new KdlNativeAbi(await loadNativeModule()); + const model = loadRobotFromUrdfModel(NATIVE_SOLVER_URDF, { + robotId: "abi", + baseLink: "base_link", + tipLink: "tool0" + }); + + expect(abi.callNumber("kdl_init", ["string"], ["{}"])).toBe(0); + const handle = abi.callNumber("kdl_create_robot", ["string"], [JSON.stringify(model)]); + expect(handle).toBeGreaterThan(0); + + const info = abi.readJsonCall<{ handle: number; nativeState: string; dof: number }>( + "kdl_get_robot_info", + ["number"], + [handle] + ); + expect(info).toMatchObject({ + handle, + dof: 2, + nativeState: "kdl_chain" + }); + + expect(abi.callNumber("kdl_destroy_robot", ["number"], [handle])).toBe(0); + }); + + it("constructs a native KDL chain and returns real FK and Jacobian data", async () => { + const native = await loadNativeModule(); + const abi = new KdlNativeAbi(native); + const model = loadRobotFromUrdfModel(NATIVE_SOLVER_URDF, { + robotId: "native", + baseLink: "base_link", + tipLink: "tool0" + }); + + expect(abi.callNumber("kdl_init", ["string"], ["{}"])).toBe(0); + const handle = abi.callNumber("kdl_create_robot", ["string"], [JSON.stringify(model)]); + expect(handle).toBeGreaterThan(0); + expect(abi.readJsonCall("kdl_get_robot_info", ["number"], [handle])).toMatchObject({ + handle, + dof: 2, + jointNames: ["joint_1", "joint_2"], + nativeState: "kdl_chain" + }); + + 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); + expect(pose).toBeTruthy(); + expect(jacobian).toBeTruthy(); + + try { + expect(abi.callNumber("kdl_fk", ["number", "number", "number", "number"], [handle, joints, 2, pose])).toBe(0); + const pose7 = readFloat64Array(native, pose!, 7); + expect(pose7[0]).toBeCloseTo(0); + expect(pose7[1]).toBeCloseTo(0.4); + expect(pose7[2]).toBeCloseTo(0); + expect(pose7[5]).toBeCloseTo(Math.SQRT1_2); + expect(pose7[6]).toBeCloseTo(Math.SQRT1_2); + + 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); + expect(jac[1]).toBeCloseTo(0, 4); + expect(jac[2]).toBeCloseTo(0, 4); + expect(jac[3]).toBeCloseTo(1, 4); + expect(jac[10]).toBeCloseTo(1, 4); + } finally { + native._free?.(joints); + if (pose) { + native._free?.(pose); + } + if (jacobian) { + native._free?.(jacobian); + } + abi.callNumber("kdl_destroy_robot", ["number"], [handle]); + } + }); + + it("normalizes C ABI failures through kdl_last_error", 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" + } + ] + }); + expect(() => abi.checkReturnCode(returnCode)).toThrowError( + expect.objectContaining({ + code: "KDL_NOT_IMPLEMENTED" + }) + ); + }); + + it("reports JSON output buffer errors without raw strings", async () => { + const native = await loadNativeModule(); + const abi = new KdlNativeAbi(native); + const model = loadRobotFromUrdfModel(NATIVE_SOLVER_URDF, { + robotId: "abi", + baseLink: "base_link", + tipLink: "tool0" + }); + + expect(abi.callNumber("kdl_init", ["string"], ["{}"])).toBe(0); + const handle = abi.callNumber("kdl_create_robot", ["string"], [JSON.stringify(model)]); + const ptr = native._malloc?.(4); + expect(ptr).toBeTruthy(); + + try { + const returnCode = abi.callNumber("kdl_get_robot_info", ["number", "number", "number"], [handle, ptr, 4]); + expect(returnCode).toBe(-1); + expect(abi.lastError()).toMatchObject({ + code: "KDL_BUFFER_TOO_SMALL", + diagnostics: [ + { + severity: "error", + code: "KDL_BUFFER_TOO_SMALL" + } + ] + }); + } finally { + if (ptr) { + native._free?.(ptr); + } + } + }); +}); diff --git a/kdl-wasm/web/tests/kdl/checks.test.ts b/kdl-wasm/web/tests/kdl/checks.test.ts new file mode 100644 index 0000000..5c92b52 --- /dev/null +++ b/kdl-wasm/web/tests/kdl/checks.test.ts @@ -0,0 +1,178 @@ +import { describe, expect, it } from "vitest"; +import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js"; +import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js"; +import type { JacobianResult, PoseTarget, ReachabilityResult } from "../../src/kdl/types.js"; + +const PLANAR_URDF = ` + + + + + + + + + + + + + + + + + +`; + +async function createRobot() { + const runtime = createKdlWorkerRuntime(); + await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] }); + const response = await dispatchKdlRpcRequest(runtime, { + id: 2, + method: "loadRobotFromUrdf", + payload: [ + PLANAR_URDF, + { + robotId: "checks", + baseLink: "base_link", + tipLink: "tool0" + } + ] + }); + expect(response.ok).toBe(true); + return { runtime, handle: response.result as number }; +} + +function poseTarget(id: string, x: number, y: number): PoseTarget { + return { + id, + pose: { + position: [x, y, 0], + quaternion: [0, 0, 0, 1] + } + }; +} + +describe("Jacobian, singularity, limits, and reachability checks", () => { + it("computes a 6xdof Jacobian with expected linear components", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 3, + method: "jacobian", + payload: [handle, [Math.PI / 2, 0.4]] + }); + + expect(response.ok).toBe(true); + const jacobian = response.result as JacobianResult; + expect(jacobian.rows).toBe(6); + expect(jacobian.cols).toBe(2); + expect(jacobian.data).toHaveLength(12); + expect(jacobian.data[0]).toBeCloseTo(-0.4, 4); + expect(jacobian.data[1]).toBeCloseTo(0, 4); + expect(jacobian.data[2]).toBeCloseTo(0, 4); + expect(jacobian.data[3]).toBeCloseTo(1, 4); + }); + + it("reports singularity warning for collapsed planar reach", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 4, + method: "checkSingularity", + payload: [handle, [0, 0]] + }); + + expect(response.result).toMatchObject({ + ok: true, + nearSingularity: true, + diagnostics: [ + { + severity: "warning", + code: "KDL_SINGULARITY" + } + ] + }); + }); + + it("checks joint and velocity limits with structured diagnostics", async () => { + const { runtime, handle } = await createRobot(); + const jointResponse = await dispatchKdlRpcRequest(runtime, { + id: 5, + method: "checkJointLimits", + payload: [handle, [0, 2]] + }); + expect(jointResponse.result).toMatchObject({ + ok: false, + diagnostics: [ + { + severity: "error", + code: "KDL_JOINT_LIMIT" + } + ] + }); + + const velocityResponse = await dispatchKdlRpcRequest(runtime, { + id: 6, + method: "checkVelocityLimits", + payload: [ + handle, + { + points: [ + { + jointVelocity: [1, 0.75], + jointAcceleration: [1, 1.5] + } + ] + } + ] + }); + expect(velocityResponse.result).toMatchObject({ + ok: false, + maxJointVelocityRatio: 1.5, + maxJointAccelerationRatio: 1.5, + diagnostics: [ + { + severity: "error", + code: "KDL_VELOCITY_LIMIT", + pointIndex: 0 + }, + { + severity: "error", + code: "KDL_ACCEL_LIMIT", + pointIndex: 0 + } + ] + }); + }); + + it("checks reachability and preserves batch order", async () => { + const { runtime, handle } = await createRobot(); + const reachable = await dispatchKdlRpcRequest(runtime, { + id: 7, + method: "checkReachability", + payload: [handle, poseTarget("ok", 0, 0.3), { positionTolerance: 1e-9 }] + }); + expect(reachable.result).toMatchObject({ + ok: true, + reachable: true, + targetId: "ok", + joints: [Math.PI / 2, 0.3] + }); + + const batch = await dispatchKdlRpcRequest(runtime, { + id: 8, + method: "checkReachabilityBatch", + payload: [handle, [poseTarget("a", 0.2, 0), poseTarget("b", 2, 0)], {}] + }); + const results = batch.result as ReachabilityResult[]; + expect(results.map((result) => result.targetId)).toEqual(["a", "b"]); + expect(results[0]?.reachable).toBe(true); + expect(results[1]).toMatchObject({ + reachable: false, + diagnostics: [ + { + severity: "error", + code: "KDL_JOINT_LIMIT" + } + ] + }); + }); +}); diff --git a/kdl-wasm/web/tests/kdl/fk.test.ts b/kdl-wasm/web/tests/kdl/fk.test.ts new file mode 100644 index 0000000..79d1d37 --- /dev/null +++ b/kdl-wasm/web/tests/kdl/fk.test.ts @@ -0,0 +1,198 @@ +import { describe, expect, it } from "vitest"; +import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js"; +import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js"; + +const SIMPLE_URDF = ` + + + + + + + + + + + + + + + + + + + + + + + + + +`; + +async function createRuntimeRobot() { + const runtime = createKdlWorkerRuntime(); + await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] }); + const response = await dispatchKdlRpcRequest(runtime, { + id: 2, + method: "loadRobotFromUrdf", + payload: [ + SIMPLE_URDF, + { + robotId: "fk", + baseLink: "base_link", + tipLink: "tool0" + } + ] + }); + expect(response.ok).toBe(true); + return { runtime, handle: response.result as number }; +} + +describe("FK and fkAllLinks", () => { + it("computes flange and tcp poses for the zero joint state", async () => { + const { runtime, handle } = await createRuntimeRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 3, + method: "fk", + payload: [handle, [0, 0]] + }); + + expect(response.ok).toBe(true); + const result = response.result as { + ok: boolean; + joints: number[]; + diagnostics: unknown[]; + flange: { position: number[]; quaternion: number[] }; + tcp: { position: number[]; quaternion: number[] }; + }; + expect(result.ok).toBe(true); + expect(result.joints).toEqual([0, 0]); + expect(result.diagnostics).toEqual([]); + expect(result.flange.position[0]).toBeCloseTo(0); + expect(result.flange.position[1]).toBeCloseTo(0); + expect(result.flange.position[2]).toBeCloseTo(0.35); + expect(result.flange.quaternion).toEqual([0, 0, 0, 1]); + expect(result.tcp.position[0]).toBeCloseTo(0); + expect(result.tcp.position[1]).toBeCloseTo(0); + expect(result.tcp.position[2]).toBeCloseTo(0.35); + expect(result.tcp.quaternion).toEqual([0, 0, 0, 1]); + }); + + it("applies revolute and prismatic joint motion in chain order", async () => { + const { runtime, handle } = await createRuntimeRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 3, + method: "fk", + payload: [handle, [Math.PI / 2, 0.2]] + }); + + expect(response.ok).toBe(true); + const result = response.result as { flange: { position: number[]; quaternion: number[] } }; + expect(result.flange.position[0]).toBeCloseTo(0); + expect(result.flange.position[1]).toBeCloseTo(0.2); + expect(result.flange.position[2]).toBeCloseTo(0.35); + expect(result.flange.quaternion[2]).toBeCloseTo(Math.SQRT1_2); + expect(result.flange.quaternion[3]).toBeCloseTo(Math.SQRT1_2); + }); + + it("returns link poses in base-to-tip order", async () => { + const { runtime, handle } = await createRuntimeRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 4, + method: "fkAllLinks", + payload: [handle, [0, 0.1]] + }); + + expect(response.ok).toBe(true); + const result = response.result as { + linkPoses: Array<{ link: string; pose: { position: number[] } }>; + }; + expect(result.linkPoses.map((entry) => entry.link)).toEqual(["base_link", "link_1", "link_2", "tool0"]); + expect(result.linkPoses[0]?.pose.position).toEqual([0, 0, 0]); + expect(result.linkPoses[3]?.pose.position[0]).toBeCloseTo(0.1); + expect(result.linkPoses[3]?.pose.position[2]).toBeCloseTo(0.35); + }); + + it("applies tool offset to tcp without changing flange", async () => { + const { runtime, handle } = await createRuntimeRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 5, + method: "fk", + payload: [ + handle, + [0, 0], + { + tool: { + position: [0, 0, 0.1], + quaternion: [0, 0, 0, 1] + } + } + ] + }); + + expect(response.ok).toBe(true); + const result = response.result as { + flange: { position: number[] }; + tcp: { position: number[] }; + }; + expect(result.flange.position[2]).toBeCloseTo(0.35); + expect(result.tcp.position[2]).toBeCloseTo(0.45); + }); + + it("writes tcp pose into a reusable Float64Array", async () => { + const { runtime, handle } = await createRuntimeRobot(); + const out = new Float64Array(7); + const response = await dispatchKdlRpcRequest(runtime, { + id: 51, + method: "fkPose7", + payload: [handle, new Float64Array([Math.PI / 2, 0.2]), out] + }); + + expect(response.ok).toBe(true); + expect(response.result).toBe(out); + expect(out[0]).toBeCloseTo(0); + expect(out[1]).toBeCloseTo(0.2); + expect(out[2]).toBeCloseTo(0.35); + expect(out[5]).toBeCloseTo(Math.SQRT1_2); + expect(out[6]).toBeCloseTo(Math.SQRT1_2); + }); + + it("returns a structured error for undersized fkPose7 output buffers", async () => { + const { runtime, handle } = await createRuntimeRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 52, + method: "fkPose7", + payload: [handle, [0, 0], new Float64Array(6)] + }); + + expect(response).toMatchObject({ + ok: false, + error: { + code: "KDL_OUTPUT_DIMENSION_MISMATCH" + } + }); + }); + + it("returns a structured dimension diagnostic", async () => { + const { runtime, handle } = await createRuntimeRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 6, + method: "fk", + payload: [handle, [0]] + }); + + expect(response).toMatchObject({ + ok: false, + error: { + code: "KDL_JOINT_DIMENSION_MISMATCH", + diagnostics: [ + { + severity: "error", + code: "KDL_JOINT_DIMENSION_MISMATCH" + } + ] + } + }); + }); +}); diff --git a/kdl-wasm/web/tests/kdl/ik.test.ts b/kdl-wasm/web/tests/kdl/ik.test.ts new file mode 100644 index 0000000..4f53202 --- /dev/null +++ b/kdl-wasm/web/tests/kdl/ik.test.ts @@ -0,0 +1,166 @@ +import { describe, expect, it } from "vitest"; +import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js"; +import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js"; +import type { IkResult, Pose } from "../../src/kdl/types.js"; + +const PLANAR_URDF = ` + + + + + + + + + + + + + + + + + + + +`; + +const UNSUPPORTED_URDF = ` + + + + + + + + + + + + + + + + + +`; + +function pose(x: number, y: number, z = 0): Pose { + return { + position: [x, y, z], + quaternion: [0, 0, 0, 1] + }; +} + +async function createRobot(urdf = PLANAR_URDF) { + const runtime = createKdlWorkerRuntime(); + await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] }); + const response = await dispatchKdlRpcRequest(runtime, { + id: 2, + method: "loadRobotFromUrdf", + payload: [ + urdf, + { + robotId: "ik", + baseLink: "base_link", + tipLink: "tool0" + } + ] + }); + expect(response.ok).toBe(true); + return { runtime, handle: response.result as number }; +} + +describe("IK and ikBatch", () => { + it("solves a reachable planar target and FK back-substitution is within tolerance", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 3, + method: "ik", + payload: [handle, [0, 0], pose(0, 0.4), { positionTolerance: 1e-9 }] + }); + + expect(response.ok).toBe(true); + const result = response.result as IkResult; + expect(result.ok).toBe(true); + expect(result.joints?.[0]).toBeCloseTo(Math.PI / 2); + expect(result.joints?.[1]).toBeCloseTo(0.4); + expect(result.residualPosition).toBeLessThan(1e-9); + + const fk = await dispatchKdlRpcRequest(runtime, { + id: 4, + method: "fk", + payload: [handle, result.joints] + }); + const fkResult = fk.result as { tcp: { position: number[] } }; + expect(fkResult.tcp.position[0]).toBeCloseTo(0); + expect(fkResult.tcp.position[1]).toBeCloseTo(0.4); + }); + + it("keeps ikBatch results in input order", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 5, + method: "ikBatch", + payload: [ + handle, + [ + [0, 0], + [0, 0] + ], + [pose(0.2, 0), pose(0, 0.3)], + {} + ] + }); + + expect(response.ok).toBe(true); + const results = response.result as IkResult[]; + expect(results).toHaveLength(2); + expect(results[0]?.joints?.[0]).toBeCloseTo(0); + expect(results[0]?.joints?.[1]).toBeCloseTo(0.2); + expect(results[1]?.joints?.[0]).toBeCloseTo(Math.PI / 2); + expect(results[1]?.joints?.[1]).toBeCloseTo(0.3); + }); + + it("returns joint_limit reason when the candidate exceeds limits", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 6, + method: "ik", + payload: [handle, [0, 0], pose(2, 0), {}] + }); + + expect(response.ok).toBe(true); + expect(response.result).toMatchObject({ + ok: false, + reason: "joint_limit", + diagnostics: [ + { + severity: "error", + code: "KDL_JOINT_LIMIT" + } + ] + }); + }); + + it("returns invalid_model reason for unsupported IK chains", async () => { + const { runtime, handle } = await createRobot(UNSUPPORTED_URDF); + const response = await dispatchKdlRpcRequest(runtime, { + id: 7, + method: "ik", + payload: [handle, [0, 0], pose(0.2, 0), {}] + }); + + expect(response.ok).toBe(true); + expect(response.result).toMatchObject({ + ok: false, + reason: "invalid_model", + diagnostics: [ + { + severity: "error", + code: "KDL_IK_UNSUPPORTED_MODEL" + } + ] + }); + }); +}); diff --git a/kdl-wasm/web/tests/kdl/path.test.ts b/kdl-wasm/web/tests/kdl/path.test.ts new file mode 100644 index 0000000..8977590 --- /dev/null +++ b/kdl-wasm/web/tests/kdl/path.test.ts @@ -0,0 +1,167 @@ +import { describe, expect, it } from "vitest"; +import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js"; +import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js"; +import type { PathPlanRequest, PathPlanResult, PathValidationResult } from "../../src/kdl/types.js"; + +const PLANAR_URDF = ` + + + + + + + + + + + + + + + + + +`; + +async function createRobot() { + const runtime = createKdlWorkerRuntime(); + await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] }); + const response = await dispatchKdlRpcRequest(runtime, { + id: 2, + method: "loadRobotFromUrdf", + payload: [ + PLANAR_URDF, + { + robotId: "path", + baseLink: "base_link", + tipLink: "tool0" + } + ] + }); + expect(response.ok).toBe(true); + return { runtime, handle: response.result as number }; +} + +function pathRequest(overrides: Partial = {}): PathPlanRequest { + return { + startJoints: [0, 0], + sampleTime: 0.05, + segments: [ + { + id: "move-home", + motion: "MOVEJ", + target: { + id: "joint_goal", + joints: [Math.PI / 2, 0.2] + }, + speed: { kind: "joint_abs", velocity: 1, acceleration: 4 }, + zone: { kind: "fine" }, + sourceMap: { line: 10, column: 5 } + }, + { + id: "line-out", + motion: "MOVEL", + target: { + id: "line_goal", + pose: { + position: [0, 0.4, 0], + quaternion: [0, 0, 0, 1] + } + }, + speed: { kind: "linear", velocity: 0.2, acceleration: 1 }, + zone: { kind: "fine" }, + sourceMap: { line: 11, column: 5 } + } + ], + ...overrides + }; +} + +describe("planPath and validatePath", () => { + it("plans multiple motion segments and merges points with segment metadata", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 3, + method: "planPath", + payload: [handle, pathRequest()] + }); + + expect(response.ok).toBe(true); + const result = response.result as PathPlanResult; + expect(result.ok).toBe(true); + expect(result.segments).toHaveLength(2); + expect(result.points.length).toBeGreaterThan(result.segments[0]!.points.length); + expect(result.points[0]).toMatchObject({ + index: 0, + time: 0, + segmentId: "move-home", + targetId: "joint_goal", + sourceMap: { line: 10 } + }); + expect(result.points.at(-1)).toMatchObject({ + segmentId: "line-out", + targetId: "line_goal", + sourceMap: { line: 11 } + }); + expect(result.points.at(-1)?.tcp.position[0]).toBeCloseTo(0, 5); + expect(result.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.4, 5); + expect(result.duration).toBeCloseTo(result.segments[0]!.duration + result.segments[1]!.duration); + for (let index = 1; index < result.points.length; index += 1) { + expect(result.points[index]!.time).toBeGreaterThan(result.points[index - 1]!.time); + expect(result.points[index]!.index).toBe(index); + } + }); + + it("validates a path and returns per-segment reports", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 4, + method: "validatePath", + payload: [handle, pathRequest()] + }); + + expect(response.ok).toBe(true); + const result = response.result as PathValidationResult; + expect(result.ok).toBe(true); + expect(result.reachable).toBe(true); + expect(result.cycleTime).toBeGreaterThan(0); + expect(result.segmentReports.map((report) => report.segmentId)).toEqual(["move-home", "line-out"]); + expect(result.segmentReports[0]).toMatchObject({ + ok: true, + motion: "MOVEJ" + }); + expect(result.segmentReports[1]).toMatchObject({ + ok: true, + motion: "MOVEL", + maxCartesianError: 0 + }); + }); + + it("returns KDL_PATH_EMPTY for empty path requests", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 5, + method: "planPath", + payload: [ + handle, + pathRequest({ + segments: [] + }) + ] + }); + + expect(response.ok).toBe(true); + expect(response.result).toMatchObject({ + ok: false, + duration: 0, + segments: [], + points: [], + diagnostics: [ + { + severity: "error", + code: "KDL_PATH_EMPTY" + } + ] + }); + }); +}); diff --git a/kdl-wasm/web/tests/kdl/performance.test.ts b/kdl-wasm/web/tests/kdl/performance.test.ts new file mode 100644 index 0000000..e497157 --- /dev/null +++ b/kdl-wasm/web/tests/kdl/performance.test.ts @@ -0,0 +1,23 @@ +import { describe, expect, it } from "vitest"; +import { runPerformanceBaseline } from "../../src/kdl/performanceBaseline.js"; + +describe("KDL performance baseline", () => { + it("records TypedArray and batch baseline metrics", async () => { + const result = await runPerformanceBaseline(); + const metrics = Object.fromEntries(result.metrics.map((metric) => [metric.name, metric])); + + expect(result.ok).toBe(true); + expect(result.diagnostics).toEqual([]); + expect(metrics.robot_init_6_axis?.totalMs).toBeLessThanOrEqual(1_000); + expect(metrics.fk_pose7_typed_array?.averageMs).toBeLessThanOrEqual(1); + expect(metrics.ik_planar_average?.averageMs).toBeLessThanOrEqual(10); + expect(metrics.reachability_batch_1000).toMatchObject({ + points: 1_000, + ok: true + }); + expect(metrics.trajectory_10s_4ms).toMatchObject({ + ok: true + }); + expect(metrics.trajectory_10s_4ms?.points).toBeGreaterThanOrEqual(2_500); + }); +}); diff --git a/kdl-wasm/web/tests/kdl/planMoveC.test.ts b/kdl-wasm/web/tests/kdl/planMoveC.test.ts new file mode 100644 index 0000000..4dfc4f5 --- /dev/null +++ b/kdl-wasm/web/tests/kdl/planMoveC.test.ts @@ -0,0 +1,193 @@ +import { describe, expect, it } from "vitest"; +import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js"; +import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js"; +import type { MoveCRequest, TrajectoryResult } from "../../src/kdl/types.js"; + +const PLANAR_URDF = ` + + + + + + + + + + + + + + + + + +`; + +async function createRobot() { + const runtime = createKdlWorkerRuntime(); + await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] }); + const response = await dispatchKdlRpcRequest(runtime, { + id: 2, + method: "loadRobotFromUrdf", + payload: [ + PLANAR_URDF, + { + robotId: "movec", + baseLink: "base_link", + tipLink: "tool0" + } + ] + }); + expect(response.ok).toBe(true); + return { runtime, handle: response.result as number }; +} + +function request(overrides: Partial): MoveCRequest { + return { + startJoints: [0, 0.5], + via: { + id: "via", + pose: { + position: [0.5, 0.5, 0], + quaternion: [0, 0, 0, 1] + } + }, + target: { + id: "arc_goal", + pose: { + position: [0, 0.5, 0], + quaternion: [0, 0, 0, 1] + } + }, + speed: { + kind: "linear", + velocity: 0.25, + acceleration: 1 + }, + zone: { + kind: "fine" + }, + sampleTime: 0.05, + ...overrides + }; +} + +describe("planMoveC", () => { + it("plans a circular TCP arc with circle metadata", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 3, + method: "planMoveC", + payload: [handle, request({})] + }); + + expect(response.ok).toBe(true); + const trajectory = response.result as TrajectoryResult; + expect(trajectory.ok).toBe(true); + expect(trajectory.motion).toBe("MOVEC"); + expect(trajectory.points.length).toBeGreaterThan(2); + expect(trajectory.points[0]).toMatchObject({ + index: 0, + time: 0, + s: 0, + motion: "MOVEC", + targetId: "arc_goal" + }); + expect(trajectory.points.at(-1)?.s).toBe(1); + expect(trajectory.points.at(-1)?.tcp.position[0]).toBeCloseTo(0, 5); + expect(trajectory.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.5, 5); + + const circle = trajectory.meta?.circle as { + center: number[]; + radius: number; + angle: number; + length: number; + direction: "cw" | "ccw"; + maxArcError: number; + }; + expect(circle.center[0]).toBeCloseTo(0.25); + expect(circle.center[1]).toBeCloseTo(0.25); + expect(circle.radius).toBeCloseTo(Math.SQRT1_2 / 2); + expect(circle.angle).toBeCloseTo(Math.PI); + expect(circle.length).toBeCloseTo((Math.SQRT1_2 / 2) * Math.PI); + expect(circle.direction).toBe("ccw"); + expect(circle.maxArcError).toBeLessThan(1e-6); + }); + + it("returns KDL_ARC_DEGENERATE for collinear points", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 4, + method: "planMoveC", + payload: [ + handle, + request({ + via: { + id: "line_mid", + pose: { + position: [0.25, 0, 0], + quaternion: [0, 0, 0, 1] + } + }, + target: { + id: "line_end", + pose: { + position: [0.75, 0, 0], + quaternion: [0, 0, 0, 1] + } + } + }) + ] + }); + + expect(response.ok).toBe(true); + expect(response.result).toMatchObject({ + ok: false, + motion: "MOVEC", + points: [], + diagnostics: [ + { + severity: "error", + code: "KDL_ARC_DEGENERATE" + } + ] + }); + }); + + it("reports zone approximation and joint-speed approximation warnings", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 5, + method: "planMoveC", + payload: [ + handle, + request({ + speed: { + kind: "joint_abs", + velocity: 0.25, + acceleration: 1 + }, + zone: { + kind: "distance", + value: 0.01 + } + }) + ] + }); + + const trajectory = response.result as TrajectoryResult; + expect(trajectory.ok).toBe(true); + expect(trajectory.diagnostics).toContainEqual( + expect.objectContaining({ + severity: "warning", + code: "KDL_MOVEC_JOINT_SPEED_APPROX" + }) + ); + expect(trajectory.diagnostics).toContainEqual( + expect.objectContaining({ + severity: "warning", + code: "KDL_ZONE_APPROX_FINE" + }) + ); + }); +}); diff --git a/kdl-wasm/web/tests/kdl/planMoveJ.test.ts b/kdl-wasm/web/tests/kdl/planMoveJ.test.ts new file mode 100644 index 0000000..a5a3a0f --- /dev/null +++ b/kdl-wasm/web/tests/kdl/planMoveJ.test.ts @@ -0,0 +1,192 @@ +import { describe, expect, it } from "vitest"; +import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js"; +import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js"; +import type { MoveJRequest, PoseTarget, TrajectoryResult } from "../../src/kdl/types.js"; + +const PLANAR_URDF = ` + + + + + + + + + + + + + + + + + +`; + +async function createRobot() { + const runtime = createKdlWorkerRuntime(); + await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] }); + const response = await dispatchKdlRpcRequest(runtime, { + id: 2, + method: "loadRobotFromUrdf", + payload: [ + PLANAR_URDF, + { + robotId: "movej", + baseLink: "base_link", + tipLink: "tool0" + } + ] + }); + expect(response.ok).toBe(true); + return { runtime, handle: response.result as number }; +} + +function baseRequest(overrides: Partial): MoveJRequest { + return { + startJoints: [0, 0], + target: { + id: "joint_goal", + joints: [0.5, 0.25] + }, + speed: { + kind: "joint_abs", + velocity: 0.5, + acceleration: 1 + }, + zone: { + kind: "fine" + }, + sampleTime: 0.1, + ...overrides + }; +} + +describe("planMoveJ", () => { + it("plans a synchronized joint trajectory to a joint target", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 3, + method: "planMoveJ", + payload: [handle, baseRequest({})] + }); + + expect(response.ok).toBe(true); + const trajectory = response.result as TrajectoryResult; + expect(trajectory.ok).toBe(true); + expect(trajectory.motion).toBe("MOVEJ"); + expect(trajectory.points.length).toBeGreaterThan(2); + expect(trajectory.points[0]).toMatchObject({ + index: 0, + time: 0, + s: 0, + joints: [0, 0], + motion: "MOVEJ", + targetId: "joint_goal" + }); + expect(trajectory.points.at(-1)?.s).toBe(1); + expect(trajectory.points.at(-1)?.joints[0]).toBeCloseTo(0.5); + expect(trajectory.points.at(-1)?.joints[1]).toBeCloseTo(0.25); + expect(trajectory.points.at(-1)?.jointVelocity[0]).toBeCloseTo(0); + expect(trajectory.points.at(-1)?.tcp.position[0]).toBeCloseTo(0.25 * Math.cos(0.5)); + expect(trajectory.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.25 * Math.sin(0.5)); + expect(trajectory.meta).toMatchObject({ + targetType: "joint", + qStart: [0, 0], + qEnd: [0.5, 0.25] + }); + }); + + it("uses IK for pose targets and warns when zone is approximated as fine", async () => { + const { runtime, handle } = await createRobot(); + const target: PoseTarget = { + id: "pose_goal", + pose: { + position: [0, 0.3, 0], + quaternion: [0, 0, 0, 1] + } + }; + const response = await dispatchKdlRpcRequest(runtime, { + id: 4, + method: "planMoveJ", + payload: [ + handle, + baseRequest({ + target, + zone: { kind: "distance", value: 0.01 } + }) + ] + }); + + const trajectory = response.result as TrajectoryResult; + expect(trajectory.ok).toBe(true); + expect(trajectory.points.at(-1)?.joints[0]).toBeCloseTo(Math.PI / 2); + expect(trajectory.points.at(-1)?.joints[1]).toBeCloseTo(0.3); + expect(trajectory.diagnostics).toContainEqual( + expect.objectContaining({ + severity: "warning", + code: "KDL_ZONE_APPROX_FINE" + }) + ); + expect(trajectory.meta).toMatchObject({ + targetType: "pose" + }); + }); + + it("returns a failed trajectory result for endpoint joint limit violations", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 5, + method: "planMoveJ", + payload: [ + handle, + baseRequest({ + target: { + id: "bad_goal", + joints: [0, 2] + } + }) + ] + }); + + expect(response.ok).toBe(true); + expect(response.result).toMatchObject({ + ok: false, + motion: "MOVEJ", + points: [], + diagnostics: [ + { + severity: "error", + code: "KDL_JOINT_LIMIT" + } + ] + }); + }); + + it("keeps velocity and acceleration within joint limits", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 6, + method: "planMoveJ", + payload: [ + handle, + baseRequest({ + speed: { kind: "joint_percent", value: 1 }, + target: { + id: "limit_goal", + joints: [1, 0.5] + } + }) + ] + }); + + const trajectory = response.result as TrajectoryResult; + expect(trajectory.ok).toBe(true); + for (const point of trajectory.points) { + expect(Math.abs(point.jointVelocity[0]!)).toBeLessThanOrEqual(1 + 1e-9); + expect(Math.abs(point.jointVelocity[1]!)).toBeLessThanOrEqual(0.5 + 1e-9); + expect(Math.abs(point.jointAcceleration[0]!)).toBeLessThanOrEqual(2 + 1e-9); + expect(Math.abs(point.jointAcceleration[1]!)).toBeLessThanOrEqual(1 + 1e-9); + } + }); +}); diff --git a/kdl-wasm/web/tests/kdl/planMoveL.test.ts b/kdl-wasm/web/tests/kdl/planMoveL.test.ts new file mode 100644 index 0000000..1234547 --- /dev/null +++ b/kdl-wasm/web/tests/kdl/planMoveL.test.ts @@ -0,0 +1,175 @@ +import { describe, expect, it } from "vitest"; +import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js"; +import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js"; +import type { MoveLRequest, TrajectoryResult } from "../../src/kdl/types.js"; + +const PLANAR_URDF = ` + + + + + + + + + + + + + + + + + +`; + +async function createRobot() { + const runtime = createKdlWorkerRuntime(); + await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] }); + const response = await dispatchKdlRpcRequest(runtime, { + id: 2, + method: "loadRobotFromUrdf", + payload: [ + PLANAR_URDF, + { + robotId: "movel", + baseLink: "base_link", + tipLink: "tool0" + } + ] + }); + expect(response.ok).toBe(true); + return { runtime, handle: response.result as number }; +} + +function request(overrides: Partial): MoveLRequest { + return { + startJoints: [Math.PI / 2, 0.2], + target: { + id: "line_goal", + pose: { + position: [0, 0.6, 0], + quaternion: [0, 0, 0, 1] + } + }, + speed: { + kind: "linear", + velocity: 0.2, + acceleration: 1 + }, + zone: { + kind: "fine" + }, + sampleTime: 0.05, + ...overrides + }; +} + +describe("planMoveL", () => { + it("plans a TCP straight-line trajectory with continuous IK seeds", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 3, + method: "planMoveL", + payload: [handle, request({})] + }); + + expect(response.ok).toBe(true); + const trajectory = response.result as TrajectoryResult; + expect(trajectory.ok).toBe(true); + expect(trajectory.motion).toBe("MOVEL"); + expect(trajectory.points.length).toBeGreaterThan(2); + expect(trajectory.points[0]).toMatchObject({ + index: 0, + time: 0, + s: 0, + motion: "MOVEL", + targetId: "line_goal" + }); + expect(trajectory.points.at(-1)?.s).toBe(1); + expect(trajectory.points.at(-1)?.tcp.position[0]).toBeCloseTo(0, 6); + expect(trajectory.points.at(-1)?.tcp.position[1]).toBeCloseTo(0.6, 6); + expect(trajectory.meta).toMatchObject({ + targetId: "line_goal", + orientationMode: "fixed" + }); + expect(trajectory.meta?.length as number).toBeCloseTo(0.4); + + for (const point of trajectory.points) { + expect(point.tcp.position[0]).toBeCloseTo(0, 5); + expect(point.tcp.position[2]).toBeCloseTo(0, 5); + expect(point.tcp.position[1]).toBeGreaterThanOrEqual(0.2 - 1e-9); + expect(point.tcp.position[1]).toBeLessThanOrEqual(0.6 + 1e-9); + } + }); + + it("returns a failed trajectory when a sampled pose is unreachable", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 4, + method: "planMoveL", + payload: [ + handle, + request({ + target: { + id: "far_goal", + pose: { + position: [0, 2, 0], + quaternion: [0, 0, 0, 1] + } + } + }) + ] + }); + + const trajectory = response.result as TrajectoryResult; + expect(trajectory.ok).toBe(false); + expect(trajectory.motion).toBe("MOVEL"); + expect(trajectory.diagnostics).toContainEqual( + expect.objectContaining({ + severity: "error", + code: "KDL_JOINT_LIMIT" + }) + ); + expect(trajectory.meta).toMatchObject({ + targetId: "far_goal" + }); + }); + + it("reports zone approximation and joint-speed approximation warnings", async () => { + const { runtime, handle } = await createRobot(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 5, + method: "planMoveL", + payload: [ + handle, + request({ + speed: { + kind: "joint_abs", + velocity: 0.2, + acceleration: 1 + }, + zone: { + kind: "distance", + value: 0.01 + } + }) + ] + }); + + const trajectory = response.result as TrajectoryResult; + expect(trajectory.ok).toBe(true); + expect(trajectory.diagnostics).toContainEqual( + expect.objectContaining({ + severity: "warning", + code: "KDL_MOVEL_JOINT_SPEED_APPROX" + }) + ); + expect(trajectory.diagnostics).toContainEqual( + expect.objectContaining({ + severity: "warning", + code: "KDL_ZONE_APPROX_FINE" + }) + ); + }); +}); diff --git a/kdl-wasm/web/tests/kdl/poseApi.test.ts b/kdl-wasm/web/tests/kdl/poseApi.test.ts new file mode 100644 index 0000000..ba6c050 --- /dev/null +++ b/kdl-wasm/web/tests/kdl/poseApi.test.ts @@ -0,0 +1,152 @@ +import { describe, expect, it } from "vitest"; +import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js"; +import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js"; +import type { Pose, PoseTarget } from "../../src/kdl/types.js"; + +async function createRuntime() { + const runtime = createKdlWorkerRuntime(); + await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] }); + return runtime; +} + +function pose(x: number, y: number, z: number): Pose { + return { + position: [x, y, z], + quaternion: [0, 0, 0, 1] + }; +} + +describe("pose transform and offset API", () => { + it("normalizes pose inputs from rpy and quaternion forms", async () => { + const runtime = await createRuntime(); + const rpyResponse = await dispatchKdlRpcRequest(runtime, { + id: 2, + method: "normalizePose", + payload: [{ xyz: [1, 2, 3], rpy: [0, 0, Math.PI / 2] }] + }); + const quatResponse = await dispatchKdlRpcRequest(runtime, { + id: 3, + method: "normalizePose", + payload: [{ xyz: [0, 0, 0], quat: [0, 0, 0, 2] }] + }); + + expect(rpyResponse.ok).toBe(true); + expect((rpyResponse.result as Pose).position).toEqual([1, 2, 3]); + expect((rpyResponse.result as Pose).quaternion[2]).toBeCloseTo(Math.SQRT1_2); + expect((rpyResponse.result as Pose).quaternion[3]).toBeCloseTo(Math.SQRT1_2); + expect(quatResponse.result).toMatchObject({ + position: [0, 0, 0], + quaternion: [0, 0, 0, 1] + }); + }); + + it("composes poses and computes an inverse pose", async () => { + const runtime = await createRuntime(); + const composeResponse = await dispatchKdlRpcRequest(runtime, { + id: 4, + method: "composePose", + payload: [pose(1, 0, 0), pose(0, 2, 0)] + }); + expect(composeResponse.result).toMatchObject({ + position: [1, 2, 0], + quaternion: [0, 0, 0, 1] + }); + + const inverseResponse = await dispatchKdlRpcRequest(runtime, { + id: 5, + method: "inversePose", + payload: [pose(1, 2, 3)] + }); + expect(inverseResponse.result).toMatchObject({ + position: [-1, -2, -3], + quaternion: [0, 0, 0, 1] + }); + + const identityResponse = await dispatchKdlRpcRequest(runtime, { + id: 6, + method: "composePose", + payload: [pose(1, 2, 3), inverseResponse.result] + }); + expect((identityResponse.result as Pose).position[0]).toBeCloseTo(0); + expect((identityResponse.result as Pose).position[1]).toBeCloseTo(0); + expect((identityResponse.result as Pose).position[2]).toBeCloseTo(0); + }); + + it("applies frame, target, and tool using the same order as FK", async () => { + const runtime = await createRuntime(); + const target: PoseTarget = { + id: "pick", + pose: pose(0.5, 0.1, 0.2) + }; + const response = await dispatchKdlRpcRequest(runtime, { + id: 7, + method: "applyToolAndFrame", + payload: [target, pose(0, 0, 0.18), pose(0.8, 0, 0.2)] + }); + + const result = response.result as Pose; + expect(result.position[0]).toBeCloseTo(1.3); + expect(result.position[1]).toBeCloseTo(0.1); + expect(result.position[2]).toBeCloseTo(0.58); + expect(result.quaternion).toEqual([0, 0, 0, 1]); + }); + + it("applies offset in frame/world by left composition and tool by right composition", async () => { + const runtime = await createRuntime(); + const target: PoseTarget = { + id: "pick", + pose: { + position: [1, 2, 3], + quaternion: [0, 0, Math.SQRT1_2, Math.SQRT1_2] + }, + frame: pose(10, 0, 0) + }; + + const frameOffset = await dispatchKdlRpcRequest(runtime, { + id: 8, + method: "applyOffset", + payload: [target, { mode: "frame", xyz: [0.1, 0, 0] }] + }); + const worldOffset = await dispatchKdlRpcRequest(runtime, { + id: 9, + method: "applyOffset", + payload: [target, { mode: "world", xyz: [0, 0.2, 0] }] + }); + const toolOffset = await dispatchKdlRpcRequest(runtime, { + id: 10, + method: "applyOffset", + payload: [target, { mode: "tool", xyz: [0.1, 0, 0] }] + }); + + expect((frameOffset.result as PoseTarget).id).toBe("pick"); + expect((frameOffset.result as PoseTarget).frame).toEqual(target.frame); + expect((frameOffset.result as PoseTarget).pose.position[0]).toBeCloseTo(1.1); + expect((frameOffset.result as PoseTarget).pose.position[1]).toBeCloseTo(2); + expect((worldOffset.result as PoseTarget).pose.position[0]).toBeCloseTo(1); + expect((worldOffset.result as PoseTarget).pose.position[1]).toBeCloseTo(2.2); + expect((toolOffset.result as PoseTarget).pose.position[0]).toBeCloseTo(1); + expect((toolOffset.result as PoseTarget).pose.position[1]).toBeCloseTo(2.1); + }); + + it("returns structured diagnostics for invalid pose inputs", async () => { + const runtime = await createRuntime(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 11, + method: "normalizePose", + payload: [{ xyz: [1, 2], rpy: [0, 0, 0] }] + }); + + expect(response).toMatchObject({ + ok: false, + error: { + code: "KDL_INVALID_POSE", + diagnostics: [ + { + severity: "error", + code: "KDL_INVALID_POSE" + } + ] + } + }); + }); +}); diff --git a/kdl-wasm/web/tests/kdl/rpc.test.ts b/kdl-wasm/web/tests/kdl/rpc.test.ts new file mode 100644 index 0000000..96e12f7 --- /dev/null +++ b/kdl-wasm/web/tests/kdl/rpc.test.ts @@ -0,0 +1,206 @@ +import { describe, expect, it } from "vitest"; +import { KdlWorkerClient, type KdlWorkerLike } from "../../src/kdl/kdlClient.js"; +import type { NativeKdlModule } from "../../src/kdl/nativeModule.js"; +import { KdlStructuredError, type KdlRpcRequest, type KdlRpcResponse } from "../../src/kdl/rpc.js"; +import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js"; +import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js"; + +class FakeWorker implements KdlWorkerLike { + readonly sent: Array> = []; + terminated = false; + private messageListeners = new Set<(event: MessageEvent) => void>(); + private errorListeners = new Set<(event: ErrorEvent) => void>(); + + postMessage(message: KdlRpcRequest): void { + this.sent.push(message); + } + + terminate(): void { + this.terminated = true; + } + + addEventListener(type: "message", listener: (event: MessageEvent) => void): void; + addEventListener(type: "error", listener: (event: ErrorEvent) => void): void; + addEventListener(type: "message" | "error", listener: unknown): void { + if (type === "message") { + this.messageListeners.add(listener as (event: MessageEvent) => void); + return; + } + this.errorListeners.add(listener as (event: ErrorEvent) => void); + } + + removeEventListener(type: "message", listener: (event: MessageEvent) => void): void; + removeEventListener(type: "error", listener: (event: ErrorEvent) => void): void; + removeEventListener(type: "message" | "error", listener: unknown): void { + if (type === "message") { + this.messageListeners.delete(listener as (event: MessageEvent) => void); + return; + } + this.errorListeners.delete(listener as (event: ErrorEvent) => void); + } + + emitResponse(response: KdlRpcResponse): void { + const event = { data: response } as MessageEvent; + for (const listener of this.messageListeners) { + listener(event); + } + } + + emitError(message: string): void { + const event = { message, error: new Error(message) } as ErrorEvent; + for (const listener of this.errorListeners) { + listener(event); + } + } +} + +describe("KDL Worker RPC", () => { + it("loads the native WASM module during init when a loader is configured", async () => { + const calls: unknown[][] = []; + const native: NativeKdlModule = { + ccall: (...args) => { + calls.push(args); + return 0; + } + }; + const runtime = createKdlWorkerRuntime(async () => native); + + const response = await dispatchKdlRpcRequest(runtime, { + id: 1, + method: "init", + payload: [{ wasmBuild: "native-test" }] + }); + + expect(response.ok).toBe(true); + expect(response.result).toMatchObject({ wasmBuild: "native-test" }); + expect(calls).toEqual([["kdl_init", "number", ["string"], ['{"wasmBuild":"native-test"}']]]); + }); + + it("normalizes native WASM initialization failures", async () => { + const runtime = createKdlWorkerRuntime(async () => { + throw new Error("cannot load kdl.js"); + }); + + const response = await dispatchKdlRpcRequest(runtime, { + id: 11, + method: "init", + payload: [{}] + }); + + expect(response.ok).toBe(false); + expect(response.error).toMatchObject({ + code: "KDL_WASM_INIT_FAILED", + diagnostics: [ + { + severity: "error", + code: "KDL_WASM_INIT_FAILED" + } + ] + }); + }); + + it("dispatches init and dispose through structured responses", async () => { + const runtime = createKdlWorkerRuntime(); + const initResponse = await dispatchKdlRpcRequest(runtime, { + id: 1, + method: "init", + payload: [{ wasmBuild: "test", useThreads: true }] + }); + + expect(initResponse.ok).toBe(true); + expect(initResponse.result).toMatchObject({ + version: "0.1.0", + wasmBuild: "test", + supportsThreads: true + }); + + const disposeResponse = await dispatchKdlRpcRequest(runtime, { + id: 2, + method: "dispose", + payload: [] + }); + + expect(disposeResponse).toMatchObject({ id: 2, ok: true }); + }); + + it("returns a structured error when an implemented method is called before init", async () => { + const response = await dispatchKdlRpcRequest(createKdlWorkerRuntime(), { + id: 7, + method: "fk", + payload: [1, new Float64Array([0])] + }); + + expect(response.ok).toBe(false); + expect(response.error).toMatchObject({ + code: "KDL_NOT_INITIALIZED", + diagnostics: [ + { + severity: "error", + code: "KDL_NOT_INITIALIZED" + } + ] + }); + }); + + it("uses unique request ids and resolves responses by id", async () => { + const workers: FakeWorker[] = []; + const client = new KdlWorkerClient(() => { + const worker = new FakeWorker(); + workers.push(worker); + return worker; + }); + + const first = client.call("init", { wasmBuild: "a" }); + const second = client.call("dispose"); + + expect(workers).toHaveLength(1); + expect(workers[0]?.sent.map((request) => request.id)).toEqual([1, 2]); + + workers[0]?.emitResponse({ id: 2, ok: true }); + workers[0]?.emitResponse({ + id: 1, + ok: true, + result: { + version: "0.1.0", + wasmBuild: "a", + supportsThreads: false, + supportsWasmFs: false + } + }); + + await expect(second).resolves.toBeUndefined(); + await expect(first).resolves.toMatchObject({ wasmBuild: "a" }); + }); + + it("rejects pending requests on worker failure and can create a fresh worker", async () => { + const workers: FakeWorker[] = []; + const client = new KdlWorkerClient(() => { + const worker = new FakeWorker(); + workers.push(worker); + return worker; + }); + + const pending = client.init(); + workers[0]?.emitError("boom"); + + await expect(pending).rejects.toMatchObject({ + code: "KDL_WORKER_CRASHED" + }); + expect(workers[0]?.terminated).toBe(true); + + const restarted = client.init({ wasmBuild: "restart" }); + expect(workers).toHaveLength(2); + workers[1]?.emitResponse({ + id: 2, + ok: true, + result: { + version: "0.1.0", + wasmBuild: "restart", + supportsThreads: false, + supportsWasmFs: false + } + }); + + await expect(restarted).resolves.toMatchObject({ wasmBuild: "restart" }); + }); +}); diff --git a/kdl-wasm/web/tests/kdl/trajectoryUtils.test.ts b/kdl-wasm/web/tests/kdl/trajectoryUtils.test.ts new file mode 100644 index 0000000..d3c077f --- /dev/null +++ b/kdl-wasm/web/tests/kdl/trajectoryUtils.test.ts @@ -0,0 +1,179 @@ +import { describe, expect, it } from "vitest"; +import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js"; +import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js"; +import type { CycleTimeResult, PathPlanResult, TrajectoryResult } from "../../src/kdl/types.js"; + +function pose(x: number, y: number, z: number) { + return { + position: [x, y, z] as [number, number, number], + quaternion: [0, 0, 0, 1] as [number, number, number, number] + }; +} + +function trajectory(overrides: Partial = {}): TrajectoryResult { + return { + ok: true, + motion: "MOVEJ", + duration: 1, + sampleTime: 0.5, + events: [], + diagnostics: [], + points: [ + { + index: 0, + time: 0, + dt: 0, + s: 0, + sd: 0, + sdd: 0, + joints: [0, 0], + jointVelocity: [0, 0], + jointAcceleration: [0, 0], + flange: pose(0, 0, 0), + tcp: pose(0, 0, 0), + motion: "MOVEJ", + segmentId: "s1", + diagnostics: [] + }, + { + index: 1, + time: 1, + dt: 1, + s: 1, + sd: 0, + sdd: 0, + joints: [1, 2], + jointVelocity: [0, 0], + jointAcceleration: [0, 0], + flange: pose(1, 0, 0), + tcp: pose(1, 2, 0), + motion: "MOVEJ", + segmentId: "s1", + diagnostics: [] + } + ], + ...overrides + }; +} + +async function createRuntime() { + const runtime = createKdlWorkerRuntime(); + await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] }); + return runtime; +} + +describe("cycle-time, resample, and diagnostics utilities", () => { + it("estimates cycle time for a trajectory and a path plan", async () => { + const runtime = await createRuntime(); + const singleResponse = await dispatchKdlRpcRequest(runtime, { + id: 2, + method: "estimateCycleTime", + payload: [trajectory()] + }); + expect(singleResponse.result).toMatchObject({ + ok: true, + motionTime: 1, + totalTime: 1, + segmentTimes: [ + { + segmentId: "s1", + motion: "MOVEJ", + duration: 1 + } + ], + diagnostics: [] + }); + + const path: PathPlanResult = { + ok: true, + duration: 3, + segments: [ + trajectory(), + trajectory({ + motion: "MOVEL", + duration: 2, + points: trajectory().points.map((point) => ({ ...point, motion: "MOVEL", segmentId: "s2" })) + }) + ], + points: [], + diagnostics: [] + }; + const pathResponse = await dispatchKdlRpcRequest(runtime, { + id: 3, + method: "estimateCycleTime", + payload: [path] + }); + expect(pathResponse.result).toMatchObject({ + ok: true, + motionTime: 3, + totalTime: 3, + segmentTimes: [ + { segmentId: "s1", motion: "MOVEJ", duration: 1 }, + { segmentId: "s2", motion: "MOVEL", duration: 2 } + ] + }); + }); + + it("resamples a trajectory with stable time and point ordering", async () => { + const runtime = await createRuntime(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 4, + method: "resampleTrajectory", + payload: [trajectory(), 0.25] + }); + + expect(response.ok).toBe(true); + const result = response.result as TrajectoryResult; + expect(result.sampleTime).toBe(0.25); + expect(result.points.map((point) => point.time)).toEqual([0, 0.25, 0.5, 0.75, 1]); + expect(result.points.map((point) => point.index)).toEqual([0, 1, 2, 3, 4]); + expect(result.points[2]?.joints).toEqual([0.5, 1]); + expect(result.points[2]?.tcp.position).toEqual([0.5, 1, 0]); + expect(result.diagnostics).toContainEqual( + expect.objectContaining({ + severity: "info", + code: "KDL_TRAJECTORY_RESAMPLED" + }) + ); + }); + + it("keeps structured diagnostics for warning and error cases", async () => { + const runtime = await createRuntime(); + const emptyResponse = await dispatchKdlRpcRequest(runtime, { + id: 5, + method: "resampleTrajectory", + payload: [ + trajectory({ + points: [] + }), + 0.1 + ] + }); + expect(emptyResponse.result).toMatchObject({ + diagnostics: [ + { + severity: "warning", + code: "KDL_RESAMPLE_EMPTY_TRAJECTORY" + } + ] + }); + + const invalidResponse = await dispatchKdlRpcRequest(runtime, { + id: 6, + method: "resampleTrajectory", + payload: [trajectory(), 0] + }); + expect(invalidResponse).toMatchObject({ + ok: false, + error: { + code: "KDL_INVALID_SAMPLE_TIME", + diagnostics: [ + { + severity: "error", + code: "KDL_INVALID_SAMPLE_TIME" + } + ] + } + }); + }); +}); diff --git a/kdl-wasm/web/tests/kdl/trapProfile.test.ts b/kdl-wasm/web/tests/kdl/trapProfile.test.ts new file mode 100644 index 0000000..3863108 --- /dev/null +++ b/kdl-wasm/web/tests/kdl/trapProfile.test.ts @@ -0,0 +1,138 @@ +import { describe, expect, it } from "vitest"; +import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js"; +import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js"; +import type { TrapProfileResult, TrapSample } from "../../src/kdl/types.js"; + +async function createRuntime() { + const runtime = createKdlWorkerRuntime(); + await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] }); + return runtime; +} + +function expectMonotonic(samples: TrapSample[]) { + for (let index = 1; index < samples.length; index += 1) { + expect(samples[index]!.time).toBeGreaterThan(samples[index - 1]!.time); + expect(samples[index]!.s).toBeGreaterThanOrEqual(samples[index - 1]!.s); + } +} + +describe("trapezoid velocity profile API", () => { + it("creates a trapezoid profile when the path can reach max velocity", async () => { + const runtime = await createRuntime(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 2, + method: "makeTrapProfile", + payload: [ + 2, + { + maxVelocity: 1, + maxAcceleration: 1, + sampleTime: 0.25 + } + ] + }); + + expect(response.ok).toBe(true); + const profile = response.result as TrapProfileResult; + expect(profile).toMatchObject({ + ok: true, + type: "trapezoid", + length: 2, + duration: 3, + tAccel: 1, + tConst: 1, + tDecel: 1, + vPeak: 1, + diagnostics: [] + }); + expect(profile.samples[0]).toMatchObject({ index: 0, time: 0, s: 0 }); + expect(profile.samples.at(-1)).toMatchObject({ time: 3, s: 1 }); + expect(profile.samples.find((sample) => sample.time === 1)?.s).toBeCloseTo(0.25); + expect(profile.samples.find((sample) => sample.time === 1)?.sd).toBeCloseTo(0.5); + expectMonotonic(profile.samples); + }); + + it("falls back to a triangle profile for short paths", async () => { + const runtime = await createRuntime(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 3, + method: "makeTrapProfile", + payload: [ + 0.5, + { + maxVelocity: 2, + maxAcceleration: 1, + sampleTime: 0.1 + } + ] + }); + + expect(response.ok).toBe(true); + const profile = response.result as TrapProfileResult; + expect(profile.type).toBe("triangle"); + expect(profile.tConst).toBe(0); + expect(profile.vPeak).toBeCloseTo(Math.sqrt(0.5)); + expect(profile.duration).toBeCloseTo(2 * Math.sqrt(0.5)); + expect(profile.samples[0]?.s).toBe(0); + expect(profile.samples.at(-1)?.s).toBe(1); + expect(profile.diagnostics).toMatchObject([ + { + severity: "info", + code: "KDL_TRAP_TRIANGLE_PROFILE" + } + ]); + expectMonotonic(profile.samples); + }); + + it("returns samples from sampleTrapProfile with strict endpoint samples", async () => { + const runtime = await createRuntime(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 4, + method: "sampleTrapProfile", + payload: [ + 1, + { + maxVelocity: 1, + maxAcceleration: 2, + sampleTime: 0.2 + } + ] + }); + + expect(response.ok).toBe(true); + const samples = response.result as TrapSample[]; + expect(samples[0]).toMatchObject({ index: 0, time: 0, s: 0 }); + expect(samples.at(-1)?.s).toBe(1); + expect(samples.at(-1)?.time).toBeCloseTo(1.5); + expectMonotonic(samples); + }); + + it("returns a structured diagnostic for invalid trap profile inputs", async () => { + const runtime = await createRuntime(); + const response = await dispatchKdlRpcRequest(runtime, { + id: 5, + method: "makeTrapProfile", + payload: [ + 1, + { + maxVelocity: 0, + maxAcceleration: 1, + sampleTime: 0.01 + } + ] + }); + + expect(response).toMatchObject({ + ok: false, + error: { + code: "KDL_INVALID_TRAP_PROFILE", + diagnostics: [ + { + severity: "error", + code: "KDL_INVALID_TRAP_PROFILE" + } + ] + } + }); + }); +}); diff --git a/kdl-wasm/web/tests/kdl/urdf.test.ts b/kdl-wasm/web/tests/kdl/urdf.test.ts new file mode 100644 index 0000000..8e7712a --- /dev/null +++ b/kdl-wasm/web/tests/kdl/urdf.test.ts @@ -0,0 +1,182 @@ +import { describe, expect, it } from "vitest"; +import { KdlStructuredError } from "../../src/kdl/rpc.js"; +import { createKdlWorkerRuntime } from "../../src/kdl/runtime.js"; +import { dispatchKdlRpcRequest } from "../../src/kdl/workerRpc.js"; +import { loadRobotFromUrdfModel } from "../../src/robot/urdfParser.js"; + +const SIMPLE_URDF = ` + + + + + + + + + + + + + + + + + + + + + + + + + +`; + +describe("URDF to NormalizedRobotModel", () => { + it("parses links, joints, origins, axes, limits, stable active joint names, and source hash", () => { + const model = loadRobotFromUrdfModel(SIMPLE_URDF, { + robotId: "r1", + baseLink: "base_link", + tipLink: "tool0" + }); + + expect(model).toMatchObject({ + robotId: "r1", + name: "simple6", + baseLink: "base_link", + tipLink: "tool0", + activeJointNames: ["joint_1", "joint_2"], + source: { type: "urdf" } + }); + expect(model.source.urdfHash).toHaveLength(64); + expect(model.links.map((link) => link.name)).toEqual(["base_link", "link_1", "link_2", "tool0"]); + expect(model.joints[0]).toMatchObject({ + name: "joint_1", + type: "revolute", + parent: "base_link", + child: "link_1", + origin: { xyz: [0, 0, 0.1], rpy: [0, 0, 0] }, + axis: [0, 0, 1] + }); + expect(model.limits).toEqual([ + { name: "joint_1", lower: -3.14, upper: 3.14, velocity: 2.5, acceleration: 5 }, + { name: "joint_2", lower: 0, upper: 0.4, velocity: 0.3, acceleration: 1.2 } + ]); + }); + + it("applies joint order and limit overrides", () => { + const model = loadRobotFromUrdfModel(SIMPLE_URDF, { + robotId: "r1", + baseLink: "base_link", + tipLink: "tool0", + jointOrder: ["joint_2", "joint_1"], + overrideLimits: [{ name: "joint_2", velocity: 0.2 }] + }); + + expect(model.activeJointNames).toEqual(["joint_2", "joint_1"]); + expect(model.limits[0]).toMatchObject({ name: "joint_2", velocity: 0.2 }); + }); + + it("returns structured diagnostics for disconnected base and tip links", () => { + let thrown: unknown; + try { + loadRobotFromUrdfModel(SIMPLE_URDF, { + robotId: "r1", + baseLink: "tool0", + tipLink: "base_link" + }); + } catch (error) { + thrown = error; + } + + expect(thrown).toBeInstanceOf(KdlStructuredError); + expect(thrown).toMatchObject({ + code: "KDL_INVALID_MODEL", + diagnostics: [ + { + severity: "error", + code: "KDL_INVALID_MODEL" + } + ] + }); + }); + + it("rejects unsupported joint types", () => { + const urdf = SIMPLE_URDF.replace('type="prismatic"', 'type="floating"'); + let thrown: unknown; + try { + loadRobotFromUrdfModel(urdf, { + robotId: "r1", + baseLink: "base_link", + tipLink: "tool0" + }); + } catch (error) { + thrown = error; + } + + expect(thrown).toBeInstanceOf(KdlStructuredError); + expect(thrown).toMatchObject({ + code: "KDL_INVALID_MODEL", + message: expect.stringContaining("Unsupported joint type") + }); + }); + + it("supports RobotHandle lifecycle through the worker runtime", async () => { + const runtime = createKdlWorkerRuntime(); + await dispatchKdlRpcRequest(runtime, { id: 1, method: "init", payload: [{}] }); + const createResponse = await dispatchKdlRpcRequest(runtime, { + id: 2, + method: "loadRobotFromUrdf", + payload: [ + SIMPLE_URDF, + { + robotId: "r1", + baseLink: "base_link", + tipLink: "tool0" + } + ] + }); + + expect(createResponse).toMatchObject({ ok: true, result: 1 }); + + const infoResponse = await dispatchKdlRpcRequest(runtime, { + id: 3, + method: "getRobotInfo", + payload: [1] + }); + expect(infoResponse.result).toMatchObject({ + handle: 1, + robotId: "r1", + name: "simple6", + dof: 2, + jointNames: ["joint_1", "joint_2"] + }); + + const limitsResponse = await dispatchKdlRpcRequest(runtime, { + id: 4, + method: "getJointLimits", + payload: [1] + }); + expect(limitsResponse.result).toEqual([ + { name: "joint_1", lower: -3.14, upper: 3.14, velocity: 2.5, acceleration: 5 }, + { name: "joint_2", lower: 0, upper: 0.4, velocity: 0.3, acceleration: 1.2 } + ]); + + const destroyResponse = await dispatchKdlRpcRequest(runtime, { + id: 5, + method: "destroyRobot", + payload: [1] + }); + expect(destroyResponse.ok).toBe(true); + + const afterDestroy = await dispatchKdlRpcRequest(runtime, { + id: 6, + method: "getRobotInfo", + payload: [1] + }); + expect(afterDestroy).toMatchObject({ + ok: false, + error: { code: "KDL_INVALID_HANDLE" } + }); + }); +}); diff --git a/kdl-wasm/web/tests/post/postProcessor.test.ts b/kdl-wasm/web/tests/post/postProcessor.test.ts new file mode 100644 index 0000000..2c8ba97 --- /dev/null +++ b/kdl-wasm/web/tests/post/postProcessor.test.ts @@ -0,0 +1,88 @@ +import { describe, expect, it } from "vitest"; +import { postProcessAllBrands } from "../../src/grl/post/index.js"; +import { parseGrl } from "../../src/grl/parser/index.js"; +import { compileSemanticProgram } from "../../src/grl/semantic/index.js"; + +const PROGRAM = `language grl 0.1 +module PostDemo + post_hint abb + const speed vj = joint(50 %) + const speed vl = linear(200 mm/s) + const zone z10 = z(10 mm) + target home = joint_target { joints: [0 deg] } + target pick = pose_target { pose: pose(500 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg) } + target mid = pose_target { pose: pose(550 mm, 50 mm, 0 mm, 0 deg, 0 deg, 0 deg) } + target place = pose_target { pose: pose(600 mm, 0 mm, 0 mm, 0 deg, 0 deg, 0 deg) } + proc main() + set_speed vl + set_zone z10 + movej home speed vj zone fine + movel pick speed vl zone z10 + movec via mid target place speed vl zone fine + io.do[1] = true + wait io.di[1] == true timeout 1 s + pulse io.do[2] duration 100 ms + alarm DONE "done" + end +end +`; + +function postAll() { + const ir = compileSemanticProgram(parseGrl(PROGRAM), { + startJoints: [0], + sampleTime: 0.004 + }); + return postProcessAllBrands(ir); +} + +describe("GRL multi-brand postprocessor", () => { + it("emits stable ABB, FANUC, and KUKA golden text", () => { + const result = postAll(); + + expect(result.outputs.abb.text).toBe(`MODULE PostDemo + PROC main() + MoveJ home,v50,fine,tool0; + MoveL pick,v200,z10,tool0; + MoveC mid,place,v200,fine,tool0; + SetDO io.do[1],TRUE; + WaitUntil io . di [ 1 ] == true; + PulseDO io.do[2],0.100; + ! unsupported ALARM + ENDPROC +ENDMODULE`); + expect(result.outputs.fanuc.text).toBe(`/PROG MAIN +/MN + 1: J home 50% FINE ; + 2: L pick 200mm/sec CNT10 ; + 3: C mid place 200mm/sec FINE ; + 4: DO[1]=TRUE ; + 5: WAIT (io . di [ 1 ] == true) ; + 6: PULSE DO[2] 100ms ; + 7: ! unsupported ALARM ; +/END`); + expect(result.outputs.kuka.text).toBe(`DEF Main() + PTP home Vel=50% + LIN pick Vel=0.200m/s C_DIS + CIRC mid, place Vel=0.200m/s + $OUT[1] = TRUE + WAIT FOR io . di [ 1 ] == true + PULSE $OUT[2] 0.100 + ! unsupported ALARM +END`); + }); + + it("reports unsupported semantics and ignored brand hints", () => { + const result = postAll(); + + expect(result.outputs.abb.filename).toBe("PostDemo.mod"); + expect(result.outputs.fanuc.filename).toBe("PostDemo.ls"); + expect(result.outputs.kuka.filename).toBe("PostDemo.src"); + expect(result.report).toEqual([ + expect.objectContaining({ brand: "abb", code: "GRL_POST_UNSUPPORTED", message: expect.stringContaining("ALARM") }), + expect.objectContaining({ brand: "fanuc", code: "GRL_POST_HINT_IGNORED" }), + expect.objectContaining({ brand: "fanuc", code: "GRL_POST_UNSUPPORTED", message: expect.stringContaining("ALARM") }), + expect.objectContaining({ brand: "kuka", code: "GRL_POST_HINT_IGNORED" }), + expect.objectContaining({ brand: "kuka", code: "GRL_POST_UNSUPPORTED", message: expect.stringContaining("ALARM") }) + ]); + }); +}); diff --git a/kdl-wasm/web/tsconfig.json b/kdl-wasm/web/tsconfig.json new file mode 100644 index 0000000..ab52ff5 --- /dev/null +++ b/kdl-wasm/web/tsconfig.json @@ -0,0 +1,16 @@ +{ + "compilerOptions": { + "target": "ES2022", + "module": "NodeNext", + "moduleResolution": "NodeNext", + "strict": true, + "noUncheckedIndexedAccess": true, + "exactOptionalPropertyTypes": true, + "esModuleInterop": true, + "forceConsistentCasingInFileNames": true, + "skipLibCheck": true, + "types": ["node", "vitest/globals"], + "lib": ["ES2022", "DOM"] + }, + "include": ["src/**/*.ts", "tests/**/*.ts"] +} diff --git a/kdl-wasm/web/vitest.config.ts b/kdl-wasm/web/vitest.config.ts new file mode 100644 index 0000000..533ebe4 --- /dev/null +++ b/kdl-wasm/web/vitest.config.ts @@ -0,0 +1,9 @@ +import { defineConfig } from "vitest/config"; + +export default defineConfig({ + test: { + environment: "node", + include: ["kdl-wasm/web/tests/**/*.test.ts"], + globals: true + } +}); diff --git a/orocos_kinematics_dynamics b/orocos_kinematics_dynamics new file mode 160000 index 0000000..5c78749 --- /dev/null +++ b/orocos_kinematics_dynamics @@ -0,0 +1 @@ +Subproject commit 5c787496c9c57460c0eb076f300afd8ac7412f68 diff --git a/package-lock.json b/package-lock.json new file mode 100644 index 0000000..9449803 --- /dev/null +++ b/package-lock.json @@ -0,0 +1,1741 @@ +{ + "name": "kdl-work", + "version": "0.1.0", + "lockfileVersion": 3, + "requires": true, + "packages": { + "": { + "name": "kdl-work", + "version": "0.1.0", + "dependencies": { + "fast-xml-parser": "^5.9.3" + }, + "devDependencies": { + "@types/node": "^22.15.30", + "typescript": "^5.8.3", + "vitest": "^3.2.4" + } + }, + "node_modules/@esbuild/aix-ppc64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/aix-ppc64/-/aix-ppc64-0.28.1.tgz", + "integrity": "sha512-Svl7tq8k/08+p6CXPpRjQ1fKX+1odH/BQbb48fV6fj3CWHhsoIOoY87w1oHXm0qEpkIK3ZfVgp0hed3XBXzXMQ==", + "cpu": [ + "ppc64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "aix" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/android-arm": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/android-arm/-/android-arm-0.28.1.tgz", + "integrity": "sha512-0k2F129Xdio1TdJfzJ8sy1Q47vUD2NnwdhiAf7drUN1EBTfPf4hsFCtmMgu/6m8JSzsBrlmVjudMBQqOfG8usQ==", + "cpu": [ + "arm" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "android" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/android-arm64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/android-arm64/-/android-arm64-0.28.1.tgz", + "integrity": "sha512-34EGEbCIAgosYz6goLcopX6Mo7NyGv9tfwEM2/7Ce2VcVRk568iSvniGWcUXIy7wEDR1wzolcxcriFVrWYcwBg==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "android" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/android-x64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/android-x64/-/android-x64-0.28.1.tgz", + "integrity": "sha512-dbwY7ltSMDWsRatcRpCnES4F+im88OCUgGZjy52shC7GqHRE/cYlxNbB4Z4UpJswpcc4Qxd2oE/ufM0p61IKng==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "android" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/darwin-arm64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/darwin-arm64/-/darwin-arm64-0.28.1.tgz", + "integrity": "sha512-TZbWkQY7kvTAXbXUT7uVACR5cMHsDiSz9z7ZKAX/RTq/WJEk3QyRr0wZpNhBDX+/0CtdqUIJlOiodQcta6tY3Q==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "darwin" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/darwin-x64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/darwin-x64/-/darwin-x64-0.28.1.tgz", + "integrity": "sha512-zfdzgK9ACBNZLI/CyHTOx81SyNbM6YXn7rxSgX97VjyiPl9W1i4Ka4fgKECEoFCKGpvBj5qArWIGgQjOwkgskQ==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "darwin" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/freebsd-arm64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/freebsd-arm64/-/freebsd-arm64-0.28.1.tgz", + "integrity": "sha512-wG2EA8ENdEI0qhkSZMjfqrdY+ziCYCPMmtZjjIwOmXFjmyzEHn+UUxk5of+SYsjtfs3VpnlC7QLzSI5hY/rOAw==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "freebsd" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/freebsd-x64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/freebsd-x64/-/freebsd-x64-0.28.1.tgz", + "integrity": "sha512-i7dZ9vQgnvSCzi/rYCXNgtF/U+eKZNJBzu3eTQbRgHnM7tNSizLOkRFAl3qzVc/Op/u5YkHHa4pf/3DOYHthLQ==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "freebsd" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/linux-arm": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/linux-arm/-/linux-arm-0.28.1.tgz", + "integrity": "sha512-qVXBOHQS+d5Y722GwJzJUtOLlX7km3CraOaGormF1pDtPd2C/l1SHRPgjLunLGe51Sh5YYWKMFDyV4SxgMQYTQ==", + "cpu": [ + "arm" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/linux-arm64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/linux-arm64/-/linux-arm64-0.28.1.tgz", + "integrity": "sha512-yHs+0uc8+nvEAfAfxrWQKK5peSNzBc4PegcMO0EJ2hT71uA7vB8Ihg2e77R2P7SG5uYjPbHlLLmve4LLLRCf0g==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/linux-ia32": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/linux-ia32/-/linux-ia32-0.28.1.tgz", + "integrity": "sha512-d1z4ZuP0ajrfz/FhGT4vv278rX8KnPPJx8i5+AtK7TYbx9Le9F1hyzurZpkEyjkGa9dUGhQow4C1NmeGvqxN2w==", + "cpu": [ + "ia32" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/linux-loong64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/linux-loong64/-/linux-loong64-0.28.1.tgz", + "integrity": "sha512-M5sRjUVZrkm1OAPR3dlOYzNmN+loZKGVi1VUQGrwuqLcbR6qeAz+famMhjASeH3YVKvZz+zT1jlh/keC3Rj/lg==", + "cpu": [ + "loong64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/linux-mips64el": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/linux-mips64el/-/linux-mips64el-0.28.1.tgz", + "integrity": "sha512-mRObBZeHh2OxcBFPWE/FjylkRgZdYuiTR3vaTozquCGOH14iP9oN4x4Ge81CoIDYQrXmIxpFumJBu5MtZpnQJQ==", + "cpu": [ + "mips64el" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/linux-ppc64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/linux-ppc64/-/linux-ppc64-0.28.1.tgz", + "integrity": "sha512-slScBsMAb3GFDcdrCgLwZtPYRoH2H/youv10QiZyRjmsP48fznoveWytSgCI/R0ZcUgpc0ZhIUEx6LHts8yrfQ==", + "cpu": [ + "ppc64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/linux-riscv64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/linux-riscv64/-/linux-riscv64-0.28.1.tgz", + "integrity": "sha512-kw0owk1o0GFETUJyW0jc0G4Yzs0BHZn0JDZ8JRT088vjJYX777BAs1fDGxAC+q831qOs2DTC96mNsG2opdfyyQ==", + "cpu": [ + "riscv64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/linux-s390x": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/linux-s390x/-/linux-s390x-0.28.1.tgz", + "integrity": "sha512-/lAIjX8aYFRByhh6L5rYtPEDRqa9de/4V/juOXcta5frjvzXO4/sqEtyytse0g3zZFuWu5cDN0MkLz2qRDD2Ag==", + "cpu": [ + "s390x" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/linux-x64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/linux-x64/-/linux-x64-0.28.1.tgz", + "integrity": "sha512-u/anNYF2mmVOEDwLtnQ1wOr3EZ9sTNGLWrsYGYwHWzGA3Si84IOkHXlbWTD1NB+9/1lcnweYKO54uhxZydNzfA==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/netbsd-arm64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/netbsd-arm64/-/netbsd-arm64-0.28.1.tgz", + "integrity": "sha512-oks0DYbLwWMmaakTsCb+zL4E+aHRVLom9IJZOAthMQEPiQmydXHkziYEsGYRx0uNV/IjEKGAV941JzH02pflqw==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "netbsd" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/netbsd-x64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/netbsd-x64/-/netbsd-x64-0.28.1.tgz", + "integrity": "sha512-aeL6lAnN89Hz43Mlh1G8ARasbuoYvSITDEx0tHh5b7jJnHcssqgjy9Yx430GDpmCa6OyrKoS0aNRjKundRizGg==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "netbsd" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/openbsd-arm64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/openbsd-arm64/-/openbsd-arm64-0.28.1.tgz", + "integrity": "sha512-MEFJe5C3R8pwXdZ5Y21oo6m7ePiS0d9pWucn99O/wvyJZChoIQKrQDxKrGeW8F5+T0okTHesAmDeiHDTIq0V/Q==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "openbsd" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/openbsd-x64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/openbsd-x64/-/openbsd-x64-0.28.1.tgz", + "integrity": "sha512-i/ZLIOafE0Z8cI/XANJAixoJL/uRAoS2xOA3rb0xN+KK0K177cMAsQYkzHtBrtMXAKuAc7HGgcWiZ/sRC1Nxgw==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "openbsd" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/openharmony-arm64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/openharmony-arm64/-/openharmony-arm64-0.28.1.tgz", + "integrity": "sha512-ge+Z7EXFNt2BO1oAMsVpiQ8EwndV9i1xXerAeTIK7AtPs3bKFXQM7nlRxDSIUIMeueR1CNXxqztLzdNeReKBJg==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "openharmony" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/sunos-x64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/sunos-x64/-/sunos-x64-0.28.1.tgz", + "integrity": "sha512-BEjgtECkL3vY+SaSQ6nzVfiALUeFxpawyp8Jmf5PtYhf1Ug40N1h/hxlhts+f1FvSvarEigdxS3BlSMI2PJLcQ==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "sunos" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/win32-arm64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/win32-arm64/-/win32-arm64-0.28.1.tgz", + "integrity": "sha512-lCv9eK/H6ZJWbE7bh2nw54CZ9M2nupBxJcTsdk/QQnWkdSjKGuxmmH8/GWrlT1eMmZfn4dGcCjRte397WqfQXA==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "win32" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/win32-ia32": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/win32-ia32/-/win32-ia32-0.28.1.tgz", + "integrity": "sha512-zvb/mB2bSCoJOpoCBgYKKpX6YM6mJBlBUVUtVj41DlZJVEB6/0CKlRYxP5wWl1C1ILiCoAU5wZZ4q1P3qeS6Eg==", + "cpu": [ + "ia32" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "win32" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@esbuild/win32-x64": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/@esbuild/win32-x64/-/win32-x64-0.28.1.tgz", + "integrity": "sha512-bm4Mowrv+GXMlpWX++EcXw/iLyd1o3+bJkC2DkWXYVvgZCqD/bSj9ctZeAMC3cIxgjRVR2Dufaiu4YPxr5gW1A==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "win32" + ], + "engines": { + "node": ">=18" + } + }, + "node_modules/@jridgewell/sourcemap-codec": { + "version": "1.5.5", + "resolved": "https://registry.npmjs.org/@jridgewell/sourcemap-codec/-/sourcemap-codec-1.5.5.tgz", + "integrity": "sha512-cYQ9310grqxueWbl+WuIUIaiUaDcj7WOq5fVhEljNVgRfOUhY9fy2zTvfoqWsnebh8Sl70VScFbICvJnLKB0Og==", + "dev": true, + "license": "MIT" + }, + "node_modules/@nodable/entities": { + "version": "2.2.0", + "resolved": "https://registry.npmjs.org/@nodable/entities/-/entities-2.2.0.tgz", + "integrity": "sha512-9uGyhaQavEUMC8AIddIjau4NsnsXhou+j5sBAGojCM1oxmQpVKTWR/9JxABD6UAv12vpIms55fPZKFQEhG6uBg==", + "funding": [ + { + "type": "github", + "url": "https://github.com/sponsors/nodable" + } + ], + "license": "MIT" + }, + "node_modules/@rollup/rollup-android-arm-eabi": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-android-arm-eabi/-/rollup-android-arm-eabi-4.62.2.tgz", + "integrity": "sha512-6o7ZLZK+BeenkZCFNDXqpbjw9bD6nuWonvS/lwQJp7NoVVxm6p3qE7qQ5jGuBjiFsgvqjD8mZAU5oWxTmbOeOg==", + "cpu": [ + "arm" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "android" + ] + }, + "node_modules/@rollup/rollup-android-arm64": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-android-arm64/-/rollup-android-arm64-4.62.2.tgz", + "integrity": "sha512-BaH7BllCACHoH1LguOU56UItGfUWjujlO65kS9LAodViaN4bwIKd7oeW/ZHJ/4ljr/7MIiENnNy3HJ0zXv8Zkw==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "android" + ] + }, + "node_modules/@rollup/rollup-darwin-arm64": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-darwin-arm64/-/rollup-darwin-arm64-4.62.2.tgz", + "integrity": "sha512-v39RCCvj4He82I9sFmk+M1VZ0PLM9sfsLVikjfx2hYBNALhrrOR2D3JjQA6AhlaSOgcR+RzrKY7e1+bT6SUO/A==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "darwin" + ] + }, + "node_modules/@rollup/rollup-darwin-x64": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-darwin-x64/-/rollup-darwin-x64-4.62.2.tgz", + "integrity": "sha512-yl0y2vq3S3lHeuXhEdss6TWfKW8vkujImO12tn4ZkG/4oghr09LvdYm2RElVjokTQiUvDUGXLGsYeLqUMCKpGA==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "darwin" + ] + }, + "node_modules/@rollup/rollup-freebsd-arm64": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-freebsd-arm64/-/rollup-freebsd-arm64-4.62.2.tgz", + "integrity": "sha512-tT4pvt4qXD+vEoezupCWi+a1F0vvDiksiHc+PxRlYTOH1I6/X4id9jPxTP+Fg+545euaFT1jJVs4CEdHZAU1vw==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "freebsd" + ] + }, + "node_modules/@rollup/rollup-freebsd-x64": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-freebsd-x64/-/rollup-freebsd-x64-4.62.2.tgz", + "integrity": "sha512-6nU5F2wCW+qvCBhTn1pdIU3bzsIoF7EUwsCDRxilWGprQR6yd508YnH9+OKFCwpfS8pjZqDUmnCAr7exax0XCg==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "freebsd" + ] + }, + "node_modules/@rollup/rollup-linux-arm-gnueabihf": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-linux-arm-gnueabihf/-/rollup-linux-arm-gnueabihf-4.62.2.tgz", + "integrity": "sha512-n1GJHPOvpIfhi3TmrCeh6S6URt9BFCt0KQE3qvexyGCTAKpR4Lg+eWvNZEqu7epxwus/8ElT3hacYEucm49SZg==", + "cpu": [ + "arm" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ] + }, + "node_modules/@rollup/rollup-linux-arm-musleabihf": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-linux-arm-musleabihf/-/rollup-linux-arm-musleabihf-4.62.2.tgz", + "integrity": "sha512-JqgflS8wEB+UXV/vS1RpRbifGBeN4D5lz8D8oOFbFZw4vedvdOgCFAjfBmIMdW3yL10XpQQ0Ambepw6MXrhOnA==", + "cpu": [ + "arm" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ] + }, + "node_modules/@rollup/rollup-linux-arm64-gnu": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-linux-arm64-gnu/-/rollup-linux-arm64-gnu-4.62.2.tgz", + "integrity": "sha512-wnFJkogWvN4jm/hQRF2UBaeUmk20j5+DmHvoyWii2b8HJDyvz1MF2OU/6ynXt2KR63rbZLWkFpoytpdc/yBuSA==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ] + }, + "node_modules/@rollup/rollup-linux-arm64-musl": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-linux-arm64-musl/-/rollup-linux-arm64-musl-4.62.2.tgz", + "integrity": "sha512-HVu2bp0zhvJ8xHEV9+UUs7S90VadmBSY3LcIMvozbPo4AuMGDWlz3ymHLHZPX4hR67TKTt8Qp5PJ5RBg/i+RMQ==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ] + }, + "node_modules/@rollup/rollup-linux-loong64-gnu": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-linux-loong64-gnu/-/rollup-linux-loong64-gnu-4.62.2.tgz", + "integrity": "sha512-mQqqAV8QaoSgr9I2fKDLY2BAVvmKjWoGiu/cSYQonsLvtqwEn1E4QYfnCOcp5zoEqNhsDYin1s6jx/VJmrxlZg==", + "cpu": [ + "loong64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ] + }, + "node_modules/@rollup/rollup-linux-loong64-musl": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-linux-loong64-musl/-/rollup-linux-loong64-musl-4.62.2.tgz", + "integrity": "sha512-IxKLoxCQ2IWi6bT2akyDUBGsOImDKB+sPp4EsTmwFQ/fMwpCKm8uLSSgP/Kx/QYUgKis6SEZ5/Nlhup0DIA0PQ==", + "cpu": [ + "loong64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ] + }, + "node_modules/@rollup/rollup-linux-ppc64-gnu": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-linux-ppc64-gnu/-/rollup-linux-ppc64-gnu-4.62.2.tgz", + "integrity": "sha512-Mk5ha2RQSgyFfmYYLkBpPnUk8D8FriBxesO1u9O75X0mHgXL1UQcH5Itl2lurWL2tj0RxV9b9tJgipac0hRY9A==", + "cpu": [ + "ppc64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ] + }, + "node_modules/@rollup/rollup-linux-ppc64-musl": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-linux-ppc64-musl/-/rollup-linux-ppc64-musl-4.62.2.tgz", + "integrity": "sha512-CjvEnqJL/0/TQ3TXX3OPIJ/kmBellrWd4heXUmHeJlTnmwjKpSJzoehLaL6Xk0ZnMHBu9dZuFADNOrtjF4v+2w==", + "cpu": [ + "ppc64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ] + }, + "node_modules/@rollup/rollup-linux-riscv64-gnu": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-linux-riscv64-gnu/-/rollup-linux-riscv64-gnu-4.62.2.tgz", + "integrity": "sha512-1SiZbzwdkaDURsew/tSOrooKiYy7EQGT6m8ufavAi9NEyQb/6VuIxFXAL1fqa4iZe3g4NbNk4P7J32z2tw5Mgg==", + "cpu": [ + "riscv64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ] + }, + "node_modules/@rollup/rollup-linux-riscv64-musl": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-linux-riscv64-musl/-/rollup-linux-riscv64-musl-4.62.2.tgz", + "integrity": "sha512-nQts12zJ3NQRoE6uYljOH89v7szzLDvG2JD/vsX+vGXU8w/At1GowTZ5/7qeFQ8m7L55rpR8Okugnuo5bgjy2Q==", + "cpu": [ + "riscv64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ] + }, + "node_modules/@rollup/rollup-linux-s390x-gnu": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-linux-s390x-gnu/-/rollup-linux-s390x-gnu-4.62.2.tgz", + "integrity": "sha512-E9/ll019jhPIJgpzfZoIkBGhcz+kKNgVWYRY0zr9srBdPPFVpvOKW8VaJKUbeK+eZXyQF9ltME+Kk6affeaPgg==", + "cpu": [ + "s390x" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ] + }, + "node_modules/@rollup/rollup-linux-x64-gnu": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-linux-x64-gnu/-/rollup-linux-x64-gnu-4.62.2.tgz", + "integrity": "sha512-5BqxR/pshjey51iliyzTD5Xi3EN0aLmQ2lZ3lvefVV9c82BvrLo2/6OT55iifpWBufs6kdwWbuOKS841DrmK9A==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ] + }, + "node_modules/@rollup/rollup-linux-x64-musl": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-linux-x64-musl/-/rollup-linux-x64-musl-4.62.2.tgz", + "integrity": "sha512-uNN83XxQrRAh/w0/pmAfibcwyb6YWt4gP+dpnQKPVJshAloQ785ii8CT8ZCIxkGg9opVsvAlGhFitSm6D1Jjpg==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "linux" + ] + }, + "node_modules/@rollup/rollup-openbsd-x64": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-openbsd-x64/-/rollup-openbsd-x64-4.62.2.tgz", + "integrity": "sha512-srjEIxSH3LRnJN6THczDHWQplqEMFiAJrTab0msUryh9kwNpkICf3Ea6q6MN/2cZwRFUNx5w+h6Hpi4QuHS6Zg==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "openbsd" + ] + }, + "node_modules/@rollup/rollup-openharmony-arm64": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-openharmony-arm64/-/rollup-openharmony-arm64-4.62.2.tgz", + "integrity": "sha512-8hOJnxgbyObnCm5AlRA3A931xX19xq80RjVTKgJOvEKWqJruP/Uf12IbAOaDjjEXYRewwHLfmF0YRIdK3OwKWA==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "openharmony" + ] + }, + "node_modules/@rollup/rollup-win32-arm64-msvc": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-win32-arm64-msvc/-/rollup-win32-arm64-msvc-4.62.2.tgz", + "integrity": "sha512-mmF4AY1i0hG/bLWUctUq59gtmgaSIRa3cu/A3JFRp/sCNEme2bgDEiDS22P9FbnJB8NJNF4jPJiSP5RHQpUTDg==", + "cpu": [ + "arm64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "win32" + ] + }, + "node_modules/@rollup/rollup-win32-ia32-msvc": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-win32-ia32-msvc/-/rollup-win32-ia32-msvc-4.62.2.tgz", + "integrity": "sha512-DZgkknc6jhHrk46V25vbAM0zZkyP0nSDkJB8/dRkLTxv470dOmWDqGoEJl/9A0dFfS7yE3REOwNDxpHwSLSt0Q==", + "cpu": [ + "ia32" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "win32" + ] + }, + "node_modules/@rollup/rollup-win32-x64-gnu": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-win32-x64-gnu/-/rollup-win32-x64-gnu-4.62.2.tgz", + "integrity": "sha512-T6xr6ucWSFto+VGajA8YH26LdpHRuP4YLHEKAtCWvJDOlnmWcDZVCI2Jmjr+IFHDlt2zRaTAKE4tfjTaWLgJBg==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "win32" + ] + }, + "node_modules/@rollup/rollup-win32-x64-msvc": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/@rollup/rollup-win32-x64-msvc/-/rollup-win32-x64-msvc-4.62.2.tgz", + "integrity": "sha512-BfzEnDJOt9T8M989/lA37EcJgat01wLRnoi5dQf3QzOH7jzpqTAzdDbVfRljVr5r+jzKqpbHeyOfAaXxAd0PAA==", + "cpu": [ + "x64" + ], + "dev": true, + "license": "MIT", + "optional": true, + "os": [ + "win32" + ] + }, + "node_modules/@types/chai": { + "version": "5.2.3", + "resolved": "https://registry.npmjs.org/@types/chai/-/chai-5.2.3.tgz", + "integrity": "sha512-Mw558oeA9fFbv65/y4mHtXDs9bPnFMZAL/jxdPFUpOHHIXX91mcgEHbS5Lahr+pwZFR8A7GQleRWeI6cGFC2UA==", + "dev": true, + "license": "MIT", + "dependencies": { + "@types/deep-eql": "*", + "assertion-error": "^2.0.1" + } + }, + "node_modules/@types/deep-eql": { + "version": "4.0.2", + "resolved": "https://registry.npmjs.org/@types/deep-eql/-/deep-eql-4.0.2.tgz", + "integrity": "sha512-c9h9dVVMigMPc4bwTvC5dxqtqJZwQPePsWjPlpSOnojbor6pGqdk541lfA7AqFQr5pB1BRdq0juY9db81BwyFw==", + "dev": true, + "license": "MIT" + }, + "node_modules/@types/estree": { + "version": "1.0.9", + "resolved": "https://registry.npmjs.org/@types/estree/-/estree-1.0.9.tgz", + "integrity": "sha512-GhdPgy1el4/ImP05X05Uw4cw2/M93BCUmnEvWZNStlCzEKME4Fkk+YpoA5OiHNQmoS7Cafb8Xa3Pya8m1Qrzeg==", + "dev": true, + "license": "MIT" + }, + "node_modules/@types/node": { + "version": "22.20.0", + "resolved": "https://registry.npmjs.org/@types/node/-/node-22.20.0.tgz", + "integrity": "sha512-QWlFW2wf3nTjC13/DqRnBpR4ZO36VJH/JVBkA/vcnmbTBNQIlnObqyqZE1tUR7+Ni23Lda8R1BxMfbXRpCUx5g==", + "dev": true, + "license": "MIT", + "dependencies": { + "undici-types": "~6.21.0" + } + }, + "node_modules/@vitest/expect": { + "version": "3.2.6", + "resolved": "https://registry.npmjs.org/@vitest/expect/-/expect-3.2.6.tgz", + "integrity": "sha512-1+7q9BtaKzEmO+fmNT3kYvoNn5Y71XWAx2Q5HRim4tTVRQVRv4uJFAQ5FbK0OPUeNP/WmVCpxYxoJdvuHVjzBQ==", + "dev": true, + "license": "MIT", + "dependencies": { + "@types/chai": "^5.2.2", + "@vitest/spy": "3.2.6", + "@vitest/utils": "3.2.6", + "chai": "^5.2.0", + "tinyrainbow": "^2.0.0" + }, + "funding": { + "url": "https://opencollective.com/vitest" + } + }, + "node_modules/@vitest/mocker": { + "version": "3.2.6", + "resolved": "https://registry.npmjs.org/@vitest/mocker/-/mocker-3.2.6.tgz", + "integrity": "sha512-EZOrpDbkKotFAP7wPAQV1UIyoGOk4oX7ynWhBhLB7v+meMHbQhU16oPpIYGTTe4oFlhpryGpgpcZP/sin3hYuw==", + "dev": true, + "license": "MIT", + "dependencies": { + "@vitest/spy": "3.2.6", + "estree-walker": "^3.0.3", + "magic-string": "^0.30.17" + }, + "funding": { + "url": "https://opencollective.com/vitest" + }, + "peerDependencies": { + "msw": "^2.4.9", + "vite": "^5.0.0 || ^6.0.0 || ^7.0.0-0" + }, + "peerDependenciesMeta": { + "msw": { + "optional": true + }, + "vite": { + "optional": true + } + } + }, + "node_modules/@vitest/pretty-format": { + "version": "3.2.6", + "resolved": "https://registry.npmjs.org/@vitest/pretty-format/-/pretty-format-3.2.6.tgz", + "integrity": "sha512-lb7XXXzmm2h2ASzFnRvQpDo6onT1NmMJA3tkGTWiBFtRJ9lxGY3d3mm/Apt36gej2bkkOVLL/yTOtufDaFa/jA==", + "dev": true, + "license": "MIT", + "dependencies": { + "tinyrainbow": "^2.0.0" + }, + "funding": { + "url": "https://opencollective.com/vitest" + } + }, + "node_modules/@vitest/runner": { + "version": "3.2.6", + "resolved": "https://registry.npmjs.org/@vitest/runner/-/runner-3.2.6.tgz", + "integrity": "sha512-HYcoSj1w5tcgUnzoF0HcyaAQjpA1gj9ftUJ7iSJSuipc02jW9gKkigwZbjFldAfYHA1fa8UZVRftdMY5msWM9Q==", + "dev": true, + "license": "MIT", + "dependencies": { + "@vitest/utils": "3.2.6", + "pathe": "^2.0.3", + "strip-literal": "^3.0.0" + }, + "funding": { + "url": "https://opencollective.com/vitest" + } + }, + "node_modules/@vitest/snapshot": { + "version": "3.2.6", + "resolved": "https://registry.npmjs.org/@vitest/snapshot/-/snapshot-3.2.6.tgz", + "integrity": "sha512-H+ZjNTWGpObenh0YnlBctAPnJSI20P81PL8BPzWpx54YXLLTm8hEsWawtcYLMrwvpK48hGxLLbCS+1KRXhsKhw==", + "dev": true, + "license": "MIT", + "dependencies": { + "@vitest/pretty-format": "3.2.6", + "magic-string": "^0.30.17", + "pathe": "^2.0.3" + }, + "funding": { + "url": "https://opencollective.com/vitest" + } + }, + "node_modules/@vitest/spy": { + "version": "3.2.6", + "resolved": "https://registry.npmjs.org/@vitest/spy/-/spy-3.2.6.tgz", + "integrity": "sha512-oq6BbH68WzcWmwtBrU9nqLeaXTR4XwJF7FSLkKEZo4i6eoXcrxjcwSuTvWBIRUTC6VC72nXYunzqgZA+IKdtxg==", + "dev": true, + "license": "MIT", + "dependencies": { + "tinyspy": "^4.0.3" + }, + "funding": { + "url": "https://opencollective.com/vitest" + } + }, + "node_modules/@vitest/utils": { + "version": "3.2.6", + "resolved": "https://registry.npmjs.org/@vitest/utils/-/utils-3.2.6.tgz", + "integrity": "sha512-lI23nIs4bnT3T8NIoh+vFaz5s2/DdP0Jgt2jxwgWljvwn82cLJtyi/If+fjFyoLMGIOz0U/fKvWE0d4jsNQEfg==", + "dev": true, + "license": "MIT", + "dependencies": { + "@vitest/pretty-format": "3.2.6", + "loupe": "^3.1.4", + "tinyrainbow": "^2.0.0" + }, + "funding": { + "url": "https://opencollective.com/vitest" + } + }, + "node_modules/anynum": { + "version": "1.0.1", + "resolved": "https://registry.npmjs.org/anynum/-/anynum-1.0.1.tgz", + "integrity": "sha512-N6//FLET/tXYNM/F6ABca1oH6fWB+KlTt909Le28WMDBk8oaT4vY17DCrwg2MvmuqUKt3Ni4N5dGJ/EoBgcO6A==", + "funding": [ + { + "type": "github", + "url": "https://github.com/sponsors/NaturalIntelligence" + } + ], + "license": "MIT" + }, + "node_modules/assertion-error": { + "version": "2.0.1", + "resolved": "https://registry.npmjs.org/assertion-error/-/assertion-error-2.0.1.tgz", + "integrity": "sha512-Izi8RQcffqCeNVgFigKli1ssklIbpHnCYc6AknXGYoB6grJqyeby7jv12JUQgmTAnIDnbck1uxksT4dzN3PWBA==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=12" + } + }, + "node_modules/cac": { + "version": "6.7.14", + "resolved": "https://registry.npmjs.org/cac/-/cac-6.7.14.tgz", + "integrity": "sha512-b6Ilus+c3RrdDk+JhLKUAQfzzgLEPy6wcXqS7f/xe1EETvsDP6GORG7SFuOs6cID5YkqchW/LXZbX5bc8j7ZcQ==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=8" + } + }, + "node_modules/chai": { + "version": "5.3.3", + "resolved": "https://registry.npmjs.org/chai/-/chai-5.3.3.tgz", + "integrity": "sha512-4zNhdJD/iOjSH0A05ea+Ke6MU5mmpQcbQsSOkgdaUMJ9zTlDTD/GYlwohmIE2u0gaxHYiVHEn1Fw9mZ/ktJWgw==", + "dev": true, + "license": "MIT", + "dependencies": { + "assertion-error": "^2.0.1", + "check-error": "^2.1.1", + "deep-eql": "^5.0.1", + "loupe": "^3.1.0", + "pathval": "^2.0.0" + }, + "engines": { + "node": ">=18" + } + }, + "node_modules/check-error": { + "version": "2.1.3", + "resolved": "https://registry.npmjs.org/check-error/-/check-error-2.1.3.tgz", + "integrity": "sha512-PAJdDJusoxnwm1VwW07VWwUN1sl7smmC3OKggvndJFadxxDRyFJBX/ggnu/KE4kQAB7a3Dp8f/YXC1FlUprWmA==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">= 16" + } + }, + "node_modules/debug": { + "version": "4.4.3", + "resolved": "https://registry.npmjs.org/debug/-/debug-4.4.3.tgz", + "integrity": "sha512-RGwwWnwQvkVfavKVt22FGLw+xYSdzARwm0ru6DhTVA3umU5hZc28V3kO4stgYryrTlLpuvgI9GiijltAjNbcqA==", + "dev": true, + "license": "MIT", + "dependencies": { + "ms": "^2.1.3" + }, + "engines": { + "node": ">=6.0" + }, + "peerDependenciesMeta": { + "supports-color": { + "optional": true + } + } + }, + "node_modules/deep-eql": { + "version": "5.0.2", + "resolved": "https://registry.npmjs.org/deep-eql/-/deep-eql-5.0.2.tgz", + "integrity": "sha512-h5k/5U50IJJFpzfL6nO9jaaumfjO/f2NjK/oYB2Djzm4p9L+3T9qWpZqZ2hAbLPuuYq9wrU08WQyBTL5GbPk5Q==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=6" + } + }, + "node_modules/es-module-lexer": { + "version": "1.7.0", + "resolved": "https://registry.npmjs.org/es-module-lexer/-/es-module-lexer-1.7.0.tgz", + "integrity": "sha512-jEQoCwk8hyb2AZziIOLhDqpm5+2ww5uIE6lkO/6jcOCusfk6LhMHpXXfBLXTZ7Ydyt0j4VoUQv6uGNYbdW+kBA==", + "dev": true, + "license": "MIT" + }, + "node_modules/esbuild": { + "version": "0.28.1", + "resolved": "https://registry.npmjs.org/esbuild/-/esbuild-0.28.1.tgz", + "integrity": "sha512-HrJrvZv5ayxBzPfwphOoNzkzOIIlifzk0KJrGK2c8R4+LKpMtpYLQeUdjnwjWv/LZlkH2laZk+4w78pi99D4Vw==", + "dev": true, + "hasInstallScript": true, + "license": "MIT", + "bin": { + "esbuild": "bin/esbuild" + }, + "engines": { + "node": ">=18" + }, + "optionalDependencies": { + "@esbuild/aix-ppc64": "0.28.1", + "@esbuild/android-arm": "0.28.1", + "@esbuild/android-arm64": "0.28.1", + "@esbuild/android-x64": "0.28.1", + "@esbuild/darwin-arm64": "0.28.1", + "@esbuild/darwin-x64": "0.28.1", + "@esbuild/freebsd-arm64": "0.28.1", + "@esbuild/freebsd-x64": "0.28.1", + "@esbuild/linux-arm": "0.28.1", + "@esbuild/linux-arm64": "0.28.1", + "@esbuild/linux-ia32": "0.28.1", + "@esbuild/linux-loong64": "0.28.1", + "@esbuild/linux-mips64el": "0.28.1", + "@esbuild/linux-ppc64": "0.28.1", + "@esbuild/linux-riscv64": "0.28.1", + "@esbuild/linux-s390x": "0.28.1", + "@esbuild/linux-x64": "0.28.1", + "@esbuild/netbsd-arm64": "0.28.1", + "@esbuild/netbsd-x64": "0.28.1", + "@esbuild/openbsd-arm64": "0.28.1", + "@esbuild/openbsd-x64": "0.28.1", + "@esbuild/openharmony-arm64": "0.28.1", + "@esbuild/sunos-x64": "0.28.1", + "@esbuild/win32-arm64": "0.28.1", + "@esbuild/win32-ia32": "0.28.1", + "@esbuild/win32-x64": "0.28.1" + } + }, + "node_modules/estree-walker": { + "version": "3.0.3", + "resolved": "https://registry.npmjs.org/estree-walker/-/estree-walker-3.0.3.tgz", + "integrity": "sha512-7RUKfXgSMMkzt6ZuXmqapOurLGPPfgj6l9uRZ7lRGolvk0y2yocc35LdcxKC5PQZdn2DMqioAQ2NoWcrTKmm6g==", + "dev": true, + "license": "MIT", + "dependencies": { + "@types/estree": "^1.0.0" + } + }, + "node_modules/expect-type": { + "version": "1.4.0", + "resolved": "https://registry.npmjs.org/expect-type/-/expect-type-1.4.0.tgz", + "integrity": "sha512-KfYbmpRm0VbLjEvVa9yGwCi9GI34xvi7A/HXYWQO65CSD2u3MczUJSuwXKFIxlGsgBQizV9q5J9NHj4VG0n+pA==", + "dev": true, + "license": "Apache-2.0", + "engines": { + "node": ">=12.0.0" + } + }, + "node_modules/fast-xml-builder": { + "version": "1.2.0", + "resolved": "https://registry.npmjs.org/fast-xml-builder/-/fast-xml-builder-1.2.0.tgz", + "integrity": "sha512-00aAWieqff+ZJhsXA4g1g7M8k+7AYoMUUHF+/zFb5U6Uv/P0Vl4QZo84/IcufzYalLuEj9928bXN9PbbFzMF0Q==", + "funding": [ + { + "type": "github", + "url": "https://github.com/sponsors/NaturalIntelligence" + } + ], + "license": "MIT", + "dependencies": { + "path-expression-matcher": "^1.5.0", + "xml-naming": "^0.1.0" + } + }, + "node_modules/fast-xml-parser": { + "version": "5.9.3", + "resolved": "https://registry.npmjs.org/fast-xml-parser/-/fast-xml-parser-5.9.3.tgz", + "integrity": "sha512-brCNCeScma/kqa54J4PIDriSSSLssRkuYaUCpvHJulGc3HGI/xxKUCTDcYkAdqJsyb//ydpbxecjC3hB9+tb/g==", + "funding": [ + { + "type": "github", + "url": "https://github.com/sponsors/NaturalIntelligence" + } + ], + "license": "MIT", + "dependencies": { + "@nodable/entities": "^2.2.0", + "fast-xml-builder": "^1.2.0", + "is-unsafe": "^1.0.1", + "path-expression-matcher": "^1.5.0", + "strnum": "^2.4.1", + "xml-naming": "^0.1.0" + }, + "bin": { + "fxparser": "src/cli/cli.js" + } + }, + "node_modules/fdir": { + "version": "6.5.0", + "resolved": "https://registry.npmjs.org/fdir/-/fdir-6.5.0.tgz", + "integrity": "sha512-tIbYtZbucOs0BRGqPJkshJUYdL+SDH7dVM8gjy+ERp3WAUjLEFJE+02kanyHtwjWOnwrKYBiwAmM0p4kLJAnXg==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=12.0.0" + }, + "peerDependencies": { + "picomatch": "^3 || ^4" + }, + "peerDependenciesMeta": { + "picomatch": { + "optional": true + } + } + }, + "node_modules/fsevents": { + "version": "2.3.3", + "resolved": "https://registry.npmjs.org/fsevents/-/fsevents-2.3.3.tgz", + "integrity": "sha512-5xoDfX+fL7faATnagmWPpbFtwh/R77WmMMqqHGS65C3vvB0YHrgF+B1YmZ3441tMj5n63k0212XNoJwzlhffQw==", + "dev": true, + "hasInstallScript": true, + "license": "MIT", + "optional": true, + "os": [ + "darwin" + ], + "engines": { + "node": "^8.16.0 || ^10.6.0 || >=11.0.0" + } + }, + "node_modules/is-unsafe": { + "version": "1.0.1", + "resolved": "https://registry.npmjs.org/is-unsafe/-/is-unsafe-1.0.1.tgz", + "integrity": "sha512-CLK2+VdgERgD96EYm5lUQssZYlRg2tkZnbsxZoacmSiRxiFJ4Nk4SzjCl+Ur+v3kXIY9dTIdb3IH22y1mZ56LA==", + "funding": [ + { + "type": "github", + "url": "https://github.com/sponsors/NaturalIntelligence" + } + ], + "license": "MIT" + }, + "node_modules/js-tokens": { + "version": "9.0.1", + "resolved": "https://registry.npmjs.org/js-tokens/-/js-tokens-9.0.1.tgz", + "integrity": "sha512-mxa9E9ITFOt0ban3j6L5MpjwegGz6lBQmM1IJkWeBZGcMxto50+eWdjC/52xDbS2vy0k7vIMK0Fe2wfL9OQSpQ==", + "dev": true, + "license": "MIT" + }, + "node_modules/loupe": { + "version": "3.2.1", + "resolved": "https://registry.npmjs.org/loupe/-/loupe-3.2.1.tgz", + "integrity": "sha512-CdzqowRJCeLU72bHvWqwRBBlLcMEtIvGrlvef74kMnV2AolS9Y8xUv1I0U/MNAWMhBlKIoyuEgoJ0t/bbwHbLQ==", + "dev": true, + "license": "MIT" + }, + "node_modules/magic-string": { + "version": "0.30.21", + "resolved": "https://registry.npmjs.org/magic-string/-/magic-string-0.30.21.tgz", + "integrity": "sha512-vd2F4YUyEXKGcLHoq+TEyCjxueSeHnFxyyjNp80yg0XV4vUhnDer/lvvlqM/arB5bXQN5K2/3oinyCRyx8T2CQ==", + "dev": true, + "license": "MIT", + "dependencies": { + "@jridgewell/sourcemap-codec": "^1.5.5" + } + }, + "node_modules/ms": { + "version": "2.1.3", + "resolved": "https://registry.npmjs.org/ms/-/ms-2.1.3.tgz", + "integrity": "sha512-6FlzubTLZG3J2a/NVCAleEhjzq5oxgHyaCU9yYXvcLsvoVaHJq/s5xXI6/XXP6tz7R9xAOtHnSO/tXtF3WRTlA==", + "dev": true, + "license": "MIT" + }, + "node_modules/nanoid": { + "version": "3.3.15", + "resolved": "https://registry.npmjs.org/nanoid/-/nanoid-3.3.15.tgz", + "integrity": "sha512-y7Wygv/7mEOvxTuEQDB8StXdMRBWf1kR/tlhAzBRUFkB2jfcLOAxO/SHmOO2zgz1pVgK29/kyupn059/bCHdjA==", + "dev": true, + "funding": [ + { + "type": "github", + "url": "https://github.com/sponsors/ai" + } + ], + "license": "MIT", + "bin": { + "nanoid": "bin/nanoid.cjs" + }, + "engines": { + "node": "^10 || ^12 || ^13.7 || ^14 || >=15.0.1" + } + }, + "node_modules/path-expression-matcher": { + "version": "1.6.1", + "resolved": "https://registry.npmjs.org/path-expression-matcher/-/path-expression-matcher-1.6.1.tgz", + "integrity": "sha512-h7bxdzhHk8Knyc4Tj+jMaa7fEEoUJy7p1qtbVgkYg1Uhpe5Np5VuGXCRZnkZvU+Q42M1vStt0ifa3ueykRJPmQ==", + "funding": [ + { + "type": "github", + "url": "https://github.com/sponsors/NaturalIntelligence" + } + ], + "license": "MIT", + "engines": { + "node": ">=14.0.0" + } + }, + "node_modules/pathe": { + "version": "2.0.3", + "resolved": "https://registry.npmjs.org/pathe/-/pathe-2.0.3.tgz", + "integrity": "sha512-WUjGcAqP1gQacoQe+OBJsFA7Ld4DyXuUIjZ5cc75cLHvJ7dtNsTugphxIADwspS+AraAUePCKrSVtPLFj/F88w==", + "dev": true, + "license": "MIT" + }, + "node_modules/pathval": { + "version": "2.0.1", + "resolved": "https://registry.npmjs.org/pathval/-/pathval-2.0.1.tgz", + "integrity": "sha512-//nshmD55c46FuFw26xV/xFAaB5HF9Xdap7HJBBnrKdAd6/GxDBaNA1870O79+9ueg61cZLSVc+OaFlfmObYVQ==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">= 14.16" + } + }, + "node_modules/picocolors": { + "version": "1.1.1", + "resolved": "https://registry.npmjs.org/picocolors/-/picocolors-1.1.1.tgz", + "integrity": "sha512-xceH2snhtb5M9liqDsmEw56le376mTZkEX/jEb/RxNFyegNul7eNslCXP9FDj/Lcu0X8KEyMceP2ntpaHrDEVA==", + "dev": true, + "license": "ISC" + }, + "node_modules/picomatch": { + "version": "4.0.4", + "resolved": "https://registry.npmjs.org/picomatch/-/picomatch-4.0.4.tgz", + "integrity": "sha512-QP88BAKvMam/3NxH6vj2o21R6MjxZUAd6nlwAS/pnGvN9IVLocLHxGYIzFhg6fUQ+5th6P4dv4eW9jX3DSIj7A==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=12" + }, + "funding": { + "url": "https://github.com/sponsors/jonschlinkert" + } + }, + "node_modules/postcss": { + "version": "8.5.15", + "resolved": "https://registry.npmjs.org/postcss/-/postcss-8.5.15.tgz", + "integrity": "sha512-FfR8sjd4em2T6fb3I2MwAJU7HWVMr9zba+enmQeeWFfCbm+UOC/0X4DS8XtpUTMwWMGbjKYP7xjfNekzyGmB3A==", + "dev": true, + "funding": [ + { + "type": "opencollective", + "url": "https://opencollective.com/postcss/" + }, + { + "type": "tidelift", + "url": "https://tidelift.com/funding/github/npm/postcss" + }, + { + "type": "github", + "url": "https://github.com/sponsors/ai" + } + ], + "license": "MIT", + "dependencies": { + "nanoid": "^3.3.12", + "picocolors": "^1.1.1", + "source-map-js": "^1.2.1" + }, + "engines": { + "node": "^10 || ^12 || >=14" + } + }, + "node_modules/rollup": { + "version": "4.62.2", + "resolved": "https://registry.npmjs.org/rollup/-/rollup-4.62.2.tgz", + "integrity": "sha512-RFnrW4lhXA3s3eqHDZvN654g8OTjzRfqpIRJYczCGB6HzphckVAi/Qh4tbPUbRuDi7s1Llv8g/NspLkttY3gTA==", + "dev": true, + "license": "MIT", + "dependencies": { + "@types/estree": "1.0.9" + }, + "bin": { + "rollup": "dist/bin/rollup" + }, + "engines": { + "node": ">=18.0.0", + "npm": ">=8.0.0" + }, + "optionalDependencies": { + "@rollup/rollup-android-arm-eabi": "4.62.2", + "@rollup/rollup-android-arm64": "4.62.2", + "@rollup/rollup-darwin-arm64": "4.62.2", + "@rollup/rollup-darwin-x64": "4.62.2", + "@rollup/rollup-freebsd-arm64": "4.62.2", + "@rollup/rollup-freebsd-x64": "4.62.2", + "@rollup/rollup-linux-arm-gnueabihf": "4.62.2", + "@rollup/rollup-linux-arm-musleabihf": "4.62.2", + "@rollup/rollup-linux-arm64-gnu": "4.62.2", + "@rollup/rollup-linux-arm64-musl": "4.62.2", + "@rollup/rollup-linux-loong64-gnu": "4.62.2", + "@rollup/rollup-linux-loong64-musl": "4.62.2", + "@rollup/rollup-linux-ppc64-gnu": "4.62.2", + "@rollup/rollup-linux-ppc64-musl": "4.62.2", + "@rollup/rollup-linux-riscv64-gnu": "4.62.2", + "@rollup/rollup-linux-riscv64-musl": "4.62.2", + "@rollup/rollup-linux-s390x-gnu": "4.62.2", + "@rollup/rollup-linux-x64-gnu": "4.62.2", + "@rollup/rollup-linux-x64-musl": "4.62.2", + "@rollup/rollup-openbsd-x64": "4.62.2", + "@rollup/rollup-openharmony-arm64": "4.62.2", + "@rollup/rollup-win32-arm64-msvc": "4.62.2", + "@rollup/rollup-win32-ia32-msvc": "4.62.2", + "@rollup/rollup-win32-x64-gnu": "4.62.2", + "@rollup/rollup-win32-x64-msvc": "4.62.2", + "fsevents": "~2.3.2" + } + }, + "node_modules/siginfo": { + "version": "2.0.0", + "resolved": "https://registry.npmjs.org/siginfo/-/siginfo-2.0.0.tgz", + "integrity": "sha512-ybx0WO1/8bSBLEWXZvEd7gMW3Sn3JFlW3TvX1nREbDLRNQNaeNN8WK0meBwPdAaOI7TtRRRJn/Es1zhrrCHu7g==", + "dev": true, + "license": "ISC" + }, + "node_modules/source-map-js": { + "version": "1.2.1", + "resolved": "https://registry.npmjs.org/source-map-js/-/source-map-js-1.2.1.tgz", + "integrity": "sha512-UXWMKhLOwVKb728IUtQPXxfYU+usdybtUrK/8uGE8CQMvrhOpwvzDBwj0QhSL7MQc7vIsISBG8VQ8+IDQxpfQA==", + "dev": true, + "license": "BSD-3-Clause", + "engines": { + "node": ">=0.10.0" + } + }, + "node_modules/stackback": { + "version": "0.0.2", + "resolved": "https://registry.npmjs.org/stackback/-/stackback-0.0.2.tgz", + "integrity": "sha512-1XMJE5fQo1jGH6Y/7ebnwPOBEkIEnT4QF32d5R1+VXdXveM0IBMJt8zfaxX1P3QhVwrYe+576+jkANtSS2mBbw==", + "dev": true, + "license": "MIT" + }, + "node_modules/std-env": { + "version": "3.10.0", + "resolved": "https://registry.npmjs.org/std-env/-/std-env-3.10.0.tgz", + "integrity": "sha512-5GS12FdOZNliM5mAOxFRg7Ir0pWz8MdpYm6AY6VPkGpbA7ZzmbzNcBJQ0GPvvyWgcY7QAhCgf9Uy89I03faLkg==", + "dev": true, + "license": "MIT" + }, + "node_modules/strip-literal": { + "version": "3.1.0", + "resolved": "https://registry.npmjs.org/strip-literal/-/strip-literal-3.1.0.tgz", + "integrity": "sha512-8r3mkIM/2+PpjHoOtiAW8Rg3jJLHaV7xPwG+YRGrv6FP0wwk/toTpATxWYOW0BKdWwl82VT2tFYi5DlROa0Mxg==", + "dev": true, + "license": "MIT", + "dependencies": { + "js-tokens": "^9.0.1" + }, + "funding": { + "url": "https://github.com/sponsors/antfu" + } + }, + "node_modules/strnum": { + "version": "2.4.1", + "resolved": "https://registry.npmjs.org/strnum/-/strnum-2.4.1.tgz", + "integrity": "sha512-M9eUSMT2dCB2cTNPG7UYj6KuK7RJR2SN2+yCV/fTW3xzTCS6EaGZ5pSMgDIjB7r8zSfTGk+dvvn9rTjpVS9Mwg==", + "funding": [ + { + "type": "github", + "url": "https://github.com/sponsors/NaturalIntelligence" + } + ], + "license": "MIT", + "dependencies": { + "anynum": "^1.0.1" + } + }, + "node_modules/tinybench": { + "version": "2.9.0", + "resolved": "https://registry.npmjs.org/tinybench/-/tinybench-2.9.0.tgz", + "integrity": "sha512-0+DUvqWMValLmha6lr4kD8iAMK1HzV0/aKnCtWb9v9641TnP/MFb7Pc2bxoxQjTXAErryXVgUOfv2YqNllqGeg==", + "dev": true, + "license": "MIT" + }, + "node_modules/tinyexec": { + "version": "0.3.2", + "resolved": "https://registry.npmjs.org/tinyexec/-/tinyexec-0.3.2.tgz", + "integrity": "sha512-KQQR9yN7R5+OSwaK0XQoj22pwHoTlgYqmUscPYoknOoWCWfj/5/ABTMRi69FrKU5ffPVh5QcFikpWJI/P1ocHA==", + "dev": true, + "license": "MIT" + }, + "node_modules/tinyglobby": { + "version": "0.2.17", + "resolved": "https://registry.npmjs.org/tinyglobby/-/tinyglobby-0.2.17.tgz", + "integrity": "sha512-wXR/dYpcqKmfWpEdZjiKJOwCNFndD0DMnrW/cYjVGttEkBfVgcLFHoNrlj47mjOVic9yyNu65alsgF4NQyTa2g==", + "dev": true, + "license": "MIT", + "dependencies": { + "fdir": "^6.5.0", + "picomatch": "^4.0.4" + }, + "engines": { + "node": ">=12.0.0" + }, + "funding": { + "url": "https://github.com/sponsors/SuperchupuDev" + } + }, + "node_modules/tinypool": { + "version": "1.1.1", + "resolved": "https://registry.npmjs.org/tinypool/-/tinypool-1.1.1.tgz", + "integrity": "sha512-Zba82s87IFq9A9XmjiX5uZA/ARWDrB03OHlq+Vw1fSdt0I+4/Kutwy8BP4Y/y/aORMo61FQ0vIb5j44vSo5Pkg==", + "dev": true, + "license": "MIT", + "engines": { + "node": "^18.0.0 || >=20.0.0" + } + }, + "node_modules/tinyrainbow": { + "version": "2.0.0", + "resolved": "https://registry.npmjs.org/tinyrainbow/-/tinyrainbow-2.0.0.tgz", + "integrity": "sha512-op4nsTR47R6p0vMUUoYl/a+ljLFVtlfaXkLQmqfLR1qHma1h/ysYk4hEXZ880bf2CYgTskvTa/e196Vd5dDQXw==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=14.0.0" + } + }, + "node_modules/tinyspy": { + "version": "4.0.4", + "resolved": "https://registry.npmjs.org/tinyspy/-/tinyspy-4.0.4.tgz", + "integrity": "sha512-azl+t0z7pw/z958Gy9svOTuzqIk6xq+NSheJzn5MMWtWTFywIacg2wUlzKFGtt3cthx0r2SxMK0yzJOR0IES7Q==", + "dev": true, + "license": "MIT", + "engines": { + "node": ">=14.0.0" + } + }, + "node_modules/typescript": { + "version": "5.9.3", + "resolved": "https://registry.npmjs.org/typescript/-/typescript-5.9.3.tgz", + "integrity": "sha512-jl1vZzPDinLr9eUt3J/t7V6FgNEw9QjvBPdysz9KfQDD41fQrC2Y4vKQdiaUpFT4bXlb1RHhLpp8wtm6M5TgSw==", + "dev": true, + "license": "Apache-2.0", + "bin": { + "tsc": "bin/tsc", + "tsserver": "bin/tsserver" + }, + "engines": { + "node": ">=14.17" + } + }, + "node_modules/undici-types": { + "version": "6.21.0", + "resolved": "https://registry.npmjs.org/undici-types/-/undici-types-6.21.0.tgz", + "integrity": "sha512-iwDZqg0QAGrg9Rav5H4n0M64c3mkR59cJ6wQp+7C4nI0gsmExaedaYLNO44eT4AtBBwjbTiGPMlt2Md0T9H9JQ==", + "dev": true, + "license": "MIT" + }, + "node_modules/vite": { + "version": "7.3.6", + "resolved": "https://registry.npmjs.org/vite/-/vite-7.3.6.tgz", + "integrity": "sha512-4XP60spRGjSZFf1qYH+dJIkK2znL3zQfl9KkOV9MkkRR/3Dls0dxaBsQPTloEc5BLXWPL9vsOxopxyKoMmDueg==", + "dev": true, + "license": "MIT", + "dependencies": { + "esbuild": "^0.27.0 || ^0.28.0", + "fdir": "^6.5.0", + "picomatch": "^4.0.3", + "postcss": "^8.5.6", + "rollup": "^4.43.0", + "tinyglobby": "^0.2.15" + }, + "bin": { + "vite": "bin/vite.js" + }, + "engines": { + "node": "^20.19.0 || >=22.12.0" + }, + "funding": { + "url": "https://github.com/vitejs/vite?sponsor=1" + }, + "optionalDependencies": { + "fsevents": "~2.3.3" + }, + "peerDependencies": { + "@types/node": "^20.19.0 || >=22.12.0", + "jiti": ">=1.21.0", + "less": "^4.0.0", + "lightningcss": "^1.21.0", + "sass": "^1.70.0", + "sass-embedded": "^1.70.0", + "stylus": ">=0.54.8", + "sugarss": "^5.0.0", + "terser": "^5.16.0", + "tsx": "^4.8.1", + "yaml": "^2.4.2" + }, + "peerDependenciesMeta": { + "@types/node": { + "optional": true + }, + "jiti": { + "optional": true + }, + "less": { + "optional": true + }, + "lightningcss": { + "optional": true + }, + "sass": { + "optional": true + }, + "sass-embedded": { + "optional": true + }, + "stylus": { + "optional": true + }, + "sugarss": { + "optional": true + }, + "terser": { + "optional": true + }, + "tsx": { + "optional": true + }, + "yaml": { + "optional": true + } + } + }, + "node_modules/vite-node": { + "version": "3.2.4", + "resolved": "https://registry.npmjs.org/vite-node/-/vite-node-3.2.4.tgz", + "integrity": "sha512-EbKSKh+bh1E1IFxeO0pg1n4dvoOTt0UDiXMd/qn++r98+jPO1xtJilvXldeuQ8giIB5IkpjCgMleHMNEsGH6pg==", + "dev": true, + "license": "MIT", + "dependencies": { + "cac": "^6.7.14", + "debug": "^4.4.1", + "es-module-lexer": "^1.7.0", + "pathe": "^2.0.3", + "vite": "^5.0.0 || ^6.0.0 || ^7.0.0-0" + }, + "bin": { + "vite-node": "vite-node.mjs" + }, + "engines": { + "node": "^18.0.0 || ^20.0.0 || >=22.0.0" + }, + "funding": { + "url": "https://opencollective.com/vitest" + } + }, + "node_modules/vitest": { + "version": "3.2.6", + "resolved": "https://registry.npmjs.org/vitest/-/vitest-3.2.6.tgz", + "integrity": "sha512-xejya+bT/j/+R/AGa1XOfRxLmNUlLtlwjRsFUILF+xHfzElmGcmFydy2gqqIrd62ptIEfwVMofd19uNWD9L7Nw==", + "dev": true, + "license": "MIT", + "dependencies": { + "@types/chai": "^5.2.2", + "@vitest/expect": "3.2.6", + "@vitest/mocker": "3.2.6", + "@vitest/pretty-format": "^3.2.6", + "@vitest/runner": "3.2.6", + "@vitest/snapshot": "3.2.6", + "@vitest/spy": "3.2.6", + "@vitest/utils": "3.2.6", + "chai": "^5.2.0", + "debug": "^4.4.1", + "expect-type": "^1.2.1", + "magic-string": "^0.30.17", + "pathe": "^2.0.3", + "picomatch": "^4.0.2", + "std-env": "^3.9.0", + "tinybench": "^2.9.0", + "tinyexec": "^0.3.2", + "tinyglobby": "^0.2.14", + "tinypool": "^1.1.1", + "tinyrainbow": "^2.0.0", + "vite": "^5.0.0 || ^6.0.0 || ^7.0.0-0", + "vite-node": "3.2.4", + "why-is-node-running": "^2.3.0" + }, + "bin": { + "vitest": "vitest.mjs" + }, + "engines": { + "node": "^18.0.0 || ^20.0.0 || >=22.0.0" + }, + "funding": { + "url": "https://opencollective.com/vitest" + }, + "peerDependencies": { + "@edge-runtime/vm": "*", + "@types/debug": "^4.1.12", + "@types/node": "^18.0.0 || ^20.0.0 || >=22.0.0", + "@vitest/browser": "3.2.6", + "@vitest/ui": "3.2.6", + "happy-dom": "*", + "jsdom": "*" + }, + "peerDependenciesMeta": { + "@edge-runtime/vm": { + "optional": true + }, + "@types/debug": { + "optional": true + }, + "@types/node": { + "optional": true + }, + "@vitest/browser": { + "optional": true + }, + "@vitest/ui": { + "optional": true + }, + "happy-dom": { + "optional": true + }, + "jsdom": { + "optional": true + } + } + }, + "node_modules/why-is-node-running": { + "version": "2.3.0", + "resolved": "https://registry.npmjs.org/why-is-node-running/-/why-is-node-running-2.3.0.tgz", + "integrity": "sha512-hUrmaWBdVDcxvYqnyh09zunKzROWjbZTiNy8dBEjkS7ehEDQibXJ7XvlmtbwuTclUiIyN+CyXQD4Vmko8fNm8w==", + "dev": true, + "license": "MIT", + "dependencies": { + "siginfo": "^2.0.0", + "stackback": "0.0.2" + }, + "bin": { + "why-is-node-running": "cli.js" + }, + "engines": { + "node": ">=8" + } + }, + "node_modules/xml-naming": { + "version": "0.1.0", + "resolved": "https://registry.npmjs.org/xml-naming/-/xml-naming-0.1.0.tgz", + "integrity": "sha512-k8KO9hrMyNk6tUWqUfkTEZbezRRpONVOzUTnc97VnCvyj6Tf9lyUR9EDAIeiVLv56jsMcoXEwjW8Kv5yPY52lw==", + "funding": [ + { + "type": "github", + "url": "https://github.com/sponsors/NaturalIntelligence" + } + ], + "license": "MIT", + "engines": { + "node": ">=16.0.0" + } + } + } +} diff --git a/package.json b/package.json new file mode 100644 index 0000000..9f10aab --- /dev/null +++ b/package.json @@ -0,0 +1,18 @@ +{ + "name": "kdl-work", + "version": "0.1.0", + "private": true, + "type": "module", + "scripts": { + "test": "vitest run --config kdl-wasm/web/vitest.config.ts", + "typecheck": "tsc -p kdl-wasm/web/tsconfig.json --noEmit" + }, + "devDependencies": { + "@types/node": "^22.15.30", + "typescript": "^5.8.3", + "vitest": "^3.2.4" + }, + "dependencies": { + "fast-xml-parser": "^5.9.3" + } +} diff --git a/work/doc/KDL_WASM计算接口设计.md b/work/doc/KDL_WASM计算接口设计.md new file mode 100644 index 0000000..5b225ec --- /dev/null +++ b/work/doc/KDL_WASM计算接口设计.md @@ -0,0 +1,1032 @@ +# KDL WASM 计算接口设计 + +版本：0.1 +适用范围：GRL 通用机器人程序、虚拟控制器、离线编程、路径验证、轨迹回放 +运行环境：浏览器 TypeScript + Web Worker + Orocos KDL WebAssembly + +## 1. 目标 + +本文定义 KDL WASM 需要暴露给 TypeScript 虚拟控制器和通用机器人程序 GRL 使用的计算函数。接口设计从 GRL 语法反推，而不是简单暴露 KDL C++ 类。 + +GRL 中直接依赖 KDL WASM 的语义包括： + +1. `joint_target`、`pose_target` 的目标点解析和可达性验证。 +2. `tool`、`frame`、`offset`、`offset_in` 的位姿变换。 +3. `movej` 关节角度差分运行。 +4. `movel` TCP 直线运行。 +5. `movec` TCP 圆弧运行。 +6. `path`、`run_path`、`operation` 的批量轨迹生成和批量诊断。 +7. 速度、加速度、zone、采样周期、节拍估算。 +8. 虚拟控制器运行时的当前 TCP、当前关节、轨迹采样点、报警诊断。 + +KDL WASM 不负责： + +1. GRL 词法和语法解析。 +2. 程序流程控制、变量、IO、wait、子程序调用。 +3. 碰撞检测和几何布尔运算。 +4. 真实品牌控制器的完整 look-ahead 和伺服细节。 +5. OPFS 项目文件管理。 + +这些由 TypeScript 层、虚拟控制器、碰撞模块和 OPFS Workspace 实现。 + +## 2. 总体架构 + +```text +GRL Source / OLP Model / Brand Import + | +TypeScript Parser + Semantic Analyzer + | +Executable IR / MotionSegmentRequest + | +KdlWorkerClient + | +kdl.worker.ts + | +KDL WASM C ABI / Embind Wrapper + | +Orocos KDL +``` + +设计原则： + +1. KDL WASM 运行在 Worker 中，避免阻塞 UI 主线程。 +2. TypeScript 业务层只使用稳定接口，不直接操作 KDL C++ 对象。 +3. WASM 内部使用 `RobotHandle` 管理机器人链、求解器和缓存。 +4. 机器人结构源数据为 URDF，TypeScript 层负责解析 XML 并生成标准模型，WASM 层负责构造 KDL `Tree/Chain`。 +5. 高频数据使用 `Float64Array`，结构化配置使用 JSON。 +6. 所有计算函数返回统一诊断，不只返回成功/失败。 + +## 3. KDL 能力映射 + +| GRL/虚拟控制器能力 | KDL 或包装层能力 | 说明 | +| --- | --- | --- | +| URDF 串联链 | `Tree`、`Chain`、`Segment`、`Joint` | 包装层从标准模型构造 | +| `movej` 终点 FK | `ChainFkSolverPos_recursive` | 关节到法兰/TCP | +| `movel/movec` 逐点 IK | `ChainIkSolverPos_NR_JL`、`ChainIkSolverPos_LMA` | 带关节限位和诊断 | +| 雅可比和奇异性 | `ChainJntToJacSolver` | 输出 Jacobian 和指标 | +| TCP 直线路径 | `Path_Line` 或包装层等价实现 | MOVEL 采样 | +| TCP 圆弧路径 | `Path_Circle` 或包装层等价实现 | MOVEC 采样 | +| 梯形速度 | `VelocityProfile_Trap` | 生成 `s/sd/sdd` | +| 轨迹段 | `Trajectory_Segment` | 路径 + 速度曲线 | +| zone/blend | `Path_RoundedComposite` 或自研 blend | P1 阶段 | + +## 4. TypeScript 顶层 API + +建议对外提供一个异步客户端： + +```ts +interface KdlWasmApi { + init(options?: KdlInitOptions): Promise; + dispose(): Promise; + + loadRobotFromUrdf(urdfXml: string, options: UrdfLoadOptions): Promise; + createRobotFromModel(model: NormalizedRobotModel): Promise; + destroyRobot(handle: RobotHandle): Promise; + getRobotInfo(handle: RobotHandle): Promise; + getJointLimits(handle: RobotHandle): Promise; + + normalizePose(input: PoseLike, options?: PoseNormalizeOptions): Promise; + composePose(a: Pose, b: Pose): Promise; + inversePose(pose: Pose): Promise; + applyToolAndFrame(target: PoseTarget, tool: Pose, frame: Pose): Promise; + applyOffset(target: PoseTarget, offset: OffsetSpec): Promise; + + fk(handle: RobotHandle, joints: Float64Array, options?: FkOptions): Promise; + fkAllLinks(handle: RobotHandle, joints: Float64Array, options?: FkOptions): Promise; + jacobian(handle: RobotHandle, joints: Float64Array, options?: JacobianOptions): Promise; + ik(handle: RobotHandle, seed: Float64Array, target: Pose, options?: IkOptions): Promise; + ikBatch(handle: RobotHandle, seeds: Float64Array[], targets: Pose[], options?: IkOptions): Promise; + + checkJointLimits(handle: RobotHandle, joints: Float64Array): Promise; + checkVelocityLimits(handle: RobotHandle, trajectory: TrajectoryResult): Promise; + checkSingularity(handle: RobotHandle, joints: Float64Array): Promise; + checkReachability(handle: RobotHandle, target: PoseTarget, options?: IkOptions): Promise; + checkReachabilityBatch(handle: RobotHandle, targets: PoseTarget[], options?: IkOptions): Promise; + + makeTrapProfile(length: number, options: TrapProfileOptions): Promise; + sampleTrapProfile(length: number, options: TrapProfileOptions): Promise; + + planMoveJ(handle: RobotHandle, request: MoveJRequest): Promise; + planMoveL(handle: RobotHandle, request: MoveLRequest): Promise; + planMoveC(handle: RobotHandle, request: MoveCRequest): Promise; + + planPath(handle: RobotHandle, request: PathPlanRequest): Promise; + validatePath(handle: RobotHandle, request: PathPlanRequest): Promise; + estimateCycleTime(input: TrajectoryResult | PathPlanResult): Promise; + resampleTrajectory(trajectory: TrajectoryResult, sampleTime: number): Promise; +} +``` + +API 命名使用 `planMoveJ/planMoveL/planMoveC`，避免与 GRL 语句名混淆。GRL 编译器把 `movej/movel/movec` 解析成 IR 后，由虚拟控制器调用这些函数。 + +## 5. 基础数据类型 + +### 5.1 句柄和运行信息 + +```ts +type RobotHandle = number; + +interface KdlRuntimeInfo { + version: string; + kdlVersion?: string; + wasmBuild: string; + supportsThreads: boolean; + supportsWasmFs: boolean; +} +``` + +### 5.2 位姿 + +内部统一使用位置 + 四元数： + +```ts +interface Pose { + position: [number, number, number]; // meter + quaternion: [number, number, number, number]; // x, y, z, w +} + +type PoseLike = + | Pose + | { xyz: [number, number, number]; rpy: [number, number, number] } + | { xyz: [number, number, number]; quat: [number, number, number, number] }; +``` + +GRL 中 `pose(x, y, z, rx, ry, rz)` 由 TypeScript 编译器规范化为 `Pose` 后传给 KDL WASM。 + +### 5.3 目标点 + +```ts +interface PoseTarget { + id?: string; + pose: Pose; + config?: RobotConfiguration; + tool?: Pose; + frame?: Pose; + extAxis?: number[]; + sourceMap?: MotionSourceMap; +} + +interface JointTarget { + id?: string; + joints: number[]; + extAxis?: number[]; + sourceMap?: MotionSourceMap; +} + +interface RobotConfiguration { + shoulder?: -1 | 0 | 1; + elbow?: -1 | 0 | 1; + wrist?: -1 | 0 | 1; + turnNumbers?: number[]; +} +``` + +### 5.4 速度和 zone + +```ts +type SpeedSpec = + | { kind: "joint_percent"; value: number } + | { kind: "joint_abs"; velocity: number; acceleration?: number } + | { kind: "linear"; velocity: number; acceleration?: number; angularVelocity?: number }; + +type ZoneSpec = + | { kind: "fine" } + | { kind: "distance"; value: number } + | { kind: "cnt"; value: number } + | { kind: "continuous" }; +``` + +首版 `zone` 可只用于诊断和后处理，运动规划先按 `fine` 到点执行。P1 阶段再实现连续 blend。 + +### 5.5 诊断 + +```ts +interface MotionDiagnostic { + severity: "info" | "warning" | "error"; + code: string; + message: string; + time?: number; + pointIndex?: number; + segmentId?: string; + targetId?: string; + sourceMap?: MotionSourceMap; + data?: Record; +} + +interface MotionSourceMap { + file?: string; + line?: number; + column?: number; + module?: string; + proc?: string; + pathId?: string; + pathPointId?: string; + operationId?: string; + brandSource?: "abb" | "fanuc" | "kuka" | "grl"; +} +``` + +诊断 code 建议固定： + +| code | 含义 | +| --- | --- | +| `KDL_INVALID_MODEL` | 机器人模型非法 | +| `KDL_TARGET_UNREACHABLE` | 目标不可达 | +| `KDL_IK_FAILED` | IK 求解失败 | +| `KDL_JOINT_LIMIT` | 关节超限 | +| `KDL_VELOCITY_LIMIT` | 速度超限 | +| `KDL_ACCEL_LIMIT` | 加速度超限 | +| `KDL_SINGULARITY` | 接近奇异 | +| `KDL_ARC_DEGENERATE` | 圆弧退化 | +| `KDL_PATH_EMPTY` | 空路径 | +| `KDL_ZONE_APPROXIMATED` | zone 被近似处理 | + +## 6. 机器人模型 API + +### 6.1 URDF 加载 + +```ts +interface UrdfLoadOptions { + robotId: string; + baseLink: string; + tipLink: string; + tool?: Pose; + base?: Pose; + jointOrder?: string[]; + overrideLimits?: JointLimitOverride[]; +} + +interface NormalizedRobotModel { + robotId: string; + name: string; + baseLink: string; + tipLink: string; + links: LinkModel[]; + joints: JointModel[]; + activeJointNames: string[]; + limits: JointLimits[]; + source: { + type: "urdf"; + urdfHash: string; + }; +} +``` + +推荐实现分工： + +1. TypeScript 解析 URDF XML，检查 link/joint 连通性和单位。 +2. TypeScript 生成 `NormalizedRobotModel`。 +3. WASM 根据标准模型构造 KDL `Tree` 和从 `baseLink` 到 `tipLink` 的 `Chain`。 +4. WASM 创建 FK、IK、Jacobian 求解器并绑定到 `RobotHandle`。 + +### 6.2 机器人信息 + +```ts +interface RobotInfo { + handle: RobotHandle; + robotId: string; + name: string; + baseLink: string; + tipLink: string; + jointNames: string[]; + dof: number; + limits: JointLimits[]; +} + +interface JointLimits { + name: string; + lower: number; + upper: number; + velocity: number; + acceleration: number; + jerk?: number; +} +``` + +GRL 编译器在编译 `joint_target` 时应检查数组长度与 `dof` 一致。 + +## 7. 位姿和坐标变换 API + +GRL 中 `tool`、`frame`、`offset`、`offset_in` 都需要位姿变换。虽然 TypeScript 也可实现这些基础变换，但建议 KDL WASM 提供一致的计算函数，避免数值约定不一致。 + +```ts +interface OffsetSpec { + mode: "frame" | "tool" | "world"; + xyz?: [number, number, number]; + rpy?: [number, number, number]; + quaternion?: [number, number, number, number]; +} +``` + +函数要求： + +1. `normalizePose`：把欧拉角或四元数输入规范化。 +2. `composePose`：计算 `a * b`。 +3. `inversePose`：计算位姿逆。 +4. `applyToolAndFrame`：把目标点、工具、工件坐标转换为机器人基坐标下 TCP 目标。 +5. `applyOffset`：实现 GRL `pick offset z 100 mm` 和 `offset_in tool z -50 mm`。 + +## 8. FK、IK、Jacobian API + +### 8.1 正解 FK + +```ts +interface FkOptions { + tool?: Pose; + frame?: Pose; + includeFlange?: boolean; +} + +interface FkResult { + ok: boolean; + flange: Pose; + tcp: Pose; + joints: number[]; + diagnostics: MotionDiagnostic[]; +} +``` + +用途： + +1. 虚拟控制器显示当前 TCP。 +2. `movel/movec` 计算当前 TCP 起点。 +3. 轨迹采样点生成 TCP 位姿。 +4. 3D 机器人 link 位姿显示。 + +### 8.2 全 link 正解 + +```ts +interface LinkPoseResult { + ok: boolean; + linkPoses: Array<{ link: string; pose: Pose }>; + diagnostics: MotionDiagnostic[]; +} +``` + +用途： + +1. 机器人模型显示。 +2. 未来碰撞检测前置数据。 +3. 轨迹回放时显示每个连杆。 + +### 8.3 逆解 IK + +```ts +interface IkOptions { + tool?: Pose; + frame?: Pose; + qMin?: number[]; + qMax?: number[]; + maxIterations?: number; + positionTolerance?: number; + orientationTolerance?: number; + seeds?: number[][]; + preferredConfig?: RobotConfiguration; + allowApproximate?: boolean; +} + +interface IkResult { + ok: boolean; + joints?: number[]; + iterations: number; + residualPosition?: number; + residualOrientation?: number; + configuration?: RobotConfiguration; + reason?: "unreachable" | "joint_limit" | "singularity" | "max_iteration" | "invalid_model"; + diagnostics: MotionDiagnostic[]; +} +``` + +IK 使用规则： + +1. `movej pose_target` 需要 IK 一次，求终点关节。 +2. `movel` 每个 TCP 采样点需要 IK。 +3. `movec` 每个圆弧采样点需要 IK。 +4. `ikBatch` 用于批量路径可达性检查。 +5. 连续轨迹中每个采样点的 seed 使用上一采样点关节，减少姿态跳变。 + +### 8.4 Jacobian 和奇异性 + +```ts +interface JacobianResult { + ok: boolean; + rows: number; + cols: number; + data: Float64Array; + diagnostics: MotionDiagnostic[]; +} + +interface SingularityResult { + ok: boolean; + nearSingularity: boolean; + manipulability?: number; + conditionNumber?: number; + diagnostics: MotionDiagnostic[]; +} +``` + +用途： + +1. 运动前诊断。 +2. MOVEL/MOVEC 采样点奇异性警告。 +3. 可达性报告和路径优化提示。 + +## 9. 梯形速度 API + +GRL 速度和加速度需要转换为轨迹采样的路径参数。P0 阶段实现梯形速度曲线。 + +```ts +interface TrapProfileOptions { + maxVelocity: number; + maxAcceleration: number; + sampleTime: number; + startVelocity?: number; + endVelocity?: number; +} + +interface TrapSample { + index: number; + time: number; + s: number; + sd: number; + sdd: number; +} + +interface TrapProfileResult { + ok: boolean; + type: "trapezoid" | "triangle"; + length: number; + duration: number; + tAccel: number; + tConst: number; + tDecel: number; + vPeak: number; + samples: TrapSample[]; + diagnostics: MotionDiagnostic[]; +} +``` + +规则： + +1. `length` 为路径长度，单位 meter、radian 或归一化长度，由调用方按运动类型决定。 +2. 距离足够长时生成梯形速度曲线。 +3. 距离不足时自动退化为三角速度曲线。 +4. 首末采样点必须严格对应 `s=0` 和 `s=1`。 +5. 所有 `TrajectoryResult` 必须保留实际速度曲线采样，便于节拍报告。 + +## 10. 轨迹数据结构 + +```ts +interface TrajectoryPoint { + index: number; + time: number; + dt: number; + s: number; + sd: number; + sdd: number; + joints: number[]; + jointVelocity: number[]; + jointAcceleration: number[]; + flange: Pose; + tcp: Pose; + tcpVelocity?: [number, number, number, number, number, number]; + tcpAcceleration?: [number, number, number, number, number, number]; + motion: "MOVEJ" | "MOVEL" | "MOVEC"; + segmentId?: string; + targetId?: string; + sourceMap?: MotionSourceMap; + diagnostics: MotionDiagnostic[]; +} + +interface TrajectoryResult { + ok: boolean; + motion: "MOVEJ" | "MOVEL" | "MOVEC"; + duration: number; + sampleTime: number; + points: TrajectoryPoint[]; + events: TrajectoryEvent[]; + diagnostics: MotionDiagnostic[]; + meta?: Record; +} +``` + +轨迹点用于： + +1. 虚拟控制器 Motion Queue。 +2. 3D 仿真回放。 +3. 可达性报告。 +4. 节拍报告。 +5. 轨迹导出和 OPFS trace。 + +## 11. MOVEJ 计算函数 + +### 11.1 请求 + +```ts +interface MoveJRequest { + startJoints: number[]; + target: JointTarget | PoseTarget; + speed: SpeedSpec; + zone: ZoneSpec; + tool?: Pose; + frame?: Pose; + sampleTime: number; + speedOverride?: number; + sourceMap?: MotionSourceMap; +} +``` + +### 11.2 计算语义 + +`planMoveJ` 对应 GRL： + +```text +movej TargetExpr [speed Speed] [zone Zone] [tool Tool] [frame Frame] +``` + +算法： + +1. 校验 `startJoints` 长度和关节限位。 +2. 如果 target 是 `joint_target`，直接得到 `qEnd`。 +3. 如果 target 是 `pose_target`，先调用 IK 得到 `qEnd`。 +4. 计算每个关节角度差 `dq[i] = qEnd[i] - qStart[i]`。 +5. 根据关节速度、加速度限制计算同步运动时长。 +6. 生成梯形速度曲线。 +7. 对每个采样点计算关节位置、速度、加速度。 +8. 对每个采样点 FK，输出 TCP。 +9. 检查关节限位、速度、加速度和奇异性。 + +### 11.3 必须返回的诊断 + +1. 目标 IK 失败。 +2. 起点或终点关节超限。 +3. 采样点速度或加速度超限。 +4. 接近奇异点。 +5. `zone` 在 P0 阶段被近似为 fine。 + +## 12. MOVEL 计算函数 + +### 12.1 请求 + +```ts +interface MoveLRequest { + startJoints: number[]; + target: PoseTarget; + speed: SpeedSpec; + zone: ZoneSpec; + tool?: Pose; + frame?: Pose; + sampleTime: number; + orientationMode?: "fixed" | "slerp" | "tool_z_lock"; + ik?: IkOptions; + speedOverride?: number; + sourceMap?: MotionSourceMap; +} +``` + +### 12.2 计算语义 + +`planMoveL` 对应 GRL： + +```text +movel TargetExpr [speed Speed] [zone Zone] [tool Tool] [frame Frame] +``` + +算法： + +1. 对 `startJoints` 做 FK，得到起点 TCP。 +2. 将目标点应用 tool/frame/offset，得到终点 TCP。 +3. 计算直线长度。 +4. 使用 `linear` 速度生成梯形速度曲线。 +5. 对每个采样点计算直线位置和姿态插补。 +6. 对每个采样点 IK，seed 使用上一采样点关节。 +7. 检查 IK 连续性、关节限位、速度、加速度、奇异性。 +8. 输出轨迹和 TCP 直线误差。 + +### 12.3 必须返回的诊断 + +1. 目标不可达。 +2. 某个采样点 IK 失败。 +3. TCP 直线误差超过容差。 +4. 姿态误差超过容差。 +5. 关节配置突变。 +6. 速度或加速度超限。 + +## 13. MOVEC 计算函数 + +### 13.1 请求 + +```ts +interface MoveCRequest { + startJoints: number[]; + via: PoseTarget; + target: PoseTarget; + speed: SpeedSpec; + zone: ZoneSpec; + tool?: Pose; + frame?: Pose; + sampleTime: number; + orientationMode?: "fixed" | "slerp"; + arcMode?: "via" | "center" | "radius"; + circleDirection?: "short" | "long" | "cw" | "ccw"; + ik?: IkOptions; + speedOverride?: number; + sourceMap?: MotionSourceMap; +} +``` + +### 13.2 计算语义 + +`planMoveC` 对应 GRL： + +```text +movec via ViaTargetExpr target EndTargetExpr [speed Speed] [zone Zone] [tool Tool] [frame Frame] +``` + +算法： + +1. 对 `startJoints` 做 FK，得到起点 TCP。 +2. 将 via 和 target 应用 tool/frame/offset。 +3. 检查三点是否重合或共线。 +4. 计算圆心、半径、法向量、圆弧角度和圆弧长度。 +5. 使用 `linear` 速度按圆弧长度生成梯形速度曲线。 +6. 对每个采样点计算圆弧 TCP 位姿。 +7. 对每个采样点 IK，seed 使用上一采样点关节。 +8. 检查圆弧误差、IK 连续性、关节限位、速度、加速度、奇异性。 + +### 13.3 圆弧元数据 + +```ts +interface CirclePlanMeta { + center: [number, number, number]; + radius: number; + normal: [number, number, number]; + angle: number; + length: number; + direction: "cw" | "ccw"; + maxArcError: number; +} +``` + +`TrajectoryResult.meta.circle` 必须包含 `CirclePlanMeta`。 + +### 13.4 必须返回的诊断 + +1. via 或 target 不可达。 +2. 三点重合、近似重合或近似共线。 +3. 半径过小或圆弧长度过短。 +4. 某个采样点 IK 失败。 +5. 圆弧误差超过容差。 +6. 速度或加速度超限。 + +## 14. Path 和 Operation 批量接口 + +### 14.1 Path 请求 + +```ts +interface MotionSegmentRequest { + id: string; + motion: "MOVEJ" | "MOVEL" | "MOVEC"; + target?: JointTarget | PoseTarget; + via?: PoseTarget; + speed: SpeedSpec; + zone: ZoneSpec; + tool?: Pose; + frame?: Pose; + sourceMap?: MotionSourceMap; +} + +interface PathPlanRequest { + startJoints: number[]; + segments: MotionSegmentRequest[]; + sampleTime: number; + speedOverride?: number; + stopOnError?: boolean; +} +``` + +### 14.2 `planPath` + +`planPath` 用于 `run_path` 展开后的整条路径轨迹生成： + +1. 按 segment 顺序调用 `planMoveJ/planMoveL/planMoveC`。 +2. 每段终点关节作为下一段起点。 +3. 合并所有轨迹点，重新编号和更新时间。 +4. 保留每段 `sourceMap`。 +5. 生成整条路径的总时长。 + +```ts +interface PathPlanResult { + ok: boolean; + duration: number; + segments: TrajectoryResult[]; + points: TrajectoryPoint[]; + diagnostics: MotionDiagnostic[]; +} +``` + +### 14.3 `validatePath` + +`validatePath` 用于离线编程路径验证，不要求必须返回完整轨迹点，可按配置只返回诊断： + +```ts +interface PathValidationResult { + ok: boolean; + reachable: boolean; + cycleTime?: number; + segmentReports: SegmentValidationReport[]; + diagnostics: MotionDiagnostic[]; +} + +interface SegmentValidationReport { + segmentId: string; + ok: boolean; + motion: "MOVEJ" | "MOVEL" | "MOVEC"; + duration?: number; + maxJointVelocityRatio?: number; + maxJointAccelerationRatio?: number; + maxCartesianError?: number; + diagnostics: MotionDiagnostic[]; +} +``` + +`run_operation` 不需要 KDL WASM 单独理解工艺，只需要 TypeScript 把 operation 展开为 start action、path、end action。KDL WASM 只处理其中的 motion segment。 + +## 15. 可达性和批量检查 + +### 15.1 单点可达性 + +```ts +interface ReachabilityResult { + ok: boolean; + reachable: boolean; + targetId?: string; + joints?: number[]; + residualPosition?: number; + residualOrientation?: number; + nearestPose?: Pose; + diagnostics: MotionDiagnostic[]; +} +``` + +### 15.2 批量可达性 + +`checkReachabilityBatch` 用于： + +1. Path 编辑器批量目标点检查。 +2. CAD 曲线采样点预检查。 +3. 自动编程生成后快速诊断。 +4. 品牌程序导入后的目标点检查。 + +批量函数必须保持输入顺序，返回结果与输入 target 一一对应。 + +## 16. 节拍估算 + +```ts +interface CycleTimeResult { + ok: boolean; + motionTime: number; + waitTime?: number; + ioTime?: number; + totalTime: number; + segmentTimes: Array<{ + segmentId?: string; + motion: "MOVEJ" | "MOVEL" | "MOVEC"; + duration: number; + }>; + diagnostics: MotionDiagnostic[]; +} +``` + +KDL WASM 只估算运动时间。`waitTime`、IO 脚本延迟、工艺设备延迟由虚拟控制器补充。 + +## 17. Worker RPC 协议 + +主线程和 Worker 建议使用统一消息格式： + +```ts +interface KdlRpcRequest { + id: number; + method: keyof KdlWasmApi; + payload: T; +} + +interface KdlRpcResponse { + id: number; + ok: boolean; + result?: T; + error?: { + code: string; + message: string; + diagnostics?: MotionDiagnostic[]; + }; +} +``` + +要求： + +1. 大数组使用 Transferable 或共享内存策略，避免频繁复制。 +2. 每个请求必须有唯一 id。 +3. Worker 崩溃或 WASM 初始化失败时，主线程能恢复并重新初始化。 +4. 对长路径计算提供进度回调或分块计算，避免 Worker 长时间无响应。 + +## 18. C ABI / Embind 暴露建议 + +不建议把 KDL C++ 类完整暴露给 TypeScript。建议底层导出少量稳定函数： + +```cpp +extern "C" { + int kdl_init(const char* options_json); + int kdl_create_robot(const char* model_json); + int kdl_destroy_robot(int robot_handle); + int kdl_get_robot_info(int robot_handle, char* out_json, int out_len); + + int kdl_fk(int robot_handle, const double* joints, int n, double* out_pose7); + int kdl_fk_all_links(int robot_handle, const double* joints, int n, char* out_json, int out_len); + int kdl_jacobian(int robot_handle, const double* joints, int n, double* out_matrix); + int kdl_ik(int robot_handle, const double* seed, int n, const double* target_pose7, const char* options_json, double* out_joints); + + int kdl_plan_movej(int robot_handle, const char* request_json, char* out_json, int out_len); + int kdl_plan_movel(int robot_handle, const char* request_json, char* out_json, int out_len); + int kdl_plan_movec(int robot_handle, const char* request_json, char* out_json, int out_len); + int kdl_plan_path(int robot_handle, const char* request_json, char* out_json, int out_len); + + int kdl_sample_trap(double length, const char* options_json, char* out_json, int out_len); + int kdl_last_error(char* out_json, int out_len); +} +``` + +说明： + +1. P0 可用 JSON 输入输出实现，简单可靠。 +2. 高频 FK/IK 批量计算可增加 TypedArray 版本，减少 JSON 开销。 +3. TypeScript API 层负责把 C ABI 包装成 Promise。 +4. 所有 C ABI 返回 `0` 表示成功，非 `0` 表示错误，错误详情通过 `kdl_last_error` 获取。 + +## 19. 内存和性能要求 + +首版目标： + +1. 单机器人 6 轴模型初始化小于 1 秒。 +2. 单次 FK 小于 1 ms。 +3. 单次 IK 平均小于 10 ms，复杂点允许更长但必须有超时。 +4. 1000 个目标点批量可达性检查可在可接受交互时间内完成。 +5. 10 秒轨迹按 4 ms 采样约 2500 点，必须能稳定生成和回放。 + +实现建议： + +1. `RobotHandle` 内缓存 FK、IK、Jacobian solver。 +2. `ikBatch` 中复用上一点结果作为 seed。 +3. 对长路径分段计算，及时返回进度。 +4. 避免每个采样点跨 Worker 往返，轨迹整段在 Worker 内完成。 +5. 大轨迹 trace 写 OPFS 由 TypeScript 层完成，WASM 不直接管理项目文件。 + +## 20. 错误处理 + +所有 API 不抛裸字符串错误，必须返回结构化错误： + +```ts +interface KdlError { + code: string; + message: string; + diagnostics: MotionDiagnostic[]; +} +``` + +错误分级： + +1. `error`：不能生成可执行轨迹，例如 IK 失败、模型非法。 +2. `warning`：可生成轨迹但存在风险，例如接近限位、zone 被近似。 +3. `info`：辅助信息，例如使用了三角速度曲线。 + +虚拟控制器处理规则： + +1. `error`：进入 alarm 或 hold。 +2. `warning`：允许仿真继续，但报告中必须显示。 +3. `info`：写入 trace 或调试面板。 + +## 21. 与 GRL 的调用关系 + +| GRL 语法 | TypeScript 编译结果 | KDL WASM 函数 | +| --- | --- | --- | +| `target home = joint_target` | `JointTarget` | `checkJointLimits` | +| `target pick = pose_target` | `PoseTarget` | `checkReachability` | +| `pick offset z 100 mm` | `OffsetSpec` | `applyOffset` | +| `movej home` | `MoveJRequest` | `planMoveJ` | +| `movel pick` | `MoveLRequest` | `planMoveL` | +| `movec via mid target end` | `MoveCRequest` | `planMoveC` | +| `run_path pick_path` | `PathPlanRequest` | `planPath` | +| Path 可达性检查 | `PathPlanRequest` | `validatePath` | +| 节拍报告 | `TrajectoryResult/PathPlanResult` | `estimateCycleTime` | + +`if/for/switch/call/wait/io` 不直接调用 KDL WASM，但它们会影响何时调用运动函数和当前运行上下文。 + +### 21.1 不应暴露给 KDL WASM 的 GRL 语义 + +以下 GRL 语义由 TypeScript 虚拟控制器执行，不进入 KDL WASM： + +| GRL 语义 | 执行位置 | 说明 | +| --- | --- | --- | +| `if/elseif/else` | 虚拟控制器 | 判断分支，决定是否执行后续运动 | +| `while/for/switch` | 虚拟控制器 | 控制程序流，可能多次触发运动函数 | +| `proc/func/call/return` | 虚拟控制器 | 调用栈、参数、作用域不属于 KDL | +| `io.do/di/ai/ao` | IO Service | KDL 不管理 IO 状态 | +| `wait/pulse/timer` | 虚拟控制器 + IO Service | KDL 不阻塞等待 IO | +| `alarm/raise/try/catch` | 虚拟控制器 | KDL 只返回诊断，不执行异常流程 | +| `operation.process` | 工艺模块 | KDL 只处理 operation 展开后的 motion segment | + +边界原则：KDL WASM 只接收已经解析好的运动请求，不读取 GRL 源码，不维护程序变量，不执行子程序。 + +## 22. P0 必须暴露的函数 + +P0 阶段必须完成以下函数： + +1. `init` +2. `loadRobotFromUrdf` +3. `createRobotFromModel` +4. `destroyRobot` +5. `getRobotInfo` +6. `getJointLimits` +7. `normalizePose` +8. `composePose` +9. `inversePose` +10. `applyOffset` +11. `applyToolAndFrame` +12. `fk` +13. `fkAllLinks` +14. `jacobian` +15. `ik` +16. `ikBatch` +17. `checkJointLimits` +18. `checkSingularity` +19. `checkReachability` +20. `checkReachabilityBatch` +21. `makeTrapProfile` +22. `sampleTrapProfile` +23. `planMoveJ` +24. `planMoveL` +25. `planMoveC` +26. `planPath` +27. `validatePath` +28. `estimateCycleTime` +29. `resampleTrajectory` + +P1 扩展： + +1. `planBlendPath` +2. `planMoveSpline` +3. `checkCollisionInputPoses`，只提供 link poses，不做碰撞本身。 +4. `optimizeSeedSequence` +5. `compareTrajectory` +6. 外部轴协调相关函数。 + +## 23. 测试要求 + +### 23.1 单元测试 + +1. URDF 到 KDL Chain 的 joint 顺序测试。 +2. FK 与原生 KDL 结果对比。 +3. IK 后再 FK，误差小于容差。 +4. Jacobian 尺寸和数值测试。 +5. 梯形速度曲线长距离/短距离测试。 +6. `applyOffset` 在 frame/tool/world 三种模式下测试。 + +### 23.2 运动测试 + +1. `planMoveJ` 关节同起同停。 +2. `planMoveL` TCP 直线误差小于容差。 +3. `planMoveC` 圆心、半径、弧长和圆弧误差正确。 +4. `planMoveC` 三点共线时返回 `KDL_ARC_DEGENERATE`。 +5. 每种运动都检查速度、加速度、限位和 source map。 + +### 23.3 集成测试 + +1. 从 GRL `movej/movel/movec` 编译为请求并生成轨迹。 +2. 从 GRL `path` 编译为 `PathPlanRequest` 并生成整条路径。 +3. 轨迹写入 OPFS 后重新加载回放。 +4. 长路径批量验证性能测试。 +5. Worker 初始化、崩溃恢复和取消请求测试。 + +## 24. 实施顺序 + +建议按以下顺序开发： + +1. WASM 工程骨架和 Worker RPC。 +2. `NormalizedRobotModel` 到 KDL Chain。 +3. FK、fkAllLinks。 +4. IK、ikBatch。 +5. Jacobian 和奇异性。 +6. 位姿变换和 offset。 +7. 梯形速度曲线。 +8. planMoveJ。 +9. planMoveL。 +10. planMoveC。 +11. planPath、validatePath。 +12. 节拍估算和诊断报告。 +13. 性能优化和 TypedArray 批量接口。 + +## 25. 结论 + +KDL WASM 对 GRL 的定位是“运动学和轨迹计算内核”。它不解释完整机器人程序，也不处理 IO/Wait/流程控制。TypeScript 虚拟控制器负责执行 GRL/IR，当遇到运动相关语义时，把已经解析好的机器人模型、当前关节、目标点、速度、zone、tool、frame 传给 KDL WASM。 + +P0 阶段只要稳定实现 URDF 模型加载、FK/IK/Jacobian、MOVEJ/MOVEL/MOVEC、梯形速度、Path 批量验证和诊断，就可以支撑通用机器人程序的离线编程、虚拟运行、轨迹回放和多品牌后处理。 diff --git a/work/doc/通用机器人离线编程虚拟控制器技术方案.md b/work/doc/通用机器人离线编程虚拟控制器技术方案.md new file mode 100644 index 0000000..f5478c1 --- /dev/null +++ b/work/doc/通用机器人离线编程虚拟控制器技术方案.md @@ -0,0 +1,3578 @@ +# 通用机器人离线编程与虚拟控制器技术方案 + +版本：0.1 +日期：2026-06-26 +目标：为 Web 端离线编程、虚拟调试、通用机器人程序解析、仿真执行和品牌后处理建立技术要求与实现路线。 + +说明：本文中的 FANUC 指发那科机器人品牌。用户原始描述中的 “FUNAC” 按 FANUC 理解。 + +## 1. 项目目标 + +本项目最终目标是实现一套可在 Web 端运行的机器人离线编程与虚拟调试系统： + +1. 定义一种通用机器人程序语言，语法和功能对标 ABB RAPID、FANUC TP/KAREL、KUKA KRL。 +2. 使用 TypeScript 编写虚拟控制器、程序解析器、运行时、后处理框架和 Web 应用逻辑。 +3. 使用 HTML 构建虚拟控制器界面，面向离线编程、运行监控、调试和后处理导出。 +4. 将 Orocos KDL 编译为 WebAssembly，在 TypeScript 中调用 KDL WASM 完成机器人正解、逆解、雅可比、轨迹插补等核心计算。 +5. Web 端文件系统使用 OPFS，支持项目文件、机器人模型、程序、轨迹、日志、后处理输出的本地持久化。 +6. 通过后处理器，将通用机器人程序转换为 ABB、FANUC、KUKA 等品牌机器人程序。 + +系统定位不是简单代码编辑器，而是“虚拟机器人控制器 + 通用程序编译器 + 运动学内核 + 离线调试环境”。 + +## 1.1 商业离线编程软件对标定位 + +主流商业离线编程和虚拟调试软件，例如 ABB RobotStudio、RoboDK、Siemens Process Simulate、Visual Components、DELMIA Robotics 等，通常不是只围绕某一门机器人脚本语言工作，而是围绕“工作站、机器人、工具、坐标系、目标点、路径、工艺操作、仿真执行、后处理输出”建立项目模型。 + +因此，本项目的通用机器人程序不应设计成单纯模仿 ABB RAPID、FANUC TP 或 KUKA KRL 的文本语言，而应设计为商业 OLP 软件常见的双层模型： + +1. 上层是离线编程对象模型 + - Robot：机器人和运动组。 + - Tool：工具、TCP、负载。 + - Frame：工件坐标、夹具坐标、用户坐标。 + - Target：目标点。 + - Path：路径，由目标点序列、工艺参数、速度、过渡、姿态策略组成。 + - Operation：工艺操作，例如搬运、焊接、喷涂、打磨、码垛。 + - Program：程序流程，引用路径和操作。 + - PostProfile：品牌后处理配置。 + +2. 下层是可执行通用程序 + - 使用 GRL 文本语法表达流程逻辑、运动指令、IO 和异常。 + - 由目标点、路径和操作树自动生成。 + - 可由用户手工编辑。 + - 可编译为统一 IR，由虚拟控制器执行。 + - 可后处理为 ABB、FANUC、KUKA 等品牌程序。 + +这样设计有 4 个直接收益： + +1. 方便从 CAD 曲线、示教点、规划路径、工艺模板自动生成通用机器人程序。 +2. 方便把同一个路径用不同品牌后处理器导出，而不丢失路径语义。 +3. 方便虚拟控制器在不依赖真实品牌控制器的情况下执行统一 IR。 +4. 方便导入 ABB、FANUC、KUKA 程序后，反解析为统一 IR 和 OLP 对象模型，进行跨品牌仿真与迁移。 + +## 2. 设计原则 + +1. 通用语言优先，品牌语言兼容 + + 通用程序语言应抽象出工业机器人共同语义：运动、坐标系、工具、目标点、速度、过渡、IO、变量、流程控制、子程序、异常、任务和中断。ABB、FANUC、KUKA 的差异通过品牌配置和后处理器处理。 + + 语法风格应贴近商业离线编程软件导出的“中性机器人程序”：清晰的运动语句、稳定的目标点引用、显式速度/过渡/工具/坐标系参数、可读的流程结构。不要把通用语言设计得过度像某一家品牌的控制器语言。 + +2. 运行语义可解释、可调试 + + 程序不能只做文本转换，必须先解析为 AST，再生成统一 IR，由虚拟控制器执行。这样才能支持断点、单步、暂停、恢复、变量监控、运动队列检查、报警追踪和路径回放。 + +3. 运动学内核独立 + + TypeScript 负责业务调度、程序解释、仿真状态和 UI；KDL WASM 负责高性能数值计算。二者通过稳定 API 交互，避免把 C++ 对象模型直接泄漏到业务层。 + +4. Web 本地优先 + + 项目数据默认存储在 OPFS 中，不依赖服务器。需要提供导入、导出、备份、版本迁移和项目压缩包交换能力。 + +5. 可扩展到多品牌、多机器人、多工艺 + + 初期支持 6 轴串联工业机器人。架构上预留外部轴、变位机、导轨、多机器人、多任务、焊接、搬运、码垛、视觉偏移等扩展点。 + +6. 安全边界明确 + + 虚拟控制器用于离线编程与虚拟调试，不能直接替代真实控制器的安全功能。导出的品牌程序必须经过真实控制器校验、现场低速试运行和安全确认。 + +7. 路径对象优先 + + 商业 OLP 软件的核心不是单条 `MoveL`，而是可管理、可重算、可后处理的 Path 和 Operation。GRL 必须允许程序引用路径对象，也必须允许路径展开为显式运动指令。 + +8. 导入和导出同等重要 + + 虚拟控制器不仅要执行 GRL，也要能够解析主流品牌程序的可读文本形态，并转换为统一 IR。这样才能服务已有产线程序的虚拟调试、迁移和改造。 + +## 2.1 商业 OLP 的典型工作流 + +系统应支持以下典型工作流： + +1. 新建工作站 + - 选择机器人模型。 + - 定义工具 TCP。 + - 定义工件坐标系。 + - 配置 IO 和后处理 profile。 + +2. 创建目标点 + - 手动输入关节或笛卡尔位姿。 + - 通过虚拟示教器 jog 生成点。 + - 从 CAD 点、曲线、边界、孔位、焊缝导入点。 + - 从已有品牌程序反解析目标点。 + +3. 创建路径 + - 选择目标点序列。 + - 设置默认运动类型、速度、过渡、工具、坐标系。 + - 设置姿态保持、法向跟随、切向跟随、固定姿态等姿态策略。 + - 自动检查可达性、关节限位、奇异点和路径连续性。 + +4. 创建操作 + - 搬运操作：接近、抓取、抬起、移动、放置、退出。 + - 焊接操作：引弧、焊接路径、收弧、清枪。 + - 喷涂操作：开喷、路径、关喷、重叠检查。 + - 打磨操作：接触、恒速路径、退出。 + +5. 生成 GRL 程序 + - 根据路径和操作模板自动生成程序。 + - 用户可查看和编辑 GRL。 + - 程序可重新编译为 IR。 + +6. 虚拟调试 + - 单步执行。 + - 断点。 + - IO 仿真。 + - 轨迹回放。 + - 检查报警、奇异点、越限、不可达点。 + +7. 后处理导出 + - 选择 ABB、FANUC、KUKA 等品牌。 + - 生成品牌程序和数据文件。 + - 生成转换报告。 + +8. 品牌程序导入 + - 导入 ABB RAPID、KUKA KRL、FANUC LS 等可读文本。 + - 解析为 Brand AST。 + - 转换为统一 IR 和 GRL。 + - 尽量恢复目标点、路径和操作结构。 + +## 2.2 商业级 OLP 能力矩阵 + +对标成熟离线编程商业软件，本项目应把能力分为“基础可用、商业可交付、产线级虚拟调试”三个层次。 + +| 能力域 | 基础可用 | 商业可交付 | 产线级虚拟调试 | +| --- | --- | --- | --- | +| 工作站建模 | 单机器人、工具、工件坐标 | 机器人库、工具库、夹具、输送线、工件、工位布局 | 多机器人、多工位、外部轴、PLC/HMI/安全设备 | +| 程序生成 | 手写 GRL、目标点运动 | Path/Operation 自动生成程序 | 从 CAD/工艺数据批量生成并版本化 | +| 机器人运动 | FK/IK、MoveJ/MoveL/MoveC | 可达性、关节限位、奇异点、姿态策略 | 品牌近似轨迹、节拍优化、外部轴协调 | +| 仿真验证 | 轨迹回放 | 碰撞检测、干涉区、工艺事件、IO trace | 真实 PLC/虚拟 PLC 联调、设备顺序验证 | +| 后处理 | ABB/FANUC/KUKA 文本输出 | 可配置 post profile、转换报告、品牌数据文件 | 程序上传/下载、品牌语义回读、现场差异追踪 | +| 调试 | 运行、暂停、单步、报警 | 断点、变量 watch、IO 面板、Wait 诊断 | 多任务、多设备、时间线、产线事件回放 | +| 数据管理 | OPFS 保存 | 项目包、资源库、模板、版本迁移 | 团队协作、权限、审阅、发布基线 | +| Sim-to-Real | 离线轨迹 | TCP/工件坐标标定、基准点偏差补偿 | 现场回传校准、程序差异比对、闭环修正 | + +商业级产品的关键不是“能生成几行机器人代码”，而是能形成完整闭环： + +```text +导入资源 -> 建站 -> 生成路径 -> 仿真验证 -> 虚拟调试 -> 后处理 -> 现场校准 -> 回读修正 +``` + +## 2.3 商业级对象模型 + +现有 OLP Object Model 需要进一步扩展为商业软件常见的工作站资源模型： + +```text +Station + Cell + RobotGroup + Robot + ExternalAxis + Tool + BaseFrame + Fixture + Part + Conveyor + SafetyZone + InterferenceZone + ProcessResource + Programs + GRL Program + Imported Brand Program + Generated Brand Program + Planning + Targets + Paths + Operations + ProcessTemplates + Validation + CollisionSets + ReachabilityReports + CycleTimeReports + IOTrace + SimulationTrace +``` + +核心对象说明： + +| 对象 | 说明 | +| --- | --- | +| `Station` | 一个完整离线编程项目，对应商业软件中的 station/cell/study | +| `Cell` | 工作站布局，包含机器人、工装、设备、工件 | +| `RobotGroup` | 机器人运动组，未来支持机器人 + 外部轴 | +| `Fixture` | 夹具或工装，带 IO 和运动状态 | +| `Part` | 工件模型、工艺曲线、孔位、焊缝、喷涂区域 | +| `Conveyor` | 输送线或变位输送设备 | +| `SafetyZone` | 安全区域、禁入区域、软限位区域 | +| `InterferenceZone` | 干涉区，用于多机器人互锁或等待 | +| `ProcessTemplate` | 搬运、焊接、喷涂、涂胶、打磨等工艺模板 | +| `ValidationReport` | 碰撞、可达性、节拍、IO、后处理报告 | + +## 2.4 商业级功能模块 + +### 2.4.1 机器人与资源库 + +需要建立资源库机制，类似商业软件的机器人库、工具库、夹具库： + +1. 机器人库 + - 品牌、型号、负载、臂展、轴数。 + - DH/URDF/自定义运动学参数。 + - 关节限位、速度、加速度。 + - 默认 tool/base。 + - 品牌后处理 profile。 + +2. 工具库 + - TCP。 + - 负载。 + - 3D 模型。 + - 工艺类型，例如夹爪、焊枪、喷枪、主轴。 + - IO 接口，例如夹紧、松开、到位反馈。 + +3. 工装与设备库 + - 夹具状态。 + - 运动机构。 + - IO 行为脚本。 + - 碰撞几何。 + +4. 工艺模板库 + - Pick and place。 + - Arc welding。 + - Spot welding。 + - Gluing/dispensing。 + - Painting/spraying。 + - Grinding/polishing。 + - Machining。 + +### 2.4.2 CAD 与几何导入 + +商业 OLP 软件通常围绕 CAD/几何创建路径。本项目应预留以下能力： + +1. 导入格式 + - MVP：OBJ、STL、glTF。 + - 商业级：STEP、IGES、JT、3DXML 等可通过插件或服务端转换支持。 + +2. 几何提取 + - 点。 + - 边。 + - 曲线。 + - 面法向。 + - 孔位。 + - 焊缝。 + - 喷涂区域边界。 + +3. 路径生成 + - 曲线采样。 + - 等距采样。 + - 面法向姿态。 + - 切向姿态。 + - 路径平滑。 + - 接近/离开路径。 + - 自动避让偏移。 + +4. 数据保留 + - 每个 Path 应保留 CAD source ID。 + - 每个 Target 应保留来源曲线、采样序号、法向、切向。 + - CAD 更新后应能重建路径并保留用户覆盖参数。 + +### 2.4.3 可达性、碰撞和干涉验证 + +商业 OLP 的核心价值是提前发现问题。系统应设计以下验证层： + +1. 可达性检查 + - IK 是否有解。 + - 是否有多解。 + - 是否符合配置约束。 + - 是否接近关节限位。 + - 是否接近奇异点。 + +2. 碰撞检测 + - 机器人自身碰撞。 + - 机器人与工件碰撞。 + - 工具与夹具碰撞。 + - 工件搬运过程碰撞。 + - 多机器人碰撞。 + +3. 干涉区 + - 定义区域。 + - 机器人进入/离开事件。 + - 与 IO/互锁结合。 + - 多机器人共享区域互锁。 + +4. 节拍分析 + - 单路径时间。 + - 单 Operation 时间。 + - 程序总周期。 + - wait 消耗时间。 + - IO/PLC 响应时间。 + - 瓶颈识别。 + +MVP 可以先做可达性、限位、奇异点和基础包围盒碰撞；商业级再做网格级碰撞、 swept volume、干涉区和节拍优化。 + +### 2.4.4 虚拟调试与虚拟调试联调 + +商业虚拟调试通常不仅运行机器人程序，还验证 PLC、HMI、夹具、输送线和安全逻辑。Web 版本可分阶段实现： + +1. 内置逻辑仿真 + - IO 脚本。 + - 设备状态机。 + - 夹具/输送线/传感器仿真。 + +2. 外部协议联调 + - OPC UA。 + - MQTT。 + - WebSocket bridge。 + - 后续扩展 Modbus TCP、EtherNet/IP、Profinet 网关。 + +3. PLC 联调 + - 初期通过 WebSocket/OPC UA 接入外部模拟 PLC。 + - 商业级支持真实 PLC 或虚拟 PLC 的信号映射。 + - 所有外部信号进入统一 IO Service。 + +4. 时间线调试 + - Robot motion timeline。 + - IO timeline。 + - Wait timeline。 + - PLC event timeline。 + - Alarm timeline。 + +### 2.4.5 校准与 Sim-to-Real + +商业离线编程必须考虑仿真到现场的偏差： + +1. TCP 校准 + - 记录理论 TCP。 + - 支持现场测量 TCP 回填。 + - 对路径重新计算。 + +2. 工件坐标校准 + - 三点法/多点法。 + - 基准点拟合。 + - 工件偏移应用到 Path。 + +3. 机器人基座校准 + - 机器人相对工作站的位置修正。 + - 多机器人之间的基准统一。 + +4. 程序回读 + - 从现场控制器导回品牌程序。 + - 与离线版本比对。 + - 识别现场修改。 + +5. 补偿报告 + - TCP 偏差。 + - Frame 偏差。 + - Target 偏差。 + - 程序差异。 + +### 2.4.6 商业级报告 + +每次仿真和导出应能生成报告： + +| 报告 | 内容 | +| --- | --- | +| Reachability Report | 不可达点、接近限位点、IK 解 | +| Collision Report | 碰撞对象、时间、路径点、严重程度 | +| Cycle Time Report | 总节拍、路径节拍、wait 时间、瓶颈 | +| IO Report | IO 映射、wait、pulse、脚本触发 | +| Post Report | 品牌映射、近似处理、不支持项 | +| Calibration Report | TCP、Frame、Base 偏差 | +| Import Report | 品牌程序导入结果、丢失语义、恢复对象 | + +报告应可导出为 JSON 和 HTML。后续可增加 PDF。 + +## 3. 总体架构 + +系统分为 8 个核心层： + +```text +HTML UI + | +TypeScript App Shell + | +Project Service / OPFS Workspace + | +Program Parser / Semantic Analyzer / IR Compiler + | +Virtual Controller Runtime + | +Motion Planner / KDL WASM Adapter + | +Robot Model / Scene Model / IO Model + | +Post Processor: ABB RAPID / FANUC / KUKA KRL +``` + +### 3.1 主要模块 + +| 模块 | 职责 | 建议实现 | +| --- | --- | --- | +| UI Shell | 页面布局、编辑器、虚拟示教器、监控面板 | HTML + TypeScript | +| Workspace | 项目文件、索引、版本、导入导出 | OPFS + TypeScript | +| Station Model | 工作站、机器人、工具、工装、工件、设备对象 | TypeScript | +| Resource Library | 机器人库、工具库、夹具库、工艺模板库 | JSON + OPFS | +| Geometry Service | 几何导入、路径采样、碰撞几何 | TypeScript + Worker | +| Parser | 通用程序词法、语法解析 | TypeScript grammar-first parser | +| Semantic Analyzer | 类型检查、符号表、坐标系和目标点检查 | TypeScript | +| IR Compiler | AST 转可执行中间表示 | TypeScript | +| Virtual Controller | 程序执行、任务状态、变量、IO、报警、调试 | TypeScript | +| Motion Engine | 运动队列、插补、速度规划、过渡处理 | TypeScript + KDL WASM | +| KDL WASM Adapter | 正解、逆解、雅可比、轨迹基础计算 | C++ KDL 编译 WASM，TS 封装 | +| Validation Engine | 可达性、限位、奇异点、碰撞、节拍验证 | TypeScript + Worker | +| Post Processor | 通用 IR 转品牌程序 | TypeScript | +| Report Engine | 可达性、碰撞、节拍、IO、后处理报告 | TypeScript | +| Test Harness | 语法、语义、运动、后处理一致性测试 | TypeScript test runner | + +### 3.2 推荐线程模型 + +Web 主线程只负责 UI 交互和轻量状态更新。以下模块建议放入 Web Worker： + +1. KDL WASM 计算。 +2. 程序解析和语义检查。 +3. 长程序的虚拟执行和轨迹预计算。 +4. OPFS 大文件读写，尤其是使用同步访问句柄时。 +5. 碰撞检测和几何采样。 +6. 长路径节拍分析和报告生成。 + +推荐结构： + +```text +Main Thread + - UI + - editor + - controller panel + +Parser Worker + - tokenize + - parse + - semantic check + - diagnostics + +Controller Worker + - interpreter + - execution clock + - variable state + - IO simulation + - motion queue + +KDL Worker + - wasm initialization + - FK/IK/Jacobian + - trajectory sampling + +Storage Worker + - OPFS read/write + - project snapshot + - import/export +``` + +## 4. 技术要求 + +### 4.1 语言与平台 + +1. 虚拟控制器核心使用 TypeScript。 +2. Web 界面使用 HTML，样式可使用 CSS，交互逻辑使用 TypeScript。 +3. KDL 使用 C++ 编译为 WebAssembly，通过 TypeScript 调用。 +4. 项目文件使用 OPFS 持久化。 +5. 系统应能在现代 Chromium 系浏览器中稳定运行，后续再验证 Firefox、Safari 的兼容性。 + +### 4.2 数值与单位 + +系统内部统一单位： + +| 类型 | 内部单位 | +| --- | --- | +| 长度 | meter | +| 角度 | radian | +| 线速度 | meter/second | +| 角速度 | radian/second | +| 时间 | second | +| 质量 | kilogram | + +UI 和程序语言可以支持 `mm`、`deg`、`m/s`、`mm/s` 等显示和输入单位，但进入 IR 和 KDL 前必须规范化。 + +### 4.3 实时性要求 + +这是 Web 虚拟控制器，不追求真实伺服周期实时性，但需要可重复、可暂停、可回放： + +1. 仿真逻辑周期建议默认 `4 ms` 或 `8 ms`，可配置。 +2. UI 刷新周期不应绑定仿真周期，建议 `requestAnimationFrame` 渲染。 +3. 运动轨迹采样应支持固定步长采样，例如 `4 ms`、`8 ms`、`12 ms`。 +4. 同一输入项目、同一版本算法，应产生可重复的轨迹结果。 + +### 4.4 浏览器存储要求 + +1. 项目默认保存在 OPFS。 +2. 支持导入导出 zip 项目包。 +3. 支持自动保存、手动保存、快照、恢复。 +4. 存储结构必须有 schema version，便于后续迁移。 +5. 不能只依赖浏览器缓存，必须提供用户可导出的备份文件。 + +### 4.5 可测试性要求 + +1. 解析器必须有语法快照测试。 +2. 语义分析必须有错误诊断测试。 +3. 运动学必须有数值回归测试。 +4. 后处理必须有 golden file 测试。 +5. 虚拟控制器必须有程序执行状态机测试。 + +## 5. 通用机器人程序语言设计 + +通用机器人程序语言暂定名为 GRL，Generic Robot Language。名称可后续调整。 + +GRL 的目标不是复制某一家品牌语法，而是建立一个稳定的中间语言，能覆盖 ABB、FANUC、KUKA 的共同能力，并保留品牌扩展元数据。 + +### 5.1 对标对象 + +| 能力 | ABB RAPID | FANUC TP/KAREL | KUKA KRL | GRL 对应设计 | +| --- | --- | --- | --- | --- | +| 程序组织 | `MODULE`、`PROC`、`FUNC`、`TRAP` | TP 程序、`CALL`、标签；KAREL 程序 | `.src/.dat`、`DEF`、函数、数据文件 | `module`、`proc`、`func`、`trap` | +| 关节运动 | `MoveJ` | `J P[...]` | `PTP` | `movej` | +| 直线运动 | `MoveL` | `L P[...]` | `LIN` | `movel` | +| 圆弧运动 | `MoveC` | `C P[...]` | `CIRC` | `movec` | +| 目标点 | `robtarget`、`jointtarget` | `P[]`、`PR[]` | `E6POS`、`E6AXIS` | `PoseTarget`、`JointTarget` | +| 工具 | `tooldata` | Tool Frame / UTOOL | `$TOOL` | `Tool` | +| 工件/基坐标 | `wobjdata` | User Frame / UFRAME | `$BASE` | `Frame` | +| 速度 | `speeddata` | 百分比、`mm/sec` 等 | `$VEL`、`$ACC` | `Speed` | +| 过渡 | `zonedata`、`fine` | `FINE`、`CNT` | `C_DIS`、`C_PTP`、`APO` | `Zone` | +| IO | `SetDO`、`WaitDI` | `DO[]`、`DI[]`、`WAIT` | `$OUT[]`、`$IN[]`、`WAIT FOR` | `io.write`、`wait` | +| 条件 | `IF`、`TEST` | `IF`、`SELECT`、`LBL/JMP` | `IF`、`SWITCH`、`LOOP` | `if`、`switch`、`while`、`for`、`label` | +| 中断 | `TRAP`、interrupt | 条件监控和后台逻辑 | `INTERRUPT`、`BRAKE`、`RESUME` | `interrupt`、`trap` | +| 错误处理 | `ERROR`、`RAISE`、`UNDO` | Alarm、异常处理依版本而异 | `HALT`、`RESUME`、消息/中断 | `try`、`catch`、`raise`、`alarm` | + +### 5.1.1 对标商业 OLP 的中性程序风格 + +商业 OLP 软件生成程序时，一般具有以下特征： + +1. 目标点和运动语句分离 + + 目标点作为数据保存，程序中通过名称或编号引用。这样便于点位重算、批量修改、路径优化和后处理。 + +2. 路径和程序流程分离 + + 路径是可编辑对象，程序流程负责调用路径、控制 IO、处理条件。这样便于从 CAD 曲线、规划点、示教点生成路径，再自动生成程序。 + +3. 运动参数显式 + + 每条运动或路径段都能明确给出运动类型、速度、过渡、工具、坐标系。默认值可存在，但展开到 IR 时必须解析为确定值。 + +4. 工艺语义保留 + + 焊接、喷涂、打磨、搬运等不应只变成一串 MoveL。通用程序需要保留 operation 类型、工艺参数和路径引用，后处理器才能生成品牌特定工艺指令或注释。 + +5. 可展开、可回写 + + `run_path weld_path` 可以展开为多条 `movel`/`movec`，也可以后处理为品牌程序。反过来，导入品牌程序时也应尽量恢复为 `path` 和 `target`。 + +因此，GRL 应同时支持两种写法： + +1. 面向人类调试的显式运动语句。 +2. 面向自动规划和后处理的路径/操作对象语句。 + +### 5.2 程序工程结构 + +推荐一个项目包含如下文件： + +```text +project.json +robots/ + robot_1.robot.json + tool_gripper.tool.json + frame_fixture.frame.json +programs/ + main.grl + weld.grl +targets/ + main.targets.json +paths/ + weld_path.path.json +operations/ + op_pick_place.operation.json +io/ + io_map.json +post/ + abb.profile.json + fanuc.profile.json + kuka.profile.json +generated/ + abb/ + fanuc/ + kuka/ +logs/ + controller.log + simulation.trace.jsonl +``` + +### 5.3 GRL 文件结构 + +示例： + +```text +module Main + + persistent tool gripper = tool { + tcp: pose(0 mm, 0 mm, 180 mm, 0 deg, 0 deg, 0 deg), + mass: 2.5 kg + } + + persistent frame fixture = frame { + origin: pose(800 mm, 0 mm, 200 mm, 0 deg, 0 deg, 0 deg) + } + + target home = joint_target { + joints: [0 deg, -30 deg, 60 deg, 0 deg, 60 deg, 0 deg] + } + + target pick = pose_target { + pose: pose(500 mm, 120 mm, 300 mm, 180 deg, 0 deg, 90 deg), + config: robot_config(0, 0, 1), + tool: gripper, + frame: fixture + } + + path pick_path { + defaults { + tool: gripper, + frame: fixture, + speed: linear(300 mm/s), + zone: z10 + } + + point approach movej target home speed joint(50%) zone fine + point p1 movel target pick offset z 100 mm + point p2 movel target pick zone fine + } + + operation pick_place { + kind: handling + path: pick_path + before p2: + io.do[1] = true + after p2: + wait io.di[1] == true timeout 2 s + } + + proc main() + set_tool gripper + set_frame fixture + + run_path pick_path + + wait io.di[1] == true timeout 2 s + io.do[2] = true + + call place() + end + + proc place() + var pose_target p2 = pick offset z 100 mm + movel p2 speed linear(200 mm/s) zone fine + end + +end +``` + +该示例体现两种程序形态： + +1. `path pick_path` 适合由规划点、CAD 曲线、工艺模板自动生成。 +2. `proc main()` 适合虚拟调试和流程控制。 + +### 5.4 顶层语法要求 + +GRL 至少支持以下顶层结构： + +1. `module`：程序模块。 +2. `import`：导入其他模块。 +3. `persistent`：持久变量，类似 ABB `PERS`、KUKA `.dat` 中的持久数据、FANUC 寄存器/位置数据。 +4. `const`：常量。 +5. `var`：局部变量。 +6. `target`：目标点。 +7. `proc`：无返回值过程。 +8. `func`：有返回值函数。 +9. `trap`：中断处理例程。 +10. `task`：多任务或后台任务定义，初期可只设计语义，不立即完整实现。 +11. `path`：路径对象，由路径点、运动类型、姿态策略和工艺参数组成。 +12. `operation`：工艺操作对象，引用 path 并绑定工艺行为。 +13. `post_hint`：后处理提示，用于声明品牌输出偏好。 + +### 5.4.1 Path 语法 + +路径对象用于承载规划点和规划路径，是连接“自动路径规划”和“机器人程序生成”的核心结构。 + +```text +path weld_seam_01 { + defaults { + tool: weld_gun, + frame: part_frame, + speed: linear(120 mm/s), + zone: z5, + posture: follow_tangent, + blend: continuous + } + + point p_start movej target t_start speed joint(30%) zone fine + point p001 movel target t001 + point p002 movel target t002 + point p003 movec via t_mid target t003 + event before p001 io.do[10] = true + event after p003 io.do[10] = false +} +``` + +Path 语法要求： + +1. 每个 `path` 有唯一名称。 +2. `defaults` 定义默认工具、坐标系、速度、过渡、姿态策略。 +3. `point` 定义路径点，可引用已有 target，也可内联 pose。 +4. `event before/after` 用于绑定路径点附近的 IO 或工艺动作。 +5. Path 可以在程序中通过 `run_path` 执行。 +6. Path 可以在编译阶段展开为运动 IR。 +7. Path 可以在后处理阶段展开为品牌运动指令和数据点。 + +### 5.4.2 Operation 语法 + +Operation 表达工艺语义，比 path 更高一层： + +```text +operation weld_op_01 { + kind: arc_welding + path: weld_seam_01 + process { + weld_id: "WELD_1" + voltage: 24.0 + current: 180.0 + weave: none + } + start_action: + io.do[20] = true + end_action: + io.do[20] = false +} +``` + +Operation 用途： + +1. 保存工艺参数。 +2. 驱动仿真时的工艺状态显示。 +3. 生成品牌程序时映射到对应品牌的工艺包、IO 或注释。 +4. 支持未来工艺库扩展。 + +首版 Operation 类型： + +| 类型 | 用途 | +| --- | --- | +| `handling` | 搬运、上下料、抓取放置 | +| `arc_welding` | 弧焊 | +| `spot_welding` | 点焊 | +| `dispensing` | 涂胶 | +| `spraying` | 喷涂 | +| `grinding` | 打磨 | +| `generic_path` | 通用路径 | + +### 5.4.3 自动路径生成到 GRL 的要求 + +从规划点和规划路径生成 GRL 时，应遵循以下规则： + +1. 每个规划点生成稳定 target 名称，例如 `P001`、`P002`，或基于工艺语义命名。 +2. 路径整体生成 `path` 对象，而不是直接散落在 `proc main()` 中。 +3. 路径默认参数写入 `defaults`，单点差异写在对应 `point` 上。 +4. 接近点、离开点、工艺开始点、工艺结束点必须明确标注。 +5. IO、焊接开关、夹爪动作等应生成 `event before/after` 或 Operation action。 +6. 若路径来自 CAD 曲线，应保留来源引用，例如曲线 ID、采样距离、法向策略。 +7. 编译器应能把 path 展开为确定的运动 IR，保证虚拟控制器不依赖 UI 对象也能运行。 + +示例： + +```text +path curve_032_generated { + source { + type: cad_curve + id: "edge_032" + sample_distance: 5 mm + normal_strategy: surface_normal + } + defaults { + tool: spray_gun + frame: part_frame + speed: linear(500 mm/s) + zone: z20 + posture: follow_normal + } + point p000 movel target T_curve_032_000 zone fine + point p001 movel target T_curve_032_001 + point p002 movel target T_curve_032_002 +} +``` + +### 5.4.4 `run_path` 和 `run_operation` + +程序流程中建议优先调用路径或操作： + +```text +proc main() + run_operation weld_op_01 + run_path retract_path +end +``` + +编译语义： + +1. `run_path path_name` 展开为 path 中的 point 和 event。 +2. `run_operation op_name` 展开为 start action、path、end action。 +3. 展开后的 IR 保留 source mapping，调试时仍能定位到 path 点和 operation。 +4. 后处理器可选择直接展开，也可以按品牌能力生成更紧凑的结构。 + +### 5.4.5 显式运动和路径调用的关系 + +GRL 同时允许： + +```text +movel pick speed linear(300 mm/s) zone z10 +run_path pick_path +``` + +约束： + +1. 手写程序、调试程序、短逻辑程序可直接写 `movej/movel/movec`。 +2. 自动规划、CAD-to-path、批量目标点应生成 `path`。 +3. 后处理器对两者生成的品牌运动语句应一致。 +4. 虚拟控制器执行时二者最终都进入同一 Motion IR。 + +### 5.5 类型系统 + +基础类型： + +| 类型 | 说明 | +| --- | --- | +| `bool` | 布尔 | +| `int` | 整数 | +| `real` | 浮点数 | +| `string` | 字符串 | +| `time` | 时间 | +| `length` | 长度 | +| `angle` | 角度 | + +机器人类型： + +| 类型 | 说明 | +| --- | --- | +| `Pose` | 位置和姿态 | +| `JointArray` | 关节角数组 | +| `PoseTarget` | 笛卡尔目标点 | +| `JointTarget` | 关节目标点 | +| `Tool` | 工具数据 | +| `Frame` | 基坐标/工件坐标 | +| `Speed` | 速度数据 | +| `Zone` | 过渡/逼近数据 | +| `Load` | 负载数据 | +| `RobotConfig` | 姿态配置，如肩、肘、腕配置 | +| `ExtAxis` | 外部轴数据 | +| `Path` | 路径对象 | +| `PathPoint` | 路径点 | +| `Operation` | 工艺操作 | +| `ProcessParam` | 工艺参数 | + +### 5.6 运动指令 + +必须支持： + +```text +movej target speed Speed zone Zone [tool Tool] [frame Frame] +movel target speed Speed zone Zone [tool Tool] [frame Frame] +movec via target speed Speed zone Zone [tool Tool] [frame Frame] +stop_motion +hold +resume +run_path path_name +run_operation operation_name +``` + +运动语义要求： + +1. `movej` 表示关节空间运动：机器人按各轴关节角度差分运行，关节同起同停，TCP 轨迹不要求是直线。 +2. `movel` 表示 TCP 直线运动：TCP 沿起点到终点的空间直线运行，关节角由每个采样点 IK 求解。 +3. `movec` 表示 TCP 圆弧运动：TCP 经过 via 点并沿圆弧运行，关节角由每个圆弧采样点 IK 求解。 +4. `zone fine` 表示精确到点，不做过渡。 +5. `zone z10` 表示允许在目标点附近按半径或品牌等价参数过渡。 +6. 每条运动必须绑定当前 `tool`、当前 `frame`、速度、过渡方式。 +7. 如果目标点本身指定了 `tool` 或 `frame`，需要定义优先级。建议：指令显式参数最高，其次目标点属性，最后控制器当前状态。 + +### 5.7 逻辑与流程控制 + +必须支持： + +```text +if condition + ... +elseif condition + ... +else + ... +end + +while condition + ... +end + +for i = 0 to 10 + ... +end + +switch value + case 1 + ... + default + ... +end + +label retry +jump retry +call sub() +return +``` + +FANUC TP 程序常见标签跳转模型必须能映射到 GRL，但 GRL 新程序建议优先使用结构化控制流。 + +### 5.8 IO 与等待 + +示例： + +```text +io.do[1] = true +io.go[1] = 16 +wait io.di[1] == true +wait io.ai[2] > 3.5 timeout 1.5 s +pulse io.do[3] duration 200 ms +wait rising(io.di[4]) timeout 5 s +wait all(io.di[1] == true, io.di[2] == false) +wait any(io.di[10] == true, timer.done("T_PICK")) +``` + +要求： + +1. 支持 DI、DO、AI、AO、GI、GO。 +2. 支持别名，例如 `clamp_closed` 映射到 `di[5]`。 +3. 支持等待超时。 +4. 支持虚拟 IO 脚本和手动面板输入。 +5. 后处理时映射到 ABB、FANUC、KUKA 对应 IO 表达。 + +### 5.8.1 IO 点类型 + +通用 IO 类型： + +| 类型 | 说明 | 常见品牌映射 | +| --- | --- | --- | +| `DI` | 数字输入 | ABB `DI`、FANUC `DI[]`、KUKA `$IN[]` | +| `DO` | 数字输出 | ABB `DO`、FANUC `DO[]`、KUKA `$OUT[]` | +| `AI` | 模拟输入 | ABB `AI`、FANUC `AI[]`、KUKA analog input | +| `AO` | 模拟输出 | ABB `AO`、FANUC `AO[]`、KUKA analog output | +| `GI` | 组输入，整数 | FANUC `GI[]`、品牌寄存器组合 | +| `GO` | 组输出，整数 | FANUC `GO[]`、品牌寄存器组合 | +| `RI` | 机器人输入，可选 | FANUC `RI[]`，初期可映射为 DI | +| `RO` | 机器人输出，可选 | FANUC `RO[]`，初期可映射为 DO | + +推荐 GRL IO 访问形式： + +```text +io.di[1] +io.do[2] +io.ai[1] +io.ao[1] +io.gi[1] +io.go[1] +io.alias.clamp_closed +``` + +其中 `io.alias.xxx` 由 IO map 映射到实际点位，便于后处理和跨品牌迁移。 + +### 5.8.2 IO 映射文件 + +项目应包含 `io/io_map.json`： + +```json +{ + "schemaVersion": 1, + "signals": [ + { + "name": "clamp_close_cmd", + "type": "DO", + "index": 1, + "initial": false, + "description": "Close clamp command", + "brand": { + "abb": "doClampClose", + "fanuc": "DO[1]", + "kuka": "$OUT[1]" + } + }, + { + "name": "clamp_closed", + "type": "DI", + "index": 1, + "initial": false, + "description": "Clamp closed sensor", + "brand": { + "abb": "diClampClosed", + "fanuc": "DI[1]", + "kuka": "$IN[1]" + } + } + ], + "groups": [ + { + "name": "part_id", + "type": "GI", + "index": 1, + "bits": ["DI[10]", "DI[11]", "DI[12]", "DI[13]"], + "initial": 0 + } + ] +} +``` + +要求： + +1. `name` 在项目内唯一。 +2. `type + index` 组合唯一。 +3. `initial` 定义仿真启动默认值。 +4. `brand` 定义后处理映射。 +5. `groups` 定义组信号与 bit 信号的关系。 + +### 5.8.3 Wait 语义 + +`wait` 是虚拟调试的关键指令，必须可暂停、可恢复、可超时、可诊断。 + +语法： + +```text +wait condition [timeout duration] [on_timeout label_or_proc] +``` + +示例： + +```text +wait io.di[1] == true +wait io.alias.clamp_closed == true timeout 2 s +wait io.ai[1] >= 3.5 timeout 500 ms on_timeout clamp_timeout +wait rising(io.di[4]) timeout 5 s +wait falling(io.di[5]) +wait changed(io.gi[1]) +wait all(io.di[1], !io.di[2], io.gi[1] == 7) +wait any(io.di[10], io.di[11]) +``` + +执行要求： + +1. 如果条件当前为真，`wait` 立即完成。 +2. 如果条件当前为假，虚拟控制器进入 `Waiting` 子状态，但控制器总状态仍可显示为 `Running/Waiting`。 +3. 等待期间程序计数器停留在 wait 指令。 +4. 等待期间运动队列默认应已停止在上一条同步点；如果允许后台运动与等待并行，必须显式设计异步语义，首版不建议支持。 +5. 超时后产生结构化报警或跳转到 `on_timeout`。 +6. 用户在 IO 面板手动改变输入时，wait 应在下一个 controller tick 被重新评估。 +7. `hold` 暂停时，wait 的超时计时器也暂停；`resume` 后继续计时。 +8. `stop` 停止时，wait 被取消。 +9. trace 中必须记录 wait 开始、完成、超时、取消。 + +### 5.8.4 Wait 条件表达式限制 + +为了保证可预测性，首版 wait 条件建议只允许： + +1. IO 读值。 +2. 变量读值。 +3. 常量。 +4. 比较运算：`==`、`!=`、`>`、`>=`、`<`、`<=`。 +5. 逻辑运算：`and`、`or`、`not`。 +6. 边沿函数：`rising()`、`falling()`、`changed()`。 +7. 组合函数：`all()`、`any()`。 +8. 定时器状态，例如 `timer.done("T1")`。 + +不建议在 wait 条件中允许： + +1. 修改变量。 +2. 调用可能有副作用的函数。 +3. 调用 KDL 运动学计算。 +4. 文件读写。 +5. 网络请求。 + +### 5.8.5 IO 脚本 + +为了虚拟调试，系统需要支持 IO 脚本模拟外部夹具、PLC、传感器。 + +示例： + +```text +io_script clamp_sim { + when io.do[1] == true delay 300 ms: + io.di[1] = true + + when io.do[1] == false delay 200 ms: + io.di[1] = false +} +``` + +用途： + +1. 模拟夹爪闭合反馈。 +2. 模拟传感器到位信号。 +3. 模拟 PLC 对机器人 DO 的响应。 +4. 自动完成测试，不需要人工点 IO。 + +执行要求： + +1. IO 脚本运行在 Controller Worker 或独立 IO Worker。 +2. IO 脚本只能修改被标记为 virtual/simulated 的输入信号，避免混淆真实输出。 +3. IO 脚本触发和写入必须进入 IO event log。 +4. IO 脚本可以启停。 +5. IO 脚本必须可重置。 + +### 5.8.6 Pulse 语义 + +`pulse` 用于输出一个短脉冲： + +```text +pulse io.do[3] duration 200 ms +``` + +执行语义： + +1. 立即将 DO 置为 true。 +2. 注册一个定时事件，在 duration 后置回 false。 +3. 如果程序被 hold，pulse 定时器是否暂停需要可配置。建议首版随控制器虚拟时间暂停。 +4. 如果 stop，未完成 pulse 应恢复为安全默认值，通常为 false。 +5. trace 记录 pulse start 和 pulse end。 + +### 5.8.7 IO 与 Wait 的品牌映射 + +后处理时必须将 GRL IO 和 wait 语义转换到品牌语法： + +| GRL | ABB RAPID | FANUC LS/TP 风格 | KUKA KRL | +| --- | --- | --- | --- | +| `io.do[1] = true` | `SetDO do1, 1;` | `DO[1]=ON ;` | `$OUT[1]=TRUE` | +| `io.do[1] = false` | `SetDO do1, 0;` | `DO[1]=OFF ;` | `$OUT[1]=FALSE` | +| `wait io.di[1] == true` | `WaitDI di1, 1;` 或 `WaitUntil di1=1;` | `WAIT DI[1]=ON ;` | `WAIT FOR $IN[1]` | +| `wait ... timeout 2 s` | 品牌支持时使用超时语义，否则生成计时循环 | 生成 timer/register 逻辑或报警跳转 | 生成计时逻辑或 `WAIT FOR` 近似 | +| `pulse io.do[3] duration 200 ms` | `PulseDO do3, 0.2;` 或展开 | `PULSE DO[3]` 或展开 | `$OUT[3]=TRUE; WAIT SEC 0.2; $OUT[3]=FALSE` | + +要求： + +1. 简单 IO 写入必须直接映射为品牌原生 IO。 +2. 简单 wait 必须直接映射为品牌原生等待。 +3. 复杂 wait 表达式如果目标品牌不支持，应展开为条件循环、计时器和报警逻辑。 +4. 边沿 wait 如果品牌不支持，应生成前值缓存逻辑或给出不支持诊断。 +5. 后处理报告必须说明哪些 wait/pulse 被原生映射，哪些被展开或近似处理。 + +### 5.9 中断与异常 + +通用语义： + +```text +interrupt clamp_lost when io.di[5] == false do trap_clamp_lost + +trap trap_clamp_lost() + stop_motion + alarm 1001 "Clamp lost" +end +``` + +初期实现建议： + +1. 支持条件中断注册。 +2. 支持中断触发后暂停当前程序。 +3. 支持报警产生。 +4. 暂不模拟真实品牌控制器全部中断细节，但 IR 中保留中断元数据，便于后处理。 + +### 5.10 品牌扩展 + +GRL 需要允许品牌扩展属性，不能为了通用性丢失信息： + +```text +@brand.abb { + conf_l: true +} + +@brand.kuka { + advance: 3 +} + +@brand.fanuc { + group: 1 +} +``` + +扩展属性只影响对应品牌后处理，不应破坏通用仿真。 + +## 6. 解析器与编译流程 + +### 6.1 编译管线 + +```text +Source Text + -> Lexer + -> Parser + -> AST + -> Symbol Table + -> Semantic Analyzer + -> IR + -> Virtual Controller Runtime + -> Post Processor +``` + +完整商业 OLP 管线建议扩展为： + +```text +OLP Object Model + - robots + - tools + - frames + - targets + - paths + - operations + | + v +GRL Source + | + v +GRL AST + | + v +Executable IR + | + +--> Virtual Controller Runtime + | + +--> Post Processor + +--> ABB RAPID + +--> FANUC LS/TP-compatible text + +--> KUKA KRL SRC/DAT +``` + +品牌程序导入管线： + +```text +ABB/FANUC/KUKA Source + -> Brand Lexer + -> Brand Parser + -> Brand AST + -> Brand Semantic Normalizer + -> Executable IR + -> Reconstructed GRL / OLP Object Model + -> Virtual Controller Runtime +``` + +### 6.2 AST 要求 + +AST 必须保留： + +1. 源文件路径。 +2. 起止行列号。 +3. 注释位置，便于格式化和后处理保留注释。 +4. 原始单位文本和规范化数值。 +5. 品牌扩展元数据。 + +### 6.2.1 三类 AST + +系统需要区分三类 AST： + +1. GRL AST + - 表示通用机器人程序。 + - 用于语义检查、格式化、诊断和编译 IR。 + +2. Brand AST + - 表示 ABB RAPID、FANUC LS、KUKA KRL 等品牌源程序。 + - 尽量忠实保留品牌语法和源代码位置。 + - 用于品牌程序导入、诊断、转换报告。 + +3. OLP Model AST + - 表示工作站对象、路径、操作、工艺参数。 + - 可来自 UI 创建、CAD 生成、GRL 解析或品牌程序反解析。 + +不要用一个 AST 同时承担三种职责，否则后续导入导出和路径重算会变得难维护。 + +### 6.3 语义检查 + +必须检查： + +1. 变量是否声明。 +2. 目标点类型是否匹配运动指令。 +3. 工具、坐标系是否存在。 +4. 单位是否正确。 +5. IO 地址是否越界。 +6. 子程序参数数量和类型是否匹配。 +7. 运动指令是否缺失速度或过渡参数。 +8. 圆弧运动中起点、过渡点、终点是否退化。 +9. 逆解是否有可行解。 +10. 关节是否超过软限位。 +11. 是否存在不可到达目标点。 +12. 是否存在未处理的后处理限制。 +13. Path 是否存在空路径、重复点名、非法事件绑定。 +14. Operation 是否引用不存在的 path。 +15. 自动生成路径是否保留必要的 source metadata。 +16. 品牌程序导入后是否存在无法恢复的语义。 + +### 6.4 IR 设计 + +IR 不应是品牌文本，而应是可执行指令对象： + +```ts +type IrInstruction = + | MotionInstruction + | AssignInstruction + | WaitInstruction + | CallInstruction + | BranchInstruction + | IoInstruction + | AlarmInstruction + | ReturnInstruction; +``` + +运动 IR 示例： + +```ts +interface MotionInstruction { + kind: "motion"; + motionType: "joint" | "linear" | "circular"; + target: TargetRef | ResolvedTarget; + via?: TargetRef | ResolvedTarget; + speed: SpeedSpec; + zone: ZoneSpec; + tool: ToolRef; + frame: FrameRef; + sourceRange: SourceRange; + brandMeta?: Record; +} +``` + +### 6.5 OLP 对象模型 + +商业离线编程软件通常以对象树组织程序，而不是只以文本文件组织。建议内部定义 OLP Object Model： + +```ts +interface OlpProjectModel { + robots: RobotModel[]; + tools: ToolModel[]; + frames: FrameModel[]; + targets: TargetModel[]; + paths: PathModel[]; + operations: OperationModel[]; + programs: ProgramModel[]; + postProfiles: PostProfile[]; +} + +interface PathModel { + id: string; + name: string; + robotId: string; + defaultToolId?: string; + defaultFrameId?: string; + defaultSpeed?: SpeedSpec; + defaultZone?: ZoneSpec; + postureStrategy?: PostureStrategy; + source?: PathSource; + points: PathPointModel[]; + events: PathEventModel[]; +} + +interface PathPointModel { + id: string; + name: string; + motionType: "joint" | "linear" | "circular"; + targetId?: string; + target?: ResolvedTarget; + viaTargetId?: string; + speed?: SpeedSpec; + zone?: ZoneSpec; + toolId?: string; + frameId?: string; + process?: Record; +} +``` + +该模型的用途： + +1. UI 文件树和路径编辑器直接操作它。 +2. 从规划点生成程序时先生成它。 +3. GRL 编译前可由它生成 GRL。 +4. 品牌程序导入后可尽量恢复它。 +5. 后处理和虚拟控制器最终仍以 IR 为准。 + +### 6.6 品牌程序导入解析 + +虚拟控制器应能解析多品牌机器人的可读程序文本，并统一执行。首版建议支持以下输入： + +| 品牌 | 首版导入格式 | 说明 | +| --- | --- | --- | +| ABB | RAPID `.mod/.sys` 文本 | 解析 `MODULE`、`PROC`、`MoveJ`、`MoveL`、`MoveC`、`robtarget`、`tooldata`、`wobjdata` | +| KUKA | KRL `.src/.dat` 文本 | 解析 `DEF`、`PTP`、`LIN`、`CIRC`、`E6POS`、`E6AXIS`、`$TOOL`、`$BASE` | +| FANUC | LS 风格文本或可读 TP 导出 | 解析 `J`、`L`、`C`、`P[]`、`PR[]`、`UTOOL`、`UFRAME`、`CALL`、`LBL/JMP` | + +导入目标不是 100% 还原真实控制器所有行为，而是： + +1. 能提取目标点。 +2. 能提取运动序列。 +3. 能提取工具和坐标系引用。 +4. 能提取速度和过渡。 +5. 能提取主要 IO 和流程控制。 +6. 能转换为统一 IR，在虚拟控制器中执行。 +7. 能生成转换报告，标明不支持或近似处理的语义。 + +### 6.6.1 品牌导入转换示例 + +ABB RAPID： + +```text +MoveL pick, v300, z10, gripper\WObj:=fixture; +``` + +转换为 GRL： + +```text +movel pick speed linear(300 mm/s) zone z10 tool gripper frame fixture +``` + +KUKA KRL： + +```text +$VEL.CP = 0.3 +LIN XPICK C_DIS +``` + +转换为 GRL： + +```text +movel XPICK speed linear(300 mm/s) zone continuous +``` + +FANUC LS： + +```text +L P[10] 300mm/sec CNT10 ; +``` + +转换为 GRL： + +```text +movel P10 speed linear(300 mm/s) zone cnt(10) +``` + +### 6.6.2 品牌程序导入限制 + +必须明确处理以下限制： + +1. 品牌控制器内部系统变量不能全部等价映射。 +2. 品牌工艺包指令可能需要插件解析。 +3. FANUC 二进制 TP 不能直接作为首版目标，优先处理 LS 或文本导出。 +4. KUKA `$ADVANCE`、逼近、异步运动等语义需要近似或专项实现。 +5. ABB RAPID 的复杂错误处理和多任务需要分阶段支持。 +6. 导入后的程序必须附带转换报告，不能假装完全等价。 + +### 6.7 GRL 生成器 + +从 OLP Model 生成 GRL 的组件称为 GRL Generator： + +```text +OlpProjectModel + -> GrlGenerator + -> .grl source files +``` + +要求： + +1. 生成稳定、可读、可 diff 的文本。 +2. 保留路径和操作结构，不要默认全部展开成散乱运动语句。 +3. 生成的 target 名称稳定。 +4. 生成的程序可以再次解析回同等 OLP Model。 +5. 支持配置生成风格： + - compact：多用 `run_path`。 + - expanded：展开为 `movej/movel/movec`。 + - debug：保留更多注释和 source metadata。 + +## 7. 虚拟控制器设计 + +虚拟控制器需要支持两类输入： + +1. GRL 程序 + - 由用户手写。 + - 由 OLP 对象模型自动生成。 + - 由路径和操作模板生成。 + +2. 品牌程序 + - ABB RAPID 文本。 + - KUKA KRL 文本。 + - FANUC LS 或可读导出文本。 + - 先解析为 Brand AST，再转换为统一 IR。 + +虚拟控制器真正执行的是统一 IR，而不是直接解释某个品牌文本。这样才能保证虚拟调试、路径重算、跨品牌后处理和报警诊断使用同一套运行时。 + +### 7.1 控制器状态机 + +建议状态： + +```text +PowerOff + -> Booting + -> MotorsOff + -> Ready + -> Manual + -> Auto + -> Running + -> Hold + -> Fault + -> EmergencyStop +``` + +基础命令： + +| 命令 | 说明 | +| --- | --- | +| `powerOn` | 上电 | +| `powerOff` | 下电 | +| `motorsOn` | 电机上使能 | +| `motorsOff` | 电机下使能 | +| `loadProgram` | 加载程序 | +| `start` | 启动 | +| `hold` | 暂停 | +| `resume` | 继续 | +| `stop` | 停止 | +| `resetFault` | 复位报警 | +| `stepInto` | 单步进入 | +| `stepOver` | 单步越过 | +| `stepMotion` | 单条运动执行 | + +### 7.2 执行模型 + +虚拟控制器包含： + +1. Program Counter：当前执行位置。 +2. Call Stack：调用栈。 +3. Scope Stack：变量作用域。 +4. Motion Queue：运动队列。 +5. IO Image：IO 镜像。 +6. Timer Table：定时器。 +7. Interrupt Table：中断表。 +8. Alarm Queue：报警队列。 +9. Trace Buffer：执行轨迹。 +10. Source Map：IR 到 GRL、Path、Operation 或品牌源程序的映射。 +11. Brand Context：当输入来自品牌程序时，保存品牌语义上下文和转换警告。 +12. IO Service：IO 点表、别名表、事件队列、边沿检测、等待条件调度。 + +### 7.3 任务模型 + +初期支持单主任务： + +```text +Task MAIN + - program: Main.main + - motion group: robot_1 +``` + +后续扩展： + +1. 后台任务，例如 PLC-like 逻辑。 +2. 多机器人任务。 +3. 独立 IO 任务。 +4. 监控任务。 +5. 协作运动任务。 + +### 7.4 运动队列 + +虚拟控制器不能执行一条运动就结束，而要模拟真实控制器的 look-ahead： + +1. 程序解释器将运动指令推入 Motion Queue。 +2. Motion Planner 根据速度、过渡、当前姿态生成轨迹。 +3. Controller Tick 按仿真时间推进。 +4. UI 读取当前关节、TCP、目标点、轨迹和状态。 + +运动队列字段： + +```ts +interface MotionQueueItem { + id: string; + instructionId: string; + pathId?: string; + pathPointId?: string; + operationId?: string; + type: "joint" | "linear" | "circular"; + startJoint: number[]; + endJoint: number[]; + startPose: Pose; + endPose: Pose; + viaPose?: Pose; + speed: SpeedSpec; + zone: ZoneSpec; + samples?: TrajectorySample[]; + status: "pending" | "planned" | "running" | "done" | "failed"; +} +``` + +当运动来自 `path` 或 `operation` 时，运动队列必须保留路径点来源。UI 才能在路径表、程序文本、3D 轨迹之间联动定位。 + +### 7.4.1 Path 执行语义 + +`run_path` 执行过程： + +1. 读取 PathModel。 +2. 合并 path defaults 和 point overrides。 +3. 解析每个目标点的 tool、frame、speed、zone。 +4. 触发 `event before`。 +5. 将运动点展开为 MotionQueueItem。 +6. 运动完成后触发 `event after`。 +7. 记录 trace，包含 pathId 和 pathPointId。 + +`run_operation` 执行过程： + +1. 读取 OperationModel。 +2. 执行 start action。 +3. 执行引用 path。 +4. 执行 end action。 +5. 将工艺状态写入 trace。 + +### 7.4.2 品牌程序执行语义 + +品牌程序导入后执行过程： + +1. Brand AST 转统一 IR。 +2. 每条 IR 保留品牌源程序行号。 +3. 品牌特有语义被转换为： + - 等价 IR。 + - 近似 IR。 + - 不支持诊断。 +4. 虚拟控制器执行 IR。 +5. UI 调试时可以显示品牌源代码当前行，也可以显示转换后的 GRL/IR。 + +示例： + +```text +KUKA LIN XPICK C_DIS + -> Brand AST + -> MotionInstruction(linear, target=XPICK, zone=continuous) + -> MotionQueueItem +``` + +### 7.5 报警与诊断 + +报警必须结构化： + +```ts +interface ControllerAlarm { + code: number; + severity: "info" | "warning" | "error" | "fatal"; + message: string; + sourceRange?: SourceRange; + timestamp: number; + detail?: unknown; +} +``` + +常见报警： + +1. 程序语法错误。 +2. 类型错误。 +3. 未定义目标点。 +4. IK 求解失败。 +5. 目标点超限。 +6. 奇异点风险。 +7. 圆弧退化。 +8. IO 等待超时。 +9. 后处理不支持某指令。 +10. 品牌程序导入语义不完整。 +11. Path 展开失败。 +12. Operation 工艺参数缺失。 + +### 7.6 虚拟 IO 实施方案 + +虚拟 IO 是虚拟调试的核心能力。它用于模拟机器人控制器与夹具、PLC、安全门、传感器、工艺设备之间的信号交互。 + +#### 7.6.1 IO 架构 + +```text +Controller Runtime + | + +-- IO Service + | + +-- IO Image + +-- IO Alias Table + +-- IO Event Queue + +-- Wait Registry + +-- Edge Detector + +-- IO Script Engine + +-- IO Trace Logger +``` + +IO Service 职责： + +1. 保存当前 IO 镜像。 +2. 提供读写接口。 +3. 管理 IO 别名。 +4. 检测边沿和变化。 +5. 调度 wait 条件。 +6. 执行 pulse 定时恢复。 +7. 执行 IO 脚本。 +8. 记录 IO 事件日志。 +9. 向 UI 推送 IO 变化。 + +#### 7.6.2 IO Image 数据结构 + +```ts +type IoSignalType = "DI" | "DO" | "AI" | "AO" | "GI" | "GO" | "RI" | "RO"; + +interface IoSignalDef { + id: string; + name: string; + type: IoSignalType; + index: number; + initial: boolean | number; + writableByProgram: boolean; + writableByUser: boolean; + writableByScript: boolean; + description?: string; + brand?: Record; +} + +interface IoSignalState { + id: string; + type: IoSignalType; + index: number; + value: boolean | number; + previousValue: boolean | number; + updatedAt: number; + source: "program" | "user" | "script" | "import" | "reset"; +} + +interface IoImage { + signals: Map; + byAddress: Map; + aliases: Map; +} +``` + +地址规范： + +```text +DI[1] -> io.di[1] +DO[1] -> io.do[1] +AI[1] -> io.ai[1] +AO[1] -> io.ao[1] +GI[1] -> io.gi[1] +GO[1] -> io.go[1] +``` + +#### 7.6.3 IO 读写规则 + +1. 程序可写 DO、AO、GO、RO。 +2. 程序默认不可写 DI、AI、GI、RI，除非该信号配置为 simulated writable。 +3. 用户可在 IO 面板手动修改虚拟输入 DI、AI、GI、RI。 +4. IO 脚本可修改配置允许的虚拟输入。 +5. 所有写入必须经过 IO Service，不能直接改 Map。 +6. 每次写入都生成 IoEvent。 +7. 重复写入相同值可配置是否记录。建议默认记录程序写入，但 UI 可折叠显示。 + +```ts +interface IoWriteRequest { + addressOrAlias: string; + value: boolean | number; + source: "program" | "user" | "script" | "import" | "reset"; + timestamp: number; + instructionId?: string; +} + +interface IoEvent { + id: string; + signalId: string; + oldValue: boolean | number; + newValue: boolean | number; + source: "program" | "user" | "script" | "import" | "reset"; + timestamp: number; + instructionId?: string; +} +``` + +#### 7.6.4 WaitInstruction IR + +```ts +interface WaitInstruction { + kind: "wait"; + condition: ExpressionNode; + timeoutMs?: number; + onTimeout?: { + kind: "alarm" | "jump" | "call" | "continue"; + target?: string; + alarmCode?: number; + message?: string; + }; + sourceRange: SourceRange; +} +``` + +执行状态： + +```ts +interface ActiveWait { + id: string; + instructionId: string; + condition: CompiledExpression; + startedAtVirtualTime: number; + timeoutAtVirtualTime?: number; + status: "waiting" | "completed" | "timeout" | "cancelled"; +} +``` + +#### 7.6.5 Wait 调度流程 + +控制器执行到 wait： + +```text +1. 编译或读取 WaitInstruction 条件表达式。 +2. 立即评估一次 condition。 +3. 若为 true，PC 前进，记录 wait completed immediate。 +4. 若为 false，创建 ActiveWait，控制器进入 Running/Waiting。 +5. 每个 controller tick 或 IO 变化事件触发重新评估。 +6. 条件为 true 时，ActiveWait completed，PC 前进。 +7. 虚拟时间超过 timeout 时，执行 onTimeout 或产生报警。 +8. hold 时暂停 timeout 虚拟时间。 +9. stop/reset 时取消 ActiveWait。 +``` + +伪代码： + +```ts +function executeWait(instruction: WaitInstruction): StepResult { + if (evalCondition(instruction.condition)) { + traceWait("completed-immediate", instruction); + return { pc: "next" }; + } + + waitRegistry.add({ + id: newId(), + instructionId: instruction.id, + condition: compileExpression(instruction.condition), + startedAtVirtualTime: clock.now(), + timeoutAtVirtualTime: instruction.timeoutMs + ? clock.now() + instruction.timeoutMs + : undefined, + status: "waiting", + }); + + return { pc: "stay", state: "waiting" }; +} +``` + +#### 7.6.6 边沿检测 + +边沿函数： + +```text +rising(io.di[1]) +falling(io.di[1]) +changed(io.gi[1]) +``` + +实现要求： + +1. IO Service 在每次 tick 开始保存 previousValue。 +2. IO 写入发生后更新 current value。 +3. wait 条件评估时可读取 previous/current。 +4. 边沿只在一个 tick 内有效。 +5. 如果多个 IO 事件在同一 tick 内发生，按事件顺序处理，并在 trace 中保留顺序。 + +#### 7.6.7 IO 与虚拟时间 + +虚拟调试必须使用 controller virtual time，而不是直接使用 wall-clock： + +1. `wait timeout` 基于虚拟时间。 +2. `pulse duration` 基于虚拟时间。 +3. IO 脚本 `delay` 基于虚拟时间。 +4. hold 时虚拟时间暂停。 +5. 单步模式下虚拟时间按 step 推进。 +6. 快进回放时虚拟时间可加速，但事件顺序必须保持。 + +#### 7.6.8 Wait 与运动同步 + +首版建议采用同步语义： + +1. `movel/movej/movec` 完成后才执行下一条 wait。 +2. `wait` 完成后才执行后续运动。 +3. 不支持品牌控制器中的复杂并行 advance run 行为。 + +后续可扩展： + +1. 允许提前规划运动队列。 +2. 支持路径事件触发 IO。 +3. 支持运动中 wait 或 sensor search。 +4. 支持品牌特定 advance run 近似。 + +#### 7.6.9 IO Trace + +IO trace 需要记录： + +```ts +interface IoTraceRecord { + time: number; + kind: "read" | "write" | "wait-start" | "wait-done" | "wait-timeout" | "pulse-start" | "pulse-end"; + signal?: string; + value?: boolean | number; + source?: string; + instructionId?: string; + programLine?: number; +} +``` + +用途： + +1. 回放虚拟调试过程。 +2. 分析 wait 卡住原因。 +3. 生成调试报告。 +4. 帮助后处理验证 IO 映射。 + +#### 7.6.10 Wait 卡住诊断 + +当程序停在 wait 时，UI 和报警系统应显示： + +1. 当前等待表达式。 +2. 当前表达式求值结果。 +3. 每个子表达式的当前值。 +4. 已等待时间。 +5. 剩余超时时间。 +6. 相关 IO 点的最近变化记录。 +7. 是否存在 IO 脚本会触发该输入。 + +示例诊断： + +```text +Waiting at line 42: + wait io.alias.clamp_closed == true timeout 2 s + +Current: + io.alias.clamp_closed -> DI[1] = false + waited: 1.24 s + timeout in: 0.76 s + last write: DO[1] = true by program at 12.380 s + script: clamp_sim enabled, scheduled DI[1] = true at 12.680 s +``` + +## 8. KDL WASM 设计 + +### 8.1 KDL 使用边界 + +KDL 适合用于： + +1. 3D 向量、位姿、旋转、坐标变换。 +2. 串联机器人运动链建模。 +3. 正向运动学。 +4. 逆向运动学。 +5. 雅可比计算。 +6. 速度级运动学。 +7. 基础轨迹与速度曲线能力。 + +需要注意：KDL 本身不是完整的碰撞检测、工艺仿真或全局路径搜索框架。避障、碰撞检测、节拍优化、工艺参数模拟需要另行设计。 + +### 8.2 总体落地架构 + +KDL WASM 不是 UI 组件，而是虚拟控制器和离线编程规划器共用的计算内核。推荐架构如下： + +```text +HTML UI / Program Editor / Path Editor + | +TypeScript Virtual Controller / OLP Planner + | +KdlWorkerClient + | +kdl.worker.ts + | +KDL WASM Module +``` + +设计要求： + +1. UI 主线程不得直接执行大批量 FK、IK、轨迹采样，应通过 Worker 调用 KDL WASM。 +2. KDL C++ 类不直接暴露给业务层，业务层只使用稳定 TypeScript API。 +3. WASM 内部可以保存 `RobotHandle`、KDL `Chain`、求解器和缓存，主线程只保存 handle。 +4. 机器人结构以 URDF 为源数据，OPFS 保存原始 URDF 和转换后的标准模型缓存。 +5. 运动函数必须面向商业离线编程工作流，不只提供单点 FK/IK，还要提供批量可达性、轨迹采样、节拍估算和诊断。 +6. 对外运动指令只保留 `MOVEJ`、`MOVEL`、`MOVEC` 三类，分别对应关节角度差分、TCP 直线、TCP 圆弧。 + +### 8.3 URDF 机器人结构定义 + +机器人结构统一使用 URDF 定义。每个机器人资源建议包含： + +```text +robots/ + {robotId}/ + robot.urdf + robot.meta.json + limits.override.json + default_tool.json + meshes/ +``` + +URDF 使用规则： + +1. `robot.urdf` 是机器人运动链的源文件。 +2. 支持 `revolute`、`continuous`、`prismatic`、`fixed` joint。 +3. 首版不支持 `planar`、`floating` joint；导入时生成明确诊断。 +4. 所有长度单位统一为 meter，角度统一为 radian，时间统一为 second。 +5. 关节限位优先使用 URDF `limit`；缺少加速度、jerk、厂商速度等级时由 `limits.override.json` 补齐。 +6. URDF mesh 只用于显示和碰撞模块，KDL WASM 只消费运动链、关节轴、origin 和 limit。 +7. 导入 ABB、FANUC、KUKA 机器人库时，也应转换为 URDF + 元数据，而不是直接把品牌模型写入运动内核。 + +URDF 到 KDL 的转换分两层实现： + +1. TypeScript 层解析 XML，生成标准 `NormalizedRobotModel`，便于浏览器诊断、缓存、版本迁移和 UI 展示。 +2. WASM 层接收标准模型，构造 KDL `Tree`/`Chain` 和求解器。 + +对业务层仍提供 `loadRobotFromUrdf`，避免上层关心解析位置： + +```ts +interface UrdfLoadOptions { + robotId: string; + baseLink: string; + tipLink: string; + tool?: Pose; + base?: Pose; + jointOrder?: string[]; + overrideLimits?: JointLimitOverride[]; +} + +interface NormalizedRobotModel { + robotId: string; + name: string; + baseLink: string; + tipLink: string; + links: LinkModel[]; + joints: JointModel[]; + activeJointNames: string[]; + limits: JointLimits[]; + source: { + type: "urdf"; + urdfHash: string; + }; +} +``` + +### 8.4 KDL WASM 计算 API 清单 + +KDL WASM 首版至少应完成以下函数。TypeScript API 使用 camelCase，GRL/品牌指令层使用大写或小写指令名均可映射。 + +```ts +interface KdlWasmApi { + init(options?: KdlInitOptions): Promise; + + loadRobotFromUrdf(urdfXml: string, options: UrdfLoadOptions): Promise; + createRobotFromModel(model: NormalizedRobotModel): Promise; + destroyRobot(handle: RobotHandle): Promise; + getRobotInfo(handle: RobotHandle): Promise; + getJointLimits(handle: RobotHandle): Promise; + + fk(handle: RobotHandle, joints: Float64Array, options?: FkOptions): Promise; + fkAllLinks(handle: RobotHandle, joints: Float64Array): Promise; + jacobian(handle: RobotHandle, joints: Float64Array, options?: JacobianOptions): Promise; + ik(handle: RobotHandle, seed: Float64Array, target: Pose, options?: IkOptions): Promise; + ikBatch(handle: RobotHandle, seeds: Float64Array[], targets: Pose[], options?: IkOptions): Promise; + + checkJointLimits(handle: RobotHandle, joints: Float64Array): Promise; + checkSingularity(handle: RobotHandle, joints: Float64Array): Promise; + checkReachability(handle: RobotHandle, target: Pose, options?: IkOptions): Promise; + + moveJ(handle: RobotHandle, start: Float64Array, target: JointTarget | PoseTarget, options: MoveJOptions): Promise; + moveL(handle: RobotHandle, start: Float64Array, target: PoseTarget, options: MoveLOptions): Promise; + moveC(handle: RobotHandle, start: Float64Array, via: PoseTarget, target: PoseTarget, options: MoveCOptions): Promise; + + planPath(handle: RobotHandle, start: Float64Array, segments: MotionSegmentRequest[], options: PathPlanOptions): Promise; + validatePath(handle: RobotHandle, start: Float64Array, segments: MotionSegmentRequest[], options: PathPlanOptions): Promise; + estimateCycleTime(trajectory: TrajectoryResult | PathPlanResult): Promise; + + makeTrapProfile(length: number, options: TrapProfileOptions): Promise; + sampleTrapProfile(length: number, options: TrapProfileOptions): Promise; + resampleTrajectory(trajectory: TrajectoryResult, sampleTime: number): Promise; +} +``` + +首版 C++/WASM 内核建议直接使用或封装这些 KDL 能力： + +| 功能 | KDL 能力 | 包装层职责 | +| --- | --- | --- | +| 串联链 | `Tree`、`Chain`、`Segment`、`Joint` | 从 URDF 标准模型构造链 | +| 正解 | `ChainFkSolverPos_recursive` | 输出法兰、TCP、各 link 位姿 | +| 雅可比 | `ChainJntToJacSolver` | 输出矩阵和奇异性指标 | +| 逆解 | `ChainIkSolverPos_NR_JL`、`ChainIkSolverPos_LMA` | seed、多解尝试、限位、失败诊断 | +| 直线路径 | `Path_Line` | 生成 MOVEL TCP 采样 | +| 圆弧路径 | `Path_Circle` | 生成 MOVEC TCP 采样 | +| 梯形速度 | `VelocityProfile_Trap` | 生成 `s/sd/sdd` 采样 | +| 轨迹段 | `Trajectory_Segment` | 组合路径与速度曲线 | +| 圆角过渡 | `Path_RoundedComposite` | 第二阶段用于 zone/blend | + +### 8.5 通用数据结构 + +位姿在内部统一使用位置 + 四元数，避免欧拉角奇异；导入导出 ABB/FANUC/KUKA 时再转换为品牌格式。 + +```ts +type RobotHandle = number; + +interface Pose { + position: [number, number, number]; + quaternion: [number, number, number, number]; +} + +interface JointLimits { + name: string; + lower: number; + upper: number; + velocity: number; + acceleration: number; + jerk?: number; +} + +interface MotionOptionsBase { + sampleTime: number; + profile: "trap" | "s_curve"; + speedOverride: number; + sourceMap?: MotionSourceMap; + tcp?: Pose; + base?: Pose; +} + +interface MoveJOptions extends MotionOptionsBase { + jointVelocity?: number[]; + jointAcceleration?: number[]; + blend?: ZoneData; +} + +interface MoveLOptions extends MotionOptionsBase { + tcpVelocity: number; + tcpAcceleration: number; + orientationMode: "fixed" | "slerp" | "tool_z_lock"; + ik: IkOptions; + blend?: ZoneData; +} + +interface MoveCOptions extends MoveLOptions { + arcMode: "via" | "center" | "radius"; + circleDirection?: "short" | "long" | "cw" | "ccw"; +} +``` + +轨迹输出点是虚拟控制器、3D 回放、节拍报告、可达性报告和后处理预览共用的数据结构： + +```ts +interface TrajectoryPoint { + index: number; + time: number; + dt: number; + s: number; + sd: number; + sdd: number; + joints: number[]; + jointVelocity: number[]; + jointAcceleration: number[]; + flange: Pose; + tcp: Pose; + tcpVelocity?: [number, number, number, number, number, number]; + tcpAcceleration?: [number, number, number, number, number, number]; + motion: "MOVEJ" | "MOVEL" | "MOVEC"; + segmentId?: string; + targetId?: string; + sourceMap?: MotionSourceMap; + diagnostics: MotionDiagnostic[]; +} + +interface TrajectoryResult { + ok: boolean; + motion: "MOVEJ" | "MOVEL" | "MOVEC"; + duration: number; + sampleTime: number; + points: TrajectoryPoint[]; + events: TrajectoryEvent[]; + diagnostics: MotionDiagnostic[]; +} +``` + +区分两类点： + +1. 规划点：用户在 OLP 路径编辑器中创建的稀疏目标点，例如 `P10`、`P20`、`P30`。 +2. 轨迹采样点：KDL WASM 根据运动方式、速度、加速度和采样周期生成的密集点，用于仿真执行和报告。 + +### 8.6 正解、逆解、雅可比和可达性 + +正解输入： + +1. 机器人句柄。 +2. 关节数组。 +3. 工具坐标。 +4. 基坐标或工件坐标。 + +正解输出： + +1. 法兰位姿。 +2. TCP 位姿。 +3. 每个连杆位姿。 +4. 是否越限。 + +IK 要求： + +1. 支持 seed joint。 +2. 支持关节限位。 +3. 支持最大迭代次数。 +4. 支持位置和姿态容差。 +5. 支持多 seed 尝试。 +6. 支持多解候选排序。 +7. 支持按配置选择解，例如肩、肘、腕配置。 +8. 支持失败原因返回。 + +IK 返回： + +```ts +interface IkResult { + ok: boolean; + joints?: number[]; + iterations: number; + residualPosition?: number; + residualOrientation?: number; + configuration?: RobotConfiguration; + reason?: "unreachable" | "joint_limit" | "singularity" | "max_iteration" | "invalid_model"; + diagnostics: MotionDiagnostic[]; +} +``` + +可达性检查不应只返回 true/false，还要返回商业软件常见的诊断内容： + +1. 最近可达解。 +2. 超限关节名称、当前值、上下限。 +3. IK 残差。 +4. 奇异性指标。 +5. 推荐处理方式，例如换姿态、改 seed、改工具、换配置。 + +### 8.7 MOVEJ 轨迹规划 + +`MOVEJ` 是关节空间运动，机器人按各轴关节角度差分运行。TCP 轨迹由关节差分后的 FK 结果自然形成，不要求是直线。目标可以是关节目标，也可以是位姿目标： + +1. 如果目标是关节数组，直接作为 `qEnd`。 +2. 如果目标是位姿，先用 IK 从 `start` 附近求解 `qEnd`。 +3. 对每个关节检查位置、速度、加速度限位。 +4. 使用同步梯形速度曲线生成归一化路径参数 `s(t)`。 +5. 每个采样点计算 `q(t) = qStart + s(t) * (qEnd - qStart)`。 +6. 每个采样点执行 FK，得到法兰和 TCP 位姿。 +7. 输出完整 `TrajectoryPoint[]`。 + +MOVEJ 的关键要求： + +1. 所有关节同起同停。 +2. 采样周期可配置，首版建议默认 `0.004 s` 或 `0.008 s`。 +3. 速度倍率 `override` 只影响规划速度，不改变目标点。 +4. 对接近限位、超过限位、奇异点附近要产生 warning 或 error。 +5. 轨迹必须可重复：同一输入、同一算法版本、同一采样周期输出一致。 + +### 8.8 MOVEL 轨迹规划 + +`MOVEL` 是 TCP 直线运动，机器人 TCP 沿起点到终点的空间直线运行。关节角不做简单差分，而是由每个 TCP 采样点 IK 求解。 + +算法流程： + +1. 用 `start` 关节做 FK，得到起点 TCP。 +2. 根据目标点、工具、工件坐标得到终点 TCP。 +3. 使用 KDL `Path_Line` 或等价实现生成 TCP 直线路径。 +4. 姿态按 `orientationMode` 插补，首版推荐四元数 slerp。 +5. 使用梯形速度曲线生成路径参数 `s(t)`。 +6. 对每个 TCP 采样点执行 IK，seed 使用上一采样点关节值。 +7. 检查每个采样点的关节限位、速度、加速度、奇异性和 IK 残差。 +8. 输出轨迹点，并保留每个点对应的原始规划点和程序行 source map。 + +MOVEL 的商业级诊断要求： + +1. TCP 直线误差最大值。 +2. 姿态误差最大值。 +3. 失败采样点的时间、路径比例、TCP 位姿和 IK 失败原因。 +4. 关节翻转或配置突变检测。 +5. 速度超限点列表。 +6. 建议降低速度、调整姿态、插入中间点或切换配置。 + +### 8.9 MOVEC 轨迹规划 + +`MOVEC` 是 TCP 圆弧运动，机器人 TCP 经过 via 点并沿圆弧方式运行，关节角由每个圆弧采样点 IK 求解。输入至少包含： + +1. 起点：由当前关节 FK 得到。 +2. 经由点 `via`。 +3. 终点 `target`。 + +算法流程： + +1. 将起点、经由点、终点转换到同一基坐标。 +2. 检查三点是否重合或近似共线。 +3. 计算圆心、半径、法向量、圆弧角度。 +4. 使用 KDL `Path_Circle` 或等价实现生成圆弧 TCP 路径。 +5. 使用梯形速度曲线按圆弧长度采样。 +6. 姿态按策略插补：首版使用起点到终点 slerp，经由点只约束位置；第二阶段支持经由点姿态约束。 +7. 对每个采样 TCP 执行 IK，seed 使用上一采样点关节值。 +8. 生成轨迹点、圆弧几何诊断和可达性诊断。 + +MOVEC 必须返回这些附加信息： + +```ts +interface CirclePlanMeta { + center: [number, number, number]; + radius: number; + normal: [number, number, number]; + angle: number; + length: number; + direction: "cw" | "ccw"; + maxArcError: number; +} +``` + +### 8.10 梯形速度曲线 + +用户提到的“梯形图运动方式”，在运动规划上下文中按梯形速度曲线理解。PLC 梯形图属于虚拟 PLC/IO 联调模块，不放在 KDL 运动内核中。 + +梯形速度曲线用于 MOVEJ、MOVEL、MOVEC 的路径参数采样。统一输出： + +```ts +interface TrapSample { + index: number; + time: number; + s: number; + sd: number; + sdd: number; +} + +interface TrapProfileResult { + type: "trapezoid" | "triangle"; + length: number; + duration: number; + tAccel: number; + tConst: number; + tDecel: number; + vPeak: number; + samples: TrapSample[]; +} +``` + +基本规则： + +1. 输入路径长度 `L`、最大速度 `vMax`、最大加速度 `aMax`、采样周期 `dt`。 +2. 如果距离足够长，生成加速、匀速、减速三段梯形速度曲线。 +3. 如果距离不足以达到 `vMax`，自动退化为三角速度曲线。 +4. `s` 表示归一化路径比例，范围 `[0, 1]`。 +5. `sd`、`sdd` 表示归一化速度和加速度。 +6. KDL WASM 内核可以使用 `VelocityProfile_Trap`，包装层负责转换为统一 `TrapSample[]`。 +7. 所有运动函数必须把实际使用的速度曲线写入 `TrajectoryResult`，便于节拍报告和调试。 + +MOVEJ 中的路径长度建议定义为满足所有关节限位的归一化长度，而不是简单欧氏长度： + +```text +jointRatio_i = abs(qEnd_i - qStart_i) / maxAllowedDelta_i +pathRatio = max(jointRatio_i) +``` + +实际实现时应根据每个关节的速度、加速度约束计算所需时间，取最大时间作为同步运动时间，再反算每个关节的采样速度和加速度。 + +### 8.11 多段路径、zone 和商业 OLP 批量能力 + +商业离线编程软件通常不是一次只算一条运动，而是对整条路径做批量验证。因此 KDL WASM 需要提供 `planPath` 和 `validatePath`： + +1. 输入当前关节和多个 `MotionSegmentRequest`。 +2. 按程序顺序生成每段 MOVEJ/MOVEL/MOVEC 轨迹。 +3. 段间继承上一段末尾关节作为下一段起点。 +4. `fine` 表示必须精确到点。 +5. `zone` 表示允许过渡，首版可先做减速到点但保留 zone 信息；第二阶段用 `Path_RoundedComposite` 或自研 blend 算法实现连续过渡。 +6. 输出整条 path 的总节拍、每段节拍、失败点、警告点、source map。 + +批量验证结果应支持 UI 直接定位： + +```ts +interface MotionDiagnostic { + severity: "info" | "warning" | "error"; + code: string; + message: string; + time?: number; + pointIndex?: number; + segmentId?: string; + targetId?: string; + sourceMap?: MotionSourceMap; + data?: Record; +} +``` + +### 8.12 和虚拟控制器的关系 + +虚拟控制器执行运动指令时不直接写机器人关节，而是走统一运动队列： + +1. 解释器读到 `movej/movel/movec`。 +2. 把当前关节、目标点、速度、zone、tool、frame 解析成 `MotionSegmentRequest`。 +3. 调用 KDL WASM 生成 `TrajectoryResult`。 +4. Motion Queue 按虚拟时间消费 `TrajectoryPoint`。 +5. UI 根据当前点更新 3D 机器人、程序当前行、路径当前点、节拍统计。 +6. 如果 KDL 返回 error，虚拟控制器进入 alarm 或 hold 状态。 + +这样可以保证从规划点生成程序、手写 GRL、多品牌导入程序都走同一套运动学和轨迹采样逻辑。 + +### 8.13 实施优先级 + +P0 必须完成： + +1. URDF 文件加载、解析、诊断和标准模型缓存。 +2. 从 URDF 标准模型创建 KDL Chain。 +3. FK、fkAllLinks、Jacobian。 +4. IK、ikBatch、关节限位和失败原因。 +5. 梯形速度曲线 `makeTrapProfile`、`sampleTrapProfile`。 +6. `moveJ` 轨迹点生成。 +7. `moveL` 轨迹点生成。 +8. `moveC` 基础圆弧轨迹点生成。 +9. `validatePath` 批量可达性和诊断。 +10. 轨迹结果可写入 OPFS trace，并可被 UI 回放。 + +P1 扩展： + +1. zone/blend 连续过渡。 +2. S 曲线速度规划。 +3. 多 IK 解稳定排序。 +4. 外部轴和变位机。 +5. 多机器人协调。 +6. 碰撞检测联动。 +7. 与真实品牌控制器轨迹差异对比。 + +## 9. OPFS 工作区设计 + +### 9.1 文件系统布局 + +OPFS 内部推荐布局： + +```text +/projects/ + /{projectId}/ + project.json + station/ + station.json + cells/ + fixtures/ + parts/ + devices/ + libraries/ + robots/ + tools/ + fixtures/ + process_templates/ + geometry/ + meshes/ + cad_sources/ + collision/ + robots/ + programs/ + targets/ + tools/ + frames/ + io/ + io_map.json + io_scripts/ + generated/ + logs/ + io_trace.jsonl + simulation_trace.jsonl + reports/ + reachability/ + collision/ + cycle_time/ + post/ + calibration/ + calibration/ + tcp/ + frames/ + bases/ + snapshots/ + .index.json + .lock +``` + +### 9.2 Project Manifest + +```json +{ + "schemaVersion": 1, + "projectId": "demo-cell", + "name": "Demo Cell", + "createdAt": "2026-06-26T00:00:00.000Z", + "updatedAt": "2026-06-26T00:00:00.000Z", + "robots": ["robots/robot_1.robot.json"], + "mainProgram": "programs/main.grl", + "postProfiles": { + "abb": "post/abb.profile.json", + "fanuc": "post/fanuc.profile.json", + "kuka": "post/kuka.profile.json" + } +} +``` + +### 9.3 Storage API + +```ts +interface WorkspaceStorage { + listProjects(): Promise; + openProject(projectId: string): Promise; + createProject(input: CreateProjectInput): Promise; + readText(path: string): Promise; + writeText(path: string, content: string): Promise; + readJson(path: string): Promise; + writeJson(path: string, value: T): Promise; + delete(path: string): Promise; + snapshot(projectId: string): Promise; + exportZip(projectId: string): Promise; + importZip(file: File): Promise; +} +``` + +### 9.4 存储安全与备份 + +OPFS 是浏览器 Origin 私有文件系统，用户通常不能像普通目录一样直接看到这些文件。因此必须提供： + +1. 显式导出项目包。 +2. 显式导入项目包。 +3. 自动快照。 +4. 项目迁移工具。 +5. 存储占用显示。 +6. 数据损坏检测。 + +### 9.5 IO 文件 + +虚拟 IO 相关文件建议放在： + +```text +io/ + io_map.json + io_scripts/ + clamp_sim.ioscript + fixture_sim.ioscript +logs/ + io_trace.jsonl +``` + +要求： + +1. `io_map.json` 保存 IO 点定义、别名、品牌映射、初始值。 +2. `io_scripts/*.ioscript` 保存虚拟 IO 脚本。 +3. `io_trace.jsonl` 保存调试过程中 IO 事件，可按运行会话分文件。 +4. 项目导出 zip 时必须包含 IO map 和 IO scripts。 +5. trace 文件可选导出，避免项目包过大。 + +### 9.6 商业级项目交付物 + +商业 OLP 项目不仅保存程序，还应保存完整交付物： + +```text +reports/ + reachability/report.json + collision/report.json + cycle_time/report.json + post/abb_export_report.json + calibration/tcp_report.json +generated/ + abb/ + fanuc/ + kuka/ +packages/ + project_export.zip + customer_delivery.zip +``` + +客户交付包建议包含： + +1. GRL 源程序。 +2. 品牌后处理程序。 +3. 目标点和路径数据。 +4. IO map。 +5. 后处理报告。 +6. 可达性报告。 +7. 碰撞报告。 +8. 节拍报告。 +9. 校准数据。 +10. 仿真 trace，可选。 + +## 10. 后处理设计 + +### 10.1 后处理目标 + +后处理器输入统一 IR 和 OLP 对象模型，输出目标品牌程序文件： + +```text +OLP Object Model + Executable IR + -> ABB RAPID generator + -> FANUC LS/TP-compatible text generator + -> KUKA KRL SRC/DAT generator +``` + +注意：FANUC 二进制 TP 文件通常需要官方工具或控制器环境转换，Web 端应优先生成可读的 LS 风格文本或中间格式，具体落地方式需按现场 FANUC 工具链确认。 + +### 10.2 品牌映射示例 + +GRL： + +```text +movel pick speed linear(300 mm/s) zone z10 tool gripper frame fixture +``` + +ABB RAPID 目标： + +```text +MoveL pick, v300, z10, gripper\WObj:=fixture; +``` + +FANUC 目标： + +```text +L P[10] 300mm/sec CNT10 ; +``` + +KUKA KRL 目标： + +```text +$VEL.CP = 0.3 +LIN XPICK C_DIS +``` + +Path 后处理示例： + +```text +run_path weld_seam_01 +``` + +后处理器应展开为目标品牌的一组运动语句，并输出对应目标点数据。若目标品牌支持工艺包，可同时生成工艺指令；若不支持，则生成 IO 和注释。 + +### 10.3 后处理配置 + +每个品牌一个 profile： + +```json +{ + "brand": "abb", + "controller": "irc5", + "robot": "irb_1200", + "units": { + "length": "mm", + "angle": "deg" + }, + "motion": { + "defaultSpeed": "v300", + "defaultZone": "z10" + }, + "ioMap": { + "clamp_closed": "di1", + "clamp_open": "do1" + } +} +``` + +### 10.4 后处理必须处理的差异 + +1. 目标点数据格式。 +2. 姿态表示方式。 +3. 构型参数。 +4. 外部轴表示。 +5. 工具和工件坐标声明。 +6. 速度和过渡等级。 +7. IO 地址格式。 +8. 程序文件组织。 +9. 行号和标签。 +10. 不支持指令的降级或报错。 +11. Path 默认参数和单点覆盖参数。 +12. Operation 工艺参数到品牌工艺包或 IO 的映射。 +13. 品牌程序数据文件拆分，例如 KUKA `.src/.dat`。 +14. 目标点命名规则和长度限制。 +15. FANUC 点位编号、寄存器、程序行号和 LS 格式限制。 + +### 10.5 后处理质量要求 + +1. 不能静默丢弃语义。 +2. 不支持的指令必须给出明确诊断。 +3. 输出程序必须格式化稳定。 +4. 每个后处理器必须有 golden file 测试。 +5. 必须能生成转换报告，列出警告、限制、映射表和人工确认项。 + +### 10.6 反向后处理：品牌程序导入 + +为了虚拟控制器可以解析多品牌机器人程序，需要建立 Brand Importer： + +```text +ABB RAPID / FANUC LS / KUKA KRL + -> Brand Importer + -> Brand AST + -> Normalized IR + -> Reconstructed GRL + -> Optional OLP Path/Operation Model +``` + +Brand Importer 输出： + +1. `brandAst`：品牌语法树。 +2. `ir`：可执行统一 IR。 +3. `grlSource`：尽量恢复的 GRL 文本。 +4. `olpModelPatch`：恢复出的目标点、路径和操作。 +5. `report`：转换报告。 + +转换报告必须列出： + +1. 成功解析的程序、目标点、工具、坐标系。 +2. 无法识别的品牌指令。 +3. 近似转换的语义。 +4. 丢失或需要人工确认的信息。 +5. 后续导出到其他品牌时的风险。 + +## 11. 虚拟控制器界面设计 + +界面目标是“离线编程和虚拟调试”，不是营销页面。第一屏应直接进入工作台。 + +### 11.1 主界面布局 + +推荐五区布局： + +```text +┌─────────────────────────────────────────────────────────────┐ +│ 顶部工具栏：项目、保存、运行、暂停、停止、单步、后处理 │ +├───────────────┬──────────────────────────┬──────────────────┤ +│ 项目对象树 │ 程序编辑器 / 路径编辑器 │ 虚拟示教器 │ +│ 机器人/路径 │ AST/IR/诊断/日志 tabs │ 状态/坐标/速度 │ +├───────────────┴──────────────────────────┴──────────────────┤ +│ 底部面板：报警、变量、IO、运动队列、调用栈、后处理报告 │ +└─────────────────────────────────────────────────────────────┘ +``` + +项目对象树应按商业 OLP 软件方式组织，而不是只显示文件： + +```text +Station + Cell Layout + Robots + Tools + Frames + Fixtures + Parts + Devices + Geometry + Targets + Paths + Operations + Programs + Brand Imports + Generated Programs + Validation Reports + Calibration +``` + +### 11.2 必须页面 + +1. 项目管理 + - 新建项目 + - 打开项目 + - 导入 zip + - 导出 zip + - 项目设置 + - 生成客户交付包 + - 项目版本快照 + +2. 工作站布局 + - 机器人放置 + - 工具和夹具放置 + - 工件放置 + - 设备和安全区放置 + - 坐标系显示 + - 干涉区显示 + +3. 资源库 + - 机器人库 + - 工具库 + - 夹具库 + - 工艺模板库 + - 后处理 profile 库 + - 资源导入导出 + +4. 几何与 CAD + - 导入 mesh 或 CAD 转换文件 + - 提取点、边、曲线、面法向 + - 从曲线生成 Path + - 显示 CAD source metadata + - 更新 CAD 后重建路径 + +5. 程序编辑 + - GRL 代码编辑 + - 语法高亮 + - 自动补全 + - 错误诊断 + - 跳转定义 + - 格式化 + - 从 Path/Operation 生成 GRL + - 从品牌程序导入后查看恢复的 GRL + +6. 虚拟示教器 + - 控制器状态 + - 手动/自动模式 + - 运行、暂停、停止、复位 + - 单步执行 + - 当前关节值 + - 当前 TCP + - 当前 Tool / Frame + - Override 速度倍率 + +7. IO 面板 + - DI/DO/AI/AO/GI/GO + - IO 别名 + - 手动切换虚拟输入 + - IO 事件日志 + - Wait 条件监控 + - Pulse 状态 + - IO 脚本启停 + - IO trace 回放 + +8. 运动监控 + - 当前运动指令 + - Motion Queue + - 目标点列表 + - 路径点列表 + - Operation 状态 + - IK 状态 + - 奇异点/限位警告 + - 轨迹采样查看 + +9. 路径编辑器 + - 路径点表格 + - 批量设置速度、zone、tool、frame + - 接近点/离开点生成 + - 路径方向反转 + - 点位重命名 + - 姿态策略设置 + - 可达性检查 + - 展开为 GRL 预览 + - 碰撞检查结果 + - 节拍估算 + +10. Operation 编辑器 + - 工艺类型选择 + - 工艺参数编辑 + - start/end action + - before/after path point event + - 工艺 trace 显示 + +11. 验证与报告 + - 可达性报告 + - 碰撞报告 + - 节拍报告 + - IO/Wait 报告 + - 后处理报告 + - 报告导出 JSON/HTML + +12. 校准与现场回读 + - TCP 校准数据 + - 工件坐标校准数据 + - Base 校准数据 + - 现场程序回读 + - 离线/现场差异比对 + +13. 后处理导出 + - 选择品牌 + - 选择 profile + - 生成程序 + - 查看转换报告 + - 导出文件 + +14. 品牌程序导入 + - 选择 ABB/FANUC/KUKA + - 导入文本文件 + - 查看解析结果 + - 查看恢复出的 target/path/program + - 查看不支持语义 + - 转换为 GRL + - 加载到虚拟控制器运行 + +15. 日志和报警 + - 控制器报警 + - 解析错误 + - 运行时错误 + - 后处理警告 + - 品牌导入警告 + +16. Wait 调试面板 + - 当前 wait 指令 + - 等待表达式 + - 子表达式求值 + - 已等待时间 + - 剩余超时时间 + - 关联 IO 点最近变化 + - 手动满足条件按钮，仅调试模式可用 + - 跳过 wait，仅调试模式可用，并必须写入 trace + +### 11.3 控制器面板状态 + +需要显示： + +1. Controller state：Ready、Running、Hold、Fault。 +2. Mode：Manual、Auto。 +3. Motors：On、Off。 +4. Program：当前程序。 +5. Line：当前行。 +6. Cycle time：仿真周期。 +7. Override：速度倍率。 +8. TCP：X、Y、Z、RX、RY、RZ。 +9. Joints：J1 到 J6。 +10. Tool / Frame：当前工具和坐标系。 +11. Path：当前路径。 +12. Path Point：当前路径点。 +13. Operation：当前工艺操作。 +14. Source：当前来源，可能是 GRL、Path、Operation、ABB、FANUC、KUKA。 +15. Wait：当前是否处于等待状态。 +16. Wait Time：当前 wait 已等待时长。 +17. Active IO Script：当前启用的 IO 脚本。 + +### 11.4 编辑器诊断 + +诊断分级： + +| 等级 | 说明 | +| --- | --- | +| Error | 阻止运行或后处理 | +| Warning | 可运行但存在风险 | +| Info | 提示 | +| Hint | 优化建议 | + +示例： + +```text +E1003: target 'pick' is not reachable by robot_1 +W2007: zone z50 may exceed short segment length +W3012: FANUC post does not support this interrupt exactly; generated fallback label logic +W4010: imported KUKA $ADVANCE behavior was approximated by GRL path look-ahead +``` + +### 11.5 IO 面板设计 + +IO 面板应采用表格和过滤器，适合调试时快速定位信号： + +| 列 | 说明 | +| --- | --- | +| Address | `DI[1]`、`DO[2]`、`GI[1]` | +| Alias | `clamp_closed` | +| Value | 当前值 | +| Previous | 上一 tick 值 | +| Source | 最后写入来源：program/user/script/reset | +| Updated | 最后更新时间 | +| Brand | ABB/FANUC/KUKA 映射 | +| Lock | 是否允许手动修改 | + +功能要求： + +1. 按类型过滤：DI、DO、AI、AO、GI、GO。 +2. 按 alias 搜索。 +3. 支持手动切换虚拟输入。 +4. 支持批量 reset。 +5. 支持查看信号最近 N 条事件。 +6. 支持将某个 IO 点加入 watch。 +7. 支持显示哪些 wait 条件依赖该 IO。 + +### 11.6 IO 脚本界面 + +IO 脚本界面用于模拟 PLC 或夹具： + +1. 脚本列表。 +2. 启用/停用。 +3. 单步触发。 +4. 查看已注册 trigger。 +5. 查看计划中的 delayed event。 +6. 查看脚本产生的 IO 写入。 +7. 脚本错误诊断。 + +### 11.7 Wait 调试体验 + +当程序停在 wait 时，界面必须避免只显示“程序卡住”。至少显示： + +```text +Program waiting: + line: 42 + expression: io.alias.clamp_closed == true + DI[1] clamp_closed: false + waited: 1.24 s / timeout: 2.00 s + related output: DO[1] clamp_close_cmd = true +``` + +可选调试动作： + +1. Set DI true：手动满足输入。 +2. Toggle related IO：切换相关信号。 +3. Run IO script：运行关联 IO 脚本。 +4. Skip wait：跳过等待，必须标记 trace 为人工干预。 +5. Abort program：停止程序并生成报警。 + +## 12. 实现路线 + +### 阶段 0：基础工程 + +目标： + +1. 建立 TypeScript 项目结构。 +2. 建立 HTML 工作台页面。 +3. 建立 OPFS workspace。 +4. 建立测试框架。 +5. 建立 KDL WASM 构建原型。 +6. 建立 OLP Object Model 基础类型。 +7. 建立 Station/Resource/Report 基础模型。 + +交付： + +1. 可打开 Web 工作台。 +2. 可新建/保存/导出项目。 +3. 可加载一个机器人模型 JSON。 +4. 可在浏览器调用 KDL WASM 做一次 FK。 +5. 可在对象树中显示 Robot、Tool、Frame、Target、Path、Operation、Program。 +6. 可保存 station.json 和基础资源库。 + +### 阶段 1：GRL 语言最小闭环 + +目标： + +1. 实现 GRL lexer/parser。 +2. 支持 `module`、`proc`、变量、目标点、`path`、`movej`、`movel`、`run_path`、`call`。 +3. 实现 AST、语义检查、诊断。 +4. 实现 OLP Model 到 GRL 的生成器原型。 + +交付： + +1. 编辑器能显示语法错误。 +2. 能将 `main.grl` 编译为 IR。 +3. 能保存 AST/IR 调试输出。 +4. 能从一组规划点生成 path 和 GRL。 + +### 阶段 2：虚拟控制器最小闭环 + +目标： + +1. 实现控制器状态机。 +2. 实现程序加载、运行、暂停、停止、单步。 +3. 实现变量和调用栈。 +4. 实现基础 IO。 + +交付： + +1. 能执行无运动逻辑程序。 +2. 能在 UI 看到当前行、变量、调用栈。 +3. 能模拟 IO wait 和 timeout。 +4. 能执行 `run_path` 展开后的 IR。 + +### 阶段 3：运动执行闭环 + +目标： + +1. 接入 URDF 机器人模型导入和标准模型缓存。 +2. 接入 KDL WASM FK、fkAllLinks、Jacobian、IK、ikBatch。 +3. 实现梯形速度曲线生成和采样。 +4. 实现 `movej` 关节空间轨迹规划。 +5. 实现 `movel` TCP 直线轨迹规划。 +6. 实现基础 `movec` 圆弧轨迹规划。 +7. 实现关节限位、速度限位、加速度限位和 IK 失败报警。 +8. 实现路径点 source map。 +9. 实现可达性报告原型。 + +交付： + +1. 程序能驱动虚拟机器人状态变化。 +2. UI 能看到关节和 TCP 更新。 +3. 能加载 URDF 并显示机器人关节链和 link 位姿。 +4. `MOVEJ/MOVEL/MOVEC` 都能输出 `TrajectoryPoint[]`。 +5. 能导出轨迹 trace。 +6. 能从路径编辑器定位当前运动点。 +7. 能生成 Path 可达性报告。 +8. 能生成基础节拍估算。 + +### 阶段 4：完整基础语言 + +目标： + +1. 支持 `movec`。 +2. 支持 `if/while/for/switch`。 +3. 支持 `Tool`、`Frame`、`Speed`、`Zone`。 +4. 支持中断和报警的基础模型。 +5. 支持 `operation`、path event、start/end action。 + +交付： + +1. 可编写完整搬运类程序。 +2. 可进行基本虚拟调试。 +3. 可查看运动队列和报警。 +4. 可从 Operation 自动生成可执行 GRL。 + +### 阶段 5：后处理 MVP + +目标： + +1. ABB RAPID 后处理。 +2. KUKA KRL 后处理。 +3. FANUC LS 风格后处理。 +4. 后处理报告。 +5. 支持 path 和 operation 展开。 + +交付： + +1. 同一 GRL 程序可导出三种品牌程序。 +2. 每个后处理器有 golden file 测试。 +3. 不支持语义能明确报错。 +4. 同一 Path 可生成 ABB/FANUC/KUKA 运动点和数据文件。 + +### 阶段 6：验证与报告 MVP + +目标： + +1. 可达性报告。 +2. 基础碰撞检测。 +3. 节拍估算。 +4. IO/Wait 报告。 +5. HTML/JSON 报告导出。 + +交付： + +1. 能对单个 Path 生成 reachability report。 +2. 能对机器人和工件/夹具做基础碰撞检测。 +3. 能计算程序总节拍和 wait 耗时。 +4. 能导出验证报告。 + +### 阶段 7：品牌程序导入 + +目标： + +1. ABB RAPID 文本导入。 +2. KUKA KRL `.src/.dat` 导入。 +3. FANUC LS 风格文本导入。 +4. Brand AST 到统一 IR。 +5. 品牌导入转换报告。 + +交付： + +1. 能导入三类品牌程序并显示解析结果。 +2. 能加载导入后的 IR 到虚拟控制器运行。 +3. 能尽量恢复 target/path/program。 +4. 无法等价转换的语义有明确警告。 + +### 阶段 8：虚拟调试增强 + +目标： + +1. 断点。 +2. 单步进入/越过。 +3. 运动断点。 +4. 轨迹回放。 +5. 变量 watch。 +6. IO 脚本。 +7. 品牌源程序行号与 IR 的联动调试。 + +交付： + +1. 可像调试程序一样调试机器人逻辑。 +2. 可复现运行 trace。 +3. 可定位 IK、IO、逻辑错误。 +4. 可在 GRL、Path、Operation、品牌源程序之间定位同一条运动。 + +### 阶段 9：商业级 OLP 扩展 + +目标： + +1. 资源库管理。 +2. CAD/mesh 导入和曲线路径生成。 +3. 工艺模板库。 +4. 校准数据管理。 +5. 客户交付包生成。 + +交付： + +1. 可从几何曲线生成 Path。 +2. 可使用工艺模板生成 Operation。 +3. 可生成客户交付包。 +4. 可保存 TCP/Frame/Base 校准数据。 + +### 阶段 10：工程化与扩展 + +目标： + +1. 多机器人。 +2. 外部轴。 +3. 工艺包。 +4. 碰撞检测。 +5. 真实控制器校验流程集成。 + +交付： + +1. 支持更复杂工作站。 +2. 支持真实项目导入导出流程。 +3. 支持品牌差异配置库。 + +## 13. 测试策略 + +### 13.1 解析器测试 + +1. 合法程序解析快照。 +2. 非法程序错误位置。 +3. 单位解析。 +4. 注释保留。 +5. 品牌扩展属性解析。 +6. `path`、`operation`、`event`、`run_path`、`run_operation` 解析。 +7. 从 OLP Model 生成 GRL 后再解析的一致性测试。 + +### 13.2 语义测试 + +1. 未定义变量。 +2. 类型不匹配。 +3. 工具缺失。 +4. 坐标系缺失。 +5. IO 越界。 +6. 不可达点。 +7. 关节超限。 +8. 空 path。 +9. 重复 path point。 +10. Operation 引用不存在的 path。 +11. Path event 引用不存在的 point。 + +### 13.3 KDL WASM 测试 + +1. URDF 导入测试：能从 URDF 解析 link、joint、origin、axis、limit，并生成稳定 `NormalizedRobotModel`。 +2. URDF 诊断测试：缺少 limit、非法 joint、base/tip 不连通、单位异常时返回明确诊断。 +3. WASM FK 与原生 KDL FK 对比。 +4. `fkAllLinks` 输出 link 位姿数量和 joint 顺序正确。 +5. Jacobian 维度、数值和奇异点指标正确。 +6. IK 求解后再 FK，位置和姿态误差小于容差。 +7. IK seed、多 seed、关节限位和失败原因测试。 +8. 边界关节值。 +9. 奇异点附近诊断。 +10. 梯形速度曲线测试：长距离为梯形，短距离自动退化为三角形。 +11. 梯形采样测试：起点终点速度为 0，`s` 单调递增，末点精确到 1。 +12. `movej` 测试：所有关节同起同停，速度和加速度不超限。 +13. `movel` 测试：TCP 直线误差小于容差，逐点 IK 连续。 +14. `movec` 测试：圆弧半径、圆心、圆弧长度和采样误差小于容差。 +15. `movec` 非法输入测试：三点重合、近似共线、半径过小时返回诊断。 +16. 从 path 批量采样后逐点 IK。 +17. 大型路径批量可达性性能测试。 +18. 轨迹 source map 测试：采样点可定位回 segment、target 和程序行。 +19. 轨迹结果序列化测试：`TrajectoryResult` 可写入 OPFS 后重新加载回放。 +20. 大批量目标点性能测试。 + +### 13.4 虚拟控制器测试 + +1. 状态机转换。 +2. 程序启动/暂停/恢复/停止。 +3. 单步执行。 +4. 调用栈。 +5. IO wait timeout。 +6. 报警复位。 +7. 中断触发。 +8. `run_path` 展开执行。 +9. `run_operation` start/end action 执行。 +10. Source map 能定位到 path point 和品牌源程序行。 +11. wait 条件立即满足。 +12. wait 条件由用户手动 IO 满足。 +13. wait 条件由 IO 脚本延迟满足。 +14. wait timeout 后报警。 +15. wait timeout 后跳转 `on_timeout`。 +16. hold 时 wait timeout 暂停。 +17. stop 时 ActiveWait 取消。 +18. pulse 输出按虚拟时间自动复位。 +19. rising/falling/changed 边沿检测。 +20. IO trace 顺序稳定。 + +### 13.4.1 商业验证测试 + +1. 可达性报告内容完整。 +2. 接近关节限位能产生 warning。 +3. IK 不可达能定位到 path point。 +4. 基础碰撞检测能定位对象和时间点。 +5. 节拍报告能统计 motion time、wait time、IO script delay。 +6. 报告 JSON schema 稳定。 +7. HTML 报告可打开且包含关键摘要。 + +### 13.5 后处理测试 + +1. GRL 到 ABB 输出。 +2. GRL 到 FANUC 输出。 +3. GRL 到 KUKA 输出。 +4. 不支持语义报错。 +5. 速度、过渡、工具、坐标系映射。 +6. 目标点数值格式稳定。 +7. Path 展开后输出稳定。 +8. Operation 工艺参数映射。 + +### 13.6 品牌导入测试 + +1. ABB RAPID 导入到 IR。 +2. KUKA KRL 导入到 IR。 +3. FANUC LS 导入到 IR。 +4. 品牌导入后再导出 GRL。 +5. 品牌导入转换报告。 +6. 不支持品牌指令的诊断。 +7. 导入程序在虚拟控制器中可执行。 + +### 13.7 商业 OLP 工作流测试 + +1. 新建 station。 +2. 从机器人库添加机器人。 +3. 从工具库添加工具。 +4. 导入 mesh。 +5. 从规划点生成 Path。 +6. 从 Path 生成 GRL。 +7. 仿真运行。 +8. 生成报告。 +9. 后处理导出。 +10. 导出客户交付包。 + +## 14. 风险与边界 + +| 风险 | 说明 | 对策 | +| --- | --- | --- | +| 品牌语义不完全一致 | 三家控制器的运动规划、过渡、异常、中断语义不同 | GRL 定义通用语义，后处理报告列出差异 | +| FANUC TP 二进制限制 | Web 端难以直接生成真实 `.TP` 二进制 | 初期生成 LS 风格文本或中间格式，配合 FANUC 工具链 | +| KDL 不负责碰撞检测 | KDL 不是完整仿真引擎 | 运动学先闭环，碰撞检测作为独立模块 | +| 浏览器存储不可见 | OPFS 文件不直接暴露给用户 | 必须提供 zip 导入导出和备份 | +| WASM 初始化成本 | 大型 WASM 初次加载慢 | Worker 懒加载、缓存、进度提示 | +| 数值差异 | WebAssembly、C++、真实控制器结果可能存在差异 | 设置容差，建立真实机器人标定与验证流程 | +| 后处理责任边界 | 导出程序不等于现场可直接高速运行 | 必须生成校验报告，并要求现场低速验证 | + +## 15. 首版 MVP 范围 + +首版建议只做这些能力： + +1. 单机器人 6 轴串联模型，机器人结构使用 URDF 定义。 +2. GRL 程序编辑、解析、诊断。 +3. OLP 对象树：Robot、Tool、Frame、Target、Path、Operation、Program。 +4. 从规划点生成 Path 和 GRL。 +5. `movej`、`movel`、基础 `movec`。 +6. `Tool`、`Frame`、`Speed`、`Zone`。 +7. 基础变量、条件、循环、子程序。 +8. DI/DO 虚拟 IO。 +9. `wait`、`wait timeout`、`pulse`、手动 IO 切换。 +10. 简单 IO 脚本，例如夹爪 DO 触发 DI 反馈。 +11. KDL WASM FK、fkAllLinks、Jacobian、IK、ikBatch。 +12. KDL WASM 梯形速度曲线和 `MOVEJ/MOVEL/MOVEC` 轨迹采样函数。 +13. 虚拟控制器运行、暂停、停止、单步。 +14. OPFS 项目保存。 +15. ABB、FANUC、KUKA 后处理原型。 +16. 至少一种品牌程序导入原型，建议优先 KUKA KRL 或 ABB RAPID 文本。 +17. 基础可达性报告。 +18. 基础节拍报告。 +19. 客户交付包导出原型。 + +不建议首版包含： + +1. 完整碰撞检测。 +2. 多机器人协调。 +3. 外部轴同步。 +4. 完整焊接/喷涂工艺包。 +5. 完整真实控制器通信。 +6. 与真实控制器完全一致的 look-ahead 和伺服行为。 +7. FANUC 二进制 TP 直接生成或直接解析。 +8. 完整 CAD kernel。 +9. 完整 PLC/现场总线实时联调。 +10. 高精度真实控制器轨迹复现。 + +## 16. 验收标准 + +MVP 可按以下标准验收： + +1. 用户能在浏览器中新建项目。 +2. 用户能导入 URDF 机器人模型，并定义工具、工件坐标和目标点。 +3. 用户能从规划点创建 Path。 +4. 用户能从 Path 和 Operation 生成 GRL 程序。 +5. 用户能手工编辑 GRL 程序。 +6. 系统能实时显示语法和语义错误。 +7. 系统能运行程序，显示当前行、变量、IO、报警、TCP、关节。 +8. KDL WASM 能从 URDF 创建运动链，并提供 FK、fkAllLinks、Jacobian、IK、ikBatch。 +9. KDL WASM 能生成梯形速度曲线采样。 +10. `movej`、`movel`、`movec` 可生成轨迹采样点。 +11. MOVEJ 轨迹按关节角度差分运行并满足关节同起同停，MOVEL 轨迹满足 TCP 直线误差容差，MOVEC 轨迹满足 TCP 圆弧误差容差。 +12. 不可达点和超限点能明确报警。 +13. 程序执行到 wait 时，能显示等待表达式、关联 IO、已等待时间和剩余超时时间。 +14. 用户能通过 IO 面板手动满足 wait。 +15. IO 脚本能模拟夹具反馈并自动满足 wait。 +16. wait timeout 能产生报警或执行 on_timeout。 +17. 项目能保存在 OPFS，并能导出 zip。 +18. 同一个 Path/Program 能导出 ABB、FANUC、KUKA 三种目标文本。 +19. 后处理输出有转换报告。 +20. 至少一种品牌程序能导入、转换为 IR 并在虚拟控制器中运行。 +21. 自动测试覆盖解析器、KDL WASM、虚拟控制器、IO/Wait、后处理和品牌导入。 +22. 能生成可达性报告和节拍报告。 +23. 能导出包含源程序、品牌程序、IO map、报告的客户交付包。 + +商业级验收可按以下标准扩展： + +1. 能从资源库创建完整工作站。 +2. 能导入几何模型并从点/曲线生成 Path。 +3. 能对 Path 做批量可达性检查。 +4. 能执行基础碰撞检测并定位碰撞对象。 +5. 能统计程序节拍并分解 motion/wait/IO 时间。 +6. 能保存和应用 TCP、Frame、Base 校准数据。 +7. 能将导入品牌程序恢复为统一 IR 并参与报告。 +8. 能生成客户可交付项目包。 + +## 17. 推荐目录结构 + +未来代码工程可采用如下结构： + +```text +src/ + app/ + main.ts + ui/ + station/ + station-model.ts + cell-layout.ts + resources.ts + devices.ts + resources/ + robot-library.ts + tool-library.ts + fixture-library.ts + process-template-library.ts + geometry/ + mesh-loader.ts + curve-sampling.ts + collision-geometry.ts + core/ + ast/ + parser/ + semantic/ + ir/ + controller/ + runtime/ + motion/ + io/ + io-service.ts + wait-registry.ts + io-script.ts + io-trace.ts + alarm/ + debug/ + kdl-wasm/ + bindings/ + loader.ts + types.ts + workspace/ + opfs.ts + project.ts + import-export.ts + olp/ + model.ts + path.ts + operation.ts + grl-generator.ts + importers/ + abb/ + fanuc/ + kuka/ + post/ + abb/ + fanuc/ + kuka/ + robot/ + model.ts + frames.ts + targets.ts + validation/ + reachability.ts + collision.ts + cycle-time.ts + validation-report.ts + calibration/ + tcp-calibration.ts + frame-calibration.ts + base-calibration.ts + program-diff.ts + reports/ + report-model.ts + html-report.ts + json-report.ts + tests/ +wasm/ + kdl/ +docs/ +``` + +## 18. 近期开发任务清单 + +建议下一步按以下顺序推进： + +1. 固化 GRL 语法草案。 +2. 建立 TypeScript monorepo 或单包工程。 +3. 实现 OPFS 项目读写。 +4. 建立 URDF 导入、诊断和 `NormalizedRobotModel` 缓存。 +5. 编译 KDL WASM 并提供 FK、fkAllLinks、Jacobian、IK、ikBatch API。 +6. 实现 KDL WASM 梯形速度曲线 `makeTrapProfile` 和 `sampleTrapProfile`。 +7. 实现 KDL WASM `moveJ` 轨迹采样。 +8. 实现 KDL WASM `moveL` 轨迹采样。 +9. 实现 KDL WASM `moveC` 轨迹采样。 +10. 实现 `validatePath` 批量可达性和轨迹诊断。 +11. 实现 GRL parser。 +12. 实现语义检查和 IR。 +13. 实现虚拟控制器状态机。 +14. 将 `movej/movel/movec` 执行接入 Motion Queue。 +15. 实现简单 HTML 工作台。 +16. 实现 Path 编辑和 Path 到 GRL 生成。 +17. 实现 ABB/KUKA/FANUC 后处理 MVP。 +18. 实现一个品牌导入 MVP。 +19. 实现虚拟 IO Service、wait、pulse、IO 脚本和 IO 调试面板。 +20. 实现 station/resource 基础模型。 +21. 实现可达性和节拍报告原型。 +22. 实现客户交付包导出原型。 + +## 19. 参考资料 + +1. ABB RAPID Technical Reference Manual，RAPID Instructions, Functions and Data Types： + https://library.e.abb.com/public/b227fcd260204c4dbeb8a58f8002fe64/Rapid_instructions.pdf + +2. ABB RAPID Overview： + https://search.abb.com/library/Download.aspx?Action=Launch&DocumentID=3HAC050947-001&DocumentPartId=&LanguageCode=en + +3. FANUC America Tech Transfer： + https://techtransfer.fanucamerica.com/ + +4. FANUC CRX / Tablet Teach Pendant 公开资料： + https://crx.fanucamerica.com/training/programming-part-1-initial-settings + https://www.fanucamerica.com/products/teach-pendant/tablet-teach-pendant + +5. KUKA KSS 8.7 产品资料与操作编程文档入口： + https://my.kuka.com/s/product/kss-87/01t1i000001PTSOAA4?language=en_US + +6. Orocos KDL 官方文档： + https://docs.orocos.org/kdl/overview.html + https://www.orocos.org/kdl.html + +7. Emscripten Embind 文档： + https://emscripten.org/docs/porting/connecting_cpp_and_javascript/embind.html + +8. MDN OPFS 文档： + https://developer.mozilla.org/en-US/docs/Web/API/File_System_API/Origin_private_file_system + +9. RoboDK Basic Guide，说明离线编程是在离线环境创建、仿真并生成特定机器人和控制器程序： + https://robodk.com/doc/en/Basic-Guide.html + +10. RoboDK Post Processors，说明后处理器负责按具体机器人控制器规则生成程序： + https://robodk.com/doc/en/Post-Processors.html + +11. RoboDK Offline Programming，说明仿真达到预期后通过 post processors 生成机器人程序，并覆盖多品牌机器人： + https://robodk.com/offline-programming + +12. ABB Downloads / RobotStudio SDK，说明 RobotStudio 支持在 PC 上进行机器人仿真和离线编程，而不停止生产： + https://www.abb.com/global/en/areas/robotics/downloads + +13. Siemens Process Simulate，说明在 3D 虚拟环境中维护和优化机器人过程，支持离线编程： + https://www.siemens.com/en-us/products/tecnomatix/process-simulate-software/ + +14. Siemens Robotics Virtual Commissioning，说明虚拟调试用于验证 OLP 创建的机器人程序是否可行、高效，并验证 reach 和 cycle time： + https://www.siemens.com/en-us/technology/robotics-virtual-commissioning/ + +15. Siemens Robotics Programming and Simulation，说明在虚拟调试环境中结合真实控制器代码、机器人程序和硬件验证完整系统功能： + https://www.siemens.com/en-us/products/tecnomatix/offerings/robotics-programming-simulation/ + +16. Visual Components Robot Offline Programming，说明 OLP 软件面向多工业机器人品牌、工艺和复杂度： + https://www.visualcomponents.com/products/robot-offline-programming/ + +17. Visual Components Robot Programming，用于对标机器人工作站规划、调试和离线编程流程： + https://www.visualcomponents.com/use-cases/robot-programming/ + +## 20. 本次修订摘要 + +本次修订重点围绕“对标商业离线编程和虚拟调试软件的做法”完善设计： + +1. 新增商业 OLP 对标定位，明确系统应采用“OLP 对象模型 + 可执行 GRL 程序 + 统一 IR”的双层结构。 +2. 新增商业 OLP 典型工作流，覆盖工作站创建、目标点创建、路径创建、操作创建、GRL 生成、虚拟调试、后处理导出、品牌程序导入。 +3. 扩展 GRL 语法，新增 `path`、`operation`、`run_path`、`run_operation`、path event、工艺参数等设计。 +4. 明确规划点和规划路径生成 GRL 的规则：优先生成 Path 对象，再由 Path 生成程序，而不是直接散落成运动语句。 +5. 新增 OLP Object Model、GRL AST、Brand AST、Executable IR 三层/多树结构，避免路径编辑、仿真执行、品牌导入导出互相耦合。 +6. 新增 ABB RAPID、KUKA KRL、FANUC LS 等品牌程序导入解析路线，使虚拟控制器可以解析多品牌机器人程序并统一执行。 +7. 扩展虚拟控制器设计，要求 IR 保留 GRL、Path、Operation、品牌源程序的 source map，支持跨视图调试。 +8. 扩展界面设计，新增 OLP 对象树、路径编辑器、Operation 编辑器、品牌程序导入向导。 +9. 更新实现路线，将品牌程序导入作为独立阶段，并把 Path/Operation 纳入 MVP 和测试策略。 +10. 补充虚拟 IO 与 wait 实施方案，覆盖 IO 点模型、IO map、wait 条件、超时、边沿检测、pulse、IO 脚本、IO trace、Wait 调试面板和测试验收。 +11. 补充商业级 OLP 能力，覆盖工作站/资源库、CAD 与路径生成、碰撞/可达性/节拍验证、虚拟调试联调、校准与 Sim-to-Real、商业报告和客户交付包。 +12. 补充 KDL WASM 可实施计算函数方案，明确 URDF 作为机器人结构源定义，要求提供 FK、IK、Jacobian、MOVEJ、MOVEL、MOVEC、梯形速度曲线、轨迹采样点、批量路径验证和诊断接口。 diff --git a/work/doc/通用机器人编程语法规范.md b/work/doc/通用机器人编程语法规范.md new file mode 100644 index 0000000..08d1d42 --- /dev/null +++ b/work/doc/通用机器人编程语法规范.md @@ -0,0 +1,1541 @@ +# 通用机器人编程语法规范 + +版本：0.1 +适用范围：离线编程、虚拟控制器、虚拟调试、多品牌后处理 +目标品牌：ABB RAPID、FANUC TP/LS/KAREL 语义、KUKA KRL + +## 1. 设计目标 + +本规范定义一种用于离线编程和虚拟调试的通用机器人程序语言，暂定名为 GRL，Generic Robot Language。GRL 不是复制某一家机器人厂商语言，而是抽象 ABB、FANUC、KUKA 等主流工业机器人共同具备的程序语义，形成稳定、可解析、可仿真、可自动生成、可后处理的中性机器人程序。 + +核心目标： + +1. 支持离线编程的完整流程：目标点、路径、工艺、IO、等待、流程控制、报警、仿真执行、报告和后处理。 +2. 方便由规划点、CAD 曲线、工艺模板、AI 规划器自动生成程序。 +3. 方便转换为 ABB RAPID、FANUC LS/TP 风格文本、KUKA KRL。 +4. 方便反向导入品牌程序，恢复为统一 IR、GRL 和 OLP 对象模型。 +5. 程序文本稳定、可读、可 diff，便于版本管理。 +6. 语义确定，编译后能生成统一可执行 IR，虚拟控制器不依赖品牌控制器细节。 + +非目标： + +1. 不追求完全复刻某一品牌控制器所有系统变量和内部调度细节。 +2. 不直接作为真实机器人安全运行的唯一依据，后处理输出必须经过品牌控制器验证和现场低速试运行。 +3. 不把 CAD、碰撞检测、机器人运动学全部写入语言本身；这些由 OLP 模型和仿真内核承担。 + +## 2. 对标原则 + +### 2.1 品牌能力映射 + +| 能力 | ABB RAPID | FANUC TP/LS | KUKA KRL | GRL | +| --- | --- | --- | --- | --- | +| 程序模块 | `MODULE` | TP 程序、KAREL 程序 | `.src/.dat`、`DEF` | `module` | +| 子程序 | `PROC` | `CALL` | `DEF` 子程序 | `proc` | +| 函数 | `FUNC` | KAREL function | `DEFFCT` | `func` | +| 中断/陷阱 | `TRAP`、interrupt | 条件监控、后台逻辑 | `INTERRUPT` | `trap`、`interrupt` | +| 关节运动 | `MoveJ` | `J P[...]` | `PTP` | `movej` | +| 直线运动 | `MoveL` | `L P[...]` | `LIN` | `movel` | +| 圆弧运动 | `MoveC` | `C P[...]` | `CIRC` | `movec` | +| 笛卡尔目标点 | `robtarget` | `P[]`、`PR[]` | `E6POS` | `pose_target` | +| 关节目标点 | `jointtarget` | joint position data | `E6AXIS` | `joint_target` | +| 工具 | `tooldata` | UTOOL | `$TOOL` | `tool` | +| 工件/基坐标 | `wobjdata` | UFRAME | `$BASE` | `frame` | +| 速度 | `speeddata` | `%`、`mm/sec` | `$VEL`、`$ACC` | `speed` | +| 过渡 | `zonedata`、`fine` | `FINE`、`CNT` | `C_DIS`、`C_PTP`、`APO` | `zone` | +| IO | `SetDO`、`WaitDI` | `DO[]`、`DI[]`、`WAIT` | `$OUT[]`、`$IN[]`、`WAIT FOR` | `io.*`、`wait` | +| 标签跳转 | `GOTO` | `LBL`、`JMP` | `GOTO` | `label`、`jump` | + +### 2.2 中性语法风格 + +GRL 采用“声明式数据 + 结构化程序 + Path/Operation 对象”的风格： + +1. 目标点、工具、坐标系、速度、过渡作为数据声明。 +2. 程序流程使用 `proc`、`if`、`while`、`for`、`switch` 等结构化控制。 +3. 短程序和调试程序可直接写 `movej/movel/movec`。 +4. 自动离线编程优先生成 `path` 和 `operation`，程序中通过 `run_path`、`run_operation` 调用。 +5. 所有运动语句显式绑定目标点、速度、过渡、工具、坐标系，允许默认值，但编译到 IR 前必须解析为确定值。 +6. 品牌特性通过 `post_hint` 和 `@brand.*` 元数据保留，不污染通用语义。 + +## 3. 文件与工程结构 + +推荐一个 OLP 项目使用以下结构： + +```text +project.json +robots/ + robot_1.urdf + robot_1.meta.json +programs/ + main.grl + weld.grl +targets/ + main.targets.json +paths/ + weld_path.path.json +operations/ + weld_op.operation.json +io/ + io_map.json +post/ + abb.profile.json + fanuc.profile.json + kuka.profile.json +generated/ + abb/ + fanuc/ + kuka/ +reports/ + reachability.json + cycle_time.json +``` + +GRL 文件扩展名建议使用 `.grl`。 + +## 4. 词法规则 + +### 4.1 编码和大小写 + +1. 文件编码使用 UTF-8。 +2. 关键字建议小写。 +3. 标识符大小写敏感，推荐使用 `snake_case`。 +4. 后处理器输出到品牌程序时按品牌习惯调整大小写。 + +### 4.2 注释 + +```text +// 单行注释 + +/* + 多行注释 +*/ +``` + +自动生成程序应在关键对象上保留来源注释，但不要为每个采样点生成大量冗余注释。 + +### 4.3 标识符 + +```text +identifier = letter { letter | digit | "_" } +``` + +示例： + +```text +pick_path +weld_op_01 +T_curve_032_000 +``` + +不建议使用中文标识符。UI 可显示中文别名，但程序内部名称应保持 ASCII，便于后处理和版本管理。 + +### 4.4 字面量和单位 + +支持带单位数值： + +```text +100 mm +0.25 m +180 deg +3.14159 rad +300 mm/s +50 % +2 s +200 ms +2.5 kg +``` + +编译器内部统一规范化： + +| 物理量 | 内部单位 | +| --- | --- | +| 长度 | meter | +| 角度 | radian | +| 时间 | second | +| 质量 | kilogram | +| 线速度 | meter/second | +| 角速度 | radian/second | +| 加速度 | meter/second^2 或 radian/second^2 | + +### 4.5 保留关键字 + +```text +language module import end +persistent const var +robot tool frame load target speed zone path operation process +proc func return call +if elseif else switch case default while for to step break continue +label jump +movej movel movec run_path run_operation +set_tool set_frame set_speed set_zone +wait pulse timer +io true false all any rising falling changed +trap interrupt enable disable raise alarm try catch finally +task sync +post_hint source defaults point event before after at +joint_target pose_target pose poseq joints robot_config ext_axis +fine continuous cnt z +``` + +## 5. 顶层结构 + +### 5.1 最小文件 + +```text +language grl 0.1 + +module Main + proc main() + // program entry + end +end +``` + +`language grl 0.1` 用于版本控制。后续语言升级时，编译器可按版本选择兼容规则。 + +### 5.2 顶层声明 + +```text +module ModuleName + import CommonTools + + persistent tool gripper = ... + persistent frame fixture = ... + const speed v_pick = linear(300 mm/s) + const zone z_pick = z10 + + target home = ... + path pick_path { ... } + operation pick_op { ... } + + proc main() + ... + end +end +``` + +顶层可包含： + +1. `import` +2. `persistent` +3. `const` +4. `target` +5. `path` +6. `operation` +7. `proc` +8. `func` +9. `trap` +10. `task` +11. `post_hint` + +## 6. 类型系统 + +### 6.1 基础类型 + +| 类型 | 说明 | +| --- | --- | +| `bool` | 布尔值 | +| `int` | 整数 | +| `real` | 浮点数 | +| `string` | 字符串 | +| `time` | 时间 | +| `length` | 长度 | +| `angle` | 角度 | +| `percent` | 百分比 | + +### 6.2 机器人类型 + +| 类型 | 说明 | +| --- | --- | +| `pose` | 笛卡尔位姿 | +| `joint_array` | 关节数组 | +| `pose_target` | 笛卡尔目标点 | +| `joint_target` | 关节目标点 | +| `tool` | 工具 TCP、负载、惯量 | +| `frame` | 工件坐标或基坐标 | +| `speed` | 运动速度参数 | +| `zone` | 到点/过渡参数 | +| `load` | 负载 | +| `robot_config` | 肩、肘、腕等机器人配置 | +| `ext_axis` | 外部轴 | +| `path` | 路径对象 | +| `operation` | 工艺操作对象 | + +### 6.3 变量声明 + +```text +const int max_retry = 3 +var int retry = 0 +var bool part_ok = false +var pose_target p_tmp = pick offset z 100 mm +persistent real counter = 0 +``` + +规则： + +1. `const` 编译后不可修改。 +2. `persistent` 在项目或控制器状态中持久化。 +3. `var` 默认为局部变量。 +4. 类型可以显式写出，自动生成程序建议显式写出。 + +## 7. 坐标、工具和目标点 + +### 7.1 工具 + +```text +persistent tool gripper = tool { + tcp: pose(0 mm, 0 mm, 180 mm, 0 deg, 0 deg, 0 deg), + mass: 2.5 kg, + cog: [0 mm, 0 mm, 80 mm] +} +``` + +### 7.2 工件坐标 + +```text +persistent frame fixture = frame { + origin: pose(800 mm, 0 mm, 200 mm, 0 deg, 0 deg, 0 deg) +} +``` + +### 7.3 关节目标点 + +```text +target home = joint_target { + joints: [0 deg, -30 deg, 60 deg, 0 deg, 60 deg, 0 deg] +} +``` + +`joint_target` 主要用于 `movej`，后处理可映射到 ABB `jointtarget`、KUKA `E6AXIS` 或 FANUC 关节位置数据。 + +### 7.4 笛卡尔目标点 + +```text +target pick = pose_target { + pose: pose(500 mm, 120 mm, 300 mm, 180 deg, 0 deg, 90 deg), + config: robot_config(0, 0, 1), + tool: gripper, + frame: fixture +} +``` + +`pose()` 参数顺序为： + +```text +pose(x, y, z, rx, ry, rz) +``` + +首版 `rx/ry/rz` 使用固定约定的欧拉角，建议内部立即转换为四元数。为了避免姿态歧义，也允许使用四元数： + +```text +poseq(500 mm, 120 mm, 300 mm, 0, 0, 0.7071068, 0.7071068) +``` + +### 7.5 偏移目标点 + +```text +var pose_target approach = pick offset z 100 mm +var pose_target p2 = pick offset x 20 mm y -10 mm z 50 mm +``` + +偏移默认在目标点所属 `frame` 下解释。需要明确坐标系时： + +```text +var pose_target p3 = pick offset_in tool z -50 mm +var pose_target p4 = pick offset_in frame fixture x 100 mm +``` + +## 8. 速度和过渡 + +### 8.1 速度 + +```text +const speed v_joint_fast = joint(80 %) +const speed v_joint_abs = joint(90 deg/s) +const speed v_pick = linear(300 mm/s) +const speed v_weld = linear(120 mm/s) +const speed v_orient = angular(90 deg/s) +``` + +运动语义： + +1. `joint(...)` 用于关节运动速度。 +2. `linear(...)` 用于 TCP 线速度。 +3. `angular(...)` 可作为姿态插补角速度限制。 +4. 加速度可作为可选参数： + +```text +const speed v_slow = linear(100 mm/s, acc 500 mm/s2) +``` + +### 8.2 过渡 + +```text +const zone z_fine = fine +const zone z10 = z(10 mm) +const zone z50 = z(50 mm) +const zone z_cnt = cnt(30) +const zone z_cont = continuous +``` + +语义： + +1. `fine`：精确到点，不做过渡。 +2. `z(distance)`：以距离近似表示过渡半径，便于映射 ABB `zonedata` 和 KUKA `APO`。 +3. `cnt(percent)`：以百分比表示连续过渡，便于映射 FANUC `CNT`。 +4. `continuous`：允许连续通过，实际过渡由后处理器或虚拟控制器策略决定。 + +## 9. 运动指令 + +### 9.1 基本语法 + +```text +movej TargetExpr [speed Speed] [zone Zone] [tool Tool] [frame Frame] +movel TargetExpr [speed Speed] [zone Zone] [tool Tool] [frame Frame] +movec via ViaTargetExpr target EndTargetExpr [speed Speed] [zone Zone] [tool Tool] [frame Frame] +``` + +示例： + +```text +movej home speed joint(60 %) zone fine tool gripper frame fixture +movel pick speed linear(300 mm/s) zone z10 +movec via arc_mid target arc_end speed linear(150 mm/s) zone fine +``` + +### 9.2 MOVEJ 语义 + +`movej` 表示关节空间运动。机器人按各轴关节角度差分运行，关节同起同停，TCP 轨迹不要求是直线。 + +编译规则： + +1. 目标为 `joint_target` 时直接使用关节数组。 +2. 目标为 `pose_target` 时先用 IK 求终点关节。 +3. 轨迹按关节空间插补。 +4. 后处理映射： + - ABB：`MoveJ` + - FANUC：`J P[...]` + - KUKA：`PTP` + +### 9.3 MOVEL 语义 + +`movel` 表示 TCP 直线运动。TCP 沿起点到终点的空间直线运行，关节角由每个 TCP 采样点 IK 求解。 + +编译规则： + +1. 目标必须能解析为笛卡尔位姿。 +2. 当前点作为直线起点。 +3. 姿态按默认策略插补，首版推荐固定姿态或 slerp。 +4. 后处理映射： + - ABB：`MoveL` + - FANUC：`L P[...]` + - KUKA：`LIN` + +### 9.4 MOVEC 语义 + +`movec` 表示 TCP 圆弧运动。TCP 从当前点出发，经过 via 点，到达 target 点，沿圆弧运行，关节角由每个圆弧采样点 IK 求解。 + +编译规则： + +1. 起点为当前 TCP。 +2. `via` 和 `target` 必须能解析为笛卡尔位姿。 +3. 三点不能重合或近似共线。 +4. 首版使用 via 点位置约束圆弧，姿态按起点到终点插补。 +5. 后处理映射： + - ABB：`MoveC` + - FANUC：`C P[...]` + - KUKA：`CIRC` + +### 9.5 当前工具和当前坐标 + +```text +set_tool gripper +set_frame fixture +set_speed linear(300 mm/s) +set_zone z10 +``` + +参数优先级： + +1. 运动语句显式参数最高。 +2. `path defaults` 次之。 +3. 目标点自带 `tool/frame` 次之。 +4. 当前控制器状态最低。 + +编译到 IR 时，所有运动必须有确定的 `tool`、`frame`、`speed`、`zone`。 + +## 10. Path 语法 + +Path 是自动离线编程的核心对象。CAD 曲线、规划点、示教点、工艺模板应优先生成 Path，而不是直接生成散乱运动语句。 + +### 10.1 基本语法 + +```text +path pick_path { + defaults { + tool: gripper, + frame: fixture, + speed: linear(300 mm/s), + zone: z10 + } + + point approach movej home zone fine + point p1 movel pick offset z 100 mm + point p2 movel pick zone fine +} +``` + +### 10.2 圆弧点 + +```text +path arc_path { + defaults { + tool: torch, + frame: part_frame, + speed: linear(120 mm/s), + zone: fine + } + + point p0 movej start + point p1 movec via mid target end +} +``` + +### 10.3 来源元数据 + +```text +path curve_032_generated { + source { + type: cad_curve + id: "edge_032" + sample_distance: 5 mm + normal_strategy: surface_normal + } + + defaults { + tool: spray_gun, + frame: part_frame, + speed: linear(500 mm/s), + zone: z20, + posture: follow_normal + } + + point p000 movel T_curve_032_000 zone fine + point p001 movel T_curve_032_001 + point p002 movel T_curve_032_002 +} +``` + +来源元数据用于： + +1. 路径重采样。 +2. 目标点重命名。 +3. 重新生成程序。 +4. 报告中定位到 CAD 曲线或工艺对象。 + +### 10.4 Path event + +```text +event before p001 io.do[10] = true +event after p010 io.do[10] = false +event at p005 distance -20 mm pulse io.do[20] duration 100 ms +``` + +语义： + +1. `before`：到达路径点之前执行。 +2. `after`：完成路径点之后执行。 +3. `at ... distance`：相对路径点提前或滞后触发，首版可编译为最近采样点事件。 +4. 后处理到品牌程序时，若品牌不支持精确路径触发，应生成转换报告。 + +### 10.5 `run_path` + +```text +proc main() + run_path pick_path +end +``` + +编译语义： + +1. 展开 Path 的 defaults、points、events。 +2. 每个 point 变为 Motion IR。 +3. event 变为 IO/Wait/Process IR。 +4. 保留 source map，调试时可从程序行定位到路径点。 + +## 11. Operation 语法 + +Operation 表达工艺语义，比 Path 更高一层。 + +```text +operation weld_op_01 { + kind: arc_welding + path: weld_seam_01 + + process { + weld_id: "WELD_1" + voltage: 24.0 + current: 180.0 + weave: none + } + + start_action: + io.do[20] = true + + end_action: + io.do[20] = false +} +``` + +首版建议支持的 `kind`： + +| kind | 用途 | +| --- | --- | +| `handling` | 搬运、上下料 | +| `arc_welding` | 弧焊 | +| `spot_welding` | 点焊 | +| `dispensing` | 涂胶 | +| `spraying` | 喷涂 | +| `grinding` | 打磨 | +| `cutting` | 切割 | +| `generic_path` | 通用路径 | + +`run_operation`： + +```text +proc main() + run_operation weld_op_01 +end +``` + +编译语义： + +1. 执行 `start_action`。 +2. 执行 operation 引用的 path。 +3. 执行 `end_action`。 +4. 保留工艺参数，供仿真 UI、报告和后处理使用。 + +## 12. IO 与等待 + +### 12.1 IO 地址 + +```text +io.di[1] +io.do[1] +io.ai[1] +io.ao[1] +io.gi[1] +io.go[1] +io.ri[1] +io.ro[1] +``` + +推荐通过 `io_map.json` 提供别名： + +```text +io.alias.clamp_open +io.alias.clamp_closed +io.alias.gripper_close_cmd +``` + +### 12.2 IO 写入 + +```text +io.do[1] = true +io.go[1] = 16 +io.ao[1] = 3.5 +``` + +### 12.3 wait + +```text +wait io.di[1] == true +wait io.alias.clamp_closed == true timeout 2 s +wait io.ai[2] > 3.5 timeout 1.5 s +wait all(io.di[1] == true, io.di[2] == false) +wait any(io.di[10] == true, timer.done("T_PICK")) +wait rising(io.di[4]) timeout 5 s +wait falling(io.di[5]) timeout 5 s +``` + +超时处理： + +```text +wait io.di[1] == true timeout 2 s on_timeout alarm "Clamp close timeout" +wait io.di[1] == true timeout 2 s on_timeout call recover_clamp() +``` + +### 12.4 pulse + +```text +pulse io.do[3] duration 200 ms +``` + +虚拟控制器语义： + +1. 立即置位输出。 +2. 虚拟时间到达 duration 后自动复位。 +3. trace 中必须记录置位和复位事件。 + +## 13. 流程控制 + +流程控制用于表达工艺分支、重试、配方选择、批量工位循环和异常恢复。GRL 新程序应优先使用结构化控制流；`label/jump` 主要用于兼容 FANUC TP/LS 等已有程序导入。 + +### 13.1 条件 `if / elseif / else` + +```text +if part_ok + run_path good_path +elseif retry < 3 + call retry_pick() +else + alarm "Pick failed" +end +``` + +语义规则： + +1. `if` 条件表达式必须能转换为 `bool`。 +2. `elseif` 可以出现零次或多次。 +3. `else` 最多出现一次，且必须位于所有 `elseif` 之后。 +4. 条件按顺序求值，命中第一条为 true 的分支后，不再执行后续分支。 +5. 每个 `if` 必须以 `end` 结束。 +6. 条件表达式允许访问变量、IO、函数返回值和比较表达式。 +7. 运动指令允许出现在任意分支内，但编译器必须保证每条分支中的工具、坐标系、速度和 zone 可解析。 + +常用条件表达式： + +```text +if io.di[1] == true +if count >= 3 and not part_ok +if is_part_ready() +if recipe_id == 2 or recipe_id == 3 +``` + +### 13.2 `while` 循环 + +```text +while retry < 3 + call try_pick() + if part_ok + break + end + retry = retry + 1 +end +``` + +语义规则： + +1. `while` 条件必须为 `bool`。 +2. 每次进入循环前求值。 +3. `break` 立即退出当前循环。 +4. `continue` 跳过本轮剩余语句，进入下一轮条件判断。 +5. 编译器建议对可能无限循环的 `while true` 给出 warning，除非循环体内包含明确的 `break`、`return`、`raise` 或 `wait`。 + +### 13.3 `for` 循环 + +```text +for i = 0 to 9 step 1 + call process_slot(i) +end +``` + +也允许省略 `step`，默认步长为 `1`： + +```text +for layer = 1 to layer_count + call weld_layer(layer) +end +``` + +反向循环： + +```text +for i = 10 to 0 step -1 + call clear_slot(i) +end +``` + +语义规则： + +1. 循环变量默认是当前 `for` 块局部变量。 +2. 起点、终点、步长必须是整数或可安全转换为整数。 +3. `step` 不能为 `0`。 +4. `break` 和 `continue` 只影响当前最近一层循环。 +5. 后处理到不支持结构化 `for` 的品牌时，可展开为标签和跳转，但必须保持 source map。 + +### 13.4 `switch / case / default` + +```text +switch recipe_id + case 1 + run_operation op_a + case 2 + run_operation op_b + default + alarm "Unknown recipe" +end +``` + +语义规则： + +1. `switch` 表达式可以是 `int`、`bool`、`string` 或枚举型。 +2. `case` 值必须是常量表达式。 +3. 每个 `case` 默认不向下贯穿，不需要写 `break`。 +4. `default` 最多出现一次。 +5. 若需要多个值进入同一分支，可以使用逗号： + +```text +switch recipe_id + case 1, 2, 3 + run_operation common_recipe + case 10 + run_operation special_recipe + default + alarm "Unknown recipe" +end +``` + +6. 导出到 FANUC TP/LS 时，`switch` 可转换为 `SELECT`、条件跳转或 `LBL/JMP` 组合。 +7. 导出到 ABB/KUKA 时，优先转换为品牌结构化分支。 + +### 13.5 `break` 和 `continue` + +```text +while true + call check_part() + if part_ok + break + end + retry = retry + 1 + if retry < max_retry + continue + end + alarm "Retry failed" + break +end +``` + +规则： + +1. `break` 只能出现在 `while`、`for` 或 `switch` 中。 +2. `continue` 只能出现在 `while` 或 `for` 中。 +3. 在 `switch` 中不需要 `break` 防止贯穿，因为 GRL 默认不贯穿。 + +### 13.6 label 和 jump + +```text +label retry +call try_pick() +if not part_ok + jump retry +end +``` + +新程序不建议优先使用 `label/jump`，但必须支持它，因为 FANUC TP/LS 程序常用 `LBL/JMP`，导入时需要映射。 + +`label/jump` 规则： + +1. `label` 名称在当前 `proc` 内唯一。 +2. `jump` 只能跳转到当前 `proc` 内的 label。 +3. 禁止 `jump` 进入另一个 `if/while/for/switch/try` 块内部。 +4. 允许 `jump` 跳出块结构，但编译器应生成 warning。 +5. 自动生成的新程序不应使用 `label/jump`，除非目标后处理配置要求 FANUC 风格输出。 + +## 14. 子程序和函数 + +### 14.1 `proc` 子程序 + +`proc` 是无返回值子程序，用于组织工艺步骤、运动流程、IO 流程和恢复逻辑。 + +```text +proc pick_part(int slot) + movej home speed joint(60 %) zone fine + run_path pick_path +end +``` + +调用： + +```text +call pick_part(1) +call pick_part(slot_id) +``` + +语义规则： + +1. `proc` 可以执行运动、IO、wait、run_path、run_operation。 +2. `proc` 可以调用其他 `proc`。 +3. 默认不允许无限递归。编译器应检测直接递归和明显的间接递归，并给出 warning 或 error。 +4. `return` 可以提前退出 `proc`，但不能携带返回值。 +5. `proc main()` 是默认入口。一个模块中最多一个默认入口。 + +### 14.2 参数方向 + +参数支持 `in`、`out`、`inout` 三种方向。未写方向时默认为 `in`。 + +```text +proc pick_part(in int slot) + call move_to_slot(slot) +end + +proc read_part(out bool ok) + ok = io.di[1] +end + +proc increment_retry(inout int retry) + retry = retry + 1 +end +``` + +规则： + +1. `in` 参数按值传入，子程序内修改不影响调用者。 +2. `out` 参数调用前不要求有有效值，子程序必须在所有正常返回路径上赋值。 +3. `inout` 参数按引用语义传入，子程序内修改会回写调用者。 +4. `out` 和 `inout` 实参必须是可赋值左值，不能是字面量或表达式。 +5. 后处理到不支持显式参数方向的品牌时，可生成临时变量、寄存器或转换报告。 + +调用示例： + +```text +var bool ok = false +var int retry = 0 + +call read_part(ok) +call increment_retry(retry) +``` + +### 14.3 `func` 函数 + +`func` 有返回值，适合计算条件、选择目标点、计算偏移、读取配方参数。函数应尽量无副作用。 + +```text +func bool is_part_ready() + return io.di[1] == true and io.di[2] == false +end +``` + +带参数函数： + +```text +func pose_target slot_pose(int slot) + return base_pick offset x (slot * 50 mm) +end + +func bool can_retry(int retry, int max_retry) + return retry < max_retry +end +``` + +调用方式： + +```text +if can_retry(retry, max_retry) + call retry_pick() +end + +movel slot_pose(slot_id) speed linear(200 mm/s) zone z10 +``` + +规则： + +1. `func` 必须声明返回类型。 +2. 所有正常返回路径必须返回兼容类型的值。 +3. `func` 允许调用其他 `func`。 +4. `func` 默认不允许执行运动指令、`wait`、`pulse`、`run_path`、`run_operation`。编译器应把这类用法作为 error 或强 warning。 +5. 如果确实需要带副作用的逻辑，应使用 `proc`。 + +### 14.4 作用域和名称解析 + +名称解析顺序： + +1. 当前块局部变量。 +2. 当前 `proc/func` 参数。 +3. 当前模块中的 `const/var/persistent/target/path/operation/proc/func`。 +4. `import` 模块导出的符号。 +5. 内置函数和内置类型。 + +规则： + +1. 内层局部变量允许遮蔽外层变量，但编译器应对同名遮蔽给出 warning。 +2. 不允许局部变量与同一作用域内的 `proc/func/target/path/operation` 重名。 +3. `for` 循环变量只在循环体内有效。 +4. `label` 只在当前 `proc` 内有效。 + +### 14.5 子程序调用和后处理 + +后处理映射： + +| GRL | ABB RAPID | FANUC LS/TP | KUKA KRL | +| --- | --- | --- | --- | +| `proc name()` | `PROC name()` | `/PROG NAME` 或子程序 | `DEF name()` | +| `call sub()` | `sub;` 或 `sub()` | `CALL SUB` | `sub()` | +| `return` | `RETURN` | `END`/跳转近似 | `RETURN` | +| `out/inout` 参数 | `VAR` 参数或数据变量 | 寄存器/PR/临时变量近似 | 参数或全局变量近似 | + +如果目标品牌或输出格式不支持某种参数模式，后处理器必须生成转换报告，不能静默丢失语义。 + +## 15. 异常、报警和中断 + +### 15.1 alarm 和 raise + +```text +alarm "Part not detected" +raise E_PICK_FAILED +``` + +### 15.2 try/catch + +```text +try + run_operation pick_op +catch E_PICK_FAILED + call recover_pick() +finally + io.do[1] = false +end +``` + +首版可先实现 `alarm`、`raise` 和简单 `catch`，复杂品牌错误恢复分阶段支持。 + +### 15.3 trap 和 interrupt + +```text +trap emergency_stop() + stop_motion + io.do[100] = false + alarm "Emergency stop input" +end + +interrupt e_stop when io.di[100] == true call emergency_stop() +enable interrupt e_stop +disable interrupt e_stop +``` + +后处理映射： + +1. ABB 可映射到 `TRAP` 和 interrupt 机制。 +2. KUKA 可映射到 `INTERRUPT DECL`、`INTERRUPT ON/OFF` 等近似结构。 +3. FANUC 首版可生成后台逻辑、条件监控或转换报告。 + +## 16. 多任务 + +首版只定义语法，不要求完整实现多任务实时调度： + +```text +task background monitor_io cycle 20 ms + if io.di[99] == true + alarm "Safety signal lost" + end +end +``` + +虚拟控制器可先按周期任务模拟；后处理时根据品牌能力生成后台任务或转换报告。 + +## 17. 品牌扩展和后处理提示 + +### 17.1 post_hint + +```text +post_hint abb { + module_name: "MainModule" + use_wobj: true +} + +post_hint fanuc { + program_name: "MAIN" + default_uframe: 1 + default_utool: 1 +} + +post_hint kuka { + src_name: "MAIN" + dat_name: "MAIN" + advance: 3 +} +``` + +### 17.2 brand metadata + +```text +@brand.abb { + conf_l: true +} + +@brand.fanuc { + group: 1 + cnt: 50 +} + +@brand.kuka { + c_dis: true +} +``` + +规则： + +1. `@brand.*` 只影响指定品牌后处理。 +2. 通用虚拟控制器不应依赖品牌 metadata 才能执行。 +3. 无法支持的品牌扩展必须进入转换报告。 + +## 18. 自动编程生成规则 + +自动生成 GRL 时，应遵循以下规则： + +1. 优先生成 `target`、`path`、`operation`，不要直接把大量运动语句塞进 `proc main()`。 +2. 目标点命名稳定，推荐 `T_{path}_{index}` 或语义名称，例如 `T_PICK_APPROACH`。 +3. 路径点命名稳定，推荐 `p000`、`p001`、`p002`。 +4. 路径整体参数放入 `defaults`，单点差异写在 point 上。 +5. 接近点、工艺开始点、工艺结束点、离开点必须显式标注。 +6. IO、夹具、焊接开关、喷涂开关应生成 path event 或 operation action。 +7. CAD 来源、曲线 ID、采样距离、法向策略必须写入 `source`。 +8. 自动生成文本必须格式化稳定，同一输入重复生成结果一致。 +9. 编译器应能把 GRL 再解析回等价 OLP 对象模型。 +10. 后处理器应能选择 `compact` 或 `expanded` 输出风格。 + +生成风格： + +```text +// compact +proc main() + run_operation weld_op_01 +end + +// expanded +proc main() + movel T001 speed linear(120 mm/s) zone z5 tool torch frame part_frame + movel T002 speed linear(120 mm/s) zone z5 tool torch frame part_frame +end +``` + +## 19. 后处理映射规则 + +### 19.1 ABB RAPID + +| GRL | ABB RAPID | +| --- | --- | +| `module` | `MODULE` | +| `proc` | `PROC` | +| `movej` | `MoveJ` | +| `movel` | `MoveL` | +| `movec` | `MoveC` | +| `pose_target` | `robtarget` | +| `joint_target` | `jointtarget` | +| `tool` | `tooldata` | +| `frame` | `wobjdata` | +| `linear(300 mm/s)` | `v300` 或自定义 `speeddata` | +| `fine`、`z(10 mm)` | `fine`、`z10` 或自定义 `zonedata` | +| `io.do[1] = true` | `SetDO do1, 1` | +| `wait io.di[1] == true` | `WaitDI di1, 1` | + +示例： + +```text +movel pick speed linear(300 mm/s) zone z10 tool gripper frame fixture +``` + +可后处理为： + +```text +MoveL pick, v300, z10, gripper\WObj:=fixture; +``` + +### 19.2 FANUC LS/TP 风格 + +| GRL | FANUC | +| --- | --- | +| `proc main()` | `/PROG MAIN` | +| `movej` | `J P[...]` | +| `movel` | `L P[...]` | +| `movec` | `C P[...]` | +| `pose_target` | `P[...]` 或 `PR[...]` | +| `tool` | UTOOL | +| `frame` | UFRAME | +| `joint(50 %)` | `50%` | +| `linear(300 mm/s)` | `300mm/sec` | +| `fine` | `FINE` | +| `cnt(50)` | `CNT50` | +| `io.do[1] = true` | `DO[1]=ON` | +| `wait io.di[1] == true` | `WAIT DI[1]=ON` | + +示例： + +```text +movel P10 speed linear(300 mm/s) zone cnt(10) +``` + +可后处理为： + +```text +L P[10] 300mm/sec CNT10 ; +``` + +### 19.3 KUKA KRL + +| GRL | KUKA KRL | +| --- | --- | +| `proc main()` | `DEF MAIN()` | +| `movej` | `PTP` | +| `movel` | `LIN` | +| `movec` | `CIRC` | +| `pose_target` | `E6POS` | +| `joint_target` | `E6AXIS` | +| `tool` | `$TOOL` | +| `frame` | `$BASE` | +| `linear(300 mm/s)` | `$VEL.CP = 0.3` | +| `fine` | no approximation | +| `continuous`、`z(...)` | `C_DIS` 或 `$APO` | +| `io.do[1] = true` | `$OUT[1] = TRUE` | +| `wait io.di[1] == true` | `WAIT FOR $IN[1] == TRUE` | + +示例： + +```text +movel XPICK speed linear(300 mm/s) zone continuous +``` + +可后处理为： + +```text +$VEL.CP = 0.3 +LIN XPICK C_DIS +``` + +## 20. 编译语义和 IR + +编译管线： + +```text +GRL Source + -> Lexer + -> Parser + -> AST + -> Symbol Table + -> Semantic Analyzer + -> Executable IR + -> Virtual Controller + -> Post Processor +``` + +运动 IR 示例： + +```ts +interface MotionInstruction { + kind: "motion"; + motionType: "joint" | "linear" | "circular"; + target: TargetRef | ResolvedTarget; + via?: TargetRef | ResolvedTarget; + speed: SpeedSpec; + zone: ZoneSpec; + tool: ToolRef; + frame: FrameRef; + sourceRange: SourceRange; + sourceObject?: { + pathId?: string; + pointId?: string; + operationId?: string; + }; + brandMeta?: Record; +} +``` + +## 21. 语义检查 + +编译器必须检查： + +1. 标识符是否重复或未声明。 +2. 类型是否匹配。 +3. 目标点类型是否适合运动指令。 +4. `movec` 是否缺少 via 点。 +5. 圆弧三点是否重合或共线。 +6. 工具、坐标系、速度、过渡是否可解析。 +7. 单位是否正确。 +8. IO 地址是否存在于 IO map 或允许范围。 +9. 子程序参数数量和类型是否匹配。 +10. `out` 参数是否在所有正常返回路径上赋值。 +11. `inout` 和 `out` 实参是否为可赋值左值。 +12. `func` 是否在所有正常返回路径上返回正确类型。 +13. `break`、`continue` 是否出现在合法结构内。 +14. `switch case` 是否为常量表达式，是否存在重复 case。 +15. `jump` 是否跳入非法块结构。 +16. `wait` 是否有可执行条件。 +17. Path 是否为空。 +18. Path point 名称是否重复。 +19. Operation 是否引用不存在的 Path。 +20. 目标点是否可达。 +21. 关节是否超限。 +22. 后处理目标品牌是否支持所用语义。 + +诊断格式建议： + +```text +E1001: target 'pick' is not defined +E2003: movec points are collinear: start=p0, via=p1, target=p2 +W3002: zone z50 is larger than segment length +W4001: KUKA post approximates GRL cnt(30) by C_DIS +``` + +## 22. EBNF 语法草案 + +以下是首版解析器可采用的简化 EBNF： + +```text +program = [ language_decl ] module_decl ; +language_decl = "language" "grl" version ; +module_decl = "module" identifier { top_decl } "end" ; + +top_decl = import_decl + | data_decl + | target_decl + | path_decl + | operation_decl + | proc_decl + | func_decl + | trap_decl + | task_decl + | post_hint_decl ; + +import_decl = "import" identifier ; +data_decl = ( "persistent" | "const" | "var" ) type identifier "=" expr ; +target_decl = "target" identifier "=" ( joint_target | pose_target ) ; + +joint_target = "joint_target" "{" "joints" ":" array [ "," ext_axis ] "}" ; +pose_target = "pose_target" "{" "pose" ":" pose_expr + [ "," "config" ":" config_expr ] + [ "," "tool" ":" identifier ] + [ "," "frame" ":" identifier ] "}" ; + +path_decl = "path" identifier "{" { path_item } "}" ; +path_item = defaults_block | source_block | path_point | path_event ; +defaults_block = "defaults" "{" { property } "}" ; +source_block = "source" "{" { property } "}" ; +path_point = "point" identifier motion_stmt ; +path_event = "event" ( "before" | "after" ) identifier statement ; + +operation_decl = "operation" identifier "{" + "kind" ":" identifier + "path" ":" identifier + [ process_block ] + [ action_block ] + "}" ; + +proc_decl = "proc" identifier "(" [ param_list ] ")" { statement } "end" ; +func_decl = "func" type identifier "(" [ param_list ] ")" { statement } "end" ; +trap_decl = "trap" identifier "(" ")" { statement } "end" ; + +statement = motion_stmt + | run_stmt + | assign_stmt + | wait_stmt + | pulse_stmt + | if_stmt + | while_stmt + | for_stmt + | switch_stmt + | call_stmt + | return_stmt + | break_stmt + | continue_stmt + | label_stmt + | jump_stmt + | alarm_stmt + | try_stmt ; + +motion_stmt = movej_stmt | movel_stmt | movec_stmt ; +target_expr = target_ref [ offset_clause ] | pose_expr ; +offset_clause = "offset" { axis length } + | "offset_in" ( "tool" | "frame" identifier ) { axis length } ; +movej_stmt = "movej" target_expr motion_params ; +movel_stmt = "movel" target_expr motion_params ; +movec_stmt = "movec" "via" target_expr "target" target_expr motion_params ; + +motion_params = [ "speed" speed_expr ] [ "zone" zone_expr ] + [ "tool" identifier ] [ "frame" identifier ] ; + +run_stmt = "run_path" identifier | "run_operation" identifier ; +wait_stmt = "wait" expr [ "timeout" duration ] [ "on_timeout" timeout_action ] ; +pulse_stmt = "pulse" io_ref "duration" duration ; +assign_stmt = lvalue "=" expr ; +call_stmt = "call" identifier "(" [ arg_list ] ")" ; +return_stmt = "return" [ expr ] ; +break_stmt = "break" ; +continue_stmt = "continue" ; + +if_stmt = "if" expr { statement } + { "elseif" expr { statement } } + [ "else" { statement } ] + "end" ; + +while_stmt = "while" expr { statement } "end" ; +for_stmt = "for" identifier "=" expr "to" expr [ "step" expr ] { statement } "end" ; + +switch_stmt = "switch" expr { case_clause } [ default_clause ] "end" ; +case_clause = "case" const_expr { "," const_expr } { statement } ; +default_clause = "default" { statement } ; + +label_stmt = "label" identifier ; +jump_stmt = "jump" identifier ; + +param_list = param { "," param } ; +param = [ "in" | "out" | "inout" ] type identifier ; +arg_list = expr { "," expr } ; + +expr = literal + | identifier + | call_expr + | io_ref + | unary_expr + | binary_expr + | "(" expr ")" ; +call_expr = identifier "(" [ arg_list ] ")" ; +``` + +## 23. 完整示例 + +```text +language grl 0.1 + +module Main + + persistent tool gripper = tool { + tcp: pose(0 mm, 0 mm, 180 mm, 0 deg, 0 deg, 0 deg), + mass: 2.5 kg + } + + persistent frame fixture = frame { + origin: pose(800 mm, 0 mm, 200 mm, 0 deg, 0 deg, 0 deg) + } + + const speed v_fast = joint(70 %) + const speed v_pick = linear(300 mm/s) + const zone z_pick = z(10 mm) + + target home = joint_target { + joints: [0 deg, -30 deg, 60 deg, 0 deg, 60 deg, 0 deg] + } + + target pick = pose_target { + pose: pose(500 mm, 120 mm, 300 mm, 180 deg, 0 deg, 90 deg), + config: robot_config(0, 0, 1), + tool: gripper, + frame: fixture + } + + target place = pose_target { + pose: pose(650 mm, -100 mm, 320 mm, 180 deg, 0 deg, 90 deg), + config: robot_config(0, 0, 1), + tool: gripper, + frame: fixture + } + + path pick_path { + defaults { + tool: gripper, + frame: fixture, + speed: v_pick, + zone: z_pick + } + + point approach movej home speed v_fast zone fine + point above_pick movel pick offset z 100 mm + point at_pick movel pick zone fine + event after at_pick io.do[1] = true + event after at_pick wait io.di[1] == true timeout 2 s on_timeout alarm "Clamp close timeout" + point leave_pick movel pick offset z 100 mm + } + + path place_path { + defaults { + tool: gripper, + frame: fixture, + speed: v_pick, + zone: z_pick + } + + point above_place movel place offset z 100 mm + point at_place movel place zone fine + event after at_place io.do[1] = false + event after at_place wait io.di[2] == true timeout 2 s on_timeout alarm "Clamp open timeout" + point leave_place movel place offset z 100 mm + } + + operation pick_op { + kind: handling + path: pick_path + process { + gripper: "main_clamp" + } + } + + operation place_op { + kind: handling + path: place_path + process { + gripper: "main_clamp" + } + } + + proc main() + set_tool gripper + set_frame fixture + + run_operation pick_op + run_operation place_op + + movej home speed v_fast zone fine + end + +end +``` + +## 24. 实施优先级 + +P0 必须实现： + +1. `language`、`module`、`proc`。 +2. `const`、`var`、`persistent`。 +3. `tool`、`frame`、`speed`、`zone`。 +4. `joint_target`、`pose_target`。 +5. `movej`、`movel`、`movec`。 +6. `path`、`point`、`event`、`run_path`。 +7. `operation`、`run_operation`。 +8. `if`、`elseif`、`else`、`while`、`for`、`switch`。 +9. `call`、`return`、`break`、`continue`。 +10. `proc` 参数方向 `in/out/inout`。 +11. `func`、函数调用表达式和返回值检查。 +12. `io.do/di`、`wait`、`pulse`。 +13. AST、语义检查、IR、source map。 +14. ABB、FANUC、KUKA 后处理原型。 + +P1 扩展： + +1. `label/jump` 和品牌标签程序导入。 +2. `trap`、`interrupt`。 +3. 多任务 `task`。 +4. 更完整的 IO 类型。 +5. 工艺模板库。 +6. 品牌程序导入。 +7. 复杂品牌扩展和转换报告。 + +## 25. 参考资料 + +1. ABB RAPID Technical Reference Manual，RAPID Instructions, Functions and Data Types： + https://library.e.abb.com/public/b227fcd260204c4dbeb8a58f8002fe64/Rapid_instructions.pdf + +2. ABB RAPID Overview： + https://search.abb.com/library/Download.aspx?Action=Launch&DocumentID=3HAC050947-001&DocumentPartId=&LanguageCode=en + +3. KUKA System Software，说明 KSS 支持 InLine forms 和 KRL 专家编程： + https://www.kuka.com/de-de/produkte-leistungen/robotersysteme/software/systemsoftware/kuka_systemsoftware + +4. KUKA Application and Robot Programming，说明 KRL、Sunrise programming、simulation/offline programming 等工作流： + https://www.kuka.com/en-de/services/service_robots-and-machines/installation-start-up-and-programming-of-robots/application-and-robot-programming + +5. FANUC PLC Motion Interface，说明 FANUC 可从上位控制发起 linear、joint、circular robot motion，并管理 speeds 与 termination types： + https://www.fanucamerica.com/products/software/plc-motion-interface + +6. FANUC ROBOGUIDE，说明 FANUC 离线编程可生成机器人程序，并支持 CAD to Path、仿真和路径开发： + https://www.fanucamerica.com/products/software/roboguide + +7. FANUC ASCII Program Loader，说明 FANUC LS 可读程序可编译为 TP 程序： + https://www.fanucamerica.com/products/controller-series/r-50ia diff --git a/work/doc/通用机器人项目主要实施步骤.md b/work/doc/通用机器人项目主要实施步骤.md new file mode 100644 index 0000000..a9d3dd1 --- /dev/null +++ b/work/doc/通用机器人项目主要实施步骤.md @@ -0,0 +1,530 @@ +# 通用机器人项目主要实施步骤 + +版本：0.1 +日期：2026-06-27 +来源：`/home/meswork/kdl_work/work/working1` +用途：作为阅读参考，概览项目从 KDL WASM 计算接口到 GRL 编译与后处理的主要实施步骤。 + +## 1. 实施主线 + +项目实施分两条主线推进： + +1. KDL WASM 计算接口线 + - 任务编号：`KW-001` 到 `KW-013` + - 对标：`KDL_WASM计算接口设计.md` + - 目标：提供稳定的 Worker API、机器人模型、运动学、轨迹规划、Path 验证、诊断和性能能力。 + +2. GRL 编程语法线 + - 任务编号：`KW-100` 到 `KW-112` + - 对标：`通用机器人编程语法规范.md` + - 目标：实现 GRL lexer/parser、AST、语义检查、IR、Path/Operation、IO/wait、后处理和自动生成。 + +两条线的集成点是：GRL 的 `movej/movel/movec/path/operation` 编译为 KDL WASM 的 `MoveJRequest/MoveLRequest/MoveCRequest/PathPlanRequest`，由 KDL WASM 返回轨迹和诊断。 + +## 2. 推荐工程结构 + +```text +/home/meswork/kdl_work/ + orocos_kinematics_dynamics/ + orocos_kdl/ + kdl-wasm/ + CMakeLists.txt + bindings/ + kdl_c_api.cpp + kdl_embind.cpp + web/ + src/ + kdl/ + kdlClient.ts + kdl.worker.ts + rpc.ts + types.ts + robot/ + urdfParser.ts + normalizedRobotModel.ts + grl/ + lexer/ + parser/ + ast/ + semantic/ + ir/ + generator/ + post/ + abb/ + fanuc/ + kuka/ + tests/ + kdl/ + grl/ + integration/ + post/ +``` + +## 3. 阶段 1：基础工程、Worker RPC 和 GRL 词法骨架 + +关联任务：`KW-001`、`KW-100`、`KW-101` + +目标： + +1. 建立 `kdl-wasm` wrapper 工程。 +2. 使用 Emscripten 编译 Orocos KDL，生成 `kdl.js`、`kdl.wasm`、`kdl.d.ts`。 +3. 建立 `KdlRpcRequest/KdlRpcResponse` 和 `KdlWorkerClient`。 +4. KDL WASM 只在 Worker 中运行。 +5. 建立 GRL lexer，支持注释、标识符、字符串、数字、单位和保留关键字。 +6. 建立 GRL parser 和 AST 骨架，支持 `language grl 0.1`、`module`、`import`、顶层声明。 + +输入： + +1. Orocos KDL 源码。 +2. GRL 源文件。 + +输出： + +1. KDL WASM 构建产物。 +2. Worker RPC 基础 API。 +3. GRL Tokens 和 AST。 + +验收重点： + +1. `init()` 返回 `KdlRuntimeInfo`。 +2. Worker 请求有唯一 id。 +3. 错误返回 `{ code, message, diagnostics }`。 +4. GRL 最小文件可解析。 +5. AST 保留 source range、注释位置、单位原文和规范化值。 + +## 4. 阶段 2：机器人模型、GRL 数据声明和共享类型 + +关联任务：`KW-002`、`KW-102` + +目标： + +1. TypeScript 解析 URDF XML。 +2. 检查 link/joint 连通性和单位。 +3. 生成 `NormalizedRobotModel`。 +4. WASM 根据标准模型构造 KDL `Tree/Chain`。 +5. 创建 `RobotHandle` 并缓存求解器。 +6. GRL 支持 `const/var/persistent`、基础类型、机器人类型。 +7. GRL 支持 `tool/frame/load/joint_target/pose_target/pose/poseq/robot_config/ext_axis/speed/zone/offset/offset_in`。 + +输入： + +1. `robot.urdf` +2. GRL 中的 tool/frame/target/speed/zone 声明。 + +输出： + +1. `NormalizedRobotModel` +2. `RobotHandle` +3. `JointTarget` +4. `PoseTarget` +5. `SpeedSpec` +6. `ZoneSpec` +7. `OffsetSpec` + +验收重点： + +1. URDF joint 顺序稳定。 +2. base/tip 不连通返回 `KDL_INVALID_MODEL`。 +3. `getRobotInfo/getJointLimits` 正确。 +4. GRL target 能编译为 KDL 共享数据结构。 +5. speed/zone 能编译为 `SpeedSpec/ZoneSpec`。 + +## 5. 阶段 3：KDL 基础运动学和 GRL 运动指令 + +关联任务：`KW-003` 到 `KW-006`、`KW-103` + +目标： + +1. 实现 KDL `fk`、`fkAllLinks`。 +2. 实现 KDL `ik`、`ikBatch`。 +3. 实现 KDL `jacobian`、`checkSingularity`、`checkJointLimits`、`checkReachability`、`checkReachabilityBatch`、`checkVelocityLimits`。 +4. 实现 KDL `normalizePose`、`composePose`、`inversePose`、`applyToolAndFrame`、`applyOffset`。 +5. GRL 支持 `movej/movel/movec` 和 `set_tool/set_frame/set_speed/set_zone`。 +6. GRL 运动指令编译为 `MotionInstruction`，再映射为 KDL request。 + +输入： + +1. `RobotHandle` +2. `JointTarget` +3. `PoseTarget` +4. `tool/frame/speed/zone` +5. GRL 运动语句。 + +输出： + +1. FK/TCP 位姿。 +2. IK 关节解。 +3. Jacobian 和奇异性诊断。 +4. `MotionInstruction` +5. `MoveJRequest/MoveLRequest/MoveCRequest` + +验收重点： + +1. FK 与 golden 数据或原生 KDL 对比在容差内。 +2. IK 后 FK 回代误差小于容差。 +3. `ikBatch` 返回顺序与输入顺序一致。 +4. 奇异点返回 `KDL_SINGULARITY` warning。 +5. `offset`、`offset_in tool`、`offset_in frame` 结果正确。 +6. GRL 编译到 IR 前能解析确定的 tool、frame、speed、zone。 + +## 6. 阶段 4：梯形速度和三类基础运动轨迹 + +关联任务：`KW-007` 到 `KW-010` + +目标： + +1. 实现 `makeTrapProfile` 和 `sampleTrapProfile`。 +2. 实现 `planMoveJ`。 +3. 实现 `planMoveL`。 +4. 实现 `planMoveC`。 + +MOVEJ 重点： + +1. `joint_target` 直接作为 `qEnd`。 +2. `pose_target` 先 IK 得到 `qEnd`。 +3. 各关节同起同停。 +4. 每个采样点 FK 输出 TCP。 + +MOVEL 重点： + +1. 起点由 `startJoints` FK 得到。 +2. 目标点应用 tool/frame/offset。 +3. 生成 TCP 直线采样。 +4. 逐点 IK，seed 使用上一采样点关节。 + +MOVEC 重点： + +1. 起点由当前关节 FK 得到。 +2. via 和 target 应用 tool/frame/offset。 +3. 检查三点重合或近似共线。 +4. 计算圆心、半径、法向、角度、弧长。 +5. `TrajectoryResult.meta.circle` 包含 `CirclePlanMeta`。 + +输入： + +1. `MoveJRequest` +2. `MoveLRequest` +3. `MoveCRequest` +4. `TrapProfileOptions` + +输出： + +1. `TrapProfileResult` +2. `TrajectoryResult` +3. `MotionDiagnostic` + +验收重点： + +1. 梯形速度曲线长距离为 trapezoid，短距离为 triangle。 +2. 采样首点 `s=0`，末点 `s=1`，`s` 单调递增。 +3. MOVEJ 关节同起同停。 +4. MOVEL TCP 直线误差小于容差。 +5. MOVEC 圆弧元数据正确。 +6. 三点共线返回 `KDL_ARC_DEGENERATE`。 +7. P0 中非 fine zone 返回 `KDL_ZONE_APPROXIMATED`。 + +## 7. 阶段 5：Path、Operation 和批量路径验证 + +关联任务：`KW-011`、`KW-104`、`KW-105` + +目标： + +1. GRL 支持 `path/defaults/source/point/event/run_path`。 +2. GRL 支持 `operation/kind/path/process/start_action/end_action/run_operation`。 +3. Path 编译为 `PathPlanRequest`。 +4. Operation 展开为 start action + path + end action。 +5. KDL 实现 `planPath` 和 `validatePath`。 + +输入： + +1. GRL Path。 +2. GRL Operation。 +3. `MotionSegmentRequest[]` +4. `PathPlanRequest` + +输出： + +1. `PathPlanResult` +2. `PathValidationResult` +3. Path source map。 +4. Operation 展开结果。 + +验收重点： + +1. 空 Path 报错。 +2. 重复 point 名称报错。 +3. `run_path` 可生成 `PathPlanRequest`。 +4. `planPath` 按 segment 顺序规划。 +5. 上一段终点关节作为下一段起点。 +6. 轨迹点合并后重新编号和更新时间。 +7. 保留 `segmentId/targetId/sourceMap`。 +8. `run_operation` 展开后 KDL 只处理 motion segment。 + +## 8. 阶段 6：IO、wait、pulse 和流程控制 + +关联任务：`KW-106`、`KW-107` + +目标： + +1. 支持 `io.di/do/ai/ao/gi/go/ri/ro`。 +2. 支持 `io.alias.*`。 +3. 支持 IO 赋值。 +4. 支持 `wait` 条件、`timeout`、`on_timeout alarm/call`。 +5. 支持 `all/any/rising/falling/changed`。 +6. 支持 `pulse`。 +7. 支持 `if/elseif/else`、`while`、`for`、`switch/case/default`。 +8. 支持 `break/continue/label/jump`。 + +输入： + +1. IO map。 +2. GRL IO/wait/pulse 语句。 +3. GRL 流程控制语句。 + +输出： + +1. `IoInstruction` +2. `WaitInstruction` +3. `BranchInstruction` +4. pulse IR。 + +验收重点： + +1. IO 地址按 io_map 或允许范围校验。 +2. wait 条件可编译。 +3. pulse trace 必须包含置位和复位事件。 +4. 条件表达式必须为 bool。 +5. `break/continue` 位置合法。 +6. `switch case` 为常量表达式且不重复。 +7. `jump` 不能跳入非法块结构。 +8. IO、wait、pulse 和流程控制不直接进入 KDL。 + +## 9. 阶段 7：proc、func、异常、报警、中断和多任务语法 + +关联任务：`KW-108`、`KW-109` + +目标： + +1. 支持 `proc`、`func`、`call`、`return`。 +2. 支持参数方向 `in/out/inout`。 +3. 实现作用域和名称解析。 +4. 对递归给出 warning 或 error。 +5. 支持 `alarm`、`raise`、`try/catch/finally`。 +6. P1 语法保留 `trap/interrupt/enable/disable/task cycle`。 + +输入： + +1. GRL 子程序和函数。 +2. GRL 异常和中断语法。 + +输出： + +1. `CallInstruction` +2. `ReturnInstruction` +3. `AlarmInstruction` +4. 异常处理 IR。 +5. P1 语法 AST。 + +验收重点： + +1. `out` 参数所有正常返回路径赋值。 +2. `inout/out` 实参必须为左值。 +3. `func` 所有正常返回路径返回兼容类型。 +4. `func` 默认不允许执行运动、wait、pulse、run_path、run_operation。 +5. P0 支持 alarm/raise/try/catch 基础语义。 +6. P1 未实现语义必须在后处理或运行时报明确诊断。 + +## 10. 阶段 8：语义检查、IR、source map 和 KDL 集成 + +关联任务：`KW-110`、`KW-012` + +目标： + +1. 实现 Symbol Table。 +2. 实现 Semantic Analyzer。 +3. 生成 Executable IR。 +4. 保留 source map。 +5. 完成 GRL 到 KDL request 的编译桥接。 +6. KDL 实现 `estimateCycleTime` 和 `resampleTrajectory`。 +7. 建立统一诊断分级：error、warning、info。 + +必须检查： + +1. 标识符重复或未声明。 +2. 类型是否匹配。 +3. 目标点类型是否适合运动指令。 +4. `movec` 是否缺少 via 点。 +5. 圆弧三点是否重合或共线。 +6. 工具、坐标系、速度、过渡是否可解析。 +7. 单位是否正确。 +8. IO 地址是否存在。 +9. 子程序参数数量和类型是否匹配。 +10. `out` 参数是否赋值。 +11. `func` 返回路径是否正确。 +12. `break/continue/jump` 是否合法。 +13. Path 是否为空或点名重复。 +14. Operation 是否引用不存在的 Path。 +15. 目标点是否可达。 +16. 关节是否超限。 +17. 后处理目标品牌是否支持所用语义。 + +输入： + +1. AST。 +2. Symbol Table。 +3. `RobotHandle`。 +4. KDL 检查结果。 + +输出： + +1. Executable IR。 +2. KDL request。 +3. `CycleTimeResult`。 +4. `MotionDiagnostic`。 + +验收重点： + +1. 完整 GRL 示例可编译为 IR。 +2. IR 运动指令可映射到 KDL request。 +3. source map 能定位 GRL 行列、path point、operation。 +4. `estimateCycleTime` 只计算运动时间。 +5. `resampleTrajectory` 时间和点序稳定。 +6. 所有错误返回结构化诊断。 + +## 11. 阶段 9：三品牌后处理原型 + +关联任务：`KW-111` + +目标： + +1. ABB RAPID 后处理。 +2. FANUC LS/TP 风格后处理。 +3. KUKA KRL 后处理。 +4. 支持 `post_hint` 和 `@brand.*`。 +5. 生成后处理转换报告。 + +输入： + +1. Executable IR。 +2. target/tool/frame/speed/zone 数据。 +3. post profile。 +4. brand metadata。 + +输出： + +1. ABB RAPID 程序。 +2. FANUC LS/TP 风格文本。 +3. KUKA KRL 程序。 +4. 转换报告。 + +验收重点： + +1. `movej/movel/movec` 三品牌 golden file 通过。 +2. target/tool/frame/speed/zone 映射正确。 +3. IO/wait 基础映射正确。 +4. `post_hint` 和 `@brand.*` 只影响指定品牌。 +5. 不支持语义进入转换报告，不能静默丢失。 + +## 12. 阶段 10：自动生成、往返和性能优化 + +关联任务：`KW-112`、`KW-013` + +目标： + +1. 自动生成 GRL 时优先生成 target/path/operation。 +2. 点名稳定。 +3. path defaults 和单点 override 稳定。 +4. source metadata 稳定。 +5. 支持 compact/expanded 输出风格。 +6. 生成 GRL 可再解析回等价对象。 +7. KDL 底层导出稳定 C ABI。 +8. 高频 FK/IK 增加 TypedArray 版本。 +9. RobotHandle 缓存 FK、IK、Jacobian solver。 +10. 长路径分块计算或提供进度。 + +输入： + +1. 自动编程对象。 +2. Path/Operation 数据。 +3. KDL 批量计算输入。 + +输出： + +1. 稳定 GRL 文本。 +2. 可回读 AST/IR。 +3. C ABI / Embind API。 +4. TypedArray 批量接口。 +5. 性能报告。 + +验收重点： + +1. 同一输入重复生成结果一致。 +2. 生成文本可 diff。 +3. 生成文本可解析、语义检查并后处理。 +4. 单机器人 6 轴初始化小于 1 秒。 +5. 单次 FK 小于 1 ms。 +6. 单次 IK 平均小于 10 ms。 +7. 1000 个目标点批量可达性检查在可接受交互时间内完成。 +8. 10 秒轨迹按 4 ms 采样约 2500 点可稳定生成和回放。 + +## 13. 推荐集成顺序 + +```text +1. KW-001 + KW-100 + KW-101 + 基础工程、Worker RPC、Lexer/Parser 骨架。 + +2. KW-002 + KW-102 + URDF/标准模型和 GRL target/tool/frame/speed/zone。 + +3. KW-003 到 KW-006 + KW-103 + 运动指令编译到 KDL FK/IK/变换。 + +4. KW-007 到 KW-010 + 梯形速度、MOVEJ、MOVEL、MOVEC。 + +5. KW-104 + KW-011 + Path 编译为 PathPlanRequest，KDL 生成整条路径。 + +6. KW-105 + KW-011 + Operation 展开后复用 Path 规划。 + +7. KW-106 到 KW-110 + 完成 P0 语义检查和 IR。 + +8. KW-111 + 三品牌后处理原型。 + +9. KW-112 + KW-013 + 自动生成、往返、性能和批量优化。 +``` + +## 14. 常用验证命令 + +实际命令以工程 `package.json` 和 CMake 配置为准。每个阶段至少应提供等效命令： + +```bash +npm run typecheck +npm run test -- grl +npm run test -- kdl +npm run test -- integration +npm run test -- post +npm run build +``` + +KDL WASM 构建： + +```bash +cd /home/meswork/kdl_work +emcmake cmake -S kdl-wasm -B kdl-wasm/build-wasm \ + -DCMAKE_BUILD_TYPE=Release \ + -DKDL_SOURCE_DIR=/home/meswork/kdl_work/orocos_kinematics_dynamics/orocos_kdl +cmake --build kdl-wasm/build-wasm -j16 +``` + +## 15. 阅读建议 + +1. 先读本文，理解项目主要阶段和集成顺序。 +2. 再读 `通用机器人项目功能与数据流程图.md`，理解功能流和数据流。 +3. 需要接口细节时读 `KDL_WASM计算接口设计.md`。 +4. 需要语言语法和后处理细节时读 `通用机器人编程语法规范.md`。 +5. 需要执行级任务和证据时读 `/home/meswork/kdl_work/work/working1` 下的实施文档。 diff --git a/work/doc/通用机器人项目功能与数据流程图-png/flow-01.mmd b/work/doc/通用机器人项目功能与数据流程图-png/flow-01.mmd new file mode 100644 index 0000000..9a1121f --- /dev/null +++ b/work/doc/通用机器人项目功能与数据流程图-png/flow-01.mmd @@ -0,0 +1,58 @@ +flowchart TD + A[项目资源] --> A1[robots/robot.urdf] + A --> A2[targets/*.json] + A --> A3[paths/*.json] + A --> A4[operations/*.json] + A --> A5[programs/*.grl] + A --> A6[io/io_map.json] + A --> A7[post/*.profile.json] + + A5 --> B[GRL Lexer] + B --> C[GRL Parser] + C --> D[GRL AST] + D --> E[Symbol Table] + E --> F[Semantic Analyzer] + + A1 --> G[URDF Parser] + G --> H[NormalizedRobotModel] + H --> I[KDL createRobotFromModel] + I --> J[RobotHandle] + + F --> K[Executable IR] + K --> L{IR 指令类型} + + L -->|MotionInstruction| M[生成 MotionSegmentRequest] + L -->|run_path| N[展开 Path points/events] + L -->|run_operation| O[展开 start_action + path + end_action] + L -->|IO / wait / pulse| P[虚拟控制器 IO Service] + L -->|if/for/switch/call/return| Q[虚拟控制器流程执行] + L -->|alarm / raise / try/catch| R[虚拟控制器报警与异常处理] + + N --> M + O --> N + + M --> S[KdlWorkerClient] + S --> T[kdl.worker.ts] + T --> U[KDL WASM API] + + U --> U1[planMoveJ] + U --> U2[planMoveL] + U --> U3[planMoveC] + U --> U4[planPath] + U --> U5[validatePath] + + U1 --> V[TrajectoryResult] + U2 --> V + U3 --> V + U4 --> W[PathPlanResult] + U5 --> X[PathValidationResult] + + V --> Y[Motion Queue / 轨迹回放] + W --> Y + X --> Z[可达性与诊断报告] + + K --> AA[Post Processor] + AA --> AA1[ABB RAPID] + AA --> AA2[FANUC LS/TP 风格文本] + AA --> AA3[KUKA KRL] + AA --> AA4[转换报告] diff --git a/work/doc/通用机器人项目功能与数据流程图-png/flow-01.png b/work/doc/通用机器人项目功能与数据流程图-png/flow-01.png new file mode 100644 index 0000000..46e2d82 Binary files /dev/null and b/work/doc/通用机器人项目功能与数据流程图-png/flow-01.png differ diff --git a/work/doc/通用机器人项目功能与数据流程图-png/flow-02.mmd b/work/doc/通用机器人项目功能与数据流程图-png/flow-02.mmd new file mode 100644 index 0000000..5c9c487 --- /dev/null +++ b/work/doc/通用机器人项目功能与数据流程图-png/flow-02.mmd @@ -0,0 +1,42 @@ +flowchart TD + A[GRL Source .grl] --> B[Lexer] + B --> C[Tokens] + C --> D[Parser] + D --> E[AST] + E --> F[Symbol Table] + F --> G[Semantic Analyzer] + + G --> G1[名称解析] + G --> G2[类型检查] + G --> G3[单位规范化] + G --> G4[tool/frame/speed/zone 解析] + G --> G5[Path/Operation 引用检查] + G --> G6[IO 地址检查] + G --> G7[运动目标可达性检查] + G --> G8[后处理能力检查] + + G1 --> H[Executable IR] + G2 --> H + G3 --> H + G4 --> H + G5 --> H + G6 --> H + G7 --> H + G8 --> H + + H --> I{IR} + I -->|MotionInstruction| J[Motion Request Builder] + I -->|WaitInstruction| K[Wait Registry] + I -->|IoInstruction| L[IO Image] + I -->|BranchInstruction| M[Program Counter] + I -->|CallInstruction| N[Call Stack] + I -->|AlarmInstruction| O[Alarm Queue] + I -->|ReturnInstruction| P[Scope/Call Stack] + + J --> Q[KDL WASM] + Q --> R[TrajectoryResult / Diagnostics] + R --> S[Motion Queue] + + H --> T[Post Processor] + T --> U[品牌程序] + T --> V[转换报告] diff --git a/work/doc/通用机器人项目功能与数据流程图-png/flow-02.png b/work/doc/通用机器人项目功能与数据流程图-png/flow-02.png new file mode 100644 index 0000000..125261f Binary files /dev/null and b/work/doc/通用机器人项目功能与数据流程图-png/flow-02.png differ diff --git a/work/doc/通用机器人项目功能与数据流程图-png/flow-03.mmd b/work/doc/通用机器人项目功能与数据流程图-png/flow-03.mmd new file mode 100644 index 0000000..ddad5ae --- /dev/null +++ b/work/doc/通用机器人项目功能与数据流程图-png/flow-03.mmd @@ -0,0 +1,48 @@ +flowchart TD + A[NormalizedRobotModel] --> B[createRobotFromModel] + B --> C[RobotHandle] + C --> D[KDL Chain / Solvers Cache] + + E[Motion Request] --> F{motion} + F -->|MOVEJ| G[planMoveJ] + F -->|MOVEL| H[planMoveL] + F -->|MOVEC| I[planMoveC] + F -->|PATH| J[planPath / validatePath] + + G --> G1[校验 startJoints 和限位] + G1 --> G2{target 类型} + G2 -->|joint_target| G3[qEnd = target.joints] + G2 -->|pose_target| G4[IK 求 qEnd] + G3 --> G5[关节差分] + G4 --> G5 + G5 --> G6[梯形速度曲线] + G6 --> G7[采样关节位置/速度/加速度] + G7 --> G8[FK 输出 TCP] + G8 --> R[TrajectoryResult] + + H --> H1[FK 得到起点 TCP] + H1 --> H2[applyToolAndFrame / applyOffset] + H2 --> H3[直线位置和姿态插补] + H3 --> H4[梯形速度曲线] + H4 --> H5[逐点 IK] + H5 --> H6[限位/速度/奇异性检查] + H6 --> R + + I --> I1[FK 得到起点 TCP] + I1 --> I2[via/target 位姿变换] + I2 --> I3[三点退化检查] + I3 --> I4[圆心/半径/法向/弧长] + I4 --> I5[圆弧采样] + I5 --> I6[逐点 IK] + I6 --> I7[圆弧误差和限位检查] + I7 --> R + + J --> J1[按 segment 顺序规划] + J1 --> J2[上一段终点关节作为下一段起点] + J2 --> J3[合并轨迹点和诊断] + J3 --> J4[保留 sourceMap] + J4 --> P[PathPlanResult / PathValidationResult] + + R --> D1[MotionDiagnostic] + P --> D1 + D1 --> D2[error / warning / info] diff --git a/work/doc/通用机器人项目功能与数据流程图-png/flow-03.png b/work/doc/通用机器人项目功能与数据流程图-png/flow-03.png new file mode 100644 index 0000000..84e8f0c Binary files /dev/null and b/work/doc/通用机器人项目功能与数据流程图-png/flow-03.png differ diff --git a/work/doc/通用机器人项目功能与数据流程图-png/flow-04.mmd b/work/doc/通用机器人项目功能与数据流程图-png/flow-04.mmd new file mode 100644 index 0000000..a3ca60b --- /dev/null +++ b/work/doc/通用机器人项目功能与数据流程图-png/flow-04.mmd @@ -0,0 +1,92 @@ +flowchart LR + subgraph Project[项目输入数据] + A1[robot.urdf] + A2[main.grl] + A3[targets / paths / operations] + A4[io_map.json] + A5[post profile] + end + + subgraph Compile[TypeScript 编译层] + B1[URDF Parser] + B2[GRL Lexer/Parser] + B3[AST] + B4[Symbol Table] + B5[Semantic Analyzer] + B6[Executable IR] + B7[Motion Request Builder] + end + + subgraph KDL[KDL Worker / WASM] + C1[KdlRpcRequest] + C2[RobotHandle] + C3[KDL Solvers] + C4[FK / IK / Jacobian] + C5[Trap Profile] + C6[Motion Planner] + C7[KdlRpcResponse] + end + + subgraph Runtime[虚拟控制器运行层] + D1[Program Counter] + D2[Call Stack] + D3[Scope Stack] + D4[Motion Queue] + D5[IO Image] + D6[Wait Registry] + D7[Alarm Queue] + D8[Trace Buffer] + end + + subgraph Output[输出数据] + E1[TrajectoryResult] + E2[PathPlanResult] + E3[PathValidationResult] + E4[MotionDiagnostic] + E5[CycleTimeResult] + E6[ABB/FANUC/KUKA 程序] + E7[转换报告] + end + + A1 --> B1 + B1 -->|NormalizedRobotModel| C1 + C1 --> C2 + C2 --> C3 + + A2 --> B2 + A3 --> B5 + A4 --> B5 + B2 --> B3 + B3 --> B4 + B4 --> B5 + B5 --> B6 + + B6 -->|MotionInstruction| B7 + B7 -->|MoveJRequest / MoveLRequest / MoveCRequest / PathPlanRequest| C1 + C1 --> C4 + C1 --> C5 + C4 --> C6 + C5 --> C6 + C6 --> C7 + + C7 --> E1 + C7 --> E2 + C7 --> E3 + C7 --> E4 + C7 --> E5 + + B6 --> D1 + B6 --> D2 + B6 --> D3 + E1 --> D4 + E2 --> D4 + B6 -->|IO / wait / pulse| D5 + B6 -->|wait| D6 + E4 --> D7 + D4 --> D8 + D5 --> D8 + D6 --> D8 + + B6 -->|IR + post profile| A5 + A5 --> E6 + A5 --> E7 diff --git a/work/doc/通用机器人项目功能与数据流程图-png/flow-04.png b/work/doc/通用机器人项目功能与数据流程图-png/flow-04.png new file mode 100644 index 0000000..4a000c2 Binary files /dev/null and b/work/doc/通用机器人项目功能与数据流程图-png/flow-04.png differ diff --git a/work/doc/通用机器人项目功能与数据流程图-png/flow-05.mmd b/work/doc/通用机器人项目功能与数据流程图-png/flow-05.mmd new file mode 100644 index 0000000..3884fbf --- /dev/null +++ b/work/doc/通用机器人项目功能与数据流程图-png/flow-05.mmd @@ -0,0 +1,16 @@ +flowchart TD + A[GRL Parser] -->|语法错误| D[Diagnostic] + B[Semantic Analyzer] -->|类型/单位/引用/IO/后处理错误| D + C[KDL WASM] -->|IK/限位/奇异/轨迹错误| D + E[Post Processor] -->|不支持或近似转换| D + + D --> F{severity} + F -->|error| G[阻止编译或进入 alarm/hold] + F -->|warning| H[允许继续但写入报告] + F -->|info| I[写入 trace 或调试信息] + + D --> J[sourceMap] + J --> J1[GRL file/line/column] + J --> J2[pathId/pathPointId] + J --> J3[operationId] + J --> J4[brandSource] diff --git a/work/doc/通用机器人项目功能与数据流程图-png/flow-05.png b/work/doc/通用机器人项目功能与数据流程图-png/flow-05.png new file mode 100644 index 0000000..9afbf0f Binary files /dev/null and b/work/doc/通用机器人项目功能与数据流程图-png/flow-05.png differ diff --git a/work/doc/通用机器人项目功能与数据流程图.md b/work/doc/通用机器人项目功能与数据流程图.md new file mode 100644 index 0000000..b193dd9 --- /dev/null +++ b/work/doc/通用机器人项目功能与数据流程图.md @@ -0,0 +1,357 @@ +# 通用机器人项目功能与数据流程图 + +版本：0.1 +日期：2026-06-27 +对标文档： + +1. `通用机器人编程语法规范.md` +2. `KDL_WASM计算接口设计.md` + +## 1. 范围说明 + +本文描述 GRL 通用机器人程序语言和 KDL WASM 计算接口之间的总体功能流程和数据传递流程。 + +核心边界： + +1. GRL 层负责程序文本、语法、语义、IR、Path、Operation、IO、wait、后处理。 +2. KDL WASM 层负责机器人模型、位姿变换、FK、IK、Jacobian、梯形速度、MOVEJ、MOVEL、MOVEC、Path 轨迹和诊断。 +3. KDL WASM 不解析 GRL，不执行流程控制、变量、IO、wait、子程序和异常逻辑。 +4. TypeScript 编译器和虚拟控制器把 GRL/IR 转换为 KDL 可执行的运动请求。 + +## 2. 总体功能流程图 + +```mermaid +flowchart TD + A[项目资源] --> A1[robots/robot.urdf] + A --> A2[targets/*.json] + A --> A3[paths/*.json] + A --> A4[operations/*.json] + A --> A5[programs/*.grl] + A --> A6[io/io_map.json] + A --> A7[post/*.profile.json] + + A5 --> B[GRL Lexer] + B --> C[GRL Parser] + C --> D[GRL AST] + D --> E[Symbol Table] + E --> F[Semantic Analyzer] + + A1 --> G[URDF Parser] + G --> H[NormalizedRobotModel] + H --> I[KDL createRobotFromModel] + I --> J[RobotHandle] + + F --> K[Executable IR] + K --> L{IR 指令类型} + + L -->|MotionInstruction| M[生成 MotionSegmentRequest] + L -->|run_path| N[展开 Path points/events] + L -->|run_operation| O[展开 start_action + path + end_action] + L -->|IO / wait / pulse| P[虚拟控制器 IO Service] + L -->|if/for/switch/call/return| Q[虚拟控制器流程执行] + L -->|alarm / raise / try/catch| R[虚拟控制器报警与异常处理] + + N --> M + O --> N + + M --> S[KdlWorkerClient] + S --> T[kdl.worker.ts] + T --> U[KDL WASM API] + + U --> U1[planMoveJ] + U --> U2[planMoveL] + U --> U3[planMoveC] + U --> U4[planPath] + U --> U5[validatePath] + + U1 --> V[TrajectoryResult] + U2 --> V + U3 --> V + U4 --> W[PathPlanResult] + U5 --> X[PathValidationResult] + + V --> Y[Motion Queue / 轨迹回放] + W --> Y + X --> Z[可达性与诊断报告] + + K --> AA[Post Processor] + AA --> AA1[ABB RAPID] + AA --> AA2[FANUC LS/TP 风格文本] + AA --> AA3[KUKA KRL] + AA --> AA4[转换报告] +``` + +## 3. GRL 编译与执行流程图 + +```mermaid +flowchart TD + A[GRL Source .grl] --> B[Lexer] + B --> C[Tokens] + C --> D[Parser] + D --> E[AST] + E --> F[Symbol Table] + F --> G[Semantic Analyzer] + + G --> G1[名称解析] + G --> G2[类型检查] + G --> G3[单位规范化] + G --> G4[tool/frame/speed/zone 解析] + G --> G5[Path/Operation 引用检查] + G --> G6[IO 地址检查] + G --> G7[运动目标可达性检查] + G --> G8[后处理能力检查] + + G1 --> H[Executable IR] + G2 --> H + G3 --> H + G4 --> H + G5 --> H + G6 --> H + G7 --> H + G8 --> H + + H --> I{IR} + I -->|MotionInstruction| J[Motion Request Builder] + I -->|WaitInstruction| K[Wait Registry] + I -->|IoInstruction| L[IO Image] + I -->|BranchInstruction| M[Program Counter] + I -->|CallInstruction| N[Call Stack] + I -->|AlarmInstruction| O[Alarm Queue] + I -->|ReturnInstruction| P[Scope/Call Stack] + + J --> Q[KDL WASM] + Q --> R[TrajectoryResult / Diagnostics] + R --> S[Motion Queue] + + H --> T[Post Processor] + T --> U[品牌程序] + T --> V[转换报告] +``` + +## 4. KDL WASM 计算流程图 + +```mermaid +flowchart TD + A[NormalizedRobotModel] --> B[createRobotFromModel] + B --> C[RobotHandle] + C --> D[KDL Chain / Solvers Cache] + + E[Motion Request] --> F{motion} + F -->|MOVEJ| G[planMoveJ] + F -->|MOVEL| H[planMoveL] + F -->|MOVEC| I[planMoveC] + F -->|PATH| J[planPath / validatePath] + + G --> G1[校验 startJoints 和限位] + G1 --> G2{target 类型} + G2 -->|joint_target| G3[qEnd = target.joints] + G2 -->|pose_target| G4[IK 求 qEnd] + G3 --> G5[关节差分] + G4 --> G5 + G5 --> G6[梯形速度曲线] + G6 --> G7[采样关节位置/速度/加速度] + G7 --> G8[FK 输出 TCP] + G8 --> R[TrajectoryResult] + + H --> H1[FK 得到起点 TCP] + H1 --> H2[applyToolAndFrame / applyOffset] + H2 --> H3[直线位置和姿态插补] + H3 --> H4[梯形速度曲线] + H4 --> H5[逐点 IK] + H5 --> H6[限位/速度/奇异性检查] + H6 --> R + + I --> I1[FK 得到起点 TCP] + I1 --> I2[via/target 位姿变换] + I2 --> I3[三点退化检查] + I3 --> I4[圆心/半径/法向/弧长] + I4 --> I5[圆弧采样] + I5 --> I6[逐点 IK] + I6 --> I7[圆弧误差和限位检查] + I7 --> R + + J --> J1[按 segment 顺序规划] + J1 --> J2[上一段终点关节作为下一段起点] + J2 --> J3[合并轨迹点和诊断] + J3 --> J4[保留 sourceMap] + J4 --> P[PathPlanResult / PathValidationResult] + + R --> D1[MotionDiagnostic] + P --> D1 + D1 --> D2[error / warning / info] +``` + +## 5. 数据传递流程图 + +```mermaid +flowchart LR + subgraph Project[项目输入数据] + A1[robot.urdf] + A2[main.grl] + A3[targets / paths / operations] + A4[io_map.json] + A5[post profile] + end + + subgraph Compile[TypeScript 编译层] + B1[URDF Parser] + B2[GRL Lexer/Parser] + B3[AST] + B4[Symbol Table] + B5[Semantic Analyzer] + B6[Executable IR] + B7[Motion Request Builder] + end + + subgraph KDL[KDL Worker / WASM] + C1[KdlRpcRequest] + C2[RobotHandle] + C3[KDL Solvers] + C4[FK / IK / Jacobian] + C5[Trap Profile] + C6[Motion Planner] + C7[KdlRpcResponse] + end + + subgraph Runtime[虚拟控制器运行层] + D1[Program Counter] + D2[Call Stack] + D3[Scope Stack] + D4[Motion Queue] + D5[IO Image] + D6[Wait Registry] + D7[Alarm Queue] + D8[Trace Buffer] + end + + subgraph Output[输出数据] + E1[TrajectoryResult] + E2[PathPlanResult] + E3[PathValidationResult] + E4[MotionDiagnostic] + E5[CycleTimeResult] + E6[ABB/FANUC/KUKA 程序] + E7[转换报告] + end + + A1 --> B1 + B1 -->|NormalizedRobotModel| C1 + C1 --> C2 + C2 --> C3 + + A2 --> B2 + A3 --> B5 + A4 --> B5 + B2 --> B3 + B3 --> B4 + B4 --> B5 + B5 --> B6 + + B6 -->|MotionInstruction| B7 + B7 -->|MoveJRequest / MoveLRequest / MoveCRequest / PathPlanRequest| C1 + C1 --> C4 + C1 --> C5 + C4 --> C6 + C5 --> C6 + C6 --> C7 + + C7 --> E1 + C7 --> E2 + C7 --> E3 + C7 --> E4 + C7 --> E5 + + B6 --> D1 + B6 --> D2 + B6 --> D3 + E1 --> D4 + E2 --> D4 + B6 -->|IO / wait / pulse| D5 + B6 -->|wait| D6 + E4 --> D7 + D4 --> D8 + D5 --> D8 + D6 --> D8 + + B6 -->|IR + post profile| A5 + A5 --> E6 + A5 --> E7 +``` + +## 6. 关键对象数据流 + +| 输入对象 | 产生阶段 | 传递到 | 输出对象 | +| --- | --- | --- | --- | +| `robot.urdf` | 项目资源 | TypeScript URDF Parser | `NormalizedRobotModel` | +| `NormalizedRobotModel` | TypeScript 编译层 | `createRobotFromModel` | `RobotHandle` | +| `tool/frame/target/speed/zone` | GRL Parser + Semantic Analyzer | IR、KDL request builder | `ToolRef`、`FrameRef`、`JointTarget`、`PoseTarget`、`SpeedSpec`、`ZoneSpec` | +| `movej` | GRL Parser | Semantic Analyzer | `MotionInstruction(joint)` | +| `movel` | GRL Parser | Semantic Analyzer | `MotionInstruction(linear)` | +| `movec` | GRL Parser | Semantic Analyzer | `MotionInstruction(circular)` | +| `path` | GRL Parser | Path compiler | `MotionSegmentRequest[]` | +| `operation` | GRL Parser | Operation compiler | start action + path + end action | +| `MotionInstruction` | IR | KDL request builder | `MoveJRequest`、`MoveLRequest`、`MoveCRequest` | +| `PathPlanRequest` | Path compiler | KDL WASM | `PathPlanResult` | +| `TrajectoryResult` | KDL WASM | Motion Queue、trace、报告 | 轨迹点、诊断、节拍输入 | +| `MotionDiagnostic` | GRL 语义检查或 KDL WASM | 编辑器、报警、报告、后处理 | error/warning/info | +| `Executable IR` | Semantic Analyzer | 虚拟控制器、后处理器 | 执行流、品牌程序 | + +## 7. KDL API 与 GRL 语法映射 + +| GRL 语法 | TypeScript 编译结果 | KDL WASM 函数 | +| --- | --- | --- | +| `target home = joint_target` | `JointTarget` | `checkJointLimits` | +| `target pick = pose_target` | `PoseTarget` | `checkReachability` | +| `pick offset z 100 mm` | `OffsetSpec` | `applyOffset` | +| `movej home` | `MoveJRequest` | `planMoveJ` | +| `movel pick` | `MoveLRequest` | `planMoveL` | +| `movec via mid target end` | `MoveCRequest` | `planMoveC` | +| `run_path pick_path` | `PathPlanRequest` | `planPath` | +| Path 可达性检查 | `PathPlanRequest` | `validatePath` | +| 节拍估算 | `TrajectoryResult/PathPlanResult` | `estimateCycleTime` | + +## 8. 不进入 KDL WASM 的数据流 + +以下 GRL 语义由 TypeScript 编译层或虚拟控制器执行，不传入 KDL WASM： + +| GRL 语义 | 执行位置 | 输出 | +| --- | --- | --- | +| `if/elseif/else` | Program Counter / BranchInstruction | 分支后的 IR 执行位置 | +| `while/for/switch` | Program Counter / BranchInstruction | 循环或选择后的 IR 执行位置 | +| `proc/func/call/return` | Call Stack / Scope Stack | 调用栈和变量作用域 | +| `io.do/di/ai/ao` | IO Image / IO Service | IO 事件和 trace | +| `wait/pulse/timer` | Wait Registry / IO Service | wait 状态、timeout、pulse trace | +| `alarm/raise/try/catch` | Alarm Queue / Exception Handler | 报警和异常处理结果 | +| `operation.process` | Operation compiler / 后处理器 | 工艺参数、转换报告 | + +## 9. 诊断传递流程 + +```mermaid +flowchart TD + A[GRL Parser] -->|语法错误| D[Diagnostic] + B[Semantic Analyzer] -->|类型/单位/引用/IO/后处理错误| D + C[KDL WASM] -->|IK/限位/奇异/轨迹错误| D + E[Post Processor] -->|不支持或近似转换| D + + D --> F{severity} + F -->|error| G[阻止编译或进入 alarm/hold] + F -->|warning| H[允许继续但写入报告] + F -->|info| I[写入 trace 或调试信息] + + D --> J[sourceMap] + J --> J1[GRL file/line/column] + J --> J2[pathId/pathPointId] + J --> J3[operationId] + J --> J4[brandSource] +``` + +## 10. 总结 + +项目的数据流以 GRL IR 为中枢： + +1. GRL 文本、Path、Operation、IO 和品牌扩展先进入 TypeScript 编译层。 +2. 编译层完成语法、语义、单位、source map 和后处理能力检查。 +3. 只有运动相关 IR 被转换为 KDL WASM request。 +4. KDL WASM 返回轨迹、节拍和结构化诊断。 +5. 虚拟控制器消费 IR 和轨迹，后处理器消费 IR 和品牌 profile。 +6. 诊断贯穿 Parser、Semantic Analyzer、KDL WASM 和 Post Processor，并通过 source map 回到 GRL、Path、Operation 或品牌源。