RobotVisualizationController.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    ArmarXGuiPlugins::RobotTrajectoryDesigner::Visualization
 * @author     Timo Birr
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "RobotVisualizationController.h"
 
#include <QGridLayout>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include "RobotAPI/libraries/core/Trajectory.h"
#include "RobotAPI/libraries/core/TrajectoryController.h"
 
#include "../Environment.h"
#include "../KinematicSolver.h"
#include "../Util/OrientationConversion.h"
#include "../Visualization/DesignerTrajectoryPlayer.h"
#include "VirtualRobot/RobotNodeSet.h"
#include "VirtualRobot/XML/RobotIO.h"
 
#define ROBOT_UPDATE_TIMER_MS 33
 
 
using namespace armarx;
using namespace VirtualRobot;
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// INITIALIZATION
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
RobotVisualizationController::RobotVisualizationController(QWidget* parent) : parent(parent)
{
    onInitComponent();
    onConnectComponent();
}
 
RobotVisualizationController::~RobotVisualizationController()
{
}
 
void
RobotVisualizationController::onInitComponent()
{
    //initialize Attributes
    this->cs = IKSolver::CartesianSelection::All;
    this->selectedWayPoint = 0;
    this->selectedTransition = -1;
    this->iKCallback = true;
    this->playerRunning = false;
 
 
    //Initialize Viewer Widget
    QWidget* viewerParent = new QWidget();
    this->viewer = RobotVisualizationPtr(new CoinRobotViewerAdapter(viewerParent));
    this->viewSplitter = new RobotVisualizationWidget(0, viewerParent, viewer);
    dynamic_cast<QGridLayout*>(parent->layout())
        ->addWidget(viewSplitter,
                    0,
                    0,
                    1,
                    3); //this needs to be done in order to let Toolbar and view overlap
    if (environment != NULL && environment->getRobot())
    {
        robotChanged(environment->getRobot());
    }
    this->setCamera(0);
    ARMARX_INFO << "RobotTrajectoryDesigner: RobotVisualizationController on init component";
}
 
void
RobotVisualizationController::onConnectComponent()
{
    ARMARX_INFO << "RobotTrajectoryDesigner: RobotVisualizationController on connect component";
}
 
void
RobotVisualizationController::onDisconnectComponent()
{
    ARMARX_INFO << "RobotTrajectoryDesigner: RobotVisualizationController on disconnect";
}
 
void
RobotVisualizationController::onExitComponent()
{
    ARMARX_INFO << "RobotTrajectoryDesigner: RobotVisualizationController on exit components";
}
 
void
RobotVisualizationController::refresh()
{
    //UPDATE WAYPOINT SELECTION
    if (selectedWayPoint != this->viewer->getSelectedWaypoint())
    {
        selectedWayPoint = this->viewer->getSelectedWaypoint();
        emit wayPointSelected(selectedWayPoint);
        if (currentTrajectory && static_cast<unsigned>(viewer->getSelectedWaypoint()) <
                                     currentTrajectory->getAllUserWaypoints().size())
        {
            std::vector<double> angles =
                currentTrajectory->getUserWaypoint(viewer->getSelectedWaypoint())->getJointAngles();
            viewer->setManipulator(currentTrajectory->getRns(),
                                   std::vector<float>(angles.begin(), angles.end()));
        }
    }
    //UPDATE MANIPULATOR MOVEMENT
    if (!selectedKinematicChain)
    {
        return;
    }
    emit manipulatorChanged(viewer->getManipulatorPose());
    KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
    PoseBasePtr destination = PoseBasePtr(new Pose(viewer->getManipulatorPose()));
    std::string currentRnsName = selectedKinematicChain->getName();
    std::vector<float> jointAngles =
        ks->solveRecursiveIK(this->robot->getRobotNodeSet(currentRnsName),
                             robot->getRobotNodeSet(currentRnsName)->getJointValues(),
                             destination,
                             cs);
    if (jointAngles.size() != 0 && iKCallback)
    {
        robot->getRobotNodeSet(currentRnsName)->setJointValues(jointAngles);
        emit poseReachable(true);
        emit tcpPoseChanged(robot->getRobotNodeSet(currentRnsName)->getTCP()->getGlobalPose());
    }
    else
    {
        emit poseReachable(false);
    }
}
 
void
RobotVisualizationController::addConnection(RobotVisualizationControllerPtr ctr)
{
    viewer->setObserver(ctr);
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// PARALLEL VIEWS
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
RobotVisualizationController::addView()
{
    this->viewSplitter->addWidget();
}
 
void
RobotVisualizationController::removeView()
{
    this->viewSplitter->removeWidget();
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// UPDATING OF VISUALIZATION
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
void
RobotVisualizationController::environmentChanged(EnvironmentPtr environment)
{
    this->environment = environment;
    robotChanged(environment->getRobot());
}
 
void
RobotVisualizationController::robotChanged(VirtualRobot::RobotPtr robot)
{
    this->robot = robot;
    if (viewer)
    {
        viewer->addRobotVisualization(this->robot, "");
    }
    kinematicChainChanged(NULL);
    this->currentTrajectory = NULL;
    this->selectedWayPoint = 0;
    this->selectedTransition = -1;
    this->setCamera(0);
}
 
void
RobotVisualizationController::updateViews(armarx::DesignerTrajectoryPtr trajectory)
{
    /*for (RobotVisualizationPtr visu : viewers)
    {
        visu->clearTrajectory();
    }*/
    viewer->clearTrajectory();
 
 
    if (!trajectory)
    {
        return;
    }
 
    this->currentTrajectory = trajectory;
    this->kinematicChainChanged(currentTrajectory->getRns());
    //clearViewers
 
 
    //Add all UserWayPoints
    int i = 0;
    if (currentTrajectory->getAllUserWaypoints().size() == 0)
    {
        return;
    }
    for (UserWaypointPtr currentPoint : currentTrajectory->getAllUserWaypoints())
    {
 
        VirtualRobot::EndEffectorPtr endEffector =
            robot->getEndEffector(trajectory->getRns()->getTCP()->getName());
        if (!endEffector)
        {
            std::vector<VirtualRobot::EndEffectorPtr> eefs;
            robot->getEndEffectors(eefs);
            for (auto eef : eefs)
            {
                if (eef->getTcp() == trajectory->getRns()->getTCP())
                {
                    endEffector = eef;
                    break;
                }
            }
        }
        Pose localPose =
            Pose(currentPoint->getPose()->position, currentPoint->getPose()->orientation);
        if (endEffector)
        {
 
            viewer->addWaypointVisualization(
                i,
                PosePtr(new Pose(trajectory->getRns()->getKinematicRoot()->toGlobalCoordinateSystem(
                    localPose.toEigen()))),
                endEffector);
            //visu->addWaypointVisualization(i, currentPoint->getPose(), robot->getEndEffector(robot->getRobotNodeSet(trajectory->getRns()->getName())->getTCP()->getName()));
        }
        else
        {
            viewer->addWaypointVisualization(
                i,
                PosePtr(new Pose(trajectory->getRns()->getKinematicRoot()->toGlobalCoordinateSystem(
                    localPose.toEigen()))),
                NULL);
        }
        i++;
    }
 
 
    //Add all Transitions
    if (currentTrajectory->getAllUserWaypoints().size() == 1)
    {
        return;
    }
    KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
    for (unsigned int k = 0; k < currentTrajectory->getNrOfUserWaypoints() - 1; k++)
    {
        TrajectoryPtr currentTransition = currentTrajectory->getTransition(k)->getTrajectory();
        std::vector<PoseBasePtr> curve = std::vector<PoseBasePtr>();
        for (const Trajectory::TrajData& element : *currentTransition)
        {
            std::vector<double> jointValues = element.getPositions();
            PoseBasePtr pose = ks->doForwardKinematic(currentTrajectory->getRns(), jointValues);
            curve.push_back(pose);
        }
        viewer->addTransitionVisualization(k, curve);
        if (static_cast<signed>(k) == selectedTransition)
        {
            viewer->highlightTransitionVisualization(k, curve);
        }
    }
    //ARMARX_INFO << "DEBUG END";
}
 
void
RobotVisualizationController::displayAllWayPoints(bool display)
{
    //TODO find out sphere size
}
 
void
RobotVisualizationController::selectedTransitionChanged(int index)
{
    KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, robot);
    TrajectoryPtr selectedTransition = currentTrajectory->getTransition(index)->getTrajectory();
 
    std::vector<PoseBasePtr> curve = std::vector<PoseBasePtr>();
 
    for (const Trajectory::TrajData& element : *selectedTransition)
    {
        std::vector<double> jointValues = element.getPositions();
        PoseBasePtr pose = ks->doForwardKinematic(currentTrajectory->getRns(), jointValues);
        curve.push_back(pose);
    }
    /*for (RobotVisualizationPtr visu : viewers)*/
    {
 
        if (currentTrajectory->getNrOfUserWaypoints() >
                static_cast<unsigned>(this->selectedTransition + 1) &&
            this->selectedTransition >= 0)
        {
            TrajectoryPtr deselectedTransition =
                currentTrajectory->getTransition(this->selectedTransition)->getTrajectory();
            std::vector<PoseBasePtr> oldCurve = std::vector<PoseBasePtr>();
 
            for (const Trajectory::TrajData& element : *deselectedTransition)
            {
 
                std::vector<double> jointValues = element.getPositions();
                PoseBasePtr pose = ks->doForwardKinematic(currentTrajectory->getRns(), jointValues);
                oldCurve.push_back(pose);
            }
            viewer->removeTransitionVisualization(this->selectedTransition);
            viewer->addTransitionVisualization(this->selectedTransition, oldCurve);
        }
        viewer->highlightTransitionVisualization(index, curve);
    }
    this->selectedTransition = index;
}
 
void
RobotVisualizationController::clearView()
{
    viewer->clearTrajectory();
    this->currentTrajectory = NULL;
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// IK CALLBACK METHODS
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
RobotVisualizationController::cartesianSelectionChanged(
    VirtualRobot::IKSolver::CartesianSelection cs)
{
    this->cs = IKSolver::CartesianSelection::All;
    refresh();
}
 
void
RobotVisualizationController::kinematicChainChanged(RobotNodeSetPtr rns)
{
    viewer->removeAllWaypointVisualizations();
    //IKCallback
 
    this->selectedKinematicChain = rns;
 
    if (selectedKinematicChain)
    {
        std::vector<float> jointAngles;
        jointAngles = std::vector<float>(
            robot->getRobotNodeSet(this->selectedKinematicChain->getName())->getJointValues());
        viewer->setManipulator(this->selectedKinematicChain, jointAngles);
    }
    this->updateViews(NULL);
}
 
void
RobotVisualizationController::setIKCallbackEnabled(bool enabled)
{
    iKCallback = enabled;
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// CAMERA UPDATING METHODS
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
RobotVisualizationController::setCamera()
{
    setCamera(0);
}
 
void
RobotVisualizationController::setCamera(int perspective)
{
    Eigen::Vector3f position;
    Eigen::Vector3f pointAtA = Eigen::Vector3f(0, 0, 0);
    Eigen::Vector3f pointAtB;
 
    switch (perspective)
    {
        case 0:
            //HIGH ANGLE
            position = Eigen::Vector3f(0, 2, 3.5);
            pointAtB = Eigen::Vector3f(0, 0, 5);
            break;
 
        case 1:
            //TOP
            position = Eigen::Vector3f(0, 0, 4);
            pointAtB = Eigen::Vector3f(0, -360, 0);
            break;
 
        case 2:
            //FRONT
            position = Eigen::Vector3f(0, 5, 2);
            pointAtB = Eigen::Vector3f(0, 0, 2);
            break;
        case 3:
            //BACK
            position = Eigen::Vector3f(0, -5, 2);
            pointAtB = Eigen::Vector3f(0, 0, 2);
            break;
 
        case 4:
            //LEFT
            position = Eigen::Vector3f(5, 0, 2);
            pointAtB = Eigen::Vector3f(0, 0, 2);
            break;
        case 5:
            //RIGHT
            position = Eigen::Vector3f(-5, 0, 2);
            pointAtB = Eigen::Vector3f(0, 0, 2);
            break;
        default:
            position = Eigen::Vector3f(0, 2, 3.5);
            pointAtB = Eigen::Vector3f(0, 0, 5);
    }
    this->viewSplitter->setCameraOfFocused(position, pointAtA, pointAtB);
    /*RobotVisualizationPtr expectedViewer = viewers[0];
    expectedViewer->setCamera(position, pointAtA, pointAtB);
    expectedViewer->updateRobotVisualization();*/
}
 
void
RobotVisualizationController::selectedWayPointChanged(int index)
{
    viewer->setSelectedWaypoint(index);
    if (currentTrajectory)
    {
        ARMARX_WARNING << "robot visualisation controller trying to update selected waypoint";
        std::vector<double> jA = currentTrajectory->getUserWaypoint(index)->getJointAngles();
        std::vector<float> jointAngles = std::vector<float>(jA.begin(), jA.end());
        viewer->setManipulator(currentTrajectory->getRns(), jointAngles);
        this->cartesianSelectionChanged(
            currentTrajectory->getUserWaypoint(index)->getIKSelection());
    }
}
 
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/// PLAY TRAJECTORY
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
void
RobotVisualizationController::playTrajectory()
{
    player = DesignerTrajectoryPlayerPtr(new DesignerTrajectoryPlayer(this->viewer, this->robot));
    player->addTrajectory(currentTrajectory);
    this->playerStarter();
}
 
void
RobotVisualizationController::playTrajectories(std::vector<DesignerTrajectoryPtr> trajectories)
{
    player = DesignerTrajectoryPlayerPtr(new DesignerTrajectoryPlayer(this->viewer, this->robot));
    for (DesignerTrajectoryPtr current : trajectories)
    {
        player->addTrajectory(current);
    }
    this->playerStarter();
}
 
void
RobotVisualizationController::trajectoryPlayerStopped()
{
    player = NULL;
    KinematicSolver::getInstance(NULL, NULL)->syncRobotPrx();
    updateViews(currentTrajectory);
    setIKCallbackEnabled(true);
    viewer->setManipulator(
        selectedKinematicChain,
        robot->getRobotNodeSet(selectedKinematicChain->getName())->getJointValues());
    playerRunning = false;
    emit trajectoryPlayerNotPlaying(!playerRunning);
    emit trajectoryPlayerPlaying(playerRunning);
}
 
void
RobotVisualizationController::playerStarter()
{
    if (playerRunning || !selectedKinematicChain)
    {
        return;
    }
    playerRunning = true;
    emit trajectoryPlayerNotPlaying(!playerRunning);
    emit trajectoryPlayerPlaying(playerRunning);
    connect(player.get(), SIGNAL(finishedPlayback()), this, SLOT(trajectoryPlayerStopped()));
    viewer->clearTrajectory();
    viewer->setManipulator(NULL, std::vector<float>());
    setIKCallbackEnabled(false);
    player->startPlayback();
}