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(),
|
||||
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;
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user