MMMExporter.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 "MMMExporter.h"
 
#include <clocale>
 
#include <boost/make_shared.hpp>
 
#include <QString>
#include <QtXml>
 
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include "../Model/DesignerTrajectory.h"
#include "../Model/UserWaypoint.h"
#include <MMM/Model/Model.h>
#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Motion/Motion.h>
#include <MMM/Motion/MotionWriterXML.h>
#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
#include <MMM/RapidXML/rapidxml.hpp>
 
armarx::MMMExporter::MMMExporter(EnvironmentPtr environment)
{
    this->environment = environment;
}
 
void
armarx::MMMExporter::exportTrajectory(std::vector<armarx::DesignerTrajectoryPtr> trajectories,
                                      const std::string file)
{
    //Ensure that locale is en - punctuation for doubles in exported file
    setlocale(LC_NUMERIC, "en_US.UTF-8");
    //ARMARX_INFO << "Received " << trajectories.size() << " trajectories";
    //Create a ModelPoseSensor. The sensors name is the used rns
    MMM::ModelReaderXMLPtr modelReader(new MMM::ModelReaderXML());
    MMM::ModelPtr modelptr = modelReader->loadModel(environment->getRobot()->getFilename());
    MMM::MotionWriterXML motionWriter;
    MMM::MotionList motions;
    MMM::RapidXMLWriter writer;
    MMM::RapidXMLWriterNodePtr metaDataNode = writer.createRootNode("RobotTrajectoryDesignerData");
    metaDataNode->append_string_node("Robot", environment->getRobot()->getFilename());
    MMM::Motion* kinematicMotion =
        new MMM::Motion("RobotTrajectoryDesigner", modelptr, modelptr, nullptr);
 
    MMM::ModelPoseSensor kinematicModelPoseSensor = MMM::ModelPoseSensor("Model");
    MMM::ModelPoseSensorMeasurementPtr kinematicModelPoseSensorMeasurement =
        boost::make_shared<MMM::ModelPoseSensorMeasurement>(MMM::ModelPoseSensorMeasurement(
            0,
            Eigen::Vector3f(1, 1, 1),
            MMM::Math::matrix4fToEulerXZY(MMM::Math::quat2eigen4f(0, 0, 0, 0))));
    kinematicModelPoseSensor.addSensorMeasurement(kinematicModelPoseSensorMeasurement);
    kinematicMotion->addSensor(boost::make_shared<MMM::ModelPoseSensor>(kinematicModelPoseSensor),
                               0);
 
    MMM::MotionPtr mptr(kinematicMotion);
    motions.push_back(mptr);
 
    //ARMARX_INFO << "Exporting";
    for (armarx::DesignerTrajectoryPtr trajectory : trajectories)
    {
        std::string location(TRAJECTORYDESIGNER_SOURCEDIR);
        location += "/data/RobotTrajectoryDesigner/Resources/UserWaypoint.xml";
        MMM::ModelPtr modelptr = modelReader->loadModel(location);
        MMM::Motion* m =
            new MMM::Motion(trajectory->getRns()->getName(), modelptr, modelptr, nullptr);
        MMM::ModelPoseSensor modelPoseSensor =
            MMM::ModelPoseSensor(trajectory->getRns()->getName());
        //Get all userWayPoints
        std::vector<armarx::UserWaypointPtr> userWayPoints = trajectory->getAllUserWaypoints();
        /*
        for (armarx::UserWaypointPtr up : userWayPoints)
        {
            ARMARX_INFO << "Exporting point:";
            ARMARX_INFO << up;
        }
        */
        //Add the userWaypoints to the ModelPoseSensor
        //ARMARX_INFO << "Now creating ModelPoses";
        MMM::RapidXMLWriterNodePtr trajectoryNode =
            metaDataNode->append_node(trajectory->getRns()->getName());
        MMM::RapidXMLWriterNodePtr userWaypointsNode = trajectoryNode->append_node("UserWaypoints");
        MMM::RapidXMLWriterNodePtr transitionsNode = trajectoryNode->append_node("Transitions");
        for (unsigned int index = 0; index < userWayPoints.size(); index++)
        {
            armarx::UserWaypointPtr userWayPoint = userWayPoints[index];
            //Get the reached Pose
            armarx::Vector3BasePtr pos = userWayPoint->getPose()->position;
            Eigen::Vector3f position = Eigen::Vector3f(pos->x, pos->y, pos->z);
            armarx::QuaternionBasePtr quaternion = userWayPoint->getPose()->orientation;
            Eigen::Matrix4f matrix = MMM::Math::quat2eigen4f(
                quaternion->qx, quaternion->qy, quaternion->qz, quaternion->qw);
            Eigen::Vector3f orientation = MMM::Math::matrix4fToEulerXZY(matrix);
            //Create a measurement containing the reached pose
            MMM::ModelPoseSensorMeasurementPtr measurement =
                boost::make_shared<MMM::ModelPoseSensorMeasurement>(MMM::ModelPoseSensorMeasurement(
                    userWayPoint->getUserTimestamp(), position, orientation));
            //Add the measurement to the sensor
            modelPoseSensor.addSensorMeasurement(measurement);
            MMM::RapidXMLWriterNodePtr userpointNode =
                userWaypointsNode->append_node("UserWaypoint");
            std::string jointAngleString;
            for (unsigned int j = 0; j < userWayPoints[index]->getJointAngles().size(); j++)
            {
                jointAngleString +=
                    boost::lexical_cast<std::string>(userWayPoints[index]->getJointAngles()[j]) +
                    " ";
            }
            MMM::RapidXMLWriterNodePtr jointAngleNode = userpointNode->append_string_node(
                "JointAngles", jointAngleString.substr(0, jointAngleString.size() - 1));
            MMM::RapidXMLWriterNodePtr timeOptimalTimeStampNode = userpointNode->append_string_node(
                "TimeOptimalTimeStamp",
                boost::lexical_cast<std::string>(
                    (userWayPoints[index]->getTimeOptimalTimestamp())));
            MMM::RapidXMLWriterNodePtr breakpointNode = userpointNode->append_string_node(
                "IsBreakPoint",
                std::to_string((int)(userWayPoints[index]->getIsTimeOptimalBreakpoint())));
            MMM::RapidXMLWriterNodePtr cartesianSelectionNode = userpointNode->append_string_node(
                "CartesianSelection",
                std::to_string((int)(userWayPoints[index]->getIKSelection())));
        }
        for (unsigned int index = 0; index < trajectory->getNrOfUserWaypoints() - 1; index++)
        {
            MMM::RapidXMLWriterNodePtr transitionNode = transitionsNode->append_node("Transition");
            MMM::RapidXMLWriterNodePtr timeOptimalDurationNode = transitionNode->append_string_node(
                "TimeOptimalDuration",
                boost::lexical_cast<std::string>(
                    trajectory->getTransition(index)->getTimeOptimalDuration()));
            MMM::RapidXMLWriterNodePtr userDurationNode = transitionNode->append_string_node(
                "UserDuration",
                boost::lexical_cast<std::string>(
                    trajectory->getTransition(index)->getUserDuration()));
            MMM::RapidXMLWriterNodePtr interpolationTypeNode = transitionNode->append_string_node(
                "InterpolationType",
                std::to_string((int)(trajectory->getTransition(index)->getInterpolationType())));
        }
        MMM::RapidXMLWriterNodePtr interBreakpointTrajectoriesNode =
            trajectoryNode->append_node("InterBreakpointTrajectories");
        for (unsigned int index = 0; index < trajectory->getInterBreakpointTrajectories().size();
             index++)
        {
            MMM::RapidXMLWriterNodePtr interBreakpointTrajectoryNode =
                interBreakpointTrajectoriesNode->append_node("InterBreakpointTrajectory");
            MMM::KinematicSensorPtr interBreakpointSensor(new MMM::KinematicSensor(
                trajectory->getRns()->getNodeNames(), trajectory->getRns()->getName()));
            for (double t : trajectory->getInterBreakpointTrajectories()[index]->getTimestamps())
            {
                std::vector<double> positions =
                    trajectory->getInterBreakpointTrajectories()[index]->getStates(t, 0);
                const Eigen::VectorXd vectord =
                    Eigen::VectorXd::Map(positions.data(), positions.size());
                const Eigen::VectorXf vector = vectord.cast<float>();
                MMM::KinematicSensorMeasurement* measurement =
                    new MMM::KinematicSensorMeasurement(t, vector);
                MMM::KinematicSensorMeasurementPtr measurementptr(measurement);
                interBreakpointSensor->addSensorMeasurement(measurementptr);
            }
            interBreakpointSensor->setName(trajectory->getRns()->getName());
            interBreakpointSensor->appendSensorXML(interBreakpointTrajectoryNode);
        }
        modelPoseSensor.setName(trajectory->getRns()->getName());
        //ARMARX_INFO << "Adding Sensor for " << trajectory->getRns()->getName();
        m->addSensor(boost::make_shared<MMM::ModelPoseSensor>(modelPoseSensor), 0);
 
        MMM::MotionPtr mptr(m);
        motions.push_back(mptr);
 
        MMM::KinematicSensorPtr kinematicSensor(new MMM::KinematicSensor(
            trajectory->getRns()->getNodeNames(), trajectory->getRns()->getName()));
        /*100FPS*/ //for (double t = 0; t < trajectory->getFinalTrajectory()->getLength(); t += 0.01)
        //Model implements fps conversion for finalTrajectory
        for (double t : trajectory->getFinalTrajectory()->getTimestamps())
        {
            std::vector<double> positions = trajectory->getFinalTrajectory()->getStates(t, 0);
            const Eigen::VectorXd vectord =
                Eigen::VectorXd::Map(positions.data(), positions.size());
            const Eigen::VectorXf vector = vectord.cast<float>();
            MMM::KinematicSensorMeasurement* measurement =
                new MMM::KinematicSensorMeasurement(t, vector);
            MMM::KinematicSensorMeasurementPtr measurementptr(measurement);
            kinematicSensor->addSensorMeasurement(measurementptr);
        }
        kinematicSensor->setName(trajectory->getRns()->getName());
        kinematicMotion->addSensor(kinematicSensor, 0);
    }
    //ARMARX_INFO << "Done with Sensors";
 
    //Write the motion to the target xml file
    //ARMARX_INFO << "Writing motion";
    std::string motionString = motionWriter.toXMLString(motions, file);
    //ARMARX_INFO << "Wrote Motion";
    std::string metaDataString = writer.print(false);
 
    //Cut and merge strings
    motionString = motionString.substr(0, motionString.find("</MMM>"));
    //ARMARX_INFO << metaDataString.find("<RobotTrajectoryDesignerData") << "loc";
    metaDataString = metaDataString.substr(metaDataString.find("<RobotTrajectoryDesignerData"));
    motionString = motionString + metaDataString + '\n' + "</MMM>";
 
    //Autoformat strings to xml format
    QString xmlInput = QString::fromStdString(motionString);
    QString xmlOutput;
    QXmlStreamReader qxmlreader(xmlInput);
    QXmlStreamWriter qxmlwriter(&xmlOutput);
    qxmlwriter.setAutoFormatting(true);
    while (!qxmlreader.atEnd())
    {
        qxmlreader.readNext();
        if (!qxmlreader.isWhitespace())
        {
            qxmlwriter.writeCurrentToken(qxmlreader);
        }
    }
 
    std::ofstream output;
    output.open(file);
    output << xmlOutput.toStdString();
    output.close();
}