FreeCAD / src /Mod /Robot /Gui /ViewProviderRobotObject.cpp

Upload folder using huggingface_hub

985c397 verified 9 months ago

17.2 kB

// SPDX-License-Identifier: LGPL-2.1-or-later

/***************************************************************************
 *   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 <QFile>
#include <sstream>

#include <Inventor/SbVec3f.h>
#include <Inventor/SoDB.h>
#include <Inventor/SoInput.h>
#include <Inventor/VRMLnodes/SoVRMLTransform.h>
#include <Inventor/actions/SoSearchAction.h>
#include <Inventor/draggers/SoJackDragger.h>
#include <Inventor/nodes/SoSeparator.h>

#include <Base/Tools.h>
#include <App/Document.h>
#include <App/VRMLObject.h>
#include <Gui/Application.h>
#include <Mod/Part/Gui/ViewProvider.h>
#include <Mod/Robot/App/RobotObject.h>

#include "ViewProviderRobotObject.h"


using namespace Gui;
using namespace RobotGui;

PROPERTY_SOURCE(RobotGui::ViewProviderRobotObject, Gui::ViewProviderGeometryObject)

ViewProviderRobotObject::ViewProviderRobotObject()
{
    ADD_PROPERTY(Manipulator, (0));

    pcRobotRoot = new Gui::SoFCSelection();
    pcRobotRoot->preselectionMode = Gui::SoFCSelection::OFF;
    pcRobotRoot->ref();

    pcSimpleRoot = new Gui::SoFCSelection();
    pcSimpleRoot->preselectionMode = Gui::SoFCSelection::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 = nullptr;
}

ViewProviderRobotObject::~ViewProviderRobotObject()
{
    pcRobotRoot->unref();
    pcSimpleRoot->unref();
    pcOffRoot->unref();
    pcTcpRoot->unref();
}

void ViewProviderRobotObject::setDragger()
{
    assert(!pcDragger);
    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);
    Gui::coinRemoveAllChildren(pcTcpRoot);
    pcDragger = nullptr;
}

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() const
{
    std::vector<std::string> StrList;
    StrList.emplace_back("VRML");
    StrList.emplace_back("Simple");
    StrList.emplace_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;
        Gui::coinRemoveAllChildren(pcRobotRoot);
        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 connection points +++++++++++++++++++++++++++++++++++++++++++++++++
        Axis1Node = Axis2Node = Axis3Node = Axis4Node = Axis5Node = Axis6Node = nullptr;
        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();
            if (node && node->getTypeId() == SoVRMLTransform::getClassTypeId()) {
                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();
            if (node && node->getTypeId() == SoVRMLTransform::getClassTypeId()) {
                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();
            if (node && node->getTypeId() == SoVRMLTransform::getClassTypeId()) {
                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();
            if (node && node->getTypeId() == SoVRMLTransform::getClassTypeId()) {
                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();
            if (node && node->getTypeId() == SoVRMLTransform::getClassTypeId()) {
                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();
            if (node && node->getTypeId() == SoVRMLTransform::getClassTypeId()) {
                Axis6Node = static_cast<SoVRMLTransform*>(node);
            }
        }
        if (Axis1Node) {
            Axis1Node->rotation.setValue(
                SbVec3f(0.0, 1.0, 0.0),
                Base::toRadians(robObj->Axis1.getValue())
            );
        }
        if (Axis2Node) {
            Axis2Node->rotation.setValue(
                SbVec3f(0.0, 1.0, 0.0),
                Base::toRadians(robObj->Axis2.getValue())
            );
        }
        if (Axis3Node) {
            Axis3Node->rotation.setValue(
                SbVec3f(0.0, 1.0, 0.0),
                Base::toRadians(robObj->Axis3.getValue())
            );
        }
        if (Axis4Node) {
            Axis4Node->rotation.setValue(
                SbVec3f(0.0, 1.0, 0.0),
                Base::toRadians(robObj->Axis4.getValue())
            );
        }
        if (Axis5Node) {
            Axis5Node->rotation.setValue(
                SbVec3f(0.0, 1.0, 0.0),
                Base::toRadians(robObj->Axis5.getValue())
            );
        }
        if (Axis6Node) {
            Axis6Node->rotation.setValue(
                SbVec3f(0.0, 1.0, 0.0),
                Base::toRadians(robObj->Axis6.getValue())
            );
        }
    }
    else if (prop == &robObj->Axis1) {
        if (Axis1Node) {
            Axis1Node->rotation.setValue(
                SbVec3f(0.0, 1.0, 0.0),
                Base::toRadians(robObj->Axis1.getValue())
            );
            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),
                Base::toRadians(robObj->Axis2.getValue())
            );
            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),
                Base::toRadians(robObj->Axis3.getValue())
            );
            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),
                Base::toRadians(robObj->Axis4.getValue())
            );
            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),
                Base::toRadians(robObj->Axis5.getValue())
            );
            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),
                Base::toRadians(robObj->Axis6.getValue())
            );
            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()
            );
        }
    }
    else if (prop == &robObj->ToolShape) {
        App::DocumentObject* o = robObj->ToolShape.getValue<App::DocumentObject*>();

        if (o && (o->isDerivedFrom<Part::Feature>() || o->isDerivedFrom<App::VRMLObject>())) {
            toolShape = Gui::Application::Instance->getViewProvider(o);
            toolShape->setTransformation(
                (robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix()
            );
        }
        else {
            toolShape = nullptr;
        }
    }
}
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 Ugly 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), Base::toRadians(A1));
    }
    if (Axis2Node) {
        Axis2Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), Base::toRadians(A2));
    }
    if (Axis3Node) {
        Axis3Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), Base::toRadians(A3));
    }
    if (Axis4Node) {
        Axis4Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), Base::toRadians(A4));
    }
    if (Axis5Node) {
        Axis5Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), Base::toRadians(A5));
    }
    if (Axis6Node) {
        Axis6Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), Base::toRadians(A6));
    }
    // 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::Rotation rot(q0, q1, q2, q3);
    Base::Vector3d pos(translation[0], translation[1], translation[2]);
    robObj->Tcp.setValue(Base::Placement(pos, rot));
}

	// SPDX-License-Identifier: LGPL-2.1-or-later

	/***************************************************************************
	* 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 <QFile>
	#include <sstream>

	#include <Inventor/SbVec3f.h>
	#include <Inventor/SoDB.h>
	#include <Inventor/SoInput.h>
	#include <Inventor/VRMLnodes/SoVRMLTransform.h>
	#include <Inventor/actions/SoSearchAction.h>
	#include <Inventor/draggers/SoJackDragger.h>
	#include <Inventor/nodes/SoSeparator.h>

	#include <Base/Tools.h>
	#include <App/Document.h>
	#include <App/VRMLObject.h>
	#include <Gui/Application.h>
	#include <Mod/Part/Gui/ViewProvider.h>
	#include <Mod/Robot/App/RobotObject.h>

	#include "ViewProviderRobotObject.h"


	using namespace Gui;
	using namespace RobotGui;

	PROPERTY_SOURCE(RobotGui::ViewProviderRobotObject, Gui::ViewProviderGeometryObject)

	ViewProviderRobotObject::ViewProviderRobotObject()
	{
	ADD_PROPERTY(Manipulator, (0));

	pcRobotRoot = new Gui::SoFCSelection();
	pcRobotRoot->preselectionMode = Gui::SoFCSelection::OFF;
	pcRobotRoot->ref();

	pcSimpleRoot = new Gui::SoFCSelection();
	pcSimpleRoot->preselectionMode = Gui::SoFCSelection::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 = nullptr;
	}

	ViewProviderRobotObject::~ViewProviderRobotObject()
	{
	pcRobotRoot->unref();
	pcSimpleRoot->unref();
	pcOffRoot->unref();
	pcTcpRoot->unref();
	}

	void ViewProviderRobotObject::setDragger()
	{
	assert(!pcDragger);
	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);
	Gui::coinRemoveAllChildren(pcTcpRoot);
	pcDragger = nullptr;
	}

	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() const
	{
	std::vector<std::string> StrList;
	StrList.emplace_back("VRML");
	StrList.emplace_back("Simple");
	StrList.emplace_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;
	Gui::coinRemoveAllChildren(pcRobotRoot);
	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 connection points +++++++++++++++++++++++++++++++++++++++++++++++++
	Axis1Node = Axis2Node = Axis3Node = Axis4Node = Axis5Node = Axis6Node = nullptr;
	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();
	if (node && node->getTypeId() == SoVRMLTransform::getClassTypeId()) {
	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();
	if (node && node->getTypeId() == SoVRMLTransform::getClassTypeId()) {
	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();
	if (node && node->getTypeId() == SoVRMLTransform::getClassTypeId()) {
	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();
	if (node && node->getTypeId() == SoVRMLTransform::getClassTypeId()) {
	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();
	if (node && node->getTypeId() == SoVRMLTransform::getClassTypeId()) {
	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();
	if (node && node->getTypeId() == SoVRMLTransform::getClassTypeId()) {
	Axis6Node = static_cast<SoVRMLTransform*>(node);
	}
	}
	if (Axis1Node) {
	Axis1Node->rotation.setValue(
	SbVec3f(0.0, 1.0, 0.0),
	Base::toRadians(robObj->Axis1.getValue())
	);
	}
	if (Axis2Node) {
	Axis2Node->rotation.setValue(
	SbVec3f(0.0, 1.0, 0.0),
	Base::toRadians(robObj->Axis2.getValue())
	);
	}
	if (Axis3Node) {
	Axis3Node->rotation.setValue(
	SbVec3f(0.0, 1.0, 0.0),
	Base::toRadians(robObj->Axis3.getValue())
	);
	}
	if (Axis4Node) {
	Axis4Node->rotation.setValue(
	SbVec3f(0.0, 1.0, 0.0),
	Base::toRadians(robObj->Axis4.getValue())
	);
	}
	if (Axis5Node) {
	Axis5Node->rotation.setValue(
	SbVec3f(0.0, 1.0, 0.0),
	Base::toRadians(robObj->Axis5.getValue())
	);
	}
	if (Axis6Node) {
	Axis6Node->rotation.setValue(
	SbVec3f(0.0, 1.0, 0.0),
	Base::toRadians(robObj->Axis6.getValue())
	);
	}
	}
	else if (prop == &robObj->Axis1) {
	if (Axis1Node) {
	Axis1Node->rotation.setValue(
	SbVec3f(0.0, 1.0, 0.0),
	Base::toRadians(robObj->Axis1.getValue())
	);
	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),
	Base::toRadians(robObj->Axis2.getValue())
	);
	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),
	Base::toRadians(robObj->Axis3.getValue())
	);
	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),
	Base::toRadians(robObj->Axis4.getValue())
	);
	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),
	Base::toRadians(robObj->Axis5.getValue())
	);
	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),
	Base::toRadians(robObj->Axis6.getValue())
	);
	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()
	);
	}
	}
	else if (prop == &robObj->ToolShape) {
	App::DocumentObject* o = robObj->ToolShape.getValue<App::DocumentObject*>();

	if (o && (o->isDerivedFrom<Part::Feature>() \|\| o->isDerivedFrom<App::VRMLObject>())) {
	toolShape = Gui::Application::Instance->getViewProvider(o);
	toolShape->setTransformation(
	(robObj->Tcp.getValue() * (robObj->ToolBase.getValue().inverse())).toMatrix()
	);
	}
	else {
	toolShape = nullptr;
	}
	}
	}
	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 Ugly 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), Base::toRadians(A1));
	}
	if (Axis2Node) {
	Axis2Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), Base::toRadians(A2));
	}
	if (Axis3Node) {
	Axis3Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), Base::toRadians(A3));
	}
	if (Axis4Node) {
	Axis4Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), Base::toRadians(A4));
	}
	if (Axis5Node) {
	Axis5Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), Base::toRadians(A5));
	}
	if (Axis6Node) {
	Axis6Node->rotation.setValue(SbVec3f(0.0, 1.0, 0.0), Base::toRadians(A6));
	}
	// 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::Rotation rot(q0, q1, q2, q3);
	Base::Vector3d pos(translation[0], translation[1], translation[2]);
	robObj->Tcp.setValue(Base::Placement(pos, rot));
	}