diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
index eaafd8ecaef369e3126bd3b548a8d21378f3ff1b..a664245ad4bdec2c570c6442fe269bcc4c0b03c7 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.cpp
@@ -18,9 +18,9 @@ OrientedTactileSensorUnit::OrientedTactileSensorUnit()
void OrientedTactileSensorUnit::onInitComponent()
{
- maxSamplesRotation = stoi(getProperty<std::string>("SamplesRotation").getValue());
- maxSamplesPressure = stoi(getProperty<std::string>("SamplesPressure").getValue());
- maxSamplesAcceleration = stoi(getProperty<std::string>("SamplesAcceleration").getValue());
+ maxSamplesRotation = getProperty<std::size_t>("SamplesRotation").getValue();
+ maxSamplesPressure = getProperty<std::size_t>("SamplesPressure").getValue();
+ maxSamplesAcceleration = getProperty<std::size_t>("SamplesAcceleration").getValue();
std::string topicName = getProperty<std::string>("TopicName").getValue();
offeringTopic(topicName);
diff --git a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
index 38aade6a48c0be4e9742ad789dcaf05413410f53..c518ac3f186d7e0662a4a7d527beb0cb4853abbf 100644
--- a/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
+++ b/source/RobotAPI/drivers/OrientedTactileSensor/OrientedTactileSensorUnit.h
@@ -36,19 +36,19 @@ namespace armarx
"65524 3 12 65534 65534 1 1208 119 58726 1000 943 ",
"Sensor Register Data to calibrate the sensor");
- defineOptionalProperty<std::string>(
+ defineOptionalProperty<std::size_t>(
"SamplesRotation",
- "20",
+ 20,
"number of orientation values to differentiate");
- defineOptionalProperty<std::string>(
+ defineOptionalProperty<std::size_t>(
"SamplesPressure",
- "10",
+ 10,
"number of pressure values to differentiate");
- defineOptionalProperty<std::string>(
+ defineOptionalProperty<std::size_t>(
"SamplesAcceleration",
- "20",
+ 20,
"number of pressure values to differentiate");
defineOptionalProperty<bool>(
@@ -134,12 +134,12 @@ namespace armarx
std::vector<RotationRate> samplesRotation;
std::vector<PressureRate> samplesPressure;
std::vector<AccelerationRate> samplesAcceleration;
- int maxSamplesRotation;
- int sampleIndexRotation;
- int maxSamplesPressure;
- int sampleIndexPressure;
- int maxSamplesAcceleration;
- int sampleIndexAcceleration;
+ std::size_t maxSamplesRotation;
+ std::size_t sampleIndexRotation;
+ std::size_t maxSamplesPressure;
+ std::size_t sampleIndexPressure;
+ std::size_t maxSamplesAcceleration;
+ std::size_t sampleIndexAcceleration;
};
}
#endif // SENSORPACKAGEUNIT_H
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 2d73095d3fabf805e32b47c6fe122e73dd1e6d71..d5923006c35cafbd85f20f96e9a28ab000fa74e5 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -43,7 +43,9 @@ set(SLICE_FILES
visualization/DebugDrawerInterface.ice
libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.ice
- libraries/RTRobotUnit/LVL1Controllers/PassThroughController.ice
+ libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.ice
+ libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitControllerConfigs.ice
+ libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughControllerConfig.ice
libraries/RTRobotUnit/RobotUnitInterface.ice
)
#core/RobotIK.ice
diff --git a/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughControllerConfig.ice b/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughControllerConfig.ice
new file mode 100644
index 0000000000000000000000000000000000000000..0ad85653867367bd3195f181d8ea75f20d7bea00
--- /dev/null
+++ b/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughControllerConfig.ice
@@ -0,0 +1,39 @@
+/*
+ * 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::LVL1KinematicUnitControllerConfigs
+ * @author Raphael Grimm ( raphael dot grimm at student dot kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_LVL1KinematicUnitControllerConfigs_SLICE_
+#define _ARMARX_ROBOTAPI_LVL1KinematicUnitControllerConfigs_SLICE_
+
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.ice>
+
+module armarx
+{
+ class LVL1HolonomicPlatformVelocityPassThroughControllerConfig extends LVL1ControllerConfig
+ {
+ string platformName;
+ float initialVelocityX;
+ float initialVelocityY;
+ float initialVelocityRotation;
+ };
+};
+#endif
diff --git a/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitControllerConfigs.ice b/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitControllerConfigs.ice
new file mode 100644
index 0000000000000000000000000000000000000000..cd0b074c863ce35d7ef68a831dd596fc661a38f3
--- /dev/null
+++ b/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitControllerConfigs.ice
@@ -0,0 +1,71 @@
+/*
+ * 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::LVL1KinematicUnitControllerConfigs
+ * @author Raphael Grimm ( raphael dot grimm at student dot kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_ROBOTAPI_LVL1KinematicUnitControllerConfigs_SLICE_
+#define _ARMARX_ROBOTAPI_LVL1KinematicUnitControllerConfigs_SLICE_
+
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.ice>
+
+module armarx
+{
+ class LVL1KinematicUnitVelocityControllerConfig extends LVL1ControllerConfig
+ {
+ string jointName;
+
+ float maxVelocity;
+ float acceleration;
+ float deceleration;
+ float maxDt;
+ float directSetLimit;
+
+ float limitPositionLo;
+ float limitPositionHi;
+
+ float initialVelocityTargetValue;
+ };
+
+ class LVL1KinematicUnitTorqueControllerConfig extends LVL1ControllerConfig
+ {
+ string jointName;
+ float maxTorque;
+ float initialTorqueTargetValue;
+ };
+
+ class LVL1KinematicUnitPositionControllerConfig extends LVL1ControllerConfig
+ {
+ string jointName;
+
+ float maxVelocity;
+ float acceleration;
+ float deceleration;
+ float maxDt;
+ float directPControlLimit;
+ float p;
+
+ float limitPositionLo;
+ float limitPositionHi;
+
+ float initialPositionTargetValue;
+ };
+};
+#endif
diff --git a/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/PassThroughController.ice b/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.ice
similarity index 79%
rename from source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/PassThroughController.ice
rename to source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.ice
index a5c5fde934df1c85fb5339c85bafadfb6b5610e2..e997918d3224c208a16d66cd5c38fa81a9f2e6fb 100644
--- a/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/PassThroughController.ice
+++ b/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.ice
@@ -13,27 +13,27 @@
* 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::PassThroughController
+ * @package RobotAPI::LVL1PassThroughController
* @author Raphael Grimm ( raphael dot grimm at student dot kit dot edu )
* @date 2017
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#ifndef _ARMARX_ROBOTAPI_PassThroughController_SLICE_
-#define _ARMARX_ROBOTAPI_PassThroughController_SLICE_
+#ifndef _ARMARX_ROBOTAPI_LVL1PassThroughController_SLICE_
+#define _ARMARX_ROBOTAPI_LVL1PassThroughController_SLICE_
#include <ArmarXCore/interface/core/BasicTypes.ice>
#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.ice>
module armarx
{
- class PassThroughControllerConfig extends LVL1ControllerConfig
+ class LVL1PassThroughControllerConfig extends LVL1ControllerConfig
{
Ice::StringSeq jointNames;
};
- interface PassThroughControllerInterface extends LVL1ControllerInterface
+ interface LVL1PassThroughControllerInterface extends LVL1ControllerInterface
{
idempotent void setJoint(string joint, float value) throws InvalidArgumentException;
idempotent void setJoints(StringFloatDictionary values) throws InvalidArgumentException;
diff --git a/source/RobotAPI/libraries/RTRobotUnit/BasicControllers.cpp b/source/RobotAPI/libraries/RTRobotUnit/BasicControllers.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9be5c8bd3aebbb90d7aa637fe407e639f365dff1
--- /dev/null
+++ b/source/RobotAPI/libraries/RTRobotUnit/BasicControllers.cpp
@@ -0,0 +1,310 @@
+/*
+ * 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
+ * @author Raphael ( ufdrv at student dot kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "BasicControllers.h"
+
+#include <boost/algorithm/clamp.hpp>
+
+#include <ArmarXCore/core/util/algorithm.h>
+
+///REMOVE
+#include <iostream>
+
+namespace armarx
+{
+ float velocityControlWithAccelerationBounds(
+ float dt, float maxDt,
+ const float currentV, float targetV, float maxV,
+ float acceleration, float deceleration,
+ float directSetVLimit
+ )
+ {
+ dt = std::min(std::abs(dt), std::abs(maxDt));
+ maxV = std::abs(maxV);
+ acceleration = std::abs(acceleration);
+ deceleration = std::abs(deceleration);
+ targetV = boost::algorithm::clamp(targetV, -maxV, maxV);
+
+ //we can have 3 cases:
+ // 1. we directly set v and ignore acc/dec (if |curr - target| <= limit)
+ // 2. we need to accelerate (if curr and target v have same sign and |curr| < |target|)
+ // 3. we need to decellerate (other cases)
+
+ //handle case 1
+ const float curverror=targetV - currentV;
+ if (std::abs(curverror) < directSetVLimit)
+ {
+ return targetV;
+ }
+
+ //handle case 2 + 3
+ const bool accelerate = sign(targetV) == sign(currentV) && // v in same direction
+ std::abs(targetV) > std::abs(currentV); // currently to slow
+
+ const float usedacc = accelerate? acceleration:-deceleration;
+ const float maxDeltaV = std::abs(usedacc * dt);
+ if(maxDeltaV >= std::abs(curverror))
+ {
+ //we change our v too much with the used acceleration
+ //but we could reach our target with a lower acceleration ->set the target
+
+ return targetV;
+ }
+ const float deltaVel = boost::algorithm::clamp(sign(currentV) * usedacc * dt, -maxDeltaV, maxDeltaV);
+ const float nextV = currentV + deltaVel;
+ return nextV;
+ }
+
+////////////////////////////
+//wip?
+ float velocityControlWithAccelerationAndPositionBounds(
+ float dt, float maxDt,
+ float currentV, float targetV, float maxV,
+ float acceleration, float deceleration,
+ float directSetVLimit,
+ float currentPosition,
+ float positionLimitLoSoft, float positionLimitHiSoft,
+ float positionSoftLimitViolationVelocity,
+ float positionLimitLoHard, float positionLimitHiHard)
+ {
+ if(currentPosition <= positionLimitLoHard || currentPosition >= positionLimitHiHard)
+ {
+ return std::nanf("1");
+ }
+
+ float softLimitViolation =0;
+ if (currentPosition <= positionLimitLoSoft)
+ {
+ softLimitViolation = -1;
+ }
+ if (currentPosition >= positionLimitHiSoft)
+ {
+ softLimitViolation = 1;
+ }
+
+ const float upperDt = std::max(std::abs(dt), std::abs(maxDt));
+ dt = std::min(std::abs(dt), std::abs(maxDt));
+ maxV = std::abs(maxV);
+
+ //we can have 4 cases:
+ // 1. we need to decelerate now or we will violate the position limits (maybe we still will violate them, e.g. if we violated them initially)
+ // 2. we directly set v and ignore acc/dec (if |curr - target| <= limit)
+ // 3. we need to accelerate (if curr and target v have same sign and |curr| < |target|)
+ // 4. we need to decellerate (other cases)
+ float nextv;
+ //handle case 1
+ const float vsquared = currentV * currentV;
+ const float brakingDist = sign(currentV) * vsquared / 2.f / std::abs(deceleration); //the braking distance points in the direction of the velocity
+
+
+ const float posIfBrakingNow = currentPosition + brakingDist;
+ if (posIfBrakingNow <= positionLimitLoSoft || posIfBrakingNow >= positionLimitHiSoft)
+ {
+ //case 1. -> brake now! (we try to have v=0 at the limit)
+ const auto limit = posIfBrakingNow <= positionLimitLoSoft ? positionLimitLoSoft : positionLimitHiSoft;
+ const float wayToGo = limit - currentPosition;
+
+ //decellerate!
+ // s = v²/(2a) <=> a = v²/(2s)
+ const float dec = std::abs(vsquared / 2.f / wayToGo);
+ const float vel = currentV - sign(currentV) * dec * upperDt;
+ nextv = boost::algorithm::clamp(vel,-maxV,maxV);
+ if(sign(currentV) != sign(nextv))
+ {
+ //stop now
+ nextv = 0;
+ }
+ }
+ else
+ {
+ //handle 2-3
+ nextv = velocityControlWithAccelerationBounds(dt, maxDt, currentV, targetV, maxV, acceleration, deceleration, directSetVLimit);
+ }
+ if(softLimitViolation == sign(nextv))
+ {
+ //the area between soft and hard limits is sticky
+ //the controller can only move out of it (not further in)
+ return 0;
+ }
+
+ //the next velocity will not violate the pos bounds harder than they are already
+ return nextv;
+ }
+
+
+ float positionThroughVelocityControlWithAccelerationBounds(
+ float dt, float maxDt,
+ float currentV, float maxV,
+ float acceleration, float deceleration,
+ float currentPosition, float targetPosition,
+ float pControlPosErrorLimit, float pControlVelLimit, float p
+ )
+ {
+ dt = std::min(std::abs(dt), std::abs(maxDt));
+ maxV = std::abs(maxV);
+ acceleration = std::abs(acceleration);
+ deceleration = std::abs(deceleration);
+ pControlPosErrorLimit = std::abs(pControlPosErrorLimit);
+ pControlVelLimit = std::abs(pControlVelLimit);
+ const float signV = sign(currentV);
+ //we can have 3 cases:
+ // 1. we use a p controller and ignore acc/dec (if |currentPosition - targetPosition| <= pControlPosErrorLimit AND |currentV| < pControlVelLimit)
+ // 2. we need to accelerate (or hold vel) (if e = (targetPosition - currentPosition)
+ // the brakingDistance have the same sign and brakingDistance < e
+ // and currentVel <= maxV)
+ // 3. we need to decellerate (other cases)
+
+ //handle case 1
+ const float positionError = targetPosition - currentPosition;
+ if (std::abs(positionError) < pControlPosErrorLimit && std::abs(currentV) < pControlVelLimit)
+ {
+ return positionError * p;
+ }
+
+ //handle case 2-3
+ const float brakingDistance = signV * currentV * currentV / 2.f / deceleration; //the braking distance points in the direction of the velocity
+ const float posIfBrakingNow = currentPosition + brakingDistance;
+ const float posErrorIfBrakingNow = targetPosition - posIfBrakingNow;
+
+ const bool decelerate =
+ std::abs(currentV) > maxV || // we need to slow down (to stay in [-maxV,maxV]
+ std::abs(posIfBrakingNow-targetPosition) <= pControlPosErrorLimit || // we want to hit the target
+ sign(posErrorIfBrakingNow) != signV; // we are moving away from the target
+ const float usedacc= decelerate ? -deceleration : acceleration;
+ const float maxDeltaV = std::abs(usedacc * dt);
+ const float deltaVel = boost::algorithm::clamp(signV * usedacc * dt, -maxDeltaV, maxDeltaV);
+ return boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
+ }
+
+ float positionThroughVelocityControlWithAccelerationAndPositionBounds(
+ float dt, float maxDt,
+ float currentV, float maxV,
+ float acceleration, float deceleration,
+ float currentPosition, float targetPosition,
+ float pControlPosErrorLimit, float pControlVelLimit, float p,
+ float positionLimitLo, float positionLimitHi
+
+ )
+ {
+ dt = std::min(std::abs(dt), std::abs(maxDt));
+ maxV = std::abs(maxV);
+ acceleration = std::abs(acceleration);
+ deceleration = std::abs(deceleration);
+ pControlPosErrorLimit = std::abs(pControlPosErrorLimit);
+ const float signV = sign(currentV);
+ //we can have 4 cases:
+ // 1. we need to decelerate now or we will violate the position limits (maybe we still will violate them)
+ // 2. we use a p controller and ignore acc/dec (if |currentPosition - targetPosition| <= pControlPosErrorLimit AND |currentPosition - targetPosition|*p < maxV)
+ // 4. we need to accelerate (or hold vel) (if e = (targetPosition - currentPosition)
+ // and the brakingDistance have the same sign and brakingDistance < e
+ // and currentVel <= maxV)
+ // 5. we need to decellerate (other cases)
+
+ //handle case 1
+ const float vsquared = currentV * currentV;
+ const float brakingDistance = signV * vsquared / 2.f / deceleration; //the braking distance points in the direction of the velocity
+ const float posIfBrakingNow = currentPosition + brakingDistance;
+ if (posIfBrakingNow <= positionLimitLo || posIfBrakingNow >= positionLimitHi)
+ {
+ //case 1. -> brake now! (we try to have v=0 at the limit)
+ const auto limit = brakingDistance > 0 ? positionLimitHi : positionLimitLo;
+ const float wayToGo = std::abs(limit - currentPosition);
+ // s = v²/(2a) <=> a = v²/(2s)
+ const float dec = std::abs(vsquared / 2.f / wayToGo);
+ const float vel = currentV - signV * dec * dt;
+ return vel;
+ }
+
+ //handle case 2-3
+ return positionThroughVelocityControlWithAccelerationBounds(
+ dt, maxDt,
+ currentV, maxV,
+ acceleration, deceleration,
+ currentPosition, boost::algorithm::clamp(targetPosition, positionLimitLo, positionLimitHi),
+ pControlPosErrorLimit, pControlVelLimit, p
+ );
+ }
+
+ ////////////////////////////
+ //wip
+
+
+ float positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition(
+ float dt, float maxDt,
+ float currentV, float maxV,
+ float acceleration, float deceleration,
+ float currentPosition, float targetPosition,
+ float pControlPosErrorLimit, float pControlVelLimit, float p,
+ float &direction,
+ float positionPeriodLo, float positionPeriodHi)
+ {
+ currentPosition = periodicClamp(currentPosition, positionPeriodLo, positionPeriodHi);
+ targetPosition = periodicClamp(targetPosition , positionPeriodLo, positionPeriodHi);
+
+ const float brakingDist = brakingDistance(currentV, deceleration);
+ const float posIfBrakingNow = currentPosition + brakingDist;
+ const float posIfBrakingNowError = targetPosition - posIfBrakingNow;
+ const float posError = targetPosition - currentPosition;
+ if(
+ std::abs(posIfBrakingNowError) <= pControlPosErrorLimit ||
+ std::abs(posError) <= pControlPosErrorLimit
+ )
+ {
+ //this allows slight overshooting (in the limits of the p controller)
+ return positionThroughVelocityControlWithAccelerationBounds(
+ dt, maxDt,
+ currentV, maxV,
+ acceleration, deceleration,
+ currentPosition, targetPosition,
+ pControlPosErrorLimit, pControlVelLimit, p
+ );
+ }
+
+ //we transform the problem with periodic bounds into
+ //a problem with position bounds
+ const float positionPeriodLength = std::abs(positionPeriodHi - positionPeriodLo);
+
+ //we shift the positions such that 0 == target
+ currentPosition = periodicClamp(currentPosition - targetPosition, 0.f, positionPeriodLength);
+ //how many times will we go ovet the target if we simply bake now?
+ const float overshoot = std::trunc((currentPosition + brakingDist)/ positionPeriodLength);
+
+ if(true||direction == 0)
+ {
+ //determine the direction to go (1 == pos vel, -1 == neg vel)
+ direction = (periodicClamp(currentPosition + brakingDist, 0.f , positionPeriodLength) >= positionPeriodLength/2) ? 1: -1;
+ }
+ //shift the target away from 0
+ targetPosition = (overshoot-std::min(0.f,-direction)) * positionPeriodLength;// - direction * pControlPosErrorLimit;
+
+ //move
+ return positionThroughVelocityControlWithAccelerationBounds(
+ dt, maxDt,
+ currentV, maxV,
+ acceleration, deceleration,
+ currentPosition, targetPosition,
+ pControlPosErrorLimit, pControlVelLimit, p
+ );
+ }
+
+
+}
diff --git a/source/RobotAPI/libraries/RTRobotUnit/BasicControllers.h b/source/RobotAPI/libraries/RTRobotUnit/BasicControllers.h
new file mode 100644
index 0000000000000000000000000000000000000000..9fb5ae18389a76c6841e410e6bee1856f55c11f6
--- /dev/null
+++ b/source/RobotAPI/libraries/RTRobotUnit/BasicControllers.h
@@ -0,0 +1,116 @@
+/*
+ * 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
+ * @author Raphael ( ufdrv at student dot kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_BasicControllers_H
+#define _ARMARX_LIB_RobotAPI_BasicControllers_H
+
+#include <cmath>
+#include <ArmarXCore/core/util/algorithm.h>
+
+namespace armarx
+{
+ /**
+ * @param v0 The initial velocity.
+ * @param deceleration The deceleration.
+ * @return The braking distance given the parameters.
+ */
+ inline float brakingDistance(float v0, float deceleration)
+ {
+ return sign(v0)*std::pow(v0, 2.f) / 2.f / std::abs(deceleration);
+ }
+
+ template<class T>
+ T periodicClamp(T value, T periodLo, T periodHi)
+ {
+ float dist = periodHi - periodLo;
+ return std::fmod(value-periodLo + dist, dist) + periodLo;
+ }
+
+ float velocityControlWithAccelerationBounds(
+ float dt, float maxDt,
+ const float currentV, float targetV, float maxV,
+ float acceleration, float deceleration,
+ float directSetVLimit);
+ /**
+ * @brief Performs velocity control while staying in positional bounds, obeying acceleration / deceleration and staying below a velocity bound.
+ *
+ * we can have 4 cases:
+ * 1. we need to decelerate now or we will violate the position limits (maybe we still will violate them. e.f. if we already violate them or the robot can't brake fast enough)
+ * 2. we directly set v and ignore acc/dec (if |currentV - targetV| <= directSetVLimit)
+ * 3. we need to accelerate (if currentV and targetV have same sign and |currentV| < |currentV|)
+ * 4. we need to decellerate (other cases)
+ * @param dt The time in seconds until the next call is made. (use the time since the last call as an approximate)
+ * @param maxDt Limits dt in case the given approximation has a sudden high value.
+ * @param currentV
+ * @param targetV
+ * @param maxV
+ * @param acceleration
+ * @param deceleration
+ * @param directSetVLimit In case |currentV - targetV| <= directSetVLimit this function will return targetV (if the position bounds are not violated)
+ * @param currentPosition
+ * @param positionLimitLo
+ * @param positionLimitHi
+ * @return The next velocity.
+ */
+ float velocityControlWithAccelerationAndPositionBounds(float dt, float maxDt,
+ float currentV, float targetV, float maxV,
+ float acceleration, float deceleration,
+ float directSetVLimit,
+ float currentPosition,
+ float positionLimitLoSoft, float positionLimitHiSoft,
+ float positionSoftLimitViolationVelocity,
+ float positionLimitLoHard, float positionLimitHiHard);
+
+ float positionThroughVelocityControlWithAccelerationBounds(float dt, float maxDt,
+ float currentV, float maxV,
+ float acceleration, float deceleration,
+ float currentPosition, float targetPosition,
+ float pControlPosErrorLimit, float pControlVelLimit, float p
+ );
+
+ float positionThroughVelocityControlWithAccelerationAndPositionBounds(
+ float dt, float maxDt,
+ float currentV, float maxV,
+ float acceleration, float deceleration,
+ float currentPosition, float targetPosition,
+ float pControlPosErrorLimit, float pControlVelLimit, float p,
+ float positionLimitLo, float positionLimitHi);
+
+ // float positionThroughVelocityControlWithAccelerationBounds(
+ // float dt, float maxDt,
+ // float currentV, float maxV,
+ // float acceleration, float deceleration,
+ // float currentPosition, float targetPosition,
+ // float pControlPosErrorLimit, float p);
+
+
+
+ float positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition(float dt, float maxDt,
+ float currentV, float maxV,
+ float acceleration, float deceleration,
+ float currentPosition, float targetPosition,
+ float pControlPosErrorLimit, float pControlVelLimit, float p,
+ float& direction,
+ float positionPeriodLo, float positionPeriodHi);
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/CMakeLists.txt b/source/RobotAPI/libraries/RTRobotUnit/CMakeLists.txt
index ce16d7a27bd7f279cb1003f1ab806aec55516537..7da09b629664a6a8e89cb141fdca203a2d81d1a0 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RTRobotUnit/CMakeLists.txt
@@ -16,18 +16,23 @@ set(LIB_FILES
LVL0Controllers/LVL0Controller.cpp
LVL1Controllers/LVL1Controller.cpp
- LVL1Controllers/PassThroughController.cpp
+ LVL1Controllers/LVL1PassThroughController.cpp
+ LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.cpp
+ LVL1Controllers/LVL1KinematicUnitPositionController.cpp
+ LVL1Controllers/LVL1KinematicUnitTorqueController.cpp
+ LVL1Controllers/LVL1KinematicUnitVelocityController.cpp
RobotUnit.cpp
+ BasicControllers.cpp
)
set(LIB_HEADERS
Constants.h
ControlModes.h
Targets/TargetBase.h
- Targets/JointPositionTarget.h
- Targets/JointVelocityTarget.h
- Targets/JointTorqueTarget.h
+ Targets/ActuatorPositionTarget.h
+ Targets/ActuatorVelocityTarget.h
+ Targets/ActuatorTorqueTarget.h
Targets/PlatformWheelVelocityTarget.h
Targets/HolonomicPlatformVelocityTarget.h
@@ -40,11 +45,17 @@ set(LIB_HEADERS
LVL0Controllers/LVL0Controller.h
LVL1Controllers/LVL1Controller.h
- LVL1Controllers/PassThroughController.h
+ LVL1Controllers/LVL1PassThroughController.h
+ LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.h
+ LVL1Controllers/LVL1KinematicUnitPositionController.h
+ LVL1Controllers/LVL1KinematicUnitTorqueController.h
+ LVL1Controllers/LVL1KinematicUnitVelocityController.h
RobotUnit.h
+ BasicControllers.h
)
+add_subdirectory(test)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/RTRobotUnit/ControlModes.h b/source/RobotAPI/libraries/RTRobotUnit/ControlModes.h
index 6365d190f37d33b45c33b301e4a078036045019b..dc2e580a42bf4301aea8f1fcfc41d965d1865591 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/ControlModes.h
+++ b/source/RobotAPI/libraries/RTRobotUnit/ControlModes.h
@@ -41,5 +41,4 @@ namespace armarx
static const std::string EmergencyStopMode = "EmergencyStopMode";
}
}
-
#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL0Controllers/LVL0Controller.h b/source/RobotAPI/libraries/RTRobotUnit/LVL0Controllers/LVL0Controller.h
index 1b9f7a20e59ce21d5b36e7e6f7c875f64529e779..3fbdcea6bf3e7e7f94d2e55bceec99c991124daf 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL0Controllers/LVL0Controller.h
+++ b/source/RobotAPI/libraries/RTRobotUnit/LVL0Controllers/LVL0Controller.h
@@ -29,22 +29,26 @@
namespace armarx
{
- class LVL0ControllerBase;
- typedef std::shared_ptr<LVL0ControllerBase> LVL0ControllerBasePtr;
+ class LVL0Controller;
+ typedef std::shared_ptr<LVL0Controller> LVL0ControllerPtr;
- class LVL0ControllerBase
+ class LVL0Controller
{
public:
- virtual void run(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
- virtual TargetBase* getTarget() const = 0;
+ virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+ virtual TargetBase* getTarget() = 0;
+ virtual const TargetBase* getTarget() const
+ {
+ return const_cast<const TargetBase*>(const_cast<LVL0Controller*>(this)->getTarget());
+ }
- virtual void resetTarget()
+ virtual void rtResetTarget()
{
getTarget()->reset();
}
- virtual bool isTargetVaild() const
+ virtual bool rtIsTargetVaild() const
{
return getTarget()->isValid();
}
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.h b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.h
index dc5d6fa517e933e7a8ad1bd4eb4b735d0a60e63c..53c36a59d564b4d199e1b89c8f7a644f52485c08 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.h
+++ b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.h
@@ -243,7 +243,7 @@ namespace armarx
class SomeController : virtual public LVL1Controller<SomeControlDataStruct>,
public SomeControllerInterface
{
- JointVelocityTargetPtr targetJointXPtr;
+ ActuatorVelocityTargetPtr targetJointXPtr;
std::vector<const float*> jointValuePtrs;
float someParamSetViaConfig;
SomeController(RobotUnitPtr robotUnit, LVL1ControllerConfigPtr config):
@@ -262,11 +262,11 @@ namespace armarx
jointValuePtrs = robotUnit->getRTKinematicDataUnit()->getJointAngles({"jointX", "jointY"});
//get pointers for the results of this controller
- targetJointXPtr = dynamic_cast<JointVelocityTarget*>(robotUnit->getJointTarget("jointX", ControlModes::VelocityMode));
+ targetJointXPtr = dynamic_cast<ActuatorVelocityTarget*>(robotUnit->getJointTarget("jointX", ControlModes::VelocityMode));
ARMARX_CHECK_EXPRESSION_W_HINT(targetJointXPtr, "The Joint jointX does not support the mode " + ControlModes::VelocityMode);
}
- SomeController(JointVelocityTargetPtr targetPtr, ControllerConfigPtr config):
+ SomeController(ActuatorVelocityTargetPtr targetPtr, ControllerConfigPtr config):
targetJointXPtr{targetPtr}
{
@@ -298,6 +298,9 @@ namespace armarx
class LVL1ControllerWithTripleBuffer: virtual public LVL1Controller
{
public:
+ using MutexType = std::recursive_mutex;
+ using LockGuardType = std::lock_guard<std::recursive_mutex>;
+
LVL1ControllerWithTripleBuffer(const ControlDataStruct& initialCommands = ControlDataStruct()):
controlDataTripleBuffer {initialCommands}
{
@@ -308,6 +311,7 @@ namespace armarx
rtUpdateControlStruct();
rtRun(sensorValuesTimestamp, timeSinceLastIteration);
}
+
protected:
const ControlDataStruct& rtGetControlStruct() const
{
@@ -323,7 +327,7 @@ namespace armarx
//just lock to be save and reduce the impact of an error
//also this allows code to unlock the mutex before calling this function
//(can happen if some lockguard in a scope is used)
- std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
+ LockGuardType lock(controlDataMutex);
controlDataTripleBuffer.commitWrite();
}
ControlDataStruct& getWriterControlStruct()
@@ -331,12 +335,25 @@ namespace armarx
return controlDataTripleBuffer.getWriteBuffer();
}
- mutable std::recursive_mutex controlDataMutex;
+ void setControlStruct(const ControlDataStruct& newStruct)
+ {
+ LockGuardType lock {controlDataMutex};
+ getWriterControlStruct() = newStruct;
+ writeControlStruct();
+ }
+
+ void reinitTripleBuffer(const ControlDataStruct& initial)
+ {
+ controlDataTripleBuffer.reinitAllBuffers(initial);
+ }
+
+
+ mutable MutexType controlDataMutex;
private:
WriteBufferedTripleBuffer<ControlDataStruct> controlDataTripleBuffer;
};
template <typename ControlDataStruct>
- using LVL1ControllerTemplatePtr = IceInternal::Handle<LVL1ControllerWithTripleBuffer<ControlDataStruct>>;
+ using LVL1ControllerWithTripleBufferPtr = IceInternal::Handle<LVL1ControllerWithTripleBuffer<ControlDataStruct>>;
struct PlatformCartesianVelocity
@@ -349,7 +366,8 @@ namespace armarx
};
- class AbstractLvl1PlatformVelocityController : public virtual LVL1ControllerWithTripleBuffer<PlatformCartesianVelocity> {
+ class AbstractLvl1PlatformVelocityController : public virtual LVL1ControllerWithTripleBuffer<PlatformCartesianVelocity>
+ {
public:
virtual void setTarget(float vx, float vy, float vAngle) = 0;
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.cpp b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9f13cbe6b2e43a5dbd0808f2ba5d0e6bf425636c
--- /dev/null
+++ b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.cpp
@@ -0,0 +1,61 @@
+/*
+ * 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::LVL1HolonomicPlatformVelocityPassThroughController
+ * @author Raphael ( ufdrv at student dot kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "LVL1HolonomicPlatformVelocityPassThroughController.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughControllerConfig.h>
+
+using namespace armarx;
+
+LVL1HolonomicPlatformVelocityPassThroughController::LVL1HolonomicPlatformVelocityPassThroughController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config)
+{
+ LVL1HolonomicPlatformVelocityPassThroughControllerConfigPtr cfg = LVL1HolonomicPlatformVelocityPassThroughControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION_W_HINT(cfg,
+ "The provided config has the wrong type! The type is " << config->ice_id()
+ << " instead of " << LVL1HolonomicPlatformVelocityPassThroughControllerConfig::ice_staticId());
+
+ target = dynamic_cast<HolonomicPlatformVelocityTarget*>(prov->getJointTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocityMode));
+ ARMARX_CHECK_EXPRESSION_W_HINT(target, "The actuator " << cfg->platformName << " has no controll mode " << ControlModes::HolonomicPlatformVelocityMode);
+
+ initialSettings.velocityX = cfg->initialVelocityX;
+ initialSettings.velocityY = cfg->initialVelocityY;
+ initialSettings.velocityRotation = cfg->initialVelocityRotation;
+ reinitTripleBuffer(initialSettings);
+}
+
+void LVL1HolonomicPlatformVelocityPassThroughController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
+{
+ target->velocityX = rtGetControlStruct().velocityX;
+ target->velocityY = rtGetControlStruct().velocityY;
+ target->velocityRotation = rtGetControlStruct().velocityRotation;
+}
+
+void LVL1HolonomicPlatformVelocityPassThroughController::setVelocites(float velocityX, float velocityY, float velocityRotation)
+{
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().velocityX = velocityX;
+ getWriterControlStruct().velocityY = velocityY;
+ getWriterControlStruct().velocityRotation = velocityRotation;
+ writeControlStruct();
+}
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.h b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.h
new file mode 100644
index 0000000000000000000000000000000000000000..87032dee9ac8c7a9508369afcd5554d2fcb80a7c
--- /dev/null
+++ b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.h
@@ -0,0 +1,62 @@
+/*
+ * 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::LVL1HolonomicPlatformVelocityPassThroughController
+ * @author Raphael ( ufdrv at student dot kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_LVL1HolonomicPlatformVelocityPassThroughController_H
+#define _ARMARX_LIB_RobotAPI_LVL1HolonomicPlatformVelocityPassThroughController_H
+
+#include "LVL1Controller.h"
+#include "../Targets/HolonomicPlatformVelocityTarget.h"
+
+namespace armarx
+{
+ struct LVL1HolonomicPlatformVelocityPassThroughControllerControlData
+ {
+ float velocityX = 0;
+ float velocityY = 0;
+ float velocityRotation = 0;
+ };
+
+ class LVL1HolonomicPlatformVelocityPassThroughController:
+ virtual public LVL1ControllerWithTripleBuffer<LVL1HolonomicPlatformVelocityPassThroughControllerControlData>
+ {
+ public:
+ LVL1HolonomicPlatformVelocityPassThroughController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config);
+
+ virtual void rtRun(const IceUtil::Time&, const IceUtil::Time&) override;
+
+ //for the platform unit
+ virtual void setVelocites(float velocityX, float velocityY, float velocityRotation);
+
+ //ice interface
+ virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override
+ {
+ return "LVL1HolonomicPlatformVelocityPassThroughController";
+ }
+ protected:
+ virtual void onInitComponent() override {}
+ virtual void onConnectComponent() override {}
+
+ HolonomicPlatformVelocityTarget* target;
+ LVL1HolonomicPlatformVelocityPassThroughControllerControlData initialSettings;
+ };
+}
+#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitPositionController.cpp b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitPositionController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0a4b6f97ce4f1db650ea43463ed496faba183c60
--- /dev/null
+++ b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitPositionController.cpp
@@ -0,0 +1,107 @@
+/*
+ * 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::LVL1KinematicUnitPositionController
+ * @author Raphael ( ufdrv at student dot kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "LVL1KinematicUnitPositionController.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "../DataUnits/KinematicDataUnit.h"
+#include "../BasicControllers.h"
+#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitControllerConfigs.h>
+using namespace armarx;
+
+LVL1KinematicUnitPositionController::LVL1KinematicUnitPositionController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config)
+{
+ LVL1KinematicUnitPositionControllerConfigPtr cfg = LVL1KinematicUnitPositionControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION_W_HINT(cfg,
+ "The provided config has the wrong type! The type is " << config->ice_id()
+ << " instead of " << LVL1KinematicUnitPositionControllerConfig::ice_staticId());
+ //make sure the used units are supported
+ auto kinunit = prov->getRTKinematicDataUnit();
+ ARMARX_CHECK_EXPRESSION_W_HINT(kinunit, "The DataProvider " << prov->getName() << " has no kinematic data unit");
+ //get pointers to sensor values from units
+ std::vector<const float*>&& jointPos = kinunit->getJointAngles({cfg->jointName});
+ ARMARX_CHECK_EXPRESSION(jointPos.size() == 1);
+ ARMARX_CHECK_EXPRESSION(jointPos.at(0));
+ currentPosition = jointPos.at(0);
+ std::vector<const float*> jointVel = kinunit->getJointVelocities({cfg->jointName});
+ ARMARX_CHECK_EXPRESSION(jointVel.size() == 1);
+ ARMARX_CHECK_EXPRESSION(jointVel.at(0));
+ currentVelocity = jointVel.at(0);
+ velocityTarget = dynamic_cast<ActuatorVelocityTarget*>(prov->getJointTarget(cfg->jointName, ControlModes::VelocityMode));
+ ARMARX_CHECK_EXPRESSION_W_HINT(velocityTarget, "The joint " << cfg->jointName << " has no controll mode " << ControlModes::VelocityMode);
+ //read rest of config
+ initialSettings.maxVelocity = std::abs(cfg->maxVelocity);
+ initialSettings.acceleration = std::abs(cfg->acceleration);
+ initialSettings.deceleration = std::abs(cfg->deceleration);
+ initialSettings.directPControlLimit = std::abs(cfg->directPControlLimit);
+ initialSettings.p = std::abs(cfg->p);
+ initialSettings.maxDt = std::abs(cfg->maxDt);
+ initialSettings.limitPositionLo = cfg->limitPositionLo;
+ initialSettings.limitPositionHi = cfg->limitPositionHi;
+ ARMARX_CHECK_EXPRESSION(initialSettings.limitPositionLo <= initialSettings.limitPositionHi);
+ initialSettings.positionTargetValue = cfg->initialPositionTargetValue;
+ reinitTripleBuffer(initialSettings);
+}
+
+void LVL1KinematicUnitPositionController::rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration)
+{
+ // auto& ct = rtGetControlStruct();
+ // velocityTarget->velocity = positionThroughVelocityControlWithAccelerationAndPositionBounds(
+ // /*dt*/ timeSinceLastIteration.toSecondsDouble(), /*maxDt*/ ct.maxDt,
+ // *currentVelocity, ct.maxVelocity,
+ // ct.acceleration, ct.deceleration,
+ // *currentPosition, ct.positionTargetValue,
+ // ct.directPControlLimit, ct.p, ct.limitPositionLo, ct.limitPositionHi
+ // );
+}
+
+void LVL1KinematicUnitPositionController::setTargetPosition(float pos, float maxVelocity, float acceleration, float deceleration)
+{
+ LockGuardType l {controlDataMutex};
+ getWriterControlStruct().positionTargetValue = pos;
+ getWriterControlStruct().maxVelocity = std::abs(maxVelocity);
+ getWriterControlStruct().acceleration = std::abs(acceleration);
+ getWriterControlStruct().deceleration = std::abs(deceleration);
+ writeControlStruct();
+}
+
+void LVL1KinematicUnitPositionController::setTargetPosition(float pos)
+{
+ LockGuardType l {controlDataMutex};
+ getWriterControlStruct().positionTargetValue = pos;
+ writeControlStruct();
+}
+
+void LVL1KinematicUnitPositionController::setControlSettings(LVL1KinematicUnitPositionControllerControlSettings settings)
+{
+ settings.maxVelocity = std::abs(settings.maxVelocity);
+ settings.acceleration = std::abs(settings.acceleration);
+ settings.deceleration = std::abs(settings.deceleration);
+ settings.directPControlLimit = std::abs(settings.directPControlLimit);
+ settings.p = std::abs(settings.p);
+ settings.maxDt = std::abs(settings.maxDt);
+ ARMARX_CHECK_EXPRESSION(settings.limitPositionLo <= settings.limitPositionHi);
+ setControlStruct(settings);
+}
+
+LVL1ControllerRegistration<LVL1KinematicUnitPositionController> registrationLVL1KinematicUnitPositionController("LVL1KinematicUnitPositionController");
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitPositionController.h b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitPositionController.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c3875ce726c32f274fd51cba5dfefc5fd6f2c46
--- /dev/null
+++ b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitPositionController.h
@@ -0,0 +1,79 @@
+/*
+ * 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::LVL1KinematicUnitPositionController
+ * @author Raphael ( ufdrv at student dot kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_LVL1KinematicUnitPositionController_H
+#define _ARMARX_LIB_RobotAPI_LVL1KinematicUnitPositionController_H
+
+#include "LVL1Controller.h"
+#include "../RobotUnit.h"
+#include "../Targets/ActuatorVelocityTarget.h"
+
+namespace armarx
+{
+ struct LVL1KinematicUnitPositionControllerControlSettings
+ {
+ //'constant' data (in some cases these variables maybe need to be changed)
+ float directPControlLimit;
+ float p;
+ float maxDt;
+
+ float limitPositionLo;
+ float limitPositionHi;
+ //variable data
+ float maxVelocity;
+ float acceleration;
+ float deceleration;
+ float positionTargetValue;
+ };
+
+ class LVL1KinematicUnitPositionController:
+ virtual public LVL1ControllerWithTripleBuffer<LVL1KinematicUnitPositionControllerControlSettings>
+ {
+ public:
+ LVL1KinematicUnitPositionController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config);
+ virtual void rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration) override;
+
+ //ice interface
+ virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override
+ {
+ return "LVL1KinematicUnitPositionController";
+ }
+
+ //interface for kunit
+ const LVL1KinematicUnitPositionControllerControlSettings& getInitialSettings()
+ {
+ return initialSettings;
+ }
+ void setTargetPosition(float pos, float maxVelocity, float acceleration, float deceleration);
+ void setTargetPosition(float pos);
+ void setControlSettings(LVL1KinematicUnitPositionControllerControlSettings settings);
+ protected:
+ virtual void onInitComponent() override {}
+ virtual void onConnectComponent() override {}
+
+ const float* currentVelocity;
+ const float* currentPosition;
+ ActuatorVelocityTarget* velocityTarget;
+ LVL1KinematicUnitPositionControllerControlSettings initialSettings;
+ };
+}
+#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitTorqueController.cpp b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitTorqueController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5c53b220bd9f9b27d1d78068a2fc3b1b98025dc2
--- /dev/null
+++ b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitTorqueController.cpp
@@ -0,0 +1,44 @@
+/*
+ * 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::LVL1KinematicUnitTorqueController
+ * @author Raphael ( ufdrv at student dot kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "LVL1KinematicUnitTorqueController.h"
+using namespace armarx;
+
+LVL1KinematicUnitTorqueController::LVL1KinematicUnitTorqueController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config)
+{
+ LVL1KinematicUnitTorqueControllerConfigPtr cfg = LVL1KinematicUnitTorqueControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION_W_HINT(cfg,
+ "The provided config has the wrong type! The type is " << config->ice_id()
+ << " instead of " << LVL1KinematicUnitTorqueControllerConfig::ice_staticId());
+
+ torqueTarget = dynamic_cast<ActuatorTorqueTarget*>(prov->getJointTarget(cfg->jointName, ControlModes::TorqueMode));
+ ARMARX_CHECK_EXPRESSION_W_HINT(torqueTarget, "The joint " << cfg->jointName << " has no controll mode " << ControlModes::TorqueMode);
+
+ //read rest of config
+ initialMaxTorque = std::abs(cfg->maxTorque);
+ maxTorque = initialMaxTorque;
+ torqueTargetValue = cfg->initialTorqueTargetValue;
+}
+
+
+
+
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitTorqueController.h b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitTorqueController.h
new file mode 100644
index 0000000000000000000000000000000000000000..4398baf00e4a97afe9d7b72158f0efbd459b7ba4
--- /dev/null
+++ b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitTorqueController.h
@@ -0,0 +1,84 @@
+/*
+ * 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::LVL1KinematicUnitTorqueController
+ * @author Raphael ( ufdrv at student dot kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_LVL1KinematicUnitTorqueController_H
+#define _ARMARX_LIB_RobotAPI_LVL1KinematicUnitTorqueController_H
+
+#include <atomic>
+
+#include <boost/algorithm/clamp.hpp>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitControllerConfigs.h>
+
+#include "LVL1Controller.h"
+#include "../RobotUnit.h"
+
+#include "../Targets/ActuatorTorqueTarget.h"
+
+namespace armarx
+{
+ class LVL1KinematicUnitTorqueController:
+ virtual public LVL1Controller
+ {
+ public:
+ inline LVL1KinematicUnitTorqueController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config);
+
+ virtual void rtRun(const IceUtil::Time&, const IceUtil::Time&) override
+ {
+ torqueTarget->torque = boost::algorithm::clamp(torqueTargetValue, - maxTorque, maxTorque);
+ }
+
+ //ice interface
+ inline virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override
+ {
+ return "LVL1KinematicUnitTorqueController";
+ }
+
+ //kunit
+ void setMaxTorque(float mt)
+ {
+ maxTorque = std::abs(mt);
+ }
+ void setTorque(float t)
+ {
+ torqueTargetValue = t;
+ }
+
+ float getInitialMaxTorque() const
+ {
+ return initialMaxTorque;
+ }
+
+ protected:
+ virtual void onInitComponent() override {}
+ virtual void onConnectComponent() override {}
+
+ std::atomic<float> maxTorque;
+ std::atomic<float> torqueTargetValue;
+ float initialMaxTorque;
+
+ ActuatorTorqueTarget* torqueTarget;
+ };
+}
+#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitVelocityController.cpp b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitVelocityController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fb72b71613445593abf71a885e4f146fa98451f2
--- /dev/null
+++ b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitVelocityController.cpp
@@ -0,0 +1,95 @@
+/*
+ * 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::LVL1KinematicUnitVelocityController
+ * @author Raphael ( ufdrv at student dot kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#include "LVL1KinematicUnitVelocityController.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "../DataUnits/KinematicDataUnit.h"
+#include "../BasicControllers.h"
+#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitControllerConfigs.h>
+using namespace armarx;
+
+LVL1KinematicUnitVelocityController::LVL1KinematicUnitVelocityController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config)
+{
+ LVL1KinematicUnitVelocityControllerConfigPtr cfg = LVL1KinematicUnitVelocityControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION_W_HINT(cfg,
+ "The provided config has the wrong type! The type is " << config->ice_id()
+ << " instead of " << LVL1KinematicUnitVelocityControllerConfig::ice_staticId());
+ //make sure the used units are supported
+ auto kinunit = prov->getRTKinematicDataUnit();
+ ARMARX_CHECK_EXPRESSION_W_HINT(kinunit, "The DataProvider " << prov->getName() << " has no kinematic data unit");
+ //get pointers to sensor values from units
+ std::vector<const float*> jointPos = kinunit->getJointAngles({cfg->jointName});
+ ARMARX_CHECK_EXPRESSION(jointPos.size() == 1);
+ ARMARX_CHECK_EXPRESSION(jointPos.at(0));
+ currentPosition = jointPos.at(0);
+ std::vector<const float*> jointVel = kinunit->getJointVelocities({cfg->jointName});
+ ARMARX_CHECK_EXPRESSION(jointVel.size() == 1);
+ ARMARX_CHECK_EXPRESSION(jointVel.at(0));
+ currentVelocity = jointVel.at(0);
+ velocityTarget = dynamic_cast<ActuatorVelocityTarget*>(prov->getJointTarget(cfg->jointName, ControlModes::VelocityMode));
+ ARMARX_CHECK_EXPRESSION_W_HINT(velocityTarget, "The joint " << cfg->jointName << " has no controll mode " << ControlModes::VelocityMode);
+ //read rest of config
+ initialSettings.maxVelocity = std::abs(cfg->maxVelocity);
+ initialSettings.acceleration = std::abs(cfg->acceleration);
+ initialSettings.deceleration = std::abs(cfg->deceleration);
+ initialSettings.directSetLimit = std::abs(cfg->directSetLimit);
+ initialSettings.maxDt = std::abs(cfg->maxDt);
+ initialSettings.limitPositionLo = cfg->limitPositionLo;
+ initialSettings.limitPositionHi = cfg->limitPositionHi;
+ ARMARX_CHECK_EXPRESSION(initialSettings.limitPositionLo <= initialSettings.limitPositionHi);
+ initialSettings.velocityTargetValue = cfg->initialVelocityTargetValue;
+ reinitTripleBuffer(initialSettings);
+}
+
+void LVL1KinematicUnitVelocityController::rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration)
+{
+ // auto& ct = rtGetControlStruct();
+ // velocityTarget->velocity = velocityControlWithAccelerationAndPositionBounds(
+ // /*dt*/ timeSinceLastIteration.toSecondsDouble(), /*maxDt*/ ct.maxDt,
+ // *currentVelocity, ct.velocityTargetValue, ct.maxVelocity,
+ // ct.acceleration, ct.deceleration,
+ // ct.directSetLimit,
+ // *currentPosition, ct.limitPositionLo, ct.limitPositionHi
+ // );
+}
+
+void LVL1KinematicUnitVelocityController::setTargetVelocity(float v)
+{
+ LockGuardType l {controlDataMutex};
+ getWriterControlStruct().velocityTargetValue = v;
+ writeControlStruct();
+}
+
+void LVL1KinematicUnitVelocityController::setControlSettings(LVL1KinematicUnitVelocityControllerControlSettings settings)
+{
+ settings.maxVelocity = std::abs(settings.maxVelocity);
+ settings.acceleration = std::abs(settings.acceleration);
+ settings.deceleration = std::abs(settings.deceleration);
+ settings.directSetLimit = std::abs(settings.directSetLimit);
+ settings.maxDt = std::abs(settings.maxDt);
+ ARMARX_CHECK_EXPRESSION(settings.limitPositionLo <= settings.limitPositionHi);
+ setControlStruct(settings);
+}
+
+LVL1ControllerRegistration<LVL1KinematicUnitVelocityController> registrationLVL1KinematicUnitVelocityController("LVL1KinematicUnitVelocityController");
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitVelocityController.h b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitVelocityController.h
new file mode 100644
index 0000000000000000000000000000000000000000..7f59f30eaa374dcfffb76217674e9de0ff764024
--- /dev/null
+++ b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitVelocityController.h
@@ -0,0 +1,78 @@
+/*
+ * 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::LVL1KinematicUnitVelocityController
+ * @author Raphael ( ufdrv at student dot kit dot edu )
+ * @date 2017
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#ifndef _ARMARX_LIB_RobotAPI_LVL1KinematicUnitVelocityController_H
+#define _ARMARX_LIB_RobotAPI_LVL1KinematicUnitVelocityController_H
+
+#include "LVL1Controller.h"
+#include "../RobotUnit.h"
+#include "../Targets/ActuatorVelocityTarget.h"
+
+namespace armarx
+{
+ struct LVL1KinematicUnitVelocityControllerControlSettings
+ {
+ //'constant' data (in some cases these variables maybe need to be changed)
+ float maxVelocity;
+ float acceleration;
+ float deceleration;
+
+ float directSetLimit;
+ float maxDt;
+
+ float limitPositionLo;
+ float limitPositionHi;
+ //variable data
+ float velocityTargetValue;
+ };
+
+ class LVL1KinematicUnitVelocityController:
+ virtual public LVL1ControllerWithTripleBuffer<LVL1KinematicUnitVelocityControllerControlSettings>
+ {
+ public:
+ LVL1KinematicUnitVelocityController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config);
+ virtual void rtRun(const IceUtil::Time&, const IceUtil::Time& timeSinceLastIteration) override;
+
+ //ice interface
+ virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override
+ {
+ return "LVL1KinematicUnitVelocityController";
+ }
+
+ //interface for kunit
+ const LVL1KinematicUnitVelocityControllerControlSettings& getInitialSettings()
+ {
+ return initialSettings;
+ }
+ void setTargetVelocity(float v);
+ void setControlSettings(LVL1KinematicUnitVelocityControllerControlSettings settings);
+ protected:
+ virtual void onInitComponent() override {}
+ virtual void onConnectComponent() override {}
+
+ const float* currentVelocity;
+ const float* currentPosition;
+ ActuatorVelocityTarget* velocityTarget;
+ LVL1KinematicUnitVelocityControllerControlSettings initialSettings;
+ };
+}
+#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/PassThroughController.cpp b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.cpp
similarity index 56%
rename from source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/PassThroughController.cpp
rename to source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.cpp
index d3f5850abe987c410fcfd07bbaa2da17f7279503..83b51d8a277d163da8dbc9e680517d7e69751a1a 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/PassThroughController.cpp
+++ b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.cpp
@@ -13,17 +13,16 @@
* 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::PassThroughController
+ * @package RobotAPI::ArmarXObjects::LVL1PassThroughController
* @author Raphael ( ufdrv at student dot kit dot edu )
* @date 2017
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#include "PassThroughController.h"
+#include "LVL1PassThroughController.h"
using namespace armarx;
-LVL1ControllerRegistration<PassThroughController<JointPositionTarget>> registrationControllerPositionPassThroughController("PositionPassThroughController");
-LVL1ControllerRegistration<PassThroughController<JointVelocityTarget>> registrationControllerVelocityPassThroughController("VelocityPassThroughController");
-LVL1ControllerRegistration<PassThroughController<JointTorqueTarget >> registrationControllerJointPassThroughController("TorquePassThroughController");
-
+LVL1ControllerRegistration<LVL1PassThroughController<ActuatorPositionTarget>> registrationControllerLVL1PositionPassThroughController("LVL1PositionPassThroughController");
+LVL1ControllerRegistration<LVL1PassThroughController<ActuatorVelocityTarget>> registrationControllerLVL1VelocityPassThroughController("LVL1VelocityPassThroughController");
+LVL1ControllerRegistration<LVL1PassThroughController<ActuatorTorqueTarget >> registrationControllerLVL1TorquePassThroughController("LVL1TorquePassThroughController");
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/PassThroughController.h b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.h
similarity index 74%
rename from source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/PassThroughController.h
rename to source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.h
index e5eb6a7b95f00161c94824022350cd45383a7c2a..e6d311b5bb7a1dfc55bf07288fc7d1627db75e48 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/PassThroughController.h
+++ b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.h
@@ -13,26 +13,26 @@
* 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::PassThroughController
+ * @package RobotAPI::ArmarXObjects::LVL1PassThroughController
* @author Raphael ( ufdrv at student dot kit dot edu )
* @date 2017
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#ifndef _ARMARX_LIB_RobotAPI_PassThroughController_H
-#define _ARMARX_LIB_RobotAPI_PassThroughController_H
+#ifndef _ARMARX_LIB_RobotAPI_LVL1PassThroughController_H
+#define _ARMARX_LIB_RobotAPI_LVL1PassThroughController_H
#include <atomic>
#include <ArmarXCore/core/util/algorithm.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include "LVL1Controller.h"
#include "../RobotUnit.h"
-#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/PassThroughController.h>
+#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.h>
-#include "../Targets/JointPositionTarget.h"
-#include "../Targets/JointTorqueTarget.h"
-#include "../Targets/JointVelocityTarget.h"
+#include "../Targets/ActuatorPositionTarget.h"
+#include "../Targets/ActuatorTorqueTarget.h"
+#include "../Targets/ActuatorVelocityTarget.h"
#include "../DataUnits/KinematicDataUnit.h"
#include "../Targets/PlatformWheelVelocityTarget.h"
@@ -56,17 +56,17 @@ namespace armarx
};
template <typename TargetType>
- struct PassThroughControllerTargetTypeTraits;
+ struct LVL1PassThroughControllerTargetTypeTraits;
template <typename TargetType>
- class PassThroughController:
+ class LVL1PassThroughController:
virtual public LVL1Controller,
- virtual public PassThroughControllerInterface
+ virtual public LVL1PassThroughControllerInterface
{
public:
- using Traits = PassThroughControllerTargetTypeTraits<TargetType>;
+ using Traits = LVL1PassThroughControllerTargetTypeTraits<TargetType>;
- inline PassThroughController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config);
+ inline LVL1PassThroughController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config);
inline virtual void rtRun(const IceUtil::Time&, const IceUtil::Time&) override;
inline virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
@@ -87,9 +87,9 @@ namespace armarx
};
template <>
- struct PassThroughControllerTargetTypeTraits<JointVelocityTarget>
+ struct LVL1PassThroughControllerTargetTypeTraits<ActuatorVelocityTarget>
{
- static float* getTargetDatamember(JointVelocityTarget& t)
+ static float* getTargetDatamember(ActuatorVelocityTarget& t)
{
return &t.velocity;
}
@@ -103,9 +103,9 @@ namespace armarx
}
};
template <>
- struct PassThroughControllerTargetTypeTraits<JointTorqueTarget>
+ struct LVL1PassThroughControllerTargetTypeTraits<ActuatorTorqueTarget>
{
- static float* getTargetDatamember(JointTorqueTarget& t)
+ static float* getTargetDatamember(ActuatorTorqueTarget& t)
{
return &t.torque;
}
@@ -119,9 +119,9 @@ namespace armarx
}
};
template <>
- struct PassThroughControllerTargetTypeTraits<JointPositionTarget>
+ struct LVL1PassThroughControllerTargetTypeTraits<ActuatorPositionTarget>
{
- static float* getTargetDatamember(JointPositionTarget& t)
+ static float* getTargetDatamember(ActuatorPositionTarget& t)
{
return &t.position;
}
@@ -136,7 +136,7 @@ namespace armarx
};
template <>
- struct PassThroughControllerTargetTypeTraits<PlatformWheelVelocityTarget>
+ struct LVL1PassThroughControllerTargetTypeTraits<PlatformWheelVelocityTarget>
{
static float* getTargetDatamember(PlatformWheelVelocityTarget& t)
{
@@ -166,24 +166,24 @@ namespace armarx
template <typename TargetType>
- std::string PassThroughController<TargetType>::getClassName(const Ice::Current&) const
+ std::string LVL1PassThroughController<TargetType>::getClassName(const Ice::Current&) const
{
- return "PassThroughController_" + Traits::getControlMode();
+ return "LVL1PassThroughController_" + Traits::getControlMode();
}
template <typename TargetType>
- PassThroughController<TargetType>::PassThroughController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config)
+ LVL1PassThroughController<TargetType>::LVL1PassThroughController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config)
{
- PassThroughControllerConfigPtr cfg = PassThroughControllerConfigPtr::dynamicCast(config);
+ LVL1PassThroughControllerConfigPtr cfg = LVL1PassThroughControllerConfigPtr::dynamicCast(config);
ARMARX_CHECK_EXPRESSION_W_HINT(cfg,
"The provided config has the wrong type! The type is " << config->ice_id()
- << " instead of " << PassThroughControllerConfig::ice_staticId());
+ << " instead of " << LVL1PassThroughControllerConfig::ice_staticId());
//make sure the used units are supported
auto kinunit = prov->getRTKinematicDataUnit();
ARMARX_CHECK_EXPRESSION_W_HINT(kinunit, "The DataProvider " << prov->getName() << " has no kinematic data unit");
//get pointers to sensor values from units
- ARMARX_INFO << "PassThroughController for " << cfg->jointNames;
+ ARMARX_INFO << "LVL1PassThroughController for " << cfg->jointNames;
jointStates = Traits::getSensorValues(kinunit, cfg->jointNames) ;
//not initialized, this is done in rtPreActivateController
@@ -191,7 +191,7 @@ namespace armarx
//get pointers for the results of this controller
lvl0Targets.reserve(cfg->jointNames.size());
- for (const auto & j : cfg->jointNames)
+ for (const auto& j : cfg->jointNames)
{
auto target = dynamic_cast<TargetType*>(prov->getJointTarget(j, Traits::getControlMode()));
ARMARX_CHECK_EXPRESSION_W_HINT(target, "The joint " << j << " has no controll mode " << Traits::getControlMode());
@@ -201,7 +201,7 @@ namespace armarx
}
template <typename TargetType>
- void PassThroughController<TargetType>::rtRun(const IceUtil::Time&, const IceUtil::Time&)
+ void LVL1PassThroughController<TargetType>::rtRun(const IceUtil::Time&, const IceUtil::Time&)
{
for (std::size_t i = 0; i < iceTargets.size(); ++i)
{
@@ -210,13 +210,13 @@ namespace armarx
}
template <typename TargetType>
- void PassThroughController<TargetType>::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ void LVL1PassThroughController<TargetType>::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
rtRun(sensorValuesTimestamp, timeSinceLastIteration);
}
template <typename TargetType>
- void PassThroughController<TargetType>::rtPreActivateController()
+ void LVL1PassThroughController<TargetType>::rtPreActivateController()
{
for (std::size_t i = 0; i < jointStates.size(); ++i)
{
@@ -227,7 +227,7 @@ namespace armarx
}
template <typename TargetType>
- void PassThroughController<TargetType>::setJoint(const std::string& name, Ice::Float value, const Ice::Current&)
+ void LVL1PassThroughController<TargetType>::setJoint(const std::string& name, Ice::Float value, const Ice::Current&)
{
auto targetIt = indices.find(name);
if (targetIt == indices.end())
@@ -238,9 +238,9 @@ namespace armarx
}
template <typename TargetType>
- void PassThroughController<TargetType>::setJoints(const StringFloatDictionary& values, const Ice::Current&)
+ void LVL1PassThroughController<TargetType>::setJoints(const StringFloatDictionary& values, const Ice::Current&)
{
- for (const auto & value : values)
+ for (const auto& value : values)
{
setJoint(value.first, value.second);
}
diff --git a/source/RobotAPI/libraries/RTRobotUnit/RobotUnit.cpp b/source/RobotAPI/libraries/RTRobotUnit/RobotUnit.cpp
index c10b03c2f32e69f20bd6b215d60c5a6fd8750801..fc02567ebc7f2ef9f0b3b7882a08646420caef7b 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/RobotUnit.cpp
+++ b/source/RobotAPI/libraries/RTRobotUnit/RobotUnit.cpp
@@ -31,7 +31,7 @@ armarx::PropertyDefinitionsPtr armarx::RobotUnit::createPropertyDefinitions()
return armarx::PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(getConfigIdentifier()));
}
-void armarx::RobotUnit::addLVL0Controller(const std::string& jointName, armarx::LVL0ControllerBase& lvl0Controller)
+void armarx::RobotUnit::addLVL0Controller(const std::string& jointName, armarx::LVL0Controller& lvl0Controller)
{
std::string controlMode = lvl0Controller.getControlMode();
GuardType guard {dataMutex};
@@ -48,14 +48,14 @@ void armarx::RobotUnit::addLVL0Controller(const std::string& jointName, armarx::
}
}
-const armarx::LVL0ControllerBase* armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode) const
+const armarx::LVL0Controller* armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode) const
{
GuardType guard {dataMutex};
ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
return lvl0Controllers.at(jointName).at(controlMode);
}
-armarx::LVL0ControllerBase* armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode)
+armarx::LVL0Controller* armarx::RobotUnit::getLVL0Controller(const std::string& jointName, const std::string& controlMode)
{
GuardType guard {dataMutex};
ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
@@ -72,7 +72,7 @@ Ice::StringSeq armarx::RobotUnit::getControllerNamesLoaded(const Ice::Current&)
GuardType guard {dataMutex};
Ice::StringSeq result;
result.reserve(lvl1Controllers.size());
- for (const auto & lvl1 : lvl1Controllers)
+ for (const auto& lvl1 : lvl1Controllers)
{
result.emplace_back(lvl1.first);
}
@@ -83,7 +83,7 @@ Ice::StringSeq armarx::RobotUnit::getControllerNamesRequested(const Ice::Current
GuardType guard {dataMutex};
Ice::StringSeq result;
result.reserve(lvl1Controllers.size());
- for (const auto & lvl1 : getRequestedLVL1Controllers())
+ for (const auto& lvl1 : getRequestedLVL1Controllers())
{
if (lvl1)
{
@@ -97,7 +97,7 @@ Ice::StringSeq armarx::RobotUnit::getControllerNamesActivated(const Ice::Current
GuardType guard {dataMutex};
Ice::StringSeq result;
result.reserve(lvl1Controllers.size());
- for (const auto & lvl1 : getActivatedLVL1Controllers())
+ for (const auto& lvl1 : getActivatedLVL1Controllers())
{
if (!lvl1)
{
@@ -112,7 +112,7 @@ armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersLoade
{
GuardType guard {dataMutex};
StringLVL1ControllerPrxDictionary result;
- for (const auto & lvl1 : lvl1Controllers)
+ for (const auto& lvl1 : lvl1Controllers)
{
result[lvl1.first] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1.second->getProxy());
}
@@ -122,7 +122,7 @@ armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersReque
{
GuardType guard {dataMutex};
StringLVL1ControllerPrxDictionary result;
- for (const auto & lvl1 : getRequestedLVL1Controllers())
+ for (const auto& lvl1 : getRequestedLVL1Controllers())
{
if (lvl1)
{
@@ -135,7 +135,7 @@ armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersActiv
{
GuardType guard {dataMutex};
StringLVL1ControllerPrxDictionary result;
- for (const auto & lvl1 : getActivatedLVL1Controllers())
+ for (const auto& lvl1 : getActivatedLVL1Controllers())
{
result[lvl1->getName()] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1->getProxy());
}
@@ -146,17 +146,17 @@ armarx::JointNameToLVL1Dictionary armarx::RobotUnit::getControllerJointAssignmen
{
GuardType guard {dataMutex};
JointNameToLVL1Dictionary result;
- for (const auto & joint : jointNames)
+ for (const auto& joint : jointNames)
{
result[joint] = "";
}
- for (const auto & lvl1 : getActivatedLVL1Controllers())
+ for (const auto& lvl1 : getActivatedLVL1Controllers())
{
if (!lvl1)
{
continue;
}
- for (const auto & jointMode : lvl1->jointControlModeMap)
+ for (const auto& jointMode : lvl1->jointControlModeMap)
{
result[jointMode.first] = lvl1->getName();
}
@@ -207,11 +207,11 @@ armarx::JointNameToControlModesDictionary armarx::RobotUnit::getJointControlMode
{
GuardType guard {dataMutex};
JointNameToControlModesDictionary result;
- for (const auto & joint : lvl0Controllers)
+ for (const auto& joint : lvl0Controllers)
{
std::vector<std::string> modes;
modes.reserve(joint.second.size());
- for (const auto & mode : joint.second)
+ for (const auto& mode : joint.second)
{
modes.emplace_back(mode.first);
}
@@ -225,7 +225,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
GuardType guard {dataMutex};
ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
//check if all of these controllers exist
- for (const auto & lvl1 : controllerRequestedNames)
+ for (const auto& lvl1 : controllerRequestedNames)
{
if (!hasLVL1Controller(lvl1))
{
@@ -240,7 +240,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
std::set<std::string> controllersToActivate {currentActiveLVL1Controllers.begin(), currentActiveLVL1Controllers.end()}; //these controllers will be set as active controllers
- for (const auto & toActivate : controllerRequestedNames)
+ for (const auto& toActivate : controllerRequestedNames)
{
if (controllersToActivate.count(toActivate))
{
@@ -253,7 +253,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
const auto& lvl1 = lvl1Controllers.at(toActivate);
//check and update the assignement map
- for (const auto & jointControlMode : lvl1->jointControlModeMap)
+ for (const auto& jointControlMode : lvl1->jointControlModeMap)
{
const auto& joint = jointControlMode.first;
const auto& currentAssignedLVL1 = lvl1ControllerAssignement.at(joint);
@@ -267,7 +267,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
continue;
}
//deactivate other controller
- for (auto & assignement : lvl1ControllerAssignement)
+ for (auto& assignement : lvl1ControllerAssignement)
{
if (assignement.second == currentAssignedLVL1)
{
@@ -280,7 +280,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
}
}
//verify (exception for collision of requested lvl1)
- for (const auto & toActivate : controllerRequestedNames)
+ for (const auto& toActivate : controllerRequestedNames)
{
if (!controllersToActivate.count(toActivate))
{
@@ -306,7 +306,7 @@ void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNam
//populate controllersRequested.lvl1Controllers
getRequestedLVL1Controllers().clear();
getRequestedLVL1Controllers().reserve(controllersToActivate.size());
- for (const auto & lvl1 : controllersToActivate)
+ for (const auto& lvl1 : controllersToActivate)
{
getRequestedLVL1Controllers().emplace_back(lvl1Controllers.at(lvl1));
}
@@ -343,14 +343,14 @@ armarx::LVL1ControllerInterfacePrx armarx::RobotUnit::loadController(const std::
throw InvalidArgumentException {ss.str()};
}
//create the controller
- jointsUsedByLVL1Controler.clear();
+ jointsUsedByLVL1Controller.clear();
RobotUnitInterfacePtr::dynamicCast(RobotUnitPtr {this});
LVL1ControllerDataProviderInterfacePtr::dynamicCast(RobotUnitPtr {this});
LVL1ControllerPtr lvl1 = factory(LVL1ControllerDataProviderInterfacePtr::dynamicCast(RobotUnitPtr {this}), config);
- lvl1->jointControlModeMap = jointsUsedByLVL1Controler;
+ lvl1->jointControlModeMap = jointsUsedByLVL1Controller;
lvl1->jointIndices.clear();
- lvl1->jointIndices.reserve(jointsUsedByLVL1Controler.size());
- for (const auto & j : jointsUsedByLVL1Controler)
+ lvl1->jointIndices.reserve(jointsUsedByLVL1Controller.size());
+ for (const auto& j : jointsUsedByLVL1Controller)
{
lvl1->jointIndices.emplace_back(jointNameIndices.at(j.first));
}
@@ -370,7 +370,7 @@ armarx::TargetBase* armarx::RobotUnit::getJointTarget(const std::string& jointNa
}
const auto lvl0 = getLVL0Controller(jointName, controlMode);
ARMARX_CHECK_EXPRESSION(lvl0->getControlMode() == controlMode);
- jointsUsedByLVL1Controler[jointName] = controlMode;
+ jointsUsedByLVL1Controller[jointName] = controlMode;
return lvl0->getTarget();
}
@@ -544,7 +544,7 @@ bool armarx::RobotUnit::rtSwitchSetup()
void armarx::RobotUnit::rtRunLVL1Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
- for (LVL1ControllerPtr & lvl1 : rtGetActivatedLVL1Controllers())
+ for (LVL1ControllerPtr& lvl1 : rtGetActivatedLVL1Controllers())
{
if (!lvl1)
{
@@ -578,9 +578,9 @@ void armarx::RobotUnit::rtDeactivateAssignedLVL1Controller(std::size_t index)
void armarx::RobotUnit::rtRunLVL0Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
{
- for (LVL0ControllerBase * lvl0 : rtGetActivatedLVL0Controllers())
+ for (LVL0Controller* lvl0 : rtGetActivatedLVL0Controllers())
{
- lvl0->run(sensorValuesTimestamp, timeSinceLastIteration);
+ lvl0->rtRun(sensorValuesTimestamp, timeSinceLastIteration);
}
}
@@ -588,7 +588,7 @@ bool armarx::RobotUnit::validateAddedLVL0Controllers() const
{
GuardType guard {dataMutex};
ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
- for (const auto & lvl0 : lvl0ControllersEmergencyStop)
+ for (const auto& lvl0 : lvl0ControllersEmergencyStop)
{
if (!lvl0)
{
diff --git a/source/RobotAPI/libraries/RTRobotUnit/RobotUnit.h b/source/RobotAPI/libraries/RTRobotUnit/RobotUnit.h
index c0b114fcb85532e744fa340f6191ff508034217c..7f7fefb967b5b94fad0130204d6b44df05520df2 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/RobotUnit.h
+++ b/source/RobotAPI/libraries/RTRobotUnit/RobotUnit.h
@@ -170,7 +170,7 @@ namespace armarx
* \subsection RobotUnit-tasks-to-implement-lvl0 Adding LVL0 controllers
* Add LVL0Controllers by calling
* \code{.cpp}
- * addLVL0Controller(const std::string& jointName, LVL0ControllerBase& lvl0Controller);
+ * addLVL0Controller(const std::string& jointName, LVL0Controller& lvl0Controller);
* \endcode
* If you add two controllers withe the same control mode for one joint, an std::invalid_argument exception is thrown.
*
@@ -245,7 +245,7 @@ namespace armarx
virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
//get buffers
/// @return The requested lvl0 controllers (none is null)
- std::vector<LVL0ControllerBase*>& getRequestedLVL0Controllers()
+ std::vector<LVL0Controller*>& getRequestedLVL0Controllers()
{
return controllersRequested.getWriteBuffer().lvl0Controllers;
}
@@ -255,7 +255,7 @@ namespace armarx
return controllersRequested.getWriteBuffer().lvl1Controllers;
}
/// @return The requested lvl0 controllers (none is null)
- const std::vector<LVL0ControllerBase*>& getRequestedLVL0Controllers() const
+ const std::vector<LVL0Controller*>& getRequestedLVL0Controllers() const
{
return controllersRequested.getWriteBuffer().lvl0Controllers;
}
@@ -265,7 +265,7 @@ namespace armarx
return controllersRequested.getWriteBuffer().lvl1Controllers;
}
/// @return The activated lvl0 controllers (none is null)
- const std::vector<LVL0ControllerBase*>& getActivatedLVL0Controllers() const
+ const std::vector<LVL0Controller*>& getActivatedLVL0Controllers() const
{
return controllersActivated.getUpToDateReadBuffer().lvl0Controllers;
}
@@ -285,15 +285,15 @@ namespace armarx
* @param lvl0Controller The controller to add.
* @throw If you add two controllers withe the same control mode for one joint, an std::invalid_argument exception is thrown.
*/
- void addLVL0Controller(const std::string& jointName, LVL0ControllerBase& lvl0Controller);
+ void addLVL0Controller(const std::string& jointName, LVL0Controller& lvl0Controller);
/// @return The LVL0Controller for given joint and control mode.
- const LVL0ControllerBase* getLVL0Controller(const std::string& jointName, const std::string& controlMode) const;
+ const LVL0Controller* getLVL0Controller(const std::string& jointName, const std::string& controlMode) const;
/// @return The LVL0Controller for given joint and control mode.
- LVL0ControllerBase* getLVL0Controller(const std::string& jointName, const std::string& controlMode);
+ LVL0Controller* getLVL0Controller(const std::string& jointName, const std::string& controlMode);
- LVL0ControllerBase* rtGetEmergencyStopLVL0Controller(std::size_t index)
+ LVL0Controller* rtGetEmergencyStopLVL0Controller(std::size_t index)
{
return lvl0ControllersEmergencyStop.at(index);
}
@@ -321,7 +321,7 @@ namespace armarx
return controllersRequested.updateReadBuffer();
}
/// @return The requested lvl0 controllers (none is null)
- const std::vector<LVL0ControllerBase*>& rtGetRequestedLVL0Controllers() const
+ const std::vector<LVL0Controller*>& rtGetRequestedLVL0Controllers() const
{
return controllersRequested.getReadBuffer().lvl0Controllers;
}
@@ -331,7 +331,7 @@ namespace armarx
return controllersRequested.getReadBuffer().lvl1Controllers;
}
/// @return The activated lvl0 controllers (none is null)
- std::vector<LVL0ControllerBase*>& rtGetActivatedLVL0Controllers()
+ std::vector<LVL0Controller*>& rtGetActivatedLVL0Controllers()
{
return controllersActivated.getWriteBuffer().lvl0Controllers;
}
@@ -341,7 +341,7 @@ namespace armarx
return controllersActivated.getWriteBuffer().lvl1Controllers;
}
/// @return The activated lvl0 controllers (none is null)
- const std::vector<LVL0ControllerBase*>& rtGetActivatedLVL0Controllers() const
+ const std::vector<LVL0Controller*>& rtGetActivatedLVL0Controllers() const
{
return controllersActivated.getWriteBuffer().lvl0Controllers;
}
@@ -351,7 +351,7 @@ namespace armarx
return controllersActivated.getWriteBuffer().lvl1Controllers;
}
//get controllers (rt)
- LVL0ControllerBase* const& rtGetActivatedLVL0Controller(std::size_t index) const
+ LVL0Controller* const& rtGetActivatedLVL0Controller(std::size_t index) const
{
return rtGetActivatedLVL0Controllers().at(index);
}
@@ -359,7 +359,7 @@ namespace armarx
{
return rtGetActivatedLVL1Controllers().at(index);
}
- LVL0ControllerBase*& rtGetActivatedLVL0Controller(std::size_t index)
+ LVL0Controller*& rtGetActivatedLVL0Controller(std::size_t index)
{
return rtGetActivatedLVL0Controllers().at(index);
}
@@ -369,7 +369,7 @@ namespace armarx
}
- LVL0ControllerBase* rtGetRequestedLVL0Controller(std::size_t index) const
+ LVL0Controller* rtGetRequestedLVL0Controller(std::size_t index) const
{
return rtGetRequestedLVL0Controllers().at(index);
}
@@ -395,7 +395,7 @@ namespace armarx
* @param oldLVL0 The old lvl0 controller (will be deactivated)
* @param newLVL0 The new lvl0 controller (will be activated)
*/
- virtual void rtSwitchLVL0Controller(std::size_t index, LVL0ControllerBase* oldLVL0, LVL0ControllerBase* newLVL0) = 0;
+ virtual void rtSwitchLVL0Controller(std::size_t index, LVL0Controller* oldLVL0, LVL0Controller* newLVL0) = 0;
protected:
@@ -421,7 +421,7 @@ namespace armarx
LVL0AndLVL1ControllerList() = default;
LVL0AndLVL1ControllerList(const LVL0AndLVL1ControllerList&) = default;
- std::vector<LVL0ControllerBase*> lvl0Controllers;
+ std::vector<LVL0Controller*> lvl0Controllers;
std::vector<LVL1ControllerPtr > lvl1Controllers;
};
/**
@@ -429,14 +429,14 @@ namespace armarx
* Is initialized by derived classes.
* May not be accessed when the controll loop is running
*/
- std::map<std::string, std::map<std::string, LVL0ControllerBase*>> lvl0Controllers;
+ std::map<std::string, std::map<std::string, LVL0Controller*>> lvl0Controllers;
/**
* @brief Holds all currently loaded LVL1 controllers (index: [instancename])
* May not be accessed in rt.
*/
std::map<std::string, LVL1ControllerPtr> lvl1Controllers;
/// @brief LVL0Controllers for Emergency stop
- std::vector<LVL0ControllerBase*> lvl0ControllersEmergencyStop;
+ std::vector<LVL0Controller*> lvl0ControllersEmergencyStop;
//used to communicate with rt
/// @brief Controllers the RT thread is supposed to activate (direction nonRT -> RT) (some lvl1 controllers may be null)
@@ -446,7 +446,7 @@ namespace armarx
//other
/// @brief Stores joints accessed via getJointTarget
- JointNameToControlModeDictionary jointsUsedByLVL1Controler;
+ JointNameToControlModeDictionary jointsUsedByLVL1Controller;
std::map<std::string, DynamicLibraryPtr> loadedLibs;
};
diff --git a/source/RobotAPI/libraries/RTRobotUnit/SyntaxCheck.cpp b/source/RobotAPI/libraries/RTRobotUnit/SyntaxCheck.cpp
index 6266f5dee13f09a64ad41ff8deac873ff8c0e4a6..9c3e574114e6d5f934aca89da845286386f074ca 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/SyntaxCheck.cpp
+++ b/source/RobotAPI/libraries/RTRobotUnit/SyntaxCheck.cpp
@@ -21,11 +21,13 @@
*/
#include "Constants.h"
+#include "ControlModes.h"
+#include "BasicControllers.h"
#include "LVL0Controllers/LVL0Controller.h"
#include "LVL1Controllers/LVL1Controller.h"
-#include "LVL1Controllers/PassThroughController.h"
+#include "LVL1Controllers/LVL1PassThroughController.h"
#include "DataUnits/ForceTorqueDataUnit.h"
#include "DataUnits/HapticDataUnit.h"
@@ -33,9 +35,10 @@
#include "DataUnits/KinematicDataUnit.h"
#include "DataUnits/PlatformDataUnit.h"
-#include "Targets/JointPositionTarget.h"
+#include "Targets/ActuatorPositionTarget.h"
#include "Targets/TargetBase.h"
-#include "Targets/JointTorqueTarget.h"
-#include "Targets/JointVelocityTarget.h"
+#include "Targets/ActuatorTorqueTarget.h"
+#include "Targets/ActuatorVelocityTarget.h"
+#include "Targets/HolonomicPlatformVelocityTarget.h"
//this file is only for syntax checks
diff --git a/source/RobotAPI/libraries/RTRobotUnit/Targets/JointPositionTarget.h b/source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorPositionTarget.h
similarity index 85%
rename from source/RobotAPI/libraries/RTRobotUnit/Targets/JointPositionTarget.h
rename to source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorPositionTarget.h
index 21bc64c9a5b421a841190e4ac8ca52c8ea505b7a..643a12e539d3fb3ee7e64d3784f6ab1c042aa4b0 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/Targets/JointPositionTarget.h
+++ b/source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorPositionTarget.h
@@ -13,7 +13,7 @@
* 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::JointPositionTarget
+ * @package RobotAPI::ArmarXObjects::ActuatorPositionTarget
* @author Raphael ( ufdrv at student dot kit dot edu )
* @date 2017
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
@@ -32,13 +32,13 @@ namespace armarx
* @ingroup RobotAPI
* A description of the library RTRobotUnit.
*
- * @class JointPositionTarget
+ * @class ActuatorPositionTarget
* @ingroup Library-RTRobotUnit
- * @brief Brief description of class JointPositionTarget.
+ * @brief Brief description of class ActuatorPositionTarget.
*
- * Detailed description of class JointPositionTarget.
+ * Detailed description of class ActuatorPositionTarget.
*/
- class JointPositionTarget: public TargetBase
+ class ActuatorPositionTarget: public TargetBase
{
public:
float position = ControllerConstants::ValueNotSetNaN;
diff --git a/source/RobotAPI/libraries/RTRobotUnit/Targets/JointTorqueTarget.h b/source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorTorqueTarget.h
similarity index 85%
rename from source/RobotAPI/libraries/RTRobotUnit/Targets/JointTorqueTarget.h
rename to source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorTorqueTarget.h
index 777d645f8d9aeb2180f4e1580e39e12823835d5b..d4e29a926c249c54a864b43be6f131f8eb70dd2d 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/Targets/JointTorqueTarget.h
+++ b/source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorTorqueTarget.h
@@ -13,7 +13,7 @@
* 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::JointTorqueTarget
+ * @package RobotAPI::ArmarXObjects::ActuatorTorqueTarget
* @author Raphael ( ufdrv at student dot kit dot edu )
* @date 2017
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
@@ -32,13 +32,13 @@ namespace armarx
* @ingroup RobotAPI
* A description of the library RTRobotUnit.
*
- * @class JointTorqueTarget
+ * @class ActuatorTorqueTarget
* @ingroup Library-RTRobotUnit
- * @brief Brief description of class JointTorqueTarget.
+ * @brief Brief description of class ActuatorTorqueTarget.
*
- * Detailed description of class JointTorqueTarget.
+ * Detailed description of class ActuatorTorqueTarget.
*/
- class JointTorqueTarget: public TargetBase
+ class ActuatorTorqueTarget: public TargetBase
{
public:
float torque = ControllerConstants::ValueNotSetNaN;
diff --git a/source/RobotAPI/libraries/RTRobotUnit/Targets/JointVelocityTarget.h b/source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorVelocityTarget.h
similarity index 79%
rename from source/RobotAPI/libraries/RTRobotUnit/Targets/JointVelocityTarget.h
rename to source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorVelocityTarget.h
index a97c8f5a5cfa6a3757e95ee53d80c20042221224..edfc60972f11de19009564e51bd189139f9e2877 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/Targets/JointVelocityTarget.h
+++ b/source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorVelocityTarget.h
@@ -13,15 +13,15 @@
* 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::JointVelocityTarget
+ * @package RobotAPI::ArmarXObjects::ActuatorVelocityTarget
* @author Raphael ( ufdrv at student dot kit dot edu )
* @date 2017
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#ifndef _ARMARX_LIB_RobotAPI_JointVelocityTarget_H
-#define _ARMARX_LIB_RobotAPI_JointVelocityTarget_H
+#ifndef _ARMARX_LIB_RobotAPI_ActuatorVelocityTarget_H
+#define _ARMARX_LIB_RobotAPI_ActuatorVelocityTarget_H
#include "TargetBase.h"
@@ -32,13 +32,13 @@ namespace armarx
* @ingroup RobotAPI
* A description of the library RTRobotUnit.
*
- * @class JointVelocityTarget
+ * @class ActuatorVelocityTarget
* @ingroup Library-RTRobotUnit
- * @brief Brief description of class JointVelocityTarget.
+ * @brief Brief description of class ActuatorVelocityTarget.
*
- * Detailed description of class JointVelocityTarget.
+ * Detailed description of class ActuatorVelocityTarget.
*/
- class JointVelocityTarget: public TargetBase
+ class ActuatorVelocityTarget: public TargetBase
{
public:
float velocity = ControllerConstants::ValueNotSetNaN;
diff --git a/source/RobotAPI/libraries/RTRobotUnit/test/BasicControllerTest.cpp b/source/RobotAPI/libraries/RTRobotUnit/test/BasicControllerTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a3ff1667357a7fe758d8901aeb2e0d772ecfb25a
--- /dev/null
+++ b/source/RobotAPI/libraries/RTRobotUnit/test/BasicControllerTest.cpp
@@ -0,0 +1,596 @@
+/*
+* 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 ArmarX::RobotAPI::Test
+* @author Raphael ( ufdrv at student dot kit dot edu )
+* @date 2017
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#define BOOST_TEST_MODULE RobotAPI::BasicControllers::Test
+#define ARMARX_BOOST_TEST
+#define CREATE_LOGFILES
+#include <random>
+#include <iostream>
+
+#include <boost/algorithm/clamp.hpp>
+
+#include <ArmarXCore/core/util/algorithm.h>
+#include <RobotAPI/Test.h>
+#include "../BasicControllers.h"
+using namespace armarx;
+//params for random tests
+const std::size_t tries = 10;
+const std::size_t ticks = 20000; // each tick is 0.75 to 1.25 ms
+//helpers for logging
+#ifdef CREATE_LOGFILES
+ #define LOG_CONTROLLER_DATA_WRITE_TO(name) change_logging_file(name)
+ #define LOG_CONTROLLER_DATA(...) f << __VA_ARGS__ << "\n"
+
+ #include <boost/filesystem.hpp>
+ #include <fstream>
+
+ static const boost::filesystem::path fpath{"controller_logfiles/"};
+ static std::ofstream f;
+ static bool isSetup = false;
+
+ void change_logging_file(const std::string& name)
+ {
+ if(f.is_open())
+ {
+ f.close();
+ }
+ if(!isSetup)
+ {
+ boost::filesystem::create_directories(fpath);
+ //create the python evaluation file
+ boost::filesystem::path tmppath(fpath/"eval.py");
+ f.open(tmppath.string());
+ f<<R"raw_str_delim(
+def consume_file(fname, save=True, show=True):
+ #get data
+ with open(fname, 'r') as f:
+ data = [ x.split(' ') for x in f.read().split('\n') if x != '']
+ #plot
+ from mpl_toolkits.axes_grid1 import host_subplot
+ import mpl_toolkits.axisartist as AA
+ import matplotlib.pyplot as plt
+
+ pos = host_subplot(111, axes_class=AA.Axes)
+ plt.subplots_adjust(right=0.75)
+ vel = pos.twinx()
+ acc = pos.twinx()
+
+
+ pos.axhline(0, color='black')
+ vel.axhline(0, color='black')
+ acc.axhline(0, color='black')
+
+ offset = 60
+ acc.axis['right'] = acc.get_grid_helper().new_fixed_axis(loc='right',
+ axes=acc,
+ offset=(offset, 0))
+
+ c={pos:'red', vel:'blue', acc:'green'}
+ b={pos:0 , vel:0 , acc:0 }
+
+ pos.set_xlabel('Time [s]')
+ pos.set_ylabel('Position')
+ vel.set_ylabel('Velocity')
+ acc.set_ylabel('Acceleration')
+
+ pos.axis['left'].label.set_color(c[pos])
+ vel.axis['right'].label.set_color(c[vel])
+ acc.axis['right'].label.set_color(c[acc])
+
+
+ def get(name,factor=1):
+ if name in data[0]:
+ return [factor*float(x[data[0].index(name)]) for x in data[1:]]
+
+ times=get('time')
+
+ def add(plt,name,factor=1, clr=None, style='-'):
+ d=get(name,factor)
+ if d is None or [0]*len(d) == d:
+ return
+ if clr is None:
+ clr=c[plt]
+ plt.plot(times, d, style,color=clr)
+ b[plt] = max([b[plt]]+[abs(x) for x in d])
+ plt.set_ylim(-b[plt]*1.1, b[plt]*1.1)
+
+ add(pos,'curpos',clr='red')
+ add(pos,'targpos',clr='red', style='-.')
+ add(pos,'posHi',clr='darkred', style='--')
+ add(pos,'posLo',clr='darkred', style='--')
+ add(pos,'posHiHard',clr='darkred', style='--')
+ add(pos,'posLoHard',clr='darkred', style='--')
+ add(pos,'brakingDist',clr='orange', style=':')
+ add(pos,'posAfterBraking',clr='magenta', style=':')
+
+ add(vel,'curvel',clr='teal')
+ add(vel,'targvel',clr='teal', style='-.')
+ add(vel,'maxv',clr='blue', style='--')
+ add(vel,'maxv',factor=-1,clr='blue', style='--')
+
+ add(acc,'curacc',clr='green')
+ add(acc,'acc',clr='darkgreen', style='--')
+ add(acc,'dec',clr='darkgreen', style='--')
+
+ plt.draw()
+ if save: plt.savefig(fname+'.png')
+ if show: plt.show()
+ if not show: plt.close()
+
+
+import sys
+import os
+def handle_path(p, show=True):
+ if '.' in p:
+ return
+ if os.path.isfile(p):
+ consume_file(p,show=show)
+ if os.path.isdir(p):
+ show=False
+ for subdir, dirs, files in os.walk(p):
+ for f in files:
+ handle_path(subdir+f,show=show)
+
+if len(sys.argv) >= 2:
+ handle_path(sys.argv[1])
+else:
+ handle_path('./')
+ )raw_str_delim";
+ isSetup=true;
+ f.close();
+ }
+ boost::filesystem::path tmppath(fpath/name);
+ f.open(tmppath.string());
+ std::cout<<"now writing to: " << boost::filesystem::absolute(tmppath).string()<<"\n";
+ }
+
+#else
+ #define LOG_CONTROLLER_DATA_WRITE_TO(name) do{}while(0)
+ #define LOG_CONTROLLER_DATA(...) do{}while(0)
+#endif
+
+
+unsigned int getSeed()
+{
+ static const auto seed = std::random_device{}();
+ std::cout << "seed = " << seed << std::endl;
+ return seed;
+}
+
+static std::mt19937 gen{getSeed()};
+
+struct Simulation
+{
+ float time=0;
+ float dt=0;
+ float maxDt = 0.001;
+
+ float curpos=0;
+ float oldpos=0;
+ float targpos=0;
+ float posHiHard= M_PI;
+ float posLoHard=-M_PI;
+ float posHi= M_PI;
+ float posLo=-M_PI;
+
+ float curvel=0;
+ float oldvel=0;
+ float targvel=0;
+ float maxvel = 10;
+
+ float curacc=0;
+ float oldacc=0;
+ float acc=0;
+ float dec=0;
+
+ float brakingDist=0;
+ float posAfterBraking=0;
+
+
+ std::uniform_real_distribution<float> vDist;
+ std::uniform_real_distribution<float> aDist;
+ std::uniform_real_distribution<float> pDist;
+ std::uniform_real_distribution<float> tDist;
+
+ void reset()
+ {
+ time=0;
+
+ curpos=0;
+ oldpos=0;
+ targpos=0;
+
+ curvel=0;
+ oldvel=0;
+ targvel=0;
+
+ curacc=0;
+ oldacc=0;
+ acc=0;
+ dec=0;
+
+ brakingDist=0;
+ posAfterBraking=0;
+
+ vDist = std::uniform_real_distribution<float>{-maxvel, maxvel};
+ aDist = std::uniform_real_distribution<float>{maxvel/4, maxvel*4 };
+ tDist = std::uniform_real_distribution<float>{maxDt*0.75f, maxDt*1.25f};
+ pDist = std::uniform_real_distribution<float>{posLoHard, posHiHard};
+ }
+
+ //updating
+ template<class FNC>
+ void tick(FNC& callee)
+ {
+ dt=tDist(gen);
+ callee();
+ log();
+ }
+
+ void updateVel(float newvel)
+ {
+ BOOST_CHECK(std::isfinite(newvel));
+ //save old
+ oldvel = curvel;
+ oldpos = curpos;
+ oldacc = curacc;
+
+ //update
+ curvel = newvel;
+ curacc=std::abs(curvel-oldvel)/dt;
+ curpos+=curvel*dt;
+ time += dt;
+ brakingDist=brakingDistance(curvel,dec);
+ posAfterBraking=curpos+brakingDist;
+ }
+
+ //checks
+ void checkVel()
+ {
+ //check v
+ BOOST_CHECK_LE(curvel,maxvel*1.01);
+ BOOST_CHECK_LE(-maxvel*1.01,curvel);
+ }
+
+ void checkPos()
+ {
+ BOOST_CHECK_LE(curpos,posHiHard);
+ BOOST_CHECK_LE(posLoHard,curpos);
+ }
+
+ void checkAcc()
+ {
+ if(sign(curvel) != sign(oldvel))
+ {
+// std::cout<< "Time["<<time<<"] velocity changed sign (from/to):" << sign(oldvel) << " / " << sign(curvel)<<"\n";
+ return;
+ }
+ if(std::abs(oldvel)>std::abs(curvel))
+ {
+ //we decellerated
+ if(!(curacc < dec*1.01))
+ {
+ std::cout << "Time["<<time<<"] violated deceleration bound! vold " << oldvel << " / vnew " << curvel << " / dv " << std::abs(oldvel-curvel) << " / dt " << dt<< " / dec " <<dec
+ <<" / (targetv " << targvel<<")\n";
+ }
+ BOOST_CHECK_LE(curacc,dec*1.01);
+ }
+ else
+ {
+ //we accellerated
+ if(!(curacc < acc*1.01))
+ {
+ std::cout << "Time["<<time<<"] violated deceleration bound! vold " << oldvel << " / vnew " << curvel << " / dv " << std::abs(oldvel-curvel) << " / dt " << dt<< " / acc " <<acc
+ <<" / (targetv " << targvel<<")\n";
+ }
+ BOOST_CHECK_LE(curacc,acc*1.01);
+ }
+ }
+
+ //logging
+ void writeHeader(const std::string& name)
+ {
+ LOG_CONTROLLER_DATA_WRITE_TO(name);
+ LOG_CONTROLLER_DATA("time curpos targpos posHiHard posLoHard posHi posLo curvel targvel maxvel curacc acc dec brakingDist posAfterBraking");
+ reset();
+ }
+ void log()
+ {
+ //output a neg val for dec and a pos val for acc
+ float outputacc;
+ if(sign(curvel) != sign(oldvel))
+ {
+ // cant determine the current acceleration correctly (since we have an acceleration and deceleration phase)
+ outputacc = 0;
+ }
+ else
+ {
+ outputacc = curacc;
+ if(std::abs(oldvel)>std::abs(curvel))
+ {
+ //we decelerated -> negative sign
+ outputacc *= -1;
+ }
+ }
+ LOG_CONTROLLER_DATA(time<<" "<<
+ curpos<<" "<<targpos<<" "<<posHiHard<<" "<<posLoHard<<" "<<posHi<<" "<<posLo<<" "<<
+ curvel<<" "<<targvel<<" "<<maxvel<<" "<<
+ outputacc<<" "<<acc<<" "<<-dec<<" "<<
+ brakingDist<<" "<<posAfterBraking);
+ }
+};
+
+
+BOOST_AUTO_TEST_CASE(velocityControlWithAccelerationBoundsTest)
+{
+ std::cout<< "starting velocityControlWithAccelerationBoundsTest \n";
+ Simulation s;
+ s.posHi=0;
+ s.posLo=0;
+ s.posHiHard=0;
+ s.posLoHard=0;
+
+ float directSetVLimit = 0.005;
+
+ auto testTick= [&]
+ {
+ s.updateVel(velocityControlWithAccelerationBounds(s.dt, s.maxDt, s.curvel, s.targvel, s.maxvel, s.acc, s.dec, directSetVLimit));
+ s.checkVel();
+ s.checkAcc();
+ };
+
+ std::cout<< "random tests\n";
+ for(std::size_t try_ = 0; try_ < tries; ++ try_)
+ {
+ s.writeHeader("velCtrl+acc_random_"+std::to_string(try_));
+ s.curvel=s.vDist(gen);
+ s.targvel=s.vDist(gen);
+ s.acc=s.aDist(gen);
+ s.dec=s.aDist(gen);
+ directSetVLimit = (std::min(s.acc,s.dec)*s.maxDt*0.75f/2.f) * 0.99f; // vlimit = accmin*dtmin/2
+
+ s.log();
+ for(std::size_t tick = 0; tick < ticks; ++tick)
+ {
+ s.tick(testTick);
+ }
+ BOOST_CHECK_EQUAL(s.curvel, s.targvel);
+ }
+ std::cout<< "bounds tests\n";
+ std::cout<< "TODO\n";
+ ///TODO
+
+ std::cout<< "done velocityControlWithAccelerationBoundsTest \n";
+}
+
+BOOST_AUTO_TEST_CASE(velocityControlWithAccelerationAndPositionBoundsTest)
+{
+ std::cout<< "starting velocityControlWithAccelerationAndPositionBoundsTest \n";
+ Simulation s;
+
+ const float positionSoftLimitViolationVelocity = 0.1;
+ float directSetVLimit = 0.005;
+ s.posLo=s.posLoHard*0.99;
+ s.posHi=s.posHiHard*0.99;
+
+ auto testTick=[&]
+ {
+ s.updateVel(velocityControlWithAccelerationAndPositionBounds(
+ s.dt, s.maxDt,
+ s.curvel, s.targvel, s.maxvel,
+ s.acc, s.dec,
+ directSetVLimit,
+ s.curpos,
+ s.posLo, s.posHi,
+ positionSoftLimitViolationVelocity,
+ s.posLoHard, s.posHiHard
+ ));
+ s.checkPos();
+ s.checkVel();
+ };
+
+ std::cout<< "random tests\n";
+ for(std::size_t try_ = 0; try_ < tries; ++ try_)
+ {
+ s.writeHeader("velCtrl+acc+pos_random_"+std::to_string(try_));
+ s.curvel=s.vDist(gen);
+ s.curpos=s.pDist(gen);
+ s.targvel=s.vDist(gen);
+ s.acc=s.aDist(gen);
+ s.dec=s.aDist(gen);
+ directSetVLimit = (std::min(s.acc,s.dec)*s.maxDt*0.75f/2.f) * 0.99f; // vlimit = accmin*dtmin/2
+ s.log();
+ for(std::size_t tick = 0; tick < ticks; ++tick)
+ {
+ s.tick(testTick);
+ }
+ }
+ std::cout<< "bounds tests\n";
+ std::cout<< "TODO\n";
+ ///TODO
+
+ std::cout<< "done velocityControlWithAccelerationAndPositionBoundsTest \n";
+}
+
+BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsTest)
+{
+ std::cout<< "starting positionThroughVelocityControlWithAccelerationBounds \n";
+ Simulation s;
+ s.posHi=0;
+ s.posLo=0;
+
+// float p = 0.05;
+// const float pControlPosErrorLimit = 0.02;
+// const float pControlVelLimit = 0.02;
+ float p = 0.1;
+ const float pControlPosErrorLimit = 0.01;
+ const float pControlVelLimit = 0.01;
+
+ auto testTick=[&]
+ {
+ s.updateVel(positionThroughVelocityControlWithAccelerationBounds(
+ s.dt, s.maxDt,
+ s.curvel, s.maxvel,
+ s.acc, s.dec,
+ s.curpos, s.targpos,
+ pControlPosErrorLimit, pControlVelLimit, p
+ ));
+ s.checkVel();
+ s.checkAcc();
+ };
+
+ std::cout<< "random tests\n";
+ for(std::size_t try_ = 0; try_ < tries; ++ try_)
+ {
+ s.writeHeader("posViaVelCtrl+acc_random_"+std::to_string(try_));
+ s.curvel=s.vDist(gen);
+ s.curpos=s.pDist(gen);
+ s.targpos=s.pDist(gen);
+ s.acc=s.aDist(gen);
+ s.dec=s.aDist(gen);
+// p = ((std::min(s.acc,s.dec)*s.maxDt*0.75-pControlVelLimit)/pControlPosErrorLimit) * 0.99f; // sometimes <0
+ s.log();
+ for(std::size_t tick = 0; tick < ticks; ++tick)
+ {
+ s.tick(testTick);
+ }
+ BOOST_CHECK_LE(std::abs(s.curpos-s.targpos), 0.01);// allow error of 0.5729577951308232 deg
+ }
+ std::cout<< "bounds tests\n";
+ std::cout<< "TODO\n";
+ ///TODO
+
+ std::cout<< "done positionThroughVelocityControlWithAccelerationBounds \n";
+}
+
+BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationAndPositionBoundsTest)
+{
+ std::cout<< "starting positionThroughVelocityControlWithAccelerationAndPositionBounds \n";
+ Simulation s;
+ s.posHi=0;
+ s.posLo=0;
+ // float p = 0.05;
+ // const float pControlPosErrorLimit = 0.02;
+ // const float pControlVelLimit = 0.02;
+ float p = 0.1;
+ const float pControlPosErrorLimit = 0.01;
+ const float pControlVelLimit = 0.01;
+
+ auto testTick=[&]
+ {
+ s.updateVel(positionThroughVelocityControlWithAccelerationAndPositionBounds(
+ s.dt, s.maxDt,
+ s.curvel, s.maxvel,
+ s.acc, s.dec,
+ s.curpos, s.targpos,
+ pControlPosErrorLimit, pControlVelLimit, p,
+ s.posLoHard, s.posHiHard));
+ s.checkPos();
+ };
+
+ std::cout<< "random tests\n";
+ for(std::size_t try_ = 0; try_ < tries; ++ try_)
+ {
+ s.writeHeader("posViaVelCtrl+acc+pos_random_"+std::to_string(try_));
+ s.curvel=s.vDist(gen);
+ s.curpos=s.pDist(gen);
+ s.targpos=s.pDist(gen);
+ s.acc=s.aDist(gen);
+ s.dec=s.aDist(gen);
+ // p = ((std::min(s.acc,s.dec)*s.maxDt*0.75-pControlVelLimit)/pControlPosErrorLimit) * 0.99f; // sometimes <0
+ s.log();
+ for(std::size_t tick = 0; tick < ticks; ++tick)
+ {
+ s.tick(testTick);
+ }
+ BOOST_CHECK_LE(std::abs(s.curpos-s.targpos), 0.05);
+ }
+ std::cout<< "bounds tests\n";
+ std::cout<< "TODO\n";
+ ///TODO
+
+ std::cout<< "done positionThroughVelocityControlWithAccelerationAndPositionBounds \n";
+}
+
+BOOST_AUTO_TEST_CASE(positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPositionTest)
+{
+ std::cout<< "starting positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition \n";
+ Simulation s;
+ s.posHi=0;
+ s.posLo=0;
+ // float p = 0.05;
+ // const float pControlPosErrorLimit = 0.02;
+ // const float pControlVelLimit = 0.02;
+ float p = 0.1;
+ const float pControlPosErrorLimit = 0.01;
+ const float pControlVelLimit = 0.01;
+ float direction;
+
+ auto testTick=[&]
+ {
+ s.updateVel(positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition(
+ s.dt, s.maxDt,
+ s.curvel, s.maxvel,
+ s.acc, s.dec,
+ s.curpos, s.targpos,
+ pControlPosErrorLimit, pControlVelLimit, p,
+ direction,
+ s.posLoHard, s.posHiHard));
+ s.checkVel();
+ s.checkAcc();
+ s.curpos=periodicClamp(s.curpos,s.posLoHard, s.posHiHard);
+ };
+
+ auto rngTestRuns=[&](int d)
+ {
+ std::cout<< "random tests (dir="<<std::to_string(d)<<"\n";
+ for(std::size_t try_ = 0; try_ < tries; ++ try_)
+ {
+ s.writeHeader("posViaVelCtrl+acc+periodicPos+dir"+std::to_string(d)+"_random_"+std::to_string(try_));
+ direction = d;
+ s.curvel=s.vDist(gen);
+ s.curpos=periodicClamp(s.pDist(gen),s.posLoHard, s.posHiHard);
+ s.targpos=periodicClamp(s.pDist(gen),s.posLoHard, s.posHiHard);
+ s.acc=s.aDist(gen);
+ s.dec=s.aDist(gen);
+ s.log();
+ for(std::size_t tick = 0; tick < ticks; ++tick)
+ {
+ s.tick(testTick);
+ }
+ BOOST_CHECK_LE(std::abs(s.curpos-s.targpos), 0.01);
+ BOOST_CHECK_LE(std::abs(s.curvel), 0.1);
+ }
+
+ };
+ rngTestRuns(0);
+ rngTestRuns(1);
+ rngTestRuns(-1);
+
+
+ std::cout<< "bounds tests\n";
+ std::cout<< "TODO\n";
+ ///TODO
+
+ std::cout<< "done positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition \n";
+}
+
diff --git a/source/RobotAPI/libraries/RTRobotUnit/test/CMakeLists.txt b/source/RobotAPI/libraries/RTRobotUnit/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..c8695b9ab1c8703fc789a737ddffc79c28b290e7
--- /dev/null
+++ b/source/RobotAPI/libraries/RTRobotUnit/test/CMakeLists.txt
@@ -0,0 +1,4 @@
+set(LIBS ${LIBS} RTRobotUnit )
+
+armarx_add_test(BasicControllerTest BasicControllerTest.cpp "${LIBS}")
+
diff --git a/source/RobotAPI/libraries/core/PIDController.cpp b/source/RobotAPI/libraries/core/PIDController.cpp
index 23f81420420b19cae3845b03fab425400310b6f6..106f8a723c75bc884a6efd8323cb4965cd325a3a 100644
--- a/source/RobotAPI/libraries/core/PIDController.cpp
+++ b/source/RobotAPI/libraries/core/PIDController.cpp
@@ -39,9 +39,9 @@ PIDController::PIDController(float Kp, float Ki, float Kd, double maxControlValu
processValue(0),
target(0),
controlValue(0),
+ controlValueDerivation(0),
maxControlValue(maxControlValue),
- maxDerivation(maxDerivation),
- controlValueDerivation(0)
+ maxDerivation(maxDerivation)
{
reset();
}