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 };
|
||||
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) {
|
||||
Eigen::Translation3d new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() };
|
||||
distance += (new_position.vector() - position.vector()).norm();
|
||||
const auto& new_position{ traj->getWayPoint(i).getFrameTransform(link_name).translation() };
|
||||
distance += (new_position - position).norm();
|
||||
position = new_position;
|
||||
}
|
||||
return distance;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user