From 495cd84a09875826af43c3c195443bf0cb6cb991 Mon Sep 17 00:00:00 2001 From: Joe Moore Date: Mon, 17 Nov 2025 12:04:38 +0000 Subject: [PATCH 1/3] Add LinkRotation cost term --- .../moveit/task_constructor/cost_terms.h | 15 ++++++++++++ core/python/bindings/src/core.cpp | 4 ++++ core/python/bindings/src/core.h | 1 + core/src/cost_terms.cpp | 24 +++++++++++++++++++ 4 files changed, 44 insertions(+) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 68b84e2b5..4fe3ce59f 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -177,6 +177,21 @@ class LinkMotion : public TrajectoryCostTerm double operator()(const SubTrajectory& s, std::string& comment) const override; }; + +/** total orientation change of a link through the trajectory + * favours trajectories which maintains the same orientation of the given link frame +*/ +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 f770131d1..c2dc0cbd9 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -171,6 +171,10 @@ void export_core(pybind11::module& m) { py::classh(m, "LinkMotion", "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, diff --git a/core/python/bindings/src/core.h b/core/python/bindings/src/core.h index 9c7f4e3de..7ab14374b 100644 --- a/core/python/bindings/src/core.h +++ b/core/python/bindings/src/core.h @@ -123,6 +123,7 @@ 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::LinkRotation) PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::cost::Clearance) PYBIND11_SMART_HOLDER_TYPE_CASTERS(moveit::task_constructor::Stage) diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 179731495..9ddb94ff2 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -221,6 +221,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 } From 994fe85dae72da296b3f67abd1fd7503f68807a5 Mon Sep 17 00:00:00 2001 From: Joe Moore Date: Mon, 17 Nov 2025 14:43:37 +0000 Subject: [PATCH 2/3] Add to path costs demo --- demo/src/alternative_path_costs.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/demo/src/alternative_path_costs.cpp b/demo/src/alternative_path_costs.cpp index ae380aa7d..4d72ee9c2 100644 --- a/demo/src/alternative_path_costs.cpp +++ b/demo/src/alternative_path_costs.cpp @@ -65,6 +65,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)); From a9cb6c92c96b1b23aca00a14887ce4af014e0b25 Mon Sep 17 00:00:00 2001 From: Joe Moore Date: Mon, 17 Nov 2025 16:29:47 +0000 Subject: [PATCH 3/3] Clang format --- core/include/moveit/task_constructor/cost_terms.h | 5 ++--- core/python/bindings/src/core.cpp | 6 +++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 4fe3ce59f..b1a025ec1 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -177,10 +177,9 @@ class LinkMotion : public TrajectoryCostTerm double operator()(const SubTrajectory& s, std::string& comment) const override; }; - -/** total orientation change of a link through the trajectory +/** total orientation change of a link through the trajectory * favours trajectories which maintains the same orientation of the given link frame -*/ + */ class LinkRotation : public TrajectoryCostTerm { public: diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index c2dc0cbd9..5aee18f1e 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -171,9 +171,9 @@ void export_core(pybind11::module& m) { py::classh(m, "LinkMotion", "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") + + 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")