只截取了GitHub 上 coppeliarobotics / coppeliskinematicsroutines / ik.cpp 里面求解有效逆解的部分，暴力迭代
？？？惊了。。。等会copy一下试试看看效果，选取一个无碰撞逆解构型有点问题，希望源码思路管用。

    // do the calculation:int startTime=0;int mi=999999999;if (maxIterations>=0)mi=maxIterations;elsestartTime=getTimeDiffInMs(-1);for (int iterationCnt=0;iterationCnt<mi;iterationCnt++){// 1. Pick a random state:for (size_t i=0;i<jointCnt;i++)joints[i]->setPosition(minVals[i]+(rand()/simReal(RAND_MAX))*rangeVals[i]);// 2. Check distances between tip and target pairs (there might be several pairs!):simReal cumulatedDist=simZero;for (size_t el=0;el<ikGroup->getIkElementCount();el++){C7Vector tipTr(tips[el]->getCumulativeTransformation());C7Vector targetTr(targets[el]->getCumulativeTransformation());C7Vector relTrInv(C7Vector::identityTransformation);if (bases[el]!=nullptr)relTrInv=bases[el]->getCumulativeTransformationPart1().getInverse();tipTr=relTrInv*tipTr;targetTr=relTrInv*targetTr;C3Vector dx(tipTr.X-targetTr.X);dx(0)*=theMetric[0];dx(1)*=theMetric[1];dx(2)*=theMetric[2];simReal angle=tipTr.Q.getAngleBetweenQuaternions(targetTr.Q)*theMetric[3];cumulatedDist+=sqrt(dx(0)*dx(0)+dx(1)*dx(1)+dx(2)*dx(2)+angle*angle);}// 3. If distance<=threshold, try to perform IK:if (cumulatedDist<=thresholdDist){if (ik_result_success==ikGroup->computeGroupIk(true)){ // 3.1 We found a configuration that works!// 3.2 Check joint limits:bool limitsOk=true;for (size_t i=0;i<jointCnt;i++){simReal pp=joints[i]->getPosition();if (joints[i]->getPositionIsCyclic()){if (rangeVals[i]<piValTimes2){while (pp>minVals[i])pp-=piValTimes2;while (pp<minVals[i])pp+=piValTimes2;if (pp>minVals[i]+rangeVals[i])limitsOk=false;}}else{if ( (pp<minVals[i])||(pp>minVals[i]+rangeVals[i]) )limitsOk=false;}}// 3.3 Finally check if the callback accepts that configuration:for (size_t i=0;i<jointCnt;i++)conf[i]=joints[i]->getPosition();if ( (validationCallback==nullptr)||validationCallback(&conf[0]) ){for (size_t i=0;i<jointCnt;i++)retConfig[i]=conf[i];retVal=1;break;}}}if (maxIterations<0){if (getTimeDiffInMs(startTime)>-maxIterations)break;}}