diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index 8184ff47598b96e8379eddfb239be3f3d1cc0739..6e53ccdb99a14be49b7325e9a17269447cc6a986 100644
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -49,6 +49,7 @@ set(LIB_FILES
NJointControllers/NJointTrajectoryController.cpp
NJointControllers/NJointKinematicUnitPassThroughController.cpp
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.cpp
+ NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
NJointControllers/NJointTCPController.cpp
NJointControllers/NJointCartesianVelocityController.cpp
NJointControllers/NJointCartesianTorqueController.cpp
@@ -109,6 +110,7 @@ set(LIB_HEADERS
NJointControllers/NJointTrajectoryController.h
NJointControllers/NJointKinematicUnitPassThroughController.h
NJointControllers/NJointHolonomicPlatformUnitVelocityPassThroughController.h
+ NJointControllers/NJointHolonomicPlatformRelativePositionController.h
NJointControllers/NJointTCPController.h
NJointControllers/NJointCartesianVelocityController.h
NJointControllers/NJointCartesianTorqueController.h
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
index 63ea69e0d6780409e3d2d9e7b875320df55097b2..bf0e105ee633abad0817a6a1375d1f99f1117b8a 100644
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointCartesianVelocityControllerWithRamp.cpp
@@ -60,10 +60,12 @@ namespace armarx
ARMARX_CHECK_EXPRESSION(filteredVelocitySensor || velocitySensor);
if (filteredVelocitySensor)
{
+ ARMARX_IMPORTANT << "Using filtered velocity for joint " << jointName;
velocitySensors.push_back(&filteredVelocitySensor->filteredvelocity);
}
else if (velocitySensor)
{
+ ARMARX_IMPORTANT << "Using raw velocity for joint " << jointName;
velocitySensors.push_back(&velocitySensor->velocity);
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..06c0445d5d12ddcf32246fcf0c0e7f64ef88fdf8
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.cpp
@@ -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 Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+#include "NJointHolonomicPlatformRelativePositionController.h"
+
+namespace armarx
+{
+ NJointControllerRegistration<NJointHolonomicPlatformRelativePositionController>
+ registrationNJointHolonomicPlatformRelativePositionController("NJointHolonomicPlatformRelativePositionController");
+
+
+ NJointHolonomicPlatformRelativePositionController::NJointHolonomicPlatformRelativePositionController(
+ NJointControllerDescriptionProviderInterfacePtr prov,
+ const NJointHolonomicPlatformRelativePositionControllerConfigPtr& cfg,
+ const VirtualRobot::RobotPtr&) :
+ pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration)
+ {
+ const SensorValueBase* sv = prov->getSensorValue(cfg->platformName);
+ this->sv = sv->asA<SensorValueHolonomicPlatform>();
+ target = prov->getControlTarget(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.p = cfg->p;
+ oriCtrl.positionPeriodLo = -M_PI;
+ oriCtrl.positionPeriodHi = M_PI;
+ }
+
+ void NJointHolonomicPlatformRelativePositionController::rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration)
+ {
+ currentPose << sv->relativePositionX,
+ sv->relativePositionY;
+ if (rtGetControlStruct().newTargetSet)
+ {
+ startPose = currentPose;
+ orientationOffset = sv->relativePositionRotation;
+ pid.reset();
+ *const_cast<bool*>(&rtGetControlStruct().newTargetSet) = false;
+ }
+
+ //position pid
+ Eigen::Vector2f relativeCurrentPose = currentPose - startPose;
+ pid.update(timeSinceLastIteration.toSecondsDouble(), relativeCurrentPose, rtGetControlStruct().target);
+
+ //rotation pid
+ // Revert the rotation by rotating by the negative angle
+ Eigen::Vector2f localTargetVelocity = Eigen::Rotation2Df(-sv->relativePositionRotation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]);
+ //ARMARX_RT_LOGF_INFO("global target vel x: %.2f y: %2.f, local target vel x: %.2f y: %2.f rotation: %2.f", pid.getControlValue()[0], pid.getControlValue()[1], localTargetVelocity[0], localTargetVelocity[1], sv->relativePositionRotation).deactivateSpam(0.1);
+
+ target->velocityX = localTargetVelocity[0];
+ target->velocityY = localTargetVelocity[1];
+ // target->velocityRotation = pid.getControlValue()[2] / rad2MMFactor;
+ oriCtrl.dt = timeSinceLastIteration.toSecondsDouble();
+ oriCtrl.accuracy = rtGetControlStruct().rotationAccuracy;
+ oriCtrl.currentPosition = sv->relativePositionRotation - orientationOffset;
+ oriCtrl.targetPosition = rtGetControlStruct().targetOrientation;
+ oriCtrl.currentV = sv->velocityRotation;
+ 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 = std::abs(oriCtrl.currentPosition - oriCtrl.targetPosition);
+ // if (orientationError < rtGetControlStruct().rotationAccuracy)
+ // {
+ // target->velocityRotation = 0;
+ // }
+ // ARMARX_INFO << deactivateSpam(0.1) << VAROUT(oriCtrl.currentPosition) << VAROUT(orientationError);
+ // ARMARX_INFO << deactivateSpam(0.1) << VAROUT(target->velocityRotation) << VAROUT(sv->velocityRotation);
+ ARMARX_RT_LOGF_INFO("current pose x: %.2f y: %2.f, error x: %.2f y: %2.f error: %2.f ori error: %2.f", currentPose[0], currentPose[1], posError[0], posError[1], posError.norm(), orientationError).deactivateSpam(0.1);
+ ARMARX_RT_LOGF_INFO("new target vel: %2.f, %2.f current vel: %2.f, %2.f", target->velocityX, target->velocityY, sv->velocityX, sv->velocityY).deactivateSpam(0.1);
+ }
+
+ void NJointHolonomicPlatformRelativePositionController::rtPreActivateController()
+ {
+ startPose[0] = sv->relativePositionX;
+ startPose[1] = sv->relativePositionY;
+ orientationOffset = sv->relativePositionRotation;
+ }
+
+ void NJointHolonomicPlatformRelativePositionController::setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy)
+ {
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().target << x, y;
+ getWriterControlStruct().targetOrientation = yaw;
+ getWriterControlStruct().translationAccuracy = translationAccuracy;
+ getWriterControlStruct().rotationAccuracy = rotationAccuracy;
+ getWriterControlStruct().newTargetSet = true;
+ writeControlStruct();
+ // rtUpdateControlStruct();
+ }
+
+
+} // namespace armarx
+
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
new file mode 100644
index 0000000000000000000000000000000000000000..a5efe0f5d5b2283f1b02bfa18be2594520849cc9
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h
@@ -0,0 +1,108 @@
+/*
+ * 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 Mirko Waechter( mirko.waechter at kit dot edu)
+ * @date 2017
+ * @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(NJointHolonomicPlatformRelativePositionControllerConfig);
+ class NJointHolonomicPlatformRelativePositionControllerConfig : virtual public NJointControllerConfig
+ {
+ public:
+ std::string platformName;
+ float p = 0.5f;
+ float i = 0.0f;
+ float d = 0.0f;
+ float maxVelocity = 300;
+ float maxAcceleration = 100;
+ float maxRotationVelocity = 0.2;
+ float maxRotationAcceleration = 0.1;
+ // float rad2MMFactor = 50.0f;
+ };
+
+ struct NJointHolonomicPlatformRelativePositionControllerTarget
+ {
+ Eigen::Vector2f target; // x,y
+ float targetOrientation;
+ float translationAccuracy = 0.0f;
+ float rotationAccuracy = 0.0f;
+ bool newTargetSet = false;
+ };
+
+ TYPEDEF_PTRS_HANDLE(NJointHolonomicPlatformRelativePositionController);
+ class NJointHolonomicPlatformRelativePositionController:
+ virtual public NJointControllerWithTripleBuffer<NJointHolonomicPlatformRelativePositionControllerTarget>
+ {
+ public:
+ using ConfigPtrT = NJointHolonomicPlatformRelativePositionControllerConfigPtr;
+
+ inline NJointHolonomicPlatformRelativePositionController(
+ NJointControllerDescriptionProviderInterfacePtr prov,
+ const NJointHolonomicPlatformRelativePositionControllerConfigPtr& 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& = GlobalIceCurrent) const override
+ {
+ return "NJointHolonomicPlatformRelativePositionController";
+ }
+
+ void setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
+
+
+ protected:
+ const SensorValueHolonomicPlatform* sv;
+ MultiDimPIDController pid;
+ PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition oriCtrl;
+ ControlTargetHolonomicPlatformVelocity* target;
+ Eigen::Vector2f startPose, currentPose;
+ float orientationOffset;
+ // float rad2MMFactor;
+
+ virtual void onInitComponent() override {}
+ virtual void onConnectComponent() override {}
+ };
+} // namespace armarx
+
+
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
index 7eff0cb03a82764463523f7ddc66245386d2ce92..5679160c7e1e8035e2bc0804ae5c86a95d008a5e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -563,6 +563,19 @@ namespace armarx
);
ARMARX_CHECK_EXPRESSION(ctrl);
unit->pt = ctrl;
+
+ NJointHolonomicPlatformRelativePositionControllerConfigPtr configRelativePositionCtrlCfg = new NJointHolonomicPlatformRelativePositionControllerConfig;
+ configRelativePositionCtrlCfg->platformName = robotPlatformName;
+ auto ctrlRelativePosition = NJointHolonomicPlatformRelativePositionControllerPtr::dynamicCast(
+ createNJointController(
+ "NJointHolonomicPlatformRelativePositionController", configName + "_RelativePositionContoller",
+ configRelativePositionCtrlCfg, false, true
+ )
+ );
+ ARMARX_CHECK_EXPRESSION(ctrlRelativePosition);
+ unit->pt = ctrl;
+ unit->relativePosCtrl = ctrlRelativePosition;
+
unit->platformSensorIndex = sensorDevices.index(robotPlatformName);
//add
addUnit(std::move(unit));
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
index 18b50b0cda5ee8abd405e2aad3efe55f47c4ff5a..ac4f64be14afb2f0cde6117575622e90a7e3a1b2 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
@@ -90,7 +90,15 @@ void armarx::PlatformSubUnit::moveTo(Ice::Float rx, Ice::Float ry, Ice::Float rr
void armarx::PlatformSubUnit::moveRelative(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current&)
{
- ARMARX_WARNING << "NYI";
+ if (!relativePosCtrl->isControllerActive())
+ {
+ relativePosCtrl->activateController();
+ }
+ //holding the mutex here could deadlock
+ // std::lock_guard<std::mutex> guard {dataMutex};
+ ARMARX_INFO << "target orientation: " << rr;
+ relativePosCtrl->setTarget(rx, ry, rr, lac, rac);
+
}
void armarx::PlatformSubUnit::setMaxVelocities(Ice::Float mxVLin, Ice::Float mxVAng, const Ice::Current&)
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
index 1b75e30f38acdf2ffd7b67b3ebe3bb32454ba6d4..a9df471978bcd44b1f776c498b421ae35a862204 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -31,6 +31,7 @@
#include <VirtualRobot/MathTools.h>
#include <RobotAPI/components/units/PlatformUnit.h>
+#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformRelativePositionController.h>
#include <RobotAPI/libraries/core/Pose.h>
#include "RobotUnitSubUnit.h"
@@ -64,6 +65,7 @@ namespace armarx
void stopPlatform(const Ice::Current& c = GlobalIceCurrent) override;
NJointHolonomicPlatformUnitVelocityPassThroughControllerPtr pt;
+ NJointHolonomicPlatformRelativePositionControllerPtr relativePosCtrl;
std::size_t platformSensorIndex;
protected:
Eigen::Matrix4f getRelativePose() const;
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
index e6496ba57fc40c6990aa4d5968f4ada6e99e73d2..03856ed0ce415286ceda7c94747c08d28a2d4dd7 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointBimanualCCDMPController.cpp
@@ -22,7 +22,7 @@ namespace armarx
if (leftRNS->getNode(i)->isLimitless())
{
- leftNullSpaceCoefs(i) = 0;
+ leftNullSpaceCoefs(i) = 1;
}
leftJointNames.push_back(jointName);
@@ -63,7 +63,7 @@ namespace armarx
if (rightRNS->getNode(i)->isLimitless())
{
- rightNullSpaceCoefs(i) = 0;
+ rightNullSpaceCoefs(i) = 1;
}
rightJointNames.push_back(jointName);