MMMSimulation.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    ArmarXSimulation::components::MMMSimulation
 * @author     Andre Meixner ( andre dot meixner at kit dot edu )
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "MMMSimulation.h"
 
#include <Ice/ObjectAdapter.h>
 
#include <SimoxUtility/math/convert.h>
#include <VirtualRobot/MathTools.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
#include <ArmarXCore/observers/filters/ButterworthFilter.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <RobotAPI/components/RobotState/RobotStateComponent.h>
#include <RobotAPI/components/units/KinematicUnitObserver.h>
#include <RobotAPI/libraries/core/Trajectory.h>
 
#include <RobotComponents/components/MMMPlayer/MMMPlayer.h>
#include <RobotComponents/components/TrajectoryPlayer/TrajectoryPlayer.h>
#include <RobotComponents/libraries/MMM/MotionFileWrapper.h>
 
#include <ArmarXSimulation/components/KinematicUnitDynamicSimulation/KinematicUnitDynamicSimulation.h>
#include <ArmarXSimulation/components/RobotPoseUnitDynamicSimulation/RobotPoseUnitDynamicSimulation.h>
#include <ArmarXSimulation/components/SelfLocalizationDynamicSimulation/SelfLocalizationDynamicSimulation.h>
 
#include <MemoryX/interface/components/WorkingMemoryInterface.h>
 
using namespace armarx;
 
void
MMMSimulation::onInitComponent()
{
    usingProxy(getProperty<std::string>("SimulatorName").getValue());
    if (getProperty<bool>("LoadToMemory"))
    {
        usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
    }
 
    trajectoryLoaded = false;
}
 
bool
MMMSimulation::isMotionLoaded(const Ice::Current&)
{
    return motionData != nullptr;
}
 
bool
MMMSimulation::loadMMMFile(const std::string& filePath,
                           const std::string& projects,
                           bool createTrajectoryPlayer,
                           const Ice::Current&)
{
    if (filePath.empty())
    {
        return false;
    }
 
    std::unique_lock lock(mmmMutex);
 
    if (!projects.empty())
    {
        std::vector<std::string> proj = armarx::Split(projects, ",;", true, true);
 
        for (std::string& p : proj)
        {
            ARMARX_INFO << "Adding to datapaths of " << p;
            armarx::CMakePackageFinder finder(p);
 
            if (!finder.packageFound())
            {
                ARMARX_WARNING << "ArmarX Package " << p << " has not been found!";
            }
            else
            {
                ARMARX_INFO << "Adding to datapaths: " << finder.getDataDir();
                armarx::ArmarXDataPath::addDataPaths(finder.getDataDir());
            }
        }
    }
 
    ArmarXDataPath::getAbsolutePath(filePath, motionPath);
 
    motionWrapper = MotionFileWrapper::create(motionPath, 0);
    if (motionWrapper)
    {
        motionData = motionWrapper->getMotionDataByModel(modelFileName);
        if (motionData)
        {
            ARMARX_INFO << "Loaded motion with model " << modelFileName << " from file "
                        << filePath;
            if (createTrajectoryPlayer && !trajectoryPlayer)
            {
                this->createTrajectoryPlayer();
            }
            return true;
        }
        else
        {
            ARMARX_ERROR << "Failed to load motion with model " << modelFileName
                         << ". Valid model names are " << motionWrapper->getModelNames();
        }
    }
    return false;
}
 
void
MMMSimulation::playMotion(const Ice::Current&)
{
    if (isMotionLoaded() && trajectoryPlayer)
    {
        trajectoryPlayer->resetTrajectoryPlayer(true);
        trajectoryPlayer->startTrajectoryPlayer();
    }
}
 
void
MMMSimulation::pauseMotion(const Ice::Current&)
{
    if (trajectoryPlayer)
    {
        trajectoryPlayer->pauseTrajectoryPlayer();
    }
}
 
void
MMMSimulation::stopMotion(const Ice::Current&)
{
    if (trajectoryPlayer)
    {
        trajectoryPlayer->stopTrajectoryPlayer();
    }
}
 
void
MMMSimulation::setLoopBack(bool state, const Ice::Current&)
{
    if (trajectoryPlayer)
    {
        trajectoryPlayer->setLoopPlayback(state);
    }
}
 
void
MMMSimulation::loadTrajectory()
{
    if (isMotionLoaded() && trajectoryPlayer)
    {
        if (trajectoryLoaded)
        {
            trajectoryPlayer->resetTrajectoryPlayer(true);
        }
        TrajectoryBasePtr t = motionData->getJointTrajectory();
        TrajectoryPtr tr = IceInternal::Handle<Trajectory>::dynamicCast(t);
        trajectoryPlayer->loadJointTraj(t);
        trajectoryPlayer->loadBasePoseTraj(motionData->getPoseTrajectory());
        trajectoryLoaded = true;
    }
}
 
void
MMMSimulation::onConnectComponent()
{
    simulatorPrx =
        getProxy<SimulatorInterfacePrx>(getProperty<std::string>("SimulatorName").getValue());
 
    initTask = new RunningTask<MMMSimulation>(this, &MMMSimulation::initialize);
    initTask->start();
}
 
void
MMMSimulation::initialize()
{
    const std::string name = this->getName();
    agentName = getProperty<std::string>("AgentName").getValue();
    const std::string mmmModelFileProject =
        getProperty<std::string>("RobotFileNameProject").getValue();
    const std::string mmmModelFilePath = getProperty<std::string>("RobotFileName").getValue();
    const std::string mmmRobotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
    const std::string simulatorName = getProperty<std::string>("SimulatorName").getValue();
    //std::string robotName = getProperty<std::string>("RobotName").getValue();
    modelFileName = std::filesystem::path(mmmModelFilePath).stem();
 
    loadMMMFile(getProperty<std::string>("MMMFile").getValue(), "", false);
    if (hasProperty("Scaling") && motionData)
    {
        motionData->setScaling(getProperty<float>("Scaling").getValue());
    }
    modelScaling = motionData ? motionData->scaling : getProperty<float>("Scaling").getValue();
 
 
    Eigen::Vector3f position(getProperty<float>("StartPose.x").getValue(),
                             getProperty<float>("StartPose.y").getValue(),
                             getProperty<float>("StartPose.z").getValue());
    Eigen::Vector3f orientation(getProperty<float>("StartPose.roll").getValue(),
                                getProperty<float>("StartPose.pitch").getValue(),
                                getProperty<float>("StartPose.yaw").getValue());
    startPose = simox::math::pos_rpy_to_mat4f(position, orientation);
    std::string robotName = simulatorPrx->addScaledRobotName(
        agentName, ArmarXDataPath::getAbsolutePath(mmmModelFilePath), modelScaling);
    agentName = robotName;
    simulatorPrx->setRobotPose(robotName, PosePtr(new Pose(startPose)));
 
    ARMARX_INFO << "Added robot " << robotName << " to simulator";
 
    const std::string robotStateConfigName = "RobotStateComponent";
    {
        Ice::StringSeq pros;
        std::string packageName("RobotAPI");
        armarx::CMakePackageFinder finder(packageName);
        std::string appPath = finder.getBinaryDir() + "/RobotStateComponentRun";
        pros.push_back(appPath);
        Ice::PropertiesPtr properties = Ice::createProperties(pros);
        //Ice::PropertiesPtr properties = getIceProperties()->clone();
 
        const std::string robotSateConfPre = getConfigDomain() + "." + robotStateConfigName + ".";
        properties->setProperty(robotSateConfPre + "AgentName", agentName);
        properties->setProperty(robotSateConfPre + "RobotFileName", mmmModelFilePath);
        properties->setProperty(robotSateConfPre + "RobotFileNameProject", mmmModelFileProject);
        properties->setProperty(robotSateConfPre + "RobotNodeSetName", mmmRobotNodeSetName);
        properties->setProperty(robotSateConfPre + "RobotModelScaling",
                                std::to_string(modelScaling));
        properties->setProperty(robotSateConfPre + "ObjectName", robotStateConfigName + name);
        properties->setProperty(getConfigDomain() + ".RobotStateObserver." + "ObjectName",
                                "RobotStateObserver" + name);
        ARMARX_DEBUG << "creating unit " << robotStateConfigName
                     << " using these properties: " << properties->getPropertiesForPrefix("");
        IceInternal::Handle<RobotStateComponent> robotStateComponent =
            Component::create<RobotStateComponent>(
                properties, robotStateConfigName, getConfigDomain());
        //robotStateComponent->getSynchronizedRobot(Ice::emptyCurrent);
        getArmarXManager()->addObject(robotStateComponent);
    }
 
    if (getProperty<bool>("LoadToMemory"))
    {
        Ice::StringSeq pros;
        std::string packageName("ArmarXSimulation");
        armarx::CMakePackageFinder finder(packageName);
        std::string appPath = finder.getBinaryDir() + "/SelfLocalizationDynamicSimulationAppRun";
        pros.push_back(appPath);
        Ice::PropertiesPtr properties = Ice::createProperties(pros);
        //Ice::PropertiesPtr properties = getIceProperties()->clone();
 
        const std::string localizationConfigName = "SelfLocalizationDynamicSimulation";
        const std::string localizationConfPre =
            getConfigDomain() + "." + localizationConfigName + ".";
        properties->setProperty(localizationConfPre + "AgentName", agentName);
        properties->setProperty(localizationConfPre + "RobotStateComponentName",
                                robotStateConfigName + name);
        properties->setProperty(localizationConfPre + "RobotName", robotName);
        properties->setProperty(localizationConfPre + "SimulatorName", simulatorName);
        properties->setProperty(localizationConfPre + "ObjectName", localizationConfigName + name);
        properties->setProperty(localizationConfPre + "WorkingMemoryName",
                                getProperty<std::string>("WorkingMemoryName").getValue());
        ARMARX_DEBUG << "creating unit " << localizationConfigName
                     << " using these properties: " << properties->getPropertiesForPrefix("");
        IceInternal::Handle<SelfLocalizationDynamicSimulation> localizationComponent =
            Component::create<SelfLocalizationDynamicSimulation>(
                properties, localizationConfigName, getConfigDomain());
        getArmarXManager()->addObject(localizationComponent);
    }
 
    std::string kinematicUnitConfigName = "KinematicUnit";
    {
        const std::string kinematicUnitConfPre =
            getConfigDomain() + "." + kinematicUnitConfigName + ".";
        Ice::PropertiesPtr properties = getIceProperties()->clone();
        properties->setProperty(kinematicUnitConfPre + "RobotName", robotName);
        properties->setProperty(kinematicUnitConfPre + "RobotFileName", mmmModelFilePath);
        properties->setProperty(kinematicUnitConfPre + "RobotFileNameProject", mmmModelFileProject);
        properties->setProperty(kinematicUnitConfPre + "RobotNodeSetName", mmmRobotNodeSetName);
        properties->setProperty(kinematicUnitConfPre + "ObjectName",
                                kinematicUnitConfigName + name);
        properties->setProperty(kinematicUnitConfPre + "SimulatorName", simulatorName);
        ARMARX_DEBUG << "creating unit " << kinematicUnitConfigName
                     << " using these properties: " << properties->getPropertiesForPrefix("");
        IceInternal::Handle<KinematicUnit> kinematicUnitComponent =
            Component::create<KinematicUnitDynamicSimulation>(
                properties, kinematicUnitConfigName, getConfigDomain());
        kinematicUnitName = kinematicUnitComponent->getName();
        getArmarXManager()->addObject(kinematicUnitComponent);
    }
 
    const std::string robotposeUnitConfigName = "RobotPoseUnit";
    {
        const std::string robotposeUnitConfPre =
            getConfigDomain() + "." + robotposeUnitConfigName + ".";
        Ice::PropertiesPtr properties = getIceProperties()->clone();
        properties->setProperty(robotposeUnitConfPre + "RobotName", robotName);
        properties->setProperty(robotposeUnitConfPre + "ObjectName",
                                robotposeUnitConfigName + name);
        properties->setProperty(robotposeUnitConfPre + "SimulatorProxyName", simulatorName);
        ARMARX_DEBUG << "creating unit " << robotposeUnitConfigName
                     << " using these properties: " << properties->getPropertiesForPrefix("");
        IceInternal::Handle<RobotPoseUnit> robotposecUnitComponent =
            Component::create<RobotPoseUnitDynamicSimulation>(
                properties, robotposeUnitConfigName, getConfigDomain());
        robotPoseUnitName = robotposecUnitComponent->getName();
        getArmarXManager()->addObject(robotposecUnitComponent);
    }
 
    if (motionData)
    {
        if (!trajectoryPlayer)
        {
            this->createTrajectoryPlayer();
        }
        if (trajectoryPlayer)
        {
            loadTrajectory();
            if (getProperty<bool>("AutoPlay").getValue())
            {
                playMotion();
            }
        }
        else
        {
            ARMARX_ERROR
                << "Could not load motion as trajectory player could not be not instanciated";
        }
    }
}
 
void
MMMSimulation::createTrajectoryPlayer()
{
    Eigen::Vector3f position = simox::math::mat4f_to_pos(startPose);
    Eigen::Vector3f orientation = simox::math::mat4f_to_rpy(startPose);
    const std::string trajectoryPlayerConfigName = "TrajectoryPlayer";
    const std::string trajectoryPlayerConfPre =
        getConfigDomain() + "." + trajectoryPlayerConfigName + ".";
    Ice::PropertiesPtr properties = getIceProperties()->clone();
    properties->setProperty(trajectoryPlayerConfPre + "KinematicTopicName", kinematicUnitName);
    properties->setProperty(trajectoryPlayerConfPre + "KinematicUnitName", kinematicUnitName);
    properties->setProperty(trajectoryPlayerConfPre + "ObjectName",
                            trajectoryPlayerConfigName + getName());
    properties->setProperty(trajectoryPlayerConfPre + "LoopPlayback",
                            std::to_string(getProperty<bool>("LoopPlayback").getValue()));
    properties->setProperty(trajectoryPlayerConfPre + "Offset.x", std::to_string(position(0)));
    properties->setProperty(trajectoryPlayerConfPre + "Offset.y", std::to_string(position(1)));
    properties->setProperty(trajectoryPlayerConfPre + "Offset.z", std::to_string(position(2)));
    properties->setProperty(trajectoryPlayerConfPre + "Offset.roll",
                            std::to_string(orientation(0)));
    properties->setProperty(trajectoryPlayerConfPre + "Offset.pitch",
                            std::to_string(orientation(1)));
    properties->setProperty(trajectoryPlayerConfPre + "Offset.yaw", std::to_string(orientation(2)));
    properties->setProperty(trajectoryPlayerConfPre + "EnableRobotPoseUnit", "true");
    properties->setProperty(trajectoryPlayerConfPre + "RobotPoseUnitName", robotPoseUnitName);
    ARMARX_DEBUG << "creating unit " << trajectoryPlayerConfigName
                 << " using these properties: " << properties->getPropertiesForPrefix("");
    auto trajectoryPlayerComponent = Component::create<TrajectoryPlayer>(
        properties, trajectoryPlayerConfigName, getConfigDomain());
    getArmarXManager()->addObject(trajectoryPlayerComponent);
    trajectoryPlayerName = trajectoryPlayerComponent->getName();
 
    waitForProxy(trajectoryPlayerName, true);
    trajectoryPlayer = getProxy<TrajectoryPlayerInterfacePrx>(trajectoryPlayerName);
}
 
void
MMMSimulation::onDisconnectComponent()
{
    if (simulatorPrx->hasRobot(agentName))
    {
        simulatorPrx->removeRobot(agentName);
    }
}
 
void
MMMSimulation::onExitComponent()
{
}
 
PropertyDefinitionsPtr
MMMSimulation::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(new MMMSimulationPropertyDefinitions(getConfigIdentifier()));
}