implement LinkMotionCost

This commit is contained in:
v4hn 2020-06-30 20:39:56 +02:00
parent 6dbc742f20
commit 9a96a7e4f6
2 changed files with 32 additions and 0 deletions

View File

@ -24,6 +24,15 @@ double PathLengthCost(const SubTrajectory& s);
/// execution duration of the whole trajectory /// execution duration of the whole trajectory
double TrajectoryDurationCost(const SubTrajectory& s); 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 /** inverse distance to collision
* *
* \arg interface compute distances using START or END interface of solution * \arg interface compute distances using START or END interface of solution

View File

@ -64,6 +64,29 @@ double TrajectoryDurationCost(const SubTrajectory& s) {
return s.trajectory() ? s.trajectory()->getDuration() : 0.0; 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<double>::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) { double ClearanceCost::operator()(const SubTrajectory& s, std::string& comment) {
collision_detection::DistanceRequest request; collision_detection::DistanceRequest request;
request.type = request.type =