MMMSimulation.h

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
 */
 
#pragma once
 
#include <ArmarXCore/core/Component.h>
 
#include <ArmarXSimulation/interface/MMMSimulationInterface.h>
 
#include <RobotAPI/interface/components/TrajectoryPlayerInterface.h>
#include <ArmarXSimulation/interface/simulator/SimulatorInterface.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <Eigen/Core>
 
#include <mutex>
 
namespace armarx
{
    // forward declaration
    class MotionFileWrapper;
    class MotionData;
 
    /**
             * \class MMMSimulationPropertyDefinitions
             * \brief
             */
    class MMMSimulationPropertyDefinitions:
        public ComponentPropertyDefinitions
    {
    public:
        MMMSimulationPropertyDefinitions(std::string prefix):
            ComponentPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("AgentName", "MMM", "");
            defineOptionalProperty<std::string>("MMMFile", "");
            defineOptionalProperty<std::string>("RobotFileName", "RobotAPI/robots/MMM/mmm.xml", "Path to MMM XML File");
            defineOptionalProperty<std::string>("RobotFileNameProject", "RobotAPI", "");
            //defineOptionalProperty<std::string>("RobotName", "MMM");
            defineOptionalProperty<std::string>("RobotNodeSetName", "Joints_Revolute");
            defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory");
            defineOptionalProperty<std::string>("SimulatorName", "Simulator");
            defineOptionalProperty<bool>("LoopPlayback", false);
            defineOptionalProperty<float>("Scaling", 1.8);
            defineOptionalProperty<float>("StartPose.x", 0);
            defineOptionalProperty<float>("StartPose.y", 0);
            defineOptionalProperty<float>("StartPose.z", 0);
            defineOptionalProperty<float>("StartPose.roll", 0);
            defineOptionalProperty<float>("StartPose.pitch", 0);
            defineOptionalProperty<float>("StartPose.yaw", 0);
            defineOptionalProperty<bool>("AutoPlay", false);
            defineOptionalProperty<bool>("LoadToMemory", false);
        }
    };
 
    /**
             * \defgroup Component-MMMSimulation MMMSimulation
             * \ingroup RobotComponents-Components
             * \brief Replays an MMM trajectory from a file.
             *
             * MMMSimulation reads an MMM trajectory from an MMM XML file (component property) and replays the motion using the KinematicUnit and its currently loaded robot.
             * The trajectory can be replayed using position control or velocity control.
             * In the latter case, the control parameters (P, I, D) can be configured via component properties.
             */
 
    /**
             * @ingroup Component-MMMSimulation
             * @brief The MMMSimulation class
             */
    class MMMSimulation :
        virtual public armarx::Component,
        public armarx::MMMSimulationInferface
    {
    public:
        /**
                 * @see armarx::ManagedIceObject::getDefaultName()
                 */
        std::string getDefaultName() const override
        {
            return "MMMSimulation";
        }
 
        bool isMotionLoaded(const Ice::Current& = Ice::emptyCurrent) override;
        bool loadMMMFile(const std::string& filePath, const std::string& projects = std::string(), bool createTrajectoryPlayer = true, const Ice::Current& = Ice::emptyCurrent) override;
        void playMotion(const Ice::Current& = Ice::emptyCurrent) override;
        void pauseMotion(const Ice::Current& = Ice::emptyCurrent) override;
        void stopMotion(const Ice::Current& = Ice::emptyCurrent) override;
        void setLoopBack(bool state, const Ice::Current& = Ice::emptyCurrent) override;
        void setStartPose(const Eigen::Matrix4f& startPose, const Ice::Current& = Ice::emptyCurrent) override
        {
            this->startPose = startPose;
            if (trajectoryPlayer)
            {
                trajectoryPlayer->setOffset(startPose);
            }
        }
 
    protected:
        /**
                 * @see armarx::ManagedIceObject::onInitComponent()
                 */
        void onInitComponent() override;
 
        /**
                 * @see armarx::ManagedIceObject::onConnectComponent()
                 */
        void onConnectComponent() override;
 
        /**
                 * @see armarx::ManagedIceObject::onDisconnectComponent()
                 */
        void onDisconnectComponent() override;
 
        /**
                 * @see armarx::ManagedIceObject::onExitComponent()
                 */
        void onExitComponent() override;
 
        /**
                 * @see PropertyUser::createPropertyDefinitions()
                 */
        PropertyDefinitionsPtr createPropertyDefinitions() override;
        void createTrajectoryPlayer();
        void loadTrajectory();
 
        TrajectoryPlayerInterfacePrx trajectoryPlayer;
        SimulatorInterfacePrx simulatorPrx;
 
        std::string kinematicUnitName;
        std::string robotPoseUnitName;
        std::string trajectoryPlayerName;
        Eigen::Matrix4f startPose;
 
        std::shared_ptr<MotionFileWrapper> motionWrapper;
        std::shared_ptr<MotionData> motionData;
        std::string motionPath;
        bool trajectoryLoaded;
        float modelScaling;
        std::string agentName;
        std::string modelFileName;
 
        std::recursive_mutex mmmMutex;
 
    private:
        void initialize();
 
        RunningTask<MMMSimulation>::pointer_type initTask;
    };
 
    using MMMSimulationPtr = ::IceInternal::Handle< ::armarx::MMMSimulation>;
 
}