mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Rewrite and relax approx comparisons
This is more readable. Adjusting the threshold to 1e-4 is required to allow for tolerances in potential sampling planner steps in between.
This commit is contained in:
parent
bd1edcbde7
commit
4026661bfe
@ -687,8 +687,9 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
|
|||||||
|
|
||||||
for (auto from_it = from_object->shape_poses_.cbegin(), from_end = from_object->shape_poses_.cend(),
|
for (auto from_it = from_object->shape_poses_.cbegin(), from_end = from_object->shape_poses_.cend(),
|
||||||
to_it = to_object->shape_poses_.cbegin(); from_it != from_end; ++from_it, ++to_it)
|
to_it = to_object->shape_poses_.cbegin(); from_it != from_end; ++from_it, ++to_it)
|
||||||
if (!from_it->matrix().array().isApprox(to_it->matrix().array()))
|
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)){
|
||||||
return false; // transforms do not match
|
return false; // transforms do not match
|
||||||
|
}
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -125,8 +125,8 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState&
|
|||||||
const unsigned int num = jm->getVariableCount();
|
const unsigned int num = jm->getVariableCount();
|
||||||
Eigen::Map<const Eigen::VectorXd> positions_from (from.getJointPositions(jm), num);
|
Eigen::Map<const Eigen::VectorXd> positions_from (from.getJointPositions(jm), num);
|
||||||
Eigen::Map<const Eigen::VectorXd> positions_to (to.getJointPositions(jm), num);
|
Eigen::Map<const Eigen::VectorXd> positions_to (to.getJointPositions(jm), num);
|
||||||
if (!positions_from.array().isApprox(positions_to.array(), 1e-4)) {
|
if (!(positions_from - positions_to).isZero(1e-4)) {
|
||||||
ROS_INFO_STREAM_ONCE_NAMED("Connect", "Deviation in joint " << jm->getName()
|
ROS_INFO_STREAM_NAMED("Connect", "Deviation in joint " << jm->getName()
|
||||||
<< ": [" << positions_from.transpose()
|
<< ": [" << positions_from.transpose()
|
||||||
<< "] != [" << positions_to.transpose() << "]");
|
<< "] != [" << positions_to.transpose() << "]");
|
||||||
return false;
|
return false;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user