Add a YAML Target pose generator by nikhil-sethi · Pull Request #89 · ros-industrial/reach

nikhil-sethi · 2025-07-17T08:08:33Z

Description

Based on the discussion here, this PR adds another target pose generator that can read YAML files with pose values directly without relying on a point cloud (.pcd) file.

A good use-case would be to save the toolpaths generated from the noether package as a YAML file and then evaluate their reachability offline.

Changes

yaml_target_pose_generator.h: Interface file for the pose generator
yaml_target_pose_generator.cpp: Source code for the generator. Important assumption is that the z-axis of the quaternions defined in the YAML file will be directly used as the target normal vector.
test_target_poses.yaml: A test yaml file which is parsed by the pose generator.

Test

A test poses file is given under the test folder. Comment out the current target pose generator and uncomment the YAMLTargetPoseGenerator along with the poses file. Then run colcon test to test the plugin.

- helpful for testing reach of generated toolpaths

marip8

Looks good overall, and I like having an alternative to the point cloud approach. I just have a few suggestions to address before merging

marip8 · 2025-09-23T15:17:18Z

+#include <pcl/io/pcd_io.h>
+#include <pcl/PCLPointField.h>


PCL includes shouldn't be necessary if we're not dealing with point clouds

Suggested change

#include <pcl/io/pcd_io.h>

#include <pcl/PCLPointField.h>

My bad. Copy paste :p

marip8 · 2025-09-23T15:24:16Z

+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<double>(), position['y'].as<double>(), position['z'].as<double>());
+    target_pose.linear() = Eigen::Quaterniond(orientation['w'].as<double>(), orientation['x'].as<double>(),
+                                              orientation['y'].as<double>(), orientation['z'].as<double>())
+                               .toRotationMatrix();
+    target_poses.push_back(target_pose);
+  }
+  return target_poses;
+}


Let's use the YAML serialization structs here, and flatten the serialization keys to [x, y, z, qw, qx, qy, qz].

Suggested change

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<double>(), position['y'].as<double>(), position['z'].as<double>());

target_pose.linear() = Eigen::Quaterniond(orientation['w'].as<double>(), orientation['x'].as<double>(),

orientation['y'].as<double>(), orientation['z'].as<double>())

.toRotationMatrix();

target_poses.push_back(target_pose);

}

return target_poses;

}

namespace YAML

{

template <typename FloatT>

struct convert<Eigen::Transform<FloatT, 3, Eigen::Isometry>>

{

using T = Eigen::Transform<FloatT, 3, Eigen::Isometry>;

static bool decode(const Node& node, T& val)

{

Eigen::Matrix<FloatT, 3, 1> trans;

trans.x() = get<FloatT>(node, "x");

trans.y() = get<FloatT>(node, "y");

trans.z() = get<FloatT>(node, "z");

Eigen::Quaternion<FloatT> quat;

quat.w() = get<FloatT>(node, "qw");

quat.x() = get<FloatT>(node, "qx");

quat.y() = get<FloatT>(node, "qy");

quat.z() = get<FloatT>(node, "qz");

val = Eigen::Translation<FloatT, 3>(trans) * quat;

return true;

}

};

// Serialization for std::vectors with Eigen-specific allocators

template <typename T>

struct convert<std::vector<T, Eigen::aligned_allocator<T>>>

{

static bool decode(const Node& node, std::vector<T, Eigen::aligned_allocator<T>>& v)

{

if (!node.IsSequence())

return false;

v.clear();

for (const auto& elem : node)

v.push_back(elem.as<T>());

return true;

}

};

} // namespace YAML

marip8 · 2025-09-23T15:31:12Z

+  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<float>(), 
+                  pose.rotation().cast<float>() * Eigen::Vector3f::UnitZ()); });
+  return target_poses;


I think the transformation using createFrame is not necessary because we already have a list of fully defined poses from the YAML file. The purpose of createFrame was to create a fully defined pose from just a position vector and a normal vector (e.g., from a point cloud point and normal). Also, unless I'm mistaken, using std::back_inserter(target_poses) is adding all of the outputs of createFrame to the existing list of poses, effectively doubling the number of reach poses. This seems like it might have been unintentional, but was there a reason this was set up this way?

Suggested change

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<float>(),

pose.rotation().cast<float>() * Eigen::Vector3f::UnitZ()); });

return target_poses;

YAML::Node poses = YAML::LoadFile(filename_);

return poses.as<VectorIsometry3d>();

marip8 · 2025-09-23T15:32:11Z

+class YAMLTargetPoseGenerator : public TargetPoseGenerator
+{
+public:
+  YAMLTargetPoseGenerator(std::string filename);


Avoids an unnecessary copy

Suggested change

YAMLTargetPoseGenerator(std::string filename);

YAMLTargetPoseGenerator(const std::string& filename);

marip8 · 2025-09-23T15:32:28Z

+
+namespace reach
+{
+YAMLTargetPoseGenerator::YAMLTargetPoseGenerator(std::string filename) : filename_(resolveURI(filename))


Suggested change

YAMLTargetPoseGenerator::YAMLTargetPoseGenerator(std::string filename) : filename_(resolveURI(filename))

YAMLTargetPoseGenerator::YAMLTargetPoseGenerator(const std::string& filename) : filename_(resolveURI(filename))

marip8 · 2025-09-23T15:34:28Z

+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


Changes/simplifies the format to align with the suggested YAML serialization changes

Suggested change

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

- x: 1.0

y: 2.0

z: 3.0

qw: 1.0

qx: 0.0

qy: 0.0

qz: 0.0

- x: 4.0

y: 5.0

z: 6.0

qw: 1.0

qx: 0.0

qy: 0.0

qz: 0.0

marip8 · 2025-09-23T15:35:57Z

+    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


Changed to align with suggested serialization method

Suggested change

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

- x: 0.3629872500896454

y: -0.06363007426261902

z: -0.0005234468844719231

qw: 0.005127601962168693

qx: 0.09166051483190088

qy: -0.0017855572788278349

qz: 0.9957755116009193

- x: 0.3251079320907593

y: -0.0632486343383789

z: 0.006282307207584381

qw: 0.005171964196712022

qx: 0.07515697809440232

qy: -0.002977202008927411

qz: 0.9971538575858108

nikhil-sethi added 2 commits July 14, 2025 10:30

Add a YAML Target pose generator

47ba7e2

- helpful for testing reach of generated toolpaths

Add some documentation for target pose generators

eb35d5a

nikhil-sethi marked this pull request as ready for review July 17, 2025 14:44

nikhil-sethi mentioned this pull request Jul 17, 2025

Add transformed yaml pose generator plugin ros-industrial/reach_ros2#45

Open

marip8 requested changes Sep 23, 2025

View reviewed changes

marip8 mentioned this pull request Sep 23, 2025

Remove implicit rotation of target pose #90

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add a YAML Target pose generator#89

Add a YAML Target pose generator#89
nikhil-sethi wants to merge 2 commits intoros-industrial:masterfrom
sam-xl:add/yaml_target_pose_gen

nikhil-sethi commented Jul 17, 2025

Uh oh!

marip8 left a comment

Uh oh!

marip8 Sep 23, 2025

Uh oh!

nikhil-sethi Oct 7, 2025

Uh oh!

marip8 Sep 23, 2025

Uh oh!

marip8 Sep 23, 2025

Uh oh!

marip8 Sep 23, 2025

Uh oh!

marip8 Sep 23, 2025

Uh oh!

marip8 Sep 23, 2025

Uh oh!

marip8 Sep 23, 2025

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

-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<double>(), position['y'].as<double>(), position['z'].as<double>());
-    target_pose.linear() = Eigen::Quaterniond(orientation['w'].as<double>(), orientation['x'].as<double>(),
-                                              orientation['y'].as<double>(), orientation['z'].as<double>())
-                               .toRotationMatrix();
-    target_poses.push_back(target_pose);
-  }
-  return target_poses;
-}
+namespace YAML
+{
+template <typename FloatT>
+struct convert<Eigen::Transform<FloatT, 3, Eigen::Isometry>>
+{
+  using T = Eigen::Transform<FloatT, 3, Eigen::Isometry>;
+  static bool decode(const Node& node, T& val)
+  {
+    Eigen::Matrix<FloatT, 3, 1> trans;
+    trans.x() = get<FloatT>(node, "x");
+    trans.y() = get<FloatT>(node, "y");
+    trans.z() = get<FloatT>(node, "z");
+    Eigen::Quaternion<FloatT> quat;
+    quat.w() = get<FloatT>(node, "qw");
+    quat.x() = get<FloatT>(node, "qx");
+    quat.y() = get<FloatT>(node, "qy");
+    quat.z() = get<FloatT>(node, "qz");
+    val = Eigen::Translation<FloatT, 3>(trans) * quat;
+    return true;
+  }
+};
+// Serialization for std::vectors with Eigen-specific allocators
+template <typename T>
+struct convert<std::vector<T, Eigen::aligned_allocator<T>>>
+{
+  static bool decode(const Node& node, std::vector<T, Eigen::aligned_allocator<T>>& v)
+  {
+    if (!node.IsSequence())
+      return false;
+    v.clear();
+    for (const auto& elem : node)
+      v.push_back(elem.as<T>());
+    return true;
+  }
+};
+}  // namespace YAML

	YAMLTargetPoseGenerator(std::string filename);
	YAMLTargetPoseGenerator(const std::string& filename);

	YAMLTargetPoseGenerator::YAMLTargetPoseGenerator(std::string filename) : filename_(resolveURI(filename))
	YAMLTargetPoseGenerator::YAMLTargetPoseGenerator(const std::string& filename) : filename_(resolveURI(filename))

Conversation

nikhil-sethi commented Jul 17, 2025

Description

Changes

Test

Uh oh!

marip8 left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants