Merge branch 'Ice3.7' of https://gitlab.com/ArmarX/RobotAPI into Ice3.7

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

@@ -252,7 +252,7 @@ namespace armarx
         return RobotUnitModule::Devices::Instance().getControlDevice(deviceName);
     }
 
-    void NJointController::useSynchronizedRtRobot(bool updateCollisionModel)
+    const VirtualRobot::RobotPtr& NJointController::useSynchronizedRtRobot(bool updateCollisionModel)
     {
         ARMARX_CHECK_EXPRESSION_W_HINT(
             NJointControllerRegistryEntry::ConstructorIsRunning(),
@@ -261,6 +261,7 @@ namespace armarx
         ARMARX_CHECK_IS_NULL_W_HINT(rtRobot, "useSynchronizedRtRobot was already called");
         rtRobot = RobotUnitModule::RobotData::Instance().cloneRobot(updateCollisionModel);
         rtRobotNodes = rtRobot->getRobotNodes();
+        return rtRobot;
     }
 
     void NJointController::onInitComponent()

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

@@ -643,7 +643,7 @@ namespace armarx
          * @param updateCollisionModel Whether the robot's collision model should be updated
          * @see rtGetRobot
          */
-        void useSynchronizedRtRobot(bool updateCollisionModel = false);
+        const VirtualRobot::RobotPtr& useSynchronizedRtRobot(bool updateCollisionModel = false);
 
         // //////////////////////////////////////////////////////////////////////////////////////// //
         // ////////////////////////////////// Component interface ///////////////////////////////// //
@@ -709,7 +709,7 @@ namespace armarx
             ARMARX_CHECK_EXPRESSION(getState() < eManagedIceObjectExiting);
             ARMARX_VERBOSE << "Adding NJointController task named '" << taskName << "' - current available thread count: " << getThreadPool()->getAvailableTaskCount();
             auto handlePtr = std::make_shared<ThreadPool::Handle>(getThreadPool()->runTask(task));
-            ARMARX_CHECK_EXPRESSION_W_HINT(handlePtr->isValid(), "Could not add task (" << taskName << " - " << typeid(task).name() << " ) - available threads: " << getThreadPool()->getAvailableTaskCount());
+            ARMARX_CHECK_EXPRESSION_W_HINT(handlePtr->isValid(), "Could not add task (" << taskName << " - " << GetTypeString(task) << " ) - available threads: " << getThreadPool()->getAvailableTaskCount());
             threadHandles[taskName] = handlePtr;
         }
         std::map<std::string, std::shared_ptr<ThreadPool::Handle>> threadHandles;

source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleBase.h

@@ -32,6 +32,7 @@
 
 #include <VirtualRobot/Robot.h>
 
+#include <ArmarXCore/util/CPPUtility/Pointer.h>
 #include <ArmarXCore/core/Component.h>
 
 #include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
@@ -50,15 +51,6 @@ namespace armarx
     TYPEDEF_PTRS_HANDLE(RobotUnit);
     TYPEDEF_PTRS_HANDLE(NJointController);
 
-    template<class T>
-    T& CheckedDeref(T* ptr)
-    {
-        if (!ptr)
-        {
-            throw std::invalid_argument {"Ptr passed to CheckedDeref is NULL"};
-        }
-        return *ptr;
-    }
     template<class Targ, class Src>
     Targ& CheckedCastAndDeref(Src* src)
     {

source/RobotAPI/components/units/RobotUnit/util/KeyValueVector.h

@@ -20,224 +20,5 @@
  *             GNU General Public License
  */
 #pragma once
+#include <ArmarXCore/util/CPPUtility/KeyValueVector.h>
 
-#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
-#include <vector>
-#include <map>
-
-namespace armarx
-{
-    /**
-     * @brief This class is pretty much similar to a map.
-     *
-     * This class stores keys and values in two vectors and uses a map to map the keys to the index of the corresponding value
-     * This enables map acces and index access.
-     * The index of an inserted element never changes (this is different to a map).
-     * Index access may be required in a high frequency case (e.g. an rt control loop)
-     */
-    template < class KeyT, class ValT,
-               class KeyContT = std::vector<KeyT>,
-               class ValContT = std::vector<ValT>,
-               class IdxT = typename ValContT::size_type,
-               class MapT = std::map<KeyT, IdxT >>
-    class KeyValueVector
-    {
-    public:
-        void            add(KeyT key, ValT value);
-        IdxT            index(const KeyT& k) const;
-        IdxT            size() const;
-
-        ValT&           at(IdxT i);
-        ValT     const& at(IdxT i) const;
-        ValT&           at(const KeyT& k);
-        ValT     const& at(const KeyT& k) const;
-        ValT&           at(const KeyT& k,       ValT& defaultVal);
-        ValT     const& at(const KeyT& k, const ValT& defaultVal) const;
-
-        ValContT const& values() const;
-        MapT     const& indices() const;
-        KeyContT const& keys() const;
-        IdxT            count(const KeyT& k) const;
-        bool            has(const KeyT& k) const;
-        void            clear();
-
-        using iterator       = typename ValContT::iterator;
-        using const_iterator = typename ValContT::const_iterator;
-
-        iterator begin();
-        iterator end();
-
-        const_iterator begin() const;
-        const_iterator end()   const;
-
-        const_iterator cbegin() const;
-        const_iterator cend()   const;
-
-
-    private:
-        template<class T, class = typename std::enable_if<meta::HasToString<T>::value>::type>
-        static std::string DescribeKey(const T* key);
-        static std::string DescribeKey(...);
-
-        MapT indices_;
-        KeyContT keys_;
-        ValContT values_;
-    };
-}
-
-// implementation
-namespace armarx
-{
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    void KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::add(KeyT key, ValT value)
-    {
-        if (indices_.find(key) != indices_.end())
-        {
-            throw std::invalid_argument {"Already added a value for this key" + DescribeKey(&key)};
-        }
-        indices_.emplace(key, size());
-        values_.emplace_back(std::move(value));
-        keys_.emplace_back(std::move(key));
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    IdxT KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::index(const KeyT& k) const
-    {
-        return indices_.at(k);
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    ValT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::at(IdxT i)
-    {
-        return values_.at(i);
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    const ValT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::at(IdxT i) const
-    {
-        return values_.at(i);
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    ValT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::at(const KeyT& k)
-    {
-        return at(index(k));
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    const ValT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::at(const KeyT& k) const
-    {
-        return at(index(k));
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    ValT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::at(const KeyT& k, ValT& defaultVal)
-    {
-        return has(k) ? at(index(k)) : defaultVal;
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    const ValT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::at(const KeyT& k, const ValT& defaultVal) const
-    {
-        return has(k) ? at(index(k)) : defaultVal;
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    IdxT KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::size() const
-    {
-        return values_.size();
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    const ValContT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::values() const
-    {
-        return values_;
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    const MapT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::indices() const
-    {
-        return indices_;
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    const KeyContT& KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::keys() const
-    {
-        return keys_;
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    IdxT KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::count(const KeyT& k) const
-    {
-        return indices_.count(k);
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    bool KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::has(const KeyT& k) const
-    {
-        return indices_.count(k);
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    void KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::clear()
-    {
-        indices_.clear();
-        keys_.clear();
-        values_.clear();
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    typename KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::iterator
-    KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::begin()
-    {
-        return values_.begin();
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    typename KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::iterator
-    KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::end()
-    {
-        return values_.end();
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    typename KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::const_iterator
-    KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::begin() const
-    {
-        return values_.cbegin();
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    typename KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::const_iterator
-    KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::end() const
-    {
-        return values_.cend();
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    typename KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::const_iterator
-    KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::cbegin() const
-    {
-        return begin();
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    typename KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::const_iterator
-    KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::cend() const
-    {
-        return end();
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    std::string KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::DescribeKey(...)
-    {
-        return "";
-    }
-
-    template < class KeyT, class ValT, class KeyContT, class ValContT, class IdxT,  class MapT>
-    template<class T, class>
-    std::string KeyValueVector<KeyT, ValT, KeyContT, ValContT, IdxT, MapT>::DescribeKey(const T* key)
-    {
-        return ": " + to_string(*key);
-    }
-}

source/RobotAPI/interface/CMakeLists.txt

@@ -18,6 +18,7 @@ set(SLICE_FILES
     core/RobotStateObserverInterface.ice
     core/Trajectory.ice
     core/CartesianSelection.ice
+    core/CartesianWaypointControllerConfig.ice
     core/CartesianPositionControllerConfig.ice
 
     selflocalisation/SelfLocalisationProcess.ice
@@ -59,7 +60,7 @@ set(SLICE_FILES
 
     units/RobotUnit/NJointJointSpaceDMPController.ice
     units/RobotUnit/NJointTaskSpaceDMPController.ice
-    units/RobotUnit/NJointBimanualForceMPController.ice    
+    units/RobotUnit/NJointBimanualForceMPController.ice
 
 	units/RobotUnit/DSControllerBase.ice
 
@@ -86,3 +87,6 @@ set(SLICE_FILES_ADDITIONAL_SOURCES
 
 # generate the interface library
 armarx_interfaces_generate_library(RobotAPI "${ROBOTAPI_INTERFACE_DEPEND}")
+
+target_link_libraries(RobotAPIInterfaces PUBLIC ArmarXCore)
+

source/RobotAPI/interface/core/CartesianWaypointControllerConfig.ice

+/*
+* 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    ArmarX::RobotAPI
+* @author     Raphael Grimm
+* @copyright  2019 Humanoids Group, H2T, KIT
+* @license    http://www.gnu.org/licenses/gpl-2.0.txt
+*             GNU General Public License
+*/
+
+#pragma once
+
+module armarx
+{
+    struct CartesianWaypointControllerConfig
+    {
+        float kpJointLimitAvoidance       = 1;
+        float jointLimitAvoidanceScale    = 2;
+
+        float thresholdOrientationNear    = 0.1;
+        float thresholdOrientationReached = 0.05;
+        float thresholdPositionNear       = 50;
+        float thresholdPositionReached    = 5;
+
+        float maxOriVel = 1;
+        float maxPosVel = 80;
+        float kpOri     = 1;
+        float kpPos     = 1;
+    };
+};
+
+

source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.cpp

@@ -35,7 +35,7 @@ CartesianVelocityControllerWithRamp::CartesianVelocityControllerWithRamp(const V
     setCurrentJointVelocity(currentJointVelocity);
 }
 
-void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::VectorXf& currentJointVelocity)
+void CartesianVelocityControllerWithRamp::setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
 {
     Eigen::MatrixXf jacobi = controller.ik->getJacobianMatrix(controller.tcp, mode);
     Eigen::MatrixXf inv = controller.ik->computePseudoInverseJacobianMatrix(jacobi, controller.ik->getJacobiRegularization(mode));

source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h

@@ -49,7 +49,7 @@ namespace armarx
         CartesianVelocityControllerWithRamp(CartesianVelocityControllerWithRamp&&) = default;
         CartesianVelocityControllerWithRamp& operator=(CartesianVelocityControllerWithRamp&&) = default;
 
-        void setCurrentJointVelocity(const Eigen::VectorXf& currentJointVelocity);
+        void setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity);
 
         Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, float jointLimitAvoidanceScale, float dt);
         Eigen::VectorXf calculate(const Eigen::VectorXf& cartesianVel, const Eigen::VectorXf& nullspaceVel, float dt);

source/RobotAPI/libraries/core/CartesianWaypointController.cpp

@@ -203,58 +203,19 @@ namespace armarx
         return ss.str();
     }
 
-    //    bool CartesianWaypointController::OptimizeNullspace(const VirtualRobot::RobotNodePtr& tcp, const VirtualRobot::RobotNodeSetPtr& rns, const Eigen::Matrix4f& target, const NullspaceOptimizationArgs& args)
-    //    {
-    //        CartesianPositionController posHelper(tcp);
-
-    //        CartesianVelocityController cartesianVelocityController{rns, tcp, args.invJacMethod};
-
-    //        const float errorOriInitial = posHelper.getOrientationError(target);
-    //        const float errorPosInitial = posHelper.getPositionError(target);
-
-    //        const float stepLength = args.stepLength;
-    //        const float eps = args.eps;
-
-    //        std::vector<float> initialJointAngles =  rns->getJointValues();
-
-    //        for (int i = 0; i < args.loops; i++)
-    //        {
-    //            Eigen::VectorXf tcpVelocities = posHelper.calculate(target, args.cartesianSelection);
-    //            Eigen::VectorXf nullspaceVel = cartesianVelocityController.calculateJointLimitAvoidance(); // calculateNullspaceVelocity(tcpVelocities, 1.0f, vrmode);
-    //            const float nullspaceLen = nullspaceVel.norm();
-    //            if (nullspaceLen > stepLength)
-    //            {
-    //                nullspaceVel = nullspaceVel / nullspaceLen * stepLength;
-    //            }
-    //            Eigen::VectorXf jointVelocities = cartesianVelocityController.calculate(tcpVelocities, nullspaceVel, args.cartesianSelection);
-
-    //            //jointVelocities = jointVelocities * stepLength;
-    //            float len = jointVelocities.norm();
-    //            if (len > stepLength)
-    //            {
-    //                jointVelocities = jointVelocities / len * stepLength;
-    //            }
+    void CartesianWaypointController::setConfig(const CartesianWaypointControllerConfig& cfg)
+    {
+        KpJointLimitAvoidance           = cfg.kpJointLimitAvoidance;
+        jointLimitAvoidanceScale        = cfg.jointLimitAvoidanceScale;
 
-    //            for (size_t n = 0; n < rns->getSize(); n++)
-    //            {
-    //                rns->getNode(n)->setJointValue(rns->getNode(n)->getJointValue() + jointVelocities(n));
-    //            }
-    //            if (len < eps)
-    //            {
-    //                break;
-    //            }
-    //        }
+        thresholdOrientationNear        = cfg.thresholdOrientationNear;
+        thresholdOrientationReached     = cfg.thresholdOrientationReached;
+        thresholdPositionNear           = cfg.thresholdPositionNear;
+        thresholdPositionReached        = cfg.thresholdPositionReached;
 
-    //        float errorOriAfter = posHelper.getOrientationError(target);
-    //        float errorPosAfter = posHelper.getPositionError(target);
-    //        if (errorOriAfter < errorOriInitial + 1.f / 180 * M_PI && errorPosAfter < errorPosInitial + 1.f)
-    //        {
-    //            return true;
-    //        }
-    //        else
-    //        {
-    //            rns->setJointValues(initialJointAngles);
-    //            return false;
-    //        }
-    //    }
+        ctrlCartesianPos2Vel.maxOriVel  = cfg.maxOriVel;
+        ctrlCartesianPos2Vel.maxPosVel  = cfg.maxPosVel;
+        ctrlCartesianPos2Vel.KpOri      = cfg.kpOri;
+        ctrlCartesianPos2Vel.KpPos      = cfg.kpPos;
+    }
 }

source/RobotAPI/libraries/core/CartesianWaypointController.h

@@ -31,6 +31,7 @@
 
 #include <RobotAPI/libraries/core/CartesianPositionController.h>
 #include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/interface/core/CartesianWaypointControllerConfig.h>
 
 #include "CartesianVelocityControllerWithRamp.h"
 
@@ -42,7 +43,6 @@ namespace Eigen
 
 namespace armarx
 {
-
     class CartesianWaypointController;
     using CartesianWaypointControllerPtr = std::shared_ptr<CartesianWaypointController>;
 
@@ -58,7 +58,7 @@ namespace armarx
                                    );
 
 
-        void setCurrentJointVelocity(const Eigen::VectorXf& currentJointVelocity)
+        void setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity)
         {
             ctrlCartesianVelWithRamps.setCurrentJointVelocity(currentJointVelocity);
         }
@@ -92,6 +92,8 @@ namespace armarx
 
         std::string getStatusText();
 
+        void setConfig(const CartesianWaypointControllerConfig& cfg);
+
         CartesianVelocityControllerWithRamp  ctrlCartesianVelWithRamps;
         CartesianPositionController ctrlCartesianPos2Vel;
 

source/RobotAPI/libraries/core/visualization/DebugDrawerTopic.h

@@ -548,13 +548,13 @@ namespace armarx
 
 
         // STATUS
-        
+
         /// Indicate whether a topic is set, i.e. visualization is enabled.
         bool enabled() const;
         /// Indicate whether a topic is set, i.e. visualization is enabled.
         operator bool() const;
-        
-        
+
+
         // OPERATORS
 
         /// Conversion operator to DebugDrawerInterfacePrx.