ViewProviderRobotObject.cpp

Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (c) 2008 Jürgen Riegel (juergen.riegel@web.de)              *
00003  *                                                                         *
00004  *   This file is part of the FreeCAD CAx development system.              *
00005  *                                                                         *
00006  *   This library is free software; you can redistribute it and/or         *
00007  *   modify it under the terms of the GNU Library General Public           *
00008  *   License as published by the Free Software Foundation; either          *
00009  *   version 2 of the License, or (at your option) any later version.      *
00010  *                                                                         *
00011  *   This library  is distributed in the hope that it will be useful,      *
00012  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00013  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
00014  *   GNU Library General Public License for more details.                  *
00015  *                                                                         *
00016  *   You should have received a copy of the GNU Library General Public     *
00017  *   License along with this library; see the file COPYING.LIB. If not,    *
00018  *   write to the Free Software Foundation, Inc., 59 Temple Place,         *
00019  *   Suite 330, Boston, MA  02111-1307, USA                                *
00020  *                                                                         *
00021  ***************************************************************************/
00022 
00023 
00024 #include "PreCompiled.h"
00025 
00026 #ifndef _PreComp_
00027 # include <Inventor/SoDB.h>
00028 # include <Inventor/SoInput.h>
00029 # include <Inventor/SbVec3f.h>
00030 # include <Inventor/nodes/SoSeparator.h>
00031 # include <Inventor/nodes/SoTransform.h>
00032 # include <Inventor/nodes/SoSphere.h>
00033 # include <Inventor/nodes/SoRotation.h>
00034 # include <Inventor/actions/SoSearchAction.h>
00035 # include <Inventor/draggers/SoJackDragger.h>
00036 # include <Inventor/draggers/SoTrackballDragger.h>
00037 # include <Inventor/VRMLnodes/SoVRMLTransform.h>
00038 # include <QFile>
00039 #endif
00040 
00041 #include "ViewProviderRobotObject.h"
00042 
00043 #include <Mod/Robot/App/RobotObject.h>
00044 #include <Mod/Part/App/PartFeature.h>
00045 #include <Mod/Part/Gui/ViewProvider.h>
00046 #include <App/Document.h>
00047 #include <App/VRMLObject.h>
00048 #include <Gui/Application.h>
00049 #include <Base/FileInfo.h>
00050 #include <Base/Stream.h>
00051 #include <Base/Console.h>
00052 #include <sstream>
00053 using namespace Gui;
00054 using namespace RobotGui;
00055 
00056 PROPERTY_SOURCE(RobotGui::ViewProviderRobotObject, Gui::ViewProviderGeometryObject)
00057 
00058 ViewProviderRobotObject::ViewProviderRobotObject()
00059   : pcDragger(0),toolShape(0)
00060 {
00061     ADD_PROPERTY(Manipulator,(0));   
00062 
00063         pcRobotRoot = new Gui::SoFCSelection();
00064     pcRobotRoot->highlightMode = Gui::SoFCSelection::OFF;
00065     //pcRobotRoot->selectionMode = Gui::SoFCSelection::SEL_OFF;
00066     //pcRobotRoot->style = Gui::SoFCSelection::BOX;
00067     pcRobotRoot->ref();
00068 
00069         pcSimpleRoot = new Gui::SoFCSelection();
00070     pcSimpleRoot->highlightMode = Gui::SoFCSelection::OFF;
00071     //pcSimpleRoot->selectionMode = Gui::SoFCSelection::SEL_OFF;
00072     pcSimpleRoot->ref();
00073 
00074     pcOffRoot = new SoGroup();
00075     pcOffRoot->ref();
00076 
00077     // set nodes for the manipulator outfit
00078     pcTcpRoot = new SoGroup();
00079     pcTcpRoot->ref();
00080 
00081 
00082     Axis1Node = Axis2Node = Axis3Node = Axis4Node = Axis5Node = Axis6Node = 0;
00083 }
00084 
00085 void ViewProviderRobotObject::setDragger()
00086 {
00087     assert(pcDragger==0);
00088     pcDragger = new SoJackDragger();
00089     pcDragger->addMotionCallback(sDraggerMotionCallback,this);
00090     pcTcpRoot->addChild(pcDragger);
00091 
00092     // set the actual TCP position
00093     Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
00094     Base::Placement loc = robObj->Tcp.getValue();
00095     SbMatrix  M;
00096     M.setTransform(SbVec3f(loc.getPosition().x,loc.getPosition().y,loc.getPosition().z),
00097                    SbRotation(loc.getRotation()[0],loc.getRotation()[1],loc.getRotation()[2],loc.getRotation()[3]),
00098                    SbVec3f(150,150,150)
00099                    );
00100     pcDragger->setMotionMatrix(M);
00101 
00102 
00103 }
00104 void ViewProviderRobotObject::resetDragger()
00105 {
00106     assert(pcDragger);
00107     pcTcpRoot->removeAllChildren();
00108     pcDragger = 0;
00109 
00110  }
00111 
00112 ViewProviderRobotObject::~ViewProviderRobotObject()
00113 {
00114     pcRobotRoot->unref();
00115     pcSimpleRoot->unref();
00116     pcOffRoot->unref();
00117 
00118 }
00119 
00120 void ViewProviderRobotObject::attach(App::DocumentObject *pcObj)
00121 {
00122     ViewProviderGeometryObject::attach(pcObj);
00123 
00124     addDisplayMaskMode(pcRobotRoot, "VRML");
00125     pcRobotRoot->objectName = pcObj->getNameInDocument();
00126     pcRobotRoot->documentName = pcObj->getDocument()->getName();
00127     pcRobotRoot->subElementName = "Main";
00128     pcRobotRoot->addChild(pcTcpRoot);
00129 
00130     addDisplayMaskMode(pcSimpleRoot, "Simple");
00131     pcSimpleRoot->objectName = pcObj->getNameInDocument();
00132     pcSimpleRoot->documentName = pcObj->getDocument()->getName();
00133     pcSimpleRoot->subElementName = "Main";
00134     pcSimpleRoot->addChild(pcTcpRoot);
00135 
00136     addDisplayMaskMode(pcOffRoot, "Off");
00137     pcOffRoot->addChild(pcTcpRoot);
00138 
00139 
00140 }
00141 
00142 void ViewProviderRobotObject::setDisplayMode(const char* ModeName)
00143 {
00144     if ( strcmp("VRML",ModeName)==0 )
00145         setDisplayMaskMode("VRML");
00146     if ( strcmp("Simple",ModeName)==0 )
00147         setDisplayMaskMode("Simple");
00148     if ( strcmp("Off",ModeName)==0 )
00149         setDisplayMaskMode("Off");
00150     ViewProviderGeometryObject::setDisplayMode( ModeName );
00151 }
00152 
00153 std::vector<std::string> ViewProviderRobotObject::getDisplayModes(void) const
00154 {
00155     std::vector<std::string> StrList;
00156     StrList.push_back("VRML");
00157     StrList.push_back("Simple");
00158     StrList.push_back("Off");
00159     return StrList;
00160 }
00161 
00162 void ViewProviderRobotObject::onChanged(const App::Property* prop)
00163 {
00164     if (prop == &Manipulator) {
00165         if (Manipulator.getValue()) {
00166             if (!this->pcDragger)
00167                 this->setDragger();
00168         }
00169         else {
00170             if (this->pcDragger)
00171                 this->resetDragger();
00172         }
00173     }
00174     else {
00175         ViewProviderGeometryObject::onChanged(prop);
00176     }
00177 }
00178 
00179 void ViewProviderRobotObject::updateData(const App::Property* prop)
00180 {
00181     Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
00182     if (prop == &robObj->RobotVrmlFile) {
00183         // read also from file
00184         const char* filename = robObj->RobotVrmlFile.getValue();
00185         QString fn = QString::fromUtf8(filename);
00186         QFile file(fn);
00187         SoInput in;
00188         pcRobotRoot->removeAllChildren();
00189         if (!fn.isEmpty() && file.open(QFile::ReadOnly)) {
00190             QByteArray buffer = file.readAll();
00191             in.setBuffer((void *)buffer.constData(), buffer.length());
00192             SoSeparator * node = SoDB::readAll(&in);
00193             if (node) pcRobotRoot->addChild(node);
00194             pcRobotRoot->addChild(pcTcpRoot);
00195         }
00196                 // search for the conection points +++++++++++++++++++++++++++++++++++++++++++++++++
00197                 Axis1Node = Axis2Node = Axis3Node = Axis4Node = Axis5Node = Axis6Node = 0;
00198                 SoSearchAction searchAction;
00199                 SoPath * path;
00200 
00201                 // Axis 1
00202                 searchAction.setName("FREECAD_AXIS1");
00203                 searchAction.setInterest(SoSearchAction::FIRST);
00204                 searchAction.setSearchingAll(FALSE);
00205                 searchAction.apply(pcRobotRoot);
00206                 path = searchAction.getPath();
00207                 if(path){
00208                         SoNode* node = path->getTail();
00209                         std::string typeName = (const char*)node->getTypeId().getName();
00210                         if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
00211                                 throw; // should not happen
00212                         Axis1Node = static_cast<SoVRMLTransform *>(node);
00213                 }
00214                 // Axis 2
00215                 searchAction.setName("FREECAD_AXIS2");
00216                 searchAction.setInterest(SoSearchAction::FIRST);
00217                 searchAction.setSearchingAll(FALSE);
00218                 searchAction.apply(pcRobotRoot);
00219                 path = searchAction.getPath();
00220                 if(path){
00221                         SoNode* node = path->getTail();
00222                         std::string typeName = (const char*)node->getTypeId().getName();
00223                         if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
00224                                 throw; // should not happen
00225                         Axis2Node = static_cast<SoVRMLTransform *>(node);
00226                 }
00227                 // Axis 3
00228                 searchAction.setName("FREECAD_AXIS3");
00229                 searchAction.setInterest(SoSearchAction::FIRST);
00230                 searchAction.setSearchingAll(FALSE);
00231                 searchAction.apply(pcRobotRoot);
00232                 path = searchAction.getPath();
00233                 if(path){
00234                         SoNode* node = path->getTail();
00235                         std::string typeName = (const char*)node->getTypeId().getName();
00236                         if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
00237                                 throw; // should not happen
00238                         Axis3Node = static_cast<SoVRMLTransform *>(node);
00239                 }
00240                 // Axis 4
00241                 searchAction.setName("FREECAD_AXIS4");
00242                 searchAction.setInterest(SoSearchAction::FIRST);
00243                 searchAction.setSearchingAll(FALSE);
00244                 searchAction.apply(pcRobotRoot);
00245                 path = searchAction.getPath();
00246                 if(path){
00247                         SoNode* node = path->getTail();
00248                         std::string typeName = (const char*)node->getTypeId().getName();
00249                         if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
00250                                 throw; // should not happen
00251                         Axis4Node = static_cast<SoVRMLTransform *>(node);
00252                 }
00253                 // Axis 5
00254                 searchAction.setName("FREECAD_AXIS5");
00255                 searchAction.setInterest(SoSearchAction::FIRST);
00256                 searchAction.setSearchingAll(FALSE);
00257                 searchAction.apply(pcRobotRoot);
00258                 path = searchAction.getPath();
00259                 if(path){
00260                         SoNode* node = path->getTail();
00261                         std::string typeName = (const char*)node->getTypeId().getName();
00262                         if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
00263                                 throw; // should not happen
00264                         Axis5Node = static_cast<SoVRMLTransform *>(node);
00265                 }
00266                 // Axis 6
00267                 searchAction.setName("FREECAD_AXIS6");
00268                 searchAction.setInterest(SoSearchAction::FIRST);
00269                 searchAction.setSearchingAll(FALSE);
00270                 searchAction.apply(pcRobotRoot);
00271                 path = searchAction.getPath();
00272                 if(path){
00273                         SoNode* node = path->getTail();
00274                         std::string typeName = (const char*)node->getTypeId().getName();
00275                         if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
00276                                 throw; // should not happen
00277                         Axis6Node = static_cast<SoVRMLTransform *>(node);
00278                 }
00279                 if(Axis1Node)
00280                         Axis1Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis1.getValue()*(M_PI/180));
00281                 if(Axis2Node)
00282                         Axis2Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis2.getValue()*(M_PI/180));
00283                 if(Axis3Node)
00284                         Axis3Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis3.getValue()*(M_PI/180));
00285                 if(Axis4Node)
00286                         Axis4Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis4.getValue()*(M_PI/180));
00287                 if(Axis5Node)
00288                         Axis5Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis5.getValue()*(M_PI/180));
00289                 if(Axis6Node)
00290                         Axis6Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis6.getValue()*(M_PI/180));
00291     }else if (prop == &robObj->Axis1) {
00292         if(Axis1Node){
00293                         Axis1Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis1.getValue()*(M_PI/180));
00294             if(toolShape)
00295                 toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00296         }
00297     }else if (prop == &robObj->Axis2) {
00298         if(Axis2Node){
00299                         Axis2Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis2.getValue()*(M_PI/180));
00300             if(toolShape)
00301                 toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00302         }
00303     }else if (prop == &robObj->Axis3) {
00304         if(Axis3Node){
00305                         Axis3Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis3.getValue()*(M_PI/180));
00306             if(toolShape)
00307                 toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00308         }
00309     }else if (prop == &robObj->Axis4) {
00310         if(Axis4Node){
00311                         Axis4Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis4.getValue()*(M_PI/180));
00312             if(toolShape)
00313                 toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00314         }
00315     }else if (prop == &robObj->Axis5) {
00316         if(Axis5Node){
00317                         Axis5Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis5.getValue()*(M_PI/180));
00318             if(toolShape)
00319                 toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00320         }
00321     }else if (prop == &robObj->Axis6) {
00322         if(Axis6Node){
00323                         Axis6Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis6.getValue()*(M_PI/180));
00324             if(toolShape)
00325                 toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00326         }
00327         }else if (prop == &robObj->Tcp) {
00328         Base::Placement loc = robObj->Tcp.getValue();
00329         SbMatrix  M;
00330         M.setTransform(SbVec3f(loc.getPosition().x,loc.getPosition().y,loc.getPosition().z),
00331                        SbRotation(loc.getRotation()[0],loc.getRotation()[1],loc.getRotation()[2],loc.getRotation()[3]),
00332                        SbVec3f(150,150,150)
00333                        );
00334         if(pcDragger)
00335             pcDragger->setMotionMatrix(M);
00336         if(toolShape)
00337             toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00338                 //pcTcpTransform->translation = SbVec3f(loc.getPosition().x,loc.getPosition().y,loc.getPosition().z);
00339                 //pcTcpTransform->rotation = SbRotation(loc.getRotation()[0],loc.getRotation()[1],loc.getRotation()[2],loc.getRotation()[3]);
00340         }else if (prop == &robObj->ToolShape) {
00341         App::DocumentObject* o = robObj->ToolShape.getValue<App::DocumentObject*>();
00342 
00343         if(o && (o->isDerivedFrom(Part::Feature::getClassTypeId()) || o->isDerivedFrom(App::VRMLObject::getClassTypeId())) ){
00344             //Part::Feature *p = dynamic_cast<Part::Feature *>(o);
00345             toolShape = Gui::Application::Instance->getViewProvider(o);
00346             toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
00347         }else
00348             toolShape = 0;
00349         }
00350 
00351 }
00352 void ViewProviderRobotObject::setAxisTo(float A1,float A2,float A3,float A4,float A5,float A6,const Base::Placement &Tcp)
00353 {
00354     Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
00355 
00356         if(Axis1Node)
00357         // FIXME Uggly hack for the wrong transformation of the Kuka 500 robot VRML the minus sign on Axis 1
00358                 Axis1Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A1*(M_PI/180));
00359         if(Axis2Node)
00360                 Axis2Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A2*(M_PI/180));
00361         if(Axis3Node)
00362                 Axis3Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A3*(M_PI/180));
00363         if(Axis4Node)
00364                 Axis4Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A4*(M_PI/180));
00365         if(Axis5Node)
00366                 Axis5Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A5*(M_PI/180));
00367         if(Axis6Node)
00368                 Axis6Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A6*(M_PI/180));
00369     // update tool position
00370     if(toolShape)
00371         toolShape->setTransformation((Tcp * (robObj->ToolBase.getValue().inverse())).toMatrix());
00372 }
00373 
00374 void ViewProviderRobotObject::sDraggerMotionCallback(void *data, SoDragger *dragger)
00375 {
00376     static_cast<ViewProviderRobotObject*>(data)->DraggerMotionCallback(dragger);
00377 }
00378 
00379 void ViewProviderRobotObject::DraggerMotionCallback(SoDragger *dragger)
00380 {
00381     float q0, q1, q2, q3;
00382 
00383     Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
00384     Base::Placement Tcp = robObj->Tcp.getValue();
00385     const SbMatrix & M = dragger->getMotionMatrix ();
00386     SbVec3f    translation;
00387     SbRotation rotation;
00388     SbVec3f    scaleFactor;
00389     SbRotation scaleOrientation;
00390     SbVec3f    center(Tcp.getPosition().x,Tcp.getPosition().y,Tcp.getPosition().z);
00391     M.getTransform(translation, rotation, scaleFactor, scaleOrientation);
00392     rotation.getValue(q0, q1, q2, q3);
00393     //Base::Console().Message("M %f %f %f\n", M[3][0], M[3][1], M[3][2]);
00394     Base::Rotation rot(q0, q1, q2, q3);
00395     Base::Vector3d pos(translation[0],translation[1],translation[2]);
00396     robObj->Tcp.setValue(Base::Placement(pos,rot));
00397 }
00398 

Generated on Wed Nov 23 19:01:01 2011 for FreeCAD by  doxygen 1.6.1