diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 26c4cfae..9728ebaa 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -40,6 +40,7 @@ #include #include +#include namespace moveit { namespace task_constructor { @@ -69,6 +70,14 @@ public: class TrajectoryCostTerm : public CostTerm { public: + enum class Mode + { + AUTO /* TRAJECTORY, or START_INTERFACE if no trajectory is given */, + START_INTERFACE, + END_INTERFACE, + TRAJECTORY + }; + double operator()(const SolutionSequence& s, std::string& comment) const override; double operator()(const WrappedSolution& s, std::string& comment) const override; }; @@ -128,6 +137,21 @@ public: std::map joints; //< joint weights }; +/// (weighted) joint-space distance to reference pose +class DistanceToReference : public TrajectoryCostTerm +{ +public: + DistanceToReference(const moveit_msgs::RobotState& ref, Mode m = Mode::AUTO, + std::map w = std::map()); + DistanceToReference(const std::map& ref, Mode m = Mode::AUTO, + std::map w = std::map()); + double operator()(const SubTrajectory& s, std::string& comment) const override; + + moveit_msgs::RobotState reference; + std::map weights; + Mode mode; +}; + /// execution duration of the whole trajectory class TrajectoryDuration : public TrajectoryCostTerm { @@ -157,14 +181,6 @@ public: class Clearance : public TrajectoryCostTerm { public: - enum class Mode - { - AUTO /* TRAJECTORY, or START_INTERFACE if no trajectory is given */, - START_INTERFACE, - END_INTERFACE, - TRAJECTORY - }; - Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group", Mode mode = Mode::AUTO); bool with_world; diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 0276430e..157d143a 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include @@ -140,6 +141,57 @@ double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/) return path_length; } +DistanceToReference::DistanceToReference(const moveit_msgs::RobotState& ref, Mode m, std::map w) + : reference(ref), weights(std::move(w)), mode(m) {} +DistanceToReference::DistanceToReference(const std::map& ref, Mode m, + std::map w) + : weights(std::move(w)), mode(m) { + reference.joint_state.name.reserve(ref.size()); + reference.joint_state.position.reserve(ref.size()); + + for (auto& item : ref) { + reference.joint_state.name.push_back(item.first); + reference.joint_state.position.push_back(item.second); + } + reference.is_diff = true; +} + +double DistanceToReference::operator()(const SubTrajectory& s, std::string& /*comment*/) const { + const auto& state = (mode == Mode::END_INTERFACE) ? s.end() : s.start(); + const auto& traj = s.trajectory(); + + moveit::core::RobotState ref_state = state->scene()->getCurrentState(); + moveit::core::robotStateMsgToRobotState(reference, ref_state, false); + + std::map w; + for (auto& item : weights) { + const robot_model::JointModel* jm = ref_state.getJointModel(item.first); + if (jm) + w.emplace(jm, item.second); + } + + auto distance = [this, &ref_state, &w](const moveit::core::RobotState& state) { + if (weights.empty()) { + return ref_state.distance(state); + } else { + double accumulated = 0.0; + for (const auto& item : w) + accumulated += item.second * ref_state.distance(state, item.first); + return accumulated; + } + }; + + if (mode == Mode::START_INTERFACE || mode == Mode::END_INTERFACE || (mode == Mode::AUTO && (traj == nullptr))) { + return distance(state->scene()->getCurrentState()); + } else { + double accumulated = 0.0; + for (size_t i = 0; i < traj->getWayPointCount(); ++i) + accumulated += distance(traj->getWayPoint(i)); + accumulated /= traj->getWayPointCount(); + return accumulated; + } +} + double TrajectoryDuration::operator()(const SubTrajectory& s, std::string& /*comment*/) const { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; }