From fe26d9344946be8a7bb18fb86f797c02edfde9d1 Mon Sep 17 00:00:00 2001
From: Jan Hausberg <jan.hausberg@kit.edu>
Date: Fri, 15 Jul 2022 12:05:03 +0200
Subject: [PATCH] StopMovementController as 0-VelocityController in
head_board::armar7de
---
.../head_board/armar7de/PWMVelocity.cpp | 34 +++++++++
.../head_board/armar7de/PWMVelocity.h | 16 ++++
.../joint_controller/StopMovement.cpp | 76 +++++++++++++++----
.../armar7de/joint_controller/StopMovement.h | 27 ++++++-
4 files changed, 136 insertions(+), 17 deletions(-)
diff --git a/source/devices/ethercat/head_board/armar7de/PWMVelocity.cpp b/source/devices/ethercat/head_board/armar7de/PWMVelocity.cpp
index 224c0a48..0f5d8fa9 100644
--- a/source/devices/ethercat/head_board/armar7de/PWMVelocity.cpp
+++ b/source/devices/ethercat/head_board/armar7de/PWMVelocity.cpp
@@ -76,6 +76,40 @@ namespace devices::ethercat::head_board::armar7de
return targetPWM;
}
+ PWMVelocityControllerConfiguration::PWMVelocityControllerConfiguration(
+ float maxVelocityRad,
+ float maxDecelerationRad,
+ float jerk,
+ float maxDt,
+ float directSetVLimit,
+ float p,
+ float i,
+ float d,
+ float maxIntegral,
+ float feedforwardVelocityToPWMFactor,
+ float feedforwardTorqueToPWMFactor,
+ float PWMDeadzone,
+ float velocityUpdatePercent,
+ float conditionalIntegralErrorTreshold,
+ bool feedForwardMode) :
+ p(p),
+ i(i),
+ d(d),
+ maxIntegral(maxIntegral),
+ feedforwardVelocityToPWMFactor(feedforwardVelocityToPWMFactor),
+ feedforwardTorqueToPWMFactor(feedforwardTorqueToPWMFactor),
+ PWMDeadzone(PWMDeadzone),
+ velocityUpdatePercent(velocityUpdatePercent),
+ conditionalIntegralErrorTreshold(conditionalIntegralErrorTreshold),
+ feedForwardMode(feedForwardMode)
+ {
+ this->maxVelocityRad = maxVelocityRad;
+ this->maxDecelerationRad = maxDecelerationRad;
+ this->jerk = jerk;
+ this->maxDt = maxDt;
+ this->directSetVLimit = directSetVLimit;
+ }
+
void
PWMVelocityController::reset(double currentVelocity)
diff --git a/source/devices/ethercat/head_board/armar7de/PWMVelocity.h b/source/devices/ethercat/head_board/armar7de/PWMVelocity.h
index 9ccd88c6..fa0897e7 100644
--- a/source/devices/ethercat/head_board/armar7de/PWMVelocity.h
+++ b/source/devices/ethercat/head_board/armar7de/PWMVelocity.h
@@ -46,6 +46,22 @@ namespace devices::ethercat::head_board::armar7de
public:
PWMVelocityControllerConfiguration() = default;
+ PWMVelocityControllerConfiguration(
+ float maxVelocityRad,
+ float maxDecelerationRad,
+ float jerk,
+ float maxDt,
+ float directSetVLimit,
+ float p,
+ float i,
+ float d,
+ float maxIntegral,
+ float feedforwardVelocityToPWMFactor,
+ float feedforwardTorqueToPWMFactor,
+ float PWMDeadzone,
+ float velocityUpdatePercent,
+ float conditionalIntegralErrorTreshold,
+ bool feedForwardMode);
static PWMVelocityControllerConfigurationCPtr
CreatePWMVelocityControllerConfigData(armarx::control::hardware_config::Config& hwConfig);
float p;
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.cpp b/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.cpp
index a25efcce..c7086d1d 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.cpp
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.cpp
@@ -1,6 +1,10 @@
#include "StopMovement.h"
+// armarx
+#include <ArmarXCore/core/ManagedIceObject.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+
// robot_devices
#include <devices/ethercat/head_board/armar7de/Data.h>
@@ -8,31 +12,73 @@
namespace devices::ethercat::head_board::armar7de::joint_controller
{
- StopMovementController::StopMovementController(DataPtr dataPtr) : dataPtr(dataPtr)
+ StopMovementController::StopMovementController(DataPtr dataPtr) :
+ JointController(),
+ config(new PWMVelocityControllerConfiguration(6, 2.5, 30, 0.003, 0.005, 300, 7000, 0, 0.1, 600, 0, 60, 0.5, 1000000, false)),
+ controller(config),
+ dataPtr(dataPtr)
{
- pid.reset(new armarx::PIDController(0, 5000, 0));
- pid->maxIntegral = 0.1;
+ // velController.acceleration = velocityControllerConfigDataPtr->maxAccelerationRad;
+ velController.deceleration = config->maxDecelerationRad;
+ velController.jerk = config->jerk;
+ velController.maxDt = config->maxDt;
+ velController.maxV = config->maxVelocityRad;
+ velController.directSetVLimit = config->directSetVLimit;
+ }
+
+
+ StopMovementController::~StopMovementController() noexcept(true)
+ {
+ stopRequested = true;
+ try
+ {
+ threadHandle.join();
+ }
+ catch (...)
+ {
+ }
}
void
StopMovementController::rtRun(const IceUtil::Time& sensorValuesTimestamp,
const IceUtil::Time& timeSinceLastIteration)
- {
- pid->update(timeSinceLastIteration.toSecondsDouble(), dataPtr->getVelocity(), 0.0);
- float targetPwm = pid->getControlValue();
+ {
+ auto currentPosition = dataPtr->getAbsoluteEncoderAngleInRad();
- if (std::isnan(targetPwm))
+ velController.currentPosition = currentPosition;
+
+ velController.currentV = lastTargetVelocity;
+ velController.currentAcc = lastTargetAcceleration;
+ velController.dt = timeSinceLastIteration.toSecondsDouble();
+ velController.targetV = 0;
+ auto r = velController.run();
+ double newVel = r.velocity;
+ double newAcc = r.acceleration;
+
+ // ARMARX_INFO << deactivateSpam(1) << VAROUT(newVel) << VAROUT(target.velocity);
+ if (std::isnan(newVel))
{
- targetPwm = 0.0f;
+ newVel = 0;
+ newAcc = 0;
}
- // ARMARX_INFO << "StopMovement";
-// ARMARX_INFO << VAROUT(dataPtr->getVelocity());
-// ARMARX_INFO << VAROUT(targetPwm);
- targetPwm = 0.0f;
+ auto targetPWM = static_cast<int>(controller.run(timeSinceLastIteration,
+ dataPtr->getVelocity(),
+ newVel,
+ 0));
+// ARMARX_RT_LOGF_INFO("target pwm: %d", targetPWM);
+// if (std::abs(targetPWM) > 300)
+// targetPWM = 0;
+ dataPtr->setMotorPwmValue(targetPWM);
+
+ lastTargetVelocity = newVel;
+ lastTargetAcceleration = newAcc;
- dataPtr->setMotorPwmValue(targetPwm);
+ // ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, max acc: %.3f",
+ // target.velocity, dataPtr->getVelocity(), targetPWM, pid->Kp, pid->Ki, pid->Kd, controller.acceleration).deactivateSpam(1);
+// ARMARX_RT_LOGF_INFO("target velocity: %.3f, current velocity: %.3f, target pwm: %d, kp: %.3f ki: %f, kd: %f, velController acc: %.3f",
+// target.velocity, dataPtr->getVelocity(), targetPWM, controller.pid->Kp, controller.pid->Ki, controller.pid->Kd, velController.currentAcc).deactivateSpam(1);
}
@@ -46,7 +92,9 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
void
StopMovementController::rtPreActivateController()
{
- pid->reset();
+ lastTargetVelocity = dataPtr->getVelocity();
+ lastTargetAcceleration = dataPtr->getAcceleration();
+ controller.reset(static_cast<double>(dataPtr->getVelocity()));
}
} // namespace devices::ethercat::head_board::armar7de::joint_controller
diff --git a/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.h b/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.h
index 71258157..e2c688fd 100644
--- a/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.h
+++ b/source/devices/ethercat/head_board/armar7de/joint_controller/StopMovement.h
@@ -2,16 +2,29 @@
// armarx
+#include <ArmarXCore/core/services/tasks/ThreadPool.h>
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
#include <RobotAPI/components/units/RobotUnit/JointControllers/JointController.h>
-#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+
+// robot_devices
+#include <devices/ethercat/head_board/armar7de/PWMVelocity.h>
+
+
+namespace armarx
+{
+ using PIDControllerPtr = std::shared_ptr<class PIDController>;
+}
namespace devices::ethercat::head_board::armar7de
{
using DataPtr = std::shared_ptr<class Data>;
+ using PWMVelocityControllerConfigurationCPtr =
+ std::shared_ptr<const class PWMVelocityControllerConfiguration>;
}
-
namespace devices::ethercat::head_board::armar7de::joint_controller
{
@@ -23,6 +36,7 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
public:
StopMovementController(DataPtr dataPtr);
+ ~StopMovementController() noexcept(true);
private:
void rtRun(const IceUtil::Time& sensorValuesTimestamp,
@@ -37,9 +51,16 @@ namespace devices::ethercat::head_board::armar7de::joint_controller
void rtPreActivateController() override;
private:
+ PWMVelocityControllerConfigurationCPtr config;
+ devices::ethercat::head_board::armar7de::PWMVelocityController controller;
+ armarx::VelocityControllerWithRampedAccelerationAndPositionBounds velController;
+
+ std::atomic<float> lastTargetVelocity, lastTargetAcceleration;
+
armarx::DummyControlTargetStopMovement target;
DataPtr dataPtr;
- armarx::PIDControllerPtr pid;
+ bool stopRequested = false;
+ mutable armarx::ThreadPool::Handle threadHandle;
};
} // namespace devices::ethercat::head_board::armar7de::joint_controller
--
GitLab