wangdequan/KDL_WORK
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

接入KDL native IK与链路采样

Browse Source
This commit is contained in:
wangdequan
2026-06-27 09:19:57 -04:00
parent ef5f14aa2a
commit 2817cba164
7 changed files with 504 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

327
kdl-wasm/bindings/kdl_c_api.cpp
View File

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