From 78981f97c1a08ff75f91165d97b5ebde3930f222 Mon Sep 17 00:00:00 2001 From: Kyle Cesare Date: Sat, 25 Nov 2023 20:17:47 -0700 Subject: [PATCH] Add API to load URDF from in-memory string. --- Cargo.lock | 1 + crates/optik-cpp/include/optik.hpp | 19 +++++++++++++++++++ crates/optik-cpp/src/lib.cpp | 15 ++++++++++++++- crates/optik-cpp/src/lib.rs | 13 +++++++++++++ crates/optik/Cargo.toml | 1 + crates/optik/src/lib.rs | 12 +++++++++++- 6 files changed, 59 insertions(+), 2 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 339a076..9b92d4b 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -444,6 +444,7 @@ dependencies = [ "rand_chacha", "rayon", "slsqp-sys", + "urdf-rs", ] [[package]] diff --git a/crates/optik-cpp/include/optik.hpp b/crates/optik-cpp/include/optik.hpp index 36ae192..0ebfc31 100644 --- a/crates/optik-cpp/include/optik.hpp +++ b/crates/optik-cpp/include/optik.hpp @@ -38,8 +38,19 @@ void SetParallelism(unsigned int n); class Robot final { public: + Robot(Robot& other) = delete; + Robot(Robot&& other) : inner_(std::move(other.inner_)) { + other.inner_ = nullptr; + }; ~Robot(); + Robot& operator=(Robot& other) = delete; + Robot& operator=(Robot&& other) { + inner_ = std::move(other.inner_); + other.inner_ = nullptr; + return *this; + }; + //! Load a URDF model file from the given path, and build a chain from the //! named base to the named end-effector link. //! @@ -49,6 +60,14 @@ class Robot final { const std::string& base_link, const std::string& ee_link); + //! See `FromUrdfFile`. Loads the model from an in-memory string. + //! + //! Panics if the string not contain a valid model file, or if either of the + //! links don't exist. + static Robot FromUrdfStr(const std::string& urdf, + const std::string& base_link, + const std::string& ee_link); + //! Draw a random generalized position vector from a uniform distribution //! subject to the joint limits. Eigen::VectorXd RandomConfiguration() const noexcept; diff --git a/crates/optik-cpp/src/lib.cpp b/crates/optik-cpp/src/lib.cpp index 9f0b66f..eb7f4a6 100644 --- a/crates/optik-cpp/src/lib.cpp +++ b/crates/optik-cpp/src/lib.cpp @@ -9,6 +9,9 @@ extern void optik_set_parallelism(unsigned int n); extern optik::detail::robot* optik_robot_from_urdf_file(const char* path, const char* base_link, const char* ee_link); +extern optik::detail::robot* optik_robot_from_urdf_str(const char* urdf, + const char* base_link, + const char* ee_link); extern void optik_robot_free(optik::detail::robot* robot); extern unsigned int optik_robot_dof(const optik::detail::robot* robot); @@ -26,7 +29,11 @@ namespace optik { void SetParallelism(unsigned int n) { optik_set_parallelism(n); } -Robot::~Robot() { optik_robot_free(inner_); } +Robot::~Robot() { + if (inner_ != nullptr) { + optik_robot_free(inner_); + } +} Robot Robot::FromUrdfFile(const std::string& path, const std::string& base_link, const std::string& ee_link) { @@ -34,6 +41,12 @@ Robot Robot::FromUrdfFile(const std::string& path, const std::string& base_link, ee_link.c_str()); } +Robot Robot::FromUrdfStr(const std::string& urdf, const std::string& base_link, + const std::string& ee_link) { + return optik_robot_from_urdf_str(urdf.c_str(), base_link.c_str(), + ee_link.c_str()); +} + Eigen::VectorXd Robot::RandomConfiguration() const noexcept { double* q_data = optik_robot_random_configuration(inner_); Eigen::VectorXd q = Eigen::Map(q_data, num_positions()); diff --git a/crates/optik-cpp/src/lib.rs b/crates/optik-cpp/src/lib.rs index bf5176d..6a23954 100644 --- a/crates/optik-cpp/src/lib.rs +++ b/crates/optik-cpp/src/lib.rs @@ -39,6 +39,19 @@ extern "C" fn optik_robot_from_urdf_file( Box::into_raw(Box::new(Robot::from_urdf_file(path, base_link, ee_link))) } +#[no_mangle] +extern "C" fn optik_robot_from_urdf_str( + urdf: *const c_char, + base_link: *const c_char, + ee_link: *const c_char, +) -> *mut Robot { + let urdf = to_str(urdf); + let base_link = to_str(base_link); + let ee_link = to_str(ee_link); + + Box::into_raw(Box::new(Robot::from_urdf_str(urdf, base_link, ee_link))) +} + #[no_mangle] extern "C" fn optik_robot_free(robot: *mut Robot) { unsafe { diff --git a/crates/optik/Cargo.toml b/crates/optik/Cargo.toml index af17866..e0a6ccb 100644 --- a/crates/optik/Cargo.toml +++ b/crates/optik/Cargo.toml @@ -17,6 +17,7 @@ rand = "0.8" rand_chacha = "0.3" rayon = "1.8" slsqp-sys = { path = "../slsqp-sys" } +urdf-rs = "0.8" [[example]] name = "example" diff --git a/crates/optik/src/lib.rs b/crates/optik/src/lib.rs index 21e5d3a..19d90ed 100644 --- a/crates/optik/src/lib.rs +++ b/crates/optik/src/lib.rs @@ -42,7 +42,17 @@ impl Robot { } pub fn from_urdf_file(path: impl AsRef, base_link: &str, ee_link: &str) -> Self { - let chain = Chain::::from_urdf_file(path).expect("error parsing URDF file!"); + let urdf = urdf_rs::read_file(path).expect("error parsing URDF file!"); + Robot::from_urdf(&urdf, base_link, ee_link) + } + + pub fn from_urdf_str(urdf: &str, base_link: &str, ee_link: &str) -> Self { + let urdf = urdf_rs::read_from_string(urdf).expect("error parsing URDF file!"); + Robot::from_urdf(&urdf, base_link, ee_link) + } + + pub fn from_urdf(urdf: &urdf_rs::Robot, base_link: &str, ee_link: &str) -> Self { + let chain = Chain::::from(urdf); let base_link = chain .find_link(base_link)