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