RobotUnitModuleControlThread.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    RobotAPI::ArmarXObjects::RobotUnit
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <thread>
 
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
 
#include "../Devices/ControlDevice.h"
#include "../Devices/SensorDevice.h"
#include "RobotUnitModuleBase.h"
 
namespace armarx
{
    class DynamicsHelper;
    class RTThreadTimingsSensorDevice;
} // namespace armarx
 
namespace armarx::RobotUnitModule
{
    class ControlThreadPropertyDefinitions : public ModuleBasePropertyDefinitions
    {
    public:
        ControlThreadPropertyDefinitions(std::string prefix) : ModuleBasePropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::size_t>(
                "NjointController_AllowedExecutionTimePerControlDeviceUntilWarning",
                2,
                "A Warning will be printed, If the execution time in micro seconds of a "
                "NJointControllerBase "
                "exceeds this parameter times the number of ControlDevices.");
            defineOptionalProperty<std::size_t>(
                "NjointController_AllowedExecutionTimePerControlDeviceUntilError",
                20,
                "An Error will be printed, If the execution time in micro seconds of a "
                "NJointControllerBase "
                "exceeds this parameter times the number of ControlDevices.");
            defineOptionalProperty<bool>(
                "EnableInverseDynamics",
                false,
                "If true, inverse dynamics are calculated for all joints given in the "
                "InverseDynamicsRobotJointSets property during each loop of "
                "the rt control thread.");
            defineOptionalProperty<std::string>("InverseDynamicsRobotJointSets",
                                                "LeftArm,RightArm",
                                                "Comma separated list of robot nodesets for which "
                                                "the inverse dynamics should be calculcated.");
            defineOptionalProperty<std::string>(
                "InverseDynamicsRobotBodySet",
                "RobotCol",
                "Robot nodeset of which the masses should be used for the inverse dynamics");
        }
    };
 
    /**
     * @ingroup Library-RobotUnit-Modules
     * @brief This \ref ModuleBase "Module" manages the \ref ControlThread.
     *
     * It offers the basic functions called in the \ref ControlThread.
     *
     * @see ModuleBase
     */
    class ControlThread : virtual public ModuleBase, virtual public RobotUnitControlThreadInterface
    {
        friend class ModuleBase;
        enum class EmergencyStopStateRequest
        {
            RequestActive,
            RequestInactive,
            RequestNone,
        };
 
    protected:
        enum class SwitchControllerMode
        {
            /**
             * @brief This modus is used if the RTThread is supported to control
             * which controllers are running. (e.g. for Calibrating)
             */
            RTThread,
            IceRequests
        };
 
    public:
        /**
         * @brief Returns the singleton instance of this class
         * @return The singleton instance of this class
         */
        static ControlThread&
        Instance()
        {
            return ModuleBase::Instance<ControlThread>();
        }
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////// RobotUnitModule hooks //////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        ///@see \ref ModuleBase::_preFinishControlThreadInitialization
        void _preFinishControlThreadInitialization();
        ///@see \ref ModuleBase::_preOnInitRobotUnit
        void _preOnInitRobotUnit();
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////// ice interface //////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public:
        /**
         * @brief Sets the \ref EmergencyStopState
         * @param state The \ref EmergencyStopState to set
         */
        void setEmergencyStopState(EmergencyStopState state);
 
        /**
         * @brief Returns the \ref ControlThread's target \ref EmergencyStopState
         * @return The \ref EmergencyStopState
         */
        EmergencyStopState getEmergencyStopState() const;
 
        /**
         * @brief Returns the \ref ControlThread's \ref EmergencyStopState
         * @return The \ref EmergencyStopState
         */
        EmergencyStopState getRtEmergencyStopState() const;
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // /////////////////////////////////// Module interface /////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    public:
        /**
         * @brief Teturns he \ref ControlThread's thread id
         * @return The \ref ControlThread's thread id
         * @see controlThreadId
         */
        std::thread::id
        getControlThreadId() const
        {
            return controlThreadId;
        }
 
    private:
        /**
         * @brief Sets the \ref EmergencyStopState without updating the topic.
         * For use in \ref RobotUnitEmergencyStopMaster.
         * @param state The \ref EmergencyStopState to set
         */
        void setEmergencyStopStateNoReportToTopic(EmergencyStopState state);
        /**
         * @brief Sets the emergency stop state to the request from rt (if there was a request).
         * For use in \ref Publisher.
         */
        void processEmergencyStopRequest();
 
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////// rt interface ///////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    protected:
        /**
         * @brief Returns the simox robot used in the control thread
         * @return The simox robot used in the control thread
         */
        const VirtualRobot::RobotPtr&
        rtGetRobot() const
        {
            return rtRobot;
        }
 
        /**
         * @brief Changes the set of active controllers.
         * Changes can be caused by a new set of requested controllers or emergency stop
         * @return Whether active controllers were changed
         */
        bool rtSwitchControllerSetup(SwitchControllerMode mode = SwitchControllerMode::IceRequests);
 
        /**
         * @brief Searches for the \ref NJointControllerBase responsible for the given \ref JointController and deactivates it.
         * Does not commit the new set of activated controllers)
         * @param ctrlDevIndex The control device index causing the problem.
         */
        void rtDeactivateAssignedNJointControllerBecauseOfError(std::size_t ctrlDevIndex);
        /**
         * @brief Deactivates a NJointControllerBase and sets all used ControlDevices to EmergencyStop.
         * Does not commit the new set of activated controllers)
         * @param nJointCtrlIndex The NJointControllerBase to deactivate (index in controllersActivated)
         */
        void rtDeactivateNJointControllerBecauseOfError(std::size_t nJointCtrlIndex);
        /**
         * @brief Runs NJoint controllers
         * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
         * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
         */
        void rtRunNJointControllers(const IceUtil::Time& sensorValuesTimestamp,
                                    const IceUtil::Time& timeSinceLastIteration);
        /**
         * @brief Runs Joint controllers and writes target values to ControlDevices
         * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
         * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
         */
        void rtRunJointControllers(const IceUtil::Time& sensorValuesTimestamp,
                                   const IceUtil::Time& timeSinceLastIteration);
        /**
         * @brief Calls \ref SensorDevice::rtReadSensorValues for all \ref SensorDevice "SensorDevices".
         * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
         * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
         */
        void rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp,
                                      const IceUtil::Time& timeSinceLastIteration);
        /**
         * @brief Execute code after updating the sensor values. Default implementation calculates the inverse dynamics.
         * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
         * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
         */
        void virtual rtPostReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp,
                                                  const IceUtil::Time& timeSinceLastIteration);
        /// @brief Deactivates all NJointControllers generating invalid targets.
        void rtHandleInvalidTargets();
        /// @brief Calls \ref JointController::rtResetTarget for all active Joint controllers
        void rtResetAllTargets();
 
        /**
         * @brief Hook for deriving classes (called AFTER a NJointControllerBase is deactivated due to an error)
         *
         * This is useful when two Control devices can be controlled by different NJointControllers but either both or none
         * have to be controlled.
         *
         * Since this hook is called after the controller with an error was deactivated an other call to
         * \ref rtDeactivateAssignedNJointControllerBecauseOfError can't lead to a cyclic execution.
         */
        virtual void
        rtDeactivatedNJointControllerBecauseOfError(const NJointControllerBasePtr& /*nJointCtrl*/)
        {
        }
 
        /**
         * @brief Updates the current \ref SensorValueBase "SensorValues" and \ref ControlTargetBase "ControlTargets"
         * for code running outside of the \ref ControlThread
         * @param sensorValuesTimestamp Timestamp of the current \ref SensorValueBase SensorValues
         * @param timeSinceLastIteration Time between the last two updates of \ref SensorValueBase SensorValues
         */
        void rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp,
                                            const IceUtil::Time& timeSinceLastIteration);
 
        /**
         * @brief Returns all \ref ControlDevice "ControlDevices"
         * @return All \ref ControlDevice "ControlDevices"
         */
        const std::vector<ControlDevicePtr>& rtGetControlDevices() const;
        /**
         * @brief Returns all \ref SensorDevice "SensorDevices"
         * @return All \ref SensorDevice "SensorDevices"
         */
        const std::vector<SensorDevicePtr>& rtGetSensorDevices();
        /**
         * @brief Returns the \ref RTThreadTimingsSensorDevice
         * @return The \ref RTThreadTimingsSensorDevice
         */
        RTThreadTimingsSensorDevice& rtGetThreadTimingsSensorDevice();
 
        /**
         * @brief Returns whether the rt thread is in emergency stop. (This function is for use in the \ref ControlThread)
         * @return Whether the rt thread is in emergency stop.
         */
        bool
        rtIsInEmergencyStop() const
        {
            return rtIsInEmergencyStop_;
        }
 
        /**
         * @brief Returns the \ref ControlThread's emergency stop state. (This function is for use in the \ref ControlThread)
         * @return The \ref ControlThread's emergency stop state
         */
        EmergencyStopState
        rtGetEmergencyStopState() const
        {
            return rtIsInEmergencyStop_ ? EmergencyStopState::eEmergencyStopActive
                                        : EmergencyStopState::eEmergencyStopInactive;
        }
 
        /**
         * @brief Set an \ref EmergencyStopState request. This request will be executed by the \ref Publisher
         * @param state The \ref EmergencyStopState to set
         */
        void rtSetEmergencyStopState(EmergencyStopState state);
 
        void rtUpdateRobotGlobalPose();
        void rtSetRobotGlobalPose(const Eigen::Matrix4f& gp, bool updateRobot = true);
 
        /// Activate a joint controller from the rt loop (only works in switch mode RTThread)
        void rtSetJointController(JointController* c, std::size_t index);
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // //////////////////////////////////// implementation //////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /**
         * @brief Returns the activated \ref JointController "JointControllers"
         * @return The activated \ref JointController "JointControllers"
         */
        std::vector<JointController*>& rtGetActivatedJointControllers();
        const std::vector<JointController*>& rtGetActivatedJointControllers() const;
        /**
         * @brief Returns the activated \ref NJointControllerBase "NJointControllers"
         * @return The activated \ref NJointControllerBase "NJointControllers"
         */
        std::vector<NJointControllerBase*>& rtGetActivatedNJointControllers();
        const std::vector<NJointControllerBase*>& rtGetActivatedNJointControllers() const;
 
        /**
         * @brief Returns a vector containing the index of the activated \ref NJointControllerBase
         * for each \ref JointController
         * @return A vector containing the index of the activated \ref NJointControllerBase
         * for each \ref JointController
         */
        std::vector<std::size_t>& rtGetActivatedJointToNJointControllerAssignement();
        const std::vector<std::size_t>& rtGetActivatedJointToNJointControllerAssignement() const;
 
 
        /**
         * @brief Returns the requested \ref JointController "JointControllers"
         * @return The requested \ref JointController "JointControllers"
         */
        const std::vector<JointController*>& rtGetRequestedJointControllers() const;
        /**
         * @brief Returns the requested \ref NJointControllerBase "NJointControllers"
         * @return The requested \ref NJointControllerBase "NJointControllers"
         */
        const std::vector<NJointControllerBase*>& rtGetRequestedNJointControllers() const;
 
        /**
         * @brief Returns a vector containing the index of the requested \ref NJointControllerBase
         * for each \ref JointController
         * @return A vector containing the index of the requested \ref NJointControllerBase
         * for each \ref JointController
         */
        const std::vector<std::size_t>& rtGetRequestedJointToNJointControllerAssignement() const;
 
        void rtSyncNJointControllerRobot(NJointControllerBase* njctrl);
 
        void dumpRtControllerSetup(std::ostream& out,
                                   const std::string& indent,
                                   const std::vector<JointController*>& activeCdevs,
                                   const std::vector<std::size_t>& activeJCtrls,
                                   const std::vector<NJointControllerBase*>& assignement) const;
        std::string dumpRtState() const;
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // ///////////////////////////////////////// Data ///////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        ///@brief Whether the ControlThread is in emergency stop
        std::atomic_bool rtIsInEmergencyStop_{false};
        ///@brief Whether the ControlThread is supposed to be in emergency stop
        std::atomic_bool emergencyStop{false};
 
        ///@brief The \ref VirtualRobot::Robot used in the \ref ControlThread
        VirtualRobot::RobotPtr rtRobot;
        ///@brief The \ref VirtualRobot::Robot's \ref VirtualRobot::RobotNode "VirtualRobot::RobotNode" used in the \ref ControlThread
        std::vector<VirtualRobot::RobotNodePtr> rtRobotNodes;
 
        ///@brief The \ref ControlThread's thread id
        std::atomic<std::thread::id> controlThreadId;
 
        /// @brief A Warning will be printed, if the execution time per ControlDev of a NJointControllerBase exceeds this parameter
        std::size_t usPerDevUntilWarn;
        /// @brief An Error will be printed, if the execution time per ControlDev of a NJointControllerBase exceeds this parameter
        std::size_t usPerDevUntilError;
 
        bool rtSwitchControllerSetupChangedControllers{false};
        std::size_t numberOfInvalidTargetsInThisIteration{0};
 
        std::vector<JointController*> preSwitchSetup_ActivatedJointControllers;
        std::vector<std::size_t> preSwitchSetup_ActivatedJointToNJointControllerAssignement;
        std::vector<NJointControllerBase*> preSwitchSetup_ActivatedNJointControllers;
 
        std::vector<JointController*> postSwitchSetup_ActivatedJointControllers;
        std::vector<std::size_t> postSwitchSetup_ActivatedJointToNJointControllerAssignement;
        std::vector<NJointControllerBase*> postSwitchSetup_ActivatedNJointControllers;
 
        std::vector<std::size_t> _activatedSynchronousNJointControllersIdx;
        std::vector<std::size_t> _activatedAsynchronousNJointControllersIdx;
 
        std::atomic<EmergencyStopStateRequest> emergencyStopStateRequest{
            EmergencyStopStateRequest::RequestNone};
 
        std::shared_ptr<DynamicsHelper> dynamicsHelpers;
 
        std::size_t _maxControllerCount = 0;
        // //////////////////////////////////////////////////////////////////////////////////////// //
        // /////////////////////////////////////// Attorneys ////////////////////////////////////// //
        // //////////////////////////////////////////////////////////////////////////////////////// //
    private:
        /**
        * \brief This class allows minimal access to private members of \ref ControlThread in a sane fashion for \ref Publisher.
        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
        */
        friend class ControlThreadAttorneyForPublisher;
        /**
        * \brief This class allows minimal access to private members of \ref ControlThread in a sane fashion for \ref RobotUnitEmergencyStopMaster.
        * \warning !! DO NOT ADD ADDITIONAL FRIENDS IF YOU DO NOT KNOW WAHT YOU ARE DOING! IF YOU DO SOMETHING WRONG YOU WILL CAUSE UNDEFINED BEHAVIOUR !!
        */
        friend class ControlThreadAttorneyForRobotUnitEmergencyStopMaster;
    };
} // namespace armarx::RobotUnitModule