From 7c17d2719b1fd1f76bfaf61ad207b37934bd4839 Mon Sep 17 00:00:00 2001
From: Raphael <ufdrv@student.kit.edu>
Date: Sat, 28 Jan 2017 10:45:33 +0100
Subject: [PATCH] compiling state
added lvl1 controller interface (+ template for buffer swapping)
added lvl0 controller interfaces
added controlmodes namespace + default control modes
all data units now are splitted into an interface (this has to be implemented) and a ptrprovider (this is a conveniene class. it stores pointer and implements the interface. if all data is already somewhere in ram, you can set the pointers of this class and have a working data unit)
---
.../RobotAPI/components/units/CMakeLists.txt | 3 -
source/RobotAPI/components/units/RobotUnit.h | 334 ------------------
.../libraries/Controllers/LVL1Controller.ice | 2 +-
.../libraries/Controllers/CMakeLists.txt | 9 +-
.../libraries/Controllers/ControlModes.h | 39 ++
.../DataUnits/ForceTorqueDataUnit.h | 7 +-
.../Controllers/DataUnits/HapticDataUnit.h | 7 +-
.../Controllers/DataUnits/IMUDataUnit.h | 7 +-
.../Controllers/DataUnits/KinematicDataUnit.h | 20 +-
.../Controllers/DataUnits/PlatformDataUnit.h | 25 +-
.../libraries/Controllers/LVL0Controller.h | 64 +---
.../libraries/Controllers/LVL1Controller.h | 183 +++++++---
.../Controllers}/RobotUnit.cpp | 36 +-
.../libraries/Controllers/RobotUnit.h | 324 +++++++++++++++++
.../libraries/Controllers/SyntaxCheck.cpp | 1 -
.../Controllers/Targets/JointPositionTarget.h | 4 +-
.../Targets/JointPositionVelocityTarget.h | 63 ----
.../Controllers/Targets/JointTargetBase.h | 4 +-
.../Controllers/Targets/JointTorqueTarget.h | 4 +-
.../Controllers/Targets/JointVelocityTarget.h | 4 +-
20 files changed, 597 insertions(+), 543 deletions(-)
delete mode 100644 source/RobotAPI/components/units/RobotUnit.h
create mode 100644 source/RobotAPI/libraries/Controllers/ControlModes.h
rename source/RobotAPI/{components/units => libraries/Controllers}/RobotUnit.cpp (68%)
create mode 100644 source/RobotAPI/libraries/Controllers/RobotUnit.h
delete mode 100644 source/RobotAPI/libraries/Controllers/Targets/JointPositionVelocityTarget.h
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 33f5e52b8..90567cf96 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -21,7 +21,6 @@ set(LIBS
RobotAPICore
ArmarXCoreObservers
ArmarXCoreEigen3Variants
- Controllers
${Simox_LIBRARIES})
set(LIB_HEADERS
@@ -42,7 +41,6 @@ set(LIB_HEADERS
KinematicUnitSimulation.h
PlatformUnit.h
RobotPoseUnit.h
- RobotUnit.h
PlatformUnitSimulation.h
KinematicUnitObserver.h
PlatformUnitObserver.h
@@ -69,7 +67,6 @@ set(LIB_FILES
PlatformUnit.cpp
PlatformUnitSimulation.cpp
RobotPoseUnit.cpp
- RobotUnit.cpp
KinematicUnitObserver.cpp
PlatformUnitObserver.cpp
SensorActorUnit.cpp
diff --git a/source/RobotAPI/components/units/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit.h
deleted file mode 100644
index 15cf2e566..000000000
--- a/source/RobotAPI/components/units/RobotUnit.h
+++ /dev/null
@@ -1,334 +0,0 @@
-/*
- * 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::RobotUnit
- * @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_UNIT_RobotAPI_RobotUnit_H
-#define _ARMARX_UNIT_RobotAPI_RobotUnit_H
-
-#include <thread>
-#include <atomic>
-#include <chrono>
-#include <mutex>
-#include <unordered_map>
-
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/util/algorithm.h>
-#include <ArmarXCore/core/util/OnScopeExit.h>
-
-#include <RobotAPI/interface/units/RobotUnitInterface.h>
-#include <RobotAPI/libraries/Controllers/LVL0Controller.h>
-
-namespace armarx
-{
- /**
- * @class RobotUnitPropertyDefinitions
- * @brief
- */
- class RobotUnitPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
- {
- public:
- RobotUnitPropertyDefinitions(std::string prefix):
- armarx::ComponentPropertyDefinitions(prefix)
- {
- //defineRequiredProperty<std::string>("PropertyName", "Description");
- //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
- }
- };
-
- /**
- * @defgroup Component-RobotUnit RobotUnit
- * @ingroup RobotAPI-Components
- * A description of the component RobotUnit.
- *
- * @class RobotUnit
- * @ingroup Component-RobotUnit
- * @brief Brief description of class RobotUnit.
- *
- * Detailed description of class RobotUnit.
- */
- class RobotUnit :
- virtual public armarx::Component,
- virtual public armarx::RobotUnitInterface
- {
- public:
- /**
- * @see armarx::ManagedIceObject::getDefaultName()
- */
- virtual std::string getDefaultName() const
- {
- return "RobotUnit";
- }
-
- // RobotUnitInterface interface
- //robot
- virtual NameControlModeMap getJointControlModeMap(const Ice::Current& = GlobalIceCurrent) const override;
-
- //controllers
- virtual StringLVL1ControllerPrxDictionary getAllControllers(const Ice::Current& = GlobalIceCurrent) const override
- {
- std::lock_guard<std::mutex> guard {slowDataMutex};
- StringLVL1ControllerPrxDictionary result;
- std::transform(
- loadedLVL1Controllers.begin(), loadedLVL1Controllers.end(), std::back_inserter(result),
- [](const LVL1ControllerPtr & lvl1)
- {
- return std::make_pair(lvl1->getName(), lvl1->getProxy());
- }
- );
- return result;
- }
- virtual StringLVL1ControllerPrxDictionary getActiveControllers(const Ice::Current& = GlobalIceCurrent) const override
- {
- std::lock_guard<std::mutex> guard {slowDataMutex};
- StringLVL1ControllerPrxDictionary result;
- std::transform(
- activeLVL1Controllers.begin(), activeLVL1Controllers.end(), std::back_inserter(result),
- [](const LVL1ControllerPtr & lvl1)
- {
- return std::make_pair(lvl1->getName(), lvl1->getProxy());
- }
- );
- return result;
- }
-
- virtual bool switchSetup(const Ice::StringSeq& , const Ice::Current& = GlobalIceCurrent) override;
- virtual LVL1ControllerInterfacePrx loadController(
- const std::string& className, const std::string& instanceName, const Ice::StringSeq& joints, const LVL1ControllerConfigPtr&,
- const Ice::Current& = GlobalIceCurrent) override;
-
-
- virtual bool loadLibFromPath(const std::string& path, const Ice::Current& = GlobalIceCurrent) const override
- {
- throw "NYI"; ///TODO mirko
- }
- virtual bool loadLibFromPackage(const std::string& package, const std::string& name, const Ice::Current& = GlobalIceCurrent) const override
- {
- throw "NYI"; ///TODO mirko
- }
- //units
- virtual KinematicUnitInterfacePrx getKinematicUnit(const Ice::Current& = GlobalIceCurrent) const override
- {
- return kinematicUnitPrx;
- }
- virtual ForceTorqueUnitInterfacePrx getForceTorqueUnit(const Ice::Current& = GlobalIceCurrent) const override
- {
- return forceTorqueUnitPrx;
- }
- virtual InertialMeasurementUnitInterfacePrx getInertialMeasurementUnit(const Ice::Current& = GlobalIceCurrent) const override
- {
- return inertialMeasurementUnitPrx;
- }
- virtual PlatformUnitInterfacePrx getPlatformUnitInterface(const Ice::Current& = GlobalIceCurrent) const override
- {
- return platformUnitPrx;
- }
-
- //functions subclasses need to implement
- virtual ControlModeBoolMap supportedControlModes(const Ice::Current& = GlobalIceCurrent) = 0;
- virtual void initializeLVL0Controllers() = 0;
- virtual void initializeBus() = 0;
- virtual void copyControlBufferOut() = 0;
- /**
- * @brief send and receive data on the bus
- */
- virtual void sendReceiveData() = 0;
- virtual void shutdownBus() = 0;
- virtual void emergencyStop() = 0;
-// virtual void stopRobot() = 0; // may be implemented as {emergencyStop();}
-
- virtual void controlLoopIteration()
- {
- ///how bridge loop while controllers are switched? (need some halt mode)
- for (auto & lvl1 : activeLVL1Controllers)
- {
- lvl1->run();
- }
- for (auto & lvl0 : activeLVL0Controllers)
- {
- lvl0->run();
- }
- copyControlBufferOut();
- sendReceiveData();
- }
-
- protected:
- /**
- * @see armarx::ManagedIceObject::onInitComponent()
- */
- virtual void onInitComponent() override
- {
-
- jointIndices = toIndexMap(jointNames);
- rtControlThread = std::thread {[this]{rtControlThread();}};
- }
-
- /**
- * @see armarx::ManagedIceObject::onConnectComponent()
- */
- virtual void onConnectComponent() override;
-
- /**
- * @see armarx::ManagedIceObject::onDisconnectComponent()
- */
- virtual void onDisconnectComponent() override;
-
- /**
- * @see armarx::ManagedIceObject::onExitComponent()
- */
- virtual void onExitComponent() override;
-
- /**
- * @see PropertyUser::createPropertyDefinitions()
- */
- virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
-
- //slow data (data changed in non rt threads)
- mutable std::mutex slowDataMutex;
- std::vector<LVL1ControllerPtr> loadedLVL1Controllers;
-
-
- //slow data: running data (this data changes only in switchSetup)
- NameControlModeMap jointControlModes;
- std::vector<LVL1ControllerPtr> activeLVL1Controllers;
- std::vector<LVL0ControllerBase*> activeLVL0Controllers;
-
-// //slow data: units
-// KinematicUnitInterfacePtr kinematicUnit;
-// KinematicUnitInterfacePrx kinematicUnitPrx;
-// ForceTorqueUnitInterfacePtr forceTorqueUnit;
-// ForceTorqueUnitInterfacePrx forceTorqueUnitPrx;
-// InertialMeasurementUnitInterfacePtr inertialMeasurementUnit;
-// InertialMeasurementUnitInterfacePrx inertialMeasurementUnitPrx;
-// PlatformUnitInterfacePtr platformUnit;
-// PlatformUnitInterfacePrx platformUnitPrx;
-
- //constant data (this data does not change after onInitComponent)
- std::vector<std::string> jointNames;
- std::unordered_map<std::string, std::size_t> jointIndices;
- std::chrono::nanoseconds controlLoopPeriod;
- std::chrono::nanoseconds controlLoopEmergencyStopPeriod;
- std::map<std::string, std::vector<LVL0ControllerBase*>> lvl0ControllerMap;
-
- private:
- enum class ControlThreadState
- {
- Unborn,
- Uninitialized,
- ControllerLoop,
- /**
- * @brief The LVL1 controllers are switched.
- * The controller loop now bridges this period.
- */
- BridgingControllerSwitch,
- Exception,
- Dead
- };
-
- //fast data (used in rt threads)
- std::thread rtControlThread;
- std::atomic_bool stopControlLoop {false};
- std::atomic<ControlThreadState> controlThreadState {ControlThreadState::Unborn};
- std::atomic_bool requestBridging {true};
- void rtControlThreadFunction()
- {
- controlThreadState = ControlThreadState::Uninitialized;
- initializeBus();
- ARMARX_ON_SCOPE_EXIT
- {
- shutdownBus();
- controlThreadState = ControlThreadState::Dead;
- };
- try
- {
- initializeLVL0Controllers();
- //validate
- for (const auto & mode : supportedControlModes)
- {
- switch (mode.first)
- {
-#define ERROR_HEAD "Wrong number of LVL0Controllers for"
-#define ERROR_TAIL ". (If supported, each joint needs one controller. If unsupported no controllers must be initialized!"
- case ePositionControl:
- if (lvl0PositionControlController.size() != mode.second * jointNames.size())
- {
- throw std::invalid_argument {ERROR_HEAD "ePositionControl" ERROR_TAIL};
- }
- break;
- case eVelocityControl:
- if (lvl0VelocityControlController.size() != mode.second * jointNames.size())
- {
- throw std::invalid_argument {ERROR_HEAD "eVelocityControl" ERROR_TAIL};
- }
- break;
- case eTorqueControl:
- if (lvl0TorqueControlController.size() != mode.second * jointNames.size())
- {
- throw std::invalid_argument {ERROR_HEAD "eTorqueControl" ERROR_TAIL};
- }
- break;
- case ePositionVelocityControl:
- if (lvl0PositionVelocityControlController.size() != mode.second * jointNames.size())
- {
- throw std::invalid_argument {ERROR_HEAD "ePositionVelocityControl" ERROR_TAIL};
- }
- break;
-#undef ERROR_HEAD
-#undef ERROR_TAIL
- default:
- if (mode.second)
- {
- throw std::invalid_argument {"Unknown ControlMode supported! mode = " + std::to_string(mode.first)};
- }
- }
- }
- controlThreadState = ControlThreadState::ControllerLoop;///TODO bridging
- //loop
- while (!stopControlLoop)
- {
- auto iterationStartTime = std::chrono::high_resolution_clock::now();
- controlLoopIteration();
- std::this_thread::sleep_until(iterationStartTime + controlLoopPeriod);
- }
-
- }
- catch (...)
- {
- controlThreadState = ControlThreadState::Exception;
- emergencyStop();
- //TODO HANDLING
- }
- }
-
- };
-}
-
-#endif
-
-/*
-
-lvl1 controller loaded + switched in non rt
-rt protects it via bool locking?
-
-how bridge a controler switch
-
-
- */
diff --git a/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice b/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice
index 65846b053..2804d5d29 100644
--- a/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice
+++ b/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice
@@ -35,7 +35,7 @@ module armarx
{
["cpp:const"] idempotent string getClassName();
["cpp:const"] idempotent string getInstanceName();
- ["cpp:const"] idempotent NameControlModeMap getJointControlModeMap();
+ ["cpp:const"] idempotent StringStringDictionary getJointControlModeMap();
["cpp:const"] idempotent bool isControllerActive();
};
diff --git a/source/RobotAPI/libraries/Controllers/CMakeLists.txt b/source/RobotAPI/libraries/Controllers/CMakeLists.txt
index 78dddbb70..edb54d7b2 100644
--- a/source/RobotAPI/libraries/Controllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/Controllers/CMakeLists.txt
@@ -14,7 +14,8 @@ set(LIB_NAME Controllers)
set(LIBS
ArmarXCoreInterfaces
- ArmarXCore
+ ArmarXCore
+ RobotAPIUnits
RobotAPIInterfaces
)
@@ -23,14 +24,16 @@ set(LIB_FILES
LVL0Controller.cpp
LVL1Controller.cpp
+
+ RobotUnit.cpp
)
set(LIB_HEADERS
Constants.h
+ ControlModes.h
Targets/JointTargetBase.h
Targets/JointPositionTarget.h
Targets/JointVelocityTarget.h
- Targets/JointPositionVelocityTarget.h
Targets/JointTorqueTarget.h
DataUnits/ForceTorqueDataUnit.h
@@ -41,6 +44,8 @@ set(LIB_HEADERS
LVL0Controller.h
LVL1Controller.h
+
+ RobotUnit.h
)
diff --git a/source/RobotAPI/libraries/Controllers/ControlModes.h b/source/RobotAPI/libraries/Controllers/ControlModes.h
new file mode 100644
index 000000000..3cdd7d8d1
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/ControlModes.h
@@ -0,0 +1,39 @@
+/*
+ * 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
+ * @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_ControlModes_H
+#define _ARMARX_LIB_RobotAPI_ControlModes_H
+
+#include <string>
+
+namespace armarx
+{
+ namespace ControlModes
+ {
+ static const std::string VelocityMode = "VelocityMode";
+ static const std::string PositionMode = "PositionMode";
+ static const std::string TorqueMode = "TorqueMode";
+ static const std::string EmergencyStopMode = "EmergencyStopMode";
+ }
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/ForceTorqueDataUnit.h b/source/RobotAPI/libraries/Controllers/DataUnits/ForceTorqueDataUnit.h
index 4c3f05469..a297a4879 100644
--- a/source/RobotAPI/libraries/Controllers/DataUnits/ForceTorqueDataUnit.h
+++ b/source/RobotAPI/libraries/Controllers/DataUnits/ForceTorqueDataUnit.h
@@ -25,9 +25,14 @@
namespace armarx
{
- class ForceTorqueDataUnit
+ class ForceTorqueDataUnitInterface
{
+ public:
+ };
+ class ForceTorqueDataUnitPtrProvider: virtual public ForceTorqueDataUnitInterface
+ {
+ public:
};
}
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/HapticDataUnit.h b/source/RobotAPI/libraries/Controllers/DataUnits/HapticDataUnit.h
index d9f6ac30d..de02fbe00 100644
--- a/source/RobotAPI/libraries/Controllers/DataUnits/HapticDataUnit.h
+++ b/source/RobotAPI/libraries/Controllers/DataUnits/HapticDataUnit.h
@@ -25,9 +25,14 @@
namespace armarx
{
- class HapticDataUnit
+ class HapticDataUnitInterface
{
+ public:
+ };
+ class HapticDataUnitPtrProvider: virtual public HapticDataUnitInterface
+ {
+ public:
};
}
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/IMUDataUnit.h b/source/RobotAPI/libraries/Controllers/DataUnits/IMUDataUnit.h
index d337532b6..3e7433cd1 100644
--- a/source/RobotAPI/libraries/Controllers/DataUnits/IMUDataUnit.h
+++ b/source/RobotAPI/libraries/Controllers/DataUnits/IMUDataUnit.h
@@ -25,9 +25,14 @@
namespace armarx
{
- class IMUDataUnit
+ class IMUDataUnitInterface
{
+ public:
+ };
+ class IMUDataUnitPtrProvider: virtual public IMUDataUnitInterface
+ {
+ public:
};
}
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/KinematicDataUnit.h b/source/RobotAPI/libraries/Controllers/DataUnits/KinematicDataUnit.h
index fbb2f91ac..b305da00e 100644
--- a/source/RobotAPI/libraries/Controllers/DataUnits/KinematicDataUnit.h
+++ b/source/RobotAPI/libraries/Controllers/DataUnits/KinematicDataUnit.h
@@ -32,10 +32,22 @@
namespace armarx
{
- class KinematicDataUnit
+ class KinematicDataUnitInterface
{
public:
- KinematicDataUnit(const std::vector<std::string>& jointNames):
+ virtual const std::vector<std::string>& getJointNames() const = 0;
+ virtual std::vector<const float*> getJointAngles(const std::vector<std::string>& joints) const = 0;
+ virtual std::vector<const float*> getJointVelocities(const std::vector<std::string>& joints) const = 0;
+ virtual std::vector<const float*> getJointTorques(const std::vector<std::string>& joints) const = 0;
+ virtual std::vector<const float*> getJointCurrents(const std::vector<std::string>& joints) const = 0;
+ virtual std::vector<const float*> getJointMotorTemperatures(const std::vector<std::string>& joints) const = 0;
+ virtual std::vector<const float*> getJointGearTemperatures(const std::vector<std::string>& joints) const = 0;
+ };
+
+ class KinematicDataUnitPtrProvider: virtual public KinematicDataUnitInterface
+ {
+ public:
+ KinematicDataUnitPtrProvider(const std::vector<std::string>& jointNames):
jointAngles(jointNames.size(), nullptr),
jointVelocites(jointNames.size(), nullptr),
jointTorques(jointNames.size(), nullptr),
@@ -45,8 +57,8 @@ namespace armarx
jointNames {jointNames},
jointIndices {toIndexMap(jointNames)}
{}
- KinematicDataUnit() = default;
- KinematicDataUnit(const KinematicDataUnit&) = default;
+ KinematicDataUnitPtrProvider() = default;
+ KinematicDataUnitPtrProvider(const KinematicDataUnitPtrProvider&) = default;
const std::vector<std::string>& getJointNames() const
{
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/PlatformDataUnit.h b/source/RobotAPI/libraries/Controllers/DataUnits/PlatformDataUnit.h
index 521e30c8d..4d4b70ee0 100644
--- a/source/RobotAPI/libraries/Controllers/DataUnits/PlatformDataUnit.h
+++ b/source/RobotAPI/libraries/Controllers/DataUnits/PlatformDataUnit.h
@@ -25,31 +25,42 @@
namespace armarx
{
- class PlatformDataUnit
+ class PlatformDataUnitInterface
{
public:
- const float* getPositionX() const
+ virtual const float* getPositionX() const = 0;
+ virtual const float* getPositionY() const = 0;
+ virtual const float* getRotation() const = 0;
+ virtual const float* getVelocityX() const = 0;
+ virtual const float* getVelocityY() const = 0;
+ virtual const float* getVelocityRotation() const = 0;
+ };
+
+ class PlatformDataUnitPtrProvider : public virtual PlatformDataUnitInterface
+ {
+ public:
+ virtual const float* getPositionX() const override
{
return positionX;
}
- const float* getPositionY() const
+ virtual const float* getPositionY() const override
{
return positionY;
}
- const float* getRotation() const
+ virtual const float* getRotation() const override
{
return rotation;
}
- const float* getVelocityX() const
+ virtual const float* getVelocityX() const override
{
return velocityX;
}
- const float* getVelocityY() const
+ virtual const float* getVelocityY() const override
{
return velocityY;
}
- const float* getVelocityRotation() const
+ virtual const float* getVelocityRotation() const override
{
return velocityRotation;
}
diff --git a/source/RobotAPI/libraries/Controllers/LVL0Controller.h b/source/RobotAPI/libraries/Controllers/LVL0Controller.h
index c2cdbb1ee..e27480702 100644
--- a/source/RobotAPI/libraries/Controllers/LVL0Controller.h
+++ b/source/RobotAPI/libraries/Controllers/LVL0Controller.h
@@ -29,59 +29,25 @@ namespace armarx
{
class LVL0ControllerBase
{
+ public:
virtual void run() = 0;
- virtual JointTargetBase* getTarget() =0;
- };
-
-// /**
-// * @defgroup Library-Controllers Controllers
-// * @ingroup RobotAPI
-// * A description of the library Controllers.
-// *
-// * @class LVL0Controller
-// * @ingroup Library-Controllers
-// * @brief Brief description of class LVL0Controller.
-// *
-// * Detailed description of class LVL0Controller.
-// */
-// template<class TargetT>
-// class LVL0Controller
-// {
-// public:
-// using TargetType = TargetT;
-// LVL0Controller()
-// {
-// resetTarget();
-// }
+ virtual JointTargetBase* getTarget() const = 0;
-// TargetType* getTarget()
-// {
-// return & target;
-// }
-// void resetTarget()
-// {
-// target.reset();
-// }
+ virtual void resetTarget()
+ {
+ getTarget()->reset();
+ }
-// virtual void run() override
-// {
-// if (!target.valid())
-// {
-// throw std::logic_error {"LVL0 Controller failed! Target is invalid"};
-// }
-// writeTarget();
-// resetTarget();
-// }
+ virtual bool isTargetVaild() const
+ {
+ return getTarget()->isValid();
+ }
-// /**
-// * @brief This function needs to be implemented for a LVL0Controller.
-// * It sends the command to the robot (or writes it in its send buffer).
-// */
-// void writeTarget() = 0;
-
-// private:
-// TargetType target;
-// };
+ virtual std::string getControlMode() const
+ {
+ return getTarget()->getControlMode();
+ }
+ };
}
#endif
diff --git a/source/RobotAPI/libraries/Controllers/LVL1Controller.h b/source/RobotAPI/libraries/Controllers/LVL1Controller.h
index 4861c98e7..3d3aca6c3 100644
--- a/source/RobotAPI/libraries/Controllers/LVL1Controller.h
+++ b/source/RobotAPI/libraries/Controllers/LVL1Controller.h
@@ -25,99 +25,182 @@
#include <map>
#include <atomic>
+#include <mutex>
#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/util/TrippleBuffer.h>
#include <RobotAPI/interface/libraries/Controllers/LVL1Controller.h>
+#include "RobotUnit.h"
#include "Targets/JointTargetBase.h"
namespace armarx
{
- class RobotUnit;
- /**
- * @defgroup Library-Controllers Controllers
- * @ingroup RobotAPI
- * A description of the library Controllers.
- *
- * @class LVL1Controller
- * @ingroup Library-Controllers
- * @brief Brief description of class LVL1Controller.
- *
- * Detailed description of class LVL1Controller.
- */
- template <typename ControlDataStruct>
+
class LVL1Controller:
virtual public LVL1ControllerInterface,
virtual public Component
{
public:
- LVL1Controller(std::string instanceName, RobotUnitPtr robotUnit, ControllerConfigPtr config);
- virtual void run(const IceUtil::Time& timestamp, const IceUtil::Time& delta) = 0;
-
+ //ice interface (must not be called in the rt thread)
virtual bool isControllerActive(const Ice::Current& = GlobalIceCurrent) const final
{
return isActive;
}
- const ControlDataStruct& readControlStruct()
- {
- return trippleBuffer.read();
- }
- void updateControlStruct()
- {
- trippleBuffer.write(data);
- }
-
- void swapBufferAndRun(const IceUtil::Time& timestamp, const IceUtil::Time& delta)
+ virtual std::string getInstanceName(const Ice::Current& = GlobalIceCurrent) const final
{
- readControlStruct();
- run(timestamp, delta);
+ return instanceName;
}
- ControlDataStruct& getWriterControlStruct()
+ virtual StringStringDictionary getJointControlModeMap(const Ice::Current& = GlobalIceCurrent) const final
{
- return controlData;
+ 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;
private:
- ControlDataStruct controlData;
friend class RobotUnit;
+ //this data is filled by the robot unit to provide convenience functions
+ std::string instanceName;
std::atomic_bool isActive {false};
+ StringStringDictionary jointControlModeMap;
};
using LVL1ControllerPtr = IceInternal::Handle<LVL1Controller>;
- class TrajectoryController : virtual public LVL1Controller<TrajectoryControlDataStruct>,
- public TrajectoryControllerInterface
+
+ /**
+ * @defgroup Library-Controllers Controllers
+ * @ingroup RobotAPI
+ * A description of the library Controllers.
+ *
+ * @class LVL1Controller
+ * @ingroup Library-Controllers
+ * @brief Brief description of class LVL1Controller.
+ *
+ * Detailed description of class LVL1Controller.
+ *
+ * \code{cpp}
+ //in ice
+ module armarx
+ {
+ struct SomeControllerConfig extends LVL1ControllerConfig
{
- JointVelocityTargetPtr targetJnt1Ptr;
+ 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;
- ExampleLvl1Controller(std::string instanceName, RobotUnitPtr robotUnit, ControllerConfigPtr config)
+ float someParamSetViaConfig;
+ ExampleLvl1Controller(RobotUnitPtr robotUnit, LVL1ControllerConfigPtr config):
+ LVL1Controller<SomeControlDataStruct>(SomeControlDataStruct(1)) //initial parameter value
{
- jointValuePtrs = robotUnit->getKinematicUnitData()->getJointAngles({"jnt1","jnt2"});
- targetJnt1Ptr = dynamic_cast<JointVelocityTarget*>(robotUnit->getJointTarget("jnt1", ControlModes::VelocityMode)); // for the results of this controller, goes to
-
+ //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->getKinematicUnitData(), "The RobotUnit " << robotUnit->getName() << " has no kinematic data unit");
+ //get pointers to sensor values from units
+ jointValuePtrs = robotUnit->getKinematicUnitData()->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);
}
- ExampleLvl1Controller(std::string instanceName, JointVelocityTargetPtr targetPtr, ControllerConfigPtr config)
+ ExampleLvl1Controller(JointVelocityTargetPtr targetPtr, ControllerConfigPtr config):
+ targetJointXPtr{targetPtr}
{
}
- void run(const IceUtil::Time& timestamp, const IceUtil::Time& delta)
+ virtual void run(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
{
- pid.update(TrajectoryControlDataStruct.pos, jointValuePtrs.at(0), delta.toMicroSeconds());
- *targetJnt1Ptr = pid.getValue();
+ pid.update(
+ rtGetControlStruct().parameterChangedViaIceCalls,
+ jointValuePtrs.at(0),
+ timeSinceLastIteration.toMicroSeconds());
+ *targetJointXPtr = pid.getValue() + someParamSetViaConfig;
}
-
- void setTrajectory(Trajectory traj, Ice::Current())
+ virtual void setSomeParamChangedViaIce(float param, const Ice::Current& = GlobalIceCurrent) override
{
- ScopedLock lock(mutex);
- controlData.pos = traj.pos.at(rand());
- updateControlStruct();
+ std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
+ getWriterControlStruct().parameterChangedViaIceCalls = param;
+ writeControlStruct();
}
- TrajectoryControlDataStruct getTrajectoryControlDataStruct(Ice::Current()) const;
}
+ }
+ * \endcode
+ */
+ template <typename ControlDataStruct>
+ class LVL1ControllerTemplate: virtual public LVL1Controller
+ {
+ public:
+ LVL1ControllerTemplate(const ControlDataStruct& initialCommands = ControlDataStruct()):
+ controlData {initialCommands},
+ controlDataTrippleBuffer {initialCommands}
+ {
+ }
+ protected:
+ const ControlDataStruct& rtGetControlStruct() const
+ {
+ return controlDataTrippleBuffer.getReadBuffer();
+ }
+ bool rtUpdateControlStruct()
+ {
+ return controlDataTrippleBuffer.getUpToDateData();
+ }
+ virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
+ {
+ rtUpdateControlStruct();
+ rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+ }
-}
+ 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);
+ controlDataTrippleBuffer.getWriteBuffer() = controlData;
+ }
+ ControlDataStruct& getWriterControlStruct()
+ {
+ return controlData;
+ }
+ mutable std::recursive_mutex controlDataMutex;
+ private:
+ TripleBuffer<ControlDataStruct> controlDataTrippleBuffer;
+ ControlDataStruct controlData;
+ };
+ template <typename ControlDataStruct>
+ using LVL1ControllerTemplatePtr = IceInternal::Handle<LVL1ControllerTemplate<ControlDataStruct>>;
+}
#endif
diff --git a/source/RobotAPI/components/units/RobotUnit.cpp b/source/RobotAPI/libraries/Controllers/RobotUnit.cpp
similarity index 68%
rename from source/RobotAPI/components/units/RobotUnit.cpp
rename to source/RobotAPI/libraries/Controllers/RobotUnit.cpp
index 8b64e7ef2..20f7b7b2d 100644
--- a/source/RobotAPI/components/units/RobotUnit.cpp
+++ b/source/RobotAPI/libraries/Controllers/RobotUnit.cpp
@@ -23,35 +23,35 @@
#include "RobotUnit.h"
-using namespace armarx;
+//using namespace armarx;
-void RobotUnit::onInitComponent()
-{
+//void RobotUnit::onInitComponent()
+//{
-}
+//}
-void RobotUnit::onConnectComponent()
-{
+//void RobotUnit::onConnectComponent()
+//{
-}
+//}
-void RobotUnit::onDisconnectComponent()
-{
+//void RobotUnit::onDisconnectComponent()
+//{
-}
+//}
-void RobotUnit::onExitComponent()
-{
+//void RobotUnit::onExitComponent()
+//{
-}
+//}
-armarx::PropertyDefinitionsPtr RobotUnit::createPropertyDefinitions()
-{
- return armarx::PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(
- getConfigIdentifier()));
-}
+//armarx::PropertyDefinitionsPtr RobotUnit::createPropertyDefinitions()
+//{
+// return armarx::PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(
+// getConfigIdentifier()));
+//}
diff --git a/source/RobotAPI/libraries/Controllers/RobotUnit.h b/source/RobotAPI/libraries/Controllers/RobotUnit.h
new file mode 100644
index 000000000..c4659b752
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/RobotUnit.h
@@ -0,0 +1,324 @@
+/*
+ * 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::RobotUnit
+ * @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_UNIT_RobotAPI_RobotUnit_H
+#define _ARMARX_UNIT_RobotAPI_RobotUnit_H
+
+#include <thread>
+#include <atomic>
+#include <chrono>
+#include <mutex>
+#include <unordered_map>
+
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/util/algorithm.h>
+#include <ArmarXCore/core/util/OnScopeExit.h>
+
+#include <RobotAPI/interface/units/RobotUnitInterface.h>
+#include <RobotAPI/libraries/Controllers/LVL0Controller.h>
+
+namespace armarx
+{
+ /**
+ * @class RobotUnitPropertyDefinitions
+ * @brief
+ */
+ class RobotUnitPropertyDefinitions:
+ public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ RobotUnitPropertyDefinitions(std::string prefix):
+ armarx::ComponentPropertyDefinitions(prefix)
+ {
+ //defineRequiredProperty<std::string>("PropertyName", "Description");
+ //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
+ }
+ };
+
+ /**
+ * @defgroup Component-RobotUnit RobotUnit
+ * @ingroup RobotAPI-Components
+ * A description of the component RobotUnit.
+ *
+ * @class RobotUnit
+ * @ingroup Component-RobotUnit
+ * @brief Brief description of class RobotUnit.
+ *
+ * Detailed description of class RobotUnit.
+ */
+ class RobotUnit :
+ virtual public armarx::Component,
+ virtual public armarx::RobotUnitInterface
+ {
+ public:
+ // /**
+ // * @see armarx::ManagedIceObject::getDefaultName()
+ // */
+ // virtual std::string getDefaultName() const
+ // {
+ // return "RobotUnit";
+ // }
+
+ // // RobotUnitInterface interface
+ // //robot
+ // virtual NameControlModeMap getJointControlModeMap(const Ice::Current& = GlobalIceCurrent) const override;
+
+ // //controllers
+ // virtual StringLVL1ControllerPrxDictionary getAllControllers(const Ice::Current& = GlobalIceCurrent) const override
+ // {
+ // std::lock_guard<std::mutex> guard {slowDataMutex};
+ // StringLVL1ControllerPrxDictionary result;
+ // std::transform(
+ // loadedLVL1Controllers.begin(), loadedLVL1Controllers.end(), std::back_inserter(result),
+ // [](const LVL1ControllerPtr & lvl1)
+ // {
+ // return std::make_pair(lvl1->getName(), lvl1->getProxy());
+ // }
+ // );
+ // return result;
+ // }
+ // virtual StringLVL1ControllerPrxDictionary getActiveControllers(const Ice::Current& = GlobalIceCurrent) const override
+ // {
+ // std::lock_guard<std::mutex> guard {slowDataMutex};
+ // StringLVL1ControllerPrxDictionary result;
+ // std::transform(
+ // activeLVL1Controllers.begin(), activeLVL1Controllers.end(), std::back_inserter(result),
+ // [](const LVL1ControllerPtr & lvl1)
+ // {
+ // return std::make_pair(lvl1->getName(), lvl1->getProxy());
+ // }
+ // );
+ // return result;
+ // }
+
+ // virtual bool switchSetup(const Ice::StringSeq&, const Ice::Current& = GlobalIceCurrent) override;
+ // virtual LVL1ControllerInterfacePrx loadController(
+ // const std::string& className, const std::string& instanceName, const Ice::StringSeq& joints, const LVL1ControllerConfigPtr&,
+ // const Ice::Current& = GlobalIceCurrent) override;
+
+
+ // virtual bool loadLibFromPath(const std::string& path, const Ice::Current& = GlobalIceCurrent) const override
+ // {
+ // throw "NYI"; ///TODO mirko
+ // }
+ // virtual bool loadLibFromPackage(const std::string& package, const std::string& name, const Ice::Current& = GlobalIceCurrent) const override
+ // {
+ // throw "NYI"; ///TODO mirko
+ // }
+ // //units
+ // virtual KinematicUnitInterfacePrx getKinematicUnit(const Ice::Current& = GlobalIceCurrent) const override
+ // {
+ // return kinematicUnitPrx;
+ // }
+ // virtual ForceTorqueUnitInterfacePrx getForceTorqueUnit(const Ice::Current& = GlobalIceCurrent) const override
+ // {
+ // return forceTorqueUnitPrx;
+ // }
+ // virtual InertialMeasurementUnitInterfacePrx getInertialMeasurementUnit(const Ice::Current& = GlobalIceCurrent) const override
+ // {
+ // return inertialMeasurementUnitPrx;
+ // }
+ // virtual PlatformUnitInterfacePrx getPlatformUnitInterface(const Ice::Current& = GlobalIceCurrent) const override
+ // {
+ // return platformUnitPrx;
+ // }
+
+ // //functions subclasses need to implement
+ // virtual ControlModeBoolMap supportedControlModes(const Ice::Current& = GlobalIceCurrent) = 0;
+ // virtual void initializeLVL0Controllers() = 0;
+ // virtual void initializeBus() = 0;
+ // virtual void copyControlBufferOut() = 0;
+ // /**
+ // * @brief send and receive data on the bus
+ // */
+ // virtual void sendReceiveData() = 0;
+ // virtual void shutdownBus() = 0;
+ // virtual void emergencyStop() = 0;
+ // // virtual void stopRobot() = 0; // may be implemented as {emergencyStop();}
+
+ // virtual void controlLoopIteration()
+ // {
+ // ///how bridge loop while controllers are switched? (need some halt mode)
+ // for (auto& lvl1 : activeLVL1Controllers)
+ // {
+ // lvl1->run();
+ // }
+ // for (auto& lvl0 : activeLVL0Controllers)
+ // {
+ // lvl0->run();
+ // }
+ // copyControlBufferOut();
+ // sendReceiveData();
+ // }
+
+ // protected:
+ // /**
+ // * @see armarx::ManagedIceObject::onInitComponent()
+ // */
+ // virtual void onInitComponent() override
+ // {
+
+ // jointIndices = toIndexMap(jointNames);
+ // rtControlThread = std::thread {[this]{rtControlThread();}};
+ // }
+
+ // /**
+ // * @see armarx::ManagedIceObject::onConnectComponent()
+ // */
+ // virtual void onConnectComponent() override;
+
+ // /**
+ // * @see armarx::ManagedIceObject::onDisconnectComponent()
+ // */
+ // virtual void onDisconnectComponent() override;
+
+ // /**
+ // * @see armarx::ManagedIceObject::onExitComponent()
+ // */
+ // virtual void onExitComponent() override;
+
+ // /**
+ // * @see PropertyUser::createPropertyDefinitions()
+ // */
+ // virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
+ // //slow data (data changed in non rt threads)
+ // mutable std::mutex slowDataMutex;
+ // std::vector<LVL1ControllerPtr> loadedLVL1Controllers;
+
+
+ // //slow data: running data (this data changes only in switchSetup)
+ // NameControlModeMap jointControlModes;
+ // std::vector<LVL1ControllerPtr> activeLVL1Controllers;
+ // std::vector<LVL0ControllerBase*> activeLVL0Controllers;
+
+ // // //slow data: units
+ // // KinematicUnitInterfacePtr kinematicUnit;
+ // // KinematicUnitInterfacePrx kinematicUnitPrx;
+ // // ForceTorqueUnitInterfacePtr forceTorqueUnit;
+ // // ForceTorqueUnitInterfacePrx forceTorqueUnitPrx;
+ // // InertialMeasurementUnitInterfacePtr inertialMeasurementUnit;
+ // // InertialMeasurementUnitInterfacePrx inertialMeasurementUnitPrx;
+ // // PlatformUnitInterfacePtr platformUnit;
+ // // PlatformUnitInterfacePrx platformUnitPrx;
+
+ // //constant data (this data does not change after onInitComponent)
+ // std::vector<std::string> jointNames;
+ // std::unordered_map<std::string, std::size_t> jointIndices;
+ // std::chrono::nanoseconds controlLoopPeriod;
+ // std::chrono::nanoseconds controlLoopEmergencyStopPeriod;
+ // std::map<std::string, std::vector<LVL0ControllerBase*>> lvl0ControllerMap;
+
+ // private:
+ // enum class ControlThreadState
+ // {
+ // Unborn,
+ // Uninitialized,
+ // ControllerLoop,
+ // /**
+ // * @brief The LVL1 controllers are switched.
+ // * The controller loop now bridges this period.
+ // */
+ // BridgingControllerSwitch,
+ // Exception,
+ // Dead
+ // };
+
+ // //fast data (used in rt threads)
+ // std::thread rtControlThread;
+ // std::atomic_bool stopControlLoop {false};
+ // std::atomic<ControlThreadState> controlThreadState {ControlThreadState::Unborn};
+ // std::atomic_bool requestBridging {true};
+ // void rtControlThreadFunction()
+ // {
+ // controlThreadState = ControlThreadState::Uninitialized;
+ // initializeBus();
+ // ARMARX_ON_SCOPE_EXIT
+ // {
+ // shutdownBus();
+ // controlThreadState = ControlThreadState::Dead;
+ // };
+ // try
+ // {
+ // initializeLVL0Controllers();
+ // //validate
+ // for (const auto& mode : supportedControlModes)
+ // {
+ // switch (mode.first)
+ // {
+ //#define ERROR_HEAD "Wrong number of LVL0Controllers for"
+ //#define ERROR_TAIL ". (If supported, each joint needs one controller. If unsupported no controllers must be initialized!"
+ // case ePositionControl:
+ // if (lvl0PositionControlController.size() != mode.second * jointNames.size())
+ // {
+ // throw std::invalid_argument {ERROR_HEAD "ePositionControl" ERROR_TAIL};
+ // }
+ // break;
+ // case eVelocityControl:
+ // if (lvl0VelocityControlController.size() != mode.second * jointNames.size())
+ // {
+ // throw std::invalid_argument {ERROR_HEAD "eVelocityControl" ERROR_TAIL};
+ // }
+ // break;
+ // case eTorqueControl:
+ // if (lvl0TorqueControlController.size() != mode.second * jointNames.size())
+ // {
+ // throw std::invalid_argument {ERROR_HEAD "eTorqueControl" ERROR_TAIL};
+ // }
+ // break;
+ // case ePositionVelocityControl:
+ // if (lvl0PositionVelocityControlController.size() != mode.second * jointNames.size())
+ // {
+ // throw std::invalid_argument {ERROR_HEAD "ePositionVelocityControl" ERROR_TAIL};
+ // }
+ // break;
+ //#undef ERROR_HEAD
+ //#undef ERROR_TAIL
+ // default:
+ // if (mode.second)
+ // {
+ // throw std::invalid_argument {"Unknown ControlMode supported! mode = " + std::to_string(mode.first)};
+ // }
+ // }
+ // }
+ // controlThreadState = ControlThreadState::ControllerLoop;///TODO bridging
+ // //loop
+ // while (!stopControlLoop)
+ // {
+ // auto iterationStartTime = std::chrono::high_resolution_clock::now();
+ // controlLoopIteration();
+ // std::this_thread::sleep_until(iterationStartTime + controlLoopPeriod);
+ // }
+
+ // }
+ // catch (...)
+ // {
+ // controlThreadState = ControlThreadState::Exception;
+ // emergencyStop();
+ // //TODO HANDLING
+ // }
+ // }
+
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/Controllers/SyntaxCheck.cpp b/source/RobotAPI/libraries/Controllers/SyntaxCheck.cpp
index 5801ed55c..8e6bcbcfe 100644
--- a/source/RobotAPI/libraries/Controllers/SyntaxCheck.cpp
+++ b/source/RobotAPI/libraries/Controllers/SyntaxCheck.cpp
@@ -31,7 +31,6 @@
#include "DataUnits/PlatformDataUnit.h"
#include "Targets/JointPositionTarget.h"
-#include "Targets/JointPositionVelocityTarget.h"
#include "Targets/JointTargetBase.h"
#include "Targets/JointTorqueTarget.h"
#include "Targets/JointVelocityTarget.h"
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointPositionTarget.h b/source/RobotAPI/libraries/Controllers/Targets/JointPositionTarget.h
index 3e00262e3..e599de3fc 100644
--- a/source/RobotAPI/libraries/Controllers/Targets/JointPositionTarget.h
+++ b/source/RobotAPI/libraries/Controllers/Targets/JointPositionTarget.h
@@ -42,9 +42,9 @@ namespace armarx
{
public:
float position = ControllerConstants::ValueNotSetNaN;
- virtual ControlMode getType() const override
+ virtual std::string getControlMode() const override
{
- return ePositionControl;
+ return ControlModes::PositionMode;
}
virtual void reset() override
{
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointPositionVelocityTarget.h b/source/RobotAPI/libraries/Controllers/Targets/JointPositionVelocityTarget.h
deleted file mode 100644
index 91774ac5b..000000000
--- a/source/RobotAPI/libraries/Controllers/Targets/JointPositionVelocityTarget.h
+++ /dev/null
@@ -1,63 +0,0 @@
-/*
- * 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::JointPositionVelocityTarget
- * @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_JointPositionVelocityTarget_H
-#define _ARMARX_LIB_RobotAPI_JointPositionVelocityTarget_H
-
-#include "JointTargetBase.h"
-
-namespace armarx
-{
- /**
- * @defgroup Library-Controllers Controllers
- * @ingroup RobotAPI
- * A description of the library Controllers.
- *
- * @class JointPositionVelocityTarget
- * @ingroup Library-Controllers
- * @brief Brief description of class JointPositionVelocityTarget.
- *
- * Detailed description of class JointPositionVelocityTarget.
- */
- class JointPositionVelocityTarget: public JointTargetBase
- {
- public:
- float position = ControllerConstants::ValueNotSetNaN;
- float velocity = ControllerConstants::ValueNotSetNaN;
- virtual ControlMode getType() const override
- {
- return ePositionVelocityControl;
- }
- virtual void reset() override
- {
- position = ControllerConstants::ValueNotSetNaN;
- velocity = ControllerConstants::ValueNotSetNaN;
- }
- virtual bool isValid() const override
- {
- return std::isfinite(position) && std::isfinite(velocity);
- }
- };
-
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointTargetBase.h b/source/RobotAPI/libraries/Controllers/Targets/JointTargetBase.h
index 8fefd7140..eb1d49e79 100644
--- a/source/RobotAPI/libraries/Controllers/Targets/JointTargetBase.h
+++ b/source/RobotAPI/libraries/Controllers/Targets/JointTargetBase.h
@@ -25,6 +25,7 @@
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include "../Constants.h"
+#include "../ControlModes.h"
namespace armarx
{
@@ -42,11 +43,10 @@ namespace armarx
class JointTargetBase
{
public:
- virtual ControlMode getType() const = 0;
+ virtual std::string getControlMode() const = 0;
virtual void reset() = 0;
virtual bool isValid() const = 0;
};
-
}
#endif
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointTorqueTarget.h b/source/RobotAPI/libraries/Controllers/Targets/JointTorqueTarget.h
index 7dfac5460..ab7548ba1 100644
--- a/source/RobotAPI/libraries/Controllers/Targets/JointTorqueTarget.h
+++ b/source/RobotAPI/libraries/Controllers/Targets/JointTorqueTarget.h
@@ -42,9 +42,9 @@ namespace armarx
{
public:
float torque = ControllerConstants::ValueNotSetNaN;
- virtual ControlMode getType() const override
+ virtual std::string getControlMode() const override
{
- return eTorqueControl;
+ return ControlModes::TorqueMode;
}
virtual void reset() override
{
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointVelocityTarget.h b/source/RobotAPI/libraries/Controllers/Targets/JointVelocityTarget.h
index 95cebea0e..7c5d3100f 100644
--- a/source/RobotAPI/libraries/Controllers/Targets/JointVelocityTarget.h
+++ b/source/RobotAPI/libraries/Controllers/Targets/JointVelocityTarget.h
@@ -42,9 +42,9 @@ namespace armarx
{
public:
float velocity = ControllerConstants::ValueNotSetNaN;
- virtual ControlMode getType() const override
+ virtual std::string getControlMode() const override
{
- return eVelocityControl;
+ return ControlModes::VelocityMode;
}
virtual void reset() override
{
--
GitLab