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 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..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,6 +125,7 @@ namespace constrained_ik /** @brief Reset the planners IK solver configuration to it default settings */ void resetSolverConfiguration(); + private: /** * @brief Preform position and orientation interpolation between start and stop. @@ -137,6 +138,15 @@ 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 */ double orientational_discretization_step_; /**< Max orientational discretization step */ bool debug_mode_; /**< Debug state */ diff --git a/constrained_ik/src/constrained_ik.cpp b/constrained_ik/src/constrained_ik.cpp index e533e22..21abc0d 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; + ROS_WARN("Unable to find ros parameter: %s", parameter_name.c_str()); + 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() diff --git a/constrained_ik/src/moveit_interface/cartesian_planner.cpp b/constrained_ik/src/moveit_interface/cartesian_planner.cpp index 08e07a1..f1fe38d 100644 --- a/constrained_ik/src/moveit_interface/cartesian_planner.cpp +++ b/constrained_ik/src/moveit_interface/cartesian_planner.cpp @@ -44,9 +44,13 @@ 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. + default_constraints_ = solver_->loadConstraintsFromParamServer(constraint_param); } bool CartesianPlanner::initializeSolver() @@ -94,6 +98,253 @@ 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]; + } + + 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 + // 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; + + // 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; + + if (user_specifies_position) + { + best_position_constraint = mostRestrictive(position_constraints); + } + + if (user_specifies_orientation) + { + best_orientation_constraint = mostRestrictive(orientation_constraints); + } + + + std::vector > result; + + // Create a position constraint, if required + Constraint* user_pos_constraint = nullptr; + if (user_specifies_position) + { + 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) + { + user_ori_constraint = createConstraint(best_orientation_constraint); + result.push_back(std::make_pair(true, user_ori_constraint)); + } + + + // 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)); + } + + // 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]; + } + + 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; @@ -117,6 +368,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())