From 51b89bae9dd43d8591c4ffa8845a1b002782f9e3 Mon Sep 17 00:00:00 2001 From: Fabian Reister <fabian.reister@kit.edu> Date: Fri, 10 Feb 2023 09:36:54 +0100 Subject: [PATCH 01/16] using null instead of "nullopt" --- .../NJointTaskspaceImpedanceController/default.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/data/armarx_control/controller_config/NJointTaskspaceImpedanceController/default.json b/data/armarx_control/controller_config/NJointTaskspaceImpedanceController/default.json index af83f341..c449fda4 100644 --- a/data/armarx_control/controller_config/NJointTaskspaceImpedanceController/default.json +++ b/data/armarx_control/controller_config/NJointTaskspaceImpedanceController/default.json @@ -105,5 +105,5 @@ }, "torqueLimit": 20, "qvelFilter": 0.9, - "desiredNullspaceJointAngles": "nullopt" + "desiredNullspaceJointAngles": null } -- GitLab From 6e9526439ac196a2382f1ff339392755cf6fe11e Mon Sep 17 00:00:00 2001 From: ARMAR-DE User <armarde@h2t> Date: Fri, 10 Feb 2023 20:28:50 +0100 Subject: [PATCH 02/16] fix: Use custom modulo implementation to reproduce behavior in Python --- source/armarx/control/ethercat/CMakeLists.txt | 7 +++ .../armarx/control/ethercat/DataInterface.h | 60 +++++++++++++++---- .../control/ethercat/test/EtherCATTest.cpp | 50 ++++++++++++++++ .../armarx/control/hardware_config/Config.cpp | 20 ++++--- .../control/hardware_config/ConfigParser.cpp | 31 ++++++---- .../armarx/control/hardware_config/Types.cpp | 48 +++++++-------- source/armarx/control/hardware_config/Types.h | 13 ++-- .../hardware_config/test/CMakeLists.txt | 6 +- 8 files changed, 169 insertions(+), 66 deletions(-) create mode 100644 source/armarx/control/ethercat/test/EtherCATTest.cpp diff --git a/source/armarx/control/ethercat/CMakeLists.txt b/source/armarx/control/ethercat/CMakeLists.txt index fe376c4f..5924ad1d 100644 --- a/source/armarx/control/ethercat/CMakeLists.txt +++ b/source/armarx/control/ethercat/CMakeLists.txt @@ -54,6 +54,13 @@ armarx_add_library(ethercat SOEM ) +armarx_add_test(ethercat_test + TEST_FILES + test/EtherCATTest.cpp + DEPENDENCIES + armarx_control::ethercat +) + target_compile_definitions(ethercat PUBLIC EC_VER1 diff --git a/source/armarx/control/ethercat/DataInterface.h b/source/armarx/control/ethercat/DataInterface.h index 999c4ab6..ceab6340 100644 --- a/source/armarx/control/ethercat/DataInterface.h +++ b/source/armarx/control/ethercat/DataInterface.h @@ -183,8 +183,9 @@ namespace armarx::control::ethercat rawValue = nullptr; zeroOffset = 0; discontinuityOffset = 0; - factor = 0; maxValue = 0; + factor = 0; + isInverted = false; } void @@ -193,19 +194,19 @@ namespace armarx::control::ethercat const char* nameForDebugging = "") { init(raw, - modularConfig.getFactor(), modularConfig.getZeroOffset(), modularConfig.getDiscontinuityOffset(), modularConfig.getMaxValue(), + modularConfig.getIsInverted(), nameForDebugging); } void init(T* raw, - float factor, float zeroOffset, float discontinuityOffset, float maxValue, + bool isInverted, const char* nameForDebugging = "") { const auto rawAsInt = reinterpret_cast<std::uint64_t>(raw); @@ -214,16 +215,16 @@ namespace armarx::control::ethercat << "\nIt has to be " << alignof(T) << ", but the data is aligned with " << rawAsInt % alignof(std::max_align_t) << "!" << "\nThis is an offset of " << (rawAsInt % alignof(T)) << " bytes!" - << "\nThe datatype is " << GetTypeString<T>() - << "\nIts size is " << sizeof(T) + << "\nThe datatype is " << GetTypeString<T>() << "\nIts size is " << sizeof(T) << "\nraw = " << raw << " bytes" << "\nThe name is '" << nameForDebugging << "'"; - this->factor = factor; + this->rawValue = raw; this->zeroOffset = static_cast<SignedT>(zeroOffset); this->discontinuityOffset = static_cast<SignedT>(discontinuityOffset); this->maxValue = static_cast<T>(maxValue); - this->rawValue = raw; + this->factor = 2 * M_PIf64 / maxValue; + this->isInverted = isInverted; read(); } @@ -231,7 +232,45 @@ namespace armarx::control::ethercat void read() { - value = ((((*rawValue) + discontinuityOffset - zeroOffset) % maxValue) - discontinuityOffset) * factor; + // "%" operator is not the real mathematical modulo operation but some creative ISO + // reinterpretation. This fixes it. + auto actual_real_modulo = [](SignedT dividend, SignedT divisor) -> SignedT + { + const SignedT result = dividend % divisor; + return result >= 0 ? result : result + divisor; + }; + + const SignedT rawValueSigned = static_cast<SignedT>(*rawValue); + const SignedT maxValueSigned = static_cast<SignedT>(maxValue); + + ARMARX_INFO << VAROUT(rawValueSigned); + ARMARX_INFO << VAROUT(maxValueSigned); + + // Normalize direction of rotation. + const SignedT sign = isInverted ? -1 : 1; + const SignedT dirnorm = actual_real_modulo((sign * rawValueSigned), maxValueSigned); + + ARMARX_INFO << VAROUT(dirnorm); + + // Raw values within monotonic_raw_value are guaranteed to not fall into a + // discontinuity. + const SignedT monotonic_dirnorm = + actual_real_modulo((dirnorm + discontinuityOffset), maxValueSigned); + + ARMARX_INFO << VAROUT(monotonic_dirnorm); + + // Zero shift. + const SignedT zeroshift_monotonic_dirnorm = monotonic_dirnorm - zeroOffset; + + ARMARX_INFO << VAROUT(zeroshift_monotonic_dirnorm); + + // Convert ticks to rad. + const double zeroshift_monotonic_dirnorm_rad = + static_cast<double>(zeroshift_monotonic_dirnorm) * factor; + + ARMARX_INFO << VAROUT(zeroshift_monotonic_dirnorm_rad); + + value = zeroshift_monotonic_dirnorm_rad; } void @@ -240,7 +279,7 @@ namespace armarx::control::ethercat //*rawValue = static_cast<T>((value / factor) - offset); } - float value; + double value; T getRaw() const @@ -252,8 +291,9 @@ namespace armarx::control::ethercat T* rawValue; SignedT zeroOffset; SignedT discontinuityOffset; - float factor; T maxValue; + double factor; + bool isInverted; }; diff --git a/source/armarx/control/ethercat/test/EtherCATTest.cpp b/source/armarx/control/ethercat/test/EtherCATTest.cpp new file mode 100644 index 00000000..7865f087 --- /dev/null +++ b/source/armarx/control/ethercat/test/EtherCATTest.cpp @@ -0,0 +1,50 @@ +#define BOOST_TEST_MODULE Armarx::Control::Hardwareconfig + +#define ARMARX_BOOST_TEST + +#include <Eigen/Core> +#include <Eigen/Geometry> + +#include <ArmarXCore/core/logging/Logging.h> + +#include <armarx/control/Test.h> +#include <armarx/control/ethercat/DataInterface.h> + + +using namespace armarx::control::ethercat; + + +constexpr float Precision = 0.0001; + + +BOOST_AUTO_TEST_CASE(modularConvertedValueConfig_ARMAR_DE_Cla1_L) +{ + ModularConvertedValue<std::uint32_t, std::int64_t> mcv; + + constexpr float zeroOffset = 611'565; + constexpr float discontinuityOffset = 0; + constexpr float maxValue = 1 << 20; // 2^20. + constexpr bool isInverted = true; + + std::uint32_t encoderValue = 0; + + encoderValue = 675'657; + mcv.init(&encoderValue, zeroOffset, discontinuityOffset, maxValue, isInverted); + BOOST_CHECK_CLOSE(mcv.value, -1.4299936683818621, Precision); + + encoderValue = 572'982; + mcv.init(&encoderValue, zeroOffset, discontinuityOffset, maxValue, isInverted); + BOOST_CHECK_CLOSE(mcv.value, -0.8147535223031191, Precision); + + encoderValue = 436'801; + mcv.init(&encoderValue, zeroOffset, discontinuityOffset, maxValue, isInverted); + BOOST_CHECK_CLOSE(mcv.value, 0.00125834361506244, Precision); + + encoderValue = 308'468; + mcv.init(&encoderValue, zeroOffset, discontinuityOffset, maxValue, isInverted); + BOOST_CHECK_CLOSE(mcv.value, 0.7702441110046249, Precision); + + encoderValue = 198'364; + mcv.init(&encoderValue, zeroOffset, discontinuityOffset, maxValue, isInverted); + BOOST_CHECK_CLOSE(mcv.value, 1.4299996604943148, Precision); +} diff --git a/source/armarx/control/hardware_config/Config.cpp b/source/armarx/control/hardware_config/Config.cpp index 448500d6..69d28445 100644 --- a/source/armarx/control/hardware_config/Config.cpp +++ b/source/armarx/control/hardware_config/Config.cpp @@ -80,8 +80,10 @@ namespace armarx::control::hardware_config { if (!i.second.isRead) { - errors.push_back(createErrorMessageWithContext("Configuration element " + i.first + - " was not expected by the Device code but defined in HardwareConfig.", *this)); + errors.push_back(createErrorMessageWithContext( + "Configuration element " + i.first + + " was not expected by the Device code but defined in HardwareConfig.", + *this)); } } return succ; @@ -158,8 +160,8 @@ namespace armarx::control::hardware_config }, [&s](types::ModularConvertedValueConfig arg) { - s << "ModularConvertedValueConfig): factor=" << arg.getFactor() - << " zeroOffset=" << arg.getZeroOffset() << std::endl; + s << "ModularConvertedValueConfig): " + << "zeroOffset=" << arg.getZeroOffset() << std::endl; }, [&s](std::string arg) { @@ -230,7 +232,9 @@ namespace armarx::control::hardware_config } void - Config::setModularConvertedValueConfig(const std::string name, types::ModularConvertedValueConfig&& value, ConfigTag tag) + Config::setModularConvertedValueConfig(const std::string name, + types::ModularConvertedValueConfig&& value, + ConfigTag tag) { set<types::ModularConvertedValueConfig>(name, value, tag); } @@ -318,11 +322,11 @@ namespace armarx::control::hardware_config } } - std::string createErrorMessageWithContext(std::string message, const Config& context) + std::string + createErrorMessageWithContext(std::string message, const Config& context) { std::stringstream ss; - ss << message - << std::endl << "Context:" << std::endl; + ss << message << std::endl << "Context:" << std::endl; context.print(ss, 0); return ss.str(); } diff --git a/source/armarx/control/hardware_config/ConfigParser.cpp b/source/armarx/control/hardware_config/ConfigParser.cpp index 3804415c..3bcd33cd 100644 --- a/source/armarx/control/hardware_config/ConfigParser.cpp +++ b/source/armarx/control/hardware_config/ConfigParser.cpp @@ -1,6 +1,5 @@ #include "ConfigParser.h" - #include <SimoxUtility/algorithm/string/string_tools.h> @@ -605,15 +604,19 @@ namespace armarx::control::hardware_config return std::nullopt; }; - if (node.has_attribute("factor") and node.has_attribute("zeroOffset") - and node.has_attribute("discontinuityOffset") and node.has_attribute("maxValue")) + if (node.has_attribute("zeroOffset") and + node.has_attribute("discontinuityOffset") and + node.has_attribute("maxValue") and node.has_attribute("isInverted")) { - std::optional<float> discontinuityOffset = evaluate(node.attribute_value("discontinuityOffset")); + std::optional<float> discontinuityOffset = + evaluate(node.attribute_value("discontinuityOffset")); bool any_error = false; if (not discontinuityOffset.has_value()) { - errors.push_back("Invalid value for discontinuityOffset in node " + nodeName); + errors.push_back(createErrorMessageWithContext( + "Invalid value for discontinuityOffset in node " + nodeName, + configNode)); any_error = true; } @@ -621,7 +624,8 @@ namespace armarx::control::hardware_config if (not maxValue.has_value()) { - errors.push_back("Invalid value for maxValue in node " + nodeName); + errors.push_back(createErrorMessageWithContext( + "Invalid value for maxValue in node " + nodeName, configNode)); any_error = true; } @@ -632,15 +636,20 @@ namespace armarx::control::hardware_config config.setModularConvertedValueConfig( name, - types::ModularConvertedValueConfig(node.attribute_as_float("factor"), - node.attribute_as_float("zeroOffset"), - discontinuityOffset.value(), - maxValue.value()), + types::ModularConvertedValueConfig( + node.attribute_as_float("zeroOffset"), + discontinuityOffset.value(), + maxValue.value(), + node.attribute_as_bool("isInverted", "true", "false")), tag); } else { - // TODO: Error handling. + errors.push_back(createErrorMessageWithContext( + "Not all required fields (zeroOffset, " + "discontinuityOffset, maxValue, isInverted) set for " + + nodeName, + configNode)); } } else if (nodeName == "String") diff --git a/source/armarx/control/hardware_config/Types.cpp b/source/armarx/control/hardware_config/Types.cpp index 4053d27d..c939c229 100644 --- a/source/armarx/control/hardware_config/Types.cpp +++ b/source/armarx/control/hardware_config/Types.cpp @@ -1,5 +1,6 @@ #include "Types.h" + namespace armarx::control::hardware_config::types { LinearConfig::LinearConfig(float factor, float offset) : @@ -58,12 +59,14 @@ namespace armarx::control::hardware_config::types return offset.value_or(0.f); } - ModularConvertedValueConfig::ModularConvertedValueConfig(float factor, float zeroOffset, float discontinuityOffset, float maxValue) : - factorAbs(std::abs(factor)), - negativeFactor(factor < 0), + ModularConvertedValueConfig::ModularConvertedValueConfig(float zeroOffset, + float discontinuityOffset, + float maxValue, + bool isInverted) : zeroOffset(zeroOffset), discontinuityOffset(discontinuityOffset), - maxValue(maxValue) + maxValue(maxValue), + isInverted{isInverted} { ; } @@ -71,16 +74,6 @@ namespace armarx::control::hardware_config::types ModularConvertedValueConfig ModularConvertedValueConfig::overrideWith(const ModularConvertedValueConfig& other) { - if (other.factorAbs.has_value()) - { - factorAbs = other.factorAbs; - } - - if (other.negativeFactor.has_value()) - { - negativeFactor = other.negativeFactor; - } - if (other.zeroOffset.has_value()) { zeroOffset = other.zeroOffset; @@ -96,13 +89,12 @@ namespace armarx::control::hardware_config::types maxValue = other.maxValue; } - return *this; - } + if (other.isInverted.has_value()) + { + isInverted = other.isInverted; + } - void - ModularConvertedValueConfig::setFactorAbsoluteValue(float factorAbs) - { - this->factorAbs = std::make_optional(factorAbs); + return *this; } void @@ -112,15 +104,9 @@ namespace armarx::control::hardware_config::types } void - ModularConvertedValueConfig::setFactorIsNegative(bool isNegative) - { - this->negativeFactor = isNegative; - } - - float - ModularConvertedValueConfig::getFactor() const + ModularConvertedValueConfig::setIsInverted(bool isInverted) { - return (negativeFactor.value_or(false) ? -1 : 1) * factorAbs.value_or(1.f); + this->isInverted = isInverted; } float @@ -141,4 +127,10 @@ namespace armarx::control::hardware_config::types return maxValue.value_or(0.f); } + bool + ModularConvertedValueConfig::getIsInverted() const + { + return isInverted.value_or(false); + } + } // namespace armarx::control::hardware_config::types diff --git a/source/armarx/control/hardware_config/Types.h b/source/armarx/control/hardware_config/Types.h index be581367..45a36283 100644 --- a/source/armarx/control/hardware_config/Types.h +++ b/source/armarx/control/hardware_config/Types.h @@ -68,26 +68,27 @@ namespace armarx::control::hardware_config::types class ModularConvertedValueConfig { public: - ModularConvertedValueConfig(float factor, float zeroOffset, float discontinuityOffset, float maxValue); + explicit ModularConvertedValueConfig(float zeroOffset, + float discontinuityOffset, + float maxValue, + bool isInverted); ModularConvertedValueConfig() = default; ModularConvertedValueConfig overrideWith(const ModularConvertedValueConfig&); - void setFactorAbsoluteValue(float factorAbs); void setZeroOffset(float zeroOffset); - void setFactorIsNegative(bool isNegative); + void setIsInverted(bool isNegative); - float getFactor() const; float getZeroOffset() const; float getDiscontinuityOffset() const; float getMaxValue() const; + bool getIsInverted() const; private: - std::optional<float> factorAbs; - std::optional<bool> negativeFactor; std::optional<float> zeroOffset; std::optional<float> discontinuityOffset; std::optional<float> maxValue; + std::optional<bool> isInverted; }; diff --git a/source/armarx/control/hardware_config/test/CMakeLists.txt b/source/armarx/control/hardware_config/test/CMakeLists.txt index 68e2e281..5d99a6a6 100644 --- a/source/armarx/control/hardware_config/test/CMakeLists.txt +++ b/source/armarx/control/hardware_config/test/CMakeLists.txt @@ -1,6 +1,6 @@ armarx_add_test(hardware_config_test - TEST_FILES - XmlReaderTest.cpp - DEPENDENCIES + TEST_FILES + XmlReaderTest.cpp + DEPENDENCIES armarx_control::hardware_config ) -- GitLab From bfd36c7f23a5f4e1067d3199d62d4674dee24dea Mon Sep 17 00:00:00 2001 From: ARMAR-DE User <armarde@h2t> Date: Fri, 10 Feb 2023 20:50:14 +0100 Subject: [PATCH 03/16] fix: Remove outputs --- source/armarx/control/ethercat/DataInterface.h | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/source/armarx/control/ethercat/DataInterface.h b/source/armarx/control/ethercat/DataInterface.h index ceab6340..8dcf4d2f 100644 --- a/source/armarx/control/ethercat/DataInterface.h +++ b/source/armarx/control/ethercat/DataInterface.h @@ -243,33 +243,22 @@ namespace armarx::control::ethercat const SignedT rawValueSigned = static_cast<SignedT>(*rawValue); const SignedT maxValueSigned = static_cast<SignedT>(maxValue); - ARMARX_INFO << VAROUT(rawValueSigned); - ARMARX_INFO << VAROUT(maxValueSigned); - // Normalize direction of rotation. const SignedT sign = isInverted ? -1 : 1; const SignedT dirnorm = actual_real_modulo((sign * rawValueSigned), maxValueSigned); - ARMARX_INFO << VAROUT(dirnorm); - // Raw values within monotonic_raw_value are guaranteed to not fall into a // discontinuity. const SignedT monotonic_dirnorm = actual_real_modulo((dirnorm + discontinuityOffset), maxValueSigned); - ARMARX_INFO << VAROUT(monotonic_dirnorm); - // Zero shift. const SignedT zeroshift_monotonic_dirnorm = monotonic_dirnorm - zeroOffset; - ARMARX_INFO << VAROUT(zeroshift_monotonic_dirnorm); - // Convert ticks to rad. const double zeroshift_monotonic_dirnorm_rad = static_cast<double>(zeroshift_monotonic_dirnorm) * factor; - ARMARX_INFO << VAROUT(zeroshift_monotonic_dirnorm_rad); - value = zeroshift_monotonic_dirnorm_rad; } -- GitLab From 7f4719400e37b6952b32aea1af9226cf749c09a7 Mon Sep 17 00:00:00 2001 From: Fabian Reister <fabian.reister@kit.edu> Date: Sun, 19 Feb 2023 16:09:05 +0100 Subject: [PATCH 04/16] joint space impedance controller --- source/armarx/control/common/CMakeLists.txt | 2 + .../JointSpaceImpedanceController.cpp | 155 ++++++++++++++++++ .../JointSpaceImpedanceController.h | 87 ++++++++++ 3 files changed, 244 insertions(+) create mode 100644 source/armarx/control/common/control_law/JointSpaceImpedanceController.cpp create mode 100644 source/armarx/control/common/control_law/JointSpaceImpedanceController.h diff --git a/source/armarx/control/common/CMakeLists.txt b/source/armarx/control/common/CMakeLists.txt index c9cf83b3..e34e1fe9 100644 --- a/source/armarx/control/common/CMakeLists.txt +++ b/source/armarx/control/common/CMakeLists.txt @@ -32,6 +32,7 @@ armarx_add_library(common control_law/TaskspaceAdmittanceController.cpp control_law/KeypointsAdmittanceController.cpp control_law/TaskspaceImpedanceController.cpp + control_law/JointSpaceImpedanceController.cpp control_law/KeypointsImpedanceController.cpp aron_conversions.cpp @@ -49,6 +50,7 @@ armarx_add_library(common control_law/TaskspaceAdmittanceController.h control_law/KeypointsAdmittanceController.h control_law/TaskspaceImpedanceController.h + control_law/JointSpaceImpedanceController.h control_law/KeypointsImpedanceController.h aron_conversions.h diff --git a/source/armarx/control/common/control_law/JointSpaceImpedanceController.cpp b/source/armarx/control/common/control_law/JointSpaceImpedanceController.cpp new file mode 100644 index 00000000..d2c932fa --- /dev/null +++ b/source/armarx/control/common/control_law/JointSpaceImpedanceController.cpp @@ -0,0 +1,155 @@ +/** + * 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 VS_CODE_USER_NAME ( VS_CODE_USER_EMAIL ) + * @date 2023 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#include "JointSpaceImpedanceController.h" + +#include <ArmarXCore/core/exceptions/local/ExpressionException.h> + +namespace armarx::control::common::control_law +{ + + JointSpaceImpedanceController::JointSpaceImpedanceController(const std::size_t numOfJoints) : + numOfJoints(numOfJoints) + { + desiredTorque.resize(static_cast<int>(numOfJoints)); + } + + void + JointSpaceImpedanceController::updateControlStatus(const Config& cfg, const RobotStatus& status) + { + // check size and value + ARMARX_CHECK(cfg.validate(numOfJoints)); + + // reset + desiredTorque.setZero(); + + // interfacing buffers + + ARMARX_CHECK_EQUAL(status.jointPosition.size(), numOfJoints); + ARMARX_CHECK_EQUAL(status.jointVelocity.size(), numOfJoints); + ARMARX_CHECK_EQUAL(status.jointAcceleration.size(), numOfJoints); + ARMARX_CHECK_EQUAL(status.gravityTorque.size(), numOfJoints); + + const Eigen::Map<const Eigen::VectorXf> jointPos(status.jointPosition.data(), + static_cast<int>(numOfJoints)); + const Eigen::Map<const Eigen::VectorXf> jointVel(status.jointVelocity.data(), + static_cast<int>(numOfJoints)); + // const auto jointAcc = Eigen::Map<const Eigen::VectorXf>(status.jointAcceleration.data(), + // static_cast<int>(numOfJoints)); + const Eigen::Map<const Eigen::VectorXf> gravityTorque(status.gravityTorque.data(), + static_cast<int>(numOfJoints)); + + if (cfg.desiredJointPos.has_value()) + { + desiredTorque += cfg.kp.cwiseProduct(cfg.desiredJointPos.value() - jointPos); + } + + // TODO filter for velocity + // s.qvel = (1 - cfg.qvelFilter) * s.qvel + cfg.qvelFilter * qvelRaw; + + + if (cfg.desiredJointVel.has_value()) + { + desiredTorque += cfg.kd.cwiseProduct(cfg.desiredJointVel.value() - jointVel); + } + else + { + desiredTorque -= cfg.kd.cwiseProduct(jointVel); + } + + // TODO filter for acceleration + // s.qacc = (1 - cfg.qaccFilter) * s.qacc + cfg.qaccFilter * qaccRaw; + + if (cfg.desiredJointAcc.has_value()) + { + // TODO desiredTorque += status.massInertia * (cfg.desiredJointAcc - jointAcc); + } + else + { + // TODO desiredTorque -= status.massInertia * jointAcc; + } + + // gravity compensation + desiredTorque -= gravityTorque; + } + + + bool + JointSpaceImpedanceController::Config::validate(std::size_t numOfJoints) const noexcept + { + if (kp.size() != static_cast<int>(numOfJoints)) + { + return false; + } + + if (not(kp.array() < 0.F).all()) + { + return false; + } + + + if (kd.size() != static_cast<int>(numOfJoints)) + { + return false; + } + + if (not(kd.array() < 0.F).all()) + { + return false; + } + + if (desiredJointPos.has_value() and + desiredJointPos->size() != static_cast<int>(numOfJoints)) + { + return false; + } + + if (desiredJointVel.has_value() and + desiredJointVel->size() != static_cast<int>(numOfJoints)) + { + return false; + } + + if (desiredJointAcc.has_value() and + desiredJointAcc->size() != static_cast<int>(numOfJoints)) + { + return false; + } + + // if (gravityTorque.size() != static_cast<int>(numOfJoints)) + // { + // return false; + // } + + return true; + } + + void + JointSpaceImpedanceController::run(ControlTarget& targets) const + { + ARMARX_CHECK_EQUAL(targets.targets.size(), numOfJoints); + + for (std::size_t i = 0; i < numOfJoints; ++i) + { + targets.targets.at(i) = desiredTorque(static_cast<int>(i)); + } + } +} // namespace armarx::control::common::control_law diff --git a/source/armarx/control/common/control_law/JointSpaceImpedanceController.h b/source/armarx/control/common/control_law/JointSpaceImpedanceController.h new file mode 100644 index 00000000..5a4a4b9d --- /dev/null +++ b/source/armarx/control/common/control_law/JointSpaceImpedanceController.h @@ -0,0 +1,87 @@ +/** + * 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 2023 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + +#pragma once + +#include <optional> + +#include <Eigen/Core> + +#include <ArmarXCore/util/CPPUtility/TripleBuffer.h> + +#include "armarx/control/common/control_law/common.h" + +namespace armarx::control::common::control_law +{ + + class JointSpaceImpedanceController + { + public: + struct Config + { + Eigen::VectorXf kp; + Eigen::VectorXf kd; + + std::optional<Eigen::VectorXf> desiredJointPos; + std::optional<Eigen::VectorXf> desiredJointVel; + std::optional<Eigen::VectorXf> desiredJointAcc; + + bool validate(std::size_t numOfJoints) const noexcept; + }; + + struct RtStatus + { + // targets + Eigen::VectorXf desiredJointTorques; + }; + + struct ControlTarget + { + std::vector<float> targets; + }; + + struct RobotStatus + { + std::vector<float> jointPosition; + std::vector<float> jointVelocity; + std::vector<float> jointAcceleration; + + std::vector<float> gravityTorque; + + Eigen::MatrixXf massInertia; + }; + + JointSpaceImpedanceController(std::size_t numOfJoints); + + // TripleBuffer<RtStatus> bufferRtToOnPublish; + + void updateControlStatus(const Config& cfg, const RobotStatus& status); + + void run(ControlTarget& targets) const; + + protected: + private: + const std::size_t numOfJoints; + + Eigen::VectorXf desiredTorque; + }; + +} // namespace armarx::control::common::control_law -- GitLab From 65aa2fd47b3beb5bfeeec801f7c3c4baec7ea33d Mon Sep 17 00:00:00 2001 From: armar-user <armar6@kit> Date: Tue, 14 Mar 2023 13:23:33 +0100 Subject: [PATCH 05/16] adapt default parameter of imepdance controller --- .../default.json | 50 +++++++++---------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/data/armarx_control/controller_config/NJointTaskspaceImpedanceController/default.json b/data/armarx_control/controller_config/NJointTaskspaceImpedanceController/default.json index 42b367bf..fc717fb0 100644 --- a/data/armarx_control/controller_config/NJointTaskspaceImpedanceController/default.json +++ b/data/armarx_control/controller_config/NJointTaskspaceImpedanceController/default.json @@ -6,12 +6,12 @@ 1 ], "data": [ - 500, - 500, - 500, - 30, - 30, - 30 + 800, + 800, + 800, + 50, + 50, + 50 ] }, "kdImpedance": { @@ -21,12 +21,12 @@ 1 ], "data": [ - 100, - 100, - 100, - 10, - 10, - 10 + 200, + 200, + 200, + 20, + 20, + 20 ] }, "kpNullspace": { @@ -36,14 +36,14 @@ 1 ], "data": [ + 65.0, + 45.0, 25.0, 15.0, 15.0, - 5.0, - 5.0, - 5.0, - 5.0, - 5.0 + 15.0, + 15.0, + 15.0 ] }, "kdNullspace": { @@ -53,14 +53,14 @@ 1 ], "data": [ - 4.0, - 2.0, - 2.0, - 1.0, - 1.0, - 1.0, - 1.0, - 2.0 + 14.0, + 10.0, + 10.0, + 6.0, + 6.0, + 6.0, + 6.0, + 6.0 ] }, "desiredPose": { -- GitLab From b781fc34e0c0214a7dd6d9a7693c8c0d69c4ef0a Mon Sep 17 00:00:00 2001 From: armar-user <armar6@kit> Date: Tue, 14 Mar 2023 19:05:43 +0100 Subject: [PATCH 06/16] add constructor and control scenario --- scenarios/ControlScenario/ControlScenario.scx | 1 + .../config/controller_creator.cfg | 4 +++ .../config/controller_creator.cfg | 4 +++ .../controller_creator/CMakeLists.txt | 4 ++- .../controller_creator/Component.cpp | 30 +++++++++++-------- .../components/controller_creator/Component.h | 7 +++-- .../controller_creator/ComponentInterface.ice | 3 +- 7 files changed, 35 insertions(+), 18 deletions(-) create mode 100644 scenarios/ControlScenario/config/controller_creator.cfg create mode 100644 scenarios/MotionControl/config/controller_creator.cfg diff --git a/scenarios/ControlScenario/ControlScenario.scx b/scenarios/ControlScenario/ControlScenario.scx index 09548f6f..d57a10a2 100644 --- a/scenarios/ControlScenario/ControlScenario.scx +++ b/scenarios/ControlScenario/ControlScenario.scx @@ -1,5 +1,6 @@ <?xml version="1.0" encoding="utf-8"?> <scenario name="ControlScenario" creation="2023-02-08.13:52:50" globalConfigName="./config/global.cfg" package="armarx_control" deploymentType="local" nodeName="NodeMain"> <application name="njoint_controller_example" instance="" package="armarx_control" nodeName="" enabled="true" iceAutoRestart="false"/> + <application name="controller_creator" instance="" package="armarx_control" nodeName="" enabled="true" iceAutoRestart="false"/> </scenario> diff --git a/scenarios/ControlScenario/config/controller_creator.cfg b/scenarios/ControlScenario/config/controller_creator.cfg new file mode 100644 index 00000000..b5d8ed6e --- /dev/null +++ b/scenarios/ControlScenario/config/controller_creator.cfg @@ -0,0 +1,4 @@ +# ================================================================== +# controller_creator properties +# ================================================================== + diff --git a/scenarios/MotionControl/config/controller_creator.cfg b/scenarios/MotionControl/config/controller_creator.cfg new file mode 100644 index 00000000..b5d8ed6e --- /dev/null +++ b/scenarios/MotionControl/config/controller_creator.cfg @@ -0,0 +1,4 @@ +# ================================================================== +# controller_creator properties +# ================================================================== + diff --git a/source/armarx/control/components/controller_creator/CMakeLists.txt b/source/armarx/control/components/controller_creator/CMakeLists.txt index 0ead9e56..92885fc0 100644 --- a/source/armarx/control/components/controller_creator/CMakeLists.txt +++ b/source/armarx/control/components/controller_creator/CMakeLists.txt @@ -16,7 +16,9 @@ armarx_add_component(controller_creator ArmarXCoreInterfaces ArmarXCoreComponentPlugins # For DebugObserver plugin. # ArmarXGui - ## ArmarXGuiComponentPlugins # For RemoteGui plugin. + ArmarXGui + ArmarXGuiComponentPlugins # For RemoteGui plugin. + RemoteGui # RobotAPI RobotUnit RobotAPICore diff --git a/source/armarx/control/components/controller_creator/Component.cpp b/source/armarx/control/components/controller_creator/Component.cpp index 4950f8c3..b4ef6b26 100644 --- a/source/armarx/control/components/controller_creator/Component.cpp +++ b/source/armarx/control/components/controller_creator/Component.cpp @@ -22,11 +22,7 @@ #include "Component.h" - -// Include headers you only need in function definitions in the .cpp. - -// #include <Eigen/Core> - +#include <Eigen/Core> // #include <SimoxUtility/color/Color.h> #include "ArmarXCore/core/time/Metronome.h" #include <ArmarXCore/core/logging/Logging.h> @@ -40,19 +36,27 @@ namespace armarx::control::components::controller_creator const std::string Component::defaultName = "controller_creator"; + Component::Component() + { + addPlugin(m_virtualRobotReaderPlugin); + } + + Component::~Component() = default; + + armarx::PropertyDefinitionsPtr Component::createPropertyDefinitions() { armarx::PropertyDefinitionsPtr def = new armarx::ComponentPropertyDefinitions(getConfigIdentifier()); - def->optional(m_properties.robotName, "robotName"); - def->optional(m_properties.robotUnitName, "robotUnitName"); + def->optional(properties.robotName, "robotName"); + def->optional(properties.robotUnitName, "robotUnitName"); - def->optional(m_properties.defaultRobotNodeSetLeft, "default_robotNodeSet_left", "default kinematic chain name of left arm."); - def->optional(m_properties.defaultRobotNodeSetRight, "default_robotNodeSet_right", "default kinematic chain name of right arm."); + def->optional(properties.defaultRobotNodeSetLeft, "default_robotNodeSet_left", "default kinematic chain name of left arm."); + def->optional(properties.defaultRobotNodeSetRight, "default_robotNodeSet_right", "default kinematic chain name of right arm."); - def->component(m_robot_unit, m_properties.robotUnitName); + def->component(m_robot_unit, properties.robotUnitName); ARMARX_CHECK_NOT_NULL(m_robot_unit); // Publish to a topic (passing the TopicListenerPrx). // def->topic(myTopicListener); @@ -116,12 +120,12 @@ namespace armarx::control::components::controller_creator } */ - m_robot = m_virtualRobotReaderPlugin->get().getRobot(m_properties.robotName); + m_robot = m_virtualRobotReaderPlugin->get().getRobot(properties.robotName); ARMARX_CHECK_NOT_NULL(m_robot); ARMARX_CHECK(synchronizeLocalClone(m_robot)); - m_rns_l = m_robot->getRobotNodeSet(m_properties.defaultRobotNodeSetLeft); - m_rns_r = m_robot->getRobotNodeSet(m_properties.defaultRobotNodeSetRight); + m_rns_l = m_robot->getRobotNodeSet(properties.defaultRobotNodeSetLeft); + m_rns_r = m_robot->getRobotNodeSet(properties.defaultRobotNodeSetRight); } diff --git a/source/armarx/control/components/controller_creator/Component.h b/source/armarx/control/components/controller_creator/Component.h index 1bc54241..6a79a04f 100644 --- a/source/armarx/control/components/controller_creator/Component.h +++ b/source/armarx/control/components/controller_creator/Component.h @@ -51,7 +51,7 @@ namespace armarx::control::components::controller_creator class Component : virtual public armarx::Component - , virtual public armarx::control::components::controller_creator::ComponentInterface + , virtual public ComponentInterface , virtual public armarx::RobotStateComponentPluginUser , virtual public control::client::ComponentPluginUser , virtual public armem::ListeningClientPluginUser @@ -61,6 +61,9 @@ namespace armarx::control::components::controller_creator { public: + Component(); + + ~Component() override; /// @see armarx::ManagedIceObject::getDefaultName() std::string getDefaultName() const override; @@ -150,7 +153,7 @@ namespace armarx::control::components::controller_creator std::string defaultRobotNodeSetLeft = "LeftArm"; std::string defaultRobotNodeSetRight = "RightArm"; }; - Properties m_properties; + Properties properties; /* Use a mutex if you access variables from different threads * (e.g. ice functions and RemoteGui_update()). std::mutex propertiesMutex; diff --git a/source/armarx/control/components/controller_creator/ComponentInterface.ice b/source/armarx/control/components/controller_creator/ComponentInterface.ice index c6e9d13c..36519323 100644 --- a/source/armarx/control/components/controller_creator/ComponentInterface.ice +++ b/source/armarx/control/components/controller_creator/ComponentInterface.ice @@ -20,11 +20,10 @@ * GNU General Public License */ -#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice> - #pragma once +#include <RobotAPI/interface/armem/client/MemoryListenerInterface.ice> module armarx { module control { module components { module controller_creator { -- GitLab From 0d52307e9a244a6f74211d7a280eb3e1846e7f65 Mon Sep 17 00:00:00 2001 From: Fabian Reister <fabian.reister@kit.edu> Date: Thu, 23 Mar 2023 09:59:34 +0100 Subject: [PATCH 07/16] removing gravity compensation (applied by the control target's controller) --- .../common/control_law/JointSpaceImpedanceController.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/source/armarx/control/common/control_law/JointSpaceImpedanceController.cpp b/source/armarx/control/common/control_law/JointSpaceImpedanceController.cpp index d2c932fa..41615bd0 100644 --- a/source/armarx/control/common/control_law/JointSpaceImpedanceController.cpp +++ b/source/armarx/control/common/control_law/JointSpaceImpedanceController.cpp @@ -87,8 +87,8 @@ namespace armarx::control::common::control_law // TODO desiredTorque -= status.massInertia * jointAcc; } - // gravity compensation - desiredTorque -= gravityTorque; + // gravity compensation => already part of the control target impl. + // desiredTorque -= gravityTorque; } -- GitLab From a9b705f31ba4436396f33f6ee588bdbf2d6b5a30 Mon Sep 17 00:00:00 2001 From: Fabian Reister <fabian.reister@kit.edu> Date: Thu, 23 Mar 2023 11:42:35 +0100 Subject: [PATCH 08/16] moving files --- .../CMakeLists.txt | 0 .../Component.cpp | 0 .../Component.h | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename examples/components/{njoint_controller_example => njoint_ts_impedance_controller_example}/CMakeLists.txt (100%) rename examples/components/{njoint_controller_example => njoint_ts_impedance_controller_example}/Component.cpp (100%) rename examples/components/{njoint_controller_example => njoint_ts_impedance_controller_example}/Component.h (100%) diff --git a/examples/components/njoint_controller_example/CMakeLists.txt b/examples/components/njoint_ts_impedance_controller_example/CMakeLists.txt similarity index 100% rename from examples/components/njoint_controller_example/CMakeLists.txt rename to examples/components/njoint_ts_impedance_controller_example/CMakeLists.txt diff --git a/examples/components/njoint_controller_example/Component.cpp b/examples/components/njoint_ts_impedance_controller_example/Component.cpp similarity index 100% rename from examples/components/njoint_controller_example/Component.cpp rename to examples/components/njoint_ts_impedance_controller_example/Component.cpp diff --git a/examples/components/njoint_controller_example/Component.h b/examples/components/njoint_ts_impedance_controller_example/Component.h similarity index 100% rename from examples/components/njoint_controller_example/Component.h rename to examples/components/njoint_ts_impedance_controller_example/Component.h -- GitLab From 6ebf03faf77be6f331a225ab4cfc076d74306277 Mon Sep 17 00:00:00 2001 From: Fabian Reister <fabian.reister@kit.edu> Date: Thu, 23 Mar 2023 11:42:46 +0100 Subject: [PATCH 09/16] cleanup /fixes --- examples/CMakeLists.txt | 2 +- .../Component.cpp | 28 ++++++++----------- .../Component.h | 5 ++-- 3 files changed, 14 insertions(+), 21 deletions(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index df329fcd..ef252504 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1 +1 @@ -add_subdirectory(components/njoint_controller_example) +add_subdirectory(components/njoint_ts_impedance_controller_example) diff --git a/examples/components/njoint_ts_impedance_controller_example/Component.cpp b/examples/components/njoint_ts_impedance_controller_example/Component.cpp index 65a39344..adcf9a75 100644 --- a/examples/components/njoint_ts_impedance_controller_example/Component.cpp +++ b/examples/components/njoint_ts_impedance_controller_example/Component.cpp @@ -37,8 +37,7 @@ #include <ArmarXCore/core/time/forward_declarations.h> #include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h> - -namespace armarx::control::components::example_client +namespace armarx::control::components::njoint_ts_impedance_controller_example { Component::Component() @@ -46,7 +45,6 @@ namespace armarx::control::components::example_client addPlugin(virtualRobotReaderPlugin); } - Component::~Component() = default; void @@ -55,20 +53,18 @@ namespace armarx::control::components::example_client // pass } - void Component::onConnectComponent() { robot = virtualRobotReaderPlugin->get().getRobot(properties.robotName); ARMARX_CHECK_NOT_NULL(robot); - virtualRobotReaderPlugin->get().synchronizeRobot(*robot, Clock::Now()); + ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, Clock::Now())); task = new armarx::RunningTask<Component>(this, &Component::runExample); task->start(); } - void Component::onDisconnectComponent() { @@ -76,14 +72,12 @@ namespace armarx::control::components::example_client task->stop(join); } - void Component::onExitComponent() { // pass } - std::string Component::getDefaultName() const { @@ -128,13 +122,15 @@ namespace armarx::control::components::example_client ARMARX_INFO << VAROUT(builder.config().desiredPose); ARMARX_INFO << VAROUT(builder.config().desiredTwist); if (builder.config().desiredNullspaceJointAngles.has_value()) + { ARMARX_INFO << VAROUT(builder.config().desiredNullspaceJointAngles.value()); + } ARMARX_INFO << VAROUT(builder.config().torqueLimit); ARMARX_INFO << VAROUT(builder.config().qvelFilter); } ARMARX_CHECK_NOT_NULL(robot); - virtualRobotReaderPlugin->get().synchronizeRobot(*robot, armarx::Clock::Now()); + ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, armarx::Clock::Now())); /// !!!! /// you have to wait for enough time to let the clone finish. Otherwise, you may get random / wrong values below when you read the joint angles and TCP pose, etc. /// !!!! @@ -143,7 +139,7 @@ namespace armarx::control::components::example_client const auto rns = robot->getRobotNodeSet(properties.rns); // Set configuration programmatically. Use this for those parts of the config that cannot be specified in advance. -// builder.config().desiredNullspaceJointAngles = rns->getJointValuesEigen(); + // builder.config().desiredNullspaceJointAngles = rns->getJointValuesEigen(); ARMARX_INFO << VAROUT(rns->getJointValuesEigen()); builder.config().desiredPose = rns->getTCP()->getPoseInRootFrame(); ARMARX_INFO << VAROUT(builder.config().desiredPose); @@ -187,11 +183,11 @@ namespace armarx::control::components::example_client const Eigen::Isometry3f global_T_tcp_desired = Eigen::Translation3f(posOffset) * robot_T_tcp_initial; -// // update config -// ctrl->config.desiredPose = global_T_tcp_desired.matrix(); + // // update config + // ctrl->config.desiredPose = global_T_tcp_desired.matrix(); -// // send new config to controller -// ctrl->updateConfig(); + // // send new config to controller + // ctrl->updateConfig(); metronome.waitForNextTick(); } @@ -199,7 +195,6 @@ namespace armarx::control::components::example_client ctrl->deactivate(); } - armarx::PropertyDefinitionsPtr Component::createPropertyDefinitions() { @@ -215,7 +210,6 @@ namespace armarx::control::components::example_client return def; } - ARMARX_DECOUPLED_REGISTER_COMPONENT(Component); -} // namespace armarx::navigation::components::example_client +} // namespace armarx::control::components::njoint_ts_impedance_controller_example diff --git a/examples/components/njoint_ts_impedance_controller_example/Component.h b/examples/components/njoint_ts_impedance_controller_example/Component.h index 08ca8f0c..57b2fca9 100644 --- a/examples/components/njoint_ts_impedance_controller_example/Component.h +++ b/examples/components/njoint_ts_impedance_controller_example/Component.h @@ -37,8 +37,7 @@ #include <armarx/control/common/type.h> #include <armarx/control/njoint_controller/controller_descriptions.h> - -namespace armarx::control::components::example_client +namespace armarx::control::components::njoint_ts_impedance_controller_example { inline const control::common::ControllerType ControllerType = @@ -111,4 +110,4 @@ namespace armarx::control::components::example_client std::optional<control::client::ControllerWrapper<ControllerType>> ctrl; }; -} // namespace armarx::control::components::example_client +} // namespace armarx::control::components::njoint_ts_impedance_controller_example -- GitLab From 65476dced0a4f208395759b975185a4846c176cd Mon Sep 17 00:00:00 2001 From: Fabian Reister <fabian.reister@kit.edu> Date: Thu, 23 Mar 2023 16:47:11 +0100 Subject: [PATCH 10/16] joint space impedance controller --- .../default.json | 41 +++ examples/CMakeLists.txt | 1 + .../CMakeLists.txt | 17 + .../Component.cpp | 213 +++++++++++ .../Component.h | 113 ++++++ .../CMakeLists.txt | 2 +- .../Component.cpp | 2 +- ...ntJointspaceImpedanceControllerExample.scx | 6 + .../config/control_memory.cfg | 272 +++++++++++++++ .../config/global.cfg | 4 + ...njoint_js_impedance_controller_example.cfg | 261 ++++++++++++++ source/armarx/control/common/CMakeLists.txt | 1 + .../control/common/aron_conversions.cpp | 317 ++++++++--------- .../armarx/control/common/aron_conversions.h | 39 +-- .../JointSpaceImpedanceController.cpp | 22 +- .../JointSpaceImpedanceController.h | 2 +- .../JointspaceImpedanceControllerConfig.xml | 38 ++ source/armarx/control/common/type.h | 28 +- .../control/njoint_controller/CMakeLists.txt | 2 + .../controller_descriptions.h | 12 + .../joint_space/ImpedanceController.cpp | 330 ++++++++++++++++++ .../joint_space/ImpedanceController.h | 123 +++++++ 22 files changed, 1641 insertions(+), 205 deletions(-) create mode 100644 data/armarx_control/controller_config/NJointJointspaceImpedanceController/default.json create mode 100644 examples/components/njoint_js_impedance_controller_example/CMakeLists.txt create mode 100644 examples/components/njoint_js_impedance_controller_example/Component.cpp create mode 100644 examples/components/njoint_js_impedance_controller_example/Component.h create mode 100644 scenarios/NJointJointspaceImpedanceControllerExample/NJointJointspaceImpedanceControllerExample.scx create mode 100644 scenarios/NJointJointspaceImpedanceControllerExample/config/control_memory.cfg create mode 100644 scenarios/NJointJointspaceImpedanceControllerExample/config/global.cfg create mode 100644 scenarios/NJointJointspaceImpedanceControllerExample/config/njoint_js_impedance_controller_example.cfg create mode 100644 source/armarx/control/common/control_law/aron/JointspaceImpedanceControllerConfig.xml create mode 100644 source/armarx/control/njoint_controller/joint_space/ImpedanceController.cpp create mode 100644 source/armarx/control/njoint_controller/joint_space/ImpedanceController.h diff --git a/data/armarx_control/controller_config/NJointJointspaceImpedanceController/default.json b/data/armarx_control/controller_config/NJointJointspaceImpedanceController/default.json new file mode 100644 index 00000000..b86b576f --- /dev/null +++ b/data/armarx_control/controller_config/NJointJointspaceImpedanceController/default.json @@ -0,0 +1,41 @@ +{ + "kpImpedance": { + "type": "float", + "dims": [ + 8, + 1 + ], + "data": [ + 65.0, + 45.0, + 25.0, + 15.0, + 15.0, + 15.0, + 15.0, + 15.0 + ] + }, + "kdImpedance": { + "type": "float", + "dims": [ + 8, + 1 + ], + "data": [ + 14.0, + 10.0, + 10.0, + 6.0, + 6.0, + 6.0, + 6.0, + 6.0 + ] + }, + "torqueLimit": 20, + "qvelFilter": 0.9, + "desiredJointAngles": null, + "desiredJointVelocities": null, + "desiredJointAcceleration": null +} diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index ef252504..2c9bab92 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1 +1,2 @@ add_subdirectory(components/njoint_ts_impedance_controller_example) +add_subdirectory(components/njoint_js_impedance_controller_example) diff --git a/examples/components/njoint_js_impedance_controller_example/CMakeLists.txt b/examples/components/njoint_js_impedance_controller_example/CMakeLists.txt new file mode 100644 index 00000000..cec337e6 --- /dev/null +++ b/examples/components/njoint_js_impedance_controller_example/CMakeLists.txt @@ -0,0 +1,17 @@ +armarx_add_component(njoint_js_impedance_controller_example + SOURCES + Component.cpp + HEADERS + Component.h + DEPENDENCIES + ArmarXCore + # ArmarXCoreComponentPlugins # For DebugObserver plugin. ArmarXGui + # ArmarXGuiComponentPlugins # For RemoteGui plugin. RobotAPI + # RobotAPICore RobotAPIInterfaces RobotAPIArmarXObjects + # RobotAPIComponentPlugins # For ArViz and other plugins. Navigation + armem_robot_state + # armem_robot + armarx_control::client + Simox::SimoxUtility + Eigen3::Eigen +) diff --git a/examples/components/njoint_js_impedance_controller_example/Component.cpp b/examples/components/njoint_js_impedance_controller_example/Component.cpp new file mode 100644 index 00000000..5515a363 --- /dev/null +++ b/examples/components/njoint_js_impedance_controller_example/Component.cpp @@ -0,0 +1,213 @@ +/** + * 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_control::ArmarXObjects::NJointControllerExample + * @author Fabian Reister ( fabian dot reister at kit dot edu ) + * @date 2023 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + + +#include "Component.h" + +#include <chrono> +#include <cmath> +#include <filesystem> +#include <thread> + +#include <Eigen/Geometry> + +#include "ArmarXCore/core/time/Metronome.h" +#include <ArmarXCore/core/logging/Logging.h> +#include <ArmarXCore/core/time/Clock.h> +#include <ArmarXCore/core/time/ClockType.h> +#include <ArmarXCore/core/time/forward_declarations.h> +#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h> + +namespace armarx::control::components::njoint_js_impedance_controller_example +{ + + Component::Component() + { + addPlugin(virtualRobotReaderPlugin); + } + + Component::~Component() = default; + + void + Component::onInitComponent() + { + // pass + } + + void + Component::onConnectComponent() + { + robot = virtualRobotReaderPlugin->get().getRobot(properties.robotName); + + ARMARX_CHECK_NOT_NULL(robot); + ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, Clock::Now())); + + task = new armarx::RunningTask<Component>(this, &Component::runExample); + task->start(); + } + + void + Component::onDisconnectComponent() + { + const bool join = true; + task->stop(join); + } + + void + Component::onExitComponent() + { + // pass + } + + std::string + Component::getDefaultName() const + { + return "NJointJSImpedanceControllerExample"; + } + + void + Component::runExample() + { + auto robotUnit = getControlComponentPlugin().getRobotUnitPlugin().getRobotUnit(); + + // Load the library that our controller is defined in. + robotUnit->loadLibFromPackage("armarx_control", "libarmarx_control_njoint_controller.so"); + + // We use a ControllerBuilder to configure the controller. This controller builder is a convenient and safe (!) + // way to create a NJointController. + + // There are various options of how to set the configuration: + // (1) load a pre-defined configuration file + // (2) set the configuration programatically + + auto builder = controllerBuilder<ControllerType>(); + builder.withNodeSet(properties.rns); + + // load pre-defined config from file + { + const std::string controllerClassName = + control::common::ControllerTypeNames.to_name(ControllerType); + + const armarx::PackagePath configPath("armarx_control", + "controller_config/" + controllerClassName + "/" + + properties.configName + ".json"); + + ARMARX_IMPORTANT << "Using config `" << configPath.toSystemPath() << "`."; + ARMARX_CHECK(std::filesystem::exists(configPath.toSystemPath())); + builder.withConfig(configPath.toSystemPath()); + + ARMARX_INFO << VAROUT(builder.config().kpImpedance); + ARMARX_INFO << VAROUT(builder.config().kdImpedance); + + // ARMARX_INFO << VAROUT(builder.config().torqueLimit); + // ARMARX_INFO << VAROUT(builder.config().qvelFilter); + } + + ARMARX_CHECK_NOT_NULL(robot); + ARMARX_CHECK( + virtualRobotReaderPlugin->get().synchronizeRobot(*robot, armarx::Clock::Now())); + /// !!!! + /// you have to wait for enough time to let the clone finish. Otherwise, you may get random / wrong values below when you read the joint angles and TCP pose, etc. + /// !!!! + Clock::WaitFor(Duration::MilliSeconds(1000)); + + const auto rns = robot->getRobotNodeSet(properties.rns); + + // Set configuration programmatically. Use this for those parts of the config that cannot be specified in advance. + // builder.config().desiredNullspaceJointAngles = rns->getJointValuesEigen(); + ARMARX_INFO << VAROUT(rns->getJointValuesEigen()); + builder.config().desiredJointAngles = rns->getJointValuesEigen(); + + builder.config().desiredJointVelocities = builder.config().desiredJointAngles; + builder.config().desiredJointVelocities->setZero(); + + // ARMARX_INFO << VAROUT(builder.config().desiredPose); + + ARMARX_TRACE; + ARMARX_INFO << "Creating controller"; + auto ctrlWrapper = builder.create(); + + // The controller is scoped by default, which means that if the ControllerWrapper goes out of scope, it will deactivate + // and delete the NJointController. To prevent this, you can use the daemonize() method if needed. + ctrlWrapper->daemonize(true); + + ARMARX_TRACE; + ARMARX_CHECK(ctrlWrapper.has_value()); + ctrl.emplace(std::move(ctrlWrapper.value())); + + Clock::WaitFor(Duration::MilliSeconds(300)); + ARMARX_INFO << "Activating controller"; + ctrl->updateConfig(); + ctrl->activate(); + + // Now the controller is active. In our example, the arm should now be compliant. + // To move the arm, we can update the config as shown below. In this example, + // the robot will execute a circular motion. + + const Eigen::Isometry3f robot_T_tcp_initial(rns->getTCP()->getPoseInRootFrame()); + + Metronome metronome(Frequency::Hertz(20)); + ARMARX_INFO << "Starting movement"; + + const auto start = Clock::Now(); + while (Clock::Now() - start < Duration::Seconds(properties.movementDurationSeconds)) + { + // const float t = (Clock::Now() - start).toSecondsDouble(); + // const float T = properties.movementDurationSeconds; + // const float x = 2 * M_PIf32 * t / T; + + // const Eigen::Vector3f posOffset{ + // properties.movementRadius * std::sin(x), std::cos(x) - 1, 0}; + + // const Eigen::Isometry3f global_T_tcp_desired = + // Eigen::Translation3f(posOffset) * robot_T_tcp_initial; + + // // update config + // ctrl->config.desiredPose = global_T_tcp_desired.matrix(); + + // // send new config to controller + // ctrl->updateConfig(); + + metronome.waitForNextTick(); + } + + ctrl->deactivate(); + } + + armarx::PropertyDefinitionsPtr + Component::createPropertyDefinitions() + { + armarx::PropertyDefinitionsPtr def = + new armarx::ComponentPropertyDefinitions(getConfigIdentifier()); + + def->optional(properties.robotName, "robotName"); + def->optional(properties.configName, "configName"); + def->optional(properties.movementRadius, "movementRadius"); + def->optional(properties.movementDurationSeconds, "movementDurationSeconds"); + + + return def; + } + + ARMARX_DECOUPLED_REGISTER_COMPONENT(Component); + +} // namespace armarx::control::components::njoint_js_impedance_controller_example diff --git a/examples/components/njoint_js_impedance_controller_example/Component.h b/examples/components/njoint_js_impedance_controller_example/Component.h new file mode 100644 index 00000000..0c4b84a2 --- /dev/null +++ b/examples/components/njoint_js_impedance_controller_example/Component.h @@ -0,0 +1,113 @@ +/** + * 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_control::ArmarXObjects::NJointControllerExample + * @author Fabian Reister ( fabian dot reister at kit dot edu ) + * @date 2023 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + + +#pragma once + + +#include <VirtualRobot/Robot.h> + +#include <ArmarXCore/util/tasks.h> + +#include <RobotAPI/libraries/armem/client/plugins.h> +#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h> +#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h> + +#include <armarx/control/client/ComponentPlugin.h> +#include <armarx/control/client/ControllerBuilder.h> +#include <armarx/control/common/type.h> +#include <armarx/control/njoint_controller/controller_descriptions.h> + +namespace armarx::control::components::njoint_js_impedance_controller_example +{ + + inline const control::common::ControllerType ControllerType = + control::common::ControllerType::JSImpedance; + + /** + * @defgroup Component-ExampleClient ExampleClient + * @ingroup armarx_navigation-Components + * A description of the component ExampleClient. + * + * @class ExampleClient + * @ingroup Component-ExampleClient + * @brief Brief description of class ExampleClient. + * + * Detailed description of class ExampleClient. + */ + class Component : + virtual public armarx::Component, + virtual public armarx::control::client::ComponentPluginUser, + virtual public armem::ListeningClientPluginUser + { + + public: + Component(); + + ~Component() override; + + protected: + /// @see PropertyUser::createPropertyDefinitions() + armarx::PropertyDefinitionsPtr createPropertyDefinitions() override; + + /// @see armarx::ManagedIceObject::onInitComponent() + void onInitComponent() override; + + /// @see armarx::ManagedIceObject::onConnectComponent() + void onConnectComponent() override; + + /// @see armarx::ManagedIceObject::onDisconnectComponent() + void onDisconnectComponent() override; + + /// @see armarx::ManagedIceObject::onExitComponent() + void onExitComponent() override; + + std::string getDefaultName() const override; + + + void runExample(); + + private: + void goalReached(); + + struct + { + std::string robotName = "Armar6"; + std::string rns = "RightArm"; + + std::string configName = "default"; + + float movementRadius = 200; // [mm] + float movementDurationSeconds = 20; // [s] + } properties; + + armarx::RunningTask<Component>::pointer_type task; + + armem::client::plugins::ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* + virtualRobotReaderPlugin = nullptr; + + VirtualRobot::RobotPtr robot; + + std::optional<control::client::ControllerWrapper<ControllerType>> ctrl; + }; + +} // namespace armarx::control::components::njoint_js_impedance_controller_example diff --git a/examples/components/njoint_ts_impedance_controller_example/CMakeLists.txt b/examples/components/njoint_ts_impedance_controller_example/CMakeLists.txt index 513f81c9..20fe147a 100644 --- a/examples/components/njoint_ts_impedance_controller_example/CMakeLists.txt +++ b/examples/components/njoint_ts_impedance_controller_example/CMakeLists.txt @@ -1,4 +1,4 @@ -armarx_add_component(njoint_controller_example +armarx_add_component(njoint_ts_impedance_controller_example SOURCES Component.cpp HEADERS diff --git a/examples/components/njoint_ts_impedance_controller_example/Component.cpp b/examples/components/njoint_ts_impedance_controller_example/Component.cpp index adcf9a75..30db8bc5 100644 --- a/examples/components/njoint_ts_impedance_controller_example/Component.cpp +++ b/examples/components/njoint_ts_impedance_controller_example/Component.cpp @@ -81,7 +81,7 @@ namespace armarx::control::components::njoint_ts_impedance_controller_example std::string Component::getDefaultName() const { - return "NJointControllerExample"; + return "NJointTSImpedanceControllerExample"; } void diff --git a/scenarios/NJointJointspaceImpedanceControllerExample/NJointJointspaceImpedanceControllerExample.scx b/scenarios/NJointJointspaceImpedanceControllerExample/NJointJointspaceImpedanceControllerExample.scx new file mode 100644 index 00000000..6e21e0d2 --- /dev/null +++ b/scenarios/NJointJointspaceImpedanceControllerExample/NJointJointspaceImpedanceControllerExample.scx @@ -0,0 +1,6 @@ +<?xml version="1.0" encoding="utf-8"?> +<scenario name="NJointJointspaceImpedanceControllerExample" creation="2023-03-23.15:51:48" globalConfigName="./config/global.cfg" package="armarx_control" deploymentType="local" nodeName="NodeMain"> + <application name="njoint_js_impedance_controller_example" instance="" package="armarx_control" nodeName="" enabled="true" iceAutoRestart="false"/> + <application name="control_memory" instance="" package="armarx_control" nodeName="" enabled="true" iceAutoRestart="false"/> +</scenario> + diff --git a/scenarios/NJointJointspaceImpedanceControllerExample/config/control_memory.cfg b/scenarios/NJointJointspaceImpedanceControllerExample/config/control_memory.cfg new file mode 100644 index 00000000..53efd6fe --- /dev/null +++ b/scenarios/NJointJointspaceImpedanceControllerExample/config/control_memory.cfg @@ -0,0 +1,272 @@ +# ================================================================== +# control_memory properties +# ================================================================== + +# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List. +# Attributes: +# - Default: Default value not mapped. +# - Case sensitivity: yes +# - Required: no +# ArmarX.AdditionalPackages = Default value not mapped. + + +# ArmarX.ApplicationName: Application name +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.ApplicationName = "" + + +# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config) +# Attributes: +# - Default: mongo/.cache +# - Case sensitivity: yes +# - Required: no +# ArmarX.CachePath = mongo/.cache + + +# ArmarX.Config: Comma-separated list of configuration files +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.Config = "" + + +# ArmarX.ControlMemory.ArVizStorageName: Name of the ArViz storage +# Attributes: +# - Default: ArVizStorage +# - Case sensitivity: yes +# - Required: no +# ArmarX.ControlMemory.ArVizStorageName = ArVizStorage + + +# ArmarX.ControlMemory.ArVizTopicName: Name of the ArViz topic +# Attributes: +# - Default: ArVizTopic +# - Case sensitivity: yes +# - Required: no +# ArmarX.ControlMemory.ArVizTopicName = ArVizTopic + + +# ArmarX.ControlMemory.EnableProfiling: enable profiler which is used for logging performance events +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.ControlMemory.EnableProfiling = false + + +# ArmarX.ControlMemory.MinimumLoggingLevel: Local logging level only for this component +# Attributes: +# - Default: Undefined +# - Case sensitivity: yes +# - Required: no +# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning} +# ArmarX.ControlMemory.MinimumLoggingLevel = Undefined + + +# ArmarX.ControlMemory.ObjectName: Name of IceGrid well-known object +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.ControlMemory.ObjectName = "" + + +# ArmarX.ControlMemory.RemoteGuiName: Name of the remote gui provider +# Attributes: +# - Default: RemoteGuiProvider +# - Case sensitivity: yes +# - Required: no +# ArmarX.ControlMemory.RemoteGuiName = RemoteGuiProvider + + +# ArmarX.ControlMemory.mem.MemoryName: Name of this memory server. +# Attributes: +# - Default: Control +# - Case sensitivity: yes +# - Required: no +# ArmarX.ControlMemory.mem.MemoryName = Control + + +# ArmarX.ControlMemory.mem.ltm.configuration: +# Attributes: +# - Default: {} +# - Case sensitivity: yes +# - Required: no +# ArmarX.ControlMemory.mem.ltm.configuration = {} + + +# ArmarX.ControlMemory.mem.ltm.enabled: +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.ControlMemory.mem.ltm.enabled = false + + +# ArmarX.ControlMemory.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS). +# Set to false to use this memory as a stand-alone. +# Attributes: +# - Default: true +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.ControlMemory.mns.MemoryNameSystemEnabled = true + + +# ArmarX.ControlMemory.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component. +# Attributes: +# - Default: MemoryNameSystem +# - Case sensitivity: yes +# - Required: no +# ArmarX.ControlMemory.mns.MemoryNameSystemName = MemoryNameSystem + + +# ArmarX.ControlMemory.p.locationGraph.visuFrequency: Visualization frequeny of locations and graph edges [Hz]. +# Attributes: +# - Default: 2 +# - Case sensitivity: yes +# - Required: no +# ArmarX.ControlMemory.p.locationGraph.visuFrequency = 2 + + +# ArmarX.ControlMemory.p.snapshotToLoad: Memory snapshot to load at start up +# (e.g. 'PriorKnowledgeData/navigation-graphs/snapshot'). +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.ControlMemory.p.snapshotToLoad = "" + + +# ArmarX.DataPath: Semicolon-separated search list for data files +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.DataPath = "" + + +# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'. +# Attributes: +# - Default: Default value not mapped. +# - Case sensitivity: yes +# - Required: no +# ArmarX.DefaultPackages = Default value not mapped. + + +# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited. +# Attributes: +# - Default: ./config/dependencies.cfg +# - Case sensitivity: yes +# - Required: no +# ArmarX.DependenciesConfig = ./config/dependencies.cfg + + +# ArmarX.DisableLogging: Turn logging off in whole application +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.DisableLogging = false + + +# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.EnableProfiling = false + + +# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;... +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.LoadLibraries = "" + + +# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI. +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.LoggingGroup = "" + + +# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog +# Attributes: +# - Default: true +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.RedirectStdout = true + + +# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles) +# Attributes: +# - Default: 3000 +# - Case sensitivity: yes +# - Required: no +# ArmarX.RemoteHandlesDeletionTimeout = 3000 + + +# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging) +# Attributes: +# - Default: 0 +# - Case sensitivity: yes +# - Required: no +# ArmarX.SecondsStartupDelay = 0 + + +# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger. +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.StartDebuggerOnCrash = false + + +# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running. +# Attributes: +# - Default: 1 +# - Case sensitivity: yes +# - Required: no +# ArmarX.ThreadPoolSize = 1 + + +# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes. +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.TopicSuffix = "" + + +# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator) +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.UseTimeServer = false + + +# ArmarX.Verbosity: Global logging level for whole application +# Attributes: +# - Default: Info +# - Case sensitivity: yes +# - Required: no +# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning} +# ArmarX.Verbosity = Info + + diff --git a/scenarios/NJointJointspaceImpedanceControllerExample/config/global.cfg b/scenarios/NJointJointspaceImpedanceControllerExample/config/global.cfg new file mode 100644 index 00000000..64c1f595 --- /dev/null +++ b/scenarios/NJointJointspaceImpedanceControllerExample/config/global.cfg @@ -0,0 +1,4 @@ +# ================================================================== +# Global Config from Scenario NJointJointspaceImpedanceControllerExample +# ================================================================== + diff --git a/scenarios/NJointJointspaceImpedanceControllerExample/config/njoint_js_impedance_controller_example.cfg b/scenarios/NJointJointspaceImpedanceControllerExample/config/njoint_js_impedance_controller_example.cfg new file mode 100644 index 00000000..af976796 --- /dev/null +++ b/scenarios/NJointJointspaceImpedanceControllerExample/config/njoint_js_impedance_controller_example.cfg @@ -0,0 +1,261 @@ +# ================================================================== +# njoint_js_impedance_controller_example properties +# ================================================================== + +# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List. +# Attributes: +# - Default: Default value not mapped. +# - Case sensitivity: yes +# - Required: no +# ArmarX.AdditionalPackages = Default value not mapped. + + +# ArmarX.ApplicationName: Application name +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.ApplicationName = "" + + +# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config) +# Attributes: +# - Default: mongo/.cache +# - Case sensitivity: yes +# - Required: no +# ArmarX.CachePath = mongo/.cache + + +# ArmarX.Config: Comma-separated list of configuration files +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.Config = "" + + +# ArmarX.DataPath: Semicolon-separated search list for data files +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.DataPath = "" + + +# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'. +# Attributes: +# - Default: Default value not mapped. +# - Case sensitivity: yes +# - Required: no +# ArmarX.DefaultPackages = Default value not mapped. + + +# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited. +# Attributes: +# - Default: ./config/dependencies.cfg +# - Case sensitivity: yes +# - Required: no +# ArmarX.DependenciesConfig = ./config/dependencies.cfg + + +# ArmarX.DisableLogging: Turn logging off in whole application +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.DisableLogging = false + + +# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.EnableProfiling = false + + +# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;... +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.LoadLibraries = "" + + +# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI. +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.LoggingGroup = "" + + +# ArmarX.NJointJSImpedanceControllerExample.EnableProfiling: enable profiler which is used for logging performance events +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.NJointJSImpedanceControllerExample.EnableProfiling = false + + +# ArmarX.NJointJSImpedanceControllerExample.MinimumLoggingLevel: Local logging level only for this component +# Attributes: +# - Default: Undefined +# - Case sensitivity: yes +# - Required: no +# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning} +# ArmarX.NJointJSImpedanceControllerExample.MinimumLoggingLevel = Undefined + + +# ArmarX.NJointJSImpedanceControllerExample.ObjectName: Name of IceGrid well-known object +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.NJointJSImpedanceControllerExample.ObjectName = "" + + +# ArmarX.NJointJSImpedanceControllerExample.RobotUnitName: Name of the RobotUnit +# Attributes: +# - Case sensitivity: yes +# - Required: yes +ArmarX.NJointJSImpedanceControllerExample.RobotUnitName = Armar6Unit + + +# ArmarX.NJointJSImpedanceControllerExample.configName: +# Attributes: +# - Default: default +# - Case sensitivity: yes +# - Required: no +# ArmarX.NJointJSImpedanceControllerExample.configName = default + + +# ArmarX.NJointJSImpedanceControllerExample.mem.robot_state.Memory: +# Attributes: +# - Default: RobotState +# - Case sensitivity: yes +# - Required: no +# ArmarX.NJointJSImpedanceControllerExample.mem.robot_state.Memory = RobotState + + +# ArmarX.NJointJSImpedanceControllerExample.mem.robot_state.localizationSegment: Name of the localization memory core segment to use. +# Attributes: +# - Default: Localization +# - Case sensitivity: yes +# - Required: no +# ArmarX.NJointJSImpedanceControllerExample.mem.robot_state.localizationSegment = Localization + + +# ArmarX.NJointJSImpedanceControllerExample.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS). +# Set to false to use this memory as a stand-alone. +# Attributes: +# - Default: true +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.NJointJSImpedanceControllerExample.mns.MemoryNameSystemEnabled = true + + +# ArmarX.NJointJSImpedanceControllerExample.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component. +# Attributes: +# - Default: MemoryNameSystem +# - Case sensitivity: yes +# - Required: no +# ArmarX.NJointJSImpedanceControllerExample.mns.MemoryNameSystemName = MemoryNameSystem + + +# ArmarX.NJointJSImpedanceControllerExample.movementDurationSeconds: +# Attributes: +# - Default: 20 +# - Case sensitivity: yes +# - Required: no +# ArmarX.NJointJSImpedanceControllerExample.movementDurationSeconds = 20 + + +# ArmarX.NJointJSImpedanceControllerExample.movementRadius: +# Attributes: +# - Default: 200 +# - Case sensitivity: yes +# - Required: no +# ArmarX.NJointJSImpedanceControllerExample.movementRadius = 200 + + +# ArmarX.NJointJSImpedanceControllerExample.robotName: +# Attributes: +# - Default: Armar6 +# - Case sensitivity: yes +# - Required: no +# ArmarX.NJointJSImpedanceControllerExample.robotName = Armar6 + + +# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog +# Attributes: +# - Default: true +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.RedirectStdout = true + + +# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles) +# Attributes: +# - Default: 3000 +# - Case sensitivity: yes +# - Required: no +# ArmarX.RemoteHandlesDeletionTimeout = 3000 + + +# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging) +# Attributes: +# - Default: 0 +# - Case sensitivity: yes +# - Required: no +# ArmarX.SecondsStartupDelay = 0 + + +# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger. +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.StartDebuggerOnCrash = false + + +# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running. +# Attributes: +# - Default: 1 +# - Case sensitivity: yes +# - Required: no +# ArmarX.ThreadPoolSize = 1 + + +# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes. +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.TopicSuffix = "" + + +# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator) +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.UseTimeServer = false + + +# ArmarX.Verbosity: Global logging level for whole application +# Attributes: +# - Default: Info +# - Case sensitivity: yes +# - Required: no +# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning} +# ArmarX.Verbosity = Info + + diff --git a/source/armarx/control/common/CMakeLists.txt b/source/armarx/control/common/CMakeLists.txt index 12be8f79..d0ce2057 100644 --- a/source/armarx/control/common/CMakeLists.txt +++ b/source/armarx/control/common/CMakeLists.txt @@ -3,6 +3,7 @@ armarx_add_aron_library(common_aron ft/aron/FTConfig.xml mp/aron/MPConfig.xml control_law/aron/TaskspaceImpedanceControllerConfig.xml + control_law/aron/JointspaceImpedanceControllerConfig.xml control_law/aron/TaskspaceAdmittanceControllerConfig.xml control_law/aron/KeypointControllerConfig.xml ) diff --git a/source/armarx/control/common/aron_conversions.cpp b/source/armarx/control/common/aron_conversions.cpp index f4e3a525..f2c3c0f6 100644 --- a/source/armarx/control/common/aron_conversions.cpp +++ b/source/armarx/control/common/aron_conversions.cpp @@ -21,43 +21,40 @@ #include "aron_conversions.h" +#include "armarx/control/common/control_law/JointSpaceImpedanceController.h" namespace armarx::control::common::ft { void - fromAron(const arondto::FTConfig &dto, FTSensor::FTConfig &bo) + fromAron(const arondto::FTConfig& dto, FTSensor::FTConfig& bo) { - bo = FTSensor::FTConfig{ - .sensorName = dto.sensorName, - .ftFilter = dto.ftFilter, - .deadZoneForce = dto.deadZoneForce, - .deadZoneTorque = dto.deadZoneTorque, - .timeLimit = dto.timeLimit, - .enableTCPGravityCompensation = dto.enableTCPGravityCompensation, - .reCalibrate = dto.reCalibrate, - .forceBaseline = dto.forceBaseline, - .torqueBaseline = dto.torqueBaseline - }; + bo = FTSensor::FTConfig{.sensorName = dto.sensorName, + .ftFilter = dto.ftFilter, + .deadZoneForce = dto.deadZoneForce, + .deadZoneTorque = dto.deadZoneTorque, + .timeLimit = dto.timeLimit, + .enableTCPGravityCompensation = dto.enableTCPGravityCompensation, + .reCalibrate = dto.reCalibrate, + .forceBaseline = dto.forceBaseline, + .torqueBaseline = dto.torqueBaseline}; } FTSensor::FTConfig - fromAron(const arondto::FTConfig &dto) + fromAron(const arondto::FTConfig& dto) { - FTSensor::FTConfig ftConfig = FTSensor::FTConfig{ - .sensorName = dto.sensorName, - .ftFilter = dto.ftFilter, - .deadZoneForce = dto.deadZoneForce, - .deadZoneTorque = dto.deadZoneTorque, - .timeLimit = dto.timeLimit, - .enableTCPGravityCompensation = dto.enableTCPGravityCompensation, - .reCalibrate = dto.reCalibrate, - .forceBaseline = dto.forceBaseline, - .torqueBaseline = dto.torqueBaseline - }; + FTSensor::FTConfig ftConfig = + FTSensor::FTConfig{.sensorName = dto.sensorName, + .ftFilter = dto.ftFilter, + .deadZoneForce = dto.deadZoneForce, + .deadZoneTorque = dto.deadZoneTorque, + .timeLimit = dto.timeLimit, + .enableTCPGravityCompensation = dto.enableTCPGravityCompensation, + .reCalibrate = dto.reCalibrate, + .forceBaseline = dto.forceBaseline, + .torqueBaseline = dto.torqueBaseline}; return ftConfig; } -} /// namespace armarx::control::common::ft - +} // namespace armarx::control::common::ft namespace armarx::control::common::control_law { @@ -65,72 +62,80 @@ namespace armarx::control::common::control_law fromAron(const arondto::TaskspaceImpedanceControllerConfig& dto, TaskspaceImpedanceController::Config& bo) { - bo = TaskspaceImpedanceController::Config{ - .kpImpedance = dto.kpImpedance, - .kdImpedance = dto.kdImpedance, - .kpNullspace = dto.kpNullspace, - .kdNullspace = dto.kdNullspace, - .desiredPose = dto.desiredPose, - .desiredTwist = dto.desiredTwist, - .desiredNullspaceJointAngles = dto.desiredNullspaceJointAngles, - .torqueLimit = dto.torqueLimit, - .qvelFilter = dto.qvelFilter - }; + bo = TaskspaceImpedanceController::Config{.kpImpedance = dto.kpImpedance, + .kdImpedance = dto.kdImpedance, + .kpNullspace = dto.kpNullspace, + .kdNullspace = dto.kdNullspace, + .desiredPose = dto.desiredPose, + .desiredTwist = dto.desiredTwist, + .desiredNullspaceJointAngles = + dto.desiredNullspaceJointAngles, + .torqueLimit = dto.torqueLimit, + .qvelFilter = dto.qvelFilter}; } void - fromAron(const arondto::TaskspaceAdmittanceControllerConfig &dto, - TaskspaceAdmittanceController::Config &bo) + fromAron(const arondto::TaskspaceAdmittanceControllerConfig& dto, + TaskspaceAdmittanceController::Config& bo) { auto ftBO = armarx::control::common::ft::fromAron(dto.ftConfig); - bo = TaskspaceAdmittanceController::Config{ - .kpImpedance = dto.kpImpedance, - .kdImpedance = dto.kdImpedance, - .kpAdmittance = dto.kpAdmittance, - .kdAdmittance = dto.kdAdmittance, - .kmAdmittance = dto.kmAdmittance, - .kpNullspace = dto.kpNullspace, - .kdNullspace = dto.kdNullspace, - .desiredPose = dto.desiredPose, - .desiredTwist = dto.desiredTwist, - .desiredNullspaceJointAngles = dto.desiredNullspaceJointAngles, - .torqueLimit = dto.torqueLimit, - .qvelFilter = dto.qvelFilter, - .ftConfig = ftBO - }; + bo = TaskspaceAdmittanceController::Config{.kpImpedance = dto.kpImpedance, + .kdImpedance = dto.kdImpedance, + .kpAdmittance = dto.kpAdmittance, + .kdAdmittance = dto.kdAdmittance, + .kmAdmittance = dto.kmAdmittance, + .kpNullspace = dto.kpNullspace, + .kdNullspace = dto.kdNullspace, + .desiredPose = dto.desiredPose, + .desiredTwist = dto.desiredTwist, + .desiredNullspaceJointAngles = + dto.desiredNullspaceJointAngles, + .torqueLimit = dto.torqueLimit, + .qvelFilter = dto.qvelFilter, + .ftConfig = ftBO}; } void - fromAron(const arondto::KeypointControllerConfig& dto, - KeypointsImpedanceController::Config& bo) + fromAron(const arondto::JointspaceImpedanceControllerConfig& dto, + JointSpaceImpedanceController::Config& bo) { - bo = KeypointsImpedanceController::Config{ - .kpImpedance = dto.kpImpedance, - .kdImpedance = dto.kdImpedance, -// .kpAdmittance = dto.kpAdmittance, - .kdAdmittance = dto.kdAdmittance, - .kmAdmittance = dto.kmAdmittance, - .kpNullspace = dto.kpNullspace, - .kdNullspace = dto.kdNullspace, -// .currentForceTorque = dto.currentForceTorque, -// .desiredPose = dto.desiredPose, -// .desiredTwist = dto.desiredTwist, - .desiredNullspaceJointAngles = dto.desiredNullspaceJointAngles, - .torqueLimit = dto.torqueLimit, - .qvelFilter = dto.qvelFilter, - .numPoints = dto.numPoints, - .keypointKp = dto.keypointKp, - .keypointKd = dto.keypointKd, - .fixedTranslation = dto.fixedTranslation, - .densityForceScaling = dto.densityForceScaling, - .keypointPositionFilter = dto.keypointPositionFilter, - .keypointVelocityFilter = dto.keypointVelocityFilter, - .isRigid = dto.isRigid + bo = JointSpaceImpedanceController::Config{ + .kp = dto.kpImpedance, + .kd = dto.kdImpedance, + .desiredJointPos = dto.desiredJointAngles, + .desiredJointVel = dto.desiredJointVelocities, + .desiredJointAcc = dto.desiredJointAcceleration }; } -} /// namespace armarx::control::common::control_law + void + fromAron(const arondto::KeypointControllerConfig& dto, KeypointsImpedanceController::Config& bo) + { + bo = KeypointsImpedanceController::Config{ + .kpImpedance = dto.kpImpedance, + .kdImpedance = dto.kdImpedance, + // .kpAdmittance = dto.kpAdmittance, + .kdAdmittance = dto.kdAdmittance, + .kmAdmittance = dto.kmAdmittance, + .kpNullspace = dto.kpNullspace, + .kdNullspace = dto.kdNullspace, + // .currentForceTorque = dto.currentForceTorque, + // .desiredPose = dto.desiredPose, + // .desiredTwist = dto.desiredTwist, + .desiredNullspaceJointAngles = dto.desiredNullspaceJointAngles, + .torqueLimit = dto.torqueLimit, + .qvelFilter = dto.qvelFilter, + .numPoints = dto.numPoints, + .keypointKp = dto.keypointKp, + .keypointKd = dto.keypointKd, + .fixedTranslation = dto.fixedTranslation, + .densityForceScaling = dto.densityForceScaling, + .keypointPositionFilter = dto.keypointPositionFilter, + .keypointVelocityFilter = dto.keypointVelocityFilter, + .isRigid = dto.isRigid}; + } +} // namespace armarx::control::common::control_law namespace armarx::control::common::mp { @@ -148,108 +153,94 @@ namespace armarx::control::common::mp } void - fromAron(const arondto::ListViaPoint &dto, MP::ListViaPoint &bo) + fromAron(const arondto::ListViaPoint& dto, MP::ListViaPoint& bo) { - bo = MP::ListViaPoint{ - .canonicalValue = dto.canonicalValue, - .viaPointValue = dto.viaPointValue - }; + bo = MP::ListViaPoint{.canonicalValue = dto.canonicalValue, + .viaPointValue = dto.viaPointValue}; } void - fromAron(const arondto::DictViaPoint &dto, MP::DictViaPoint &bo) + fromAron(const arondto::DictViaPoint& dto, MP::DictViaPoint& bo) { - bo = MP::DictViaPoint{ - .canonicalValue = dto.canonicalValue, - .viaPointValue = dto.viaPointValue - }; + bo = MP::DictViaPoint{.canonicalValue = dto.canonicalValue, + .viaPointValue = dto.viaPointValue}; } void - fromAron(const arondto::MPConfig &dto, MP::MPConfig &bo) + fromAron(const arondto::MPConfig& dto, MP::MPConfig& bo) { std::vector<MP::ListViaPoint> viaPoints; - std::transform(dto.viaPoints.begin(), dto.viaPoints.end(), std::back_inserter(viaPoints), - [](const auto& vp) -> MP::ListViaPoint - { - return fromAron(vp); - }); - bo = MP::MPConfig{ - .name = dto.name, - .className = dto.className, - .mpTypeString = dto.mpTypeString, - .mpMode = dto.mpMode, - .mpStyle = dto.mpStyle, - .regressionModel = dto.regressionModel, - .kernelSize = dto.kernelSize, - .damping = dto.damping, - .tau = dto.tau, - .amplitude = dto.amplitude, - .durationSec = dto.durationSec, - .fileList = dto.fileList, - .stopWithMP = dto.stopWithMP, - .viaPoints = viaPoints, - .enablePhaseStop = dto.enablePhaseStop, /// phase stop - .maxValue = dto.maxValue, - .slop = dto.slop, - .goDist = dto.goDist, - .backDist = dto.backDist, - .psKpPos = dto.psKpPos, - .psKdPos = dto.psKdPos, - .psKpOri = dto.psKpOri, - .psKdOri = dto.psKdOri, - .psMM2Radian = dto.psMM2Radian - }; + std::transform(dto.viaPoints.begin(), + dto.viaPoints.end(), + std::back_inserter(viaPoints), + [](const auto& vp) -> MP::ListViaPoint { return fromAron(vp); }); + bo = MP::MPConfig{.name = dto.name, + .className = dto.className, + .mpTypeString = dto.mpTypeString, + .mpMode = dto.mpMode, + .mpStyle = dto.mpStyle, + .regressionModel = dto.regressionModel, + .kernelSize = dto.kernelSize, + .damping = dto.damping, + .tau = dto.tau, + .amplitude = dto.amplitude, + .durationSec = dto.durationSec, + .fileList = dto.fileList, + .stopWithMP = dto.stopWithMP, + .viaPoints = viaPoints, + .enablePhaseStop = dto.enablePhaseStop, /// phase stop + .maxValue = dto.maxValue, + .slop = dto.slop, + .goDist = dto.goDist, + .backDist = dto.backDist, + .psKpPos = dto.psKpPos, + .psKdPos = dto.psKdPos, + .psKpOri = dto.psKpOri, + .psKdOri = dto.psKdOri, + .psMM2Radian = dto.psMM2Radian}; } MP::MPConfig - fromAron(const arondto::MPConfig &dto) + fromAron(const arondto::MPConfig& dto) { std::vector<MP::ListViaPoint> viaPoints; - std::transform(dto.viaPoints.begin(), dto.viaPoints.end(), std::back_inserter(viaPoints), - [](const auto& vp) -> MP::ListViaPoint - { - return fromAron(vp); - }); - MP::MPConfig mpConfig = MP::MPConfig{ - .name = dto.name, - .className = dto.className, - .mpTypeString = dto.mpTypeString, - .mpMode = dto.mpMode, - .mpStyle = dto.mpStyle, - .regressionModel = dto.regressionModel, - .kernelSize = dto.kernelSize, - .damping = dto.damping, - .tau = dto.tau, - .amplitude = dto.amplitude, - .durationSec = dto.durationSec, - .fileList = dto.fileList, - .stopWithMP = dto.stopWithMP, - .viaPoints = viaPoints, - .enablePhaseStop = dto.enablePhaseStop, /// phase stop - .maxValue = dto.maxValue, - .slop = dto.slop, - .goDist = dto.goDist, - .backDist = dto.backDist, - .psKpPos = dto.psKpPos, - .psKdPos = dto.psKdPos, - .psKpOri = dto.psKpOri, - .psKdOri = dto.psKdOri, - .psMM2Radian = dto.psMM2Radian - }; + std::transform(dto.viaPoints.begin(), + dto.viaPoints.end(), + std::back_inserter(viaPoints), + [](const auto& vp) -> MP::ListViaPoint { return fromAron(vp); }); + MP::MPConfig mpConfig = MP::MPConfig{.name = dto.name, + .className = dto.className, + .mpTypeString = dto.mpTypeString, + .mpMode = dto.mpMode, + .mpStyle = dto.mpStyle, + .regressionModel = dto.regressionModel, + .kernelSize = dto.kernelSize, + .damping = dto.damping, + .tau = dto.tau, + .amplitude = dto.amplitude, + .durationSec = dto.durationSec, + .fileList = dto.fileList, + .stopWithMP = dto.stopWithMP, + .viaPoints = viaPoints, + .enablePhaseStop = dto.enablePhaseStop, /// phase stop + .maxValue = dto.maxValue, + .slop = dto.slop, + .goDist = dto.goDist, + .backDist = dto.backDist, + .psKpPos = dto.psKpPos, + .psKdPos = dto.psKdPos, + .psKpOri = dto.psKpOri, + .psKdOri = dto.psKdOri, + .psMM2Radian = dto.psMM2Radian}; return mpConfig; } void - fromAron(const arondto::MPListConfig &dto, MP::MPListConfig &bo) + fromAron(const arondto::MPListConfig& dto, MP::MPListConfig& bo) { - std::transform(dto.mpList.begin(), dto.mpList.end(), std::back_inserter(bo.mpList), - [](const auto& mp) -> MP::MPConfig - { - return fromAron(mp); - }); + std::transform(dto.mpList.begin(), + dto.mpList.end(), + std::back_inserter(bo.mpList), + [](const auto& mp) -> MP::MPConfig { return fromAron(mp); }); } -} /// namespace armarx::control::common::mp - - - +} // namespace armarx::control::common::mp diff --git a/source/armarx/control/common/aron_conversions.h b/source/armarx/control/common/aron_conversions.h index 9b1dbb90..222d2f71 100644 --- a/source/armarx/control/common/aron_conversions.h +++ b/source/armarx/control/common/aron_conversions.h @@ -23,17 +23,18 @@ #include <type_traits> -#include <armarx/control/common/control_law/TaskspaceImpedanceController.h> -#include <armarx/control/common/control_law/TaskspaceAdmittanceController.h> +#include <armarx/control/common/control_law/JointSpaceImpedanceController.h> #include <armarx/control/common/control_law/KeypointsImpedanceController.h> -#include <armarx/control/common/mp/MP.h> -#include <armarx/control/common/ft/FTSensor.h> - -#include <armarx/control/common/control_law/aron/TaskspaceImpedanceControllerConfig.aron.generated.h> -#include <armarx/control/common/control_law/aron/TaskspaceAdmittanceControllerConfig.aron.generated.h> +#include <armarx/control/common/control_law/TaskspaceAdmittanceController.h> +#include <armarx/control/common/control_law/TaskspaceImpedanceController.h> +#include <armarx/control/common/control_law/aron/JointspaceImpedanceControllerConfig.aron.generated.h> #include <armarx/control/common/control_law/aron/KeypointControllerConfig.aron.generated.h> -#include <armarx/control/common/mp/aron/MPConfig.aron.generated.h> +#include <armarx/control/common/control_law/aron/TaskspaceAdmittanceControllerConfig.aron.generated.h> +#include <armarx/control/common/control_law/aron/TaskspaceImpedanceControllerConfig.aron.generated.h> +#include <armarx/control/common/ft/FTSensor.h> #include <armarx/control/common/ft/aron/FTConfig.aron.generated.h> +#include <armarx/control/common/mp/MP.h> +#include <armarx/control/common/mp/aron/MPConfig.aron.generated.h> namespace armarx { @@ -52,16 +53,13 @@ namespace armarx return config; } -} /// namespace armarx - +} // namespace armarx namespace armarx::control::common::ft { - void fromAron(const arondto::FTConfig& dto, - FTSensor::FTConfig& bo); -} /// namespace armarx::control::common::ft - + void fromAron(const arondto::FTConfig& dto, FTSensor::FTConfig& bo); +} // namespace armarx::control::common::ft namespace armarx::control::common::control_law { @@ -77,16 +75,17 @@ namespace armarx::control::common::control_law void fromAron(const arondto::KeypointControllerConfig& dto, KeypointsImpedanceController::Config& bo); -} /// namespace armarx::control::common::control_law + void fromAron(const arondto::JointspaceImpedanceControllerConfig& dto, + JointSpaceImpedanceController::Config& bo); +} // namespace armarx::control::common::control_law namespace armarx::control::common::mp { MP::ListViaPoint fromAron(const arondto::ListViaPoint& dto); MP::DictViaPoint fromAron(const arondto::DictViaPoint& dto); - void fromAron(const arondto::ListViaPoint &dto, MP::ListViaPoint &bo); - void fromAron(const arondto::DictViaPoint &dto, MP::DictViaPoint &bo); + void fromAron(const arondto::ListViaPoint& dto, MP::ListViaPoint& bo); + void fromAron(const arondto::DictViaPoint& dto, MP::DictViaPoint& bo); void fromAron(const arondto::MPConfig& dto, MP::MPConfig& bo); - void fromAron(const arondto::MPListConfig &dto, MP::MPListConfig &bo); -} /// namespace armarx::control::common::mp - + void fromAron(const arondto::MPListConfig& dto, MP::MPListConfig& bo); +} // namespace armarx::control::common::mp diff --git a/source/armarx/control/common/control_law/JointSpaceImpedanceController.cpp b/source/armarx/control/common/control_law/JointSpaceImpedanceController.cpp index 41615bd0..8c44ce8c 100644 --- a/source/armarx/control/common/control_law/JointSpaceImpedanceController.cpp +++ b/source/armarx/control/common/control_law/JointSpaceImpedanceController.cpp @@ -21,6 +21,7 @@ #include "JointSpaceImpedanceController.h" +#include "ArmarXCore/core/logging/Logging.h" #include <ArmarXCore/core/exceptions/local/ExpressionException.h> namespace armarx::control::common::control_law @@ -46,16 +47,16 @@ namespace armarx::control::common::control_law ARMARX_CHECK_EQUAL(status.jointPosition.size(), numOfJoints); ARMARX_CHECK_EQUAL(status.jointVelocity.size(), numOfJoints); ARMARX_CHECK_EQUAL(status.jointAcceleration.size(), numOfJoints); - ARMARX_CHECK_EQUAL(status.gravityTorque.size(), numOfJoints); + // ARMARX_CHECK_EQUAL(status.gravityTorque.size(), numOfJoints); const Eigen::Map<const Eigen::VectorXf> jointPos(status.jointPosition.data(), static_cast<int>(numOfJoints)); const Eigen::Map<const Eigen::VectorXf> jointVel(status.jointVelocity.data(), static_cast<int>(numOfJoints)); - // const auto jointAcc = Eigen::Map<const Eigen::VectorXf>(status.jointAcceleration.data(), - // static_cast<int>(numOfJoints)); - const Eigen::Map<const Eigen::VectorXf> gravityTorque(status.gravityTorque.data(), - static_cast<int>(numOfJoints)); + const Eigen::Map<const Eigen::VectorXf> jointAcc(status.jointAcceleration.data(), + static_cast<int>(numOfJoints)); + // const Eigen::Map<const Eigen::VectorXf> gravityTorque(status.gravityTorque.data(), + // static_cast<int>(numOfJoints)); if (cfg.desiredJointPos.has_value()) { @@ -97,40 +98,47 @@ namespace armarx::control::common::control_law { if (kp.size() != static_cast<int>(numOfJoints)) { + ARMARX_WARNING << "kp size mismatch"; return false; } - if (not(kp.array() < 0.F).all()) + if (not(kp.array() > 0.F).all()) { + ARMARX_WARNING << "kp not positive"; return false; } if (kd.size() != static_cast<int>(numOfJoints)) { + ARMARX_WARNING << "kd size mismatch"; return false; } - if (not(kd.array() < 0.F).all()) + if (not(kd.array() > 0.F).all()) { + ARMARX_WARNING << "kd not positive"; return false; } if (desiredJointPos.has_value() and desiredJointPos->size() != static_cast<int>(numOfJoints)) { + ARMARX_WARNING << "desiredJointPos size mismatch"; return false; } if (desiredJointVel.has_value() and desiredJointVel->size() != static_cast<int>(numOfJoints)) { + ARMARX_WARNING << "desiredJointVel size mismatch"; return false; } if (desiredJointAcc.has_value() and desiredJointAcc->size() != static_cast<int>(numOfJoints)) { + ARMARX_WARNING << "desiredJointAcc size mismatch"; return false; } diff --git a/source/armarx/control/common/control_law/JointSpaceImpedanceController.h b/source/armarx/control/common/control_law/JointSpaceImpedanceController.h index 5a4a4b9d..d3c62f43 100644 --- a/source/armarx/control/common/control_law/JointSpaceImpedanceController.h +++ b/source/armarx/control/common/control_law/JointSpaceImpedanceController.h @@ -64,7 +64,7 @@ namespace armarx::control::common::control_law std::vector<float> jointVelocity; std::vector<float> jointAcceleration; - std::vector<float> gravityTorque; + // std::vector<float> gravityTorque; Eigen::MatrixXf massInertia; }; diff --git a/source/armarx/control/common/control_law/aron/JointspaceImpedanceControllerConfig.xml b/source/armarx/control/common/control_law/aron/JointspaceImpedanceControllerConfig.xml new file mode 100644 index 00000000..e9435446 --- /dev/null +++ b/source/armarx/control/common/control_law/aron/JointspaceImpedanceControllerConfig.xml @@ -0,0 +1,38 @@ +<?xml version="1.0" encoding="UTF--1" ?> +<AronTypeDefinition> + <CodeIncludes> + <Include include="<Eigen/Core>" /> + </CodeIncludes> + <GenerateTypes> + <Object name='armarx::control::common::control_law::arondto::JointspaceImpedanceControllerConfig'> + + <ObjectChild key='kpImpedance'> + <Matrix rows="-1" cols="1" type="float32" /> + </ObjectChild> + <ObjectChild key='kdImpedance'> + <Matrix rows="-1" cols="1" type="float32" /> + </ObjectChild> + + <ObjectChild key='desiredJointAngles'> + <Matrix rows="-1" cols="1" type="float32" optional="true" /> + </ObjectChild> + + <ObjectChild key='desiredJointVelocities'> + <Matrix rows="-1" cols="1" type="float32" optional="true" /> + </ObjectChild> + + <ObjectChild key='desiredJointAcceleration'> + <Matrix rows="-1" cols="1" type="float32" optional="true" /> + </ObjectChild> +<!-- + <ObjectChild key='torqueLimit'> + <float /> + </ObjectChild> --> + + <!-- <ObjectChild key='qvelFilter'> + <float /> + </ObjectChild> --> + + </Object> + </GenerateTypes> +</AronTypeDefinition> diff --git a/source/armarx/control/common/type.h b/source/armarx/control/common/type.h index f3ce40ce..b5a0b329 100644 --- a/source/armarx/control/common/type.h +++ b/source/armarx/control/common/type.h @@ -39,20 +39,23 @@ namespace armarx::control::common KVILImpedanceMP, QPWholeBodyImpedance, QPWholeBodyVelocity, + JSImpedance }; inline const simox::meta::EnumNames<ControllerType> ControllerTypeShort{ - {ControllerType::TSAdmittance, "TSAdmittance"}, - {ControllerType::TSImpedance, "TSImpedance"}, - {ControllerType::KptAdmittance, "KeypointAdmittance"}, - {ControllerType::KptImpedance, "KeypointImpedance"}, - {ControllerType::TSAdmittanceMP, "TSAdmittanceMP"}, - {ControllerType::TSImpedanceMP, "TSImpedanceMP"}, - {ControllerType::KptAdmittanceMP, "KeypointAdmittanceMP"}, - {ControllerType::KptImpedanceMP, "KeypointImpedanceMP"}, - {ControllerType::KVILImpedanceMP, "KVILImpedanceMP"}, - {ControllerType::QPWholeBodyImpedance, "QPWholeBodyImpedance"}, - {ControllerType::QPWholeBodyVelocity, "QPWholeBodyVelocity"}}; + {ControllerType::TSAdmittance, "TSAdmittance"}, + {ControllerType::TSImpedance, "TSImpedance"}, + {ControllerType::KptAdmittance, "KeypointAdmittance"}, + {ControllerType::KptImpedance, "KeypointImpedance"}, + {ControllerType::TSAdmittanceMP, "TSAdmittanceMP"}, + {ControllerType::TSImpedanceMP, "TSImpedanceMP"}, + {ControllerType::KptAdmittanceMP, "KeypointAdmittanceMP"}, + {ControllerType::KptImpedanceMP, "KeypointImpedanceMP"}, + {ControllerType::KVILImpedanceMP, "KVILImpedanceMP"}, + {ControllerType::QPWholeBodyImpedance, "QPWholeBodyImpedance"}, + {ControllerType::QPWholeBodyVelocity, "QPWholeBodyVelocity"}, + {ControllerType::JSImpedance, "JSImpedance"}, + }; inline const simox::meta::EnumNames<ControllerType> ControllerTypeNames{ @@ -64,6 +67,7 @@ namespace armarx::control::common {ControllerType::TSImpedanceMP, "NJointTSImpedanceMPController"}, {ControllerType::KptAdmittanceMP, "NJointKeypointsAdmittanceMPController"}, {ControllerType::KptImpedanceMP, "NJointKeypointsImpedanceMPController"}, - {ControllerType::KVILImpedanceMP, "NJointKVILImpedanceMPController"}}; + {ControllerType::KVILImpedanceMP, "NJointKVILImpedanceMPController"}, + {ControllerType::JSImpedance, "NJointJointspaceImpedanceController"}}; } // namespace armarx::control::common diff --git a/source/armarx/control/njoint_controller/CMakeLists.txt b/source/armarx/control/njoint_controller/CMakeLists.txt index 2b35e0c4..b31b3c3c 100644 --- a/source/armarx/control/njoint_controller/CMakeLists.txt +++ b/source/armarx/control/njoint_controller/CMakeLists.txt @@ -16,6 +16,7 @@ armarx_add_library(njoint_controller task_space/ImpedanceController.cpp # task_space/KeypointsAdmittanceController.cpp task_space/KeypointsImpedanceController.cpp + joint_space/ImpedanceController.cpp HEADERS controller_descriptions.h @@ -25,6 +26,7 @@ armarx_add_library(njoint_controller task_space/ImpedanceController.h # task_space/KeypointsAdmittanceController.h task_space/KeypointsImpedanceController.h + joint_space/ImpedanceController.h DEPENDENCIES ArmarXCoreObservers diff --git a/source/armarx/control/njoint_controller/controller_descriptions.h b/source/armarx/control/njoint_controller/controller_descriptions.h index bb5d2cb4..34c64156 100644 --- a/source/armarx/control/njoint_controller/controller_descriptions.h +++ b/source/armarx/control/njoint_controller/controller_descriptions.h @@ -26,6 +26,7 @@ #include "armarx/control/common/control_law/KeypointsImpedanceController.h" #include <armarx/control/client/ControllerDescription.h> #include <armarx/control/common/control_law/aron/TaskspaceImpedanceControllerConfig.aron.generated.h> +#include <armarx/control/common/control_law/aron/JointspaceImpedanceControllerConfig.aron.generated.h> #include <armarx/control/common/control_law/aron/TaskspaceAdmittanceControllerConfig.aron.generated.h> #include <armarx/control/common/control_law/aron/KeypointControllerConfig.aron.generated.h> #include <armarx/control/common/type.h> @@ -52,6 +53,17 @@ namespace armarx::control::client constexpr static const char* name = "NJointTaskspaceImpedanceController"; }; + template <> + struct ControllerDescription<armarx::control::common::ControllerType::JSImpedance> + { + using AronDTO = + armarx::control::common::control_law::arondto::JointspaceImpedanceControllerConfig; + + using BO = armarx::control::common::control_law::TaskspaceImpedanceController::Config; + + constexpr static const char* name = "NJointJointspaceImpedanceController"; + }; + template <> struct ControllerDescription<armarx::control::common::ControllerType::TSAdmittance> { diff --git a/source/armarx/control/njoint_controller/joint_space/ImpedanceController.cpp b/source/armarx/control/njoint_controller/joint_space/ImpedanceController.cpp new file mode 100644 index 00000000..b2b630c3 --- /dev/null +++ b/source/armarx/control/njoint_controller/joint_space/ImpedanceController.cpp @@ -0,0 +1,330 @@ +/** + * 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 2023 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + + +#include "ImpedanceController.h" + +#include <VirtualRobot/MathTools.h> + +#include <ArmarXCore/core/ArmarXObjectScheduler.h> +#include <ArmarXCore/core/time/CycleUtil.h> +#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h" + +#include "armarx/control/common/control_law/JointSpaceImpedanceController.h" +#include <armarx/control/common/aron_conversions.h> +#include <armarx/control/common/control_law/aron/TaskspaceImpedanceControllerConfig.aron.generated.h> +#include <armarx/control/common/utils.h> + +namespace armarx::control::njoint_controller::joint_space +{ + NJointControllerRegistration<NJointJointspaceImpedanceController> + registrationControllerNJointJointspaceImpedanceController( + "NJointJointspaceImpedanceController"); + + NJointJointspaceImpedanceController::NJointJointspaceImpedanceController( + const RobotUnitPtr& robotUnit, + const NJointControllerConfigPtr& config, + const VirtualRobot::RobotPtr&) : + robotUnit(robotUnit) + { + ConfigPtrT cfg = ConfigPtrT::dynamicCast(config); + + ARMARX_CHECK_EXPRESSION(cfg); + ARMARX_CHECK_EXPRESSION(robotUnit); + ARMARX_CHECK_EXPRESSION(!cfg->nodeSetName.empty()); + + + useSynchronizedRtRobot(); + kinematicChainName = cfg->nodeSetName; + VirtualRobot::RobotNodeSetPtr rtRns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName); + + nonRtRobot = robotUnit->cloneRobot(); + VirtualRobot::RobotNodeSetPtr nonRtRns = nonRtRobot->getRobotNodeSet(cfg->nodeSetName); + robotUnit->updateVirtualRobot(nonRtRobot); + + ARMARX_CHECK_EXPRESSION(rtRns) << cfg->nodeSetName; + ARMARX_CHECK_EXPRESSION(nonRtRns) << cfg->nodeSetName; + + controller.emplace(rtRns->getSize()); + + // controller.initialize(nonRtRns); + + jointNames.clear(); + for (size_t i = 0; i < rtRns->getSize(); ++i) + { + std::string jointName = rtRns->getNode(i)->getName(); + jointNames.push_back(jointName); + ControlTargetBase* ct = useControlTarget(jointName, ControlModes::Torque1DoF); + ARMARX_CHECK_EXPRESSION(ct); + const SensorValueBase* sv = useSensorValue(jointName); + ARMARX_CHECK_EXPRESSION(sv); + auto* casted_ct = ct->asA<ControlTarget1DoFActuatorTorque>(); + ARMARX_CHECK_EXPRESSION(casted_ct); + targets.push_back(casted_ct); + + // const auto* torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>(); + const auto* accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>(); + const auto* velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>(); + const auto* positionSensor = sv->asA<SensorValue1DoFActuatorPosition>(); + + // if (torqueSensor == nullptr) + // { + // ARMARX_WARNING << "No Torque sensor available for " << jointName; + // } + if (accelerationSensor == nullptr) + { + ARMARX_WARNING << "No acceleration sensor available for " << jointName; + } + if (velocitySensor == nullptr) + { + ARMARX_WARNING << "No velocity sensor available for " << jointName; + } + if (positionSensor == nullptr) + { + ARMARX_WARNING << "No position sensor available for " << jointName; + } + + // sensorDevices.torqueSensors.push_back(torqueSensor); + sensorDevices.accelerationSensors.push_back(accelerationSensor); + sensorDevices.velocitySensors.push_back(velocitySensor); + sensorDevices.positionSensors.push_back(positionSensor); + }; + + { + auto configData = ::armarx::fromAron<AronDTO, BO>(cfg->config); + + validateConfigData(configData); + + ARMARX_INFO << VAROUT(configData.kp); + ARMARX_INFO << VAROUT(configData.kd); + if (configData.desiredJointPos.has_value()) + { + ARMARX_INFO << VAROUT(configData.desiredJointPos.value()); + } + + if (configData.desiredJointVel.has_value()) + { + ARMARX_INFO << VAROUT(configData.desiredJointVel.value()); + } + + if (configData.desiredJointAcc.has_value()) + { + ARMARX_INFO << VAROUT(configData.desiredJointAcc.value()); + } + + // ARMARX_INFO << VAROUT(configData.torqueLimit); + // ARMARX_INFO << VAROUT(configData.qvelFilter); + + bufferUserToAdditionalTask.reinitAllBuffers(configData); + bufferUserToRt.reinitAllBuffers(configData); + } + + controlTargets.targets.resize(rtRns->getSize(), 0.); + } + + std::string + NJointJointspaceImpedanceController::getClassName(const Ice::Current&) const + { + return "NJointJointspaceImpedanceController"; + } + + // std::string NJointJointspaceImpedanceController::getKinematicChainName(const Ice::Current&) + // { + // return kinematicChainName; + // } + + void + NJointJointspaceImpedanceController::onInitNJointController() + { + runTask("NJointJointspaceImpedanceController", + [&] + { + CycleUtil c(1); + getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted); + ARMARX_IMPORTANT + << "Create a new thread alone NJointTaskspaceAdmittanceController"; + while (getState() == eManagedIceObjectStarted) + { + if (isControllerActive()) + { + additionalTask(); + } + c.waitForCycleDuration(); + } + }); + } + + void + NJointJointspaceImpedanceController::additionalTask() + { + robotUnit->updateVirtualRobot(nonRtRobot); + + controller->updateControlStatus(bufferUserToAdditionalTask.getUpToDateReadBuffer(), + bufferRtToAdditionalTask.getUpToDateReadBuffer()); + } + + void + NJointJointspaceImpedanceController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/, + const IceUtil::Time& timeSinceLastIteration) + { + double deltaT = timeSinceLastIteration.toSecondsDouble(); + auto& userConfig = bufferUserToRt.getUpToDateReadBuffer(); + + if (rtFirstRun.load()) + { + rtFirstRun.store(false); + rtReady.store(true); + } + + auto& s = bufferRtToAdditionalTask.getWriteBuffer(); + size_t nDoF = sensorDevices.positionSensors.size(); + for (size_t i = 0; i < nDoF; ++i) + { + s.jointPosition[i] = sensorDevices.positionSensors[i]->position; + s.jointVelocity[i] = sensorDevices.velocitySensors[i]->velocity; + s.jointAcceleration[i] = sensorDevices.accelerationSensors[i]->acceleration; + // s.jointTorque[i] = sensorDevices.torqueSensors[i]->torque; + } + // s.deltaT = deltaT; + bufferRtToAdditionalTask.commitWrite(); + + controller->run(controlTargets); + + for (unsigned int i = 0; i < controlTargets.targets.size(); i++) + { + targets.at(i)->torque = controlTargets.targets.at(i); + if (!targets.at(i)->isValid()) + { + targets.at(i)->torque = 0; + } + } + } + + void + NJointJointspaceImpedanceController::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto, + const Ice::Current& iceCurrent) + { + auto configData = ::armarx::fromAron<AronDTO, BO>(dto); + + validateConfigData(configData); + + bufferUserToAdditionalTask.getWriteBuffer() = configData; + bufferUserToAdditionalTask.commitWrite(); + + bufferUserToRt.getWriteBuffer() = configData; + bufferUserToRt.commitWrite(); + } + + void + NJointJointspaceImpedanceController::validateConfigData(BO& configData) + { + const auto nDoF = jointNames.size(); + + const auto checkSize = [nDoF](const auto& v) + { ARMARX_CHECK_EQUAL((unsigned)v.rows(), nDoF); }; + + const auto checkSizeOptional = [nDoF](const auto& v) + { + if (not v.has_value()) + { + return; + } + ARMARX_CHECK_EQUAL((unsigned)v->rows(), nDoF); + }; + + const auto checkNonNegative = [](const auto& v) { ARMARX_CHECK((v.array() >= 0).all()); }; + + checkSizeOptional(configData.desiredJointPos); + checkSizeOptional(configData.desiredJointVel); + checkSizeOptional(configData.desiredJointAcc); + checkSize(configData.kp); + checkSize(configData.kd); + + checkNonNegative(configData.kp); + checkNonNegative(configData.kd); + } + + void + NJointJointspaceImpedanceController::onPublish(const SensorAndControl&, + const DebugDrawerInterfacePrx&, + const DebugObserverInterfacePrx& debugObs) + { + StringVariantBaseMap datafields; + + // auto nonRtData = controller->bufferNonRtToOnPublish.getUpToDateReadBuffer(); + // auto rtData = controller->bufferRtToOnPublish.getUpToDateReadBuffer(); + auto configData = bufferUserToAdditionalTask.getUpToDateReadBuffer(); + + + // common::debugEigenPose(datafields, "current_pose", nonRtData.currentPose); + common::debugEigenVec(datafields, "kp", configData.kp); + common::debugEigenVec(datafields, "kd", configData.kd); + + + // common::debugEigenVec(datafields, "forceImpedance", rtData.forceImpedance); + // common::debugEigenVec(datafields, "desiredJointTorques", rtData.desiredJointTorques); + + debugObs->setDebugChannel("NJointJointspaceImpedanceController", datafields); + } + + void + NJointJointspaceImpedanceController::rtPreActivateController() + { + VirtualRobot::RobotNodeSetPtr rns = rtGetRobot()->getRobotNodeSet(kinematicChainName); + Eigen::Matrix4f currentPose = rns->getTCP()->getPoseInRootFrame(); + ARMARX_IMPORTANT << "rt preactivate controller with target pose\n\n" << currentPose; + // controller.preactivateInit(rns); + if (reInitPreActivate.load()) + { + auto& c = bufferUserToAdditionalTask.getWriteBuffer(); + c.desiredJointPos = rns->getJointValuesEigen(); + + bufferUserToAdditionalTask.reinitAllBuffers(c); + bufferUserToRt.reinitAllBuffers(c); + + reInitPreActivate.store(false); + } + + const auto nDoF = rns->getSize(); + + { + law::JointSpaceImpedanceController::RobotStatus rtToNonRtStatus; + rtToNonRtStatus.jointPosition.resize(nDoF, 0.); + rtToNonRtStatus.jointVelocity.resize(nDoF, 0.); + rtToNonRtStatus.jointAcceleration.resize(nDoF, 0.); + // rtToNonRtStatus.jointTorque.resize(nDoF, 0.); + for (size_t i = 0; i < nDoF; ++i) + { + rtToNonRtStatus.jointPosition[i] = sensorDevices.positionSensors[i]->position; + rtToNonRtStatus.jointVelocity[i] = sensorDevices.velocitySensors[i]->velocity; + rtToNonRtStatus.jointAcceleration[i] = sensorDevices.accelerationSensors[i]->acceleration; + // rtToNonRtStatus.jointTorque[i] = sensorDevices.torqueSensors[i]->torque; + } + bufferRtToAdditionalTask.reinitAllBuffers(rtToNonRtStatus); + } + } + + void + NJointJointspaceImpedanceController::rtPostDeactivateController() + { + // controller.isInitialized.store(false); + } +} // namespace armarx::control::njoint_controller::joint_space diff --git a/source/armarx/control/njoint_controller/joint_space/ImpedanceController.h b/source/armarx/control/njoint_controller/joint_space/ImpedanceController.h new file mode 100644 index 00000000..0400dc2e --- /dev/null +++ b/source/armarx/control/njoint_controller/joint_space/ImpedanceController.h @@ -0,0 +1,123 @@ +/** + * 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 2023 + * @copyright http://www.gnu.org/licenses/gpl-2.0.txt + * GNU General Public License + */ + + +#pragma once + +#include <VirtualRobot/Robot.h> + +#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h> +#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h> +#include <RobotAPI/components/units/RobotUnit/RobotUnit.h> +#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h> + +#include <armarx/control/common/control_law/JointSpaceImpedanceController.h> +#include <armarx/control/interface/ConfigurableNJointControllerInterface.h> +#include <armarx/control/njoint_controller/controller_descriptions.h> +#include <armarx/control/njoint_controller/task_space/ControllerInterface.h> + +namespace armarx::control::njoint_controller::joint_space +{ + namespace law = armarx::control::common::control_law; + + /** + * @defgroup Library-NJointJointspaceImpedanceController NJointJointspaceImpedanceController + * @ingroup Library-RobotUnit-NJointControllers + * A description of the library NJointJointspaceImpedanceController. + * + * @class NJointJointspaceImpedanceController + * @ingroup Library-NJointJointspaceImpedanceController + * @brief Brief description of class NJointJointspaceImpedanceController. + * + * Detailed description of class NJointJointspaceImpedanceController. + */ + class NJointJointspaceImpedanceController : + virtual public NJointController, + virtual public ::armarx::control::ConfigurableNJointControllerInterface + { + public: + using ConfigPtrT = ConfigurableNJointControllerConfigPtr; + using AronDTO = law::arondto::JointspaceImpedanceControllerConfig; + using BO = law::JointSpaceImpedanceController::Config; + + NJointJointspaceImpedanceController(const RobotUnitPtr& robotUnit, + const NJointControllerConfigPtr& config, + const VirtualRobot::RobotPtr&); + + std::string getClassName(const Ice::Current&) const override; + + void rtRun(const IceUtil::Time& sensorValuesTimestamp, + const IceUtil::Time& timeSinceLastIteration) override; + + /// NJointController interface + // std::string getKinematicChainName(const Ice::Current&) override; + + void updateConfig(const ::armarx::aron::data::dto::DictPtr& dto, + const Ice::Current& iceCurrent = Ice::emptyCurrent) override; + + void validateConfigData(BO& config); + + protected: + virtual void additionalTask(); + void onPublish(const SensorAndControl&, + const DebugDrawerInterfacePrx&, + const DebugObserverInterfacePrx&) override; + + /// devices + struct SensorDevices + { + // std::vector<const SensorValue1DoFActuatorTorque*> torqueSensors; + std::vector<const SensorValue1DoFActuatorAcceleration*> accelerationSensors; + std::vector<const SensorValue1DoFActuatorVelocity*> velocitySensors; + std::vector<const SensorValue1DoFActuatorPosition*> positionSensors; + }; + + SensorDevices sensorDevices; + + std::vector<ControlTarget1DoFActuatorTorque*> targets; + + /// variables + std::string kinematicChainName; + std::vector<std::string> jointNames; + + std::optional<law::JointSpaceImpedanceController> controller; + + /// set buffers + TripleBuffer<BO> bufferUserToAdditionalTask; + TripleBuffer<BO> bufferUserToRt; + TripleBuffer<law::JointSpaceImpedanceController::RobotStatus> bufferRtToAdditionalTask; + + std::atomic_bool rtFirstRun = true; + std::atomic_bool rtReady = false; + std::atomic_bool reInitPreActivate = false; + + VirtualRobot::RobotPtr nonRtRobot; + + common::control_law::JointSpaceImpedanceController::ControlTarget controlTargets; + + RobotUnitPtr robotUnit; + + /// NJointControllerBase interface + void onInitNJointController() override; + void rtPreActivateController() override; + void rtPostDeactivateController() override; + }; +} // namespace armarx::control::njoint_controller::joint_space -- GitLab From 0eb2c9abb794b08e4a556eecc8e662e5e1611d15 Mon Sep 17 00:00:00 2001 From: armar-user <armar6@kit> Date: Thu, 23 Mar 2023 17:26:39 +0100 Subject: [PATCH 11/16] tested on ARMAR-6. adding configs --- .../compilant.json | 41 +++++++++++++++++++ .../default.json | 32 +++++++-------- .../semi_stiff.json | 41 +++++++++++++++++++ .../stiff.json | 41 +++++++++++++++++++ .../Component.cpp | 8 +++- ...njoint_js_impedance_controller_example.cfg | 5 ++- 6 files changed, 149 insertions(+), 19 deletions(-) create mode 100644 data/armarx_control/controller_config/NJointJointspaceImpedanceController/compilant.json create mode 100644 data/armarx_control/controller_config/NJointJointspaceImpedanceController/semi_stiff.json create mode 100644 data/armarx_control/controller_config/NJointJointspaceImpedanceController/stiff.json diff --git a/data/armarx_control/controller_config/NJointJointspaceImpedanceController/compilant.json b/data/armarx_control/controller_config/NJointJointspaceImpedanceController/compilant.json new file mode 100644 index 00000000..b86b576f --- /dev/null +++ b/data/armarx_control/controller_config/NJointJointspaceImpedanceController/compilant.json @@ -0,0 +1,41 @@ +{ + "kpImpedance": { + "type": "float", + "dims": [ + 8, + 1 + ], + "data": [ + 65.0, + 45.0, + 25.0, + 15.0, + 15.0, + 15.0, + 15.0, + 15.0 + ] + }, + "kdImpedance": { + "type": "float", + "dims": [ + 8, + 1 + ], + "data": [ + 14.0, + 10.0, + 10.0, + 6.0, + 6.0, + 6.0, + 6.0, + 6.0 + ] + }, + "torqueLimit": 20, + "qvelFilter": 0.9, + "desiredJointAngles": null, + "desiredJointVelocities": null, + "desiredJointAcceleration": null +} diff --git a/data/armarx_control/controller_config/NJointJointspaceImpedanceController/default.json b/data/armarx_control/controller_config/NJointJointspaceImpedanceController/default.json index b86b576f..051b8f12 100644 --- a/data/armarx_control/controller_config/NJointJointspaceImpedanceController/default.json +++ b/data/armarx_control/controller_config/NJointJointspaceImpedanceController/default.json @@ -6,14 +6,14 @@ 1 ], "data": [ - 65.0, - 45.0, - 25.0, - 15.0, - 15.0, - 15.0, - 15.0, - 15.0 + 325.0, + 225.0, + 125.0, + 75.0, + 75.0, + 75.0, + 75.0, + 75.0 ] }, "kdImpedance": { @@ -23,14 +23,14 @@ 1 ], "data": [ - 14.0, - 10.0, - 10.0, - 6.0, - 6.0, - 6.0, - 6.0, - 6.0 + 70.0, + 50.0, + 50.0, + 30.0, + 30.0, + 30.0, + 30.0, + 30.0 ] }, "torqueLimit": 20, diff --git a/data/armarx_control/controller_config/NJointJointspaceImpedanceController/semi_stiff.json b/data/armarx_control/controller_config/NJointJointspaceImpedanceController/semi_stiff.json new file mode 100644 index 00000000..6e5e6a42 --- /dev/null +++ b/data/armarx_control/controller_config/NJointJointspaceImpedanceController/semi_stiff.json @@ -0,0 +1,41 @@ +{ + "kpImpedance": { + "type": "float", + "dims": [ + 8, + 1 + ], + "data": [ + 195.0, + 135.0, + 75.0, + 45.0, + 45.0, + 45.0, + 45.0, + 45.0 + ] + }, + "kdImpedance": { + "type": "float", + "dims": [ + 8, + 1 + ], + "data": [ + 42.0, + 30.0, + 30.0, + 18.0, + 18.0, + 18.0, + 18.0, + 18.0 + ] + }, + "torqueLimit": 20, + "qvelFilter": 0.9, + "desiredJointAngles": null, + "desiredJointVelocities": null, + "desiredJointAcceleration": null +} diff --git a/data/armarx_control/controller_config/NJointJointspaceImpedanceController/stiff.json b/data/armarx_control/controller_config/NJointJointspaceImpedanceController/stiff.json new file mode 100644 index 00000000..31b9b17f --- /dev/null +++ b/data/armarx_control/controller_config/NJointJointspaceImpedanceController/stiff.json @@ -0,0 +1,41 @@ +{ + "kpImpedance": { + "type": "float", + "dims": [ + 8, + 1 + ], + "data": [ + 650.0, + 450.0, + 250.0, + 150.0, + 150.0, + 150.0, + 150.0, + 150.0 + ] + }, + "kdImpedance": { + "type": "float", + "dims": [ + 8, + 1 + ], + "data": [ + 140.0, + 100.0, + 100.0, + 60.0, + 60.0, + 60.0, + 60.0, + 60.0 + ] + }, + "torqueLimit": 20, + "qvelFilter": 0.9, + "desiredJointAngles": null, + "desiredJointVelocities": null, + "desiredJointAcceleration": null +} diff --git a/examples/components/njoint_js_impedance_controller_example/Component.cpp b/examples/components/njoint_js_impedance_controller_example/Component.cpp index 5515a363..aa450d65 100644 --- a/examples/components/njoint_js_impedance_controller_example/Component.cpp +++ b/examples/components/njoint_js_impedance_controller_example/Component.cpp @@ -200,7 +200,13 @@ namespace armarx::control::components::njoint_js_impedance_controller_example new armarx::ComponentPropertyDefinitions(getConfigIdentifier()); def->optional(properties.robotName, "robotName"); - def->optional(properties.configName, "configName"); + def->optional(properties.configName, "configName") + .map({ + {"default", "default"}, + {"semi_stiff", "semi_stiff"}, + {"stiff", "stiff"}, + {"compilant", "compilant"}, + }); def->optional(properties.movementRadius, "movementRadius"); def->optional(properties.movementDurationSeconds, "movementDurationSeconds"); diff --git a/scenarios/NJointJointspaceImpedanceControllerExample/config/njoint_js_impedance_controller_example.cfg b/scenarios/NJointJointspaceImpedanceControllerExample/config/njoint_js_impedance_controller_example.cfg index af976796..89e3fe51 100644 --- a/scenarios/NJointJointspaceImpedanceControllerExample/config/njoint_js_impedance_controller_example.cfg +++ b/scenarios/NJointJointspaceImpedanceControllerExample/config/njoint_js_impedance_controller_example.cfg @@ -130,7 +130,8 @@ ArmarX.NJointJSImpedanceControllerExample.RobotUnitName = Armar6Unit # - Default: default # - Case sensitivity: yes # - Required: no -# ArmarX.NJointJSImpedanceControllerExample.configName = default +# - Possible values: {compilant, default, semi_stiff, stiff} +ArmarX.NJointJSImpedanceControllerExample.configName = semi_stiff # ArmarX.NJointJSImpedanceControllerExample.mem.robot_state.Memory: @@ -172,7 +173,7 @@ ArmarX.NJointJSImpedanceControllerExample.RobotUnitName = Armar6Unit # - Default: 20 # - Case sensitivity: yes # - Required: no -# ArmarX.NJointJSImpedanceControllerExample.movementDurationSeconds = 20 +ArmarX.NJointJSImpedanceControllerExample.movementDurationSeconds = 200 # ArmarX.NJointJSImpedanceControllerExample.movementRadius: -- GitLab From 50797a3dbcf517790232028be5b3b28f55672aac Mon Sep 17 00:00:00 2001 From: armar-user <armar6@kit> Date: Thu, 23 Mar 2023 23:19:27 +0100 Subject: [PATCH 12/16] minor; scenario config update --- .../config/njoint_js_impedance_controller_example.cfg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scenarios/NJointJointspaceImpedanceControllerExample/config/njoint_js_impedance_controller_example.cfg b/scenarios/NJointJointspaceImpedanceControllerExample/config/njoint_js_impedance_controller_example.cfg index 89e3fe51..66de189f 100644 --- a/scenarios/NJointJointspaceImpedanceControllerExample/config/njoint_js_impedance_controller_example.cfg +++ b/scenarios/NJointJointspaceImpedanceControllerExample/config/njoint_js_impedance_controller_example.cfg @@ -131,7 +131,7 @@ ArmarX.NJointJSImpedanceControllerExample.RobotUnitName = Armar6Unit # - Case sensitivity: yes # - Required: no # - Possible values: {compilant, default, semi_stiff, stiff} -ArmarX.NJointJSImpedanceControllerExample.configName = semi_stiff +ArmarX.NJointJSImpedanceControllerExample.configName = stiff # ArmarX.NJointJSImpedanceControllerExample.mem.robot_state.Memory: -- GitLab From f81085e9dcf0dd69570b5c2216d220c3c4476d96 Mon Sep 17 00:00:00 2001 From: armar-user <armar6@kit> Date: Thu, 23 Mar 2023 23:21:59 +0100 Subject: [PATCH 13/16] cherry-picking controller builder from origin/master --- .../armarx/control/client/ControllerBuilder.h | 55 ++++++++++++++----- .../armarx/control/client/ControllerWrapper.h | 3 +- 2 files changed, 42 insertions(+), 16 deletions(-) diff --git a/source/armarx/control/client/ControllerBuilder.h b/source/armarx/control/client/ControllerBuilder.h index ab247357..0c720ce5 100644 --- a/source/armarx/control/client/ControllerBuilder.h +++ b/source/armarx/control/client/ControllerBuilder.h @@ -27,6 +27,8 @@ #include <type_traits> #include <SimoxUtility/json/json.hpp> +#include "ArmarXCore/core/exceptions/LocalException.h" +#include "ArmarXCore/core/time/Clock.h" #include "RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h" #include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h> @@ -146,15 +148,6 @@ namespace armarx::control::client return *this; } - ControllerBuilder& - withNamePrefix(const std::string& namePrefix) - { - if (namePrefix.empty()) - return *this; - this->controllerNamePrefix = this->controllerNamePrefix + "_" + namePrefix; - return *this; - } - /** * @brief Creates the controller with the default name * @@ -187,7 +180,7 @@ namespace armarx::control::client } config->nodeSetName = nodeSetName.value(); - controllerName = controllerNamePrefix + "_" + nodeSetName.value() + "_" + name; + const auto controllerName = controllerNamePrefix + "_" + name; const std::string controllerClassName = ControllerDescriptionType::name; // reuse or create controller @@ -200,12 +193,20 @@ namespace armarx::control::client { if (not controller) { + ARMARX_INFO << "Controller does not exist yet."; + return false; + } + + if(not allowReuse_) + { + ARMARX_INFO << "Not allowed to reuse controller."; return false; } // check if controller has correct type if (controller->getClassName() != controllerClassName) { + ARMARX_INFO << "Controller class does not match."; return false; } @@ -218,11 +219,36 @@ namespace armarx::control::client return true; }(); + if((not allowReuse_) and controller) + { + ARMARX_TRACE; + + ARMARX_INFO << "Deleting existing controller `" << controllerName << "`"; + try + { + ARMARX_TRACE; + controllerCreator->deactivateAndDeleteNJointController(controllerName); + } + catch(...) + { + ARMARX_WARNING << GetHandledExceptionString(); + } + + ARMARX_INFO << "Done deleting"; + + Clock::WaitFor(Duration::MilliSeconds(150)); + } + // create controller if necessary if (not canReuseController) { + ARMARX_TRACE; + + ARMARX_INFO << "Creating controller `" << controllerName << "`"; + controller = controllerCreator->createNJointController( controllerClassName, controllerName, config); + ARMARX_CHECK_NOT_NULL(controller); } if (not controller) @@ -245,9 +271,9 @@ namespace armarx::control::client return std::move(ctrlWrapper); } - std::string getControllerName() + ControllerBuilder& allowReuse(const bool allowReuse) { - return controllerName; + this->allowReuse_ = allowReuse; } private: @@ -285,7 +311,8 @@ namespace armarx::control::client std::optional<std::string> nodeSetName; - std::string controllerNamePrefix; - std::string controllerName; + const std::string controllerNamePrefix; + + bool allowReuse_ = false; }; } // namespace armarx::control::client diff --git a/source/armarx/control/client/ControllerWrapper.h b/source/armarx/control/client/ControllerWrapper.h index f7ab44c9..33f19196 100644 --- a/source/armarx/control/client/ControllerWrapper.h +++ b/source/armarx/control/client/ControllerWrapper.h @@ -23,7 +23,6 @@ #include <optional> #include <type_traits> -#include "ArmarXCore/core/logging/Logging.h" #include <RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h> @@ -72,7 +71,7 @@ namespace armarx::control::client void updateConfig() { - ARMARX_VERBOSE << "Updating config for controller `" << controllerName << "`"; + ARMARX_INFO << "Updating config for controller `" << controllerName << "`"; ARMARX_CHECK_NOT_NULL(controller); controller->updateConfig(config.toAronDTO()); -- GitLab From 4d27136c0c5ceec7b61e0d58daba765ee1d704b4 Mon Sep 17 00:00:00 2001 From: armar-user <armar6@kit.edu> Date: Fri, 24 Mar 2023 19:36:30 +0100 Subject: [PATCH 14/16] update controller creator for python bindings --- .../config/controller_creator.cfg | 273 ++++++++++++++++++ .../TaskspaceImpedanceController.cpp | 5 +- .../controller_creator/Component.cpp | 199 ++++++++++++- .../components/controller_creator/Component.h | 6 + .../controller_creator/ComponentInterface.ice | 5 +- 5 files changed, 475 insertions(+), 13 deletions(-) diff --git a/scenarios/ControlScenario/config/controller_creator.cfg b/scenarios/ControlScenario/config/controller_creator.cfg index b5d8ed6e..b6e09757 100644 --- a/scenarios/ControlScenario/config/controller_creator.cfg +++ b/scenarios/ControlScenario/config/controller_creator.cfg @@ -2,3 +2,276 @@ # controller_creator properties # ================================================================== +# ArmarX.AdditionalPackages: List of additional ArmarX packages which should be in the list of default packages. If you have custom packages, which should be found by the gui or other apps, specify them here. Comma separated List. +# Attributes: +# - Default: Default value not mapped. +# - Case sensitivity: yes +# - Required: no +# ArmarX.AdditionalPackages = Default value not mapped. + + +# ArmarX.ApplicationName: Application name +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.ApplicationName = "" + + +# ArmarX.CachePath: Path for cache files. If relative path AND env. variable ARMARX_CONFIG_DIR is set, the cache path will be made relative to ARMARX_CONFIG_DIR. Otherwise if relative it will be relative to the default ArmarX config dir (${ARMARX_WORKSPACE}/armarx_config) +# Attributes: +# - Default: mongo/.cache +# - Case sensitivity: yes +# - Required: no +# ArmarX.CachePath = mongo/.cache + + +# ArmarX.Config: Comma-separated list of configuration files +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.Config = "" + + +# ArmarX.DataPath: Semicolon-separated search list for data files +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.DataPath = "" + + +# ArmarX.DefaultPackages: List of ArmarX packages which are accessible by default. Comma separated List. If you want to add your own packages and use all default ArmarX packages, use the property 'AdditionalPackages'. +# Attributes: +# - Default: Default value not mapped. +# - Case sensitivity: yes +# - Required: no +# ArmarX.DefaultPackages = Default value not mapped. + + +# ArmarX.DependenciesConfig: Path to the (usually generated) config file containing all data paths of all dependent projects. This property usually does not need to be edited. +# Attributes: +# - Default: ./config/dependencies.cfg +# - Case sensitivity: yes +# - Required: no +# ArmarX.DependenciesConfig = ./config/dependencies.cfg + + +# ArmarX.DisableLogging: Turn logging off in whole application +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.DisableLogging = false + + +# ArmarX.EnableProfiling: Enable profiling of CPU load produced by this application +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.EnableProfiling = false + + +# ArmarX.LoadLibraries: Libraries to load at start up of the application. Must be enabled by the Application with enableLibLoading(). Format: PackageName:LibraryName;... or /absolute/path/to/library;... +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.LoadLibraries = "" + + +# ArmarX.LoggingGroup: The logging group is transmitted with every ArmarX log message over Ice in order to group the message in the GUI. +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.LoggingGroup = "" + + +# ArmarX.RedirectStdout: Redirect std::cout and std::cerr to ArmarXLog +# Attributes: +# - Default: true +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.RedirectStdout = true + + +# ArmarX.RemoteHandlesDeletionTimeout: The timeout (in ms) before a remote handle deletes the managed object after the use count reached 0. This time can be used by a client to increment the count again (may be required when transmitting remote handles) +# Attributes: +# - Default: 3000 +# - Case sensitivity: yes +# - Required: no +# ArmarX.RemoteHandlesDeletionTimeout = 3000 + + +# ArmarX.SecondsStartupDelay: The startup will be delayed by this number of seconds (useful for debugging) +# Attributes: +# - Default: 0 +# - Case sensitivity: yes +# - Required: no +# ArmarX.SecondsStartupDelay = 0 + + +# ArmarX.StartDebuggerOnCrash: If this application crashes (segmentation fault) qtcreator will attach to this process and start the debugger. +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.StartDebuggerOnCrash = false + + +# ArmarX.ThreadPoolSize: Size of the ArmarX ThreadPool that is always running. +# Attributes: +# - Default: 1 +# - Case sensitivity: yes +# - Required: no +# ArmarX.ThreadPoolSize = 1 + + +# ArmarX.TopicSuffix: Suffix appended to all topic names for outgoing topics. This is mainly used to direct all topics to another name for TopicReplaying purposes. +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.TopicSuffix = "" + + +# ArmarX.UseTimeServer: Enable using a global Timeserver (e.g. from ArmarXSimulator) +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.UseTimeServer = false + + +# ArmarX.Verbosity: Global logging level for whole application +# Attributes: +# - Default: Info +# - Case sensitivity: yes +# - Required: no +# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning} +# ArmarX.Verbosity = Info + + +# ArmarX.controller_creator.EnableProfiling: enable profiler which is used for logging performance events +# Attributes: +# - Default: false +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.controller_creator.EnableProfiling = false + + +# ArmarX.controller_creator.MinimumLoggingLevel: Local logging level only for this component +# Attributes: +# - Default: Undefined +# - Case sensitivity: yes +# - Required: no +# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning} +# ArmarX.controller_creator.MinimumLoggingLevel = Undefined + + +# ArmarX.controller_creator.ObjectName: Name of IceGrid well-known object +# Attributes: +# - Default: "" +# - Case sensitivity: yes +# - Required: no +# ArmarX.controller_creator.ObjectName = "" + + +# ArmarX.controller_creator.RemoteStateComponentName: Name of the robot state component +# Attributes: +# - Default: RobotStateComponent +# - Case sensitivity: yes +# - Required: no +# ArmarX.controller_creator.RemoteStateComponentName = RobotStateComponent + + +# ArmarX.controller_creator.RobotUnitName: Name of the RobotUnit +# Attributes: +# - Case sensitivity: yes +# - Required: yes +ArmarX.controller_creator.RobotUnitName = Armar6Unit + + +# ArmarX.controller_creator.cmp.RobotUnit: Ice object name of the `RobotUnit` component. +# Attributes: +# - Default: Armar6Unit +# - Case sensitivity: yes +# - Required: no +# ArmarX.controller_creator.cmp.RobotUnit = Armar6Unit + + +# ArmarX.controller_creator.default_robotNodeSet_left: default kinematic chain name of left arm. +# Attributes: +# - Default: LeftArm +# - Case sensitivity: yes +# - Required: no +# ArmarX.controller_creator.default_robotNodeSet_left = LeftArm + + +# ArmarX.controller_creator.default_robotNodeSet_right: default kinematic chain name of right arm. +# Attributes: +# - Default: RightArm +# - Case sensitivity: yes +# - Required: no +# ArmarX.controller_creator.default_robotNodeSet_right = RightArm + + +# ArmarX.controller_creator.mem.robot_state.Memory: +# Attributes: +# - Default: RobotState +# - Case sensitivity: yes +# - Required: no +# ArmarX.controller_creator.mem.robot_state.Memory = RobotState + + +# ArmarX.controller_creator.mem.robot_state.localizationSegment: Name of the localization memory core segment to use. +# Attributes: +# - Default: Localization +# - Case sensitivity: yes +# - Required: no +# ArmarX.controller_creator.mem.robot_state.localizationSegment = Localization + + +# ArmarX.controller_creator.mns.MemoryNameSystemEnabled: Whether to use (and depend on) the Memory Name System (MNS). +# Set to false to use this memory as a stand-alone. +# Attributes: +# - Default: true +# - Case sensitivity: yes +# - Required: no +# - Possible values: {0, 1, false, no, true, yes} +# ArmarX.controller_creator.mns.MemoryNameSystemEnabled = true + + +# ArmarX.controller_creator.mns.MemoryNameSystemName: Name of the Memory Name System (MNS) component. +# Attributes: +# - Default: MemoryNameSystem +# - Case sensitivity: yes +# - Required: no +# ArmarX.controller_creator.mns.MemoryNameSystemName = MemoryNameSystem + + +# ArmarX.controller_creator.robotName: +# Attributes: +# - Default: Armar6 +# - Case sensitivity: yes +# - Required: no +# ArmarX.controller_creator.robotName = Armar6 + + +# ArmarX.controller_creator.robotUnitName: +# Attributes: +# - Default: Armar6Unit +# - Case sensitivity: yes +# - Required: no +# ArmarX.controller_creator.robotUnitName = Armar6Unit + + diff --git a/source/armarx/control/common/control_law/TaskspaceImpedanceController.cpp b/source/armarx/control/common/control_law/TaskspaceImpedanceController.cpp index fa610ee6..37d9274c 100644 --- a/source/armarx/control/common/control_law/TaskspaceImpedanceController.cpp +++ b/source/armarx/control/common/control_law/TaskspaceImpedanceController.cpp @@ -137,7 +137,10 @@ namespace armarx::control::common::control_law { if ((sRt.desiredPose.block<3, 1>(0, 3) - c.desiredPose.block<3, 1>(0, 3)).norm() > 100.0f) { - ARMARX_RT_LOGF_WARN("new target \n\n %s\n\nis too far away from\n\n %s", VAROUT(c.desiredPose), VAROUT(sRt.desiredPose)); + ARMARX_RT_LOGF_WARN("new target is too far away from the current target"); +// ARMARX_RT_LOGF_WARN("new target \n\n [%f, %f, %f]\n\nis too far away from the current target\n\n [%f, %, %s]", +// c.desiredPose(0, 3), c.desiredPose(1, 3), c.desiredPose(2, 3), +// sRt.desiredPose(0, 3), sRt.desiredPose(1, 3), sRt.desiredPose(2, 3)); } else { diff --git a/source/armarx/control/components/controller_creator/Component.cpp b/source/armarx/control/components/controller_creator/Component.cpp index b4ef6b26..168b60a0 100644 --- a/source/armarx/control/components/controller_creator/Component.cpp +++ b/source/armarx/control/components/controller_creator/Component.cpp @@ -57,7 +57,6 @@ namespace armarx::control::components::controller_creator def->optional(properties.defaultRobotNodeSetRight, "default_robotNodeSet_right", "default kinematic chain name of right arm."); def->component(m_robot_unit, properties.robotUnitName); - ARMARX_CHECK_NOT_NULL(m_robot_unit); // Publish to a topic (passing the TopicListenerPrx). // def->topic(myTopicListener); @@ -87,6 +86,7 @@ namespace armarx::control::components::controller_creator // Keep debug observer data until calling `sendDebugObserverBatch()`. // (Requires the armarx::DebugObserverComponentPluginUser.) // setDebugObserverBatchModeEnabled(true); + } @@ -94,6 +94,7 @@ namespace armarx::control::components::controller_creator Component::onConnectComponent() { // Do things after connecting to topics and components. + ARMARX_CHECK_NOT_NULL(m_robot_unit); /* (Requires the armarx::DebugObserverComponentPluginUser.) // Use the debug observer to log data over time. @@ -127,6 +128,8 @@ namespace armarx::control::components::controller_creator m_rns_l = m_robot->getRobotNodeSet(properties.defaultRobotNodeSetLeft); m_rns_r = m_robot->getRobotNodeSet(properties.defaultRobotNodeSetRight); + ARMARX_IMPORTANT << "controller createor is ready"; + } @@ -186,6 +189,7 @@ namespace armarx::control::components::controller_creator ARMARX_CHECK(synchronizeLocalClone(m_robot)); const auto rns = m_robot->getRobotNodeSet(robotNodeSet); + builder.config().desiredPose = rns->getTCP()->getPoseInRootFrame(); ARMARX_IMPORTANT << VAROUT(builder.config().desiredPose); @@ -194,6 +198,8 @@ namespace armarx::control::components::controller_creator auto ctrlWrapper = builder.create(); const std::string controllerName = builder.getControllerName(); + m_robotNodeSets[controllerName] = rns; + m_controllerJointNumbers[controllerName] = rns->getJointValues().size(); // The controller is scoped by default, which means that if the ControllerWrapper goes out of scope, it will deactivate // and delete the NJointController. To prevent this, you can use the daemonize() method if needed. @@ -256,6 +262,8 @@ namespace armarx::control::components::controller_creator auto ctrlWrapper = builder.create(); const std::string controllerName = builder.getControllerName(); + m_robotNodeSets[controllerName] = rns; + m_controllerJointNumbers[controllerName] = rns->getJointValues().size(); // The controller is scoped by default, which means that if the ControllerWrapper goes out of scope, it will deactivate // and delete the NJointController. To prevent this, you can use the daemonize() method if needed. @@ -263,30 +271,35 @@ namespace armarx::control::components::controller_creator ARMARX_TRACE; ARMARX_CHECK(ctrlWrapper.has_value()); - m_ctrlTSAdmittance[robotNodeSet] = std::nullopt; - m_ctrlTSAdmittance[robotNodeSet].emplace(std::move(ctrlWrapper.value())); + m_ctrlTSAdmittance[controllerName] = std::nullopt; + m_ctrlTSAdmittance[controllerName].emplace(std::move(ctrlWrapper.value())); Clock::WaitFor(Duration::MilliSeconds(300)); ARMARX_INFO << "Activating controller"; - m_ctrlTSAdmittance[robotNodeSet]->updateConfig(); - m_ctrlTSAdmittance[robotNodeSet]->activate(); + m_ctrlTSAdmittance[controllerName]->updateConfig(); + m_ctrlTSAdmittance[controllerName]->activate(); - m_prevTargetPose[robotNodeSet] = rns->getTCP()->getPoseInRootFrame(); - m_prevNullspaceJointTarget[robotNodeSet] = rns->getJointValuesEigen(); + m_prevTargetPose[controllerName] = rns->getTCP()->getPoseInRootFrame(); + m_prevNullspaceJointTarget[controllerName] = rns->getJointValuesEigen(); return controllerName; } std::string - Component::createController(const std::string &controllerNamePrefix, const std::string &robotNodeSet, const std::string &controllerType, const std::string &configFilename, const Ice::Current &) + Component::createController( + const std::string& controllerNamePrefix, + const std::string& robotNodeSet, + const std::string& controllerType, + const std::string& configFilename, + const Ice::Current&) { ARMARX_INFO << "creating controller of type " << controllerType << " with robot node set " << robotNodeSet; - std::string controllerName; - ARMARX_INFO << VAROUT(robotNodeSet) << VAROUT(controllerType) << VAROUT(configFilename); RobotUnitInterfacePrx robotUnit = getControlComponentPlugin().getRobotUnitPlugin().getRobotUnit(); robotUnit->loadLibFromPackage("armarx_control", "libarmarx_control_njoint_controller"); + std::string controllerName; + auto type = control::common::ControllerTypeShort.from_name(controllerType); switch (type) { case control::common::ControllerType::TSImpedance: @@ -299,10 +312,174 @@ namespace armarx::control::components::controller_creator default: ARMARX_INFO << "controller type: " << controllerType << " is not supported yet"; } - return controllerName; } + bool + Component::updateTargetPose( + const std::string &controllerName, + const std::string& controllerType, + const Ice::FloatSeq& targetPoseVector, + const Ice::Current&) + { + auto names = control::common::ControllerTypeShort.names(); + const bool is_in = names.find(controllerType) != names.end(); + ARMARX_CHECK_EQUAL(7, targetPoseVector.size()); + if (!is_in) + { + ARMARX_ERROR << "controller type not supported"; + return false; + } + if (targetPoseVector.size() != 7) + { + ARMARX_ERROR << "target pose vector " << targetPoseVector << " should follow (x, y, z, w, rx, ry, rz) convention"; + return false; + } + + m_prevTargetPose[controllerName] = VirtualRobot::MathTools::quat2eigen4f(targetPoseVector.at(4), targetPoseVector.at(5), targetPoseVector.at(6), targetPoseVector.at(3)); + m_prevTargetPose[controllerName](0, 3) = targetPoseVector.at(0); + m_prevTargetPose[controllerName](1, 3) = targetPoseVector.at(1); + m_prevTargetPose[controllerName](2, 3) = targetPoseVector.at(2); + + auto type = control::common::ControllerTypeShort.from_name(controllerType); + switch (type) { + case control::common::ControllerType::TSImpedance: + ARMARX_CHECK(m_ctrlTSImpedance[controllerName]); + m_ctrlTSImpedance[controllerName]->config.desiredPose = m_prevTargetPose[controllerName]; + m_ctrlTSImpedance[controllerName]->config.desiredNullspaceJointAngles = m_prevNullspaceJointTarget[controllerName]; + m_ctrlTSImpedance[controllerName]->updateConfig(); + break; + case control::common::ControllerType::TSAdmittance: + ARMARX_CHECK(m_ctrlTSAdmittance[controllerName]); + m_ctrlTSAdmittance[controllerName]->config.desiredPose = m_prevTargetPose[controllerName]; + m_ctrlTSAdmittance[controllerName]->config.desiredNullspaceJointAngles = m_prevNullspaceJointTarget[controllerName]; + m_ctrlTSAdmittance[controllerName]->updateConfig(); + break; + + default: + ARMARX_INFO << "controller type: " << controllerType << " is not supported yet"; + return false; + } + return true; + } + + bool + Component::updateTargetNullspaceJointAngles( + const std::string &controllerName, + const std::string &controllerType, + const Ice::FloatSeq& targetNullspaceJointAngle, + const Ice::Current &) + { + auto names = control::common::ControllerTypeShort.names(); + const bool is_in = names.find(controllerType) != names.end(); + ARMARX_CHECK_EQUAL(m_controllerJointNumbers[controllerName], targetNullspaceJointAngle.size()); + if (!is_in) + { + ARMARX_ERROR << "controller type not supported"; + return false; + } + + Eigen::VectorXf targetJointAngles = Eigen::VectorXf::Zero(m_controllerJointNumbers[controllerName]); + for (size_t i = 0; i < m_controllerJointNumbers[controllerName]; i++) + { + targetJointAngles(i) = targetNullspaceJointAngle[i]; + } + + auto type = control::common::ControllerTypeShort.from_name(controllerType); + switch (type) { + case control::common::ControllerType::TSImpedance: + ARMARX_CHECK(m_ctrlTSImpedance[controllerName]); + m_ctrlTSImpedance[controllerName]->config.desiredNullspaceJointAngles = targetJointAngles; + m_ctrlTSImpedance[controllerName]->updateConfig(); + break; + case control::common::ControllerType::TSAdmittance: + ARMARX_CHECK(m_ctrlTSAdmittance[controllerName]); + m_ctrlTSAdmittance[controllerName]->config.desiredNullspaceJointAngles = targetJointAngles; + m_ctrlTSAdmittance[controllerName]->updateConfig(); + break; + + default: + ARMARX_INFO << "controller type: " << controllerType << " is not supported yet"; + return false; + } + return true; + } + + bool + Component::updateTarget( + const std::string &controllerName, + const std::string &controllerType, + const Ice::FloatSeq& targetPoseVector, + const Ice::FloatSeq& targetNullspaceJointAngle, + const Ice::Current &) + { + auto names = control::common::ControllerTypeShort.names(); + const bool is_in = names.find(controllerType) != names.end(); + if (!is_in) + { + ARMARX_ERROR << "controller type not supported"; + return false; + } + + bool updataPose = false; + if (targetPoseVector.size() > 0) + { + if (targetPoseVector.size() != 7) + { + ARMARX_ERROR << "target pose vector " << targetPoseVector << " should follow (x, y, z, w, rx, ry, rz) convention"; + return false; + } + updataPose = true; + m_prevTargetPose[controllerName] = VirtualRobot::MathTools::quat2eigen4f(targetPoseVector.at(4), targetPoseVector.at(5), targetPoseVector.at(6), targetPoseVector.at(3)); + m_prevTargetPose[controllerName](0, 3) = targetPoseVector.at(0); + m_prevTargetPose[controllerName](1, 3) = targetPoseVector.at(1); + m_prevTargetPose[controllerName](2, 3) = targetPoseVector.at(2); + } + + bool updateNullspace = false; + if (targetNullspaceJointAngle.size() > 0) + { + for (size_t i = 0; i < m_controllerJointNumbers[controllerName]; i++) + { + m_prevNullspaceJointTarget[controllerName](i) = targetNullspaceJointAngle[i]; + } + } + + + auto type = control::common::ControllerTypeShort.from_name(controllerType); + switch (type) { + case control::common::ControllerType::TSImpedance: + ARMARX_CHECK(m_ctrlTSImpedance[controllerName]); + if (updataPose) + { + m_ctrlTSImpedance[controllerName]->config.desiredPose = m_prevTargetPose[controllerName]; + } + if (updateNullspace) + { + m_ctrlTSImpedance[controllerName]->config.desiredNullspaceJointAngles = m_prevNullspaceJointTarget[controllerName]; + } + m_ctrlTSImpedance[controllerName]->updateConfig(); + break; + case control::common::ControllerType::TSAdmittance: + ARMARX_CHECK(m_ctrlTSAdmittance[controllerName]); + if (updataPose) + { + m_ctrlTSAdmittance[controllerName]->config.desiredPose = m_prevTargetPose[controllerName]; + } + if (updateNullspace) + { + m_ctrlTSAdmittance[controllerName]->config.desiredNullspaceJointAngles = m_prevNullspaceJointTarget[controllerName]; + } + m_ctrlTSAdmittance[controllerName]->updateConfig(); + break; + + default: + ARMARX_INFO << "controller type: " << controllerType << " is not supported yet"; + return false; + } + return true; + } + Ice::FloatSeq Component::getPoseInRootFrame(const std::string& nodeName, const Ice::Current&) diff --git a/source/armarx/control/components/controller_creator/Component.h b/source/armarx/control/components/controller_creator/Component.h index 6a79a04f..46100cd3 100644 --- a/source/armarx/control/components/controller_creator/Component.h +++ b/source/armarx/control/components/controller_creator/Component.h @@ -107,6 +107,10 @@ namespace armarx::control::components::controller_creator Ice::FloatSeq getPrevTargetPose(const std::string& controllerName, const Ice::Current& = Ice::emptyCurrent) override; Ice::FloatSeq getPrevNullspaceTargetAngle(const std::string& controllerName, const Ice::Current& = Ice::emptyCurrent) override; + bool updateTargetPose(const std::string &controllerName, const std::string& controllerType, const Ice::FloatSeq& targetPoseVector, const Ice::Current & = Ice::emptyCurrent) override; + bool updateTargetNullspaceJointAngles(const std::string &controllerName, const std::string &controllerType, const Ice::FloatSeq& targetNullspaceJointAngle, const Ice::Current & = Ice::emptyCurrent) override; + bool updateTarget(const std::string &controllerName, const std::string &controllerType, const Ice::FloatSeq& targetPoseVector, const Ice::FloatSeq& targetNullspaceJointAngle, const Ice::Current & = Ice::emptyCurrent) override; + private: // Private methods go here. @@ -139,6 +143,8 @@ namespace armarx::control::components::controller_creator std::map<std::string, std::optional<control::client::ControllerWrapper<control::common::ControllerType::TSImpedance>>> m_ctrlTSImpedance; std::map<std::string, std::optional<control::client::ControllerWrapper<control::common::ControllerType::TSAdmittance>>> m_ctrlTSAdmittance; + std::map<std::string, VirtualRobot::RobotNodeSetPtr> m_robotNodeSets; + std::map<std::string, unsigned int> m_controllerJointNumbers; std::map<std::string, Eigen::Matrix4f> m_prevTargetPose; std::map<std::string, Eigen::VectorXf> m_prevNullspaceJointTarget; diff --git a/source/armarx/control/components/controller_creator/ComponentInterface.ice b/source/armarx/control/components/controller_creator/ComponentInterface.ice index 36519323..11c03cdf 100644 --- a/source/armarx/control/components/controller_creator/ComponentInterface.ice +++ b/source/armarx/control/components/controller_creator/ComponentInterface.ice @@ -30,8 +30,11 @@ module armarx { module control { module components { module controller_creato interface ComponentInterface extends armarx::armem::client::MemoryListenerInterface { - string createController(string controllerNamePrefix, string robotNodeSet, string controllerType, string configFilename); + bool updateTargetPose(string controllerName, string controllerType, Ice::FloatSeq targetPoseVector); + bool updateTargetNullspaceJointAngles(string controllerName, string controllerType, Ice::FloatSeq targetNullspaceJointAngle); + bool updateTarget(string controllerName, string controllerType, Ice::FloatSeq targetPoseVector, Ice::FloatSeq targetNullspaceJointAngle); + Ice::FloatSeq getPoseInRootFrame(string node_name); Ice::FloatSeq getPrevTargetPose(string controllerName); Ice::FloatSeq getPrevNullspaceTargetAngle(string controllerName); -- GitLab From 948916cd326efb655966a4d22542b3a325c9c964 Mon Sep 17 00:00:00 2001 From: Jianfeng Gao <jianfeng.gao@kit.edu> Date: Fri, 24 Mar 2023 19:46:41 +0100 Subject: [PATCH 15/16] recover deleted features --- .../armarx/control/client/ControllerBuilder.h | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/source/armarx/control/client/ControllerBuilder.h b/source/armarx/control/client/ControllerBuilder.h index 0c720ce5..3b3ecbab 100644 --- a/source/armarx/control/client/ControllerBuilder.h +++ b/source/armarx/control/client/ControllerBuilder.h @@ -148,6 +148,15 @@ namespace armarx::control::client return *this; } + ControllerBuilder& + withNamePrefix(const std::string& namePrefix) + { + if (namePrefix.empty()) + return *this; + this->controllerNamePrefix = this->controllerNamePrefix + "_" + namePrefix; + return *this; + } + /** * @brief Creates the controller with the default name * @@ -180,7 +189,7 @@ namespace armarx::control::client } config->nodeSetName = nodeSetName.value(); - const auto controllerName = controllerNamePrefix + "_" + name; + controllerName = controllerNamePrefix + "_" + nodeSetName.value() + "_" + name; const std::string controllerClassName = ControllerDescriptionType::name; // reuse or create controller @@ -276,6 +285,11 @@ namespace armarx::control::client this->allowReuse_ = allowReuse; } + std::string getControllerName() + { + return controllerName; + } + private: bool initDefaultConfig() @@ -312,7 +326,7 @@ namespace armarx::control::client std::optional<std::string> nodeSetName; const std::string controllerNamePrefix; - + std::string controllerName; bool allowReuse_ = false; }; } // namespace armarx::control::client -- GitLab From b63325fbbf7e5d98011aeab7c1048149354aaaad Mon Sep 17 00:00:00 2001 From: Jianfeng Gao <jianfeng.gao@kit.edu> Date: Fri, 24 Mar 2023 19:53:06 +0100 Subject: [PATCH 16/16] fix compiling --- source/armarx/control/client/ControllerBuilder.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/armarx/control/client/ControllerBuilder.h b/source/armarx/control/client/ControllerBuilder.h index 3b3ecbab..fb57a747 100644 --- a/source/armarx/control/client/ControllerBuilder.h +++ b/source/armarx/control/client/ControllerBuilder.h @@ -325,7 +325,7 @@ namespace armarx::control::client std::optional<std::string> nodeSetName; - const std::string controllerNamePrefix; + std::string controllerNamePrefix; std::string controllerName; bool allowReuse_ = false; }; -- GitLab