diff --git a/core/src/stage.cpp b/core/src/stage.cpp index 1f01125a..1eb6cc10 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -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; } diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 68a68939..b1a4a9a0 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -125,8 +125,8 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState& const unsigned int num = jm->getVariableCount(); Eigen::Map positions_from (from.getJointPositions(jm), num); Eigen::Map 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;