demo_construct_state_database.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
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 Willow Garage 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: Ioan Sucan */
36 
40 
41 #include <boost/math/constants/constants.hpp>
42 
43 static const std::string ROBOT_DESCRIPTION = "robot_description";
44 
45 moveit_msgs::Constraints getConstraints()
46 {
47  moveit_msgs::OrientationConstraint ocm;
48  ocm.link_name = "r_wrist_roll_link";
49  ocm.header.frame_id = "torso_lift_link";
50  ocm.orientation.x = 0;
51  ocm.orientation.y = 0;
52  ocm.orientation.z = 0;
53  ocm.orientation.w = 1.0;
54  ocm.absolute_x_axis_tolerance = 0.1;
55  ocm.absolute_y_axis_tolerance = 0.1;
56  ocm.absolute_z_axis_tolerance = boost::math::constants::pi<double>();
57  ocm.weight = 1.0;
58  moveit_msgs::Constraints cmsg;
59  cmsg.orientation_constraints.resize(1, ocm);
60  cmsg.name = ocm.link_name + ":upright";
61  return cmsg;
62 }
63 
64 void computeDB(const robot_model::RobotModelPtr& robot_model, unsigned int ns, unsigned int ne)
65 {
66  planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(robot_model));
68  moveit_msgs::Constraints c = getConstraints();
70  opt.state_space_parameterization = "PoseModel";
71  opt.samples = ns;
72  opt.edges_per_sample = ne;
73  opt.explicit_motions = true;
74  opt.max_edge_length = 0.2;
75  opt.explicit_points_resolution = 0.05;
76  opt.max_explicit_points = 10;
77 
78  ompl_interface.getConstraintsLibrary().addConstraintApproximation(c, "right_arm", ps, opt);
79  ompl_interface.getConstraintsLibrary().saveConstraintApproximations("~/constraints_approximation_database");
80  ROS_INFO("Done");
81 }
82 
83 /*
84 bool constructConstraintApproximation(moveit_msgs::ConstructConstraintApproximation::Request &req,
85 moveit_msgs::ConstructConstraintApproximation::Response &res)
86 {
87  planning_scene::PlanningScenePtr diff_scene = psm_.getPlanningScene()->diff();
88  robot_state::robotStateMsgToRobotState(*psm_.getPlanningScene()->getTransforms(), req.start_state,
89 diff_scene->getCurrentStateNonConst());
90  ompl_interface::ConstraintApproximationConstructionResults ca_res =
91  ompl_interface_.getConstraintsLibrary().addConstraintApproximation(req.constraint, req.group,
92 req.state_space_parameterization,
93  diff_scene, req.samples, req.edges_per_sample);
94  if (ca_res.approx)
95  {
96  res.sampling_success_rate = ca_res.sampling_success_rate;
97  res.state_sampling_time = ca_res.state_sampling_time;
98  res.state_connection_time = ca_res.state_connection_time;
99  res.filename = ca_res.approx->getFilename();
100  return ompl_interface_.saveConstraintApproximations();
101  }
102  else
103  return false;
104 }
105 */
106 
107 int main(int argc, char** argv)
108 {
109  ros::init(argc, argv, "construct_ompl_state_database", ros::init_options::AnonymousName);
110 
112  spinner.start();
113 
114  unsigned int nstates = 1000;
115  unsigned int nedges = 0;
116 
117  if (argc > 1)
118  try
119  {
120  nstates = boost::lexical_cast<unsigned int>(argv[1]);
121  }
122  catch (...)
123  {
124  }
125 
126  if (argc > 2)
127  try
128  {
129  nedges = boost::lexical_cast<unsigned int>(argv[2]);
130  }
131  catch (...)
132  {
133  }
134 
136  computeDB(rml.getModel(), nstates, nedges);
137 
138  ros::shutdown();
139  return 0;
140 }
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
The MoveIt interface to OMPL.
void spinner()
void saveConstraintApproximations(const std::string &path)
void computeDB(const robot_model::RobotModelPtr &robot_model, unsigned int ns, unsigned int ne)
ConstraintApproximationConstructionResults addConstraintApproximation(const moveit_msgs::Constraints &constr_sampling, const moveit_msgs::Constraints &constr_hard, const std::string &group, const planning_scene::PlanningSceneConstPtr &scene, const ConstraintApproximationConstructionOptions &options)
moveit_msgs::Constraints getConstraints()
#define ROS_INFO(...)
ConstraintsLibrary & getConstraintsLibrary()
static const std::string ROBOT_DESCRIPTION
ROSCPP_DECL void shutdown()
const robot_model::RobotModelPtr & getModel() const
int main(int argc, char **argv)


ompl
Author(s): Ioan Sucan
autogenerated on Thu May 24 2018 02:49:22