From 27f4dbbeb845a09331c52242d6be588d4fe20dbc Mon Sep 17 00:00:00 2001
From: Frederik Koch <ulxvc@student.kit.edu>
Date: Mon, 20 May 2024 18:59:49 +0200
Subject: [PATCH] KneeHip balance controller now works
---
.../NJointKneeHipController.cpp | 146 ++++++++++++++----
.../NJointKneeHipController.h | 46 ++++--
2 files changed, 146 insertions(+), 46 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp
index faefcca81..1fe823ca4 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp
@@ -1,9 +1,15 @@
#include "NJointKneeHipController.h"
+#include <algorithm>
+#include <cmath>
#include <SimoxUtility/math/distance/angle_between.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
+#include "RobotAPI/components/units/RobotUnit/ControlModes.h"
+#include "RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h"
+#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.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>
@@ -28,8 +34,8 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(robotUnit);
// config
- ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION(cfg);
+ // ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
+ // ARMARX_CHECK_EXPRESSION(cfg);
// const arondto::Config configData = arondto::Config::FromAron(cfg->config);
@@ -55,13 +61,19 @@ namespace armarx
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);
+ // auto kneeBase = useControlTarget(kneeJointName, armarx::ControlModes::Position1DoF);
+ // _rtKneeCtrlTarget = kneeBase->asA<ControlTarget1DoFActuatorPosition>();
+ // ARMARX_CHECK_NOT_NULL(_rtKneeCtrlTarget);
+
+ // _rtHipCtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
+ // hipJointName, armarx::ControlModes::Position1DoF);
+ auto hipBase = useControlTarget(hipJointName, armarx::ControlModes::Velocity1DoF);
+ _rtHipCtrlTarget = hipBase->asA<ControlTarget1DoFActuatorVelocity>();
ARMARX_CHECK_NOT_NULL(_rtHipCtrlTarget);
}
+ const SensorValueBase* sv = useSensorValue(_rtKneeNode->getName());
+ velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ // positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
ARMARX_RT_LOGF("Nodes set up successfully");
}
@@ -93,42 +105,88 @@ namespace armarx
std::string
NJointKneeHipController::getClassName(const Ice::Current&) const
{
- return "KneeHipController";
+ return "NJointKneeHipController";
+ }
+
+ double
+ NJointKneeHipController::calculate_dAnkle_dKnee(double kneeAngle, double shank, double p1, double p2, double p3)
+ {
+ constexpr auto squared = [](const double t) { return t * t; };
+ const double k1 = shank / p1;
+ const double k2 = shank / p3;
+ const double k3 = (squared(shank) + squared(p1) + squared(p3) - squared(p2)) / (2 * p1 * p3);
+ const double theta = kneeAngle;
+
+ double T_fkt = sin(theta) * (((squared(k1)+k1+1)*k2-k1*k3)*cos(theta)-squared(k3)+(k1-1)*k2*k3+k1*squared(k2)+k1+1)
+ +((k3+k2)*cos(theta)+k1+1)*sqrt(-squared(k1)*squared(cos(theta))-(2*k1*k3-2*k2)*cos(theta)-squared(k3)+squared(k2)+1);
+
+
+ double B_fkt = sqrt(-squared(k1)*squared(cos(theta))-(2*k1*k3-2*k2)*cos(theta)-squared(k3)+squared(k2)+1)
+ * (sin(theta) * sqrt(-squared(k1)*squared(cos(theta))-(2*k1*k3-2*k2)*cos(theta)-squared(k3)+squared(k2)+1)
+ + k1*squared(cos(theta))+(k3+(k1+2)*k2)*cos(theta)+k2*k3+squared(k2)+1);
+
+
+ double dAnkle_dKnee = T_fkt / B_fkt;
+
+ return dAnkle_dKnee;
}
+
+ // double NJointKneeHipController::calculate_psi(double kneeAngle, double shank, double p1, double p2, double p3)
+ // {
+ // constexpr auto squared = [](const double t) { return t * t; };
+ // const double k1 = shank / p1;
+ // const double k2 = shank / p3;
+ // const double k3 = (squared(shank) + squared(p1) + squared(p3) - squared(p2)) / (2 * p1 * p3);
+ // const double theta = kneeAngle;
+
+ // /* Solution of Freudenstein Equation*/
+ // double psi = 2 * atan((cos(theta) + sqrt(cos(theta) * cos(theta) -
+ // 4 * (k1 * cos(theta) + cos(theta) + k2 + k3) *
+ // (k1 * cos(theta) - cos(theta) - k2 + k3))) /
+ // (k1 * cos(theta) + cos(theta) + k2 + k3));
+
+ // return psi;
+ // }
+
+
void
NJointKneeHipController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
- const IceUtil::Time& /*timeSinceLastIteration*/)
+ 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());
+ auto& debugData = bufferRtToOnPublish_.getWriteBuffer();
+
- // hip joint has positive angles leaning forwards (inverse to hipAngle)
- float targetHipAngle =
- currentHipJointValue + hipAngle - M_PI_2 + rtGetControlStruct().bowAngle;
- float targetKneeAngle = rtGetControlStruct().kneeAngle;
+ double kneeVelocity = velocitySensor->velocity;
+ debugData.kneeVelocity = kneeVelocity;
+
+ double kneeAngle = _rtKneeNode->getJointValue();
+ debugData.kneeAngle = 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;
+ double deltaKnee = kneeVelocity;
+ double dHip_dKnee = 1 - calculate_dAnkle_dKnee(kneeAngle, 280, 84.372, 269.964, 45);
+
+ double deltaHip = dHip_dKnee * deltaKnee;
+ double hipVelocity = deltaHip;
+ debugData.hipVelocity = hipVelocity;
+
+ std::clamp(hipVelocity, -0.5, 0.5);
+ _rtHipCtrlTarget->velocity = hipVelocity;
+
+
+
+ /* Position approach */
+
+ // double theta = _rtKneeNode->getJointValue();
+ // double psi = calculate_psi(theta, 280, 84.372, 269.964, 45);
+ // double target = theta - psi + M_PI/5;
+
+ // _rtHipCtrlTarget->position = target;
+
+ bufferRtToOnPublish_.commitWrite();
}
void
@@ -141,5 +199,27 @@ namespace armarx
{
}
+ void
+ NJointKneeHipController::onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx& debugObservers)
+ {
+
+ StringVariantBaseMap datafields;
+
+ const auto& debugData = bufferRtToOnPublish_.getUpToDateReadBuffer();
+
+ datafields["localTimestamp"] = new Variant(debugData.localTimestamp);
+
+ datafields["kneeAngle"] = new Variant(debugData.kneeAngle);
+ datafields["kneeVelocity"] = new Variant(debugData.kneeVelocity);
+ datafields["hipAngle"] = new Variant(debugData.hipAngle);
+ datafields["hipVelocity"] = new Variant(debugData.hipVelocity);
+
+ debugObservers->setDebugChannel("NJointKneeHipController",
+ datafields);
+
+ }
+
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.h
index 94fc676f8..270d36a68 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.h
@@ -21,6 +21,8 @@
*/
#pragma once
+#include <ArmarXCore/interface/core/BasicTypes.h>
+#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h"
#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
@@ -41,23 +43,17 @@ namespace armarx
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(NJointKneeHipControllerConfig);
+
TYPEDEF_PTRS_HANDLE(NJointKneeHipController);
class NJointKneeHipController :
virtual public armarx::NJointControllerWithTripleBuffer<Config>
{
public:
- using ConfigPtrT = NJointKneeHipControllerConfigPtr;
+ // using ConfigPtrT = NJointKneeHipControllerPtr;
+ using ConfigPtrT = NJointControllerConfigPtr;
NJointKneeHipController(RobotUnit* robotUnit,
const armarx::NJointControllerConfigPtr& config,
@@ -67,7 +63,10 @@ namespace armarx
// void updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
// const Ice::Current& iceCurrent = Ice::emptyCurrent) ;
-
+ void onPublish(const SensorAndControl&,
+ const DebugDrawerInterfacePrx&,
+ const DebugObserverInterfacePrx& debugObserver) override;
+
protected:
//
// NJointControllerInterface interface
@@ -85,6 +84,8 @@ namespace armarx
void rtPreActivateController() override;
void rtPostDeactivateController() override;
+
+
// ----------------
private:
@@ -93,9 +94,28 @@ namespace armarx
VirtualRobot::RobotNodePtr _rtKneeNode;
VirtualRobot::RobotNodePtr _rtHipNode;
VirtualRobot::RobotNodePtr _rtHipBodyNode;
+ const SensorValue1DoFActuatorVelocity* velocitySensor;
+ // const SensorValue1DoFActuatorPosition* positionSensor;
+ static double calculate_dAnkle_dKnee(double kneeAngle, double shank, double p1, double p2, double p3);
+ static double calculate_psi(double kneeAngle, double shank, double p1, double p2, double p3);
+
+ // armarx::ControlTarget1DoFActuatorPosition* _rtKneeCtrlTarget;
+ armarx::ControlTarget1DoFActuatorVelocity* _rtHipCtrlTarget;
+
+ struct DebugData
+ {
+ std::int64_t localTimestamp;
+
+ double kneeAngle;
+ double kneeVelocity;
+ double hipAngle;
+ double hipVelocity;
+
+
+ };
+
+ TripleBuffer<DebugData> bufferRtToOnPublish_;
- armarx::ControlTarget1DoFActuatorPosition* _rtKneeCtrlTarget;
- armarx::ControlTarget1DoFActuatorPosition* _rtHipCtrlTarget;
};
} // namespace armarx
--
GitLab