source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.cpp

@@ -129,8 +129,8 @@ namespace armarx
         }
         //visu
         {
-            _tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
-            _tripFakeRobotGP.commitWrite();
+            _tripRt2NonRtRobotGP.getWriteBuffer().setIdentity();
+            _tripRt2NonRtRobotGP.commitWrite();
         }
     }
 
@@ -281,6 +281,9 @@ namespace armarx
             }
         }
         _tripRt2NonRt.commitWrite();
+
+        _tripRt2NonRtRobotGP.getWriteBuffer() = _rtRobot->getGlobalPose();
+        _tripRt2NonRtRobotGP.commitWrite();
     }
 
     void
@@ -412,17 +415,13 @@ namespace armarx
     NJointCartesianWaypointController::setVisualizationRobotGlobalPose(const Eigen::Matrix4f& p,
                                                                        const Ice::Current&)
     {
-        std::lock_guard g{_tripFakeRobotGPWriteMutex};
-        _tripFakeRobotGP.getWriteBuffer() = p;
-        _tripFakeRobotGP.commitWrite();
+        ; // No longer used ...
     }
 
     void
     NJointCartesianWaypointController::resetVisualizationRobotGlobalPose(const Ice::Current&)
     {
-        std::lock_guard g{_tripFakeRobotGPWriteMutex};
-        _tripFakeRobotGP.getWriteBuffer()(0, 0) = std::nanf("");
-        _tripFakeRobotGP.commitWrite();
+        ; // No longer used ...
     }
 
     void
@@ -477,7 +476,7 @@ namespace armarx
             std::lock_guard g{_tripRt2NonRtMutex};
             const auto& buf = _tripRt2NonRt.getUpToDateReadBuffer();
 
-            const Eigen::Matrix4f fakeGP = _tripFakeRobotGP.getUpToDateReadBuffer();
+            const Eigen::Matrix4f fakeGP = _tripRt2NonRtRobotGP.getUpToDateReadBuffer();
             const Eigen::Matrix4f gp = std::isfinite(fakeGP(0, 0)) ? fakeGP : buf.rootPose;
 
             if (buf.tcp != buf.tcpTarg)

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianWaypointController.h

@@ -146,8 +146,7 @@ namespace armarx
         mutable std::recursive_mutex _tripRt2NonRtMutex;
         TripleBuffer<RtToNonRtData> _tripRt2NonRt;
 
-        mutable std::recursive_mutex _tripFakeRobotGPWriteMutex;
-        TripleBuffer<Eigen::Matrix4f> _tripFakeRobotGP;
+        TripleBuffer<Eigen::Matrix4f> _tripRt2NonRtRobotGP;
 
         //publish data
         std::atomic_size_t _publishWpsNum{0};

source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp

@@ -722,7 +722,7 @@ namespace armarx::RobotUnitModule
             for (std::size_t idxRobot = 0; idxRobot < nodes.size(); ++idxRobot)
             {
                 const VirtualRobot::RobotNodePtr& node = nodes.at(idxRobot);
-                if (node->isRotationalJoint() || node->isTranslationalJoint())
+                if (node->isJoint())
                 {
                     const auto& name = node->getName();
                     if (sensorDevices.has(name))

source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp

@@ -494,10 +494,13 @@ namespace armarx::RobotUnitModule
         ARMARX_TRACE;
         throwIfInControlThread(BOOST_CURRENT_FUNCTION);
         std::lock_guard<std::mutex> guard{rtLoggingMutex};
-        if (!rtDataStreamingEntry.count(receiver))
+        
+        if (rtDataStreamingEntry.count(receiver) == 0u)
         {
-            throw InvalidArgumentException{"stopDataStreaming called for a nonexistent log"};
+            ARMARX_INFO << "stopDataStreaming called for a nonexistent log";
+            return;
         }
+
         ARMARX_INFO_S << "RobotUnit: request to stop DataStreaming for " << receiver->ice_id();
         rtDataStreamingEntry.at(receiver).stopStreaming = true;
     }

source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleSelfCollisionChecker.cpp

@@ -21,12 +21,17 @@
  */
 
 #include "RobotUnitModuleSelfCollisionChecker.h"
+#include <algorithm>
+#include <cstddef>
+#include <string>
 
 #include <SimoxUtility/algorithm/string/string_tools.h>
 #include <VirtualRobot/CollisionDetection/CollisionChecker.h>
 #include <VirtualRobot/Obstacle.h>
 #include <VirtualRobot/RobotNodeSet.h>
 
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/time/Metronome.h"
 #include <ArmarXCore/core/util/OnScopeExit.h>
 
 #include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerBase.h>
@@ -219,6 +224,58 @@ namespace armarx::RobotUnitModule
                 node->getCollisionModel()->inflateModel(minSelfDistance / 2.f);
             }
         }
+
+        // Remove / filter collision pairs according to robot model (XML: Physics/IgnoreCollision)
+        {
+            ARMARX_VERBOSE << "Removing ignored collision pairs";
+            // introduce vector to remove elements "in-place" via remove-erase-if idiom (not possible for sets)
+            std::vector<std::pair<std::string, std::string>> validNamePairsToCheck(namePairsToCheck.begin(), namePairsToCheck.end());
+
+            const auto isCollisionIgnored = [this](const std::string& a, const std::string& b) -> bool {
+
+                if(a == FLOOR_OBJ_STR or b == FLOOR_OBJ_STR)
+                {
+                    return false;
+                }
+
+                const auto nodeA = selfCollisionAvoidanceRobot->getRobotNode(a);
+                const auto nodeB = selfCollisionAvoidanceRobot->getRobotNode(b);
+
+                if(nodeA == nullptr or nodeB == nullptr)
+                {
+                    return false;
+                }
+
+                const std::vector<std::string> nodesIgnoredByA = nodeA->getIgnoredCollisionModels();
+                const std::vector<std::string> nodesIgnoredByB = nodeB->getIgnoredCollisionModels();
+
+                if(std::find(nodesIgnoredByA.begin(), nodesIgnoredByA.end(), b) != nodesIgnoredByA.end())
+                {
+                    ARMARX_VERBOSE << "Ignoring collision between nodes: " << a << " -- " << b;
+                    return true;
+                }
+
+                if(std::find(nodesIgnoredByB.begin(), nodesIgnoredByB.end(), a) != nodesIgnoredByB.end())
+                {
+                    ARMARX_VERBOSE << "Ignoring collision between nodes: " << b << " -- " << a;
+                    return true;
+                }
+
+                return false;
+
+            };
+
+            validNamePairsToCheck.erase(std::remove_if(validNamePairsToCheck.begin(), validNamePairsToCheck.end(), [&isCollisionIgnored](const auto& p) -> bool {
+                const auto& [a, b] = p;
+                return isCollisionIgnored(a, b);
+            }), validNamePairsToCheck.end());
+
+            ARMARX_VERBOSE << "Removed " << (namePairsToCheck.size() - validNamePairsToCheck.size()) << " collision pairs.";
+
+            // copy over name pairs which should not be ignored
+            namePairsToCheck = std::set(validNamePairsToCheck.begin(), validNamePairsToCheck.end());
+        }
+
         //collect pairs
         for (const auto& pair : namePairsToCheck)
         {
@@ -227,14 +284,18 @@ namespace armarx::RobotUnitModule
                     ? floor->getSceneObject(0)
                     : selfCollisionAvoidanceRobot->getRobotNode(pair.first);
 
+            ARMARX_CHECK_NOT_NULL(first) << pair.first;
+
             VirtualRobot::SceneObjectPtr second =
                 (pair.second == FLOOR_OBJ_STR)
                     ? floor->getSceneObject(0)
                     : selfCollisionAvoidanceRobot->getRobotNode(pair.second);
 
+            ARMARX_CHECK_NOT_NULL(second) << pair.second;
+
             nodePairsToCheck.emplace_back(first, second);
         }
-        ARMARX_CHECK_EQUAL(nodePairsToCheck.size(), nodePairsToCheck.size());
+        ARMARX_CHECK_EQUAL(nodePairsToCheck.size(), namePairsToCheck.size());
     }
 
     void
@@ -303,21 +364,23 @@ namespace armarx::RobotUnitModule
         };
         while (true)
         {
-            const auto startT = std::chrono::high_resolution_clock::now();
+            const auto freq = checkFrequency.load();
+
+            core::time::Metronome metronome(Frequency::Hertz(freq));
+
             //done
             if (isShuttingDown())
             {
                 return;
             }
-            const auto freq = checkFrequency.load();
             const bool inEmergencyStop =
                 _module<ControlThread>().getEmergencyStopState() == eEmergencyStopActive;
             if (inEmergencyStop || freq == 0)
             {
-                ARMARX_INFO << deactivateSpam() << "Self collision checker: skipping check "
+                ARMARX_VERBOSE << deactivateSpam() << "Self collision checker: skipping check "
                             << VAROUT(freq) << " " << VAROUT(inEmergencyStop);
                 //currently wait
-                std::this_thread::sleep_for(std::chrono::microseconds{1000});
+                std::this_thread::sleep_for(std::chrono::microseconds{1'000});
                 continue;
             }
             //update robot + check
@@ -332,6 +395,7 @@ namespace armarx::RobotUnitModule
                     bool allJoints0 = true;
                     for (const auto& node : selfCollisionAvoidanceRobotNodes)
                     {
+                        ARMARX_CHECK_NOT_NULL(node);
                         if (0 != node->getJointValue())
                         {
                             allJoints0 = false;
@@ -348,6 +412,10 @@ namespace armarx::RobotUnitModule
                 for (std::size_t idx = 0; idx < nodePairsToCheck.size(); ++idx)
                 {
                     const auto& pair = nodePairsToCheck.at(idx);
+
+                    ARMARX_CHECK_NOT_NULL(pair.first);
+                    ARMARX_CHECK_NOT_NULL(pair.second);
+
                     if (selfCollisionAvoidanceRobot->getCollisionChecker()->checkCollision(
                             pair.first, pair.second))
                     {
@@ -370,9 +438,16 @@ namespace armarx::RobotUnitModule
                                    << nodePairsToCheck.size() << " pairs";
                 }
             }
+
             //sleep remaining
-            std::this_thread::sleep_until(
-                startT + std::chrono::microseconds{static_cast<int64_t>(1000000 / freq)});
+            const auto duration = metronome.waitForNextTick();
+
+            if(not duration.isPositive())
+            {
+                ARMARX_WARNING << deactivateSpam(10) << 
+                    "Self collision checking took too long. "
+                    "Exceeding time budget by " << duration.toMilliSecondsDouble() << "ms.";
+            }
         }
     }
 

source/RobotAPI/drivers/GamepadUnit/GamepadUnit.cpp

@@ -33,6 +33,7 @@ void GamepadUnit::onInitComponent()
     ARMARX_TRACE;
     offeringTopic(getProperty<std::string>("GamepadTopicName").getValue());
     deviceName = getProperty<std::string>("GamepadDeviceName").getValue();
+    deviceEventName = getProperty<std::string>("GamepadForceFeedbackName").getValue();
     readTask = new RunningTask<GamepadUnit>(this, &GamepadUnit::run, "GamepadUnit");
 }
 
@@ -69,15 +70,23 @@ void GamepadUnit::onConnectComponent()
             ARMARX_INFO << deactivateSpam(100000, std::to_string(dataTimestamp->getTimestamp())) << "No new signal from gamepad for " << age.toMilliSecondsDouble() << " milliseconds. Not sending data. Timeout: " <<  getProperty<int>("PublishTimeout").getValue() << " ms";
         }
     }, 30);
+    
     sendTask->start();
     ARMARX_TRACE;
     openGamepadConnection();
 }
 
+void GamepadUnit::vibrate(const ::Ice::Current&)
+{
+    ARMARX_INFO << "vibration!";
+    js.executeEffect();
+}
+
 bool GamepadUnit::openGamepadConnection()
 {
-    if (js.open(deviceName))
+    if (js.open(deviceName, deviceEventName))
     {
+
         ARMARX_TRACE;
         ARMARX_INFO << "opened a gamepad named " << js.name << " with " << js.numberOfAxis << " axis and " << js.numberOfButtons << " buttons.";
         if (js.numberOfAxis == 8 && js.numberOfButtons == 11)
@@ -185,4 +194,3 @@ armarx::PropertyDefinitionsPtr GamepadUnit::createPropertyDefinitions()
     return armarx::PropertyDefinitionsPtr(new GamepadUnitPropertyDefinitions(
             getConfigIdentifier()));
 }
-

source/RobotAPI/drivers/GamepadUnit/GamepadUnit.h

@@ -53,6 +53,7 @@ namespace armarx
             //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
             defineOptionalProperty<std::string>("GamepadTopicName", "GamepadValues", "Name of the Gamepad Topic");
             defineOptionalProperty<std::string>("GamepadDeviceName", "/dev/input/js2", "device that will be opened as a gamepad");
+            defineOptionalProperty<std::string>("GamepadForceFeedbackName", "", "device that will be used for force feedback, leave empty to disable. See RobotAPI/source/RobotAPI/drivers/GamepadUnit/README.md for more details.");
             defineOptionalProperty<int>("PublishTimeout", 2000, "In Milliseconds. Timeout after which the gamepad data is not published after, if no new data was read from the gamepad");
         }
     };
@@ -69,7 +70,8 @@ namespace armarx
      * Detailed description of class GamepadUnit.
      */
     class GamepadUnit :
-        virtual public armarx::Component
+        virtual public armarx::Component,
+        virtual public GamepadUnitInterface
     {
     public:
         /**
@@ -108,6 +110,8 @@ namespace armarx
 
         bool openGamepadConnection();
 
+        void vibrate(const ::Ice::Current& = ::Ice::emptyCurrent) override;
+
     private:
         GamepadUnitListenerPrx topicPrx;
         RunningTask<GamepadUnit>::pointer_type readTask;
@@ -116,9 +120,9 @@ namespace armarx
         void run();
         std::mutex mutex;
         std::string deviceName;
+        std::string deviceEventName;
         Joystick js;
         GamepadData data;
         TimestampVariantPtr dataTimestamp;
     };
 }
-

source/RobotAPI/drivers/GamepadUnit/Joystick.h

@@ -22,12 +22,17 @@
 
 #pragma once
 
-#include<linux/joystick.h>
-#include<sys/stat.h>
-#include<fcntl.h>
+#include <cstdint>
+
+#include <fcntl.h>
+#include <sys/poll.h>
+#include <sys/stat.h>
+#include <unistd.h>
 
 #include <ArmarXCore/core/Component.h>
 
+#include <linux/joystick.h>
+
 namespace armarx
 {
 
@@ -36,21 +41,39 @@ namespace armarx
 
     private:
         int fd = -1;
+        int fdEvent = -1;
         js_event event;
 
     public:
-
         std::vector<int16_t> axis;
         std::vector<bool> buttonsPressed;
         int numberOfAxis;
         int numberOfButtons;
         std::string name;
 
-        bool open(std::string const& deviceName)
+        bool
+        open(std::string const& deviceName, std::string const& deviceEventName)
         {
+
             fd = ::open(deviceName.c_str(), O_RDONLY);
+
+            if (!deviceEventName.empty())
+            {
+                ARMARX_INFO << "Force feedback enabled";
+                fdEvent = ::open(deviceEventName.c_str(), O_RDWR | O_CLOEXEC);
+
+                ARMARX_CHECK(fdEvent != -1);
+            } else {
+                ARMARX_INFO << "Force feedback disabled";
+            }
+
             if (fd != -1)
             {
+                // ARMARX_INFO << "before";
+                // executeEffect();
+                // ARMARX_INFO << "after";
+
+
                 ioctl(fd, JSIOCGAXES, &numberOfAxis);
                 ioctl(fd, JSIOCGBUTTONS, &numberOfButtons);
                 name.resize(255);
@@ -59,15 +82,21 @@ namespace armarx
                 name = name.c_str();
                 buttonsPressed.resize(numberOfButtons, false);
             }
+
+            ARMARX_INFO << "execute effect";
+            executeEffect();
+
             return fd != -1;
         }
 
-        bool opened() const
+        bool
+        opened() const
         {
             return fd != -1;
         }
 
-        bool pollEvent()
+        bool
+        pollEvent()
         {
             int bytes = read(fd, &event, sizeof(event));
 
@@ -89,10 +118,165 @@ namespace armarx
             return true;
         }
 
-        void close()
+        void
+        executeEffect(int gain = 100, const int nTimes = 1)
+        {
+            // this feature is disabled
+            if(fdEvent < 0) return;
+
+            // see https://docs.kernel.org/input/ff.html
+
+
+            // https://xnux.eu/devices/feature/vibrator.html
+
+            int ret;
+            // pollfd pfds[1];
+            int effects;
+
+            // fd = open_event_dev("vibrator", O_RDWR | O_CLOEXEC);
+            // syscall_error(fd < 0, "Can't open vibrator event device");
+
+            ret = ioctl(fdEvent, EVIOCGEFFECTS, &effects);
+            ARMARX_CHECK(ret >= 0);
+            // syscall_error(ret < 0, "EVIOCGEFFECTS failed");
+
+            // ARMARX_CHECK(effects & FF_RUMBLE);
+
+            // Set the gain of the device
+            {
+                // int gain; between 0 and 100
+                struct input_event ie; // structure used to communicate with the driver
+
+                ie.type = EV_FF;
+                ie.code = FF_GAIN;
+                ie.value = 0xFFFFUL * gain / 100;
+
+                if (write(fdEvent, &ie, sizeof(ie)) == -1)
+                {
+                    perror("set gain");
+                }
+            }
+
+
+            ff_effect e;
+            // e.type = FF_RUMBLE;
+            // e.id = -1;
+            // e.replay.length = 5000;
+            // e.replay.delay = 500;
+            // e.u.rumble.strong_magnitude = 1;
+
+            e.type = FF_PERIODIC;
+            e.id = -1;
+            e.replay.length = 5000;
+            e.replay.delay = 500;
+            e.u.periodic.waveform = FF_SQUARE;
+            e.u.periodic.period = 1000;
+            e.u.periodic.magnitude = 0xFF;
+            e.u.periodic.offset = 0xFF;
+
+            ret = ioctl(fdEvent, EVIOCSFF, &e);
+            ARMARX_CHECK(ret >= 0);
+
+            // syscall_error(ret < 0, "EVIOCSFF failed");
+
+            ARMARX_INFO << VAROUT(e.id);
+
+            input_event play;
+            play.type = EV_FF;
+            play.code = static_cast<std::uint16_t>(e.id);
+            play.value = 1;
+
+            ret = write(fdEvent, &play, sizeof(play));
+            ARMARX_CHECK(ret >= 0);
+
+            ARMARX_INFO << "Executing effect";
+
+
+            for (int i = 0; i < 5; i++)
+            {
+                
+                input_event statusIe; // structure used to communicate with the driver
+                statusIe.type = EV_FF_STATUS;
+                statusIe.code = e.id;
+                // statusIe.value = 0;
+
+                ret = write(fdEvent, &statusIe, sizeof(statusIe));
+
+                // ARMARX_CHECK()
+                // ret should be FF_STATUS_PLAYING
+                ARMARX_INFO << VAROUT(ret);
+
+                sleep(1);
+            }
+
+
+            // syscall_error(ret < 0, "write failed");
+
+            ARMARX_INFO << "Executing effect";
+            // sleep(6);
+
+
+            input_event stop;
+            stop.type = EV_FF;
+            stop.code = e.id;
+            stop.value = 0;
+            const int stopStatus = write(fdEvent, static_cast<const void*>(&stop), sizeof(stop));
+
+
+            ret = ioctl(fdEvent, EVIOCRMFF, e.id);
+            ARMARX_CHECK(ret >= 0);
+
+            // syscall_error(ret < 0, "EVIOCRMFF failed");
+
+            // close(fdEvent);
+
+
+            /**/
+            // Set the gain of the device
+            // {
+            //     // int gain; between 0 and 100
+            //     struct input_event ie; // structure used to communicate with the driver
+
+            //     ie.type = EV_FF;
+            //     ie.code = FF_GAIN;
+            //     ie.value = 0xFFFFUL * gain / 100;
+
+            //     if (write(fd, &ie, sizeof(ie)) == -1)
+            //     {
+            //         perror("set gain");
+            //     }
+            // }
+
+
+            // struct input_event play;
+            // struct input_event stop;
+
+            // // upload request to device
+            // ff_effect effect;
+            // const auto uploadStatus = ioctl(fd, EVIOCSFF, &effect);
+
+            // // Play n times
+            // play.type = EV_FF;
+            // play.code = effect.id;
+            // play.value = nTimes;
+
+            // const int playStatus = write(fd, static_cast<const void*>(&play), sizeof(play));
+
+            // // Stop an effect
+            // stop.type = EV_FF;
+            // stop.code = effect.id;
+            // stop.value = 0;
+            // const int stopStatus = write(fd, static_cast<const void*>(&stop), sizeof(stop));
+
+            // // remove effect
+            // ioctl(fd, EVIOCRMFF, effect.id);
+        }
+
+        void
+        close()
         {
             ::close(fd);
             fd = -1;
         }
     };
-}
+} // namespace armarx

source/RobotAPI/gui-plugins/HandUnitPlugin/HandUnitConfigDialog.cpp

@@ -36,11 +36,11 @@ armarx::HandUnitConfigDialog::HandUnitConfigDialog(QWidget* parent) :
 
 
     proxyFinderLeftHand = new IceProxyFinder<HandUnitInterfacePrx>(this);
-    proxyFinderLeftHand->setSearchMask("*Unit");
+    proxyFinderLeftHand->setSearchMask("*LeftHandUnit");
     ui->proxyFinderContainerLeftHand->addWidget(proxyFinderLeftHand, 0, 0, 1, 1);
 
     proxyFinderRightHand = new IceProxyFinder<HandUnitInterfacePrx>(this);
-    proxyFinderRightHand->setSearchMask("*Unit");
+    proxyFinderRightHand->setSearchMask("*RightHandUnit");
     ui->proxyFinderContainerRightHand->addWidget(proxyFinderRightHand, 0, 0, 1, 1);
 }
 

source/RobotAPI/interface/units/GamepadUnit.ice

@@ -62,8 +62,9 @@ module armarx
         bool rightStickButton;
 
     };
-    interface GamepadUnitInterface extends armarx::SensorActorUnitInterface
+    interface GamepadUnitInterface // extends armarx::SensorActorUnitInterface
     {
+        void vibrate();
     };
 
     interface GamepadUnitListener
@@ -78,4 +79,3 @@ module armarx
     };
 
 };
-