From 5d4a0fbee6d8e242d429781f8c6bc75920b11194 Mon Sep 17 00:00:00 2001
From: Markus Grotz <Markus Grotz markus.grotz@kit.edu>
Date: Mon, 4 Jan 2021 13:44:44 +0100
Subject: [PATCH] update NJointHolonomicPlatformGlobalPositionController
---
...onomicPlatformGlobalPositionController.cpp | 67 ++++++++-----------
...olonomicPlatformGlobalPositionController.h | 20 ++++--
2 files changed, 41 insertions(+), 46 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index 3cc0e13c3..bc246422b 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -33,25 +33,19 @@ namespace armarx
const RobotUnitPtr&,
const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
const VirtualRobot::RobotPtr&) :
- pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration)
+ pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration),
+ opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration)
{
const SensorValueBase* sv = useSensorValue(cfg->platformName);
this->sv = sv->asA<SensorValueHolonomicPlatform>();
target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
- ARMARX_CHECK_EXPRESSION(target) << "The actuator '" << cfg->platformName << "' has no control mode " << ControlModes::HolonomicPlatformVelocity;
- pid.threadSafe = false;
- oriCtrl.maxV = cfg->maxRotationVelocity;
- oriCtrl.acceleration = cfg->maxRotationAcceleration;
- oriCtrl.deceleration = cfg->maxRotationAcceleration;
- oriCtrl.maxDt = 0.1;
- oriCtrl.pid->Kp = cfg->p;
- oriCtrl.positionPeriodLo = -M_PI;
- oriCtrl.positionPeriodHi = M_PI;
+ pid.threadSafe = false;
pid.preallocate(2);
+ opid.threadSafe = false;
}
void NJointHolonomicPlatformGlobalPositionController::rtRun(const IceUtil::Time& currentTime, const IceUtil::Time& timeSinceLastIteration)
@@ -63,18 +57,23 @@ namespace armarx
if (rtGetControlStruct().newTargetSet)
{
pid.reset();
+ opid.reset();
*const_cast<bool*>(&rtGetControlStruct().newTargetSet) = false;
+ isAborted = false;
}
-
- if ((rtGetControlStruct().lastUpdate + IceUtil::Time::seconds(2)) < currentTime)
+ if (isAborted)
+ {
+ return;
+ }
+ else if ((rtGetControlStruct().lastUpdate + IceUtil::Time::seconds(2)) < currentTime)
{
-
// ARMARX_RT_LOGF_WARNING << deactivateSpam(0.5) << "Waiting for global pos";
target->velocityX = 0;
target->velocityY = 0;
target->velocityRotation = 0;
+ isAborted = true;
return;
}
@@ -82,36 +81,24 @@ namespace armarx
float relativeOrientation = currentOrientation - rtGetControlStruct().startOrientation;
Eigen::Vector2f relativeCurrentPosition = currentPosition - rtGetControlStruct().startPosition;
- Eigen::Vector2f updatedPosition = rtGetControlStruct().globalPosition + relativeCurrentPosition;
- float updatedOrientation = rtGetControlStruct().globalOrientation + relativeOrientation;
- updatedOrientation = std::atan2(std::sin(updatedOrientation), std::cos(updatedOrientation));
+ Eigen::Vector2f updatedPosition = rtGetControlStruct().globalPosition;// + relativeCurrentPosition;
+ float updatedOrientation = rtGetControlStruct().globalOrientation;//+ relativeOrientation;
- pid.update(timeSinceLastIteration.toSecondsDouble(), updatedPosition, rtGetControlStruct().target);
+ float relativeGlobalOrientation = rtGetControlStruct().globalOrientation - getWriterControlStruct().startOrientation;
+ relativeGlobalOrientation = std::atan2(std::sin(relativeGlobalOrientation), std::cos(relativeGlobalOrientation));
- oriCtrl.dt = timeSinceLastIteration.toSecondsDouble();
- oriCtrl.accuracy = rtGetControlStruct().rotationAccuracy;
- oriCtrl.currentPosition = updatedOrientation;
- oriCtrl.targetPosition = getWriterControlStruct().targetOrientation;
- oriCtrl.currentV = sv->velocityRotation;
+ float relativeTargetOrientation = rtGetControlStruct().targetOrientation - getWriterControlStruct().startOrientation;
+ relativeTargetOrientation = std::atan2(std::sin(relativeTargetOrientation), std::cos(relativeTargetOrientation));
- Eigen::Vector2f localTargetVelocity = Eigen::Rotation2Df(-updatedOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
- target->velocityX = localTargetVelocity(0);
- target->velocityY = localTargetVelocity(1);
- target->velocityRotation = oriCtrl.run();
- Eigen::Vector2f posError = pid.target.head(2) - pid.processValue.head(2);
- if (posError.norm() < rtGetControlStruct().translationAccuracy)
- {
- // target->velocityX = 0;
- // target->velocityY = 0;
- }
+ pid.update(timeSinceLastIteration.toSecondsDouble(), updatedPosition, rtGetControlStruct().target);
+ //opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(updatedOrientation), rtGetControlStruct().targetOrientation);
+ opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(relativeGlobalOrientation), relativeTargetOrientation);
- float orientationError = oriCtrl.currentPosition - oriCtrl.targetPosition;
- orientationError = std::atan2(std::sin(orientationError), std::cos(orientationError));
- if (std::fabs(orientationError) < rtGetControlStruct().rotationAccuracy)
- {
- //target->velocityRotation = 0;
- }
+ Eigen::Vector2f velocities = Eigen::Rotation2Df(-updatedOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
+ target->velocityX = velocities.x();
+ target->velocityY = velocities.y();
+ target->velocityRotation = static_cast<float>(opid.getControlValue());
}
void NJointHolonomicPlatformGlobalPositionController::rtPreActivateController()
@@ -135,7 +122,7 @@ namespace armarx
void NJointHolonomicPlatformGlobalPositionController::setGlobalPos(const PlatformPose& currentPose)
{
- // ..todo:: lock :)
+ // ..todo: check if norm is too large
getWriterControlStruct().globalPosition << currentPose.x, currentPose.y;
getWriterControlStruct().globalOrientation = currentPose.rotationAroundZ;
@@ -145,6 +132,8 @@ namespace armarx
getWriterControlStruct().lastUpdate = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
writeControlStruct();
+
+
}
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
index c6c7e6cd9..0f5943ee5 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -51,10 +51,16 @@ namespace armarx
float p = 1.0f;
float i = 0.0f;
float d = 0.0f;
- float maxVelocity = 300;
- float maxAcceleration = 500;
- float maxRotationVelocity = 0.5;
- float maxRotationAcceleration = 0.5;
+ float maxVelocity = 250;
+ float maxAcceleration = 250;
+
+ float p_rot = 0.7f;
+ float i_rot = 0.0f;
+ float d_rot = 0.0f;
+
+ float maxRotationVelocity = 0.7;
+ float maxRotationAcceleration = 0.7;
+
};
struct NJointHolonomicPlatformGlobalPositionControllerTarget
@@ -65,12 +71,12 @@ namespace armarx
float rotationAccuracy = 0.0f;
bool newTargetSet = false;
-
Eigen::Vector2f startPosition;
float startOrientation;
Eigen::Vector2f globalPosition;
float globalOrientation;
+
IceUtil::Time lastUpdate = IceUtil::Time::seconds(0);
};
@@ -108,13 +114,13 @@ namespace armarx
protected:
const SensorValueHolonomicPlatform* sv;
MultiDimPIDController pid;
+ PIDController opid;
- PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition oriCtrl;
ControlTargetHolonomicPlatformVelocity* target;
-
Eigen::Vector2f currentPosition;
float currentOrientation;
+ bool isAborted = false;
};
} // namespace armarx
--
GitLab