1045 lines
32 KiB
C++
1045 lines
32 KiB
C++
#include <algorithm>
|
|
#include <cctype>
|
|
#include <cmath>
|
|
#include <cstring>
|
|
#include <map>
|
|
#include <memory>
|
|
#include <sstream>
|
|
#include <stdexcept>
|
|
#include <string>
|
|
#include <utility>
|
|
#include <vector>
|
|
|
|
#ifdef __EMSCRIPTEN__
|
|
#include <emscripten/emscripten.h>
|
|
#else
|
|
#define EMSCRIPTEN_KEEPALIVE
|
|
#endif
|
|
|
|
#ifdef KDL_WASM_HAS_OROCOS_KDL
|
|
#include "chain.hpp"
|
|
#include "chainfksolverpos_recursive.hpp"
|
|
#include "chainiksolverpos_lma.hpp"
|
|
#include "chainjnttojacsolver.hpp"
|
|
#include "jntarray.hpp"
|
|
#include "segment.hpp"
|
|
#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<JsonValue> array_value;
|
|
std::map<std::string, JsonValue> 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<unsigned char>(input_[pos_]))) {
|
|
pos_ += 1;
|
|
}
|
|
if (pos_ < input_.size() && input_[pos_] == '.') {
|
|
pos_ += 1;
|
|
while (pos_ < input_.size() && std::isdigit(static_cast<unsigned char>(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<unsigned char>(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<unsigned char>(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<std::string> active_joint_names;
|
|
std::vector<NativeJoint> joints;
|
|
std::vector<std::string> link_order;
|
|
#ifdef KDL_WASM_HAS_OROCOS_KDL
|
|
KDL::Chain chain;
|
|
std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver;
|
|
std::unique_ptr<KDL::ChainJntToJacSolver> jac_solver;
|
|
std::unique_ptr<KDL::ChainIkSolverPos_LMA> ik_solver;
|
|
#endif
|
|
};
|
|
|
|
std::map<int, RobotRecord> 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<int>(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<std::size_t>(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;
|
|
}
|
|
|
|
#ifdef KDL_WASM_HAS_OROCOS_KDL
|
|
void append_pose_json(std::ostringstream& json, const KDL::Frame& frame) {
|
|
double x = 0.0;
|
|
double y = 0.0;
|
|
double z = 0.0;
|
|
double w = 1.0;
|
|
frame.M.GetQuaternion(x, y, z, w);
|
|
json << "{\"position\":[" << frame.p.x() << "," << frame.p.y() << "," << frame.p.z()
|
|
<< "],\"quaternion\":[" << x << "," << y << "," << z << "," << w << "]}";
|
|
}
|
|
#endif
|
|
|
|
const JsonValue& required_object_property(const JsonValue& object, const std::string& key) {
|
|
const JsonValue* value = object.get(key);
|
|
if (!value) {
|
|
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<std::string> 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<std::string> 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;
|
|
}
|
|
|
|
double number_property(const JsonValue& object, const std::string& key) {
|
|
const JsonValue& value = required_object_property(object, key);
|
|
if (value.kind != JsonValue::Kind::Number) {
|
|
throw std::runtime_error("Expected number model property: " + key);
|
|
}
|
|
return value.number_value;
|
|
}
|
|
|
|
double number_property_or(const JsonValue& object, const std::string& key, double fallback) {
|
|
const JsonValue* value = object.get(key);
|
|
if (!value) {
|
|
return fallback;
|
|
}
|
|
if (value->kind != JsonValue::Kind::Number) {
|
|
throw std::runtime_error("Expected numeric property: " + key);
|
|
}
|
|
return value->number_value;
|
|
}
|
|
|
|
void copy_number_triple(const JsonValue& object, const std::string& key, double out[3]) {
|
|
const JsonValue& value = required_object_property(object, key);
|
|
if (value.kind != JsonValue::Kind::Array || value.array_value.size() != 3) {
|
|
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<NativeJoint> parse_joints(const JsonValue& model,
|
|
const std::vector<std::string>& 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<NativeJoint> 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();
|
|
record.link_order.clear();
|
|
record.link_order.push_back(record.base_link);
|
|
for (const NativeJoint& joint : record.joints) {
|
|
const KDL::Frame origin = frame_from_xyz_rpy(joint.xyz, joint.rpy);
|
|
if (joint.xyz[0] != 0.0 || joint.xyz[1] != 0.0 || joint.xyz[2] != 0.0 ||
|
|
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.link_order.push_back(joint.child);
|
|
}
|
|
record.fk_solver = std::make_unique<KDL::ChainFkSolverPos_recursive>(record.chain);
|
|
record.jac_solver = std::make_unique<KDL::ChainJntToJacSolver>(record.chain);
|
|
record.ik_solver = std::make_unique<KDL::ChainIkSolverPos_LMA>(record.chain);
|
|
}
|
|
|
|
KDL::JntArray jnt_array_from_input(const RobotRecord& record, const double* joints, int n) {
|
|
if (joints == nullptr) {
|
|
throw std::runtime_error("Joint input is required");
|
|
}
|
|
if (n != static_cast<int>(record.active_joint_names.size())) {
|
|
throw std::runtime_error("Joint vector dimension does not match robot DOF");
|
|
}
|
|
KDL::JntArray q(static_cast<unsigned int>(n));
|
|
for (int index = 0; index < n; index += 1) {
|
|
q(static_cast<unsigned int>(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]);
|
|
}
|
|
|
|
KDL::Frame frame_from_pose7(const double* pose7) {
|
|
if (pose7 == nullptr) {
|
|
throw std::runtime_error("Target pose buffer is required");
|
|
}
|
|
const double norm =
|
|
std::sqrt(pose7[3] * pose7[3] + pose7[4] * pose7[4] + pose7[5] * pose7[5] + pose7[6] * pose7[6]);
|
|
if (norm <= 0.0) {
|
|
throw std::runtime_error("Target pose quaternion must be non-zero");
|
|
}
|
|
return KDL::Frame(
|
|
KDL::Rotation::Quaternion(pose7[3] / norm, pose7[4] / norm, pose7[5] / norm, pose7[6] / norm),
|
|
KDL::Vector(pose7[0], pose7[1], pose7[2]));
|
|
}
|
|
#endif
|
|
|
|
RobotRecord parse_robot_record(const char* model_json) {
|
|
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<int, RobotRecord>::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;
|
|
}
|
|
|
|
double distance_for_velocity_change(double from_velocity, double to_velocity, double acceleration) {
|
|
return std::max(0.0, (to_velocity * to_velocity - from_velocity * from_velocity) / (2.0 * acceleration));
|
|
}
|
|
|
|
double clamp01(double value) {
|
|
return std::max(0.0, std::min(1.0, value));
|
|
}
|
|
|
|
struct TrapProfile {
|
|
std::string type;
|
|
double length = 0.0;
|
|
double sample_time = 0.0;
|
|
double start_velocity = 0.0;
|
|
double end_velocity = 0.0;
|
|
double max_acceleration = 0.0;
|
|
double duration = 0.0;
|
|
double t_accel = 0.0;
|
|
double t_const = 0.0;
|
|
double t_decel = 0.0;
|
|
double v_peak = 0.0;
|
|
bool triangle_diagnostic = false;
|
|
bool zero_length_diagnostic = false;
|
|
};
|
|
|
|
struct TrapPoint {
|
|
double distance = 0.0;
|
|
double velocity = 0.0;
|
|
double acceleration = 0.0;
|
|
};
|
|
|
|
TrapProfile make_trap_profile(double length, const char* options_json) {
|
|
if (options_json == nullptr) {
|
|
throw std::runtime_error("Trap profile options JSON is required");
|
|
}
|
|
const JsonValue options = JsonParser(options_json).parse();
|
|
if (options.kind != JsonValue::Kind::Object) {
|
|
throw std::runtime_error("Trap profile options must be a JSON object");
|
|
}
|
|
|
|
TrapProfile profile;
|
|
profile.length = length;
|
|
profile.sample_time = number_property(options, "sampleTime");
|
|
profile.start_velocity = number_property_or(options, "startVelocity", 0.0);
|
|
profile.end_velocity = number_property_or(options, "endVelocity", 0.0);
|
|
const double max_velocity = number_property(options, "maxVelocity");
|
|
profile.max_acceleration = number_property(options, "maxAcceleration");
|
|
|
|
if (!std::isfinite(length) || length < 0.0) {
|
|
throw std::runtime_error("Trap profile length must be a finite non-negative number");
|
|
}
|
|
if (!std::isfinite(max_velocity) || max_velocity <= 0.0) {
|
|
throw std::runtime_error("maxVelocity must be a finite positive number");
|
|
}
|
|
if (!std::isfinite(profile.max_acceleration) || profile.max_acceleration <= 0.0) {
|
|
throw std::runtime_error("maxAcceleration must be a finite positive number");
|
|
}
|
|
if (!std::isfinite(profile.sample_time) || profile.sample_time <= 0.0) {
|
|
throw std::runtime_error("sampleTime must be a finite positive number");
|
|
}
|
|
if (length == 0.0 && (profile.start_velocity > 0.0 || profile.end_velocity > 0.0)) {
|
|
throw std::runtime_error("Zero-length trap profile requires zero startVelocity and endVelocity");
|
|
}
|
|
if (!std::isfinite(profile.start_velocity) || profile.start_velocity < 0.0 ||
|
|
profile.start_velocity > max_velocity) {
|
|
throw std::runtime_error("startVelocity must be finite, non-negative, and no greater than maxVelocity");
|
|
}
|
|
if (!std::isfinite(profile.end_velocity) || profile.end_velocity < 0.0 ||
|
|
profile.end_velocity > max_velocity) {
|
|
throw std::runtime_error("endVelocity must be finite, non-negative, and no greater than maxVelocity");
|
|
}
|
|
|
|
if (length == 0.0) {
|
|
profile.type = "triangle";
|
|
profile.zero_length_diagnostic = true;
|
|
return profile;
|
|
}
|
|
|
|
const double d_accel_to_max =
|
|
distance_for_velocity_change(profile.start_velocity, max_velocity, profile.max_acceleration);
|
|
const double d_decel_from_max =
|
|
distance_for_velocity_change(profile.end_velocity, max_velocity, profile.max_acceleration);
|
|
profile.type = "trapezoid";
|
|
profile.v_peak = max_velocity;
|
|
|
|
if (d_accel_to_max + d_decel_from_max <= length) {
|
|
profile.t_const = (length - d_accel_to_max - d_decel_from_max) / max_velocity;
|
|
} else {
|
|
profile.type = "triangle";
|
|
profile.triangle_diagnostic = true;
|
|
profile.v_peak = std::sqrt(std::max(
|
|
0.0,
|
|
profile.max_acceleration * length +
|
|
(profile.start_velocity * profile.start_velocity +
|
|
profile.end_velocity * profile.end_velocity) /
|
|
2.0));
|
|
if (profile.v_peak + 1e-12 < std::max(profile.start_velocity, profile.end_velocity)) {
|
|
throw std::runtime_error("Profile length is too short for the requested startVelocity/endVelocity");
|
|
}
|
|
profile.t_const = 0.0;
|
|
}
|
|
|
|
profile.t_accel = std::max(0.0, (profile.v_peak - profile.start_velocity) / profile.max_acceleration);
|
|
profile.t_decel = std::max(0.0, (profile.v_peak - profile.end_velocity) / profile.max_acceleration);
|
|
profile.duration = profile.t_accel + profile.t_const + profile.t_decel;
|
|
return profile;
|
|
}
|
|
|
|
TrapPoint sample_trap_at_time(const TrapProfile& profile, double time) {
|
|
const double accel_distance =
|
|
profile.start_velocity * profile.t_accel +
|
|
0.5 * profile.max_acceleration * profile.t_accel * profile.t_accel;
|
|
const double const_distance = profile.v_peak * profile.t_const;
|
|
const double accel_end = profile.t_accel;
|
|
const double const_end = profile.t_accel + profile.t_const;
|
|
|
|
if (time <= accel_end) {
|
|
return {
|
|
profile.start_velocity * time + 0.5 * profile.max_acceleration * time * time,
|
|
profile.start_velocity + profile.max_acceleration * time,
|
|
profile.max_acceleration};
|
|
}
|
|
|
|
if (time <= const_end) {
|
|
const double local_time = time - profile.t_accel;
|
|
return {accel_distance + profile.v_peak * local_time, profile.v_peak, 0.0};
|
|
}
|
|
|
|
const double local_time = std::min(time - const_end, profile.t_decel);
|
|
return {
|
|
accel_distance + const_distance + profile.v_peak * local_time -
|
|
0.5 * profile.max_acceleration * local_time * local_time,
|
|
std::max(profile.end_velocity, profile.v_peak - profile.max_acceleration * local_time),
|
|
-profile.max_acceleration};
|
|
}
|
|
|
|
std::string trap_profile_json(const TrapProfile& profile) {
|
|
std::ostringstream json;
|
|
json << "{\"ok\":true,\"type\":\"" << profile.type << "\",\"length\":" << profile.length
|
|
<< ",\"duration\":" << profile.duration << ",\"tAccel\":" << profile.t_accel
|
|
<< ",\"tConst\":" << profile.t_const << ",\"tDecel\":" << profile.t_decel
|
|
<< ",\"vPeak\":" << profile.v_peak << ",\"samples\":[";
|
|
|
|
if (profile.duration == 0.0) {
|
|
json << "{\"index\":0,\"time\":0,\"s\":0,\"sd\":0,\"sdd\":0}";
|
|
} else {
|
|
std::vector<double> times;
|
|
times.push_back(0.0);
|
|
for (double time = profile.sample_time; time < profile.duration - 1e-12;
|
|
time += profile.sample_time) {
|
|
times.push_back(time);
|
|
}
|
|
times.push_back(profile.duration);
|
|
for (std::size_t index = 0; index < times.size(); index += 1) {
|
|
if (index > 0) {
|
|
json << ",";
|
|
}
|
|
const TrapPoint sample = sample_trap_at_time(profile, times[index]);
|
|
const double s =
|
|
index == 0 ? 0.0 : index == times.size() - 1 ? 1.0 : clamp01(sample.distance / profile.length);
|
|
json << "{\"index\":" << index << ",\"time\":" << times[index] << ",\"s\":" << s
|
|
<< ",\"sd\":" << sample.velocity / profile.length
|
|
<< ",\"sdd\":" << sample.acceleration / profile.length << "}";
|
|
}
|
|
}
|
|
|
|
json << "],\"diagnostics\":[";
|
|
bool wrote_diagnostic = false;
|
|
if (profile.zero_length_diagnostic) {
|
|
json << "{\"severity\":\"info\",\"code\":\"KDL_TRAP_ZERO_LENGTH\","
|
|
"\"message\":\"Trap profile length is zero\"}";
|
|
wrote_diagnostic = true;
|
|
}
|
|
if (profile.triangle_diagnostic) {
|
|
if (wrote_diagnostic) {
|
|
json << ",";
|
|
}
|
|
json << "{\"severity\":\"info\",\"code\":\"KDL_TRAP_TRIANGLE_PROFILE\","
|
|
"\"message\":\"Trap profile length is too short to reach maxVelocity; using triangle profile\"}";
|
|
}
|
|
json << "]}";
|
|
return json.str();
|
|
}
|
|
|
|
} // namespace
|
|
|
|
extern "C" {
|
|
|
|
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) {
|
|
#ifdef KDL_WASM_HAS_OROCOS_KDL
|
|
try {
|
|
const auto it = find_robot(robot_handle);
|
|
if (it == robots.end()) {
|
|
return -1;
|
|
}
|
|
RobotRecord& record = it->second;
|
|
const KDL::JntArray q = jnt_array_from_input(record, joints, n);
|
|
std::vector<KDL::Frame> frames(record.chain.getNrOfSegments());
|
|
const int result = record.fk_solver->JntToCart(q, frames);
|
|
if (result != 0) {
|
|
set_last_error("KDL_FK_FAILED", "Native KDL link FK solver failed");
|
|
return -1;
|
|
}
|
|
|
|
std::ostringstream json;
|
|
json << "{\"ok\":true,\"linkPoses\":[";
|
|
json << "{\"link\":\"" << json_escape(record.base_link)
|
|
<< "\",\"pose\":{\"position\":[0,0,0],\"quaternion\":[0,0,0,1]}}";
|
|
for (std::size_t segment_index = 0; segment_index < frames.size(); segment_index += 1) {
|
|
const KDL::Segment& segment = record.chain.getSegment(static_cast<unsigned int>(segment_index));
|
|
const std::string& segment_name = segment.getName();
|
|
if (segment_name.size() >= 7 &&
|
|
segment_name.compare(segment_name.size() - 7, 7, "_origin") == 0) {
|
|
continue;
|
|
}
|
|
json << ",{\"link\":\"" << json_escape(segment_name) << "\",\"pose\":";
|
|
append_pose_json(json, frames[segment_index]);
|
|
json << "}";
|
|
}
|
|
json << "],\"diagnostics\":[]}";
|
|
const int write_result = write_json(json.str(), out_json, out_len);
|
|
if (write_result == 0) {
|
|
clear_last_error();
|
|
}
|
|
return write_result;
|
|
} catch (const std::exception& error) {
|
|
set_last_error("KDL_FK_FAILED", error.what());
|
|
return -1;
|
|
}
|
|
#else
|
|
(void)robot_handle;
|
|
(void)joints;
|
|
(void)n;
|
|
(void)out_json;
|
|
(void)out_len;
|
|
return not_implemented("kdl_fk_all_links requires KDL_SOURCE_DIR");
|
|
#endif
|
|
}
|
|
|
|
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<unsigned int>(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<unsigned int>(row), static_cast<unsigned int>(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) {
|
|
#ifdef KDL_WASM_HAS_OROCOS_KDL
|
|
(void)options_json;
|
|
try {
|
|
const auto it = find_robot(robot_handle);
|
|
if (it == robots.end()) {
|
|
return -1;
|
|
}
|
|
RobotRecord& record = it->second;
|
|
if (out_joints == nullptr) {
|
|
throw std::runtime_error("Output joint buffer is required");
|
|
}
|
|
const KDL::JntArray q_seed = jnt_array_from_input(record, seed, n);
|
|
const KDL::Frame target = frame_from_pose7(target_pose7);
|
|
KDL::JntArray q_out(static_cast<unsigned int>(n));
|
|
const int result = record.ik_solver->CartToJnt(q_seed, target, q_out);
|
|
if (result != 0) {
|
|
set_last_error("KDL_IK_FAILED", record.ik_solver->strError(result));
|
|
return -1;
|
|
}
|
|
for (int index = 0; index < n; index += 1) {
|
|
out_joints[index] = q_out(static_cast<unsigned int>(index));
|
|
}
|
|
clear_last_error();
|
|
return 0;
|
|
} catch (const std::exception& error) {
|
|
set_last_error("KDL_IK_FAILED", error.what());
|
|
return -1;
|
|
}
|
|
#else
|
|
(void)robot_handle;
|
|
(void)seed;
|
|
(void)n;
|
|
(void)target_pose7;
|
|
(void)options_json;
|
|
(void)out_joints;
|
|
return not_implemented("kdl_ik requires KDL_SOURCE_DIR");
|
|
#endif
|
|
}
|
|
|
|
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) {
|
|
try {
|
|
const TrapProfile profile = make_trap_profile(length, options_json);
|
|
const int result = write_json(trap_profile_json(profile), out_json, out_len);
|
|
if (result == 0) {
|
|
clear_last_error();
|
|
}
|
|
return result;
|
|
} catch (const std::exception& error) {
|
|
set_last_error("KDL_INVALID_TRAP_PROFILE", error.what());
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
EMSCRIPTEN_KEEPALIVE
|
|
int kdl_last_error(char* out_json, int out_len) {
|
|
return write_json(last_error, out_json, out_len);
|
|
}
|
|
|
|
}
|