Add random pose generator stage (ros2) (#497)

* Add random pose generator * Fix clang-tidy * Apply suggestions from code review Co-authored-by: Sebastian Castro <[email protected]> * Add warning and format --------- Co-authored-by: Henning Kayser <[email protected]> Co-authored-by: Sebastian Castro <[email protected]>
moveit · Oct 24, 2023 · 8375e6e · 8375e6e
1 parent fb02c7f
commit 8375e6e
Show file tree

Hide file tree

Showing 4 changed files with 283 additions and 2 deletions.
diff --git a/core/include/moveit/task_constructor/stages/generate_random_pose.h b/core/include/moveit/task_constructor/stages/generate_random_pose.h
@@ -0,0 +1,116 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2023, PickNik Inc
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Inc nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Authors: Henning Kayser
+   Desc:    Generator Stage for randomized poses based on GeneratePose
+*/
+
+#pragma once
+
+#include <moveit/task_constructor/stages/generate_pose.h>
+
+#include <random>
+
+namespace moveit::task_constructor::stages {
+
+class GenerateRandomPose : public GeneratePose
+{
+public:
+	GenerateRandomPose(const std::string& name = "generate random pose");
+
+	bool canCompute() const override;
+	void compute() override;
+
+	/*
+	 * Function specification for pose dimension samplers.
+	 * The input parameter is the target_pose value used as seed, the returned value is the sampling result.
+	 * */
+	typedef std::function<double(double)> PoseDimensionSampler;
+	enum PoseDimension
+	{
+		X,
+		Y,
+		Z,
+		ROLL,
+		PITCH,
+		YAW
+	};
+
+	/*
+	 * Configure a RandomNumberDistribution (see https://en.cppreference.com/w/cpp/numeric/random) sampler for a
+	 * PoseDimension (X/Y/Z/ROLL/PITCH/YAW) for randomizing the pose.
+	 * The distribution_param is applied to the specified distribution method while the target pose value is used as
+	 * seed. The order in which the PoseDimension samplers are specified matters as the samplers are applied in sequence.
+	 * That way it's possible to implement different Euler angles (i.e. XYZ, ZXZ, YXY) or even construct more complex
+	 * sampling regions by applying translations after rotations.
+	 * Currently supported distributions are:
+	 * * std::uniform_real_distrubtion: distribution_param specifies the full value range around the target_pose value
+	 * * std::normal_distribution: distribution_param is used as stddev, target_pose value is mean
+	 *
+	 * Other distributions can be used by setting the PoseDimensionSampler directly using the function overload.
+	 */
+	template <template <class Realtype = double> class RandomNumberDistribution>
+	void sampleDimension(const PoseDimension pose_dimension, double distribution_param) {
+		sampleDimension(pose_dimension, getPoseDimensionSampler<RandomNumberDistribution>(distribution_param));
+	}
+
+	/*
+	 * Specify a sampling function of type PoseDimensionSampler for randomizing a pose dimension.
+	 */
+	void sampleDimension(const PoseDimension pose_dimension, const PoseDimensionSampler& pose_dimension_sampler) {
+		pose_dimension_samplers_.emplace_back(std::make_pair(pose_dimension, pose_dimension_sampler));
+	}
+
+	/*
+	 * Limits the number of generated solutions
+	 */
+	void setMaxSolutions(size_t max_solutions) { setProperty("max_solutions", max_solutions); }
+
+private:
+	/* \brief Allocate the sampler function for the specified random distribution */
+	template <template <class Realtype = double> class RandomNumberDistribution>
+	PoseDimensionSampler getPoseDimensionSampler(double /* distribution_param */) {
+		static_assert(sizeof(RandomNumberDistribution<double>) == -1, "This distribution type is not supported!");
+		throw 0;  // suppress -Wreturn-type
+	}
+
+	std::vector<std::pair<PoseDimension, PoseDimensionSampler>> pose_dimension_samplers_;
+};
+template <>
+GenerateRandomPose::PoseDimensionSampler
+GenerateRandomPose::getPoseDimensionSampler<std::normal_distribution>(double stddev);
+template <>
+GenerateRandomPose::PoseDimensionSampler
+GenerateRandomPose::getPoseDimensionSampler<std::uniform_real_distribution>(double range);
+}  // namespace moveit::task_constructor::stages
diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt
@@ -8,6 +8,7 @@ add_library(${PROJECT_NAME}_stages SHARED
 	${PROJECT_INCLUDE}/stages/generate_pose.h
 	${PROJECT_INCLUDE}/stages/generate_grasp_pose.h
 	${PROJECT_INCLUDE}/stages/generate_place_pose.h
+	${PROJECT_INCLUDE}/stages/generate_random_pose.h
 	${PROJECT_INCLUDE}/stages/compute_ik.h
 	${PROJECT_INCLUDE}/stages/passthrough.h
 	${PROJECT_INCLUDE}/stages/predicate_filter.h
@@ -28,6 +29,7 @@ add_library(${PROJECT_NAME}_stages SHARED
 	generate_pose.cpp
 	generate_grasp_pose.cpp
 	generate_place_pose.cpp
+	generate_random_pose.cpp
 	compute_ik.cpp
 	passthrough.cpp
 	predicate_filter.cpp

diff --git a/core/src/stages/generate_pose.cpp b/core/src/stages/generate_pose.cpp
@@ -76,9 +76,12 @@ void GeneratePose::compute() {
 	planning_scene::PlanningSceneConstPtr scene = s.end()->scene()->diff();
 
 	geometry_msgs::msg::PoseStamped target_pose = properties().get<geometry_msgs::msg::PoseStamped>("pose");
-	if (target_pose.header.frame_id.empty())
+	if (target_pose.header.frame_id.empty()) {
 		target_pose.header.frame_id = scene->getPlanningFrame();
-	else if (!scene->knowsFrameTransform(target_pose.header.frame_id)) {
+		RCLCPP_WARN(rclcpp::get_logger("GenerateRandomPose"),
+		            "No target pose frame specified, defaulting to scene planning frame: '%s'",
+		            target_pose.header.frame_id.c_str());
+	} else if (!scene->knowsFrameTransform(target_pose.header.frame_id)) {
 		RCLCPP_WARN(LOGGER, "Unknown frame: '%s'", target_pose.header.frame_id.c_str());
 		return;
 	}

diff --git a/core/src/stages/generate_random_pose.cpp b/core/src/stages/generate_random_pose.cpp
@@ -0,0 +1,160 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2023, PickNik Inc
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Inc nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Authors: Henning Kayser */
+
+#include <moveit/task_constructor/stages/generate_random_pose.h>
+#include <moveit/task_constructor/storage.h>
+#include <moveit/task_constructor/marker_tools.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <rviz_marker_tools/marker_creation.h>
+
+#include <Eigen/Geometry>
+#include <tf2_eigen/tf2_eigen.hpp>
+
+#include <chrono>
+
+namespace {
+// TODO(henningkayser): support user-defined random number engines
+std::random_device RD;
+std::mt19937 GEN(RD());
+}  // namespace
+
+namespace moveit::task_constructor::stages {
+
+GenerateRandomPose::GenerateRandomPose(const std::string& name) : GeneratePose(name) {
+	auto& p = properties();
+	p.declare<geometry_msgs::msg::PoseStamped>("pose", "target pose to pass on in spawned states");
+	p.declare<size_t>("max_solutions", 20, "maximum number of spawned solutions in case randomized sampling is enabled");
+	p.property("timeout").setDefaultValue(1.0 /* seconds */);
+}
+
+template <>
+GenerateRandomPose::PoseDimensionSampler
+GenerateRandomPose::getPoseDimensionSampler<std::normal_distribution>(double stddev) {
+	return [stddev, &gen = GEN](double mean) {
+		static std::normal_distribution<double> dist(mean, stddev);
+		return dist(gen);
+	};
+}
+
+template <>
+GenerateRandomPose::PoseDimensionSampler
+GenerateRandomPose::getPoseDimensionSampler<std::uniform_real_distribution>(double range) {
+	return [range, &gen = GEN](double mean) {
+		static std::uniform_real_distribution<double> dist(mean - 0.5 * range, mean + 0.5 * range);
+		return dist(gen);
+	};
+}
+
+bool GenerateRandomPose::canCompute() const {
+	return GeneratePose::canCompute() && !pose_dimension_samplers_.empty();
+}
+
+void GenerateRandomPose::compute() {
+	if (upstream_solutions_.empty())
+		return;
+
+	planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff();
+	geometry_msgs::msg::PoseStamped target_pose = properties().get<geometry_msgs::msg::PoseStamped>("pose");
+	if (target_pose.header.frame_id.empty()) {
+		target_pose.header.frame_id = scene->getPlanningFrame();
+		RCLCPP_WARN(rclcpp::get_logger("GenerateRandomPose"),
+		            "No target pose frame specified, defaulting to scene planning frame: '%s'",
+		            target_pose.header.frame_id.c_str());
+	} else if (!scene->knowsFrameTransform(target_pose.header.frame_id)) {
+		RCLCPP_WARN(rclcpp::get_logger("GenerateRandomPose"), "Unknown frame: '%s'", target_pose.header.frame_id.c_str());
+		return;
+	}
+
+	const auto& spawn_target_pose = [&](const geometry_msgs::msg::PoseStamped& target_pose) {
+		InterfaceState state(scene);
+		state.properties().set("target_pose", target_pose);
+
+		SubTrajectory trajectory;
+		trajectory.setCost(0.0);
+
+		rviz_marker_tools::appendFrame(trajectory.markers(), target_pose, 0.1, "pose frame");
+
+		spawn(std::move(state), std::move(trajectory));
+	};
+
+	// Spawn initial pose
+	spawn_target_pose(target_pose);
+
+	if (pose_dimension_samplers_.empty())
+		return;
+
+	// Use target pose as seed pose
+	const geometry_msgs::msg::PoseStamped seed_pose = target_pose;
+	Eigen::Isometry3d seed, sample;
+	tf2::fromMsg(seed_pose.pose, seed);
+	double elapsed_time = 0.0;
+	const auto start_time = std::chrono::steady_clock::now();
+	size_t spawned_solutions = 0;
+	const size_t max_solutions = properties().get<size_t>("max_solutions");
+	while (elapsed_time < timeout() && ++spawned_solutions < max_solutions) {
+		// Randomize pose using specified dimension samplers applied in the order
+		// in which they have been specified
+		sample = seed;
+		for (const auto& pose_dim_sampler : pose_dimension_samplers_) {
+			switch (pose_dim_sampler.first) {
+				case X:
+					sample.translate(Eigen::Vector3d(pose_dim_sampler.second(0.0), 0, 0));
+					break;
+				case Y:
+					sample.translate(Eigen::Vector3d(0, pose_dim_sampler.second(0.0), 0));
+					break;
+				case Z:
+					sample.translate(Eigen::Vector3d(0, 0, pose_dim_sampler.second(0.0)));
+					break;
+				case ROLL:
+					sample.rotate(Eigen::AngleAxisd(pose_dim_sampler.second(0.0), Eigen::Vector3d::UnitX()));
+					break;
+				case PITCH:
+					sample.rotate(Eigen::AngleAxisd(pose_dim_sampler.second(0.0), Eigen::Vector3d::UnitY()));
+					break;
+				case YAW:
+					sample.rotate(Eigen::AngleAxisd(pose_dim_sampler.second(0.0), Eigen::Vector3d::UnitZ()));
+			}
+		}
+		target_pose.pose = tf2::toMsg(sample);
+
+		// Spawn sampled pose
+		spawn_target_pose(target_pose);
+
+		elapsed_time = std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count();
+	}
+}
+}  // namespace moveit::task_constructor::stages