pr2_ompl_planning_tests: test_planner.cpp Source File

Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <gtest/gtest.h>
00038 #include <planning_scene_monitor/planning_scene_monitor.h>
00039 #include <ompl_interface_ros/ompl_interface_ros.h>
00040 #include <moveit_msgs/GetMotionPlan.h>
00041 #include <kinematic_constraints/utils.h>
00042 
00043 static const std::string PLANNER_SERVICE_NAME="/ompl_planning/plan_kinematic_path";
00044 static const std::string ROBOT_DESCRIPTION="robot_description";
00045 
00046 TEST(OmplInterface, JointModelStateConversion)
00047 {
00048   planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
00049   planning_scene::PlanningScenePtr scene = psm.getPlanningScene();
00050   EXPECT_TRUE(scene->isConfigured());
00051   ompl_interface_ros::OMPLInterfaceROS oi(scene->getRobotModel());
00052   ompl_interface::ModelBasedPlanningContextPtr pc = oi.getPlanningContext("arms", "JointModel");
00053   planning_models::RobotState *kstate(scene->getRobotModel());
00054   ompl::base::ScopedState<> ostate1(pc->getOMPLStateSpace());
00055   ompl::base::ScopedState<> ostate2(ostate1.getSpace());
00056   for (int i = 0 ; i < 100 ; ++i)
00057   {
00058     ostate1.random();
00059     kstate.setToRandomValues();
00060     pc->getOMPLStateSpace()->copyToRobotState(kstate, ostate1.get());
00061     pc->getOMPLStateSpace()->copyToOMPLState(ostate2.get(), kstate);
00062     EXPECT_EQ(ostate1, ostate2);
00063   }
00064 }
00065 
00066 TEST(OmplPlanning, JointGoal)
00067 {
00068     ros::NodeHandle nh;
00069     ros::service::waitForService(PLANNER_SERVICE_NAME);
00070 
00071     ros::ServiceClient planning_service_client = nh.serviceClient<moveit_msgs::GetMotionPlan>(PLANNER_SERVICE_NAME);
00072     EXPECT_TRUE(planning_service_client.exists());
00073     EXPECT_TRUE(planning_service_client.isValid());
00074 
00075     moveit_msgs::GetMotionPlan::Request mplan_req;
00076     moveit_msgs::GetMotionPlan::Response mplan_res;
00077 
00078     planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
00079     planning_scene::PlanningScene &scene = *psm.getPlanningScene();
00080     EXPECT_TRUE(scene.isConfigured());
00081 
00082 
00083     // try a Joint goal
00084     mplan_req.motion_plan_request.group_name = "right_arm";
00085     mplan_req.motion_plan_request.num_planning_attempts = 5;
00086     mplan_req.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00087     const std::vector<std::string>& joint_names = scene.getRobotModel()->getJointModelGroup("right_arm")->getJointModelNames();
00088     mplan_req.motion_plan_request.goal_constraints.resize(1);
00089     mplan_req.motion_plan_request.goal_constraints[0].joint_constraints.resize(joint_names.size());
00090     for(unsigned int i = 0; i < joint_names.size(); i++)
00091     {
00092         mplan_req.motion_plan_request.goal_constraints[0].joint_constraints[i].joint_name = joint_names[i];
00093         mplan_req.motion_plan_request.goal_constraints[0].joint_constraints[i].position = 0.0;
00094         mplan_req.motion_plan_request.goal_constraints[0].joint_constraints[i].tolerance_above = 0.001;
00095         mplan_req.motion_plan_request.goal_constraints[0].joint_constraints[i].tolerance_below = 0.001;
00096         mplan_req.motion_plan_request.goal_constraints[0].joint_constraints[i].weight = 1.0;
00097     }
00098     mplan_req.motion_plan_request.goal_constraints[0].joint_constraints[0].position = -2.0;
00099     mplan_req.motion_plan_request.goal_constraints[0].joint_constraints[3].position = -.2;
00100     mplan_req.motion_plan_request.goal_constraints[0].joint_constraints[5].position = -.2;
00101 
00102     ASSERT_TRUE(planning_service_client.call(mplan_req, mplan_res));
00103     ASSERT_EQ(mplan_res.error_code.val, mplan_res.error_code.SUCCESS);
00104     EXPECT_GT(mplan_res.trajectory.joint_trajectory.points.size(), 0);
00105 }
00106 
00107 TEST(OmplPlanning, PositionGoal)
00108 {
00109     ros::NodeHandle nh;
00110     ros::service::waitForService(PLANNER_SERVICE_NAME);
00111     ros::ServiceClient planning_service_client = nh.serviceClient<moveit_msgs::GetMotionPlan>(PLANNER_SERVICE_NAME);
00112     EXPECT_TRUE(planning_service_client.exists());
00113     EXPECT_TRUE(planning_service_client.isValid());
00114 
00115     moveit_msgs::GetMotionPlan::Request mplan_req;
00116     moveit_msgs::GetMotionPlan::Response mplan_res;
00117 
00118     planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
00119     planning_scene::PlanningScene &scene = *psm.getPlanningScene();
00120     EXPECT_TRUE(scene.isConfigured());
00121 
00122     // try a position goal
00123     mplan_req.motion_plan_request.group_name = "right_arm";
00124     mplan_req.motion_plan_request.num_planning_attempts = 1;
00125     mplan_req.motion_plan_request.allowed_planning_time = ros::Duration(25.0);
00126 
00127     moveit_msgs::PositionConstraint pcm;
00128     pcm.link_name = "r_wrist_roll_link";
00129     pcm.header.frame_id = scene.getRobotModel()->getModelFrame();
00130     pcm.target_point_offset.x = 0;
00131     pcm.target_point_offset.y = 0;
00132     pcm.target_point_offset.z = 0;
00133     pcm.constraint_region.primitives.resize(1);
00134     pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00135     pcm.constraint_region.primitives[0].dimensions.x = 0.001;
00136 
00137     pcm.constraint_region.primitive_poses.resize(1);
00138     pcm.constraint_region.primitive_poses[0].position.x = 0.55;
00139     pcm.constraint_region.primitive_poses[0].position.y = -0.2;
00140     pcm.constraint_region.primitive_poses[0].position.z = 1.25;
00141     pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00142     pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00143     pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00144     pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00145     pcm.weight = 1.0;
00146 
00147     mplan_req.motion_plan_request.goal_constraints.resize(1);
00148     mplan_req.motion_plan_request.goal_constraints[0].position_constraints.push_back(pcm);
00149 
00150     ASSERT_TRUE(planning_service_client.call(mplan_req, mplan_res));
00151     ASSERT_EQ(mplan_res.error_code.val, mplan_res.error_code.SUCCESS);
00152     EXPECT_GT(mplan_res.trajectory.joint_trajectory.points.size(), 0);
00153 }
00154 
00155 TEST(OmplPlanning, OrientationGoal)
00156 {
00157     ros::NodeHandle nh;
00158     ros::service::waitForService(PLANNER_SERVICE_NAME);
00159     ros::ServiceClient planning_service_client = nh.serviceClient<moveit_msgs::GetMotionPlan>(PLANNER_SERVICE_NAME);
00160     EXPECT_TRUE(planning_service_client.exists());
00161     EXPECT_TRUE(planning_service_client.isValid());
00162 
00163     moveit_msgs::GetMotionPlan::Request mplan_req;
00164     moveit_msgs::GetMotionPlan::Response mplan_res;
00165 
00166     planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
00167     planning_scene::PlanningScene &scene = *psm.getPlanningScene();
00168     EXPECT_TRUE(scene.isConfigured());
00169 
00170     // try a position goal
00171     mplan_req.motion_plan_request.group_name = "left_arm";
00172     mplan_req.motion_plan_request.num_planning_attempts = 1;
00173     mplan_req.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00174 
00175 
00176     moveit_msgs::OrientationConstraint ocm;
00177     ocm.link_name = "l_wrist_roll_link";
00178     ocm.header.frame_id = scene.getRobotModel()->getModelFrame();
00179     ocm.orientation.x = 0.0;
00180     ocm.orientation.y = 0.0;
00181     ocm.orientation.z = 0.0;
00182     ocm.orientation.w = 1.0;
00183     ocm.absolute_x_axis_tolerance = 0.2;
00184     ocm.absolute_y_axis_tolerance = 0.1;
00185     ocm.absolute_z_axis_tolerance = 0.4;
00186     ocm.weight = 1.0;
00187 
00188     mplan_req.motion_plan_request.goal_constraints.resize(1);
00189     mplan_req.motion_plan_request.goal_constraints[0].orientation_constraints.push_back(ocm);
00190 
00191     ASSERT_TRUE(planning_service_client.call(mplan_req, mplan_res));
00192     ASSERT_EQ(mplan_res.error_code.val, mplan_res.error_code.SUCCESS);
00193     EXPECT_GT(mplan_res.trajectory.joint_trajectory.points.size(), 0);
00194 }
00195 
00196 
00197 
00198 TEST(OmplPlanning, PoseGoal)
00199 {
00200     ros::NodeHandle nh;
00201     ros::service::waitForService(PLANNER_SERVICE_NAME);
00202     ros::ServiceClient planning_service_client = nh.serviceClient<moveit_msgs::GetMotionPlan>(PLANNER_SERVICE_NAME);
00203     EXPECT_TRUE(planning_service_client.exists());
00204     EXPECT_TRUE(planning_service_client.isValid());
00205 
00206     moveit_msgs::GetMotionPlan::Request mplan_req;
00207     moveit_msgs::GetMotionPlan::Response mplan_res;
00208 
00209     planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
00210     planning_scene::PlanningScene &scene = *psm.getPlanningScene();
00211     EXPECT_TRUE(scene.isConfigured());
00212 
00213     // try a position goal
00214     mplan_req.motion_plan_request.group_name = "left_arm";
00215     mplan_req.motion_plan_request.num_planning_attempts = 1;
00216     mplan_req.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00217 
00218     moveit_msgs::OrientationConstraint ocm;
00219     ocm.link_name = "l_wrist_roll_link";
00220     ocm.header.frame_id = scene.getRobotModel()->getModelFrame();
00221     ocm.orientation.x = 0.0;
00222     ocm.orientation.y = 0.0;
00223     ocm.orientation.z = 0.0;
00224     ocm.orientation.w = 1.0;
00225     ocm.absolute_x_axis_tolerance = 0.2;
00226     ocm.absolute_y_axis_tolerance = 0.1;
00227     ocm.absolute_z_axis_tolerance = 0.4;
00228     ocm.weight = 1.0;
00229 
00230     mplan_req.motion_plan_request.goal_constraints.resize(1);
00231     mplan_req.motion_plan_request.goal_constraints[0].orientation_constraints.push_back(ocm);
00232 
00233     moveit_msgs::PositionConstraint pcm;
00234     pcm.link_name = "l_wrist_roll_link";
00235     pcm.target_point_offset.x = 0;
00236     pcm.target_point_offset.y = 0;
00237     pcm.target_point_offset.z = 0;
00238     pcm.constraint_region.primitives.resize(1);
00239     pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
00240     pcm.constraint_region.primitives[0].dimensions.x = 0.001;
00241 
00242     pcm.header.frame_id = scene.getRobotModel()->getModelFrame();
00243     pcm.constraint_region.primitive_poses.resize(1);
00244     pcm.constraint_region.primitive_poses[0].position.x = 0.55;
00245     pcm.constraint_region.primitive_poses[0].position.y = 0.2;
00246     pcm.constraint_region.primitive_poses[0].position.z = 1.25;
00247     pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
00248     pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
00249     pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
00250     pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
00251     pcm.weight = 1.0;
00252     mplan_req.motion_plan_request.goal_constraints[0].position_constraints.push_back(pcm);
00253 
00254     ASSERT_TRUE(planning_service_client.call(mplan_req, mplan_res));
00255     ASSERT_EQ(mplan_res.error_code.val, mplan_res.error_code.SUCCESS);
00256     EXPECT_GT(mplan_res.trajectory.joint_trajectory.points.size(), 0);
00257 }
00258 
00259 TEST(OmplPlanning, SimplePoseGoal)
00260 {
00261     planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
00262     planning_scene::PlanningScene &scene = *psm.getPlanningScene();
00263     EXPECT_TRUE(scene.isConfigured());
00264 
00265     geometry_msgs::PoseStamped pose;
00266     pose.header.frame_id = scene.getRobotModel()->getModelFrame();
00267     pose.pose.position.x = 0.55;
00268     pose.pose.position.y = 0.2;
00269     pose.pose.position.z = 1.25;
00270     pose.pose.orientation.x = 0.0;
00271     pose.pose.orientation.y = 0.0;
00272     pose.pose.orientation.z = 0.0;
00273     pose.pose.orientation.w = 1.0;
00274     moveit_msgs::Constraints goal = kinematic_constraints::constructGoalConstraints("l_wrist_roll_link", pose);
00275 
00276     moveit_msgs::GetMotionPlan::Request mplan_req;
00277     moveit_msgs::GetMotionPlan::Response mplan_res;
00278 
00279     mplan_req.motion_plan_request.group_name = "left_arm";
00280     mplan_req.motion_plan_request.num_planning_attempts = 1;
00281     mplan_req.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00282     mplan_req.motion_plan_request.goal_constraints.push_back(goal);
00283 
00284 
00285     ros::NodeHandle nh;
00286     ros::service::waitForService(PLANNER_SERVICE_NAME);
00287     ros::ServiceClient planning_service_client = nh.serviceClient<moveit_msgs::GetMotionPlan>(PLANNER_SERVICE_NAME);
00288     EXPECT_TRUE(planning_service_client.exists());
00289     EXPECT_TRUE(planning_service_client.isValid());
00290 
00291     ASSERT_TRUE(planning_service_client.call(mplan_req, mplan_res));
00292     ASSERT_EQ(mplan_res.error_code.val, mplan_res.error_code.SUCCESS);
00293     EXPECT_GT(mplan_res.trajectory.joint_trajectory.points.size(), 0);
00294 }
00295 
00296 int main(int argc, char **argv)
00297 {
00298   testing::InitGoogleTest(&argc, argv);
00299   ros::init(argc, argv, "test_ompl_planning");
00300   ros::AsyncSpinner spinner(1);
00301   spinner.start();
00302 
00303   return RUN_ALL_TESTS();
00304 }