moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
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
68
71{
72public:
73 TestPlanningContext(const std::string& robot_name, const std::string& group_name)
74 : LoadTestRobot(robot_name, group_name), node_(std::make_shared<rclcpp::Node>("planning_context_manager_test"))
75 {
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 pc->solve(res);
112 ASSERT_TRUE(res.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
113 }
114
115 void testPathConstraints(const std::vector<double>& start, const std::vector<double>& goal)
116 {
117 SCOPED_TRACE("testPathConstraints");
118
119 // create all the test specific input necessary to make the getPlanningContext call possible
120 const auto& joint_names = joint_model_group_->getJointModelNames();
121
123 pconfig_settings.group = group_name_;
124 pconfig_settings.name = group_name_;
125 pconfig_settings.config = { { "enforce_joint_model_state_space", "0" },
126 { "projection_evaluator", "joints(" + joint_names[0] + "," + joint_names[1] + ")" },
127 { "type", "geometric::PRM" } };
128
129 planning_interface::PlannerConfigurationMap pconfig_map{ { pconfig_settings.name, pconfig_settings } };
130 moveit_msgs::msg::MoveItErrorCodes error_code;
132
133 // give it some more time, as rejection sampling of the path constraints occasionally takes some time
134 request.allowed_planning_time = 10.0;
135
136 // create path constraints around start state, to make sure they are satisfied
137 robot_state_->setJointGroupPositions(joint_model_group_, start);
138 Eigen::Isometry3d ee_pose = robot_state_->getGlobalLinkTransform(ee_link_name_);
139 geometry_msgs::msg::Quaternion ee_orientation = tf2::toMsg(Eigen::Quaterniond(ee_pose.rotation()));
140
141 // setup the planning context manager
143 pcm.setPlannerConfigurations(pconfig_map);
144
145 // ORIENTATION CONSTRAINTS
146 // ***********************
147 request.path_constraints.orientation_constraints.push_back(createOrientationConstraint(ee_orientation));
148
149 // See if the planning context manager returns the expected planning context
150 auto pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_, false);
151
152 EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr);
153
154 // As the joint_model_group_ has exactly one constraint, we expect a constrained planning state space
155 EXPECT_NE(dynamic_cast<ompl_interface::ConstrainedPlanningStateSpace*>(pc->getOMPLStateSpace().get()), nullptr);
156
158 pc->solve(response);
159 ASSERT_TRUE(response.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
160
161 // Are the path constraints created in the planning context?
162 auto path_constraints = pc->getPathConstraints();
163 EXPECT_FALSE(path_constraints->empty());
164 EXPECT_EQ(path_constraints->getOrientationConstraints().size(), 1u);
165 EXPECT_TRUE(path_constraints->getPositionConstraints().empty());
166 EXPECT_TRUE(path_constraints->getJointConstraints().empty());
167 EXPECT_TRUE(path_constraints->getVisibilityConstraints().empty());
168
169 // Check if all the states in the solution satisfy the path constraints.
170 // A detailed response returns 3 solutions: the ompl solution, the simplified solution and the interpolated
171 // solution. We test all of them here.
172 for (const robot_trajectory::RobotTrajectoryPtr& trajectory : response.trajectory)
173 {
174 for (std::size_t pt_index = 0; pt_index < trajectory->getWayPointCount(); ++pt_index)
175 {
176 EXPECT_TRUE(path_constraints->decide(trajectory->getWayPoint(pt_index)).satisfied);
177 }
178 }
179
180 // POSITION CONSTRAINTS
181 // ***********************
182 request.path_constraints.orientation_constraints.clear();
183 request.path_constraints.position_constraints.push_back(createPositionConstraint(
184 { ee_pose.translation().x(), ee_pose.translation().y(), ee_pose.translation().z() }, { 0.1, 0.1, 0.1 }));
185
186 // See if the planning context manager returns the expected planning context
187 pc = pcm.getPlanningContext(planning_scene_, request, error_code, node_, false);
188
189 EXPECT_NE(pc->getOMPLSimpleSetup(), nullptr);
190
191 // As the joint_model_group_ has exactly one constraint, we expect a constrained planning state space
192 EXPECT_NE(dynamic_cast<ompl_interface::ConstrainedPlanningStateSpace*>(pc->getOMPLStateSpace().get()), nullptr);
193
194 // Create a new response, because the solve method does not clear the given response
196 pc->solve(response2);
197 ASSERT_TRUE(response2.error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
198
199 // Are the path constraints created in the planning context?
200 path_constraints = pc->getPathConstraints();
201 EXPECT_FALSE(path_constraints->empty());
202 EXPECT_EQ(path_constraints->getPositionConstraints().size(), 1u);
203 EXPECT_TRUE(path_constraints->getOrientationConstraints().empty());
204
205 // Check if all the states in the solution satisfy the path constraints.
206 // A detailed response returns 3 solutions: the ompl solution, the simplified solution and the interpolated
207 // solution. We test all of them here.
208 for (const robot_trajectory::RobotTrajectoryPtr& trajectory : response2.trajectory)
209 {
210 for (std::size_t pt_index = 0; pt_index < trajectory->getWayPointCount(); ++pt_index)
211 {
212 EXPECT_TRUE(path_constraints->decide(trajectory->getWayPoint(pt_index)).satisfied);
213 }
214 }
215 }
216
217protected:
218 void SetUp() override
219 {
220 // create all the fixed input necessary for all planning context managers
221 constraint_sampler_manager_ = std::make_shared<constraint_samplers::ConstraintSamplerManager>();
222 planning_scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
223 }
224
226 planning_interface::MotionPlanRequest createRequest(const std::vector<double>& start,
227 const std::vector<double>& goal) const
228 {
230 request.group_name = group_name_;
231 request.allowed_planning_time = 5.0;
232
233 // fill out start state in request
235 start_state.setToDefaultValues();
236 start_state.setJointGroupPositions(joint_model_group_, start);
237 moveit::core::robotStateToRobotStateMsg(start_state, request.start_state);
238
239 // fill out goal state in request
241 goal_state.setToDefaultValues();
243
244 moveit_msgs::msg::Constraints joint_goal =
246
247 request.goal_constraints.push_back(joint_goal);
248
249 return request;
250 }
251
253 moveit_msgs::msg::PositionConstraint createPositionConstraint(std::array<double, 3> position,
254 std::array<double, 3> dimensions)
255 {
256 shape_msgs::msg::SolidPrimitive box;
257 box.type = shape_msgs::msg::SolidPrimitive::BOX;
258 box.dimensions.resize(3);
259 box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = dimensions[0];
260 box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = dimensions[1];
261 box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = dimensions[2];
262
263 geometry_msgs::msg::Pose box_pose;
264 box_pose.position.x = position[0];
265 box_pose.position.y = position[1];
266 box_pose.position.z = position[2];
267 box_pose.orientation.w = 1.0;
268
269 moveit_msgs::msg::PositionConstraint pc;
270 pc.header.frame_id = base_link_name_;
271 pc.link_name = ee_link_name_;
272 pc.constraint_region.primitives.push_back(box);
273 pc.constraint_region.primitive_poses.push_back(box_pose);
274
275 return pc;
276 }
277
279 moveit_msgs::msg::OrientationConstraint
280 createOrientationConstraint(const geometry_msgs::msg::Quaternion& nominal_orientation)
281 {
282 moveit_msgs::msg::OrientationConstraint oc;
283 oc.header.frame_id = base_link_name_;
284 oc.link_name = ee_link_name_;
285 oc.orientation = nominal_orientation;
286 oc.absolute_x_axis_tolerance = 0.3;
287 oc.absolute_y_axis_tolerance = 0.3;
288 oc.absolute_z_axis_tolerance = 0.3;
289
290 return oc;
291 }
292
293 ompl_interface::ModelBasedStateSpacePtr state_space_;
295 ompl_interface::ModelBasedPlanningContextPtr planning_context_;
296 planning_scene::PlanningScenePtr planning_scene_;
297
298 constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_;
299
302 // std::shared_ptr<kinematics::KinematicsBase> ik_plugin_;
303
306 rclcpp::Node::SharedPtr node_;
307};
308
309/***************************************************************************
310 * Run all tests on the Panda robot
311 * ************************************************************************/
313{
314protected:
316 {
317 }
318};
319
321{
322 // use the panda "ready" state from the srdf config as start state
323 // we know this state should be within limits and self-collision free
324 testSimpleRequest({ 0., -0.785, 0., -2.356, 0, 1.571, 0.785 }, { 0., -0.785, 0., -2.356, 0, 1.571, 0.685 });
325}
326
327// TODO(seng): This test is temporarily disabled as it is flaky since #1300. Re-enable when #2015 is resolved.
328// TEST_F(PandaTestPlanningContext, testPathConstraints)
329// {
330// testPathConstraints({ 0., -0.785, 0., -2.356, 0., 1.571, 0.785 }, { .0, -0.785, 0., -2.356, 0., 1.571, 0.685 });
331// }
332
333/***************************************************************************
334 * Run all tests on the Fanuc robot
335 * ************************************************************************/
337{
338protected:
340 {
341 }
342};
343
345{
346 testSimpleRequest({ 0., 0., 0., 0., 0., 0. }, { 0., 0., 0., 0., 0., 0.1 });
347}
348
349TEST_F(FanucTestPlanningContext, testPathConstraints)
350{
351 testPathConstraints({ 0., 0., 0., 0., 0., 0. }, { 0., 0., 0., 0., 0., 0.1 });
352}
353
354/***************************************************************************
355 * MAIN
356 * ************************************************************************/
357int main(int argc, char** argv)
358{
359 testing::InitGoogleTest(&argc, argv);
360 rclcpp::init(argc, argv);
361
362 const int ret = RUN_ALL_TESTS();
363 rclcpp::shutdown();
364 return ret;
365}
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...
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:152
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.
void setNodeLoggerName(const std::string &name)
Call once after creating a node to initialize logging namespaces.
Definition logger.cpp:73
std::map< std::string, PlannerConfigurationSettings > PlannerConfigurationMap
Map from PlannerConfigurationSettings.name to PlannerConfigurationSettings.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
moveit_msgs::msg::MoveItErrorCodes error_code
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)