allow to consider specific joints only in cost terms

This commit is contained in:
j-kuehn 2021-05-25 10:40:32 +02:00 committed by Michael Görner
parent 84ccd679d9
commit 67fe4faa30
2 changed files with 24 additions and 5 deletions

View File

@ -115,9 +115,12 @@ public:
/// trajectory length (interpolated between waypoints)
class PathLength : public TrajectoryCostTerm
{
// TODO(v4hn): allow to consider specific joints only
public:
double operator()(const SubTrajectory& s, std::string& comment) const override;
PathLength() = default;
PathLength(std::vector<std::string> j) : joints{ std::move(j) } {};
double operator()(const SubTrajectory&, std::string&) const override;
std::vector<std::string> joints;
};
/// execution duration of the whole trajectory

View File

@ -110,12 +110,28 @@ double Constant::operator()(const WrappedSolution& /*s*/, std::string& /*comment
double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/) const {
const auto& traj = s.trajectory();
if (traj == nullptr)
if (traj == nullptr || traj->getWayPointCount() == 0)
return 0.0;
std::vector<const robot_model::JointModel*> joint_models;
joint_models.reserve(joints.size());
const auto& first_waypoint = traj->getWayPoint(0);
for (auto& joint : joints) {
joint_models.push_back(first_waypoint.getJointModel(joint));
}
double path_length{ 0.0 };
for (size_t i = 1; i < traj->getWayPointCount(); ++i)
path_length += traj->getWayPoint(i - 1).distance(traj->getWayPoint(i));
for (size_t i = 1; i < traj->getWayPointCount(); ++i) {
auto& last = traj->getWayPoint(i - 1);
auto& curr = traj->getWayPoint(i);
if (joints.empty()) {
path_length += last.distance(curr);
} else {
for (const auto& model : joint_models) {
path_length += last.distance(curr, model);
}
}
}
return path_length;
}