QVec RMat::QMat::extractAnglesR_min() const { QVec r = extractAnglesR(); QVec v1 = r.subVector(0,2); QVec v2 = r.subVector(3,5); if (v1.norm2() < v2.norm2()) return v1; return v2; }
std::tuple< bool, QString > Sampler::checkRobotValidStateAtTarget(const QVec& target) const { if( target.size() != 6 ) return std::make_tuple(false, QString("Invalid target vector. A 6D vector is required")); return checkRobotValidStateAtTarget(target.subVector(0,2), target.subVector(3,5)); }