moveit2
The MoveIt Motion Planning Framework for ROS 2.
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 std::string& group, const moveit_msgs::msg::MotionPlanRequest& req,
47  const moveit::core::RobotModelConstPtr& robot_model) const
48 {
49  const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group);
50  if (jmg)
51  {
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 (const auto& jt : slv.second)
64  {
65  vc += jt.first->getVariableCount();
66  bc += jt.second.bijection_.size();
67  }
68  if (vc == jmg->getVariableCount() && vc == bc)
69  ik = true;
70  }
71 
72  if (ik)
73  {
74  // if we have path constraints, we prefer interpolating in pose space
75  if ((!req.path_constraints.position_constraints.empty() ||
76  !req.path_constraints.orientation_constraints.empty()) &&
77  req.path_constraints.joint_constraints.empty() && req.path_constraints.visibility_constraints.empty())
78  return 150;
79  else
80  return 50;
81  }
82  }
83  return -1;
84 }
85 
86 ompl_interface::ModelBasedStateSpacePtr
88 {
89  return std::make_shared<PoseModelStateSpace>(space_spec);
90 }
const std::pair< KinematicsSolver, KinematicsSolverMap > & getGroupKinematics() const
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
std::map< const JointModelGroup *, KinematicsSolver > KinematicsSolverMap
Map from group instances to allocator functions & bijections.
ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification &space_spec) const override
int canRepresentProblem(const std::string &group, const moveit_msgs::msg::MotionPlanRequest &req, const moveit::core::RobotModelConstPtr &robot_model) const override
Decide whether the type of state space constructed by this factory could represent problems specified...
static const std::string PARAMETERIZATION_TYPE
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest