diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 3beb8db91..337a8d15d 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/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 diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index 12d9679f9..357b29ce8 100644 --- a/core/python/bindings/src/core.cpp +++ b/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(), "link_name"_a); + py::classh( + m, "LinkRotation", "Computes total orientation change 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); diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 4373d7d1f..72ed04096 100644 --- a/core/src/cost_terms.cpp +++ b/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::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 } diff --git a/demo/src/alternative_path_costs.cpp b/demo/src/alternative_path_costs.cpp index d72e92bbd..0a693096d 100644 --- a/demo/src/alternative_path_costs.cpp +++ b/demo/src/alternative_path_costs.cpp @@ -61,6 +61,12 @@ int main(int argc, char** argv) { connect->setCostTerm(std::make_unique("panda_link4")); alternatives->add(std::move(connect)); } + { + auto connect{ std::make_unique( + "eef rotation", stages::Connect::GroupPlannerVector{ { "panda_arm", pipeline } }) }; + connect->setCostTerm(std::make_unique("panda_hand")); + alternatives->add(std::move(connect)); + } t.add(std::move(alternatives));