mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
implement LinkMotionCost
This commit is contained in:
parent
6dbc742f20
commit
9a96a7e4f6
@ -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
|
||||
|
||||
@ -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<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) {
|
||||
collision_detection::DistanceRequest request;
|
||||
request.type =
|
||||
|
||||
Loading…
Reference in New Issue
Block a user