Skip to content
Snippets Groups Projects
Commit b01d772c authored by Mirko Wächter's avatar Mirko Wächter
Browse files

intermediate version of min jerk control

parent c5d2ce18
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.cpp
+ 142
− 129
View file @ b01d772c
......@@ -27,20 +27,24 @@ JointPWMPositionController::JointPWMPositionController(const std::string deviceN
actorIndex = board->getActorIndex(deviceName);
dataPtr = jointData;
posController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
posController.deceleration = velocityControllerConfigDataPtr->maxDecelerationRad;
posController.desiredDeceleration = velocityControllerConfigDataPtr->maxDecelerationRad;
posController.desiredJerk = 100;
posController.maxDt = velocityControllerConfigDataPtr->maxDt;
posController.maxV = velocityControllerConfigDataPtr->maxVelocityRad;
posController.p = 4;
posController.phase2SwitchDistance = 0.1;
ARMARX_CHECK_GREATER_EQUAL(jointData->getSoftLimitHi(), jointData->getSoftLimitLo());
// controller.positionLimitHiHard = dataPtr->getHardLimitHi();
posController.positionLimitHi = jointData->getSoftLimitHi();
// posController.positionLimitHi = jointData->getSoftLimitHi();
// controller.positionLimitLoHard = dataPtr->getHardLimitLo();
posController.positionLimitLo = jointData->getSoftLimitLo();
posController.pControlPosErrorLimit = 0.02f;
posController.pid->Kp = posController.calculateProportionalGain();
ARMARX_IMPORTANT << "position Kp " << posController.pid->Kp;
// posController.positionLimitLo = jointData->getSoftLimitLo();
// posController.pControlPosErrorLimit = 0.02f;
// posController.pid->Kp = posController.calculateProportionalGain();
// ARMARX_IMPORTANT << "position Kp " << posController.pid->Kp;
this->isLimitless = jointData->isLimitless();
pidPosController.reset(new PIDController(velocityControllerConfigDataPtr->p, velocityControllerConfigDataPtr->i, velocityControllerConfigDataPtr->d));
pidPosController->maxIntegral = velocityControllerConfigDataPtr->maxIntegral;
}
......@@ -62,9 +66,9 @@ void JointPWMPositionController::rtRun(const IceUtil::Time& sensorValuesTimestam
if (target.isValid())
{
auto currentPosition = dataPtr->getPosition();
float targetPosition = boost::algorithm::clamp(target.position,
std::min(currentPosition, posController.positionLimitLo), // lo or current position
std::max(currentPosition, posController.positionLimitHi)); // hi or current position
// float targetPosition = boost::algorithm::clamp(target.position,
// std::min(currentPosition, posController.positionLimitLo), // lo or current position
// std::max(currentPosition, posController.positionLimitHi)); // hi or current position
if (isLimitless)
{
......@@ -78,9 +82,11 @@ void JointPWMPositionController::rtRun(const IceUtil::Time& sensorValuesTimestam
}
posController.currentV = lastTargetVelocity;
posController.dt = timeSinceLastIteration.toSecondsDouble();
posController.targetPosition = targetPosition;
posController.setTargetPosition(target.position);
// ARMARX_CHECK_EXPRESSION(posController.validParameters());
double newVel = posController.run();
auto r = posController.run();
posController.currentAcc = r.acceleration;
double newVel = r.velocity;
// double newVel = posController.p * (posController.targetPosition - posController.currentPosition);
// newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
// ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel);
......@@ -88,13 +94,16 @@ void JointPWMPositionController::rtRun(const IceUtil::Time& sensorValuesTimestam
{
newVel = 0;
}
auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel));
pidPosController->update(timeSinceLastIteration.toSecondsDouble(), currentPosition, r.position);
auto targetPWM = pidPosController->getControlValue();
// auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration, dataPtr->getVelocity(), newVel));
newVel = math::MathUtils::LimitTo(newVel, posController.maxV);
if (std::isnan(targetPWM))
{
throw LocalException() << "Target PWM of " << getParent().getDeviceName() << " is NaN!";
}
ARMARX_INFO << deactivateSpam(0.1) << VAROUT(newVel) << VAROUT(r.acceleration) << VAROUT(r.position) << VAROUT(targetPWM);
dataPtr->setTargetPWM(targetPWM);
this->targetPWM = targetPWM;
lastTargetVelocity = newVel;
......@@ -119,6 +128,7 @@ ControlTargetBase* JointPWMPositionController::getControlTarget()
void JointPWMPositionController::rtPreActivateController()
{
lastTargetVelocity = dataPtr->getVelocity();
posController.currentAcc = 0;// TODO: read acceleration
controller.reset(dataPtr->getVelocity());
}
......@@ -133,123 +143,126 @@ StringVariantBaseMap JointPWMPositionController::publish(const DebugDrawerInterf
if (!remoteGui)
{
// threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
// {
// std::string guiTabName;
// while (!stopRequested)
// {
// ManagedIceObjectPtr object;
// ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
// try
// {
// object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
// ARMARX_CHECK_EXPRESSION(object);
// remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
// if (!remoteGui)
// {
// continue;
// }
// ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
// guiTabName = getParent().getDeviceName() + getControlMode();
// break;
// }
// catch (...)
// {
// sleep(1);
// }
// }
// if (remoteGui)
// {
// ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
// using namespace RemoteGui;
// auto vLayout = makeVBoxLayout();
// {
// WidgetPtr KpLabel = makeTextLabel("Kp: ");
// WidgetPtr KiSlider = makeFloatSlider("KpSlider")
// .min(0.0f).max(100.0f)
// .value(posController.pid->Kp);
// WidgetPtr line = makeHBoxLayout()
// .children({KpLabel, KiSlider});
// vLayout.addChild(line);
// }
// {
// WidgetPtr KiLabel = makeTextLabel("Ki: ");
// WidgetPtr KiSlider = makeFloatSlider("KiSlider")
// .min(0.0f).max(100.0f)
// .value(0);
// WidgetPtr line = makeHBoxLayout()
// .children({KiLabel, KiSlider});
// vLayout.addChild(line);
// }
// {
// WidgetPtr KdLabel = makeTextLabel("Kd: ");
// WidgetPtr KdSlider = makeFloatSlider("KdSlider")
// .min(0.0f).max(10.0f)
// .steps(1000)
// .value(0);
// WidgetPtr line = makeHBoxLayout()
// .children({KdLabel, KdSlider});
// vLayout.addChild(line);
// vLayout.addChild(new VSpacer);
// }
// // WidgetPtr spin = makeFloatSpinBox("KpSpin")
// // .min(0.0f).max(2.0f)
// // .steps(20).decimals(2)
// // .value(0.4f);
// WidgetPtr groupBox = makeGroupBox("GroupBox")
// .label("Group")
// .child(vLayout);
// remoteGui->createTab(guiTabName, groupBox);
// while (!stopRequested)
// {
// RemoteGui::TabProxy tab(remoteGui, guiTabName);
// tab.receiveUpdates();
// this->posController.pid->Kp = tab.getValue<float>("KpSlider").get();
// this->posController.pid->Ki = tab.getValue<float>("KiSlider").get();
// this->posController.pid->Kd = tab.getValue<float>("KdSlider").get();
// usleep(100000);
// }
// }
// });
threadHandle = Application::getInstance()->getThreadPool()->runTask([this]
{
std::string guiTabName;
while (!stopRequested)
{
ManagedIceObjectPtr object;
ARMARX_IMPORTANT << deactivateSpam(1) << "Trying to get parent";
try
{
object = ManagedIceObjectPtr::dynamicCast(getParent().getOwner());
ARMARX_CHECK_EXPRESSION(object);
remoteGui = object->getProxy<RemoteGuiInterfacePrx>("RemoteGuiProvider", false, "", false);
if (!remoteGui)
{
continue;
}
ARMARX_IMPORTANT << deactivateSpam(1) << "Got Proxy";
guiTabName = getParent().getDeviceName() + getControlMode();
break;
}
catch (...)
{
sleep(1);
}
}
if (remoteGui)
{
ARMARX_IMPORTANT << "Creating GUI " << guiTabName;
using namespace RemoteGui;
auto vLayout = makeVBoxLayout();
{
WidgetPtr KpLabel = makeTextLabel("Kp: ");
WidgetPtr KpSlider = makeFloatSlider("KpSlider")
.min(0.0f).max(pidPosController->Kp * 5)
.value(pidPosController->Kp);
WidgetPtr KpLabelValue = makeTextLabel(std::to_string(pidPosController->Kp * 5));
WidgetPtr line = makeHBoxLayout()
.children({KpLabel, KpSlider, KpLabelValue});
vLayout.addChild(line);
}
{
WidgetPtr KiLabel = makeTextLabel("Ki: ");
WidgetPtr KiSlider = makeFloatSlider("KiSlider")
.min(0.0f).max(pidPosController->Ki * 5)
.value(pidPosController->Ki);
WidgetPtr KiLabelValue = makeTextLabel(std::to_string(pidPosController->Ki * 5));
WidgetPtr line = makeHBoxLayout()
.children({KiLabel, KiSlider, KiLabelValue});
vLayout.addChild(line);
}
{
WidgetPtr KdLabel = makeTextLabel("Kd: ");
WidgetPtr KdSlider = makeFloatSlider("KdSlider")
.min(-10.0f).max(10.0f)
.steps(1000)
.value(0);
// WidgetPtr KdLabelValue = makeTextLabel(std::to_string(controller.pid->Kd*5));
WidgetPtr line = makeHBoxLayout()
.children({KdLabel, KdSlider/*, KdLabelValue*/});
vLayout.addChild(line);
vLayout.addChild(new VSpacer);
}
// WidgetPtr spin = makeFloatSpinBox("KpSpin")
// .min(0.0f).max(2.0f)
// .steps(20).decimals(2)
// .value(0.4f);
WidgetPtr groupBox = makeGroupBox("GroupBox")
.label("Group")
.child(vLayout);
remoteGui->createTab(guiTabName, groupBox);
while (!stopRequested)
{
RemoteGui::TabProxy tab(remoteGui, guiTabName);
tab.receiveUpdates();
this->controller.pid->Kp = tab.getValue<float>("KpSlider").get();
this->controller.pid->Ki = tab.getValue<float>("KiSlider").get();
this->controller.pid->Kd = tab.getValue<float>("KdSlider").get();
usleep(100000);
}
}
});
}
return {{"lastTargetVelocity", new Variant(lastTargetVelocity.load())},
{"targetPosition", new Variant(posController.targetPosition)},
{"posError", new Variant(posController.targetPosition - posController.currentPosition)},
{"pidError", new Variant(controller.pid->previousError)},
{"targetPosition", new Variant(posController.getTargetPosition())},
{"posError", new Variant(posController.getTargetPosition() - posController.currentPosition)},
{"pidError", new Variant(pidPosController->previousError)},
{"filteredVelocity", new Variant(controller.lastActualVelocity.load())},
{"pidIntegralCV", new Variant(controller.pid->integral * controller.pid->Ki)},
{"pidIntegral", new Variant(controller.pid->integral)},
{"pidPropCV", new Variant(controller.pid->previousError * controller.pid->Kp)},
{"pidDiffCV", new Variant(controller.pid->derivative * controller.pid->Kd)},
{"pospidIntegralCV", new Variant(posController.pid->integral * posController.pid->Ki)},
{"pospidIntegral", new Variant(posController.pid->integral)},
{"pospidPropCV", new Variant(posController.pid->previousError * posController.pid->Kp)},
{"pospidDiffCV", new Variant(posController.pid->derivative * posController.pid->Kd)},
{"pidUsed", new Variant(posController.getCurrentlyPIDActive())},
{"pidIntegralCV", new Variant(pidPosController->integral * pidPosController->Ki)},
{"pidIntegral", new Variant(pidPosController->integral)},
{"pidPropCV", new Variant(pidPosController->previousError * pidPosController->Kp)},
{"pidDiffCV", new Variant(pidPosController->derivative * controller.pid->Kd)},
// {"pospidIntegralCV", new Variant(posController.pid->integral * posController.pid->Ki)},
// {"pospidIntegral", new Variant(posController.pid->integral)},
// {"pospidPropCV", new Variant(posController.pid->previousError * posController.pid->Kp)},
// {"pospidDiffCV", new Variant(posController.pid->derivative * posController.pid->Kd)},
// {"pidUsed", new Variant(posController.getCurrentlyPIDActive())},
{"desiredPWM", new Variant(targetPWM.load())}
......
source/RobotAPI/libraries/KITGripperEtherCAT/KITGripperBasisBoard/JointController/JointPWMPositionController.h
+ 3
− 2
View file @ b01d772c
......@@ -37,8 +37,9 @@ namespace armarx
protected:
PWMVelocityControllerConfigurationCPtr config;
PWMVelocityController controller;
PositionThroughVelocityControllerWithAccelerationAndPositionBounds posController;
// PositionThroughVelocityControllerWithAccelerationAndPositionBounds posController;
MinJerkPositionController posController;
PIDControllerPtr pidPosController;
ControlTarget1DoFActuatorPosition target;
std::atomic<double> lastTargetVelocity, targetPWM;
......
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