From 1c275b0f1336ccd98137bb946eece5334406c996 Mon Sep 17 00:00:00 2001
From: zhou <you.zhou@kit.edu>
Date: Thu, 15 Feb 2018 14:08:11 +0100
Subject: [PATCH] fixed name issue of robotik and cvc
---
data/RobotAPI/VariantInfo-RobotAPI.xml | 8 ++++++++
.../components/units/RobotUnit/CMakeLists.txt | 12 +++++++++++
.../NJointCartesianVelocityController.cpp | 20 +++++++++----------
.../NJointCartesianVelocityController.h | 6 +++---
.../NJointCartesianVelocityController.ice | 15 ++++++++------
.../core/CartesianPositionController.cpp | 16 +++++++++++++++
.../core/CartesianPositionController.h | 3 +++
7 files changed, 61 insertions(+), 19 deletions(-)
diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 8b3475ac1..af488467e 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/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"
diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index ea9a07589..a4a5ab39c 100644
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/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}")
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
index c86504e4f..8e97708c5 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.cpp
+++ b/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;
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
index 9375868ff..df9409436 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityController.h
+++ b/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&);
};
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityController.ice
index bde601c05..743e5cbe6 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityController.ice
+++ b/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);
};
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.cpp b/source/RobotAPI/libraries/core/CartesianPositionController.cpp
index 8f1c64ddc..8caa1a53b 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.cpp
+++ b/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();
+}
diff --git a/source/RobotAPI/libraries/core/CartesianPositionController.h b/source/RobotAPI/libraries/core/CartesianPositionController.h
index 0c41ab9f9..c0d8642ee 100644
--- a/source/RobotAPI/libraries/core/CartesianPositionController.h
+++ b/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;
--
GitLab