diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index dbaaf4873bd64b8aede4752f0b8cdc8de3029579..16f091a3ac5ca8ae10f5b812761b179b86efc6ea 100755
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -43,7 +43,9 @@ set(LIB_FILES
NJointControllers/NJointController.cpp
NJointControllers/NJointTrajectoryController.cpp
NJointControllers/NJointKinematicUnitPassThroughController.cpp
+ NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.cpp
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
+ NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
NJointControllers/NJointTCPController.cpp
@@ -121,8 +123,11 @@ 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
NJointControllers/NJointHolonomicPlatformRelativePositionController.h
NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
NJointControllers/NJointTCPController.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
old mode 100755
new mode 100644
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
old mode 100755
new mode 100644
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
index bd440c1b0b124795cbe519299bfca7931a64d3bf..892a1ae7e554387630efa35d048ee6cd44f02584 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 37df1f055d7daeb61630e62698585e484ed6cc75..ef08635a5f8329e80020f47b1eb8b49b3a894b7a 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 0000000000000000000000000000000000000000..0db22289e2d7be967f79ac044a88270c06f4a47f
--- /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 0000000000000000000000000000000000000000..7a9acbe71cf958b2a1027c639466972221948965
--- /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 0000000000000000000000000000000000000000..c864e700da2ac4c4159893c8099de48e43ee87ae
--- /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
new file mode 100644
index 0000000000000000000000000000000000000000..2e5642d3fb78c95afd81644dec24d4c13f313ff4
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.cpp
@@ -0,0 +1,159 @@
+/*
+ * 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 "NJointHolonomicPlatformVelocityControllerWithRamp.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+
+namespace armarx
+{
+ NJointHolonomicPlatformVelocityControllerWithRamp::
+ NJointHolonomicPlatformVelocityControllerWithRamp(RobotUnit* prov,
+ ConfigPtrT cfg,
+ const VirtualRobot::RobotPtr&) :
+ ramp(cfg->maxPositionAcceleration, cfg->maxOrientationAcceleration)
+ {
+ target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)
+ ->asA<ControlTargetHolonomicPlatformVelocity>();
+ ARMARX_CHECK_EXPRESSION(target)
+ << "The actuator " << cfg->platformName << " has no control mode "
+ << ControlModes::HolonomicPlatformVelocity;
+
+ 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";
+ }
+
+ void
+ NJointHolonomicPlatformVelocityControllerWithRamp::rtPreActivateController()
+ {
+ activationVelocity(0) = velocitySensor->velocityX;
+ activationVelocity(1) = velocitySensor->velocityY;
+ activationVelocity(2) = velocitySensor->velocityRotation;
+ activationTime = IceUtil::Time::now();
+
+ // init velocity ramp
+ ramp.init(activationVelocity);
+ }
+
+ void
+ NJointHolonomicPlatformVelocityControllerWithRamp::rtRun(
+ const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration)
+ {
+ auto commandAge = sensorValuesTimestamp - rtGetControlStruct().commandTimestamp;
+
+ if (commandAge > maxCommandDelay && // command must be recent
+ (rtGetControlStruct().velocityX != 0.0f || rtGetControlStruct().velocityY != 0.0f ||
+ rtGetControlStruct().velocityRotation !=
+ 0.0f)) // only throw error if any command is not zero
+ {
+ throw LocalException(
+ "Platform target velocity was not set for a too long time: delay: ")
+ << commandAge.toSecondsDouble()
+ << " s, max allowed delay: " << maxCommandDelay.toSecondsDouble() << " s";
+ }
+ else
+ {
+ 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());
+ }
+
+ target->velocityX = result(0);
+ target->velocityY = result(1);
+ target->velocityRotation = result(2);
+ }
+ }
+
+ void
+ NJointHolonomicPlatformVelocityControllerWithRamp::setMaxAccelerations(
+ float maxPositionAcceleration,
+ float maxOrientationAcceleration)
+ {
+ ramp.setMaxPositionAcceleration(maxPositionAcceleration);
+ ramp.setMaxOrientationAcceleration(maxOrientationAcceleration);
+ }
+
+ 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
new file mode 100644
index 0000000000000000000000000000000000000000..8cd957b50d7911524c6e5d68a91ad4359213f145
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h
@@ -0,0 +1,102 @@
+/*
+ * 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 <VirtualRobot/Robot.h>
+
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerInterface.h>
+
+#include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
+#include "NJointControllerWithTripleBuffer.h"
+
+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 :
+ virtual public NJointControllerConfig
+ {
+ public:
+ std::string platformName;
+
+ float maxPositionAcceleration;
+ float maxOrientationAcceleration;
+ };
+
+ TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformVelocityControllerWithRamp);
+
+ class NJointHolonomicPlatformVelocityControllerWithRamp :
+ virtual public NJointHolonomicPlatformVelocityControllerInterface
+ {
+ public:
+ using ConfigPtrT = NJointHolonomicPlatformVelocityControllerWithRampConfigPtr;
+
+ NJointHolonomicPlatformVelocityControllerWithRamp(RobotUnit* prov,
+ ConfigPtrT config,
+ const VirtualRobot::RobotPtr&);
+
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp,
+ const IceUtil::Time& timeSinceLastIteration) override;
+
+
+ void setMaxAccelerations(float maxPositionAcceleration, float maxOrientationAcceleration);
+
+ //ice interface
+ std::string
+ getClassName(const Ice::Current& = Ice::emptyCurrent) const override
+ {
+ return "NJointHolonomicPlatformVelocityControllerWithRamp";
+ }
+
+ protected:
+ void rtPreActivateController() override;
+
+ protected:
+ Cartesian2DimVelocityRamp ramp;
+
+ Eigen::Vector3f activationVelocity;
+ IceUtil::Time activationTime;
+
+ 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 16af4f8c064391718d3aef43efac649030d41ed8..34168e9a242f8c43da13eb43e2a809594ebbc226 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,23 +451,62 @@ namespace armarx::RobotUnitModule
Component::create<UnitT>(properties, configName, getConfigDomain());
//config
ARMARX_TRACE;
- 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;
+ 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 =
+ 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;
+ }
+ else
+ {
+ ARMARX_ERROR << "Invalid Platform velocity controller specified '"
+ << NJointHolonomicPlatformVelocityControllerTypesNames.to_name(
+ platformControllerType)
+ << "'";
+ }
NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg =
new NJointHolonomicPlatformRelativePositionControllerConfig;
@@ -479,7 +520,6 @@ namespace armarx::RobotUnitModule
false,
true));
ARMARX_CHECK_EXPRESSION(ctrlRelativePosition);
- unit->pt = ctrl;
unit->relativePosCtrl = ctrlRelativePosition;
// unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
@@ -496,7 +536,6 @@ namespace armarx::RobotUnitModule
true));
ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(ctrlGlobalPosition);
- unit->pt = ctrl;
unit->globalPosCtrl = ctrlGlobalPosition;
ARMARX_TRACE;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.h
index 6a423722690a411ab270477bcacd387118ba8d61..f33c7ee47065d3bb7c5f1302d806d3af8229920a 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 90d5443bfd71a795ba244f650ae97b24988359e2..dd41099f1f79411eb23fcd7e484b9c57b7cd42a8 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -31,10 +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/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"
@@ -93,7 +93,7 @@ namespace armarx
void stopPlatform(const Ice::Current& c = Ice::emptyCurrent) override;
- NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr pt;
+ NJointHolonomicPlatformVelocityControllerInterfacePtr pt;
NJointHolonomicPlatformRelativePositionControllerPtr relativePosCtrl;
NJointHolonomicPlatformGlobalPositionControllerPtr globalPosCtrl;