From 9a96a7e4f66d7691f4a5af3480302583dae0839e Mon Sep 17 00:00:00 2001 From: v4hn Date: Tue, 30 Jun 2020 20:39:56 +0200 Subject: [PATCH] implement LinkMotionCost --- .../moveit/task_constructor/cost_terms.h | 9 ++++++++ core/src/cost_terms.cpp | 23 +++++++++++++++++++ 2 files changed, 32 insertions(+) diff --git a/core/include/moveit/task_constructor/cost_terms.h b/core/include/moveit/task_constructor/cost_terms.h index 5a6bc8ed..d16b6722 100644 --- a/core/include/moveit/task_constructor/cost_terms.h +++ b/core/include/moveit/task_constructor/cost_terms.h @@ -24,6 +24,15 @@ double PathLengthCost(const SubTrajectory& s); /// execution duration of the whole trajectory double TrajectoryDurationCost(const SubTrajectory& s); +struct LinkMotionCost +{ + LinkMotionCost(std::string link_name) : link_name(link_name) {} + + double operator()(const SubTrajectory&, std::string&); + + std::string link_name; +}; + /** inverse distance to collision * * \arg interface compute distances using START or END interface of solution diff --git a/core/src/cost_terms.cpp b/core/src/cost_terms.cpp index 49a16d1f..bbdad413 100644 --- a/core/src/cost_terms.cpp +++ b/core/src/cost_terms.cpp @@ -64,6 +64,29 @@ double TrajectoryDurationCost(const SubTrajectory& s) { return s.trajectory() ? s.trajectory()->getDuration() : 0.0; } +double LinkMotionCost::operator()(const SubTrajectory& s, std::string& comment) { + const auto& traj{ s.trajectory() }; + + if (traj == nullptr || traj->getWayPointCount() == 0) + return 0.0; + + if (!traj->getWayPoint(0).knowsFrameTransform(link_name)) { + boost::format desc("LinkMotionCost: frame '%1%' unknown in trajectory"); + desc % link_name; + comment = desc.str(); + return std::numeric_limits::infinity(); + } + + double distance{ 0.0 }; + Eigen::Translation3d position{ traj->getWayPoint(0).getFrameTransform(link_name).translation() }; + for (size_t i{ 1 }; i < traj->getWayPointCount(); ++i) { + Eigen::Translation3d new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() }; + distance += (new_position.vector() - position.vector()).norm(); + position = new_position; + } + return distance; +} + double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) { collision_detection::DistanceRequest request; request.type =