【QT+OpenCascade+RL】安川机器人仿真器源码学习

主界面

该仿真器通过加载障碍物模型，设置关节起始配置，设置关节终点配置（后续可设置多个终点），再之后开启运动规划线程即可实现机械臂的避障运动规划。

机械臂基本操作

该软件基于QT开发，OpenCascade作为显示内核，借助RoboticsLibrary库作为为运动学正逆解计算、碰撞检测和运动规划引擎。该软件可以实现以下功能：1.DH坐标系形状的绘制。2.移动机械臂的连杆在场景中的位置。3.场景的gif以及视频录制。4.滑动条设置关节位置……

运动规划操作演示视频

程序框架

部分代码：

封装的RLAPI接口：

#include "RLConvertAPI.h"


//=======================================================================
//function : RLConvertAPI
//purpose  : *
//=======================================================================
RLConvertAPI::RLConvertAPI(QString &JointMdlFile, QString &JointSgFile, QString &JointModelFile,
                           QObject *parent) : QObject(parent)
  ,mJointMdlFileName(JointMdlFile)
  ,mJointSgFileName(JointSgFile)
  ,mJointModelFilePath(JointModelFile)
{
    aWriterAPI = new RLAPI_Writer;
    aReaderAPI = new RLAPI_Reader;
}


//=======================================================================
//function : RLConvertAPI
//purpose  : *
//=======================================================================
RLConvertAPI::~RLConvertAPI()
{
    delete aReaderAPI;
    aReaderAPI = nullptr;
    delete aWriterAPI;
    aWriterAPI = nullptr;
}


//=======================================================================
//function : LoadFiles
//purpose  : * 加载文件
//=======================================================================
void RLConvertAPI::InitLoadData()
{
    if(aReaderAPI==nullptr)
        aReaderAPI = new RLAPI_Reader;


    emit RLSatutsChanged(RLStatus::ReadingMdlXML);
    aReaderAPI->ReadModelXMLFIle(mJointMdlFileName);


    emit RLSatutsChanged(RLStatus::ReadingSceneXML);
    aReaderAPI->ReadSceneXMLFile(mJointSgFileName);


    emit RLSatutsChanged(RLStatus::ParsingAssembleArgs);
    aReaderAPI->ReadAssembleArgs(mJointSgFileName);


    emit RLSatutsChanged(RLStatus::ParsingMotionArgs);
    aReaderAPI->ReadAixsDirection(mJointMdlFileName);


    emit RLSatutsChanged(RLStatus::ReadingJointFiles);
    aReaderAPI->ReadJointModels(mJointModelFilePath);


    SetJointValue(aReaderAPI->JointModel->getHomePosition());
    aReaderAPI->MeasureAISShape->SetLocalTransformation(aReaderAPI->MeasureModelTrsf);
    MotionMaxValues = aReaderAPI->JointModel->getMaximum();
    MotionMinValues = aReaderAPI->JointModel->getMinimum();
    MotionSpeedValues = aReaderAPI->JointModel->getSpeed();
}


//=======================================================================
//function : ImportSceneModel
//purpose  : * 写入场景模型vrml文件  .wrl文件//
//=======================================================================
void RLConvertAPI::ImportSceneModel(const QString &theName)
{
    mImportModelFileName = theName;


    QFileInfo aInfo(mImportModelFileName);
    QString Path = aInfo.filePath().remove(aInfo.fileName());
    QString theVrmlFile(Path+aInfo.baseName()+".wrl");
    aInfo.setFile(theVrmlFile);


    if(!aInfo.exists())
    {
        //1、从形状构建网格 build the mesh from the shape
        Handle(Poly_Triangulation) aMesh = aWriterAPI->BuildMesh(aReaderAPI->ReadAModelFile(theName)->Shape());


        //2、生成 generate a vrml file that has the same base name
        aWriterAPI->WriteMeshToVrml(aMesh,theVrmlFile);
    }


    //3、创建场景VRML文件creat the scene vrml file
    aWriterAPI->GenerateSceneVrmlFile(theVrmlFile ,aReaderAPI->SceneVrmlFileName);


    //4、创建场景XML文件creat the scene xml file
    aWriterAPI->GenerateSceneXMLFile(aReaderAPI->SceneVrmlFileName,mJointSgFileName,aReaderAPI->JointModel->getDof());
}


//=======================================================================
//function : Reset
//purpose  : *重置场景模型
//=======================================================================
void RLConvertAPI::ResetSceneModel()
{    
    delete aReaderAPI->SceneModel;
    aReaderAPI->SceneModel = nullptr;
    aReaderAPI->CollisionScene.reset();
    aReaderAPI->AssembleTrsfs.clear();
    aReaderAPI->MotionCenters.clear();
    aReaderAPI->MeasureAISShape.reset(nullptr);


    aReaderAPI->ReadSceneXMLFile(mJointSgFileName);//读取场景xml文件//
    aReaderAPI->ReadAssembleArgs(mJointSgFileName);//读取装配参数//


    SetJointValue(aReaderAPI->JointModel->getHomePosition());//设置关节位置：关节模型的 home位置//
    aReaderAPI->MeasureAISShape->SetLocalTransformation(aReaderAPI->MeasureModelTrsf);//设置测量模型位姿//
}


//=======================================================================
//function : SetIndexedJointValue
//purpose  : *设置指定索引的关节值//
//=======================================================================
void RLConvertAPI::SetIndexedJointValue(const int &index, const double &angle)
{
    rl::mdl::Kinematic* aKinematic = dynamic_cast<rl::mdl::Kinematic*>(aReaderAPI->JointModel.get());//关节模型的运动学对象
    rl::math::Vector pos = aKinematic->getPosition();//获取关节位置矢量//
    if(aReaderAPI->JointType.at(index)==RLAPI_JointType::Revolute)//转动副//
        pos(index) = angle * rl::math::constants::deg2rad;//rad used in calculate but deg used in ui
    else if(aReaderAPI->JointType.at(index)==RLAPI_JointType::Prismatic)//移动副//
        pos(index) = angle;
    else//其他转动副//
        pos(index) = angle * rl::math::constants::deg2rad;
    SetJointValue(pos);//设置关节位置//
}


//=======================================================================
//function : SetJointValue
//purpose  : * 根据矢量设定关节位置
//=======================================================================
void RLConvertAPI::SetJointValue(const rl::math::Vector &aVector)
{
    rl::mdl::Kinematic* aKinematic = dynamic_cast<rl::mdl::Kinematic*>(aReaderAPI->JointModel.get());//运动学对象//


    aKinematic->setPosition(aVector);//设置运动学对象的关节位置矢量//
    aKinematic->forwardPosition();//位置正解//


    for (std::size_t i = 0; i<aReaderAPI->SceneModel->getNumBodies(); ++i)//遍历场景模型的所有零件//
    {
        rl::math::Transform theTrans = aKinematic->getBodyFrame(i);//获取零件坐标系位姿//
        aReaderAPI->CollisionScene->getModel(0)->getBody(i)->setFrame(theTrans);//获取碰撞场景中运动学对象的零件i，设置其位姿//
    }
    //设置一个累积Trsf来改变当前轴之后的轴//
    gp_Trsf CumulativeTrsf;//set an Cumulative Trsf to change the axis after the current one
    for(int i=0;i<aReaderAPI->MotionAxis.size();++i)
    {
        gp_Trsf aTrsf = aReaderAPI->JointAISShapes.at(i+1)->Shape().Location().Transformation();//下一关节形状的变换矩阵//
        if(aReaderAPI->JointType.at(i)==RLAPI_JointType::Revolute)//转动副//
            aTrsf.SetRotation(aReaderAPI->MotionAxis.at(i),aVector(i));//旋转变换//


        else if(aReaderAPI->JointType.at(i)==RLAPI_JointType::Prismatic)//移动副//
            aTrsf.SetTranslation(gp_Vec(aReaderAPI->MotionAxis.at(i).Direction())*aVector(i));//平移变换//


        CumulativeTrsf.Multiply(aTrsf);//累积变换//


        gp_Trsf workTrsf;//the real work Trsf ,E * Cumulative * Assemble
        workTrsf.Multiply(CumulativeTrsf);//相对运动位姿//
        workTrsf.Multiply(aReaderAPI->AssembleTrsfs.value(i));//装配参数：初始位姿矩阵//


        //set the value JointAISShapes a public member in the class,when u change here ,u needn't to display them again in the main function, update the occWidget is enough
        //在类中设置值JointAISShapes为公共成员，当你在这里更改时，你不需要在main函数中再次显示它们，更新occWidget就足够了
        aReaderAPI->JointAISShapes.at(i+1)->SetLocalTransformation(workTrsf);
    }


    //here is the signal to update the occWidget这里是更新occWidget的信号//
    emit JointPositionChanged();
    IsCollision();//碰撞检测//
}


//=======================================================================
//function : SetIndexedInverseValue
//purpose  : * 设置tcp某项，进行逆解//
//=======================================================================
bool RLConvertAPI::SetIndexedInverseValue(const int &index, const double &value)
{
    if (rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(aReaderAPI->JointModel.get()))
    {
        rl::math::Transform transform = kinematic->getOperationalPosition(0);//当前tcp位置//
        rl::math::Vector3 orientation = transform.linear().eulerAngles(2, 1, 0).reverse();//当前tcp欧拉角//


        switch (index)
        {
        case 0:
        case 1:
        case 2:
            transform.translation()(index) = value;//设置tcp的 x或y或z
            break;
        case 3:
            transform.linear() = (
                        rl::math::AngleAxis(orientation.z(), rl::math::Vector3::UnitZ()) *
                        rl::math::AngleAxis(orientation.y(), rl::math::Vector3::UnitY()) *
                        rl::math::AngleAxis(value * rl::math::constants::deg2rad, rl::math::Vector3::UnitX())
                        ).toRotationMatrix();//绕x轴旋转//
            break;
        case 4:
            transform.linear() = (
                        rl::math::AngleAxis(orientation.z(), rl::math::Vector3::UnitZ()) *
                        rl::math::AngleAxis(value * rl::math::constants::deg2rad, rl::math::Vector3::UnitY()) *
                        rl::math::AngleAxis(orientation.x(), rl::math::Vector3::UnitX())
                        ).toRotationMatrix();//绕y轴旋转//
            break;
        case 5:
            transform.linear() = (
                        rl::math::AngleAxis(value * rl::math::constants::deg2rad, rl::math::Vector3::UnitZ()) *
                        rl::math::AngleAxis(orientation.y(), rl::math::Vector3::UnitY()) *
                        rl::math::AngleAxis(orientation.x(), rl::math::Vector3::UnitX())
                        ).toRotationMatrix();//绕z轴旋转//
            break;
        default:
            break;
        }


        rl::math::Vector q = kinematic->getPosition();//获取更新前关节位置//
        rl::mdl::NloptInverseKinematics* aInverse = new rl::mdl::NloptInverseKinematics(kinematic);//位置逆解对象//
        aInverse->setIterations(10000);//迭代次数//
        aInverse->setDuration(std::chrono::milliseconds(500));//持续时间500ms//


        for (std::size_t i = 0; i < kinematic->getOperationalDof(); ++i)//操作空间自由度//
        {
            aInverse->addGoal(i == 0 ? transform : kinematic->getOperationalPosition(i), i);//设置逆解目标//
        }


        bool solved = aInverse->solve();//逆解求解//


        if (solved)//有解//
        {
            rl::math::Vector currentPos = aReaderAPI->JointModel->getPosition();//新的当前关节位置//
            SetJointValue(currentPos);//更新关节位置//
            delete aInverse;
            return true;
        }
        else//无解//
        {
            kinematic->setPosition(q);//恢复求解前关节位置//
            kinematic->forwardPosition();//正解计算//
            delete aInverse;//释放逆解对象//
            return false;//求解失败//
        }
    }
    return false;
}


//=======================================================================
//function : SetInverseValue
//purpose  : * 设置tcp 位姿矢量 求逆解//
//=======================================================================
bool RLConvertAPI::SetInverseValue(const QList<double> &TCPInfo)
{
    Q_ASSERT(TCPInfo.size() == 6);


    if (rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(aReaderAPI->JointModel.get()))
    {
        rl::math::Transform transform = kinematic->getOperationalPosition(0);


        transform.translation()(0) = TCPInfo[0];
        transform.translation()(1) = TCPInfo[1];
        transform.translation()(2) = TCPInfo[2];
        transform.linear() = (
                    rl::math::AngleAxis(TCPInfo[5], rl::math::Vector3::UnitZ()) *
                    rl::math::AngleAxis(TCPInfo[4], rl::math::Vector3::UnitY()) *
                    rl::math::AngleAxis(TCPInfo[3], rl::math::Vector3::UnitX())
                ).toRotationMatrix();


        rl::math::Vector q = kinematic->getPosition();
        rl::mdl::NloptInverseKinematics* aInverse = new rl::mdl::NloptInverseKinematics(kinematic);
        aInverse->setIterations(10000);
        aInverse->setDuration(std::chrono::milliseconds(500));


        for (std::size_t i = 0; i < kinematic->getOperationalDof(); ++i)
        {
            aInverse->addGoal(i == 0 ? transform : kinematic->getOperationalPosition(i), i);
        }


        bool solved = aInverse->solve();


        if (solved)
        {
            rl::math::Vector currentPos = aReaderAPI->JointModel->getPosition();
            SetJointValue(currentPos);
            delete aInverse;
            return true;
        }
        else
        {
            kinematic->setPosition(q);
            kinematic->forwardPosition();
            delete aInverse;
            return false;
        }
    }
    return false;
}


//=======================================================================
//function : UnLockBasePosition
//purpose  : * 锁定基座位置//
//=======================================================================
void RLConvertAPI::LockBasePosition()
{   //两个移动轴 设定最大 最小关节位置为当前关节位置//
    aReaderAPI->JointModel->getJoint(0)->max(0) = aReaderAPI->JointModel->getPosition()(0);
    aReaderAPI->JointModel->getJoint(0)->min(0) = aReaderAPI->JointModel->getPosition()(0);
    aReaderAPI->JointModel->getJoint(1)->max(0) = aReaderAPI->JointModel->getPosition()(1);
    aReaderAPI->JointModel->getJoint(1)->min(0) = aReaderAPI->JointModel->getPosition()(1);
}


//=======================================================================
//function : UnLockBasePosition
//purpose  : *  解锁基座位置//
//=======================================================================
void RLConvertAPI::UnLockBasePosition()
{   //恢复两个移动轴关节位置上下限为默认设置//
    aReaderAPI->JointModel->getJoint(0)->max(0) = MotionMaxValues[0];
    aReaderAPI->JointModel->getJoint(0)->min(0) = MotionMinValues[0];
    aReaderAPI->JointModel->getJoint(1)->max(0) = MotionMaxValues[1];
    aReaderAPI->JointModel->getJoint(1)->min(0) = MotionMinValues[1];
}


//=======================================================================
//function : IsCollision
//purpose  : *  碰撞检测//
//=======================================================================
void RLConvertAPI::IsCollision()
{
    if (rl::sg::SimpleScene* simpleScene = dynamic_cast<rl::sg::SimpleScene*>(aReaderAPI->CollisionScene.get()))//简单场景--碰撞场景//
    {
        bool collision = false;   //默认不碰撞//
        size_t index=0;
        //遍历碰撞场景的 A/B 集合
        for(std::size_t i=1;i<aReaderAPI->CollisionScene->getModel(0)->getNumBodies();++i)//body0和参考面接触，collision恒为true
        {
            for(std::size_t j=0;j<aReaderAPI->CollisionScene->getModel(1)->getNumBodies();++j)
            {
                collision = simpleScene->areColliding(aReaderAPI->CollisionScene->getModel(0)->getBody(i),
                                                     aReaderAPI->CollisionScene->getModel(1)->getBody(j));//判断碰撞//
                if(collision)//如果碰撞//
                {
                    index = i;//获得碰撞的关节索引//
                    emit JointCollision(index);//发送关节碰撞信号  参数-关节索引//
                    return;
                }
            }
        }


        //self check 机器人自碰撞检测//
        bool selfCollision = false;
        size_t aIndex=0,bIndex=0;
        //  判断机械臂的 两两连杆是否碰撞//
        for(std::size_t m=0;m<aReaderAPI->CollisionScene->getModel(0)->getNumBodies();++m)
        {
            for(std::size_t n=aReaderAPI->CollisionScene->getModel(0)->getNumBodies()-1;n>m;--n)
            {
                selfCollision = simpleScene->areColliding(aReaderAPI->CollisionScene->getModel(0)->getBody(m),
                                                     aReaderAPI->CollisionScene->getModel(0)->getBody(n));
                if(selfCollision)
                {
                    if(n-m==1)
                        continue;


                    aIndex=m;
                    bIndex=n;
                    emit SelfCollision(aIndex,bIndex);//自碰撞信号//
                    return;
                }
            }
        }


        emit NoCollision();//无碰撞//
    }
}


//=======================================================================
//function : GetJointPosition
//purpose  : *  获取关节位置//
//=======================================================================
rl::math::Vector RLConvertAPI::GetJointPosition()
{
    rl::math::Vector currentPos = aReaderAPI->JointModel->getPosition();//从关节模型获取当前关节位置//
    return currentPos;
}


//=======================================================================
//function : GetOperationalPosition
//purpose  : * 获取tcp位姿//
//=======================================================================
QList<double> RLConvertAPI::GetOperationalPosition()
{
    QList<double> pos;


    rl::math::Transform Tr = aReaderAPI->JointModel->getOperationalPosition(0);
    rl::math::Vector3 OR = Tr.rotation().eulerAngles(2, 1, 0).reverse();//欧拉角 ZYX//
    pos.append(Tr.translation()(0));
    pos.append(Tr.translation()(1));
    pos.append(Tr.translation()(2));
    pos.append(OR(0)* rl::math::constants::rad2deg);
    pos.append(OR(1)* rl::math::constants::rad2deg);
    pos.append(OR(2)* rl::math::constants::rad2deg);


    return pos;
}


//=======================================================================
//function : GetJointVelocity
//purpose  : *  获取关节速度矢量//
//=======================================================================
QList<double> RLConvertAPI::GetJointVelocity()
{
    QList<double> speed;
    rl::math::Vector s = aReaderAPI->JointModel->getVelocity();//从关节模型获取关节速度矢量//
    for (int i=0;i<s.size();++i)
    {
        speed.append(s[i]);
    }
    return speed;
}


//=======================================================================
//function : GetJointAcceleration
//purpose  : *  获取关节加速度矢量//
//=======================================================================
QList<double> RLConvertAPI::GetJointAcceleration()
{
    QList<double> Acceleration;
    rl::math::Vector a = aReaderAPI->JointModel->getAcceleration();
    for (int i=0;i<a.size();++i)
    {
        Acceleration.append(a[i]);
    }
    return Acceleration;
}

 OCC部件：

#include "occWidget.h"
//构造函数//
OCCWidget::OCCWidget(QWidget *parent) :QWidget(parent)
{
    if (m_context.IsNull())//交互上下文为空//
    {
        Handle(Aspect_DisplayConnection) m_display_donnection = new Aspect_DisplayConnection();//显示连接//
        if (m_graphicDriver.IsNull())//图形驱动器为空//
        {
            m_graphicDriver = new OpenGl_GraphicDriver(m_display_donnection);//初始化opengl图形驱动器//
        }
        WId window_handle =  winId();//窗口句柄//
        Handle(WNT_Window) wind = new WNT_Window((Aspect_Handle) window_handle);
        m_viewer = new V3d_Viewer(m_graphicDriver);//观察者  查看器//
        m_view = m_viewer->CreateView();//查看器创建视图//
        m_view->SetWindow(wind);//设置视图的窗口为  OCCWidget的窗口//


        if (!wind->IsMapped())
        {
            wind->Map();
        }
        m_context = new AIS_InteractiveContext(m_viewer);//获取查看器 （观察者）的交互上下文//
        m_viewer->SetDefaultLights();//设置查看器的默认光照//
        m_viewer->SetLightOn();//开启查看器光照//
        //设置视图//
        m_view->ChangeRenderingParams().Method = Graphic3d_RM_RASTERIZATION;//改变渲染参数的方法：Graphic3d_RM_RASTERIZATION//
        m_view->ChangeRenderingParams().IsAntialiasingEnabled = Standard_False;//是否启用了反锯齿：false
        m_view->SetBgGradientColors(Quantity_NOC_GRAY80,Quantity_NOC_WHITESMOKE,Aspect_GFM_VER);//设置背景梯度颜色//
        m_view->MustBeResized();//视图必须可改变尺寸//
        m_context->SetDisplayMode(AIS_Shaded, Standard_True);//设置上下文显示模式  AIS_Shaded //
    }


    setAttribute(Qt::WA_PaintOnScreen);//在屏幕绘制//
    setAttribute(Qt::WA_NoSystemBackground);//无系统背景//
    setBackgroundRole( QPalette::NoRole );//设置颜色角色//
    setFocusPolicy( Qt::StrongFocus );//强聚焦策略//
    setMouseTracking(true);//鼠标跟踪//
    //上下文风格颜色设置//
    m_context->HighlightStyle()->SetColor(Quantity_NOC_CYAN1);//上下文高亮颜色//
    m_context->SelectionStyle()->SetColor(Quantity_NOC_CYAN1);//上下文选中风格-颜色//
    m_context->HighlightStyle(Prs3d_TypeOfHighlight_LocalDynamic)->SetColor(Quantity_NOC_CYAN1);
    m_context->HighlightStyle(Prs3d_TypeOfHighlight_LocalSelected)->SetColor(Quantity_NOC_CYAN1);
    //显示模式  透明度设置//
    m_context->HighlightStyle()->SetDisplayMode(1);
    m_context->SelectionStyle()->SetDisplayMode(1);
    m_context->SelectionStyle()->SetTransparency(0.2f);
    m_context->HighlightStyle()->SetTransparency(0.2f);
    //主选择器-拣选最近：false//
    m_context->MainSelector()->SetPickClosest(Standard_False);
    //框选橡皮筋：初始化矩形//
    m_rubberBand = new QRubberBand(QRubberBand::Rectangle,this);
    m_rubberBand->setStyle(QStyleFactory::create("windows"));//框选风格//
    //操作器设置//
    m_manipulator = new AIS_Manipulator();//初始化AIS操作器//
    m_manipulator->SetPart (0, AIS_MM_Scaling, Standard_True);//设置缩放//
    m_manipulator->SetPart (1, AIS_MM_Rotation, Standard_True);//设置旋转//
    m_manipulator->SetPart (2, AIS_MM_Translation, Standard_True);//设置平移//
    m_manipulator->EnableMode (AIS_MM_Translation);//启用平移//
    m_manipulator->EnableMode (AIS_MM_Rotation);//启用旋转//
    m_manipulator->EnableMode (AIS_MM_Scaling);//启用缩放//
    //创建cube//
    Handle(AIS_ViewCube) aCube = new AIS_ViewCube();
    aCube->SetAutoStartAnimation(Standard_True);//自动开启动画//
    aCube->SetSize(60);//设置cube尺寸//
    aCube->SetFontHeight(13);//字体高度13//
    m_context->Display(aCube,Standard_False);//上下文显示cube//
}
//绘制事件//
void OCCWidget::paintEvent(QPaintEvent *)
{
    m_view->Redraw();//重绘视图//
}
//缩放视图//
void OCCWidget::resizeEvent(QResizeEvent *)
{
    if( !m_view.IsNull() )
    {
        m_view->MustBeResized();
    }
}
//鼠标按下事件//
void OCCWidget::mousePressEvent(QMouseEvent *event)
{
    if(event->button()==Qt::RightButton)//右键//
    {
        m_view->StartRotation(event->x(),event->y());//视图旋转//
    }
    else if(event->button()==Qt::LeftButton)//左键//
    {
        m_context->MoveTo(event->pos().x(),event->pos().y(),m_view,Standard_True);//上下文移动到单击点//
        startPnt = event->pos();//起点//


        if(!m_manipulator->IsAttached())//操作器未附加//
            m_rubberBand->setGeometry(QRect(startPnt,QSize(1,1)));//框选1像素区域//
        else
            m_manipulator->StartTransform(event->pos().x(),event->pos().y(),m_view);//平移操作器//
    }
    else if(event->button() == Qt::MidButton)//中键//
    {   //记录中键位置//
        midBtn_x=event->x();
        midBtn_y=event->y();
    }
}
//鼠标释放//
void OCCWidget::mouseReleaseEvent(QMouseEvent *event)
{
    unsetCursor();
    m_context->MoveTo(event->pos().x(),event->pos().y(),m_view,Standard_True);   //上下文移动//


    if(event->button()==Qt::LeftButton)//左键//
    {
        if(!m_rubberBand->isHidden()&&!m_manipulator->IsAttached())//框选未隐藏或者操作器未附加//
            m_rubberBand->hide();//隐藏框选器//


        if(m_manipulator->IsAttached())//操作器附加，停止平移//
            m_manipulator->StopTransform();


        if(event->pos()==startPnt)//释放位置在起点//
        {
            AIS_StatusOfPick t_pick_status = AIS_SOP_NothingSelected;//啥都没选中//
            if(qApp->keyboardModifiers()==Qt::ControlModifier)//ctrl按键//
            {
                t_pick_status = m_context->ShiftSelect(true);// 多选 //
            }
            else
            {
                t_pick_status = m_context->Select(true);   //选中//
            }


            if(t_pick_status == AIS_SOP_OneSelected || t_pick_status == AIS_SOP_SeveralSelected)//单选或者多选//
            {
                emit selectShapeChanged(m_context->SelectedShape());//发送选择形状改变信号：参数-选择的形状//
                m_context-> InitSelected();//初始化选择//
            }


            emit pickPixel(event->x(),event->y());//发送拣选像素信号：参数 位置x,y//
        }
        else//
        {
            m_context->Select(startPnt.x(),startPnt.y(),event->x(),event->y(),m_view,Standard_True);//选择区域//
            m_context-> InitSelected();
        }
    }
}
//鼠标移动事件//
void OCCWidget::mouseMoveEvent(QMouseEvent *event)
{
    if(event->buttons()&Qt::RightButton)//右键//
    {
        setCursor(Qt::ClosedHandCursor);//设置鼠标形状//
        m_view->Rotation(event->x(),event->y());//视图旋转//
    }
    else if(event->buttons()&Qt::MidButton)//中键//
    {
        setCursor(Qt::SizeAllCursor);//
        m_view->Pan(event->pos().x()-midBtn_x,midBtn_y-event->pos().y());//zoom//
        midBtn_x=event->x();//更新中键位置//
        midBtn_y=event->y();
    }
    else if(event->buttons()&Qt::LeftButton)//左键移动//
    {
        if(!m_manipulator->IsAttached())//操作器未附加//
        {
            m_rubberBand->show();//框选//
            m_rubberBand->setGeometry(QRect(startPnt,event->pos()).normalized());
        }
        else//移动操作器//
        {
            m_manipulator->Transform(event->pos().x(),event->pos().y(),m_view);
            m_view->Redraw();
        }
    }
    else//未按下移动//
    {
        m_context->MoveTo(event->pos().x(),event->pos().y(),m_view,Standard_True);
    }
}
//滚轮事件zoom//
void OCCWidget::wheelEvent(QWheelEvent *event)
{
    m_view->StartZoomAtPoint(event->pos().x(),event->pos().y());
    m_view->ZoomAtPoint(0, 0, event->angleDelta().y(), 0);
}
//绘制引擎//
QPaintEngine *OCCWidget::paintEngine() const
{
    return nullptr;
}

规划线程：

#include "RLAPI_PlanThread.h"
#include <QDebug>


bool RLAPI_PlanThread::PlannerSolved = false;


//=======================================================================
//function : RLAPI_PlanThread
//purpose  : * 规划线程构造函数
//=======================================================================
RLAPI_PlanThread::RLAPI_PlanThread(const rl::mdl::Dynamic &aDynamic, rl::sg::pqp::Scene &aScene, const double &minModelSize,
                                   QObject *parent) : QThread(parent),minMdlSize(minModelSize)
{
    mDynamic = new rl::mdl::Dynamic(aDynamic);//动态对象
    mPlanScene = &aScene;//指向主线程中的场景point to the scene in the main thread
}


//=======================================================================
//function : RLAPI_PlanThread
//purpose  : * 规划线程析构函数//
//=======================================================================
RLAPI_PlanThread::~RLAPI_PlanThread()
{
    delete mDynamic;
    mDynamic = nullptr;
    mPlanScene = nullptr;//if delete here, pointer in the main thread will be held on
}


//=======================================================================
//function : GetComputeArguments
//purpose  : * 获取计算参数  设置起点和终点//
//=======================================================================
void RLAPI_PlanThread::GetComputeArguments(const rl::math::Vector &startPnt, const rl::math::Vector &endPnt)
{
    mStartPnt = startPnt;
    mEndPnt = endPnt;
}


//=======================================================================
//function : pause
//purpose  : * 暂停线程//
//=======================================================================
void RLAPI_PlanThread::pause()
{
    if(isRunning())
        doCollisionWait = true;//碰撞等待=true//
}


//=======================================================================
//function : resume
//purpose  : * 恢复线程//
//=======================================================================
void RLAPI_PlanThread::resume()
{
    if(isRunning())
    {
        doCollisionWait = false;//碰撞等待=false//
        waitCondition.wakeAll();
    }
}


//=======================================================================
//function : run
//purpose  : *  路径规划 并发送准备更新关节位置信号// https://www.roboticslibrary.org/tutorials/
//=======================================================================
void RLAPI_PlanThread::run()
{
    rl::plan::SimpleModel *mPlanModel = new rl::plan::SimpleModel();//新建规划模型-简单模型//
    rl::plan::AdvancedOptimizer *mPathOptimizer = new rl::plan::AdvancedOptimizer;//高级优化器//
    rl::plan::GnatNearestNeighbors nearestNeighbors(mPlanModel);//获取最近邻//
    rl::plan::Prm planner;//规划器//http://doc.roboticslibrary.org/0.7.0/d7/d24/classrl_1_1plan_1_1_prm.html
    rl::plan::UniformSampler sampler;//采样器//
    rl::plan::RecursiveVerifier verifier;//递归的验证器//


    mPlanModel->mdl = mDynamic;//设置规划模型的 mdl//
    mPlanModel->model = mPlanScene->getModel(0);//设置规划模型的运动学模型//
    mPlanModel->scene = mPlanScene;//设置规划模型的场景//


    verifier.delta = 10.0f * rl::math::constants::deg2rad;//验证器 增量10度//
    verifier.model = mPlanModel;//验证器的模型：规划模型//


    mPathOptimizer->model = mPlanModel;//路径优化器的模型--规划模型//
    mPathOptimizer->verifier = &verifier;//路径优化器的  验证器//
    mPathOptimizer->ratio = 0.01;//
    mPathOptimizer->length = 0.2*minMdlSize;


    sampler.model = mPlanModel;//采样器的模型-规划模型//


    planner.start = &mStartPnt;//规划器起点关节配置//
    planner.goal = &mEndPnt;//规划器终点关节配置//
    planner.model = mPlanModel;//规划器的模型-规划模型//
    planner.verifier = &verifier;//规划器的验证器//
    planner.setNearestNeighbors(&nearestNeighbors);//设置规划器的最近邻//
    planner.sampler = &sampler;//设置规划器的采样器//


    //verify the start and the goal.if there is an invalid value ,thread will be crashed
    //验证开始和目标。如果有一个无效的值，线程将崩溃//
    if(!planner.verify())
    {
        emit ComputeFailed(ComputeError::InvalidConfig);//计算失败//
        return;
    }


    PlannerSolved = false;
    QTimer aComputeTimer;//计算超时定时器//
    aComputeTimer.singleShot(8000,this,[=](){
        if(!PlannerSolved)//检查未求解//
            emit ComputeTimeOut();//发送超时信号//
        return;
    });


    QElapsedTimer aElapsedTimer;
    aElapsedTimer.start();//计算时间定时器//


    PlannerSolved = planner.solve();//求解//


    if(PlannerSolved)//找到解//
    {
        emit ComputeSuccess(aElapsedTimer.elapsed());//计算成功//


        rl::plan::VectorList solvedPath = planner.getPath();//获取求解的路径//


        verifier.delta *= 2;
        mPathOptimizer->process(solvedPath);//优化求解路径//
        qDebug()<<"compute+optimize "<<aElapsedTimer.elapsed();//输出计算时间//


        rl::math::Vector tmpPos(mPlanModel->getDofPosition());//获取tcp位姿参数//


        rl::plan::VectorList::iterator i = solvedPath.begin();
        rl::plan::VectorList::iterator j = ++solvedPath.begin();


        //路径插值//
        //1、添加起点  add the start
        if (i != solvedPath.end() && j != solvedPath.end())//遍历求解的优化路径//
        {
            emit ReadyToSetJointValue(*i);//发送准备设置关节位置信号//
            QThread::usleep(50000);//等待50ms//
        }


        rl::math::Real delta = 10.0f;
        double pathLength = 0;


        //2、插值 add the interpolations until the goal
        for (; i != solvedPath.end() && j != solvedPath.end(); ++i, ++j)
        {
            pathLength += mPlanModel->distance(*i ,*j);//计算路径段距离//
            rl::math::Real stepLength = std::ceil(mPlanModel->distance(*i, *j) / delta);   //插值步数//


            for (std::size_t k = 1; k < stepLength + 1; ++k)
            {
                if(doCollisionWait)//碰撞等待
                {
                    aMutex.lock();
                    waitCondition.wait(&aMutex);//等待条件满足//
                    aMutex.unlock();
                }
                mPlanModel->interpolate(*i, *j, k/stepLength, tmpPos);//插值tmpPos//
                emit ReadyToSetJointValue(tmpPos);//发送准备设置关节位置信号//
                QThread::usleep(50000);//等待50ms//
            }
        }
        PlannerSolved = false;//恢复为未找到解状态//
        emit ComputeOver(pathLength);//计算完毕//
    }
    else//没找到解//
    {
        emit ComputeFailed(ComputeError::ArgumentError);
        return;
    }
    delete mPlanModel;
    delete mPathOptimizer;
}

The End