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