Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Constraint samplers seed #1411

Merged
merged 14 commits into from
Jul 28, 2022
Merged

Constraint samplers seed #1411

Show file tree
Hide file tree
Changes from 11 commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
3f70efc
added seed uint input to constructor
henrygerardmoore Jun 29, 2022
4afbc30
re-enabled disabled tests
henrygerardmoore Jun 29, 2022
4537d94
changed initialization
henrygerardmoore Jun 29, 2022
c01a6d7
added set seeds so tests will pass as expected and will not rely on rng
henrygerardmoore Jun 30, 2022
55239b4
removed unnecessary seeds
henrygerardmoore Jun 30, 2022
2e5a32b
removed stub function
henrygerardmoore Jun 30, 2022
3ee1412
removed setting default seed
henrygerardmoore Jun 30, 2022
cfdc88d
Merge branch 'main' into constraint_sampler_seed
henrygerardmoore Jun 30, 2022
52e26f9
removed commented out code
henrygerardmoore Jun 30, 2022
c77fac8
Merge branch 'main' into constraint_sampler_seed
henrygerardmoore Jul 19, 2022
6e1bdfa
added direct inclusion of dependencies
henrygerardmoore Jul 19, 2022
7d2e5bc
Merge branch 'main' into constraint_sampler_seed
henrygerardmoore Jul 25, 2022
a55d329
Merge branch 'main' into constraint_sampler_seed
henrygerardmoore Jul 26, 2022
ec494bf
Merge branch 'main' into constraint_sampler_seed
Jul 28, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 40 additions & 6 deletions ...core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,11 +40,11 @@
#include <moveit/macros/class_forward.h>
#include <random_numbers/random_numbers.h>
#include "rclcpp/rclcpp.hpp"
#include <string>
#include <Eigen/Geometry>

namespace constraint_samplers
{
random_numbers::RandomNumberGenerator createSeededRNG([[maybe_unused]] const std::string& seed_param);

MOVEIT_CLASS_FORWARD(JointConstraintSampler); // Defines JointConstraintSamplerPtr, ConstPtr, WeakPtr... etc

/**
Expand All @@ -70,8 +70,25 @@ class JointConstraintSampler : public ConstraintSampler
*
*/
JointConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name)
: ConstraintSampler(scene, group_name)
, random_number_generator_(createSeededRNG("~joint_constraint_sampler_random_seed"))
: ConstraintSampler(scene, group_name), random_number_generator_(random_numbers::RandomNumberGenerator())
henrygerardmoore marked this conversation as resolved.
Show resolved Hide resolved
{
}

/**
* Constructor
*
* @param [in] scene The planning scene used to check the constraint
*
* @param [in] group_name The group name associated with the
* constraint. Will be invalid if no group name is passed in or the
* joint model group cannot be found in the kinematic model
*
* @param [in] seed The rng seed to be used
*
*/
JointConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name,
unsigned int seed)
: ConstraintSampler(scene, group_name), random_number_generator_(random_numbers::RandomNumberGenerator(seed))
{
}

Expand Down Expand Up @@ -308,8 +325,25 @@ class IKConstraintSampler : public ConstraintSampler
*
*/
IKConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name)
: ConstraintSampler(scene, group_name)
, random_number_generator_(createSeededRNG("~ik_constraint_sampler_random_seed"))
: ConstraintSampler(scene, group_name), random_number_generator_(random_numbers::RandomNumberGenerator())
{
}

/**
* \brief Constructor
*
* @param [in] scene The planning scene used to check the constraint
*
* @param [in] group_name The group name associated with the
* constraint. Will be invalid if no group name is passed in or the
* joint model group cannot be found in the kinematic model
*
* @param [in] seed The rng seed to be used
*
*/
IKConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name,
unsigned int seed)
: ConstraintSampler(scene, group_name), random_number_generator_(random_numbers::RandomNumberGenerator(seed))
{
}

Expand Down
14 changes: 0 additions & 14 deletions moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,20 +44,6 @@ namespace constraint_samplers
{
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_constraint_samplers.default_constraint_samplers");

random_numbers::RandomNumberGenerator createSeededRNG([[maybe_unused]] const std::string& seed_param)
{
// int rng_seed;
// if (ros::param::get(seed_param, rng_seed))
// {
// ROS_DEBUG_STREAM_NAMED("constraint_samplers", "Creating random number generator with seed " << rng_seed);
// return random_numbers::RandomNumberGenerator(rng_seed);
// }
// else
// {
return random_numbers::RandomNumberGenerator(12345);
// }
}

bool JointConstraintSampler::configure(const moveit_msgs::msg::Constraints& constr)
{
// construct the constraints
Expand Down
16 changes: 6 additions & 10 deletions moveit_core/constraint_samplers/test/test_constraint_samplers.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1130,10 +1130,9 @@ TEST_F(LoadPlanningModelsPr2, SubgroupPoseConstraintsSampler)
(double)succ / (double)NT);
}

TEST_F(LoadPlanningModelsPr2, DISABLED_JointConstraintsSamplerSeeded)
TEST_F(LoadPlanningModelsPr2, JointConstraintsSamplerSeeded)
{
// ros::param::set("~joint_constraint_sampler_random_seed", 12345);
constraint_samplers::JointConstraintSampler seeded_sampler1(ps_, "right_arm");
constraint_samplers::JointConstraintSampler seeded_sampler1(ps_, "right_arm", 314159);
kinematic_constraints::JointConstraint jc(robot_model_);
moveit_msgs::msg::JointConstraint jcm;
jcm.position = 0.42;
Expand All @@ -1152,7 +1151,7 @@ TEST_F(LoadPlanningModelsPr2, DISABLED_JointConstraintsSamplerSeeded)
const double* joint_positions = ks.getVariablePositions();
const std::vector<double> joint_positions_v(joint_positions, joint_positions + ks.getVariableCount());

constraint_samplers::JointConstraintSampler seeded_sampler2(ps_, "right_arm");
constraint_samplers::JointConstraintSampler seeded_sampler2(ps_, "right_arm", 314159);
EXPECT_TRUE(seeded_sampler2.configure(js));
ks.setToDefaultValues();
EXPECT_TRUE(seeded_sampler2.sample(ks, ks, 1));
Expand All @@ -1161,7 +1160,6 @@ TEST_F(LoadPlanningModelsPr2, DISABLED_JointConstraintsSamplerSeeded)
using namespace testing;
EXPECT_THAT(joint_positions_v, ContainerEq(joint_positions_v2));

// ros::param::del("~joint_constraint_sampler_random_seed");
constraint_samplers::JointConstraintSampler seeded_sampler3(ps_, "right_arm");
EXPECT_TRUE(seeded_sampler3.configure(js));
ks.setToDefaultValues();
Expand All @@ -1172,9 +1170,8 @@ TEST_F(LoadPlanningModelsPr2, DISABLED_JointConstraintsSamplerSeeded)
EXPECT_THAT(joint_positions_v2, Not(ContainerEq(joint_positions_v3)));
}

TEST_F(LoadPlanningModelsPr2, DISABLED_IKConstraintsSamplerSeeded)
TEST_F(LoadPlanningModelsPr2, IKConstraintsSamplerSeeded)
{
// ros::param::set("~ik_constraint_sampler_random_seed", 12345);
kinematic_constraints::PositionConstraint pc(robot_model_);
moveit_msgs::msg::PositionConstraint pcm;

Expand All @@ -1201,7 +1198,7 @@ TEST_F(LoadPlanningModelsPr2, DISABLED_IKConstraintsSamplerSeeded)
moveit::core::Transforms& tf = ps_->getTransformsNonConst();
EXPECT_TRUE(pc.configure(pcm, tf));

constraint_samplers::IKConstraintSampler seeded_sampler1(ps_, "left_arm");
constraint_samplers::IKConstraintSampler seeded_sampler1(ps_, "left_arm", 265358);
EXPECT_TRUE(seeded_sampler1.configure(constraint_samplers::IKSamplingPose(pc)));

moveit::core::RobotState ks(robot_model_);
Expand All @@ -1214,7 +1211,7 @@ TEST_F(LoadPlanningModelsPr2, DISABLED_IKConstraintsSamplerSeeded)
const Eigen::Isometry3d root_to_left_tool1 = ks.getFrameTransform("l_gripper_tool_frame", &found);
EXPECT_TRUE(found);

constraint_samplers::IKConstraintSampler seeded_sampler2(ps_, "left_arm");
constraint_samplers::IKConstraintSampler seeded_sampler2(ps_, "left_arm", 265358);
EXPECT_TRUE(seeded_sampler2.configure(constraint_samplers::IKSamplingPose(pc)));
ks.setToDefaultValues();
ks.update();
Expand All @@ -1224,7 +1221,6 @@ TEST_F(LoadPlanningModelsPr2, DISABLED_IKConstraintsSamplerSeeded)
const Eigen::Isometry3d root_to_left_tool2 = ks.getFrameTransform("l_gripper_tool_frame", &found);
EXPECT_TRUE(found);

// ros::param::del("~ik_constraint_sampler_random_seed");
constraint_samplers::IKConstraintSampler seeded_sampler3(ps_, "left_arm");
EXPECT_TRUE(seeded_sampler3.configure(constraint_samplers::IKSamplingPose(pc)));
ks.setToDefaultValues();
Expand Down