TCPMover.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* 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    ArmarX::
* @author     Mirko Waechter ( mirko.waechter at kit dot edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "TCPMover.h"
 
 
//VirtualRobot
#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/Nodes/RobotNode.h>
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
// C++ includes
#include <sstream>
 
// Qt includes
#include <QDialogButtonBox>
#include <QLineEdit>
#include <QScrollBar>
 
using namespace armarx;
using namespace VirtualRobot;
 
TCPMover::TCPMover() : robotRequested(false)
 
{
 
    //    handData[selectedTCP][0] = handData[selectedTCP][1] = handData[selectedTCP][2] = 0.0f;
    //    handData[selectedTCP][3] = handData[selectedTCP][4] = handData[selectedTCP][5] = 0.0f;
    ui.setupUi(getWidget());
    ui.gridLayout->setEnabled(false);
    // SIGNALS AND SLOTS CONNECTIONS
    connect(ui.cbselectedTCP, SIGNAL(currentIndexChanged(int)), this, SLOT(selectHand(int)));
    connect(ui.BtnRequestControl, SIGNAL(clicked(bool)), this, SLOT(robotControl(bool)));
    connect(ui.btnUp, SIGNAL(clicked()), this, SLOT(moveUp()));
    connect(ui.btnDown, SIGNAL(clicked()), this, SLOT(moveDown()));
    connect(ui.btnZUp, SIGNAL(clicked()), this, SLOT(moveZUp()));
    connect(ui.btnZDown, SIGNAL(clicked()), this, SLOT(moveZDown()));
    connect(ui.btnLeft, SIGNAL(clicked()), this, SLOT(moveRight()));
    connect(ui.btnRight, SIGNAL(clicked()), this, SLOT(moveLeft()));
    connect(ui.btnIncreaseAlpha, SIGNAL(clicked()), this, SLOT(increaseAlpha()));
    connect(ui.btnDecreaseAlpha, SIGNAL(clicked()), this, SLOT(decreaseAlpha()));
    connect(ui.btnIncreaseBeta, SIGNAL(clicked()), this, SLOT(increaseBeta()));
    connect(ui.btnDecreaseBeta, SIGNAL(clicked()), this, SLOT(decreaseBeta()));
    connect(ui.btnIncreaseGamma, SIGNAL(clicked()), this, SLOT(increaseGamma()));
    connect(ui.btnDecreaseGamma, SIGNAL(clicked()), this, SLOT(decreaseGamma()));
 
    connect(ui.btnRight, SIGNAL(clicked()), this, SLOT(moveLeft()));
    connect(ui.btnStop, SIGNAL(clicked()), this, SLOT(stopMoving()));
    connect(ui.btnResetJoinAngles, SIGNAL(clicked()), this, SLOT(reset()));
}
 
TCPMover::~TCPMover()
{
}
 
void
armarx::TCPMover::loadSettings(QSettings* settings)
{
    tcpMoverUnitName =
        settings->value("TCPControlUnitName", "TCPControlUnit").toString().toStdString();
}
 
void
armarx::TCPMover::saveSettings(QSettings* settings)
{
    settings->setValue("TCPControlUnitName", tcpMoverUnitName.c_str());
}
 
QPointer<QDialog>
TCPMover::getConfigDialog(QWidget* parent)
{
    if (!configDialog)
    {
        configDialog = new TCPMoverConfigDialog(parent);
    }
 
    return qobject_cast<TCPMoverConfigDialog*>(configDialog);
}
 
void
TCPMover::configured()
{
    tcpMoverUnitName = qobject_cast<TCPMoverConfigDialog*>(getConfigDialog())
                           ->proxyFinder->getSelectedProxyName()
                           .toStdString();
}
 
void
armarx::TCPMover::onInitComponent()
{
    //    kinematicUnitFile = getProperty<std::string>("RobotFileName", KINEMATIC_UNIT_FILE_DEFAULT).getValueOrDefault();
    //    kinematicUnitName = getProperty<std::string>("RobotNodeSetName",KINEMATIC_UNIT_NAME_DEFAULT).getValueOrDefault();
 
    //    // Load Robot
    //    ARMARX_VERBOSE << ": Loading KinematicUnit " << kinematicUnitName << " from '" << kinematicUnitFile << "' ..." << flush;
    //    robot = RobotIO::loadRobot(kinematicUnitFile);
    //    if (!robot)
    //    {
    //        ARMARX_ERROR << "Could not find Robot XML file with name '" << kinematicUnitFile << "'" << flush;
    //    }
 
 
    //    usingProxy(kinematicUnitName);
    usingProxy(tcpMoverUnitName);
    usingProxy("RobotStateComponent");
}
 
void
armarx::TCPMover::onConnectComponent()
{
 
    tcpMoverUnitPrx = getProxy<TCPControlUnitInterfacePrx>(tcpMoverUnitName);
    robotStateComponentPrx = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
 
    robotPrx = robotStateComponentPrx->getSynchronizedRobot();
    NameList nodeSets = robotPrx->getRobotNodeSets();
    QStringList nodeSetsQStr;
 
    for (unsigned int i = 0; i < nodeSets.size(); i++)
    {
        if (!robotPrx->getRobotNodeSet(nodeSets.at(i))->tcpName.empty())
        {
            tcpData[nodeSets.at(i)].resize(6, 0.f);
            nodeSetsQStr << QString::fromStdString(nodeSets.at(i));
        }
    }
 
    QString selected = ui.cbselectedTCP->currentText();
    ui.cbselectedTCP->clear();
    ui.cbselectedTCP->addItems(nodeSetsQStr);
    int index = ui.cbselectedTCP->findText(selected);
 
    if (index != -1)
    {
        ui.cbselectedTCP->setCurrentIndex(index);
    }
 
    refFrame = robotStateComponentPrx->getSynchronizedRobot()->getRootNode()->getName();
 
    //    kinematicUnitPrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
 
    //    tcpNodeSetName = "LARM";
    //    tcpNodeSet = robot->getRobotNodeSet(tcpNodeSetName);
    //    NameControlModeMap modeMap;
    //    for(unsigned int i = 0; i < tcpNodeSet->getSize(); i++)
    //    {
    //        modeMap[tcpNodeSet->getNode(i)->getName()] = eVelocityControl;
    //    }
    //    kinematicUnitPrx->switchControlMode(modeMap);
    //    ik = DifferentialIKPtr(new DifferentialIK(tcpNodeSet));
    ui.gridLayout->setEnabled(false);
}
 
void
TCPMover::onExitComponent()
{
 
    //    if(robotRequested)
    //        kinematicUnitPrx->release();
}
 
void
TCPMover::moveUp()
{
    //    moveRelative(-1,0,0);
    tcpData[selectedTCP][0]++;
    execMove();
}
 
void
TCPMover::moveDown()
{
    //    moveRelative(1,0,0);
    tcpData[selectedTCP][0]--;
    execMove();
}
 
void
TCPMover::moveZUp()
{
    tcpData[selectedTCP][2]++;
    execMove();
}
 
void
TCPMover::moveZDown()
{
    tcpData[selectedTCP][2]--;
    execMove();
}
 
void
TCPMover::moveLeft()
{
    //    moveRelative(0,1,0);
    tcpData[selectedTCP][1]++;
    execMove();
}
 
void
TCPMover::moveRight()
{
    tcpData[selectedTCP][1]--;
    execMove();
 
    //    moveRelative(0,-1,0);
}
 
void
TCPMover::increaseAlpha()
{
    tcpData[selectedTCP].at(3) += 2.f;
    execMove();
}
 
void
TCPMover::decreaseAlpha()
{
    tcpData[selectedTCP].at(3) -= 2.f;
    execMove();
}
 
void
TCPMover::increaseBeta()
{
    tcpData[selectedTCP].at(4) += 2.f;
    execMove();
}
 
void
TCPMover::decreaseBeta()
{
    tcpData[selectedTCP].at(4) -= 2.f;
    execMove();
}
 
void
TCPMover::increaseGamma()
{
    tcpData[selectedTCP].at(5) += 2.f;
    execMove();
}
 
void
TCPMover::decreaseGamma()
{
    tcpData[selectedTCP].at(5) -= 2.f;
    execMove();
}
 
void
TCPMover::stopMoving()
{
    tcpData[selectedTCP][0] = tcpData[selectedTCP][1] = tcpData[selectedTCP][2] = 0.0f;
 
    execMove();
}
 
void
TCPMover::reset()
{
    tcpData[selectedTCP][0] = tcpData[selectedTCP][1] = tcpData[selectedTCP][2] = 0.0f;
    tcpData[selectedTCP].at(3) = tcpData[selectedTCP].at(4) = tcpData[selectedTCP].at(5) = 0.0f;
    execMove();
    tcpMoverUnitPrx->release();
    //     tcpMoverUnitPrx->resetArmToHomePosition(eLeftHand);
}
 
void
TCPMover::robotControl(bool request)
{
    tcpMoverUnitPrx->request();
    //    try{
    //        if(request)
    //            kinematicUnitPrx->request();
    //        else
    //            kinematicUnitPrx->release();
    //    }catch(ResourceUnavailableException &e){
    //        ui.BtnRequestControl->setChecked(false);
 
    //    }catch(ResourceNotOwnedException &e){
    //        ui.BtnRequestControl->setChecked(true);
    //    }
    //    robotRequested = ui.BtnRequestControl->isChecked();
}
 
void
TCPMover::selectHand(int index)
{
    selectedTCP = ui.cbselectedTCP->itemText(index).toStdString();
}
 
void
TCPMover::execMove()
{
    ARMARX_INFO << "Setting new velos and orientation";
    ui.lblXValue->setText(QString::number(tcpData[selectedTCP][0]));
    ui.lblYValue->setText(QString::number(tcpData[selectedTCP][1]));
    ui.lblZValue->setText(QString::number(tcpData[selectedTCP][2]));
    ui.lblAlphaValue->setText(QString::number(tcpData[selectedTCP].at(3)));
    ui.lblBetaValue->setText(QString::number(tcpData[selectedTCP].at(4)));
    ui.lblGammaValue->setText(QString::number(tcpData[selectedTCP].at(5)));
 
    Eigen::Vector3f vec;
    vec << tcpData[selectedTCP][0], tcpData[selectedTCP][1], tcpData[selectedTCP][2];
    vec *= ui.sbFactor->value();
    const auto agentName = robotPrx->getName();
    FramedDirectionPtr tcpVel = new FramedDirection(vec, refFrame, agentName);
    Eigen::Vector3f vecOri;
    vecOri << tcpData[selectedTCP].at(3) / 180.f * 3.145f,
        tcpData[selectedTCP].at(4) / 180.f * 3.145f, tcpData[selectedTCP].at(5) / 180.f * 3.145f;
    vecOri *= ui.sbFactor->value();
    FramedDirectionPtr tcpOri = new FramedDirection(vecOri, refFrame, agentName);
 
    if (!ui.cbTranslation->isChecked())
    {
        tcpVel = NULL;
    }
 
    if (!ui.cbOrientation->isChecked())
    {
        tcpOri = NULL;
    }
 
    tcpMoverUnitPrx->begin_setTCPVelocity(
        selectedTCP, ui.edtTCPName->text().toStdString(), tcpVel, tcpOri);
 
    //    tcpMoverUnitPrx->setCartesianTCPVelocity(selectedTCP, handData[selectedTCP][0], handData[selectedTCP][1], handData[selectedTCP][2], ui.sbFactor->value());
    //    tcpMoverUnitPrx->setTCPOrientation(selectedTCP, handData[selectedTCP].at(3)/180.f*3.145f, handData[selectedTCP].at(4)/180.f*3.145f, handData[selectedTCP].at(5)/180.f*3.145f);
}
 
void
TCPMover::moveRelative(float x, float y, float z)
{
    //    RobotNodePtr tcpNode = tcpNodeSet->getTCP();
    //    // calculate cartesian error
    //    Eigen::Matrix4f newPosRelative;
    //    newPosRelative
    //            << 1, 0, 0, 10*ui.sbFactor->value(),
    //               0, 1, 0, 0,
    //               0, 0, 1, 0,
    //               0, 0, 0, 1;
    //    Eigen::VectorXf errorCartVec(3);
    //    errorCartVec << x*ui.sbFactor->value(),y*ui.sbFactor->value(),z*ui.sbFactor->value();
    ////    errorCartVec.segment(0,3) = newPosRelative.block(0,3,3,1);
 
 
    //    Eigen::MatrixXf Ji = ik->getPseudoInverseJacobianMatrix(tcpNode, IKSolver::Position);
    //    // calculat joint error
    //    Eigen::VectorXf errorJoint(tcpNodeSet->getSize());
    //    errorJoint = Ji * errorCartVec;
    //    std::vector<float> angles = tcpNodeSet->getJointValues();
    //    std::vector<float> targetAngles(tcpNodeSet->getSize());
    //    NameValueMap targetAnglesMap;
 
    //    for(unsigned int i = 0; i < tcpNodeSet->getSize(); i++)
    //    {
    ////        float newAngle = angles[i] + errorJoint(i);
    //        float newAngle = errorJoint(i);
    //        targetAngles[i] = newAngle;
    //        targetAnglesMap[tcpNodeSet->getNode(i)->getName()] = newAngle;
    //        //ARMARX_VERBOSE << tcpNodeSet->getNode(i)->getName() << ": " << newAngle;
    //    }
    //    tcpNodeSet->setJointValues(targetAngles);
    //    kinematicUnitPrx->setJointVelocities(targetAnglesMap);
}
 
/*void TCPMoverConfigDialog::setupUI(QWidget *parent)
{
    this->setWindowTitle("Enter name of TCPMoverUnit");
 
    layout = new QGridLayout(parent);
 
 
    proxyFinder->setSearchMask("*Unit");
    layout->addWidget(proxyFinder, 0, 0, 1, 2);
 
    //label = new QLabel("TCPControlUnit name:", parent);
    //layout->addWidget(label, 0,0);
 
    //editTCPMoverUnitName = new QLineEdit("TCPControlUnit", parent);
    //layout->addWidget(editTCPMoverUnitName, 0, 1);
 
    buttonBox = new QDialogButtonBox(parent);
    buttonBox->setObjectName(QString::fromUtf8("buttonBox"));
    buttonBox->setOrientation(Qt::Horizontal);
    buttonBox->setStandardButtons(QDialogButtonBox::Cancel|QDialogButtonBox::Ok);
 
    layout->addWidget(buttonBox, 1, 0, 1, 2);
 
 
    QObject::connect(buttonBox, SIGNAL(accepted()), this, SLOT(accept()));
    QObject::connect(buttonBox, SIGNAL(rejected()), this, SLOT(reject()));
 
 
 
}
 
 
TCPMoverConfigDialog::TCPMoverConfigDialog(QWidget *parent) :
    QDialog(parent)
{
    proxyFinder = new IceProxyFinder<TCPControlUnitInterfacePrx>(this);
    setupUI(this);
}*/