diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp
index be4cc405af977f735a67d233b700940ee62dce4d..8a88d0003359db74713dc914fcdd161a0c92cd54 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp
@@ -1,7 +1,9 @@
#include "NJointKneeHipController.h"
#include <algorithm>
#include <cmath>
+#include <cstdlib>
#include <memory>
+#include <Ice/Config.h>
#include <SimoxUtility/math/distance/angle_between.h>
@@ -45,13 +47,9 @@ namespace armarx
_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";
-
_rtKneeNode = _rtRobot->getRobotNode(kneeJointName);
_rtHipNode = _rtRobot->getRobotNode(hipJointName);
ARMARX_CHECK_NOT_NULL(_rtKneeNode);
@@ -63,8 +61,13 @@ namespace armarx
// _rtHipCtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
// hipJointName, armarx::ControlModes::Position1DoF);
+ auto kneeBase = useControlTarget(kneeJointName, armarx::ControlModes::Torque1DoF);
auto hipBase = useControlTarget(hipJointName, armarx::ControlModes::Velocity1DoF);
+ // auto hipBase = useControlTarget(hipJointName, armarx::ControlModes::Position1DoF);
+
+ _rtKneeCtrlTarget = kneeBase->asA<ControlTarget1DoFActuatorTorque>();
_rtHipCtrlTarget = hipBase->asA<ControlTarget1DoFActuatorVelocity>();
+ // _rtHipCtrlTarget = hipBase->asA<ControlTarget1DoFActuatorPosition>();
ARMARX_CHECK_NOT_NULL(_rtHipCtrlTarget);
}
@@ -163,7 +166,9 @@ namespace armarx
/* Build Sensors */
const SensorValueBase* sv = useSensorValue(_rtKneeNode->getName());
+ positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
velocitySensor = sv->asA<SensorValue1DoFActuatorVelocity>();
+ accelerationSensor = sv->asA<SensorValue1DoFActuatorAcceleration>();
torqueSensor = sv->asA<SensorValue1DoFActuatorTorque>();
// positionSensor = sv->asA<SensorValue1DoFActuatorPosition>();
@@ -197,7 +202,7 @@ namespace armarx
{
if (isControllerActive())
{
- ARMARX_IMPORTANT << "additional task started";
+ // ARMARX_IMPORTANT << "additional task started";
additionalTask();
} else {
// ARMARX_IMPORTANT << "controller not active, additional Task skipped";
@@ -234,6 +239,7 @@ namespace armarx
Eigen::VectorXd q = jointValuesBuffer.jointValues.cast<double>();
+ // q[1] -= M_PI/20;
// ARMARX_IMPORTANT << "before getGravityMatrix";
// ARMARX_IMPORTANT << q.transpose();
@@ -243,19 +249,23 @@ namespace armarx
// ARMARX_IMPORTANT << "before writing to buffer";
double kneeTorque = torque(dynamBuffer.kneeId);
- ARMARX_IMPORTANT << torque.transpose();
+ // ARMARX_IMPORTANT << torque.transpose();
// store results (onPublish)
auto& dynamicsBuffer = torques_AdditionalTaskToRt.getWriteBuffer();
- dynamicsBuffer.torque = -0.5*kneeTorque;
+ dynamicsBuffer.torque = kneeTorque;
+ // dynamicsBuffer.torque = -(0.5*kneeTorque - 10);
+ // dynamicsBuffer.torque = -kneeTorque;
torques_AdditionalTaskToRt.commitWrite();
auto& debugData = debugBuffer.getWriteBuffer();
- debugData.kneeTorqueSim = -0.5*kneeTorque;
+ debugData.kneeTorqueSim = kneeTorque;
+ // debugData.kneeTorqueSim = -(0.5*kneeTorque - 10);
+ // debugData.kneeTorqueSim = -kneeTorque;
debugBuffer.commitWrite();
- ARMARX_IMPORTANT << "Finished additional Task";
+ // ARMARX_IMPORTANT << "Finished additional Task";
}
@@ -319,6 +329,8 @@ namespace armarx
{
// ARMARX_IMPORTANT << "KneeHip rtRun";
+
+
manageKnee();
// ARMARX_IMPORTANT << "after Knee";
manageHip();
@@ -329,13 +341,89 @@ namespace armarx
void
NJointKneeHipController::manageKnee()
{
- auto& robotBuffer = jointValues_rtToAdditionalTask.getWriteBuffer();
+ auto& debugDataKnee = debugBufferKnee.getWriteBuffer();
+ /* Send current config to RBDL task*/
+ auto& robotBuffer = jointValues_rtToAdditionalTask.getWriteBuffer();
std::string rnsName = "FourbarStructure";
robotBuffer.jointValues = _rtRobot->getRobotNodeSet(rnsName)->getJointValuesEigen();
-
jointValues_rtToAdditionalTask.commitWrite();
+ /* Mass-Spring-Damper-System constants*/
+ const double damperConstant = -100;
+ const double springConstant = 0;
+ const double mass = 8;
+
+ /* Position and Velocity targets*/
+
+ const double thetaTarg = 60 * (M_PI/180);
+ const double dthetaTarg = 0;
+
+ /* Impedance Controller */
+
+ const auto& torquesRBDL =
+ torques_AdditionalTaskToRt.getUpToDateReadBuffer();
+
+ double kneeAngle = _rtKneeNode->getJointValue();
+ double thetaSens = kneeAngle;
+ double thetaDiff = thetaTarg - thetaSens;
+
+ debugDataKnee.thetaDiff = thetaDiff;
+
+
+ double dThetaSens = velocitySensor->velocity;
+ double dthetaDiff = dthetaTarg - dThetaSens;
+ // if (abs(dthetaDiff) < 0.005) {
+ // dthetaDiff = 0;
+ // }
+ debugDataKnee.dthetaDiff = dthetaDiff;
+
+
+ double ddthetaSens = accelerationSensor->acceleration;
+
+ double springImp = springConstant * thetaDiff;
+ double damperImp = damperConstant * dthetaDiff;
+ double massImp = mass * ddthetaSens;
+
+ double tauImp = springImp + damperImp + massImp;
+
+ double tauSens = torqueSensor->torque;
+ // auto tauCompPre = torquesRBDL.torque;
+ // auto correction = (_rtKneeNode->getJointValue() - 0.873) * 30;
+
+
+ // auto tauComp = -torquesRBDL.torque;
+
+ /* Regression Values*/
+ double a = 0.00035;
+ double t = 20;
+ double c = 0;
+
+ double thetaSensDeg = thetaSens * (180/M_PI);
+ auto tauComp = a * pow((thetaSensDeg - t), 3) + c;
+
+ // auto tauComp = tauCompPre - (0.873 - _rtKneeNode->getJointValue()) * 45;
+ double tauDiff = tauImp + tauComp;
+
+ debugDataKnee.springImp = springImp;
+ debugDataKnee.damperImp = damperImp;
+ debugDataKnee.massImp = massImp;
+ debugDataKnee.tauImp = tauImp;
+ debugDataKnee.tauSens = tauSens;
+ debugDataKnee.tauComp = tauComp;
+ debugDataKnee.tauDiff = tauDiff;
+ debugBufferKnee.commitWrite();
+
+
+ std::clamp(tauDiff, -40.0, 170.0);
+ _rtKneeCtrlTarget->torque = tauDiff;
+ // _rtKneeCtrlTarget->torque = 0;
+
+
+
+
+
+
}
@@ -352,12 +440,13 @@ namespace armarx
double kneeAngle = _rtKneeNode->getJointValue();
debugData.kneeAngle = kneeAngle;
+ debugData.kneeAngleDeg = kneeAngle * (180/M_PI);
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;
+ double globalRotation = kneeAngle - ankleAngle + M_PI / 4 - hipAngle;
debugData.globalRotation = globalRotation;
@@ -370,6 +459,7 @@ namespace armarx
std::clamp(hipVelocity, -0.5, 0.5);
_rtHipCtrlTarget->velocity = hipVelocity;
+ // _rtHipCtrlTarget->position = kneeAngle - ankleAngle + M_PI / 4;
debugBuffer.commitWrite();
@@ -397,9 +487,11 @@ namespace armarx
StringVariantBaseMap datafields;
const auto& debugData = debugBuffer.getUpToDateReadBuffer();
+ const auto& debugDataKnee = debugBufferKnee.getUpToDateReadBuffer();
datafields["localTimestamp"] = new Variant(debugData.localTimestamp);
datafields["kneeAngle"] = new Variant(debugData.kneeAngle);
+ datafields["kneeAngleDeg"] = new Variant(debugData.kneeAngleDeg);
datafields["kneeVelocity"] = new Variant(debugData.kneeVelocity);
datafields["hipAngle"] = new Variant(debugData.hipAngle);
datafields["hipVelocity"] = new Variant(debugData.hipVelocity);
@@ -407,6 +499,15 @@ namespace armarx
datafields["ankleAngle"] = new Variant(debugData.ankleAngle);
datafields["kneeTorqueMeasured"] = new Variant(debugData.kneeTorqueSen);
datafields["kneeTorqueRBDL"] = new Variant(debugData.kneeTorqueSim);
+ datafields["tauDiff"] = new Variant(debugDataKnee.tauDiff);
+ datafields["tauSens"] = new Variant(debugDataKnee.tauSens);
+ datafields["springImp"] = new Variant(debugDataKnee.springImp);
+ datafields["damperImp"] = new Variant(debugDataKnee.damperImp);
+ datafields["massImp"] = new Variant(debugDataKnee.massImp);
+ datafields["tauImp"] = new Variant(debugDataKnee.tauImp);
+ datafields["tauComp"] = new Variant(debugDataKnee.tauComp);
+ datafields["thetaDiff"] = new Variant(debugDataKnee.thetaDiff);
+ datafields["dthetaDiff"] = new Variant(debugDataKnee.dthetaDiff);
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 15330d7466b3cbba3b2936d0c727f995e1fb2e83..627925b476b1adb2b8e3217b7e2534841b834faa 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.h
@@ -110,20 +110,23 @@ namespace armarx
+ const SensorValue1DoFActuatorPosition* positionSensor;
const SensorValue1DoFActuatorVelocity* velocitySensor;
const SensorValue1DoFActuatorTorque* torqueSensor;
- // const SensorValue1DoFActuatorPosition* positionSensor;
+ const SensorValue1DoFActuatorAcceleration* accelerationSensor;
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::ControlTarget1DoFActuatorTorque* _rtKneeCtrlTarget;
armarx::ControlTarget1DoFActuatorVelocity* _rtHipCtrlTarget;
+ // armarx::ControlTarget1DoFActuatorPosition* _rtHipCtrlTarget;
struct DebugData
{
std::int64_t localTimestamp;
double kneeAngle;
+ double kneeAngleDeg;
double kneeVelocity;
double hipAngle;
double hipVelocity;
@@ -134,6 +137,19 @@ namespace armarx
};
+ struct DebugKnee
+ {
+ double tauDiff;
+ double tauSens;
+ double tauImp;
+ double tauComp;
+ double thetaDiff;
+ double dthetaDiff;
+ double springImp;
+ double damperImp;
+ double massImp;
+ };
+
struct JointValues
{
Eigen::VectorXf jointValues;
@@ -150,6 +166,7 @@ namespace armarx
};
TripleBuffer<DebugData> debugBuffer;
+ TripleBuffer<DebugKnee> debugBufferKnee;
TripleBuffer<JointValues> jointValues_rtToAdditionalTask;
TripleBuffer<Dynamics> dynamics_rtToAdditionalTask;
TripleBuffer<Torques> torques_AdditionalTaskToRt;