bool kinematic_constraints::OrientationConstraint::equal(const KinematicConstraint &other, double margin) const { if (other.getType() != type_) return false; const OrientationConstraint &o = static_cast<const OrientationConstraint&>(other); if (o.link_model_ == link_model_ && desired_rotation_frame_id_ == o.desired_rotation_frame_id_) { Eigen::Matrix3d diff = desired_rotation_matrix_.inverse() * o.desired_rotation_matrix_; if (!diff.isIdentity(margin)) return false; return fabs(absolute_x_axis_tolerance_ - o.absolute_x_axis_tolerance_) <= margin && fabs(absolute_y_axis_tolerance_ - o.absolute_y_axis_tolerance_) <= margin && fabs(absolute_z_axis_tolerance_ - o.absolute_z_axis_tolerance_) <= margin; } return false; }