56#include <gtest/gtest.h>
64#include <ompl/geometric/SimpleSetup.h>
75std::ostream&
operator<<(std::ostream& os,
const std::vector<double>& v)
95 SCOPED_TRACE(
"testConstructor");
102 SCOPED_TRACE(
"testJointLimits");
105 auto checker = std::make_shared<ompl_interface::StateValidityChecker>(
planning_context_.get());
115 RCLCPP_DEBUG_STREAM(
getLogger(), ompl_state.reals());
119 EXPECT_TRUE(checker->isValid(ompl_state.get()));
125 RCLCPP_DEBUG_STREAM(
getLogger(), ompl_state.reals());
127 EXPECT_FALSE(checker->isValid(ompl_state.get()));
133 SCOPED_TRACE(
"testSelfCollision");
136 auto checker = std::make_shared<ompl_interface::StateValidityChecker>(
planning_context_.get());
146 RCLCPP_DEBUG_STREAM(
getLogger(), ompl_state.reals());
149 EXPECT_FALSE(checker->isValid(ompl_state.get()));
157 SCOPED_TRACE(
"testPathConstraints");
159 ASSERT_NE(
planning_context_,
nullptr) <<
"Initialize planning context before adding path constraints.";
165 moveit_msgs::msg::Constraints path_constraints;
167 path_constraints.name =
"test_position_constraints";
169 { ee_pose.translation().x(), ee_pose.translation().y(), ee_pose.translation().z() }, { 0.1, 0.1, 0.1 }));
171 moveit_msgs::msg::MoveItErrorCodes error_code_not_used;
172 ASSERT_TRUE(
planning_context_->setPathConstraints(path_constraints, &error_code_not_used));
174 auto checker = std::make_shared<ompl_interface::StateValidityChecker>(
planning_context_.get());
182 RCLCPP_DEBUG_STREAM(
getLogger(), ompl_state.reals());
184 EXPECT_TRUE(checker->isValid(ompl_state.get()));
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;
190 ASSERT_TRUE(
planning_context_->setPathConstraints(path_constraints_2, &error_code_not_used));
196 EXPECT_FALSE(checker->isValid(ompl_state.get()));
210 state_space_ = std::make_shared<ompl_interface::JointModelStateSpace>(space_spec);
216 ASSERT_NE(
state_space_,
nullptr) <<
"Initialize state space before creating the planning context.";
232 std::array<double, 3> dimensions)
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];
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;
247 moveit_msgs::msg::PositionConstraint pc;
250 pc.constraint_region.primitives.push_back(box);
251 pc.constraint_region.primitive_poses.push_back(box_pose);
282 testJointLimits({ 0., -0.785, 0., -2.356, 0., 1.571, 0.785 });
289 testSelfCollision({ 2.31827, -0.169668, 2.5225, -2.98568, -0.36355, 0.808339, 0.0843406 });
296 testPathConstraints({ 0., -0.785, 0., -2.356, 0., 1.571, 0.785 });
318 testJointLimits({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 });
325 testSelfCollision({ -2.95993, -0.682185, -2.43873, -0.939784, 3.0544, 0.882294 });
331 testPathConstraints({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 });
339 testing::InitGoogleTest(&argc, argv);
340 return RUN_ALL_TESTS();
Generic implementation of the tests that can be executed on different robots.
void setupPlanningContext()
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.
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_
std::string ee_link_name_
LoadTestRobot(const std::string &robot_name, const std::string &group_name)
const moveit::core::JointModelGroup * joint_model_group_
std::string base_link_name_
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
ModelBasedStateSpacePtr state_space_
og::SimpleSetupPtr ompl_simple_setup_
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.