Merge remote-tracking branch 'origin/master' into feature/armem_robot_state_update

source/RobotAPI/components/ArViz/CMakeLists.txt

@@ -16,6 +16,7 @@ set(SOURCES
     Client/elements/Robot.cpp
     Client/elements/RobotHand.cpp
     Client/drawer/ArVizDrawerBase.cpp
+    Client/ScopedClient.cpp
 
     Coin/ElementVisualizer.cpp
 
@@ -65,6 +66,7 @@ set(HEADERS
     Client/Layer.h
     Client/Elements.h
     Client/Client.h
+    Client/ScopedClient.h
     Client/ClientCGALExtensions.h
     Client/Color.h
 

source/RobotAPI/components/ArViz/Client/Client.h

@@ -51,7 +51,7 @@ namespace armarx::viz
 
         // ////////////////////////////////////////////////////////////////// //
         //layer
-        Layer layer(std::string const& name) const
+        virtual Layer layer(std::string const& name) const
         {
             return Layer(componentName, name);
         }

source/RobotAPI/components/ArViz/Client/Elements.h

@@ -122,6 +122,13 @@ namespace armarx::viz
         )
             : Box(name, b)
         {}
+
+        Box(std::string const& id)
+            : ElementOps(id)
+        {
+            pose(Eigen::Matrix4f::Identity());
+        }
+
     };
 
 

source/RobotAPI/components/ArViz/Client/ScopedClient.cpp

+/*
+ * 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/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+
+#include "ScopedClient.h"
+
+namespace armarx::viz
+{
+
+    Layer ScopedClient::layer(std::string const& name) const
+    {
+        layers.insert(name);
+
+        return Client::layer(name);
+    }
+
+    ScopedClient::~ScopedClient()
+    {
+        for (const auto& layer : layers)
+        {
+            Client::commitDeleteLayer(layer);
+        }
+    }
+
+}  // namespace armarx::viz

source/RobotAPI/components/ArViz/Client/ScopedClient.h

+/*
+ * 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/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include <set>
+
+#include "Client.h"
+
+namespace armarx::viz
+{
+    /**
+     * @brief `viz::Client` that will delete (clear) committed layers when destroyed.
+     *
+     * Note that, as a consequence, a network call be performed in the destructor.
+     *
+     * This might be useful if you have a class `MyTask` perform visualizing while performing some task,
+     * but whose visualization should be removed when the task finished.
+     * In this case, `MyTask` can have a `viz::ScopedClient` (which can be created from a regular `viz::Client`).
+     * When destructing the instance of `MyTask`, all visualization done by `MyTask` will be cleared
+     * (as `viz::ScopedClient` will go out of scope as well).
+     *
+     */
+    class ScopedClient: virtual public Client
+    {
+    public:
+        using Client::Client;
+
+        Layer layer(std::string const& name) const override;
+
+        virtual ~ScopedClient();
+
+    private:
+        mutable std::set<std::string> layers;
+    };
+}  // namespace armarx::viz
\ No newline at end of file

source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp

@@ -57,7 +57,7 @@ namespace armarx
         controller->activateController();
     }
 
-    void VelocityControllerHelper::setTargetVelocity(const Eigen::VectorXf& cv)
+    void VelocityControllerHelper::setTargetVelocity(const Eigen::Vector6f& cv)
     {
         controller->setTargetVelocity(cv(0), cv(1), cv(2), cv(3), cv(4), cv(5));
     }

source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.h

@@ -43,7 +43,7 @@ namespace armarx
 
         void init();
 
-        void setTargetVelocity(const Eigen::VectorXf& cv);
+        void setTargetVelocity(const Eigen::Vector6f& cv);
 
         void setNullSpaceControl(bool enabled);
 

source/RobotAPI/libraries/armem/util/util.h

@@ -21,10 +21,11 @@
 
 #pragma once
 
-#include "ArmarXCore/core/logging/Logging.h"
 #include <vector>
 #include <optional>
 
+#include "ArmarXCore/core/logging/Logging.h"
+
 #include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
 #include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
 #include <RobotAPI/libraries/aron/core/codegenerator/codeWriter/cpp/AronCppClass.h>

source/RobotAPI/libraries/core/CartesianVelocityControllerWithRamp.h

@@ -49,7 +49,7 @@ namespace armarx
         CartesianVelocityControllerWithRamp(CartesianVelocityControllerWithRamp&&) = default;
         CartesianVelocityControllerWithRamp& operator=(CartesianVelocityControllerWithRamp&&) = default;
 
-        [[deprecated("compued null space velocity does not match pseudo inverse svd method in simox. never use this function.")]]
+        [[deprecated("computed null space velocity does not match pseudo inverse svd method in simox. never use this function.")]]
         void setCurrentJointVelocity(const Eigen::Ref<const Eigen::VectorXf>& currentJointVelocity);
 
         void switchMode(const Eigen::VectorXf& currentJointVelocity, VirtualRobot::IKSolver::CartesianSelection mode);