From bafa5e1c3ba454bfded6b1aba856b5a8a3e98ecf Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Tobias=20Gr=C3=B6ger?= <tobias.groeger@student.kit.edu>
Date: Sun, 28 Apr 2024 15:03:10 +0200
Subject: [PATCH] Implement controller
---
...omicPlatformVelocityControllerWithRamp.cpp | 41 +++++++++++++++----
...onomicPlatformVelocityControllerWithRamp.h | 16 +++++---
2 files changed, 43 insertions(+), 14 deletions(-)
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
index 475f9c37b..77486fe6d 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
@@ -29,10 +29,9 @@
namespace armarx
{
NJointHolonomicPlatformVelocityControllerWithRamp::
- NJointHolonomicPlatformUnitVelocityPassThroughController(
- RobotUnit* prov,
- NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr cfg,
- const VirtualRobot::RobotPtr&) :
+ NJointHolonomicPlatformVelocityControllerWithRamp(RobotUnit* prov,
+ ConfigPtrT cfg,
+ const VirtualRobot::RobotPtr&) :
maxCommandDelay(IceUtil::Time::milliSeconds(500))
{
target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
@@ -41,16 +40,26 @@ namespace armarx
<< "The actuator " << cfg->platformName << " has no control mode "
<< ControlModes::HolonomicPlatformVelocity;
+ NJointHolonomicPlatformVelocityControllerWithRampControlData initialSettings;
initialSettings.velocityX = cfg->initialVelocityX;
initialSettings.velocityY = cfg->initialVelocityY;
initialSettings.velocityRotation = cfg->initialVelocityRotation;
reinitTripleBuffer(initialSettings);
+
+ // init velocity ramp
+ Eigen::VectorXf state(6);
+ state << initialSettings.velocityX, initialSettings.velocityY, 0, 0, 0,
+ initialSettings.velocityRotation;
+ ramp.setState(state, VirtualRobot::IKSolver::CartesianSelection::All);
+
+ ramp.setMaxPositionAcceleration(cfg->maxPositionAcceleration);
+ ramp.setMaxOrientationAcceleration(cfg->maxOrientationAcceleration);
}
void
NJointHolonomicPlatformVelocityControllerWithRamp::rtRun(
const IceUtil::Time& sensorValuesTimestamp,
- const IceUtil::Time&)
+ const IceUtil::Time& timeSinceLastIteration)
{
auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
@@ -66,9 +75,14 @@ namespace armarx
}
else
{
- target->velocityX = rtGetControlStruct().velocityX;
- target->velocityY = rtGetControlStruct().velocityY;
- target->velocityRotation = rtGetControlStruct().velocityRotation;
+ Eigen::VectorXf x(6);
+ x << rtGetControlStruct().velocityX, rtGetControlStruct().velocityY, 0, 0, 0,
+ rtGetControlStruct().velocityRotation;
+
+ Eigen::VectorXf result = ramp.update(x, timeSinceLastIteration.toSecondsDouble());
+ target->velocityX = result(0);
+ target->velocityY = result(1);
+ target->velocityRotation = result(5);
}
}
@@ -85,6 +99,15 @@ namespace armarx
writeControlStruct();
}
+ void
+ NJointHolonomicPlatformVelocityControllerWithRamp::setMaxAccelerations(
+ float maxPositionAcceleration,
+ float maxOrientationAcceleration)
+ {
+ ramp.setMaxPositionAcceleration(maxPositionAcceleration);
+ ramp.setMaxOrientationAcceleration(maxOrientationAcceleration);
+ }
+
IceUtil::Time
NJointHolonomicPlatformVelocityControllerWithRamp::getMaxCommandDelay() const
{
@@ -99,6 +122,6 @@ namespace armarx
}
NJointControllerRegistration<NJointHolonomicPlatformVelocityControllerWithRamp>
- registrationNJointHolonomicPlatformUnitVelocityPassThroughController(
+ registrationNJointHolonomicPlatformVelocityControllerWithRamp(
"NJointHolonomicPlatformVelocityControllerWithRamp");
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
index 12d8f43e6..186d7c581 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
@@ -43,6 +43,9 @@ namespace armarx
float initialVelocityX;
float initialVelocityY;
float initialVelocityRotation;
+
+ float maxPositionAcceleration;
+ float maxOrientationAcceleration;
};
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformVelocityControllerWithRampControlData);
@@ -65,15 +68,18 @@ namespace armarx
public:
using ConfigPtrT = NJointHolonomicPlatformVelocityControllerWithRampConfigPtr;
- NJointHolonomicPlatformVelocityControllerWithRampConfig(RobotUnit* prov,
- ConfigPtrT config,
- const VirtualRobot::RobotPtr&);
+ NJointHolonomicPlatformVelocityControllerWithRamp(RobotUnit* prov,
+ ConfigPtrT config,
+ const VirtualRobot::RobotPtr&);
- void rtRun(const IceUtil::Time&, const IceUtil::Time&) override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
//for the platform unit
void setVelocites(float velocityX, float velocityY, float velocityRotation);
+ void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration);
+
//ice interface
std::string
getClassName(const Ice::Current& = Ice::emptyCurrent) const override
@@ -84,12 +90,12 @@ namespace armarx
IceUtil::Time getMaxCommandDelay() const;
void setMaxCommandDelay(const IceUtil::Time& value);
+
protected:
IceUtil::Time maxCommandDelay;
CartesianVelocityRamp ramp;
ControlTargetHolonomicPlatformVelocity* target;
- NJointHolonomicPlatformVelocityControllerWithRampControlData initialSettings;
};
} // namespace armarx
--
GitLab