From 9286aaff711064eb8bfc8112879414fec8e701dc Mon Sep 17 00:00:00 2001
From: Frederik Koch <ulxvc@student.kit.edu>
Date: Wed, 22 May 2024 17:42:09 +0200
Subject: [PATCH] RBDL now runs in RT loop

---
 .../NJointKneeHipController.cpp               | 274 +++++++++++++++---
 .../NJointKneeHipController.h                 |  40 ++-
 .../RobotUnitModules/RobotUnitModuleUnits.cpp |  17 --
 .../RobotUnit/NJointKneeHipController.ice     |  51 ++++
 4 files changed, 323 insertions(+), 59 deletions(-)
 create mode 100644 source/RobotAPI/interface/units/RobotUnit/NJointKneeHipController.ice

diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp
index 1fe823ca4..be4cc405a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp
@@ -1,14 +1,19 @@
 #include "NJointKneeHipController.h"
 #include <algorithm>
 #include <cmath>
+#include <memory>
 
 #include <SimoxUtility/math/distance/angle_between.h>
 
+#include "ArmarXCore/core/ArmarXObjectScheduler.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 <ArmarXCore/core/time/CycleUtil.h>
+#include <VirtualRobot/Dynamics/Dynamics.h>
+
 
 
 // #include <armar7/rt/units/njoint_controller/common/controller_types.h>
@@ -33,12 +38,6 @@ namespace armarx
 
         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");
         {
@@ -51,15 +50,12 @@ namespace armarx
             // _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);
 
             // auto kneeBase = useControlTarget(kneeJointName, armarx::ControlModes::Position1DoF);
             // _rtKneeCtrlTarget = kneeBase->asA<ControlTarget1DoFActuatorPosition>();
@@ -71,8 +67,104 @@ namespace armarx
             _rtHipCtrlTarget = hipBase->asA<ControlTarget1DoFActuatorVelocity>();
             ARMARX_CHECK_NOT_NULL(_rtHipCtrlTarget);
         }
+
+        _nonrtRobot = _rtRobot->clone();
+
+        /* Initialize RBDL*/
+
+
+        std::string rnsName = "FourbarStructure";
+        std::string rnsBodyName = "FourbarStructureBody";
+
+        VirtualRobot::RobotNodeSetPtr rns = _nonrtRobot->getRobotNodeSet(rnsName);
+        VirtualRobot::RobotNodeSetPtr rnsBodies = _nonrtRobot->getRobotNodeSet(rnsBodyName);
+
+        VirtualRobot::Dynamics dynamics = VirtualRobot::Dynamics(rns, rnsBodies, false);
+        auto dynamicsPtr = std::make_shared<VirtualRobot::Dynamics>(dynamics);
+        auto model = dynamics.getModel();
+
+         /* Declare Constraints for Fourbar structure */
+
+        auto p3_id = model->GetBodyId("P3");
+        // VR_ASSERT(p3_id != std::numeric_limits<unsigned int>::max());
+
+        auto knee_id = model->GetBodyId("Knee");
+        // VR_ASSERT(knee_id != std::numeric_limits<unsigned int>::max());
+
+        RigidBodyDynamics::ConstraintSet loopConstraintSet;
+
+        bool stabilizationEnabled = true;
+        float stabilizationFactor = 0.1;
+
+        // x
+        loopConstraintSet.AddLoopConstraint(p3_id,
+                                            knee_id,
+                                            RigidBodyDynamics::Math::SpatialTransform(),
+                                            RigidBodyDynamics::Math::SpatialTransform(),
+                                            {0, 0, 0, 1, 0, 0},
+                                            stabilizationEnabled,
+                                            stabilizationFactor);
+
+        // y
+        loopConstraintSet.AddLoopConstraint(p3_id,
+                                            knee_id,
+                                            RigidBodyDynamics::Math::SpatialTransform(),
+                                            RigidBodyDynamics::Math::SpatialTransform(),
+                                            {0, 0, 0, 0, 1, 0},
+                                            stabilizationEnabled,
+                                            stabilizationFactor);
+
+        // // z
+        // loopConstraintSet.AddLoopConstraint(p3_id,
+        //                                     knee_id,
+        //                                     RigidBodyDynamics::Math::SpatialTransform(),
+        //                                     RigidBodyDynamics::Math::SpatialTransform(),
+        //                                     {0, 0, 0, 0, 0, 1},
+        //                                     stabilizationEnabled,
+        //                                     stabilizationFactor);
+
+        // rx
+        loopConstraintSet.AddLoopConstraint(p3_id,
+                                            knee_id,
+                                            RigidBodyDynamics::Math::SpatialTransform(),
+                                            RigidBodyDynamics::Math::SpatialTransform(),
+                                            {1, 0, 0, 0, 0, 0},
+                                            stabilizationEnabled,
+                                            stabilizationFactor);
+
+        // ry
+        loopConstraintSet.AddLoopConstraint(p3_id,
+                                            knee_id,
+                                            RigidBodyDynamics::Math::SpatialTransform(),
+                                            RigidBodyDynamics::Math::SpatialTransform(),
+                                            {0, 1, 0, 0, 0, 0},
+                                            stabilizationEnabled,
+                                            stabilizationFactor);
+
+        // rz
+        loopConstraintSet.AddLoopConstraint(p3_id,
+                                            knee_id,
+                                            RigidBodyDynamics::Math::SpatialTransform(),
+                                            RigidBodyDynamics::Math::SpatialTransform(),
+                                            {0, 0, 1, 0, 0, 0},
+                                            stabilizationEnabled,
+                                            stabilizationFactor);
+
+        dynamics.setConstraintSet(loopConstraintSet);
+
+        auto& dynamBuffer =
+            dynamics_rtToAdditionalTask.getWriteBuffer();
+        dynamBuffer.kneeId = 1;
+        dynamBuffer.dynamics.emplace(dynamics);
+        dynamics_rtToAdditionalTask.commitWrite();
+
+        
+
+        /* Build Sensors */
+
         const SensorValueBase* sv = useSensorValue(_rtKneeNode->getName());
         velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+        torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
         // positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
 
         ARMARX_RT_LOGF("Nodes set up successfully");
@@ -95,8 +187,79 @@ namespace armarx
     void
     NJointKneeHipController::onInitNJointController()
     {
+        runTask("NJointKneeHipControllerAdditionalTask",
+                [&]
+                {
+                    CycleUtil c(20);
+                    getObjectScheduler()->waitForObjectStateMinimum(eManagedIceObjectStarted);
+                    ARMARX_IMPORTANT << "Create a new thread alone NJointKneeHipController controller";
+                    while (getState() == eManagedIceObjectStarted)
+                    {
+                        if (isControllerActive())
+                        {
+                            ARMARX_IMPORTANT << "additional task started";
+                            additionalTask();
+                        } else {
+                            // ARMARX_IMPORTANT << "controller not active, additional Task skipped"; 
+                        }
+                        c.waitForCycleDuration();
+                    }
+                });
+
+        ARMARX_INFO << "NJointKneeHipController::onInitNJointController done.";
     }
 
+
+    void
+    NJointKneeHipController::additionalTask()
+    {
+        ARMARX_TRACE;
+
+        // ARMARX_IMPORTANT << "before read buffer";
+
+        const auto& dynamBuffer =
+            dynamics_rtToAdditionalTask.getUpToDateReadBuffer();
+
+        const auto& jointValuesBuffer =
+            jointValues_rtToAdditionalTask.getUpToDateReadBuffer();
+
+
+        // ARMARX_IMPORTANT << "before dynamics";
+
+        if (!dynamBuffer.dynamics.has_value()) {
+            ARMARX_ERROR << "dynamics buffer is empty!";
+            return;
+        }
+        auto dynamics = dynamBuffer.dynamics.value();
+       
+        
+        Eigen::VectorXd q = jointValuesBuffer.jointValues.cast<double>();
+
+        // ARMARX_IMPORTANT << "before getGravityMatrix";
+        // ARMARX_IMPORTANT << q.transpose();
+
+        Eigen::VectorXd torque = dynamics.getGravityMatrix(q);
+        
+
+        // ARMARX_IMPORTANT << "before writing to buffer";
+        double kneeTorque = torque(dynamBuffer.kneeId);
+        ARMARX_IMPORTANT << torque.transpose();
+
+        // store results (onPublish)
+
+        auto& dynamicsBuffer = torques_AdditionalTaskToRt.getWriteBuffer();
+        dynamicsBuffer.torque = -0.5*kneeTorque;
+        torques_AdditionalTaskToRt.commitWrite();
+
+        auto& debugData = debugBuffer.getWriteBuffer();
+        debugData.kneeTorqueSim = -0.5*kneeTorque;
+        debugBuffer.commitWrite();
+
+        ARMARX_IMPORTANT << "Finished additional Task";
+        
+    }
+
+
     void
     NJointKneeHipController::onConnectNJointController()
     {
@@ -132,71 +295,97 @@ namespace armarx
     }
 
 
-    // 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;
-    // }
+    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((sin(theta) + 0.5*sqrt(4 * sin(theta) * sin(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)
     {
+        // ARMARX_IMPORTANT << "KneeHip rtRun";
 
-        auto& debugData = bufferRtToOnPublish_.getWriteBuffer();
-        
+        manageKnee();
+        // ARMARX_IMPORTANT << "after Knee";
+        manageHip();
+        // ARMARX_IMPORTANT << "after hip";
+    }
+
+
+    void
+    NJointKneeHipController::manageKnee()
+    {
+        auto& robotBuffer = jointValues_rtToAdditionalTask.getWriteBuffer();
 
+        std::string rnsName = "FourbarStructure";
+        robotBuffer.jointValues = _rtRobot->getRobotNodeSet(rnsName)->getJointValuesEigen();
+
+        jointValues_rtToAdditionalTask.commitWrite(); 
+
+    }
+
+
+    void
+    NJointKneeHipController::manageHip()
+    {
+        auto& debugData = debugBuffer.getWriteBuffer();
+
+        double kneeTorque = torqueSensor->torque;
+        debugData.kneeTorqueSen = kneeTorque;
 
         double kneeVelocity = velocitySensor->velocity;
         debugData.kneeVelocity = kneeVelocity;
-        
+
         double kneeAngle = _rtKneeNode->getJointValue();
         debugData.kneeAngle = kneeAngle;
 
 
+        double hipAngle = _rtHipNode->getJointValue();
+        double ankleAngle = calculate_psi(kneeAngle, 280, 84.372, 269.964, 45);
+        debugData.ankleAngle = ankleAngle;
+        double globalRotation = kneeAngle - ankleAngle + M_PI / 5 - hipAngle;
+        debugData.globalRotation = globalRotation;
+
+
         double deltaKnee = kneeVelocity;
         double dHip_dKnee = 1 - calculate_dAnkle_dKnee(kneeAngle, 280, 84.372, 269.964, 45);
 
-        double deltaHip =  dHip_dKnee * deltaKnee;
+        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();
+        debugBuffer.commitWrite();
     }
 
+
     void
     NJointKneeHipController::rtPreActivateController()
     {
+        rtReady.store(true);
     }
 
     void
     NJointKneeHipController::rtPostDeactivateController()
     {
+        rtReady.store(false);
     }
 
     void
@@ -207,14 +396,17 @@ namespace armarx
 
         StringVariantBaseMap datafields;
 
-        const auto& debugData = bufferRtToOnPublish_.getUpToDateReadBuffer();
+        const auto& debugData = debugBuffer.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);
+        datafields["globalRotation"] = new Variant(debugData.globalRotation);
+        datafields["ankleAngle"] = new Variant(debugData.ankleAngle);
+        datafields["kneeTorqueMeasured"] = new Variant(debugData.kneeTorqueSen);
+        datafields["kneeTorqueRBDL"] = new Variant(debugData.kneeTorqueSim);
 
         debugObservers->setDebugChannel("NJointKneeHipController",
                                         datafields);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.h
index 270d36a68..15330d746 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.h
@@ -21,6 +21,11 @@
  */
 #pragma once
 
+#include <cstddef>
+#include <optional>
+#include <VirtualRobot/Dynamics/Dynamics.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
 #include <ArmarXCore/interface/core/BasicTypes.h>
 #include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h"
 #include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
@@ -74,6 +79,8 @@ namespace armarx
 
         void onInitNJointController() override;
 
+        void additionalTask();
+
         void onConnectNJointController() override;
 
         std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override;
@@ -84,6 +91,10 @@ namespace armarx
         void rtPreActivateController() override;
         void rtPostDeactivateController() override;
 
+        void manageKnee();
+        void manageHip();
+
+
         
 
         // ----------------
@@ -91,10 +102,16 @@ namespace armarx
     private:
         // rt variables
         VirtualRobot::RobotPtr _rtRobot;
+        VirtualRobot::RobotPtr _nonrtRobot;
         VirtualRobot::RobotNodePtr _rtKneeNode;
         VirtualRobot::RobotNodePtr _rtHipNode;
         VirtualRobot::RobotNodePtr _rtHipBodyNode;
+        std::atomic_bool rtReady = false;
+        
+
+
         const SensorValue1DoFActuatorVelocity* velocitySensor;
+        const SensorValue1DoFActuatorTorque* torqueSensor;
         // 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);
@@ -110,11 +127,32 @@ namespace armarx
             double kneeVelocity;
             double hipAngle;
             double hipVelocity;
+            double globalRotation;
+            double ankleAngle;
+            double kneeTorqueSen;
+            double kneeTorqueSim;
 
+        };
 
+        struct JointValues
+        {
+            Eigen::VectorXf jointValues;
+        };
+        struct Dynamics
+        {
+            unsigned int kneeId;
+            std::optional<VirtualRobot::Dynamics> dynamics = std::nullopt;
+        };
+
+        struct Torques
+        {
+            double torque;
         };
 
-        TripleBuffer<DebugData> bufferRtToOnPublish_;
+        TripleBuffer<DebugData> debugBuffer;
+        TripleBuffer<JointValues> jointValues_rtToAdditionalTask;
+        TripleBuffer<Dynamics> dynamics_rtToAdditionalTask;
+        TripleBuffer<Torques> torques_AdditionalTaskToRt;
 
     };
 
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 49b6d3002..b722b6240 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -25,7 +25,6 @@
 #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>
@@ -342,22 +341,6 @@ namespace armarx::RobotUnitModule
 
                 };
 
-                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(
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointKneeHipController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointKneeHipController.ice
new file mode 100644
index 000000000..1e04758ce
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointKneeHipController.ice
@@ -0,0 +1,51 @@
+/*
+ * 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    RobotAPI::NJointControllerInterface
+ * @author     Mirko Waechter ( mirko dot waechter at kit dot edu )
+ * @date       2017
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <RobotAPI/interface/units/RobotUnit/NJointController.ice>
+#include <RobotAPI/interface/core/Trajectory.ice>
+
+module armarx
+{
+
+    // class NJointCartesianVelocityControllerConfig extends NJointControllerConfig
+    // {
+    //     string nodeSetName = "";
+    //     string tcpName = "";
+    //     NJointCartesianVelocityControllerMode::CartesianSelection mode = NJointCartesianVelocityControllerMode::eAll;
+    // };
+
+    class NJointKneeHipControllerConfig extends NJointControllerConfig
+    {
+        std::string deviceName1;
+        std::string deviceName2;
+        std::string controlMode;
+    };
+
+    // interface NJointCartesianVelocityControllerInterface extends NJointControllerInterface
+    // {
+    //     void setControllerTarget(float x, float y, float z, float roll, float pitch, float yaw, float avoidJointLimitsKp, NJointCartesianVelocityControllerMode::CartesianSelection mode);
+    //     void setTorqueKp(StringFloatDictionary torqueKp);
+    //     void setNullspaceJointVelocities(StringFloatDictionary nullspaceJointVelocities);
+    // };
+};
-- 
GitLab