Add a YAML Target pose generator

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)

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)

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 <reach/interfaces/target_pose_generator.h>
+
+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

src/plugins/plugins.cpp

@@ -3,6 +3,7 @@
 #include <reach/plugins/multiplicative_evaluator.h>
 #include <reach/plugins/no_op.h>
 #include <reach/plugins/point_cloud_target_pose_generator.h>
+#include <reach/plugins/yaml_target_pose_generator.h>
 
 #include <reach/plugin_utils.h>
 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)

src/plugins/yaml_target_pose_generator.cpp

@@ -0,0 +1,55 @@
+#include <reach/plugins/yaml_target_pose_generator.h>
+#include <reach/plugin_utils.h>
+#include <reach/types.h>
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/PCLPointField.h>
+#include <yaml-cpp/yaml.h>
+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<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 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<float>(), 
+                  pose.rotation().cast<float>() * Eigen::Vector3f::UnitZ()); });
+  return target_poses;
+}
+
+TargetPoseGenerator::ConstPtr YAMLTargetPoseGeneratorFactory::create(const YAML::Node& config) const
+{
+  return std::make_shared<YAMLTargetPoseGenerator>(get<std::string>(config, "poses"));
+}
+
+}  // namespace reach

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)

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

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