46 const moveit_msgs::MotionPlanRequest& req,
47 const robot_model::RobotModelConstPtr& kmodel)
const 49 const robot_model::JointModelGroup* jmg = kmodel->getJointModelGroup(group);
52 const std::pair<robot_model::JointModelGroup::KinematicsSolver, robot_model::JointModelGroup::KinematicsSolverMap>&
53 slv = jmg->getGroupKinematics();
57 ik = jmg->getVariableCount() == slv.first.bijection_.size();
58 else if (!slv.second.empty())
63 for (robot_model::JointModelGroup::KinematicsSolverMap::const_iterator jt = slv.second.begin();
64 jt != slv.second.end(); ++jt)
66 vc += jt->first->getVariableCount();
67 bc += jt->second.bijection_.size();
69 if (vc == jmg->getVariableCount() && vc == bc)
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())
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
PoseModelStateSpaceFactory()
same_shared_ptr< ModelBasedStateSpace, ompl::base::StateSpacePtr >::type ModelBasedStateSpacePtr
virtual ModelBasedStateSpacePtr allocStateSpace(const ModelBasedStateSpaceSpecification &space_spec) const