From 47ba7e2a32e877181d488a3f53f9c470afa5422f Mon Sep 17 00:00:00 2001 From: Nikhil Sethi Date: Thu, 10 Jul 2025 13:41:41 +0200 Subject: [PATCH 1/2] Add a YAML Target pose generator - helpful for testing reach of generated toolpaths --- CMakeLists.txt | 1 + .../plugins/yaml_target_pose_generator.h | 42 ++++++++++++++ src/plugins/plugins.cpp | 2 + src/plugins/yaml_target_pose_generator.cpp | 55 +++++++++++++++++++ test/CMakeLists.txt | 2 +- test/reach_study.yaml | 3 + test/test_target_poses.yaml | 19 +++++++ 7 files changed, 123 insertions(+), 1 deletion(-) create mode 100644 include/reach/plugins/yaml_target_pose_generator.h create mode 100644 src/plugins/yaml_target_pose_generator.cpp create mode 100644 test/test_target_poses.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index 2df1c6d7..dbb02b7f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,6 +63,7 @@ add_library( # Implementations src/plugins/multiplicative_evaluator.cpp src/plugins/point_cloud_target_pose_generator.cpp + src/plugins/yaml_target_pose_generator.cpp src/plugins/console_logger.cpp src/plugins/boost_progress_console_logger.cpp src/plugins/no_op.cpp) diff --git a/include/reach/plugins/yaml_target_pose_generator.h b/include/reach/plugins/yaml_target_pose_generator.h new file mode 100644 index 00000000..270c83f3 --- /dev/null +++ b/include/reach/plugins/yaml_target_pose_generator.h @@ -0,0 +1,42 @@ +/* + * Copyright 2019 Southwest Research Institute + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef REACH_PLUGINS_YAML_TARGET_POSE_GENERATOR_H +#define REACH_PLUGINS_YAML_TARGET_POSE_GENERATOR_H + +#include + +namespace reach +{ +class YAMLTargetPoseGenerator : public TargetPoseGenerator +{ +public: + YAMLTargetPoseGenerator(std::string filename); + + VectorIsometry3d generate() const override; + +private: + std::string filename_; +}; + +struct YAMLTargetPoseGeneratorFactory : public TargetPoseGeneratorFactory +{ + using TargetPoseGeneratorFactory::TargetPoseGeneratorFactory; + TargetPoseGenerator::ConstPtr create(const YAML::Node& config) const override; +}; + +} // namespace reach + +#endif // REACH_PLUGINS_YAML_TARGET_POSE_GENERATOR_H diff --git a/src/plugins/plugins.cpp b/src/plugins/plugins.cpp index e1d6b303..0853a1ce 100644 --- a/src/plugins/plugins.cpp +++ b/src/plugins/plugins.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include EXPORT_LOGGER_PLUGIN(reach::BoostProgressConsoleLoggerFactory, BoostProgressConsoleLogger) @@ -12,3 +13,4 @@ EXPORT_EVALUATOR_PLUGIN(reach::NoOpEvaluatorFactory, NoOpEvaluator) EXPORT_IK_SOLVER_PLUGIN(reach::NoOpIKSolverFactory, NoOpIKSolver) EXPORT_DISPLAY_PLUGIN(reach::NoOpDisplayFactory, NoOpDisplay) EXPORT_TARGET_POSE_GENERATOR_PLUGIN(reach::PointCloudTargetPoseGeneratorFactory, PointCloudTargetPoseGenerator) +EXPORT_TARGET_POSE_GENERATOR_PLUGIN(reach::YAMLTargetPoseGeneratorFactory, YAMLTargetPoseGenerator) diff --git a/src/plugins/yaml_target_pose_generator.cpp b/src/plugins/yaml_target_pose_generator.cpp new file mode 100644 index 00000000..fcfd686d --- /dev/null +++ b/src/plugins/yaml_target_pose_generator.cpp @@ -0,0 +1,55 @@ +#include +#include +#include + +#include +#include +#include +using namespace reach; + +static VectorIsometry3d parseYAML(YAML::Node pose_file) +{ + VectorIsometry3d target_poses; + for (const auto& pose : pose_file["poses"]) + { + auto position = pose["position"]; + auto orientation = pose["orientation"]; + + Eigen::Isometry3d target_pose; + target_pose.translation() = + Eigen::Vector3d(position['x'].as(), position['y'].as(), position['z'].as()); + target_pose.linear() = Eigen::Quaterniond(orientation['w'].as(), orientation['x'].as(), + orientation['y'].as(), orientation['z'].as()) + .toRotationMatrix(); + target_poses.push_back(target_pose); + } + return target_poses; +} + +namespace reach +{ +YAMLTargetPoseGenerator::YAMLTargetPoseGenerator(std::string filename) : filename_(resolveURI(filename)) +{ +} + +VectorIsometry3d YAMLTargetPoseGenerator::generate() const +{ + // Check if file exists + if (!boost::filesystem::exists(filename_)) + throw std::runtime_error("File '" + filename_ + "' does not exist"); + + YAML::Node poses = YAML::LoadFile(filename_); + VectorIsometry3d target_poses = parseYAML(poses); + + std::transform(target_poses.begin(), target_poses.end(), std::back_inserter(target_poses), + [](const Eigen::Isometry3d& pose) { return createFrame(pose.translation().cast(), + pose.rotation().cast() * Eigen::Vector3f::UnitZ()); }); + return target_poses; +} + +TargetPoseGenerator::ConstPtr YAMLTargetPoseGeneratorFactory::create(const YAML::Node& config) const +{ + return std::make_shared(get(config, "poses")); +} + +} // namespace reach diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index e5ee83c1..43d06619 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -14,4 +14,4 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib) -install(FILES part.pcd reach_study.yaml DESTINATION share/${PROJECT_NAME}/test) +install(FILES part.pcd reach_study.yaml test_target_poses.yaml DESTINATION share/${PROJECT_NAME}/test) diff --git a/test/reach_study.yaml b/test/reach_study.yaml index 18577603..0acd20b6 100644 --- a/test/reach_study.yaml +++ b/test/reach_study.yaml @@ -15,6 +15,9 @@ evaluator: target_pose_generator: name: PointCloudTargetPoseGenerator pcd_file: package://reach/test/part.pcd + # Uncomment to test YAML target pose generator + # name: YAMLTargetPoseGenerator + # poses: package://reach/test/test_target_poses.yaml display: name: NoOpDisplay diff --git a/test/test_target_poses.yaml b/test/test_target_poses.yaml new file mode 100644 index 00000000..1ae4c147 --- /dev/null +++ b/test/test_target_poses.yaml @@ -0,0 +1,19 @@ +poses: + - position: + x: 1.0 + y: 2.0 + z: 3.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 + - position: + x: 4.0 + y: 5.0 + z: 6.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 1.0 \ No newline at end of file From eb35d5abd367cadef6044a1c31055a742bf5dae1 Mon Sep 17 00:00:00 2001 From: Nikhil Sethi Date: Thu, 17 Jul 2025 16:43:20 +0200 Subject: [PATCH 2/2] Add some documentation for target pose generators --- README.md | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/README.md b/README.md index 3e19dc8d..18c283fa 100644 --- a/README.md +++ b/README.md @@ -37,6 +37,47 @@ The `reach` package also provides the interface definition for the required reac - Generates Cartesian target poses that the robot should attempt to reach during the reach study - These target poses are expected to be relative to the kinematic base frame of the robot - The z-axis of the target poses is expected to oppose the z-axis of the robot kinematic tip frame + + Currently two pose generators are supported: + + - **PointCloudTargetPoseGenerator**: This pose generator accepts a point cloud file (`.pcd`) with normals as input and calculates the reachability for each point + normal in the file. Syntax for the `reach_study.yaml`: + ``` + target_pose_generator: + name: PointCloudTargetPoseGenerator + pcd_file: package://reach/test/part.pcd + ``` + + - **YAMLTargetPoseGenerator**: This pose generator directly accepts a `.yaml` file containing poses and evaluates reachability of each pose in the file. Syntax of `poses.yaml`: + + ``` + poses: + - position: + x: 0.3629872500896454 + y: -0.06363007426261902 + z: -0.0005234468844719231 + orientation: + x: 0.09166051483190088 + y: -0.0017855572788278349 + z: 0.9957755116009193 + w: 0.005127601962168693 + - position: + x: 0.3251079320907593 + y: -0.0632486343383789 + z: 0.006282307207584381 + orientation: + x: 0.07515697809440232 + y: -0.002977202008927411 + z: 0.9971538575858108 + w: 0.005171964196712022 + ``` + + Syntax for reach_study.yaml + ``` + ``` + target_pose_generator: + name: PointCloudTargetPoseGenerator + poses: /path/to/poses.yaml + 1. [`IKSolver`](include/reach/interfaces/ik_solver.h) - Calculates the inverse kinematics solution for the robot at an input 6 degree-of-freedom Cartesian target 1. [`Evaluator`](include/reach/interfaces/evaluator.h)