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,
00065            unsigned int ns, unsigned int ne)
00066 {
00067   planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(robot_model));
00068   ompl_interface::OMPLInterface ompl_interface(robot_model);
00069   moveit_msgs::Constraints c = getConstraints();
00070   ompl_interface::ConstraintApproximationConstructionOptions opt;
00071   opt.state_space_parameterization = "PoseModel";
00072   opt.samples = ns;
00073   opt.edges_per_sample = ne;
00074   opt.explicit_motions = true;
00075   opt.max_edge_length = 0.2;
00076   opt.explicit_points_resolution = 0.05;
00077   opt.max_explicit_points = 10;
00078 
00079   ompl_interface.getConstraintsLibrary().addConstraintApproximation(c, "right_arm", ps, opt);
00080   ompl_interface.getConstraintsLibrary().saveConstraintApproximations("~/constraints_approximation_database");
00081   ROS_INFO("Done");
00082 }
00083 
00084   /*
00085   bool constructConstraintApproximation(moveit_msgs::ConstructConstraintApproximation::Request &req, 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, diff_scene->getCurrentStateNonConst());
00089     ompl_interface::ConstraintApproximationConstructionResults ca_res =
00090       ompl_interface_.getConstraintsLibrary().addConstraintApproximation(req.constraint, req.group, req.state_space_parameterization,
00091                                                                          diff_scene, req.samples, req.edges_per_sample);
00092     if (ca_res.approx)
00093     {
00094       res.sampling_success_rate = ca_res.sampling_success_rate;
00095       res.state_sampling_time = ca_res.state_sampling_time;
00096       res.state_connection_time = ca_res.state_connection_time;
00097       res.filename = ca_res.approx->getFilename();
00098       return ompl_interface_.saveConstraintApproximations();
00099     }
00100     else
00101       return false;
00102   }
00103   */
00104 
00105 int main(int argc, char **argv)
00106 {
00107   ros::init(argc, argv, "construct_ompl_state_database", ros::init_options::AnonymousName);
00108 
00109   ros::AsyncSpinner spinner(1);
00110   spinner.start();
00111 
00112   unsigned int nstates = 1000;
00113   unsigned int nedges = 0;
00114 
00115   if (argc > 1)
00116     try
00117     {
00118       nstates = boost::lexical_cast<unsigned int>(argv[1]);
00119     }
00120     catch(...)
00121     {
00122     }
00123 
00124   if (argc > 2)
00125     try
00126     {
00127       nedges = boost::lexical_cast<unsigned int>(argv[2]);
00128     }
00129     catch(...)
00130     {
00131     }
00132 
00133   robot_model_loader::RobotModelLoader rml(ROBOT_DESCRIPTION);
00134   computeDB(rml.getModel(), nstates, nedges);
00135 
00136   ros::shutdown();
00137   return 0;
00138 }


ompl
Author(s): Ioan Sucan
autogenerated on Wed Sep 16 2015 04:42:27