From 8375e6ef3e498b16b311bb7c00c487acc44890b2 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Tue, 24 Oct 2023 17:06:05 +0200 Subject: [PATCH] Add random pose generator stage (ros2) (#497) * Add random pose generator * Fix clang-tidy * Apply suggestions from code review Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> * Add warning and format --------- Co-authored-by: Henning Kayser Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- .../stages/generate_random_pose.h | 116 +++++++++++++ core/src/stages/CMakeLists.txt | 2 + core/src/stages/generate_pose.cpp | 7 +- core/src/stages/generate_random_pose.cpp | 160 ++++++++++++++++++ 4 files changed, 283 insertions(+), 2 deletions(-) create mode 100644 core/include/moveit/task_constructor/stages/generate_random_pose.h create mode 100644 core/src/stages/generate_random_pose.cpp diff --git a/core/include/moveit/task_constructor/stages/generate_random_pose.h b/core/include/moveit/task_constructor/stages/generate_random_pose.h new file mode 100644 index 00000000..348b8251 --- /dev/null +++ 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 + +#include + +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 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