fixed name issue of robotik and cvc

data/RobotAPI/VariantInfo-RobotAPI.xml

@@ -99,6 +99,14 @@
             propertyName="HapticUnitObserverName"
             propertyIsOptional="true"
             propertyDefaultValue="HapticUnitObserver" />
+        <Proxy include="RobotAPI/interface/units/HandUnitInterface.h"
+			humanName="Hand Unit "
+            typeName="HandUnitInterfacePrx"
+            memberName="handUnit"
+            getterName="getHandUnit"
+            propertyName="handUnitName"
+            propertyIsOptional="true"
+            propertyDefaultValue="HandUnit" />
         <Proxy include="RobotAPI/interface/units/WeissHapticUnit.h"
 			humanName="Weiss Haptic Unit"
             typeName="WeissHapticUnitInterfacePrx"

source/RobotAPI/components/units/RobotUnit/CMakeLists.txt

@@ -115,6 +115,18 @@ set(LIB_HEADERS
     Devices/RTThreadTimingsSensorDevice.h
 )
 
+find_package(DMP QUIET)
+if (DMP_FOUND)
+    list(APPEND LIB_HEADERS NJointControllers/NJointDMPCartesianVelocityController.h)
+    list(APPEND LIB_FILES NJointControllers/NJointDMPCartesianVelocityController.cpp)
+    list(APPEND LIBS ${DMP_LIBRARIES})
+    include_directories(${DMP_INCLUDE_DIRS})
+    link_directories(${DMP_LIB_DIRS})
+endif()
+
+
+
+
 add_subdirectory(test)
 
 armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")

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

@@ -264,35 +264,35 @@ namespace armarx
         return (VirtualRobot::IKSolver::CartesianSelection)0;
     }
 
-    NJointCartesianVelocityControllerCartesianSelection NJointCartesianVelocityController::IceModeFromString(const std::string mode)
+    NJointCartesianVelocityControllerMode::CartesianSelection NJointCartesianVelocityController::IceModeFromString(const std::string mode)
     {
         if (mode == "Position")
         {
-            return NJointCartesianVelocityControllerCartesianSelection::ePosition;
+            return NJointCartesianVelocityControllerMode::CartesianSelection::ePosition;
         }
         if (mode == "Orientation")
         {
-            return NJointCartesianVelocityControllerCartesianSelection::eOrientation;
+            return NJointCartesianVelocityControllerMode::CartesianSelection::eOrientation;
         }
         if (mode == "Both")
         {
-            return NJointCartesianVelocityControllerCartesianSelection::eAll;
+            return NJointCartesianVelocityControllerMode::CartesianSelection::eAll;
         }
         ARMARX_ERROR_S << "invalid mode " << mode;
-        return (NJointCartesianVelocityControllerCartesianSelection)0;
+        return (NJointCartesianVelocityControllerMode::CartesianSelection)0;
     }
 
-    VirtualRobot::IKSolver::CartesianSelection NJointCartesianVelocityController::ModeFromIce(const NJointCartesianVelocityControllerCartesianSelection mode)
+    VirtualRobot::IKSolver::CartesianSelection NJointCartesianVelocityController::ModeFromIce(const NJointCartesianVelocityControllerMode::CartesianSelection mode)
     {
-        if (mode == NJointCartesianVelocityControllerCartesianSelection::ePosition)
+        if (mode == NJointCartesianVelocityControllerMode::CartesianSelection::ePosition)
         {
             return VirtualRobot::IKSolver::CartesianSelection::Position;
         }
-        if (mode == NJointCartesianVelocityControllerCartesianSelection::eOrientation)
+        if (mode == NJointCartesianVelocityControllerMode::CartesianSelection::eOrientation)
         {
             return VirtualRobot::IKSolver::CartesianSelection::Orientation;
         }
-        if (mode == NJointCartesianVelocityControllerCartesianSelection::eAll)
+        if (mode == NJointCartesianVelocityControllerMode::CartesianSelection::eAll)
         {
             return VirtualRobot::IKSolver::CartesianSelection::All;
         }
@@ -401,7 +401,7 @@ namespace armarx
 
 
 
-void armarx::NJointCartesianVelocityController::setControllerTarget(float x, float y, float z, float roll, float pitch, float yaw, float avoidJointLimitsKp, NJointCartesianVelocityControllerCartesianSelection mode, const Ice::Current&)
+void armarx::NJointCartesianVelocityController::setControllerTarget(float x, float y, float z, float roll, float pitch, float yaw, float avoidJointLimitsKp, NJointCartesianVelocityControllerMode::CartesianSelection mode, const Ice::Current&)
 {
     LockGuardType guard {controlDataMutex};
     getWriterControlStruct().xVel = x;

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

@@ -113,8 +113,8 @@ namespace armarx
         WidgetDescription::HBoxLayoutPtr createJointSlidersLayout(float min, float max, float defaultValue) const;
 
         static VirtualRobot::IKSolver::CartesianSelection ModeFromString(const std::string mode);
-        static NJointCartesianVelocityControllerCartesianSelection IceModeFromString(const std::string mode);
-        static VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointCartesianVelocityControllerCartesianSelection mode);
+        static NJointCartesianVelocityControllerMode::CartesianSelection IceModeFromString(const std::string mode);
+        static VirtualRobot::IKSolver::CartesianSelection ModeFromIce(const NJointCartesianVelocityControllerMode::CartesianSelection mode);
     protected:
         void rtPreActivateController() override;
         void rtPostDeactivateController() override;
@@ -132,7 +132,7 @@ namespace armarx
 
         // NJointCartesianVelocityControllerInterface interface
     public:
-        void setControllerTarget(float x, float y, float z, float roll, float pitch, float yaw, float avoidJointLimitsKp, NJointCartesianVelocityControllerCartesianSelection mode, const Ice::Current&);
+        void setControllerTarget(float x, float y, float z, float roll, float pitch, float yaw, float avoidJointLimitsKp, NJointCartesianVelocityControllerMode::CartesianSelection mode, const Ice::Current&);
         void setTorqueKp(const StringFloatDictionary& torqueKp, const Ice::Current&);
         void setNullspaceJointVelocities(const StringFloatDictionary& nullspaceJointVelocities, const Ice::Current&);
     };

source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityController.ice

@@ -28,25 +28,28 @@
 
 module armarx
 {
-    enum NJointCartesianVelocityControllerCartesianSelection
+    module NJointCartesianVelocityControllerMode
     {
-        ePosition = 7,
-        eOrientation = 8,
-        eAll = 15
+        enum CartesianSelection
+        {
+            ePosition = 7,
+            eOrientation = 8,
+            eAll = 15
+        };
     };
 
     class NJointCartesianVelocityControllerConfig extends NJointControllerConfig
     {
         string nodeSetName = "";
         string tcpName = "";
-        NJointCartesianVelocityControllerCartesianSelection mode = eAll;
+        NJointCartesianVelocityControllerMode::CartesianSelection mode = NJointCartesianVelocityControllerMode::eAll;
     };
 
 
 
     interface NJointCartesianVelocityControllerInterface extends NJointControllerInterface
     {
-        void setControllerTarget(float x, float y, float z, float roll, float pitch, float yaw, float avoidJointLimitsKp, NJointCartesianVelocityControllerCartesianSelection mode);
+        void setControllerTarget(float x, float y, float z, float roll, float pitch, float yaw, float avoidJointLimitsKp, NJointCartesianVelocityControllerMode::CartesianSelection mode);
         void setTorqueKp(StringFloatDictionary torqueKp);
         void setNullspaceJointVelocities(StringFloatDictionary nullspaceJointVelocities);
     };

source/RobotAPI/libraries/core/CartesianPositionController.cpp

@@ -67,3 +67,19 @@ Eigen::VectorXf CartesianPositionController::calculate(const Eigen::Matrix4f& ta
     }
     return cartesianVel;
 }
+
+float CartesianPositionController::getPositionError(const Eigen::Matrix4f& targetPose)
+{
+    Eigen::Vector3f targetPos = targetPose.block<3, 1>(0, 3);
+    Eigen::Vector3f errPos = targetPos - tcp->getPositionInRootFrame();
+    return errPos.norm();
+}
+
+float CartesianPositionController::getOrientationError(const Eigen::Matrix4f& targetPose)
+{
+    Eigen::Matrix3f targetOri = targetPose.block<3, 3>(0, 0);
+    Eigen::Matrix3f tcpOri = tcp->getPoseInRootFrame().block<3, 3>(0, 0);
+    Eigen::Matrix3f oriDir = targetOri * tcpOri.inverse();
+    Eigen::AngleAxisf aa(oriDir);
+    return aa.angle();
+}

source/RobotAPI/libraries/core/CartesianPositionController.h

@@ -43,6 +43,9 @@ namespace armarx
 
         Eigen::VectorXf calculate(const Eigen::Matrix4f& targetPose, VirtualRobot::IKSolver::CartesianSelection mode);
 
+        float getPositionError(const Eigen::Matrix4f& targetPose);
+        float getOrientationError(const Eigen::Matrix4f& targetPose);
+
         //CartesianVelocityController velocityController;
         float KpPos = 1;
         float KpOri = 1;