From 89d83047e8e8e8cacad12ab803bfc1aa1869dd8b Mon Sep 17 00:00:00 2001
From: Raphael <ufdrv@student.kit.edu>
Date: Thu, 9 Feb 2017 18:48:41 +0100
Subject: [PATCH] added lvl1 passthroughcontroller
added more metainfo to lvl1 (for the robot unit)
added helperfunctions to the robot unit + updated the recommended implementation
all functions called in rt now have the rt prefix
---
source/RobotAPI/interface/CMakeLists.txt | 5 +-
.../LVL1Controller.ice | 0
.../RTControllers/PassThroughController.ice | 42 ++++
.../RobotUnitInterface.ice | 2 +-
.../BasicRTControllers/CMakeLists.txt | 20 ++
.../PassThroughController.cpp | 25 ++
.../PassThroughController.h | 220 ++++++++++++++++
source/RobotAPI/libraries/CMakeLists.txt | 4 +-
.../CMakeLists.txt | 17 +-
.../Constants.h | 0
.../ControlModes.h | 8 +-
.../DataUnits/ForceTorqueDataUnit.h | 0
.../DataUnits/HapticDataUnit.h | 0
.../DataUnits/IMUDataUnit.h | 0
.../DataUnits/KinematicDataUnit.h | 0
.../DataUnits/PlatformDataUnit.h | 0
.../LVL0Controller.cpp | 0
.../LVL0Controller.h | 0
.../LVL1Controller.cpp | 19 ++
.../LVL1Controller.h | 43 +++-
.../RobotUnit.cpp | 234 ++++++++++++++++--
.../RobotUnit.h | 172 ++++++++-----
.../SyntaxCheck.cpp | 0
.../Targets/JointPositionTarget.h | 6 +-
.../Targets/JointTargetBase.h | 6 +-
.../Targets/JointTorqueTarget.h | 6 +-
.../Targets/JointVelocityTarget.h | 6 +-
27 files changed, 711 insertions(+), 124 deletions(-)
rename source/RobotAPI/interface/libraries/{Controllers => RTControllers}/LVL1Controller.ice (100%)
create mode 100644 source/RobotAPI/interface/libraries/RTControllers/PassThroughController.ice
rename source/RobotAPI/interface/libraries/{Controllers => RTControllers}/RobotUnitInterface.ice (97%)
create mode 100644 source/RobotAPI/libraries/BasicRTControllers/CMakeLists.txt
create mode 100644 source/RobotAPI/libraries/BasicRTControllers/PassThroughController.cpp
create mode 100644 source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/CMakeLists.txt (63%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/Constants.h (100%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/ControlModes.h (78%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/DataUnits/ForceTorqueDataUnit.h (100%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/DataUnits/HapticDataUnit.h (100%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/DataUnits/IMUDataUnit.h (100%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/DataUnits/KinematicDataUnit.h (100%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/DataUnits/PlatformDataUnit.h (100%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/LVL0Controller.cpp (100%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/LVL0Controller.h (100%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/LVL1Controller.cpp (75%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/LVL1Controller.h (83%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/RobotUnit.cpp (65%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/RobotUnit.h (66%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/SyntaxCheck.cpp (100%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/Targets/JointPositionTarget.h (91%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/Targets/JointTargetBase.h (90%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/Targets/JointTorqueTarget.h (91%)
rename source/RobotAPI/libraries/{Controllers => RobotRTControllers}/Targets/JointVelocityTarget.h (91%)
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 448c67665..1ccbfdb6b 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -39,8 +39,9 @@ set(SLICE_FILES
visualization/DebugDrawerInterface.ice
- libraries/Controllers/LVL1Controller.ice
- libraries/Controllers/RobotUnitInterface.ice
+ libraries/RTControllers/LVL1Controller.ice
+ libraries/RTControllers/PassThroughController.ice
+ libraries/RTControllers/RobotUnitInterface.ice
)
#core/RobotIK.ice
diff --git a/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice b/source/RobotAPI/interface/libraries/RTControllers/LVL1Controller.ice
similarity index 100%
rename from source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice
rename to source/RobotAPI/interface/libraries/RTControllers/LVL1Controller.ice
diff --git a/source/RobotAPI/interface/libraries/RTControllers/PassThroughController.ice b/source/RobotAPI/interface/libraries/RTControllers/PassThroughController.ice
new file mode 100644
index 000000000..7566f8c32
--- /dev/null
+++ b/source/RobotAPI/interface/libraries/RTControllers/PassThroughController.ice
@@ -0,0 +1,42 @@
+/*
+ * 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::PassThroughController
+ * @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_PassThroughController_SLICE_
+#define _ARMARX_ROBOTAPI_PassThroughController_SLICE_
+
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <RobotAPI/interface/libraries/RTControllers/LVL1Controller.ice>
+
+module armarx
+{
+ class PassThroughControllerConfig extends LVL1ControllerConfig
+ {
+ Ice::StringSeq jointNames;
+ };
+
+ interface PassThroughControllerInterface extends LVL1ControllerInterface
+ {
+ idempotent void setJoint(string joint, float value) throws InvalidArgumentException;
+ idempotent void setJoints(StringFloatDictionary values) throws InvalidArgumentException;
+ };
+};
+#endif
diff --git a/source/RobotAPI/interface/libraries/Controllers/RobotUnitInterface.ice b/source/RobotAPI/interface/libraries/RTControllers/RobotUnitInterface.ice
similarity index 97%
rename from source/RobotAPI/interface/libraries/Controllers/RobotUnitInterface.ice
rename to source/RobotAPI/interface/libraries/RTControllers/RobotUnitInterface.ice
index b78a9e2f4..aeeae71cc 100644
--- a/source/RobotAPI/interface/libraries/Controllers/RobotUnitInterface.ice
+++ b/source/RobotAPI/interface/libraries/RTControllers/RobotUnitInterface.ice
@@ -25,7 +25,7 @@
#include <ArmarXCore/interface/core/UserException.ice>
-#include <RobotAPI/interface/libraries/Controllers/LVL1Controller.ice>
+#include <RobotAPI/interface/libraries/RTControllers/LVL1Controller.ice>
#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
#include <RobotAPI/interface/units/ForceTorqueUnit.ice>
diff --git a/source/RobotAPI/libraries/BasicRTControllers/CMakeLists.txt b/source/RobotAPI/libraries/BasicRTControllers/CMakeLists.txt
new file mode 100644
index 000000000..5ed672502
--- /dev/null
+++ b/source/RobotAPI/libraries/BasicRTControllers/CMakeLists.txt
@@ -0,0 +1,20 @@
+armarx_component_set_name("BasicRTControllers")
+armarx_set_target("Library: BasicRTControllers")
+
+set(LIB_NAME BasicRTControllers)
+
+set(LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ RobotAPIInterfaces
+ RobotRTControllers
+)
+
+set(LIB_FILES
+ PassThroughController.cpp
+)
+set(LIB_HEADERS
+ PassThroughController.h
+)
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.cpp b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.cpp
new file mode 100644
index 000000000..268586743
--- /dev/null
+++ b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.cpp
@@ -0,0 +1,25 @@
+/*
+ * 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::PassThroughController
+ * @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 "PassThroughController.h"
+
+using namespace armarx;
diff --git a/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
new file mode 100644
index 000000000..05c70890c
--- /dev/null
+++ b/source/RobotAPI/libraries/BasicRTControllers/PassThroughController.h
@@ -0,0 +1,220 @@
+/*
+ * 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::PassThroughController
+ * @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_PassThroughController_H
+#define _ARMARX_LIB_RobotAPI_PassThroughController_H
+
+#include <atomic>
+#include <ArmarXCore/core/util/algorithm.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/libraries/RobotRTControllers/LVL1Controller.h>
+#include <RobotAPI/libraries/RobotRTControllers/RobotUnit.h>
+#include <RobotAPI/interface/libraries/RTControllers/PassThroughController.h>
+
+#include <RobotAPI/libraries/RobotRTControllers/Targets/JointPositionTarget.h>
+#include <RobotAPI/libraries/RobotRTControllers/Targets/JointTorqueTarget.h>
+#include <RobotAPI/libraries/RobotRTControllers/Targets/JointVelocityTarget.h>
+#include <RobotAPI/libraries/RobotRTControllers/DataUnits/KinematicDataUnit.h>
+
+namespace armarx
+{
+ template <typename T>
+ struct AtomicWrapper
+ {
+ std::atomic<T> val;
+
+ AtomicWrapper( ):val{ }{}
+ AtomicWrapper(const T& v ):val{v }{}
+ AtomicWrapper(const std::atomic<T> &val ):val{val.load() }{}
+ AtomicWrapper(const AtomicWrapper &other):val{other.val.load()}{}
+
+ AtomicWrapper &operator=(const AtomicWrapper &other)
+ {
+ val.store(other.val.load());
+ return *this;
+ }
+ };
+
+ template <typename TargetType>
+ struct PassThroughControllerTargetTypeTraits;
+
+ template <typename TargetType>
+ class PassThroughController:
+ virtual public LVL1Controller,
+ virtual public PassThroughControllerInterface
+ {
+ public:
+ using Traits = PassThroughControllerTargetTypeTraits<TargetType>;
+
+ inline PassThroughController(IceInternal::Handle<RobotUnit> robotUnit, LVL1ControllerConfigPtr config);
+
+ inline virtual void rtRun(const IceUtil::Time&, const IceUtil::Time&) override;
+ inline virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+ inline virtual void rtPreActivateController() override;
+
+ //ice interface
+ inline virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override;
+ inline virtual void setJoint(const std::string& name, Ice::Float value, const Ice::Current & = GlobalIceCurrent) override;
+ inline virtual void setJoints(const StringFloatDictionary& values, const Ice::Current & = GlobalIceCurrent) override;
+ protected:
+ std::vector<float*> lvl0Targets;
+ std::vector<const float*> jointStates;
+ std::vector<AtomicWrapper<float>> iceTargets;
+ std::unordered_map<std::string, std::size_t> indices;
+
+ virtual void onInitComponent() override {}
+ virtual void onConnectComponent() override {}
+ };
+
+ template <>
+ struct PassThroughControllerTargetTypeTraits<JointVelocityTarget>
+ {
+ static float* getTargetDatamember(JointVelocityTarget& t)
+ {
+ return &t.velocity;
+ }
+ static std::vector<const float*> getSensorValues(const KinematicDataUnitInterface* ku, const std::vector<std::string>& v)
+ {
+ return ku->getJointVelocities(v);
+ }
+ static std::string getControlMode()
+ {
+ return ControlModes::VelocityMode;
+ }
+ };
+ template <>
+ struct PassThroughControllerTargetTypeTraits<JointTorqueTarget>
+ {
+ static float* getTargetDatamember(JointTorqueTarget& t)
+ {
+ return &t.torque;
+ }
+ static std::vector<const float*> getSensorValues(const KinematicDataUnitInterface* ku, const std::vector<std::string>& v)
+ {
+ return ku->getJointTorques(v);
+ }
+ static std::string getControlMode()
+ {
+ return ControlModes::TorqueMode;
+ }
+ };
+ template <>
+ struct PassThroughControllerTargetTypeTraits<JointPositionTarget>
+ {
+ static float* getTargetDatamember(JointPositionTarget& t)
+ {
+ return &t.position;
+ }
+ static std::vector<const float*> getSensorValues(const KinematicDataUnitInterface* ku, const std::vector<std::string>& v)
+ {
+ return ku->getJointAngles(v);
+ }
+ static std::string getControlMode()
+ {
+ return ControlModes::PositionMode;
+ }
+ };
+
+ LVL1ControllerRegistration<PassThroughController<JointPositionTarget>> registrationSomeControllerPositionPassThroughController("PositionPassThroughController");
+ LVL1ControllerRegistration<PassThroughController<JointVelocityTarget>> registrationSomeControllerVelocityPassThroughController("VelocityPassThroughController");
+ LVL1ControllerRegistration<PassThroughController<JointTorqueTarget >> registrationSomeControllerJointPassThroughController ("JointPassThroughController");
+
+ template <typename TargetType>
+ std::string PassThroughController<TargetType>::getClassName(const Ice::Current&) const
+ {
+ return "PassThroughController_" + Traits::getControlMode();
+ }
+
+ template <typename TargetType>
+ PassThroughController<TargetType>::PassThroughController(IceInternal::Handle<RobotUnit> robotUnit, LVL1ControllerConfigPtr config)
+ {
+ PassThroughControllerConfigPtr cfg = PassThroughControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION_W_HINT(cfg,
+ "The provided config has the wrong type! The type is " << config->ice_id()
+ << " instead of " << PassThroughControllerConfig::ice_staticId());
+
+ //make sure the used units are supported
+ auto kinunit = robotUnit->getRTKinematicDataUnit();
+ ARMARX_CHECK_EXPRESSION_W_HINT(kinunit, "The RobotUnit " << robotUnit->getName() << " has no kinematic data unit");
+ //get pointers to sensor values from units
+ jointStates = Traits::getSensorValues(kinunit, cfg->jointNames) ;
+
+ //not initialized, this is done in rtPreActivateController
+ iceTargets.resize(cfg->jointNames.size(),0);
+
+ //get pointers for the results of this controller
+ lvl0Targets.reserve(cfg->jointNames.size());
+ for (const auto & j : cfg->jointNames)
+ {
+ auto target = dynamic_cast<TargetType*>(robotUnit->getJointTarget(j, Traits::getControlMode()));
+ ARMARX_CHECK_EXPRESSION_W_HINT(target, "The joint " << j << " has no controll mode " << Traits::getControlMode());
+ lvl0Targets.emplace_back(Traits::getTargetDatamember(*target));
+ }
+ indices = toIndexMap(cfg->jointNames);
+ }
+
+ template <typename TargetType>
+ void PassThroughController<TargetType>::rtRun(const IceUtil::Time&, const IceUtil::Time&)
+ {
+ for (std::size_t i = 0; i < iceTargets.size(); ++i)
+ {
+ *lvl0Targets.at(i) = iceTargets.at(i).val.load();
+ }
+ }
+
+ template <typename TargetType>
+ void PassThroughController<TargetType>::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+ }
+
+ template <typename TargetType>
+ void PassThroughController<TargetType>::rtPreActivateController()
+ {
+ for (std::size_t i = 0; i < jointStates.size(); ++i)
+ {
+ iceTargets.at(i).val.store(*jointStates.at(i));
+ }
+ }
+
+ template <typename TargetType>
+ void PassThroughController<TargetType>::setJoint(const std::string& name, Ice::Float value, const Ice::Current &)
+ {
+ auto targetIt = indices.find(name);
+ if(targetIt == indices.end())
+ {
+ throw InvalidArgumentException{"The joint " + name + " is not controlled by this (" + getName() + ") controller"};
+ }
+ iceTargets.at(targetIt->second).val.store(value);
+ }
+
+ template <typename TargetType>
+ void PassThroughController<TargetType>::setJoints(const StringFloatDictionary& values, const Ice::Current &)
+ {
+ for(const auto& value:values)
+ {
+ setJoint(value.first, value.second);
+ }
+ }
+
+}
+#endif
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index a9bcbc478..6d274fa76 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -1,3 +1,5 @@
add_subdirectory(core)
add_subdirectory(widgets)
-add_subdirectory(Controllers)
+add_subdirectory(RobotRTControllers)
+add_subdirectory(BasicRTControllers)
+
diff --git a/source/RobotAPI/libraries/Controllers/CMakeLists.txt b/source/RobotAPI/libraries/RobotRTControllers/CMakeLists.txt
similarity index 63%
rename from source/RobotAPI/libraries/Controllers/CMakeLists.txt
rename to source/RobotAPI/libraries/RobotRTControllers/CMakeLists.txt
index edb54d7b2..1ca0215a8 100644
--- a/source/RobotAPI/libraries/Controllers/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotRTControllers/CMakeLists.txt
@@ -1,16 +1,7 @@
-armarx_component_set_name("Controllers")
-armarx_set_target("Library: Controllers")
-
-set(LIB_NAME Controllers)
-
-#find_package(MyLib QUIET)
-#armarx_build_if(MyLib_FOUND "MyLib not available")
-#
-# all include_directories must be guarded by if(Xyz_FOUND)
-# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
-#if(MyLib_FOUND)
-# include_directories(${MyLib_INCLUDE_DIRS})
-#endif()
+armarx_component_set_name("RobotRTControllers")
+armarx_set_target("Library: RobotRTControllers")
+
+set(LIB_NAME RobotRTControllers)
set(LIBS
ArmarXCoreInterfaces
diff --git a/source/RobotAPI/libraries/Controllers/Constants.h b/source/RobotAPI/libraries/RobotRTControllers/Constants.h
similarity index 100%
rename from source/RobotAPI/libraries/Controllers/Constants.h
rename to source/RobotAPI/libraries/RobotRTControllers/Constants.h
diff --git a/source/RobotAPI/libraries/Controllers/ControlModes.h b/source/RobotAPI/libraries/RobotRTControllers/ControlModes.h
similarity index 78%
rename from source/RobotAPI/libraries/Controllers/ControlModes.h
rename to source/RobotAPI/libraries/RobotRTControllers/ControlModes.h
index 3cdd7d8d1..c81774b41 100644
--- a/source/RobotAPI/libraries/Controllers/ControlModes.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/ControlModes.h
@@ -29,10 +29,10 @@ 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";
+ static const std::string VelocityMode = "VelocityMode";
+ static const std::string PositionMode = "PositionMode";
+ static const std::string TorqueMode = "TorqueMode";
+ static const std::string EmergencyStopMode = "EmergencyStopMode";
}
}
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/ForceTorqueDataUnit.h b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/ForceTorqueDataUnit.h
similarity index 100%
rename from source/RobotAPI/libraries/Controllers/DataUnits/ForceTorqueDataUnit.h
rename to source/RobotAPI/libraries/RobotRTControllers/DataUnits/ForceTorqueDataUnit.h
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/HapticDataUnit.h b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/HapticDataUnit.h
similarity index 100%
rename from source/RobotAPI/libraries/Controllers/DataUnits/HapticDataUnit.h
rename to source/RobotAPI/libraries/RobotRTControllers/DataUnits/HapticDataUnit.h
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/IMUDataUnit.h b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/IMUDataUnit.h
similarity index 100%
rename from source/RobotAPI/libraries/Controllers/DataUnits/IMUDataUnit.h
rename to source/RobotAPI/libraries/RobotRTControllers/DataUnits/IMUDataUnit.h
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/KinematicDataUnit.h b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/KinematicDataUnit.h
similarity index 100%
rename from source/RobotAPI/libraries/Controllers/DataUnits/KinematicDataUnit.h
rename to source/RobotAPI/libraries/RobotRTControllers/DataUnits/KinematicDataUnit.h
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/PlatformDataUnit.h b/source/RobotAPI/libraries/RobotRTControllers/DataUnits/PlatformDataUnit.h
similarity index 100%
rename from source/RobotAPI/libraries/Controllers/DataUnits/PlatformDataUnit.h
rename to source/RobotAPI/libraries/RobotRTControllers/DataUnits/PlatformDataUnit.h
diff --git a/source/RobotAPI/libraries/Controllers/LVL0Controller.cpp b/source/RobotAPI/libraries/RobotRTControllers/LVL0Controller.cpp
similarity index 100%
rename from source/RobotAPI/libraries/Controllers/LVL0Controller.cpp
rename to source/RobotAPI/libraries/RobotRTControllers/LVL0Controller.cpp
diff --git a/source/RobotAPI/libraries/Controllers/LVL0Controller.h b/source/RobotAPI/libraries/RobotRTControllers/LVL0Controller.h
similarity index 100%
rename from source/RobotAPI/libraries/Controllers/LVL0Controller.h
rename to source/RobotAPI/libraries/RobotRTControllers/LVL0Controller.h
diff --git a/source/RobotAPI/libraries/Controllers/LVL1Controller.cpp b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.cpp
similarity index 75%
rename from source/RobotAPI/libraries/Controllers/LVL1Controller.cpp
rename to source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.cpp
index f2ebd3f92..0b8b25547 100644
--- a/source/RobotAPI/libraries/Controllers/LVL1Controller.cpp
+++ b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.cpp
@@ -26,3 +26,22 @@
using namespace armarx;
+
+void LVL1Controller::rtActivateController()
+{
+ if (!isActive)
+ {
+ rtPreActivateController();
+ isActive = true;
+ }
+}
+
+void LVL1Controller::rtDeactivateController()
+{
+ if (isActive)
+ {
+ isActive = false;
+ rtPostDeactivateController();
+ }
+}
+
diff --git a/source/RobotAPI/libraries/Controllers/LVL1Controller.h b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
similarity index 83%
rename from source/RobotAPI/libraries/Controllers/LVL1Controller.h
rename to source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
index e894ca8e2..c765dbf48 100644
--- a/source/RobotAPI/libraries/Controllers/LVL1Controller.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/LVL1Controller.h
@@ -32,7 +32,7 @@
#include <ArmarXCore/core/util/Registrar.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
-#include <RobotAPI/interface/libraries/Controllers/LVL1Controller.h>
+#include <RobotAPI/interface/libraries/RTControllers/LVL1Controller.h>
#include "Targets/JointTargetBase.h"
@@ -61,11 +61,41 @@ namespace armarx
//c++ interface (local calls) (can only be called in the rt thread)
virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+ /**
+ * @brief This function is called before the controller is activated.
+ * You can use it to activate a thread again (DO NOT SPAWN NEW THREADS!) e.g. via a std::atomic_bool.
+ */
+ virtual void rtPreActivateController() {}
+ /**
+ * @brief This function is called after the controller is deactivated.
+ * You can use it to deactivate a thread (DO NOT JOIN THREADS!) e.g. via a std::atomic_bool.
+ */
+ virtual void rtPostDeactivateController() {}
+
+ bool rtUsesJoint(std::size_t jointindex) const
+ {
+ return std::find(jointIndices.begin(), jointIndices.end(), jointindex) == jointIndices.end();
+ }
+ const std::vector<std::size_t>& rtGetJointIndices() const
+ {
+ return jointIndices;
+ }
private:
friend class RobotUnit;
+ void rtActivateController();
+ void rtDeactivateController();
+
//this data is filled by the robot unit to provide convenience functions
std::atomic_bool isActive {false};
JointNameToControlModeDictionary jointControlModeMap;
+ std::vector<std::size_t> jointIndices;
+
+ // ManagedIceObject interface
+ protected:
+ std::string getDefaultName() const
+ {
+ return getClassName();
+ }
};
using LVL1ControllerPtr = IceInternal::Handle<LVL1Controller>;
@@ -85,12 +115,12 @@ namespace armarx
};
/**
- * @defgroup Library-Controllers Controllers
+ * @defgroup Library-RobotRTControllers RobotRTControllers
* @ingroup RobotAPI
- * A description of the library Controllers.
+ * A description of the library RobotRTControllers.
*
* @class LVL1Controller
- * @ingroup Library-Controllers
+ * @ingroup Library-RobotRTControllers
* @brief Brief description of class LVL1Controller.
*
* Detailed description of class LVL1Controller.
@@ -137,9 +167,9 @@ namespace armarx
//read the config
someParamSetViaConfig = someConfig->parameterSetViaIceCalls
//make sure the used units are supported
- ARMARX_CHECK_EXPRESSION_W_HINT(robotUnit->getKinematicDataUnit(), "The RobotUnit " << robotUnit->getName() << " has no kinematic data unit");
+ ARMARX_CHECK_EXPRESSION_W_HINT(robotUnit->getRTKinematicDataUnit(), "The RobotUnit " << robotUnit->getName() << " has no kinematic data unit");
//get pointers to sensor values from units
- jointValuePtrs = robotUnit->getKinematicDataUnit()->getJointAngles({"jointX", "jointY"});
+ jointValuePtrs = robotUnit->getRTKinematicDataUnit()->getJointAngles({"jointX", "jointY"});
//get pointers for the results of this controller
targetJointXPtr = dynamic_cast<JointVelocityTarget*>(robotUnit->getJointTarget("jointX", ControlModes::VelocityMode));
@@ -169,6 +199,7 @@ namespace armarx
}
}
//register the controller
+ //this line has to appear in some cpp of your lib to do its registration (including it is enough)
LVL1ControllerRegistration<SomeController> registrationSomeController("SomeController");
}
* \endcode
diff --git a/source/RobotAPI/libraries/Controllers/RobotUnit.cpp b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
similarity index 65%
rename from source/RobotAPI/libraries/Controllers/RobotUnit.cpp
rename to source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
index 7ea9bdb26..99718eaf2 100644
--- a/source/RobotAPI/libraries/Controllers/RobotUnit.cpp
+++ b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.cpp
@@ -33,13 +33,18 @@ armarx::PropertyDefinitionsPtr armarx::RobotUnit::createPropertyDefinitions()
void armarx::RobotUnit::addLVL0Controller(const std::string& jointName, armarx::LVL0ControllerBase& lvl0Controller)
{
+ std::string controlMode = lvl0Controller.getControlMode();
GuardType guard {dataMutex};
- if (hasLVL0Controller(jointName, lvl0Controller.getControlMode()))
+ if (hasLVL0Controller(jointName, controlMode))
{
- ARMARX_ERROR << "A LVL0Controller for " + jointName + "(" + lvl0Controller.getControlMode() + ") does already exist!";
- throw std::invalid_argument {"A LVL0Controller for " + jointName + "(" + lvl0Controller.getControlMode() + ") does already exist!"};
+ ARMARX_ERROR << "A LVL0Controller for " + jointName + "(" + controlMode + ") does already exist!";
+ throw std::invalid_argument {"A LVL0Controller for " + jointName + "(" + controlMode + ") does already exist!"};
+ }
+ lvl0Controllers[jointName][controlMode] = &lvl0Controller;
+ if(controlMode == ControlModes::EmergencyStopMode)
+ {
+ lvl0ControllersEmergencyStop.at(jointNameIndices.at(jointName)) = &lvl0Controller;
}
- lvl0Controllers[jointName][lvl0Controller.getControlMode()] = &lvl0Controller;
}
const armarx::LVL0ControllerBase& armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode) const
@@ -54,6 +59,14 @@ armarx::LVL0ControllerBase& armarx::RobotUnit::getLVL0Controller(const std::stri
return *lvl0Controllers.at(jointName).at(controlMode);
}
+armarx::RobotUnit::RobotUnit(const std::vector<std::string> &jointNames):
+ jointNames{jointNames},
+ jointNameIndices{toIndexMap(jointNames)},
+ controllersRequested{jointNames.size()},
+ controllersActivated{jointNames.size()},
+ lvl0ControllersEmergencyStop{jointNames.size()}
+{}
+
Ice::StringSeq armarx::RobotUnit::getControllersKnown(const Ice::Current&) const
{
return LVL1ControllerRegistry::getKeys();
@@ -75,9 +88,12 @@ Ice::StringSeq armarx::RobotUnit::getControllerNamesRequested(const Ice::Current
GuardType guard {dataMutex};
Ice::StringSeq result;
result.reserve(lvl1Controllers.size());
- for (const auto & lvl1 : controllersRequested.getWriteBuffer().lvl1Controllers)
+ for (const auto & lvl1 : getRequestedLVL1Controllers())
{
- result.emplace_back(lvl1->getName());
+ if(lvl1)
+ {
+ result.emplace_back(lvl1->getName());
+ }
}
return result;
}
@@ -86,7 +102,7 @@ Ice::StringSeq armarx::RobotUnit::getControllerNamesActivated(const Ice::Current
GuardType guard {dataMutex};
Ice::StringSeq result;
result.reserve(lvl1Controllers.size());
- for (const auto & lvl1 : controllersActivated.getUpToDateReadBuffer().lvl1Controllers)
+ for (const auto & lvl1 : getActivatedLVL1Controllers())
{
result.emplace_back(lvl1->getName());
}
@@ -107,9 +123,12 @@ armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersReque
{
GuardType guard {dataMutex};
StringLVL1ControllerPrxDictionary result;
- for (const auto & lvl1 : controllersRequested.getWriteBuffer().lvl1Controllers)
+ for (const auto & lvl1 : getRequestedLVL1Controllers())
{
- result[lvl1->getName()] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1->getProxy());
+ if(lvl1)
+ {
+ result[lvl1->getName()] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1->getProxy());
+ }
}
return result;
}
@@ -117,7 +136,7 @@ armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersActiv
{
GuardType guard {dataMutex};
StringLVL1ControllerPrxDictionary result;
- for (const auto & lvl1 : controllersActivated.getUpToDateReadBuffer().lvl1Controllers)
+ for (const auto & lvl1 : getActivatedLVL1Controllers())
{
result[lvl1->getName()] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1->getProxy());
}
@@ -128,7 +147,7 @@ armarx::JointNameToLVL1Dictionary armarx::RobotUnit::getControllerJointAssignmen
{
GuardType guard {dataMutex};
JointNameToLVL1Dictionary result;
- for (const auto & lvl1 : controllersActivated.getUpToDateReadBuffer().lvl1Controllers)
+ for (const auto & lvl1 : getActivatedLVL1Controllers())
{
for (const auto & jointMode : lvl1->jointControlModeMap)
{
@@ -142,7 +161,7 @@ armarx::JointNameToControlModeDictionary armarx::RobotUnit::getJointControlModes
{
GuardType guard {dataMutex};
JointNameToControlModeDictionary result;
- const auto& requestedModes = controllersRequested.getWriteBuffer().lvl0Controllers;
+ const auto& requestedModes = getRequestedLVL0Controllers();
ARMARX_CHECK_AND_THROW(jointNames.size() == requestedModes.size(), std::logic_error);
for (std::size_t i = 0; i < jointNames.size(); ++i)
{
@@ -154,7 +173,7 @@ armarx::JointNameToControlModeDictionary armarx::RobotUnit::getJointControlModes
{
GuardType guard {dataMutex};
JointNameToControlModeDictionary result;
- const auto& activatedModes = controllersActivated.getUpToDateReadBuffer().lvl0Controllers;
+ const auto& activatedModes = getActivatedLVL0Controllers();
ARMARX_CHECK_AND_THROW(jointNames.size() == activatedModes.size(), std::logic_error);
for (std::size_t i = 0; i < jointNames.size(); ++i)
{
@@ -247,25 +266,25 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
}
}
//verify (warn for orphant joints) + populate controllersRequested.lvl0Controllers
- controllersRequested.getWriteBuffer().lvl0Controllers.resize(jointNames.size());
+ getRequestedLVL0Controllers().resize(jointNames.size());
for (std::size_t i = 0; i < jointNames.size(); ++i)
{
const auto& joint = jointNames.at(i);
if (lvl1ControllerAssignement[joint].empty())
{
- controllersRequested.getWriteBuffer().lvl0Controllers.at(i) = &getLVL0Controller(joint, ControlModes::EmergencyStopMode);
+ getRequestedLVL0Controllers().at(i) = &getLVL0Controller(joint, ControlModes::EmergencyStopMode);
ARMARX_WARNING << "Joint '" << joint << "' has no lvl1 controller assigned!";
continue;
}
const auto& lvl0Mode = lvl1Controllers.at(lvl1ControllerAssignement[joint])->jointControlModeMap.at(joint);
- controllersRequested.getWriteBuffer().lvl0Controllers.at(i) = &getLVL0Controller(joint, lvl0Mode);
+ getRequestedLVL0Controllers().at(i) = &getLVL0Controller(joint, lvl0Mode);
}
//populate controllersRequested.lvl1Controllers
- controllersRequested.getWriteBuffer().lvl1Controllers.clear();
- controllersRequested.getWriteBuffer().lvl1Controllers.reserve(controllersToActivate.size());
+ getRequestedLVL1Controllers().clear();
+ getRequestedLVL1Controllers().reserve(controllersToActivate.size());
for (const auto & lvl1 : controllersToActivate)
{
- controllersRequested.getWriteBuffer().lvl1Controllers.emplace_back(lvl1Controllers.at(lvl1));
+ getRequestedLVL1Controllers().emplace_back(lvl1Controllers.at(lvl1));
}
//now change the assignement
@@ -303,6 +322,13 @@ armarx::LVL1ControllerInterfacePrx armarx::RobotUnit::loadController(const std::
jointsUsedByLVL1Controler.clear();
LVL1ControllerPtr lvl1 = factory(this, config);
lvl1->jointControlModeMap = jointsUsedByLVL1Controler;
+ lvl1->jointIndices.clear();
+ lvl1->jointIndices.reserve(jointsUsedByLVL1Controler.size());
+ for(const auto& j: jointsUsedByLVL1Controler)
+ {
+ lvl1->jointIndices.emplace_back(jointNameIndices.at(j.first));
+ }
+
getArmarXManager()->addObject(lvl1, instanceName);
const auto prx = lvl1->getProxy(-1);
lvl1Controllers[instanceName] = lvl1;
@@ -328,6 +354,18 @@ bool armarx::RobotUnit::hasLVL1Controller(const std::string& name) const
return lvl1Controllers.end() != lvl1Controllers.find(name);
}
+void armarx::RobotUnit::setLVL1ControllerActive(const armarx::LVL1ControllerPtr& lvl1, bool isActive)
+{
+ if (isActive)
+ {
+ lvl1->rtActivateController();
+ }
+ else
+ {
+ lvl1->rtDeactivateController();
+ }
+}
+
bool armarx::RobotUnit::hasLVL0Controller(const std::string& jointName, const std::string& controlMode) const
{
GuardType guard {dataMutex};
@@ -413,3 +451,161 @@ bool armarx::RobotUnit::loadLibFromPackage(const std::string& package, const std
ARMARX_ERROR << "Could not find library " << name << " in package " << package;
return false;
}
+
+bool armarx::RobotUnit::rtControllersWereSwitched() const
+{
+ return controllersRequested.getUpToDateData();
+}
+
+std::vector<armarx::LVL0ControllerBase *> &armarx::RobotUnit::getRequestedLVL0Controllers()
+{
+ return controllersRequested.getWriteBuffer().lvl0Controllers;
+}
+
+std::vector<armarx::LVL1ControllerPtr> &armarx::RobotUnit::getRequestedLVL1Controllers()
+{
+ return controllersRequested.getWriteBuffer().lvl1Controllers;
+}
+
+const std::vector<armarx::LVL0ControllerBase *> &armarx::RobotUnit::getRequestedLVL0Controllers() const
+{
+ return controllersRequested.getWriteBuffer().lvl0Controllers;
+}
+
+const std::vector<armarx::LVL1ControllerPtr> &armarx::RobotUnit::getRequestedLVL1Controllers() const
+{
+ return controllersRequested.getWriteBuffer().lvl1Controllers;
+}
+
+const std::vector<armarx::LVL0ControllerBase *> &armarx::RobotUnit::getActivatedLVL0Controllers() const
+{
+ return controllersActivated.getUpToDateReadBuffer().lvl0Controllers;
+}
+
+const std::vector<armarx::LVL1ControllerPtr> &armarx::RobotUnit::getActivatedLVL1Controllers() const
+{
+ return controllersActivated.getUpToDateReadBuffer().lvl1Controllers;
+}
+
+const std::vector<armarx::LVL0ControllerBase *> &armarx::RobotUnit::rtGetRequestedLVL0Controllers() const
+{
+ return controllersRequested.getReadBuffer().lvl0Controllers;
+}
+
+const std::vector<armarx::LVL1ControllerPtr> &armarx::RobotUnit::rtGetRequestedLVL1Controllers() const
+{
+ return controllersRequested.getReadBuffer().lvl1Controllers;
+}
+
+std::vector<armarx::LVL0ControllerBase *> &armarx::RobotUnit::rtGetActivatedLVL0Controllers()
+{
+ return controllersActivated.getWriteBuffer().lvl0Controllers;
+}
+
+std::vector<armarx::LVL1ControllerPtr> &armarx::RobotUnit::rtGetActivatedLVL1Controllers()
+{
+ return controllersActivated.getWriteBuffer().lvl1Controllers;
+}
+
+const std::vector<armarx::LVL0ControllerBase *> &armarx::RobotUnit::rtGetActivatedLVL0Controllers() const
+{
+ return controllersActivated.getWriteBuffer().lvl0Controllers;
+}
+
+const std::vector<armarx::LVL1ControllerPtr> &armarx::RobotUnit::rtGetActivatedLVL1Controllers() const
+{
+ return controllersActivated.getWriteBuffer().lvl1Controllers;
+}
+
+void armarx::RobotUnit::rtCommitActivatedLVL1Controllers()
+{
+ controllersActivated.commitWrite();
+}
+
+bool armarx::RobotUnit::rtSwitchSetup()
+{
+ if(!rtControllersWereSwitched())
+ {
+ return false;
+ }
+ //handle lvl1
+ for(std::size_t i = 0; i < rtGetRequestedLVL1Controllers().size(); ++i)
+ {
+ //deactivate old
+ if(rtGetActivatedLVL1Controllers().at(i))
+ {
+ rtGetActivatedLVL1Controllers().at(i)->rtDeactivateController();
+ }
+ //activate new
+ if(rtGetRequestedLVL1Controllers().at(i))
+ {
+ rtGetRequestedLVL1Controllers().at(i)->rtActivateController();
+ }
+ //update activated
+ rtGetActivatedLVL1Controllers().at(i) = rtGetRequestedLVL1Controllers().at(i);
+ }
+ //handle lvl0
+ for(std::size_t i = 0; i < rtGetRequestedLVL0Controllers().size(); ++i)
+ {
+ if(rtGetRequestedLVL0Controllers().at(i) != rtGetActivatedLVL0Controllers().at(i))
+ {
+ rtSwitchLVL0Controller(i, rtGetActivatedLVL0Controllers().at(i), rtGetRequestedLVL0Controllers().at(i));
+ rtGetActivatedLVL0Controllers().at(i) = rtGetRequestedLVL0Controllers().at(i);
+ }
+ }
+ return true;
+}
+
+void armarx::RobotUnit::rtRunLVL1Controllers(const IceUtil::Time &sensorValuesTimestamp, const IceUtil::Time &timeSinceLastIteration)
+{
+ for(LVL1ControllerPtr& lvl1: rtGetActivatedLVL1Controllers())
+ {
+ if(!lvl1)
+ {
+ continue;
+ }
+ lvl1->rtSwapBufferAndRun(sensorValuesTimestamp, timeSinceLastIteration);
+ }
+}
+
+void armarx::RobotUnit::rtDeactivateAssignedLVL1Controller(std::size_t index)
+{
+ for(std::size_t i = 0; i < rtGetRequestedLVL1Controllers().size(); ++i)
+ {
+ if(!rtGetRequestedLVL1Controllers().at(i))
+ {
+ continue;
+ }
+ if(rtGetRequestedLVL1Controllers().at(i)->rtUsesJoint(index))
+ {
+ rtGetRequestedLVL1Controllers().at(i)->rtDeactivateController();
+ for(auto used:rtGetRequestedLVL1Controllers().at(i)->rtGetJointIndices())
+ {
+ rtSwitchLVL0Controller(used, rtGetActivatedLVL0Controllers().at(i), lvl0ControllersEmergencyStop.at(i));
+ rtGetActivatedLVL0Controllers().at(i) = lvl0ControllersEmergencyStop.at(i);
+ }
+ rtGetActivatedLVL1Controllers().at(i) = nullptr;
+ return;
+ }
+ }
+}
+
+void armarx::RobotUnit::rtRunLVL0Controllers()
+{
+ for(LVL0ControllerBase* lvl0: rtGetActivatedLVL0Controllers())
+ {
+ lvl0->run();
+ }
+}
+
+bool armarx::RobotUnit::rtValidateLVL0ControllerSetup() const
+{
+ for(const auto& lvl0: lvl0ControllersEmergencyStop)
+ {
+ if(!lvl0)
+ {
+ return false;
+ }
+ }
+ return true;
+}
diff --git a/source/RobotAPI/libraries/Controllers/RobotUnit.h b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h
similarity index 66%
rename from source/RobotAPI/libraries/Controllers/RobotUnit.h
rename to source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h
index 58fc29c02..7672f0cee 100644
--- a/source/RobotAPI/libraries/Controllers/RobotUnit.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/RobotUnit.h
@@ -36,7 +36,7 @@
#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
-#include <RobotAPI/interface/libraries/Controllers/RobotUnitInterface.h>
+#include <RobotAPI/interface/libraries/RTControllers/RobotUnitInterface.h>
#include "LVL0Controller.h"
#include "LVL1Controller.h"
@@ -102,64 +102,61 @@ namespace armarx
* {
* try
* {
- * initYourCommunication();
+ * initYourCommunication();//implement this
+ * passYourLVL0ControllersToTheRobotUnit();//implement this
+ * if(!rtValidateLVL0ControllerSetup())
+ * {
+ * //report errors
+ * throw std::logic_error{"lvl0 controller setup invalid"};
+ * }
* IceUtil::Time currentStateTimestamp = TimeUtil::GetTime();
* IceUtil::Time lastStateTimestamp = TimeUtil::GetTime();
- * getTheCurrentRobotState();
+ * getTheCurrentRobotState();//implement this
* while(isNotStopped())
* {
* const IceUtil::Time startIteration = TimeUtil::GetTime();
- * if(controllersRequested.getUpToDateData())
- * {
- * //the controllers were switched
- * //change the joint controll modes (they should use the new lvl0 controllers)
- * //update controllersActivated to inform the user which controllers are in use
- * controllersActivated.getWriteBuffer().lvl0Controllers = ...;
- * controllersActivated.getWriteBuffer().lvl1Controllers = controllersRequested.getReadBuffer().lvl1Controllers;
- * controllersActivated.commitWrite();
- * }
- * //run the lvl1 controllers
- * for(LVL1ControllerPtr& lvl1: controllersActivated.getWriteBuffer().lvl1Controllers)
+ * //switching controllers
+ * if(rtSwitchSetup())
* {
- * if(!lvl1)
+ * if(switchSetupError)
* {
- * continue;
+ * //report this error and exit
+ * throw std::logic_error{"switching the controller setup failed"};
* }
- * lvl1->rtSwapBufferAndRun(currentStateTimestamp, currentStateTimestamp - lastStateTimestamp);
+ * //validate the setup if you need to
+ * //if you had to change something, call rtCommitActivatedLVL1Controllers();
* }
+ * //run the lvl1 controllers
+ * rtRunLVL1Controllers(currentStateTimestamp, currentStateTimestamp - lastStateTimestamp);
* //validate targets
- * for(LVL0ControllerBase* lvl0: controllersActivated.getWriteBuffer().lvl0Controllers)
+ * for(std::size_t i = 0; i<rtGetActivatedLVL0Controllers().size(); ++i)
* {
- * if(!lvl0->isTargetVaild())
+ * if(!rtGetActivatedLVL0Controllers().at(i)->isTargetVaild())
* {
* //handle this error!
* //you probably should log this error to some error struct
- * //then identify the misbehaving lvl1 controller
- * //set all lvl0 controllers controlled by it to ControlModes::EmergencyStopMode
- * //update controllersActivated.getWriteBuffer().lvl0Controllers
- * //remove the lvl controller from controllersActivated.getWriteBuffer().lvl1Controllers (set it to nullptr)
- * controllersActivated.commitWrite()
+ * rtDeactivateAssignedLVL1Controller(i);
+ * rtCommitActivatedLVL1Controllers();
* }
* }
- * for(LVL0ControllerBase* lvl0: controllersActivated.getWriteBuffer().lvl0Controllers)
- * {
- * lvl0->run();
- * }
+ * //run lvl0
+ * rtRunLVL0Controllers();
*
+ * //time keeping + communicating
* currentStateTimestamp = TimeUtil::GetTime();
* lastStateTimestamp = currentStateTimestamp;
- * getTheCurrentRobotStateAndSendTheCurrentControllCommands();
- * communicateWithYourPublisherThread();
+ * getTheCurrentRobotStateAndSendTheCurrentControllCommands();//implement this
+ * communicateWithYourPublisherThread();//implement this
* //maybe meassure how long a loop took and log this information to your error struct
* TimeUtil::Sleep(IceUtil::Time::microSeconds(1000)-(currentStateTimestamp-startIteration));
* }
* }
* catch(...)
* {
- * emergencyStop();
- * dumpDebuggingData();
- * doSomeErrorHandling();
- * shutEverythingGracefullyDown();
+ * emergencyStop();//implement this
+ * dumpDebuggingData();//implement this
+ * doSomeErrorHandling();//implement this
+ * shutEverythingGracefullyDown();//implement this
* }
* }
* \endcode
@@ -191,6 +188,9 @@ namespace armarx
public:
using MutexType = std::recursive_mutex;
using GuardType = std::lock_guard<MutexType>;
+
+ RobotUnit(const std::vector<std::string>& jointNames);
+
//controllers
virtual Ice::StringSeq getControllersKnown(const Ice::Current& = GlobalIceCurrent) const override;
virtual Ice::StringSeq getControllerNamesLoaded(const Ice::Current& = GlobalIceCurrent) const override;
@@ -205,30 +205,66 @@ namespace armarx
virtual JointNameToControlModeDictionary getJointControlModesRequested(const Ice::Current& = GlobalIceCurrent) const override;
virtual JointNameToControlModeDictionary getJointControlModesActivated(const Ice::Current& = GlobalIceCurrent) const override;
virtual JointNameToControlModesDictionary getJointControlModesSupported(const Ice::Current& = GlobalIceCurrent) const override;
- //loading and switching
+ //loading and switching (ice)
virtual void switchSetup(const Ice::StringSeq& controllerRequestedNames, const Ice::Current& = GlobalIceCurrent) override;
virtual LVL1ControllerInterfacePrx loadController(const std::string& className, const std::string& instanceName, const LVL1ControllerConfigPtr& config, const Ice::Current& = GlobalIceCurrent) override;
virtual bool loadLibFromPath(const std::string& path, const Ice::Current& = GlobalIceCurrent) override;
virtual bool loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current& = GlobalIceCurrent) override;
+ //get buffers (ice)
+ /// @return The requested lvl0 controllers (none is null)
+ std::vector<LVL0ControllerBase*>& getRequestedLVL0Controllers();
+ /// @return The requested lvl1 controllers (some may be null)
+ std::vector<LVL1ControllerPtr >& getRequestedLVL1Controllers();
+ /// @return The requested lvl0 controllers (none is null)
+ const std::vector<LVL0ControllerBase*>& getRequestedLVL0Controllers() const;
+ /// @return The requested lvl1 controllers (some may be null)
+ const std::vector<LVL1ControllerPtr >& getRequestedLVL1Controllers() const;
+ /// @return The activated lvl0 controllers (none is null)
+ const std::vector<LVL0ControllerBase*>& getActivatedLVL0Controllers() const;
+ /// @return The activated lvl1 controllers (some may be null)
+ const std::vector<LVL1ControllerPtr >& getActivatedLVL1Controllers() const;
+ //buffers (rt)
+ bool rtControllersWereSwitched() const;
+ /// @return The requested lvl0 controllers (none is null)
+ const std::vector<LVL0ControllerBase*>& rtGetRequestedLVL0Controllers() const;
+ /// @return The requested lvl1 controllers (some may be null)
+ const std::vector<LVL1ControllerPtr >& rtGetRequestedLVL1Controllers() const;
+ /// @return The activated lvl0 controllers (none is null)
+ std::vector<LVL0ControllerBase*>& rtGetActivatedLVL0Controllers();
+ /// @return The activated lvl1 controllers (some may be null)
+ std::vector<LVL1ControllerPtr >& rtGetActivatedLVL1Controllers();
+ /// @return The activated lvl0 controllers (none is null)
+ const std::vector<LVL0ControllerBase*>& rtGetActivatedLVL0Controllers() const;
+ /// @return The activated lvl1 controllers (some may be null)
+ const std::vector<LVL1ControllerPtr >& rtGetActivatedLVL1Controllers() const;
+ void rtCommitActivatedLVL1Controllers();
+
+ //switching (rt)
+ bool rtSwitchSetup();
+ void rtRunLVL1Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtDeactivateAssignedLVL1Controller(std::size_t index);
+ void rtRunLVL0Controllers();
+
+ virtual void rtSwitchLVL0Controller(std::size_t index, LVL0ControllerBase* oldLVL0, LVL0ControllerBase* newLVL0) = 0;
+ //checks (rt)
+ virtual bool rtValidateLVL0ControllerSetup() const;
//units (ice)
virtual KinematicUnitInterfacePrx getKinematicUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
virtual ForceTorqueUnitInterfacePrx getForceTorqueUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
virtual InertialMeasurementUnitInterfacePrx getInertialMeasurementUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
virtual PlatformUnitInterfacePrx getPlatformUnitInterface(const Ice::Current& = GlobalIceCurrent) const = 0;
- //units (rt)
- virtual const ForceTorqueDataUnitInterface& getRTForceTorqueDataUnit() const = 0;
- virtual const HapticDataUnitInterface& getRTHapticDataUnit() const = 0;
- virtual const IMUDataUnitInterface& getRTIMUDataUnit() const = 0;
- virtual const KinematicDataUnitInterface& getRTKinematicDataUnit() const = 0;
- virtual const PlatformDataUnitInterface& getRTPlatformDataUnit() const = 0;
- //targets
+ //units (ice but results are used in rt)
+ virtual const ForceTorqueDataUnitInterface* getRTForceTorqueDataUnit() const = 0;
+ virtual const HapticDataUnitInterface* getRTHapticDataUnit() const = 0;
+ virtual const IMUDataUnitInterface* getRTIMUDataUnit() const = 0;
+ virtual const KinematicDataUnitInterface* getRTKinematicDataUnit() const = 0;
+ virtual const PlatformDataUnitInterface* getRTPlatformDataUnit() const = 0;
+ //targets (ice)
JointTargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode);
protected:
- /**
- * @see PropertyUser::createPropertyDefinitions()
- */
+ /// @see PropertyUser::createPropertyDefinitions()
virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
/**
@@ -238,44 +274,41 @@ namespace armarx
* @throw If you add two controllers withe the same control mode for one joint, an std::invalid_argument exception is thrown.
*/
void addLVL0Controller(const std::string& jointName, LVL0ControllerBase& lvl0Controller);
- /**
- * @return The LVL0Controller for given joint and control mode.
- */
+
+ /// @return The LVL0Controller for given joint and control mode.
const LVL0ControllerBase& getLVL0Controller(const std::string& jointName, const std::string& controlMode) const;
- /**
- * @return The LVL0Controller for given joint and control mode.
- */
+
+ /// @return The LVL0Controller for given joint and control mode.
LVL0ControllerBase& getLVL0Controller(const std::string& jointName, const std::string& controlMode);
- /**
- * @return Whether a LVL0Controller for given joint and control mode exists.
- */
+
+ /// @return Whether a LVL0Controller for given joint and control mode exists.
bool hasLVL0Controller(const std::string& jointName, const std::string& controlMode) const;
- /**
- * @return Whether a LVL1Controller for this name is loaded.
- */
+ /// @return Whether a LVL1Controller for this name is loaded.
bool hasLVL1Controller(const std::string& name) const;
-
- static void setLVL1ControllerActive(const LVL1ControllerPtr& lvl1, bool isActive = 1)
- {
- lvl1->isActive = isActive;
- }
+ static void setLVL1ControllerActive(const LVL1ControllerPtr& lvl1, bool isActive = 1);
struct LVL0AndLVL1ControllerList
{
+ LVL0AndLVL1ControllerList(std::size_t size):
+ lvl0Controllers{size,nullptr},
+ lvl1Controllers{size,nullptr}
+ {}
+
+ LVL0AndLVL1ControllerList() = default;
+ LVL0AndLVL1ControllerList(const LVL0AndLVL1ControllerList&) = default;
+
std::vector<LVL0ControllerBase*> lvl0Controllers;
std::vector<LVL1ControllerPtr > lvl1Controllers;
};
mutable MutexType dataMutex;
//data accessible in RT and nonRT
const std::vector<std::string> jointNames;
+ const std::unordered_map<std::string, std::size_t> jointNameIndices;
+ /// @brief
std::atomic_bool switchSetupError {false};
//used to communicate with rt
- /// @brief Controllers the RT thread is supposed to activate (direction nonRT -> RT)
- TripleBuffer<LVL0AndLVL1ControllerList> controllersRequested;
- /// @brief Controllers the RT thread is currently running (direction RT -> nonRT)
- TripleBuffer<LVL0AndLVL1ControllerList> controllersActivated;
//non rt data
/**
* @brief Holds pointer to all lvl0 controllers. (index: [jointname][controlmode])
@@ -292,7 +325,14 @@ namespace armarx
private:
bool loadLibFromAbsolutePath(const std::string& path);
+ /// @brief Stores joints accessed via getJointTarget
JointNameToControlModeDictionary jointsUsedByLVL1Controler;
+ /// @brief Controllers the RT thread is supposed to activate (direction nonRT -> RT) (some lvl1 controllers may be null)
+ TripleBuffer<LVL0AndLVL1ControllerList> controllersRequested;
+ /// @brief Controllers the RT thread is currently running (direction RT -> nonRT) (some lvl1 controllers may be null)
+ TripleBuffer<LVL0AndLVL1ControllerList> controllersActivated;
+ /// @brief LVL0Controllers for Emergency stop
+ std::vector<LVL0ControllerBase*> lvl0ControllersEmergencyStop;
};
using RobotUnitPtr = IceInternal::Handle<RobotUnit>;
diff --git a/source/RobotAPI/libraries/Controllers/SyntaxCheck.cpp b/source/RobotAPI/libraries/RobotRTControllers/SyntaxCheck.cpp
similarity index 100%
rename from source/RobotAPI/libraries/Controllers/SyntaxCheck.cpp
rename to source/RobotAPI/libraries/RobotRTControllers/SyntaxCheck.cpp
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointPositionTarget.h b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointPositionTarget.h
similarity index 91%
rename from source/RobotAPI/libraries/Controllers/Targets/JointPositionTarget.h
rename to source/RobotAPI/libraries/RobotRTControllers/Targets/JointPositionTarget.h
index e599de3fc..6e7205de9 100644
--- a/source/RobotAPI/libraries/Controllers/Targets/JointPositionTarget.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointPositionTarget.h
@@ -28,12 +28,12 @@
namespace armarx
{
/**
- * @defgroup Library-Controllers Controllers
+ * @defgroup Library-RobotRTControllers RobotRTControllers
* @ingroup RobotAPI
- * A description of the library Controllers.
+ * A description of the library RobotRTControllers.
*
* @class JointPositionTarget
- * @ingroup Library-Controllers
+ * @ingroup Library-RobotRTControllers
* @brief Brief description of class JointPositionTarget.
*
* Detailed description of class JointPositionTarget.
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointTargetBase.h b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointTargetBase.h
similarity index 90%
rename from source/RobotAPI/libraries/Controllers/Targets/JointTargetBase.h
rename to source/RobotAPI/libraries/RobotRTControllers/Targets/JointTargetBase.h
index eb1d49e79..3f9736b89 100644
--- a/source/RobotAPI/libraries/Controllers/Targets/JointTargetBase.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointTargetBase.h
@@ -30,12 +30,12 @@
namespace armarx
{
/**
- * @defgroup Library-Controllers Controllers
+ * @defgroup Library-RobotRTControllers RobotRTControllers
* @ingroup RobotAPI
- * A description of the library Controllers.
+ * A description of the library RobotRTControllers.
*
* @class JointTargetBase
- * @ingroup Library-Controllers
+ * @ingroup Library-RobotRTControllers
* @brief Brief description of class JointTargetBase.
*
* Detailed description of class JointTargetBase.
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointTorqueTarget.h b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointTorqueTarget.h
similarity index 91%
rename from source/RobotAPI/libraries/Controllers/Targets/JointTorqueTarget.h
rename to source/RobotAPI/libraries/RobotRTControllers/Targets/JointTorqueTarget.h
index ab7548ba1..9421f2218 100644
--- a/source/RobotAPI/libraries/Controllers/Targets/JointTorqueTarget.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointTorqueTarget.h
@@ -28,12 +28,12 @@
namespace armarx
{
/**
- * @defgroup Library-Controllers Controllers
+ * @defgroup Library-RobotRTControllers RobotRTControllers
* @ingroup RobotAPI
- * A description of the library Controllers.
+ * A description of the library RobotRTControllers.
*
* @class JointTorqueTarget
- * @ingroup Library-Controllers
+ * @ingroup Library-RobotRTControllers
* @brief Brief description of class JointTorqueTarget.
*
* Detailed description of class JointTorqueTarget.
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointVelocityTarget.h b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointVelocityTarget.h
similarity index 91%
rename from source/RobotAPI/libraries/Controllers/Targets/JointVelocityTarget.h
rename to source/RobotAPI/libraries/RobotRTControllers/Targets/JointVelocityTarget.h
index 7c5d3100f..a4b307de9 100644
--- a/source/RobotAPI/libraries/Controllers/Targets/JointVelocityTarget.h
+++ b/source/RobotAPI/libraries/RobotRTControllers/Targets/JointVelocityTarget.h
@@ -28,12 +28,12 @@
namespace armarx
{
/**
- * @defgroup Library-Controllers Controllers
+ * @defgroup Library-RobotRTControllers RobotRTControllers
* @ingroup RobotAPI
- * A description of the library Controllers.
+ * A description of the library RobotRTControllers.
*
* @class JointVelocityTarget
- * @ingroup Library-Controllers
+ * @ingroup Library-RobotRTControllers
* @brief Brief description of class JointVelocityTarget.
*
* Detailed description of class JointVelocityTarget.
--
GitLab