From 69b769ef1c6a349fdf608505c98681c6d310469f Mon Sep 17 00:00:00 2001
From: Markus Grotz <Markus Grotz markus.grotz@kit.edu>
Date: Wed, 31 Jul 2019 09:33:05 +0200
Subject: [PATCH] add NJointHolonomicPlatformGlobalPositionController
---
.../components/units/RobotUnit/CMakeLists.txt | 2 +
...onomicPlatformGlobalPositionController.cpp | 151 ++++++++++++++++++
...olonomicPlatformGlobalPositionController.h | 122 ++++++++++++++
.../RobotUnitModules/RobotUnitModuleUnits.cpp | 18 +++
.../units/RobotUnit/Units/PlatformSubUnit.h | 1 +
5 files changed, 294 insertions(+)
mode change 100644 => 100755 source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
create mode 100755 source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
create mode 100755 source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
old mode 100644
new mode 100755
index 8b255aed2..60ce76b94
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -43,6 +43,7 @@ set(LIB_FILES
NJointControllers/NJointKinematicUnitPassThroughController.cpp
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
+ NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
NJointControllers/NJointTCPController.cpp
NJointControllers/NJointCartesianVelocityController.cpp
NJointControllers/NJointCartesianTorqueController.cpp
@@ -111,6 +112,7 @@ set(LIB_HEADERS
NJointControllers/NJointKinematicUnitPassThroughController.h
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
NJointControllers/NJointHolonomicPlatformRelativePositionController.h
+ NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
NJointControllers/NJointTCPController.h
NJointControllers/NJointCartesianVelocityController.h
NJointControllers/NJointCartesianTorqueController.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
new file mode 100755
index 000000000..222b7f42c
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -0,0 +1,151 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 ArmarX
+ * @author Markus Grotz (markus.grotz at kit dot edu)
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "NJointHolonomicPlatformGlobalPositionController.h"
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointHolonomicPlatformGlobalPositionController>
+ registrationNJointHolonomicPlatformGlobalPositionController("NJointHolonomicPlatformGlobalPositionController");
+
+
+ NJointHolonomicPlatformGlobalPositionController::NJointHolonomicPlatformGlobalPositionController(
+ const RobotUnitPtr&,
+ const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
+ const VirtualRobot::RobotPtr&) :
+ pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration)
+
+ {
+ const SensorValueBase* sv = useSensorValue(cfg->platformName);
+ this->sv = sv->asA<SensorValueHolonomicPlatform>();
+ target = useControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
+ ARMARX_CHECK_EXPRESSION_W_HINT(target, "The actuator '" << cfg->platformName << "' has no control mode " << ControlModes::HolonomicPlatformVelocity);
+
+ pid.threadSafe = false;
+
+ oriCtrl.maxV = cfg->maxRotationVelocity;
+ oriCtrl.acceleration = cfg->maxRotationAcceleration;
+ oriCtrl.deceleration = cfg->maxRotationAcceleration;
+ oriCtrl.maxDt = 0.1;
+ oriCtrl.pid->Kp = cfg->p;
+ oriCtrl.positionPeriodLo = -M_PI;
+ oriCtrl.positionPeriodHi = M_PI;
+ pid.preallocate(2);
+
+ }
+
+ void NJointHolonomicPlatformGlobalPositionController::rtRun(const IceUtil::Time& currentTime, const IceUtil::Time& timeSinceLastIteration)
+ {
+ currentPosition << sv->relativePositionX, sv->relativePositionY;
+ currentOrientation = sv->relativePositionRotation;
+
+
+ if (rtGetControlStruct().newTargetSet)
+ {
+ pid.reset();
+ *const_cast<bool*>(&rtGetControlStruct().newTargetSet) = false;
+ }
+
+ if ((rtGetControlStruct().lastUpdate + IceUtil::Time::seconds(2)) < currentTime)
+ {
+
+ // ARMARX_RT_LOGF_WARNING << deactivateSpam(0.5) << "Waiting for global pos";
+
+ target->velocityX = 0;
+ target->velocityY = 0;
+
+ target->velocityRotation = 0;
+
+ return;
+ }
+
+ float relativeOrientation = currentOrientation - rtGetControlStruct().startOrientation;
+ Eigen::Vector2f relativeCurrentPosition = currentPosition - rtGetControlStruct().startPosition;
+
+ Eigen::Vector2f updatedPosition = rtGetControlStruct().globalPosition + relativeCurrentPosition;
+ float updatedOrientation = rtGetControlStruct().globalOrientation + relativeOrientation;
+ updatedOrientation = std::atan2(std::sin(updatedOrientation), std::cos(updatedOrientation));
+
+ pid.update(timeSinceLastIteration.toSecondsDouble(), updatedPosition, rtGetControlStruct().target);
+
+ oriCtrl.dt = timeSinceLastIteration.toSecondsDouble();
+ oriCtrl.accuracy = rtGetControlStruct().rotationAccuracy;
+ oriCtrl.currentPosition = updatedOrientation;
+ oriCtrl.targetPosition = getWriterControlStruct().targetOrientation;
+ oriCtrl.currentV = sv->velocityRotation;
+
+ Eigen::Vector2f localTargetVelocity = Eigen::Rotation2Df(-updatedOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
+ target->velocityX = localTargetVelocity(0);
+ target->velocityY = localTargetVelocity(1);
+ target->velocityRotation = oriCtrl.run();
+
+ Eigen::Vector2f posError = pid.target.head(2) - pid.processValue.head(2);
+ if (posError.norm() < rtGetControlStruct().translationAccuracy)
+ {
+ // target->velocityX = 0;
+ // target->velocityY = 0;
+ }
+
+ float orientationError = oriCtrl.currentPosition - oriCtrl.targetPosition;
+ orientationError = std::atan2(std::sin(orientationError), std::cos(orientationError));
+ if (std::fabs(orientationError) < rtGetControlStruct().rotationAccuracy)
+ {
+ //target->velocityRotation = 0;
+ }
+ }
+
+ void NJointHolonomicPlatformGlobalPositionController::rtPreActivateController()
+ {
+ }
+
+ void NJointHolonomicPlatformGlobalPositionController::setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy)
+ {
+ // todo do we really need a recursive mutex?
+
+ std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
+
+ getWriterControlStruct().target << x, y;
+ getWriterControlStruct().targetOrientation = std::atan2(std::sin(yaw), std::cos(yaw));
+ getWriterControlStruct().translationAccuracy = translationAccuracy;
+ getWriterControlStruct().rotationAccuracy = rotationAccuracy;
+ getWriterControlStruct().newTargetSet = true;
+ writeControlStruct();
+ }
+
+
+ void NJointHolonomicPlatformGlobalPositionController::setGlobalPos(float x, float y, float yaw, const IceUtil::Time& time = ::IceUtil::Time::now())
+ {
+ // ..todo:: lock :)
+
+ getWriterControlStruct().globalPosition << x, y;
+ getWriterControlStruct().globalOrientation = yaw;
+
+ getWriterControlStruct().startPosition = currentPosition;
+ getWriterControlStruct().startOrientation = currentOrientation;
+
+ getWriterControlStruct().lastUpdate = time;
+ writeControlStruct();
+ }
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
new file mode 100755
index 000000000..b151f3802
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.h
@@ -0,0 +1,122 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * 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 ArmarX
+ * @author Markus Grotz (markus.grotz at kit dot edu)
+ * @date 2019
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#pragma once
+
+
+#include <atomic>
+
+#include <VirtualRobot/Robot.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "NJointController.h"
+#include "../RobotUnit.h"
+#include "../SensorValues/SensorValueHolonomicPlatform.h"
+
+#include "../ControlTargets/ControlTarget1DoFActuator.h"
+#include <RobotAPI/libraries/core/PIDController.h>
+
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h>
+
+#include <RobotAPI/components/units/RobotUnit/BasicControllers.h>
+namespace armarx
+{
+
+ TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformGlobalPositionControllerConfig);
+ class NJointHolonomicPlatformGlobalPositionControllerConfig : virtual public NJointControllerConfig
+ {
+ public:
+ std::string platformName;
+ float p = 1.0f;
+ float i = 0.0f;
+ float d = 0.0f;
+ float maxVelocity = 300;
+ float maxAcceleration = 500;
+ float maxRotationVelocity = 0.5;
+ float maxRotationAcceleration = 0.5;
+ };
+
+ struct NJointHolonomicPlatformGlobalPositionControllerTarget
+ {
+ Eigen::Vector2f target; // x,y
+ float targetOrientation;
+ float translationAccuracy = 0.0f;
+ float rotationAccuracy = 0.0f;
+ bool newTargetSet = false;
+
+
+ Eigen::Vector2f startPosition;
+ float startOrientation;
+
+ Eigen::Vector2f globalPosition;
+ float globalOrientation;
+ IceUtil::Time lastUpdate = IceUtil::Time::seconds(0);
+ };
+
+ TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformGlobalPositionController);
+
+ /**
+ * @brief The NJointHolonomicPlatformGlobalPositionController class
+ * @ingroup Library-RobotUnit-NJointControllers
+ */
+ class NJointHolonomicPlatformGlobalPositionController:
+ virtual public NJointControllerWithTripleBuffer<NJointHolonomicPlatformGlobalPositionControllerTarget>
+ {
+ public:
+ using ConfigPtrT = NJointHolonomicPlatformGlobalPositionControllerConfigPtr;
+
+ inline NJointHolonomicPlatformGlobalPositionController(
+ const RobotUnitPtr& robotUnit,
+ const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
+ const VirtualRobot::RobotPtr&);
+
+ inline virtual void rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration) override;
+ inline virtual void rtPreActivateController() override;
+
+ //ice interface
+ inline virtual std::string getClassName(const Ice::Current& = Ice::emptyCurrent) const override
+ {
+ return "NJointHolonomicPlatformGlobalPositionController";
+ }
+
+ void setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
+
+ void setGlobalPos(float x, float y, float yaw, const IceUtil::Time&);
+
+
+ protected:
+ const SensorValueHolonomicPlatform* sv;
+ MultiDimPIDController pid;
+
+ PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition oriCtrl;
+ ControlTargetHolonomicPlatformVelocity* target;
+
+
+ Eigen::Vector2f currentPosition;
+ float currentOrientation;
+
+ };
+} // namespace armarx
+
+
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 59a461de6..27c760de9 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -354,6 +354,24 @@ namespace armarx
unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
//add
addUnit(std::move(unit));
+
+
+ NJointHolonomicPlatformGlobalPositionControllerConfigPtr configGlobalPositionCtrlCfg = new NJointHolonomicPlatformGlobalPositionControllerConfig;
+ configGlobalPositionCtrlCfg->platformName = _module<RobotData>().getRobotPlatformName();
+ auto ctrlGlobalPosition = NJointHolonomicPlatformGlobalPositionControllerPtr::dynamicCast(
+ _module<ControllerManagement>().createNJointController(
+ "NJointHolonomicPlatformGlobalPositionController",
+ getName() + "_" + configName + "_GlobalPositionContoller",
+ configGlobalPositionCtrlCfg, false, true
+ )
+ );
+ ARMARX_CHECK_EXPRESSION(ctrlGlobalPosition);
+ unit->pt = ctrl;
+ unit->globalPosCtrl = ctrlGlobalPosition;
+
+ unit->platformSensorIndex = _module<Devices>().getSensorDevices().index(_module<RobotData>().getRobotPlatformName());
+ //add
+ addUnit(std::move(unit));
}
void Units::initializeForceTorqueUnit()
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
index bac5190ed..6184af471 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -66,6 +66,7 @@ namespace armarx
NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr pt;
NJointHolonomicPlatformRelativePositionControllerPtr relativePosCtrl;
+ NJointHolonomicPlatformGlobalPositionControllerPtr globalPosCtrl;
std::size_t platformSensorIndex;
protected:
Eigen::Matrix4f getRelativePose() const;
--
GitLab