同步KDL工程源码到云仓库
This commit is contained in:
wangdequan
731
kdl-wasm/bindings/kdl_c_api.cpp
Normal file
731
kdl-wasm/bindings/kdl_c_api.cpp
Normal file
@@ -0,0 +1,731 @@
|
||||
#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 "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;
|
||||
#ifdef KDL_WASM_HAS_OROCOS_KDL
|
||||
KDL::Chain chain;
|
||||
std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver;
|
||||
std::unique_ptr<KDL::ChainJntToJacSolver> jac_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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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();
|
||||
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<KDL::ChainFkSolverPos_recursive>(record.chain);
|
||||
record.jac_solver = std::make_unique<KDL::ChainJntToJacSolver>(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]);
|
||||
}
|
||||
#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;
|
||||
}
|
||||
|
||||
} // 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<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) {
|
||||
(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);
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user