/*
* 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::LVL1Controller
* @author Raphael ( ufdrv at student dot kit dot edu )
* @date 2017
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#ifndef _ARMARX_LIB_RobotAPI_LVL1Controller_H
#define _ARMARX_LIB_RobotAPI_LVL1Controller_H
#include <map>
#include <atomic>
#include <mutex>
#include <functional>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/util/Registrar.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/interface/libraries/RTControllers/LVL1Controller.h>
#include "Targets/TargetBase.h"
//units
#include "DataUnits/ForceTorqueDataUnit.h"
#include "DataUnits/HapticDataUnit.h"
#include "DataUnits/IMUDataUnit.h"
#include "DataUnits/KinematicDataUnit.h"
#include "DataUnits/PlatformDataUnit.h"
namespace armarx
{
class LVL1ControllerDataProviderInterface: virtual public Ice::Object
{
public:
virtual const ForceTorqueDataUnitInterface* getRTForceTorqueDataUnit() const = 0;
virtual const HapticDataUnitInterface* getRTHapticDataUnit() const = 0;
virtual const IMUDataUnitInterface* getRTIMUDataUnit() const = 0;
virtual const KinematicDataUnitInterface* getRTKinematicDataUnit() const = 0;
virtual const PlatformDataUnitInterface* getRTPlatformDataUnit() const = 0;
virtual TargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode) = 0;
virtual std::string getName() const = 0;
};
using LVL1ControllerDataProviderInterfacePtr = IceInternal::Handle<LVL1ControllerDataProviderInterface>;
/**
* @defgroup Library-RobotRTControllers RobotRTControllers
* @ingroup RobotAPI
* A description of the library RobotRTControllers.
*/
/**
* @ingroup Library-RobotRTControllers
* @brief A high level controller writing its results into targes.
*
* This is the abstract base class for all LVL1Controllers.
* It implements basic management routines required by the RobotUnit and some basic ice calls.
* LVL1Controllers are intantiated and managed by RobotUnits.
*
* Each LVL1Controller has to do the following in their constructor:
* \li Request joints in exactly one control mode from the robot unit (the control mode can't be changed later)
* You will receive pointer targets where it can store its results.
* \li Get pointer to the state of required joints / sensors. (you can't request other data at a later point)
* You will receive pointer to the values.
*
* Each controller has two parts:
* \li The RT controll loop
* \li The NonRT ice communication
*
* \section rtcontrollloop The RT controll loop (rtRun)
* Here you have access to the robots current state and write your results into targets.
* \warning This part has to satisfy realtime conditions!
* All realtime able functions of this class have the 'rt' prefix.
*
* You must not:
* \li call any blocking operation
* \li allocate ram on the heap (since this is a blocking operation)
* \li resize any datastructure (e.g. vector::resize) (this sometimes allocates ram on the heap)
* \li insert new datafields into datastructures (e.g. vector::push_back) (this sometimes allocates ram on the heap)
* \li write to any stream (e.g. ARMARX_VERBOSE, std::cout, print, filestreams) (this sometimes blocks/allocates ram on the heap)
* \li make network calls (e.g. through ice) (this blocks/allocates ram on the heap) (using begin_... end_... version of ice calls IS NO SOLUTION)
* \li do any expensive calculations (e.g. calculate IK, run some solver, invert big matrices)
*
* \section nonrtpart The NonRT ice communication
* This part consits of any ice communication.
* Here you could get new controll parameters or targets from other components.
*
* \section rtnrtcomm Communication between RT and NonRT
* All communication between RT and NonRT has to be lockfree.
* So you have to use constructs like atomics or TripleBuffers (See armarx::LVL1ControllerWithTripleBuffer).
*
* \image html LVL1ControllerGeneralDataFlow.svg "General Dataflow in a LVL1Controller"
*
*
*
*
* \image html LVL1ControllerAtomicDataFlow.svg "Dataflow in a LVL1Controller using atomics for communication between RT and NonRT"
*
*
*
*
* \image html LVL1ControllerTripleBufferDataFlow.svg "Dataflow in a LVL1Controller using a triple buffer for communication between RT and NonRT"
*
* \section expensivework How to do expensive calculations
* You have to execute expensive calculations in a different worker thread.
* This thread could calculate target values at its own speed (e.g. 100 Hz).
*
* While rtRun runs at a higher frequency (e.g. 1000 Hz) and:
* \li reads target values
* \li optionally passes the target values to a PID controller
* \li writes them to the targets
* \li sends the sensor values to the worker thread.
*
* If you do some additional calculation in rtRun, you maybe need to pass config parameters from NonRT to RT using a nonblocking method.
*
* \image html LVL1ControllerWorkerThreadDataFlow.svg "Dataflow in a LVL1Controller using a worker thread"
*
*/
class LVL1Controller:
virtual public LVL1ControllerInterface,
virtual public Component
{
public:
//ice interface (must not be called in the rt thread)
virtual bool isControllerActive(const Ice::Current& = GlobalIceCurrent) const final
{
return isActive;
}
virtual std::string getInstanceName(const Ice::Current& = GlobalIceCurrent) const final
{
return getName();
}
virtual StringStringDictionary getJointControlModeMap(const Ice::Current& = GlobalIceCurrent) const final
{
return jointControlModeMap;
}
virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override = 0;
//c++ interface (local calls) (must not be called in the rt thread)
//c++ interface (local calls) (can only be called in the rt thread)
virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
/**
* @brief This function is called before the controller is activated.
* You can use it to activate a thread again (DO NOT SPAWN NEW THREADS!) e.g. via a std::atomic_bool.
*/
virtual void rtPreActivateController() {}
/**
* @brief This function is called after the controller is deactivated.
* You can use it to deactivate a thread (DO NOT JOIN THREADS!) e.g. via a std::atomic_bool.
*/
virtual void rtPostDeactivateController() {}
bool rtUsesJoint(std::size_t jointindex) const
{
return std::find(jointIndices.begin(), jointIndices.end(), jointindex) == jointIndices.end();
}
const std::vector<std::size_t>& rtGetJointIndices() const
{
return jointIndices;
}
private:
friend class RobotUnit;
void rtActivateController();
void rtDeactivateController();
//this data is filled by the robot unit to provide convenience functions
std::atomic_bool isActive {false};
JointNameToControlModeDictionary jointControlModeMap;
std::vector<std::size_t> jointIndices;
// ManagedIceObject interface
protected:
std::string getDefaultName() const
{
return getClassName();
}
};
using LVL1ControllerPtr = IceInternal::Handle<LVL1Controller>;
using LVL1ControllerRegistry = Registrar<std::function<LVL1ControllerPtr(LVL1ControllerDataProviderInterfacePtr robotUnit, LVL1ControllerConfigPtr config)>>;
template<class ControllerType>
struct LVL1ControllerRegistration
{
LVL1ControllerRegistration(const std::string& name)
{
LVL1ControllerRegistry::registerElement(name, [](LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config)
{
ARMARX_CHECK_EXPRESSION_W_HINT(prov, "DataProvider is NULL!");
return new ControllerType(prov, config);
});
}
};
/**
* @ingroup Library-RobotRTControllers
* @brief Brief description of class LVL1Controller.
*
* Detailed description of class LVL1Controller.
*
* \code{cpp}
//in ice
module armarx
{
struct SomeControllerConfig extends LVL1ControllerConfig
{
float paramSetViaConfig;
};
interface SomeControllerInterface extends LVL1ControllerInterface
{
void setSomeParamChangedViaIce(float param);
};
};
//in c++
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
namespace armarx
{
struct SomeControlDataStruct
{
float parameterChangedViaIceCalls;
};
class SomeController : virtual public LVL1Controller<SomeControlDataStruct>,
public SomeControllerInterface
{
JointVelocityTargetPtr targetJointXPtr;
std::vector<const float*> jointValuePtrs;
float someParamSetViaConfig;
SomeController(RobotUnitPtr robotUnit, LVL1ControllerConfigPtr config):
LVL1Controller<SomeControlDataStruct>(SomeControlDataStruct(1)) //initial parameter value
{
//cast the config to your config type
SomeControlDataStructPtr someConfig = SomeControlDataStructPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION_W_HINT(someConfig,
"The provided config has the wrong type! The type is " << config->ice_id()
<< " instead of " << SomeControllerConfig::ice_staticId());
//read the config
someParamSetViaConfig = someConfig->parameterSetViaIceCalls
//make sure the used units are supported
ARMARX_CHECK_EXPRESSION_W_HINT(robotUnit->getRTKinematicDataUnit(), "The RobotUnit " << robotUnit->getName() << " has no kinematic data unit");
//get pointers to sensor values from units
jointValuePtrs = robotUnit->getRTKinematicDataUnit()->getJointAngles({"jointX", "jointY"});
//get pointers for the results of this controller
targetJointXPtr = dynamic_cast<JointVelocityTarget*>(robotUnit->getJointTarget("jointX", ControlModes::VelocityMode));
ARMARX_CHECK_EXPRESSION_W_HINT(targetJointXPtr, "The Joint jointX does not support the mode " + ControlModes::VelocityMode);
}
SomeController(JointVelocityTargetPtr targetPtr, ControllerConfigPtr config):
targetJointXPtr{targetPtr}
{
}
virtual void run(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
{
pid.update(
rtGetControlStruct().parameterChangedViaIceCalls,
jointValuePtrs.at(0),
timeSinceLastIteration.toMicroSeconds());
*targetJointXPtr = pid.getValue() + someParamSetViaConfig;
}
virtual void setSomeParamChangedViaIce(float param, const Ice::Current& = GlobalIceCurrent) override
{
std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
getWriterControlStruct().parameterChangedViaIceCalls = param;
writeControlStruct();
}
}
//register the controller
//this line has to appear in some cpp of your lib to do its registration (including it is enough)
LVL1ControllerRegistration<SomeController> registrationSomeController("SomeController");
}
* \endcode
*/
template <typename ControlDataStruct>
class LVL1ControllerWithTripleBuffer: virtual public LVL1Controller
{
public:
LVL1ControllerWithTripleBuffer(const ControlDataStruct& initialCommands = ControlDataStruct()):
controlDataTripleBuffer {initialCommands}
{
}
virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
{
rtUpdateControlStruct();
rtRun(sensorValuesTimestamp, timeSinceLastIteration);
}
protected:
const ControlDataStruct& rtGetControlStruct() const
{
return controlDataTripleBuffer.getReadBuffer();
}
bool rtUpdateControlStruct()
{
return controlDataTripleBuffer.updateReadBuffer();
}
void writeControlStruct()
{
//just lock to be save and reduce the impact of an error
//also this allows code to unlock the mutex before calling this function
//(can happen if some lockguard in a scope is used)
std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
controlDataTripleBuffer.commitWrite();
}
ControlDataStruct& getWriterControlStruct()
{
return controlDataTripleBuffer.getWriteBuffer();
}
mutable std::recursive_mutex controlDataMutex;
private:
WriteBufferedTripleBuffer<ControlDataStruct> controlDataTripleBuffer;
};
template <typename ControlDataStruct>
using LVL1ControllerTemplatePtr = IceInternal::Handle<LVL1ControllerWithTripleBuffer<ControlDataStruct>>;
struct PlatformCartesianVelocity
{
float vx;
float vy;
float vAngle;
PlatformCartesianVelocity() : vx(0), vy(0), vAngle(0)
{ }
};
class AbstractLvl1PlatformVelocityController : public virtual LVL1ControllerWithTripleBuffer<PlatformCartesianVelocity> {
public:
virtual void setTarget(float vx, float vy, float vAngle) = 0;
};
typedef boost::shared_ptr <AbstractLvl1PlatformVelocityController> AbstractLvl1PlatformVelocityControllerPtr;
}
#endif