kindred/create
1
1
Fork 0
Code Issues 37 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity
Files
3a8f6dff5993e548cedf12cd66ecb688572848a8
create/src/Mod/Robot/Gui/ViewProviderRobotObject.cpp
wmayer 120ca87015 + unify DLL export defines to namespace names 
git-svn-id: https://free-cad.svn.sourceforge.net/svnroot/free-cad/trunk@5000 e8eeb9e2-ec13-0410-a4a9-efa5cf37419d
2011-10-10 13:44:52 +00:00

399 lines
16 KiB
C++
Raw Blame History

/***************************************************************************
* Copyright (c) 2008 Jürgen Riegel (juergen.riegel@web.de) *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/
#include "PreCompiled.h"
#ifndef _PreComp_
# include <Inventor/SoDB.h>
# include <Inventor/SoInput.h>
# include <Inventor/SbVec3f.h>
# include <Inventor/nodes/SoSeparator.h>
# include <Inventor/nodes/SoTransform.h>
# include <Inventor/nodes/SoSphere.h>
# include <Inventor/nodes/SoRotation.h>
# include <Inventor/actions/SoSearchAction.h>
# include <Inventor/draggers/SoJackDragger.h>
# include <Inventor/draggers/SoTrackballDragger.h>
# include <Inventor/VRMLnodes/SoVRMLTransform.h>
# include <QFile>
#endif
#include "ViewProviderRobotObject.h"
#include <Mod/Robot/App/RobotObject.h>
#include <Mod/Part/App/PartFeature.h>
#include <Mod/Part/Gui/ViewProvider.h>
#include <App/Document.h>
#include <App/VRMLObject.h>
#include <Gui/Application.h>
#include <Base/FileInfo.h>
#include <Base/Stream.h>
#include <Base/Console.h>
#include <sstream>
using namespace Gui;
using namespace RobotGui;
PROPERTY_SOURCE(RobotGui::ViewProviderRobotObject, Gui::ViewProviderGeometryObject)
ViewProviderRobotObject::ViewProviderRobotObject()
: pcDragger(0),toolShape(0)
{
ADD_PROPERTY(Manipulator,(0));
pcRobotRoot = new Gui::SoFCSelection();
pcRobotRoot->highlightMode = Gui::SoFCSelection::OFF;
//pcRobotRoot->selectionMode = Gui::SoFCSelection::SEL_OFF;
//pcRobotRoot->style = Gui::SoFCSelection::BOX;
pcRobotRoot->ref();
pcSimpleRoot = new Gui::SoFCSelection();
pcSimpleRoot->highlightMode = Gui::SoFCSelection::OFF;
//pcSimpleRoot->selectionMode = Gui::SoFCSelection::SEL_OFF;
pcSimpleRoot->ref();
pcOffRoot = new SoGroup();
pcOffRoot->ref();
// set nodes for the manipulator outfit
pcTcpRoot = new SoGroup();
pcTcpRoot->ref();
Axis1Node = Axis2Node = Axis3Node = Axis4Node = Axis5Node = Axis6Node = 0;
}
void ViewProviderRobotObject::setDragger()
{
assert(pcDragger==0);
pcDragger = new SoJackDragger();
pcDragger->addMotionCallback(sDraggerMotionCallback,this);
pcTcpRoot->addChild(pcDragger);
// set the actual TCP position
Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
Base::Placement loc = robObj->Tcp.getValue();
SbMatrix M;
M.setTransform(SbVec3f(loc.getPosition().x,loc.getPosition().y,loc.getPosition().z),
SbRotation(loc.getRotation()[0],loc.getRotation()[1],loc.getRotation()[2],loc.getRotation()[3]),
SbVec3f(150,150,150)
);
pcDragger->setMotionMatrix(M);
}
void ViewProviderRobotObject::resetDragger()
{
assert(pcDragger);
pcTcpRoot->removeAllChildren();
pcDragger = 0;
}
ViewProviderRobotObject::~ViewProviderRobotObject()
{
pcRobotRoot->unref();
pcSimpleRoot->unref();
pcOffRoot->unref();
}
void ViewProviderRobotObject::attach(App::DocumentObject *pcObj)
{
ViewProviderGeometryObject::attach(pcObj);
addDisplayMaskMode(pcRobotRoot, "VRML");
pcRobotRoot->objectName = pcObj->getNameInDocument();
pcRobotRoot->documentName = pcObj->getDocument()->getName();
pcRobotRoot->subElementName = "Main";
pcRobotRoot->addChild(pcTcpRoot);
addDisplayMaskMode(pcSimpleRoot, "Simple");
pcSimpleRoot->objectName = pcObj->getNameInDocument();
pcSimpleRoot->documentName = pcObj->getDocument()->getName();
pcSimpleRoot->subElementName = "Main";
pcSimpleRoot->addChild(pcTcpRoot);
addDisplayMaskMode(pcOffRoot, "Off");
pcOffRoot->addChild(pcTcpRoot);
}
void ViewProviderRobotObject::setDisplayMode(const char* ModeName)
{
if ( strcmp("VRML",ModeName)==0 )
setDisplayMaskMode("VRML");
if ( strcmp("Simple",ModeName)==0 )
setDisplayMaskMode("Simple");
if ( strcmp("Off",ModeName)==0 )
setDisplayMaskMode("Off");
ViewProviderGeometryObject::setDisplayMode( ModeName );
}
std::vector<std::string> ViewProviderRobotObject::getDisplayModes(void) const
{
std::vector<std::string> StrList;
StrList.push_back("VRML");
StrList.push_back("Simple");
StrList.push_back("Off");
return StrList;
}
void ViewProviderRobotObject::onChanged(const App::Property* prop)
{
if (prop == &Manipulator) {
if (Manipulator.getValue()) {
if (!this->pcDragger)
this->setDragger();
}
else {
if (this->pcDragger)
this->resetDragger();
}
}
else {
ViewProviderGeometryObject::onChanged(prop);
}
}
void ViewProviderRobotObject::updateData(const App::Property* prop)
{
Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
if (prop == &robObj->RobotVrmlFile) {
// read also from file
const char* filename = robObj->RobotVrmlFile.getValue();
QString fn = QString::fromUtf8(filename);
QFile file(fn);
SoInput in;
pcRobotRoot->removeAllChildren();
if (!fn.isEmpty() && file.open(QFile::ReadOnly)) {
QByteArray buffer = file.readAll();
in.setBuffer((void *)buffer.constData(), buffer.length());
SoSeparator * node = SoDB::readAll(&in);
if (node) pcRobotRoot->addChild(node);
pcRobotRoot->addChild(pcTcpRoot);
}
// search for the conection points +++++++++++++++++++++++++++++++++++++++++++++++++
Axis1Node = Axis2Node = Axis3Node = Axis4Node = Axis5Node = Axis6Node = 0;
SoSearchAction searchAction;
SoPath * path;
// Axis 1
searchAction.setName("FREECAD_AXIS1");
searchAction.setInterest(SoSearchAction::FIRST);
searchAction.setSearchingAll(FALSE);
searchAction.apply(pcRobotRoot);
path = searchAction.getPath();
if(path){
SoNode* node = path->getTail();
std::string typeName = (const char*)node->getTypeId().getName();
if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
throw; // should not happen
Axis1Node = static_cast<SoVRMLTransform *>(node);
}
// Axis 2
searchAction.setName("FREECAD_AXIS2");
searchAction.setInterest(SoSearchAction::FIRST);
searchAction.setSearchingAll(FALSE);
searchAction.apply(pcRobotRoot);
path = searchAction.getPath();
if(path){
SoNode* node = path->getTail();
std::string typeName = (const char*)node->getTypeId().getName();
if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
throw; // should not happen
Axis2Node = static_cast<SoVRMLTransform *>(node);
}
// Axis 3
searchAction.setName("FREECAD_AXIS3");
searchAction.setInterest(SoSearchAction::FIRST);
searchAction.setSearchingAll(FALSE);
searchAction.apply(pcRobotRoot);
path = searchAction.getPath();
if(path){
SoNode* node = path->getTail();
std::string typeName = (const char*)node->getTypeId().getName();
if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
throw; // should not happen
Axis3Node = static_cast<SoVRMLTransform *>(node);
}
// Axis 4
searchAction.setName("FREECAD_AXIS4");
searchAction.setInterest(SoSearchAction::FIRST);
searchAction.setSearchingAll(FALSE);
searchAction.apply(pcRobotRoot);
path = searchAction.getPath();
if(path){
SoNode* node = path->getTail();
std::string typeName = (const char*)node->getTypeId().getName();
if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
throw; // should not happen
Axis4Node = static_cast<SoVRMLTransform *>(node);
}
// Axis 5
searchAction.setName("FREECAD_AXIS5");
searchAction.setInterest(SoSearchAction::FIRST);
searchAction.setSearchingAll(FALSE);
searchAction.apply(pcRobotRoot);
path = searchAction.getPath();
if(path){
SoNode* node = path->getTail();
std::string typeName = (const char*)node->getTypeId().getName();
if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
throw; // should not happen
Axis5Node = static_cast<SoVRMLTransform *>(node);
}
// Axis 6
searchAction.setName("FREECAD_AXIS6");
searchAction.setInterest(SoSearchAction::FIRST);
searchAction.setSearchingAll(FALSE);
searchAction.apply(pcRobotRoot);
path = searchAction.getPath();
if(path){
SoNode* node = path->getTail();
std::string typeName = (const char*)node->getTypeId().getName();
if (!node || node->getTypeId() != SoVRMLTransform::getClassTypeId())
throw; // should not happen
Axis6Node = static_cast<SoVRMLTransform *>(node);
}
if(Axis1Node)
Axis1Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis1.getValue()*(M_PI/180));
if(Axis2Node)
Axis2Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis2.getValue()*(M_PI/180));
if(Axis3Node)
Axis3Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis3.getValue()*(M_PI/180));
if(Axis4Node)
Axis4Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis4.getValue()*(M_PI/180));
if(Axis5Node)
Axis5Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis5.getValue()*(M_PI/180));
if(Axis6Node)
Axis6Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis6.getValue()*(M_PI/180));
}else if (prop == &robObj->Axis1) {
if(Axis1Node){
Axis1Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis1.getValue()*(M_PI/180));
if(toolShape)
toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
}
}else if (prop == &robObj->Axis2) {
if(Axis2Node){
Axis2Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis2.getValue()*(M_PI/180));
if(toolShape)
toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
}
}else if (prop == &robObj->Axis3) {
if(Axis3Node){
Axis3Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis3.getValue()*(M_PI/180));
if(toolShape)
toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
}
}else if (prop == &robObj->Axis4) {
if(Axis4Node){
Axis4Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis4.getValue()*(M_PI/180));
if(toolShape)
toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
}
}else if (prop == &robObj->Axis5) {
if(Axis5Node){
Axis5Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis5.getValue()*(M_PI/180));
if(toolShape)
toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
}
}else if (prop == &robObj->Axis6) {
if(Axis6Node){
Axis6Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),robObj->Axis6.getValue()*(M_PI/180));
if(toolShape)
toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
}
}else if (prop == &robObj->Tcp) {
Base::Placement loc = robObj->Tcp.getValue();
SbMatrix M;
M.setTransform(SbVec3f(loc.getPosition().x,loc.getPosition().y,loc.getPosition().z),
SbRotation(loc.getRotation()[0],loc.getRotation()[1],loc.getRotation()[2],loc.getRotation()[3]),
SbVec3f(150,150,150)
);
if(pcDragger)
pcDragger->setMotionMatrix(M);
if(toolShape)
toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
//pcTcpTransform->translation = SbVec3f(loc.getPosition().x,loc.getPosition().y,loc.getPosition().z);
//pcTcpTransform->rotation = SbRotation(loc.getRotation()[0],loc.getRotation()[1],loc.getRotation()[2],loc.getRotation()[3]);
}else if (prop == &robObj->ToolShape) {
App::DocumentObject* o = robObj->ToolShape.getValue<App::DocumentObject*>();
if(o && (o->isDerivedFrom(Part::Feature::getClassTypeId()) || o->isDerivedFrom(App::VRMLObject::getClassTypeId())) ){
//Part::Feature *p = dynamic_cast<Part::Feature *>(o);
toolShape = Gui::Application::Instance->getViewProvider(o);
toolShape->setTransformation((robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix());
}else
toolShape = 0;
}
}
void ViewProviderRobotObject::setAxisTo(float A1,float A2,float A3,float A4,float A5,float A6,const Base::Placement &Tcp)
{
Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
if(Axis1Node)
// FIXME Uggly hack for the wrong transformation of the Kuka 500 robot VRML the minus sign on Axis 1
Axis1Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A1*(M_PI/180));
if(Axis2Node)
Axis2Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A2*(M_PI/180));
if(Axis3Node)
Axis3Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A3*(M_PI/180));
if(Axis4Node)
Axis4Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A4*(M_PI/180));
if(Axis5Node)
Axis5Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A5*(M_PI/180));
if(Axis6Node)
Axis6Node->rotation.setValue(SbVec3f(0.0,1.0,0.0),A6*(M_PI/180));
// update tool position
if(toolShape)
toolShape->setTransformation((Tcp * (robObj->ToolBase.getValue().inverse())).toMatrix());
}
void ViewProviderRobotObject::sDraggerMotionCallback(void *data, SoDragger *dragger)
{
static_cast<ViewProviderRobotObject*>(data)->DraggerMotionCallback(dragger);
}
void ViewProviderRobotObject::DraggerMotionCallback(SoDragger *dragger)
{
float q0, q1, q2, q3;
Robot::RobotObject* robObj = static_cast<Robot::RobotObject*>(pcObject);
Base::Placement Tcp = robObj->Tcp.getValue();
const SbMatrix & M = dragger->getMotionMatrix ();
SbVec3f translation;
SbRotation rotation;
SbVec3f scaleFactor;
SbRotation scaleOrientation;
SbVec3f center(Tcp.getPosition().x,Tcp.getPosition().y,Tcp.getPosition().z);
M.getTransform(translation, rotation, scaleFactor, scaleOrientation);
rotation.getValue(q0, q1, q2, q3);
//Base::Console().Message("M %f %f %f\n", M[3][0], M[3][1], M[3][2]);
Base::Rotation rot(q0, q1, q2, q3);
Base::Vector3d pos(translation[0],translation[1],translation[2]);
robObj->Tcp.setValue(Base::Placement(pos,rot));
}
Reference in New Issue View Git Blame Copy Permalink