From c26ac5d65b8963861273308b415f580482a4befa Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Mon, 29 Apr 2024 22:00:51 +0200
Subject: [PATCH] Test
---
...micPlatformVelocityControllerInterface.cpp | 2 +-
...omicPlatformVelocityControllerWithRamp.cpp | 82 +++++++++++++++----
...onomicPlatformVelocityControllerWithRamp.h | 20 ++++-
3 files changed, 87 insertions(+), 17 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.cpp
index 8ca4311f4..0db22289e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.cpp
@@ -30,7 +30,7 @@ namespace armarx
{
NJointHolonomicPlatformVelocityControllerInterface::
NJointHolonomicPlatformVelocityControllerInterface() :
- maxCommandDelay(IceUtil::Time::milliSeconds(800))
+ maxCommandDelay(IceUtil::Time::milliSeconds(500))
{
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
index 3a31f21bb..29d3f9991 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
@@ -31,7 +31,8 @@ namespace armarx
NJointHolonomicPlatformVelocityControllerWithRamp::
NJointHolonomicPlatformVelocityControllerWithRamp(RobotUnit* prov,
ConfigPtrT cfg,
- const VirtualRobot::RobotPtr&)
+ const VirtualRobot::RobotPtr&) :
+ ramp(cfg->maxPositionAcceleration, cfg->maxOrientationAcceleration)
{
target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
->asA<ControlTargetHolonomicPlatformVelocity>();
@@ -45,8 +46,8 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(velocitySensor)
<< "Sensor value for " << cfg->platformName << " has invalid type";
- ramp.setMaxPositionAcceleration(cfg->maxPositionAcceleration);
- ramp.setMaxOrientationAcceleration(cfg->maxOrientationAcceleration);
+ // ramp.setMaxPositionAcceleration(cfg->maxPositionAcceleration);
+ // ramp.setMaxOrientationAcceleration(cfg->maxOrientationAcceleration);
}
void
@@ -57,14 +58,15 @@ namespace armarx
const float velRot = velocitySensor->velocityRotation;
// init velocity ramp
- Eigen::VectorXf state(6);
- state << velX, velY, 0, 0, 0, velRot;
- ramp.setState(state, VirtualRobot::IKSolver::CartesianSelection::All);
-
- getWriterControlStruct().velocityX = velX;
- getWriterControlStruct().velocityY = velY;
- getWriterControlStruct().velocityRotation = velRot;
- writeControlStruct();
+ ramp.init(Eigen::Vector3f(velX, velY, velRot));
+
+ NJointHolonomicPlatformVelocityControllerControlData data;
+ data.velocityX = velX;
+ data.velocityY = velY;
+ data.velocityRotation = velRot;
+ data.commandTimestamp = IceUtil::Time::now();
+ setControlStruct(data);
+ // reinitTripleBuffer(data);
}
void
@@ -86,14 +88,14 @@ namespace armarx
}
else
{
- Eigen::VectorXf x(6);
- x << rtGetControlStruct().velocityX, rtGetControlStruct().velocityY, 0, 0, 0,
- rtGetControlStruct().velocityRotation;
+ Eigen::Vector3f x(rtGetControlStruct().velocityX,
+ rtGetControlStruct().velocityY,
+ rtGetControlStruct().velocityRotation);
Eigen::VectorXf result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
target->velocityX = result(0);
target->velocityY = result(1);
- target->velocityRotation = result(5);
+ target->velocityRotation = result(2);
}
}
@@ -109,4 +111,54 @@ namespace armarx
NJointControllerRegistration<NJointHolonomicPlatformVelocityControllerWithRamp>
registrationNJointHolonomicPlatformVelocityControllerWithRamp(
"NJointHolonomicPlatformVelocityControllerWithRamp");
+
+ RtRamp::RtRamp(float maxPos, float maxRot) :
+ maxPositionAcceleration(maxPos), maxOrientationAcceleration(maxRot)
+ {
+ }
+
+ void
+ RtRamp::init(const Eigen::Vector3f& state)
+ {
+ this->state = state;
+ }
+
+ Eigen::Vector3f
+ RtRamp::update(const Eigen::Vector3f& target, float dt)
+ {
+ if (dt <= 0)
+ {
+ return state;
+ }
+ Eigen::Vector3f delta = target - state;
+ float factor = 1;
+
+ Eigen::Vector2f posDelta = delta.block<2, 1>(0, 0);
+ float posFactor = posDelta.norm() / maxPositionAcceleration / dt;
+ factor = std::max(factor, posFactor);
+
+ float oriFactor = std::abs(delta(2)) / maxOrientationAcceleration / dt;
+ factor = std::max(factor, oriFactor);
+
+ if (factor < 1)
+ {
+ throw LocalException("Error factor negative!");
+ }
+
+ state += delta / factor;
+ return state;
+ }
+
+ void
+ RtRamp::setMaxPositionAcceleration(float maxPositionAcceleration)
+ {
+ this->maxPositionAcceleration = maxPositionAcceleration;
+ }
+
+ void
+ RtRamp::setMaxOrientationAcceleration(float maxOrientationAcceleration)
+ {
+ this->maxOrientationAcceleration = maxOrientationAcceleration;
+ }
+
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
index e5110624a..5b32fab17 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
@@ -33,6 +33,24 @@
namespace armarx
{
+ class RtRamp
+ {
+ public:
+ RtRamp(float maxPos, float maxRot);
+ void init(const Eigen::Vector3f& state);
+
+ Eigen::Vector3f update(const Eigen::Vector3f& target, float dt);
+
+ void setMaxPositionAcceleration(float maxPositionAcceleration);
+ void setMaxOrientationAcceleration(float maxOrientationAcceleration);
+
+ private:
+ float maxPositionAcceleration = 0;
+ float maxOrientationAcceleration = 0;
+
+ Eigen::Vector3f state;
+ };
+
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformVelocityControllerWithRampConfig);
class NJointHolonomicPlatformVelocityControllerWithRampConfig :
@@ -74,7 +92,7 @@ namespace armarx
void rtPreActivateController() override;
protected:
- CartesianVelocityRamp ramp;
+ RtRamp ramp;
ControlTargetHolonomicPlatformVelocity* target;
const SensorValueHolonomicPlatformVelocity* velocitySensor;
--
GitLab