From 6d203d3aa994003d94b1ea8b1a1b79bc2dd5ce43 Mon Sep 17 00:00:00 2001
From: Raphael <ufdrv@student.kit.edu>
Date: Wed, 18 Jan 2017 17:49:09 +0100
Subject: [PATCH] added basic structure and implementation for the new
controller setup
---
source/RobotAPI/interface/CMakeLists.txt | 3 +-
.../libraries/Controllers/LVL1Controller.ice | 33 +++++
source/RobotAPI/libraries/CMakeLists.txt | 2 +-
.../libraries/Controllers/CMakeLists.txt | 48 +++++++
.../libraries/Controllers/Constatnts.h | 37 +++++
.../DataUnits/ForceTorqueDataUnit.h | 34 +++++
.../Controllers/DataUnits/HapticDataUnit.h | 34 +++++
.../Controllers/DataUnits/IMUDataUnit.h | 34 +++++
.../Controllers/DataUnits/KinematicDataUnit.h | 128 ++++++++++++++++++
.../Controllers/DataUnits/PlatformDataUnit.h | 69 ++++++++++
.../libraries/Controllers/LVL0Controller.cpp | 28 ++++
.../libraries/Controllers/LVL0Controller.h | 81 +++++++++++
.../libraries/Controllers/LVL1Controller.cpp | 28 ++++
.../libraries/Controllers/LVL1Controller.h | 56 ++++++++
.../libraries/Controllers/SyntaxCheck.cpp | 37 +++++
.../Controllers/Targets/JointPositionTarget.h | 61 +++++++++
.../Targets/JointPositionVelocityTarget.h | 63 +++++++++
.../Controllers/Targets/JointTargetBase.h | 50 +++++++
.../Controllers/Targets/JointTorqueTarget.h | 61 +++++++++
.../Controllers/Targets/JointVelocityTarget.h | 61 +++++++++
20 files changed, 946 insertions(+), 2 deletions(-)
create mode 100644 source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice
create mode 100644 source/RobotAPI/libraries/Controllers/CMakeLists.txt
create mode 100644 source/RobotAPI/libraries/Controllers/Constatnts.h
create mode 100644 source/RobotAPI/libraries/Controllers/DataUnits/ForceTorqueDataUnit.h
create mode 100644 source/RobotAPI/libraries/Controllers/DataUnits/HapticDataUnit.h
create mode 100644 source/RobotAPI/libraries/Controllers/DataUnits/IMUDataUnit.h
create mode 100644 source/RobotAPI/libraries/Controllers/DataUnits/KinematicDataUnit.h
create mode 100644 source/RobotAPI/libraries/Controllers/DataUnits/PlatformDataUnit.h
create mode 100644 source/RobotAPI/libraries/Controllers/LVL0Controller.cpp
create mode 100644 source/RobotAPI/libraries/Controllers/LVL0Controller.h
create mode 100644 source/RobotAPI/libraries/Controllers/LVL1Controller.cpp
create mode 100644 source/RobotAPI/libraries/Controllers/LVL1Controller.h
create mode 100644 source/RobotAPI/libraries/Controllers/SyntaxCheck.cpp
create mode 100644 source/RobotAPI/libraries/Controllers/Targets/JointPositionTarget.h
create mode 100644 source/RobotAPI/libraries/Controllers/Targets/JointPositionVelocityTarget.h
create mode 100644 source/RobotAPI/libraries/Controllers/Targets/JointTargetBase.h
create mode 100644 source/RobotAPI/libraries/Controllers/Targets/JointTorqueTarget.h
create mode 100644 source/RobotAPI/libraries/Controllers/Targets/JointVelocityTarget.h
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 11dd79e23..38347386c 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -35,10 +35,11 @@ set(SLICE_FILES
units/UnitInterface.ice
units/ATINetFTUnit.ice
-
components/ViewSelectionInterface.ice
visualization/DebugDrawerInterface.ice
+
+ libraries/Controllers/LVL1Controller.ice
)
#core/RobotIK.ice
diff --git a/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice b/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice
new file mode 100644
index 000000000..630d01d84
--- /dev/null
+++ b/source/RobotAPI/interface/libraries/Controllers/LVL1Controller.ice
@@ -0,0 +1,33 @@
+/*
+ * 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::LVL1ControllerInterface
+ * @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
+ */
+
+module armarx
+{
+ class LVL1ControllerConfig
+ {
+ };
+
+ class LVL1ControllerInterface
+ {
+
+ };
+};
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index c91c3c67b..a9bcbc478 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -1,3 +1,3 @@
add_subdirectory(core)
add_subdirectory(widgets)
-
+add_subdirectory(Controllers)
diff --git a/source/RobotAPI/libraries/Controllers/CMakeLists.txt b/source/RobotAPI/libraries/Controllers/CMakeLists.txt
new file mode 100644
index 000000000..b95e6d68c
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/CMakeLists.txt
@@ -0,0 +1,48 @@
+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()
+
+set(LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ RobotAPIInterfaces
+)
+
+set(LIB_FILES
+ SyntaxCheck.cpp
+
+ LVL0Controller.cpp
+ LVL1Controller.cpp
+)
+set(LIB_HEADERS
+ Constatnts.h
+
+ Targets/JointTargetBase.h
+ Targets/JointPositionTarget.h
+ Targets/JointVelocityTarget.h
+ Targets/JointPositionVelocityTarget.h
+ Targets/JointTorqueTarget.h
+
+ DataUnits/ForceTorqueDataUnit.h
+ DataUnits/HapticDataUnit.h
+ DataUnits/IMUDataUnit.h
+ DataUnits/KinematicDataUnit.h
+ DataUnits/PlatformDataUnit.h
+
+ LVL0Controller.h
+ LVL1Controller.h
+)
+
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
diff --git a/source/RobotAPI/libraries/Controllers/Constatnts.h b/source/RobotAPI/libraries/Controllers/Constatnts.h
new file mode 100644
index 000000000..022b52330
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/Constatnts.h
@@ -0,0 +1,37 @@
+/*
+ * 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_ControllerConstatns_H
+#define _ARMARX_LIB_RobotAPI_ControllerConstatns_H
+
+#include <cmath>
+#include <string>
+
+namespace armarx
+{
+ namespace ControllerConstatns
+ {
+ const float ValueNotSetNaN = std::nanf(std::to_string((1 << 16) - 1).c_str());
+ }
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/ForceTorqueDataUnit.h b/source/RobotAPI/libraries/Controllers/DataUnits/ForceTorqueDataUnit.h
new file mode 100644
index 000000000..4c3f05469
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/DataUnits/ForceTorqueDataUnit.h
@@ -0,0 +1,34 @@
+/*
+ * 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::ForceTorqueDataUnit
+ * @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_ForceTorqueDataUnit_H
+#define _ARMARX_LIB_RobotAPI_ForceTorqueDataUnit_H
+
+namespace armarx
+{
+ class ForceTorqueDataUnit
+ {
+
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/HapticDataUnit.h b/source/RobotAPI/libraries/Controllers/DataUnits/HapticDataUnit.h
new file mode 100644
index 000000000..d9f6ac30d
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/DataUnits/HapticDataUnit.h
@@ -0,0 +1,34 @@
+/*
+ * 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::HapticDataUnit
+ * @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_HapticDataUnit_H
+#define _ARMARX_LIB_RobotAPI_HapticDataUnit_H
+
+namespace armarx
+{
+ class HapticDataUnit
+ {
+
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/IMUDataUnit.h b/source/RobotAPI/libraries/Controllers/DataUnits/IMUDataUnit.h
new file mode 100644
index 000000000..d337532b6
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/DataUnits/IMUDataUnit.h
@@ -0,0 +1,34 @@
+/*
+ * 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::IMUDataUnit
+ * @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_IMUDataUnit_H
+#define _ARMARX_LIB_RobotAPI_IMUDataUnit_H
+
+namespace armarx
+{
+ class IMUDataUnit
+ {
+
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/KinematicDataUnit.h b/source/RobotAPI/libraries/Controllers/DataUnits/KinematicDataUnit.h
new file mode 100644
index 000000000..6cf3a555c
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/DataUnits/KinematicDataUnit.h
@@ -0,0 +1,128 @@
+/*
+ * 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::KinematicDataUnit
+ * @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_KinematicDataUnit_H
+#define _ARMARX_LIB_RobotAPI_KinematicDataUnit_H
+
+#include <vector>
+#include <string>
+#include <unordered_map>
+#include <algorithm>
+
+#include "../Constatnts.h"
+
+namespace armarx
+{
+ class KinematicDataUnit
+ {
+ template<class T>
+ static std::unordered_map<T, std::size_t> toInexMap(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),
+ jointNames {jointNames},
+ jointIndices {toInexMap(jointNames)}
+ {}
+ KinematicDataUnit() = default;
+ KinematicDataUnit(const KinematicDataUnit&) = default;
+
+ const std::vector<std::string>& getJointNames() const
+ {
+ return jointNames;
+ }
+ std::size_t getJointIndex(const std::string& joint) const
+ {
+ return jointIndices.at(joint);
+ }
+
+ virtual std::vector<const float*> getJointAngles(const std::vector<std::string>& joints)const
+ {
+ return getPointers(joints, jointAngles);
+ }
+ virtual std::vector<const float*> getJointVelocities(const std::vector<std::string>& joints)const
+ {
+ return getPointers(joints, jointVelocites);
+ }
+ virtual std::vector<const float*> getJointTorques(const std::vector<std::string>& joints)const
+ {
+ return getPointers(joints, jointTorques);
+ }
+ virtual std::vector<const float*> getJointCurrents(const std::vector<std::string>& joints)const
+ {
+ return getPointers(joints, jointCurrents);
+ }
+ virtual std::vector<const float*> getJointMotorTemperatures(const std::vector<std::string>& joints)const
+ {
+ return getPointers(joints, jointMotorTemperatures);
+ }
+ virtual std::vector<const float*> getJointGearTemperatures(const std::vector<std::string>& joints)const
+ {
+ return getPointers(joints, jointGearTempertures);
+ }
+
+ 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;
+ 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*> result;
+ result.resize(joints.size());
+ std::transform(
+ joints.begin(), joints.end(), std::back_inserter(result),
+ [targets, this](const std::string & joint)
+ {
+ return &targets.at(getJointIndex(joint));
+ }
+ );
+ return result;
+ }
+ const std::vector<std::string> jointNames;
+ const std::unordered_map<std::string, std::size_t> jointIndices;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/Controllers/DataUnits/PlatformDataUnit.h b/source/RobotAPI/libraries/Controllers/DataUnits/PlatformDataUnit.h
new file mode 100644
index 000000000..3fccfc890
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/DataUnits/PlatformDataUnit.h
@@ -0,0 +1,69 @@
+/*
+ * 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::PlatformDataUnit
+ * @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_PlatformDataUnit_H
+#define _ARMARX_LIB_RobotAPI_PlatformDataUnit_H
+
+#include "../Constatnts.h"
+
+namespace armarx
+{
+ class PlatformDataUnit
+ {
+ public:
+ const float* getPositionX() const
+ {
+ return &positionX;
+ }
+ const float* getPositionY() const
+ {
+ return &positionY;
+ }
+ const float* getRotation() const
+ {
+ return &rotation;
+ }
+
+ const float* getVelocityX() const
+ {
+ return &velocityX;
+ }
+ const float* getVelocityY() const
+ {
+ return &velocityY;
+ }
+ const float* getVelocityRotation() const
+ {
+ return &velocityRotation;
+ }
+ protected:
+ float positionX = ControllerConstatns::ValueNotSetNaN;
+ float positionY = ControllerConstatns::ValueNotSetNaN;
+ float rotation = ControllerConstatns::ValueNotSetNaN;
+
+ float velocityX = ControllerConstatns::ValueNotSetNaN;
+ float velocityY = ControllerConstatns::ValueNotSetNaN;
+ float velocityRotation = ControllerConstatns::ValueNotSetNaN;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/Controllers/LVL0Controller.cpp b/source/RobotAPI/libraries/Controllers/LVL0Controller.cpp
new file mode 100644
index 000000000..f1314225d
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/LVL0Controller.cpp
@@ -0,0 +1,28 @@
+/*
+ * 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::LVL0Controller
+ * @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 "LVL0Controller.h"
+
+
+using namespace armarx;
+
+
diff --git a/source/RobotAPI/libraries/Controllers/LVL0Controller.h b/source/RobotAPI/libraries/Controllers/LVL0Controller.h
new file mode 100644
index 000000000..e38510288
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/LVL0Controller.h
@@ -0,0 +1,81 @@
+/*
+ * 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::LVL0Controller
+ * @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_LVL0Controller_H
+#define _ARMARX_LIB_RobotAPI_LVL0Controller_H
+
+#include <stdexcept>
+
+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
+ {
+ public:
+ using TargetType = TargetT;
+ LVL0Controller()
+ {
+ resetTarget();
+ }
+
+ TargetType* getTarget()
+ {
+ return target;
+ }
+
+ void resetTarget()
+ {
+ target.reset();
+ }
+
+ void run()
+ {
+ if (!target.valid())
+ {
+ throw std::logic_error {"LVL0 Controller failed! Target is invalid"};
+ }
+ writeTarget();
+ resetTarget();
+ }
+
+ /**
+ * @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.cpp b/source/RobotAPI/libraries/Controllers/LVL1Controller.cpp
new file mode 100644
index 000000000..f2ebd3f92
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/LVL1Controller.cpp
@@ -0,0 +1,28 @@
+/*
+ * 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::LVL1Controller
+ * @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 "LVL1Controller.h"
+
+
+using namespace armarx;
+
+
diff --git a/source/RobotAPI/libraries/Controllers/LVL1Controller.h b/source/RobotAPI/libraries/Controllers/LVL1Controller.h
new file mode 100644
index 000000000..8e44980e0
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/LVL1Controller.h
@@ -0,0 +1,56 @@
+/*
+ * 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::LVL1Controller
+ * @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_LVL1Controller_H
+#define _ARMARX_LIB_RobotAPI_LVL1Controller_H
+
+#include <map>
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+
+#include "Targets/JointTargetBase.h"
+
+namespace armarx
+{
+ /**
+ * @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.
+ */
+ class LVL1Controller
+ {
+ 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;
+ };
+
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/Controllers/SyntaxCheck.cpp b/source/RobotAPI/libraries/Controllers/SyntaxCheck.cpp
new file mode 100644
index 000000000..b6406e2dd
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/SyntaxCheck.cpp
@@ -0,0 +1,37 @@
+/*
+ * 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
+ */
+
+#include "Constatnts.h"
+#include "LVL0Controller.h"
+#include "LVL1Controller.h"
+
+#include "DataUnits/ForceTorqueDataUnit.h"
+#include "DataUnits/HapticDataUnit.h"
+#include "DataUnits/IMUDataUnit.h"
+#include "DataUnits/KinematicDataUnit.h"
+#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
new file mode 100644
index 000000000..9bc7d9d13
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/Targets/JointPositionTarget.h
@@ -0,0 +1,61 @@
+/*
+ * 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::JointPositionTarget
+ * @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_JointPositionTarget_H
+#define _ARMARX_LIB_RobotAPI_JointPositionTarget_H
+
+#include "JointTargetBase.h"
+
+namespace armarx
+{
+ /**
+ * @defgroup Library-Controllers Controllers
+ * @ingroup RobotAPI
+ * A description of the library Controllers.
+ *
+ * @class JointPositionTarget
+ * @ingroup Library-Controllers
+ * @brief Brief description of class JointPositionTarget.
+ *
+ * Detailed description of class JointPositionTarget.
+ */
+ class JointPositionTarget: public JointTargetBase
+ {
+ public:
+ float position = ControllerConstatns::ValueNotSetNaN;
+ virtual ControlMode getType() const override
+ {
+ return ePositionControl;
+ }
+ virtual void reset()
+ {
+ position = ControllerConstatns::ValueNotSetNaN;
+ }
+ bool isValid() const
+ {
+ return std::isfinite(position);
+ }
+ };
+
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointPositionVelocityTarget.h b/source/RobotAPI/libraries/Controllers/Targets/JointPositionVelocityTarget.h
new file mode 100644
index 000000000..d2622cc91
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/Targets/JointPositionVelocityTarget.h
@@ -0,0 +1,63 @@
+/*
+ * 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 = ControllerConstatns::ValueNotSetNaN;
+ float velocity = ControllerConstatns::ValueNotSetNaN;
+ virtual ControlMode getType() const override
+ {
+ return ePositionVelocityControl;
+ }
+ virtual void reset()
+ {
+ position = ControllerConstatns::ValueNotSetNaN;
+ velocity = ControllerConstatns::ValueNotSetNaN;
+ }
+ bool isValid() const
+ {
+ 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
new file mode 100644
index 000000000..0dc9ee2b6
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/Targets/JointTargetBase.h
@@ -0,0 +1,50 @@
+/*
+ * 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::JointTargetBase
+ * @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_JointTargetBase_H
+#define _ARMARX_LIB_RobotAPI_JointTargetBase_H
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include "../Constatnts.h"
+
+namespace armarx
+{
+ /**
+ * @defgroup Library-Controllers Controllers
+ * @ingroup RobotAPI
+ * A description of the library Controllers.
+ *
+ * @class JointTargetBase
+ * @ingroup Library-Controllers
+ * @brief Brief description of class JointTargetBase.
+ *
+ * Detailed description of class JointTargetBase.
+ */
+ class JointTargetBase
+ {
+ public:
+ virtual ControlMode getType() const = 0;
+ };
+
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointTorqueTarget.h b/source/RobotAPI/libraries/Controllers/Targets/JointTorqueTarget.h
new file mode 100644
index 000000000..8b4ffad81
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/Targets/JointTorqueTarget.h
@@ -0,0 +1,61 @@
+/*
+ * 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::JointTorqueTarget
+ * @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_JointTorqueTarget_H
+#define _ARMARX_LIB_RobotAPI_JointTorqueTarget_H
+
+#include "JointTargetBase.h"
+
+namespace armarx
+{
+ /**
+ * @defgroup Library-Controllers Controllers
+ * @ingroup RobotAPI
+ * A description of the library Controllers.
+ *
+ * @class JointTorqueTarget
+ * @ingroup Library-Controllers
+ * @brief Brief description of class JointTorqueTarget.
+ *
+ * Detailed description of class JointTorqueTarget.
+ */
+ class JointTorqueTarget: public JointTargetBase
+ {
+ public:
+ float torque = ControllerConstatns::ValueNotSetNaN;
+ virtual ControlMode getType() const override
+ {
+ return eTorqueControl;
+ }
+ virtual void reset()
+ {
+ torque = ControllerConstatns::ValueNotSetNaN;
+ }
+ bool isValid() const
+ {
+ return std::isfinite(torque);
+ }
+ };
+
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/Controllers/Targets/JointVelocityTarget.h b/source/RobotAPI/libraries/Controllers/Targets/JointVelocityTarget.h
new file mode 100644
index 000000000..8f44b2c05
--- /dev/null
+++ b/source/RobotAPI/libraries/Controllers/Targets/JointVelocityTarget.h
@@ -0,0 +1,61 @@
+/*
+ * 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::JointVelocityTarget
+ * @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_JointVelocityTarget_H
+#define _ARMARX_LIB_RobotAPI_JointVelocityTarget_H
+
+#include "JointTargetBase.h"
+
+namespace armarx
+{
+ /**
+ * @defgroup Library-Controllers Controllers
+ * @ingroup RobotAPI
+ * A description of the library Controllers.
+ *
+ * @class JointVelocityTarget
+ * @ingroup Library-Controllers
+ * @brief Brief description of class JointVelocityTarget.
+ *
+ * Detailed description of class JointVelocityTarget.
+ */
+ class JointVelocityTarget: public JointTargetBase
+ {
+ public:
+ float velocity = ControllerConstatns::ValueNotSetNaN;
+ virtual ControlMode getType() const override
+ {
+ return eVelocityControl;
+ }
+ virtual void reset()
+ {
+ velocity = ControllerConstatns::ValueNotSetNaN;
+ }
+ bool isValid() const
+ {
+ return std::isfinite(velocity);
+ }
+ };
+
+}
+
+#endif
--
GitLab