From 53ade2238b25913012572a5301c0e2b95f123869 Mon Sep 17 00:00:00 2001
From: Markus Grotz <markus.grotz@kit.edu>
Date: Tue, 2 Jul 2019 17:07:56 +0200
Subject: [PATCH] update NJointHolonomicPlatformRelativePositionController
---
...omicPlatformRelativePositionController.cpp | 23 ++++++++++---------
...onomicPlatformRelativePositionController.h | 5 ++--
2 files changed, 15 insertions(+), 13 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
index 3243b686d..5fa536025 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
@@ -54,23 +54,24 @@ namespace armarx
void NJointHolonomicPlatformRelativePositionController::rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration)
{
- currentPose << sv->relativePositionX,
- sv->relativePositionY;
+ currentPosition << sv->relativePositionX, sv->relativePositionY;
+ currentOrientation = sv->relativePositionRotation;
if (rtGetControlStruct().newTargetSet)
{
- startPose = currentPose;
- orientationOffset = sv->relativePositionRotation;
+ startPosition = currentPosition;
+ startOrientation = currentOrientation;
pid.reset();
*const_cast<bool*>(&rtGetControlStruct().newTargetSet) = false;
}
//position pid
- Eigen::Vector2f relativeCurrentPose = currentPose - startPose;
- pid.update(timeSinceLastIteration.toSecondsDouble(), relativeCurrentPose, rtGetControlStruct().target);
+ Eigen::Vector2f relativeCurrentPosition = currentPosition - startPosition;
+ pid.update(timeSinceLastIteration.toSecondsDouble(), relativeCurrentPosition, rtGetControlStruct().target);
+ float relativeOrientation = currentOrientation - startOrientation;
//rotation pid
// Revert the rotation by rotating by the negative angle
- Eigen::Vector2f localTargetVelocity = Eigen::Rotation2Df(-sv->relativePositionRotation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
+ Eigen::Vector2f localTargetVelocity = Eigen::Rotation2Df(-currentOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
//ARMARX_RT_LOGF_INFO("global target vel x: %.2f y: %2.f, local target vel x: %.2f y: %2.f rotation: %2.f", pid.getControlValue()[0], pid.getControlValue()[1], localTargetVelocity[0], localTargetVelocity[1], sv->relativePositionRotation).deactivateSpam(0.1);
target->velocityX = localTargetVelocity[0];
@@ -78,7 +79,7 @@ namespace armarx
// target->velocityRotation = pid.getControlValue()[2] / rad2MMFactor;
oriCtrl.dt = timeSinceLastIteration.toSecondsDouble();
oriCtrl.accuracy = rtGetControlStruct().rotationAccuracy;
- oriCtrl.currentPosition = sv->relativePositionRotation - orientationOffset;
+ oriCtrl.currentPosition = relativeOrientation;
oriCtrl.targetPosition = rtGetControlStruct().targetOrientation;
oriCtrl.currentV = sv->velocityRotation;
target->velocityRotation = oriCtrl.run();
@@ -101,9 +102,9 @@ namespace armarx
void NJointHolonomicPlatformRelativePositionController::rtPreActivateController()
{
- startPose[0] = sv->relativePositionX;
- startPose[1] = sv->relativePositionY;
- orientationOffset = sv->relativePositionRotation;
+ startPosition[0] = sv->relativePositionX;
+ startPosition[1] = sv->relativePositionY;
+ startOrientation = sv->relativePositionRotation;
}
void NJointHolonomicPlatformRelativePositionController::setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
index cf4e73188..abbd25d99 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
@@ -101,8 +101,9 @@ namespace armarx
MultiDimPIDController pid;
PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition oriCtrl;
ControlTargetHolonomicPlatformVelocity* target;
- Eigen::Vector2f startPose, currentPose;
- float orientationOffset;
+ Eigen::Vector2f startPosition, currentPosition;
+ float startOrientation;
+ float currentOrientation;
// float rad2MMFactor;
};
} // namespace armarx
--
GitLab