ManipulatorVisualization.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "ManipulatorVisualization.h"
 
//Virtual Robot includes
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <VirtualRobot/XML/RobotIO.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
 
/* Coin includes */
#include <Inventor/actions/SoGetMatrixAction.h>
#include <Inventor/actions/SoSearchAction.h>
#include <Inventor/SbViewportRegion.h>
#include <Inventor/nodes/SoCube.h>
 
 
#include <iostream>
 
ManipulatorVisualization::ManipulatorVisualization() : isVisualizing(false), hasEndEffectorVisualizer(false), localTransformation(Eigen::Matrix4f::Identity())
{
    //Im gonna live forever :)
    this->ref();
}
 
ManipulatorVisualization::~ManipulatorVisualization()
{
    //Life makes no sense anymore
    this->unref();
}
 
void ManipulatorVisualization::setVisualization(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr nodeSet)
{
    //Completely forget anything we displayed earlier
    //This also works when this is the first time we display something
    this->removeVisualization();
 
    //First of all add the end effector
    VirtualRobot::EndEffectorPtr endEffector = robot->getEndEffector(nodeSet->getTCP()->getName());
    if (!endEffector)
    {
        std::vector<VirtualRobot::EndEffectorPtr> eefs;
        robot->getEndEffectors(eefs);
        for (auto eef : eefs)
        {
            if (eef->getTcp() == nodeSet->getTCP())
            {
                endEffector = eef;
                break;
            }
        }
    }
    this->hasEndEffectorVisualizer = endEffector ? true : false;
 
    //Display all indicators in the same color and with transparency
    this->material = new SoMaterial();
    this->material->transparency = 0.5;
    this->material->setOverride(true);
    this->addChild(material);
 
    //if no end effector is defined for the chain, just add a placeholder
    if (this->hasEndEffectorVisualizer)
    {
        VirtualRobot::RobotPtr endEffectorRobot = endEffector->createEefRobot("", "");
        VirtualRobot::CoinVisualizationPtr endEffectorVisualization = endEffectorRobot->getVisualization<VirtualRobot::CoinVisualization>();
        this->addChild(endEffectorVisualization->getCoinVisualization());
    }
    else
    {
        SoCube* cube = new SoCube();
        cube->width = 0.1;
        cube->height = 0.1;
        cube->depth = 0.1;
 
        this->addChild(cube);
    }
 
    //And now wrap this bad boy with our manipulator
    this->manip.reset(new SoTransformerManip());
 
    //We need this null separator to remove the scale knobs
    //This won't lead to memory leak, because this separator
    //will be deleted when the manipulator itself gets removed
    //from the scene graph
    SoSeparator* nullSep = new SoSeparator;
 
    //Make all scale knobs disappear
    manip->getDragger()->setPart("scale1", nullSep);
    manip->getDragger()->setPart("scale2", nullSep);
    manip->getDragger()->setPart("scale3", nullSep);
    manip->getDragger()->setPart("scale4", nullSep);
    manip->getDragger()->setPart("scale5", nullSep);
    manip->getDragger()->setPart("scale6", nullSep);
    manip->getDragger()->setPart("scale7", nullSep);
    manip->getDragger()->setPart("scale8", nullSep);
 
    //Stores the global position and pose of tcp
    Eigen::Matrix4f globalMat = nodeSet->getTCP()->getGlobalPose();
 
    if (this->hasEndEffectorVisualizer)
    {
        //Stores the local transformation from the endeffector base to the TCP node
        this->localTransformation = endEffector->getBase()->getTransformationTo(nodeSet->getTCP());
 
        globalMat = endEffector->getBase()->getGlobalPose();
    }
 
    //Now go ahead and set new position
    //Factor 1000 to match coin unit system
    globalMat(0, 3) /= 1000;
    globalMat(1, 3) /= 1000;
    globalMat(2, 3) /= 1000;
    manip->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(globalMat));
 
    this->insertChild(manip.get(), 0);
 
    this->isVisualizing = true;
}
 
void ManipulatorVisualization::removeVisualization()
{
    //Remove all children and reset manip pointer
    //This should bring ref counter of Inventor down to zero and free memory
    this->removeAllChildren();
    manip.reset();
 
    this->isVisualizing = false;
    this->hasEndEffectorVisualizer = false;
    this->localTransformation =  Eigen::Matrix4f::Identity();
}
 
void ManipulatorVisualization::setColor(float r, float g, float b)
{
    if (this->getIsVisualizing())
    {
        this->material->ambientColor.setValue(r, g, b);
    }
}
 
void ManipulatorVisualization::addManipFinishCallback(SoDraggerCB* func, void* data)
{
    if (this->getIsVisualizing())
    {
        this->manip->getDragger()->addFinishCallback(func, data);
    }
}
 
void ManipulatorVisualization::addManipMovedCallback(SoDraggerCB* func, void* data)
{
    if (this->getIsVisualizing())
    {
        this->manip->getDragger()->addMotionCallback(func, data);
    }
}
 
Eigen::Matrix4f ManipulatorVisualization::getUserDesiredPose()
{
    if (this->getIsVisualizing())
    {
        SoGetMatrixAction* action = new SoGetMatrixAction(SbViewportRegion());
        SoSearchAction sa;
        sa.setNode(manip.get());
        sa.setSearchingAll(TRUE);                // Search all nodes
        SoBaseKit::setSearchingChildren(TRUE);   // Even inside nodekits
        sa.apply(this);
 
        action->apply(sa.getPath());
 
        SbMatrix matrix = action->getMatrix();
 
        Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();
        mat(0, 0) = matrix[0][0];
        mat(0, 1) = matrix[1][0];
        mat(0, 2) = matrix[2][0];
        mat(0, 3) = matrix[3][0] * 1000;
 
        mat(1, 0) = matrix[0][1];
        mat(1, 1) = matrix[1][1];
        mat(1, 2) = matrix[2][1];
        mat(1, 3) = matrix[3][1] * 1000;
 
        mat(2, 0) = matrix[0][2];
        mat(2, 1) = matrix[1][2];
        mat(2, 2) = matrix[2][2];
        mat(2, 3) = matrix[3][2] * 1000;
 
        mat(3, 0) = matrix[0][3];
        mat(3, 1) = matrix[1][3];
        mat(3, 2) = matrix[2][3];
        mat(3, 3) = matrix[3][3];
 
        //We need to take local transformation into account, because otherwise the visualizer
        //shows a wrong position for tcp (offset equals local transformation of tcp in node set then)
        //Just apply local transformation to fix this
        if (this->hasEndEffectorVisualizer)
        {
            mat = mat * this->localTransformation;
        }
 
        return mat;
    }
 
    return Eigen::Matrix4f::Identity();
}
 
std::string ManipulatorVisualization::getUserDesiredPoseString()
{
    Eigen::Matrix4f mat = this->getUserDesiredPose();
 
    //Convert to string
    std::stringstream buffer;
    buffer << mat;
    return buffer.str();
}
 
void ManipulatorVisualization::setUserDesiredPose(Eigen::Matrix4f globalPose)
{
    if (this->hasEndEffectorVisualizer)
    {
        globalPose = globalPose * this->localTransformation.inverse();
    }
    globalPose(0, 3) /= 1000;
    globalPose(1, 3) /= 1000;
    globalPose(2, 3) /= 1000;
    manip->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(globalPose));
}