added CartesianPositionControllerConfig Variant

data/RobotAPI/VariantInfo-RobotAPI.xml

@@ -13,6 +13,7 @@
         <Variant baseType="::armarx::LinkedDirectionBase" dataType="::armarx::LinkedDirection" humanName="LinkedDirection" include="RobotAPI/libraries/core/LinkedPose.h" />
         <Variant baseType="::armarx::OrientedPointBase" dataType="::armarx::OrientedPoint" humanName="OrientedPoint" include="RobotAPI/libraries/core/OrientedPoint.h" />
         <Variant baseType="::armarx::FramedOrientedPointBase" dataType="::armarx::FramedOrientedPoint" humanName="FramedOrientedPoint" include="RobotAPI/libraries/core/FramedOrientedPoint.h" />
+        <Variant baseType="::armarx::CartesianPositionControllerConfigBase" dataType="::armarx::CartesianPositionControllerConfig" humanName="CartesianPositionControllerConfig" include="RobotAPI/libraries/core/CartesianPositionController.h" />
         <Variant baseType="::armarx::TrajectoryBase" dataType="::armarx::Trajectory" humanName="Trajectory"  include="RobotAPI/libraries/core/Trajectory.h"/>
     </Lib>
     <Lib name="RobotAPIInterfaces">

source/RobotAPI/interface/CMakeLists.txt

@@ -18,6 +18,7 @@ set(SLICE_FILES
     core/RobotStateObserverInterface.ice
     core/Trajectory.ice
     core/CartesianSelection.ice
+    core/CartesianPositionControllerConfig.ice
 
     selflocalisation/SelfLocalisationProcess.ice
 

source/RobotAPI/interface/core/CartesianPositionControllerConfig.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     Martin Miller
+* @copyright  2015 Humanoids Group, KIT
+* @license    http://www.gnu.org/licenses/gpl-2.0.txt
+*             GNU General Public License
+*/
+
+#pragma once
+
+#include <ArmarXCore/interface/observers/VariantBase.ice>
+#include <ArmarXCore/interface/observers/Filters.ice>
+
+module armarx
+{
+    /**
+    * [CartesianPositionControllerConfigBase] defines the config for CartesianPositionController
+    */
+    class CartesianPositionControllerConfigBase extends VariantDataClass
+    {
+        float KpPos = 1;
+        float KpOri = 1;
+        float maxPosVel = -1;
+        float maxOriVel = -1;
+
+        float thresholdOrientationNear = -1;
+        float thresholdOrientationReached = -1;
+        float thresholdPositionNear = -1;
+        float thresholdPositionReached = -1;
+    };
+
+
+
+};
+

source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.cpp

@@ -51,6 +51,18 @@ PositionControllerHelper::PositionControllerHelper(const VirtualRobot::RobotNode
     }
 }
 
+void PositionControllerHelper::readConfig(const CartesianPositionControllerConfigBasePtr& config)
+{
+    thresholdOrientationNear = config->thresholdOrientationNear;
+    thresholdOrientationReached = config->thresholdOrientationReached;
+    thresholdPositionNear = config->thresholdPositionNear;
+    thresholdPositionReached = config->thresholdPositionReached;
+    posController.KpOri = config->KpOri;
+    posController.KpPos = config->KpPos;
+    posController.maxOriVel = config->maxOriVel;
+    posController.maxPosVel = config->maxPosVel;
+}
+
 void PositionControllerHelper::update()
 {
     updateRead();
@@ -122,7 +134,7 @@ void PositionControllerHelper::clearFeedForwardVelocity()
 void PositionControllerHelper::immediateHardStop(bool clearTargets)
 {
     velocityControllerHelper->controller->immediateHardStop();
-    if(clearTargets)
+    if (clearTargets)
     {
         setTarget(posController.getTcp()->getPoseInRootFrame());
     }

source/RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h

@@ -53,6 +53,8 @@ namespace armarx
         PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<Eigen::Matrix4f>& waypoints);
         PositionControllerHelper(const VirtualRobot::RobotNodePtr& tcp, const VelocityControllerHelperPtr& velocityControllerHelper, const std::vector<PosePtr>& waypoints);
 
+        void readConfig(const CartesianPositionControllerConfigBasePtr& config);
+
         // read data and write targets, call this if you are unsure, anywhere in your control loop.
         // use updateRead and updateWrite for better performance
         void update();

source/RobotAPI/libraries/core/CartesianPositionController.cpp

@@ -105,3 +105,59 @@ VirtualRobot::RobotNodePtr CartesianPositionController::getTcp() const
 {
     return tcp;
 }
+
+
+
+#define SS_OUT(x) ss << #x << " = " << x << "\n"
+#define SET_FLT(x) obj->setFloat(#x, x)
+#define GET_FLT(x) x = obj->getFloat(#x);
+
+CartesianPositionControllerConfig::CartesianPositionControllerConfig()
+{
+}
+
+std::string CartesianPositionControllerConfig::output(const Ice::Current& c) const
+{
+    std::stringstream ss;
+
+    SS_OUT(KpPos);
+    SS_OUT(KpOri);
+    SS_OUT(maxPosVel);
+    SS_OUT(maxOriVel);
+    SS_OUT(thresholdOrientationNear);
+    SS_OUT(thresholdOrientationReached);
+    SS_OUT(thresholdPositionNear);
+    SS_OUT(thresholdPositionReached);
+
+    return ss.str();
+}
+
+void CartesianPositionControllerConfig::serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
+{
+    AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+    SET_FLT(KpPos);
+    SET_FLT(KpOri);
+    SET_FLT(maxPosVel);
+    SET_FLT(maxOriVel);
+    SET_FLT(thresholdOrientationNear);
+    SET_FLT(thresholdOrientationReached);
+    SET_FLT(thresholdPositionNear);
+    SET_FLT(thresholdPositionReached);
+
+}
+
+void CartesianPositionControllerConfig::deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
+{
+    AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
+
+    GET_FLT(KpPos);
+    GET_FLT(KpOri);
+    GET_FLT(maxPosVel);
+    GET_FLT(maxOriVel);
+    GET_FLT(thresholdOrientationNear);
+    GET_FLT(thresholdOrientationReached);
+    GET_FLT(thresholdPositionNear);
+    GET_FLT(thresholdPositionReached);
+
+}

source/RobotAPI/libraries/core/CartesianPositionController.h

@@ -30,6 +30,10 @@
 #include <VirtualRobot/Robot.h>
 #include <Eigen/Dense>
 
+#include <RobotAPI/interface/core/CartesianPositionControllerConfig.h>
+#include <ArmarXCore/observers/variant/Variant.h>
+#include <ArmarXCore/observers/AbstractObjectSerializer.h>
+
 namespace armarx
 {
     class CartesianPositionController;
@@ -58,5 +62,50 @@ namespace armarx
     private:
         VirtualRobot::RobotNodePtr tcp;
     };
+
+    namespace VariantType
+    {
+        // variant types
+        const VariantTypeId CartesianPositionControllerConfig = Variant::addTypeName("::armarx::CartesianPositionControllerConfig");
+    }
+
+    class CartesianPositionControllerConfig : virtual public CartesianPositionControllerConfigBase
+    {
+    public:
+        CartesianPositionControllerConfig();
+
+
+        // inherited from VariantDataClass
+        Ice::ObjectPtr ice_clone() const override
+        {
+            return this->clone();
+        }
+        VariantDataClassPtr clone(const Ice::Current& = ::Ice::Current()) const override
+        {
+            return new CartesianPositionControllerConfig(*this);
+        }
+        std::string output(const Ice::Current& c = ::Ice::Current()) const override;
+        VariantTypeId getType(const Ice::Current& = ::Ice::Current()) const override
+        {
+            return VariantType::CartesianPositionControllerConfig;
+        }
+        bool validate(const Ice::Current& = ::Ice::Current()) override
+        {
+            return true;
+        }
+
+        friend std::ostream& operator<<(std::ostream& stream, const CartesianPositionControllerConfig& rhs)
+        {
+            stream << "CartesianPositionControllerConfig: " << std::endl << rhs.output() << std::endl;
+            return stream;
+        }
+
+    public: // serialization
+        void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const override;
+        void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) override;
+
+    };
+
+    typedef IceInternal::Handle<CartesianPositionControllerConfig> CartesianPositionControllerConfigPtr;
 }
 

source/RobotAPI/libraries/core/RobotAPIObjectFactories.cpp

@@ -36,6 +36,7 @@
 #include <RobotAPI/libraries/core/observerfilters/MedianDerivativeFilterV3.h>
 #include <RobotAPI/libraries/core/OrientedPoint.h>
 #include <RobotAPI/libraries/core/FramedOrientedPoint.h>
+#include <RobotAPI/libraries/core/CartesianPositionController.h>
 
 
 using namespace armarx;
@@ -58,6 +59,7 @@ ObjectFactoryMap RobotAPIObjectFactories::getFactories()
 
     add<armarx::OrientedPointBase, armarx::OrientedPoint>(map);
     add<armarx::FramedOrientedPointBase, armarx::FramedOrientedPoint>(map);
+    add<armarx::CartesianPositionControllerConfigBase, armarx::CartesianPositionControllerConfig>(map);
 
     add<armarx::PoseMedianFilterBase, armarx::filters::PoseMedianFilter>(map);
     add<armarx::PoseAverageFilterBase, armarx::filters::PoseAverageFilter>(map);