demo_construct_state_database.cpp
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 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 <moveit/ompl_interface/ompl_interface.h>
00038 #include <moveit/robot_model_loader/robot_model_loader.h>
00039 #include <moveit/profiler/profiler.h>
00040 
00041 #include <boost/math/constants/constants.hpp>
00042 
00043 static const std::string ROBOT_DESCRIPTION = "robot_description";
00044 
00045 moveit_msgs::Constraints getConstraints()
00046 {
00047   moveit_msgs::OrientationConstraint ocm;
00048   ocm.link_name = "r_wrist_roll_link";
00049   ocm.header.frame_id = "torso_lift_link";
00050   ocm.orientation.x = 0;
00051   ocm.orientation.y = 0;
00052   ocm.orientation.z = 0;
00053   ocm.orientation.w = 1.0;
00054   ocm.absolute_x_axis_tolerance = 0.1;
00055   ocm.absolute_y_axis_tolerance = 0.1;
00056   ocm.absolute_z_axis_tolerance = boost::math::constants::pi<double>();
00057   ocm.weight = 1.0;
00058   moveit_msgs::Constraints cmsg;
00059   cmsg.orientation_constraints.resize(1, ocm);
00060   cmsg.name = ocm.link_name + ":upright";
00061   return cmsg;
00062 }
00063 
00064 void computeDB(const robot_model::RobotModelPtr& robot_model, unsigned int ns, unsigned int ne)
00065 {
00066   planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(robot_model));
00067   ompl_interface::OMPLInterface ompl_interface(robot_model);
00068   moveit_msgs::Constraints c = getConstraints();
00069   ompl_interface::ConstraintApproximationConstructionOptions opt;
00070   opt.state_space_parameterization = "PoseModel";
00071   opt.samples = ns;
00072   opt.edges_per_sample = ne;
00073   opt.explicit_motions = true;
00074   opt.max_edge_length = 0.2;
00075   opt.explicit_points_resolution = 0.05;
00076   opt.max_explicit_points = 10;
00077 
00078   ompl_interface.getConstraintsLibrary().addConstraintApproximation(c, "right_arm", ps, opt);
00079   ompl_interface.getConstraintsLibrary().saveConstraintApproximations("~/constraints_approximation_database");
00080   ROS_INFO("Done");
00081 }
00082 
00083 /*
00084 bool constructConstraintApproximation(moveit_msgs::ConstructConstraintApproximation::Request &req,
00085 moveit_msgs::ConstructConstraintApproximation::Response &res)
00086 {
00087   planning_scene::PlanningScenePtr diff_scene = psm_.getPlanningScene()->diff();
00088   robot_state::robotStateMsgToRobotState(*psm_.getPlanningScene()->getTransforms(), req.start_state,
00089 diff_scene->getCurrentStateNonConst());
00090   ompl_interface::ConstraintApproximationConstructionResults ca_res =
00091     ompl_interface_.getConstraintsLibrary().addConstraintApproximation(req.constraint, req.group,
00092 req.state_space_parameterization,
00093                                                                        diff_scene, req.samples, req.edges_per_sample);
00094   if (ca_res.approx)
00095   {
00096     res.sampling_success_rate = ca_res.sampling_success_rate;
00097     res.state_sampling_time = ca_res.state_sampling_time;
00098     res.state_connection_time = ca_res.state_connection_time;
00099     res.filename = ca_res.approx->getFilename();
00100     return ompl_interface_.saveConstraintApproximations();
00101   }
00102   else
00103     return false;
00104 }
00105 */
00106 
00107 int main(int argc, char** argv)
00108 {
00109   ros::init(argc, argv, "construct_ompl_state_database", ros::init_options::AnonymousName);
00110 
00111   ros::AsyncSpinner spinner(1);
00112   spinner.start();
00113 
00114   unsigned int nstates = 1000;
00115   unsigned int nedges = 0;
00116 
00117   if (argc > 1)
00118     try
00119     {
00120       nstates = boost::lexical_cast<unsigned int>(argv[1]);
00121     }
00122     catch (...)
00123     {
00124     }
00125 
00126   if (argc > 2)
00127     try
00128     {
00129       nedges = boost::lexical_cast<unsigned int>(argv[2]);
00130     }
00131     catch (...)
00132     {
00133     }
00134 
00135   robot_model_loader::RobotModelLoader rml(ROBOT_DESCRIPTION);
00136   computeDB(rml.getModel(), nstates, nedges);
00137 
00138   ros::shutdown();
00139   return 0;
00140 }


ompl
Author(s): Ioan Sucan
autogenerated on Mon Jul 24 2017 02:21:33