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 <condition_variable>
#include <mutex>
#include <thread>
 
#include <VirtualRobot/Robot.h>
 
#include <ArmarXCore/core/Component.h>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include <ArmarXSimulation/interface/RobotUnitSimulationInterface.h>
 
#include "Devices/ForceTorqueSimulationSensorDevice.h"
#include "Devices/GlobalRobotPoseSimulationSensorDevice.h"
#include "Devices/IMUSimulationSensorDevice.h"
#include "Devices/JointSimulationDevice.h"
#include "Devices/PlatformSimulationDevice.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()
         */
        static std::string
        GetDefaultName()
        {
            return "RobotUnitSimulation";
        }
 
        std::string
        getDefaultName() const override
        {
            return GetDefaultName();
        }
 
        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;
        }
    };
} // namespace armarx