CostTerms: joint-weighted costs, expose to python

core/include/moveit/task_constructor/cost_terms.h

@@ -40,6 +40,7 @@
 
 #include <moveit/task_constructor/storage.h>
 #include <moveit/task_constructor/utils.h>
+#include <moveit_msgs/RobotState.h>
 
 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<std::string> j) : joints{ std::move(j) } {};
+	/// Limit measurements to given joint names
+	PathLength(std::vector<std::string> joints);
+	/// Limit measurements to given joints and use given weighting
+	PathLength(std::map<std::string, double> j) : joints(std::move(j)) {}
 	double operator()(const SubTrajectory& s, std::string& comment) const override;
 
-	std::vector<std::string> joints;
+	std::map<std::string, double> 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<std::string, double> w = std::map<std::string, double>());
+	DistanceToReference(const std::map<std::string, double>& ref, Mode m = Mode::AUTO,
+	                    std::map<std::string, double> w = std::map<std::string, double>());
+	double operator()(const SubTrajectory& s, std::string& comment) const override;
+
+	moveit_msgs::RobotState reference;
+	std::map<std::string, double> 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;

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<SerialContainer>("task pipeline"));

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

core/python/bindings/src/core.cpp

@@ -33,6 +33,8 @@
  *********************************************************************/
 
 #include "core.h"
+#include <pybind11/stl.h>
+#include <pybind11/functional.h>
 #include <moveit/python/task_constructor/properties.h>
 #include <moveit/task_constructor/container_p.h>
 #include <moveit/task_constructor/task.h>
@@ -157,6 +159,36 @@ void export_core(pybind11::module& m) {
 	    .def(PYBIND11_BOOL_ATTR,
 	         [](const moveit::core::MoveItErrorCode& err) { return pybind11::cast(static_cast<bool>(err)); });
 
+	py::classh<CostTerm>(m, "CostTerm", "Base class for cost calculation in stages");
+	auto tct = py::classh<TrajectoryCostTerm, CostTerm>(m, "TrajectoryCostTerm",
+	                                                    "Base class for cost calculation of trajectories");
+	py::enum_<TrajectoryCostTerm::Mode>(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<cost::PathLength, TrajectoryCostTerm>(m, "PathLength",
+	                                                 "Computes joint-based path length along trajectory")
+	    .def(py::init<>())
+	    .def(py::init<std::vector<std::string>>())
+	    .def(py::init<std::map<std::string, double>>());
+	py::classh<cost::DistanceToReference, TrajectoryCostTerm>(m, "DistanceToReference",
+	                                                          "Computes joint-based distance to reference pose")
+	    .def(py::init<const moveit_msgs::RobotState&, TrajectoryCostTerm::Mode, std::map<std::string, double>>(),
+	         "reference"_a, "mode"_a = TrajectoryCostTerm::Mode::AUTO, "weights"_a = std::map<std::string, double>())
+	    .def(py::init<const std::map<std::string, double>&, TrajectoryCostTerm::Mode, std::map<std::string, double>>(),
+	         "reference"_a, "mode"_a = TrajectoryCostTerm::Mode::AUTO, "weights"_a = std::map<std::string, double>());
+	py::classh<cost::TrajectoryDuration, TrajectoryCostTerm>(m, "TrajectoryDuration", "Computes duration of trajectory")
+	    .def(py::init<>());
+	py::classh<cost::LinkMotion, TrajectoryCostTerm>(m, "LinkMotion",
+	                                                 "Computes Cartesian path length of given link along trajectory")
+	    .def(py::init<std::string>(), "link_name"_a);
+
+	py::classh<cost::Clearance, TrajectoryCostTerm>(m, "Clearance", "Computes inverse distance to collision objects")
+	    .def(py::init<bool, bool, std::string, TrajectoryCostTerm::Mode>(), "with_world"_a = true,
+	         "cumulative"_a = false, "group_property"_a = "group", "mode"_a = TrajectoryCostTerm::Mode::AUTO);
+
 	auto stage =
 	    properties::class_<Stage, PyStage<>>(m, "Stage", "Abstract base class of all stages.")
 	        .property<double>("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<void (Stage::*)(const CostTermConstPtr&)>("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"(

core/python/bindings/src/core.h

@@ -37,6 +37,7 @@
 #include <moveit/task_constructor/stage.h>
 #include <moveit/task_constructor/container.h>
 #include <moveit/task_constructor/cost_queue.h>
+#include <moveit/task_constructor/cost_terms.h>
 #include <moveit/utils/moveit_error_code.h>
 #include <pybind11/smart_holder.h>
 
@@ -119,6 +120,14 @@ PYBIND11_SMART_HOLDER_TYPE_CASTERS(ordered<moveit::task_constructor::SolutionBas
 PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::InterfaceState)
 PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::core::MoveItErrorCode)
 
+PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::CostTerm)
+PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::TrajectoryCostTerm)
+PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::PathLength)
+PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::DistanceToReference)
+PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::TrajectoryDuration)
+PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::LinkMotion)
+PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::Clearance)
+
 PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Stage)
 PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingEitherWay)
 PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::PropagatingForward)

core/python/test/test_mtc.py

@@ -272,6 +272,12 @@ def test_PropertyMaps(self):
                 print("error in class {}: {}".format(stage, ex))
                 raise
 
+    def test_CostTerm(self):
+        stage = stages.CurrentState()
+        weights = {"joint_{}".format(i + 1): 1.0 for i in range(6)}
+        costs = core.PathLength(weights)
+        stage.setCostTerm(costs)
+
 
 class TestContainer(unittest.TestCase):
     def __init__(self, *args, **kwargs):

core/src/cost_terms.cpp

@@ -40,6 +40,7 @@
 #include <moveit/collision_detection/collision_common.h>
 #include <moveit/robot_trajectory/robot_trajectory.h>
 #include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_state/conversions.h>
 
 #include <Eigen/Geometry>
 
@@ -106,17 +107,23 @@ double Constant::operator()(const WrappedSolution& /*s*/, std::string& /*comment
 	return cost;
 }
 
+PathLength::PathLength(std::vector<std::string> 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<const robot_model::JointModel*> joint_models;
-	joint_models.reserve(joints.size());
+	std::map<const robot_model::JointModel*, double> 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<std::string, double> w)
+  : reference(ref), weights(std::move(w)), mode(m) {}
+DistanceToReference::DistanceToReference(const std::map<std::string, double>& ref, Mode m,
+                                         std::map<std::string, double> 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<const robot_model::JointModel*, double> 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;
 }