coppeliasim/vrep 源码学习 inverse kinematics

最新推荐文章于 2025-03-20 15:12:19 发布

转载最新推荐文章于 2025-03-20 15:12:19 发布 · 648 阅读

1 ·

CC 4.0 BY-SA版权

原文链接：https://github.com/CoppeliaRobotics/coppeliaKinematicsRoutines/blob/master/ik.cpp

文章标签：

#c++

VREP 专栏收录该内容

10 篇文章

订阅专栏

本文介绍了一种通过暴力迭代方式求解机器人逆运动学的方法。该方法从GitHub上的coppeliasim项目中摘取，通过随机选择关节角度并检查是否满足目标位置条件来寻找有效的逆解。

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

    // do the calculation:
                int startTime=0;
                int mi=999999999;
                if (maxIterations>=0)
                    mi=maxIterations;
                else
                    startTime=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;
                    }
                }