MotionFileWrapper.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2020, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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    RobotComponents::Libraries::MMM
 * @author     Andre Meixner
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "MotionFileWrapper.h"
 
#include <Eigen/LU>
 
#include <SimoxUtility/math/convert.h>
#include <SimoxUtility/xml/rapidxml/RapidXMLWrapper.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/observers/filters/ButterworthFilter.h>
 
#include <MMM/Model/ModelProcessorWinter.h>
#include <MMM/Motion/Legacy/LegacyMotion.h>
#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
#include <MMM/Motion/MotionReaderXML.h>
#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
#include <MMM/Motion/XMLTools.h>
 
using namespace armarx;
using namespace MMM;
 
MotionFileWrapperPtr
MotionFileWrapper::create(const std::string& motionFilePath,
                          double butterworthFilterCutOffFreq,
                          const std::string relativeModelRoot)
{
    MotionFileWrapperPtr wrapper(new MotionFileWrapper(butterworthFilterCutOffFreq));
 
    simox::xml::RapidXMLWrapperRootNodePtr root =
        simox::xml::RapidXMLWrapperRootNode::FromFile(motionFilePath);
    if (root && root->name() == xml::tag::MMM_ROOT)
    {
        if (!root->has_attribute(xml::attribute::VERSION))
        {
            if (!wrapper->loadLegacyMotion(motionFilePath))
            {
                return nullptr;
            }
        }
        else
        {
            std::string version = root->attribute_value(xml::attribute::VERSION);
            if (version == "2.0")
            {
                if (!wrapper->loadMotion(motionFilePath, relativeModelRoot))
                {
                    return nullptr;
                }
            }
            else if (version == "1.0 ")
            {
                if (!wrapper->loadLegacyMotion(motionFilePath, relativeModelRoot))
                {
                    return nullptr;
                }
            }
            else
            {
                ARMARX_ERROR << "Could not load mmm motion with version '" << version << "'";
                return nullptr;
            }
        }
    }
    else
    {
        return nullptr;
    }
 
    return wrapper;
}
 
MotionFileWrapper::MotionFileWrapper(int butterworthFilterCutOffFreq) :
    butterworthFilterCutOffFreq(butterworthFilterCutOffFreq)
{
}
 
bool
MotionFileWrapper::loadMotion(const std::string& motionFilePath,
                              const std::string relativeModelRoot)
{
    ARMARX_INFO << "Loading motion";
    MotionRecordingPtr motions;
    try
    {
        MotionReaderXMLPtr motionReader(new MotionReaderXML());
        motions = motionReader->loadMotionRecording(motionFilePath);
    }
    catch (Exception::MMMException& e)
    {
        ARMARX_ERROR << "Could not load mmm motion! " << e.what();
        return false;
    }
 
    Eigen::Matrix4f modelRootTransformation = Eigen::Matrix4f::Identity();
    for (auto motion : *motions)
    {
        ModelPtr model = motion->getModel();
        if (model)
        {
            if (!model->getFilename().empty() &&
                std::filesystem::path(model->getFilename()).stem() == relativeModelRoot)
            {
                auto modelPoseSensor = motion->getSensorByType<ModelPoseSensor>();
                if (modelPoseSensor && modelPoseSensor->getTimesteps().size() > 0)
                {
                    auto modelPoseSensorMeasurement = modelPoseSensor->getDerivedMeasurement(
                        modelPoseSensor->getTimesteps().at(0));
                    Eigen::Matrix4f rootPose = modelPoseSensorMeasurement->getRootPose();
                    Eigen::Vector3f orientation = simox::math::mat4f_to_rpy(rootPose);
                    modelRootTransformation.block<3,3>(0, 0) =
                        simox::math::rpy_to_mat3f(0, 0, -orientation(2));
                    modelRootTransformation.block<3,1>(0, 3) =
                        Eigen::Vector3f(-rootPose(0, 3), -rootPose(1, 3), 0);
                }
            }
        }
    }
 
    for (auto motion : *motions)
    {
        MotionDataPtr m(new MotionData());
 
        ModelPtr model = motion->getModel();
        if (model)
        {
            m->modelPath = model->getFilename(); // TODO if model could not be loaded!
        }
 
        ModelPoseSensorPtr modelPoseSensor = motion->getSensorByType<ModelPoseSensor>();
        if (!modelPoseSensor)
        {
            ARMARX_ERROR << "Ignoring motion with name '" << motion->getName()
                         << "', because it has no model pose sensor!";
            break;
        }
 
        std::vector<float> t = modelPoseSensor->getTimesteps();
        m->timestamp = ::Ice::DoubleSeq(t.begin(), t.end());
        m->numberOfFrames = m->timestamp.size();
        for (float timestep : m->timestamp)
        {
            ModelPoseSensorMeasurementPtr modelPoseSensorMeasurement =
                modelPoseSensor->getDerivedMeasurement(timestep);
            Eigen::Matrix4f pose =
                modelRootTransformation * modelPoseSensorMeasurement->getRootPose();
            m->poseTrajData[timestep] = getTrajData(pose);
        }
 
        KinematicSensorList kinematicSensors = motion->getSensorsByType<KinematicSensor>();
        for (KinematicSensorPtr kinematicSensor : kinematicSensors)
        {
            KinematicSensorPtr kSensor = kinematicSensor;
            std::vector<std::string> jointNames = kinematicSensor->getJointNames();
            m->jointNames.insert(m->jointNames.end(), jointNames.begin(), jointNames.end());
            if (butterworthFilterCutOffFreq > 0.0f)
            {
                kSensor = KinematicSensorPtr(new KinematicSensor(jointNames));
                std::vector<filters::ButterworthFilter> filters;
                for (size_t j = 0; j < jointNames.size(); j++)
                {
                    filters.push_back(
                        filters::ButterworthFilter(butterworthFilterCutOffFreq, 100, Lowpass, 5));
                }
 
                bool initialized = false;
                for (float timestep : m->timestamp)
                {
                    KinematicSensorMeasurementPtr kinematicSensorMeasurement =
                        kinematicSensor->getDerivedMeasurement(timestep); // TODO nullptr
                    if (!kinematicSensorMeasurement)
                    {
                        MMM_ERROR << "No kinematic measurement at timestep " << timestep
                                  << " for motion with name '" << motion->getName() << "'";
                        goto end;
                    }
                    Eigen::VectorXf jointAngles = kinematicSensorMeasurement->getJointAngles();
                    for (size_t j = 0; j < jointNames.size(); j++)
                    {
                        if (!initialized)
                        {
                            filters[j].setInitialValue(jointAngles[j]);
                            initialized = true;
                        }
                        filters[j].update(0, new Variant(jointAngles[j]));
                        jointAngles[j] = filters[j].getValue()->getDouble();
                    }
                    kSensor->addSensorMeasurement(KinematicSensorMeasurementPtr(
                        new KinematicSensorMeasurement(timestep, jointAngles)));
                }
            }
 
            for (float timestep : m->timestamp)
            {
                KinematicSensorMeasurementPtr kinematicSensorMeasurement =
                    kSensor->getDerivedMeasurement(timestep);
                if (kinematicSensorMeasurement)
                {
                    m->jointTrajData.push_back(
                        getTrajData(kinematicSensorMeasurement->getJointAngles()));
                }
                else
                {
                    MMM_ERROR << "No kinematic measurement at timestep " << timestep
                              << " for motion with name '" << motion->getName() << "'";
                    goto end;
                }
            }
        end:
            continue;
        }
        if (model)
        {
            m->scaling = model->getScaling();
        }
        else
        {
            auto modelProcessor =
                std::dynamic_pointer_cast<MMM::ModelProcessorWinter>(motion->getModelProcessor());
            if (modelProcessor)
            {
                m->scaling = modelProcessor->getHeight();
            }
            else
            {
                m->scaling = 1.0;
            }
        }
 
        motionNames.push_back(motion->getName());
        motionData[motion->getName()] = m;
    }
    return motionData.size();
}
 
bool
MotionFileWrapper::loadLegacyMotion(const std::string& motionFilePath,
                                    const std::string relativeModelRoot)
{
    ARMARX_INFO << "Loading legacy motion";
    LegacyMotionReaderXML reader;
    LegacyMotionList motions = reader.loadAllMotions(motionFilePath);
 
    Eigen::Matrix4f modelRootTransformation = Eigen::Matrix4f::Identity();
    for (auto motion : motions)
    {
        ModelPtr model = motion->getModel();
        std::filesystem::path filename =
            model ? std::filesystem::path(model->getFilename()) : motion->getModelFilePath();
        if (std::filesystem::path(filename).stem() == relativeModelRoot)
        {
            auto motionFrames = motion->getMotionFrames();
            if (motionFrames.size() > 0)
            {
                Eigen::Matrix4f rootPose = motionFrames.at(0)->getRootPose();
                Eigen::Vector3f orientation = simox::math::mat4f_to_rpy(rootPose);
                modelRootTransformation.block<3,3>(0, 0) =
                    simox::math::rpy_to_mat3f(0, 0, -orientation(2));
                modelRootTransformation.block<3,1>(0, 3) =
                    Eigen::Vector3f(-rootPose(0, 3), -rootPose(1, 3), 0);
            }
        }
    }
 
    for (auto motion : motions)
    {
        MotionDataPtr m(new MotionData());
        ModelPtr model = motion->getModel();
        m->modelPath = model ? model->getFilename() : motion->getModelFilePath().c_str();
        m->numberOfFrames = motion->getNumFrames();
        m->jointNames = motion->getJointNames();
 
        if (butterworthFilterCutOffFreq > 0.0f)
        {
            for (size_t j = 0; j < motion->getJointNames().size(); j++)
            {
                filters::ButterworthFilter filter(butterworthFilterCutOffFreq, 100, Lowpass, 5);
                filter.setInitialValue(motion->getMotionFrame(0)->joint[j]);
                for (size_t i = 0; i < motion->getNumFrames(); i++)
                {
                    filter.update(0, new Variant(motion->getMotionFrame(i)->joint[j]));
                    motion->getMotionFrame(i)->joint[j] = filter.getValue()->getDouble();
                }
            }
        }
 
 
        for (size_t i = 0; i < motion->getNumFrames(); ++i)
        {
            MMM::MotionFramePtr frame = motion->getMotionFrame(i);
            Eigen::Matrix4f pose = modelRootTransformation * frame->getRootPose();
            m->poseTrajData[frame->timestep] = getTrajData(pose);
            m->timestamp.push_back(frame->timestep);
            m->jointTrajData.push_back(getTrajData(frame->joint));
        }
 
        if (model)
        {
            m->scaling = model->getScaling();
        }
        else
        {
            auto modelProcessor =
                std::dynamic_pointer_cast<MMM::ModelProcessorWinter>(motion->getModelProcessor());
            if (modelProcessor)
            {
                m->scaling = modelProcessor->getHeight();
            }
            else
            {
                m->scaling = 1.0;
            }
        }
 
        motionNames.push_back(motion->getName());
        motionData[motion->getName()] = m;
    }
    return motionData.size();
}
 
::Ice::DoubleSeq
MotionFileWrapper::getTrajData(const Eigen::Matrix4f& rootPose)
{
    VirtualRobot::MathTools::Quaternion q = VirtualRobot::MathTools::eigen4f2quat(rootPose);
    ::Ice::DoubleSeq curData;
    curData.push_back(rootPose(0, 3));
    curData.push_back(rootPose(1, 3));
    curData.push_back(rootPose(2, 3));
    curData.push_back(q.w);
    curData.push_back(q.x);
    curData.push_back(q.y);
    curData.push_back(q.z);
    return curData;
}
 
DoubleSeqSeq
MotionFileWrapper::getTrajData(const Eigen::VectorXf& jointAngles)
{
    DoubleSeqSeq curdata;
    for (int j = 0; j < jointAngles.rows(); j++)
    {
        ::Ice::DoubleSeq dimData;
        dimData.push_back(jointAngles[j]);
        curdata.push_back(dimData);
    }
    return curdata;
}
 
MotionDataPtr
MotionFileWrapper::getFirstMotionData()
{
    if (motionNames.size() == 0)
    {
        return nullptr;
    }
    else
    {
        return motionData[motionNames.at(0)];
    }
}
 
MotionDataPtr
MotionFileWrapper::getMotionDataByModel(const std::string& modelName)
{
    for (auto m : motionData)
    {
        if (std::filesystem::path(m.second->modelPath).stem() == modelName)
        {
            return m.second;
        }
    }
    return nullptr;
}
 
MotionDataPtr
MotionFileWrapper::getMotionData(const std::string& motionName)
{
    return motionData[motionName];
}
 
::Ice::StringSeq
MotionFileWrapper::getModelNames()
{
    ::Ice::StringSeq modelNames;
    for (auto m : motionData)
    {
        modelNames.push_back(std::filesystem::path(m.second->modelPath).stem());
    }
    return modelNames;
}
 
::Ice::StringSeq
MotionFileWrapper::getMotionNames()
{
    return motionNames;
}