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:
v4hn 2018-05-30 16:09:07 +02:00
parent bd1edcbde7
commit 4026661bfe
2 changed files with 4 additions and 3 deletions

View File

@ -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(),
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 true;
}

View File

@ -125,8 +125,8 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState&
const unsigned int num = jm->getVariableCount();
Eigen::Map<const Eigen::VectorXd> positions_from (from.getJointPositions(jm), num);
Eigen::Map<const Eigen::VectorXd> positions_to (to.getJointPositions(jm), num);
if (!positions_from.array().isApprox(positions_to.array(), 1e-4)) {
ROS_INFO_STREAM_ONCE_NAMED("Connect", "Deviation in joint " << jm->getName()
if (!(positions_from - positions_to).isZero(1e-4)) {
ROS_INFO_STREAM_NAMED("Connect", "Deviation in joint " << jm->getName()
<< ": [" << positions_from.transpose()
<< "] != [" << positions_to.transpose() << "]");
return false;