TrajectoryImporter.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::ImportExport
 * @author     Liran Dattner
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "TrajectoryImporter.h"
#include <RobotAPI/libraries/core/Trajectory.h>
#include <iostream>
#include <fstream>
#include <ArmarXCore/interface/observers/Serialization.h>
#include <ArmarXCore/interface/serialization/JSONSerialization.h>
#include <ArmarXCore/util/json/JSONObject.h>
#include <Ice/CommunicatorF.h>
#include "../KinematicSolver.h"
#include <algorithm>
 
armarx::TrajectoryImporter::TrajectoryImporter(armarx::EnvironmentPtr environment) : environment(environment)
{
 
}
 
std::vector<armarx::DesignerTrajectoryPtr> armarx::TrajectoryImporter::importTrajectory(std::string file)
{
    //Use initialized ice communicator instead of initializing a new one
    const Ice::Current& c = Ice::emptyCurrent;
    auto communicator = c.adapter.get()->getCommunicator();
    Ice::CommunicatorPtr ic = Ice::CommunicatorPtr(communicator);
 
    std::string jsonString;
 
    //Read a serialized trajectory from a file
    std::ifstream input;
    input.open(file);
    std::stringstream b;
    b << input.rdbuf();
    input.close();
 
    jsonString = b.str();
 
    //deserialize using the jsonSerializer
    JSONObject jsonSerializer = JSONObject(ic);
    jsonSerializer.fromString(jsonString);
 
    TrajectoryPtr trajectory = new Trajectory();
    trajectory->deserialize(&jsonSerializer, c);
    std::vector<std::string> nodeNames = trajectory->getDimensionNames();
    std::vector<VirtualRobot::RobotNodePtr> nodeSet;
    for (unsigned int i = 0; i < nodeNames.size(); i++)
    {
        nodeSet.push_back(environment->getRobot()->getRobotNode(nodeNames[i]));
    }
 
    VirtualRobot::RobotNodeSetPtr rns;
    //Get the correct rns
    for (VirtualRobot::RobotNodeSetPtr set : environment->getRobot()->getRobotNodeSets())   //Check if Robot has an rns that has EXACTLY the needed nodes.
    {
next:
        std::vector<VirtualRobot::RobotNodePtr> nodes = set->getAllRobotNodes();
        for (VirtualRobot::RobotNodePtr node : nodeSet)
        {
            if (std::find(nodes.begin(), nodes.end(), node) == nodes.end())
            {
                goto next;  //continue next;
            }
        }
        if (rns->getAllRobotNodes().size() == nodes.size())
        {
            rns = set;
            break;
        }
    }
 
    if (rns == NULL)
    {
        throw NotImplementedYetException(); //No rns has been found
    }
    //Make all points of the trajectory userWaypoints
    std::vector<UserWaypointPtr> points = std::vector<UserWaypointPtr>();
 
    for (unsigned int i = 1; i < trajectory->size(); i++)
    {
        std::vector<double> jointAngles;
        for (const Trajectory::TrajData& element : *trajectory)
        {
            jointAngles = element.getPositions();
        }
        PoseBasePtr pose = KinematicSolver::getInstance(environment->getScene(), environment->getRobot())->doForwardKinematic(rns, jointAngles);
        UserWaypointPtr userPoint = std::make_shared<UserWaypoint>(UserWaypoint(pose));
        //Make the points timeOptimalBreakPoints, so the calculation does not automatically get called
        userPoint->setIsTimeOptimalBreakpoint(true);
        points.push_back(userPoint);
    }
    DesignerTrajectoryPtr result = std::make_shared<DesignerTrajectory>(DesignerTrajectory(points[0], rns));
    for (unsigned int i = 1; i < points.size(); i++)
    {
        result->addLastUserWaypoint(points[i]);
    }
    return {result};
}