|
The DeprecatedNJointPeriodicTSDMPCompliantController class. More...
Public Types | |
using | ConfigPtrT = DeprecatedNJointPeriodicTSDMPCompliantControllerConfigPtr |
Public Types inherited from NJointControllerWithTripleBuffer< DeprecatedNJointPeriodicTSDMPCompliantControllerControlData > | |
using | LockGuardType = std::lock_guard< std::recursive_mutex > |
using | MutexType = std::recursive_mutex |
Public Types inherited from NJointControllerBase | |
using | ConfigPtrT = NJointControllerConfigPtr |
using | GenerateConfigDescriptionFunctionSignature = WidgetDescription::WidgetPtr(*)(const VirtualRobot::RobotPtr &, const std::map< std::string, ConstControlDevicePtr > &controlDevices, const std::map< std::string, ConstSensorDevicePtr > &sensorDevices) |
template<class ConfigPrtType > | |
using | GenerateConfigFromVariantsFunctionSignature = ConfigPrtType(*)(const StringVariantBaseMap &) |
Public Types inherited from ManagedIceObject | |
using | PeriodicTaskPtr = IceUtil::Handle< SimplePeriodicTask< std::function< void(void)> >> |
Public Member Functions | |
DeprecatedNJointPeriodicTSDMPCompliantController (const RobotUnitPtr &, const NJointControllerConfigPtr &config, const VirtualRobot::RobotPtr &) | |
double | getCanVal (const Ice::Current &) override |
std::string | getClassName (const Ice::Current &) const |
Eigen::Vector6f | getFilteredForceTorque (Eigen::Vector3f &forceBaseline, Eigen::Vector3f &torqueBaseline, Eigen::Vector6f &handCompensatedFT) |
std::string | getKinematicChainName (const Ice::Current &) |
Eigen::Vector6f | getTCPGravityCompensation (Eigen::Matrix4f ¤tPose, float tcpMass) |
bool | isFinished (const Ice::Current &) override |
void | learnDMPFromFiles (const Ice::StringSeq &fileNames, const Ice::Current &) override |
void | learnDMPFromTrajectory (const TrajectoryBasePtr &trajectory, const Ice::Current &) override |
void | pauseDMP (const Ice::Current &) override |
void | resetDMP (const Ice::Current &) override |
void | resumeDMP (const Ice::Current &) override |
void | rtRun (const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) |
TODO make protected and use attorneys. More... | |
void | runDMP (const Ice::DoubleSeq &goals, Ice::Double tau, const Ice::Current &) override |
void | setAmplitude (Ice::Double amp, const Ice::Current &) override |
void | setGoals (const Ice::DoubleSeq &goals, const Ice::Current &) override |
void | setKdImpedance (const Eigen::Vector6f &kdImpedance, const Ice::Current &) override |
void | setKpImpedance (const Eigen::Vector6f &kpImpedance, const Ice::Current &) override |
void | setSpeed (Ice::Double times, const Ice::Current &) override |
void | setTargetForceInRootFrame (Ice::Float force, const Ice::Current &) override |
void | stopDMP (const Ice::Current &) override |
Public Member Functions inherited from NJointControllerWithTripleBuffer< DeprecatedNJointPeriodicTSDMPCompliantControllerControlData > | |
NJointControllerWithTripleBuffer (const DeprecatedNJointPeriodicTSDMPCompliantControllerControlData &initialCommands=DeprecatedNJointPeriodicTSDMPCompliantControllerControlData()) | |
void | rtSwapBufferAndRun (const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration) override |
Public Member Functions inherited from NJointControllerBase | |
void | activateController (const Ice::Current &=Ice::emptyCurrent) final 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) final override |
void | deleteController (const Ice::Current &=Ice::emptyCurrent) final override |
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. More... | |
ConstSensorDevicePtr | peekSensorDevice (const std::string &deviceName) const |
Get a const ptr to the given SensorDevice. More... | |
const std::string & | rtGetClassName () const |
Returns the class name. More... | |
const std::vector< std::size_t > & | rtGetControlDeviceUsedIndices () const |
Returns the indices of all ControlDevice's this NJointControllerBase calculates a ControlTarget for. More... | |
bool | rtGetErrorState () const |
Sets the error state to true. More... | |
const std::string & | rtGetInstanceName () const |
Returns the instance name. More... | |
std::size_t | rtGetNumberOfUsedControlDevices () const |
Returns the number of used ControlDevices. More... | |
const VirtualRobot::RobotPtr & | rtGetRobot () |
TODO make protected and use attorneys. More... | |
const std::vector< VirtualRobot::RobotNodePtr > & | rtGetRobotNodes () |
Returns the nodes of the virtual robot used by this NJointControllerBase in the rtRun. More... | |
bool | rtUsesControlDevice (std::size_t deviceIndex) const |
Returns whether this NJointControllerBase calculates a ControlTarget for the given ControlDevice. More... | |
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. More... | |
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. More... | |
template<class T > | |
const T * | useSensorValue (const std::string &deviceName) const |
Get a const ptr to the given SensorDevice's SensorValue. More... | |
const SensorValueBase * | useSensorValue (const std::string &sensorDeviceName) const |
Get a const ptr to the given SensorDevice's SensorValue. More... | |
const VirtualRobot::RobotPtr & | useSynchronizedRtRobot (bool updateCollisionModel=false) |
Requests a VirtualRobot for use in rtRun *. More... | |
~NJointControllerBase () override | |
Public Member Functions inherited from ManagedIceObject | |
void | enableProfiler (bool enable) |
setProfiler allows setting ManagedIceObject::profiler to a new instance (if the new instance is actually not a null pointer) More... | |
std::string | generateSubObjectName (const std::string &subObjectName) |
Generates a unique name for a sub object from a general name. More... | |
ArmarXManagerPtr | getArmarXManager () const |
Returns the ArmarX manager used to add and remove components. More... | |
ManagedIceObjectConnectivity | getConnectivity () const |
Retrieve connectivity of the object (topcis as well as proxies) More... | |
IceManagerPtr | getIceManager () const |
Returns the IceManager. More... | |
VariantBasePtr | getMetaInfo (const std::string &id) |
StringVariantBaseMap | getMetaInfoMap () const |
std::string | getName () const |
Retrieve name of object. More... | |
Ice::ObjectAdapterPtr | getObjectAdapter () const |
Returns object's Ice adapter. More... | |
ArmarXObjectSchedulerPtr | getObjectScheduler () const |
PeriodicTaskPtr | getPeriodicTask (const std::string &name) |
Profiler::ProfilerPtr | getProfiler () const |
getProfiler returns an instance of armarx::Profiler More... | |
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. More... | |
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 . More... | |
Ice::ObjectPrx | getProxy (long timeoutMs=0, bool waitForScheduler=true) const |
Returns the proxy of this object (optionally it waits for the proxy) More... | |
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). More... | |
template<class Prx > | |
void | getProxy (Prx &prx, long timeoutMs=0, bool waitForScheduler=true) const |
int | getState () const |
Retrieve current state of the ManagedIceObject. More... | |
template<class TopicProxyType > | |
TopicProxyType | getTopic (const std::string &name) |
Returns a proxy of the specified topic. More... | |
template<class TopicProxyType > | |
void | getTopic (TopicProxyType &topicProxy, const std::string &name) |
Assigns a proxy of the specified topic to topicProxy . More... | |
std::vector< std::string > | getUnresolvedDependencies () const |
returns the names of all unresolved dependencies More... | |
ManagedIceObject (ManagedIceObject const &other) | |
void | offeringTopic (const std::string &name) |
Registers a topic for retrival after initialization. More... | |
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. More... | |
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. More... | |
bool | usingProxy (const std::string &name, const std::string &endpoints="") |
Registers a proxy for retrieval after initialization and adds it to the dependency list. More... | |
void | usingTopic (const std::string &name, bool orderedPublishing=false) |
Registers a proxy for subscription after initialization. More... | |
void | waitForObjectScheduler () |
Waits until the ObjectScheduler could resolve all dependencies. More... | |
void | waitForProxy (std::string const &name, bool addToDependencies) |
Public Member Functions inherited from Logging | |
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. More... | |
MessageTypeT | getEffectiveLoggingLevel () const |
Logging () | |
void | setLocalMinimumLoggingLevel (MessageTypeT level) |
With setLocalMinimumLoggingLevel the minimum verbosity-level of log-messages can be set. More... | |
void | setTag (const LogTag &tag) |
void | setTag (const std::string &tagName) |
virtual | ~Logging () |
Public Member Functions inherited from DeprecatedNJointPeriodicTSDMPCompliantControllerInterface | |
double | getCanVal () |
string | getKinematicChainName () |
bool | isFinished () |
void | learnDMPFromFiles (Ice::StringSeq trajfiles) |
void | learnDMPFromTrajectory (TrajectoryBase trajectory) |
void | pauseDMP () |
void | resetDMP () |
void | resumeDMP () |
void | runDMP (Ice::DoubleSeq goals, double tau) |
void | setAmplitude (double amplitude) |
void | setGoals (Ice::DoubleSeq goals) |
void | setKdImpedance (Eigen::Vector6f kdImpedance) |
void | setKpImpedance (Eigen::Vector6f kpImpedance) |
void | setSpeed (double times) |
void | setTargetForceInRootFrame (float force) |
void | stopDMP () |
Protected Member Functions | |
void | controllerRun () |
void | onDisconnectNJointController () |
void | onInitNJointController () |
virtual void | onPublish (const SensorAndControl &, const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) |
Protected Member Functions inherited from NJointControllerWithTripleBuffer< DeprecatedNJointPeriodicTSDMPCompliantControllerControlData > | |
DeprecatedNJointPeriodicTSDMPCompliantControllerControlData & | getWriterControlStruct () |
void | reinitTripleBuffer (const DeprecatedNJointPeriodicTSDMPCompliantControllerControlData &initial) |
const DeprecatedNJointPeriodicTSDMPCompliantControllerControlData & | rtGetControlStruct () const |
bool | rtUpdateControlStruct () |
void | setControlStruct (const DeprecatedNJointPeriodicTSDMPCompliantControllerControlData &newStruct) |
void | writeControlStruct () |
Protected Member Functions inherited from NJointControllerBase | |
std::string | getDefaultName () const override |
ThreadPoolPtr | getThreadPool () const |
void | onConnectComponent () final |
virtual void | onConnectNJointController () |
void | onDisconnectComponent () final |
void | onExitComponent () final |
virtual void | onExitNJointController () |
void | onInitComponent () final |
virtual void | onPublishActivation (const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) |
virtual void | onPublishDeactivation (const DebugDrawerInterfacePrx &, const DebugObserverInterfacePrx &) |
virtual void | rtPostDeactivateController () |
This function is called after the controller is deactivated. More... | |
virtual void | rtPreActivateController () |
This function is called before the controller is activated. More... | |
void | rtSetErrorState () |
Sets the error state to true. More... | |
template<typename Task > | |
void | runTask (const std::string &taskName, Task &&task) |
Executes a given task in a separate thread from the Application ThreadPool. More... | |
Protected Member Functions inherited from ManagedIceObject | |
template<class PluginT , class... ParamsT> | |
PluginT * | addPlugin (const std::string prefix="", ParamsT &&...params) |
template<class PluginT , class... ParamsT> | |
void | addPlugin (PluginT *&targ, const std::string prefix="", ParamsT &&...params) |
template<class PluginT , class... ParamsT> | |
void | addPlugin (std::experimental::observer_ptr< PluginT > &targ, const std::string prefix="", ParamsT &&...params) |
Ice::CommunicatorPtr | getCommunicator () const |
std::unique_ptr< ManagedIceObjectPlugin > & | getPluginPointer (std::type_info const &type, std::string const &prefix) |
ManagedIceObject () | |
Protected default constructor. More... | |
virtual void | postOnConnectComponent () |
virtual void | postOnDisconnectComponent () |
virtual void | postOnExitComponent () |
virtual void | postOnInitComponent () |
virtual void | preOnConnectComponent () |
virtual void | preOnDisconnectComponent () |
virtual void | preOnExitComponent () |
virtual void | preOnInitComponent () |
bool | removeProxyDependency (const std::string &name) |
This function removes the dependency of this object on the in parameter name specified object. More... | |
void | setName (std::string name) |
Override name of well-known object. More... | |
void | terminate () |
Initiates termination of this IceManagedObject. More... | |
~ManagedIceObject () override | |
Protected Member Functions inherited from Logging | |
bool | checkLogLevel (MessageTypeT level) const |
const LogSenderPtr & | getLogSender () const |
Retrieve log sender. More... | |
LogSenderPtr | loghelper (const char *file, int line, const char *function) const |
Additional Inherited Members | |
Static Public Member Functions inherited from NJointControllerBase | |
template<class ItT > | |
static std::optional< std::vector< char > > | AreNotInConflict (ItT first, ItT last) |
Static Public Member Functions inherited from ManagedIceObject | |
static std::string | generateSubObjectName (const std::string &superObjectName, const std::string &subObjectName) |
Generates a unique name for a sub object from a general name and unique name. More... | |
static std::string | GetObjectStateAsString (int state) |
Static Public Attributes inherited from NJointControllerBase | |
static const NJointControllerBasePtr | NullPtr {nullptr} |
Static Public Attributes inherited from ManagedIceObject | |
static const ManagedIceObjectPtr | NullPtr |
A nullptr to be used when a const ref to a nullptr is required. More... | |
Protected Attributes inherited from NJointControllerWithTripleBuffer< DeprecatedNJointPeriodicTSDMPCompliantControllerControlData > | |
MutexType | controlDataMutex |
Protected Attributes inherited from NJointControllerBase | |
std::map< std::string, std::shared_ptr< ThreadPool::Handle > > | threadHandles |
std::mutex | threadHandlesMutex |
Protected Attributes inherited from Logging | |
MessageTypeT | minimumLoggingLevel |
SpamFilterDataPtr | spamFilter |
LogTag | tag |
The DeprecatedNJointPeriodicTSDMPCompliantController class.
Definition at line 41 of file DeprecatedNJointPeriodicTSDMPCompliantController.h.
using ConfigPtrT = DeprecatedNJointPeriodicTSDMPCompliantControllerConfigPtr |
Definition at line 46 of file DeprecatedNJointPeriodicTSDMPCompliantController.h.
DeprecatedNJointPeriodicTSDMPCompliantController | ( | const RobotUnitPtr & | , |
const NJointControllerConfigPtr & | config, | ||
const VirtualRobot::RobotPtr & | |||
) |
setup force sensor and filters
Setup tool
setup DMP
initialize control parameters and buffers
Definition at line 10 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
protected |
Definition at line 199 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
override |
Definition at line 622 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
virtual |
Implements NJointControllerBase.
Definition at line 194 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
Eigen::Vector6f getFilteredForceTorque | ( | Eigen::Vector3f & | forceBaseline, |
Eigen::Vector3f & | torqueBaseline, | ||
Eigen::Vector6f & | handCompensatedFT | ||
) |
Definition at line 497 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
std::string getKinematicChainName | ( | const Ice::Current & | ) |
Definition at line 546 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
Eigen::Vector6f getTCPGravityCompensation | ( | Eigen::Matrix4f & | currentPose, |
float | tcpMass | ||
) |
Definition at line 483 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
override |
Definition at line 572 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
override |
Definition at line 551 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
override |
Definition at line 560 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
protectedvirtual |
Reimplemented from NJointControllerBase.
Definition at line 738 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
protectedvirtual |
initialize control parameters and buffers
start mp thread
Reimplemented from NJointControllerBase.
Definition at line 144 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
protectedvirtual |
Reimplemented from NJointControllerBase.
Definition at line 658 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
override |
Definition at line 646 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
override |
Definition at line 628 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
override |
Definition at line 651 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
virtual |
TODO make protected and use attorneys.
-------------------------— get current kinematics ------------------------------------------—
-------------------------— update control parameters ------------------------------------------—
-------------------------— get current force torque value ------------------------------------------—
-------------------------— get control targets ------------------------------------------—
rt is ready only if the FT sensor is calibrated, and the rt2interface buffer is updated (see above)
update tool information
convert to mm/s as in ArmarX
--------------------------— Impedance control ------------------------------------------— calculate pose error between virtual pose and current pose !!! This is very important: you have to keep postion and orientation both with UI unit (meter, radian) to calculate impedance force.
--------------------------— Nullspace PD Control -----------------------------------------------—
--------------------------— Map TS target force to JS -----------------------------------------------—
--------------------------— Send torque command to joint device -----------------------------------------------—
Implements SynchronousNJointController.
Definition at line 264 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
override |
Definition at line 603 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
override |
Definition at line 590 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
override |
Definition at line 584 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
override |
Definition at line 539 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
override |
Definition at line 532 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
override |
Definition at line 577 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
override |
Definition at line 596 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.
|
override |
Definition at line 638 of file DeprecatedNJointPeriodicTSDMPCompliantController.cpp.