moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
test_state_validity_checker.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 */
51#include "load_test_robot.h"
52
53#include <limits>
54#include <ostream>
55
56#include <gtest/gtest.h>
57
63
64#include <ompl/geometric/SimpleSetup.h>
65
67constexpr bool VERBOSE = false;
68
69rclcpp::Logger getLogger()
70{
71 return moveit::getLogger("moveit.planners.ompl.test_state_validity_checker");
72}
73
75std::ostream& operator<<(std::ostream& os, const std::vector<double>& v)
76{
77 os << "( ";
78 for (auto value : v)
79 os << value << ", ";
80 os << " )";
81 return os;
82}
83
85class TestStateValidityChecker : public ompl_interface_testing::LoadTestRobot, public testing::Test
86{
87public:
88 TestStateValidityChecker(const std::string& robot_name, const std::string& group_name)
89 : LoadTestRobot(robot_name, group_name)
90 {
91 }
92
94 {
95 SCOPED_TRACE("testConstructor");
96 EXPECT_TRUE(planning_context_.get());
97 }
98
100 void testJointLimits(const std::vector<double>& position_in_limits)
101 {
102 SCOPED_TRACE("testJointLimits");
103
104 // create a validity checker for this test
105 auto checker = std::make_shared<ompl_interface::StateValidityChecker>(planning_context_.get());
106 checker->setVerbose(VERBOSE);
107
108 robot_state_->setJointGroupPositions(joint_model_group_, position_in_limits);
109
110 // use a scoped OMPL state so we don't have to call allocState and freeState
111 // (as recommended in the OMPL documantion)
112 ompl::base::ScopedState<> ompl_state(state_space_);
113 state_space_->copyToOMPLState(ompl_state.get(), *robot_state_);
114
115 RCLCPP_DEBUG_STREAM(getLogger(), ompl_state.reals());
116
117 // assume the given position is not in self-collision
118 // and there are no collision objects or path constraints so this state should be valid
119 EXPECT_TRUE(checker->isValid(ompl_state.get()));
120
121 // move first joint obviously outside any joint limits
122 ompl_state->as<ompl_interface::JointModelStateSpace::StateType>()->values[0] = std::numeric_limits<double>::max();
123 ompl_state->as<ompl_interface::JointModelStateSpace::StateType>()->clearKnownInformation();
124
125 RCLCPP_DEBUG_STREAM(getLogger(), ompl_state.reals());
126
127 EXPECT_FALSE(checker->isValid(ompl_state.get()));
128 }
129
131 void testSelfCollision(const std::vector<double>& position_in_self_collision)
132 {
133 SCOPED_TRACE("testSelfCollision");
134
135 // create a validity checker for this test
136 auto checker = std::make_shared<ompl_interface::StateValidityChecker>(planning_context_.get());
137 checker->setVerbose(VERBOSE);
138
139 robot_state_->setJointGroupPositions(joint_model_group_, position_in_self_collision);
140
141 // use a scoped OMPL state so we don't have to call allocState and freeState
142 // (as recommended in the OMPL documantion)
143 ompl::base::ScopedState<> ompl_state(state_space_);
144 state_space_->copyToOMPLState(ompl_state.get(), *robot_state_);
145
146 RCLCPP_DEBUG_STREAM(getLogger(), ompl_state.reals());
147
148 // the given state is known to be in self-collision, we check it here
149 EXPECT_FALSE(checker->isValid(ompl_state.get()));
150
151 // but it should respect the joint limits
152 EXPECT_TRUE(robot_state_->satisfiesBounds());
153 }
154
155 void testPathConstraints(const std::vector<double>& position_in_joint_limits)
156 {
157 SCOPED_TRACE("testPathConstraints");
158
159 ASSERT_NE(planning_context_, nullptr) << "Initialize planning context before adding path constraints.";
160
161 // set the robot to a known position that is within the joint limits and collision free
162 robot_state_->setJointGroupPositions(joint_model_group_, position_in_joint_limits);
163
164 // create position constraints around the given robot state
165 moveit_msgs::msg::Constraints path_constraints;
166 Eigen::Isometry3d ee_pose = robot_state_->getGlobalLinkTransform(ee_link_name_);
167 path_constraints.name = "test_position_constraints";
168 path_constraints.position_constraints.push_back(createPositionConstraint(
169 { ee_pose.translation().x(), ee_pose.translation().y(), ee_pose.translation().z() }, { 0.1, 0.1, 0.1 }));
170
171 moveit_msgs::msg::MoveItErrorCodes error_code_not_used;
172 ASSERT_TRUE(planning_context_->setPathConstraints(path_constraints, &error_code_not_used));
173
174 auto checker = std::make_shared<ompl_interface::StateValidityChecker>(planning_context_.get());
175 checker->setVerbose(VERBOSE);
176
177 // use a scoped OMPL state so we don't have to call allocState and freeState
178 // (as recommended in the OMPL documantion)
179 ompl::base::ScopedState<> ompl_state(state_space_);
180 state_space_->copyToOMPLState(ompl_state.get(), *robot_state_);
181
182 RCLCPP_DEBUG_STREAM(getLogger(), ompl_state.reals());
183
184 EXPECT_TRUE(checker->isValid(ompl_state.get()));
185
186 // move the position constraints away from the current end-effector position to make it fail
187 moveit_msgs::msg::Constraints path_constraints_2(path_constraints);
188 path_constraints_2.position_constraints.at(0).constraint_region.primitive_poses.at(0).position.z += 0.2;
189
190 ASSERT_TRUE(planning_context_->setPathConstraints(path_constraints_2, &error_code_not_used));
191
192 // clear the cached validity of the state before checking again,
193 // otherwise the path constraints will not be checked.
194 ompl_state->as<ompl_interface::JointModelStateSpace::StateType>()->clearKnownInformation();
195
196 EXPECT_FALSE(checker->isValid(ompl_state.get()));
197 }
198
199protected:
200 void SetUp() override
201 {
202 // setup all the input we need to create a StateValidityChecker
205 };
206
208 {
210 state_space_ = std::make_shared<ompl_interface::JointModelStateSpace>(space_spec);
211 state_space_->computeLocations(); // this gets normally called in the state space factory
212 }
213
215 {
216 ASSERT_NE(state_space_, nullptr) << "Initialize state space before creating the planning context.";
217
219 planning_context_spec_.ompl_simple_setup_ = std::make_shared<ompl::geometric::SimpleSetup>(state_space_);
221 std::make_shared<ompl_interface::ModelBasedPlanningContext>(group_name_, planning_context_spec_);
222
223 planning_scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
224 planning_context_->setPlanningScene(planning_scene_);
226 start_state.setToDefaultValues();
227 planning_context_->setCompleteInitialState(start_state);
228 }
229
231 moveit_msgs::msg::PositionConstraint createPositionConstraint(std::array<double, 3> position,
232 std::array<double, 3> dimensions)
233 {
234 shape_msgs::msg::SolidPrimitive box;
235 box.type = shape_msgs::msg::SolidPrimitive::BOX;
236 box.dimensions.resize(3);
237 box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_X] = dimensions[0];
238 box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y] = dimensions[1];
239 box.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z] = dimensions[2];
240
241 geometry_msgs::msg::Pose box_pose;
242 box_pose.position.x = position[0];
243 box_pose.position.y = position[1];
244 box_pose.position.z = position[2];
245 box_pose.orientation.w = 1.0;
246
247 moveit_msgs::msg::PositionConstraint pc;
248 pc.header.frame_id = base_link_name_;
249 pc.link_name = ee_link_name_;
250 pc.constraint_region.primitives.push_back(box);
251 pc.constraint_region.primitive_poses.push_back(box_pose);
252
253 return pc;
254 }
255
256 ompl_interface::ModelBasedStateSpacePtr state_space_;
258 ompl_interface::ModelBasedPlanningContextPtr planning_context_;
259 planning_scene::PlanningScenePtr planning_scene_;
260};
261
262// /***************************************************************************
263// * Run all tests on the Panda robot
264// * ************************************************************************/
266{
267protected:
268 PandaValidity() : TestStateValidityChecker("panda", "panda_arm")
269 {
270 }
271};
272
273TEST_F(PandaValidity, testConstructor)
274{
275 testConstructor();
276}
277
278TEST_F(PandaValidity, testJointLimits)
279{
280 // use the panda "ready" state from the srdf config
281 // we know this state should be within limits and self-collision free
282 testJointLimits({ 0., -0.785, 0., -2.356, 0., 1.571, 0.785 });
283}
284
285TEST_F(PandaValidity, testSelfCollision)
286{
287 // the given state has self collision between "hand" and "panda_link2"
288 // (I just tried a couple of random states until I found one that collided.)
289 testSelfCollision({ 2.31827, -0.169668, 2.5225, -2.98568, -0.36355, 0.808339, 0.0843406 });
290}
291
292TEST_F(PandaValidity, testPathConstraints)
293{
294 // use the panda "ready" state from the srdf config
295 // we know this state should be within limits and self-collision free
296 testPathConstraints({ 0., -0.785, 0., -2.356, 0., 1.571, 0.785 });
297}
298
299/***************************************************************************
300 * Run all tests on the Fanuc robot
301 * ************************************************************************/
303{
304protected:
305 FanucTest() : TestStateValidityChecker("fanuc", "manipulator")
306 {
307 }
308};
309
310TEST_F(FanucTest, createStateValidityChecker)
311{
312 testConstructor();
313}
314
315TEST_F(FanucTest, testJointLimits)
316{
317 // I assume the Fanucs's zero state is within limits and self-collision free
318 testJointLimits({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 });
319}
320
321TEST_F(FanucTest, testSelfCollision)
322{
323 // the given state has self collision between "base_link" and "link_5"
324 // (I just tried a couple of random states until I found one that collided.)
325 testSelfCollision({ -2.95993, -0.682185, -2.43873, -0.939784, 3.0544, 0.882294 });
326}
327
328TEST_F(FanucTest, testPathConstraints)
329{
330 // I assume the Fanucs's zero state is within limits and self-collision free
331 testPathConstraints({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 });
332}
333
334/***************************************************************************
335 * MAIN
336 * ************************************************************************/
337int main(int argc, char** argv)
338{
339 testing::InitGoogleTest(&argc, argv);
340 return RUN_ALL_TESTS();
341}
Generic implementation of the tests that can be executed on different robots.
ompl_interface::ModelBasedStateSpacePtr state_space_
void testJointLimits(const std::vector< double > &position_in_limits)
void testSelfCollision(const std::vector< double > &position_in_self_collision)
planning_scene::PlanningScenePtr planning_scene_
moveit_msgs::msg::PositionConstraint createPositionConstraint(std::array< double, 3 > position, std::array< double, 3 > dimensions)
Helper function to create a position constraint.
void testPathConstraints(const std::vector< double > &position_in_joint_limits)
ompl_interface::ModelBasedPlanningContextSpecification planning_context_spec_
ompl_interface::ModelBasedPlanningContextPtr planning_context_
TestStateValidityChecker(const std::string &robot_name, const std::string &group_name)
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition robot_state.h:90
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
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_
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
Definition logger.cpp:79
int main(int argc, char **argv)
rclcpp::Logger getLogger()
TEST_F(PandaValidity, testConstructor)
constexpr bool VERBOSE
This flag sets the verbosity level for the state validity checker.
std::ostream & operator<<(std::ostream &os, const std::vector< double > &v)
Pretty print std:vectors.