From ad68bd4922f2ed1d249368e08399a3292f96e077 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Thu, 2 May 2024 18:51:22 +0200
Subject: [PATCH] Fix set current velocity in controller pre activate
---
...omicPlatformVelocityControllerWithRamp.cpp | 27 ++++++++++++-------
...onomicPlatformVelocityControllerWithRamp.h | 3 +++
2 files changed, 20 insertions(+), 10 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
index 4e9d7fefe..2e5642d3f 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
@@ -50,14 +50,13 @@ namespace armarx
void
NJointHolonomicPlatformVelocityControllerWithRamp::rtPreActivateController()
{
- const float velX = velocitySensor->velocityX;
- const float velY = velocitySensor->velocityY;
- const float velRot = velocitySensor->velocityRotation;
+ activationVelocity(0) = velocitySensor->velocityX;
+ activationVelocity(1) = velocitySensor->velocityY;
+ activationVelocity(2) = velocitySensor->velocityRotation;
+ activationTime = IceUtil::Time::now();
// init velocity ramp
- ramp.init(Eigen::Vector3f(velX, velY, velRot));
-
- setVelocites(velX, velY, velRot);
+ ramp.init(activationVelocity);
}
void
@@ -79,11 +78,19 @@ namespace armarx
}
else
{
- Eigen::Vector3f x(rtGetControlStruct().velocityX,
- rtGetControlStruct().velocityY,
- rtGetControlStruct().velocityRotation);
+ Eigen::Vector3f result;
+ if (activationTime > rtGetControlStruct().commandTimestamp)
+ {
+ result = ramp.update(activationVelocity, timeSinceLastIteration.toSecondsDouble());
+ }
+ else
+ {
+ Eigen::Vector3f x(rtGetControlStruct().velocityX,
+ rtGetControlStruct().velocityY,
+ rtGetControlStruct().velocityRotation);
+ result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
+ }
- Eigen::VectorXf result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
target->velocityX = result(0);
target->velocityY = result(1);
target->velocityRotation = result(2);
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
index 9d2adec8f..8cd957b50 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
@@ -93,6 +93,9 @@ namespace armarx
protected:
Cartesian2DimVelocityRamp ramp;
+ Eigen::Vector3f activationVelocity;
+ IceUtil::Time activationTime;
+
ControlTargetHolonomicPlatformVelocity* target;
const SensorValueHolonomicPlatformVelocity* velocitySensor;
};
--
GitLab