From d4bc11c2c32e44894d351b82479190c9a01c5bb2 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 18:40:37 +0200
Subject: [PATCH] Implement properties to choose controller
Add a common base class for both HolonomicPlatformVelocity controllers
and add a property with which to choose one.
Make the acceleration limits also configurable via properties.
---
.../components/units/RobotUnit/CMakeLists.txt | 3 +
...tformUnitVelocityPassThroughController.cpp | 29 +-------
...latformUnitVelocityPassThroughController.h | 26 +------
...micPlatformVelocityControllerInterface.cpp | 63 ++++++++++++++++
...nomicPlatformVelocityControllerInterface.h | 59 +++++++++++++++
...HolonomicPlatformVelocityControllerTypes.h | 43 +++++++++++
...omicPlatformVelocityControllerWithRamp.cpp | 53 ++++----------
...onomicPlatformVelocityControllerWithRamp.h | 34 ++-------
.../RobotUnitModules/RobotUnitModuleUnits.cpp | 73 ++++++++++++++-----
.../RobotUnitModules/RobotUnitModuleUnits.h | 20 +++++
.../units/RobotUnit/Units/PlatformSubUnit.h | 5 +-
11 files changed, 272 insertions(+), 136 deletions(-)
create mode 100644 source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.cpp
create mode 100644 source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h
create mode 100644 source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerTypes.h
diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index bfbe4b911..16f091a3a 100755
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -43,6 +43,7 @@ set(LIB_FILES
NJointControllers/NJointController.cpp
NJointControllers/NJointTrajectoryController.cpp
NJointControllers/NJointKinematicUnitPassThroughController.cpp
+ NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.cpp
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
@@ -122,6 +123,8 @@ set(LIB_HEADERS
NJointControllers/NJointControllerRegistry.h
NJointControllers/NJointControllerWithTripleBuffer.h
NJointControllers/NJointTrajectoryController.h
+ NJointControllers/NJointHolonomicPlatformVelocityControllerTypes.h
+ NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h
NJointControllers/NJointKinematicUnitPassThroughController.h
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
index bd440c1b0..892a1ae7e 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
@@ -32,8 +32,7 @@ namespace armarx
NJointHolonomicPlatformUnitVelocityPassThroughController(
RobotUnit* prov,
NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr cfg,
- const VirtualRobot::RobotPtr&) :
- maxCommandDelay(IceUtil::Time::milliSeconds(500))
+ const VirtualRobot::RobotPtr&)
{
target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
->asA<ControlTargetHolonomicPlatformVelocity>();
@@ -72,32 +71,6 @@ namespace armarx
}
}
- void
- NJointHolonomicPlatformUnitVelocityPassThroughController::setVelocites(float velocityX,
- float velocityY,
- float velocityRotation)
- {
- LockGuardType guard{controlDataMutex};
- getWriterControlStruct().velocityX = velocityX;
- getWriterControlStruct().velocityY = velocityY;
- getWriterControlStruct().velocityRotation = velocityRotation;
- getWriterControlStruct().commandTimestamp = IceUtil::Time::now();
- writeControlStruct();
- }
-
- IceUtil::Time
- NJointHolonomicPlatformUnitVelocityPassThroughController::getMaxCommandDelay() const
- {
- return maxCommandDelay;
- }
-
- void
- NJointHolonomicPlatformUnitVelocityPassThroughController::setMaxCommandDelay(
- const IceUtil::Time& value)
- {
- maxCommandDelay = value;
- }
-
NJointControllerRegistration<NJointHolonomicPlatformUnitVelocityPassThroughController>
registrationNJointHolonomicPlatformUnitVelocityPassThroughController(
"NJointHolonomicPlatformUnitVelocityPassThroughController");
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
index 37df1f055..ef08635a5 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
@@ -24,6 +24,8 @@
#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h>
+
#include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
#include "../util.h"
#include "NJointControllerWithTripleBuffer.h"
@@ -45,24 +47,12 @@ namespace armarx
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformUnitVelocityPassThroughController);
- class NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData
- {
- public:
- float velocityX = 0;
- float velocityY = 0;
- float velocityRotation = 0;
- IceUtil::Time commandTimestamp;
- };
-
- TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformUnitVelocityPassThroughController);
-
/**
* @brief The NJointHolonomicPlatformUnitVelocityPassThroughController class
* @ingroup Library-RobotUnit-NJointControllers
*/
class NJointHolonomicPlatformUnitVelocityPassThroughController :
- virtual public NJointControllerWithTripleBuffer<
- NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData>
+ virtual public NJointHolonomicPlatformVelocityControllerInterface
{
public:
using ConfigPtrT = NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr;
@@ -74,9 +64,6 @@ namespace armarx
void rtRun(const IceUtil::Time&, const IceUtil::Time&) override;
- //for the platform unit
- void setVelocites(float velocityX, float velocityY, float velocityRotation);
-
//ice interface
std::string
getClassName(const Ice::Current& = Ice::emptyCurrent) const override
@@ -84,13 +71,8 @@ namespace armarx
return "NJointHolonomicPlatformUnitVelocityPassThroughController";
}
- IceUtil::Time getMaxCommandDelay() const;
- void setMaxCommandDelay(const IceUtil::Time& value);
-
protected:
- IceUtil::Time maxCommandDelay;
-
ControlTargetHolonomicPlatformVelocity* target;
- NJointHolonomicPlatformUnitVelocityPassThroughControllerControlData initialSettings;
+ NJointHolonomicPlatformVelocityControllerControlData initialSettings;
};
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.cpp
new file mode 100644
index 000000000..0db22289e
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.cpp
@@ -0,0 +1,63 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::NJointHolonomicPlatformUnitVelocityPassThroughController
+ * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "NJointHolonomicPlatformVelocityControllerInterface.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
+namespace armarx
+{
+ NJointHolonomicPlatformVelocityControllerInterface::
+ NJointHolonomicPlatformVelocityControllerInterface() :
+ maxCommandDelay(IceUtil::Time::milliSeconds(500))
+ {
+ }
+
+ void
+ NJointHolonomicPlatformVelocityControllerInterface::setVelocites(float velocityX,
+ float velocityY,
+ float velocityRotation)
+ {
+ LockGuardType guard{controlDataMutex};
+ getWriterControlStruct().velocityX = velocityX;
+ getWriterControlStruct().velocityY = velocityY;
+ getWriterControlStruct().velocityRotation = velocityRotation;
+ getWriterControlStruct().commandTimestamp = IceUtil::Time::now();
+ writeControlStruct();
+ }
+
+ IceUtil::Time
+ NJointHolonomicPlatformVelocityControllerInterface::getMaxCommandDelay() const
+ {
+ return maxCommandDelay;
+ }
+
+ void
+ NJointHolonomicPlatformVelocityControllerInterface::setMaxCommandDelay(
+ const IceUtil::Time& value)
+ {
+ maxCommandDelay = value;
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h
new file mode 100644
index 000000000..7a9acbe71
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h
@@ -0,0 +1,59 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::NJointHolonomicPlatformVelocityControllerInterface
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2024
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include "NJointControllerWithTripleBuffer.h"
+
+namespace armarx
+{
+
+ TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformVelocityControllerControlData);
+
+ class NJointHolonomicPlatformVelocityControllerControlData
+ {
+ public:
+ float velocityX = 0;
+ float velocityY = 0;
+ float velocityRotation = 0;
+ IceUtil::Time commandTimestamp;
+ };
+
+ TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformVelocityControllerInterface);
+
+ class NJointHolonomicPlatformVelocityControllerInterface :
+ virtual public NJointControllerWithTripleBuffer<
+ NJointHolonomicPlatformVelocityControllerControlData>
+ {
+ public:
+ NJointHolonomicPlatformVelocityControllerInterface();
+
+
+ void setVelocites(float velocityX, float velocityY, float velocityRotation);
+
+ IceUtil::Time getMaxCommandDelay() const;
+ void setMaxCommandDelay(const IceUtil::Time& value);
+
+ protected:
+ IceUtil::Time maxCommandDelay;
+ };
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerTypes.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerTypes.h
new file mode 100644
index 000000000..c864e700d
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerTypes.h
@@ -0,0 +1,43 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::NJointHolonomicPlatformVelocityControllerTypes
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2024
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <SimoxUtility/meta/enum/EnumNames.hpp>
+
+namespace armarx
+{
+ enum class NJointHolonomicPlatformVelocityControllerTypes
+ {
+ PassThroughController,
+ ControllerWithRamp
+ };
+
+
+ inline const simox::meta::EnumNames<NJointHolonomicPlatformVelocityControllerTypes>
+ NJointHolonomicPlatformVelocityControllerTypesNames{
+ {NJointHolonomicPlatformVelocityControllerTypes::PassThroughController,
+ "PassThroughController"},
+ {NJointHolonomicPlatformVelocityControllerTypes::ControllerWithRamp,
+ "ControllerWithRamp"}};
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
index 77486fe6d..5b5d9f5d1 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
@@ -31,8 +31,7 @@ namespace armarx
NJointHolonomicPlatformVelocityControllerWithRamp::
NJointHolonomicPlatformVelocityControllerWithRamp(RobotUnit* prov,
ConfigPtrT cfg,
- const VirtualRobot::RobotPtr&) :
- maxCommandDelay(IceUtil::Time::milliSeconds(500))
+ const VirtualRobot::RobotPtr&)
{
target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
->asA<ControlTargetHolonomicPlatformVelocity>();
@@ -40,20 +39,24 @@ 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);
+ const auto sensor = useSensorValue(cfg->platformName);
+ ARMARX_CHECK_EXPRESSION(sensor) << "No sensor value for " << cfg->platformName;
+ 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
+ NJointHolonomicPlatformVelocityControllerWithRamp::rtPreActivateController()
+ {
// init velocity ramp
Eigen::VectorXf state(6);
- state << initialSettings.velocityX, initialSettings.velocityY, 0, 0, 0,
- initialSettings.velocityRotation;
+ state << velocitySensor->velocityX, velocitySensor->velocityY, 0, 0, 0,
+ velocitySensor->velocityRotation;
ramp.setState(state, VirtualRobot::IKSolver::CartesianSelection::All);
-
- ramp.setMaxPositionAcceleration(cfg->maxPositionAcceleration);
- ramp.setMaxOrientationAcceleration(cfg->maxOrientationAcceleration);
}
void
@@ -86,19 +89,6 @@ namespace armarx
}
}
- void
- NJointHolonomicPlatformVelocityControllerWithRamp::setVelocites(float velocityX,
- float velocityY,
- float velocityRotation)
- {
- LockGuardType guard{controlDataMutex};
- getWriterControlStruct().velocityX = velocityX;
- getWriterControlStruct().velocityY = velocityY;
- getWriterControlStruct().velocityRotation = velocityRotation;
- getWriterControlStruct().commandTimestamp = IceUtil::Time::now();
- writeControlStruct();
- }
-
void
NJointHolonomicPlatformVelocityControllerWithRamp::setMaxAccelerations(
float maxPositionAcceleration,
@@ -108,19 +98,6 @@ namespace armarx
ramp.setMaxOrientationAcceleration(maxOrientationAcceleration);
}
- IceUtil::Time
- NJointHolonomicPlatformVelocityControllerWithRamp::getMaxCommandDelay() const
- {
- return maxCommandDelay;
- }
-
- void
- NJointHolonomicPlatformVelocityControllerWithRamp::setMaxCommandDelay(
- const IceUtil::Time& value)
- {
- maxCommandDelay = value;
- }
-
NJointControllerRegistration<NJointHolonomicPlatformVelocityControllerWithRamp>
registrationNJointHolonomicPlatformVelocityControllerWithRamp(
"NJointHolonomicPlatformVelocityControllerWithRamp");
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
index 9070f0287..e5110624a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
@@ -13,9 +13,9 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
- * @package RobotAPI::ArmarXObjects::NJointHolonomicPlatformUnitVelocityPassThroughController
- * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
- * @date 2017
+ * @package RobotAPI::NJointHolonomicPlatformVelocityControllerInterface
+ * @author Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @date 2024
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
@@ -24,6 +24,7 @@
#include <VirtualRobot/Robot.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h>
#include <RobotAPI/libraries/core/CartesianVelocityRamp.h>
#include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
@@ -39,30 +40,15 @@ namespace armarx
{
public:
std::string platformName;
- float initialVelocityX;
- float initialVelocityY;
- float initialVelocityRotation;
float maxPositionAcceleration;
float maxOrientationAcceleration;
};
- TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformVelocityControllerWithRampControlData);
-
- class NJointHolonomicPlatformVelocityControllerWithRampControlData
- {
- public:
- float velocityX = 0;
- float velocityY = 0;
- float velocityRotation = 0;
- IceUtil::Time commandTimestamp;
- };
-
TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformVelocityControllerWithRamp);
class NJointHolonomicPlatformVelocityControllerWithRamp :
- virtual public NJointControllerWithTripleBuffer<
- NJointHolonomicPlatformVelocityControllerWithRampControlData>
+ virtual public NJointHolonomicPlatformVelocityControllerInterface
{
public:
using ConfigPtrT = NJointHolonomicPlatformVelocityControllerWithRampConfigPtr;
@@ -74,8 +60,6 @@ namespace armarx
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);
@@ -86,15 +70,13 @@ namespace armarx
return "NJointHolonomicPlatformVelocityControllerWithRamp";
}
- IceUtil::Time getMaxCommandDelay() const;
- void setMaxCommandDelay(const IceUtil::Time& value);
-
-
protected:
- IceUtil::Time maxCommandDelay;
+ void rtPreActivateController() override;
+ protected:
CartesianVelocityRamp ramp;
ControlTargetHolonomicPlatformVelocity* target;
+ const SensorValueHolonomicPlatformVelocity* velocitySensor;
};
} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 7a60318dc..80c426cc9 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -28,6 +28,8 @@
#include "RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h"
#include "RobotAPI/components/units/RobotUnit/Units/LocalizationSubUnit.h"
#include <RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h>
#include "../RobotUnit.h"
#include "../SensorValues/SensorValue1DoFActuator.h"
@@ -449,27 +451,60 @@ namespace armarx::RobotUnitModule
Component::create<UnitT>(properties, configName, getConfigDomain());
//config
ARMARX_TRACE;
- NJointHolonomicPlatformVelocityControllerWithRampConfigPtr config =
- new NJointHolonomicPlatformVelocityControllerWithRampConfig;
- config->initialVelocityX = 0;
- config->initialVelocityY = 0;
- config->initialVelocityRotation = 0;
- config->platformName = _module<RobotData>().getRobotPlatformName();
+ const NJointHolonomicPlatformVelocityControllerTypes platformControllerType =
+ getProperty<NJointHolonomicPlatformVelocityControllerTypes>(
+ "PlatformUnitVelocityControllerType");
+ if (platformControllerType ==
+ NJointHolonomicPlatformVelocityControllerTypes::PassThroughController)
+ {
+ NJointHolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config =
+ new NJointHolonomicPlatformUnitVelocityPassThroughControllerConfig;
+ config->initialVelocityX = 0;
+ config->initialVelocityY = 0;
+ config->initialVelocityRotation = 0;
+ config->platformName = _module<RobotData>().getRobotPlatformName();
+
+ auto ctrl = NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr::dynamicCast(
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformUnitVelocityPassThroughController",
+ getName() + "_" + configName + "_VelPTContoller",
+ config,
+ false,
+ true));
+ ARMARX_TRACE;
+ ARMARX_CHECK_EXPRESSION(ctrl);
+ unit->pt = ctrl;
+ ARMARX_TRACE;
+ }
+ else if (platformControllerType ==
+ NJointHolonomicPlatformVelocityControllerTypes::ControllerWithRamp)
+ {
+ NJointHolonomicPlatformVelocityControllerWithRampConfigPtr config =
+ new NJointHolonomicPlatformVelocityControllerWithRampConfig;
+ config->platformName = _module<RobotData>().getRobotPlatformName();
- config->maxPositionAcceleration = 800;
- config->maxOrientationAcceleration = 80;
+ config->maxPositionAcceleration =
+ getProperty<float>("PlatformUnitMaximumPositionAcceleration");
+ config->maxOrientationAcceleration =
+ getProperty<float>("PlatformUnitMaximumOrientationAcceleration");
- auto ctrl = NJointHolonomicPlatformVelocityControllerWithRampPtr::dynamicCast(
- _module<ControllerManagement>().createNJointController(
- "NJointHolonomicPlatformVelocityControllerWithRamp",
- getName() + "_" + configName + "_VelPTContoller",
- config,
- false,
- true));
- ARMARX_TRACE;
- ARMARX_CHECK_EXPRESSION(ctrl);
- unit->pt = ctrl;
- ARMARX_TRACE;
+ auto ctrl = NJointHolonomicPlatformVelocityControllerWithRampPtr::dynamicCast(
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformVelocityControllerWithRamp",
+ getName() + "_" + configName + "_VelPTContoller",
+ config,
+ false,
+ true));
+ ARMARX_TRACE;
+ ARMARX_CHECK_EXPRESSION(ctrl);
+ unit->pt = ctrl;
+ ARMARX_TRACE;
+ }
+ else
+ {
+ ARMARX_ERROR << "Invalid Platform velocity controller specified "
+ << ""; //platformControllerType;
+ }
NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg =
new NJointHolonomicPlatformRelativePositionControllerConfig;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
index 6a4237226..f33c7ee47 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
@@ -22,6 +22,7 @@
#pragma once
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerTypes.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
#include "../Units/RobotUnitSubUnit.h"
@@ -43,6 +44,25 @@ namespace armarx::RobotUnitModule
defineOptionalProperty<std::string>(
"PlatformUnitName", "PlatformUnit", "The name of the created platform unit");
+ auto& types = defineOptionalProperty<NJointHolonomicPlatformVelocityControllerTypes>(
+ "PlatformUnitVelocityControllerType",
+ NJointHolonomicPlatformVelocityControllerTypes::PassThroughController,
+ "Which controller to use for velocity control of the platform");
+ for (const auto& [type, name] :
+ NJointHolonomicPlatformVelocityControllerTypesNames.map())
+ {
+ types.map(name, type);
+ }
+
+ defineOptionalProperty<float>(
+ "PlatformUnitMaximumPositionAcceleration",
+ 800,
+ "The maximum allowed acceleration for the position of the platform");
+ defineOptionalProperty<float>(
+ "PlatformUnitMaximumOrientationAcceleration",
+ 80,
+ "The maximum allowed acceleration for the orientation of the platform");
+
defineOptionalProperty<std::string>(
"ForceTorqueUnitName",
"ForceTorqueUnit",
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
index 5c2bb4a8d..dd41099f1 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -31,11 +31,10 @@
#include <RobotAPI/components/units/PlatformUnit.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h>
-#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h>
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/libraries/core/Pose.h>
-#include "../NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h"
#include "../SensorValues/SensorValueHolonomicPlatform.h"
#include "RobotUnitSubUnit.h"
@@ -94,7 +93,7 @@ namespace armarx
void stopPlatform(const Ice::Current& c = Ice::emptyCurrent) override;
- NJointHolonomicPlatformVelocityControllerWithRampPtr pt;
+ NJointHolonomicPlatformVelocityControllerInterfacePtr pt;
NJointHolonomicPlatformRelativePositionControllerPtr relativePosCtrl;
NJointHolonomicPlatformGlobalPositionControllerPtr globalPosCtrl;
--
GitLab