MMMImporter.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 "MMMImporter.h"
#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
#include "../Environment.h"
#include "../KinematicSolver.h"
#include <MMM/RapidXML/rapidxml.hpp>
#include <VirtualRobot/RobotNodeSet.h>
#include "../Model/UserWaypoint.h"
#include <boost/make_shared.hpp>
#include <MMM/RapidXML/RapidXMLReader.h>
#include <MMM/Motion/MotionReaderXML.h>
#include <sstream>
#include <iostream>
#include <boost/lexical_cast.hpp>
#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensorFactory.h>
 
static constexpr const char* NODE_METADATA = "RobotTrajectoryDesignerData";
static constexpr const char* NODE_USERWAYPOINTS = "UserWaypoints";
static constexpr const char* NODE_USERWAYPOINT = "UserWaypoint";
static constexpr const char* NODE_JOINTANGLES = "JointAngles";
static constexpr const char* NODE_TIMEOPTIMALTIMESTAMP = "TimeOptimalTimeStamp";
static constexpr const char* NODE_ISBREAKPOINT = "IsBreakPoint";
static constexpr const char* NODE_IKSELECTION = "CartesianSelection";
 
static constexpr const char* NODE_TRANSITIONS = "Transitions";
static constexpr const char* NODE_TRANSITION = "Transition";
static constexpr const char* NODE_TIMEOPTIMALDURATION = "TimeOptimalDuration";
static constexpr const char* NODE_USERDURATION = "UserDuration";
static constexpr const char* NODE_INTERPOLATIONTYPE = "InterpolationType";
 
static constexpr const char* NODE_INTERBREAKPOINTTRAJECTORIES = "InterBreakpointTrajectories";
static constexpr const char* NODE_INTERBREAKPOINTTRAJECTORY = "InterBreakpointTrajectory";
 
static constexpr const char* NODE_ROBOT = "Robot";
 
armarx::MMMImporter::MMMImporter(armarx::EnvironmentPtr environment) : environment(environment)
{
 
}
 
std::vector<armarx::DesignerTrajectoryPtr> armarx::MMMImporter::importTrajectory(std::string file)
{
    //ARMARX_INFO << "Creating MotionReader";
    std::vector<std::string> libpaths;
    MMM::MotionReaderXMLPtr motionReader = boost::make_shared<MMM::MotionReaderXML>(libpaths);
    //ARMARX_INFO << "Getting motions";
    MMM::MotionList motions = motionReader->loadAllMotions(file, true);
    //motions.pop_back();     //Do not scan RobotTrajectoryDesignerData
    //ARMARX_INFO << motions.size() << "Motions detected";
    std::vector<DesignerTrajectoryPtr> trajectories;
    std::set<std::string> nodeSets;
    //ARMARX_INFO << "Scanning Motions";
    MMM::RapidXMLReaderPtr metaDataReader = MMM::RapidXMLReader::FromFile(file);
    MMM::RapidXMLReaderNodePtr mmmNode = metaDataReader->getRoot();
    MMM::RapidXMLReaderNodePtr metaDataNode = mmmNode->first_node(NODE_METADATA);
    MMM::RapidXMLReaderNodePtr robotNode = metaDataNode->first_node(NODE_ROBOT);
    if (robotNode->value() != environment->getRobot()->getFilename())
    {
        //ARMARX_INFO << "MMM file was exported for another robot";
        throw InvalidArgumentException("MMM file was exported for another robot");
    }
    for (MMM::MotionPtr motion : motions)
    {
        if (motion->getSensorsByType("ModelPoseSensor").size() > 1) //Assert that one ModelPoseSensor or none (one RobotNodeset or none) are affected
        {
            //ARMARX_INFO << "Motion with multiple RobotNodeSets";
            //Ignore every sensor except the first one
        }
        //ARMARX_INFO << "Making ModelPoseSensor from Motion";
        MMM::SensorPtr sensor = motion->getSensorsByType("ModelPose")[0];
        if (sensor == NULL)
        {
            break;
        }
        MMM::ModelPoseSensor mp = dynamic_cast<MMM::ModelPoseSensor&>(*sensor); //Will throw if sensor is not a modelposesensor
        MMM::ModelPoseSensorPtr modelPoseSensor = boost::make_shared<MMM::ModelPoseSensor>(mp);
        if (!environment->getRobot()->hasRobotNodeSet(motion->getName()))
        {
            //ARMARX_INFO << motion->getName() << " is not a rns";
            //Skip the motion
            continue;
        }
        //ARMARX_INFO << motion->getName() << " being imported";
        VirtualRobot::RobotNodeSetPtr rns = environment->getRobot()->getRobotNodeSet(motion->getName());
        MMM::RapidXMLReaderNodePtr trajectoryNode = metaDataNode->first_node(motion->getName().c_str());
        MMM::RapidXMLReaderNodePtr userWaypointsNode = trajectoryNode->first_node(NODE_USERWAYPOINTS);
        MMM::RapidXMLReaderNodePtr transitionsNode = trajectoryNode->first_node(NODE_TRANSITIONS);
 
        MMM::RapidXMLReaderNodePtr userWaypointNode = userWaypointsNode->first_node(NODE_USERWAYPOINT);
        MMM::RapidXMLReaderNodePtr transitionNode = transitionsNode->first_node(NODE_TRANSITION);
        std::vector<float> times = modelPoseSensor->getTimesteps();
        std::vector<UserWaypointPtr> userPoints;
        unsigned int breakpoints = 1;
        //ARMARX_INFO << "Filling in time info";
        for (double t : times)
        {
            if (t != 0)
            {
                userWaypointNode = userWaypointNode->next_sibling(NODE_USERWAYPOINT);
            }
            MMM::ModelPoseSensorMeasurementPtr measurement = modelPoseSensor->getDerivedMeasurement(t);
 
 
 
            //Get metaData
            //ARMARX_INFO << "Getting meta data";
            MMM::RapidXMLReaderNodePtr jointAnglesNode = userWaypointNode->first_node(NODE_JOINTANGLES);
            std::string jointAngleValues = jointAnglesNode->value();
            std::vector<double> jointAngles;
            std::istringstream js(jointAngleValues);
            std::string j;
            //ARMARX_INFO << "Reading jointangles";
            while (getline(js, j, ' '))
            {
                jointAngles.push_back(boost::lexical_cast<double>(j));
            }
            KinematicSolverPtr ks = KinematicSolver::getInstance(NULL, environment->getRobot());
            PoseBasePtr pose = ks->doForwardKinematicRelative(rns, jointAngles);
            UserWaypointPtr userPoint = std::make_shared<UserWaypoint>(UserWaypoint(pose));
            userPoint->setUserTimestamp(t);
            userPoint->setJointAngles(jointAngles);
            //ARMARX_INFO << "Reading timeOptimalTimeStamp";
            MMM::RapidXMLReaderNodePtr timeOptimalTimeStampNode = userWaypointNode->first_node(NODE_TIMEOPTIMALTIMESTAMP);
            userPoint->setTimeOptimalTimestamp(boost::lexical_cast<double>(timeOptimalTimeStampNode->value()));
            //ARMARX_INFO << "Reading ikSelection";
            MMM::RapidXMLReaderNodePtr ikSelectionNode = userWaypointNode->first_node(NODE_IKSELECTION);
            userPoint->setIKSelection((IKSelection)(std::stoi(ikSelectionNode->value())));
            //ARMARX_INFO << "Reading isTimeOptimalBreakpoint";
            MMM::RapidXMLReaderNodePtr isTimeOptimalBreakPointNode = userWaypointNode->first_node(NODE_ISBREAKPOINT);
            userPoint->setIsTimeOptimalBreakpoint(boost::lexical_cast<bool>(isTimeOptimalBreakPointNode->value()));
            if (boost::lexical_cast<bool>(isTimeOptimalBreakPointNode->value()))
            {
                if (t > 0 && t != times.back())
                {
                    breakpoints ++;
                }
            }
            userPoints.push_back(userPoint);
        }
        DesignerTrajectoryPtr trajectory = std::make_shared<DesignerTrajectory>(DesignerTrajectory(userPoints[0], rns));
        for (unsigned int i = 0; i < userPoints.size(); i++)
        {
            if (i > 0)
            {
                trajectory->addLastUserWaypoint(userPoints[i]);
            }
        }
        //ARMARX_INFO << "Processing transitions";
        for (unsigned int i = 0; i < userPoints.size() - 1; i++)
        {
            if (i > 0)
            {
                transitionNode = transitionNode->next_sibling(NODE_TRANSITION);
            }
            TransitionPtr transition = trajectory->getTransition(i);
            //ARMARX_INFO << "Reading timeOptimalDuration";
            MMM::RapidXMLReaderNodePtr timeOptimalDurationNode = transitionNode->first_node(NODE_TIMEOPTIMALDURATION);
            transition->setTimeOptimalDuration(boost::lexical_cast<double>(timeOptimalDurationNode->value()));
            //ARMARX_INFO << "Reading userDuration";
            MMM::RapidXMLReaderNodePtr userDurationNode = transitionNode->first_node(NODE_USERDURATION);
            transition->setUserDuration(boost::lexical_cast<double>(userDurationNode->value()));
            //ARMARX_INFO << "Reading interpolationType";
            MMM::RapidXMLReaderNodePtr interpolationTypeNode = transitionNode->first_node(NODE_INTERPOLATIONTYPE);
            transition->setInterpolationType((InterpolationType)(std::stoi(interpolationTypeNode->value())));
        }
        //ARMARX_INFO << "Processing interbreakpoint trajectories";
        std::vector<armarx::TrajectoryPtr> interBreakpointTrajectories;
        MMM::RapidXMLReaderNodePtr interBreakpointTrajectoriesNode = trajectoryNode->first_node(NODE_INTERBREAKPOINTTRAJECTORIES);
        MMM::RapidXMLReaderNodePtr interBreakpointTrajectoryNode = interBreakpointTrajectoriesNode->first_node(NODE_INTERBREAKPOINTTRAJECTORY);
        //ARMARX_INFO << breakpoints << " interbreakpoint trajectories";
        for (unsigned int i = 0; i < breakpoints; i++)
        {
            if (i > 0)
            {
                interBreakpointTrajectoryNode = interBreakpointTrajectoryNode->next_sibling(NODE_INTERBREAKPOINTTRAJECTORY);
            }
            MMM::KinematicSensorFactory factory;
            MMM::SensorPtr sensorPtr = factory.createSensor(interBreakpointTrajectoryNode->first_node(MMM::XML::NODE_SENSOR));
            MMM::KinematicSensor interBreakpointTrajectorySensor = dynamic_cast<MMM::KinematicSensor&>(*sensorPtr);
            std::vector<float> interBreakpointTimeStamps = interBreakpointTrajectorySensor.getTimesteps();
            std::vector<std::vector<double>> nodeData;
            //Initialize the vectors for the respective joints, fill in the first timestamp (0)
            for (unsigned int i = 0; i < rns->getSize(); i++)
            {
                MMM::KinematicSensorMeasurementPtr measurement = interBreakpointTrajectorySensor.getDerivedMeasurement(0);
                nodeData.push_back({measurement->getJointAngles()[i]});
            }
            //Fill in the data for all other timestamps into the now existing joint values
            for (float t : interBreakpointTimeStamps)
            {
                if (t == interBreakpointTimeStamps[0])
                {
                    continue;
                }
                MMM::KinematicSensorMeasurementPtr measurement = interBreakpointTrajectorySensor.getDerivedMeasurement(t);
                Eigen::VectorXf floatPositions = measurement->getJointAngles();
                std::vector<double> doublePositions;
                for (int j = 0; j < floatPositions.size(); j++)
                {
                    doublePositions.push_back(floatPositions[j]);
                }
                Eigen::VectorXd positions = Eigen::VectorXd::Map(&doublePositions[0], doublePositions.size());
                for (unsigned int i = 0; i < positions.size(); i++)
                {
                    nodeData[i].push_back(positions[i]);
                }
            }
            //Get the correct names for the dimensions
            Ice::StringSeq dimensionNames = rns->getNodeNames();
            //Construct the armarx Trajectory from the given data
            std::vector<double>doubleTimeStamps(interBreakpointTimeStamps.begin(), interBreakpointTimeStamps.end());
            TrajectoryPtr traj = new Trajectory(nodeData, doubleTimeStamps, dimensionNames);
            interBreakpointTrajectories.push_back(traj);
        }
        trajectory->setInterBreakpointTrajectories(interBreakpointTrajectories);
        trajectories.push_back(trajectory);
        if (nodeSets.find(rns->getName()) != nodeSets.end())
        {
            //ARMARX_INFO << "Multiple motions using the same RobotNodeSet";
            trajectories.clear();
            return trajectories;
        }
        nodeSets.insert(rns->getName());
    }
    //ARMARX_INFO << trajectories.size() << "RobotNodeSet Trajectories have been found and could be imported";
    for (DesignerTrajectoryPtr designerTrajectory : trajectories)
    {
        //ARMARX_INFO << "Trajectory with " << designerTrajectory->getAllUserWaypoints().size() << " points";
    }
    return trajectories;
    throw ("Not implemented");
}