moveit2
The MoveIt Motion Planning Framework for ROS 2.
test_planning_context_manager.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2020, KU Leuven
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of KU Leuven nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Jeroen De Maeyer */
36 
53 #include "load_test_robot.h"
54 
55 #include <gtest/gtest.h>
56 
57 #include <tf2_eigen/tf2_eigen.hpp>
58 
67 
68 // static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit.ompl_planning.test.test_planning_context_manager");
69 
71 class TestPlanningContext : public ompl_interface_testing::LoadTestRobot, public testing::Test
72 {
73 public:
74  TestPlanningContext(const std::string& robot_name, const std::string& group_name)
75  : LoadTestRobot(robot_name, group_name), node_(std::make_shared<rclcpp::Node>("planning_context_manager_test"))
76  {
77  }
78 
79  void testSimpleRequest(const std::vector<double>& start, const std::vector<double>& goal)
80  {
81  SCOPED_TRACE("testSimpleRequest");
82 
83  // create all the test specific input necessary to make the getPlanningContext call possible
85  pconfig_settings.group = group_name_;
86  pconfig_settings.name = group_name_;
87  pconfig_settings.config = { { "enforce_joint_model_state_space", "0" } };
88 
89  planning_interface::PlannerConfigurationMap pconfig_map{ { pconfig_settings.name, pconfig_settings } };
90  moveit_msgs::msg::MoveItErrorCodes error_code;
92 
93  // setup the planning context manager
95  pcm.setPlannerConfigurations(pconfig_map);
96 
97  // see if it returns the expected planning context
98  auto pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_, false);
99 
100  // the planning context should have a simple setup created
101  EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr);
102 
103  // the OMPL state space in the planning context should be of type JointModelStateSpace
104  EXPECT_NE(dynamic_cast<ompl_interface::JointModelStateSpace*>(pc->getOMPLStateSpace().get()), nullptr);
105 
106  // there did not magically appear path constraints in the planning context
107  EXPECT_TRUE(pc->getPathConstraints()->empty());
108 
109  // solve the planning problem
111  ASSERT_TRUE(pc->solve(res));
112  }
113 
114  void testPathConstraints(const std::vector<double>& start, const std::vector<double>& goal)
115  {
116  SCOPED_TRACE("testPathConstraints");
117 
118  // create all the test specific input necessary to make the getPlanningContext call possible
119  const auto& joint_names = joint_model_group_->getJointModelNames();
120 
122  pconfig_settings.group = group_name_;
123  pconfig_settings.name = group_name_;
124  pconfig_settings.config = { { "enforce_joint_model_state_space", "0" },
125  { "projection_evaluator", "joints(" + joint_names[0] + "," + joint_names[1] + ")" },
126  { "type", "geometric::PRM" } };
127 
128  planning_interface::PlannerConfigurationMap pconfig_map{ { pconfig_settings.name, pconfig_settings } };
129  moveit_msgs::msg::MoveItErrorCodes error_code;
131 
132  // give it some more time, as rejection sampling of the path constraints occasionally takes some time
133  request.allowed_planning_time = 10.0;
134 
135  // create path constraints around start state, to make sure they are satisfied
136  robot_state_->setJointGroupPositions(joint_model_group_, start);
137  Eigen::Isometry3d ee_pose = robot_state_->getGlobalLinkTransform(ee_link_name_);
138  geometry_msgs::msg::Quaternion ee_orientation = tf2::toMsg(Eigen::Quaterniond(ee_pose.rotation()));
139 
140  // setup the planning context manager
142  pcm.setPlannerConfigurations(pconfig_map);
143 
144  // ORIENTATION CONSTRAINTS
145  // ***********************
146  request.path_constraints.orientation_constraints.push_back(createOrientationConstraint(ee_orientation));
147 
148  // See if the planning context manager returns the expected planning context
149  auto pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_, false);
150 
151  EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr);
152 
153  // As the joint_model_group_ has exactly one constraint, we expect a constrained planning state space
154  EXPECT_NE(dynamic_cast<ompl_interface::ConstrainedPlanningStateSpace*>(pc->getOMPLStateSpace().get()), nullptr);
155 
157  ASSERT_TRUE(pc->solve(response));
158 
159  // Are the path constraints created in the planning context?
160  auto path_constraints = pc->getPathConstraints();
161  EXPECT_FALSE(path_constraints->empty());
162  EXPECT_EQ(path_constraints->getOrientationConstraints().size(), 1u);
163  EXPECT_TRUE(path_constraints->getPositionConstraints().empty());
164  EXPECT_TRUE(path_constraints->getJointConstraints().empty());
165  EXPECT_TRUE(path_constraints->getVisibilityConstraints().empty());
166 
167  // Check if all the states in the solution satisfy the path constraints.
168  // A detailed response returns 3 solutions: the ompl solution, the simplified solution and the interpolated
169  // solution. We test all of them here.
170  for (const robot_trajectory::RobotTrajectoryPtr& trajectory : response.trajectory)
171  {
172  for (std::size_t pt_index = 0; pt_index < trajectory->getWayPointCount(); ++pt_index)
173  {
174  EXPECT_TRUE(path_constraints->decide(trajectory->getWayPoint(pt_index)).satisfied);
175  }
176  }
177 
178  // POSITION CONSTRAINTS
179  // ***********************
180  request.path_constraints.orientation_constraints.clear();
181  request.path_constraints.position_constraints.push_back(createPositionConstraint(
182  { ee_pose.translation().x(), ee_pose.translation().y(), ee_pose.translation().z() }, { 0.1, 0.1, 0.1 }));
183 
184  // See if the planning context manager returns the expected planning context
185  pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_, false);
186 
187  EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr);
188 
189  // As the joint_model_group_ has exactly one constraint, we expect a constrained planning state space
190  EXPECT_NE(dynamic_cast<ompl_interface::ConstrainedPlanningStateSpace*>(pc->getOMPLStateSpace().get()), nullptr);
191 
192  // Create a new response, because the solve method does not clear the given response
194  ASSERT_TRUE(pc->solve(response2));
195 
196  // Are the path constraints created in the planning context?
197  path_constraints = pc->getPathConstraints();
198  EXPECT_FALSE(path_constraints->empty());
199  EXPECT_EQ(path_constraints->getPositionConstraints().size(), 1u);
200  EXPECT_TRUE(path_constraints->getOrientationConstraints().empty());
201 
202  // Check if all the states in the solution satisfy the path constraints.
203  // A detailed response returns 3 solutions: the ompl solution, the simplified solution and the interpolated
204  // solution. We test all of them here.
205  for (const robot_trajectory::RobotTrajectoryPtr& trajectory : response2.trajectory)
206  {
207  for (std::size_t pt_index = 0; pt_index < trajectory->getWayPointCount(); ++pt_index)
208  {
209  EXPECT_TRUE(path_constraints->decide(trajectory->getWayPoint(pt_index)).satisfied);
210  }
211  }
212  }
213 
214 protected:
215  void SetUp() override
216  {
217  // create all the fixed input necessary for all planning context managers
218  constraint_sampler_manager_ = std::make_shared<constraint_samplers::ConstraintSamplerManager>();
219  planning_scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
220  }
221 
223  planning_interface::MotionPlanRequest createRequest(const std::vector<double>& start,
224  const std::vector<double>& goal) const
225  {
227  request.group_name = group_name_;
228  request.allowed_planning_time = 5.0;
229 
230  // fill out start state in request
232  start_state.setToDefaultValues();
233  start_state.setJointGroupPositions(joint_model_group_, start);
234  moveit::core::robotStateToRobotStateMsg(start_state, request.start_state);
235 
236  // fill out goal state in request
238  goal_state.setToDefaultValues();
239  goal_state.setJointGroupPositions(joint_model_group_, goal);
240 
241  moveit_msgs::msg::Constraints joint_goal =
243 
244  request.goal_constraints.push_back(joint_goal);
245 
246  return request;
247  }
248 
250  moveit_msgs::msg::PositionConstraint createPositionConstraint(std::array<double, 3> position,
251  std::array<double, 3> dimensions)
252  {
253  shape_msgs::msg::SolidPrimitive box;
254  box.type = shape_msgs::msg::SolidPrimitive::BOX;
255  box.dimensions.resize(3);
256  box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = dimensions[0];
257  box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = dimensions[1];
258  box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = dimensions[2];
259 
260  geometry_msgs::msg::Pose box_pose;
261  box_pose.position.x = position[0];
262  box_pose.position.y = position[1];
263  box_pose.position.z = position[2];
264  box_pose.orientation.w = 1.0;
265 
266  moveit_msgs::msg::PositionConstraint pc;
267  pc.header.frame_id = base_link_name_;
268  pc.link_name = ee_link_name_;
269  pc.constraint_region.primitives.push_back(box);
270  pc.constraint_region.primitive_poses.push_back(box_pose);
271 
272  return pc;
273  }
274 
276  moveit_msgs::msg::OrientationConstraint
277  createOrientationConstraint(const geometry_msgs::msg::Quaternion& nominal_orientation)
278  {
279  moveit_msgs::msg::OrientationConstraint oc;
280  oc.header.frame_id = base_link_name_;
281  oc.link_name = ee_link_name_;
282  oc.orientation = nominal_orientation;
283  oc.absolute_x_axis_tolerance = 0.3;
284  oc.absolute_y_axis_tolerance = 0.3;
285  oc.absolute_z_axis_tolerance = 0.3;
286 
287  return oc;
288  }
289 
290  ompl_interface::ModelBasedStateSpacePtr state_space_;
292  ompl_interface::ModelBasedPlanningContextPtr planning_context_;
293  planning_scene::PlanningScenePtr planning_scene_;
294 
295  constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_;
296 
299  // std::shared_ptr<kinematics::KinematicsBase> ik_plugin_;
300 
303  rclcpp::Node::SharedPtr node_;
304 };
305 
306 /***************************************************************************
307  * Run all tests on the Panda robot
308  * ************************************************************************/
310 {
311 protected:
313  {
314  }
315 };
316 
317 TEST_F(PandaTestPlanningContext, testSimpleRequest)
318 {
319  // use the panda "ready" state from the srdf config as start state
320  // we know this state should be within limits and self-collision free
321  testSimpleRequest({ 0., -0.785, 0., -2.356, 0, 1.571, 0.785 }, { 0., -0.785, 0., -2.356, 0, 1.571, 0.685 });
322 }
323 
324 // TODO(seng): This test is temporarily disabled as it is flaky since #1300. Re-enable when #2015 is resolved.
325 // TEST_F(PandaTestPlanningContext, testPathConstraints)
326 // {
327 // testPathConstraints({ 0., -0.785, 0., -2.356, 0., 1.571, 0.785 }, { .0, -0.785, 0., -2.356, 0., 1.571, 0.685 });
328 // }
329 
330 /***************************************************************************
331  * Run all tests on the Fanuc robot
332  * ************************************************************************/
334 {
335 protected:
336  FanucTestPlanningContext() : TestPlanningContext("fanuc", "manipulator")
337  {
338  }
339 };
340 
341 TEST_F(FanucTestPlanningContext, testSimpleRequest)
342 {
343  testSimpleRequest({ 0., 0., 0., 0., 0., 0. }, { 0., 0., 0., 0., 0., 0.1 });
344 }
345 
346 TEST_F(FanucTestPlanningContext, testPathConstraints)
347 {
348  testPathConstraints({ 0., 0., 0., 0., 0., 0. }, { 0., 0., 0., 0., 0., 0.1 });
349 }
350 
351 /***************************************************************************
352  * MAIN
353  * ************************************************************************/
354 int main(int argc, char** argv)
355 {
356  testing::InitGoogleTest(&argc, argv);
357  rclcpp::init(argc, argv);
358 
359  const int ret = RUN_ALL_TESTS();
360  rclcpp::shutdown();
361  return ret;
362 }
Generic implementation of the tests that can be executed on different robots.
TestPlanningContext(const std::string &robot_name, const std::string &group_name)
planning_interface::MotionPlanRequest createRequest(const std::vector< double > &start, const std::vector< double > &goal) const
moveit_msgs::msg::OrientationConstraint createOrientationConstraint(const geometry_msgs::msg::Quaternion &nominal_orientation)
Helper function to create a orientation constraint.
moveit_msgs::msg::PositionConstraint createPositionConstraint(std::array< double, 3 > position, std::array< double, 3 > dimensions)
Helper function to create a position constraint.
ompl_interface::ModelBasedStateSpacePtr state_space_
void testSimpleRequest(const std::vector< double > &start, const std::vector< double > &goal)
void testPathConstraints(const std::vector< double > &start, const std::vector< double > &goal)
planning_scene::PlanningScenePtr planning_scene_
ompl_interface::ModelBasedPlanningContextSpecification planning_context_spec_
constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_
ompl_interface::ModelBasedPlanningContextPtr planning_context_
const std::vector< std::string > & getJointModelNames() const
Get the names of the joints in this group. These are the names of the joints returned by getJointMode...
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
Definition: robot_state.h:605
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
State space to be used in combination with OMPL's constrained planning functionality.
ModelBasedPlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, moveit_msgs::msg::MoveItErrorCodes &error_code, const rclcpp::Node::SharedPtr &node, bool use_constraints_approximations) const
Returns a planning context to OMPLInterface, which in turn passes it to OMPLPlannerManager.
void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap &pconfig)
Specify configurations for the planners.
Robot independent test class setup.
moveit::core::RobotStatePtr robot_state_
moveit::core::RobotModelPtr robot_model_
LoadTestRobot(const std::string &robot_name, const std::string &group_name)
const moveit::core::JointModelGroup * joint_model_group_
moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
Generates a constraint message intended to be used as a goal constraint for a joint group....
Definition: utils.cpp:145
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
std::map< std::string, PlannerConfigurationSettings > PlannerConfigurationMap
Map from PlannerConfigurationSettings.name to PlannerConfigurationSettings.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
std::vector< robot_trajectory::RobotTrajectoryPtr > trajectory
Specify the settings for a particular planning algorithm, for a particular group. The Planner plugin ...
std::map< std::string, std::string > config
Key-value pairs of settings that get passed to the planning algorithm.
std::string group
The group (as defined in the SRDF) this configuration is meant for.
int main(int argc, char **argv)
TEST_F(PandaTestPlanningContext, testSimpleRequest)