diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index dbbc368e0..9728ebaa9 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 @@ class CostTerm 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; }; @@ -113,15 +122,34 @@ class Constant : public CostTerm double cost; }; -/// trajectory length (interpolated between waypoints) +/// trajectory length with optional weighting for different joints class PathLength : public TrajectoryCostTerm { public: + /// By default, all joints are considered with same weight of 1.0 PathLength() = default; - PathLength(std::vector j) : joints{ std::move(j) } {}; + /// Limit measurements to given joint names + PathLength(std::vector joints); + /// Limit measurements to given joints and use given weighting + PathLength(std::map j) : joints(std::move(j)) {} double operator()(const SubTrajectory& s, std::string& comment) const override; - std::vector joints; + 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 @@ -153,14 +181,6 @@ class LinkMotion : public TrajectoryCostTerm 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/include/moveit/task_constructor/task.h b/core/include/moveit/task_constructor/task.h index f8e2c5dc1..54145abb6 100644 --- a/core/include/moveit/task_constructor/task.h +++ b/core/include/moveit/task_constructor/task.h @@ -73,6 +73,7 @@ class Task : protected WrapperBase { public: PRIVATE_CLASS(Task) + using WrapperBase::setCostTerm; Task(const std::string& ns = "", bool introspection = true, ContainerBase::pointer&& container = std::make_unique("task pipeline")); diff --git a/core/python/CMakeLists.txt b/core/python/CMakeLists.txt index 61c81cd79..cf877722f 100644 --- a/core/python/CMakeLists.txt +++ b/core/python/CMakeLists.txt @@ -1,10 +1,11 @@ -# As we rely on some not yet released pybind11 PRs, we are employing -# our own fork of it, imported via git submodule. -# See https://github.com/pybind/pybind11/pull/2687. +# We rely on pybind11's smart_holder branch imported pybind11 via git submodule # pybind11 must use the ROS python version set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION_STRING}) +# Use minimum-size optimization for pybind11 bindings +add_compile_options("-Os") + # create symlink to grant access to downstream packages in devel space add_custom_target(pybind11_devel_symlink ALL COMMAND ${CMAKE_COMMAND} -E create_symlink ${CMAKE_CURRENT_SOURCE_DIR}/pybind11 diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index 2f4fdff43..3f3cc00f0 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -33,6 +33,8 @@ *********************************************************************/ #include "core.h" +#include +#include #include #include #include @@ -157,6 +159,36 @@ void export_core(pybind11::module& m) { .def(PYBIND11_BOOL_ATTR, [](const moveit::core::MoveItErrorCode& err) { return pybind11::cast(static_cast(err)); }); + py::classh(m, "CostTerm", "Base class for cost calculation in stages"); + auto tct = py::classh(m, "TrajectoryCostTerm", + "Base class for cost calculation of trajectories"); + py::enum_(tct, "Mode", "Specify which states are considered for collision checking") + .value("AUTO", TrajectoryCostTerm::Mode::AUTO, "TRAJECTORY (if available) or START_INTERFACE") + .value("START_INTERFACE", TrajectoryCostTerm::Mode::START_INTERFACE, "Only consider start state") + .value("END_INTERFACE", TrajectoryCostTerm::Mode::END_INTERFACE, "Only consider end state") + .value("TRAJECTORY", TrajectoryCostTerm::Mode::TRAJECTORY, "Consider whole trajectory"); + + py::classh(m, "PathLength", + "Computes joint-based path length along trajectory") + .def(py::init<>()) + .def(py::init>()) + .def(py::init>()); + py::classh(m, "DistanceToReference", + "Computes joint-based distance to reference pose") + .def(py::init>(), + "reference"_a, "mode"_a = TrajectoryCostTerm::Mode::AUTO, "weights"_a = std::map()) + .def(py::init&, TrajectoryCostTerm::Mode, std::map>(), + "reference"_a, "mode"_a = TrajectoryCostTerm::Mode::AUTO, "weights"_a = std::map()); + py::classh(m, "TrajectoryDuration", "Computes duration of trajectory") + .def(py::init<>()); + py::classh(m, "LinkMotion", + "Computes Cartesian path length of given link along trajectory") + .def(py::init(), "link_name"_a); + + py::classh(m, "Clearance", "Computes inverse distance to collision objects") + .def(py::init(), "with_world"_a = true, + "cumulative"_a = false, "group_property"_a = "group", "mode"_a = TrajectoryCostTerm::Mode::AUTO); + auto stage = properties::class_>(m, "Stage", "Abstract base class of all stages.") .property("timeout", "float: Maximally allowed time [s] per computation step") @@ -168,6 +200,15 @@ void export_core(pybind11::module& m) { "PropertyMap: PropertyMap of the stage (read-only)") .def_property_readonly("solutions", &Stage::solutions, "Successful Solutions of the stage (read-only)") .def_property_readonly("failures", &Stage::failures, "Solutions: Failed Solutions of the stage (read-only)") + .def("setCostTerm", &Stage::setCostTerm, + "Specify a CostTerm for calculation of stage costs") + .def( + "setCostTerm", [](Stage& self, const LambdaCostTerm::SubTrajectorySignature& f) { self.setCostTerm(f); }, + "Specify a function to calculate trajectory costs") + .def( + "setCostTerm", + [](Stage& self, const LambdaCostTerm::SubTrajectoryShortSignature& f) { self.setCostTerm(f); }, + "Specify a function to calculate trajectory costs") .def("reset", &Stage::reset, "Reset the Stage. Clears all solutions, interfaces and inherited properties") .def("init", &Stage::init, "Initialize the stage once before planning. " @@ -407,6 +448,15 @@ void export_core(pybind11::module& m) { return py::make_iterator(children.begin(), children.end()); }, py::keep_alive<0, 1>()) // keep container alive as long as iterator lives + .def( + "setCostTerm", [](Task& self, const CostTermConstPtr& c) { self.setCostTerm(c); }, + "Specify a CostTerm for calculation of stage costs") + .def( + "setCostTerm", [](Task& self, const LambdaCostTerm::SubTrajectorySignature& f) { self.setCostTerm(f); }, + "Specify a function to calculate trajectory costs") + .def( + "setCostTerm", [](Task& self, const LambdaCostTerm::SubTrajectoryShortSignature& f) { self.setCostTerm(f); }, + "Specify a function to calculate trajectory costs") .def("reset", &Task::reset, "Reset task (and all its stages)") .def("init", py::overload_cast<>(&Task::init), "Initialize the task (and all its stages)") .def("plan", &Task::plan, "max_solutions"_a = 0, R"( diff --git a/core/python/bindings/src/core.h b/core/python/bindings/src/core.h index 175c92bf4..f69e8bd2e 100644 --- a/core/python/bindings/src/core.h +++ b/core/python/bindings/src/core.h @@ -37,6 +37,7 @@ #include #include #include +#include #include #include @@ -119,6 +120,14 @@ PYBIND11_SMART_HOLDER_TYPE_CASTERS(ordered #include #include +#include #include @@ -106,17 +107,23 @@ double Constant::operator()(const WrappedSolution& /*s*/, std::string& /*comment return cost; } +PathLength::PathLength(std::vector joints) { + for (auto& j : joints) + this->joints.emplace(std::move(j), 1.0); +} + double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/) const { const auto& traj = s.trajectory(); if (traj == nullptr || traj->getWayPointCount() == 0) return 0.0; - std::vector joint_models; - joint_models.reserve(joints.size()); + std::map weights; const auto& first_waypoint = traj->getWayPoint(0); - for (auto& joint : joints) { - joint_models.push_back(first_waypoint.getJointModel(joint)); + for (auto& joint_weight : joints) { + const robot_model::JointModel* jm = first_waypoint.getJointModel(joint_weight.first); + if (jm) + weights.emplace(jm, joint_weight.second); } double path_length{ 0.0 }; @@ -126,14 +133,65 @@ double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/) if (joints.empty()) { path_length += last.distance(curr); } else { - for (const auto& model : joint_models) { - path_length += last.distance(curr, model); + for (const auto& item : weights) { + path_length += item.second * last.distance(curr, item.first); } } } 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; }