PropertyBrowserWidget.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2012-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    MemoryX::gui-plugins::SceneEditor
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
/* Qt headers */
#include <QGridLayout>
#include <QIcon>
#include <QLabel>
#include <QMap>
 
#include <VirtualRobot/MathTools.h>
 
/* boost headers */
#include <boost/format.hpp>
 
/* QtPropertyBrower headers */
#include <ArmarXGui/libraries/qtpropertybrowser/src/QtProperty>
 
#include "../controller/RotateTranslateOperation.h"
#include "PropertyBrowserWidget.h"
 
gui::PropertyBrowserWidget::PropertyBrowserWidget(const controller::ControllerPtr& control,
                                                  QWidget* parent) :
    QtTreePropertyBrowser(parent),
    control(control),
    spinBoxFactory(new QtDoubleSpinBoxFactory(this)),
    doubleManager(new QtDoublePropertyManager(this)),
    groupManager(new QtGroupPropertyManager(this)),
    stringManager(new QtStringPropertyManager(this))
{
    setHeaderVisible(false);
 
    connect(doubleManager,
            SIGNAL(valueChanged(QtProperty*, double)),
            this,
            SLOT(valueChanged(QtProperty*, double)));
}
 
void
gui::PropertyBrowserWidget::setProperties(const memoryx::ObjectClassPtr& objectClass,
                                          std::string collection)
{
    this->clearAll();
 
    if (objectClass && collection.length() != 0)
    {
        this->setClassAttributes(objectClass, collection, true);
        this->setFactoryForManager(qobject_cast<QtDoublePropertyManager*>(doubleManager),
                                   qobject_cast<QtDoubleSpinBoxFactory*>(spinBoxFactory));
    }
}
 
void
gui::PropertyBrowserWidget::setProperties(scene3D::SceneObjectPtr sceneObject)
{
    this->clearAll();
 
    if (sceneObject)
    {
        currentSceneObject = sceneObject;
        this->setInstanceAttributes();
        this->setFactoryForManager(qobject_cast<QtDoublePropertyManager*>(doubleManager),
                                   qobject_cast<QtDoubleSpinBoxFactory*>(spinBoxFactory));
    }
}
 
void
gui::PropertyBrowserWidget::valueChanged(QtProperty* property, double newValue)
{
    //only react to changes done in propertyBrowser
    if (!propertyToId.contains(property) || !currentSceneObject ||
        propertyToValue[property] == newValue)
    {
        return;
    }
 
    if (controller::ControllerPtr controller = control.lock())
    {
        propertyToValue[property] = newValue;
        std::shared_ptr<std::vector<controller::OperationPtr>> operations(
            new std::vector<controller::OperationPtr>());
        // Create RotateTranslateOperation for each object
        controller::OperationPtr operation(new controller::RotateTranslateOperation(
            controller->getMemoryXController(),
            controller->getScene(),
            currentSceneObject->getObjectId(),
            currentSceneObject->getRotation(),
            this->createNewRotation(property, newValue),
            currentSceneObject->getTranslation(),
            this->createNewTranslation(property, newValue)));
        operations->push_back(operation);
        controller->execute(controller::Controller::EXECUTE_ON_SCENE |
                                controller::Controller::EXECUTE_ON_WM |
                                controller::Controller::UNDOABLE,
                            operations);
    }
}
 
void
gui::PropertyBrowserWidget::setInstanceAttributes()
{
    QtProperty* instanceAttributes = groupManager->addProperty(tr("Instance Attributes"));
 
    QtProperty* objectID = stringManager->addProperty(tr("Object ID"));
    stringManager->setValue(objectID, QString::fromStdString(currentSceneObject->getObjectId()));
    instanceAttributes->addSubProperty(objectID);
 
    // rotation as quaternion properties
    QtProperty* quaternion = groupManager->addProperty(tr("Rotation as Quaternion"));
 
    float xQuat, yQuat, zQuat, wQuat;
    currentSceneObject->getRotation().getValue(xQuat, yQuat, zQuat, wQuat);
 
    QtProperty* quaternionX = doubleManager->addProperty(tr("x"));
    QtProperty* quaternionY = doubleManager->addProperty(tr("y"));
    QtProperty* quaternionZ = doubleManager->addProperty(tr("z"));
    QtProperty* quaternionW = doubleManager->addProperty(tr("w"));
 
    // update values in propertyToValue to not react to the comming changes
    propertyToValue[quaternionX] = (double)xQuat;
    propertyToValue[quaternionY] = (double)yQuat;
    propertyToValue[quaternionZ] = (double)zQuat;
    propertyToValue[quaternionW] = (double)wQuat;
 
    doubleManager->setValue(quaternionX, (double)xQuat);
    doubleManager->setValue(quaternionY, (double)yQuat);
    doubleManager->setValue(quaternionZ, (double)zQuat);
    doubleManager->setValue(quaternionW, (double)wQuat);
 
    doubleManager->setSingleStep(quaternionW, 0.01);
    doubleManager->setSingleStep(quaternionX, 0.01);
    doubleManager->setSingleStep(quaternionY, 0.01);
    doubleManager->setSingleStep(quaternionZ, 0.01);
 
    quaternion->addSubProperty(quaternionX);
    quaternion->addSubProperty(quaternionY);
    quaternion->addSubProperty(quaternionZ);
    quaternion->addSubProperty(quaternionW);
 
    // rotation as roll-pitch-yaw properties
    QtProperty* rpy = groupManager->addProperty(tr("Roll-Pitch-Yaw"));
 
    QtProperty* roll = doubleManager->addProperty(tr("Roll in deg"));
    QtProperty* pitch = doubleManager->addProperty(tr("Pitch in deg"));
    QtProperty* yaw = doubleManager->addProperty(tr("Yaw in deg"));
 
    Eigen::Vector3f rpyVector;
    VirtualRobot::MathTools::eigen4f2rpy(
        VirtualRobot::MathTools::quat2eigen4f(xQuat, yQuat, zQuat, wQuat), rpyVector);
 
    float rollDeg, pitchDeg, yawDeg;
 
    rollDeg = VirtualRobot::MathTools::rad2deg(rpyVector(0));
    pitchDeg = VirtualRobot::MathTools::rad2deg(rpyVector(1));
    yawDeg = VirtualRobot::MathTools::rad2deg(rpyVector(2));
 
    propertyToValue[roll] = rollDeg;
    propertyToValue[pitch] = pitchDeg;
    propertyToValue[yaw] = yawDeg;
 
    doubleManager->setValue(roll, rollDeg);
    doubleManager->setValue(pitch, pitchDeg);
    doubleManager->setValue(yaw, yawDeg);
 
    doubleManager->setSingleStep(roll, 1);
    doubleManager->setSingleStep(pitch, 1);
    doubleManager->setSingleStep(yaw, 1);
 
    rpy->addSubProperty(roll);
    rpy->addSubProperty(pitch);
    rpy->addSubProperty(yaw);
 
    // translation properties
    QtProperty* translation = groupManager->addProperty(tr("Translation"));
 
    float xPos, yPos, zPos;
    currentSceneObject->getTranslation().getValue(xPos, yPos, zPos);
 
    QtProperty* translationX = doubleManager->addProperty(tr("X Position"));
    QtProperty* translationY = doubleManager->addProperty(tr("Y Position"));
    QtProperty* translationZ = doubleManager->addProperty(tr("Z Position"));
 
    // update values in propertyToValue to not react to the comming changes
    propertyToValue[translationX] = (double)xPos;
    propertyToValue[translationY] = (double)yPos;
    propertyToValue[translationZ] = (double)zPos;
 
    doubleManager->setValue(translationX, (double)xPos);
    doubleManager->setValue(translationY, (double)yPos);
    doubleManager->setValue(translationZ, (double)zPos);
 
    doubleManager->setSingleStep(translationX, 0.01);
    doubleManager->setSingleStep(translationY, 0.01);
    doubleManager->setSingleStep(translationZ, 0.01);
 
    translation->addSubProperty(translationX);
    translation->addSubProperty(translationY);
    translation->addSubProperty(translationZ);
 
    QtProperty* matrix = stringManager->addProperty(tr("Matrix"));
 
    stringManager->setValue(matrix, QString::fromStdString(this->getMatrix()));
 
    instanceAttributes->addSubProperty(quaternion);
    instanceAttributes->addSubProperty(rpy);
    instanceAttributes->addSubProperty(translation);
    instanceAttributes->addSubProperty(matrix);
 
    // store ids of changable properties
    this->insertInMap(quaternionX, "x");
    this->insertInMap(quaternionY, "y");
    this->insertInMap(quaternionZ, "z");
    this->insertInMap(quaternionW, "w");
    this->insertInMap(roll, "Roll in deg");
    this->insertInMap(pitch, "Pitch in deg");
    this->insertInMap(yaw, "Yaw in deg");
    this->insertInMap(translationX, "X Position");
    this->insertInMap(translationY, "Y Position");
    this->insertInMap(translationZ, "Z Position");
    this->insertInMap(matrix, "Matrix");
 
    QtBrowserItem* instancesSection = this->addProperty(instanceAttributes);
    setExpanded(instancesSection, true);
 
    std::string collection = currentSceneObject->getCollection();
    std::string objectClass = currentSceneObject->getClassId();
 
    if (controller::ControllerPtr controller = control.lock())
    {
        this->setClassAttributes(
            controller->getMemoryXController()->getPriorKnowlegdeController()->getObjectClassPtr(
                objectClass, collection),
            collection,
            false);
    }
}
 
void
gui::PropertyBrowserWidget::setClassAttributes(const memoryx::ObjectClassPtr& objectClass,
                                               const std::string& collection,
                                               bool expanded)
{
    QtProperty* classAttributes = groupManager->addProperty(tr("Class Attributes"));
 
    QtProperty* className = stringManager->addProperty(tr("Class Name"));
    stringManager->setValue(className, QString::fromStdString(objectClass->getName()));
    classAttributes->addSubProperty(className);
 
    QtProperty* collectionName = stringManager->addProperty(tr("Collection"));
    stringManager->setValue(collectionName, QString::fromStdString(collection));
    classAttributes->addSubProperty(collectionName);
 
    QtProperty* objectClassID = stringManager->addProperty(tr("Object Class ID"));
    stringManager->setValue(objectClassID, QString::fromStdString(objectClass->getId()));
    classAttributes->addSubProperty(objectClassID);
 
    std::map<std::string, std::string>::iterator iterator;
 
 
    if (controller::ControllerPtr controller = control.lock())
    {
        std::map<std::string, std::string> attributes =
            controller->getMemoryXController()->getPriorKnowlegdeController()->getAllAttributes(
                objectClass);
 
        for (iterator = attributes.begin(); iterator != attributes.end(); ++iterator)
        {
            QtProperty* property =
                stringManager->addProperty(QString::fromStdString(iterator->first));
            stringManager->setValue(property, QString::fromStdString(iterator->second));
            classAttributes->addSubProperty(property);
        }
    }
 
    QtBrowserItem* classesSection = this->addProperty(classAttributes);
 
    setExpanded(classesSection, expanded);
}
 
void
gui::PropertyBrowserWidget::clearAll()
{
    this->clear();
    groupManager->clear();
    doubleManager->clear();
    stringManager->clear();
    propertyToId.clear();
    idToProperty.clear();
    propertyToValue.clear();
    currentSceneObject = NULL;
}
 
void
gui::PropertyBrowserWidget::updateSceneObject(std::string objectID)
{
    if (!currentSceneObject || objectID != currentSceneObject->getObjectId())
    {
        return;
    }
 
    // Set new rotation values
    float xQuat, yQuat, zQuat, wQuat;
    currentSceneObject->getRotation().getValue(xQuat, yQuat, zQuat, wQuat);
 
    // update values in propertyToValue to not react to the comming changes
    propertyToValue[idToProperty[QString("x")]] = (double)xQuat;
    propertyToValue[idToProperty[QString("y")]] = (double)yQuat;
    propertyToValue[idToProperty[QString("z")]] = (double)zQuat;
    propertyToValue[idToProperty[QString("w")]] = (double)wQuat;
 
    doubleManager->setValue(idToProperty[QString("x")], (double)xQuat);
    doubleManager->setValue(idToProperty[QString("y")], (double)yQuat);
    doubleManager->setValue(idToProperty[QString("z")], (double)zQuat);
    doubleManager->setValue(idToProperty[QString("w")], (double)wQuat);
 
    Eigen::Vector3f rpyVector;
    VirtualRobot::MathTools::eigen4f2rpy(
        VirtualRobot::MathTools::quat2eigen4f(xQuat, yQuat, zQuat, wQuat), rpyVector);
 
    float rollDeg, pitchDeg, yawDeg;
 
    rollDeg = VirtualRobot::MathTools::rad2deg(rpyVector(0));
    pitchDeg = VirtualRobot::MathTools::rad2deg(rpyVector(1));
    yawDeg = VirtualRobot::MathTools::rad2deg(rpyVector(2));
 
 
    propertyToValue[idToProperty[QString("Roll in deg")]] = rollDeg;
    propertyToValue[idToProperty[QString("Pitch in deg")]] = pitchDeg;
    propertyToValue[idToProperty[QString("Yaw in deg")]] = yawDeg;
 
    doubleManager->setValue(idToProperty[QString("Roll in deg")], rollDeg);
    doubleManager->setValue(idToProperty[QString("Pitch in deg")], pitchDeg);
    doubleManager->setValue(idToProperty[QString("Yaw in deg")], yawDeg);
 
    // Set new translation values
    float xPos, yPos, zPos;
    currentSceneObject->getTranslation().getValue(xPos, yPos, zPos);
 
    // update values in propertyToValue to not react to the comming changes
    propertyToValue[idToProperty[QString("X Position")]] = (double)xPos;
    propertyToValue[idToProperty[QString("Y Position")]] = (double)yPos;
    propertyToValue[idToProperty[QString("Z Position")]] = (double)zPos;
 
    doubleManager->setValue(idToProperty[QString("X Position")], (double)xPos);
    doubleManager->setValue(idToProperty[QString("Y Position")], (double)yPos);
    doubleManager->setValue(idToProperty[QString("Z Position")], (double)zPos);
 
 
    // Set new matrix
    stringManager->setValue(idToProperty[QString("Matrix")],
                            QString::fromStdString(this->getMatrix()));
}
 
void
gui::PropertyBrowserWidget::insertInMap(QtProperty* property, QString id)
{
    propertyToId[property] = id;
    idToProperty[id] = property;
}
 
SbRotation
gui::PropertyBrowserWidget::createNewRotation(QtProperty* property, double val)
{
    SbRotation newRotation = currentSceneObject->getRotation();
    float xQuat, yQuat, zQuat, wQuat;
    newRotation.getValue(xQuat, yQuat, zQuat, wQuat);
 
    Eigen::Vector3f rpy;
    VirtualRobot::MathTools::eigen4f2rpy(
        VirtualRobot::MathTools::quat2eigen4f(xQuat, yQuat, zQuat, wQuat), rpy);
    Eigen::Matrix4f rotationMatrix = Eigen::Matrix4f::Identity();
    VirtualRobot::MathTools::Quaternion quaternion;
 
    QString id = propertyToId[property];
    if (id == QString("x") &&
        !((float)val == yQuat && yQuat == zQuat && zQuat == wQuat && wQuat == 0.0f))
    {
        newRotation = SbRotation((float)val, yQuat, zQuat, wQuat);
    }
    else if (id == QString("y") &&
             !((float)val == xQuat && xQuat == zQuat && zQuat == wQuat && wQuat == 0.0f))
    {
        newRotation = SbRotation(xQuat, (float)val, zQuat, wQuat);
    }
    else if (id == QString("z") &&
             !((float)val == xQuat && xQuat == yQuat && yQuat == wQuat && wQuat == 0.0f))
    {
        newRotation = SbRotation(xQuat, yQuat, (float)val, wQuat);
    }
    else if (id == QString("w") &&
             !((float)val == xQuat && xQuat == yQuat && yQuat == zQuat && zQuat == 0.0f))
    {
        newRotation = SbRotation(xQuat, yQuat, zQuat, (float)val);
    }
    else if (id == QString("Roll in deg"))
    {
        VirtualRobot::MathTools::rpy2eigen4f(
            VirtualRobot::MathTools::deg2rad((float)val), rpy(1), rpy(2), rotationMatrix);
        quaternion = VirtualRobot::MathTools::eigen4f2quat(rotationMatrix);
        newRotation = SbRotation(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
    }
    else if (id == QString("Pitch in deg"))
    {
        VirtualRobot::MathTools::rpy2eigen4f(
            rpy(0), VirtualRobot::MathTools::deg2rad((float)val), rpy(2), rotationMatrix);
        quaternion = VirtualRobot::MathTools::eigen4f2quat(rotationMatrix);
        newRotation = SbRotation(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
    }
    else if (id == QString("Yaw in deg"))
    {
        VirtualRobot::MathTools::rpy2eigen4f(
            rpy(0), rpy(1), VirtualRobot::MathTools::deg2rad((float)val), rotationMatrix);
        quaternion = VirtualRobot::MathTools::eigen4f2quat(rotationMatrix);
        newRotation = SbRotation(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
    }
    else
    {
        newRotation = currentSceneObject->getRotation();
    }
 
    // for debug purpose
    float x, y, z, w;
    newRotation.getValue(x, y, z, w);
    Eigen::Vector3f rot;
    VirtualRobot::MathTools::eigen4f2rpy(VirtualRobot::MathTools::quat2eigen4f(x, y, z, w), rot);
 
    ARMARX_INFO_S << "Quat: \n" << x << ", \n" << y << ", \n" << z << ", \n" << w;
    ARMARX_INFO_S << "RPY: \n" << rot;
 
    return newRotation;
}
 
SbVec3f
gui::PropertyBrowserWidget::createNewTranslation(QtProperty* property, double val)
{
    SbVec3f newPosition;
    float xPos, yPos, zPos;
    currentSceneObject->getTranslation().getValue(xPos, yPos, zPos);
 
    QString id = propertyToId[property];
 
    if (id == QString("X Position"))
    {
        newPosition = SbVec3f((float)val, yPos, zPos);
    }
    else if (id == QString("Y Position"))
    {
        newPosition = SbVec3f(xPos, (float)val, zPos);
    }
    else if (id == QString("Z Position"))
    {
        newPosition = SbVec3f(xPos, yPos, (float)val);
    }
    else
    {
        newPosition = currentSceneObject->getTranslation();
    }
 
    return newPosition;
}
 
std::string
gui::PropertyBrowserWidget::getMatrix()
{
    SbMatrix sbMatrix;
    sbMatrix.setRotate(currentSceneObject->getRotation());
    sbMatrix.setTranslate(currentSceneObject->getTranslation());
    std::string matrixToString;
 
    for (int i = 0; i < 4; ++i)
    {
        matrixToString.append(boost::str(boost::format("%3.2g %3.2g %3.2g %3.2g") % sbMatrix[i][0] %
                                         sbMatrix[i][1] % sbMatrix[i][2] % sbMatrix[i][3]));
 
        if (i < 3)
        {
            matrixToString.append("\n");
        }
    }
 
    return matrixToString;
}