diff --git a/VirtualRobot/IK/ConstrainedOptimizationIK.cpp b/VirtualRobot/IK/ConstrainedOptimizationIK.cpp index 5e9479ee2889f6d38533c00e239760430d0ad33e..a67fdb59613ef868a737377708313a0ef061f598 100644 --- a/VirtualRobot/IK/ConstrainedOptimizationIK.cpp +++ b/VirtualRobot/IK/ConstrainedOptimizationIK.cpp @@ -137,6 +137,15 @@ bool ConstrainedOptimizationIK::solve(bool stepwise) } break; } + + // Check initial configuration against joint limits + for(unsigned int i = 0; i < nodeSet->getSize(); i++) + { + if(x[i] < nodeSet->getNode(i)->getJointLimitLo() || x[i] > nodeSet->getNode(i)->getJointLimitHi()) + { + THROW_VR_EXCEPTION("Initial configuration outside of joint limits"); + } + } } double min_f; @@ -154,7 +163,7 @@ bool ConstrainedOptimizationIK::solve(bool stepwise) { // This is something more severe, we still check the result and proceed // with the next attempt. - VR_INFO << "Warning: NLOPT exception while optimizing" << std::endl; + VR_INFO << "Warning: NLOPT exception while optimizing: " << e.what() << std::endl; } for(int i = 0; i < size; i++) @@ -178,7 +187,6 @@ bool ConstrainedOptimizationIK::solve(bool stepwise) currentMinError = currentError; bestJointValues = x; } - } if(bestJointValues.size() > 0) {