From 6685a0aac29d9db40b5865305533fb7d38a4fcc9 Mon Sep 17 00:00:00 2001
From: Raphael <ufdrv@student.kit.edu>
Date: Wed, 25 Jan 2017 15:06:18 +0100
Subject: [PATCH] temp state
---
source/RobotAPI/components/CMakeLists.txt | 1 +
.../RobotAPI/components/units/CMakeLists.txt | 3 +
.../RobotAPI/components/units/RobotUnit.cpp | 57 +++
source/RobotAPI/components/units/RobotUnit.h | 341 ++++++++++++++++++
source/RobotAPI/interface/CMakeLists.txt | 1 +
.../libraries/Controllers/LVL1Controller.ice | 15 +-
.../units/KinematicUnitInterface.ice | 8 +-
.../interface/units/RobotUnitInterface.ice | 60 +++
.../libraries/Controllers/Constants.h | 2 +-
.../Controllers/DataUnits/KinematicDataUnit.h | 46 +--
.../Controllers/DataUnits/PlatformDataUnit.h | 26 +-
.../libraries/Controllers/LVL0Controller.h | 96 ++---
.../libraries/Controllers/LVL1Controller.h | 23 +-
.../libraries/Controllers/SyntaxCheck.cpp | 4 +-
.../Controllers/Targets/JointPositionTarget.h | 4 +-
.../Targets/JointPositionVelocityTarget.h | 8 +-
.../Controllers/Targets/JointTargetBase.h | 2 +-
.../Controllers/Targets/JointTorqueTarget.h | 4 +-
.../Controllers/Targets/JointVelocityTarget.h | 4 +-
19 files changed, 594 insertions(+), 111 deletions(-)
create mode 100644 source/RobotAPI/components/units/RobotUnit.cpp
create mode 100644 source/RobotAPI/components/units/RobotUnit.h
create mode 100644 source/RobotAPI/interface/units/RobotUnitInterface.ice
diff --git a/source/RobotAPI/components/CMakeLists.txt b/source/RobotAPI/components/CMakeLists.txt
index bf391c4c6..09e360143 100644
--- a/source/RobotAPI/components/CMakeLists.txt
+++ b/source/RobotAPI/components/CMakeLists.txt
@@ -3,3 +3,4 @@ add_subdirectory(DebugDrawer)
add_subdirectory(RobotState)
add_subdirectory(EarlyVisionGraph)
add_subdirectory(ViewSelection)
+
diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 90567cf96..33f5e52b8 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -21,6 +21,7 @@ set(LIBS
RobotAPICore
ArmarXCoreObservers
ArmarXCoreEigen3Variants
+ Controllers
${Simox_LIBRARIES})
set(LIB_HEADERS
@@ -41,6 +42,7 @@ set(LIB_HEADERS
KinematicUnitSimulation.h
PlatformUnit.h
RobotPoseUnit.h
+ RobotUnit.h
PlatformUnitSimulation.h
KinematicUnitObserver.h
PlatformUnitObserver.h
@@ -67,6 +69,7 @@ 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.cpp b/source/RobotAPI/components/units/RobotUnit.cpp
new file mode 100644
index 000000000..8b64e7ef2
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit.cpp
@@ -0,0 +1,57 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::RobotUnit
+ * @author Raphael ( ufdrv at student dot kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "RobotUnit.h"
+
+
+using namespace armarx;
+
+
+void RobotUnit::onInitComponent()
+{
+
+}
+
+
+void RobotUnit::onConnectComponent()
+{
+
+}
+
+
+void RobotUnit::onDisconnectComponent()
+{
+
+}
+
+
+void RobotUnit::onExitComponent()
+{
+
+}
+
+armarx::PropertyDefinitionsPtr RobotUnit::createPropertyDefinitions()
+{
+ return armarx::PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(
+ getConfigIdentifier()));
+}
+
diff --git a/source/RobotAPI/components/units/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit.h
new file mode 100644
index 000000000..0f013519c
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit.h
@@ -0,0 +1,341 @@
+/*
+ * 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::vector<LVL0Controller<ePositionControl>*> lvl0PositionControlController;
+ std::vector<LVL0Controller<eVelocityControl>*> lvl0VelocityControlController;
+ std::vector<LVL0Controller<eTorqueControl>*> lvl0TorqueControlController;
+ std::vector<LVL0Controller<ePositionVelocityControl>*> lvl0PositionVelocityControlController;
+
+
+
+
+ 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/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 38347386c..bb2e60ceb 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -30,6 +30,7 @@ set(SLICE_FILES
units/KinematicUnitInterface.ice
units/PlatformUnitInterface.ice
units/RobotPoseUnitInterface.ice
+ units/RobotUnitInterface.ice
units/TCPControlUnit.ice
units/TCPMoverUnitInterface.ice
units/UnitInterface.ice
diff --git a/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice b/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice
index 630d01d84..65846b053 100644
--- a/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice
+++ b/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice
@@ -20,14 +20,25 @@
* GNU General Public License
*/
+#ifndef _ARMARX_ROBOTAPI_LVL1ControllerInterface_SLICE_
+#define _ARMARX_ROBOTAPI_LVL1ControllerInterface_SLICE_
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
+
module armarx
{
class LVL1ControllerConfig
{
};
- class LVL1ControllerInterface
+ interface LVL1ControllerInterface
{
-
+ ["cpp:const"] idempotent string getClassName();
+ ["cpp:const"] idempotent string getInstanceName();
+ ["cpp:const"] idempotent NameControlModeMap getJointControlModeMap();
+ ["cpp:const"] idempotent bool isControllerActive();
};
+
+ dictionary<string, LVL1ControllerInterface*> StringLVL1ControllerPrxDictionary;
};
+#endif
diff --git a/source/RobotAPI/interface/units/KinematicUnitInterface.ice b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
index eb6acb2d5..09d6ecfdd 100644
--- a/source/RobotAPI/interface/units/KinematicUnitInterface.ice
+++ b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
@@ -128,9 +128,13 @@ module armarx
**/
dictionary<string, float> NameValueMap;
/**
- * [NameControlModeMap] defined. This data container is mostly used to assign control modes to e.g. joints which are identified by name.
- **/
+ * [NameControlModeMap] defined. This data container is mostly used to assign control modes to e.g. joints which are identified by name.
+ **/
dictionary<string, ControlMode> NameControlModeMap;
+ /**
+ * [ControlModeBoolMap] defined. This data container is mostly used to signal for each control mode, whether it is supported by some component (e.g. a RobotUnit).
+ **/
+ dictionary<ControlMode, bool> ControlModeBoolMap;
/**
* [NameStatusMap] defined. This data container is mostly used to a status to e.g. a joint which is identified by name.
**/
diff --git a/source/RobotAPI/interface/units/RobotUnitInterface.ice b/source/RobotAPI/interface/units/RobotUnitInterface.ice
new file mode 100644
index 000000000..1dbee2d22
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnitInterface.ice
@@ -0,0 +1,60 @@
+/*
+ * 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 Grimm ( raphael dot grimm at student dot kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_UNITS_RobotUnit_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_RobotUnit_SLICE_
+
+#include <RobotAPI/interface/libraries/Controllers/LVL1Controller.ice>
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
+#include <RobotAPI/interface/units/ForceTorqueUnit.ice>
+#include <RobotAPI/interface/units/InertialMeasurementUnit.ice>
+#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
+
+module armarx
+{
+ interface RobotUnitInterface
+ {
+ //robot
+ ["cpp:const"] idempotent NameControlModeMap getJointControlModeMap();
+
+ //controllers
+ ["cpp:const"] idempotent StringLVL1ControllerPrxDictionary getAllControllers();
+ ["cpp:const"] idempotent StringLVL1ControllerPrxDictionary getActiveControllers();
+
+ bool switchSetup(Ice::StringSeq controllerInstanceNames);
+ LVL1ControllerInterface* loadController(string className, string instanceName, Ice::StringSeq joints, LVL1ControllerConfig config);
+
+ ["cpp:const"] idempotent bool loadLibFromPath(string path);
+ ["cpp:const"] idempotent bool loadLibFromPackage(string name, string package);
+
+ ["cpp:const"] idempotent ControlModeBoolMap supportedControlModes();
+
+ //units
+ ["cpp:const"] idempotent KinematicUnitInterface* getKinematicUnit();
+ ["cpp:const"] idempotent ForceTorqueUnitInterface* getForceTorqueUnit();
+ ["cpp:const"] idempotent InertialMeasurementUnitInterface* getInertialMeasurementUnit();
+ ["cpp:const"] idempotent PlatformUnitInterface* getPlatformUnitInterface();
+ };
+};
+
+#endif
diff --git a/source/RobotAPI/libraries/Controllers/Constants.h b/source/RobotAPI/libraries/Controllers/Constants.h
index 34b9ca483..74e538838 100644
--- a/source/RobotAPI/libraries/Controllers/Constants.h
+++ b/source/RobotAPI/libraries/Controllers/Constants.h
@@ -30,7 +30,7 @@ namespace armarx
{
namespace ControllerConstants
{
- const float ValueNotSetNaN = std::nanf(std::to_string((1 << 16) - 1).c_str());
+ static const float ValueNotSetNaN = std::nanf(std::to_string((1 << 16) - 1).c_str());
}
}
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/KinematicDataUnit.h b/source/RobotAPI/libraries/Controllers/DataUnits/KinematicDataUnit.h
index c41fa6d27..fbb2f91ac 100644
--- a/source/RobotAPI/libraries/Controllers/DataUnits/KinematicDataUnit.h
+++ b/source/RobotAPI/libraries/Controllers/DataUnits/KinematicDataUnit.h
@@ -28,36 +28,20 @@
#include <unordered_map>
#include <algorithm>
-#include "../Constatnts.h"
+#include <ArmarXCore/core/util/algorithm.h>
namespace armarx
{
class KinematicDataUnit
{
- template<class T>
- static std::unordered_map<T, std::size_t> toIndexMap(const std::vector<T>& vec)
- {
- std::unordered_map<T, std::size_t> result;
- result.reserve(vec.size());
- for (std::size_t i = 0; i < vec.size(); ++i)
- {
- auto emplacement = result.emplace(vec.at(i), i).second;
- if (!emplacement)
- {
- throw std::invalid_argument {"The given vector did not contain unique elements!"};
- }
- }
- return result;
- }
-
public:
KinematicDataUnit(const std::vector<std::string>& jointNames):
- jointAngles(jointNames.size(), ControllerConstatns::ValueNotSetNaN),
- jointVelocites(jointNames.size(), ControllerConstatns::ValueNotSetNaN),
- jointTorques(jointNames.size(), ControllerConstatns::ValueNotSetNaN),
- jointCurrents(jointNames.size(), ControllerConstatns::ValueNotSetNaN),
- jointMotorTemperatures(jointNames.size(), ControllerConstatns::ValueNotSetNaN),
- jointGearTempertures(jointNames.size(), ControllerConstatns::ValueNotSetNaN),
+ jointAngles(jointNames.size(), nullptr),
+ jointVelocites(jointNames.size(), nullptr),
+ jointTorques(jointNames.size(), nullptr),
+ jointCurrents(jointNames.size(), nullptr),
+ jointMotorTemperatures(jointNames.size(), nullptr),
+ jointGearTempertures(jointNames.size(), nullptr),
jointNames {jointNames},
jointIndices {toIndexMap(jointNames)}
{}
@@ -99,15 +83,15 @@ namespace armarx
}
protected:
- std::vector<float> jointAngles;
- std::vector<float> jointVelocites;
- std::vector<float> jointTorques;
- std::vector<float> jointCurrents;
- std::vector<float> jointMotorTemperatures;
- std::vector<float> jointGearTempertures;
+ std::vector<float*> jointAngles;
+ std::vector<float*> jointVelocites;
+ std::vector<float*> jointTorques;
+ std::vector<float*> jointCurrents;
+ std::vector<float*> jointMotorTemperatures;
+ std::vector<float*> jointGearTempertures;
private:
template<class T>
- std::vector<const T*> getPointers(const std::vector<std::string>& joints, const std::vector<T>& targets) const
+ std::vector<const T*> getPointers(const std::vector<std::string>& joints, const std::vector<T*>& targets) const
{
std::vector<const T*> result;
result.resize(joints.size());
@@ -115,7 +99,7 @@ namespace armarx
joints.begin(), joints.end(), std::back_inserter(result),
[targets, this](const std::string & joint)
{
- return &targets.at(getJointIndex(joint));
+ return targets.at(getJointIndex(joint));
}
);
return result;
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/PlatformDataUnit.h b/source/RobotAPI/libraries/Controllers/DataUnits/PlatformDataUnit.h
index 3fccfc890..521e30c8d 100644
--- a/source/RobotAPI/libraries/Controllers/DataUnits/PlatformDataUnit.h
+++ b/source/RobotAPI/libraries/Controllers/DataUnits/PlatformDataUnit.h
@@ -23,8 +23,6 @@
#ifndef _ARMARX_LIB_RobotAPI_PlatformDataUnit_H
#define _ARMARX_LIB_RobotAPI_PlatformDataUnit_H
-#include "../Constatnts.h"
-
namespace armarx
{
class PlatformDataUnit
@@ -32,37 +30,37 @@ namespace armarx
public:
const float* getPositionX() const
{
- return &positionX;
+ return positionX;
}
const float* getPositionY() const
{
- return &positionY;
+ return positionY;
}
const float* getRotation() const
{
- return &rotation;
+ return rotation;
}
const float* getVelocityX() const
{
- return &velocityX;
+ return velocityX;
}
const float* getVelocityY() const
{
- return &velocityY;
+ return velocityY;
}
const float* getVelocityRotation() const
{
- return &velocityRotation;
+ return velocityRotation;
}
protected:
- float positionX = ControllerConstatns::ValueNotSetNaN;
- float positionY = ControllerConstatns::ValueNotSetNaN;
- float rotation = ControllerConstatns::ValueNotSetNaN;
+ float* positionX;
+ float* positionY;
+ float* rotation;
- float velocityX = ControllerConstatns::ValueNotSetNaN;
- float velocityY = ControllerConstatns::ValueNotSetNaN;
- float velocityRotation = ControllerConstatns::ValueNotSetNaN;
+ float* velocityX;
+ float* velocityY;
+ float* velocityRotation;
};
}
diff --git a/source/RobotAPI/libraries/Controllers/LVL0Controller.h b/source/RobotAPI/libraries/Controllers/LVL0Controller.h
index fafa5b0ac..c2cdbb1ee 100644
--- a/source/RobotAPI/libraries/Controllers/LVL0Controller.h
+++ b/source/RobotAPI/libraries/Controllers/LVL0Controller.h
@@ -23,59 +23,65 @@
#ifndef _ARMARX_LIB_RobotAPI_LVL0Controller_H
#define _ARMARX_LIB_RobotAPI_LVL0Controller_H
-#include <stdexcept>
+#include "Targets/JointTargetBase.h"
namespace armarx
{
- /**
- * @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
+ class LVL0ControllerBase
{
- public:
- using TargetType = TargetT;
- LVL0Controller()
- {
- resetTarget();
- }
+ virtual void run() = 0;
+ virtual JointTargetBase* getTarget() =0;
+ };
- TargetType* getTarget()
- {
- return & target;
- }
+// /**
+// * @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();
+// }
- void resetTarget()
- {
- target.reset();
- }
+// TargetType* getTarget()
+// {
+// return & target;
+// }
- void run()
- {
- if (!target.valid())
- {
- throw std::logic_error {"LVL0 Controller failed! Target is invalid"};
- }
- writeTarget();
- resetTarget();
- }
+// void resetTarget()
+// {
+// target.reset();
+// }
- /**
- * @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;
+// virtual void run() override
+// {
+// if (!target.valid())
+// {
+// throw std::logic_error {"LVL0 Controller failed! Target is invalid"};
+// }
+// writeTarget();
+// resetTarget();
+// }
- private:
- TargetType target;
- };
+// /**
+// * @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;
+// };
}
#endif
diff --git a/source/RobotAPI/libraries/Controllers/LVL1Controller.h b/source/RobotAPI/libraries/Controllers/LVL1Controller.h
index 8e44980e0..20acdae24 100644
--- a/source/RobotAPI/libraries/Controllers/LVL1Controller.h
+++ b/source/RobotAPI/libraries/Controllers/LVL1Controller.h
@@ -24,13 +24,17 @@
#define _ARMARX_LIB_RobotAPI_LVL1Controller_H
#include <map>
+#include <atomic>
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <ArmarXCore/core/Component.h>
+
+#include <RobotAPI/interface/libraries/Controllers/LVL1Controller.h>
#include "Targets/JointTargetBase.h"
namespace armarx
{
+ class RobotUnit;
/**
* @defgroup Library-Controllers Controllers
* @ingroup RobotAPI
@@ -42,14 +46,25 @@ namespace armarx
*
* Detailed description of class LVL1Controller.
*/
- class LVL1Controller
+ class LVL1Controller:
+ virtual public LVL1ControllerInterface,
+ virtual public Component
{
public:
LVL1Controller(/*string instanceName, RobotUnitPtr robotUnit, ControllerConfig config*/);
virtual void run() = 0;
- virtual std::map<std::string, ControlMode> getJointTargetDefinitions() = 0;
- virtual void setJointTargetPtrs(std::vector<JointTargetBase*> ptrs) = 0;
+ virtual std::map<std::string, ControlMode> getJointControlmodeMap(const Ice::Current& = GlobalIceCurrent) = 0;
+ //virtual void setJointTargetPtrs(std::vector<JointTargetBase*> ptrs) = 0;
+
+ virtual bool isControllerActive(const Ice::Current& = GlobalIceCurrent) const final
+ {
+ return isActive;
+ }
+ private:
+ friend class RobotUnit;
+ std::atomic_bool isActive {false};
};
+ using LVL1ControllerPtr = IceInternal::Handle<LVL1Controller>;
}
diff --git a/source/RobotAPI/libraries/Controllers/SyntaxCheck.cpp b/source/RobotAPI/libraries/Controllers/SyntaxCheck.cpp
index b6406e2dd..5801ed55c 100644
--- a/source/RobotAPI/libraries/Controllers/SyntaxCheck.cpp
+++ b/source/RobotAPI/libraries/Controllers/SyntaxCheck.cpp
@@ -20,7 +20,7 @@
* GNU General Public License
*/
-#include "Constatnts.h"
+#include "Constants.h"
#include "LVL0Controller.h"
#include "LVL1Controller.h"
@@ -35,3 +35,5 @@
#include "Targets/JointTargetBase.h"
#include "Targets/JointTorqueTarget.h"
#include "Targets/JointVelocityTarget.h"
+
+//this file is only for syntax checks
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointPositionTarget.h b/source/RobotAPI/libraries/Controllers/Targets/JointPositionTarget.h
index 39c788d5d..3e00262e3 100644
--- a/source/RobotAPI/libraries/Controllers/Targets/JointPositionTarget.h
+++ b/source/RobotAPI/libraries/Controllers/Targets/JointPositionTarget.h
@@ -41,14 +41,14 @@ namespace armarx
class JointPositionTarget: public JointTargetBase
{
public:
- float position = ControllerConstatns::ValueNotSetNaN;
+ float position = ControllerConstants::ValueNotSetNaN;
virtual ControlMode getType() const override
{
return ePositionControl;
}
virtual void reset() override
{
- position = ControllerConstatns::ValueNotSetNaN;
+ position = ControllerConstants::ValueNotSetNaN;
}
virtual bool isValid() const override
{
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointPositionVelocityTarget.h b/source/RobotAPI/libraries/Controllers/Targets/JointPositionVelocityTarget.h
index 5cd022809..91774ac5b 100644
--- a/source/RobotAPI/libraries/Controllers/Targets/JointPositionVelocityTarget.h
+++ b/source/RobotAPI/libraries/Controllers/Targets/JointPositionVelocityTarget.h
@@ -41,16 +41,16 @@ namespace armarx
class JointPositionVelocityTarget: public JointTargetBase
{
public:
- float position = ControllerConstatns::ValueNotSetNaN;
- float velocity = ControllerConstatns::ValueNotSetNaN;
+ float position = ControllerConstants::ValueNotSetNaN;
+ float velocity = ControllerConstants::ValueNotSetNaN;
virtual ControlMode getType() const override
{
return ePositionVelocityControl;
}
virtual void reset() override
{
- position = ControllerConstatns::ValueNotSetNaN;
- velocity = ControllerConstatns::ValueNotSetNaN;
+ position = ControllerConstants::ValueNotSetNaN;
+ velocity = ControllerConstants::ValueNotSetNaN;
}
virtual bool isValid() const override
{
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointTargetBase.h b/source/RobotAPI/libraries/Controllers/Targets/JointTargetBase.h
index d7077bde9..8fefd7140 100644
--- a/source/RobotAPI/libraries/Controllers/Targets/JointTargetBase.h
+++ b/source/RobotAPI/libraries/Controllers/Targets/JointTargetBase.h
@@ -24,7 +24,7 @@
#define _ARMARX_LIB_RobotAPI_JointTargetBase_H
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include "../Constatnts.h"
+#include "../Constants.h"
namespace armarx
{
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointTorqueTarget.h b/source/RobotAPI/libraries/Controllers/Targets/JointTorqueTarget.h
index 84860c756..7dfac5460 100644
--- a/source/RobotAPI/libraries/Controllers/Targets/JointTorqueTarget.h
+++ b/source/RobotAPI/libraries/Controllers/Targets/JointTorqueTarget.h
@@ -41,14 +41,14 @@ namespace armarx
class JointTorqueTarget: public JointTargetBase
{
public:
- float torque = ControllerConstatns::ValueNotSetNaN;
+ float torque = ControllerConstants::ValueNotSetNaN;
virtual ControlMode getType() const override
{
return eTorqueControl;
}
virtual void reset() override
{
- torque = ControllerConstatns::ValueNotSetNaN;
+ torque = ControllerConstants::ValueNotSetNaN;
}
virtual bool isValid() const override
{
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointVelocityTarget.h b/source/RobotAPI/libraries/Controllers/Targets/JointVelocityTarget.h
index 88265541b..95cebea0e 100644
--- a/source/RobotAPI/libraries/Controllers/Targets/JointVelocityTarget.h
+++ b/source/RobotAPI/libraries/Controllers/Targets/JointVelocityTarget.h
@@ -41,14 +41,14 @@ namespace armarx
class JointVelocityTarget: public JointTargetBase
{
public:
- float velocity = ControllerConstatns::ValueNotSetNaN;
+ float velocity = ControllerConstants::ValueNotSetNaN;
virtual ControlMode getType() const override
{
return eVelocityControl;
}
virtual void reset() override
{
- velocity = ControllerConstatns::ValueNotSetNaN;
+ velocity = ControllerConstants::ValueNotSetNaN;
}
virtual bool isValid() const override
{
--
GitLab