Link rotation cost (moveit#726)

Add TrajectoryCostTerm measuring total orientation change of a link along the trajectory

Author: rhaschke

core/include/moveit/task_constructor/cost_terms.h

@@ -177,6 +177,18 @@ class LinkMotion : public TrajectoryCostTerm
 	double operator()(const SubTrajectory& s, std::string& comment) const override;
 };
 
+/** total orientation change of a link through the trajectory */
+class LinkRotation : public TrajectoryCostTerm
+{
+public:
+	LinkRotation(std::string link_name);
+
+	std::string link_name;
+
+	using TrajectoryCostTerm::operator();
+	double operator()(const SubTrajectory& s, std::string& comment) const override;
+};
+
 /** inverse distance to collision
  *
  * \arg with_world check distances to world objects or look at self-collisions

core/python/bindings/src/core.cpp

@@ -172,6 +172,10 @@ void export_core(pybind11::module& m) {
 	                                                 "Computes Cartesian path length of given link along trajectory")
 	    .def(py::init<std::string>(), "link_name"_a);
 
+	py::classh<cost::LinkRotation, TrajectoryCostTerm>(
+	    m, "LinkRotation", "Computes total orientation change 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);

core/src/cost_terms.cpp

@@ -220,6 +220,30 @@ double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) cons
 	return distance;
 }
 
+LinkRotation::LinkRotation(std::string link) : link_name{ std::move(link) } {}
+
+double LinkRotation::operator()(const SubTrajectory& s, std::string& comment) const {
+	const auto& traj{ s.trajectory() };
+
+	if (traj == nullptr || traj->getWayPointCount() == 0)
+		return 0.0;
+
+	if (!traj->getWayPoint(0).knowsFrameTransform(link_name)) {
+		comment = fmt::format("LinkRotationCost: frame '{}' unknown in trajectory", link_name);
+		return std::numeric_limits<double>::infinity();
+	}
+
+	double angular_distance{ 0.0 };
+
+	Eigen::Quaterniond q{ traj->getWayPoint(0).getFrameTransform(link_name).linear() };
+	for (size_t i{ 1 }; i < traj->getWayPointCount(); ++i) {
+		const Eigen::Quaterniond next_q{ traj->getWayPoint(i).getFrameTransform(link_name).linear() };
+		angular_distance += q.angularDistance(next_q);
+		q = next_q;
+	}
+	return angular_distance;
+}
+
 Clearance::Clearance(bool with_world, bool cumulative, std::string group_property, Mode mode)
   : with_world{ with_world }
   , cumulative{ cumulative }

demo/src/alternative_path_costs.cpp

@@ -61,6 +61,12 @@ int main(int argc, char** argv) {
 		connect->setCostTerm(std::make_unique<cost::LinkMotion>("panda_link4"));
 		alternatives->add(std::move(connect));
 	}
+	{
+		auto connect{ std::make_unique<stages::Connect>(
+			 "eef rotation", stages::Connect::GroupPlannerVector{ { "panda_arm", pipeline } }) };
+		connect->setCostTerm(std::make_unique<cost::LinkRotation>("panda_hand"));
+		alternatives->add(std::move(connect));
+	}
 
 	t.add(std::move(alternatives));