RobotUnitSimulation.h

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    ArmarXSimulation::ArmarXObjects::RobotUnitSimulation
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <thread>
#include <mutex>
#include <condition_variable>
 
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/Component.h>
 
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
 
#include <ArmarXSimulation/interface/RobotUnitSimulationInterface.h>
 
#include "Devices/ForceTorqueSimulationSensorDevice.h"
#include "Devices/IMUSimulationSensorDevice.h"
#include "Devices/JointSimulationDevice.h"
#include "Devices/PlatformSimulationDevice.h"
#include "Devices/GlobalRobotPoseSimulationSensorDevice.h"
 
namespace armarx
{
    using ClockT = std::chrono::high_resolution_clock;
 
    //we can't store durations in atoms -> store nanoseconds
    inline std::uintmax_t nowNS()
    {
        return std::chrono::duration_cast<std::chrono::nanoseconds>(ClockT::now().time_since_epoch()).count();
    }
 
    template<class T>
    struct TripleBufferWithGuardAndTime
    {
        TripleBufferWithGuardAndTime(std::condition_variable* cv): cv {cv} {}
 
        std::unique_lock<std::recursive_mutex> guard()
        {
            return std::unique_lock<std::recursive_mutex> {m};
        }
 
        std::size_t getLastWriteT() const
        {
            return lastwriteNs;
        }
        bool isLastWrite1SecondAgo() const
        {
            return (lastwriteNs + 1000000000 < nowNS());
        }
 
        void reinit(const T& i)
        {
            tb.reinitAllBuffers(i);
        }
 
        T& getWriteBuffer()
        {
            return tb.getWriteBuffer();
        }
        const T& getReadBuffer() const
        {
            return tb.getReadBuffer();
        }
        void write()
        {
            auto g = guard();
            lastwriteNs = nowNS();
            tb.commitWrite();
            cv->notify_all();
        }
        void read()
        {
            tb.updateReadBuffer();
        }
 
    private:
        mutable std::recursive_mutex m;
        WriteBufferedTripleBuffer<T> tb;
        std::atomic<std::uintmax_t> lastwriteNs {0};
        std::condition_variable* cv;
    };
 
 
 
    /**
     * @class RobotUnitSimulationPropertyDefinitions
     * @brief
     */
    class RobotUnitSimulationPropertyDefinitions:
        public RobotUnitPropertyDefinitions
    {
    public:
        RobotUnitSimulationPropertyDefinitions(std::string prefix):
            RobotUnitPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("SimulatorName", "Simulator", "Name of the simulator component that should be used");
            defineOptionalProperty<bool>("SynchronizedSimulation", false, "If the simulation should be synchronized", PropertyDefinitionBase::eModifiable);
            defineOptionalProperty<std::size_t>("ControlIterationMs", 10, "Time each iteration should take (in ms)");
            defineOptionalProperty<std::string>("SimulatorRobotListenerInterfaceTopic", "", "Name of the simulator topic for SimulatorRobotListenerInterface (empty defaults to Simulator_Robot_<ROBOTNAME>");
            defineOptionalProperty<std::string>("SimulatorForceTorqueListenerInterfaceTopic", "ForceTorqueDynamicSimulationValues", "Name of the simulator topic for SimulatorForceTorqueListenerInterface");
            
            defineOptionalProperty<float>("maxLinearPlatformVelocity", 2000, "[mm/s]");
            defineOptionalProperty<float>("maxAngularPlatformVelocity", 1, "[rad/s]");
 
            defineOptionalProperty<std::string>(
                "ForceTorqueSensorMapping", "",
                "A list of mappings to modify the forceTorque sensors given in the robot format. "
                "Syntax (comma SV of colon SV): 'NameInFile:ReportName[:ReportFrame],NameInFile2:ReportName2[:ReportFrame2]' "
                "This would cause reporting of the ft sensor called 'NameInFile' under the name 'ReportName' and optionally change the report frame to 'ReportFrame'. "
                "IMPORTANT: (1) The original reporting frame of the simulator has to be the parent node of 'NameInFile'."
                " (2) The transformation from the original reporting frame to 'ReportFrame' should be fix "
                "(there should be no joints in the kinematic chain between them),"
                " since the transformation is done without access to the joint values"
            );
        }
    };
 
 
    /**
     * @defgroup Component-RobotUnitSimulation RobotUnitSimulation
     * @ingroup ArmarXSimulation-Components
     * A description of the component RobotUnitSimulation.
     *
     * @class RobotUnitSimulation
     * @ingroup Component-RobotUnitSimulation
     * @brief Brief description of class RobotUnitSimulation.
     *
     * Detailed description of class RobotUnitSimulation.
     */
    class RobotUnitSimulation :
        virtual public RobotUnit,
        virtual public RobotUnitSimulationSinterface
    {
    public:
        RobotUnitSimulation():
            torquesTB {&cvGotSensorData},
            anglesTB {&cvGotSensorData},
            velocitiesTB {&cvGotSensorData},
            robPoseTB {&cvGotSensorData},
            robVelTB {&cvGotSensorData},
            forceTorqueTB {&cvGotSensorData}
        {}
        /**
         * @see armarx::ManagedIceObject::getDefaultName()
         */
        std::string getDefaultName() const override
        {
            return "RobotUnitSimulation";
        }
 
        void componentPropertiesUpdated(const std::set<std::string>& changedProperties) override;
 
    protected:
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
        {
            return armarx::PropertyDefinitionsPtr(new RobotUnitSimulationPropertyDefinitions(getConfigIdentifier()));
        }
 
        // RobotUnit interface
    protected:
        void onInitRobotUnit() override;
        void onConnectRobotUnit() override;
 
        void joinControlThread() override;
 
        void rtTask();
 
        std::atomic_bool shutdownRtThread {false};
        std::thread rtThread;
        SimulatorInterfacePrx simulatorPrx;
        std::atomic<long> simulationDataTimestampInMicroSeconds{0};
        std::string simulatorPrxName;
        std::string robotName;
 
        float maxLinearPlatformVelocity{0};
        float maxAngularPlatformVelocity{0};
 
        VirtualRobot::RobotPtr robot;
 
    public:
        void reportState(SimulatedRobotState const& state, const Ice::Current& = Ice::emptyCurrent) override;
 
    private:
        void updateForceTorque(ForceTorqueData const& ftData, IceUtil::Time timestamp);
        void fillTB(TripleBufferWithGuardAndTime<std::vector<float>>& b, const NameValueMap& nv, const std::string name) const;
        void rtUpdateSensors(bool wait);
        void rtSendCommands();
 
        bool skipReport() const;
 
        const std::string& getMappedFTName(const std::string& name)const
        {
            return ftMappings.count(name) ? ftMappings.at(name).sensorName : name;
        }
        const std::string& getMappedFTReportingFrame(const std::string& name, const std::string& unmappedFrame)const
        {
            return ftMappings.count(name) ? ftMappings.at(name).reportFrame : unmappedFrame;
        }
        Eigen::Matrix3f getMappedFTReportingTransformation(const std::string& name)const
        {
            return ftMappings.count(name) ? ftMappings.at(name).reportTransformation : Eigen::Matrix3f::Identity();
        }
 
        void rtPollRobotState();
        std::atomic_bool isPolling {false};
 
        struct RobVel
        {
            Eigen::Vector3f lin {std::nanf(""), std::nanf(""), std::nanf("")};
            Eigen::Vector3f ang {std::nanf(""), std::nanf(""), std::nanf("")};
        };
        struct FT
        {
            Eigen::Vector3f force {std::nanf(""), std::nanf(""), std::nanf("")};
            Eigen::Vector3f torque {std::nanf(""), std::nanf(""), std::nanf("")};
        };
        // ////////////////////////////////////////////////////////////////////////////////////////////////// //
        //devs
        // ////////////////////////////////////////////////////////////////////////////////////////////////// //
        std::condition_variable cvGotSensorData;
        std::atomic<bool> gotSensorData{false};
        IceUtil::Time sensorValuesTimestamp;
        //joints
        KeyValueVector<std::string, JointSimulationDevicePtr> jointDevs;
        TripleBufferWithGuardAndTime<std::vector<float>> torquesTB;
        TripleBufferWithGuardAndTime<std::vector<float>> anglesTB;
        TripleBufferWithGuardAndTime<std::vector<float>> velocitiesTB;
        NameValueMap velocitiesCtrl;
        NameValueMap anglesCtrl;
        NameValueMap torquesCtrl;
        //platform
        PlatformSimulationDevicePtr platformDev;
        //pos/vel
        TripleBufferWithGuardAndTime<Eigen::Matrix4f> robPoseTB;
        TripleBufferWithGuardAndTime<RobVel> robVelTB;
        //sensor
        KeyValueVector<std::string, ForceTorqueSimulationSensorDevicePtr> forceTorqueDevs;
        TripleBufferWithGuardAndTime<std::vector<FT>> forceTorqueTB;
        std::vector<AtomicWrapper<std::uintmax_t>> forceTorqueTimes;
        struct FTMappingData
        {
            std::string sensorName;
            std::string originalReportFrame;
            std::string reportFrame;
            Eigen::Matrix3f reportTransformation;
        };
        std::map<std::string, FTMappingData> ftMappings;
        //pose
        // GlobalRobotPoseSimulationSensorDevicePtr globalPoseDevice;
        ///TODO IMU
        // ////////////////////////////////////////////////////////////////////////////////////////////////// //
        //other
        // ////////////////////////////////////////////////////////////////////////////////////////////////// //
        //settings
        std::atomic_bool synchronizedSimulation;
        IceUtil::Time controlIterationMs;
        std::string simulatorRobotListenerInterfaceTopic;
        std::string simulatorForceTorqueListenerInterfaceTopic;
        //metadata
        std::atomic<std::uintmax_t> iterationCount {0};
 
        // RobotUnit interface
    public:
        IceUtil::Time getControlThreadTargetPeriod() const override
        {
            return controlIterationMs;
        }
 
        // RobotUnitManagementInterface interface
    public:
        bool isSimulation(const Ice::Current&) const override
        {
            return true;
        }
    };
}