pose_model_state_space_factory.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, 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 
39 
41 {
43 }
44 
46  const moveit_msgs::MotionPlanRequest& req,
47  const robot_model::RobotModelConstPtr& kmodel) const
48 {
49  const robot_model::JointModelGroup* jmg = kmodel->getJointModelGroup(group);
50  if (jmg)
51  {
52  const std::pair<robot_model::JointModelGroup::KinematicsSolver, robot_model::JointModelGroup::KinematicsSolverMap>&
53  slv = jmg->getGroupKinematics();
54  bool ik = false;
55  // check that we have a direct means to compute IK
56  if (slv.first)
57  ik = jmg->getVariableCount() == slv.first.bijection_.size();
58  else if (!slv.second.empty())
59  {
60  // or an IK solver for each of the subgroups
61  unsigned int vc = 0;
62  unsigned int bc = 0;
63  for (robot_model::JointModelGroup::KinematicsSolverMap::const_iterator jt = slv.second.begin();
64  jt != slv.second.end(); ++jt)
65  {
66  vc += jt->first->getVariableCount();
67  bc += jt->second.bijection_.size();
68  }
69  if (vc == jmg->getVariableCount() && vc == bc)
70  ik = true;
71  }
72 
73  if (ik)
74  {
75  // if we have path constraints, we prefer interpolating in pose space
76  if ((!req.path_constraints.position_constraints.empty() ||
77  !req.path_constraints.orientation_constraints.empty()) &&
78  req.path_constraints.joint_constraints.empty() && req.path_constraints.visibility_constraints.empty())
79  return 150;
80  else
81  return 50;
82  }
83  }
84  return -1;
85 }
86 
89 {
90  return ModelBasedStateSpacePtr(new PoseModelStateSpace(space_spec));
91 }
virtual int canRepresentProblem(const std::string &group, const moveit_msgs::MotionPlanRequest &req, const robot_model::RobotModelConstPtr &robot_model) const
Decide whether the type of state space constructed by this factory could represent problems specified...
static const std::string PARAMETERIZATION_TYPE
same_shared_ptr< ModelBasedStateSpace, ompl::base::StateSpacePtr >::type ModelBasedStateSpacePtr
virtual ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification &space_spec) const


ompl
Author(s): Ioan Sucan
autogenerated on Mon Jan 15 2018 03:51:38