From 256e8b1200be8e478091b88ce70d1abfc4ea4210 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 23:14:34 +0200
Subject: [PATCH] Implement separate 2dim CartesianVelocityRamp
---
...omicPlatformVelocityControllerWithRamp.cpp | 72 ++++++++++++++-----
...onomicPlatformVelocityControllerWithRamp.h | 21 +++++-
.../RobotUnitModules/RobotUnitModuleUnits.cpp | 6 +-
3 files changed, 79 insertions(+), 20 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
index 3a31f21bb..4e9d7fefe 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>();
@@ -44,9 +45,6 @@ namespace armarx
velocitySensor = sensor->asA<SensorValueHolonomicPlatformVelocity>();
ARMARX_CHECK_EXPRESSION(velocitySensor)
<< "Sensor value for " << cfg->platformName << " has invalid type";
-
- ramp.setMaxPositionAcceleration(cfg->maxPositionAcceleration);
- ramp.setMaxOrientationAcceleration(cfg->maxOrientationAcceleration);
}
void
@@ -57,14 +55,9 @@ 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));
+
+ setVelocites(velX, velY, velRot);
}
void
@@ -86,14 +79,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 +102,51 @@ namespace armarx
NJointControllerRegistration<NJointHolonomicPlatformVelocityControllerWithRamp>
registrationNJointHolonomicPlatformVelocityControllerWithRamp(
"NJointHolonomicPlatformVelocityControllerWithRamp");
+
+ Cartesian2DimVelocityRamp ::Cartesian2DimVelocityRamp(float maxPositionAcceleration,
+ float maxOrientationAcceleration) :
+ maxPositionAcceleration(maxPositionAcceleration),
+ maxOrientationAcceleration(maxOrientationAcceleration)
+ {
+ }
+
+ void
+ Cartesian2DimVelocityRamp::init(const Eigen::Vector3f& state)
+ {
+ this->state = state;
+ }
+
+ Eigen::Vector3f
+ Cartesian2DimVelocityRamp::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);
+
+ state += delta / factor;
+ return state;
+ }
+
+ void
+ Cartesian2DimVelocityRamp::setMaxPositionAcceleration(float maxPositionAcceleration)
+ {
+ this->maxPositionAcceleration = maxPositionAcceleration;
+ }
+
+ void
+ Cartesian2DimVelocityRamp::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..9d2adec8f 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
@@ -25,7 +25,6 @@
#include <VirtualRobot/Robot.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h>
-#include <RobotAPI/libraries/core/CartesianVelocityRamp.h>
#include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
#include "NJointControllerWithTripleBuffer.h"
@@ -33,6 +32,24 @@
namespace armarx
{
+ class Cartesian2DimVelocityRamp
+ {
+ public:
+ Cartesian2DimVelocityRamp(float maxPositionAcceleration, float maxOrientationAcceleration);
+ 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; // [velX,velY,velRotation]
+ };
+
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformVelocityControllerWithRampConfig);
class NJointHolonomicPlatformVelocityControllerWithRampConfig :
@@ -74,7 +91,7 @@ namespace armarx
void rtPreActivateController() override;
protected:
- CartesianVelocityRamp ramp;
+ Cartesian2DimVelocityRamp ramp;
ControlTargetHolonomicPlatformVelocity* target;
const SensorValueHolonomicPlatformVelocity* velocitySensor;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 80c426cc9..34168e9a2 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -502,8 +502,10 @@ namespace armarx::RobotUnitModule
}
else
{
- ARMARX_ERROR << "Invalid Platform velocity controller specified "
- << ""; //platformControllerType;
+ ARMARX_ERROR << "Invalid Platform velocity controller specified '"
+ << NJointHolonomicPlatformVelocityControllerTypesNames.to_name(
+ platformControllerType)
+ << "'";
}
NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg =
--
GitLab