From 1e12b313a983bb282c9f15f0e810276e6b223ce4 Mon Sep 17 00:00:00 2001 From: Jonathan Meyer Date: Fri, 13 Oct 2017 10:11:09 -0500 Subject: [PATCH 1/5] Added a new loadConstraints function that returns a vector of Constraints and does not immediately install them --- constrained_ik/src/constrained_ik.cpp | 36 ++++++++++++++++++--------- 1 file changed, 24 insertions(+), 12 deletions(-) diff --git a/constrained_ik/src/constrained_ik.cpp b/constrained_ik/src/constrained_ik.cpp index e533e22..1d0d054 100644 --- a/constrained_ik/src/constrained_ik.cpp +++ b/constrained_ik/src/constrained_ik.cpp @@ -45,27 +45,43 @@ Constrained_IK::Constrained_IK(const ros::NodeHandle &nh) : nh_(nh) } void Constrained_IK::addConstraintsFromParamServer(const std::string ¶meter_name) +{ + std::vector > constraints = loadConstraintsFromParamServer(parameter_name); + + for (std::size_t i = 0; i < constraints.size(); ++i) + { + const bool is_primary = constraints[i].first; + Constraint* constraint = constraints[i].second; + + if (is_primary) + addConstraint(constraint, constraint_types::Primary); + else + addConstraint(constraint, constraint_types::Auxiliary); + } +} + +std::vector > Constrained_IK::loadConstraintsFromParamServer(const std::string& parameter_name) { XmlRpc::XmlRpcValue constraints_xml; boost::shared_ptr > constraint_loader; constraint_loader.reset(new pluginlib::ClassLoader("constrained_ik", "constrained_ik::Constraint")); + std::vector > constraints; + if (!nh_.getParam(parameter_name, constraints_xml)) { ROS_ERROR("Unable to find ros parameter: %s", parameter_name.c_str()); - ROS_BREAK(); - return; + return constraints; } if(constraints_xml.getType() != XmlRpc::XmlRpcValue::TypeArray) { ROS_ERROR("ROS parameter %s must be an array", parameter_name.c_str()); - ROS_BREAK(); - return; + return constraints; } - for (int i=0; icreateUnmanagedInstance(class_name); - constraint->loadParameters(constraint_xml); - if (is_primary) - addConstraint(constraint, constraint_types::Primary); - else - addConstraint(constraint, constraint_types::Auxiliary); - + constraints.push_back(std::make_pair(is_primary, constraint)); } catch (pluginlib::PluginlibException& ex) { ROS_ERROR("Couldn't load constraint named %s.\n Error: %s", class_name.c_str(), ex.what()); - ROS_BREAK(); } } else @@ -100,6 +110,8 @@ void Constrained_IK::addConstraintsFromParamServer(const std::string ¶meter_ ROS_ERROR("Constraint must have class(string) and primary(boolean) members"); } } + + return constraints; } void Constrained_IK::loadDefaultSolverConfiguration() From dbfe61855aa86019169688afab5b458eea8c0215 Mon Sep 17 00:00:00 2001 From: Jonathan Meyer Date: Fri, 13 Oct 2017 13:06:37 -0500 Subject: [PATCH 2/5] Fixup --- constrained_ik/include/constrained_ik/constrained_ik.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/constrained_ik/include/constrained_ik/constrained_ik.h b/constrained_ik/include/constrained_ik/constrained_ik.h index 1af3141..94a07c3 100644 --- a/constrained_ik/include/constrained_ik/constrained_ik.h +++ b/constrained_ik/include/constrained_ik/constrained_ik.h @@ -101,6 +101,8 @@ class Constrained_IK */ virtual void addConstraintsFromParamServer(const std::string ¶meter_name); + virtual std::vector > loadConstraintsFromParamServer(const std::string& parameter_name); + /** * @brief computes the inverse kinematics for the given pose of the tip link * @param goal cartesian pose to solve the inverse kinematics about From 65964d8a21fdbe8f4719e24506df39f3848f084b Mon Sep 17 00:00:00 2001 From: Jonathan Meyer Date: Fri, 13 Oct 2017 14:58:04 -0500 Subject: [PATCH 3/5] Initially working impl of loading a combination of default constraints from ROS params and loading extra constraints via user request constraints. --- .../include/constrained_ik/constraint.h | 9 + .../constraints/goal_orientation.h | 5 + .../constrained_ik/constraints/goal_pose.h | 6 + .../constraints/goal_position.h | 5 + .../constraints/goal_tool_pointing.h | 5 + .../moveit_interface/cartesian_planner.h | 4 + .../moveit_interface/cartesian_planner.cpp | 159 +++++++++++++++++- 7 files changed, 192 insertions(+), 1 deletion(-) diff --git a/constrained_ik/include/constrained_ik/constraint.h b/constrained_ik/include/constrained_ik/constraint.h index de211d4..abf436b 100644 --- a/constrained_ik/include/constrained_ik/constraint.h +++ b/constrained_ik/include/constrained_ik/constraint.h @@ -50,6 +50,15 @@ class Constraint public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + static const unsigned TYPE_POSITION = 1u; + static const unsigned TYPE_ORIENTATION = 2u; + static const unsigned TYPE_OTHER = 4u; + + virtual unsigned constraintType() const + { + return TYPE_OTHER; + } + /** * @brief This structure is to be used by all constraints to store specific data * that needs to get updated every iteration of the solver. diff --git a/constrained_ik/include/constrained_ik/constraints/goal_orientation.h b/constrained_ik/include/constrained_ik/constraints/goal_orientation.h index 3cc2528..443ae68 100644 --- a/constrained_ik/include/constrained_ik/constraints/goal_orientation.h +++ b/constrained_ik/include/constrained_ik/constraints/goal_orientation.h @@ -56,6 +56,11 @@ class GoalOrientation : public Constraint GoalOrientation(); + virtual unsigned constraintType() const + { + return TYPE_ORIENTATION; + } + /** @brief see base class for documentation*/ constrained_ik::ConstraintResults evalConstraint(const SolverState &state) const override; diff --git a/constrained_ik/include/constrained_ik/constraints/goal_pose.h b/constrained_ik/include/constrained_ik/constraints/goal_pose.h index 1e20883..8ea1667 100644 --- a/constrained_ik/include/constrained_ik/constraints/goal_pose.h +++ b/constrained_ik/include/constrained_ik/constraints/goal_pose.h @@ -50,6 +50,12 @@ class GoalPose : public ConstraintGroup this->add(position_); this->add(orientation_); } + + virtual unsigned constraintType() const + { + return position_->constraintType() | orientation_->constraintType(); + } + /** * @brief Setter for the orientation weights * @param weight_orientation values to set the orientation weights diff --git a/constrained_ik/include/constrained_ik/constraints/goal_position.h b/constrained_ik/include/constrained_ik/constraints/goal_position.h index 6eb9f47..fa64450 100644 --- a/constrained_ik/include/constrained_ik/constraints/goal_position.h +++ b/constrained_ik/include/constrained_ik/constraints/goal_position.h @@ -57,6 +57,11 @@ class GoalPosition : public Constraint GoalPosition(); + virtual unsigned constraintType() const + { + return TYPE_POSITION; + } + /** @brief see base class for documentation*/ constrained_ik::ConstraintResults evalConstraint(const SolverState &state) const override; diff --git a/constrained_ik/include/constrained_ik/constraints/goal_tool_pointing.h b/constrained_ik/include/constrained_ik/constraints/goal_tool_pointing.h index 2ea2c4a..4f915da 100644 --- a/constrained_ik/include/constrained_ik/constraints/goal_tool_pointing.h +++ b/constrained_ik/include/constrained_ik/constraints/goal_tool_pointing.h @@ -57,6 +57,11 @@ class GoalToolPointing : public Constraint GoalToolPointing(); + virtual unsigned constraintType() const + { + return TYPE_POSITION | TYPE_ORIENTATION; + } + /** @brief see base class for documentation*/ constrained_ik::ConstraintResults evalConstraint(const SolverState &state) const override; diff --git a/constrained_ik/include/constrained_ik/moveit_interface/cartesian_planner.h b/constrained_ik/include/constrained_ik/moveit_interface/cartesian_planner.h index c4db2fa..1220e1f 100644 --- a/constrained_ik/include/constrained_ik/moveit_interface/cartesian_planner.h +++ b/constrained_ik/include/constrained_ik/moveit_interface/cartesian_planner.h @@ -125,6 +125,8 @@ namespace constrained_ik /** @brief Reset the planners IK solver configuration to it default settings */ void resetSolverConfiguration(); + std::vector > resolveConstraints() const; + private: /** * @brief Preform position and orientation interpolation between start and stop. @@ -137,6 +139,8 @@ namespace constrained_ik std::vector > interpolateCartesian(const Eigen::Affine3d& start, const Eigen::Affine3d& stop, double ds, double dt) const; + std::vector > default_constraints_; /**< These parameters are loaded from YAML files and may be overridden by + constraints specified through planning requests. */ double translational_discretization_step_; /**< Max translational discretization step */ double orientational_discretization_step_; /**< Max orientational discretization step */ bool debug_mode_; /**< Debug state */ diff --git a/constrained_ik/src/moveit_interface/cartesian_planner.cpp b/constrained_ik/src/moveit_interface/cartesian_planner.cpp index 08e07a1..18df9e7 100644 --- a/constrained_ik/src/moveit_interface/cartesian_planner.cpp +++ b/constrained_ik/src/moveit_interface/cartesian_planner.cpp @@ -44,9 +44,17 @@ namespace constrained_ik robot_description_("robot_description") { resetPlannerConfiguration(); + + // TODO: Defer construction of the constrained_ik solver until solve is called solver_.reset(new Constrained_IK(nh)); std::string constraint_param = "constrained_ik_solver/" + getGroupName() + "/constraints"; - solver_->addConstraintsFromParamServer(constraint_param); + // TODO: We want to load our default set of cost functions here and store them for use + // when solve() is called. + ROS_INFO("Load from params"); +// solver_->addConstraintsFromParamServer(constraint_param); + + ROS_INFO("Load into defaults"); + default_constraints_ = solver_->loadConstraintsFromParamServer(constraint_param); } bool CartesianPlanner::initializeSolver() @@ -94,6 +102,144 @@ namespace constrained_ik solver_->loadDefaultSolverConfiguration(); } + std::vector > CartesianPlanner::resolveConstraints() const + { + // This function attempts to sanely combine goal constraints given by the user with constraints + // specified as "default" through the parameter server / YAML file interfaces. + + // This planner currently only uses goal constraints and ignores path constraints: the path is specified implictly. + // With high DOF, we could optimize across several goals but our primary purpose here is low DOF industrial bots. + + // User requested goal constraints take precedence over default ones + + // Warn if user specified any path constraints + if (request_.path_constraints.joint_constraints.size() > 0 || + request_.path_constraints.position_constraints.size() > 0 || + request_.path_constraints.orientation_constraints.size() > 0 || + request_.path_constraints.visibility_constraints.size() > 0) + { + ROS_WARN("Cartesian Planner does not (currently) support path constraints."); + } + + // First, let's see if the user specifies position + std::vector position_constraints; + for (std::size_t i = 0; i < request_.goal_constraints.size(); ++i) + { + if (request_.goal_constraints[i].position_constraints.size() > 0) + { + position_constraints.insert(position_constraints.end(), + request_.goal_constraints[i].position_constraints.begin(), + request_.goal_constraints[i].position_constraints.end()); + } + } + const bool user_specifies_position = position_constraints.size() > 0; + + // Next lets look at orientation + std::vector orientation_constraints; + for (std::size_t i = 0; i < request_.goal_constraints.size(); ++i) + { + if (request_.goal_constraints[i].orientation_constraints.size() > 0) + { + orientation_constraints.insert(orientation_constraints.end(), + request_.goal_constraints[i].orientation_constraints.begin(), + request_.goal_constraints[i].orientation_constraints.end()); + } + } + const bool user_specifies_orientation = orientation_constraints.size() > 0; + + // If the user has specified position or orientation, we have to choose the best one... + moveit_msgs::PositionConstraint best_position_constraint; + moveit_msgs::OrientationConstraint best_orientation_constraint; + + if (user_specifies_position) + { + ROS_WARN_STREAM("User specifies position123"); + } + + if (user_specifies_orientation) + { + ROS_WARN_STREAM("User specifies orientation"); + } + + Constraint* user_pos_constraint = nullptr; + Constraint* user_ori_constraint = nullptr; + + std::vector > result; + + // Create a position constraint, if required + if (user_specifies_position) + { + boost::shared_ptr > constraint_loader; + constraint_loader.reset( + new pluginlib::ClassLoader("constrained_ik", "constrained_ik::Constraint")); + + user_pos_constraint = constraint_loader->createUnmanagedInstance("constrained_ik/GoalPosition"); + + +// user_pos_constraint = new constrained_ik::constraints::GoalPosition(); + XmlRpc::XmlRpcValue params; + params["position_tolerance"] = 0.001; // TODO: comes from user + params["weights"].setSize(3); + params["weights"][0] = 1.0; + params["weights"][1] = 1.0; + params["weights"][2] = 1.0; + params["debug"] = XmlRpc::XmlRpcValue(false); + user_pos_constraint->loadParameters(params); + result.push_back(std::make_pair(true, user_pos_constraint)); + } + + // Create a orientation constraint, if required + if (user_specifies_orientation) + { + boost::shared_ptr > constraint_loader; + constraint_loader.reset( + new pluginlib::ClassLoader("constrained_ik", "constrained_ik::Constraint")); + + user_ori_constraint = constraint_loader->createUnmanagedInstance("constrained_ik/GoalToolOrientation"); + + XmlRpc::XmlRpcValue params; + params["orientation_tolerance"] = 0.001; // TODO: comes from user + params["weights"].setSize(3); + params["weights"][0] = 1.0; + params["weights"][1] = 1.0; + params["weights"][2] = 1.0; + params["debug"] = XmlRpc::XmlRpcValue(false); + ROS_INFO("WHAT"); + user_ori_constraint->loadParameters(params); + result.push_back(std::make_pair(true, user_ori_constraint)); + + } + + // Add user constraints to the results as required + +// Now we need to remove constraints from the default set as necessary + std::vector to_delete; + for (std::size_t i = 0; i < default_constraints_.size(); ++i) + { + Constraint* c = default_constraints_[i].second; + if ( (c->constraintType() & Constraint::TYPE_POSITION) && user_specifies_position) + { + to_delete.push_back(c); + continue; + } + + if ( (c->constraintType() & Constraint::TYPE_ORIENTATION) && user_specifies_orientation) + { + to_delete.push_back(c); + continue; + } + + result.push_back(std::make_pair(default_constraints_[i].first, c)); + } + + for (std::size_t i = 0; i < to_delete.size(); ++i) + { + delete to_delete[i]; + } + + return result; + } + bool CartesianPlanner::solve(planning_interface::MotionPlanDetailedResponse &res) { planning_interface::MotionPlanResponse response; @@ -117,6 +263,17 @@ namespace constrained_ik robot_model_ = planning_scene_->getRobotModel(); + // TODO: Create our solver here - we want to generate an intelligent union of the constraints + // specified in the parameter file and the constraints specified in the moveit request + std::vector > solver_constraints = resolveConstraints(); + for (std::size_t i = 0; i < solver_constraints.size(); ++i) + { + if (solver_constraints[i].first) + solver_->addConstraint(solver_constraints[i].second, constraint_types::Primary); + else + solver_->addConstraint(solver_constraints[i].second, constraint_types::Auxiliary); + } + // Load solver if not already loaded if(!solver_->isInitialized()) if (!initializeSolver()) From 4af038506f8c80d550da318f014815ccf8ca6e55 Mon Sep 17 00:00:00 2001 From: Jonathan Meyer Date: Fri, 13 Oct 2017 16:29:53 -0500 Subject: [PATCH 4/5] fixup --- .../moveit_interface/cartesian_planner.h | 8 +- .../moveit_interface/cartesian_planner.cpp | 161 +++++++++++++----- 2 files changed, 130 insertions(+), 39 deletions(-) diff --git a/constrained_ik/include/constrained_ik/moveit_interface/cartesian_planner.h b/constrained_ik/include/constrained_ik/moveit_interface/cartesian_planner.h index 1220e1f..5d4b862 100644 --- a/constrained_ik/include/constrained_ik/moveit_interface/cartesian_planner.h +++ b/constrained_ik/include/constrained_ik/moveit_interface/cartesian_planner.h @@ -125,7 +125,6 @@ namespace constrained_ik /** @brief Reset the planners IK solver configuration to it default settings */ void resetSolverConfiguration(); - std::vector > resolveConstraints() const; private: /** @@ -139,6 +138,13 @@ namespace constrained_ik std::vector > interpolateCartesian(const Eigen::Affine3d& start, const Eigen::Affine3d& stop, double ds, double dt) const; + // Constraint additions + std::vector > resolveConstraints() const; + + Constraint* createConstraint(const moveit_msgs::PositionConstraint& pos_constraint) const; + + Constraint* createConstraint(const moveit_msgs::OrientationConstraint& orient_constraint) const; + std::vector > default_constraints_; /**< These parameters are loaded from YAML files and may be overridden by constraints specified through planning requests. */ double translational_discretization_step_; /**< Max translational discretization step */ diff --git a/constrained_ik/src/moveit_interface/cartesian_planner.cpp b/constrained_ik/src/moveit_interface/cartesian_planner.cpp index 18df9e7..72fd184 100644 --- a/constrained_ik/src/moveit_interface/cartesian_planner.cpp +++ b/constrained_ik/src/moveit_interface/cartesian_planner.cpp @@ -102,6 +102,64 @@ namespace constrained_ik solver_->loadDefaultSolverConfiguration(); } + + static double constraintTolerance(const moveit_msgs::PositionConstraint& pos_constraint) + { + double tolerance = std::numeric_limits::max(); + for (std::size_t i = 0; i < pos_constraint.constraint_region.primitives.size(); ++i) + { + const shape_msgs::SolidPrimitive& p = pos_constraint.constraint_region.primitives[i]; + + double this_tolerance = std::numeric_limits::max(); + if (p.type == shape_msgs::SolidPrimitive::BOX) + { + if (p.dimensions.size() != 3) continue; + this_tolerance = std::min(std::min(p.dimensions[0], p.dimensions[1]), p.dimensions[2]) / 2.0; + } + + if (p.type == shape_msgs::SolidPrimitive::SPHERE) + { + if (p.dimensions.size() != 1) continue; + this_tolerance = p.dimensions[0]; + } + + if (this_tolerance != std::numeric_limits::max()) + { + tolerance = std::min(tolerance, this_tolerance); + } + } + return tolerance; + } + + static double constraintTolerance(const moveit_msgs::OrientationConstraint& ori_constraint) + { + double tolerance = std::min(std::min(ori_constraint.absolute_x_axis_tolerance, + ori_constraint.absolute_y_axis_tolerance), + ori_constraint.absolute_z_axis_tolerance); + return tolerance; + } + + template + static Constraint mostRestrictive(const std::vector& cs) + { + assert(cs.size() > 0); + + double min_tolerance = std::numeric_limits::max(); + unsigned min_index = 0; + + for (unsigned i = 0; i < cs.size(); ++i) + { + double this_tol = constraintTolerance(cs[i]); + if (this_tol < min_tolerance) + { + min_tolerance = min_tolerance; + min_index = i; + } + } + + return cs[min_index]; + } + std::vector > CartesianPlanner::resolveConstraints() const { // This function attempts to sanely combine goal constraints given by the user with constraints @@ -153,66 +211,35 @@ namespace constrained_ik if (user_specifies_position) { - ROS_WARN_STREAM("User specifies position123"); + best_position_constraint = mostRestrictive(position_constraints); } if (user_specifies_orientation) { - ROS_WARN_STREAM("User specifies orientation"); + best_orientation_constraint = mostRestrictive(orientation_constraints); } - Constraint* user_pos_constraint = nullptr; - Constraint* user_ori_constraint = nullptr; std::vector > result; // Create a position constraint, if required + Constraint* user_pos_constraint = nullptr; if (user_specifies_position) { - boost::shared_ptr > constraint_loader; - constraint_loader.reset( - new pluginlib::ClassLoader("constrained_ik", "constrained_ik::Constraint")); - - user_pos_constraint = constraint_loader->createUnmanagedInstance("constrained_ik/GoalPosition"); - - -// user_pos_constraint = new constrained_ik::constraints::GoalPosition(); - XmlRpc::XmlRpcValue params; - params["position_tolerance"] = 0.001; // TODO: comes from user - params["weights"].setSize(3); - params["weights"][0] = 1.0; - params["weights"][1] = 1.0; - params["weights"][2] = 1.0; - params["debug"] = XmlRpc::XmlRpcValue(false); - user_pos_constraint->loadParameters(params); + user_pos_constraint = createConstraint(best_position_constraint); result.push_back(std::make_pair(true, user_pos_constraint)); } // Create a orientation constraint, if required + Constraint* user_ori_constraint = nullptr; if (user_specifies_orientation) { - boost::shared_ptr > constraint_loader; - constraint_loader.reset( - new pluginlib::ClassLoader("constrained_ik", "constrained_ik::Constraint")); - - user_ori_constraint = constraint_loader->createUnmanagedInstance("constrained_ik/GoalToolOrientation"); - - XmlRpc::XmlRpcValue params; - params["orientation_tolerance"] = 0.001; // TODO: comes from user - params["weights"].setSize(3); - params["weights"][0] = 1.0; - params["weights"][1] = 1.0; - params["weights"][2] = 1.0; - params["debug"] = XmlRpc::XmlRpcValue(false); - ROS_INFO("WHAT"); - user_ori_constraint->loadParameters(params); + user_ori_constraint = createConstraint(best_orientation_constraint); result.push_back(std::make_pair(true, user_ori_constraint)); - } - // Add user constraints to the results as required -// Now we need to remove constraints from the default set as necessary + // Now we need to remove constraints from the default set as necessary std::vector to_delete; for (std::size_t i = 0; i < default_constraints_.size(); ++i) { @@ -232,6 +259,7 @@ namespace constrained_ik result.push_back(std::make_pair(default_constraints_[i].first, c)); } + // Cleanup constraints that were loaded but not used - they will not be de-allocated for (std::size_t i = 0; i < to_delete.size(); ++i) { delete to_delete[i]; @@ -240,6 +268,63 @@ namespace constrained_ik return result; } + Constraint* CartesianPlanner::createConstraint(const moveit_msgs::PositionConstraint& pos_constraint) const + { + Constraint* user_pos_constraint = nullptr; + + boost::shared_ptr > constraint_loader; + constraint_loader.reset( + new pluginlib::ClassLoader("constrained_ik", "constrained_ik::Constraint")); + + user_pos_constraint = constraint_loader->createUnmanagedInstance("constrained_ik/GoalPosition"); + + XmlRpc::XmlRpcValue params; + + double tolerance = constraintTolerance(pos_constraint); + if (tolerance == std::numeric_limits::max() || tolerance == 0.0) + tolerance = 0.001; + + double weight = pos_constraint.weight != 0.0 ? pos_constraint.weight : 1.0; + + params["position_tolerance"] = tolerance; + params["weights"].setSize(3); + params["weights"][0] = weight; + params["weights"][1] = weight; + params["weights"][2] = weight; + params["debug"] = XmlRpc::XmlRpcValue(false); + user_pos_constraint->loadParameters(params); + + return user_pos_constraint; + } + + Constraint* CartesianPlanner::createConstraint(const moveit_msgs::OrientationConstraint& orient_constraint) const + { + Constraint* user_ori_constraint = nullptr; + + boost::shared_ptr > constraint_loader; + constraint_loader.reset( + new pluginlib::ClassLoader("constrained_ik", "constrained_ik::Constraint")); + + user_ori_constraint = constraint_loader->createUnmanagedInstance("constrained_ik/GoalToolOrientation"); + + double tolerance = constraintTolerance(orient_constraint); + tolerance = std::max(tolerance, 0.0001); + + // Determine weight + double weight = orient_constraint.weight != 0.0 ? orient_constraint.weight : 1.0; + + XmlRpc::XmlRpcValue params; + params["orientation_tolerance"] = tolerance; // TODO: comes from user + params["weights"].setSize(3); + params["weights"][0] = weight; + params["weights"][1] = weight; + params["weights"][2] = weight; + params["debug"] = XmlRpc::XmlRpcValue(false); + user_ori_constraint->loadParameters(params); + + return user_ori_constraint; + } + bool CartesianPlanner::solve(planning_interface::MotionPlanDetailedResponse &res) { planning_interface::MotionPlanResponse response; From 9076d7fe0831d5e3644675e7c18386c6974aa2e3 Mon Sep 17 00:00:00 2001 From: Jonathan Meyer Date: Fri, 13 Oct 2017 17:07:11 -0500 Subject: [PATCH 5/5] Added a check to handle the case where no pose constraints are placed in the goal and no pose constraints are loaded via defaults. --- constrained_ik/src/constrained_ik.cpp | 2 +- .../moveit_interface/cartesian_planner.cpp | 28 ++++++++++++++++--- 2 files changed, 25 insertions(+), 5 deletions(-) diff --git a/constrained_ik/src/constrained_ik.cpp b/constrained_ik/src/constrained_ik.cpp index 1d0d054..21abc0d 100644 --- a/constrained_ik/src/constrained_ik.cpp +++ b/constrained_ik/src/constrained_ik.cpp @@ -71,7 +71,7 @@ std::vector > Constrained_IK::loadConstraintsFromPa if (!nh_.getParam(parameter_name, constraints_xml)) { - ROS_ERROR("Unable to find ros parameter: %s", parameter_name.c_str()); + ROS_WARN("Unable to find ros parameter: %s", parameter_name.c_str()); return constraints; } diff --git a/constrained_ik/src/moveit_interface/cartesian_planner.cpp b/constrained_ik/src/moveit_interface/cartesian_planner.cpp index 72fd184..f1fe38d 100644 --- a/constrained_ik/src/moveit_interface/cartesian_planner.cpp +++ b/constrained_ik/src/moveit_interface/cartesian_planner.cpp @@ -50,10 +50,6 @@ namespace constrained_ik std::string constraint_param = "constrained_ik_solver/" + getGroupName() + "/constraints"; // TODO: We want to load our default set of cost functions here and store them for use // when solve() is called. - ROS_INFO("Load from params"); -// solver_->addConstraintsFromParamServer(constraint_param); - - ROS_INFO("Load into defaults"); default_constraints_ = solver_->loadConstraintsFromParamServer(constraint_param); } @@ -160,6 +156,18 @@ namespace constrained_ik return cs[min_index]; } + static bool constraintsPose(const std::vector >& cs) + { + for (std::size_t i = 0; i < cs.size(); ++i) + { + bool this_constrains = (cs[i].second->constraintType() & constrained_ik::Constraint::TYPE_POSITION) || + (cs[i].second->constraintType() & constrained_ik::Constraint::TYPE_ORIENTATION); + if (this_constrains) + return true; + } + return false; + } + std::vector > CartesianPlanner::resolveConstraints() const { // This function attempts to sanely combine goal constraints given by the user with constraints @@ -205,6 +213,18 @@ namespace constrained_ik } const bool user_specifies_orientation = orientation_constraints.size() > 0; + // Check for no default constraints and no position/orientation goal constraints + if (!user_specifies_orientation && !user_specifies_position && !constraintsPose(default_constraints_)) + { + ROS_WARN_STREAM("No pose goal constraints or default constraints on pose. Generating a default set."); + moveit_msgs::PositionConstraint dummy_pos; + moveit_msgs::OrientationConstraint dummy_orient; + std::vector > res; + res.push_back(std::make_pair(true, createConstraint(dummy_pos))); + res.push_back(std::make_pair(true, createConstraint(dummy_orient))); + return res; + } + // If the user has specified position or orientation, we have to choose the best one... moveit_msgs::PositionConstraint best_position_constraint; moveit_msgs::OrientationConstraint best_orientation_constraint;