From cb5b7262bc33005149f8ff952c72e47312556184 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Sat, 7 May 2016 01:44:54 +0200
Subject: [PATCH] Kinematic Unit Simulation supports now position control with
PID controller instead of instantly reaching the target
---
.../units/KinematicUnitSimulation.cpp | 51 ++++++++++++++++---
.../units/KinematicUnitSimulation.h | 11 +++-
.../RobotAPI/libraries/core/PIDController.cpp | 13 +++++
.../RobotAPI/libraries/core/PIDController.h | 2 +
4 files changed, 68 insertions(+), 9 deletions(-)
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
index 088be6ebe..5b81f9c98 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
@@ -34,13 +34,14 @@
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/TimeUtil.h>
+
#include <algorithm>
using namespace armarx;
void KinematicUnitSimulation::onInitKinematicUnit()
{
-
+ usePDControllerForPosMode = getProperty<bool>("UsePDControllerForJointControl").getValue();
for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
{
std::string jointName = (*it)->getName();
@@ -68,6 +69,10 @@ void KinematicUnitSimulation::onInitKinematicUnit()
std::string jointName = (*it)->getName();
jointStates[jointName] = KinematicUnitSimulationJointState();
jointStates[jointName].init();
+ positionControlPIDController[jointName] = PIDControllerPtr(new PIDController(
+ getProperty<float>("Kp").getValue(),
+ getProperty<float>("Ki").getValue(),
+ getProperty<float>("Kd").getValue()));
}
}
@@ -102,6 +107,10 @@ void KinematicUnitSimulation::simulationFunction()
bool aPosValueChanged = false;
bool aVelValueChanged = false;
+ auto signedMin = [](float newValue, float minAbsValue)
+ {
+ return std::copysign(std::min<float>(fabs(newValue), fabs(minAbsValue)), newValue);
+ };
{
boost::mutex::scoped_lock lock(jointStatesMutex);
@@ -117,12 +126,29 @@ void KinematicUnitSimulation::simulationFunction()
double change = (it->second.velocity * timeDeltaInSeconds);
double randomNoiseValue = distribution(generator);
- change += randomNoiseValue;
+ change *= 1 + randomNoiseValue;
newAngle = it->second.angle + change;
}
else if (it->second.controlMode == ePositionControl)
{
- newAngle = it->second.angle;
+ if (!usePDControllerForPosMode)
+ {
+ newAngle = it->second.angle;
+ }
+ else
+ {
+ PIDControllerPtr pid = positionControlPIDController[it->first];
+ if (pid)
+ {
+ float velValue = (it->second.velocity != 0.0f ? signedMin(pid->getControlValue(), it->second.velocity) : pid->getControlValue()); //restrain to max velocity
+ velValue *= 1 + distribution(generator);
+ pid->update(it->second.angle);
+ newAngle = it->second.angle +
+ velValue *
+ timeDeltaInSeconds;
+
+ }
+ }
}
// constrain the angle
@@ -233,8 +259,21 @@ void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngl
targetJointAngle = std::min<float>(targetJointAngle, jointInfos[targetJointName].limitHi);
}
- iterJoints->second.angle = targetJointAngle;
- actualJointAngles[targetJointName] = iterJoints->second.angle;
+ if (!usePDControllerForPosMode)
+ {
+ iterJoints->second.angle = targetJointAngle;
+ actualJointAngles[targetJointName] = iterJoints->second.angle;
+ }
+ else
+ {
+ PIDControllerPtr pid = positionControlPIDController[it->first];
+ if (pid)
+ {
+ pid->reset();
+ pid->setTarget(targetJointAngle);
+
+ }
+ }
}
else
{
@@ -362,7 +401,6 @@ bool mapEntryEqualsString(std::pair<std::string, KinematicUnitSimulationJointSta
void KinematicUnitSimulation::requestJoints(const StringSequence& joints, const Ice::Current& c)
{
- ARMARX_INFO << flush;
// if one of the joints belongs to this unit, lock access to this unit for other components except for the requesting one
KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
@@ -374,7 +412,6 @@ void KinematicUnitSimulation::requestJoints(const StringSequence& joints, const
void KinematicUnitSimulation::releaseJoints(const StringSequence& joints, const Ice::Current& c)
{
- ARMARX_INFO << flush;
// if one of the joints belongs to this unit, unlock access
KinematicUnitSimulationJointStates::const_iterator it = std::find_first_of(jointStates.begin(), jointStates.end(), joints.begin(), joints.end(), mapEntryEqualsString);
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.h b/source/RobotAPI/components/units/KinematicUnitSimulation.h
index 57e3c9f4c..4b730a0f4 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.h
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.h
@@ -29,6 +29,7 @@
#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
#include <ArmarXCore/core/system/ImportExportComponent.h>
+#include <RobotAPI/libraries/core/PIDController.h>
#include <random>
#include <IceUtil/Time.h>
@@ -111,7 +112,12 @@ namespace armarx
KinematicUnitPropertyDefinitions(prefix)
{
defineOptionalProperty<int>("IntervalMs", 10, "The time in milliseconds between two calls to the simulation method.");
- defineOptionalProperty<float>("Noise", 0.0f, "Noise is added in velocity mode with (rand()/RAND_MAX-0.5)*2*Noise. Value in Degree");
+ defineOptionalProperty<float>("Noise", 0.0f, "Gaussian noise is added to the velocity. Value in Degree");
+ defineOptionalProperty<bool>("UsePDControllerForJointControl", true, "If true a PD controller is also used in Position Mode instead of setting the joint angles instantly");
+ defineOptionalProperty<float>("Kp", 3, "proportional gain of the PID position controller.");
+ defineOptionalProperty<float>("Ki", 0.001, "integral gain of the PID position controller.");
+ defineOptionalProperty<float>("Kd", 0.0, "derivative gain of the PID position controller.");
+
}
};
@@ -173,7 +179,8 @@ namespace armarx
int intervalMs;
std::normal_distribution<double> distribution;
std::default_random_engine generator;
-
+ bool usePDControllerForPosMode;
+ std::map<std::string, PIDControllerPtr> positionControlPIDController;
PeriodicTask<KinematicUnitSimulation>::pointer_type simulationTask;
};
}
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 7a82a5d6a..441fb1177 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -66,6 +66,19 @@ void PIDController::update(double measuredValue, double targetValue)
lastUpdateTime = now;
}
+
+void PIDController::update(double measuredValue)
+{
+ ScopedRecursiveLock lock(mutex);
+ update(measuredValue, target);
+}
+
+void PIDController::setTarget(double newTarget)
+{
+ ScopedRecursiveLock lock(mutex);
+ target = newTarget;
+}
+
void PIDController::update(double deltaSec, double measuredValue, double targetValue)
{
ScopedRecursiveLock lock(mutex);
diff --git a/source/RobotAPI/libraries/core/PIDController.h b/source/RobotAPI/libraries/core/PIDController.h
index c1c1529a4..8e9578b02 100644
--- a/source/RobotAPI/libraries/core/PIDController.h
+++ b/source/RobotAPI/libraries/core/PIDController.h
@@ -38,6 +38,8 @@ namespace armarx
PIDController(float Kp, float Ki, float Kd);
void update(double deltaSec, double measuredValue, double targetValue);
void update(double measuredValue, double targetValue);
+ void update(double measuredValue);
+ void setTarget(double newTarget);
double getControlValue() const;
void reset();
--
GitLab