mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
simplify LinkMotion computation
review feedback
This commit is contained in:
parent
05932ad5f1
commit
64e2440c53
@ -116,10 +116,10 @@ double LinkMotion::compute(const SubTrajectory& s, std::string& comment) const {
|
|||||||
}
|
}
|
||||||
|
|
||||||
double distance{ 0.0 };
|
double distance{ 0.0 };
|
||||||
Eigen::Translation3d position{ traj->getWayPoint(0).getFrameTransform(link_name).translation() };
|
Eigen::Vector3d position{ traj->getWayPoint(0).getFrameTransform(link_name).translation() };
|
||||||
for (size_t i{ 1 }; i < traj->getWayPointCount(); ++i) {
|
for (size_t i{ 1 }; i < traj->getWayPointCount(); ++i) {
|
||||||
Eigen::Translation3d new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() };
|
const auto& new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() };
|
||||||
distance += (new_position.vector() - position.vector()).norm();
|
distance += (new_position - position).norm();
|
||||||
position = new_position;
|
position = new_position;
|
||||||
}
|
}
|
||||||
return distance;
|
return distance;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user