void activateController (const Ice::Current &=Ice::emptyCurrent) override void callDescribedFunction (const std::string &, const StringVariantBaseMap &, const Ice::Current &=Ice::emptyCurrent) override void deactivateAndDeleteController (const Ice::Current &=Ice::emptyCurrent) final override void deactivateController (const Ice::Current &=Ice::emptyCurrent) override void deleteController (const Ice::Current &=Ice::emptyCurrent) final override std::string getClassName (const Ice::Current &=Ice::emptyCurrent) const override=0 const std::map< std::string, const JointController * > & getControlDevicesUsedJointController () const std::vector< char > & getControlDeviceUsedBitmap () const StringStringDictionary getControlDeviceUsedControlModeMap (const Ice::Current &=Ice::emptyCurrent) const final override const std::vector< std::size_t > & getControlDeviceUsedIndices () const NJointControllerDescription getControllerDescription (const Ice::Current &=Ice::emptyCurrent) const final override NJointControllerDescriptionWithStatus getControllerDescriptionWithStatus (const Ice::Current &=Ice::emptyCurrent) const final override NJointControllerStatus getControllerStatus (const Ice::Current &=Ice::emptyCurrent) const final override WidgetDescription::StringWidgetDictionary getFunctionDescriptions (const Ice::Current &=Ice::emptyCurrent) const override std::string getInstanceName (const Ice::Current &=Ice::emptyCurrent) const final override bool hasControllerError (const Ice::Current &=Ice::emptyCurrent) const final override bool isControllerActive (const Ice::Current &=Ice::emptyCurrent) const final override bool isControllerRequested (const Ice::Current &=Ice::emptyCurrent) const final override bool isDeletable (const Ice::Current &=Ice::emptyCurrent) const final override std::optional< std::vector< char > > isNotInConflictWith (const NJointControllerBasePtr &other) const std::optional< std::vector< char > > isNotInConflictWith (const std::vector< char > &used) const NJointControllerBase () ConstControlDevicePtr peekControlDevice (const std::string &deviceName) const Get a const ptr to the given ControlDevice . ConstSensorDevicePtr peekSensorDevice (const std::string &deviceName) const Get a const ptr to the given SensorDevice . const std::string & rtGetClassName () const Returns the class name. const std::vector< std::size_t > & rtGetControlDeviceUsedIndices () const Returns the indices of all ControlDevice 's this NJointControllerBase calculates a ControlTarget for. bool rtGetErrorState () const Sets the error state to true. const std::string & rtGetInstanceName () const Returns the instance name. std::size_t rtGetNumberOfUsedControlDevices () const Returns the number of used ControlDevices . const VirtualRobot::RobotPtr & rtGetRobot () TODO make protected and use attorneys. const std::vector< VirtualRobot::RobotNodePtr > & rtGetRobotNodes () Returns the nodes of the virtual robot used by this NJointControllerBase in the rtRun. bool rtUsesControlDevice (std::size_t deviceIndex) const Returns whether this NJointControllerBase calculates a ControlTarget for the given ControlDevice . ControlTargetBase * useControlTarget (const std::string &deviceName, const std::string &controlMode) Declares to calculate the ControlTarget for the given ControlDevice in the given ControlMode when rtRun is called. template<class T > T * useControlTarget (const std::string &deviceName, const std::string &controlMode) Declares to calculate the ControlTarget for the given ControlDevice in the given ControlMode when rtRun is called. template<class T > const T * useSensorValue (const std::string &deviceName) const Get a const ptr to the given SensorDevice 's SensorValue . const SensorValueBase * useSensorValue (const std::string &sensorDeviceName) const Get a const ptr to the given SensorDevice 's SensorValue . const VirtualRobot::RobotPtr & useSynchronizedRtRobot (bool updateCollisionModel=false) Requests a VirtualRobot for use in rtRun *. ~NJointControllerBase () override void enableProfiler (bool enable) setProfiler allows setting ManagedIceObject::profiler to a new instance (if the new instance is actually not a null pointer) std::string generateSubObjectName (const std::string &subObjectName) Generates a unique name for a sub object from a general name. ArmarXManagerPtr getArmarXManager () const Returns the ArmarX manager used to add and remove components. ManagedIceObjectConnectivity getConnectivity () const Retrieve connectivity of the object (topcis as well as proxies) IceManagerPtr getIceManager () const Returns the IceManager . VariantBasePtr getMetaInfo (const std::string &id) StringVariantBaseMap getMetaInfoMap () const std::string getName () const Retrieve name of object. Ice::ObjectAdapterPtr getObjectAdapter () const Returns object's Ice adapter. ArmarXObjectSchedulerPtr getObjectScheduler () const PeriodicTaskPtr getPeriodicTask (const std::string &name) Profiler::ProfilerPtr getProfiler () const getProfiler returns an instance of armarx::Profiler template<class ProxyTarg, class... Args> void getProxy (const char *name, IceInternal::ProxyHandle < ProxyTarg > &proxy, Args &&... args) template<class ProxyType > ProxyType getProxy (const std::string &name, bool addToDependencies=false, const std::string &endpoints="", bool throwOnProxyError=true) Retrieves a proxy object. template<class ProxyTarg, class... Args> void getProxy (const std::string &name, IceInternal::ProxyHandle < ProxyTarg > &proxy, Args &&... args) template<class ProxyTarg, class... Args> void getProxy (IceInternal::ProxyHandle < ProxyTarg > &proxy, const char *name, Args &&... args) template<class ProxyTarg, class... Args> void getProxy (IceInternal::ProxyHandle < ProxyTarg > &proxy, const std::string &name, Args &&... args) Assigns a proxy to proxy. Ice::ObjectPrx getProxy (long timeoutMs=0, bool waitForScheduler=true) const Returns the proxy of this object (optionally it waits for the proxy) template<class Prx> Prx getProxy (long timeoutMs=0, bool waitForScheduler=true) const template<class ProxyType > void getProxy (ProxyType &proxy, const char *name, bool addToDependencies=false, const std::string &endpoints="", bool throwOnProxyError=true) Overload to allow using string literals as name (solve ambiguous overload). template<class Prx> void getProxy (Prx &prx, long timeoutMs=0, bool waitForScheduler=true) const int getState () const Retrieve current state of the ManagedIceObject . template<class TopicProxyType> TopicProxyType getTopic (const std::string &name) Returns a proxy of the specified topic. template<class TopicProxyType> void getTopic (TopicProxyType &topicProxy, const std::string &name) Assigns a proxy of the specified topic to topicProxy. std::vector< std::string > getUnresolvedDependencies () const returns the names of all unresolved dependencies ManagedIceObject (ManagedIceObject const &other) void offeringTopic (const std::string &name) Registers a topic for retrival after initialization. void preambleGetTopic (std::string const &name) void setMetaInfo (const std::string &id, const VariantBasePtr &value) Allows to set meta information that can be queried live via Ice interface on the ArmarXManager . void startPeriodicTask (const std::string &uniqueName, std::function< void(void)> f, int periodMs, bool assureMeanInterval=false, bool forceSystemTime=true) bool stopPeriodicTask (const std::string &name) bool unsubscribeFromTopic (const std::string &name) Unsubscribe from a topic. bool usingProxy (const std::string &name, const std::string &endpoints="") Registers a proxy for retrieval after initialization and adds it to the dependency list. void usingTopic (const std::string &name, bool orderedPublishing=false) Registers a proxy for subscription after initialization. void waitForObjectScheduler () Waits until the ObjectScheduler could resolve all dependencies. void waitForProxy (std::string const &name, bool addToDependencies) SpamFilterDataPtr deactivateSpam (float deactivationDurationSec=10.0f, const std::string &identifier="", bool deactivate=true) const disables the logging for the current line for the given amount of seconds. MessageTypeT getEffectiveLoggingLevel () const Logging () void setLocalMinimumLoggingLevel (MessageTypeT level) With setLocalMinimumLoggingLevel the minimum verbosity-level of log-messages can be set. void setTag (const LogTag &tag ) void setTag (const std::string &tagName) virtual ~Logging ()
A high level controller writing its results into ControlTargets .
This is the abstract base class for all NJointControllers. It implements basic management routines required by the RobotUnit and some basic ice calls. NJointControllers are instantiated and managed by the RobotUnit .
A NJointControllerBase calculates ControlTargets for a set of ControlDevices in a specific ControlMode. This ControlMode is defined for each ControlDevice during construction.
A NJointControllerBase can is requested / not requested and active / inactive. All four combinations are possible.
If the user wants this NJointControllerBase to be executed it is in a requested state (see isControllerRequested ). Otherwise the controler is not requested.
Calling activateController sets the state to requested. Calling deactivateController sets the state to not requested. If the NJointControllerBase causes an error or a different NJointControllerBase using one or more of the same ControlDevice 's is requested, this NJointControllerBase is deactivated.
Is the NJointControllerBase executed by the ControlThread it is active (see isControllerActive ).
To be executed by the ControlThread, the NJointControllerBase has to be requested at some point in the past.
If a controller is active, it has to write a valid ControlTarget for each of its ControlDevicea (if it fails, it will be deactivated).
In the Constructor, a NJointControllerBase has to declare all ControlDevices it uses.
The constructor takes a pointer to a configuration structure of the type ConfigPtrT . If an implementation requires a special configuration structure (e.g.: SomeOtherCfg), it has to override this type by calling adding
NJointControllerConfigPtr ConfigPtrT
Definition NJointControllerBase.h:587
to it's clas definition. The type SomeOtherCfg has to derive NJointControllerConfigPtr.
There is a way to generate a small gui widget for controller construction (see this section ).
A NJointControllerBase can use peekControlDevice to examine a ControlDevice before using it (e.g.: checking the supported ControlTargets ).
If a ControlDevice should be used by this NJointControllerBase , it has to call useControlDevice. This sets the ControlMode for the ControlDevice and returns a pointer to the ControlTarget . This pointer has to be used to send commands in each iteration of rtRun. The ControlMode can't be changed afterwards (A new NJointControllerBase has to be created).
A NJointControllerBase can use peekSensorDevice to examine a SensorDevice before using it.
If a SensorDevice should be used by this NJointControllerBase , it has to call useSensorDevice. This returns a pointer to the SensorValue .
If the controller needs a simox robot in rtRun, it should call useSynchronizedRtRobot . This will provide a simoxRobot via rtGetRobot . This robot is synchronized with the real robots state before calling rtRun
Each NJointControllerBase has two parts:
The RT controll loop
The NonRT ice communication
Warning This part has to satisfy realtime conditions! All realtime functions of NJointControllerBase have the 'rt' prefix. Here the NJointControllerBase has access to the robot's current state and has to write results into ControlTargets .
It must not:
call any blocking operation
allocate ram on the heap (since this is a blocking operation)
resize any datastructure (e.g. vector::resize) (this sometimes allocates ram on the heap)
insert new datafields into datastructures (e.g. vector::push_back) (this sometimes allocates ram on the heap)
write to any stream (e.g. ARMARX_VERBOSE, std::cout, print, filestreams) (this sometimes blocks/allocates ram on the heap)
make network calls (e.g. through ice) (this blocks/allocates ram on the heap) (using begin_... end_... version of ice calls IS NO SOLUTION)
do any expensive calculations (e.g. calculate IK, run some solver, invert big matrices)
This part consits of any ice communication. Here the NJointControllerBase can get new controll parameters or targets from other components.
All communication between RT and NonRT has to be lockfree. The NJointControllerBase has to use constructs like atomics or TripleBuffers (See NJointControllerWithTripleBuffer ).
General Dataflow in a NJointControllerBase
Dataflow in a NJointControllerBase using atomics for communication between RT and NonRT
Dataflow in a NJointControllerBase using a triple buffer for communication between RT and NonRT
Dataflow in a NJointControllerBase as a Graph of the two participating domains
The above image shows the two participating domains: one RT thread and multiple ICE threads. If data has to flow along an arrow, you need some construct for non blocking message passing.
Warning If you use TrippleBuffers or WriteBufferedTrippleBuffers you need a separate one for each arrow. 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:
reads target values
optionally passes the target values to a PID controller
writes them to the targets
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.
Dataflow in a NJointControllerBase using a worker thread
Dataflow in a NJointControllerBase using a worker thread as a Graph of the three participating domains
The above image shows the three participating domains: one RT thread, one worker trhead and multiple ICE threads. If data has to flow along an arrow, you need some construct for non blocking message passing.
Warning If you use TrippleBuffers or WriteBufferedTrippleBuffers you need a separate one for each arrow. By implementing these functions:
(
const std::map<std::string, ConstControlDevicePtr>& controlDevices,
const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
);
std::shared_ptr< class Robot > RobotPtr
Definition Bus.h:19
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition ManagedIceObject.h:110
The RobotUnitGui will provide a widget to configure and construct a NJointControllerBase of this type.
More examples can be found in the Tutorials Package
Note The code can be found in the Tutorial package Include headers
Open the namespace
{
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition ArmarXTimeserver.cpp:28
Typedef some pointers
#define TYPEDEF_PTRS_HANDLE(T)
Definition ManagedIceObject.h:86
The config has to inherit NJointControllerConfig and consists only of the joint name
class NJointPositionPassThroughControllerConfig : virtual public NJointControllerConfig
{
public :
NJointPositionPassThroughControllerConfig(const std::string& name): deviceName {name} {}
std::string deviceName;
};
The controller class has to inherit NJointControllerBase
{
public :
NJointControllerBase()
Definition NJointController.cpp:398
The NJointControllerBase provides a config widget for the RobotUnitGuiPlugin
(
const std::map<std::string, ConstControlDevicePtr>& controlDevices,
const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
);
The ctor receives a pointer to the RobotUnit , a pointer to the config and a pointer to a VirtualRobot model.
NJointPositionPassThroughController(
NJointControllerConfigPtr config,
);
IceUtil::Handle< class RobotUnit > RobotUnitPtr
Definition FTSensor.h:34
The function to provide the class name.
std::string
getClassName (
const Ice::Current&)
const override ;
std::string getClassName(const Ice::Current &=Ice::emptyCurrent) const override=0
This controller provides widgets for function calls.
void callDescribedFunction(const std::string &, const StringVariantBaseMap &, const Ice::Current &=Ice::emptyCurrent) override
Definition NJointControllerBase.h:829
WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current &=Ice::emptyCurrent) const override
Definition NJointControllerBase.h:823
The run function executed to calculate new targets
void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override ;
Hooks for this NJointControllerBase to execute code during publishing.
virtual void onPublishActivation(const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition NJointControllerBase.h:1064
virtual void onPublishDeactivation(const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition NJointControllerBase.h:1069
virtual void onPublish(const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &)
Definition NJointControllerBase.h:1074
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugObserverInterface > DebugObserverInterfacePrx
Definition JointController.h:44
::IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface > DebugDrawerInterfacePrx
Definition JointController.h:42
detail::ControlThreadOutputBufferEntry SensorAndControl
Definition NJointControllerBase.h:73
This NJointControllerBase uses one position SensorValue, calculates one position ControlTarget and stores the current position target and position value in atomics.
const SensorValue1DoFActuatorPosition* sensor;
ControlTarget1DoFActuatorPosition* target;
std::atomic<float> targetPos {0};
std::atomic<float> currentPos {0};
Close the class and the namespace
Include the required headers.
#include "NJointPositionPassThroughController.h"
Open the namespace
This generates a config widget used to display a config gui in RobotUnitGui. It consist out of a HBoxLayout containing a Label and a StringComboBox
WidgetDescription::WidgetPtr NJointPositionPassThroughController::GenerateConfigDescription(
const VirtualRobot::RobotPtr &,
const std::map<std::string, ConstControlDevicePtr>& controlDevices,
const std::map<std::string, ConstSensorDevicePtr>& sensorDevices)
{
LabelPtr label =
new Label ;
label->text = "control device name" ;
layout->children.emplace_back(label);
StringComboBoxPtr box = new StringComboBox;
box->defaultIndex = 0;
The StringComboBox only contains the names of ControlDevices accepting a position ControlTarget and providing a position SensorValue
for (const auto & name2dev : controlDevices)
{
const auto & name = name2dev.first;
if (
dev->hasJointController(ControlModes::Position1DoF) &&
dev->getJointController(ControlModes::Position1DoF)->getControlTarget()->isA<ControlTarget1DoFActuatorPosition>() &&
sensorDevices.count(name) &&
sensorDevices.at(name)->getSensorValue()->isA<SensorValue1DoFActuatorPosition>()
)
{
box->options.emplace_back(name);
}
}
box->name = "name" ;
layout->children.emplace_back(box);
return layout;
}
std::shared_ptr< const class ControlDevice > ConstControlDevicePtr
Definition NJointControllerBase.h:77
This turns a map of variants into the config required by this NJointControllerBase . The Variant for the key 'name' defines the ControlDevice .
NJointControllerConfigPtr NJointPositionPassThroughController::GenerateConfigFromVariants(
const StringVariantBaseMap & values)
{
return new NJointPositionPassThroughControllerConfig {values.at("name" )->getString()};
}
The constructors implementation.
NJointPositionPassThroughController::NJointPositionPassThroughController(
NJointControllerConfigPtr config,
)
{
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition ExpressionException.h:73
It checks whether the provided config has the correct type.
NJointPositionPassThroughControllerConfigPtr cfg = NJointPositionPassThroughControllerConfigPtr::dynamicCast(config);
Then it retrieves the SensorValue and ControlTarget, casts then to the required types and stores them to the member variables.
sensor = sv->
asA <SensorValue1DoFActuatorPosition>();
target = ct->
asA <ControlTarget1DoFActuatorPosition>();
}
Brief description of class JointControlTargetBase.
Definition ControlTargetBase.h:48
const T * asA() const
Definition ControlTargetBase.h:76
const SensorValueBase * useSensorValue(const std::string &sensorDeviceName) const
Get a const ptr to the given SensorDevice's SensorValue.
Definition NJointController.cpp:383
ControlTargetBase * useControlTarget(const std::string &deviceName, const std::string &controlMode)
Declares to calculate the ControlTarget for the given ControlDevice in the given ControlMode when rtR...
Definition NJointController.cpp:410
The SensorValueBase class.
Definition SensorValueBase.h:41
const T * asA() const
Definition SensorValueBase.h:82
The class name has to be unique, hence the c++ class name should be used.
std::string NJointPositionPassThroughController::getClassName(const Ice::Current&) const
return "NJointPositionPassThroughController" ;
}
This NJointControllerBase provides two widgets for function calls.
The first widget has a Label and a FloatSpinBox .
{
LabelPtr label =
new Label ;
label->text = "positiontarget" ;
layoutSetPos->children.emplace_back(label);
spin->defaultValue = 0;
spin->max = 1;
spin->min = -1;
The FloatSpinBox creates a variant called 'spinmearound'
spin->name = "spinmearound" ;
layoutSetPos->children.emplace_back(spin);
}
The second widget also has a Label and a FloatSpinBox .
{
LabelPtr label =
new Label ;
label->text = "positiontarget / 2" ;
layoutSetHalfPos->children.emplace_back(label);
spin->defaultValue = 0;
spin->max = 0.5;
spin->min = -0.5;
spin->name = "spinmehalfaround" ;
layoutSetHalfPos->children.emplace_back(spin);
}
This returns a map of boths widgets. The keys will be used to identify the called function.
return {{"SetPosition" , layoutSetPos}, {"SetPositionHalf" , layoutSetHalfPos}};
}
This function is called from the RobotUnitGuiPlugin when the prior defined functions are called. Both functions set the target position.
void NJointPositionPassThroughController::callDescribedFunction(
const std::string& name,
const StringVariantBaseMap & value,
const Ice::Current&)
{
if (name == "SetPosition" )
{
targetPos = value.at("spinmearound" )->getFloat();
}
else if (name == "SetPositionHalf" )
{
targetPos = value.at("spinmehalfaround" )->getFloat() * 2;
}
else
{
}
}
#define ARMARX_ERROR
The logging level for unexpected behaviour, that must be fixed.
Definition Logging.h:194
The rtRun function sets the ControlTarget to the target value set via the atomic and stores the current SensorValue to the other atomic.
void NJointPositionPassThroughController::rtRun(const IceUtil::Time& t, const IceUtil::Time&)
{
target->position = targetPos;
currentPos = sensor->position;
}
#define ARMARX_RT_LOGF_IMPORTANT(...)
Definition ControlThreadOutputBuffer.h:346
#define ARMARX_RT_LOGF_ERROR(...)
Definition ControlThreadOutputBuffer.h:352
The publish activation hook does nothing, but could be used to call DebugDraver::setRobotVisu.
The publish deactivation hook clears the DebugDrawer layer
{
}
std::string getInstanceName(const Ice::Current &=Ice::emptyCurrent) const final override
Definition NJointControllerBase.h:804
The publish hook draws a sphere with radius of the position stored in the atomic times 2000 to the DebugDrawer.
{
drawer->setSphereVisu(
"Layer_" +
getInstanceName (),
"positionball" ,
new Vector3 , armarx::DrawColor {1, 1, 1, 1}, currentPos * 2000);
}
The Vector3 class.
Definition Pose.h:113
This registers a factory for this NJointControllerBase . This factory is used by the RobotUnit to create the NJointControllerBase . The passed name has to match the name returned by getClassName .
Definition NJointControllerRegistry.h:231
This statically asserts a factory for the type NJointPositionPassThroughController is present.
#define ARMARX_ASSERT_NJOINTCONTROLLER_HAS_CONSTRUCTION_GUI(T)
Definition NJointControllerRegistry.h:242
Close the namespace
Inheritance diagram for NJointControllerBase: