Skip to content
Snippets Groups Projects
Commit 27f4dbbe authored by Frederik Koch's avatar Frederik Koch
Browse files

KneeHip balance controller now works

parent cace9caa
No related branches found
No related tags found
1 merge request!408Current control
Pipeline #19383 failed
Stage: build-and-test
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.cpp
+ 113
− 33
View file @ 27f4dbbe
#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
source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointKneeHipController.h
+ 33
− 13
View file @ 27f4dbbe
......@@ -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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment