diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index dbaaf4873bd64b8aede4752f0b8cdc8de3029579..626d2fb4c770f6ac632fd36618d28be861c9f28c 100755
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -53,6 +53,7 @@ set(LIB_FILES
NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
NJointControllers/NJointCartesianWaypointController.cpp
NJointControllers/NJointCartesianNaturalPositionController.cpp
+ NJointControllers/NJointKneeHipController.cpp
ControllerParts/CartesianImpedanceController.cpp
@@ -131,6 +132,7 @@ set(LIB_HEADERS
NJointControllers/NJointCartesianVelocityControllerWithRamp.h
NJointControllers/NJointCartesianWaypointController.h
NJointControllers/NJointCartesianNaturalPositionController.h
+ NJointControllers/NJointKneeHipController.h
ControllerParts/CartesianImpedanceController.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..faefcca81ae8160e2db3f94761ed4f22b40fd1e0
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp
@@ -0,0 +1,145 @@
+#include "NJointKneeHipController.h"
+
+#include <SimoxUtility/math/distance/angle_between.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/interface/observers/VariantBase.h>
+
+// #include <armar7/rt/units/njoint_controller/common/controller_types.h>
+// #include <armar7/rt/units/njoint_controller/knee_hip_controller/aron/ControllerConfig.aron.generated.h>
+// #include <armar7/rt/units/njoint_controller/knee_hip_controller/aron_conversions.h>
+
+
+
+
+namespace armarx
+
+{
+ const armarx::NJointControllerRegistration<NJointKneeHipController>
+ registrationControllerNJointKneeHipController(
+ "NJointKneeHipController");
+
+ NJointKneeHipController::NJointKneeHipController(RobotUnit* robotUnit,
+ const armarx::NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr&)
+ {
+ ARMARX_RT_LOGF("Creating knee hip controller");
+
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+
+ // config
+ ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION(cfg);
+
+ // const arondto::Config configData = arondto::Config::FromAron(cfg->config);
+
+ // robot
+ ARMARX_RT_LOGF("Setting up nodes");
+ {
+ _rtRobot = useSynchronizedRtRobot();
+ _rtRobot->setThreadsafe(false);
+ ARMARX_CHECK_NOT_NULL(_rtRobot);
+
+ // _rtKneeNode = _rtRobot->getRobotNode(configData.params.kneeJointName);
+ // _rtHipNode = _rtRobot->getRobotNode(configData.params.hipJointName);
+ // _rtHipBodyNode = _rtRobot->getRobotNode(configData.params.hipBodyName);
+ const auto *kneeJointName = "Knee";
+ const auto *hipJointName = "Hip";
+ const auto *hipBodyName = "Hip_body";
+
+
+ _rtKneeNode = _rtRobot->getRobotNode(kneeJointName);
+ _rtHipNode = _rtRobot->getRobotNode(hipJointName);
+ _rtHipBodyNode = _rtRobot->getRobotNode(hipBodyName);
+ ARMARX_CHECK_NOT_NULL(_rtKneeNode);
+ ARMARX_CHECK_NOT_NULL(_rtHipNode);
+ ARMARX_CHECK_NOT_NULL(_rtHipBodyNode);
+
+ _rtKneeCtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
+ kneeJointName, armarx::ControlModes::Position1DoF);
+ ARMARX_CHECK_NOT_NULL(_rtKneeCtrlTarget);
+ _rtHipCtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
+ hipJointName, armarx::ControlModes::Position1DoF);
+ ARMARX_CHECK_NOT_NULL(_rtHipCtrlTarget);
+ }
+
+ ARMARX_RT_LOGF("Nodes set up successfully");
+ }
+
+ NJointKneeHipController::~NJointKneeHipController() = default;
+
+ // void
+ // KneeHipController::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
+ // const Ice::Current& /*iceCurrent*/)
+ // {
+ // // const auto updateConfigDto = arondto::Config::FromAron(dto);
+
+ // // Config config;
+ // // fromAron(updateConfigDto, config);
+
+ // // setControlStruct(config);
+ // }
+
+ void
+ NJointKneeHipController::onInitNJointController()
+ {
+ }
+
+ void
+ NJointKneeHipController::onConnectNJointController()
+ {
+ }
+
+ std::string
+ NJointKneeHipController::getClassName(const Ice::Current&) const
+ {
+ return "KneeHipController";
+ }
+
+ void
+ NJointKneeHipController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
+ const IceUtil::Time& /*timeSinceLastIteration*/)
+ {
+ const float currentHipJointValue = _rtHipNode->getJointValue();
+ const auto hipRotation =
+ _rtHipBodyNode->getOrientationInRootFrame() * Eigen::Vector3f::UnitY();
+
+ // angle between robot root and hip body in the yz-plane
+ // leaning forwards < 90° = upright < leaning backwards
+ const float hipAngle = simox::math::angle_between_vec3f_vec3f(
+ Eigen::Vector3f::UnitY(), hipRotation, Eigen::Vector3f::UnitX());
+
+ // hip joint has positive angles leaning forwards (inverse to hipAngle)
+ float targetHipAngle =
+ currentHipJointValue + hipAngle - M_PI_2 + rtGetControlStruct().bowAngle;
+
+ float targetKneeAngle = rtGetControlStruct().kneeAngle;
+
+ // check reachability
+ const bool targetReachable = _rtKneeNode->checkJointLimits(targetKneeAngle) &&
+ _rtHipNode->checkJointLimits(targetHipAngle);
+ if (not targetReachable)
+ {
+ // limit joint ranges to avoid damage
+ targetKneeAngle = std::clamp(
+ targetKneeAngle, _rtKneeNode->getJointLimitLow(), _rtKneeNode->getJointLimitHigh());
+ targetHipAngle = std::clamp(
+ targetHipAngle, _rtHipNode->getJointLimitLow(), _rtHipNode->getJointLimitHigh());
+ }
+
+ _rtKneeCtrlTarget->position = targetKneeAngle;
+ _rtHipCtrlTarget->position = targetHipAngle;
+ }
+
+ void
+ NJointKneeHipController::rtPreActivateController()
+ {
+ }
+
+ void
+ NJointKneeHipController::rtPostDeactivateController()
+ {
+ }
+
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.h
new file mode 100644
index 0000000000000000000000000000000000000000..94fc676f84312ae446ee16a8a040c05d9a734e24
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.h
@@ -0,0 +1,101 @@
+/*
+ * 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 armar7_rt
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2023
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
+#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
+
+namespace armarx
+{
+ struct Params
+ {
+ std::string kneeJointName;
+ std::string hipJointName;
+ std::string hipBodyName;
+ };
+
+ struct Config
+ {
+ Params params;
+ float kneeAngle;
+ float bowAngle;
+ };
+
+ TYPEDEF_PTRS_HANDLE(NJointKneeHipControllerConfig);
+
+ class NJointKneeHipControllerConfig : virtual public armarx::NJointControllerConfig
+ {
+ public:
+ std::string deviceName1;
+ std::string deviceName;
+ std::string controlMode;
+ };
+
+ TYPEDEF_PTRS_HANDLE(NJointKneeHipController);
+
+ class NJointKneeHipController :
+ virtual public armarx::NJointControllerWithTripleBuffer<Config>
+ {
+ public:
+ using ConfigPtrT = NJointKneeHipControllerConfigPtr;
+
+ NJointKneeHipController(RobotUnit* robotUnit,
+ const armarx::NJointControllerConfigPtr& config,
+ const VirtualRobot::RobotPtr& /* unused */);
+
+ ~NJointKneeHipController() override;
+
+ // void updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
+ // const Ice::Current& iceCurrent = Ice::emptyCurrent) ;
+
+ protected:
+ //
+ // NJointControllerInterface interface
+ //
+
+ void onInitNJointController() override;
+
+ void onConnectNJointController() override;
+
+ std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override;
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+
+ void rtPreActivateController() override;
+ void rtPostDeactivateController() override;
+
+ // ----------------
+
+ private:
+ // rt variables
+ VirtualRobot::RobotPtr _rtRobot;
+ VirtualRobot::RobotNodePtr _rtKneeNode;
+ VirtualRobot::RobotNodePtr _rtHipNode;
+ VirtualRobot::RobotNodePtr _rtHipBodyNode;
+
+ armarx::ControlTarget1DoFActuatorPosition* _rtKneeCtrlTarget;
+ armarx::ControlTarget1DoFActuatorPosition* _rtHipCtrlTarget;
+ };
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 5304a0aa8a0e6716b6c9c8266c01d9db61e9ee6a..49b6d3002006fa8b6675e3b4748e4bd31b50e53a 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -25,6 +25,7 @@
#include <ArmarXCore/core/IceGridAdmin.h>
#include <ArmarXCore/core/IceManager.h>
+#include "RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.h"
#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h"
#include "RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h"
#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
@@ -277,6 +278,7 @@ namespace armarx::RobotUnitModule
auto controlMode = requestedControlMode;
using CtargT = std::remove_const_t<std::remove_reference_t<decltype(*tpptr)>>;
NJointKinematicUnitPassThroughControllerPtr& pt = ctrl;
+
auto testMode = [&](const auto& m)
{
@@ -325,17 +327,37 @@ namespace armarx::RobotUnitModule
std::replace(ctrl_name.begin(), ctrl_name.end(), '.', '_');
std::replace(ctrl_name.begin(), ctrl_name.end(), '-', '_');
std::replace(ctrl_name.begin(), ctrl_name.end(), ':', '_');
- const NJointControllerBasePtr& nJointCtrl =
+ const NJointControllerBasePtr& nJointCtrl1 =
_module<ControllerManagement>().createNJointController(
"NJointKinematicUnitPassThroughController",
ctrl_name,
config,
false,
true);
- pt = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(nJointCtrl);
+ pt = NJointKinematicUnitPassThroughControllerPtr::dynamicCast(nJointCtrl1);
ARMARX_CHECK_EXPRESSION(pt);
}
+
+
+
};
+
+ if (controlDevice->getDeviceName() == "Knee")
+ {
+ NJointKneeHipControllerConfigPtr config = new NJointKneeHipControllerConfig;
+ config->controlMode = positionControlMode;
+ config->deviceName = controlDeviceName;
+ auto ctrl_name = "KneeHipController";
+
+ const NJointControllerBasePtr& nJointCtrl2 =
+ _module<ControllerManagement>().createNJointController(
+ "NJointKneeHipController", ctrl_name, config, false, true);
+ NJointKneeHipControllerPtr kh =
+ NJointKneeHipControllerPtr::dynamicCast(nJointCtrl2);
+ ARMARX_CHECK_EXPRESSION(kh);
+ }
+
+
init_controller(
positionControlMode, ad.ctrlPos, (ControlTarget1DoFActuatorPosition*)0);
init_controller(