diff --git a/source/RobotAPI/components/units/CMakeLists.txt b/source/RobotAPI/components/units/CMakeLists.txt
index 7c99e406382161c50b689c9f1a5bea1c64b317ee..fe8179da6045e7307ef47cb9fb78d0ffc7ff9465 100644
--- a/source/RobotAPI/components/units/CMakeLists.txt
+++ b/source/RobotAPI/components/units/CMakeLists.txt
@@ -86,3 +86,4 @@ set(LIB_FILES
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
add_subdirectory(relays)
+add_subdirectory(RobotUnit)
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6e7e8235191760481ccb3e8cfbef38585f7723c6
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.cpp
@@ -0,0 +1,724 @@
+/*
+ * 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>
+#include <ArmarXCore/core/logging/Logging.h>
+
+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 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 p
+ )
+ {
+ dt = std::min(std::abs(dt), std::abs(maxDt));
+ maxV = std::abs(maxV);
+ acceleration = std::abs(acceleration);
+ deceleration = std::abs(deceleration);
+ const float signV = sign(currentV);
+ //we can have 3 cases:
+ // 1. we use a p controller and ignore acc/dec (if velocity target from p controller < velocity target from ramp controller)
+ // 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;
+ float newTargetVelPController = 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);
+
+ float newTargetVel = boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
+ bool usePID = std::abs(newTargetVelPController) < std::abs(newTargetVel);
+ // ARMARX_INFO << deactivateSpam(0.2) << VAROUT(usePID) << VAROUT(dt) << VAROUT(decelerate) << VAROUT(usedacc) << VAROUT(maxDeltaV) << VAROUT(deltaVel) << VAROUT(newTargetVel) << VAROUT(newTargetVelPController)
+ // << VAROUT(currentPosition) << VAROUT(targetPosition);
+ if (usePID)
+ {
+ return newTargetVelPController;
+ }
+ else
+ {
+ return newTargetVel;
+ }
+ }
+
+ float positionThroughVelocityControlWithAccelerationAndPositionBounds(
+ float dt, float maxDt,
+ float currentV, float maxV,
+ float acceleration, float deceleration,
+ float currentPosition, float targetPosition,
+ 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);
+ 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),
+ p
+ );
+ }
+
+ ////////////////////////////
+ //wip
+
+
+ float positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition(
+ float dt, float maxDt,
+ float currentV, float maxV,
+ float acceleration, float deceleration,
+ float currentPosition, float targetPosition,
+ float pControlPosErrorLimit, 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,
+ 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,
+ p
+ );
+ }
+
+ bool VelocityControllerWithAccelerationBounds::validParameters() const
+ {
+ return maxV > 0 &&
+ acceleration > 0 &&
+ deceleration > 0 &&
+ targetV <= maxV &&
+ targetV >= -maxV;
+ }
+
+ float VelocityControllerWithAccelerationBounds::run() const
+ {
+ const float useddt = std::min(std::abs(dt), std::abs(maxDt));
+
+ //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 * useddt);
+ 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 * useddt, -maxDeltaV, maxDeltaV);
+ const float nextV = currentV + deltaVel;
+ return nextV;
+ }
+
+ float VelocityControllerWithAccelerationBounds::estimateTime() const
+ {
+ const float curverror = std::abs(targetV - currentV);
+ if (curverror < directSetVLimit)
+ {
+ return maxDt;
+ }
+ return curverror / ((targetV > currentV) ? acceleration : deceleration);
+ }
+
+ bool VelocityControllerWithAccelerationAndPositionBounds::validParameters() const
+ {
+ return VelocityControllerWithAccelerationBounds::validParameters() &&
+ positionLimitLoHard < positionLimitLoSoft &&
+ positionLimitLoSoft < positionLimitHiSoft &&
+ positionLimitHiSoft < positionLimitHiHard;
+ }
+
+ float VelocityControllerWithAccelerationAndPositionBounds::run() const
+ {
+ 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));
+
+ //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 = VelocityControllerWithAccelerationBounds::run();
+ }
+ 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;
+ }
+
+ bool PositionThroughVelocityControllerWithAccelerationBounds::validParameters() const
+ {
+ return maxV > 0 &&
+ acceleration > 0 &&
+ deceleration > 0 &&
+ pControlPosErrorLimit > 0 &&
+ pControlVelLimit > 0 &&
+ p > 0;
+ }
+
+ float PositionThroughVelocityControllerWithAccelerationBounds::run() const
+ {
+ const float useddt = std::min(std::abs(dt), std::abs(maxDt));
+ 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 * useddt);
+ const float deltaVel = boost::algorithm::clamp(signV * usedacc * useddt, -maxDeltaV, maxDeltaV);
+ return boost::algorithm::clamp(currentV + deltaVel, -maxV, maxV);
+ }
+
+ float PositionThroughVelocityControllerWithAccelerationBounds::estimateTime() const
+ {
+ // const float brakingDistance = sign(currentV) * currentV * currentV / 2.f / deceleration;
+ const float posError = targetPosition - currentPosition;
+ const float posIfBrakingNow = currentPosition + brakingDistance(currentV, deceleration);
+ //we are near the goal if we break now
+ if (std::abs(posIfBrakingNow - targetPosition) <= pControlPosErrorLimit && currentV <= pControlVelLimit)
+ {
+ return std::sqrt(2 * std::abs(brakingDistance(currentV, deceleration)) / deceleration);
+ }
+ const float posErrorIfBrakingNow = targetPosition - posIfBrakingNow;
+
+ float t = 0;
+ float curVel = currentV;
+ float curPos = currentPosition;
+ auto curPosEr = [&] {return targetPosition - curPos;};
+ // auto curBrake = [&] {return std::abs(brakingDistance(curVel, deceleration));};
+
+ //check for overshooting of target
+ if (sign(posError) != sign(posErrorIfBrakingNow))
+ {
+ t = currentV / deceleration;
+ curVel = 0;
+ curPos = posIfBrakingNow;
+ }
+ //check for to high v
+ if (std::abs(curVel) > maxV)
+ {
+ const float dv = curVel - maxV;
+ t += dv / deceleration;
+ curPos += 0.5f / deceleration * dv * dv * sign(curVel);
+ }
+ //curBrake()<=curPosEr() && curVel <= maxV
+ auto tr = trapeze(curVel, acceleration, maxV, deceleration, 0, curPosEr());
+ return t + tr.at(0).dt + tr.at(1).dt + tr.at(2).dt;
+ // //where are we if we acc to max and then dec
+ // const deltas acc = accetlerateToVelocity(curVel, acceleration, maxV);
+ // const deltas dec = accetlerateToVelocity(maxV , deceleration, 0);
+ // const float dist = acc.dx + dec.dx;
+ // if (std::abs(dist - curPosEr()) <= pControlPosErrorLimit)
+ // {
+ // //after this we are at the target
+ // return t + acc.dt + dec.dt;
+ // }
+ // if (dist < curPosEr())
+ // {
+ // //calc how long to acc + dec
+ // return t + acc.dt + dec.dt;
+ // }
+ // if (dist > curPosEr())
+ // {
+ // return t + acc.dt + dec.dt + (dist - curPosEr()) / maxV;
+ // }
+
+ // const float posErrorAfterBraking = curPosEr - curBrake;
+ // return t;
+
+ }
+
+ bool PositionThroughVelocityControllerWithAccelerationAndPositionBounds::validParameters() const
+ {
+ return PositionThroughVelocityControllerWithAccelerationBounds::validParameters() &&
+ positionLimitLo < positionLimitHi &&
+ targetPosition <= positionLimitHi &&
+ positionLimitLo <= targetPosition;
+ }
+
+ float PositionThroughVelocityControllerWithAccelerationAndPositionBounds::run() const
+ {
+ const float useddt = std::min(std::abs(dt), std::abs(maxDt));
+ 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 * useddt;
+ return vel;
+ }
+
+ //handle case 2-3
+ return PositionThroughVelocityControllerWithAccelerationBounds::run();
+ }
+
+ std::array<deltas, 3> trapeze(float v0, float acc, float vMax, float dec, float vt, float dx)
+ {
+ acc = std::abs(acc);
+ vMax = std::abs(vMax);
+ dec = std::abs(dec);
+ auto calc = [&](float vmax)
+ {
+ const deltas dacc = accetlerateToVelocity(v0 , acc, vmax);
+ const deltas ddec = accetlerateToVelocity(vmax, dec, vt);
+ const float dxMax = dacc.dx + ddec.dx;
+ if (sign(dxMax) == sign(dx))
+ {
+ if (abs(dxMax) <= dx)
+ {
+ deltas mid;
+ mid.dx = dxMax - dx;
+ mid.dv = vMax;
+ mid.dt = mid.dx / mid.dv;
+ return std::make_pair(true, std::array<deltas, 3> {dacc, mid, ddec});
+ }
+ //we need a lower vMax (vx)
+ //in all following formulas # elem {0,t}
+ //a0=acc , at = dec
+ //vx=v0+dv0 = vt + dvt -> dv# = vx-v#
+ //dx=dx0+dxt
+ //dx#=dv#^2/2/a#+dv#/a#*v#
+ //dx#=(vx-v#)^2/2/a#+(vx-v#)/a#*v#
+ //dx#=vx^2*(1/2/a#) + vx*(v#/2 - v#/a#) + (v#^2/2/a# - v#^2/2)
+ //dx=vx^2*(1/2/a0+1/2/at) + vx*(v0/2 - v0/a0 + vt/2 - vt/at) + (v0^2/2/a0 - v0^2/2 +vt^2/2/at - vt^2/2)
+ //0 =vx^2*(1/2/a0+1/2/at) + vx*(v0/2 - v0/a0 + vt/2 - vt/at) + (v0^2/2/a0 - v0^2/2 +vt^2/2/at - vt^2/2 -dx)
+ //div = (1/2/a0 + 1/2/at )
+ //p = (v0/2 - v0/a0 + vt/2 - vt/at )/div
+ //q = (v0^2/2/a0 - v0^2/2 + vt^2/2/at - vt^2/2 -dx)/div
+ // -> vx1/2 = pq(p,q)
+ const float a0 = acc;
+ const float at = dec;
+ const float div = (1.f / 2.f / a0 + 1.f / 2.f / at);
+ const float p = (v0 / 2.f - v0 / a0 + vt / 2.f - vt / at) / div;
+ const float q = (v0 * v0 / 2.f * (1.f / a0 - 1.f) + vt * vt / 2.f * (1.f / at - 1.f) - dx) / div;
+ const auto vxs = pq(p, q);
+ //one or both of the results may be invalid
+ bool vx1Valid = std::isfinite(vxs.first) && std::abs(vxs.first) <= std::abs(vmax);
+ bool vx2Valid = std::isfinite(vxs.second) && std::abs(vxs.second) <= std::abs(vmax);
+ switch (vx1Valid + vx2Valid)
+ {
+ case 0:
+ return std::make_pair(false, std::array<deltas, 3>());
+ case 1:
+ {
+ float vx = vx1Valid ? vxs.first : vxs.second;
+ const deltas daccvx = accetlerateToVelocity(v0, acc, vx);
+ const deltas ddecvx = accetlerateToVelocity(vx, dec, vt);
+ deltas mid;
+ mid.dx = 0;
+ mid.dv = vx;
+ mid.dt = 0;
+ return std::make_pair(false, std::array<deltas, 3> {daccvx, mid, ddecvx});
+ }
+ case 2:
+ {
+ const deltas daccvx1 = accetlerateToVelocity(v0, acc, vxs.first);
+ const deltas ddecvx1 = accetlerateToVelocity(vxs.first, dec, vt);
+ const deltas daccvx2 = accetlerateToVelocity(v0, acc, vxs.second);
+ const deltas ddecvx2 = accetlerateToVelocity(vxs.second, dec, vt);
+ deltas mid;
+ mid.dx = 0;
+ mid.dt = 0;
+ if (daccvx1.dt + ddecvx1.dt < daccvx2.dt + ddecvx2.dt)
+ {
+ mid.dv = vxs.first;
+ return std::make_pair(false, std::array<deltas, 3> {daccvx1, mid, ddecvx1});
+ }
+ mid.dv = vxs.second;
+ return std::make_pair(false, std::array<deltas, 3> {daccvx2, mid, ddecvx2});
+ }
+ default:
+ throw std::logic_error {"unreachable code (bool + bool neither 0,1,2)"};
+ }
+ }
+ return std::make_pair(false, std::array<deltas, 3>());
+ };
+
+ const auto plusVMax = calc(vMax);
+ const auto negVMax = calc(-vMax);
+ switch (plusVMax.first + negVMax.first)
+ {
+ case 0:
+ throw std::invalid_argument("could not find a trapez to reach the goal");
+ case 1:
+ return plusVMax.first ? plusVMax.second : negVMax.second;
+ case 2:
+ {
+ const float dt1 = plusVMax.second.at(0).dt + plusVMax.second.at(1).dt + plusVMax.second.at(2).dt;
+ const float dt2 = negVMax .second.at(0).dt + negVMax .second.at(1).dt + negVMax .second.at(2).dt;
+ return dt1 < dt2 ? plusVMax.second : negVMax.second;
+ }
+ default:
+ throw std::logic_error {"unreachable code (bool + bool neither 0,1,2)"};
+ }
+ }
+
+ float PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition::run() const
+ {
+ //run should be static, but we need to adapt the parabs for the subcontroler -> have the subcontroler as value;
+ PositionThroughVelocityControllerWithAccelerationBounds sub = *this; // slice parent class from this
+ sub.currentPosition = periodicClamp(currentPosition, positionPeriodLo, positionPeriodHi);
+ sub.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 PositionThroughVelocityControllerWithAccelerationBounds::run();
+ }
+
+ //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
+ sub.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);
+
+ ///TODO check if there may be a case requiring the direction
+ float direction = 0;
+ if (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
+ sub.targetPosition = (overshoot - std::min(0.f, -direction)) * positionPeriodLength; // - direction * pControlPosErrorLimit;
+
+ //move
+ return sub.run();
+ }
+
+
+
+
+
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/BasicControllers.h b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
new file mode 100644
index 0000000000000000000000000000000000000000..a63c1dbc365795f7d387be4b11e40bc729caf82e
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/BasicControllers.h
@@ -0,0 +1,204 @@
+/*
+ * 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 <type_traits>
+#include <ArmarXCore/core/util/algorithm.h>
+
+namespace armarx
+{
+ template<class T, class = typename std::enable_if<std:: is_floating_point<T>::value>::type >
+ T periodicClamp(T value, T periodLo, T periodHi)
+ {
+ float dist = periodHi - periodLo;
+ return std::fmod(value - periodLo + dist, dist) + periodLo;
+ }
+
+ template<class T, class = typename std::enable_if<std:: is_floating_point<T>::value>::type >
+ std::pair<T, T> pq(T p, T q)
+ {
+ T a = - p / 2;
+ T b = std::sqrt(std::pow(p / 2, 2) - q);
+ return {a + b, a - b};
+ }
+
+ struct deltas
+ {
+ float dv;
+ float dx;
+ float dt;
+ };
+
+ inline deltas accetlerateToVelocity(float v0, float acc, float vt)
+ {
+ acc = std::abs(acc);
+ deltas d;
+ d.dv = vt - v0;
+ d.dt = std::abs(d.dv / acc);
+ d.dx = sign(d.dv) * d.dv * d.dv / 2.f / acc + v0 * d.dt;
+ return d;
+ }
+
+
+ std::array<deltas, 3> trapeze(float v0, float acc, float vMax, float dec, float vt, float dx);
+
+ // inline std::array<deltas, 3> trapezeWithDt(float v0, float acc, float vMax, float dec, float vt, float dx, float dt)
+ // {
+
+ // }
+
+ /**
+ * @param v0 The initial velocity.
+ * @param deceleration The deceleration.
+ * @return The braking distance given the parameters.
+ */
+ inline float brakingDistance(float v0, float deceleration)
+ {
+ return accetlerateToVelocity(v0, deceleration, 0).dx;
+ }
+
+
+
+ struct VelocityControllerWithAccelerationBounds
+ {
+ float dt;
+ float maxDt;
+ float currentV;
+ float targetV;
+ float maxV;
+ float acceleration;
+ float deceleration;
+ float directSetVLimit;
+
+ bool validParameters() const;
+ float run() const;
+ float estimateTime() const;
+ };
+
+ 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.
+ */
+ struct VelocityControllerWithAccelerationAndPositionBounds: VelocityControllerWithAccelerationBounds
+ {
+ float currentPosition;
+ float positionLimitLoSoft;
+ float positionLimitHiSoft;
+ float positionLimitLoHard;
+ float positionLimitHiHard;
+
+ bool validParameters() const;
+ float run() const;
+ };
+
+
+
+ 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 positionLimitLoHard, float positionLimitHiHard);
+
+
+ struct PositionThroughVelocityControllerWithAccelerationBounds
+ {
+ float dt;
+ float maxDt;
+ float currentV;
+ float maxV;
+ float acceleration;
+ float deceleration;
+ float currentPosition;
+ float targetPosition;
+ float pControlPosErrorLimit;
+ float pControlVelLimit;
+ float p;
+
+ bool validParameters() const;
+ float run() const;
+ float estimateTime() const;
+ };
+ float positionThroughVelocityControlWithAccelerationBounds(float dt, float maxDt,
+ float currentV, float maxV,
+ float acceleration, float deceleration,
+ float currentPosition, float targetPosition,
+ float p);
+
+ struct PositionThroughVelocityControllerWithAccelerationAndPositionBounds: PositionThroughVelocityControllerWithAccelerationBounds
+ {
+ float positionLimitLo;
+ float positionLimitHi;
+
+ bool validParameters() const;
+ float run() const;
+ };
+ float positionThroughVelocityControlWithAccelerationAndPositionBounds(float dt, float maxDt,
+ float currentV, float maxV,
+ float acceleration, float deceleration,
+ float currentPosition, float targetPosition, float p,
+ float positionLimitLo, float positionLimitHi);
+
+ struct PositionThroughVelocityControllerWithAccelerationBoundsAndPeriodicPosition: PositionThroughVelocityControllerWithAccelerationBounds
+ {
+ // float direction;
+ float positionPeriodLo;
+ float positionPeriodHi;
+ float run() const;
+ };
+ float positionThroughVelocityControlWithAccelerationBoundsAndPeriodicPosition(float dt, float maxDt,
+ float currentV, float maxV,
+ float acceleration, float deceleration,
+ float currentPosition, float targetPosition,
+ float pControlPosErrorLimit, float p,
+ float& direction,
+ float positionPeriodLo, float positionPeriodHi);
+}
+
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a84c20b4f17ec336de52cbb4ef8c44592a6a6bf0
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -0,0 +1,91 @@
+set(LIB_NAME RobotUnit)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+set(LIBS
+ ArmarXCoreInterfaces
+ ArmarXCore
+ ArmarXGuiInterfaces
+ RobotAPIUnits
+ RobotAPIInterfaces
+)
+
+set(LIB_FILES
+ SyntaxCheck.cpp
+ RobotUnit.cpp
+ BasicControllers.cpp
+
+ ControlTargets/ControlTargetBase.cpp
+
+ Units/RobotUnitSubUnit.cpp
+ Units/ForceTorqueSubUnit.cpp
+# Units/HapticSubUnit.cpp
+ Units/InertialMeasurementSubUnit.cpp
+ Units/KinematicSubUnit.cpp
+ Units/PlatformSubUnit.cpp
+
+ LVL0Controllers/LVL0Controller.cpp
+
+ LVL1Controllers/LVL1Controller.cpp
+ LVL1Controllers/LVL1TrajectoryController.cpp
+ LVL1Controllers/LVL1KinematicUnitPassThroughController.cpp
+ LVL1Controllers/LVL1HolonomicPlatformUnitVelocityPassThroughController.cpp
+
+# LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.cpp
+# LVL1Controllers/LVL1KinematicUnitPositionController.cpp
+# LVL1Controllers/LVL1KinematicUnitTorqueController.cpp
+# LVL1Controllers/LVL1KinematicUnitVelocityController.cpp
+
+ Devices/SensorDevice.cpp
+ Devices/ControlDevice.cpp
+)
+set(LIB_HEADERS
+ util.h
+ structs.h
+ Constants.h
+ ControlModes.h
+ RobotUnit.h
+ BasicControllers.h
+
+ ControlTargets/ControlTargetBase.h
+ ControlTargets/ControlTarget1DoFActuator.h
+ ControlTargets/ControlTargetHolonomicPlatformVelocity.h
+
+ SensorValues/SensorValueBase.h
+ SensorValues/SensorValue1DoFActuator.h
+ SensorValues/SensorValueIMU.h
+ SensorValues/SensorValueForceTorque.h
+ SensorValues/SensorValueHolonomicPlatform.h
+ SensorValues/SensorValueRTThreadTimings.h
+
+ Units/RobotUnitSubUnit.h
+ Units/ForceTorqueSubUnit.h
+# Units/HapticSubUnit.h
+ Units/InertialMeasurementSubUnit.h
+ Units/KinematicSubUnit.h
+ Units/PlatformSubUnit.h
+
+ LVL0Controllers/LVL0Controller.h
+
+ LVL1Controllers/LVL1Controller.h
+ LVL1Controllers/LVL1Controller.hpp
+ LVL1Controllers/LVL1TrajectoryController.h
+ LVL1Controllers/LVL1KinematicUnitPassThroughController.h
+ LVL1Controllers/LVL1HolonomicPlatformUnitVelocityPassThroughController.h
+# LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.h
+# LVL1Controllers/LVL1KinematicUnitPositionController.h
+# LVL1Controllers/LVL1KinematicUnitTorqueController.h
+# LVL1Controllers/LVL1KinematicUnitVelocityController.h
+
+ Devices/DeviceBase.h
+ Devices/SensorDevice.h
+ Devices/ControlDevice.h
+
+ Devices/RTThreadTimingsSensorDevice.h
+)
+
+add_subdirectory(test)
+
+armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
diff --git a/source/RobotAPI/libraries/RTRobotUnit/Constants.h b/source/RobotAPI/components/units/RobotUnit/Constants.h
similarity index 100%
rename from source/RobotAPI/libraries/RTRobotUnit/Constants.h
rename to source/RobotAPI/components/units/RobotUnit/Constants.h
diff --git a/source/RobotAPI/libraries/RTRobotUnit/ControlModes.h b/source/RobotAPI/components/units/RobotUnit/ControlModes.h
similarity index 64%
rename from source/RobotAPI/libraries/RTRobotUnit/ControlModes.h
rename to source/RobotAPI/components/units/RobotUnit/ControlModes.h
index dc2e580a42bf4301aea8f1fcfc41d965d1865591..8a98aec0f01a7628f1525554bd2438fadfc4a2df 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/ControlModes.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlModes.h
@@ -30,15 +30,17 @@ namespace armarx
namespace ControlModes
{
//'normal' actor modes
- static const std::string PositionMode = "PositionMode";
- static const std::string TorqueMode = "TorqueMode";
- static const std::string VelocityMode = "VelocityMode";
+ static const std::string Position1DoF = "ControlMode_Position1DoF";
+ static const std::string Torque1DoF = "ControlMode_Torque1DoF";
+ static const std::string Velocity1DoF = "ControlMode_Velocity1DoF";
//modes for holonomic platforms
- static const std::string HolonomicPlatformVelocityMode = "HolonomicPlatformVelocityMode";
+ static const std::string HolonomicPlatformVelocity = "ControlMode_HolonomicPlatformVelocity";
+ static const std::string HolonomicPlatformRelativePosition = "ControlMode_HolonomicPlatformRelativePosition";
//special sentinel modes
- static const std::string EmergencyStopMode = "EmergencyStopMode";
+ static const std::string EmergencyStop = "ControlMode_EmergencyStop";
+ static const std::string StopMovement = "ControlMode_StopMovement";
}
}
#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h
new file mode 100644
index 0000000000000000000000000000000000000000..260854a0018d359dce37c2f7f7bf92be0649e34e
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.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::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_ControlTarget1DoFActuator_H
+#define _ARMARX_LIB_RobotAPI_ControlTarget1DoFActuator_H
+
+#include "ControlTargetBase.h"
+#include <ArmarXCore/observers/variant/Variant.h>
+
+namespace armarx
+{
+#define make_ControlTarget1DoFActuator(name,varname,cmode) \
+ class name: public ControlTargetBase \
+ { \
+ public: \
+ float varname = ControllerConstants::ValueNotSetNaN; \
+ virtual const std::string& getControlMode() const override \
+ { \
+ return cmode; \
+ } \
+ virtual void reset() override \
+ { \
+ varname = ControllerConstants::ValueNotSetNaN; \
+ } \
+ virtual bool isValid() const override \
+ { \
+ return std::isfinite(varname); \
+ } \
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override \
+ { \
+ return {{#varname, {new TimedVariant{varname,timestamp}}}}; \
+ } \
+ DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION \
+ }
+
+ make_ControlTarget1DoFActuator(ControlTarget1DoFActuatorPosition, position, ControlModes::Position1DoF);
+ make_ControlTarget1DoFActuator(ControlTarget1DoFActuatorVelocity, velocity, ControlModes::Velocity1DoF);
+ make_ControlTarget1DoFActuator(ControlTarget1DoFActuatorTorque , torque , ControlModes::Torque1DoF);
+
+#undef make_ControlTarget1DoFActuator
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/DataUnits/HapticDataUnit.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.cpp
similarity index 71%
rename from source/RobotAPI/libraries/RTRobotUnit/DataUnits/HapticDataUnit.h
rename to source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.cpp
index de02fbe004fb45c8b8f08eafeac404bae1d9200c..2a48abf52d71f29ac6a9ea09837c96cbba8fb4fc 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/DataUnits/HapticDataUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.cpp
@@ -13,27 +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::HapticDataUnit
+ * @package RobotAPI::ArmarXObjects::ControlTargetBase
* @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_HapticDataUnit_H
-#define _ARMARX_LIB_RobotAPI_HapticDataUnit_H
+#include "ControlTargetBase.h"
namespace armarx
{
- class HapticDataUnitInterface
- {
- public:
- };
-
- class HapticDataUnitPtrProvider: virtual public HapticDataUnitInterface
- {
- public:
- };
}
-
-#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..69f6111757e3d13cfdce324c72dbc743da6b7b2e
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
@@ -0,0 +1,131 @@
+/*
+ * 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::ControlTargetBase
+ * @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_JointControlTargetBase_H
+#define _ARMARX_LIB_RobotAPI_JointControlTargetBase_H
+
+#include <memory>
+#include <string>
+#include <map>
+
+#include <ArmarXCore/observers/variant/TimedVariant.h>
+#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+
+#include "../util.h"
+#include "../Constants.h"
+#include "../ControlModes.h"
+
+namespace armarx
+{
+ /**
+ * @defgroup Library-RobotUnit RobotUnit
+ * @ingroup RobotAPI
+ * A description of the library RobotUnit.
+ *
+ * @class ControlTargetBase
+ * @ingroup Library-RobotUnit
+ * @brief Brief description of class JointControlTargetBase.
+ *
+ * Detailed description of class ControlTargetBase.
+ */
+ class ControlTargetBase
+ {
+ public:
+ virtual void copyTo(ControlTargetBase* target) const = 0;
+ virtual std::unique_ptr<ControlTargetBase> clone() const = 0;
+
+ virtual const std::string& getControlMode() const = 0;
+ virtual std::string getControlTargetType(bool withoutNamespaceSpecifier = false) const = 0;
+ virtual void reset() = 0;
+ virtual bool isValid() const = 0;
+
+ /// @brief used to send the data to the DebugObserverTopic and to other Components (e.g. GUI widgets)
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const
+ {
+ return {};
+ }
+
+ template<class T>
+ bool isA() const
+ {
+ return asA<T>();
+ }
+
+ template<class T>
+ const T* asA() const
+ {
+ return dynamic_cast<const T*>(this);
+ }
+
+ template<class T>
+ T* asA()
+ {
+ return dynamic_cast<T*>(this);
+ }
+
+ template<class T, class = typename std::enable_if<std::is_base_of<ControlTargetBase, T>::value>::type>
+ void copyTo(std::unique_ptr<T>& target) const
+ {
+ copyTo(target.get());
+ }
+
+ virtual ~ControlTargetBase() = default;
+ };
+
+#define DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION \
+ virtual void copyTo(ControlTargetBase* target) const override \
+ { \
+ using this_type = typename std::decay<decltype(*this)>::type; \
+ *(target->asA<this_type>()) = *this; \
+ } \
+ virtual std::unique_ptr<ControlTargetBase> clone() const override \
+ { \
+ using this_type = typename std::decay<decltype(*this)>::type; \
+ std::unique_ptr<ControlTargetBase> c {new this_type}; \
+ ControlTargetBase::copyTo(c); \
+ return c; \
+ } \
+ virtual std::string getControlTargetType(bool withoutNamespaceSpecifier = false) const override \
+ { \
+ return getTypeName(*this, withoutNamespaceSpecifier); \
+ }
+
+#define make_DummyControlTarget(Suffix,ControlMode) \
+ class DummyControlTarget##Suffix : public ControlTargetBase \
+ { \
+ public: \
+ virtual const std::string& getControlMode() const \
+ { \
+ return ControlMode; \
+ } \
+ virtual void reset() {} \
+ virtual bool isValid() const \
+ { \
+ return true; \
+ } \
+ DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION \
+ }
+ make_DummyControlTarget(EmergencyStop, ControlModes::EmergencyStop);
+ make_DummyControlTarget(StopMovement, ControlModes::StopMovement);
+#undef make_DummyControlTarget
+}
+#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/Targets/HolonomicPlatformVelocityTarget.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h
similarity index 53%
rename from source/RobotAPI/libraries/RTRobotUnit/Targets/HolonomicPlatformVelocityTarget.h
rename to source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h
index 09b0923a876d1d5f6392a5bf19c28404c0892024..04572f1ce5c42e58824d9dcc9dd816ceef93e72f 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/Targets/HolonomicPlatformVelocityTarget.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetHolonomicPlatformVelocity.h
@@ -13,40 +13,41 @@
* 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::HolonomicPlatformVelocityTarget
+ * @package RobotAPI::ArmarXObjects::ControlTargetHolonomicPlatformVelocity
* @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_HolonomicPlatformVelocityTarget_H
-#define _ARMARX_LIB_RobotAPI_HolonomicPlatformVelocityTarget_H
+#ifndef _ARMARX_LIB_RobotAPI_ControlTargetHolonomicPlatformVelocity_H
+#define _ARMARX_LIB_RobotAPI_ControlTargetHolonomicPlatformVelocity_H
-#include "TargetBase.h"
+#include "ControlTargetBase.h"
+#include <ArmarXCore/observers/variant/Variant.h>
namespace armarx
{
/**
- * @defgroup Library-RTRobotUnit RTRobotUnit
+ * @defgroup Library-RobotUnit RobotUnit
* @ingroup RobotAPI
- * A description of the library RTRobotUnit.
+ * A description of the library RobotUnit.
*
- * @class HolonomicPlatformVelocityTarget
- * @ingroup Library-RTRobotUnit
- * @brief Brief description of class HolonomicPlatformVelocityTarget.
+ * @class ControlTargetHolonomicPlatformVelocity
+ * @ingroup Library-RobotUnit
+ * @brief Brief description of class ControlTargetHolonomicPlatformVelocity.
*
- * Detailed description of class HolonomicPlatformVelocityTarget.
+ * Detailed description of class ControlTargetHolonomicPlatformVelocity.
*/
- class HolonomicPlatformVelocityTarget: public TargetBase
+ class ControlTargetHolonomicPlatformVelocity: public ControlTargetBase
{
public:
float velocityX = ControllerConstants::ValueNotSetNaN;
float velocityY = ControllerConstants::ValueNotSetNaN;
float velocityRotation = ControllerConstants::ValueNotSetNaN;
- virtual std::string getControlMode() const override
+ virtual const std::string& getControlMode() const override
{
- return ControlModes::HolonomicPlatformVelocityMode;
+ return ControlModes::HolonomicPlatformVelocity;
}
virtual void reset() override
{
@@ -58,6 +59,17 @@ namespace armarx
{
return std::isfinite(velocityX) && std::isfinite(velocityY) && std::isfinite(velocityRotation);
}
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ return
+ {
+ {"velocityX", {new TimedVariant{velocityX ,timestamp}}},
+ {"velocityY", {new TimedVariant{velocityY ,timestamp}}},
+ {"velocityRotation", {new TimedVariant{velocityRotation,timestamp}}},
+ };
+ }
+
+ DETAIL_ControlTargetBase_DEFAULT_METHOD_IMPLEMENTATION
};
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..31a251ad1a3b1a427dc8a45ec9030c993a896d28
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.cpp
@@ -0,0 +1,132 @@
+/*
+ * 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 "ControlDevice.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+namespace armarx
+{
+ const ControlDevicePtr ControlDevice::NullPtr
+ {
+ nullptr
+ };
+
+ LVL0Controller* ControlDevice::getLVL0EmergencyStopController()
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ lVL0EmergencyStopController,
+ "ControlDevice::getLVL0EmergencyStopController called for a ControlDevice without a LVL0Controller with the ControlMode ControlModes::EmergencyStop"
+ << " (add a LVL0Controller with this ControlMode)"
+ );
+ return lVL0EmergencyStopController;
+ }
+
+ LVL0Controller* ControlDevice::getLVL0StopMovementController()
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ lVL0StopMovementController,
+ "ControlDevice::getLVL0StopMovementController called for a ControlDevice without a LVL0Controller with the ControlMode ControlModes::StopMovement"
+ << " (add a LVL0Controller with this ControlMode)"
+ );
+ return lVL0StopMovementController;
+ }
+
+ void ControlDevice::rtSetActiveLVL0Controller(LVL0Controller* lvl0)
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(lvl0, "Called ControlDevice::rtSetActiveLVL0Controller with a nullptr (Don't do this)")
+ if (activelvl0Controller == lvl0)
+ {
+ return;
+ }
+ if (lvl0->parent != this)
+ {
+ throw std::logic_error
+ {
+ "ControlDevice(" + getDeviceName() + ")::rtSetActiveLVL0Controller: tried to switch to a lvl0 controller from a different ControlDevice " +
+ "(You should only call ControlDevice::rtSetActiveLVL0Controller with LVL0Controllers added to the ControlDevice via ControlDevice::addLVL0Controller)"
+ };
+ }
+ if (activelvl0Controller)
+ {
+ activelvl0Controller->rtDeactivate();
+ }
+ lvl0->rtActivate();
+ activelvl0Controller = lvl0;
+ }
+
+ void ControlDevice::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ auto activeLvl0 = rtGetActiveLVL0Controller();
+ ARMARX_CHECK_EXPRESSION(activeLvl0);
+ activeLvl0->rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+ rtWriteTargetValues(sensorValuesTimestamp, timeSinceLastIteration);
+ }
+
+ void ControlDevice::addLVL0Controller(LVL0Controller* lvl0)
+ {
+ if (!lvl0)
+ {
+ throw std::invalid_argument
+ {
+ "ControlDevice(" + getDeviceName() + ")::addLVL0Controller: lvl0 controller is nullptr (Don't try nullptrs as LVL0Controller)"
+ };
+ }
+ if (lvl0->parent)
+ {
+ throw std::logic_error
+ {
+ "ControlDevice(" + getDeviceName() + ")::addLVL0Controller: tried to add a lvl0 controller multiple times" +
+ "(Don't try to add a LVL0Controller multiple to a ControlDevice)"
+ };
+ }
+ const std::string& mode = lvl0->getControlMode();
+ if (lvl0Controllers.has(mode))
+ {
+ throw std::invalid_argument
+ {
+ "ControlDevice(" + getDeviceName() + ")::addLVL0Controller: lvl0 controller for mode " + mode + " was already added" +
+ "(Don't try to add multiple LVL0Controller with the same ControlMode)"
+ };
+ }
+ lvl0->parent = this;
+ lvl0->rtResetTarget();
+ if (mode == ControlModes::EmergencyStop)
+ {
+ lVL0EmergencyStopController = lvl0;
+ }
+ if (mode == ControlModes::StopMovement)
+ {
+ lVL0StopMovementController = lvl0;
+ }
+ if (!lvl0->getControlTarget())
+ {
+ throw std::logic_error
+ {
+ "ControlDevice(" + getDeviceName() + ")::addLVL0Controller: The LVL0Controller has no ControlTarget. (mode = " + mode + ")" +
+ "(Don't try to add LVL0Controller without a ControlTarget)"
+ };
+ }
+ lvl0Controllers.add(mode, std::move(lvl0));
+ }
+
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h
new file mode 100644
index 0000000000000000000000000000000000000000..5e1079ae54735346cef1b1e7653ac9e98e5c64b2
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/ControlDevice.h
@@ -0,0 +1,148 @@
+/*
+ * 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_ControlDevice_H
+#define _ARMARX_LIB_RobotAPI_ControlDevice_H
+
+#include <string>
+#include <memory>
+#include <atomic>
+
+#include"DeviceBase.h"
+#include "../util.h"
+#include "../ControlModes.h"
+#include "../LVL0Controllers/LVL0Controller.h"
+
+namespace armarx
+{
+ TYPEDEF_PTRS_SHARED(ControlDevice);
+
+ namespace ControlDeviceTags
+ {
+ using namespace DeviceTags;
+ const static std::string PeriodicPosition = "ControlDeviceTag_PeriodicPosition";
+ const static std::string CreateNoDefaultController = "ControlDeviceTag_CreateNoDefaultController";
+ const static std::string ForcePositionThroughVelocity = "ControlDeviceTag_ForcePositionThroughVelocity";
+ }
+
+ /**
+ * @brief The ControlDevice class represents a logical unit accepting commands via its LVL0Controllers.
+ *
+ * It holds a set of LVL0Controllers with different Controll modes.
+ * It always has a LVL0Controller with mode ControlModes::EmergencyStop which is used in case of an error.
+ * It always has a LVL0Controller with mode ControlModes::StopMovement which is used when the controled device should stop any movement.
+ *
+ * It holds a set of tags.
+ * These tags are used by different components to create default controllers, check for abilities or present output to the user.
+ */
+ class ControlDevice: public virtual DeviceBase
+ {
+ public:
+ /// @brief A static const nullptr in case a const ref to a nullptr needs to be returned
+ static const ControlDevicePtr NullPtr;
+
+ /// @brief Create a ControlDevice with the given name
+ ControlDevice(const std::string& name): DeviceBase {name} {}
+ //controller management
+ /// @return the lVL0EmergencyStopController of this ControlDevice
+ LVL0Controller* rtGetLVL0EmergencyStopController()
+ {
+ return lVL0EmergencyStopController;
+ }
+ /// @return the lVL0EmergencyStopController of this ControlDevice
+ LVL0Controller* getLVL0EmergencyStopController();
+ /// @return the lVL0StopMovementController of this ControlDevice
+ LVL0Controller* rtGetLVL0StopMovementController()
+ {
+ return lVL0StopMovementController;
+ }
+ /// @return the lVL0StopMovementController of this ControlDevice
+ LVL0Controller* getLVL0StopMovementController();
+ /// @return the active LVL0Controller of this ControlDevice
+ LVL0Controller* rtGetActiveLVL0Controller()
+ {
+ return activelvl0Controller;
+ }
+ /// @return all LVL0Controllers of this ControlDevice
+ const std::vector<LVL0Controller*>& rtGetLVL0Controllers()
+ {
+ return lvl0Controllers.values();
+ }
+ std::vector<const LVL0Controller*> getLVL0Controllers() const
+ {
+ return {lvl0Controllers.values().begin(), lvl0Controllers.values().end()};
+ }
+ /// @return the LVL0Controller for the given mode of this ControlDevice (or nullptr if there is no controller for this mode)
+ LVL0Controller* getLVL0Controller(const std::string& mode)
+ {
+ return lvl0Controllers.at(mode, nullptr);
+ }
+ const LVL0Controller* getLVL0Controller(const std::string& mode) const
+ {
+ return lvl0Controllers.at(mode, nullptr);
+ }
+ LVL0Controller* getLVL0Controller(std::size_t i)
+ {
+ return lvl0Controllers.at(i);
+ }
+ const LVL0Controller* getLVL0Controller(std::size_t i) const
+ {
+ return lvl0Controllers.at(i);
+ }
+ bool hasLVL0Controller(const std::string& mode) const
+ {
+ return lvl0Controllers.has(mode);
+ }
+ const std::vector<std::string>& getControlModes() const
+ {
+ return lvl0Controllers.keys();
+ }
+ /**
+ * @brief Activates the given LVL0Controller for this device
+ * @throw std::logic_error if the lvl0 controller does not belong to this ControlDevice
+ */
+ virtual void rtSetActiveLVL0Controller(LVL0Controller* lvl0);
+ /**
+ * @brief runs the active LVL0 Controller and write the target values into the control device
+ * @param sensorValuesTimestamp
+ * @param timeSinceLastIteration
+ * @see writeTargetValues()
+ */
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ virtual void rtWriteTargetValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) {}
+ protected:
+ /**
+ * @brief adds the lvl0 controller to this ControlDevice
+ * @throw std::logic_error if the LVL0Controller was already added to a ControlDevice
+ * @throw std::invalid_argument if a LVL0Controller with the same control mode was already added to this device
+ */
+ void addLVL0Controller(LVL0Controller* lvl0);
+ private:
+ KeyValueVector<std::string, LVL0Controller*> lvl0Controllers;
+ LVL0Controller* activelvl0Controller {nullptr};
+ LVL0Controller* lVL0EmergencyStopController {nullptr};
+ LVL0Controller* lVL0StopMovementController {nullptr};
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h b/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..51d8dbbba1d66fa74e7550b4fea445e830267165
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/DeviceBase.h
@@ -0,0 +1,69 @@
+/*
+ * 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_DeviceBase_H
+#define _ARMARX_LIB_RobotAPI_DeviceBase_H
+
+#include "../util.h"
+
+namespace armarx
+{
+
+ namespace DeviceTags
+ {
+ }
+ class DeviceBase
+ {
+ public:
+ /// @brief Create a Device with the given name
+ DeviceBase(const std::string& name): deviceName {name} {}
+ virtual ~DeviceBase() = default;
+ /// @return The Device's name
+ const std::string& getDeviceName() const
+ {
+ return deviceName;
+ }
+ //tags
+ /// @return Thes set of tags for this Device
+ const std::set<std::string>& getTags() const
+ {
+ return tags;
+ }
+ /// @return Whether the device has the given tag
+ bool hasTag(const std::string& tag) const
+ {
+ return getTags().count(tag);
+ }
+
+ protected:
+ /// @brief adds the given tag to the Device
+ void addDeviceTag(const std::string& tag)
+ {
+ tags.emplace(tag);
+ }
+ private:
+ std::set<std::string> tags;
+ const std::string deviceName;
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
new file mode 100644
index 0000000000000000000000000000000000000000..61dec2e7a9582d41c98845d2f4775295de310012
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/RTThreadTimingsSensorDevice.h
@@ -0,0 +1,147 @@
+/*
+ * 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_RTThreadTimingsSensorDevice_H
+#define _ARMARX_LIB_RobotAPI_RTThreadTimingsSensorDevice_H
+
+#include"SensorDevice.h"
+#include "../SensorValues/SensorValueRTThreadTimings.h"
+
+namespace armarx
+{
+ TYPEDEF_PTRS_SHARED(RTThreadTimingsSensorDevice);
+
+ class RTThreadTimingsSensorDevice: virtual public SensorDevice
+ {
+ public:
+ RTThreadTimingsSensorDevice(const std::string& name): DeviceBase(name), SensorDevice(name) {}
+ virtual const SensorValueBase* getSensorValue() const = 0;
+
+ virtual void rtMarkRtLoopStart() = 0;
+ virtual void rtMarkRtLoopPreSleep() = 0;
+
+ virtual void rtMarkRtBusSendReceiveStart() = 0;
+ virtual void rtMarkRtBusSendReceiveEnd() = 0;
+ protected:
+ friend class RobotUnit;
+
+ virtual void rtMarkRtSwitchControllerSetupStart() = 0;
+ virtual void rtMarkRtSwitchControllerSetupEnd() = 0;
+
+ virtual void rtMarkRtRunLVL1ControllersStart() = 0;
+ virtual void rtMarkRtRunLVL1ControllersEnd() = 0;
+
+ virtual void rtMarkRtHandleInvalidTargetsStart() = 0;
+ virtual void rtMarkRtHandleInvalidTargetsEnd() = 0;
+
+ virtual void rtMarkRtRunLVL0ControllersStart() = 0;
+ virtual void rtMarkRtRunLVL0ControllersEnd() = 0;
+
+
+ virtual void rtMarkRtReadSensorDeviceValuesStart() = 0;
+ virtual void rtMarkRtReadSensorDeviceValuesEnd() = 0;
+
+ virtual void rtMarkRtUpdateSensorAndControlBufferStart() = 0;
+ virtual void rtMarkRtUpdateSensorAndControlBufferEnd() = 0;
+
+ virtual void rtMarkRtResetAllTargetsStart() = 0;
+ virtual void rtMarkRtResetAllTargetsEnd() = 0;
+
+ };
+
+ template<class SensorValueType = SensorValueRTThreadTimings>
+ class RTThreadTimingsSensorDeviceImpl: public RTThreadTimingsSensorDevice
+ {
+ public:
+ static_assert(
+ std::is_base_of<SensorValueRTThreadTimings, SensorValueType>::value,
+ "The template parameter of RTThreadTimingsSensorDeviceImpl should be derived from SensorValueRTThreadTimings"
+ );
+ RTThreadTimingsSensorDeviceImpl(const std::string& name): DeviceBase(name), SensorDevice(name), RTThreadTimingsSensorDevice(name) {}
+ virtual const SensorValueBase* getSensorValue() const
+ {
+ return &value;
+ }
+
+ SensorValueType value;
+
+ virtual void rtMarkRtLoopStart() override
+ {
+ std::chrono::microseconds now = MicroNow();
+ value.rtLoopRoundTripTime = now - rtLoopStart;
+ rtLoopStart = now;
+ }
+ virtual void rtMarkRtLoopPreSleep() override
+ {
+ value.rtLoopDuration = MicroNow() - rtLoopStart;
+ }
+ virtual void rtMarkRtBusSendReceiveStart() override
+ {
+ std::chrono::microseconds now = MicroNow();
+ value.rtBusSendReceiveRoundTripTime = now - rtBusSendReceiveStart;
+ rtBusSendReceiveStart = now;
+ }
+ virtual void rtMarkRtBusSendReceiveEnd() override
+ {
+ value.rtBusSendReceiveDuration = MicroNow() - rtBusSendReceiveStart;
+ }
+
+ static std::chrono::microseconds MicroNow()
+ {
+ return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch());
+ }
+ protected:
+#define make_markRT_X_Start_End(name) \
+ virtual void rtMarkRt##name##Start() override \
+ { \
+ std::chrono::microseconds now = MicroNow(); \
+ value.rt##name##RoundTripTime = now - rt##name##Start; \
+ rt##name##Start=now; \
+ } \
+ virtual void rtMarkRt##name##End() override \
+ { \
+ value.rt##name##Duration=MicroNow() - rt##name##Start; \
+ }
+
+ make_markRT_X_Start_End(SwitchControllerSetup)
+ make_markRT_X_Start_End(RunLVL1Controllers)
+ make_markRT_X_Start_End(HandleInvalidTargets)
+ make_markRT_X_Start_End(RunLVL0Controllers)
+ make_markRT_X_Start_End(ReadSensorDeviceValues)
+ make_markRT_X_Start_End(UpdateSensorAndControlBuffer)
+ make_markRT_X_Start_End(ResetAllTargets)
+#undef make_markRT_X_Start_End
+ protected:
+ std::chrono::microseconds rtSwitchControllerSetupStart;
+ std::chrono::microseconds rtRunLVL1ControllersStart;
+ std::chrono::microseconds rtHandleInvalidTargetsStart;
+ std::chrono::microseconds rtRunLVL0ControllersStart;
+ std::chrono::microseconds rtBusSendReceiveStart;
+ std::chrono::microseconds rtReadSensorDeviceValuesStart;
+ std::chrono::microseconds rtUpdateSensorAndControlBufferStart;
+ std::chrono::microseconds rtResetAllTargetsStart;
+ std::chrono::microseconds rtLoopStart;
+ };
+
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/DataUnits/IMUDataUnit.h b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.cpp
similarity index 74%
rename from source/RobotAPI/libraries/RTRobotUnit/DataUnits/IMUDataUnit.h
rename to source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.cpp
index 3e7433cd1f1d9d69ede38d384433a4238a6cae09..20ca86f2e8077738ba8c80546b53e242d7c70352 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/DataUnits/IMUDataUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.cpp
@@ -13,27 +13,20 @@
* 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::IMUDataUnit
+ * @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_IMUDataUnit_H
-#define _ARMARX_LIB_RobotAPI_IMUDataUnit_H
+#include "SensorDevice.h"
namespace armarx
{
- class IMUDataUnitInterface
+ const SensorDevicePtr SensorDevice::NullPtr
{
- public:
+ nullptr
};
- class IMUDataUnitPtrProvider: virtual public IMUDataUnitInterface
- {
- public:
- };
}
-
-#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h
new file mode 100644
index 0000000000000000000000000000000000000000..d8b4a5dca04e0e575674367066ff16096825df10
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Devices/SensorDevice.h
@@ -0,0 +1,60 @@
+/*
+ * 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_SensorDevice_H
+#define _ARMARX_LIB_RobotAPI_SensorDevice_H
+
+#include"DeviceBase.h"
+#include "../SensorValues/SensorValueBase.h"
+#include "../util.h"
+
+#include <IceUtil/Time.h>
+
+namespace armarx
+{
+ TYPEDEF_PTRS_SHARED(SensorDevice);
+
+ namespace SensorDeviceTags
+ {
+ using namespace DeviceTags;
+ }
+
+ class SensorDevice: public virtual DeviceBase
+ {
+ public:
+ /// @brief A static const nullptr in case a const ref to a nullptr needs to be returned
+ static const SensorDevicePtr NullPtr;
+ /// @brief Create a SensorDevice with the given name
+ SensorDevice(const std::string& name): DeviceBase {name} {}
+ /// @return The SensorDevice's sensor value
+ virtual const SensorValueBase* getSensorValue() const = 0;
+
+ std::string getSensorValueType(bool withoutNamespaceSpecifier = false) const
+ {
+ return getSensorValue()->getSensorValueType(withoutNamespaceSpecifier);
+ }
+
+ virtual void rtReadSensorValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) {}
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL0Controllers/LVL0Controller.cpp b/source/RobotAPI/components/units/RobotUnit/LVL0Controllers/LVL0Controller.cpp
similarity index 79%
rename from source/RobotAPI/libraries/RTRobotUnit/LVL0Controllers/LVL0Controller.cpp
rename to source/RobotAPI/components/units/RobotUnit/LVL0Controllers/LVL0Controller.cpp
index f1314225db47875c050245ae3a477a13cad453ef..8b25b9df97a86c596914313b13d6e6aaf0ec1973 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL0Controllers/LVL0Controller.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/LVL0Controllers/LVL0Controller.cpp
@@ -22,7 +22,12 @@
#include "LVL0Controller.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
using namespace armarx;
-
+ControlDevice& LVL0Controller::getParent() const
+{
+ ARMARX_CHECK_EXPRESSION_W_HINT(parent, "This LVL0Controller is not owned by a ControlDevice");
+ return *parent;
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/LVL0Controllers/LVL0Controller.h b/source/RobotAPI/components/units/RobotUnit/LVL0Controllers/LVL0Controller.h
new file mode 100644
index 0000000000000000000000000000000000000000..1a16efbdf8b7fe91c3e991ff10e6930ef633638e
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/LVL0Controllers/LVL0Controller.h
@@ -0,0 +1,99 @@
+/*
+ * 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::LVL0Controller
+ * @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_LVL0Controller_H
+#define _ARMARX_LIB_RobotAPI_LVL0Controller_H
+
+#include "../ControlTargets/ControlTargetBase.h"
+#include "../util.h"
+
+#include <memory>
+#include <atomic>
+
+namespace armarx
+{
+ class ControlDevice;
+
+ class LVL0Controller
+ {
+ public:
+ virtual ~LVL0Controller() = default;
+
+ virtual ControlTargetBase* getControlTarget() = 0;
+
+ virtual const ControlTargetBase* getControlTarget() const
+ {
+ return const_cast<const ControlTargetBase*>(const_cast<LVL0Controller*>(this)->getControlTarget());
+ }
+
+ virtual void rtResetTarget()
+ {
+ getControlTarget()->reset();
+ }
+
+ virtual bool rtIsTargetVaild() const
+ {
+ return getControlTarget()->isValid();
+ }
+
+ virtual const std::string& getControlMode() const
+ {
+ return getControlTarget()->getControlMode();
+ }
+ ControlDevice& getParent() const;
+
+ protected:
+ ControlDevice& rtGetParent() const
+ {
+ return *parent;
+ }
+ //called by the owning ControlDevice
+ /// @brief called when this LVL0Controller is run
+ virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+ /// @brief called when this LVL0Controller is activated
+ virtual void rtPreActivateController()
+ {
+
+ }
+ virtual void rtPostDeactivateController()
+ {
+
+ }
+ /// @brief called when this LVL0Controller is deactivated
+ private:
+ friend class ControlDevice;
+ std::atomic_bool activated {false};
+ ControlDevice* parent {nullptr};
+ void rtActivate()
+ {
+ rtPreActivateController();
+ activated = true;
+ }
+ void rtDeactivate()
+ {
+ activated = false;
+ rtPostDeactivateController();
+ rtResetTarget();
+ }
+ };
+}
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1Controller.cpp b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1Controller.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9e77d64f87c9eeb12a8f99c5ba87d92b025a3b75
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1Controller.cpp
@@ -0,0 +1,199 @@
+/*
+ * 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::LVL1Controller
+ * @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 "LVL1Controller.h"
+
+#include <ArmarXCore/core/util/algorithm.h>
+
+#include "../RobotUnit.h"
+
+using namespace armarx;
+
+const LVL1ControllerPtr LVL1Controller::NullPtr
+{
+ nullptr
+};
+
+bool LVL1Controller::isControllerActive(const Ice::Current&) const
+{
+ return isActive;
+}
+bool LVL1Controller::isControllerRequested(const Ice::Current&) const
+{
+ return isRequested;
+}
+
+bool LVL1Controller::hasControllerError(const Ice::Current&) const
+{
+ return deactivatedBecauseOfError;
+}
+
+std::string LVL1Controller::getInstanceName(const Ice::Current&) const
+{
+ return getName();
+}
+
+LVL1ControllerDescription LVL1Controller::getControllerDescription(const Ice::Current&) const
+{
+ LVL1ControllerDescription d;
+ d.className = getClassName();
+ ARMARX_CHECK_EXPRESSION(getProxy(-1));
+ d.controller = LVL1ControllerInterfacePrx::uncheckedCast(getProxy(-1));
+ d.controlModeAssignment = controlDeviceControlModeMap;
+ d.instanceName = getInstanceName();
+ return d;
+}
+
+StringStringDictionary LVL1Controller::getControlDeviceUsedControlModeMap(const Ice::Current&) const
+{
+ return controlDeviceControlModeMap;
+}
+
+boost::optional<std::vector<char> > LVL1Controller::isNotInConflictWith(const std::vector<char>& used) const
+{
+ ARMARX_CHECK_EXPRESSION(used.size() == controlDeviceUsedBitmap.size());
+ auto result = used;
+ for (std::size_t i = 0; i < controlDeviceUsedBitmap.size(); ++i)
+ {
+ if (controlDeviceUsedBitmap.at(i))
+ {
+ if (used.at(i))
+ {
+ return boost::optional<std::vector<char>>();
+ }
+ result.at(i) = true;
+ }
+
+ }
+ return {true, std::move(result)};
+}
+
+LVL1ControllerStatus LVL1Controller::getControllerStatus(const Ice::Current&) const
+{
+ LVL1ControllerStatus s;
+ s.active = isActive;
+ s.instanceName = getInstanceName();
+ s.requested = isRequested;
+ s.error = deactivatedBecauseOfError;
+ s.timestampUSec = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch()).count();
+ return s;
+}
+
+LVL1ControllerDescriptionWithStatus LVL1Controller::getControllerDescriptionWithStatus(const Ice::Current&) const
+{
+ LVL1ControllerDescriptionWithStatus ds;
+ ds.description = getControllerDescription();
+ ds.status = getControllerStatus();
+ return ds;
+}
+
+RobotUnitInterfacePrx LVL1Controller::getRobotUnit(const Ice::Current&) const
+{
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+ return RobotUnitInterfacePrx::uncheckedCast(robotUnit->getProxy(-1));
+}
+
+void LVL1Controller::activateController(const Ice::Current&) const
+{
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+ robotUnit->activateController(getInstanceName());
+}
+
+void LVL1Controller::deactivateController(const Ice::Current&) const
+{
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+ robotUnit->deactivateController(getInstanceName());
+}
+
+void LVL1Controller::publish(const SensorAndControl& sac, const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer)
+{
+ const bool active = isActive;
+ if (publishActive != active)
+ {
+ publishActive = active;
+ if (active)
+ {
+ onPublishActivation(draw, observer);
+ }
+ else
+ {
+ onPublishDeactivation(draw, observer);
+ }
+ }
+ if (active)
+ {
+ onPublish(sac, draw, observer);
+ }
+}
+
+void LVL1Controller::rtActivateController()
+{
+ if (!isActive)
+ {
+ deactivatedBecauseOfError = false;
+ ARMARX_INFO << "activating lvl1controller " << getInstanceName() << " " << this;
+ rtPreActivateController();
+ isActive = true;
+ }
+ else
+ {
+ ARMARX_INFO << "lvl1controller " << getInstanceName() << " is already active " << this;
+ }
+}
+
+void LVL1Controller::rtDeactivateController()
+{
+ if (isActive)
+ {
+ isActive = false;
+ ARMARX_INFO << "deactivating lvl1controller " << getInstanceName() << " " << this;
+ rtPostDeactivateController();
+ }
+ else
+ {
+ ARMARX_INFO << "lvl1controller " << getInstanceName() << " is already deactivated " << this;
+ }
+}
+
+void LVL1Controller::rtDeactivateControllerBecauseOfError()
+{
+ deactivatedBecauseOfError = true;
+ rtDeactivateController();
+}
+
+void LVL1Controller::robotUnitInit(RobotUnit* ru, std::size_t ctrlId, StringStringDictionary ctrlDeviceControlModeMap, std::vector<char> ctrlDeviceUsedBitmap, std::vector<std::size_t> ctrlDeviceUsedIndices)
+{
+ ARMARX_CHECK_EXPRESSION(ru);
+ ARMARX_CHECK_EXPRESSION(ctrlDeviceControlModeMap.size());
+ ARMARX_CHECK_EXPRESSION(ctrlId != std::numeric_limits<std::size_t>::max());
+ ARMARX_CHECK_EXPRESSION_W_HINT(ru->getNumberOfControlDevices() == ctrlDeviceUsedBitmap.size(),
+ "ru->getNumberOfControlDevices() == ctrlDeviceUsedBitmap.size() ->" +
+ std::to_string(ru->getNumberOfControlDevices()) + " > " + std::to_string(ctrlDeviceUsedBitmap.size()));
+ ARMARX_CHECK_EXPRESSION(ctrlDeviceControlModeMap.size() == ctrlDeviceUsedIndices.size());
+
+ robotUnit = ru;
+ id = ctrlId;
+ controlDeviceControlModeMap = std::move(ctrlDeviceControlModeMap);
+ controlDeviceUsedBitmap = std::move(ctrlDeviceUsedBitmap);
+ controlDeviceUsedIndices = std::move(ctrlDeviceUsedIndices);
+}
+
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.h b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1Controller.h
similarity index 50%
rename from source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.h
rename to source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1Controller.h
index 53c36a59d564b4d199e1b89c8f7a644f52485c08..9ac907a6342aa65a23c0d959bf547cd34367a3ab 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.h
+++ b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1Controller.h
@@ -27,48 +27,78 @@
#include <atomic>
#include <mutex>
#include <functional>
+#include <unordered_set>
+
+#include <boost/optional.hpp>
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/util/Registrar.h>
#include <ArmarXCore/core/util/TripleBuffer.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.h>
+#include <ArmarXGui/interface/WidgetDescription.h>
+
+#include <RobotAPI/interface/units/RobotUnit/LVL1Controller.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include "../Targets/TargetBase.h"
+#include "../ControlTargets/ControlTargetBase.h"
//units
-#include "../DataUnits/ForceTorqueDataUnit.h"
-#include "../DataUnits/HapticDataUnit.h"
-#include "../DataUnits/IMUDataUnit.h"
-#include "../DataUnits/KinematicDataUnit.h"
-#include "../DataUnits/PlatformDataUnit.h"
+#include "../Devices/ControlDevice.h"
+#include "../Devices/SensorDevice.h"
+
+#include "../util.h"
+#include "../structs.h"
+
+namespace VirtualRobot
+{
+ class Robot;
+}
namespace armarx
{
- class LVL1ControllerDataProviderInterface: virtual public Ice::Object
+ TYPEDEF_PTRS_HANDLE(LVL1Controller);
+ TYPEDEF_PTRS_HANDLE(LVL1ControllerDescriptionProviderInterface);
+
+ class RobotUnit;
+
+ class LVL1ControllerDescriptionProviderInterface: virtual public Ice::Object
{
public:
- virtual const ForceTorqueDataUnitInterface* getRTForceTorqueDataUnit() const = 0;
- virtual const HapticDataUnitInterface* getRTHapticDataUnit() const = 0;
- virtual const IMUDataUnitInterface* getRTIMUDataUnit() const = 0;
- virtual const KinematicDataUnitInterface* getRTKinematicDataUnit() const = 0;
- virtual const PlatformDataUnitInterface* getRTPlatformDataUnit() const = 0;
+ virtual ~LVL1ControllerDescriptionProviderInterface() = default;
- virtual TargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode) = 0;
+ virtual ConstSensorDevicePtr getSensorDevice(const std::string& sensorDeviceName) const = 0;
+ virtual const SensorValueBase* getSensorValue(const std::string& sensorDeviceName) const
+ {
+ auto sd = getSensorDevice(sensorDeviceName);
+ return sd ? sd->getSensorValue() : nullptr;
+ }
+ virtual ConstControlDevicePtr getControlDevice(const std::string& deviceName) = 0;
+ virtual ControlTargetBase* getControlTarget(const std::string& deviceName, const std::string& controlMode) = 0;
virtual std::string getName() const = 0;
+
+ template<class T>
+ const T* getSensorValue(const std::string& sensorDeviceName) const
+ {
+ return dynamic_cast<T*>(getSensorValue(sensorDeviceName));
+ }
+ template<class T>
+ T* getControlTarget(const std::string& deviceName, const std::string& controlMode)
+ {
+ return dynamic_cast<T*>(getControlTarget(deviceName, controlMode));
+ }
};
- using LVL1ControllerDataProviderInterfacePtr = IceInternal::Handle<LVL1ControllerDataProviderInterface>;
/**
- * @defgroup Library-RTRobotUnit RTRobotUnit
+ * @defgroup Library-RobotUnit RobotUnit
* @ingroup RobotAPI
- * A description of the library RTRobotUnit.
+ * A description of the library RobotUnit.
*/
/**
- * @ingroup Library-RTRobotUnit
+ * @ingroup Library-RobotUnit
* @brief A high level controller writing its results into targes.
*
* This is the abstract base class for all LVL1Controllers.
@@ -136,27 +166,70 @@ namespace armarx
*/
class LVL1Controller:
virtual public LVL1ControllerInterface,
- virtual public Component
+ virtual public ManagedIceObject
{
public:
+ //add these static functions if you want to provide a gui for construction
+ // static WidgetDescription::WidgetPtr GenerateConfigDescription
+ // (
+ // const boost::shared_ptr<VirtualRobot::Robot>&,
+ // const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ // const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
+ // )
+ // {
+ // return a nullptr / Widget if you want remote configuration but do not need any parameters
+ // }
+ // static LVL1ControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap&)
+ // {
+ // return the config that is passed to your controller
+ // }
+ using GenerateConfigDescriptionFunctionSignature = WidgetDescription::WidgetPtr(*)
+ (
+ const boost::shared_ptr<VirtualRobot::Robot>&,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
+ );
+ using GenerateConfigFromVariantsFunctionSignature = LVL1ControllerConfigPtr(*)(const StringVariantBaseMap&);
+
+ static const LVL1ControllerPtr NullPtr;
+
+ virtual ~LVL1Controller() = default;
//ice interface (must not be called in the rt thread)
- virtual bool isControllerActive(const Ice::Current& = GlobalIceCurrent) const final
+ virtual bool isControllerActive(const Ice::Current& = GlobalIceCurrent) const final;
+ virtual bool isControllerRequested(const Ice::Current& = GlobalIceCurrent) const final;
+ virtual bool hasControllerError(const Ice::Current& = GlobalIceCurrent) const final;
+
+ virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override = 0;
+ virtual std::string getInstanceName(const Ice::Current& = GlobalIceCurrent) const final;
+
+ virtual LVL1ControllerDescription getControllerDescription(const Ice::Current& = GlobalIceCurrent) const final;
+ virtual LVL1ControllerStatus getControllerStatus(const Ice::Current& = GlobalIceCurrent) const final;
+ virtual LVL1ControllerDescriptionWithStatus getControllerDescriptionWithStatus(const Ice::Current& = GlobalIceCurrent) const final;
+ virtual RobotUnitInterfacePrx getRobotUnit(const Ice::Current& = GlobalIceCurrent) const final;
+
+ virtual void activateController(const Ice::Current& = GlobalIceCurrent) const final;
+ virtual void deactivateController(const Ice::Current& = GlobalIceCurrent) const final;
+
+ virtual WidgetDescription::StringWidgetDictionary getFunctionDescriptions(const Ice::Current& = GlobalIceCurrent) const override
{
- return isActive;
+ return {};
}
- virtual std::string getInstanceName(const Ice::Current& = GlobalIceCurrent) const final
+ virtual void callDescribedFunction(const std::string&, const StringVariantBaseMap&, const Ice::Current& = GlobalIceCurrent) override
{
- return getName();
}
- virtual StringStringDictionary getJointControlModeMap(const Ice::Current& = GlobalIceCurrent) const final
+
+ std::size_t getId() const
{
- return jointControlModeMap;
+ return id;
}
- virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override = 0;
- //c++ interface (local calls) (must not be called in the rt thread)
- //c++ interface (local calls) (can only be called in the rt thread)
+
+ //c++ interface (local calls) (must be called in the rt thread)
virtual void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
- virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) = 0;
+ virtual void rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+ }
+
/**
* @brief This function is called before the controller is activated.
* You can use it to activate a thread again (DO NOT SPAWN NEW THREADS!) e.g. via a std::atomic_bool.
@@ -168,23 +241,78 @@ namespace armarx
*/
virtual void rtPostDeactivateController() {}
- bool rtUsesJoint(std::size_t jointindex) const
+ bool rtUsesControlDevice(std::size_t deviceIndex) const
{
- return std::find(jointIndices.begin(), jointIndices.end(), jointindex) == jointIndices.end();
+ return controlDeviceUsedBitmap.at(deviceIndex);
}
- const std::vector<std::size_t>& rtGetJointIndices() const
+
+ std::size_t rtGetId() const
+ {
+ return id;
+ }
+
+ //used control devices
+ virtual StringStringDictionary getControlDeviceUsedControlModeMap(const Ice::Current& = GlobalIceCurrent) const final;
+ const std::vector<char>& getControlDeviceUsedBitmap() const
+ {
+ return controlDeviceUsedBitmap;
+ }
+ const std::vector<std::size_t>& rtGetControlDeviceUsedIndices() const
+ {
+ return controlDeviceUsedIndices;
+ }
+ const std::vector<std::size_t>& getControlDeviceUsedIndices() const
+ {
+ return controlDeviceUsedIndices;
+ }
+
+ //check for conflict
+ boost::optional<std::vector<char>> isNotInConflictWith(const LVL1ControllerPtr& other) const
{
- return jointIndices;
+ return isNotInConflictWith(other->getControlDeviceUsedBitmap());
}
+ boost::optional<std::vector<char>> isNotInConflictWith(const std::vector<char>& used) const;
+
+ template<class ItT>
+ static boost::optional<std::vector<char>> AreNotInConflict(ItT first, ItT last);
+
+ //publish thread hooks
+ virtual void onPublishActivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) {}
+ virtual void onPublishDeactivation(const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) {}
+ virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) {}
private:
- friend class RobotUnit;
+ void publish(const SensorAndControl& sac, const DebugDrawerInterfacePrx& draw, const DebugObserverInterfacePrx& observer);
+
void rtActivateController();
void rtDeactivateController();
+ void rtDeactivateControllerBecauseOfError();
+
+ //firends
+ friend class RobotUnitLVL1ControllerAccess;
+
+ void robotUnitInit(
+ RobotUnit* ru,
+ std::size_t ctrlId,
+ StringStringDictionary ctrlDeviceControlModeMap,
+ std::vector<char> ctrlDeviceUsedBitmap,
+ std::vector<std::size_t> ctrlDeviceUsedIndices
+ );
+
+ RobotUnit* robotUnit;
+ std::size_t id = std::numeric_limits<std::size_t>::max();
+ StringStringDictionary controlDeviceControlModeMap;
+ std::vector<char> controlDeviceUsedBitmap;
+ std::vector<std::size_t> controlDeviceUsedIndices;
//this data is filled by the robot unit to provide convenience functions
std::atomic_bool isActive {false};
- JointNameToControlModeDictionary jointControlModeMap;
- std::vector<std::size_t> jointIndices;
+ std::atomic_bool isRequested {false};
+ std::atomic_bool deactivatedBecauseOfError {false};
+
+ bool publishActive {false};
+
+ bool statusReportedActive {false};
+ bool statusReportedRequested {false};
// ManagedIceObject interface
protected:
@@ -192,108 +320,50 @@ namespace armarx
{
return getClassName();
}
- };
- using LVL1ControllerPtr = IceInternal::Handle<LVL1Controller>;
-
- using LVL1ControllerRegistry = Registrar<std::function<LVL1ControllerPtr(LVL1ControllerDataProviderInterfacePtr robotUnit, LVL1ControllerConfigPtr config)>>;
- template<class ControllerType>
- struct LVL1ControllerRegistration
- {
- LVL1ControllerRegistration(const std::string& name)
+ public:
+ struct IdComp
{
- LVL1ControllerRegistry::registerElement(name, [](LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr config)
+ template<class PtrT1, class PtrT2>
+ bool operator()(PtrT1 l, PtrT2 r) const
{
- ARMARX_CHECK_EXPRESSION_W_HINT(prov, "DataProvider is NULL!");
- return new ControllerType(prov, config);
- });
- }
- };
-
- /**
- * @ingroup Library-RTRobotUnit
- * @brief Brief description of class LVL1Controller.
- *
- * Detailed description of class LVL1Controller.
- *
- * \code{cpp}
- //in ice
- module armarx
- {
- struct SomeControllerConfig extends LVL1ControllerConfig
- {
- float paramSetViaConfig;
+ //null is bigger than any other
+ if (!l)
+ {
+ return false;
+ }
+ if (!r)
+ {
+ return true;
+ }
+ return l->id < r->id;
+ }
};
- interface SomeControllerInterface extends LVL1ControllerInterface
+ // ManagedIceObject interface
+ protected:
+ virtual void onInitComponent() override
{
- void setSomeParamChangedViaIce(float param);
- };
+ }
+ virtual void onConnectComponent() override
+ {
+ }
};
-
- //in c++
- #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
- namespace armarx
+ class LVL1ControllerRegistryEntry
{
- struct SomeControlDataStruct
- {
- float parameterChangedViaIceCalls;
- };
-
- class SomeController : virtual public LVL1Controller<SomeControlDataStruct>,
- public SomeControllerInterface
- {
- ActuatorVelocityTargetPtr targetJointXPtr;
- std::vector<const float*> jointValuePtrs;
- float someParamSetViaConfig;
- SomeController(RobotUnitPtr robotUnit, LVL1ControllerConfigPtr config):
- LVL1Controller<SomeControlDataStruct>(SomeControlDataStruct(1)) //initial parameter value
- {
- //cast the config to your config type
- SomeControlDataStructPtr someConfig = SomeControlDataStructPtr::dynamicCast(config);
- ARMARX_CHECK_EXPRESSION_W_HINT(someConfig,
- "The provided config has the wrong type! The type is " << config->ice_id()
- << " instead of " << SomeControllerConfig::ice_staticId());
- //read the config
- someParamSetViaConfig = someConfig->parameterSetViaIceCalls
- //make sure the used units are supported
- ARMARX_CHECK_EXPRESSION_W_HINT(robotUnit->getRTKinematicDataUnit(), "The RobotUnit " << robotUnit->getName() << " has no kinematic data unit");
- //get pointers to sensor values from units
- jointValuePtrs = robotUnit->getRTKinematicDataUnit()->getJointAngles({"jointX", "jointY"});
-
- //get pointers for the results of this controller
- 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(ActuatorVelocityTargetPtr targetPtr, ControllerConfigPtr config):
- targetJointXPtr{targetPtr}
- {
+ private:
+ friend class RobotUnit;
+ virtual LVL1ControllerPtr create(LVL1ControllerDescriptionProviderInterfacePtr, const LVL1ControllerConfigPtr&, const boost::shared_ptr<VirtualRobot::Robot>&) const = 0;
+ virtual WidgetDescription::WidgetPtr GenerateConfigDescription(const boost::shared_ptr<VirtualRobot::Robot>&, const std::map<std::string, ConstControlDevicePtr>&, const std::map<std::string, ConstSensorDevicePtr>&) const = 0;
+ virtual LVL1ControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap&) const = 0;
+ virtual bool hasRemoteConfiguration() const = 0;
+ };
+ using LVL1ControllerRegistry = Registrar<std::unique_ptr<LVL1ControllerRegistryEntry>>;
- }
+ template<class ControllerType>
+ struct LVL1ControllerRegistration;
- virtual void run(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override
- {
- pid.update(
- rtGetControlStruct().parameterChangedViaIceCalls,
- jointValuePtrs.at(0),
- timeSinceLastIteration.toMicroSeconds());
- *targetJointXPtr = pid.getValue() + someParamSetViaConfig;
- }
- virtual void setSomeParamChangedViaIce(float param, const Ice::Current& = GlobalIceCurrent) override
- {
- std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
- getWriterControlStruct().parameterChangedViaIceCalls = param;
- writeControlStruct();
- }
- }
- //register the controller
- //this line has to appear in some cpp of your lib to do its registration (including it is enough)
- LVL1ControllerRegistration<SomeController> registrationSomeController("SomeController");
- }
- * \endcode
- */
template <typename ControlDataStruct>
class LVL1ControllerWithTripleBuffer: virtual public LVL1Controller
{
@@ -354,27 +424,6 @@ namespace armarx
};
template <typename ControlDataStruct>
using LVL1ControllerWithTripleBufferPtr = IceInternal::Handle<LVL1ControllerWithTripleBuffer<ControlDataStruct>>;
-
-
- struct PlatformCartesianVelocity
- {
- float vx;
- float vy;
- float vAngle;
- PlatformCartesianVelocity() : vx(0), vy(0), vAngle(0)
- { }
- };
-
-
- class AbstractLvl1PlatformVelocityController : public virtual LVL1ControllerWithTripleBuffer<PlatformCartesianVelocity>
- {
-
- public:
- virtual void setTarget(float vx, float vy, float vAngle) = 0;
- };
-
- typedef boost::shared_ptr <AbstractLvl1PlatformVelocityController> AbstractLvl1PlatformVelocityControllerPtr;
-
-
}
+#include "LVL1Controller.hpp"
#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1Controller.hpp b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1Controller.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..cede05a859b7a698f0c7da9906ad898ed21cd30b
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1Controller.hpp
@@ -0,0 +1,135 @@
+/*
+ * 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::LVL1Controller
+ * @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_LVL1Controller_HPP
+#define _ARMARX_LIB_RobotAPI_LVL1Controller_HPP
+
+#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
+
+namespace armarx
+{
+ template<class ItT>
+ boost::optional<std::vector<char>> LVL1Controller::AreNotInConflict(ItT first, ItT last)
+ {
+ if (first == last)
+ {
+ return {true, std::vector<char>{}};
+ }
+ std::size_t n = (*first)->getControlDeviceUsedBitmap().size();
+ std::vector<char> inuse(n, false);
+ while (first != last)
+ {
+ auto r = (*first)->isNotInConflictWith(inuse);
+ if (!r)
+ {
+ return r;
+ }
+ inuse = std::move(r.get());
+ ++first;
+ }
+ return {true, std::move(inuse)};
+ }
+
+ namespace detail
+ {
+ template<class LVL1ControllerT, class = void, class = void>
+ class LVL1ControllerRegistryEntryHelper : public LVL1ControllerRegistryEntry
+ {
+ virtual LVL1ControllerPtr create(LVL1ControllerDescriptionProviderInterfacePtr prov, const LVL1ControllerConfigPtr& config, const boost::shared_ptr<VirtualRobot::Robot>& rob) const final
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(prov, "DataProvider is NULL!");
+ return new LVL1ControllerT(prov, config, rob);
+ }
+ virtual WidgetDescription::WidgetPtr GenerateConfigDescription(const boost::shared_ptr<VirtualRobot::Robot>&, const std::map<std::string, ConstControlDevicePtr>&, const std::map<std::string, ConstSensorDevicePtr>&) const final
+ {
+ ARMARX_CHECK_EXPRESSION(!"This function should never be called");
+ }
+ virtual LVL1ControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap&) const
+ {
+ ARMARX_CHECK_EXPRESSION(!"This function should never be called");
+ }
+ virtual bool hasRemoteConfiguration() const
+ {
+ return false;
+ }
+ };
+
+ ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(hasGenerateConfigDescription, GenerateConfigDescription, LVL1Controller::GenerateConfigDescriptionFunctionSignature);
+ ARMARX_META_MAKE_HAS_MEMBER_FNC_CHECK(hasGenerateConfigFromVariants, GenerateConfigFromVariants, LVL1Controller::GenerateConfigFromVariantsFunctionSignature);
+
+#define ARMARX_ASSERT_LVL1CONTROLLER_HAS_CONSTRUCTION_GUI(T) \
+ static_assert \
+ ( \
+ ::armarx::detail::hasGenerateConfigDescription<T>::value && \
+ ::armarx::detail::hasGenerateConfigFromVariants<T>::value, \
+ #T " does not offer a construction gui" \
+ )
+
+ template<class LVL1ControllerT>
+ class LVL1ControllerRegistryEntryHelper <
+ LVL1ControllerT,
+ // meta::void_t<decltype(LVL1ControllerT::GenerateConfigDescription)>,
+ typename std::enable_if < hasGenerateConfigDescription<LVL1ControllerT>::value >::type,
+ // meta::void_t<decltype(LVL1ControllerT::GenerateConfigFromVariants)
+ typename std::enable_if < hasGenerateConfigFromVariants<LVL1ControllerT>::value >::type
+ > : public LVL1ControllerRegistryEntry
+ {
+ virtual LVL1ControllerPtr create(LVL1ControllerDescriptionProviderInterfacePtr prov, const LVL1ControllerConfigPtr& config, const boost::shared_ptr<VirtualRobot::Robot>& rob) const final
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(prov, "DataProvider is NULL!");
+ return new LVL1ControllerT(prov, config, rob);
+ }
+ virtual WidgetDescription::WidgetPtr GenerateConfigDescription(
+ const boost::shared_ptr<VirtualRobot::Robot>& robot,
+ const std::map<std::string, ConstControlDevicePtr>& controlDevices,
+ const std::map<std::string, ConstSensorDevicePtr>& sensorDevices
+ ) const final
+ {
+ WidgetDescription::WidgetPtr cfg = LVL1ControllerT::GenerateConfigDescription(robot, controlDevices, sensorDevices);
+ if (!cfg)
+ {
+ cfg = new WidgetDescription::Widget;
+ }
+ return cfg;
+ }
+ virtual LVL1ControllerConfigPtr GenerateConfigFromVariants(const StringVariantBaseMap& variants) const
+ {
+ return LVL1ControllerT::GenerateConfigFromVariants(variants);
+ }
+ virtual bool hasRemoteConfiguration() const
+ {
+ return true;
+ }
+ };
+ }
+
+ using LVL1ControllerRegistry = Registrar<std::unique_ptr<LVL1ControllerRegistryEntry>>;
+ template<class ControllerType>
+ struct LVL1ControllerRegistration
+ {
+ LVL1ControllerRegistration(const std::string& name)
+ {
+ LVL1ControllerRegistry::registerElement(name, std::unique_ptr<LVL1ControllerRegistryEntry>(new detail::LVL1ControllerRegistryEntryHelper<ControllerType>));
+ }
+ };
+}
+#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1HolonomicPlatformUnitVelocityPassThroughController.cpp
similarity index 50%
rename from source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.cpp
rename to source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1HolonomicPlatformUnitVelocityPassThroughController.cpp
index 5d3805bd03eec6bf416a6cc4e4b96e22dd023039..6874ebde43fc50adf2d02044f470c23541adcb93 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1HolonomicPlatformUnitVelocityPassThroughController.cpp
@@ -13,30 +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::LVL1HolonomicPlatformVelocityPassThroughController
+ * @package RobotAPI::ArmarXObjects::LVL1HolonomicPlatformUnitVelocityPassThroughController
* @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 "LVL1HolonomicPlatformUnitVelocityPassThroughController.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)
+LVL1HolonomicPlatformUnitVelocityPassThroughController::LVL1HolonomicPlatformUnitVelocityPassThroughController(LVL1ControllerDescriptionProviderInterfacePtr prov, LVL1ControllerConfigPtr config, const VirtualRobot::RobotPtr&)
{
- 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());
+ LVL1HolonomicPlatformUnitVelocityPassThroughControllerConfigPtr cfg = LVL1HolonomicPlatformUnitVelocityPassThroughControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "The provided config has the wrong type! The type is " << config->ice_id());
- 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);
+ target = prov->getControlTarget(cfg->platformName, ControlModes::HolonomicPlatformVelocity)->asA<ControlTargetHolonomicPlatformVelocity>();
+ ARMARX_CHECK_EXPRESSION_W_HINT(target, "The actuator " << cfg->platformName << " has no control mode " << ControlModes::HolonomicPlatformVelocity);
initialSettings.velocityX = cfg->initialVelocityX;
initialSettings.velocityY = cfg->initialVelocityY;
@@ -44,16 +40,15 @@ LVL1HolonomicPlatformVelocityPassThroughController::LVL1HolonomicPlatformVelocit
reinitTripleBuffer(initialSettings);
}
-void LVL1HolonomicPlatformVelocityPassThroughController::rtRun(const IceUtil::Time&, const IceUtil::Time&)
+void LVL1HolonomicPlatformUnitVelocityPassThroughController::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, const Ice::Current&)
+void LVL1HolonomicPlatformUnitVelocityPassThroughController::setVelocites(float velocityX, float velocityY,
+ float velocityRotation)
{
LockGuardType guard {controlDataMutex};
getWriterControlStruct().velocityX = velocityX;
@@ -62,6 +57,5 @@ void LVL1HolonomicPlatformVelocityPassThroughController::setVelocites(float velo
writeControlStruct();
}
-
-LVL1ControllerRegistration<LVL1HolonomicPlatformVelocityPassThroughController>
-registrationLVL1HolonomicPlatformVelocityPassThroughController("LVL1HolonomicPlatformVelocityPassThroughController");
+LVL1ControllerRegistration<LVL1HolonomicPlatformUnitVelocityPassThroughController>
+registrationLVL1HolonomicPlatformUnitVelocityPassThroughController("LVL1HolonomicPlatformUnitVelocityPassThroughController");
diff --git a/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1HolonomicPlatformUnitVelocityPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1HolonomicPlatformUnitVelocityPassThroughController.h
new file mode 100644
index 0000000000000000000000000000000000000000..31f0dbbf5d0190cb90a50ce298204cd2a0e52c5a
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1HolonomicPlatformUnitVelocityPassThroughController.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::LVL1HolonomicPlatformUnitVelocityPassThroughController
+ * @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_LVL1HolonomicPlatformUnitVelocityPassThroughController_H
+#define _ARMARX_LIB_RobotAPI_LVL1HolonomicPlatformUnitVelocityPassThroughController_H
+
+#include <VirtualRobot/Robot.h>
+
+#include "LVL1Controller.h"
+#include "../ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
+#include "../util.h"
+
+namespace armarx
+{
+
+ TYPEDEF_PTRS_HANDLE(LVL1HolonomicPlatformUnitVelocityPassThroughControllerConfig);
+ class LVL1HolonomicPlatformUnitVelocityPassThroughControllerConfig: virtual public LVL1ControllerConfig
+ {
+ public:
+ std::string platformName;
+ float initialVelocityX;
+ float initialVelocityY;
+ float initialVelocityRotation;
+ };
+
+ TYPEDEF_PTRS_HANDLE(LVL1HolonomicPlatformUnitVelocityPassThroughController);
+ class LVL1HolonomicPlatformUnitVelocityPassThroughControllerControlData
+ {
+ public:
+ float velocityX = 0;
+ float velocityY = 0;
+ float velocityRotation = 0;
+ };
+
+ TYPEDEF_PTRS_HANDLE(LVL1HolonomicPlatformUnitVelocityPassThroughController);
+ class LVL1HolonomicPlatformUnitVelocityPassThroughController:
+ virtual public LVL1ControllerWithTripleBuffer<LVL1HolonomicPlatformUnitVelocityPassThroughControllerControlData>
+ {
+ public:
+ LVL1HolonomicPlatformUnitVelocityPassThroughController(LVL1ControllerDescriptionProviderInterfacePtr prov, LVL1ControllerConfigPtr config, const VirtualRobot::RobotPtr&);
+
+ virtual void rtRun(const IceUtil::Time&, const IceUtil::Time&) override;
+
+ //for the platform unit
+ void setVelocites(float velocityX, float velocityY, float velocityRotation);
+
+ //ice interface
+ virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override
+ {
+ return "LVL1HolonomicPlatformUnitVelocityPassThroughController";
+ }
+ protected:
+ virtual void onInitComponent() override {}
+ virtual void onConnectComponent() override {}
+
+ ControlTargetHolonomicPlatformVelocity* target;
+ LVL1HolonomicPlatformUnitVelocityPassThroughControllerControlData initialSettings;
+ };
+}
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1KinematicUnitPassThroughController.cpp b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1KinematicUnitPassThroughController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..138f6033c6cfaefb31ab28d3c4a1689eba71680a
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1KinematicUnitPassThroughController.cpp
@@ -0,0 +1,64 @@
+/*
+ * 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::LVL1KinematicUnitPassThroughController
+ * @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 "LVL1KinematicUnitPassThroughController.h"
+using namespace armarx;
+
+LVL1ControllerRegistration<LVL1KinematicUnitPassThroughController> registrationControllerLVL1KinematicUnitPassThroughController("LVL1KinematicUnitPassThroughController");
+
+LVL1KinematicUnitPassThroughController::LVL1KinematicUnitPassThroughController(LVL1ControllerDescriptionProviderInterfacePtr prov, const LVL1ControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+{
+ LVL1KinematicUnitPassThroughControllerConfigPtr cfg = LVL1KinematicUnitPassThroughControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "The provided config has the wrong type! The type is " << config->ice_id());
+
+ const SensorValueBase* sv = prov->getSensorValue(cfg->deviceName);
+ ControlTargetBase* ct = prov->getControlTarget(cfg->deviceName, cfg->controlMode);
+ //get sensor
+ if (cfg->controlMode == ControlModes::Position1DoF)
+ {
+ ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorPosition>());
+ sensor = &(sv->asA<SensorValue1DoFActuatorPosition>()->position);
+ ARMARX_CHECK_EXPRESSION(ct->asA<ControlTarget1DoFActuatorPosition>());
+ target = &(ct->asA<ControlTarget1DoFActuatorPosition>()->position);
+ sensorToControlOnActivateFactor = 1;
+ }
+ else if (cfg->controlMode == ControlModes::Velocity1DoF)
+ {
+ ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorVelocity>());
+ sensor = &(sv->asA<SensorValue1DoFActuatorVelocity>()->velocity);
+ ARMARX_CHECK_EXPRESSION(ct->asA<ControlTarget1DoFActuatorVelocity>());
+ target = &(ct->asA<ControlTarget1DoFActuatorVelocity>()->velocity);
+ sensorToControlOnActivateFactor = 1;
+ }
+ else if (cfg->controlMode == ControlModes::Torque1DoF)
+ {
+ ARMARX_CHECK_EXPRESSION(sv->asA<SensorValue1DoFActuatorTorque>());
+ sensor = &(sv->asA<SensorValue1DoFActuatorTorque>()->torque);
+ ARMARX_CHECK_EXPRESSION(ct->asA<ControlTarget1DoFActuatorTorque>());
+ target = &(ct->asA<ControlTarget1DoFActuatorTorque>()->torque);
+ sensorToControlOnActivateFactor = 0;
+ }
+ else
+ {
+ throw InvalidArgumentException {"Unsupported control mode: " + cfg->controlMode};
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1KinematicUnitPassThroughController.h b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1KinematicUnitPassThroughController.h
new file mode 100644
index 0000000000000000000000000000000000000000..bf8e62a5e90e839ff63035ee41dc3fb9417ef664
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1KinematicUnitPassThroughController.h
@@ -0,0 +1,94 @@
+/*
+ * 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::LVL1KinematicUnitPassThroughController
+ * @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_LVL1KinematicUnitPassThroughController_H
+#define _ARMARX_LIB_RobotAPI_LVL1KinematicUnitPassThroughController_H
+
+#include <atomic>
+
+#include <VirtualRobot/Robot.h>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include "LVL1Controller.h"
+#include "../RobotUnit.h"
+#include "../SensorValues/SensorValue1DoFActuator.h"
+
+#include "../ControlTargets/ControlTarget1DoFActuator.h"
+
+namespace armarx
+{
+ TYPEDEF_PTRS_HANDLE(LVL1KinematicUnitPassThroughControllerConfig);
+ class LVL1KinematicUnitPassThroughControllerConfig : virtual public LVL1ControllerConfig
+ {
+ public:
+ std::string deviceName;
+ std::string controlMode;
+ };
+
+
+ TYPEDEF_PTRS_HANDLE(LVL1KinematicUnitPassThroughController);
+ class LVL1KinematicUnitPassThroughController:
+ virtual public LVL1Controller
+ {
+ public:
+ inline LVL1KinematicUnitPassThroughController(LVL1ControllerDescriptionProviderInterfacePtr prov, const LVL1ControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ inline virtual void rtRun(const IceUtil::Time&, const IceUtil::Time&) override
+ {
+ *target = control;
+ }
+ inline virtual void rtPreActivateController() override
+ {
+ ARMARX_IMPORTANT << "<<<<<<<<<<<<<<<<<<<<<< ACT " << this;
+ control = (*sensor) * sensorToControlOnActivateFactor;
+ }
+ virtual void rtPostDeactivateController() override
+ {
+ ARMARX_IMPORTANT << "<<<<<<<<<<<<<<<<<<<<<< DEC " << this;
+ }
+
+ //ice interface
+ inline virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override
+ {
+ return "LVL1KinematicUnitPassThroughController";
+ }
+
+ void set(float val)
+ {
+ control = val;
+ }
+
+ protected:
+ std::atomic<float> control {0};
+ float* target {nullptr};
+ const float* sensor
+ {
+ nullptr
+ };
+ float sensorToControlOnActivateFactor {0};
+
+ virtual void onInitComponent() override {}
+ virtual void onConnectComponent() override {}
+ };
+}
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1TrajectoryController.cpp b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1TrajectoryController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6a79e673031739340335fcc01e65881acf05b514
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1TrajectoryController.cpp
@@ -0,0 +1,140 @@
+#include "LVL1TrajectoryController.h"
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <VirtualRobot/TimeOptimalTrajectory/TimeOptimalTrajectory.h>
+
+
+namespace armarx
+{
+
+ LVL1ControllerRegistration<LVL1TrajectoryController> registrationControllerLVL1TrajectoryController("LVL1TrajectoryController");
+
+
+ LVL1TrajectoryController::LVL1TrajectoryController(LVL1ControllerDescriptionProviderInterfacePtr prov, const LVL1ControllerConfigPtr& config, const VirtualRobot::RobotPtr&)
+ {
+ cfg = LVL1TrajectoryControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_EXPRESSION_W_HINT(cfg, "Needed type: LVL1TrajectoryControllerConfigPtr");
+ // trajectory = TrajectoryPtr::dynamicCast(cfg->trajectory);
+
+ for (size_t i = 0; i < cfg->jointNames.size(); i++)
+ {
+ auto jointName = cfg->jointNames.at(i);
+ ControlTargetBase* ct = prov->getControlTarget(jointName, ControlModes::Velocity1DoF);
+ const SensorValueBase* sv = prov->getSensorValue(jointName);
+ targets.push_back(ct->asA<ControlTarget1DoFActuatorVelocity>());
+ sensors.push_back(sv->asA<SensorValue1DoFActuatorPosition>());
+ PIDs.push_back(PIDControllerPtr(new PIDController(cfg->PID_p,
+ cfg->PID_i, cfg->PID_d,
+ cfg->maxVelocity)));
+ }
+
+
+ }
+
+ std::string LVL1TrajectoryController::getClassName(const Ice::Current&) const
+ {
+ return "LVL1TrajectoryController";
+ }
+
+ void LVL1TrajectoryController::rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ TIMING_START(TrajectoryControl);
+ const Trajectory& trajectory = *rtGetControlStruct().trajectory;
+ if (startTime.toMicroSeconds() == 0)
+ {
+ startTime = sensorValuesTimestamp;
+ }
+ auto timeSinceStart = sensorValuesTimestamp - startTime;
+ auto positions = trajectory.getStates(timeSinceStart.toSecondsDouble(), 0);
+ auto velocities = trajectory.getStates(timeSinceStart.toSecondsDouble(), 1);
+ // size_t dim = trajectory->dim();
+ for (size_t d = 0; d < targets.size(); ++d)
+ {
+ ARMARX_CHECK_EXPRESSION(d < targets.size());
+ ARMARX_CHECK_EXPRESSION(d < PIDs.size());
+ ARMARX_CHECK_EXPRESSION(d < sensors.size());
+ ARMARX_CHECK_EXPRESSION(d < positions.size());
+ ARMARX_CHECK_EXPRESSION(d < velocities.size());
+ PIDControllerPtr& pid = PIDs.at(d);
+ pid->update(timeSinceLastIteration.toSecondsDouble(),
+ sensors.at(d)->position,
+ positions.at(d));
+ double newVel = pid->getControlValue();
+ newVel += velocities.at(1);
+ targets.at(d)->velocity = newVel;
+ }
+ TIMING_END(TrajectoryControl);
+
+ }
+
+ void LVL1TrajectoryController::setTrajectory(const TrajectoryBasePtr& t, const Ice::Current&)
+ {
+ TIMING_START(TrajectoryOptimization);
+ ARMARX_CHECK_EXPRESSION(t);
+ TrajectoryPtr trajectory = TrajectoryPtr::dynamicCast(t);
+ ARMARX_CHECK_EXPRESSION(trajectory);
+ size_t trajectoryDimensions = trajectory->dim();
+ ARMARX_CHECK_EXPRESSION(trajectoryDimensions == targets.size());
+ std::list<Eigen::VectorXd> pathPoints;
+ float timeStep = cfg->preSamplingStepMs / 1000;
+ Eigen::VectorXd maxVelocities = Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxVelocity);
+ Eigen::VectorXd maxAccelerations = Eigen::VectorXd::Constant(trajectoryDimensions, cfg->maxAcceleration);
+ for (const Trajectory::TrajData & element : *trajectory)
+ {
+ Eigen::VectorXd waypoint(trajectoryDimensions);
+ for (size_t i = 0; i < trajectoryDimensions; ++i)
+ {
+ waypoint(i) = element.getPosition(i);
+ }
+ pathPoints.emplace_back(waypoint);
+ }
+
+ VirtualRobot::Path p(pathPoints, cfg->maxDeviation);
+ VirtualRobot::TimeOptimalTrajectory timeOptimalTraj(p, maxVelocities, maxAccelerations);
+
+
+ TrajectoryPtr newTraj = new Trajectory();
+
+ Ice::DoubleSeq newTimestamps;
+ double duration = timeOptimalTraj.getDuration();
+ for (double t = 0.0; t < duration; t += timeStep)
+ {
+ newTimestamps.push_back(t);
+ }
+ newTimestamps.push_back(duration);
+
+ for (size_t d = 0; d < trajectoryDimensions; d++)
+ {
+ // Ice::DoubleSeq position;
+ // for (double t = 0.0; t < duration; t += timeStep)
+ // {
+ // position.push_back(timeOptimalTraj.getPosition(t)[d]);
+ // }
+ // position.push_back(timeOptimalTraj.getPosition(duration)[d]);
+ // newTraj->addDimension(position, newTimestamps, trajectory->getDimensionName(d));
+
+ Ice::DoubleSeq derivs;
+ for (double t = 0.0; t < duration; t += timeStep)
+ {
+ derivs.clear();
+ derivs.push_back(timeOptimalTraj.getPosition(t)[d]);
+ derivs.push_back(timeOptimalTraj.getVelocity(t)[d]);
+ newTraj->addDerivationsToDimension(d, t, derivs);
+ }
+ derivs.clear();
+ derivs.push_back(timeOptimalTraj.getPosition(duration)[d]);
+ derivs.push_back(timeOptimalTraj.getVelocity(duration)[d]);
+ newTraj->addDerivationsToDimension(d, duration, derivs);
+ }
+ newTraj->setDimensionNames(trajectory->getDimensionNames());
+ TIMING_END(TrajectoryOptimization);
+ LockGuardType guard {controlDataMutex};
+ getWriterControlStruct().trajectory = newTraj;
+ writeControlStruct();
+ }
+
+ void LVL1TrajectoryController::rtPreActivateController()
+ {
+ startTime = IceUtil::Time();
+ }
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1TrajectoryController.h b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1TrajectoryController.h
new file mode 100644
index 0000000000000000000000000000000000000000..5b3ab3e477d29f738c79192ca81d96b2d259360a
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/LVL1Controllers/LVL1TrajectoryController.h
@@ -0,0 +1,53 @@
+#ifndef ARMARX_LVL1TRAJECTORYCONTROLLER_H
+#define ARMARX_LVL1TRAJECTORYCONTROLLER_H
+
+#include "LVL1Controller.h"
+#include <VirtualRobot/Robot.h>
+#include <RobotAPI/interface/units/RobotUnit/LVL1TrajectoryController.h>
+#include <RobotAPI/libraries/core/PIDController.h>
+#include <RobotAPI/libraries/core/Trajectory.h>
+#include <RobotAPI/components/units/RobotUnit/ControlTargets/ControlTarget1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+
+
+namespace armarx
+{
+ TYPEDEF_PTRS_HANDLE(LVL1HolonomicPlatformUnitVelocityPassThroughController);
+ class LVL1HolonomicPlatformUnitVelocityPassThroughControllerControlData
+ {
+ public:
+ TrajectoryPtr trajectory = TrajectoryPtr(new Trajectory());
+ };
+
+ class LVL1TrajectoryController :
+ public LVL1ControllerWithTripleBuffer<LVL1HolonomicPlatformUnitVelocityPassThroughControllerControlData>,
+ public LVL1TrajectoryControllerInterface
+ {
+ public:
+ LVL1TrajectoryController(LVL1ControllerDescriptionProviderInterfacePtr prov, const LVL1ControllerConfigPtr& config, const VirtualRobot::RobotPtr&);
+
+ // LVL1ControllerInterface interface
+ std::string getClassName(const Ice::Current&) const override;
+
+ // LVL1Controller interface
+ void rtPreActivateController() override;
+ void rtRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration) override;
+
+ // LVL1TrajectoryControllerInterface interface
+ void setTrajectory(const TrajectoryBasePtr& t, const Ice::Current&) override;
+ private:
+ // TrajectoryPtr trajectory;
+ IceUtil::Time startTime;
+ std::vector<PIDControllerPtr> PIDs;
+ std::vector<ControlTarget1DoFActuatorVelocity*> targets;
+ std::vector<const SensorValue1DoFActuatorPosition*> sensors;
+ LVL1TrajectoryControllerConfigPtr cfg;
+ virtual void onInitComponent() override {}
+ virtual void onConnectComponent() override {}
+
+
+ };
+
+}
+
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0d3c98080aba74788b01c116447a758dd063dcd5
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.cpp
@@ -0,0 +1,2263 @@
+/*
+ * 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::RobotUnit
+ * @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 "RobotUnit.h"
+
+#include <algorithm>
+
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/Nodes/ForceTorqueSensor.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/RobotNodeSet.h>
+
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/system/DynamicLibrary.h>
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
+
+#include "Units/ForceTorqueSubUnit.h"
+#include "Units/InertialMeasurementSubUnit.h"
+#include "Units/KinematicSubUnit.h"
+#include "Units/PlatformSubUnit.h"
+
+
+namespace armarx
+{
+ namespace detail
+ {
+ template<class TimeT = std::chrono::microseconds>
+ struct TimerTag
+ {
+ //assume it is std::chrono
+
+ using ClockT = std::chrono::high_resolution_clock;
+ const ClockT::time_point beg;
+
+ TimerTag() : beg {ClockT::now()} {}
+
+ TimeT stop()
+ {
+ return std::chrono::duration_cast<TimeT>(ClockT::now() - beg);
+ }
+ };
+
+ template<>
+ struct TimerTag<TimestampVariant> : TimerTag<>
+ {
+ TimestampVariant stop()
+ {
+ return {TimerTag<std::chrono::microseconds>::stop().count()};
+ }
+ };
+
+ template<>
+ struct TimerTag<IceUtil::Time> : TimerTag<>
+ {
+ TimestampVariant stop()
+ {
+ return IceUtil::Time::microSeconds(TimerTag<std::chrono::microseconds>::stop().count());
+ }
+ };
+
+ template <class Fun, class TimeT>
+ TimeT operator*(TimerTag<TimeT>&& t, Fun&& fn)
+ {
+ fn();
+ return t.stop();
+ }
+
+ //for virtual time
+
+ template<class TimeT = IceUtil::Time>
+ struct VirtualTimerTag;
+
+ template<>
+ struct VirtualTimerTag<TimestampVariant>
+ {
+ const IceUtil::Time beg;
+ VirtualTimerTag() : beg {TimeUtil::GetTime()} {}
+ TimestampVariant stop()
+ {
+ return {TimeUtil::GetTime() - beg};
+ }
+ };
+
+ template<>
+ struct VirtualTimerTag<IceUtil::Time>
+ {
+ const IceUtil::Time beg;
+ VirtualTimerTag() : beg {TimeUtil::GetTime()} {}
+ TimestampVariant stop()
+ {
+ return TimeUtil::GetTime() - beg;
+ }
+ };
+
+ template <class Fun, class TimeT>
+ TimeT operator*(VirtualTimerTag<TimeT>&& t, Fun&& fn)
+ {
+ fn();
+ return t.stop();
+ }
+ }
+}
+#define ARMARX_TIMER(...) ::armarx::detail::TimerTag<__VA_ARGS__>{} *[&]
+#define ARMARX_VIRTUAL_TIMER(...) ::armarx::detail::VirtualTimerTag<__VA_ARGS__>{} *[&]
+
+
+namespace std
+{
+ std::string to_string(armarx::RobotUnitState s)
+ {
+ switch (s)
+ {
+ case armarx::RobotUnitState::PreComponentInitialization:
+ return "PreComponentInitialization";
+ case armarx::RobotUnitState::InitializingDevices:
+ return "InitializingDevices";
+ case armarx::RobotUnitState::InitializingUnits:
+ return "InitializingUnits";
+ case armarx::RobotUnitState::WaitingForRTThreadInitialization:
+ return "WaitingForRTThreadInitialization";
+ case armarx::RobotUnitState::Running:
+ return "Running";
+ case armarx::RobotUnitState::Exiting:
+ return "Exiting";
+ }
+ throw std::invalid_argument {"Unknown state " + std::to_string(static_cast<std::size_t>(s))};
+ }
+}
+
+namespace armarx
+{
+
+ namespace detail
+ {
+ std::function<std::string(const std::string&)> makeDebugObserverNameFixer(const std::string& prefix)
+ {
+ return
+ [prefix](const std::string & name)
+ {
+ std::string s = prefix + name;
+ std::replace(s.begin(), s.end(), '.', '_');
+ std::replace(s.begin(), s.end(), ':', '_');
+ return s;
+ };
+ }
+ }
+
+
+ const std::string RobotUnit::rtThreadTimingsSensorDeviceName = "RTThreadTimings";
+
+ const std::set<RobotUnitState> RobotUnit::devicesReadyStates {RobotUnitState::InitializingUnits, RobotUnitState::WaitingForRTThreadInitialization, RobotUnitState::Running};
+ RobotUnit::GuardType RobotUnit::getGuard() const
+ {
+ ARMARX_CHECK_EXPRESSION_W_HINT(std::this_thread::get_id() != rtThreadId, "Atempted to lock mutex in RT thread");
+ return GuardType {dataMutex};
+ }
+
+ std::chrono::microseconds RobotUnit::MicroNow()
+ {
+ return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch());
+ }
+
+ void RobotUnit::checkLVL1ControllerClassName(const std::string& className) const
+ {
+ if (!LVL1ControllerRegistry::has(className))
+ {
+ std::stringstream ss;
+ ss << "Requested controller class '" << className << "' unknown! Known classes:" << LVL1ControllerRegistry::getKeys()
+ << " (If this class exists in a different lib then load it via loadLibFromPath(path) or loadLibFromPackage(package, lib))";
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ }
+
+ void RobotUnit::addControlDevice(const ControlDevicePtr& cd)
+ {
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
+ controlDevices.add(cd->getDeviceName(), cd);
+ ARMARX_INFO << "added ControlDevice " << cd->getDeviceName();
+ }
+
+ void RobotUnit::addSensorDevice(const SensorDevicePtr& sd)
+ {
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
+ if (!sd)
+ {
+ std::stringstream ss;
+ ss << "RobotUnit::addSensorDevice: SensorDevice is nullptr";
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ if (!sd->getSensorValue())
+ {
+ std::stringstream ss;
+ ss << "RobotUnit::addSensorDevice: SensorDevice " << sd->getDeviceName() << " has null SensorValue (this is not allowed)";
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ if (sd->getDeviceName() == rtThreadTimingsSensorDeviceName)
+ {
+ if (!std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd))
+ {
+ throw InvalidArgumentException
+ {
+ "You tried to add a SensorDevice with the name " + sd->getDeviceName() + " which does not derive from RTThreadTimingsSensorDevice. (Don't do this)"
+ };
+ }
+ sensorDevices.add(sd->getDeviceName(), sd); //this checks if we already added such a device (do this before setting timingSensorDevice)
+ rtThreadTimingsSensorDevice = std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd);
+ }
+ else
+ {
+ sensorDevices.add(sd->getDeviceName(), sd);
+ }
+ ARMARX_INFO << "added SensorDevice " << sd->getDeviceName() << " (valuetype = " << sd->getSensorValueType() << ")";
+ }
+
+ RTThreadTimingsSensorDevicePtr RobotUnit::createRTThreadTimingSensorDevice() const
+ {
+ return std::make_shared<RTThreadTimingsSensorDeviceImpl<>>(rtThreadTimingsSensorDeviceName);
+ }
+
+ void RobotUnit::finishDeviceInitialization()
+ {
+ auto beg = MicroNow();
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingDevices, __FUNCTION__);
+ if (!sensorDevices.has(rtThreadTimingsSensorDeviceName))
+ {
+ addSensorDevice(createRTThreadTimingSensorDevice());
+ }
+ state = RobotUnitState::InitializingUnits;
+ //check added devices
+ ARMARX_DEBUG << "checking ControlDevices:";
+ for (const ControlDevicePtr& controlDevice : controlDevices.values())
+ {
+ ARMARX_CHECK_EXPRESSION(controlDevice);
+ ARMARX_DEBUG << "----" << controlDevice->getDeviceName();
+ if (!controlDevice->hasLVL0Controller(ControlModes::EmergencyStop))
+ {
+ std::stringstream s;
+ s << "ControlDevice " << controlDevice->getDeviceName()
+ << " has no LVL0Controller with ControlMode " << ControlModes::EmergencyStop
+ << " (to fix this, add a LVL0Controller with this ControlMode to the ControlDevice).\nAvailable controllers: "
+ << controlDevice->getControlModes();
+ ARMARX_ERROR << "--------" << s.str();
+ throw LogicError {s.str()};
+
+ }
+ if (!controlDevice->hasLVL0Controller(ControlModes::StopMovement))
+ {
+ std::stringstream s;
+ s << "ControlDevice " << controlDevice->getDeviceName()
+ << " has no LVL0Controller with ControlMode \"" << ControlModes::StopMovement
+ << "\" (to fix this, add a LVL0Controller with this ControlMode to the ControlDevice) \nAvailable controllers: "
+ << controlDevice->getControlModes();
+ ARMARX_ERROR << "--------" << s.str();
+ throw LogicError {s.str()};
+ }
+ }
+ ARMARX_DEBUG << "checking sensor devices:";
+ for (const SensorDevicePtr& sensorDevice : sensorDevices.values())
+ {
+ ARMARX_CHECK_EXPRESSION(sensorDevice);
+ ARMARX_DEBUG << "----" << sensorDevice->getDeviceName();
+ if (!sensorDevice->getSensorValue())
+ {
+ std::stringstream s;
+ s << "SensorDevice " << sensorDevice->getSensorValue() << " has null SensorValue";
+ ARMARX_ERROR << "--------" << s.str();
+ throw LogicError {s.str()};
+ }
+ ARMARX_CHECK_EXPRESSION(sensorDevice);
+ }
+
+ ARMARX_DEBUG << "initializing buffers:";
+ {
+ //ctrl buffers
+ {
+ ARMARX_DEBUG << "----initializing controller buffers";
+ controllersActivated.reinitAllBuffers(LVL0AndLVL1Controllers {getControllerBufferSize()});
+ LVL0AndLVL1Controllers ctrlinitReq(getControllerBufferSize());
+ for (std::size_t i = 0 ; i < getControllerBufferSize(); ++i)
+ {
+ ctrlinitReq.lvl0Controllers.at(i) = controlDevices.at(i)->rtGetLVL0StopMovementController();
+ }
+ controllersRequested.reinitAllBuffers(ctrlinitReq);
+ controllersRequested.commitWrite();//to make sure the diry bit is activated (to trigger rt switch)
+ }
+ //sensor/control buffer
+ {
+ ARMARX_DEBUG << "----initializing SensorValue/ControlTartget buffer";
+ SensorAndControl scinitRead;
+ SensorAndControl scinitHidden;
+ SensorAndControl scinitWrite;
+ scinitRead.sensors.reserve(getNumberOfSensorDevices());
+ scinitHidden.sensors.reserve(getNumberOfSensorDevices());
+ scinitWrite.sensors.reserve(getNumberOfSensorDevices());
+ ARMARX_DEBUG << "--------cloning SensorValues";
+ for (ConstSensorDevicePtr s : sensorDevices.values())
+ {
+ ARMARX_DEBUG << "------------of device " << s->getDeviceName() << "(type = " << s->getSensorValueType() << ")";
+ scinitRead.sensors.emplace_back(s->getSensorValue()->clone());
+ scinitHidden.sensors.emplace_back(s->getSensorValue()->clone());
+ scinitWrite.sensors.emplace_back(s->getSensorValue()->clone());
+ }
+ scinitRead.control.reserve(getControllerBufferSize());
+ scinitHidden.control.reserve(getControllerBufferSize());
+ scinitWrite.control.reserve(getControllerBufferSize());
+ ARMARX_DEBUG << "--------cloning ControlTargets";
+ for (ConstControlDevicePtr c : controlDevices.values())
+ {
+ ARMARX_DEBUG << "------------of device " << c->getDeviceName();
+ scinitRead.control.emplace_back();
+ scinitHidden.control.emplace_back();
+ scinitWrite.control.emplace_back();
+ auto& scinitReadCD = scinitRead.control.back();
+ auto& scinitHiddenCD = scinitHidden.control.back();
+ auto& scinitWriteCD = scinitWrite.control.back();
+ scinitReadCD.reserve(c->getLVL0Controllers().size());
+ scinitHiddenCD.reserve(c->getLVL0Controllers().size());
+ scinitWriteCD.reserve(c->getLVL0Controllers().size());
+ for (const LVL0Controller* lvl0 : c->getLVL0Controllers())
+ {
+ ARMARX_DEBUG << "----------------mode " << lvl0->getControlMode() << "(type = " << lvl0->getControlTarget()->getControlTargetType() << ")";
+ scinitReadCD.emplace_back(lvl0->getControlTarget()->clone());
+ scinitHiddenCD.emplace_back(lvl0->getControlTarget()->clone());
+ scinitWriteCD.emplace_back(lvl0->getControlTarget()->clone());
+ }
+ }
+ sensorAndControl.reinitAllBuffers(std::move(scinitWrite), std::move(scinitHidden), std::move(scinitRead));
+ }
+ }
+ ARMARX_DEBUG << "copying device ptrs to const ptr map";
+ {
+ for (const auto& dev : controlDevices.values())
+ {
+ controlDevicesConstPtr[dev->getDeviceName()] = dev;
+ }
+ for (const auto& dev : sensorDevices.values())
+ {
+ sensorDevicesConstPtr[dev->getDeviceName()] = dev;
+ }
+ }
+ ARMARX_VERBOSE << "ControlDevices:\n" << controlDevices.keys();
+ ARMARX_VERBOSE << "SensorDevices:\n" << sensorDevices.keys();
+ ARMARX_INFO << "finishDeviceInitialization...done! " << (MicroNow() - beg).count() << " us";
+ }
+
+ void RobotUnit::initializeDefaultUnits()
+ {
+ auto beg = MicroNow();
+ {
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ ARMARX_INFO << "initializing default units";
+ initializeKinematicUnit();
+ initializePlatformUnit();
+ initializeForceTorqueUnit();
+ initializeInertialMeasurementUnit();
+ }
+ ARMARX_INFO << "initializing default units...done! " << (MicroNow() - beg).count() << " us";
+ }
+
+ void RobotUnit::initializeKinematicUnit()
+ {
+ using UnitT = KinematicSubUnit;
+ using IfaceT = KinematicUnitInterface;
+
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ //check if unit is already added
+ if (getUnit(IfaceT::ice_staticId()))
+ {
+ return;
+ }
+ //add ctrl et al
+ std::map<std::string, UnitT::ActuatorData> devs;
+ for (const ControlDevicePtr& controlDevice : controlDevices.values())
+ {
+ const auto& controlDeviceName = controlDevice->getDeviceName();
+ if (!controlDevice->hasTag(ControlDeviceTags::CreateNoDefaultController))
+ {
+ UnitT::ActuatorData ad;
+ ad.name = controlDeviceName;
+ ad.sensorDeviceIndex = sensorDevices.has(controlDeviceName) ? sensorDevices.index(controlDeviceName) : std::numeric_limits<std::size_t>::max();
+ /// TODO use better kinematic unit controllers (posThroughVel + ramps)
+#define initializeKinematicUnit_tmp_helper(mode, CtargT, ctrl) \
+ { \
+ const auto& controlMode = mode; \
+ LVL1KinematicUnitPassThroughControllerPtr& pt = ctrl; \
+ LVL0Controller* lvl0 = controlDevice->getLVL0Controller(controlMode); \
+ if (lvl0 && lvl0->getControlTarget()->isA<CtargT>()) \
+ { \
+ LVL1KinematicUnitPassThroughControllerConfigPtr config = new LVL1KinematicUnitPassThroughControllerConfig; \
+ config->controlMode=controlMode; \
+ config->deviceName=controlDeviceName; \
+ LVL1ControllerPtr lvl1 = createController( \
+ "LVL1KinematicUnitPassThroughController", \
+ "LVL1KU_PTCtrl_"+controlDeviceName+"_"+controlMode, \
+ config \
+ ); \
+ pt = LVL1KinematicUnitPassThroughControllerPtr::dynamicCast(lvl1); \
+ ARMARX_CHECK_EXPRESSION(pt); \
+ } \
+ }
+ initializeKinematicUnit_tmp_helper(ControlModes::Position1DoF, ControlTarget1DoFActuatorPosition, ad.ctrlPos)
+ initializeKinematicUnit_tmp_helper(ControlModes::Velocity1DoF, ControlTarget1DoFActuatorVelocity, ad.ctrlVel)
+ initializeKinematicUnit_tmp_helper(ControlModes::Torque1DoF , ControlTarget1DoFActuatorTorque , ad.ctrlTor)
+#undef initializeKinematicUnit_tmp_helper
+
+
+ if (ad.ctrlPos || ad.ctrlTor || ad.ctrlVel)
+ {
+ devs[controlDeviceName] = std::move(ad);
+ }
+ }
+ }
+ ARMARX_CHECK_EXPRESSION_W_HINT(!devs.empty(), "no 1DoF ControlDevice found with matching SensorDevice");
+ //add it
+ const std::string configName = getProperty<std::string>("KinematicUnitName");
+ const std::string& configDomain = "ArmarX";
+ const std::string confPre = configDomain + "." + configName + ".";
+ Ice::PropertiesPtr properties = getIceProperties()->clone();
+ //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
+ //fill properties
+ properties->setProperty(confPre + "RobotNodeSetName" , robotNodeSetName);
+ properties->setProperty(confPre + "RobotFileName" , robotFileName);
+ properties->setProperty(confPre + "RobotFileNameProject", robotProjectName);
+ IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties , configName, configDomain);
+ //fill devices (sensor + controller)
+ unit->setupData(getProperty<std::string>("RobotFileName").getValue(), robot->clone(robot->getName()), std::move(devs), this);
+ //add
+ addUnit(unit);
+ }
+
+ void RobotUnit::initializePlatformUnit()
+ {
+ using UnitT = PlatformSubUnit;
+ using IfaceT = PlatformUnitInterface;
+
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ //check if unit is already added
+ if (getUnit(IfaceT::ice_staticId()))
+ {
+ return;
+ }
+ //is there a platform dev?
+ if (getRobotPlatformName().empty())
+ {
+ ARMARX_INFO << "no platform unit created since no platform name was given";
+ return;
+ }
+ if (!controlDevices.has(getRobotPlatformName()))
+ {
+ ARMARX_WARNING << "no platform unit created since the platform control device with name '" << getRobotPlatformName() << "' was not found";
+ return;
+ }
+ const ControlDevicePtr& controlDevice = controlDevices.at(robotPlatformName);
+ const SensorDevicePtr& sensorDevice = sensorDevices.at(robotPlatformName);
+ LVL0Controller* lvl0vel = controlDevice->getLVL0Controller(ControlModes::HolonomicPlatformVelocity);
+ ARMARX_CHECK_EXPRESSION(lvl0vel);
+ ARMARX_CHECK_EXPRESSION(sensorDevice->getSensorValue()->asA<SensorValueHolonomicPlatform>());
+ //add it
+ const std::string configName = getProperty<std::string>("PlatformUnitName");
+ const std::string& configDomain = "ArmarX";
+ const std::string confPre = configDomain + "." + configName + ".";
+ Ice::PropertiesPtr properties = getIceProperties()->clone();
+ //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
+ //fill properties
+ properties->setProperty(confPre + "PlatformName", robotPlatformName);
+ IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties , configName, configDomain);
+ //config
+ LVL1HolonomicPlatformUnitVelocityPassThroughControllerConfigPtr config = new LVL1HolonomicPlatformUnitVelocityPassThroughControllerConfig;
+ config->initialVelocityX = 0;
+ config->initialVelocityY = 0;
+ config->initialVelocityRotation = 0;
+ config->platformName = robotPlatformName;
+ auto ctrl = LVL1HolonomicPlatformUnitVelocityPassThroughControllerPtr::dynamicCast(
+ createController("LVL1HolonomicPlatformUnitVelocityPassThroughController", configName + "_VelPTContoller", config)
+ );
+ ARMARX_CHECK_EXPRESSION(ctrl);
+ unit->pt = ctrl;
+ unit->platformSensorIndex = sensorDevices.index(robotPlatformName);
+ //add
+ addUnit(unit);
+ }
+
+ void RobotUnit::initializeForceTorqueUnit()
+ {
+ using UnitT = ForceTorqueSubUnit;
+ using IfaceT = ForceTorqueUnitInterface;
+
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ //check if unit is already added
+ if (getUnit(IfaceT::ice_staticId()))
+ {
+ return;
+ }
+ //check if it is required
+ auto sensors = getRobot()->getSensors<VirtualRobot::ForceTorqueSensor>();
+ std::vector<ForceTorqueSubUnit::DeviceData> ftdevs;
+ ftdevs.reserve(sensors.size());
+ for (const auto& ft : sensors)
+ {
+ auto deviceName = ft->getName();
+ if (sensorDevices.has(deviceName))
+ {
+ const SensorDevicePtr& sensorDevice = sensorDevices.at(deviceName);
+ if (sensorDevice->getSensorValue()->isA<SensorValueForceTorque>())
+ {
+ ForceTorqueSubUnit::DeviceData dat;
+ dat.sensorIndex = sensorDevices.index(deviceName);
+ dat.deviceName = std::move(deviceName);
+ dat.frame = ft->getRobotNode()->getName();
+ ftdevs.emplace_back(std::move(dat));
+ }
+ else
+ {
+ ARMARX_WARNING << "ForceTorqueSensorDevice " << deviceName << " has no SensorValueForceTorque";
+ }
+ }
+ else
+ {
+ ARMARX_WARNING << "ForceTorqueSensor with name " << deviceName << " has no corresponding sensor device";
+ }
+ }
+ if (ftdevs.empty())
+ {
+ ARMARX_WARNING << "no force torque unit created since there are no force torque sensor devices";
+ return;
+ }
+ //add it
+ const std::string configName = getProperty<std::string>("ForceTorqueUnitName");
+ const std::string& configDomain = "ArmarX";
+ const std::string confPre = configDomain + "." + configName + ".";
+ Ice::PropertiesPtr properties = getIceProperties()->clone();
+ //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
+ //fill properties
+ properties->setProperty(confPre + "AgentName", robot->getName());
+ properties->setProperty(confPre + "ForceTorqueTopicName", getProperty<std::string>("ForceTorqueTopicName").getValue());
+ ARMARX_INFO << "creating unit " << configName << " using these properties: " << properties->getPropertiesForPrefix("");
+ IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties , configName, configDomain);
+ unit->devs = ftdevs;
+ //add
+ addUnit(unit);
+ }
+
+ void RobotUnit::initializeInertialMeasurementUnit()
+ {
+ using UnitT = InertialMeasurementSubUnit;
+ using IfaceT = InertialMeasurementUnitInterface;
+
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ //check if unit is already added
+ if (getUnit(IfaceT::ice_staticId()))
+ {
+ return;
+ }
+ //check if it is required
+ std::map<std::string, std::size_t> imudevs;
+ for (auto i : getIndices(sensorDevices.keys()))
+ {
+ const SensorDevicePtr& sensorDevice = sensorDevices.at(i);
+ if (sensorDevice->getSensorValue()->isA<SensorValueIMU>())
+ {
+ imudevs[sensorDevice->getDeviceName()] = i;
+ }
+ }
+ if (imudevs.empty())
+ {
+ ARMARX_IMPORTANT << "no inertial measurement unit created since there are no imu sensor devices";
+ return;
+ }
+ //add it
+ const std::string configName = getProperty<std::string>("InertialMeasurementUnitName");
+ const std::string& configDomain = "ArmarX";
+ const std::string confPre = configDomain + "." + configName + ".";
+ Ice::PropertiesPtr properties = getIceProperties()->clone();
+ //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
+ //fill properties
+ properties->setProperty(confPre + "IMUTopicName", getProperty<std::string>("IMUTopicName").getValue());
+ IceInternal::Handle<UnitT> unit = Component::create<UnitT>(properties , configName, configDomain);
+ unit->devs = imudevs;
+ //add
+ addUnit(unit);
+ }
+
+ void RobotUnit::addUnit(ManagedIceObjectPtr unit)
+ {
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ getArmarXManager()->addObjectAsync(unit, "");
+ units.emplace_back(std::move(unit));
+ }
+
+ void RobotUnit::finishUnitInitialization()
+ {
+
+ auto beg = MicroNow();
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
+ state = RobotUnitState::WaitingForRTThreadInitialization;
+ //start publisher
+ int pubtimestep = getProperty<std::size_t>("PublishPeriodMs");
+ if (!pubtimestep)
+ {
+ pubtimestep = 1;
+ }
+ publishNewSensorDataTime = TimeUtil::GetTime();
+ publisherTask = new PublisherTaskT([&] {publish();}, pubtimestep, false, getName() + "_PublisherTask");
+ ARMARX_INFO << "starting publisher with timestep " << pubtimestep;
+ publisherTask->start();
+ ARMARX_INFO << "finishUnitInitialization...done! " << (MicroNow() - beg).count() << " us";
+ }
+
+ void RobotUnit::initRTThread()
+ {
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::WaitingForRTThreadInitialization, __FUNCTION__);
+ rtThreadId = std::this_thread::get_id();
+ state = RobotUnitState::Running;
+ }
+
+ bool RobotUnit::rtSwitchControllerSetup()
+ {
+ rtThreadTimingsSensorDevice->rtMarkRtSwitchControllerSetupStart();
+ if (!rtRequestedControllersChanged())
+ {
+ rtThreadTimingsSensorDevice->rtMarkRtSwitchControllerSetupEnd();
+ return false;
+ }
+
+ //handle lvl1
+ {
+ //"merge"
+ std::size_t n = rtGetControllerBufferSize();
+ std::size_t idxAct = 0;
+ std::size_t idxReq = 0;
+ for (std::size_t i = 0; i < n; ++i)
+ {
+ //skip nullptrs in act
+ while (idxAct < n && !rtGetActivatedLVL1Controllers().at(idxAct))
+ {
+ ++idxAct;
+ }
+ const LVL1ControllerPtr& req = idxReq < n ? rtGetRequestedLVL1Controllers().at(idxReq) : nullptr; //may be null
+ const LVL1ControllerPtr& act = idxAct < n ? rtGetActivatedLVL1Controllers().at(idxAct) : nullptr; //may be null if it is the last
+ std::size_t reqId = req ? req->rtGetId() : std::numeric_limits<std::size_t>::max();
+ std::size_t actId = act ? act->rtGetId() : std::numeric_limits<std::size_t>::max();
+
+ if (reqId < actId)
+ {
+ //new ctrl
+ RtActivateLVL1Controller(req);
+ ++idxReq;
+ }
+ else if (reqId > actId)
+ {
+ RtDeactivateLVL1Controller(act);
+ ++idxAct;
+ }
+ else //if(reqId == actId)
+ {
+ //same ctrl or both null ctrl
+ ++idxReq;
+ ++idxAct;
+ }
+ if (idxAct >= n && !req)
+ {
+ break;
+ }
+ }
+ rtGetActivatedLVL1Controllers() = rtGetRequestedLVL1Controllers();
+ }
+
+ //handle lvl0
+ {
+ for (std::size_t i = 0; i < rtGetControllerBufferSize(); ++i)
+ {
+ auto& controlDevice = controlDevices.at(i);
+ const auto requestedLVL0 = rtGetRequestedLVL0Controllers().at(i);
+ controlDevice->rtSetActiveLVL0Controller(requestedLVL0);
+ rtGetActivatedLVL0Controllers().at(i) = requestedLVL0;
+ }
+ }
+ rtWriteActivatedControllers();
+ rtThreadTimingsSensorDevice->rtMarkRtSwitchControllerSetupEnd();
+ return true;
+ }
+
+ void RobotUnit::rtDeactivateAssignedLVL1Controller(std::size_t ctrlDevIndex, bool writeActivatedControlers)
+ {
+ std::size_t lvl1Index = std::numeric_limits<std::size_t>::max();
+ for (std::size_t i = 0; i < rtGetControllerBufferSize(); ++i)
+ {
+ const LVL1ControllerPtr& lvl1 = rtGetActivatedLVL1Controllers().at(i);
+ if (lvl1 && lvl1->rtUsesControlDevice(ctrlDevIndex))
+ {
+ //found corresponding lvl1
+ lvl1Index = i;
+ break;
+ }
+ }
+ ARMARX_CHECK_EXPRESSION_W_HINT(
+ lvl1Index < rtGetControllerBufferSize(),
+ lvl1Index << " < " << rtGetControllerBufferSize() <<
+ ": no lvl1 controller controls this device (name = "
+ << controlDevices.at(ctrlDevIndex)->getDeviceName()
+ << ", ControlMode = " << rtGetActivatedLVL0Controllers().at(ctrlDevIndex)->getControlMode() << ")!" << "\n"
+ << VAROUT(rtGetActivatedLVL0Controllers()) << "\n"
+ << VAROUT(rtGetActivatedLVL1Controllers())
+ );
+ const LVL1ControllerPtr& lvl1 = rtGetActivatedLVL1Controllers().at(lvl1Index);
+ RtDeactivateLVL1ControllerBecauseOfError(lvl1);
+ for (auto ctrlDevIdx : lvl1->rtGetControlDeviceUsedIndices())
+ {
+ const ControlDevicePtr& controlDevice = controlDevices.at(ctrlDevIdx);
+ LVL0Controller* es = controlDevice->rtGetLVL0EmergencyStopController();
+ controlDevice->rtSetActiveLVL0Controller(es);
+ rtGetActivatedLVL0Controllers().at(ctrlDevIdx) = es;
+ }
+ rtGetActivatedLVL1Controllers().at(lvl1Index) = nullptr;
+
+ if (writeActivatedControlers)
+ {
+ ARMARX_IMPORTANT << "SWITCH CTRLS deac";
+ rtWriteActivatedControllers();
+ }
+ }
+
+ void RobotUnit::rtRunLVL1Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ rtThreadTimingsSensorDevice->rtMarkRtRunLVL1ControllersStart();
+ for (LVL1Controller* lvl1 : rtGetActivatedLVL1Controllers())
+ {
+ try
+ {
+ if (lvl1)
+ {
+ lvl1->rtSwapBufferAndRun(sensorValuesTimestamp, timeSinceLastIteration);
+ }
+ }
+ catch (...)
+ {
+ ARMARX_INFO << "Lvl1 Controller " << lvl1->getInstanceName() << " threw an exception and is now deactivated: " << GetHandledExceptionString();
+ RtSetLVL1ControllerActive(lvl1, 0, true);
+ }
+ }
+ rtThreadTimingsSensorDevice->rtMarkRtRunLVL1ControllersEnd();
+ }
+
+ void RobotUnit::rtRunLVL0Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ rtThreadTimingsSensorDevice->rtMarkRtRunLVL0ControllersStart();
+ for (const ControlDevicePtr& device : this->controlDevices.values())
+ {
+ device->rtRun(sensorValuesTimestamp, timeSinceLastIteration);
+ }
+ rtThreadTimingsSensorDevice->rtMarkRtRunLVL0ControllersEnd();
+ }
+
+ void RobotUnit::rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ rtThreadTimingsSensorDevice->rtMarkRtReadSensorDeviceValuesStart();
+ for (const SensorDevicePtr& device : this->sensorDevices.values())
+ {
+ device->rtReadSensorValues(sensorValuesTimestamp, timeSinceLastIteration);
+ }
+ rtThreadTimingsSensorDevice->rtMarkRtReadSensorDeviceValuesEnd();
+ }
+
+ void RobotUnit::rtHandleInvalidTargets()
+ {
+ rtThreadTimingsSensorDevice->rtMarkRtHandleInvalidTargetsStart();
+ bool oneIsInvalid = 0;
+ for (std::size_t i = 0; i < rtGetControllerBufferSize(); ++i)
+ {
+ if (!rtGetActivatedLVL0Controllers().at(i)->rtIsTargetVaild())
+ {
+ rtDeactivateAssignedLVL1Controller(i, false);
+ oneIsInvalid = true;
+ }
+ }
+ if (oneIsInvalid)
+ {
+ ARMARX_VERBOSE << "ONE IS INVALID";
+ rtWriteActivatedControllers();
+ }
+ rtThreadTimingsSensorDevice->rtMarkRtHandleInvalidTargetsEnd();
+ }
+
+ void RobotUnit::rtResetAllTargets()
+ {
+ rtThreadTimingsSensorDevice->rtMarkRtResetAllTargetsStart();
+ for (const ControlDevicePtr& controlDev : controlDevices.values())
+ {
+ controlDev->rtGetActiveLVL0Controller()->rtResetTarget();
+ }
+ rtThreadTimingsSensorDevice->rtMarkRtResetAllTargetsEnd();
+ }
+
+ ConstSensorDevicePtr RobotUnit::getSensorDevice(const std::string& sensorDeviceName) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return sensorDevices.at(sensorDeviceName, SensorDevice::NullPtr);
+ }
+
+ ConstControlDevicePtr RobotUnit::getControlDevice(const std::string& deviceName)
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controlDevices.at(deviceName, ControlDevice::NullPtr);
+ }
+
+ ControlTargetBase* RobotUnit::getControlTarget(const std::string& deviceName, const std::string& controlMode)
+ {
+ static const std::string fname = __FUNCTION__;
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ if (std::this_thread::get_id() != initLVL1ThreadId)
+ {
+ throw std::logic_error
+ {
+ fname + ": Must be only called during instantiation of a LVL1 controller from the instantiated controler " +
+ "(Don't try to call this function outside of a LVL1Controller's constructor)"
+ };
+ }
+ if (initLVL1UsedControlTargets.find(deviceName) != initLVL1UsedControlTargets.end())
+ {
+ throw std::logic_error
+ {
+ fname + ": Must not request two ControlTargets for the same device. (got = " +
+ initLVL1UsedControlTargets.at(deviceName) + ", requested " + controlMode + ") " +
+ "(You can only have a ControlDevice in one ControlMode. " +
+ "To check all available ControlModes for this device, get the ControlDevice via getControlDevice(" + deviceName + ") and querry it)"
+ };
+ }
+ // we should allow these modes for debugging reasons + there is no real reason to forbid them
+ //if (controlMode == ControlModes::EmergencyStop)
+ //{
+ // throw std::invalid_argument {"RobotUnit::getControlTarget: Can't get target for the control mode " + ControlModes::EmergencyStop};
+ //}
+ //if (controlMode == ControlModes::StopMovement)
+ //{
+ // throw std::invalid_argument {"RobotUnit::getControlTarget: Can't get target for the control mode " + ControlModes::StopMovement};
+ //}
+ ControlDevicePtr controlDevice = controlDevices.at(deviceName, ControlDevice::NullPtr);
+ if (!controlDevice)
+ {
+ return nullptr;
+ }
+ if (!controlDevice->hasLVL0Controller(controlMode))
+ {
+ return nullptr;
+ }
+ //there is a device and a target as was requested:
+ initLVL1UsedControlTargets[deviceName] = controlMode;
+ return controlDevice->getLVL0Controller(controlMode)->getControlTarget();
+ }
+
+ LVL1ControllerInterfacePrx RobotUnit::createController(const std::string& className, const std::string& instanceName, const LVL1ControllerConfigPtr& config, const Ice::Current&)
+ {
+ //no lock required
+ return LVL1ControllerInterfacePrx::uncheckedCast(createController(className, instanceName, config)->getProxy(-1, true));
+ }
+
+ LVL1ControllerInterfacePrx RobotUnit::createControllerFromVariantConfig(const std::string& className, const std::string& instanceName, const StringVariantBaseMap& variants, const Ice::Current&)
+ {
+ //no lock required
+ checkLVL1ControllerClassName(className);
+ if (!LVL1ControllerRegistry::get(className)->hasRemoteConfiguration())
+ {
+ std::stringstream ss;
+ ss << "Requested controller class '" << className << "' allows no remote configuration" << LVL1ControllerRegistry::getKeys()
+ << " (Implement 'static WidgetDescription::WidgetPtr " << className << "::GenerateConfigDescription()'"
+ << " and 'static LVL1ControllerConfigPtr " << className << "::GenerateConfigFromVariants(const StringVariantBaseMap&)' to allow remote configuration";
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ return createController(className, instanceName, LVL1ControllerRegistry::get(className)->GenerateConfigFromVariants(variants), GlobalIceCurrent/*to select ice overload*/);
+ }
+
+ LVL1ControllerPtr RobotUnit::createController(const std::string& className, const std::string& instanceName, const LVL1ControllerConfigPtr& config)
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ if (instanceName.empty())
+ {
+ ARMARX_ERROR << "The instance name is empty! (give a unique name)";
+ throw InvalidArgumentException {"The instance name is empty! (give a unique name)"};
+ }
+ //check if we would be able to create the class
+ checkLVL1ControllerClassName(className);
+ auto& factory = LVL1ControllerRegistry::get(className);
+
+ //check if the instance name is already in use
+ if (lvl1Controllers.count(instanceName))
+ {
+ std::stringstream ss;
+ ss << "There already is a controller instance with the name '" << instanceName << "'. Use a different instance name instead."
+ << " Other used instance names are " << getLVL1ControllerNames();
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ //create the controller
+ ARMARX_CHECK_EXPRESSION(initLVL1UsedControlTargets.empty());//in case someone broke things
+ initLVL1ThreadId = std::this_thread::get_id();
+ ARMARX_ON_SCOPE_EXIT
+ {
+ initLVL1UsedControlTargets.clear();
+ initLVL1ThreadId = std::thread::id{};
+ };
+ LVL1ControllerPtr lvl1 = factory->create(LVL1ControllerDescriptionProviderInterfacePtr::dynamicCast(RobotUnitPtr {this}), config, robot);
+
+ std::vector<char> ctrlDeviceUsedBitmap(getNumberOfControlDevices(), false);
+ std::vector<std::size_t> ctrlDeviceUsedIndices;
+ ctrlDeviceUsedIndices.reserve(getNumberOfControlDevices());
+ for (const auto& cd2targ : initLVL1UsedControlTargets)
+ {
+ auto idx = controlDevices.index(cd2targ.first);
+ ctrlDeviceUsedBitmap.at(idx) = true;
+ ctrlDeviceUsedIndices.emplace_back(idx);
+ }
+ InitLVL1Controler(lvl1, this, lvl1ControllerNextId++, initLVL1UsedControlTargets, std::move(ctrlDeviceUsedBitmap), std::move(ctrlDeviceUsedIndices));
+ getArmarXManager()->addObjectAsync(lvl1, instanceName);
+ lvl1Controllers[instanceName] = lvl1;
+ listenerPrx->lvl1ControllerAdded(instanceName);
+ return lvl1;
+ }
+
+ void RobotUnit::setActivateControllersRequest(std::set<LVL1ControllerPtr, LVL1Controller::IdComp>&& ctrls)
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ //erase nullptr
+ ctrls.erase(nullptr);
+ ARMARX_CHECK_EXPRESSION(ctrls.size() <= getControllerBufferSize());
+ if (std::set<LVL1ControllerPtr, LVL1Controller::IdComp> {getRequestedLVL1Controllers().begin(), getRequestedLVL1Controllers().end()} == ctrls)
+ {
+ //redirty the flag to swap in the same set again
+ controllersRequested.commitWrite();
+ return;
+ }
+ LVL0AndLVL1Controllers request {getControllerBufferSize()};
+ std::size_t idx = 0;
+ for (const LVL1ControllerPtr& lvl1 : ctrls)
+ {
+ request.lvl1Controllers.at(idx++) = lvl1.get();
+ //add lvl0 for this ctrl
+ for (const auto& cd2cm : lvl1->getControlDeviceUsedControlModeMap())
+ {
+ std::size_t lvl0idx = controlDevices.index(cd2cm.first);
+ if (request.lvl0Controllers.at(lvl0idx))
+ {
+ std::stringstream ss;
+ ss << "RobotUnit:setActivateControllersRequest controllers to activate are in conflict!\ncontrollers:";
+ for (const auto& ctrl : ctrls)
+ {
+ ss << "\n" << ctrl->getInstanceName();
+ }
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ request.lvl0Controllers.at(lvl0idx) = controlDevices.at(lvl0idx)->getLVL0Controller(cd2cm.second);
+ }
+ }
+ for (auto i : getIndices(request.lvl0Controllers))
+ {
+ LVL0Controller*& lvl0 = request.lvl0Controllers.at(i);
+ if (!lvl0)
+ {
+ LVL0Controller* lvl0Old = getRequestedLVL0Controllers().at(i);
+ if (lvl0Old == controlDevices.at(i)->getLVL0StopMovementController())
+ {
+ //don't replace this controller with emergency stop
+ lvl0 = lvl0Old;
+ }
+ else
+ {
+ //no one controls this device -> emergency stop
+ lvl0 = controlDevices.at(i)->getLVL0EmergencyStopController();
+ }
+ }
+ }
+ writeRequestedControllers(std::move(request));
+ ARMARX_INFO << "requested controllers:\n" << getRequestedLVL1ControllerNames();
+ }
+
+ void RobotUnit::activateController(const std::string& name, const Ice::Current&)
+ {
+ activateControllers({name});
+ }
+
+ void RobotUnit::activateControllers(const Ice::StringSeq& names, const Ice::Current&)
+ {
+ ARMARX_DEBUG << "requesting controller activation for:\n" << names;
+ if (names.empty())
+ {
+ return;
+ }
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ auto ctrlsToActVec = getLVL1ControllersNotNull(names); //also checks if these controllers exist
+ ARMARX_DEBUG << "all requested controllers exist" << std::flush;
+ std::set<LVL1ControllerPtr, LVL1Controller::IdComp> ctrlsToAct {ctrlsToActVec.begin(), ctrlsToActVec.end()};
+ ARMARX_CHECK_EXPRESSION(!ctrlsToAct.count(nullptr));
+ std::set<LVL1ControllerPtr, LVL1Controller::IdComp> ctrls {getRequestedLVL1Controllers().begin(), getRequestedLVL1Controllers().end()};
+ ctrls.erase(nullptr);
+ //check for conflict
+ std::vector<char> inuse;
+ //check requested controllers
+ {
+ auto r = LVL1Controller::AreNotInConflict(ctrlsToAct.begin(), ctrlsToAct.end());
+ if (!r)
+ {
+ std::stringstream ss;
+ ss << "RobotUnit::activateControllers: requested controllers are in conflict!\ncontrollers:\n" << names;
+ ARMARX_ERROR << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ inuse = std::move(r.get());
+ }
+ ARMARX_DEBUG << "all requested controllers are conflict free" << std::flush;
+ ARMARX_DEBUG << "inuse field \n" << inuse;
+ //add already active controllers if they are conflict free
+ {
+ if (ctrls.empty())
+ {
+ ARMARX_DEBUG << "no already requested LVL1Controllers";
+ }
+ for (const LVL1ControllerPtr& lvl1 : ctrls)
+ {
+ if (ctrlsToAct.count(lvl1))
+ {
+ continue;
+ }
+ auto r = lvl1->isNotInConflictWith(inuse);
+ if (r)
+ {
+ ARMARX_DEBUG << "keeping already requested LVL1Controller '" << lvl1->getInstanceName() << "' in list of requested controllers";
+ ctrlsToAct.insert(lvl1);
+ inuse = std::move(r.get());
+ }
+ else
+ {
+ ARMARX_INFO << "removing already requested LVL1Controller '" << lvl1->getInstanceName() << "' from list of requested controllers";
+ }
+ }
+ ARMARX_DEBUG << "inuse field \n" << inuse;
+ }
+ setActivateControllersRequest(std::move(ctrlsToAct));
+ }
+
+ void RobotUnit::deactivateController(const std::string& name, const Ice::Current&)
+ {
+ deactivateControllers(Ice::StringSeq {name});
+ }
+
+ void RobotUnit::deactivateControllers(const Ice::StringSeq& names, const Ice::Current&)
+ {
+ if (names.empty())
+ {
+ return;
+ }
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ auto ctrlsDeacVec = getLVL1ControllersNotNull(names); //also checks if these controllers exist
+ std::set<LVL1ControllerPtr, LVL1Controller::IdComp> ctrls {getRequestedLVL1Controllers().begin(), getRequestedLVL1Controllers().end()};
+ for (const auto& lvl1ToDeactivate : ctrlsDeacVec)
+ {
+ ctrls.erase(lvl1ToDeactivate);
+ }
+ setActivateControllersRequest(std::move(ctrls));
+ }
+
+ void RobotUnit::switchControllerSetup(const Ice::StringSeq& newSetup, const Ice::Current&)
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ auto ctrlsToActVec = getLVL1ControllersNotNull(newSetup); //also checks if these controllers exist
+ setActivateControllersRequest({ctrlsToActVec.begin(), ctrlsToActVec.end()});
+ }
+
+ void RobotUnit::publish()
+ {
+ auto begPublish = Now();
+ static const int spamdelay = 1;
+ std::set<RobotUnitState> skipStates
+ {
+ RobotUnitState::PreComponentInitialization,
+ RobotUnitState::InitializingDevices,
+ RobotUnitState::InitializingUnits,
+ RobotUnitState::WaitingForRTThreadInitialization
+ };
+ if (skipStates.count(state))
+ {
+ ARMARX_DEBUG << deactivateSpam(spamdelay) << "skipping publishing in state " << state;
+ return;
+ }
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::Running, __FUNCTION__);
+ auto debugObserverBatchPrx = debugObserverPrx->ice_batchOneway();
+ //swap buffers in
+ const bool haveActivatedControllersChanged = activatedControllersChanged();
+ const bool haveSensorAndControlValsChanged = sensorAndControlBufferChanged();
+ //setup datastructures
+ const auto& sensorAndControlBuffer = getSensorAndControlBuffer();
+ const auto& activatedControllers = getActivatedControllers();
+ const auto& requestedControllers = getRequestedControllers();
+
+ const auto timestamp = sensorAndControlBuffer.sensorValuesTimestamp;
+
+ const auto numSensorDevices = getNumberOfSensorDevices();
+ const auto numControlDevices = getNumberOfControlDevices();
+
+ auto listenerBatchPrx = listenerPrx->ice_batchOneway();
+ const bool publishDebugObserver = !(publishIterationCount % debugObserverSkipIterations);
+ const bool debugObserverPublishControlTargetsThisIteration = debugObserverPublishControlTargets;
+ const bool debugObserverPublishSensorValuesThisIteration = debugObserverPublishSensorValues;
+ StringVariantBaseMap debugObserverMap;
+
+ if (haveSensorAndControlValsChanged)
+ {
+ //update units
+ debugObserverMap["publishTimings_UnitUpdate"] = new TimedVariant
+ {
+ ARMARX_TIMER(TimestampVariant)
+ {
+ ARMARX_DEBUG << deactivateSpam(spamdelay) << "updating units with new sensor values";
+ publishNewSensorDataTime = TimeUtil::GetTime();
+ for (ManagedIceObjectPtr& u : units)
+ {
+ RobotUnitSubUnitPtr rsu = RobotUnitSubUnitPtr::dynamicCast(u);
+ if (rsu && rsu->getObjectScheduler() && rsu->getProxy())
+ {
+ debugObserverMap["publishTimings_UnitUpdate_" + rsu->getName()] = new TimedVariant
+ {
+ ARMARX_TIMER(TimestampVariant){rsu->update(sensorAndControlBuffer, activatedControllers);},
+ timestamp
+ };
+ }
+ }
+ },
+ timestamp
+ };
+ //publish sensor updates
+
+ debugObserverMap["publishTimings_SensorUpdates"] = new TimedVariant
+ {
+ ARMARX_TIMER(TimestampVariant)
+ {
+ for (std::size_t sensidx = 0 ; sensidx < numSensorDevices; ++sensidx)
+ {
+ const SensorDevice& sensDev = *(sensorDevices.at(sensidx));
+ const SensorValueBase& sensVal = *(sensorAndControlBuffer.sensors.at(sensidx));
+ auto variants = sensVal.toVariants(timestamp);
+
+ if (!variants.empty())
+ {
+ if (debugObserverPublishSensorValuesThisIteration && publishDebugObserver)
+ {
+ transformMapKeys(
+ variants, debugObserverMap,
+ detail::makeDebugObserverNameFixer(sensDev.getDeviceName() + "_" + sensVal.getSensorValueType(true) + "_")
+ );
+ }
+ SensorDeviceStatus status;
+ status.deviceName = sensDev.getDeviceName();
+ status.sensorValue = std::move(variants);
+ status.timestampUSec = MicroNow().count();
+ listenerBatchPrx->sensorDeviceStatusChanged(status);
+ }
+ }
+ },
+ timestamp
+ };
+ }
+ else
+ {
+ const double sensSecondsAgo = (TimeUtil::GetTime() - publishNewSensorDataTime).toSecondsDouble();
+ if (sensSecondsAgo > 1)
+ {
+ ARMARX_WARNING << deactivateSpam(spamdelay) << "RobotUnit::publish: Last sensor value update is " << sensSecondsAgo << " seconds ago!";
+ }
+ }
+ if (haveSensorAndControlValsChanged || haveActivatedControllersChanged)
+ {
+ debugObserverMap["publishTimings_ControlUpdates"] = new TimedVariant
+ {
+ ARMARX_TIMER(TimestampVariant)
+ {
+ for (std::size_t ctrlidx = 0 ; ctrlidx < numControlDevices; ++ctrlidx)
+ {
+ const ControlDevice& ctrlDev = *(controlDevices.at(ctrlidx));
+
+ StringToStringVariantBaseMapMap variants;
+ for (const auto& ctrlVal : sensorAndControlBuffer.control.at(ctrlidx))
+ {
+ const auto& controlMode = ctrlVal->getControlMode();
+ variants[controlMode] = ctrlVal->toVariants(timestamp);
+ if (
+ haveSensorAndControlValsChanged &&
+ !variants[controlMode].empty() &&
+ debugObserverPublishControlTargetsThisIteration &&
+ publishDebugObserver
+ )
+ {
+ transformMapKeys(
+ variants[controlMode], debugObserverMap,
+ detail::makeDebugObserverNameFixer(ctrlDev.getDeviceName() + "_" + controlMode + "_")
+ );
+ }
+ }
+ ControlDeviceStatus status;
+ const auto activeLVL0 = activatedControllers.lvl0Controllers.at(ctrlidx);
+ status.activeControlMode = activeLVL0 ? activeLVL0->getControlMode() : std::string {"!!LVL0Controller is nullptr!!"};
+ status.deviceName = ctrlDev.getDeviceName();
+ ARMARX_CHECK_EXPRESSION(requestedControllers.lvl0Controllers.at(ctrlidx));
+ status.requestedControlMode = requestedControllers.lvl0Controllers.at(ctrlidx)->getControlMode();
+ status.controlTargetValues = std::move(variants);
+ status.timestampUSec = MicroNow().count();
+ listenerBatchPrx->controlDeviceStatusChanged(status);
+ }
+ },
+ timestamp
+ };
+ }
+ //call publish hook + publish LVL1Controller changes
+ debugObserverMap["publishTimings_LVL1Updates"] = new TimedVariant
+ {
+ ARMARX_TIMER(TimestampVariant)
+ {
+ for (const LVL1ControllerPtr& lvl1 : getMapValues(lvl1Controllers))
+ {
+ debugObserverMap["publishTimings_LVL1Updates_" + lvl1->getInstanceName()] = new TimedVariant
+ {
+ ARMARX_TIMER(TimestampVariant)
+ {
+ //run some hook for active (used for visu)
+ PublishLVL1Controller(lvl1, sensorAndControlBuffer, debugDrawerPrx, debugObserverBatchPrx);
+ if (
+ GetLVL1ControllerStatusReportedActive(lvl1) != lvl1->isControllerActive() ||
+ GetLVL1ControllerStatusReportedRequested(lvl1) != lvl1->isControllerRequested()
+ )
+ {
+ UpdateLVL1ControllerStatusReported(lvl1);
+ listenerBatchPrx->lVl1ControllerStatusChanged(lvl1->getControllerStatus());
+ }
+ },
+ timestamp
+ };
+ }
+ },
+ timestamp
+ };
+
+ //report new class names
+ debugObserverMap["publishTimings_ClassNameUpdates"] = new TimedVariant
+ {
+ ARMARX_TIMER(TimestampVariant)
+ {
+ const auto classNames = LVL1ControllerRegistry::getKeys();
+ if (lastReportedClasses.size() != classNames.size())
+ {
+ for (const auto& name : classNames)
+ {
+ if (!lastReportedClasses.count(name))
+ {
+ listenerBatchPrx->lvl1ControllerClassAdded(name);
+ lastReportedClasses.emplace(name);
+ }
+ }
+ }
+ },
+ timestamp
+ };
+ debugObserverMap["publishTimings_RobotUnitListenerFlush"] = new TimedVariant {ARMARX_TIMER(TimestampVariant){listenerBatchPrx->ice_flushBatchRequests();}, timestamp};
+
+ if (publishDebugObserver)
+ {
+ debugObserverMap["publishTimings_LastDebugObserverFlush"] = new TimedVariant {TimestampVariant{lastDebugObserverFlush.count()}, timestamp};
+ debugObserverMap["publishTimings_LastPublishLoop"] = new TimedVariant {TimestampVariant{lastPublishLoop.count()}, timestamp};
+ debugObserverBatchPrx->setDebugChannel(getName(), debugObserverMap);
+ lastDebugObserverFlush = ARMARX_TIMER()
+ {
+ debugObserverBatchPrx->ice_flushBatchRequests();
+ };
+ }
+ else
+ {
+ lastDebugObserverFlush = std::chrono::microseconds {0};
+ }
+
+ //warn if there are unset lvl0 controllers
+ {
+ const auto& lvl0s = getActivatedLVL0Controllers();
+ if (std::any_of(lvl0s.begin(), lvl0s.end(), [](const LVL0Controller * lvl0)
+ {
+ return !lvl0;
+ }))
+ {
+ ARMARX_WARNING << "RobotUnit::publish: Some activated LVL0Controllers are reported to be nullptr! "
+ << "(did you forget to call rtSwitchControllerSetup()?)";
+ }
+ }
+ ++publishIterationCount;
+ lastPublishLoop = MicroToNow(begPublish);
+ }
+
+ void RobotUnit::removeAllUnits()
+ {
+ for (ManagedIceObjectPtr& unit : units)
+ {
+ checkRefCountIsOne(unit, unit->getName());
+ getArmarXManager()->removeObjectBlocking(unit);
+ checkRefCountIsOne(unit, unit->getName());
+ }
+ units.clear();
+ }
+
+ void RobotUnit::finishRunning()
+ {
+ if (state == RobotUnitState::Exiting)
+ {
+ ARMARX_ERROR << "RobotUnit::finishRunning called multiple times!";
+ return;
+ }
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::Running, __FUNCTION__);
+ state = RobotUnitState::Exiting;
+ //const ptr maps
+ controlDevicesConstPtr.clear();
+ sensorDevicesConstPtr.clear();
+ //publisher
+ publisherTask->stop();
+ while (publisherTask->isFunctionExecuting() || publisherTask->isRunning())
+ {
+ ARMARX_FATAL << deactivateSpam(0.1) << "PUBLISHER TASK IS RUNNING EVEN AFTER IT WAS STOPPED!";
+ }
+ checkRefCountIsOne(publisherTask, "publisherTask");
+ publisherTask = nullptr;
+ //RT
+ joinRTThread();
+ //units
+ removeAllUnits();
+ //lvl1
+ for (auto& n2lvl1 : lvl1Controllers)
+ {
+ LVL1ControllerPtr& lvl1 = n2lvl1.second;
+ getArmarXManager()->removeObjectBlocking(lvl1);
+ checkRefCountIsOne(lvl1, lvl1->getName());
+ }
+ lvl1Controllers.clear();
+ //clear devices
+ sensorDevices.clear();
+ controlDevices.clear();
+ //robot
+ if (robot.use_count() != 1)
+ {
+ ARMARX_WARNING << "The robot's smart pointer use count is " << robot.use_count();
+ }
+ robot.reset();
+ ARMARX_INFO << "RobotUnit finished running";
+ }
+
+ bool RobotUnit::loadLibFromPath(const std::string& path, const Ice::Current&)
+ {
+ std::string absPath;
+ if (ArmarXDataPath::getAbsolutePath(path, absPath))
+ {
+ return loadLibFromAbsolutePath(absPath);
+ }
+ else
+ {
+ ARMARX_ERROR << "Could not find library " + path;
+ return false;
+ }
+ }
+
+ bool RobotUnit::loadLibFromPackage(const std::string& package, const std::string& libname, const Ice::Current&)
+ {
+ CMakePackageFinder finder(package);
+ if (!finder.packageFound())
+ {
+ ARMARX_ERROR << "Could not find package '" << package << "'";
+ return false;
+ }
+
+ for (auto libDirPath : Split(finder.getLibraryPaths(), ";"))
+ {
+ boost::filesystem::path fullPath = libDirPath;
+ fullPath /= "lib" + libname + "." + DynamicLibrary::GetSharedLibraryFileExtension();
+ if (!boost::filesystem::exists(fullPath))
+ {
+ fullPath = libDirPath;
+ fullPath /= libname;
+ if (!boost::filesystem::exists(fullPath))
+ {
+ continue;
+ }
+ }
+ if (loadLibFromAbsolutePath(fullPath.string()))
+ {
+ return true;
+ }
+ }
+ ARMARX_ERROR << "Could not find library " << libname << " in package " << package;
+ return false;
+ }
+
+ bool RobotUnit::loadLibFromAbsolutePath(const std::string& path)
+ {
+ auto guard = getGuard();
+ if (loadedLibs.count(path))
+ {
+ return true;
+ }
+ DynamicLibraryPtr lib(new DynamicLibrary());
+ try
+ {
+ lib->load(path);
+ }
+ catch (...)
+ {
+ handleExceptions();
+ return false;
+ }
+
+ if (lib->isLibraryLoaded())
+ {
+ loadedLibs[path] = lib;
+ }
+ else
+ {
+ ARMARX_ERROR << "Could not load lib " + path + ": " + lib->getErrorMessage();
+ return false;
+ }
+ return true;
+ }
+
+ Ice::StringSeq RobotUnit::getLVL1ControllerClassNames(const Ice::Current&) const
+ {
+ return LVL1ControllerRegistry::getKeys();
+ }
+
+ LVL1ControllerClassDescription RobotUnit::getLVL1ControllerClassDescription(const std::string& className, const Ice::Current&) const
+ {
+ while (state == RobotUnitState::PreComponentInitialization || state == RobotUnitState::InitializingDevices)
+ {
+ //this phase should only last short so busy waiting is ok
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ }
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ checkLVL1ControllerClassName(className);
+ LVL1ControllerClassDescription data;
+ data.className = className;
+ if (LVL1ControllerRegistry::get(className)->hasRemoteConfiguration())
+ {
+ data.configDescription = LVL1ControllerRegistry::get(className)->GenerateConfigDescription(robot, controlDevicesConstPtr, sensorDevicesConstPtr);
+ }
+ return data;
+ }
+
+ LVL1ControllerClassDescriptionSeq RobotUnit::getLVL1ControllerClassDescriptions(const Ice::Current&) const
+ {
+ while (state == RobotUnitState::PreComponentInitialization || state == RobotUnitState::InitializingDevices)
+ {
+ //this phase should only last short so busy waiting is ok
+ std::this_thread::sleep_for(std::chrono::milliseconds(50));
+ }
+ auto guard = getGuard();
+ LVL1ControllerClassDescriptionSeq r;
+ r.reserve(LVL1ControllerRegistry::getKeys().size());
+ for (const auto& key : LVL1ControllerRegistry::getKeys())
+ {
+ r.emplace_back(getLVL1ControllerClassDescription(key));
+ }
+ return r;
+ }
+
+ Ice::StringSeq RobotUnit::getLVL1ControllerNames(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ return getMapKeys(lvl1Controllers);
+ }
+
+ Ice::StringSeq RobotUnit::getRequestedLVL1ControllerNames(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ return GetNonNullNames(getRequestedLVL1Controllers());
+ }
+
+ Ice::StringSeq RobotUnit::getActivatedLVL1ControllerNames(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ return GetNonNullNames(getActivatedLVL1Controllers());
+ }
+
+ LVL1ControllerInterfacePrx RobotUnit::getLVL1Controller(const std::string& name, const Ice::Current&) const
+ {
+ LVL1ControllerPtr ctrl;
+ {
+ auto guard = getGuard();
+ auto it = lvl1Controllers.find(name);
+ if (it == lvl1Controllers.end())
+ {
+ return nullptr;
+ }
+ ctrl = it->second;
+ }
+ return LVL1ControllerInterfacePrx::uncheckedCast(ctrl->getProxy(-1, true));
+ }
+
+ LVL1ControllerStatus RobotUnit::getLVL1ControllerStatus(const std::string& name, const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return getLVL1ControllerNotNull(name)->getControllerStatus();
+ }
+
+ LVL1ControllerStatusSeq RobotUnit::getLVL1ControllerStatuses(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ if (!areDevicesReady())
+ {
+ return {};
+ }
+ LVL1ControllerStatusSeq r;
+ r.reserve(lvl1Controllers.size());
+ for (const auto& lvl1 : lvl1Controllers)
+ {
+ r.emplace_back(lvl1.second->getControllerStatus());
+ }
+ return r;
+ }
+
+ LVL1ControllerDescription RobotUnit::getLVL1ControllerDescription(const std::string& name, const Ice::Current&) const
+ {
+ LVL1ControllerPtr lvl1;
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ lvl1 = getLVL1ControllerNotNull(name);
+ }
+ return lvl1->getControllerDescription();
+ }
+
+ LVL1ControllerDescriptionSeq RobotUnit::getLVL1ControllerDescriptions(const Ice::Current&) const
+ {
+ std::map<std::string, LVL1ControllerPtr> lvl1ControllersCopy;
+ {
+ auto guard = getGuard();
+ if (!areDevicesReady())
+ {
+ return {};
+ }
+ lvl1ControllersCopy = lvl1Controllers;
+ }
+ LVL1ControllerDescriptionSeq r;
+ r.reserve(lvl1ControllersCopy.size());
+ for (const auto& lvl1 : lvl1ControllersCopy)
+ {
+ r.emplace_back(lvl1.second->getControllerDescription());
+ }
+ return r;
+ }
+
+ LVL1ControllerDescriptionWithStatus RobotUnit::getLVL1ControllerDescriptionWithStatus(const std::string& name, const Ice::Current&) const
+ {
+ LVL1ControllerPtr lvl1;
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ lvl1 = getLVL1ControllerNotNull(name);
+ }
+ return lvl1->getControllerDescriptionWithStatus();
+ }
+
+ LVL1ControllerDescriptionWithStatusSeq RobotUnit::getLVL1ControllerDescriptionsWithStatuses(const Ice::Current&) const
+ {
+ std::map<std::string, LVL1ControllerPtr> lvl1ControllersCopy;
+ {
+ auto guard = getGuard();
+ if (!areDevicesReady())
+ {
+ return {};
+ }
+ lvl1ControllersCopy = lvl1Controllers;
+ }
+ LVL1ControllerDescriptionWithStatusSeq r;
+ r.reserve(lvl1ControllersCopy.size());
+ for (const auto& lvl1 : lvl1ControllersCopy)
+ {
+ r.emplace_back(lvl1.second->getControllerDescriptionWithStatus());
+ }
+ return r;
+ }
+
+ Ice::StringSeq RobotUnit::getControlDeviceNames(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controlDevices.keys();
+ }
+
+ ControlDeviceDescription armarx::RobotUnit::RobotUnit::getControlDeviceDescription(std::size_t idx) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ const ControlDevicePtr& controlDevice = controlDevices.at(idx);
+ ControlDeviceDescription data;
+ data.deviceName = controlDevice->getDeviceName();
+ data.tags.assign(controlDevice->getTags().begin(), controlDevice->getTags().end());
+ for (const auto& lvl0 : controlDevice->getLVL0Controllers())
+ {
+ data.contolModeToTargetType[lvl0->getControlMode()] = lvl0->getControlTarget()->getControlTargetType();
+ }
+ return data;
+ }
+
+ ControlDeviceDescription RobotUnit::getControlDeviceDescription(const std::string& name, const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ if (!controlDevices.has(name))
+ {
+ std::stringstream ss;
+ ss << "RobotUnit::getControlDeviceDescription: There is no ControlDevice '" << name
+ << "'. There are these ControlDevices: " << controlDevices.keys();
+ throw InvalidArgumentException {ss.str()};
+ }
+ return getControlDeviceDescription(controlDevices.index(name));
+ }
+
+ ControlDeviceDescriptionSeq RobotUnit::getControlDeviceDescriptions(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ if (!areDevicesReady())
+ {
+ return {};
+ }
+ ControlDeviceDescriptionSeq r;
+ r.reserve(getNumberOfControlDevices());
+ for (auto idx : getIndices(controlDevices.values()))
+ {
+ r.emplace_back(getControlDeviceDescription(idx));
+ }
+ return r;
+ }
+
+ ControlDeviceStatus armarx::RobotUnit::RobotUnit::getControlDeviceStatus(std::size_t idx) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ const ControlDevicePtr& controlDevice = controlDevices.at(idx);
+ ControlDeviceStatus status;
+ const auto activeLVL0 = getActivatedLVL0Controllers().at(idx);
+ status.activeControlMode = activeLVL0 ? activeLVL0->getControlMode() : std::string {"!!LVL0Controller is nullptr!!"};
+ status.deviceName = controlDevice->getDeviceName();
+ ARMARX_CHECK_EXPRESSION(getRequestedLVL0Controllers().at(idx));
+ status.requestedControlMode = getRequestedLVL0Controllers().at(idx)->getControlMode();
+ for (const auto& targ : getSensorAndControlBuffer().control.at(idx))
+ {
+ status.controlTargetValues[targ->getControlMode()] = targ->toVariants(getSensorAndControlBuffer().sensorValuesTimestamp);
+ }
+ status.timestampUSec = MicroNow().count();
+ return status;
+ }
+
+ ControlDeviceStatus RobotUnit::getControlDeviceStatus(const std::string& name, const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ if (!controlDevices.has(name))
+ {
+ std::stringstream ss;
+ ss << "RobotUnit::getControlDeviceStatus: There is no ControlDevice '" << name
+ << "'. There are these ControlDevices: " << controlDevices.keys();
+ throw InvalidArgumentException {ss.str()};
+ }
+ return getControlDeviceStatus(controlDevices.index(name));
+ }
+
+ ControlDeviceStatusSeq RobotUnit::getControlDeviceStatuses(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ if (!areDevicesReady())
+ {
+ return {};
+ }
+ ControlDeviceStatusSeq r;
+ r.reserve(getNumberOfControlDevices());
+ for (auto idx : getIndices(controlDevices.values()))
+ {
+ r.emplace_back(getControlDeviceStatus(idx));
+ }
+ return r;
+ }
+
+ Ice::StringSeq RobotUnit::getSensorDeviceNames(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return sensorDevices.keys();
+ }
+
+ SensorDeviceDescription armarx::RobotUnit::RobotUnit::getSensorDeviceDescription(std::size_t idx) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
+ SensorDeviceDescription data;
+ data.deviceName = sensorDevice->getDeviceName();
+ data.tags.assign(sensorDevice->getTags().begin(), sensorDevice->getTags().end());
+ data.sensorValueType = sensorDevice->getSensorValueType();
+ return data;
+ }
+
+ SensorDeviceDescription RobotUnit::getSensorDeviceDescription(const std::string& name, const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ if (!sensorDevices.has(name))
+ {
+ std::stringstream ss;
+ ss << "RobotUnit::getSensorDeviceDescription: There is no SensorDevice '" << name
+ << "'. There are these SensorDevices: " << sensorDevices.keys();
+ throw InvalidArgumentException {ss.str()};
+ }
+ return getSensorDeviceDescription(sensorDevices.index(name));
+ }
+
+ SensorDeviceDescriptionSeq RobotUnit::getSensorDeviceDescriptions(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ if (!areDevicesReady())
+ {
+ return {};
+ }
+ SensorDeviceDescriptionSeq r;
+ r.reserve(getNumberOfSensorDevices());
+ for (auto idx : getIndices(sensorDevices.values()))
+ {
+ r.emplace_back(getSensorDeviceDescription(idx));
+ }
+ return r;
+ }
+
+ SensorDeviceStatus armarx::RobotUnit::RobotUnit::getSensorDeviceStatus(std::size_t idx) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ const SensorDevicePtr& sensorDevice = sensorDevices.at(idx);
+ SensorDeviceStatus status;
+ status.deviceName = sensorDevice->getDeviceName();
+ status.sensorValue = getSensorAndControlBuffer().sensors.at(idx)->toVariants(getSensorAndControlBuffer().sensorValuesTimestamp);
+ status.timestampUSec = MicroNow().count();
+ return status;
+ }
+
+ SensorDeviceStatus RobotUnit::getSensorDeviceStatus(const std::string& name, const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ if (!sensorDevices.has(name))
+ {
+ std::stringstream ss;
+ ss << "RobotUnit::getSensorDeviceStatus: There is no SensorDevice '" << name
+ << "'. There are these SensorDevices: " << sensorDevices.keys();
+ throw InvalidArgumentException {ss.str()};
+ }
+ return getSensorDeviceStatus(sensorDevices.index(name));
+ }
+
+ SensorDeviceStatusSeq RobotUnit::getSensorDeviceStatuses(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ if (!areDevicesReady())
+ {
+ return {};
+ }
+ SensorDeviceStatusSeq r;
+ r.reserve(getNumberOfSensorDevices());
+ for (auto idx : getIndices(sensorDevices.values()))
+ {
+ r.emplace_back(getSensorDeviceStatus(idx));
+ }
+ return r;
+ }
+
+ std::vector<armarx::LVL1ControllerPtr> RobotUnit::getLVL1ControllersNotNull(const std::vector<std::string>& names) const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ std::vector<LVL1ControllerPtr> ctrl;
+ ctrl.reserve(names.size());
+ for (const auto& name : names)
+ {
+ auto it = lvl1Controllers.find(name);
+ if (it == lvl1Controllers.end())
+ {
+ std::stringstream ss;
+ ss << "RobotUnit: there is no LVL1Controller with name '" << name
+ << "'. Existing LVL1Controllers are: " << getLVL1ControllerNames();
+ throw InvalidArgumentException {ss.str()};
+ }
+ if (!it->second)
+ {
+ std::stringstream ss;
+ ss << "RobotUnit: The LVL1Controller with name '" << name
+ << "'. Is a nullptr! This should never be the case (invariant)! \nMap:\n" << lvl1Controllers;
+ ARMARX_FATAL << ss.str();
+ throw InvalidArgumentException {ss.str()};
+ }
+ ctrl.emplace_back(it->second);
+ }
+ return ctrl;
+ }
+ LVL1ControllerPtr RobotUnit::getLVL1ControllerNotNull(const std::string& name) const
+ {
+ return getLVL1ControllersNotNull(std::vector<std::string> {name}).at(0);
+ }
+
+ StringLVL1ControllerPrxDictionary RobotUnit::getAllLVL1Controllers(const Ice::Current&) const
+ {
+ std::map<std::string, LVL1ControllerPtr> lvl1ControllersCopy;
+ {
+ auto guard = getGuard();
+ //copy to keep lock retention time low
+ lvl1ControllersCopy = lvl1Controllers;
+ }
+ StringLVL1ControllerPrxDictionary result;
+ for (const auto& pair : lvl1ControllersCopy)
+ {
+ result[pair.first] = LVL1ControllerInterfacePrx::uncheckedCast(pair.second->getProxy(-1, true));
+ }
+ return result;
+ }
+
+ Ice::ObjectProxySeq RobotUnit::getUnits(const Ice::Current&) const
+ {
+ std::vector<ManagedIceObjectPtr> unitsCopy;
+ {
+ auto guard = getGuard();
+ //copy to keep lock retention time low
+ unitsCopy = units;
+ }
+ Ice::ObjectProxySeq r;
+ r.reserve(unitsCopy.size());
+ for (const ManagedIceObjectPtr& unit : unitsCopy)
+ {
+ r.emplace_back(unit->getProxy(-1, true));
+ }
+ return r;
+ }
+
+ const ManagedIceObjectPtr& RobotUnit::getUnit(const std::string& staticIceId) const
+ {
+ auto guard = getGuard();
+ for (const ManagedIceObjectPtr& unit : units)
+ {
+ if (unit->ice_isA(staticIceId))
+ {
+ return unit;
+ }
+ }
+ return ManagedIceObject::NullPtr;
+ }
+
+ Ice::ObjectPrx RobotUnit::getUnit(const std::string& staticIceId, const Ice::Current&) const
+ {
+ //no lock required
+ ManagedIceObjectPtr unit = getUnit(staticIceId);
+ if (unit)
+ {
+ return unit->getProxy(-1, true);
+ }
+ return nullptr;
+ }
+
+ KinematicUnitInterfacePrx RobotUnit::getKinematicUnit(const Ice::Current&) const
+ {
+ //no lock required
+ return KinematicUnitInterfacePrx::uncheckedCast(getUnit(KinematicUnitInterface::ice_staticId(), GlobalIceCurrent));
+ }
+
+ ForceTorqueUnitInterfacePrx RobotUnit::getForceTorqueUnit(const Ice::Current&) const
+ {
+ //no lock required
+ return ForceTorqueUnitInterfacePrx::uncheckedCast(getUnit(ForceTorqueUnitInterface::ice_staticId(), GlobalIceCurrent));
+ }
+
+ InertialMeasurementUnitInterfacePrx RobotUnit::getInertialMeasurementUnit(const Ice::Current&) const
+ {
+ //no lock required
+ return InertialMeasurementUnitInterfacePrx::uncheckedCast(getUnit(InertialMeasurementUnitInterface::ice_staticId(), GlobalIceCurrent));
+ }
+
+ PlatformUnitInterfacePrx RobotUnit::getPlatformUnit(const Ice::Current&) const
+ {
+ //no lock required
+ return PlatformUnitInterfacePrx::uncheckedCast(getUnit(PlatformUnitInterface::ice_staticId(), GlobalIceCurrent));
+ }
+
+ const VirtualRobot::RobotPtr& RobotUnit::getRobot() const
+ {
+ auto guard = getGuard();
+ throwIfStateIsNot({RobotUnitState::InitializingDevices, RobotUnitState::InitializingUnits, RobotUnitState::WaitingForRTThreadInitialization, RobotUnitState::Running}, __FUNCTION__);
+ ARMARX_CHECK_EXPRESSION(robot);
+ return robot;
+ }
+
+ std::string RobotUnit::getRobotName() const
+ {
+ auto guard = getGuard();
+ throwIfStateIsNot({RobotUnitState::InitializingDevices, RobotUnitState::InitializingUnits, RobotUnitState::WaitingForRTThreadInitialization, RobotUnitState::Running}, __FUNCTION__);
+ return getRobot()->getName();
+ }
+
+ std::size_t RobotUnit::getNumberOfControlDevices() const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controlDevices.size();
+ }
+
+ std::size_t RobotUnit::getNumberOfSensorDevices() const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return sensorDevices.size();
+ }
+
+ const DebugDrawerInterfacePrx& RobotUnit::getDebugDrawerProxy() const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return debugDrawerPrx;
+ }
+
+ std::string RobotUnit::getListenerTopicName(const Ice::Current&) const
+ {
+ auto guard = getGuard();
+ return robotUnitListenerTopicName;
+ }
+
+ const RobotUnitListenerPrx& RobotUnit::getListenerProxy() const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return listenerPrx;
+ }
+
+ PropertyDefinitionsPtr RobotUnit::createPropertyDefinitions()
+ {
+ return PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(getConfigIdentifier()));
+ }
+
+ void RobotUnit::icePropertiesUpdated(const std::set<std::string>& changedProperties)
+ {
+
+ if (changedProperties.count("DebugObserverPublishSensorValues"))
+ {
+ debugObserverPublishSensorValues = getProperty<bool>("DebugObserverPublishSensorValues").getValue();
+ }
+ if (changedProperties.count("DebugObserverPublishControlTargets"))
+ {
+ debugObserverPublishControlTargets = getProperty<bool>("DebugObserverPublishControlTargets").getValue();
+ }
+ if (changedProperties.count("DebugObserverPrintEveryNthIterations"))
+ {
+ debugObserverSkipIterations = getProperty<std::uint64_t>("DebugObserverPrintEveryNthIterations").getValue();
+ }
+ }
+
+ void RobotUnit::onInitComponent()
+ {
+ {
+ auto guard = getGuard();
+ throwIfStateIsNot(RobotUnitState::PreComponentInitialization, __FUNCTION__);
+ robotUnitListenerTopicName = getProperty<std::string>("RobotUnitListenerTopicName").getValue();
+ offeringTopic(getListenerTopicName());
+ offeringTopic(getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue());
+
+ debugObserverPublishSensorValues = getProperty<bool>("DebugObserverPublishSensorValues").getValue();
+ debugObserverPublishControlTargets = getProperty<bool>("DebugObserverPublishControlTargets").getValue();
+
+ //load robot
+ {
+ robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
+ robotProjectName = getProperty<std::string>("RobotFileNameProject").getValue();
+ robotFileName = getProperty<std::string>("RobotFileName").getValue();
+ robotPlatformName = getProperty<std::string>("PlatformName").getValue();
+ debugObserverSkipIterations = getProperty<std::uint64_t>("DebugObserverPrintEveryNthIterations").getValue();
+ if (debugObserverSkipIterations < 1)
+ {
+ debugObserverSkipIterations = 1;
+ }
+ Ice::StringSeq includePaths;
+ if (!robotProjectName.empty())
+ {
+ CMakePackageFinder finder(robotProjectName);
+ Ice::StringSeq projectIncludePaths;
+ auto pathsString = finder.getDataDir();
+ boost::split(projectIncludePaths,
+ pathsString,
+ boost::is_any_of(";,"),
+ boost::token_compress_on);
+ includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
+ }
+ if (!ArmarXDataPath::getAbsolutePath(robotFileName, robotFileName, includePaths))
+ {
+ throw UserException("Could not find robot file " + robotFileName);
+ }
+ // read the robot
+ try
+ {
+ robot = VirtualRobot::RobotIO::loadRobot(robotFileName, VirtualRobot::RobotIO::eFull);
+ }
+ catch (VirtualRobot::VirtualRobotException& e)
+ {
+ throw UserException(e.what());
+ }
+ ARMARX_INFO << "Loaded robot:"
+ << "\n\tProject : " << robotProjectName
+ << "\n\tRobot file : " << robotFileName
+ << "\n\tRobotNodeSet : " << robotNodeSetName
+ << "\n\tNodeNames : " << robot->getRobotNodeSet(robotNodeSetName)->getNodeNames();
+ }
+ ARMARX_CHECK_EXPRESSION(robot);
+ state = RobotUnitState::InitializingDevices;
+ }
+ onInitRobotUnit();
+ }
+
+ void RobotUnit::onConnectComponent()
+ {
+ listenerPrx = getTopic<RobotUnitListenerPrx>(getListenerTopicName());
+ debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerUpdatesTopicName").getValue());
+ debugObserverPrx = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverTopicName").getValue());
+ onConnectRobotUnit();
+ }
+
+ void RobotUnit::onDisconnectComponent()
+ {
+ onDisconnectRobotUnit();
+ }
+
+ void RobotUnit::onExitComponent()
+ {
+ onExitRobotUnit();
+ finishRunning();
+ }
+
+ bool RobotUnit::rtRequestedControllersChanged() const
+ {
+ return controllersRequested.updateReadBuffer();
+ }
+
+ const LVL0AndLVL1Controllers& RobotUnit::rtGetRequestedControllers() const
+ {
+ return controllersRequested.getReadBuffer();
+ }
+
+ const std::vector<armarx::LVL0Controller*>& RobotUnit::rtGetRequestedLVL0Controllers() const
+ {
+ return rtGetRequestedControllers().lvl0Controllers;
+ }
+
+ const std::vector<armarx::LVL1Controller*>& RobotUnit::rtGetRequestedLVL1Controllers() const
+ {
+ return rtGetRequestedControllers().lvl1Controllers;
+ }
+
+ void RobotUnit::rtWriteActivatedControllers()
+ {
+ controllersActivated.commitWrite();
+ }
+
+ LVL0AndLVL1Controllers& RobotUnit::rtGetActivatedControllers()
+ {
+ return controllersActivated.getWriteBuffer();
+ }
+
+ std::vector<armarx::LVL0Controller*>& RobotUnit::rtGetActivatedLVL0Controllers()
+ {
+ return rtGetActivatedControllers().lvl0Controllers;
+ }
+
+ std::vector<armarx::LVL1Controller*>& RobotUnit::rtGetActivatedLVL1Controllers()
+ {
+ return rtGetActivatedControllers().lvl1Controllers;
+ }
+
+ void RobotUnit::rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
+ {
+ rtThreadTimingsSensorDevice->rtMarkRtUpdateSensorAndControlBufferStart();
+ SensorAndControl& sc = sensorAndControl.getWriteBuffer();
+ sc.sensorValuesTimestamp = sensorValuesTimestamp;
+ sc.timeSinceLastIteration = timeSinceLastIteration;
+ for (std::size_t sensIdx = 0; sensIdx < rtGetNumberOfSensorDevices(); ++sensIdx)
+ {
+ sensorDevices.at(sensIdx)->getSensorValue()->copyTo(sc.sensors.at(sensIdx));
+ }
+ for (std::size_t ctrlIdx = 0; ctrlIdx < controlDevices.size(); ++ctrlIdx)
+ {
+ ControlDevice& controlDevice = *controlDevices.at(ctrlIdx);
+ auto& tv = sc.control.at(ctrlIdx);
+ for (std::size_t targIdx = 0; targIdx < controlDevice.rtGetLVL0Controllers().size(); ++targIdx)
+ {
+ LVL0Controller& lvl0 = *controlDevice.rtGetLVL0Controllers().at(targIdx);
+ lvl0.getControlTarget()->copyTo(tv.at(targIdx));
+ }
+ }
+ sensorAndControl.commitWrite();
+ rtThreadTimingsSensorDevice->rtMarkRtUpdateSensorAndControlBufferEnd();
+ }
+
+ void RobotUnit::writeRequestedControllers(LVL0AndLVL1Controllers &&setOfControllers)
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ //check lvl1
+ const auto& lvl1s = setOfControllers.lvl1Controllers;
+ std::set<LVL1ControllerPtr> lvl1set {lvl1s.begin(), lvl1s.end()};
+ lvl1set.erase(nullptr);
+ //# lvl1
+ const std::size_t nlvl1s = std::count_if(lvl1s.begin(), lvl1s.end(), [](const LVL1ControllerPtr & p)
+ {
+ return p;
+ });
+
+ ARMARX_CHECK_EXPRESSION(nlvl1s == lvl1set.size());
+ ARMARX_CHECK_EXPRESSION(lvl1s.size() == getControllerBufferSize());
+ //first nlvl1s not null
+ ARMARX_CHECK_EXPRESSION(std::all_of(lvl1s.begin(), lvl1s.begin() + nlvl1s, [](LVL1Controller * p)
+ {
+ return p;
+ }));
+ //last getNumberOfControlDevices()-nlvl1s null
+ ARMARX_CHECK_EXPRESSION(std::all_of(lvl1s.begin() + nlvl1s, lvl1s.end(), [](LVL1Controller * p)
+ {
+ return !p;
+ }));
+ //conflict free and sorted
+ ARMARX_CHECK_EXPRESSION(std::is_sorted(lvl1s.begin(), lvl1s.end(), LVL1Controller::IdComp {}));
+ ARMARX_CHECK_EXPRESSION(LVL1Controller::AreNotInConflict(lvl1s.begin(), lvl1s.begin() + nlvl1s));
+
+ //check lvl0
+ const auto& lvl0s = setOfControllers.lvl0Controllers;
+ ARMARX_CHECK_EXPRESSION(lvl0s.size() == getControllerBufferSize());
+ ARMARX_CHECK_EXPRESSION(std::all_of(lvl0s.begin(), lvl0s.end(), [](LVL0Controller * p)
+ {
+ return p;
+ }));
+
+ //set requested state
+ for (const auto& name2lvl1 : lvl1Controllers)
+ {
+ const LVL1ControllerPtr& lvl1 = name2lvl1.second;
+ SetLVL1ControllerRequested(lvl1, lvl1set.count(lvl1));
+ }
+
+ controllersRequested.getWriteBuffer() = std::move(setOfControllers);
+ controllersRequested.commitWrite();
+ }
+
+ const LVL0AndLVL1Controllers& RobotUnit::getRequestedControllers() const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controllersRequested.getWriteBuffer();
+ }
+
+ const std::vector<armarx::LVL0Controller*>& RobotUnit::getRequestedLVL0Controllers() const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return getRequestedControllers().lvl0Controllers;
+ }
+
+ const std::vector<armarx::LVL1Controller*>& RobotUnit::getRequestedLVL1Controllers() const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return getRequestedControllers().lvl1Controllers;
+ }
+
+ bool RobotUnit::activatedControllersChanged() const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controllersActivated.updateReadBuffer();
+ }
+
+ const LVL0AndLVL1Controllers& RobotUnit::getActivatedControllers() const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return controllersActivated.getReadBuffer();
+ }
+
+ const std::vector<armarx::LVL0Controller*>& RobotUnit::getActivatedLVL0Controllers() const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return getActivatedControllers().lvl0Controllers;
+ }
+
+ const std::vector<armarx::LVL1Controller*>& RobotUnit::getActivatedLVL1Controllers() const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return getActivatedControllers().lvl1Controllers;
+ }
+
+ bool RobotUnit::sensorAndControlBufferChanged() const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return sensorAndControl.updateReadBuffer();
+ }
+
+ const SensorAndControl& RobotUnit::getSensorAndControlBuffer() const
+ {
+ auto guard = getGuard();
+ throwIfDevicesNotReady(__FUNCTION__);
+ return sensorAndControl.getReadBuffer();
+ }
+
+ std::ostream& operator<<(std::ostream& o, RobotUnitState s)
+ {
+ o << std::to_string(s);
+ return o;
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnit.h b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..c545dea662726e58738dfe64cdebd8cab40e6ca3
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnit.h
@@ -0,0 +1,665 @@
+/*
+ * 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::RobotUnit
+ * @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_UNIT_RobotAPI_RobotUnit_H
+#define _ARMARX_UNIT_RobotAPI_RobotUnit_H
+
+#include <thread>
+#include <atomic>
+#include <chrono>
+#include <mutex>
+#include <unordered_map>
+#include <memory>
+
+#include <ArmarXCore/core/util/OnScopeExit.h>
+#include <ArmarXCore/core/Component.h>
+#include <ArmarXCore/core/ArmarXManager.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/util/algorithm.h>
+#include <ArmarXCore/core/util/TripleBuffer.h>
+#include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <ArmarXCore/observers/DebugObserver.h>
+#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+
+#include "LVL0Controllers/LVL0Controller.h"
+#include "LVL1Controllers/LVL1Controller.h"
+#include "Devices/ControlDevice.h"
+#include "Devices/SensorDevice.h"
+#include "Devices/RTThreadTimingsSensorDevice.h"
+#include "util.h"
+#include "structs.h"
+
+namespace armarx
+{
+ TYPEDEF_PTRS_HANDLE(RobotSubUnitUpdateInterface);
+ class RobotSubUnitUpdateInterface: virtual public ManagedIceObject {};
+
+ TYPEDEF_PTRS_HANDLE(RobotUnit);
+
+ class DynamicLibrary;
+ typedef boost::shared_ptr<DynamicLibrary> DynamicLibraryPtr;
+ /**
+ * @class RobotUnitPropertyDefinitions
+ * @brief
+ */
+ class RobotUnitPropertyDefinitions:
+ public armarx::ComponentPropertyDefinitions
+ {
+ public:
+ RobotUnitPropertyDefinitions(std::string prefix):
+ armarx::ComponentPropertyDefinitions(prefix)
+ {
+ defineOptionalProperty<std::string>("RobotUnitListenerTopicName", "RobotUnitListenerTopic", "The topic receiving events for RobotUnitListener");
+ defineOptionalProperty<std::string>("DebugDrawerUpdatesTopicName", "DebugDrawerUpdates", "The topic receiving events for debug drawing");
+ defineOptionalProperty<std::size_t>("PublishPeriodMs", 10, "Milliseconds between each publish");
+
+ defineRequiredProperty<std::string>("RobotFileName", "Robot file name, e.g. robot_model.xml");
+ defineOptionalProperty<std::string>("RobotFileNameProject", "", "Project in which the robot filename is located (if robot is loaded from an external project)");
+
+ defineOptionalProperty<std::string>("KinematicUnitName", "KinematicUnit", "The name of the created kinematic unit");
+ defineOptionalProperty<std::string>("RobotNodeSetName", "Robot", "Robot node set name as defined in robot xml file, e.g. 'LeftArm'");
+
+ defineOptionalProperty<std::string>("PlatformUnitName", "PlatformUnit", "The name of the created platform unit");
+ defineOptionalProperty<std::string>("PlatformName", "Platform", "Name of the platform (will publish values on PlatformName + 'State')");
+
+ defineOptionalProperty<std::string>("ForceTorqueUnitName", "ForceTorqueUnit", "The name of the created force troque unit (empty = default)");
+ defineOptionalProperty<std::string>("ForceTorqueTopicName", "ForceTorqueValues", "Name of the topic on which the force torque sensor values are provided");
+
+ defineOptionalProperty<std::string>("InertialMeasurementUnitName", "InertialMeasurementUnit", "The name of the created inertial measurement unit (empty = default)");
+ defineOptionalProperty<std::string>("IMUTopicName", "IMUValues", "Name of the IMU Topic.");
+
+ defineOptionalProperty<bool>("DebugObserverPublishSensorValues", true, "Whether sensor values are send to the debug observer topic", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<bool>("DebugObserverPublishControlTargets", true, "Whether control targets are send to the debug observer topic", PropertyDefinitionBase::eModifiable);
+ defineOptionalProperty<std::string>("DebugObserverTopicName", "DebugObserver", "The topic where updates are send to");
+ defineOptionalProperty<std::uint64_t>("DebugObserverPrintEveryNthIterations", 1, "Only every nth iteration data is send to the debug observer", PropertyDefinitionBase::eModifiable);
+ }
+ };
+
+ /// @brief The current state of the multi step initialization of a RobotUnit
+ enum class RobotUnitState : std::size_t
+ {
+ PreComponentInitialization,
+ InitializingDevices,
+ //ok to use devices/create controllers
+ InitializingUnits,
+ WaitingForRTThreadInitialization,
+ Running,
+ Exiting
+ };
+}
+namespace std
+{
+ std::string to_string(armarx::RobotUnitState s);
+}
+
+namespace armarx
+{
+ std::ostream& operator<<(std::ostream& o, RobotUnitState s);
+
+ /**
+ * @brief This class is a friend of LVL1Controller.
+ *
+ * It encapsulates all access to privates of a LVL1Controller
+ * to reduce the amount of code which could break invariants
+ * in LVL1Controller and to collect all such code in one place
+ * to make this problem more manageable.
+ *
+ * Also the private functions of LVL1Controller should not be made public to prevent
+ * other classes (e.g. KinematicSubUnit) or derived LVL1Controllers
+ * from accessing them
+ *
+ */
+ class RobotUnitLVL1ControllerAccess
+ {
+ protected:
+ static void RtActivateLVL1Controller(const LVL1ControllerPtr& lvl1)
+ {
+ lvl1->rtActivateController();
+ }
+ static void RtDeactivateLVL1Controller(const LVL1ControllerPtr& lvl1)
+ {
+ lvl1->rtDeactivateController();
+ }
+ static void RtDeactivateLVL1ControllerBecauseOfError(const LVL1ControllerPtr& lvl1)
+ {
+ lvl1->rtDeactivateControllerBecauseOfError();
+ }
+
+ static void RtSetLVL1ControllerActive(const LVL1ControllerPtr& lvl1, bool activate, bool deactivateBecauseOfError = false)
+ {
+ if (activate)
+ {
+ RtActivateLVL1Controller(lvl1);
+ }
+ else
+ {
+ if (deactivateBecauseOfError)
+ {
+ RtDeactivateLVL1ControllerBecauseOfError(lvl1);
+ }
+ else
+ {
+ RtDeactivateLVL1Controller(lvl1);
+ }
+ }
+ }
+ static void InitLVL1Controler(
+ LVL1ControllerPtr lvl1,
+ RobotUnit* ru,
+ std::size_t ctrlId,
+ StringStringDictionary ctrlDeviceControlModeMap,
+ std::vector<char> ctrlDeviceUsedBitmap,
+ std::vector<std::size_t> ctrlDeviceUsedIndices
+ )
+ {
+ lvl1->robotUnitInit(ru, ctrlId, std::move(ctrlDeviceControlModeMap), std::move(ctrlDeviceUsedBitmap), std::move(ctrlDeviceUsedIndices));
+ }
+ static void PublishLVL1Controller(
+ LVL1ControllerPtr lvl1,
+ const SensorAndControl& sac,
+ const DebugDrawerInterfacePrx& draw,
+ const DebugObserverInterfacePrx& observer
+ )
+ {
+ lvl1 ->publish(sac, draw, observer);
+ }
+ bool GetLVL1ControllerStatusReportedActive(LVL1ControllerPtr lvl1)
+ {
+ return lvl1->statusReportedActive;
+ }
+ bool GetLVL1ControllerStatusReportedRequested(LVL1ControllerPtr lvl1)
+ {
+ return lvl1->statusReportedRequested;
+ }
+ void UpdateLVL1ControllerStatusReported(LVL1ControllerPtr lvl1)
+ {
+ lvl1->statusReportedActive = lvl1->isActive;
+ lvl1->statusReportedRequested = lvl1->isRequested;
+ }
+ void SetLVL1ControllerRequested(LVL1ControllerPtr lvl1, bool requested)
+ {
+ lvl1->isRequested = requested;
+ }
+ };
+
+ class RobotUnit :
+ virtual public Component,
+ virtual public RobotUnitInterface,
+ virtual public LVL1ControllerDescriptionProviderInterface,
+ virtual public RobotUnitLVL1ControllerAccess
+ {
+ protected:
+ using PublisherTaskT = SimplePeriodicTask<std::function<void(void)>>;
+ public:
+ using MutexType = std::recursive_mutex;
+ using GuardType = std::unique_lock<MutexType>;
+ //using GuardType = messaging_unique_lock<MutexType>;
+ using ClockT = std::chrono::high_resolution_clock;
+ using TimePointT = std::chrono::high_resolution_clock::time_point;
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // data
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ //devices
+ private:
+ /// data may only be added during and oly be used after State::InitializingDevices
+ KeyValueVector<std::string, ControlDevicePtr> controlDevices;
+ /// data may only be added during and oly be used after State::InitializingDevices
+ KeyValueVector<std::string, SensorDevicePtr > sensorDevices;
+
+ std::map<std::string, ConstControlDevicePtr> controlDevicesConstPtr;
+ std::map<std::string, ConstSensorDevicePtr> sensorDevicesConstPtr;
+
+ RTThreadTimingsSensorDevicePtr rtThreadTimingsSensorDevice;
+ static const std::string rtThreadTimingsSensorDeviceName;
+ //triple buffers
+ private:
+ /// @brief Controllers the RT thread is supposed to activate (direction nonRT -> RT) (some lvl1 controllers may be null)
+ /// data may only be used after State::InitializingDevices
+ /// all LVL1Controller have to be sorted by id (null controllers are at the end)
+ WriteBufferedTripleBuffer<LVL0AndLVL1Controllers> controllersRequested;
+ /// @brief Controllers the RT thread is currently running (direction RT -> nonRT) (some lvl1 controllers may be null)
+ /// data may only be used after State::InitializingDevices
+ /// all LVL1Controller are sorted by id (null controllers may be anywhere)
+ WriteBufferedTripleBuffer<LVL0AndLVL1Controllers> controllersActivated;
+ /// @brief Transfers the current sensor values and control targets (direction RT -> nonRT)
+ /// data may only be used after State::InitializingDevices
+ TripleBuffer<SensorAndControl> sensorAndControl;
+
+ //(instantiating) lvl1 controllers
+ private:
+ /// data may only be used after State::InitializingDevices
+ std::thread::id initLVL1ThreadId;
+ /// data may only be used after State::InitializingDevices
+ std::map<std::string, std::string> initLVL1UsedControlTargets;
+ /// @brief Holds all currently loaded LVL1 controllers (index: [instancename])(May not be accessed in rt.)
+ std::map<std::string, LVL1ControllerPtr> lvl1Controllers;
+ std::size_t lvl1ControllerNextId {0};
+
+ //listeners
+ private:
+ std::string robotUnitListenerTopicName;
+ RobotUnitListenerPrx listenerPrx;
+ DebugDrawerInterfacePrx debugDrawerPrx;
+
+ //publishing
+ private:
+ std::uint64_t publishIterationCount {0};
+
+ IceUtil::Time publishNewSensorDataTime;
+ PublisherTaskT::pointer_type publisherTask;
+ std::set<std::string> lastReportedClasses;
+
+ std::atomic_bool debugObserverPublishSensorValues;
+ std::atomic_bool debugObserverPublishControlTargets;
+ std::atomic<std::uint64_t> debugObserverSkipIterations;
+
+ std::chrono::microseconds lastDebugObserverFlush;
+ std::chrono::microseconds lastPublishLoop;
+ DebugObserverInterfacePrx debugObserverPrx;
+
+
+ //other
+ private:
+ /// @brief The RobotUnit's state
+ std::atomic<RobotUnitState> state {RobotUnitState::PreComponentInitialization};
+ std::map<std::string, DynamicLibraryPtr> loadedLibs;
+ std::vector<ManagedIceObjectPtr> units;
+ std::thread::id rtThreadId;
+ mutable MutexType dataMutex;
+ static const std::set<RobotUnitState> devicesReadyStates;
+
+ //robot
+ private:
+ boost::shared_ptr<VirtualRobot::Robot> robot;
+ std::string robotNodeSetName;
+ std::string robotProjectName;
+ std::string robotFileName;
+ std::string robotPlatformName;
+
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // util
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ RobotUnitState getRobotUnitState() const
+ {
+ return state;
+ }
+ virtual std::string getName() const override
+ {
+ return Component::getName();
+ }
+ protected:
+ template<class ExceptT = LogicError>
+ inline void throwIfStateIsNot(const std::set<RobotUnitState>& sinit, const std::string& fnc) const;
+ template<class ExceptT = LogicError>
+ inline void throwIfStateIsNot(RobotUnitState s, const std::string& fnc) const;
+ template<class ExceptT = LogicError>
+ inline void throwIfDevicesNotReady(const std::string& fnc) const;
+
+ template<class Cont>
+ static Ice::StringSeq GetNonNullNames(const Cont& c);
+
+ template<class PtrT>
+ void checkRefCountIsOne(const PtrT& p, const std::string& name) const;
+ GuardType getGuard() const;
+ inline const KeyValueVector<std::string, SensorDevicePtr>& getSensorDevices() const;
+ inline const KeyValueVector<std::string, ControlDevicePtr>& getControlDevices() const;
+
+ void checkLVL1ControllerClassName(const std::string& className) const;
+
+ //time
+ static TimePointT Now()
+ {
+ return ClockT::now();
+ }
+ static std::chrono::microseconds MicroToNow(TimePointT last)
+ {
+ return std::chrono::duration_cast<std::chrono::microseconds>(Now() - last);
+ }
+ static std::chrono::microseconds MicroNow();
+
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // other
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ /// @see PropertyUser::createPropertyDefinitions()
+ virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+ virtual std::string getDefaultName() const override
+ {
+ return "RobotUnit";
+ }
+ // PropertyUser interface
+ public:
+ virtual void icePropertiesUpdated(const std::set<std::string>& changedProperties) override;
+ protected:
+ virtual void onInitRobotUnit() {}
+ virtual void onConnectRobotUnit() {}
+ virtual void onDisconnectRobotUnit() {}
+ /// @brief called in RobotUnit::onExitComponent() before calling RobotUnit::finishRunning
+ virtual void onExitRobotUnit() {}
+ virtual void onInitComponent() final;
+ virtual void onConnectComponent() final;
+ virtual void onDisconnectComponent() final;
+ virtual void onExitComponent() final;
+
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // TripleBuffer
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ protected:
+ //rt requested
+ bool rtRequestedControllersChanged() const;
+ const LVL0AndLVL1Controllers& rtGetRequestedControllers() const;
+ const std::vector<LVL0Controller*>& rtGetRequestedLVL0Controllers() const;
+ const std::vector<LVL1Controller*>& rtGetRequestedLVL1Controllers() const;
+ // rt activated
+ void rtWriteActivatedControllers();
+ LVL0AndLVL1Controllers& rtGetActivatedControllers();
+ std::vector<LVL0Controller*>& rtGetActivatedLVL0Controllers();
+ std::vector<LVL1Controller*>& rtGetActivatedLVL1Controllers();
+ // rt sensor and control
+ //declaration further down
+ //void rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+
+ // non rt requested
+ void writeRequestedControllers(LVL0AndLVL1Controllers&& setOfControllers);
+ const LVL0AndLVL1Controllers& getRequestedControllers() const;
+ const std::vector<LVL0Controller*>& getRequestedLVL0Controllers() const;
+ const std::vector<LVL1Controller*>& getRequestedLVL1Controllers() const;
+ // non rt activated
+ bool activatedControllersChanged() const;
+ const LVL0AndLVL1Controllers& getActivatedControllers() const;
+ const std::vector<LVL0Controller*>& getActivatedLVL0Controllers() const;
+ const std::vector<LVL1Controller*>& getActivatedLVL1Controllers() const;
+ // non rt sensor and control
+ bool sensorAndControlBufferChanged() const;
+ const SensorAndControl& getSensorAndControlBuffer() const;
+
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // State: InitializingDevices
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ protected:
+ void addControlDevice(const ControlDevicePtr& cd);
+ void addSensorDevice(const SensorDevicePtr& sd);
+
+ virtual RTThreadTimingsSensorDevicePtr createRTThreadTimingSensorDevice() const;
+
+ void finishDeviceInitialization();
+
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // State: InitializingUnits
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ protected:
+ virtual void initializeDefaultUnits();
+ virtual void initializeKinematicUnit();
+ virtual void initializePlatformUnit();
+ virtual void initializeForceTorqueUnit();
+ virtual void initializeInertialMeasurementUnit();
+ void addUnit(ManagedIceObjectPtr unit);
+ void finishUnitInitialization();
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // State: Running (RT)
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ virtual void initRTThread();
+
+ //switching helper (rt)
+ //is in the base class RobotUnitLVL1ControllerAccess that is a friend of LVL1Controller
+ //static void RtSetLVL1ControllerActive(const LVL1ControllerPtr& lvl1, bool activate = 1, bool deactivateBecauseOfError = false);
+ void rtDeactivateAssignedLVL1Controller(std::size_t ctrlDevIndex, bool writeActivatedControlers = true);
+
+ //run (these functions should be called in the control loop)
+ bool rtSwitchControllerSetup();
+ void rtRunLVL1Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtHandleInvalidTargets();
+ /**
+ * @brief Runs LVL0 controllers and writes target values to ControlDevices
+ * @param sensorValuesTimestamp
+ * @param timeSinceLastIteration
+ */
+ void rtRunLVL0Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtReadSensorDeviceValues(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ //rtBusSendReceive call this robot specific function here
+ void rtUpdateSensorAndControlBuffer(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
+ void rtResetAllTargets();
+
+ RTThreadTimingsSensorDevice& rtGetRTThreadTimingsSensorDevice()
+ {
+ return *rtThreadTimingsSensorDevice;
+ }
+
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // State: InitializingUnits + Running (NON RT)
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ //create lvl1 controllers
+ virtual ConstSensorDevicePtr getSensorDevice(const std::string& sensorDeviceName) const;
+ virtual ConstControlDevicePtr getControlDevice(const std::string& deviceName);
+ virtual ControlTargetBase* getControlTarget(const std::string& deviceName, const std::string& controlMode) override;
+ virtual LVL1ControllerInterfacePrx createController(const std::string& className, const std::string& instanceName, const LVL1ControllerConfigPtr& config, const Ice::Current&) override;
+ virtual LVL1ControllerInterfacePrx createControllerFromVariantConfig(const std::string& className, const std::string& instanceName, const StringVariantBaseMap& variants, const Ice::Current&) override;
+ LVL1ControllerPtr createController(const std::string& className, const std::string& instanceName, const LVL1ControllerConfigPtr& config);
+
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // State: Running (NON RT)
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ //switching
+ virtual void activateController(const std::string& name, const Ice::Current& = GlobalIceCurrent) override;
+ virtual void activateControllers(const Ice::StringSeq& names, const Ice::Current& = GlobalIceCurrent) override;
+ virtual void deactivateController(const std::string& name, const Ice::Current& = GlobalIceCurrent) override;
+ virtual void deactivateControllers(const Ice::StringSeq& names, const Ice::Current& = GlobalIceCurrent) override;
+ virtual void switchControllerSetup(const Ice::StringSeq& newSetup, const Ice::Current& = GlobalIceCurrent) override;
+ protected:
+ void setActivateControllersRequest(std::set<LVL1ControllerPtr, LVL1Controller::IdComp>&& ctrls);
+ // publishing
+ protected:
+ virtual void publish();
+
+ //shutting down
+ protected:
+ ///@brief joint your thread in this hook. (used by RobotUnit::finishRunning())
+ virtual void joinRTThread() = 0;
+ ///@brief delete all units (used by RobotUnit::finishRunning())
+ virtual void removeAllUnits();
+ private:
+ void finishRunning();
+
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // Lib loading
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ virtual bool loadLibFromPath(const std::string& path, const Ice::Current& = GlobalIceCurrent) override;
+ virtual bool loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current& = GlobalIceCurrent) override;
+ private:
+ bool loadLibFromAbsolutePath(const std::string& path);
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ // querry data
+ // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
+ public:
+ //lvl1
+ virtual Ice::StringSeq getLVL1ControllerNames(const Ice::Current& = GlobalIceCurrent) const override;
+ virtual Ice::StringSeq getRequestedLVL1ControllerNames(const Ice::Current& = GlobalIceCurrent) const override;
+ virtual Ice::StringSeq getActivatedLVL1ControllerNames(const Ice::Current& = GlobalIceCurrent) const override;
+
+ virtual LVL1ControllerInterfacePrx getLVL1Controller(const std::string& name, const Ice::Current&) const override;
+ virtual StringLVL1ControllerPrxDictionary getAllLVL1Controllers(const Ice::Current&) const override;
+ std::vector<LVL1ControllerPtr> getLVL1ControllersNotNull(const std::vector<std::string>& names) const;
+ LVL1ControllerPtr getLVL1ControllerNotNull(const std::string& name) const;
+
+ virtual LVL1ControllerStatus getLVL1ControllerStatus(const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
+ virtual LVL1ControllerStatusSeq getLVL1ControllerStatuses(const Ice::Current&) const override;
+
+ virtual LVL1ControllerDescription getLVL1ControllerDescription(const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
+ virtual LVL1ControllerDescriptionSeq getLVL1ControllerDescriptions(const Ice::Current&) const override;
+
+ virtual LVL1ControllerDescriptionWithStatus getLVL1ControllerDescriptionWithStatus(const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
+ virtual LVL1ControllerDescriptionWithStatusSeq getLVL1ControllerDescriptionsWithStatuses(const Ice::Current&) const override;
+ //control devices
+ virtual Ice::StringSeq getControlDeviceNames(const Ice::Current&) const override;
+ ControlDeviceDescription getControlDeviceDescription(std::size_t cdidx) const;
+ virtual ControlDeviceDescription getControlDeviceDescription(const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
+ virtual ControlDeviceDescriptionSeq getControlDeviceDescriptions(const Ice::Current&) const override;
+ ControlDeviceStatus getControlDeviceStatus(std::size_t cdidx) const;
+ virtual ControlDeviceStatus getControlDeviceStatus(const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
+ virtual ControlDeviceStatusSeq getControlDeviceStatuses(const Ice::Current&) const override;
+
+ //sensor devices
+ virtual Ice::StringSeq getSensorDeviceNames(const Ice::Current&) const override;
+ SensorDeviceDescription getSensorDeviceDescription(std::size_t cdidx) const;
+ virtual SensorDeviceDescription getSensorDeviceDescription(const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
+ virtual SensorDeviceDescriptionSeq getSensorDeviceDescriptions(const Ice::Current&) const override;
+ SensorDeviceStatus getSensorDeviceStatus(std::size_t idx) const;
+ virtual SensorDeviceStatus getSensorDeviceStatus(const std::string& name, const Ice::Current& = GlobalIceCurrent) const override;
+ virtual SensorDeviceStatusSeq getSensorDeviceStatuses(const Ice::Current&) const override;
+
+ //lvl1controller classes
+ virtual Ice::StringSeq getLVL1ControllerClassNames(const Ice::Current& = GlobalIceCurrent) const override;
+ virtual LVL1ControllerClassDescription getLVL1ControllerClassDescription(const std::string& className, const Ice::Current& = GlobalIceCurrent) const override;
+ virtual LVL1ControllerClassDescriptionSeq getLVL1ControllerClassDescriptions(const Ice::Current&) const override;
+
+ //units
+ virtual Ice::ObjectProxySeq getUnits(const Ice::Current& = GlobalIceCurrent) const override;
+ virtual Ice::ObjectPrx getUnit(const std::string& staticIceId, const Ice::Current&) const override;
+ const ManagedIceObjectPtr& getUnit(const std::string& staticIceId) const;
+ virtual KinematicUnitInterfacePrx getKinematicUnit(const Ice::Current& = GlobalIceCurrent) const override;
+ virtual ForceTorqueUnitInterfacePrx getForceTorqueUnit(const Ice::Current& = GlobalIceCurrent) const override;
+ virtual InertialMeasurementUnitInterfacePrx getInertialMeasurementUnit(const Ice::Current& = GlobalIceCurrent) const override;
+ virtual PlatformUnitInterfacePrx getPlatformUnit(const Ice::Current& = GlobalIceCurrent) const override;
+
+ //other
+ const boost::shared_ptr<VirtualRobot::Robot>& getRobot() const;
+ std::string getRobotName() const;
+ const std::string& getRobotNodetSeName() const
+ {
+ return robotNodeSetName;
+ }
+ const std::string& getRobotProjectName() const
+ {
+ return robotProjectName;
+ }
+ const std::string& getRobotFileName() const
+ {
+ return robotFileName;
+ }
+ const std::string& getRobotPlatformName() const
+ {
+ return robotPlatformName;
+ }
+
+
+ std::size_t getNumberOfControlDevices() const;
+ std::size_t getNumberOfSensorDevices() const;
+ std::size_t rtGetControllerBufferSize() const
+ {
+ return controlDevices.size();
+ }
+ std::size_t rtGetNumberOfSensorDevices() const
+ {
+ return sensorDevices.size();
+ }
+ std::size_t getControllerBufferSize() const
+ {
+ return getNumberOfControlDevices();
+ }
+
+ const DebugDrawerInterfacePrx& getDebugDrawerProxy() const;
+
+ virtual std::string getListenerTopicName(const Ice::Current& = GlobalIceCurrent) const override;
+
+ bool areDevicesReady() const
+ {
+ return devicesReadyStates.count(state);
+ }
+ protected:
+ const RobotUnitListenerPrx& getListenerProxy() const;
+ };
+
+ const KeyValueVector<std::string, SensorDevicePtr>& RobotUnit::getSensorDevices() const
+ {
+ return sensorDevices;
+ }
+
+ const KeyValueVector<std::string, ControlDevicePtr>& RobotUnit::getControlDevices() const
+ {
+ return controlDevices;
+ }
+
+ template<class ExceptT>
+ void RobotUnit::throwIfStateIsNot(const std::set<RobotUnitState>& sinit, const std::string& fnc) const
+ {
+ std::set<RobotUnitState> sset {sinit};
+ if (! sset.count(state))
+ {
+ std::stringstream ss;
+ ss << fnc << ": Can't be called if state is not in (";
+ bool fst = true;
+ for (RobotUnitState st : sset)
+ {
+ if (!fst)
+ {
+ ss << ", ";
+ }
+ ss << st;
+ fst = false;
+ }
+ ss << " (current state " << getRobotUnitState() << ")";
+ ARMARX_ERROR << ss.str();
+ throw ExceptT {ss.str()};
+ }
+ }
+ template<class ExceptT>
+ void RobotUnit::throwIfStateIsNot(RobotUnitState s, const std::string& fnc) const
+ {
+ throwIfStateIsNot<ExceptT>(std::set<RobotUnitState> {s}, fnc);
+ }
+ template<class ExceptT>
+ void RobotUnit::throwIfDevicesNotReady(const std::string& fnc) const
+ {
+ throwIfStateIsNot<ExceptT>(devicesReadyStates, fnc);
+ }
+
+ template<class Cont>
+ Ice::StringSeq RobotUnit::GetNonNullNames(const Cont& c)
+ {
+ Ice::StringSeq result;
+ result.reserve(c.size());
+ for (const auto& e : c)
+ {
+ if (e)
+ {
+ result.emplace_back(e->getName());
+ }
+ }
+ return result;
+ }
+
+ template<class PtrT>
+ void RobotUnit::checkRefCountIsOne(const PtrT& p, const std::string& name) const
+ {
+ const auto uc = p->__getRef();
+ if (uc != 1)
+ {
+ ARMARX_WARNING << "Pointer " << name << " should have usecount 1 but instead hase usecount " << uc;
+ }
+ }
+}
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
new file mode 100644
index 0000000000000000000000000000000000000000..487f25e47a7851e9f4f25b2febb446a4842a0fbb
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h
@@ -0,0 +1,129 @@
+/*
+ * 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 ( 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_LIB_RobotAPI_SensorValue1DoFActuator_H
+#define _ARMARX_LIB_RobotAPI_SensorValue1DoFActuator_H
+
+#include "SensorValueBase.h"
+
+#include <ArmarXCore/core/util/algorithm.h>
+
+namespace armarx
+{
+ class SensorValue1DoFActuatorPosition : virtual public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ float position;
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ return {{"position", {new TimedVariant{position, timestamp}}}};
+ }
+ };
+ class SensorValue1DoFActuatorVelocity : virtual public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ float velocity;
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ return {{"velocity", {new TimedVariant{velocity, timestamp}}}};
+ }
+ };
+ class SensorValue1DoFActuatorAcceleration : virtual public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ float acceleration;
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ return {{"acceleration", {new TimedVariant{acceleration, timestamp}}}};
+ }
+ };
+ class SensorValue1DoFActuatorTorque : virtual public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ float torque;
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ return {{"torque", {new TimedVariant{torque, timestamp}}}};
+ }
+ };
+
+
+ class SensorValue1DoFActuator :
+ virtual public SensorValue1DoFActuatorPosition,
+ virtual public SensorValue1DoFActuatorVelocity,
+ virtual public SensorValue1DoFActuatorAcceleration,
+ virtual public SensorValue1DoFActuatorTorque
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ auto map = SensorValue1DoFActuatorPosition::toVariants(timestamp);
+ mergeMaps(map, SensorValue1DoFActuatorVelocity::toVariants(timestamp), MergeMapsMode::OverrideNoValues);
+ mergeMaps(map, SensorValue1DoFActuatorAcceleration::toVariants(timestamp), MergeMapsMode::OverrideNoValues);
+ mergeMaps(map, SensorValue1DoFActuatorTorque::toVariants(timestamp), MergeMapsMode::OverrideNoValues);
+ return map;
+ }
+ };
+
+
+ class SensorValue1DoFActuatorCurrent : virtual public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ float motorCurrent;
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ return {{"motorCurrent", {new TimedVariant{motorCurrent, timestamp}}}};
+ }
+ };
+ class SensorValue1DoFActuatorMotorTemperature : virtual public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ float motorTemperature;
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ return {{"motorTemperature", {new TimedVariant{motorTemperature, timestamp}}}};
+ }
+ };
+
+ class SensorValue1DoFRealActuator :
+ virtual public SensorValue1DoFActuator,
+ virtual public SensorValue1DoFActuatorCurrent,
+ virtual public SensorValue1DoFActuatorMotorTemperature
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ auto map = SensorValue1DoFActuator::toVariants(timestamp);
+ mergeMaps(map, SensorValue1DoFActuatorCurrent::toVariants(timestamp), MergeMapsMode::OverrideNoValues);
+ mergeMaps(map, SensorValue1DoFActuatorMotorTemperature::toVariants(timestamp), MergeMapsMode::OverrideNoValues);
+ return map;
+ }
+ };
+}
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..24b5ecf955b92d5c7ced332bb560776589d20ae2
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
@@ -0,0 +1,105 @@
+/*
+ * 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 ( 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_LIB_RobotAPI_SensorValueBase_H
+#define _ARMARX_LIB_RobotAPI_SensorValueBase_H
+
+#include <typeinfo>
+#include <memory>
+#include <string>
+#include <map>
+
+#include <ArmarXCore/observers/variant/TimedVariant.h>
+
+#include "../util.h"
+
+namespace armarx
+{
+ class SensorValueBase
+ {
+ public:
+ virtual void copyTo(SensorValueBase* target) const = 0;
+ virtual std::unique_ptr<SensorValueBase> clone() const = 0;
+ virtual std::string getSensorValueType(bool withoutNamespaceSpecifier) const = 0;
+
+ /// @brief used to send the data to the DebugObserverTopic and to other Components (e.g. GUI widgets)
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const
+ {
+ return {};
+ }
+
+ template<class T>
+ bool isA() const
+ {
+ return dynamic_cast<const T*>(this);
+ }
+
+ template<class T>
+ const T* asA() const
+ {
+ return dynamic_cast<const T*>(this);
+ }
+
+ template<class T>
+ T* asA()
+ {
+ return dynamic_cast<T*>(this);
+ }
+
+ template<class T, class = typename std::enable_if<std::is_base_of<SensorValueBase, T>::value>::type>
+ void copyTo(std::unique_ptr<T>& target) const
+ {
+ copyTo(target.get());
+ }
+
+ virtual ~SensorValueBase() = default;
+ };
+}
+
+#define DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION \
+ using SensorValueBase = ::armarx::SensorValueBase; \
+ virtual void copyTo(SensorValueBase* target) const override \
+ { \
+ using this_type = typename std::decay<decltype(*this)>::type; \
+ *(target->asA<this_type>()) = *this; \
+ } \
+ virtual std::unique_ptr<SensorValueBase> clone() const override \
+ { \
+ using this_type = typename std::decay<decltype(*this)>::type; \
+ std::unique_ptr<SensorValueBase> c {new this_type}; \
+ SensorValueBase::copyTo(c); \
+ return c; \
+ } \
+ virtual std::string getSensorValueType(bool withoutNamespaceSpecifier = false) const override \
+ { \
+ return getTypeName(*this, withoutNamespaceSpecifier); \
+ }
+
+namespace armarx
+{
+ class SensorValueDummy : public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ };
+}
+
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
new file mode 100644
index 0000000000000000000000000000000000000000..04588cb5561bed5e9c6d17628c486c5ff0ac41b8
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h
@@ -0,0 +1,48 @@
+/*
+ * 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 ( 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_LIB_RobotAPI_SensorValueForceTorque_H
+#define _ARMARX_LIB_RobotAPI_SensorValueForceTorque_H
+
+#include "Eigen/Core"
+#include "SensorValueBase.h"
+
+#include <RobotAPI/libraries/core/Pose.h>
+
+namespace armarx
+{
+ class SensorValueForceTorque : public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ Eigen::Vector3f torque;
+ Eigen::Vector3f force;
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ return
+ {
+ {"torque", {new TimedVariant{Vector3{torque}, timestamp}}},
+ {"force", {new TimedVariant{Vector3{force }, timestamp}}},
+ };
+ }
+ };
+}
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h
new file mode 100644
index 0000000000000000000000000000000000000000..8206fac66c3841c95c7c4874bce97fe4ea613ff6
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h
@@ -0,0 +1,145 @@
+/*
+ * 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 ( 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_LIB_RobotAPI_SensorValueHolonomicPlatformVelocity_H
+#define _ARMARX_LIB_RobotAPI_SensorValueHolonomicPlatformVelocity_H
+
+#include "SensorValueBase.h"
+
+#include <ArmarXCore/core/util/algorithm.h>
+
+namespace armarx
+{
+ class SensorValueHolonomicPlatformVelocity : virtual public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ float velocityX;
+ float velocityY;
+ float velocityRotation;
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ return
+ {
+ {"velocityX", {new TimedVariant(velocityX,timestamp)}},
+ {"velocityY", {new TimedVariant(velocityY,timestamp)}},
+ {"velocityRotation", {new TimedVariant(velocityRotation,timestamp)}},
+ };
+ }
+ };
+
+ class SensorValueHolonomicPlatformAcceleration : virtual public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ float accelerationX;
+ float accelerationY;
+ float accelerationRotation;
+ void deriveAccelerationFromVelocityDelta(float dvx, float dvy, float dvrot, float dt)
+ {
+ accelerationX = dvx / dt;
+ accelerationY = dvy / dt;
+ accelerationRotation = dvrot / dt;
+ }
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ return
+ {
+ {"accelerationX", {new TimedVariant(accelerationX,timestamp)}},
+ {"accelerationY", {new TimedVariant(accelerationY,timestamp)}},
+ {"accelerationRotation", {new TimedVariant(accelerationRotation,timestamp)}},
+ };
+ }
+ };
+
+ class SensorValueHolonomicPlatformRelativePosition : virtual public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ float relativePositionX;
+ float relativePositionY;
+ float relativePositionRotation;
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ return
+ {
+ {"relativePositionX", {new TimedVariant(relativePositionX,timestamp)}},
+ {"relativePositionY", {new TimedVariant(relativePositionY,timestamp)}},
+ {"relativePositionRotation", {new TimedVariant(relativePositionRotation,timestamp)}},
+ };
+ }
+ };
+
+ class SensorValueHolonomicPlatformAbsolutePosition : virtual public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ float absolutePositionX;
+ float absolutePositionY;
+ float absolutePositionRotation;
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ return
+ {
+ {"absolutePositionX", {new TimedVariant(absolutePositionX,timestamp)}},
+ {"absolutePositionY", {new TimedVariant(absolutePositionY,timestamp)}},
+ {"absolutePositionRotation", {new TimedVariant(absolutePositionRotation,timestamp)}},
+ };
+ }
+ };
+
+ class SensorValueHolonomicPlatform :
+ virtual public SensorValueHolonomicPlatformVelocity,
+ virtual public SensorValueHolonomicPlatformAcceleration,
+ virtual public SensorValueHolonomicPlatformRelativePosition
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+
+ void setVelocitiesAndDeriveAcceleration(float vx, float vy, float vrot, float dt)
+ {
+ deriveAccelerationFromVelocityDelta(vx - velocityX, vy - velocityY, vrot - velocityRotation, dt);
+ velocityX = vx;
+ velocityY = vy;
+ velocityRotation = vrot;
+ }
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ auto map = SensorValueHolonomicPlatformVelocity::toVariants(timestamp);
+ mergeMaps(map, SensorValueHolonomicPlatformAcceleration::toVariants(timestamp), MergeMapsMode::OverrideNoValues);
+ mergeMaps(map, SensorValueHolonomicPlatformRelativePosition::toVariants(timestamp), MergeMapsMode::OverrideNoValues);
+ return map;
+ }
+ };
+
+ class SensorValueHolonomicPlatformWithAbsolutePosition : virtual public SensorValueHolonomicPlatform, virtual public SensorValueHolonomicPlatformAbsolutePosition
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ auto map = SensorValueHolonomicPlatform::toVariants(timestamp);
+ mergeMaps(map, SensorValueHolonomicPlatformAbsolutePosition::toVariants(timestamp), MergeMapsMode::OverrideNoValues);
+ return map;
+ }
+ };
+}
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h
new file mode 100644
index 0000000000000000000000000000000000000000..87801a24024a5358e4b751924dcaa3cbd6c7c494
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueIMU.h
@@ -0,0 +1,55 @@
+/*
+ * 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 ( 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_LIB_RobotAPI_SensorValueIMU_H
+#define _ARMARX_LIB_RobotAPI_SensorValueIMU_H
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "SensorValueBase.h"
+
+#include <ArmarXCore/core/util/algorithm.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+namespace armarx
+{
+ class SensorValueIMU : public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+ Eigen::Vector3f linearVelocity;
+ Eigen::Vector3f angularVelocity;
+ Eigen::Vector3f linearAcceleration;
+ Eigen::Quaternionf orientation;
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ return
+ {
+ {"linearVelocity", {new TimedVariant{Vector3{linearVelocity},timestamp}}},
+ {"angularVelocity", {new TimedVariant{Vector3{angularVelocity},timestamp}}},
+ {"linearAcceleration", {new TimedVariant{Vector3{linearAcceleration},timestamp}}},
+ {"orientation", {new TimedVariant{Quaternion{orientation},timestamp}}},
+ };
+ }
+ };
+}
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueRTThreadTimings.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueRTThreadTimings.h
new file mode 100644
index 0000000000000000000000000000000000000000..a685736ab6d9dc324c839a5ee56f035b5c5a5614
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueRTThreadTimings.h
@@ -0,0 +1,91 @@
+/*
+ * 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 ( 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_LIB_RobotAPI_SensorValueRTThreadTimings_H
+#define _ARMARX_LIB_RobotAPI_SensorValueRTThreadTimings_H
+
+#include "SensorValueBase.h"
+
+#include <chrono>
+
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
+
+namespace armarx
+{
+ class SensorValueRTThreadTimings : public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+
+ std::chrono::microseconds rtSwitchControllerSetupDuration;
+ std::chrono::microseconds rtSwitchControllerSetupRoundTripTime;
+
+ std::chrono::microseconds rtRunLVL1ControllersDuration;
+ std::chrono::microseconds rtRunLVL1ControllersRoundTripTime;
+
+ std::chrono::microseconds rtHandleInvalidTargetsDuration;
+ std::chrono::microseconds rtHandleInvalidTargetsRoundTripTime;
+
+ std::chrono::microseconds rtRunLVL0ControllersDuration;
+ std::chrono::microseconds rtRunLVL0ControllersRoundTripTime;
+
+ std::chrono::microseconds rtBusSendReceiveDuration;
+ std::chrono::microseconds rtBusSendReceiveRoundTripTime;
+
+ std::chrono::microseconds rtReadSensorDeviceValuesDuration;
+ std::chrono::microseconds rtReadSensorDeviceValuesRoundTripTime;
+
+ std::chrono::microseconds rtUpdateSensorAndControlBufferDuration;
+ std::chrono::microseconds rtUpdateSensorAndControlBufferRoundTripTime;
+
+ std::chrono::microseconds rtResetAllTargetsDuration;
+ std::chrono::microseconds rtResetAllTargetsRoundTripTime;
+
+ std::chrono::microseconds rtLoopDuration;
+ std::chrono::microseconds rtLoopRoundTripTime;
+
+ virtual std::map<std::string, VariantBasePtr> toVariants(const IceUtil::Time& timestamp) const override
+ {
+ return
+ {
+ {"rtSwitchControllerSetup.Duration", new TimedVariant{TimestampVariant{rtSwitchControllerSetupDuration.count() }, timestamp}},
+ {"rtSwitchControllerSetup.RoundTripTime", new TimedVariant{TimestampVariant{rtSwitchControllerSetupRoundTripTime.count() }, timestamp}},
+ {"rtRunLVL1Controllers.Duration", new TimedVariant{TimestampVariant{rtRunLVL1ControllersDuration.count() }, timestamp}},
+ {"rtRunLVL1Controllers.RoundTripTime", new TimedVariant{TimestampVariant{rtRunLVL1ControllersRoundTripTime.count() }, timestamp}},
+ {"rtHandleInvalidTargets.Duration", new TimedVariant{TimestampVariant{rtHandleInvalidTargetsDuration.count() }, timestamp}},
+ {"rtHandleInvalidTargets.RoundTripTime", new TimedVariant{TimestampVariant{rtHandleInvalidTargetsRoundTripTime.count() }, timestamp}},
+ {"rtRunLVL0Controllers.Duration", new TimedVariant{TimestampVariant{rtRunLVL0ControllersDuration.count() }, timestamp}},
+ {"rtRunLVL0Controllers.RoundTripTime", new TimedVariant{TimestampVariant{rtRunLVL0ControllersRoundTripTime.count() }, timestamp}},
+ {"rtBusSendReceive.Duration", new TimedVariant{TimestampVariant{rtBusSendReceiveDuration.count() }, timestamp}},
+ {"rtBusSendReceive.RoundTripTime", new TimedVariant{TimestampVariant{rtBusSendReceiveRoundTripTime.count() }, timestamp}},
+ {"rtReadSensorDeviceValues.Duration", new TimedVariant{TimestampVariant{rtReadSensorDeviceValuesDuration.count() }, timestamp}},
+ {"rtReadSensorDeviceValues.RoundTripTime", new TimedVariant{TimestampVariant{rtReadSensorDeviceValuesRoundTripTime.count() }, timestamp}},
+ {"rtUpdateSensorAndControlBuffer.Duration", new TimedVariant{TimestampVariant{rtUpdateSensorAndControlBufferDuration.count() }, timestamp}},
+ {"rtUpdateSensorAndControlBuffer.RoundTripTime", new TimedVariant{TimestampVariant{rtUpdateSensorAndControlBufferRoundTripTime.count()}, timestamp}},
+ {"rtResetAllTargets.Duration", new TimedVariant{TimestampVariant{rtResetAllTargetsDuration.count() }, timestamp}},
+ {"rtResetAllTargets.RoundTripTime", new TimedVariant{TimestampVariant{rtResetAllTargetsRoundTripTime.count() }, timestamp}},
+ {"rtLoop.Duration", new TimedVariant{TimestampVariant{rtLoopDuration.count() }, timestamp}},
+ {"rtLoop.RoundTripTime", new TimedVariant{TimestampVariant{rtLoopRoundTripTime.count() }, timestamp}},
+ };
+ }
+ };
+}
+#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/SyntaxCheck.cpp b/source/RobotAPI/components/units/RobotUnit/SyntaxCheck.cpp
similarity index 52%
rename from source/RobotAPI/libraries/RTRobotUnit/SyntaxCheck.cpp
rename to source/RobotAPI/components/units/RobotUnit/SyntaxCheck.cpp
index 9c3e574114e6d5f934aca89da845286386f074ca..ae4e49be703b61fd5f306a5bcb95d8e584e9b396 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/SyntaxCheck.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/SyntaxCheck.cpp
@@ -20,25 +20,39 @@
* GNU General Public License
*/
+
+
+
#include "Constants.h"
#include "ControlModes.h"
#include "BasicControllers.h"
+#include "RobotUnit.h"
+#include "structs.h"
+#include "util.h"
+
+#include"Units/ForceTorqueSubUnit.h"
+#include"Units/InertialMeasurementSubUnit.h"
+#include"Units/KinematicSubUnit.h"
+#include"Units/PlatformSubUnit.h"
+#include"Units/RobotUnitSubUnit.h"
+
+#include "Devices/ControlDevice.h"
+#include "Devices/DeviceBase.h"
+#include "Devices/SensorDevice.h"
#include "LVL0Controllers/LVL0Controller.h"
#include "LVL1Controllers/LVL1Controller.h"
-#include "LVL1Controllers/LVL1PassThroughController.h"
-
-#include "DataUnits/ForceTorqueDataUnit.h"
-#include "DataUnits/HapticDataUnit.h"
-#include "DataUnits/IMUDataUnit.h"
-#include "DataUnits/KinematicDataUnit.h"
-#include "DataUnits/PlatformDataUnit.h"
+#include "LVL1Controllers/LVL1KinematicUnitPassThroughController.h"
+#include "LVL1Controllers/LVL1HolonomicPlatformUnitVelocityPassThroughController.h"
-#include "Targets/ActuatorPositionTarget.h"
-#include "Targets/TargetBase.h"
-#include "Targets/ActuatorTorqueTarget.h"
-#include "Targets/ActuatorVelocityTarget.h"
-#include "Targets/HolonomicPlatformVelocityTarget.h"
+#include "ControlTargets/ControlTargetBase.h"
+#include "ControlTargets/ControlTarget1DoFActuator.h"
+#include "ControlTargets/ControlTargetHolonomicPlatformVelocity.h"
+#include "SensorValues/SensorValueBase.h"
+#include "SensorValues/SensorValue1DoFActuator.h"
+#include "SensorValues/SensorValueHolonomicPlatform.h"
+#include "SensorValues/SensorValueIMU.h"
+#include "SensorValues/SensorValueForceTorque.h"
//this file is only for syntax checks
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6b0faa5943dd167de89d13289d11f0b5a6ed7fbe
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.cpp
@@ -0,0 +1,68 @@
+/*
+ * 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::ForceTorqueSubUnit
+ * @author Raphael ( 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
+ */
+#include "ForceTorqueSubUnit.h"
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/libraries/core/FramedPose.h>
+
+void armarx::ForceTorqueSubUnit::update(const armarx::SensorAndControl& sc, const LVL0AndLVL1Controllers&)
+{
+ if(!getProxy())
+ {
+ //this unit is not initialized yet
+ ARMARX_IMPORTANT << deactivateSpam(1) << "not initialized yet";
+ return;
+ }
+ if(!listenerPrx)
+ {
+ ARMARX_IMPORTANT << deactivateSpam(1) << "listener is not set";
+ return;
+ }
+ for (DeviceData& dev : devs)
+ {
+ const SensorValueBase* sensorValBase = sc.sensors.at(dev.sensorIndex).get();
+ ARMARX_CHECK_EXPRESSION(sensorValBase->isA<SensorValueForceTorque>());
+ const SensorValueForceTorque& sensorVal = *sensorValBase->asA<SensorValueForceTorque>();
+ ARMARX_CHECK_EXPRESSION(listenerPrx);
+ listenerPrx->reportSensorValues(
+ dev.deviceName,
+ new armarx::FramedDirection(sensorVal.force, dev.frame, agentName),
+ new armarx::FramedDirection(sensorVal.torque, dev.frame, agentName)
+ );
+ }
+}
+
+void armarx::ForceTorqueSubUnit::setOffset(const armarx::FramedDirectionBasePtr &, const armarx::FramedDirectionBasePtr &, const Ice::Current &)
+{
+ ARMARX_WARNING << "NYI";
+}
+
+void armarx::ForceTorqueSubUnit::setToNull(const Ice::Current &)
+{
+ ARMARX_WARNING << "NYI";
+}
+
+void armarx::ForceTorqueSubUnit::onInitForceTorqueUnit()
+{
+ agentName = getProperty<std::string>("AgentName").getValue();
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..3506935399b6a96c384e3eb328a1329d876d3dec
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.h
@@ -0,0 +1,63 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::ArmarXObjects::ForceTorqueSubUnit
+ * @author Raphael ( 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_LIB_RobotAPI_ForceTorqueSubUnit_H
+#define _ARMARX_LIB_RobotAPI_ForceTorqueSubUnit_H
+
+#include "RobotUnitSubUnit.h"
+
+#include <RobotAPI/components/units/ForceTorqueUnit.h>
+
+#include "../SensorValues/SensorValueForceTorque.h"
+
+namespace armarx
+{
+ TYPEDEF_PTRS_HANDLE(ForceTorqueSubUnit);
+ class ForceTorqueSubUnit:
+ virtual public RobotUnitSubUnit,
+ virtual public ForceTorqueUnit
+ {
+ public:
+ virtual void update(const SensorAndControl& sc, const LVL0AndLVL1Controllers& c) override;
+
+ // ForceTorqueUnitInterface interface
+ virtual void setOffset(const FramedDirectionBasePtr&, const FramedDirectionBasePtr&, const Ice::Current& = GlobalIceCurrent) override;
+ virtual void setToNull(const Ice::Current& = GlobalIceCurrent) override;
+
+ // ForceTorqueUnit interface
+ virtual void onInitForceTorqueUnit() override;
+ virtual void onStartForceTorqueUnit() override {}
+ virtual void onExitForceTorqueUnit() override {}
+
+ struct DeviceData
+ {
+ std::string deviceName;
+ std::size_t sensorIndex;
+ std::string frame;
+ };
+
+ std::vector<DeviceData> devs;
+ private:
+ std::string agentName;
+ };
+}
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8cf07e4ee667ce6864c71e05eba4da4645a8dfdc
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.cpp
@@ -0,0 +1,51 @@
+/*
+ * 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::InertialMeasurementSubUnit
+ * @author Raphael ( 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
+ */
+#include "InertialMeasurementSubUnit.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+void armarx::InertialMeasurementSubUnit::update(const armarx::SensorAndControl& sc, const LVL0AndLVL1Controllers&)
+{
+ if(!getProxy())
+ {
+ //this unit is not initialized yet
+ return;
+ }
+ if(!IMUTopicPrx)
+ {
+ ARMARX_IMPORTANT << deactivateSpam(1) << "listener is not set";
+ return;
+ }
+ TimestampVariantPtr t = new TimestampVariant(sc.sensorValuesTimestamp);
+ for (auto nam2idx : devs)
+ {
+ const auto devidx = nam2idx.second;
+ const auto& dev = nam2idx.first;
+ const SensorValueBase* sv = sc.sensors.at(devidx).get();
+ ARMARX_CHECK_EXPRESSION(sv->isA<SensorValueIMU>());
+ // const SensorValueIMU* s = sv->asA<SensorValueIMU>();
+ IMUData data;
+ // data.acceleration = s->linearAcceleration;
+ // data.gyroscopeRotation = s->angularVelocity;
+ IMUTopicPrx->reportSensorValues(dev, "name", data, t);
+ ///TODO fix unclear imu stuff
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..3c4a5a6808715dc1b11007f3c337ea15db1ff95c
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/InertialMeasurementSubUnit.h
@@ -0,0 +1,53 @@
+/*
+ * 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::InertialMeasurementSubUnit
+ * @author Raphael ( 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_LIB_RobotAPI_InertialMeasurementSubUnit_H
+#define _ARMARX_LIB_RobotAPI_InertialMeasurementSubUnit_H
+
+#include "RobotUnitSubUnit.h"
+
+#include <ArmarXCore/observers/variant/TimestampVariant.h>
+
+#include <RobotAPI/components/units/InertialMeasurementUnit.h>
+
+#include "../SensorValues/SensorValueIMU.h"
+
+namespace armarx
+{
+ TYPEDEF_PTRS_HANDLE(InertialMeasurementSubUnit);
+ class InertialMeasurementSubUnit:
+ virtual public RobotUnitSubUnit,
+ virtual public InertialMeasurementUnit
+ {
+ public:
+ virtual void update(const SensorAndControl& sc, const LVL0AndLVL1Controllers& c);
+
+ // InertialMeasurementUnit interface
+ protected:
+ virtual void onInitIMU() override {}
+ virtual void onStartIMU() override {}
+ virtual void onExitIMU() override {}
+ public:
+ std::map<std::string, std::size_t> devs;
+ };
+}
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c595351fe68783c91840329ad49bb22f01fe25af
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.cpp
@@ -0,0 +1,233 @@
+/*
+ * 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::KinematicSubUnit
+ * @author Raphael ( 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
+ */
+#include "KinematicSubUnit.h"
+
+void armarx::KinematicSubUnit::setupData(std::string relRobFile, VirtualRobot::RobotPtr rob, std::map<std::string, ActuatorData> newDevs, RobotUnit* newRobotUnit)
+{
+ std::lock_guard<std::mutex> guard {dataMutex};
+ ARMARX_CHECK_EXPRESSION(getState() == eManagedIceObjectCreated);
+
+ ARMARX_CHECK_EXPRESSION(!robot);
+ ARMARX_CHECK_EXPRESSION(rob);
+ robot = rob;
+
+ ARMARX_CHECK_EXPRESSION(!robotUnit);
+ ARMARX_CHECK_EXPRESSION(newRobotUnit);
+ robotUnit = newRobotUnit;
+
+ ARMARX_CHECK_EXPRESSION(relativeRobotFile.empty());
+ ARMARX_CHECK_EXPRESSION(!relRobFile.empty());
+ relativeRobotFile = relRobFile;
+
+ devs = std::move(newDevs);
+}
+
+void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const armarx::LVL0AndLVL1Controllers& c)
+{
+ if(!getProxy())
+ {
+ //this unit is not initialized yet
+ ARMARX_IMPORTANT << deactivateSpam(1) << "not initialized yet";
+ return;
+ }
+ if(!listenerPrx)
+ {
+ ARMARX_IMPORTANT << deactivateSpam(1) << "listener is not set";
+ return;
+ }
+ std::lock_guard<std::mutex> guard {dataMutex};
+ bool ctrlModesAValueChanged = false;
+ bool angAValueChanged = false;
+ bool velAValueChanged = false;
+ bool accAValueChanged = false;
+ bool torAValueChanged = false;
+ bool motorCurrentAValueChanged = false;
+ bool motorTemperaturesAValueChanged = false;
+
+ long timestamp = sc.sensorValuesTimestamp.toMicroSeconds();
+ std::set<LVL1Controller*> lvl1s {c.lvl1Controllers.begin(), c.lvl1Controllers.end()};
+ std::vector<std::string> actuatorsWithoutSensor;
+ actuatorsWithoutSensor.reserve(devs.size());
+ for (const auto& name2actuatorData : devs)
+ {
+ const ActuatorData& actuatorData = name2actuatorData.second;
+ const auto& name = actuatorData.name;
+ //mode
+ {
+ ControlMode c = eUnknown;
+ if (actuatorData.ctrlPos && lvl1s.count(actuatorData.ctrlPos.get()))
+ {
+ c = ePositionControl;
+ }
+ if (actuatorData.ctrlVel && lvl1s.count(actuatorData.ctrlVel.get()))
+ {
+ ARMARX_CHECK_EXPRESSION(eUnknown == c);
+ c = eVelocityControl;
+ }
+ if (actuatorData.ctrlTor && lvl1s.count(actuatorData.ctrlTor.get()))
+ {
+ ARMARX_CHECK_EXPRESSION(eUnknown == c);
+ c = eTorqueControl;
+ }
+ ctrlModesAValueChanged = ctrlModesAValueChanged || !ctrlModes.count(name) || (ctrlModes.at(name) != c);
+ ctrlModes[name] = c;
+ }
+ if (actuatorData.sensorDeviceIndex < sc.sensors.size())
+ {
+ const SensorValueBase* s = sc.sensors.at(actuatorData.sensorDeviceIndex).get();
+ UpdateNameValueMap<SensorValue1DoFActuatorPosition , &SensorValue1DoFActuatorPosition::position >(ang, name, s, angAValueChanged);
+ UpdateNameValueMap<SensorValue1DoFActuatorVelocity , &SensorValue1DoFActuatorVelocity::velocity >(vel, name, s, velAValueChanged);
+ UpdateNameValueMap<SensorValue1DoFActuatorAcceleration , &SensorValue1DoFActuatorAcceleration::acceleration >(acc, name, s, accAValueChanged);
+ UpdateNameValueMap<SensorValue1DoFActuatorTorque , &SensorValue1DoFActuatorTorque::torque >(tor, name, s, torAValueChanged);
+ UpdateNameValueMap<SensorValue1DoFActuatorCurrent , &SensorValue1DoFActuatorCurrent::motorCurrent >(motorCurrents, name, s, motorCurrentAValueChanged);
+ UpdateNameValueMap<SensorValue1DoFActuatorMotorTemperature, &SensorValue1DoFActuatorMotorTemperature::motorTemperature>(motorTemperatures, name, s, motorTemperaturesAValueChanged);
+ }
+ else
+ {
+ actuatorsWithoutSensor.emplace_back(name);
+ }
+ }
+ if (!actuatorsWithoutSensor.empty())
+ {
+ ARMARX_WARNING << deactivateSpam(5) << "these actuators have no sensor!:\n" << actuatorsWithoutSensor;
+ }
+ ARMARX_DEBUG << deactivateSpam(1) << "reporting updated data:\n"
+ << ctrlModes.size() << " \tcontrol modes (updated = " << ctrlModesAValueChanged << ")\n"
+ << ang.size() << " \tjoint angles (updated = " << angAValueChanged << ")\n"
+ << vel.size() << " \tjoint velocities (updated = " << velAValueChanged << ")\n"
+ << acc.size() << " \tjoint accelerations (updated = " << accAValueChanged << ")\n"
+ << tor.size() << " \tjoint torques (updated = " << torAValueChanged << ")\n"
+ << motorCurrents.size() << " \tmotor currents (updated = " << motorCurrentAValueChanged << ")\n"
+ << motorTemperatures.size() << " \tmotor temperatures (updated = " << motorTemperaturesAValueChanged << ")\n";
+ auto prx = listenerPrx->ice_batchOneway();
+ prx->reportControlModeChanged(ctrlModes, timestamp, ctrlModesAValueChanged);
+ prx->reportJointAngles(ang, timestamp, angAValueChanged);
+ prx->reportJointVelocities(vel, timestamp, velAValueChanged);
+ prx->reportJointAccelerations(acc, timestamp, accAValueChanged);
+ prx->reportJointTorques(tor, timestamp, torAValueChanged);
+ prx->reportJointCurrents(motorCurrents, timestamp, motorCurrentAValueChanged);
+ prx->reportJointMotorTemperatures(motorTemperatures, timestamp, motorTemperaturesAValueChanged);
+ prx->ice_flushBatchRequests();
+}
+
+void armarx::KinematicSubUnit::requestJoints(const Ice::StringSeq &, const Ice::Current &)
+{
+ ARMARX_WARNING << "NYI";
+}
+
+void armarx::KinematicSubUnit::releaseJoints(const Ice::StringSeq &, const Ice::Current &)
+{
+ ARMARX_WARNING << "NYI";
+}
+
+void armarx::KinematicSubUnit::setJointAngles(const armarx::NameValueMap& angles, const Ice::Current&)
+{
+ std::lock_guard<std::mutex> guard {dataMutex};
+ for (const auto& n2v : angles)
+ {
+ if (devs.count(n2v.first))
+ {
+ devs.at(n2v.first).ctrlPos->set(n2v.second);
+ }
+ }
+}
+
+void armarx::KinematicSubUnit::setJointVelocities(const armarx::NameValueMap& velocities, const Ice::Current&)
+{
+ std::lock_guard<std::mutex> guard {dataMutex};
+ ARMARX_IMPORTANT << VAROUT(velocities);
+ for (const auto& n2v : velocities)
+ {
+ if (devs.count(n2v.first))
+ {
+ devs.at(n2v.first).ctrlVel->set(n2v.second);
+ }
+ }
+}
+
+void armarx::KinematicSubUnit::setJointTorques(const armarx::NameValueMap& torques, const Ice::Current&)
+{
+ std::lock_guard<std::mutex> guard {dataMutex};
+ for (const auto& n2v : torques)
+ {
+ if (devs.count(n2v.first))
+ {
+ devs.at(n2v.first).ctrlTor->set(n2v.second);
+ }
+ }
+}
+
+void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMap& ncm, const Ice::Current&)
+{
+ std::vector<std::string> toActivate;
+ {
+ std::lock_guard<std::mutex> guard {dataMutex};
+ toActivate.reserve(ncm.size());
+ for (const auto& n2c : ncm)
+ {
+ if (devs.count(n2c.first))
+ {
+ auto ctrl = devs.at(n2c.first).getController(n2c.second);
+ if (ctrl && !ctrl->isControllerActive())
+ {
+ toActivate.emplace_back(ctrl->getInstanceName());
+ }
+ }
+ }
+ ARMARX_VERBOSE << "switching control modes activates these LVL1Controllers:\n" << toActivate << std::flush;
+ ARMARX_CHECK_EXPRESSION(robotUnit);
+ }
+ robotUnit->activateControllers(toActivate);
+}
+
+void armarx::KinematicSubUnit::setJointAccelerations(const armarx::NameValueMap &, const Ice::Current &)
+{
+ ARMARX_WARNING << "NYI";
+}
+
+void armarx::KinematicSubUnit::setJointDecelerations(const armarx::NameValueMap &, const Ice::Current &)
+{
+ ARMARX_WARNING << "NYI";
+}
+
+armarx::NameControlModeMap armarx::KinematicSubUnit::getControlModes(const Ice::Current&)
+{
+ std::lock_guard<std::mutex> guard {dataMutex};
+ return ctrlModes;
+}
+
+const armarx::LVL1ControllerPtr armarx::KinematicSubUnit::ActuatorData::getController(armarx::ControlMode c) const
+{
+ switch (c)
+ {
+ case ePositionVelocityControl:
+ return ctrlPos;
+ case ePositionControl:
+ return ctrlPos;
+ case eVelocityControl:
+ return ctrlVel;
+ case eTorqueControl:
+ return ctrlTor;
+ default:
+ return nullptr;
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..7bbf7d699ca8362a577483a16177af5953bdeecf
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/KinematicSubUnit.h
@@ -0,0 +1,109 @@
+/*
+ * 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::KinematicSubUnit
+ * @author Raphael ( 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_LIB_RobotAPI_KinematicSubUnit_H
+#define _ARMARX_LIB_RobotAPI_KinematicSubUnit_H
+
+#include <mutex>
+
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+
+#include <RobotAPI/components/units/KinematicUnit.h>
+
+#include "RobotUnitSubUnit.h"
+#include "../util.h"
+#include "../SensorValues/SensorValue1DoFActuator.h"
+#include "../LVL1Controllers/LVL1KinematicUnitPassThroughController.h"
+
+namespace armarx
+{
+ class RobotUnit;
+
+ TYPEDEF_PTRS_HANDLE(KinematicSubUnit);
+ class KinematicSubUnit:
+ virtual public RobotUnitSubUnit,
+ virtual public KinematicUnit
+ {
+ public:
+ struct ActuatorData
+ {
+ std::string name;
+ std::size_t sensorDeviceIndex;
+ LVL1KinematicUnitPassThroughControllerPtr ctrlPos;
+ LVL1KinematicUnitPassThroughControllerPtr ctrlVel;
+ LVL1KinematicUnitPassThroughControllerPtr ctrlTor;
+
+ const LVL1ControllerPtr getController(ControlMode c) const;
+ };
+
+ void setupData(std::string relRobFile, VirtualRobot::RobotPtr rob, std::map<std::string, ActuatorData> newDevs, RobotUnit* newRobotUnit);
+ virtual void update(const SensorAndControl& sc, const LVL0AndLVL1Controllers& c) override;
+
+ // KinematicUnitInterface interface
+ virtual void requestJoints(const Ice::StringSeq&, const Ice::Current&) override;
+ virtual void releaseJoints(const Ice::StringSeq&, const Ice::Current&) override;
+
+ virtual void setJointAngles(const NameValueMap& angles, const Ice::Current&) override;
+ virtual void setJointVelocities(const NameValueMap& velocities, const Ice::Current&) override;
+ virtual void setJointTorques(const NameValueMap& torques, const Ice::Current&) override;
+ virtual void switchControlMode(const NameControlModeMap& ncm, const Ice::Current&) override;
+
+ virtual void setJointAccelerations(const NameValueMap&, const Ice::Current&) override;
+ virtual void setJointDecelerations(const NameValueMap&, const Ice::Current&) override;
+
+ virtual NameControlModeMap getControlModes(const Ice::Current&) override;
+
+ // KinematicUnit interface
+ virtual void onInitKinematicUnit() override {}
+ virtual void onStartKinematicUnit() override {}
+ virtual void onExitKinematicUnit() override {}
+ private:
+ std::map<std::string, ActuatorData> devs;
+ // never use this when holding the mutex! (could cause deadlocks)
+ RobotUnit* robotUnit;
+ mutable std::mutex dataMutex;
+ NameControlModeMap ctrlModes;
+ NameValueMap ang;
+ NameValueMap vel;
+ NameValueMap acc;
+ NameValueMap tor;
+ NameValueMap motorCurrents;
+ NameValueMap motorTemperatures;
+
+ template<class SensorValueT, float SensorValueT::* Member>
+ static void UpdateNameValueMap(NameValueMap& nvm, const std::string& name, const SensorValueBase* sv, bool& changeState)
+ {
+ const SensorValueT* s = sv->asA<SensorValueT>();
+ if (!s)
+ {
+ return;
+ };
+ const float v = s->*Member;
+ if (!nvm.count(name) || (nvm.at(name) != v))
+ {
+ changeState = true;
+ }
+ nvm[name] = v;
+ }
+ };
+}
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..94a3d1f267b65454802899cad820135d068abc92
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
@@ -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::ArmarXObjects::PlatformSubUnit
+ * @author Raphael ( 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
+ */
+#include "PlatformSubUnit.h"
+
+void armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc, const LVL0AndLVL1Controllers&)
+{
+ if(!getProxy())
+ {
+ //this unit is not initialized yet
+ ARMARX_IMPORTANT << deactivateSpam(1) << "not initialized yet";
+ return;
+ }
+ if(!listenerPrx)
+ {
+ ARMARX_IMPORTANT << deactivateSpam(1) << "listener is not set";
+ return;
+ }
+ const SensorValueBase* sensorValue = sc.sensors.at(platformSensorIndex).get();
+ ARMARX_CHECK_EXPRESSION(sensorValue);
+ std::lock_guard<std::mutex> guard {dataMutex};
+ //pos
+ {
+ ARMARX_CHECK_EXPRESSION(sensorValue->isA<SensorValueHolonomicPlatformRelativePosition>());
+ const SensorValueHolonomicPlatformRelativePosition* s = sensorValue->asA<SensorValueHolonomicPlatformRelativePosition>();
+ relX = s->relativePositionX;
+ relY = s->relativePositionY;
+ relR = s->relativePositionRotation;
+
+ const Eigen::Matrix4f glob = getCorrectedPose();
+ const float globR = VirtualRobot::MathTools::eigen4f2rpy(glob)(2);
+ listenerPrx->reportPlatformPose(glob(0, 3), glob(1, 3), globR);
+ }
+ //vel
+ {
+ ARMARX_CHECK_EXPRESSION(sensorValue->isA<SensorValueHolonomicPlatformVelocity>());
+ const SensorValueHolonomicPlatformVelocity* s = sensorValue->asA<SensorValueHolonomicPlatformVelocity>();
+ listenerPrx->reportPlatformVelocity(s->velocityX, s->velocityY , s->velocityRotation);
+ }
+}
+
+void armarx::PlatformSubUnit::move(Ice::Float vx, Ice::Float vy, Ice::Float vr, const Ice::Current&)
+{
+ //holding the mutex here could deadlock
+ if (!pt->isControllerActive())
+ {
+ pt->activateController();
+ }
+ std::lock_guard<std::mutex> guard {dataMutex};
+ pt->setVelocites(
+ boost::algorithm::clamp(vx, -maxVLin, maxVLin),
+ boost::algorithm::clamp(vy, -maxVLin, maxVLin),
+ boost::algorithm::clamp(vr, -maxVAng, maxVAng)
+ );
+}
+
+void armarx::PlatformSubUnit::moveTo(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current &)
+{
+ ARMARX_WARNING << "NYI";
+}
+
+void armarx::PlatformSubUnit::moveRelative(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current &)
+{
+ ARMARX_WARNING << "NYI";
+}
+
+void armarx::PlatformSubUnit::setMaxVelocities(Ice::Float mxVLin, Ice::Float mxVAng, const Ice::Current&)
+{
+ std::lock_guard<std::mutex> guard {dataMutex};
+ maxVLin = std::abs(mxVLin);
+ maxVAng = std::abs(mxVAng);
+}
+
+void armarx::PlatformSubUnit::setGlobalPose(armarx::PoseBasePtr globalPose, const Ice::Current&)
+{
+ std::lock_guard<std::mutex> guard {dataMutex};
+ PosePtr p = PosePtr::dynamicCast(globalPose);
+ relPositionCorrection = p->toEigen() * getRelativePose().inverse();
+}
+
+armarx::PoseBasePtr armarx::PlatformSubUnit::getGlobalPose(const Ice::Current&)
+{
+ std::lock_guard<std::mutex> guard {dataMutex};
+ return new Pose {getCorrectedPose()};
+}
+
+void armarx::PlatformSubUnit::stopPlatform(const Ice::Current& c)
+{
+ move(0, 0, 0);
+}
+
+Eigen::Matrix4f armarx::PlatformSubUnit::getRelativePose() const
+{
+ Eigen::Matrix4f rp = VirtualRobot::MathTools::rpy2eigen4f(0, 0, relR);
+ rp(0, 3) = relX;
+ rp(1, 3) = relY;
+ return rp;
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
new file mode 100644
index 0000000000000000000000000000000000000000..cd15f2c4ee40bab2fcef27aacb7680fee97a6e09
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -0,0 +1,87 @@
+/*
+ * 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::PlatformSubUnit
+ * @author Raphael ( 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_LIB_RobotAPI_PlatformSubUnit_H
+#define _ARMARX_LIB_RobotAPI_PlatformSubUnit_H
+
+#include <mutex>
+
+#include <boost/algorithm/clamp.hpp>
+
+#include <Eigen/Core>
+
+#include <VirtualRobot/MathTools.h>
+
+#include <RobotAPI/components/units/PlatformUnit.h>
+#include <RobotAPI/libraries/core/Pose.h>
+
+#include "RobotUnitSubUnit.h"
+#include "../LVL1Controllers/LVL1HolonomicPlatformUnitVelocityPassThroughController.h"
+#include "../SensorValues/SensorValueHolonomicPlatform.h"
+
+namespace armarx
+{
+ TYPEDEF_PTRS_HANDLE(PlatformSubUnit);
+ class PlatformSubUnit:
+ virtual public RobotUnitSubUnit,
+ virtual public PlatformUnit
+ {
+ public:
+ virtual void update(const SensorAndControl& sc, const LVL0AndLVL1Controllers& c) override;
+
+ // PlatformUnitInterface interface
+ virtual void move(Ice::Float vx, Ice::Float vy, Ice::Float vr, const Ice::Current& = GlobalIceCurrent) override;
+ virtual void moveTo(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current& = GlobalIceCurrent) override;
+ virtual void moveRelative(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current& = GlobalIceCurrent) override;
+
+ virtual void setMaxVelocities(Ice::Float mxVLin, Ice::Float mxVAng, const Ice::Current& = GlobalIceCurrent) override;
+
+ virtual void setGlobalPose(PoseBasePtr globalPose, const Ice::Current& = GlobalIceCurrent) /*override*/;
+ virtual PoseBasePtr getGlobalPose(const Ice::Current& = GlobalIceCurrent) /*override*/;
+
+ // PlatformUnit interface
+ virtual void onInitPlatformUnit() override {}
+ virtual void onStartPlatformUnit() override {}
+ virtual void onExitPlatformUnit() override {}
+ virtual void stopPlatform(const Ice::Current& c = GlobalIceCurrent) override;
+
+ LVL1HolonomicPlatformUnitVelocityPassThroughControllerPtr pt;
+ std::size_t platformSensorIndex;
+ protected:
+ Eigen::Matrix4f getRelativePose() const;
+ Eigen::Matrix4f getCorrectedPose() const
+ {
+ return relPositionCorrection * getRelativePose();
+ }
+
+ mutable std::mutex dataMutex;
+
+ Ice::Float maxVLin = std::numeric_limits<Ice::Float>::infinity();
+ Ice::Float maxVAng = std::numeric_limits<Ice::Float>::infinity();
+
+ float relX = 0;
+ float relY = 0;
+ float relR = 0;
+ Eigen::Matrix4f relPositionCorrection = Eigen::Matrix4f::Identity();
+ };
+}
+#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/DataUnits/ForceTorqueDataUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.cpp
similarity index 68%
rename from source/RobotAPI/libraries/RTRobotUnit/DataUnits/ForceTorqueDataUnit.h
rename to source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.cpp
index a297a4879603508ad6a1098a76b8bcffb3d0c4bd..481cfac2884ff6689fe51112ab9232630d2316a2 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/DataUnits/ForceTorqueDataUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.cpp
@@ -13,27 +13,10 @@
* 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::ForceTorqueDataUnit
+ * @package RobotAPI::ArmarXObjects::RobotUnitSubUnit
* @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_ForceTorqueDataUnit_H
-#define _ARMARX_LIB_RobotAPI_ForceTorqueDataUnit_H
-
-namespace armarx
-{
- class ForceTorqueDataUnitInterface
- {
- public:
- };
-
- class ForceTorqueDataUnitPtrProvider: virtual public ForceTorqueDataUnitInterface
- {
- public:
- };
-}
-
-#endif
+#include "RobotUnitSubUnit.h"
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.cpp b/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h
similarity index 62%
rename from source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.cpp
rename to source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h
index 0b8b25547253c80beff59dd74c323fe2b7a7ecb9..9e915547b41faa4984048b5fb67f9e54ec7a59d6 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h
@@ -13,35 +13,29 @@
* 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::LVL1Controller
+ * @package RobotAPI::ArmarXObjects::RobotUnitSubUnit
* @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 "LVL1Controller.h"
+#ifndef _ARMARX_LIB_RobotAPI_RobotUnitSubUnit_H
+#define _ARMARX_LIB_RobotAPI_RobotUnitSubUnit_H
+#include <ArmarXCore/core/ManagedIceObject.h>
-using namespace armarx;
+#include "../util.h"
+#include "../structs.h"
-
-
-void LVL1Controller::rtActivateController()
-{
- if (!isActive)
- {
- rtPreActivateController();
- isActive = true;
- }
-}
-
-void LVL1Controller::rtDeactivateController()
+namespace armarx
{
- if (isActive)
+ TYPEDEF_PTRS_HANDLE(RobotUnitSubUnit);
+ class RobotUnitSubUnit: public virtual ManagedIceObject
{
- isActive = false;
- rtPostDeactivateController();
- }
+ public:
+ virtual void update(const SensorAndControl& sc, const LVL0AndLVL1Controllers& c) = 0;
+ };
}
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/structs.h b/source/RobotAPI/components/units/RobotUnit/structs.h
new file mode 100644
index 0000000000000000000000000000000000000000..a589aadb4886f6a430dd8ab144bf1618b5bc7cda
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/structs.h
@@ -0,0 +1,63 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package RobotAPI::RobotUnit
+ * @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_UNIT_RobotAPI_RobotUnit_structs_H
+#define _ARMARX_UNIT_RobotAPI_RobotUnit_structs_H
+
+#include <memory>
+#include <vector>
+#include "SensorValues/SensorValueBase.h"
+#include "ControlTargets/ControlTargetBase.h"
+
+namespace armarx
+{
+ class LVL0Controller;
+ class LVL1Controller;
+
+ /// @brief Structure used by the RobotUnit to swap out sensor values and control targets
+ struct SensorAndControl
+ {
+ //since this is not automatically propagated from a vector of noncopyable elements
+ SensorAndControl(const SensorAndControl&) = delete;
+ SensorAndControl& operator=(const SensorAndControl&) = delete;
+ //since deleting ctors stops autogeneration for everything
+ SensorAndControl() = default;
+ SensorAndControl(SensorAndControl&&) = default;
+ SensorAndControl& operator=(SensorAndControl &&) = default;
+
+ IceUtil::Time sensorValuesTimestamp;
+ IceUtil::Time timeSinceLastIteration;
+ std::vector<std::unique_ptr<SensorValueBase>> sensors;
+ std::vector<std::vector<std::unique_ptr<ControlTargetBase>>> control;
+ };
+ /// @brief Structure used by the RobotUnit to swap lists of lvl0 and lvl1 controllers
+ struct LVL0AndLVL1Controllers
+ {
+ LVL0AndLVL1Controllers(std::size_t n = 0)
+ : lvl0Controllers(n),
+ lvl1Controllers(n)
+ {}
+ std::vector<LVL0Controller*> lvl0Controllers;
+ std::vector<LVL1Controller*> lvl1Controllers;
+ };
+}
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp b/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7e822a463808e5523f7f8afd771a49880954dc29
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/test/BasicControllerTest.cpp
@@ -0,0 +1,669 @@
+/*
+* 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 = [&]
+ {
+ VelocityControllerWithAccelerationBounds ctrl;
+ ctrl.dt = s.dt;
+ ctrl.maxDt = s.maxDt;
+ ctrl.currentV = s.curvel;
+ ctrl.targetV = s.targvel;
+ ctrl.maxV = s.maxvel;
+ ctrl.acceleration = s.acc;
+ ctrl.deceleration = s.dec;
+ ctrl.directSetVLimit = directSetVLimit;
+ BOOST_CHECK(ctrl.validParameters());
+ // s.updateVel(velocityControlWithAccelerationBounds(s.dt, s.maxDt, s.curvel, s.targvel, s.maxvel, s.acc, s.dec, directSetVLimit));
+ s.updateVel(ctrl.run());
+ 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 = [&]
+ {
+ VelocityControllerWithAccelerationAndPositionBounds ctrl;
+ ctrl.dt = s.dt;
+ ctrl.maxDt = s.maxDt;
+ ctrl.currentV = s.curvel;
+ ctrl.targetV = s.targvel;
+ ctrl.maxV = s.maxvel;
+ ctrl.acceleration = s.acc;
+ ctrl.deceleration = s.dec;
+ ctrl.directSetVLimit = directSetVLimit;
+ ctrl.currentPosition = s.curpos;
+ ctrl.positionLimitLoSoft = s.posLo;
+ ctrl.positionLimitHiSoft = s.posHi;
+ ctrl.positionLimitLoHard = s.posLoHard;
+ ctrl.positionLimitHiHard = s.posHiHard;
+ BOOST_CHECK(ctrl.validParameters());
+ s.updateVel(ctrl.run());
+ //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 = [&]
+ {
+ PositionThroughVelocityControllerWithAccelerationBounds ctrl;
+ ctrl.dt = s.dt;
+ ctrl.maxDt = s.maxDt;
+ ctrl.currentV = s.curvel;
+ ctrl.maxV = s.maxvel;
+ ctrl.acceleration = s.acc;
+ ctrl.deceleration = s.dec;
+ ctrl.currentPosition = s.curpos;
+ ctrl.targetPosition = s.targpos;
+ ctrl.pControlPosErrorLimit = pControlPosErrorLimit;
+ ctrl.pControlVelLimit = pControlVelLimit;
+ ctrl.p = p;
+ BOOST_CHECK(ctrl.validParameters());
+ s.updateVel(ctrl.run());
+ //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 = [&]
+ {
+ PositionThroughVelocityControllerWithAccelerationAndPositionBounds ctrl;
+ ctrl.dt = s.dt;
+ ctrl.maxDt = s.maxDt;
+ ctrl.currentV = s.curvel;
+ ctrl.maxV = s.maxvel;
+ ctrl.acceleration = s.acc;
+ ctrl.deceleration = s.dec;
+ ctrl.currentPosition = s.curpos;
+ ctrl.targetPosition = s.targpos;
+ ctrl.pControlPosErrorLimit = pControlPosErrorLimit;
+ ctrl.pControlVelLimit = pControlVelLimit;
+ ctrl.p = p;
+ ctrl.positionLimitLo = s.posLoHard;
+ ctrl.positionLimitHi = s.posHiHard;
+ BOOST_CHECK(ctrl.validParameters());
+ s.updateVel(ctrl.run());
+ //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 = [&]
+ {
+ PositionThroughVelocityControllerWithAccelerationAndPositionBounds ctrl;
+ ctrl.dt = s.dt;
+ ctrl.maxDt = s.maxDt;
+ ctrl.currentV = s.curvel;
+ ctrl.maxV = s.maxvel;
+ ctrl.acceleration = s.acc;
+ ctrl.deceleration = s.dec;
+ ctrl.currentPosition = s.curpos;
+ ctrl.targetPosition = s.targpos;
+ ctrl.pControlPosErrorLimit = pControlPosErrorLimit;
+ ctrl.pControlVelLimit = pControlVelLimit;
+ ctrl.p = p;
+ ctrl.positionLimitLo = s.posLoHard;
+ ctrl.positionLimitHi = s.posHiHard;
+ BOOST_CHECK(ctrl.validParameters());
+ s.updateVel(ctrl.run());
+ //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);
+
+ 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/components/units/RobotUnit/test/CMakeLists.txt
similarity index 70%
rename from source/RobotAPI/libraries/RTRobotUnit/test/CMakeLists.txt
rename to source/RobotAPI/components/units/RobotUnit/test/CMakeLists.txt
index c8695b9ab1c8703fc789a737ddffc79c28b290e7..bc75c469343c89d07f893d786d72de4a16d5fe18 100644
--- a/source/RobotAPI/libraries/RTRobotUnit/test/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/test/CMakeLists.txt
@@ -1,4 +1,4 @@
-set(LIBS ${LIBS} RTRobotUnit )
+set(LIBS ${LIBS} RobotUnit )
armarx_add_test(BasicControllerTest BasicControllerTest.cpp "${LIBS}")
diff --git a/source/RobotAPI/components/units/RobotUnit/util.h b/source/RobotAPI/components/units/RobotUnit/util.h
new file mode 100644
index 0000000000000000000000000000000000000000..da93e8a7d7f4a61242a1e07314f97ae5de661833
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/util.h
@@ -0,0 +1,296 @@
+/*
+ * 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::RobotUnit
+ * @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_UNIT_RobotAPI_RobotUnit_util_H
+#define _ARMARX_UNIT_RobotAPI_RobotUnit_util_H
+
+#include <boost/units/detail/utility.hpp>
+#include <string>
+namespace armarx
+{
+ template<class T>
+ std::string getTypeName(const T&, bool withoutNamespaceSpecifier = false)
+ {
+ const std::string demangled = boost::units::detail::demangle(typeid(T).name());
+ if (withoutNamespaceSpecifier)
+ {
+ std::size_t substrStart = 0;
+ std::size_t level = 0;
+ for (int i = static_cast<int>(demangled.size() - 1); i >= 0; --i)
+ {
+
+ if (!level && demangled.at(i) == ':')
+ {
+ substrStart = i + 1;
+ break;
+ }
+ level += (demangled.at(i) == '>') - (demangled.at(i) == '<');
+ }
+ return demangled.substr(substrStart);
+ }
+ return demangled;
+ }
+}
+
+#include <string>
+
+namespace std
+{
+ inline const std::string& to_string(const std::string& s)
+ {
+ return s;
+ }
+ inline std::string to_string(std::string&& s)
+ {
+ return std::move(s);
+ }
+}
+
+#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
+#include <vector>
+#include <map>
+
+namespace armarx
+{
+ /**
+ * @brief This class is pretty much similar to a map.
+ *
+ * This class stores keys and values in two vectors and uses a map to map the keys to the index of the corresponding value
+ * This enables map acces and index access.
+ * The index of an inserted element never changes (this is different to a map).
+ * Index access may be required in a high frequency case (e.g. an rt control loop)
+ */
+ template < class KeyT, class ValT,
+ class KeyContT = std::vector<KeyT>,
+ class ValContT = std::vector<ValT>,
+ class IdxT = typename ValContT::size_type,
+ class MapT = std::map<KeyT, IdxT >>
+ class KeyValueVector
+ {
+ public:
+ void add(KeyT key, ValT value)
+ {
+ if (indices_.find(key) != indices_.end())
+ {
+ throw std::invalid_argument {"Already added a value for this key" + DescribeKey(&key)};
+ }
+ indices_.emplace(key, size());
+ values_.emplace_back(std::move(value));
+ keys_.emplace_back(std::move(key));
+ }
+ IdxT index(const KeyT& k) const
+ {
+ return indices_.at(k);
+ }
+ ValT& at(IdxT i)
+ {
+ return values_.at(i);
+ }
+ ValT const& at(IdxT i) const
+ {
+ return values_.at(i);
+ }
+ ValT& at(const KeyT& k)
+ {
+ return at(index(k));
+ }
+ ValT const& at(const KeyT& k) const
+ {
+ return at(index(k));
+ }
+ ValT& at(const KeyT& k, ValT& defaultVal)
+ {
+ return has(k) ? at(index(k)) : defaultVal;
+ }
+ ValT const& at(const KeyT& k, const ValT& defaultVal) const
+ {
+ return has(k) ? at(index(k)) : defaultVal;
+ }
+ IdxT size() const
+ {
+ return values_.size();
+ }
+ ValContT const& values() const
+ {
+ return values_;
+ }
+ MapT const& indices() const
+ {
+ return indices_;
+ }
+ KeyContT const& keys() const
+ {
+ return keys_;
+ }
+ IdxT count(const KeyT& k) const
+ {
+ return indices_.count(k);
+ }
+ bool has(const KeyT& k) const
+ {
+ return indices_.count(k);
+ }
+ void clear()
+ {
+ indices_.clear();
+ keys_.clear();
+ values_.clear();
+ }
+
+ private:
+ template<class T, class = typename std::enable_if<meta::HasToString<T>::value>::type>
+ static std::string DescribeKey(const T* key)
+ {
+ return ": " + std::to_string(*key);
+ }
+ static std::string DescribeKey(...)
+ {
+ return "";
+ }
+
+ MapT indices_;
+ KeyContT keys_;
+ ValContT values_;
+ };
+}
+
+#include <atomic>
+namespace armarx
+{
+ template <typename T>
+ struct AtomicWrapper
+ {
+ std::atomic<T> atom;
+
+ AtomicWrapper() : atom {} {}
+ AtomicWrapper(const T& v) : atom {v} {}
+ AtomicWrapper(const std::atomic<T>& a) : atom {a.load()} {}
+ AtomicWrapper(const AtomicWrapper& other) : atom {other.atom.load()} {}
+ AtomicWrapper(AtomicWrapper&&) = default;
+ AtomicWrapper& operator=(const T& v)
+ {
+ atom.store(v);
+ return *this;
+ }
+ AtomicWrapper& operator=(const std::atomic<T>& a)
+ {
+ atom.store(a.load());
+ return *this;
+ }
+ AtomicWrapper& operator=(const AtomicWrapper& other)
+ {
+ atom.store(other.atom.load());
+ return *this;
+ }
+ AtomicWrapper& operator=(AtomicWrapper &&) = default;
+
+ operator T() const
+ {
+ return atom.load();
+ }
+ };
+
+}
+
+#include <mutex>
+#include <thread>
+#include <ArmarXCore/core/logging/Logging.h>
+namespace armarx
+{
+ template<class MutexT>
+ struct messaging_unique_lock
+ {
+ using mutex_type = MutexT;
+ messaging_unique_lock(messaging_unique_lock&&) = default;
+ messaging_unique_lock(MutexT& mtx):
+ l {mtx, std::defer_lock},
+ m {&mtx}
+ {
+ bool isFree = l.try_lock();
+
+ if (isFree)
+ {
+ std::cout << "[thread " << std::this_thread::get_id() << "] locking mutex(" << m << ", already locked " << lock_count[m] << " times) (mutex was free) (type "
+ << getTypeName(mtx.native_handle()) << ") :\n"
+ << (lock_count[m] ? std::string {}: LogSender::CreateBackTrace()) << std::flush;
+ }
+ else
+ {
+ std::cout << "[thread " << std::this_thread::get_id() << "] locking mutex(" << m << ", already locked " << lock_count[m] << " times) (mutex was not free) (type "
+ << getTypeName(mtx.native_handle()) << ") :\n"
+ << (lock_count[m] ? std::string {}: LogSender::CreateBackTrace()) << std::flush;
+ l.lock();
+ std::cout << "[thread " << std::this_thread::get_id() << "] locking mutex(" << m << ", now locked " << lock_count[m] << " times) (mutex was not free in 10ms)...done" << std::flush;
+ }
+ ++lock_count[m];
+ }
+ ~messaging_unique_lock()
+ {
+ if (l.owns_lock())
+ {
+ --lock_count[m];
+ std::cout << "[thread " << std::this_thread::get_id() << "] unlocking mutex(" << m << ", still locked " << lock_count[m] << " times)" << std::flush;
+ }
+ }
+ std::unique_lock<MutexT> l;
+ const MutexT* const m;
+
+ static thread_local std::map<const MutexT*, std::size_t> lock_count;
+ };
+ template<class MutexT>
+ thread_local std::map<const MutexT*, std::size_t> messaging_unique_lock<MutexT>::lock_count {};
+}
+
+#define TYPEDEF_PTRS_SHARED(T) \
+ class T; \
+ using T##Ptr = std::shared_ptr<T>; \
+ using Const##T##Ptr = std::shared_ptr<const T>
+
+#define TYPEDEF_PTRS_HANDLE(T) \
+ class T; \
+ using T##Ptr = IceUtil::Handle<T>
+
+
+
+//#include <memory>
+//#include <boost/shared_ptr.hpp>
+//#include <IceUtil/Handle.h>
+//namespace armarx
+//{
+// template<class T>
+// class SharedPtr
+// {
+// public:
+
+// private:
+// struct SharedPtrInterface
+// {
+// void reset();
+// T* get();
+// const T* get();
+// };
+
+// instance
+// };
+//}
+
+#endif
diff --git a/source/RobotAPI/components/units/RobotUnit/util.h.orig b/source/RobotAPI/components/units/RobotUnit/util.h.orig
new file mode 100644
index 0000000000000000000000000000000000000000..facd776573ea9d5f8820b2475f5fd19b06da372a
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/util.h.orig
@@ -0,0 +1,301 @@
+/*
+ * 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::RobotUnit
+ * @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_UNIT_RobotAPI_RobotUnit_util_H
+#define _ARMARX_UNIT_RobotAPI_RobotUnit_util_H
+
+#include <boost/units/detail/utility.hpp>
+#include <string>
+namespace armarx
+{
+ template<class T>
+ std::string getTypeName(const T&, bool withoutNamespaceSpecifier = false)
+ {
+ const std::string demangled = boost::units::detail::demangle(typeid(T).name());
+ if (withoutNamespaceSpecifier)
+ {
+ std::size_t substrStart = 0;
+ std::size_t level = 0;
+ for (int i = static_cast<int>(demangled.size() - 1); i >= 0; --i)
+ {
+
+ if (!level && demangled.at(i) == ':')
+ {
+ substrStart = i + 1;
+ break;
+ }
+ level += (demangled.at(i) == '>') - (demangled.at(i) == '<');
+ }
+ return demangled.substr(substrStart);
+ }
+ return demangled;
+ }
+}
+
+#include <string>
+
+namespace std
+{
+ inline const std::string& to_string(const std::string& s)
+ {
+ return s;
+ }
+ inline std::string to_string(std::string&& s)
+ {
+ return std::move(s);
+ }
+}
+
+#include <ArmarXCore/core/util/TemplateMetaProgramming.h>
+#include <vector>
+#include <map>
+
+namespace armarx
+{
+ /**
+ * @brief This class is pretty much similar to a map.
+ *
+ * This class stores keys and values in two vectors and uses a map to map the keys to the index of the corresponding value
+ * This enables map acces and index access.
+ * The index of an inserted element never changes (this is different to a map).
+ * Index access may be required in a high frequency case (e.g. an rt control loop)
+ */
+ template < class KeyT, class ValT,
+ class KeyContT = std::vector<KeyT>,
+ class ValContT = std::vector<ValT>,
+ class IdxT = typename ValContT::size_type,
+ class MapT = std::map<KeyT, IdxT >>
+ class KeyValueVector
+ {
+ public:
+ void add(KeyT key, ValT value)
+ {
+ if (indices_.find(key) != indices_.end())
+ {
+ throw std::invalid_argument {"Already added a value for this key" + describeKey(&key)};
+ }
+ indices_.emplace(key, size());
+ values_.emplace_back(std::move(value));
+ keys_.emplace_back(std::move(key));
+ }
+ IdxT index(const KeyT& k) const
+ {
+ return indices_.at(k);
+ }
+ ValT& at(IdxT i)
+ {
+ return values_.at(i);
+ }
+ ValT const& at(IdxT i) const
+ {
+ return values_.at(i);
+ }
+ ValT& at(const KeyT& k)
+ {
+ return at(index(k));
+ }
+ ValT const& at(const KeyT& k) const
+ {
+ return at(index(k));
+ }
+ ValT& at(const KeyT& k, ValT& defaultVal)
+ {
+ return has(k) ? at(index(k)) : defaultVal;
+ }
+ ValT const& at(const KeyT& k, const ValT& defaultVal) const
+ {
+ return has(k) ? at(index(k)) : defaultVal;
+ }
+ IdxT size() const
+ {
+ return values_.size();
+ }
+ ValContT const& values() const
+ {
+ return values_;
+ }
+ MapT const& indices() const
+ {
+ return indices_;
+ }
+ KeyContT const& keys() const
+ {
+ return keys_;
+ }
+ IdxT count(const KeyT& k) const
+ {
+ return indices_.count(k);
+ }
+ bool has(const KeyT& k) const
+ {
+ return indices_.count(k);
+ }
+ void clear()
+ {
+ indices_.clear();
+ keys_.clear();
+ values_.clear();
+ }
+
+ private:
+ template<class T, class = typename std::enable_if<meta::HasToString<T>::value>::type>
+ static std::string describeKey(const T* key)
+ {
+ return ": " + std::to_string(*key);
+ }
+ static std::string describeKey(...)
+ {
+ return "";
+ }
+
+ MapT indices_;
+ KeyContT keys_;
+ ValContT values_;
+ };
+}
+
+#include <atomic>
+namespace armarx
+{
+ template <typename T>
+ struct AtomicWrapper
+ {
+ std::atomic<T> atom;
+
+ AtomicWrapper() : atom {} {}
+ AtomicWrapper(const T& v) : atom {v} {}
+ AtomicWrapper(const std::atomic<T>& a) : atom {a.load()} {}
+ AtomicWrapper(const AtomicWrapper& other) : atom {other.atom.load()} {}
+ AtomicWrapper(AtomicWrapper&&) = default;
+ AtomicWrapper& operator=(const T& v)
+ {
+ atom.store(v);
+ return *this;
+ }
+ AtomicWrapper& operator=(const std::atomic<T>& a)
+ {
+ atom.store(a.load());
+ return *this;
+ }
+ AtomicWrapper& operator=(const AtomicWrapper& other)
+ {
+ atom.store(other.atom.load());
+ return *this;
+ }
+ AtomicWrapper& operator=(AtomicWrapper &&) = default;
+
+ operator T() const
+ {
+ return atom.load();
+ }
+ };
+
+}
+
+#include <mutex>
+#include <thread>
+#include <ArmarXCore/core/logging/Logging.h>
+namespace armarx
+{
+ template<class MutexT>
+ struct messaging_unique_lock
+ {
+ using mutex_type = MutexT;
+ messaging_unique_lock(messaging_unique_lock&&) = default;
+ messaging_unique_lock(MutexT& mtx):
+ l {mtx, std::defer_lock},
+ m {&mtx}
+ {
+<<<<<<< HEAD
+ bool isFree = l.try_lock();
+
+ if (isFree)
+ {
+ std::cout << "[thread " << std::this_thread::get_id() << "] locking mutex(" << m << ", already locked " << lock_count[m] << " times) (mutex was free) (type "
+ << getTypeName(mtx.native_handle()) << ") :\n"
+ << (lock_count[m] ? std::string {}: LogSender::CreateBackTrace()) << std::flush;
+ }
+ else
+ {
+ std::cout << "[thread " << std::this_thread::get_id() << "] locking mutex(" << m << ", already locked " << lock_count[m] << " times) (mutex was not free) (type "
+ << getTypeName(mtx.native_handle()) << ") :\n"
+ << (lock_count[m] ? std::string {}: LogSender::CreateBackTrace()) << std::flush;
+ l.lock();
+ std::cout << "[thread " << std::this_thread::get_id() << "] locking mutex(" << m << ", now locked " << lock_count[m] << " times) (mutex was not free in 10ms)...done" << std::flush;
+ }
+=======
+std::cout << "[thread " << std::this_thread::get_id() << "] locking mutex(" << m << ", already locked " << lock_count[m] << " times):\n" << (lock_count[m] ? std::string{}: LogSender::CreateBackTrace()) << std::flush;
+ l.lock();
+>>>>>>> 16039a95e1fda778a127463d88c6c4d36ce40c07
+ ++lock_count[m];
+ }
+ ~messaging_unique_lock()
+ {
+ if (l.owns_lock())
+ {
+ --lock_count[m];
+ std::cout << "[thread " << std::this_thread::get_id() << "] unlocking mutex(" << m << ", still locked " << lock_count[m] << " times)" << std::flush;
+ }
+ }
+ std::unique_lock<MutexT> l;
+ const MutexT* const m;
+
+ static thread_local std::map<const MutexT*, std::size_t> lock_count;
+ };
+ template<class MutexT>
+ thread_local std::map<const MutexT*, std::size_t> messaging_unique_lock<MutexT>::lock_count {};
+}
+
+#define TYPEDEF_PTRS_SHARED(T) \
+ class T; \
+ using T##Ptr = std::shared_ptr<T>; \
+ using Const##T##Ptr = std::shared_ptr<const T>
+
+#define TYPEDEF_PTRS_HANDLE(T) \
+ class T; \
+ using T##Ptr = IceUtil::Handle<T>
+
+
+
+//#include <memory>
+//#include <boost/shared_ptr.hpp>
+//#include <IceUtil/Handle.h>
+//namespace armarx
+//{
+// template<class T>
+// class SharedPtr
+// {
+// public:
+
+// private:
+// struct SharedPtrInterface
+// {
+// void reset();
+// T* get();
+// const T* get();
+// };
+
+// instance
+// };
+//}
+
+#endif
diff --git a/source/RobotAPI/interface/CMakeLists.txt b/source/RobotAPI/interface/CMakeLists.txt
index 73a7048e0d97cd2235661f12c6b1623965e4f464..994893ab9bec42af1920282536410329be80ffac 100644
--- a/source/RobotAPI/interface/CMakeLists.txt
+++ b/source/RobotAPI/interface/CMakeLists.txt
@@ -2,7 +2,7 @@
### CMakeLists.txt file for ArmarX Interfaces
###
-set(ROBOTAPI_INTERFACE_DEPEND ArmarXCore)
+set(ROBOTAPI_INTERFACE_DEPEND ArmarXCore ArmarXGui)
set(SLICE_FILES
observers/KinematicUnitObserverInterface.ice
@@ -40,16 +40,14 @@ set(SLICE_FILES
units/GamepadUnit.ice
units/MetaWearIMUInterface.ice
units/MetaWearIMU.ice
+ units/RobotUnit/LVL1Controller.ice
+ units/RobotUnit/LVL1TrajectoryController.ice
+ units/RobotUnit/RobotUnitInterface.ice
components/ViewSelectionInterface.ice
visualization/DebugDrawerInterface.ice
- libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.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
deleted file mode 100644
index e3256ae867ec27d348d0e75d03dae53cf3df6f63..0000000000000000000000000000000000000000
--- a/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughControllerConfig.ice
+++ /dev/null
@@ -1,44 +0,0 @@
-/*
- * 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;
- };
-
- interface LVL1HolonomicPlatformVelocityPassThroughControllerInterface extends LVL1ControllerInterface
- {
- idempotent void setVelocites(float velocityX, float velocityY, float velocityRotation);
- };
-};
-#endif
diff --git a/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitControllerConfigs.ice b/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitControllerConfigs.ice
deleted file mode 100644
index cd0b074c863ce35d7ef68a831dd596fc661a38f3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitControllerConfigs.ice
+++ /dev/null
@@ -1,71 +0,0 @@
-/*
- * 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/LVL1PassThroughController.ice b/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.ice
deleted file mode 100644
index e997918d3224c208a16d66cd5c38fa81a9f2e6fb..0000000000000000000000000000000000000000
--- a/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.ice
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * 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::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_LVL1PassThroughController_SLICE_
-#define _ARMARX_ROBOTAPI_LVL1PassThroughController_SLICE_
-
-#include <ArmarXCore/interface/core/BasicTypes.ice>
-#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.ice>
-
-module armarx
-{
- class LVL1PassThroughControllerConfig extends LVL1ControllerConfig
- {
- Ice::StringSeq jointNames;
- };
-
- interface LVL1PassThroughControllerInterface extends LVL1ControllerInterface
- {
- idempotent void setJoint(string joint, float value) throws InvalidArgumentException;
- idempotent void setJoints(StringFloatDictionary values) throws InvalidArgumentException;
- };
-};
-#endif
diff --git a/source/RobotAPI/interface/libraries/RTRobotUnit/RobotUnitInterface.ice b/source/RobotAPI/interface/libraries/RTRobotUnit/RobotUnitInterface.ice
deleted file mode 100644
index 2435b80f1ff44ecb4825028b07dcd79adf12073f..0000000000000000000000000000000000000000
--- a/source/RobotAPI/interface/libraries/RTRobotUnit/RobotUnitInterface.ice
+++ /dev/null
@@ -1,77 +0,0 @@
-/*
- * 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::RobotUnit
- * @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_UNITS_RobotUnit_SLICE_
-#define _ARMARX_ROBOTAPI_UNITS_RobotUnit_SLICE_
-
-#include <ArmarXCore/interface/core/UserException.ice>
-
-#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.ice>
-
-#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
-#include <RobotAPI/interface/units/ForceTorqueUnit.ice>
-#include <RobotAPI/interface/units/InertialMeasurementUnit.ice>
-#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
-
-module armarx
-{
- dictionary<string, Ice::StringSeq> JointNameToControlModesDictionary;
-
- dictionary<string, string> JointNameToLVL1Dictionary;
-
- interface RobotUnitInterface
- {
- //controllers
- //names
- ["cpp:const"] idempotent Ice::StringSeq getControllersKnown();
- ["cpp:const"] idempotent Ice::StringSeq getControllerNamesLoaded();
- ["cpp:const"] idempotent Ice::StringSeq getControllerNamesRequested();
- ["cpp:const"] idempotent Ice::StringSeq getControllerNamesActivated();
- //proxy
- ["cpp:const"] idempotent StringLVL1ControllerPrxDictionary getControllersLoaded();
- ["cpp:const"] idempotent StringLVL1ControllerPrxDictionary getControllersRequested();
- ["cpp:const"] idempotent StringLVL1ControllerPrxDictionary getControllersActivated();
- //assignement
- ["cpp:const"] idempotent JointNameToLVL1Dictionary getControllerJointAssignment();
-
- //modes
- ["cpp:const"] idempotent JointNameToControlModeDictionary getJointControlModesRequested();
- ["cpp:const"] idempotent JointNameToControlModeDictionary getJointControlModesActivated();
- ["cpp:const"] idempotent JointNameToControlModesDictionary getJointControlModesSupported();
-
- //loading
- void switchSetup(Ice::StringSeq controllerInstanceNames) throws InvalidArgumentException;
- LVL1ControllerInterface* loadController(string className, string instanceName, LVL1ControllerConfig config) throws InvalidArgumentException;
-
- bool loadLibFromPath(string path);
- bool loadLibFromPackage(string package, string lib);
-
-
- //units
- ["cpp:const"] idempotent KinematicUnitInterface* getKinematicUnit();
- ["cpp:const"] idempotent ForceTorqueUnitInterface* getForceTorqueUnit();
- ["cpp:const"] idempotent InertialMeasurementUnitInterface* getInertialMeasurementUnit();
- ["cpp:const"] idempotent PlatformUnitInterface* getPlatformUnitInterface();
- };
-};
-
-#endif
diff --git a/source/RobotAPI/interface/units/RobotUnit/LVL1Controller.ice b/source/RobotAPI/interface/units/RobotUnit/LVL1Controller.ice
new file mode 100644
index 0000000000000000000000000000000000000000..a2e0f286a83b2e34cf9d9a60fea37b5991b67113
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/LVL1Controller.ice
@@ -0,0 +1,85 @@
+/*
+ * 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::LVL1ControllerInterface
+ * @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_LVL1ControllerInterface_SLICE_
+#define _ARMARX_ROBOTAPI_LVL1ControllerInterface_SLICE_
+
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+#include <ArmarXCore/interface/core/UserException.ice>
+#include <ArmarXGui/interface/WidgetDescription.ice>
+
+module armarx
+{
+ interface LVL1ControllerInterface;
+ interface RobotUnitInterface;
+
+ class LVL1ControllerConfig{};
+
+ struct LVL1ControllerDescription
+ {
+ string instanceName;
+ string className;
+ LVL1ControllerInterface* controller;
+ StringStringDictionary controlModeAssignment;
+ };
+ sequence<LVL1ControllerDescription> LVL1ControllerDescriptionSeq;
+
+ struct LVL1ControllerStatus
+ {
+ string instanceName;
+ bool active = false;
+ bool requested = false;
+ bool error = false;
+ long timestampUSec = 0;
+ };
+ sequence<LVL1ControllerStatus> LVL1ControllerStatusSeq;
+
+ struct LVL1ControllerDescriptionWithStatus
+ {
+ LVL1ControllerStatus status;
+ LVL1ControllerDescription description;
+ };
+ sequence<LVL1ControllerDescriptionWithStatus> LVL1ControllerDescriptionWithStatusSeq;
+
+ interface LVL1ControllerInterface
+ {
+ ["cpp:const"] idempotent string getClassName();
+ ["cpp:const"] idempotent string getInstanceName();
+ ["cpp:const"] idempotent StringStringDictionary getControlDeviceUsedControlModeMap();
+ ["cpp:const"] idempotent bool isControllerActive();
+ ["cpp:const"] idempotent bool isControllerRequested();
+ ["cpp:const"] idempotent bool hasControllerError();
+ ["cpp:const"] idempotent LVL1ControllerStatus getControllerStatus();
+ ["cpp:const"] idempotent LVL1ControllerDescription getControllerDescription();
+ ["cpp:const"] idempotent LVL1ControllerDescriptionWithStatus getControllerDescriptionWithStatus();
+ ["cpp:const"] idempotent RobotUnitInterface* getRobotUnit();
+
+ ["cpp:const"] void activateController();
+ ["cpp:const"] void deactivateController();
+
+ ["cpp:const"] idempotent WidgetDescription::StringWidgetDictionary getFunctionDescriptions();
+ void callDescribedFunction(string fuinctionName, StringVariantBaseMap values) throws InvalidArgumentException;
+ };
+
+ dictionary<string, LVL1ControllerInterface*> StringLVL1ControllerPrxDictionary;
+};
+#endif
diff --git a/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.ice b/source/RobotAPI/interface/units/RobotUnit/LVL1TrajectoryController.ice
similarity index 52%
rename from source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.ice
rename to source/RobotAPI/interface/units/RobotUnit/LVL1TrajectoryController.ice
index a229edc6ead598adad1cf61d0af6f630b616a389..47b8383bc2c725b6f5ea3a3ac70ee74f768ae6d7 100644
--- a/source/RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1Controller.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/LVL1TrajectoryController.ice
@@ -14,32 +14,35 @@
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package RobotAPI::LVL1ControllerInterface
- * @author Raphael Grimm ( raphael dot grimm at student dot kit dot edu )
+ * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
* @date 2017
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
-#ifndef _ARMARX_ROBOTAPI_LVL1ControllerInterface_SLICE_
-#define _ARMARX_ROBOTAPI_LVL1ControllerInterface_SLICE_
+#ifndef _ARMARX_ROBOTAPI_LVL1TrajectoryControllerInterface_SLICE_
+#define _ARMARX_ROBOTAPI_LVL1TrajectoryControllerInterface_SLICE_
-#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
+#include <RobotAPI/interface/units/RobotUnit/LVL1Controller.ice>
+#include <RobotAPI/interface/core/Trajectory.ice>
module armarx
{
- dictionary<string, string> JointNameToControlModeDictionary;
- class LVL1ControllerConfig
+ class LVL1TrajectoryControllerConfig extends LVL1ControllerConfig
{
+ float maxAcceleration = 3;
+ float maxVelocity = 1.4;
+ float maxDeviation = 5;
+ float preSamplingStepMs = 10;
+ Ice::StringSeq jointNames;
+ float PID_p = 1;
+ float PID_i = 0;
+ float PID_d = 0;
};
- interface LVL1ControllerInterface
+ interface LVL1TrajectoryControllerInterface extends LVL1ControllerInterface
{
- ["cpp:const"] idempotent string getClassName();
- ["cpp:const"] idempotent string getInstanceName();
- ["cpp:const"] idempotent JointNameToControlModeDictionary getJointControlModeMap();
- ["cpp:const"] idempotent bool isControllerActive();
+ void setTrajectory(TrajectoryBase traj);
};
-
- dictionary<string, LVL1ControllerInterface*> StringLVL1ControllerPrxDictionary;
};
#endif
diff --git a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
new file mode 100644
index 0000000000000000000000000000000000000000..01db232c7530b245c841c575f0b1f2183e16e96e
--- /dev/null
+++ b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
@@ -0,0 +1,160 @@
+/*
+ * 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::RobotUnit
+ * @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_UNITS_RobotUnit_SLICE_
+#define _ARMARX_ROBOTAPI_UNITS_RobotUnit_SLICE_
+
+#include <ArmarXCore/interface/core/UserException.ice>
+#include <ArmarXCore/interface/core/BasicTypes.ice>
+
+#include <ArmarXGui/interface/WidgetDescription.ice>
+
+#include <RobotAPI/interface/units/RobotUnit/LVL1Controller.ice>
+
+#include <RobotAPI/interface/units/KinematicUnitInterface.ice>
+#include <RobotAPI/interface/units/ForceTorqueUnit.ice>
+#include <RobotAPI/interface/units/InertialMeasurementUnit.ice>
+#include <RobotAPI/interface/units/PlatformUnitInterface.ice>
+
+module armarx
+{
+ dictionary<string, Ice::StringSeq> ControlDeviceNameToControlModesDictionary;
+
+ dictionary<string, string> ControlDeviceNameToLVL1ControllerNameDictionary;
+
+ struct ControlDeviceDescription
+ {
+ string deviceName;
+ StringStringDictionary contolModeToTargetType;
+ Ice::StringSeq tags;
+ };
+ sequence<ControlDeviceDescription> ControlDeviceDescriptionSeq;
+ struct ControlDeviceStatus
+ {
+ string deviceName;
+ string activeControlMode;
+ string requestedControlMode;
+ StringToStringVariantBaseMapMap controlTargetValues;
+ long timestampUSec = 0;
+ };
+ sequence<ControlDeviceStatus> ControlDeviceStatusSeq;
+
+
+ struct SensorDeviceDescription
+ {
+ string deviceName;
+ string sensorValueType;
+ Ice::StringSeq tags;
+ };
+ sequence<SensorDeviceDescription> SensorDeviceDescriptionSeq;
+ struct SensorDeviceStatus
+ {
+ string deviceName;
+ StringVariantBaseMap sensorValue;
+ long timestampUSec = 0;
+ };
+ sequence<SensorDeviceStatus> SensorDeviceStatusSeq;
+
+
+ struct LVL1ControllerClassDescription
+ {
+ string className;
+ WidgetDescription::Widget configDescription;
+ };
+ sequence<LVL1ControllerClassDescription> LVL1ControllerClassDescriptionSeq;
+
+ interface RobotUnitInterface
+ {
+ //controllers
+ //names
+ ["cpp:const"] idempotent Ice::StringSeq getLVL1ControllerNames();
+ ["cpp:const"] idempotent Ice::StringSeq getRequestedLVL1ControllerNames();
+ ["cpp:const"] idempotent Ice::StringSeq getActivatedLVL1ControllerNames();
+ //proxy/information
+ ["cpp:const"] idempotent LVL1ControllerInterface* getLVL1Controller(string name);
+ ["cpp:const"] idempotent StringLVL1ControllerPrxDictionary getAllLVL1Controllers();
+
+ ["cpp:const"] idempotent LVL1ControllerStatus getLVL1ControllerStatus(string name) throws InvalidArgumentException;
+ ["cpp:const"] idempotent LVL1ControllerStatusSeq getLVL1ControllerStatuses();
+
+ ["cpp:const"] idempotent LVL1ControllerDescription getLVL1ControllerDescription(string name) throws InvalidArgumentException;
+ ["cpp:const"] idempotent LVL1ControllerDescriptionSeq getLVL1ControllerDescriptions();
+
+ ["cpp:const"] idempotent LVL1ControllerDescriptionWithStatus getLVL1ControllerDescriptionWithStatus(string name) throws InvalidArgumentException;
+ ["cpp:const"] idempotent LVL1ControllerDescriptionWithStatusSeq getLVL1ControllerDescriptionsWithStatuses();
+
+ //devices
+ ["cpp:const"] idempotent Ice::StringSeq getControlDeviceNames() throws LogicError;
+ ["cpp:const"] idempotent ControlDeviceDescription getControlDeviceDescription(string name) throws InvalidArgumentException, LogicError;
+ ["cpp:const"] idempotent ControlDeviceDescriptionSeq getControlDeviceDescriptions() throws LogicError;
+ ["cpp:const"] idempotent ControlDeviceStatus getControlDeviceStatus(string name) throws InvalidArgumentException, LogicError;
+ ["cpp:const"] idempotent ControlDeviceStatusSeq getControlDeviceStatuses() throws LogicError;
+
+ ["cpp:const"] idempotent Ice::StringSeq getSensorDeviceNames() throws LogicError;
+ ["cpp:const"] idempotent SensorDeviceDescription getSensorDeviceDescription(string name) throws InvalidArgumentException, LogicError;
+ ["cpp:const"] idempotent SensorDeviceDescriptionSeq getSensorDeviceDescriptions() throws LogicError;
+ ["cpp:const"] idempotent SensorDeviceStatus getSensorDeviceStatus(string name) throws InvalidArgumentException, LogicError;
+ ["cpp:const"] idempotent SensorDeviceStatusSeq getSensorDeviceStatuses() throws LogicError;
+ //classes
+ ["cpp:const"] idempotent Ice::StringSeq getLVL1ControllerClassNames();
+ ["cpp:const"] idempotent LVL1ControllerClassDescription getLVL1ControllerClassDescription(string name) throws InvalidArgumentException;
+ ["cpp:const"] idempotent LVL1ControllerClassDescriptionSeq getLVL1ControllerClassDescriptions();
+
+ //switching
+ void switchControllerSetup(Ice::StringSeq newSetup) throws InvalidArgumentException, LogicError;
+
+ void activateController(string controllerInstanceName) throws InvalidArgumentException, LogicError;
+ void activateControllers(Ice::StringSeq controllerInstanceNames) throws InvalidArgumentException, LogicError;
+ void deactivateController(string controllerInstanceName)throws InvalidArgumentException, LogicError;
+ void deactivateControllers(Ice::StringSeq controllerInstanceNames)throws InvalidArgumentException, LogicError;
+
+ //creting controllers
+ LVL1ControllerInterface* createController(string className, string instanceName, LVL1ControllerConfig config) throws InvalidArgumentException, LogicError;
+ LVL1ControllerInterface* createControllerFromVariantConfig(string className, string instanceName, StringVariantBaseMap config) throws InvalidArgumentException, LogicError;
+
+ //loading
+ bool loadLibFromPath(string path);
+ bool loadLibFromPackage(string package, string libname);
+
+ //units
+ ["cpp:const"] idempotent Object* getUnit(string staticIceId);
+ ["cpp:const"] idempotent Ice::ObjectProxySeq getUnits();
+ ["cpp:const"] idempotent KinematicUnitInterface* getKinematicUnit();
+ ["cpp:const"] idempotent ForceTorqueUnitInterface* getForceTorqueUnit();
+ ["cpp:const"] idempotent InertialMeasurementUnitInterface* getInertialMeasurementUnit();
+ ["cpp:const"] idempotent PlatformUnitInterface* getPlatformUnit();
+
+ //other
+ ["cpp:const"] idempotent string getListenerTopicName();
+ };
+
+ interface RobotUnitListener
+ {
+ void lVl1ControllerStatusChanged(LVL1ControllerStatus status);
+ void controlDeviceStatusChanged(ControlDeviceStatus status);
+ void sensorDeviceStatusChanged(SensorDeviceStatus status);
+ void lvl1ControllerClassAdded(string className);
+ void lvl1ControllerAdded(string instanceName);
+ };
+};
+
+#endif
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index d7fc2aeeecee68490f37a56ee67cc042f554a638..10a98da381b0b0b4da90b92fdecfad188f08a1e7 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -1,4 +1,4 @@
add_subdirectory(core)
add_subdirectory(widgets)
-add_subdirectory(RTRobotUnit)
+add_subdirectory(RobotAPIVariantWidget)
diff --git a/source/RobotAPI/libraries/RTRobotUnit/BasicControllers.cpp b/source/RobotAPI/libraries/RTRobotUnit/BasicControllers.cpp
deleted file mode 100644
index 9be5c8bd3aebbb90d7aa637fe407e639f365dff1..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/BasicControllers.cpp
+++ /dev/null
@@ -1,310 +0,0 @@
-/*
- * 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
deleted file mode 100644
index 9fb5ae18389a76c6841e410e6bee1856f55c11f6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/BasicControllers.h
+++ /dev/null
@@ -1,116 +0,0 @@
-/*
- * 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
deleted file mode 100644
index eeda8773d2a3124e0af4b9c0519fe2d3e7f8b97b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/CMakeLists.txt
+++ /dev/null
@@ -1,61 +0,0 @@
-set(LIB_NAME RTRobotUnit)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-set(LIBS
- ArmarXCoreInterfaces
- ArmarXCore
- RobotAPIUnits
- RobotAPIInterfaces
-)
-
-set(LIB_FILES
- SyntaxCheck.cpp
-
- LVL0Controllers/LVL0Controller.cpp
-
- LVL1Controllers/LVL1Controller.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/ActuatorPositionTarget.h
- Targets/ActuatorVelocityTarget.h
- Targets/ActuatorTorqueTarget.h
- Targets/PlatformWheelVelocityTarget.h
- Targets/HolonomicPlatformVelocityTarget.h
-
- DataUnits/ForceTorqueDataUnit.h
- DataUnits/HapticDataUnit.h
- DataUnits/IMUDataUnit.h
- DataUnits/KinematicDataUnit.h
- DataUnits/PlatformDataUnit.h
-
- LVL0Controllers/LVL0Controller.h
-
- LVL1Controllers/LVL1Controller.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/DataUnits/KinematicDataUnit.h b/source/RobotAPI/libraries/RTRobotUnit/DataUnits/KinematicDataUnit.h
deleted file mode 100644
index fb5c6851f10434f6d54a6c7e4730df5f4b928d76..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/DataUnits/KinematicDataUnit.h
+++ /dev/null
@@ -1,232 +0,0 @@
-/*
- * 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::KinematicDataUnit
- * @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_KinematicDataUnit_H
-#define _ARMARX_LIB_RobotAPI_KinematicDataUnit_H
-
-#include <vector>
-#include <string>
-#include <unordered_map>
-#include <algorithm>
-
-#include <ArmarXCore/core/util/algorithm.h>
-
-namespace armarx
-{
- class KinematicDataUnitInterface
- {
- public:
- virtual const std::vector<std::string>& getJointNames() const = 0;
-
- virtual std::vector<const float*> getJointAngles(const std::vector<std::string>& joints) const = 0;
-
- virtual std::vector<const float*> getJointVelocities(const std::vector<std::string>& joints) const = 0;
-
- virtual std::vector<const float*> getJointTorques(const std::vector<std::string>& joints) const = 0;
-
- virtual std::vector<const float*> getJointCurrents(const std::vector<std::string>& joints) const = 0;
-
- virtual std::vector<const float*> getJointMotorTemperatures(const std::vector<std::string>& joints) const = 0;
-
- virtual std::vector<const float*> getJointGearTemperatures(const std::vector<std::string>& joints) const = 0;
-
- virtual std::vector<const int32_t*>
- getJointRawPositionValues(const std::vector<std::string>& joints) const = 0;
-
- virtual std::vector<const int32_t*>
- getJointRawVelocityValues(const std::vector<std::string>& joints) const = 0;
-
- virtual std::vector<const int16_t*> getJointRawCurrentValues(const std::vector<std::string>& joints) const = 0;
-
- virtual std::vector<const int32_t*> getJointRawTorqueValues(const std::vector<std::string>& joints) const = 0;
-
- virtual std::vector<const int32_t*>
- getJointRawGearTemperatures(const std::vector<std::string>& joints) const = 0;
-
- virtual std::vector<const int32_t*>
- getJointRawAbsPositionValues(const std::vector<std::string>& joints) const = 0;
-
- virtual std::vector<const int16_t*>
- getJointRawMotorTemperatureValues(const std::vector<std::string>& joints) const = 0;
- };
-
- class KinematicDataUnitPtrProvider : virtual public KinematicDataUnitInterface
- {
- public:
- KinematicDataUnitPtrProvider(const std::vector<std::string>& jointNames) :
- jointAngles(jointNames.size(), nullptr),
- jointVelocites(jointNames.size(), nullptr),
- jointTorques(jointNames.size(), nullptr),
- jointCurrents(jointNames.size(), nullptr),
- jointMotorTemperatures(jointNames.size(), nullptr),
- jointGearTemperatures(jointNames.size(), nullptr),
- jointAbsPositions(jointNames.size(), nullptr),
- jointRawPositionValues(jointNames.size(), nullptr),
- jointRawVelocityValues(jointNames.size(), nullptr),
- jointRawCurrentValues(jointNames.size(), nullptr),
- jointRawTorqueValues(jointNames.size(), nullptr),
- jointRawGearTemperatures(jointNames.size(), nullptr),
- jointRawAbsPositionValues(jointNames.size(), nullptr),
- jointRawMotorTemperatureValues(jointNames.size(), nullptr),
- jointNames {jointNames},
- jointIndices {toIndexMap(jointNames)} {}
-
- KinematicDataUnitPtrProvider() = default;
-
- KinematicDataUnitPtrProvider(const KinematicDataUnitPtrProvider&) = default;
-
- void setJointNames(std::vector<std::string>& names)
- {
- jointNames = names;
- jointIndices = toIndexMap(names);
-
- jointAngles = std::vector<float*>(names.size(), nullptr);
- jointVelocites = std::vector<float*>(jointNames.size(), nullptr);
- jointTorques = std::vector<float*>(jointNames.size(), nullptr);
- jointCurrents = std::vector<float*>(jointNames.size(), nullptr);
- jointMotorTemperatures = std::vector<float*>(jointNames.size(), nullptr);
- jointGearTemperatures = std::vector<float*>(jointNames.size(), nullptr);
- jointAbsPositions = std::vector<float*>(jointNames.size(), nullptr);
- }
-
- const std::vector<std::string>& getJointNames() const
- {
- return jointNames;
- }
-
- std::size_t getJointIndex(const std::string& joint) const
- {
- return jointIndices.at(joint);
- }
-
- virtual std::vector<const float*> getJointAngles(const std::vector<std::string>& joints) const
- {
- return getPointers(joints, jointAngles);
- }
-
- virtual std::vector<const float*> getJointVelocities(const std::vector<std::string>& joints) const
- {
- return getPointers(joints, jointVelocites);
- }
-
- virtual std::vector<const float*> getJointTorques(const std::vector<std::string>& joints) const
- {
- return getPointers(joints, jointTorques);
- }
-
- virtual std::vector<const float*> getJointCurrents(const std::vector<std::string>& joints) const
- {
- return getPointers(joints, jointCurrents);
- }
-
- virtual std::vector<const float*> getJointMotorTemperatures(const std::vector<std::string>& joints) const
- {
- return getPointers(joints, jointMotorTemperatures);
- }
-
- virtual std::vector<const float*> getJointGearTemperatures(const std::vector<std::string>& joints) const
- {
- return getPointers(joints, jointGearTemperatures);
- }
-
- virtual std::vector<const float*> getJointAbsPositions(const std::vector<std::string>& joints) const
- {
- return getPointers(joints, jointAbsPositions);
- }
-
- virtual std::vector<const int32_t*> getJointRawPositionValues(const std::vector<std::string>& joints) const
- {
- return getPointers(joints, jointRawPositionValues);
- }
-
- virtual std::vector<const int32_t*> getJointRawVelocityValues(const std::vector<std::string>& joints) const
- {
- return getPointers(joints, jointRawVelocityValues);
- }
-
- virtual std::vector<const int16_t*> getJointRawCurrentValues(const std::vector<std::string>& joints) const
- {
- return getPointers(joints, jointRawCurrentValues);
- }
-
- virtual std::vector<const int32_t*> getJointRawTorqueValues(const std::vector<std::string>& joints) const
- {
- return getPointers(joints, jointRawTorqueValues);
- }
-
- virtual std::vector<const int32_t*> getJointRawGearTemperatures(const std::vector<std::string>& joints) const
- {
- return getPointers(joints, jointRawGearTemperatures);
- }
-
- virtual std::vector<const int32_t*>
- getJointRawAbsPositionValues(const std::vector<std::string>& joints) const
- {
- return getPointers(joints, jointRawAbsPositionValues);
- }
-
- virtual std::vector<const int16_t*>
- getJointRawMotorTemperatureValues(const std::vector<std::string>& joints) const
- {
- return getPointers(joints, jointRawMotorTemperatureValues);
- }
-
-
- protected:
- std::vector<float*> jointAngles;
- std::vector<float*> jointVelocites;
- std::vector<float*> jointTorques;
- std::vector<float*> jointCurrents;
- std::vector<float*> jointMotorTemperatures;
- std::vector<float*> jointGearTemperatures;
- std::vector<float*> jointAbsPositions;
-
- std::vector<int32_t*> jointRawPositionValues;
- std::vector<int32_t*> jointRawVelocityValues;
- std::vector<int16_t*> jointRawCurrentValues;
- std::vector<int32_t*> jointRawTorqueValues;
- std::vector<int32_t*> jointRawGearTemperatures;
- std::vector<int32_t*> jointRawAbsPositionValues;
- std::vector<int16_t*> jointRawMotorTemperatureValues;
- private:
- template<class T>
- std::vector<const T*>
- getPointers(const std::vector<std::string>& joints, const std::vector<T*>& targets) const
- {
- std::vector<const T*> result;
- result.reserve(joints.size());
- std::transform(
- joints.begin(), joints.end(), std::back_inserter(result),
- [targets, this](const std::string & joint)
- {
- return targets.at(getJointIndex(joint));
- }
- );
- return result;
- }
-
- std::vector<std::string> jointNames;
- std::unordered_map<std::string, std::size_t> jointIndices;
- };
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/DataUnits/PlatformDataUnit.h b/source/RobotAPI/libraries/RTRobotUnit/DataUnits/PlatformDataUnit.h
deleted file mode 100644
index 0f965ade3d392ea3ee1d6ab7b886f67a93fd142e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/DataUnits/PlatformDataUnit.h
+++ /dev/null
@@ -1,88 +0,0 @@
-/*
- * 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::PlatformDataUnit
- * @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_PlatformDataUnit_H
-#define _ARMARX_LIB_RobotAPI_PlatformDataUnit_H
-
-namespace armarx
-{
- class PlatformDataUnitInterface
- {
- public:
- virtual const float* getFrontRightWheelVelocity() const = 0;
- virtual const float* getFrontLeftWheelVelocity() const = 0;
- virtual const float* getRearRightWheelVelocity() const = 0;
- virtual const float* getRearLeftWheelVelocity() const = 0;
- };
-
- class PlatformDataUnitPtrProvider : public virtual PlatformDataUnitInterface
- {
- public:
- const float* getFrontRightWheelVelocity() const override
- {
- return frontRightVelocity;
- }
-
- const float* getFrontLeftWheelVelocity() const override
- {
- return frontLeftVelocity;
- }
-
- const float* getRearRightWheelVelocity() const override
- {
- return rearRightVelocity;
- }
-
- const float* getRearLeftWheelVelocity() const override
- {
- return rearLeftVelocity;
- }
-
- protected:
- void setFrontRightVelocityPtr(float* velocity)
- {
- frontRightVelocity = velocity;
- }
-
- void setFrontLeftVelocityPtr(float* velocity)
- {
- frontRightVelocity = velocity;
- }
-
- void setRearRightVelocityPtr(float* velocity)
- {
- frontRightVelocity = velocity;
- }
-
- void setRearLeftVelocityPtr(float* velocity)
- {
- frontRightVelocity = velocity;
- }
-
- float* frontRightVelocity;
- float* frontLeftVelocity;
- float* rearRightVelocity;
- float* rearLeftVelocity;
- };
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL0Controllers/LVL0Controller.h b/source/RobotAPI/libraries/RTRobotUnit/LVL0Controllers/LVL0Controller.h
deleted file mode 100644
index 3fbdcea6bf3e7e7f94d2e55bceec99c991124daf..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL0Controllers/LVL0Controller.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/*
- * 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::LVL0Controller
- * @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_LVL0Controller_H
-#define _ARMARX_LIB_RobotAPI_LVL0Controller_H
-
-#include "../Targets/TargetBase.h"
-
-#include <memory>
-
-namespace armarx
-{
- class LVL0Controller;
- typedef std::shared_ptr<LVL0Controller> LVL0ControllerPtr;
-
- class LVL0Controller
- {
- public:
- 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 rtResetTarget()
- {
- getTarget()->reset();
- }
-
- virtual bool rtIsTargetVaild() const
- {
- return getTarget()->isValid();
- }
-
- virtual std::string getControlMode() const
- {
- return getTarget()->getControlMode();
- }
- };
-}
-#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.h b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.h
deleted file mode 100644
index 893aa9639955ab52a0b523a69c1c8bfe2ac75a4e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughController.h
+++ /dev/null
@@ -1,64 +0,0 @@
-/*
- * 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"
-#include <RobotAPI/interface/libraries/RTRobotUnit/LVL1Controllers/LVL1HolonomicPlatformVelocityPassThroughControllerConfig.h>
-
-namespace armarx
-{
- struct LVL1HolonomicPlatformVelocityPassThroughControllerControlData
- {
- float velocityX = 0;
- float velocityY = 0;
- float velocityRotation = 0;
- };
-
- class LVL1HolonomicPlatformVelocityPassThroughController:
- virtual public LVL1ControllerWithTripleBuffer<LVL1HolonomicPlatformVelocityPassThroughControllerControlData>,
- virtual public LVL1HolonomicPlatformVelocityPassThroughControllerInterface
- {
- 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, const Ice::Current& = GlobalIceCurrent) override;
-
- //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
deleted file mode 100644
index 0a4b6f97ce4f1db650ea43463ed496faba183c60..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitPositionController.cpp
+++ /dev/null
@@ -1,107 +0,0 @@
-/*
- * 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
deleted file mode 100644
index 8c3875ce726c32f274fd51cba5dfefc5fd6f2c46..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitPositionController.h
+++ /dev/null
@@ -1,79 +0,0 @@
-/*
- * 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
deleted file mode 100644
index 5c53b220bd9f9b27d1d78068a2fc3b1b98025dc2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitTorqueController.cpp
+++ /dev/null
@@ -1,44 +0,0 @@
-/*
- * 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
deleted file mode 100644
index 4398baf00e4a97afe9d7b72158f0efbd459b7ba4..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitTorqueController.h
+++ /dev/null
@@ -1,84 +0,0 @@
-/*
- * 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
deleted file mode 100644
index fb72b71613445593abf71a885e4f146fa98451f2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitVelocityController.cpp
+++ /dev/null
@@ -1,95 +0,0 @@
-/*
- * 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
deleted file mode 100644
index 7f59f30eaa374dcfffb76217674e9de0ff764024..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1KinematicUnitVelocityController.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/*
- * 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/LVL1PassThroughController.cpp b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.cpp
deleted file mode 100644
index 83b51d8a277d163da8dbc9e680517d7e69751a1a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-/*
- * 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::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 "LVL1PassThroughController.h"
-using namespace armarx;
-
-LVL1ControllerRegistration<LVL1PassThroughController<ActuatorPositionTarget>> registrationControllerLVL1PositionPassThroughController("LVL1PositionPassThroughController");
-LVL1ControllerRegistration<LVL1PassThroughController<ActuatorVelocityTarget>> registrationControllerLVL1VelocityPassThroughController("LVL1VelocityPassThroughController");
-LVL1ControllerRegistration<LVL1PassThroughController<ActuatorTorqueTarget >> registrationControllerLVL1TorquePassThroughController("LVL1TorquePassThroughController");
diff --git a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.h b/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.h
deleted file mode 100644
index e6d311b5bb7a1dfc55bf07288fc7d1627db75e48..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/LVL1Controllers/LVL1PassThroughController.h
+++ /dev/null
@@ -1,250 +0,0 @@
-/*
- * 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::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_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/LVL1PassThroughController.h>
-
-#include "../Targets/ActuatorPositionTarget.h"
-#include "../Targets/ActuatorTorqueTarget.h"
-#include "../Targets/ActuatorVelocityTarget.h"
-#include "../DataUnits/KinematicDataUnit.h"
-#include "../Targets/PlatformWheelVelocityTarget.h"
-
-namespace armarx
-{
- template <typename T>
- struct AtomicWrapper
- {
- std::atomic<T> val;
-
- AtomicWrapper(): val { } {}
- AtomicWrapper(const T& v): val {v } {}
- AtomicWrapper(const std::atomic<T>& val): val {val.load() } {}
- AtomicWrapper(const AtomicWrapper& other): val {other.val.load()} {}
-
- AtomicWrapper& operator=(const AtomicWrapper& other)
- {
- val.store(other.val.load());
- return *this;
- }
- };
-
- template <typename TargetType>
- struct LVL1PassThroughControllerTargetTypeTraits;
-
- template <typename TargetType>
- class LVL1PassThroughController:
- virtual public LVL1Controller,
- virtual public LVL1PassThroughControllerInterface
- {
- public:
- using Traits = LVL1PassThroughControllerTargetTypeTraits<TargetType>;
-
- 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;
- inline virtual void rtPreActivateController() override;
-
- //ice interface
- inline virtual std::string getClassName(const Ice::Current& = GlobalIceCurrent) const override;
- inline virtual void setJoint(const std::string& name, Ice::Float value, const Ice::Current& = GlobalIceCurrent) override;
- inline virtual void setJoints(const StringFloatDictionary& values, const Ice::Current& = GlobalIceCurrent) override;
- protected:
- std::vector<float*> lvl0Targets;
- std::vector<const float*> jointStates;
- std::vector<AtomicWrapper<float>> iceTargets;
- std::unordered_map<std::string, std::size_t> indices;
-
- virtual void onInitComponent() override {}
- virtual void onConnectComponent() override {}
- };
-
- template <>
- struct LVL1PassThroughControllerTargetTypeTraits<ActuatorVelocityTarget>
- {
- static float* getTargetDatamember(ActuatorVelocityTarget& t)
- {
- return &t.velocity;
- }
- static std::vector<const float*> getSensorValues(const KinematicDataUnitInterface* ku, const std::vector<std::string>& v)
- {
- return ku->getJointVelocities(v);
- }
- static std::string getControlMode()
- {
- return ControlModes::VelocityMode;
- }
- };
- template <>
- struct LVL1PassThroughControllerTargetTypeTraits<ActuatorTorqueTarget>
- {
- static float* getTargetDatamember(ActuatorTorqueTarget& t)
- {
- return &t.torque;
- }
- static std::vector<const float*> getSensorValues(const KinematicDataUnitInterface* ku, const std::vector<std::string>& v)
- {
- return ku->getJointTorques(v);
- }
- static std::string getControlMode()
- {
- return ControlModes::TorqueMode;
- }
- };
- template <>
- struct LVL1PassThroughControllerTargetTypeTraits<ActuatorPositionTarget>
- {
- static float* getTargetDatamember(ActuatorPositionTarget& t)
- {
- return &t.position;
- }
- static std::vector<const float*> getSensorValues(const KinematicDataUnitInterface* ku, const std::vector<std::string>& v)
- {
- return ku->getJointAngles(v);
- }
- static std::string getControlMode()
- {
- return ControlModes::PositionMode;
- }
- };
-
- template <>
- struct LVL1PassThroughControllerTargetTypeTraits<PlatformWheelVelocityTarget>
- {
- static float* getTargetDatamember(PlatformWheelVelocityTarget& t)
- {
- return &t.velocity;
- }
- static const float* getFrontRightWheelVelocity(const PlatformDataUnitInterface* pu)
- {
- return pu->getFrontRightWheelVelocity();
- }
- static const float* getFrontLeftWheelVelocity(const PlatformDataUnitInterface* pu)
- {
- return pu->getFrontLeftWheelVelocity();
- }
- static const float* getRearRightWheelVelocity(const PlatformDataUnitInterface* pu)
- {
- return pu->getRearRightWheelVelocity();
- }
- static const float* getRearLeftWheelVelocity(const PlatformDataUnitInterface* pu)
- {
- return pu->getRearLeftWheelVelocity();
- }
- static std::string getControlMode()
- {
- return ControlModes::VelocityMode;
- }
- };
-
-
- template <typename TargetType>
- std::string LVL1PassThroughController<TargetType>::getClassName(const Ice::Current&) const
- {
- return "LVL1PassThroughController_" + Traits::getControlMode();
- }
-
- template <typename TargetType>
- LVL1PassThroughController<TargetType>::LVL1PassThroughController(LVL1ControllerDataProviderInterfacePtr prov, LVL1ControllerConfigPtr 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 " << 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 << "LVL1PassThroughController for " << cfg->jointNames;
- jointStates = Traits::getSensorValues(kinunit, cfg->jointNames) ;
-
- //not initialized, this is done in rtPreActivateController
- iceTargets.resize(cfg->jointNames.size(), 0);
-
- //get pointers for the results of this controller
- lvl0Targets.reserve(cfg->jointNames.size());
- 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());
- lvl0Targets.emplace_back(Traits::getTargetDatamember(*target));
- }
- indices = toIndexMap(cfg->jointNames);
- }
-
- template <typename TargetType>
- void LVL1PassThroughController<TargetType>::rtRun(const IceUtil::Time&, const IceUtil::Time&)
- {
- for (std::size_t i = 0; i < iceTargets.size(); ++i)
- {
- *lvl0Targets.at(i) = iceTargets.at(i).val.load();
- }
- }
-
- template <typename TargetType>
- void LVL1PassThroughController<TargetType>::rtSwapBufferAndRun(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
- {
- rtRun(sensorValuesTimestamp, timeSinceLastIteration);
- }
-
- template <typename TargetType>
- void LVL1PassThroughController<TargetType>::rtPreActivateController()
- {
- for (std::size_t i = 0; i < jointStates.size(); ++i)
- {
- iceTargets.at(i).val.store(*jointStates.at(i));
- //set it to zero
- iceTargets.at(i).val.store(0.0);
- }
- }
-
- template <typename TargetType>
- void LVL1PassThroughController<TargetType>::setJoint(const std::string& name, Ice::Float value, const Ice::Current&)
- {
- auto targetIt = indices.find(name);
- if (targetIt == indices.end())
- {
- throw InvalidArgumentException {"The joint " + name + " is not controlled by this (" + getName() + ") controller"};
- }
- iceTargets.at(targetIt->second).val.store(value);
- }
-
- template <typename TargetType>
- void LVL1PassThroughController<TargetType>::setJoints(const StringFloatDictionary& values, const Ice::Current&)
- {
- for (const auto& value : values)
- {
- setJoint(value.first, value.second);
- }
- }
-
-}
-#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/RobotUnit.cpp b/source/RobotAPI/libraries/RTRobotUnit/RobotUnit.cpp
deleted file mode 100644
index fc02567ebc7f2ef9f0b3b7882a08646420caef7b..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/RobotUnit.cpp
+++ /dev/null
@@ -1,600 +0,0 @@
-/*
- * 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::RobotUnit
- * @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 "RobotUnit.h"
-
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-
-#include <ArmarXCore/core/system/DynamicLibrary.h>
-
-armarx::PropertyDefinitionsPtr armarx::RobotUnit::createPropertyDefinitions()
-{
- return armarx::PropertyDefinitionsPtr(new RobotUnitPropertyDefinitions(getConfigIdentifier()));
-}
-
-void armarx::RobotUnit::addLVL0Controller(const std::string& jointName, armarx::LVL0Controller& lvl0Controller)
-{
- std::string controlMode = lvl0Controller.getControlMode();
- GuardType guard {dataMutex};
- ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
- if (hasLVL0Controller(jointName, controlMode))
- {
- ARMARX_ERROR << "A LVL0Controller for " + jointName + "(" + controlMode + ") does already exist!";
- throw std::invalid_argument {"A LVL0Controller for " + jointName + "(" + controlMode + ") does already exist!"};
- }
- lvl0Controllers[jointName][controlMode] = &lvl0Controller;
- if (controlMode == ControlModes::EmergencyStopMode)
- {
- lvl0ControllersEmergencyStop.at(jointNameIndices.at(jointName)) = &lvl0Controller;
- }
-}
-
-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::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!");
- return lvl0Controllers.at(jointName).at(controlMode);
-}
-
-Ice::StringSeq armarx::RobotUnit::getControllersKnown(const Ice::Current&) const
-{
- return LVL1ControllerRegistry::getKeys();
-}
-
-Ice::StringSeq armarx::RobotUnit::getControllerNamesLoaded(const Ice::Current&) const
-{
- GuardType guard {dataMutex};
- Ice::StringSeq result;
- result.reserve(lvl1Controllers.size());
- for (const auto& lvl1 : lvl1Controllers)
- {
- result.emplace_back(lvl1.first);
- }
- return result;
-}
-Ice::StringSeq armarx::RobotUnit::getControllerNamesRequested(const Ice::Current&) const
-{
- GuardType guard {dataMutex};
- Ice::StringSeq result;
- result.reserve(lvl1Controllers.size());
- for (const auto& lvl1 : getRequestedLVL1Controllers())
- {
- if (lvl1)
- {
- result.emplace_back(lvl1->getName());
- }
- }
- return result;
-}
-Ice::StringSeq armarx::RobotUnit::getControllerNamesActivated(const Ice::Current&) const
-{
- GuardType guard {dataMutex};
- Ice::StringSeq result;
- result.reserve(lvl1Controllers.size());
- for (const auto& lvl1 : getActivatedLVL1Controllers())
- {
- if (!lvl1)
- {
- continue;
- }
- result.emplace_back(lvl1->getName());
- }
- return result;
-}
-
-armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersLoaded(const Ice::Current&) const
-{
- GuardType guard {dataMutex};
- StringLVL1ControllerPrxDictionary result;
- for (const auto& lvl1 : lvl1Controllers)
- {
- result[lvl1.first] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1.second->getProxy());
- }
- return result;
-}
-armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersRequested(const Ice::Current&) const
-{
- GuardType guard {dataMutex};
- StringLVL1ControllerPrxDictionary result;
- for (const auto& lvl1 : getRequestedLVL1Controllers())
- {
- if (lvl1)
- {
- result[lvl1->getName()] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1->getProxy());
- }
- }
- return result;
-}
-armarx::StringLVL1ControllerPrxDictionary armarx::RobotUnit::getControllersActivated(const Ice::Current&) const
-{
- GuardType guard {dataMutex};
- StringLVL1ControllerPrxDictionary result;
- for (const auto& lvl1 : getActivatedLVL1Controllers())
- {
- result[lvl1->getName()] = LVL1ControllerInterfacePrx::uncheckedCast(lvl1->getProxy());
- }
- return result;
-}
-
-armarx::JointNameToLVL1Dictionary armarx::RobotUnit::getControllerJointAssignment(const Ice::Current&) const
-{
- GuardType guard {dataMutex};
- JointNameToLVL1Dictionary result;
- for (const auto& joint : jointNames)
- {
- result[joint] = "";
- }
- for (const auto& lvl1 : getActivatedLVL1Controllers())
- {
- if (!lvl1)
- {
- continue;
- }
- for (const auto& jointMode : lvl1->jointControlModeMap)
- {
- result[jointMode.first] = lvl1->getName();
- }
- }
- return result;
-}
-
-armarx::JointNameToControlModeDictionary armarx::RobotUnit::getJointControlModesRequested(const Ice::Current&) const
-{
- GuardType guard {dataMutex};
- JointNameToControlModeDictionary result;
- const auto& requestedModes = getRequestedLVL0Controllers();
- ARMARX_CHECK_AND_THROW(jointNames.size() == requestedModes.size(), std::logic_error);
- for (std::size_t i = 0; i < jointNames.size(); ++i)
- {
- if (requestedModes.at(i))
- {
- result[jointNames.at(i)] = requestedModes.at(i)->getControlMode();
- }
- else
- {
- result[jointNames.at(i)] = "<NO LVL0CONTROLLER SET>";
- }
- }
- return result;
-}
-armarx::JointNameToControlModeDictionary armarx::RobotUnit::getJointControlModesActivated(const Ice::Current&) const
-{
- GuardType guard {dataMutex};
- JointNameToControlModeDictionary result;
- const auto& activatedModes = getActivatedLVL0Controllers();
- ARMARX_CHECK_AND_THROW(jointNames.size() == activatedModes.size(), std::logic_error);
- for (std::size_t i = 0; i < jointNames.size(); ++i)
- {
- if (activatedModes.at(i))
- {
- result[jointNames.at(i)] = activatedModes.at(i)->getControlMode();
- }
- else
- {
- result[jointNames.at(i)] = "<NO LVL0CONTROLLER SET>";
- }
-
- }
- return result;
-}
-armarx::JointNameToControlModesDictionary armarx::RobotUnit::getJointControlModesSupported(const Ice::Current&) const
-{
- GuardType guard {dataMutex};
- JointNameToControlModesDictionary result;
- for (const auto& joint : lvl0Controllers)
- {
- std::vector<std::string> modes;
- modes.reserve(joint.second.size());
- for (const auto& mode : joint.second)
- {
- modes.emplace_back(mode.first);
- }
- result[joint.first] = std::move(modes);
- }
- return result;
-}
-
-void armarx::RobotUnit::switchSetup(const Ice::StringSeq& controllerRequestedNames, const Ice::Current&)
-{
- 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)
- {
- if (!hasLVL1Controller(lvl1))
- {
- throw InvalidArgumentException {"No controller of the name '" + lvl1 + "' is loaded!"};
- }
- }
- //check controllers (is there a collision/which controllers need to be deactivated)
- const auto currentActiveLVL1Controllers = getControllerNamesActivated();
- auto lvl1ControllerAssignement = getControllerJointAssignment();
-
- ARMARX_INFO << "current active lvl1 controllers:\n" << currentActiveLVL1Controllers;
-
- std::set<std::string> controllersToActivate {currentActiveLVL1Controllers.begin(), currentActiveLVL1Controllers.end()}; //these controllers will be set as active controllers
-
- for (const auto& toActivate : controllerRequestedNames)
- {
- if (controllersToActivate.count(toActivate))
- {
- ARMARX_INFO << "controller already active: " << toActivate;
- continue;
- }
- controllersToActivate.emplace(toActivate);
-
- ARMARX_INFO << "activate '" << toActivate << "'";
-
- const auto& lvl1 = lvl1Controllers.at(toActivate);
- //check and update the assignement map
- for (const auto& jointControlMode : lvl1->jointControlModeMap)
- {
- const auto& joint = jointControlMode.first;
- const auto& currentAssignedLVL1 = lvl1ControllerAssignement.at(joint);
- if (toActivate == currentAssignedLVL1)
- {
- continue;
- }
- if (currentAssignedLVL1.empty())
- {
- lvl1ControllerAssignement[joint] = toActivate;
- continue;
- }
- //deactivate other controller
- for (auto& assignement : lvl1ControllerAssignement)
- {
- if (assignement.second == currentAssignedLVL1)
- {
- assignement.second.clear();
- }
- }
- controllersToActivate.erase(currentAssignedLVL1);
- ARMARX_INFO << "deactivated '" << currentAssignedLVL1 << "' (caused by activation of '" << toActivate << "')";
- lvl1ControllerAssignement[joint] = toActivate;
- }
- }
- //verify (exception for collision of requested lvl1)
- for (const auto& toActivate : controllerRequestedNames)
- {
- if (!controllersToActivate.count(toActivate))
- {
- switchSetupError = true;
- ARMARX_ERROR << "The given set of controllers is in conflict and could not be activated! (conflicting is: " + toActivate + ")";
- throw InvalidArgumentException {"The given set of controllers is in conflict and could not be activated! (conflicting is: " + toActivate + ")"};
- }
- }
- //verify (warn for orphant joints) + populate controllersRequested.lvl0Controllers
- getRequestedLVL0Controllers().resize(jointNames.size());
- for (std::size_t i = 0; i < jointNames.size(); ++i)
- {
- const auto& joint = jointNames.at(i);
- if (lvl1ControllerAssignement[joint].empty())
- {
- getRequestedLVL0Controllers().at(i) = getLVL0Controller(joint, ControlModes::EmergencyStopMode);
- ARMARX_WARNING << "Joint '" << joint << "' has no lvl1 controller assigned!";
- continue;
- }
- const auto& lvl0Mode = lvl1Controllers.at(lvl1ControllerAssignement[joint])->jointControlModeMap.at(joint);
- getRequestedLVL0Controllers().at(i) = getLVL0Controller(joint, lvl0Mode);
- }
- //populate controllersRequested.lvl1Controllers
- getRequestedLVL1Controllers().clear();
- getRequestedLVL1Controllers().reserve(controllersToActivate.size());
- for (const auto& lvl1 : controllersToActivate)
- {
- getRequestedLVL1Controllers().emplace_back(lvl1Controllers.at(lvl1));
- }
-
- //now change the assignement
- controllersRequested.commitWrite();
- ARMARX_INFO << "requested lvl1 controllers:\n" << getControllerNamesRequested();
-}
-
-armarx::LVL1ControllerInterfacePrx armarx::RobotUnit::loadController(const std::string& className, const std::string& instanceName, const armarx::LVL1ControllerConfigPtr& config, const Ice::Current&)
-{
- if (instanceName.empty())
- {
- ARMARX_ERROR << "The instance name is empty!";
- throw InvalidArgumentException {"The instance name is empty!"};
- }
- //check if we would be able to create the class
- if (!LVL1ControllerRegistry::has(className))
- {
- std::stringstream ss;
- ss << "Requested controller class '" << className << "' unknown! Known classes:" << LVL1ControllerRegistry::getKeys();
- ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
- }
- auto factory = LVL1ControllerRegistry::get(className);
-
- GuardType guard {dataMutex};
- //check if the instance name is already in use
- if (hasLVL1Controller(instanceName))
- {
- std::stringstream ss;
- ss << "There already is a controller instance with the name '" << instanceName << "'";
- ARMARX_ERROR << ss.str();
- throw InvalidArgumentException {ss.str()};
- }
- //create the controller
- jointsUsedByLVL1Controller.clear();
- RobotUnitInterfacePtr::dynamicCast(RobotUnitPtr {this});
- LVL1ControllerDataProviderInterfacePtr::dynamicCast(RobotUnitPtr {this});
- LVL1ControllerPtr lvl1 = factory(LVL1ControllerDataProviderInterfacePtr::dynamicCast(RobotUnitPtr {this}), config);
- lvl1->jointControlModeMap = jointsUsedByLVL1Controller;
- lvl1->jointIndices.clear();
- lvl1->jointIndices.reserve(jointsUsedByLVL1Controller.size());
- for (const auto& j : jointsUsedByLVL1Controller)
- {
- lvl1->jointIndices.emplace_back(jointNameIndices.at(j.first));
- }
-
- getArmarXManager()->addObject(lvl1, instanceName);
- const auto prx = lvl1->getProxy(-1);
- lvl1Controllers[instanceName] = lvl1;
- return LVL1ControllerInterfacePrx::uncheckedCast(prx);
-}
-
-armarx::TargetBase* armarx::RobotUnit::getJointTarget(const std::string& jointName, const std::string& controlMode)
-{
- GuardType guard {dataMutex};
- if (!hasLVL0Controller(jointName, controlMode))
- {
- return nullptr;
- }
- const auto lvl0 = getLVL0Controller(jointName, controlMode);
- ARMARX_CHECK_EXPRESSION(lvl0->getControlMode() == controlMode);
- jointsUsedByLVL1Controller[jointName] = controlMode;
- return lvl0->getTarget();
-}
-
-void armarx::RobotUnit::setJointNames(const std::vector<std::string>& names)
-{
- GuardType guard {dataMutex};
- ARMARX_CHECK_EXPRESSION_W_HINT(!areJointNamesSet(), "JointNames were already set!");
- jointNames = names;
- jointNameIndices = toIndexMap(jointNames);
- lvl0ControllersEmergencyStop.resize(jointNames.size());
- controllersRequested.reinitAllBuffers(LVL0AndLVL1ControllerList(jointNames.size()));
- controllersActivated.reinitAllBuffers(LVL0AndLVL1ControllerList(jointNames.size()));
-}
-
-bool armarx::RobotUnit::hasLVL1Controller(const std::string& name) const
-{
- GuardType guard {dataMutex};
- ARMARX_CHECK_EXPRESSION_W_HINT(areJointNamesSet(), "Joint names a were not set!");
- return lvl1Controllers.end() != lvl1Controllers.find(name);
-}
-
-void armarx::RobotUnit::rtSetLVL1ControllerActive(const armarx::LVL1ControllerPtr& lvl1, bool isActive)
-{
- if (isActive)
- {
- lvl1->rtActivateController();
- }
- else
- {
- lvl1->rtDeactivateController();
- }
-}
-
-bool armarx::RobotUnit::areJointNamesSet() const
-{
- GuardType guard {dataMutex};
- return !jointNames.empty();
-}
-
-bool armarx::RobotUnit::hasLVL0Controller(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.end() != lvl0Controllers.find(jointName) &&
- lvl0Controllers.at(jointName).end() != lvl0Controllers.at(jointName).find(controlMode) &&
- lvl0Controllers.at(jointName).at(controlMode);
-}
-
-
-bool armarx::RobotUnit::loadLibFromAbsolutePath(const std::string& path)
-{
- GuardType guard {dataMutex};
- if (loadedLibs.count(path) > 0)
- {
- return true;
- }
- DynamicLibraryPtr lib(new DynamicLibrary());
- try
- {
- lib->load(path);
- }
- catch (...)
- {
- handleExceptions();
- return false;
- }
-
- if (lib->isLibraryLoaded())
- {
- loadedLibs[path] = lib;
- }
- else
- {
- ARMARX_ERROR << "Could not load lib " + path + ": " + lib->getErrorMessage();
- return false;
- }
-
-
- return true;
-}
-
-bool armarx::RobotUnit::loadLibFromPath(const std::string& path, const Ice::Current&)
-{
- std::string absPath;
- if (ArmarXDataPath::getAbsolutePath(path, absPath))
- {
- return loadLibFromAbsolutePath(absPath);
- }
- else
- {
- ARMARX_ERROR << "Could not find library " + path;
- return false;
- }
-}
-
-bool armarx::RobotUnit::loadLibFromPackage(const std::string& package, const std::string& name, const Ice::Current&)
-{
- CMakePackageFinder finder(package);
- if (!finder.packageFound())
- {
- ARMARX_ERROR << "Could not find package '" << package << "'";
- return false;
- }
-
- for (auto libDirPath : armarx::Split(finder.getLibraryPaths(), ";"))
- {
- boost::filesystem::path fullPath = libDirPath;
- fullPath /= "lib" + name + "." + DynamicLibrary::GetSharedLibraryFileExtension();
- if (!boost::filesystem::exists(fullPath))
- {
- fullPath = libDirPath;
- fullPath /= name;
- if (!boost::filesystem::exists(fullPath))
- {
- continue;
- }
- }
- if (loadLibFromAbsolutePath(fullPath.string()))
- {
- return true;
- }
- }
- ARMARX_ERROR << "Could not find library " << name << " in package " << package;
- return false;
-}
-
-bool armarx::RobotUnit::rtSwitchSetup()
-{
- if (!rtControllersWereSwitched())
- {
- return false;
- }
-
- //handle lvl1
- for (std::size_t i = 0; i < rtGetRequestedLVL1Controllers().size(); ++i)
- {
- //deactivate old
- if (rtGetActivatedLVL1Controller(i))
- {
- rtGetActivatedLVL1Controller(i)->rtDeactivateController();
- }
-
- //activate new
- if (rtGetRequestedLVL1Controller(i))
- {
- rtGetRequestedLVL1Controller(i)->rtActivateController();
- }
- //update activated
- rtGetActivatedLVL1Controller(i) = rtGetRequestedLVL1Controller(i);
- }
- //handle lvl0
- for (std::size_t i = 0; i < rtGetRequestedLVL0Controllers().size(); ++i)
- {
- if (rtGetRequestedLVL0Controller(i) != rtGetActivatedLVL0Controller(i))
- {
- rtSwitchLVL0Controller(i, rtGetActivatedLVL0Controller(i), rtGetRequestedLVL0Controller(i));
- rtGetActivatedLVL0Controller(i) = rtGetRequestedLVL0Controller(i);
- }
-
- if (!rtGetRequestedLVL0Controller(i))
- {
- //no lvl0 controller is set!
- rtSwitchLVL0Controller(i, rtGetActivatedLVL0Controller(i), rtGetEmergencyStopLVL0Controller(i));
- rtGetActivatedLVL0Controller(i) = rtGetActivatedLVL0Controller(i);
- }
- }
-
-
- return true;
-}
-
-void armarx::RobotUnit::rtRunLVL1Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-{
- for (LVL1ControllerPtr& lvl1 : rtGetActivatedLVL1Controllers())
- {
- if (!lvl1)
- {
- continue;
- }
- lvl1->rtSwapBufferAndRun(sensorValuesTimestamp, timeSinceLastIteration);
- }
-}
-
-void armarx::RobotUnit::rtDeactivateAssignedLVL1Controller(std::size_t index)
-{
- for (std::size_t i = 0; i < rtGetRequestedLVL1Controllers().size(); ++i)
- {
- if (!rtGetRequestedLVL1Controller(i))
- {
- continue;
- }
- if (rtGetRequestedLVL1Controller(i)->rtUsesJoint(index))
- {
- rtGetRequestedLVL1Controller(i)->rtDeactivateController();
- for (auto used : rtGetRequestedLVL1Controller(i)->rtGetJointIndices())
- {
- rtSwitchLVL0Controller(used, rtGetActivatedLVL0Controller(i), rtGetEmergencyStopLVL0Controller(i));
- rtGetActivatedLVL0Controller(i) = rtGetEmergencyStopLVL0Controller(i);
- }
- rtGetActivatedLVL1Controller(i) = nullptr;
- return;
- }
- }
-}
-
-void armarx::RobotUnit::rtRunLVL0Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration)
-{
- for (LVL0Controller* lvl0 : rtGetActivatedLVL0Controllers())
- {
- lvl0->rtRun(sensorValuesTimestamp, timeSinceLastIteration);
- }
-}
-
-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)
- {
- if (!lvl0)
- {
- return false;
- }
- }
- return true;
-
-}
diff --git a/source/RobotAPI/libraries/RTRobotUnit/RobotUnit.h b/source/RobotAPI/libraries/RTRobotUnit/RobotUnit.h
deleted file mode 100644
index 7f7fefb967b5b94fad0130204d6b44df05520df2..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/RobotUnit.h
+++ /dev/null
@@ -1,456 +0,0 @@
-/*
- * 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::RobotUnit
- * @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_UNIT_RobotAPI_RobotUnit_H
-#define _ARMARX_UNIT_RobotAPI_RobotUnit_H
-
-#include <thread>
-#include <atomic>
-#include <chrono>
-#include <mutex>
-#include <unordered_map>
-
-#include <ArmarXCore/core/Component.h>
-#include <ArmarXCore/core/ArmarXManager.h>
-#include <ArmarXCore/core/time/TimeUtil.h>
-#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
-#include <ArmarXCore/core/util/algorithm.h>
-#include <ArmarXCore/core/util/TripleBuffer.h>
-
-#include <RobotAPI/interface/libraries/RTRobotUnit/RobotUnitInterface.h>
-#include "LVL0Controllers/LVL0Controller.h"
-#include "LVL1Controllers/LVL1Controller.h"
-
-namespace armarx
-{
- class DynamicLibrary;
- typedef boost::shared_ptr<DynamicLibrary> DynamicLibraryPtr;
- /**
- * @class RobotUnitPropertyDefinitions
- * @brief
- */
- class RobotUnitPropertyDefinitions:
- public armarx::ComponentPropertyDefinitions
- {
- public:
- RobotUnitPropertyDefinitions(std::string prefix):
- armarx::ComponentPropertyDefinitions(prefix)
- {
- //defineRequiredProperty<std::string>("PropertyName", "Description");
- //defineOptionalProperty<std::string>("PropertyName", "DefaultValue", "Description");
- }
- };
-
- /**
- * @defgroup Component-RobotUnit RobotUnit
- * @ingroup RobotAPI-Components
- * A description of the component RobotUnit.
- *
- * @class RobotUnit
- * @ingroup Component-RobotUnit
- * @brief Brief description of class RobotUnit.
- *
- * \section Datastructures you should have
- * \li A thread for RT
- * \li A thread for publishing + errorreporting
- * \li A Tripplebuffer to publish the current robot state + current targets
- * \li A WriteBufferedTripleBuffer containing an error and logging structure (to communicate such information to the publisher thread)
- *
- * \section RobotUnit-tasks-to-implement Tasks to implement when deriving from this class
- * When implementing this class you have to handle the following tasks:
- * \li Implement the real time thread.
- * \li Implement the publisher thread.
- * \li Implement the communication between both threads.
- * \li Load all LVL0 controllers and add them to the unit.
- *
- * \subsection RobotUnit-tasks-to-implement-rt Implementing the RT thread
- * The thread has to be RT.
- * So you must not do anything of the following:
- * \li Call blocking actions (e.g. mutex::lock)
- * \li allocate ram using new (this calls lock on a mutex)
- * \li print output (this may allocate ram or block)
- * \li change the size of a datasructure (strings, vectors, etc) (this may allocate ram)
- *
- * The structure for a thread is:
- * \code{.cpp}
- * void rtThread()
- * {
- * try
- * {
- * initYourCommunication();//implement this
- * passYourLVL0ControllersToTheRobotUnit();//implement this
- * if(!validateAddedLVL0Controllers())
- * {
- * //report errors
- * throw std::logic_error{"lvl0 controller setup invalid"};
- * }
- *
- * //at this point the RT thread becomes RT (calls before this may not be rt)
- * IceUtil::Time currentStateTimestamp = TimeUtil::GetTime();
- * IceUtil::Time lastStateTimestamp = TimeUtil::GetTime();
- * getTheCurrentRobotState();//implement this
- * while(isNotStopped())//implement this
- * {
- * const IceUtil::Time startIteration = TimeUtil::GetTime();
- * //switching controllers
- * if(rtSwitchSetup())
- * {
- * if(switchSetupError)
- * {
- * //report this error and exit
- * throw std::logic_error{"switching the controller setup failed"};
- * }
- * //validate the setup if you need to
- * //if you had to change something, call rtCommitActivatedControllers();
- * }
- * //run the lvl1 controllers
- * rtRunLVL1Controllers(currentStateTimestamp, currentStateTimestamp - lastStateTimestamp);
- * //validate targets
- * for(std::size_t i = 0; i<rtGetActivatedLVL0Controllers().size(); ++i)
- * {
- * if(!rtGetActivatedLVL0Controller(i)->isTargetVaild())
- * {
- * //handle this error!
- * //you probably should log this error to some error struct
- * rtDeactivateAssignedLVL1Controller(i);
- * rtCommitActivatedControllers();
- * }
- * }
- * //run lvl0
- * rtRunLVL0Controllers();
- *
- * //time keeping + communicating
- * lastStateTimestamp = currentStateTimestamp;
- * currentStateTimestamp = TimeUtil::GetTime();
- * getTheCurrentRobotStateAndSendTheCurrentControllCommands();//implement this
- * communicateWithYourPublisherThread();//implement this
- * //maybe meassure how long a loop took and log this information to your error struct
- * TimeUtil::Sleep(IceUtil::Time::microSeconds(1000)-(currentStateTimestamp-startIteration));
- * }
- * }
- * catch(...)
- * {
- * emergencyStop();//implement this
- * dumpDebuggingData();//implement this
- * doSomeErrorHandling();//implement this
- * shutEverythingGracefullyDown();//implement this
- * }
- * }
- * \endcode
- *
- * \subsection RobotUnit-tasks-to-implement-pub Implementing the publisher thread
- *
- * \subsection RobotUnit-tasks-to-implement-com Implementing the communication between both threads
- * The communication between both threads has to be RT.
- * So you have to use atomics or TrippleBuffers (or any other non-blocking method)
- * Data you should communicate is:
- * \li Current state (joint modes/values)
- * \li Current goals
- * \li all error messages.
- *
- * \subsection RobotUnit-tasks-to-implement-lvl0 Adding LVL0 controllers
- * Add LVL0Controllers by calling
- * \code{.cpp}
- * 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.
- *
- * Each joint needs a LVL0Controller for the mode ControlModes::EmergencyStopMode.
- * This controller should activate the motor brakes.
- */
- class RobotUnit :
- virtual public Component,
- virtual public RobotUnitInterface,
- virtual public LVL1ControllerDataProviderInterface
- {
- public:
- using MutexType = std::recursive_mutex;
- using GuardType = std::lock_guard<MutexType>;
-
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- //ice LVL1ControllerDataProviderInterface
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- virtual std::string getName() const override
- {
- return Component::getName();
- }
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- //ice interface
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- //controllers
- virtual Ice::StringSeq getControllersKnown(const Ice::Current& = GlobalIceCurrent) const override;
- virtual Ice::StringSeq getControllerNamesLoaded(const Ice::Current& = GlobalIceCurrent) const override;
- virtual Ice::StringSeq getControllerNamesRequested(const Ice::Current& = GlobalIceCurrent) const override;
- virtual Ice::StringSeq getControllerNamesActivated(const Ice::Current& = GlobalIceCurrent) const override;
-
- virtual StringLVL1ControllerPrxDictionary getControllersLoaded(const Ice::Current& = GlobalIceCurrent) const override;
- virtual StringLVL1ControllerPrxDictionary getControllersRequested(const Ice::Current& = GlobalIceCurrent) const override;
- virtual StringLVL1ControllerPrxDictionary getControllersActivated(const Ice::Current& = GlobalIceCurrent) const override;
- virtual JointNameToLVL1Dictionary getControllerJointAssignment(const Ice::Current& = GlobalIceCurrent) const override;
- //modes
- virtual JointNameToControlModeDictionary getJointControlModesRequested(const Ice::Current& = GlobalIceCurrent) const override;
- virtual JointNameToControlModeDictionary getJointControlModesActivated(const Ice::Current& = GlobalIceCurrent) const override;
- virtual JointNameToControlModesDictionary getJointControlModesSupported(const Ice::Current& = GlobalIceCurrent) const override;
- //loading and switching
- virtual void switchSetup(const Ice::StringSeq& controllerRequestedNames, const Ice::Current& = GlobalIceCurrent) override;
- virtual LVL1ControllerInterfacePrx loadController(const std::string& className, const std::string& instanceName, const LVL1ControllerConfigPtr& config, const Ice::Current& = GlobalIceCurrent) override;
- virtual bool loadLibFromPath(const std::string& path, const Ice::Current& = GlobalIceCurrent) override;
- virtual bool loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current& = GlobalIceCurrent) override;
-
- //units
- virtual KinematicUnitInterfacePrx getKinematicUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
- virtual ForceTorqueUnitInterfacePrx getForceTorqueUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
- virtual InertialMeasurementUnitInterfacePrx getInertialMeasurementUnit(const Ice::Current& = GlobalIceCurrent) const = 0;
- virtual PlatformUnitInterfacePrx getPlatformUnitInterface(const Ice::Current& = GlobalIceCurrent) const = 0;
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // used by lvl1 controllers
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- //units (nonrt but results are used in rt)
- virtual const ForceTorqueDataUnitInterface* getRTForceTorqueDataUnit() const = 0;
- virtual const HapticDataUnitInterface* getRTHapticDataUnit() const = 0;
- virtual const IMUDataUnitInterface* getRTIMUDataUnit() const = 0;
- virtual const KinematicDataUnitInterface* getRTKinematicDataUnit() const = 0;
- virtual const PlatformDataUnitInterface* getRTPlatformDataUnit() const = 0;
- //targets (nonrt but results are used in rt)
- virtual TargetBase* getJointTarget(const std::string& jointName, const std::string& controlMode) override;
-
- protected:
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // NonRT
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // setup the data structures
- void setJointNames(const std::vector<std::string>& names);
-
- // Component
- /// @see PropertyUser::createPropertyDefinitions()
- virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
- //get buffers
- /// @return The requested lvl0 controllers (none is null)
- std::vector<LVL0Controller*>& getRequestedLVL0Controllers()
- {
- return controllersRequested.getWriteBuffer().lvl0Controllers;
- }
- /// @return The requested lvl1 controllers (some may be null)
- std::vector<LVL1ControllerPtr >& getRequestedLVL1Controllers()
- {
- return controllersRequested.getWriteBuffer().lvl1Controllers;
- }
- /// @return The requested lvl0 controllers (none is null)
- const std::vector<LVL0Controller*>& getRequestedLVL0Controllers() const
- {
- return controllersRequested.getWriteBuffer().lvl0Controllers;
- }
- /// @return The requested lvl1 controllers (some may be null)
- const std::vector<LVL1ControllerPtr >& getRequestedLVL1Controllers() const
- {
- return controllersRequested.getWriteBuffer().lvl1Controllers;
- }
- /// @return The activated lvl0 controllers (none is null)
- const std::vector<LVL0Controller*>& getActivatedLVL0Controllers() const
- {
- return controllersActivated.getUpToDateReadBuffer().lvl0Controllers;
- }
- /// @return The activated lvl1 controllers (some may be null)
- const std::vector<LVL1ControllerPtr >& getActivatedLVL1Controllers() const
- {
- return controllersActivated.getUpToDateReadBuffer().lvl1Controllers;
- }
-
- //checks
- /// @return Return whether the set of added LVL0Controllers is ok (e.g. if each joint has one EmergencyStop controller)
- virtual bool validateAddedLVL0Controllers() const;
-
- /**
- * @brief Add a LVL0Controller for a specified joint.
- * @param jointName The joint controlled by the controller
- * @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, LVL0Controller& lvl0Controller);
-
- /// @return The LVL0Controller for given joint and control mode.
- const LVL0Controller* getLVL0Controller(const std::string& jointName, const std::string& controlMode) const;
-
- /// @return The LVL0Controller for given joint and control mode.
- LVL0Controller* getLVL0Controller(const std::string& jointName, const std::string& controlMode);
-
- LVL0Controller* rtGetEmergencyStopLVL0Controller(std::size_t index)
- {
- return lvl0ControllersEmergencyStop.at(index);
- }
-
- /// @return Whether a LVL0Controller for given joint and control mode exists.
- bool hasLVL0Controller(const std::string& jointName, const std::string& controlMode) const;
-
- /// @return Whether a LVL1Controller for this name is loaded.
- bool hasLVL1Controller(const std::string& name) const;
-
- static void rtSetLVL1ControllerActive(const LVL1ControllerPtr& lvl1, bool isActive = 1);
-
- //other
- private:
- bool areJointNamesSet() const;
- bool loadLibFromAbsolutePath(const std::string& path);
-
- protected:
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // RT
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // get buffers (rt)
- bool rtControllersWereSwitched() const
- {
- return controllersRequested.updateReadBuffer();
- }
- /// @return The requested lvl0 controllers (none is null)
- const std::vector<LVL0Controller*>& rtGetRequestedLVL0Controllers() const
- {
- return controllersRequested.getReadBuffer().lvl0Controllers;
- }
- /// @return The requested lvl1 controllers (some may be null)
- const std::vector<LVL1ControllerPtr >& rtGetRequestedLVL1Controllers() const
- {
- return controllersRequested.getReadBuffer().lvl1Controllers;
- }
- /// @return The activated lvl0 controllers (none is null)
- std::vector<LVL0Controller*>& rtGetActivatedLVL0Controllers()
- {
- return controllersActivated.getWriteBuffer().lvl0Controllers;
- }
- /// @return The activated lvl1 controllers (some may be null)
- std::vector<LVL1ControllerPtr >& rtGetActivatedLVL1Controllers()
- {
- return controllersActivated.getWriteBuffer().lvl1Controllers;
- }
- /// @return The activated lvl0 controllers (none is null)
- const std::vector<LVL0Controller*>& rtGetActivatedLVL0Controllers() const
- {
- return controllersActivated.getWriteBuffer().lvl0Controllers;
- }
- /// @return The activated lvl1 controllers (some may be null)
- const std::vector<LVL1ControllerPtr >& rtGetActivatedLVL1Controllers() const
- {
- return controllersActivated.getWriteBuffer().lvl1Controllers;
- }
- //get controllers (rt)
- LVL0Controller* const& rtGetActivatedLVL0Controller(std::size_t index) const
- {
- return rtGetActivatedLVL0Controllers().at(index);
- }
- const LVL1ControllerPtr& rtGetActivatedLVL1Controller(std::size_t index) const
- {
- return rtGetActivatedLVL1Controllers().at(index);
- }
- LVL0Controller*& rtGetActivatedLVL0Controller(std::size_t index)
- {
- return rtGetActivatedLVL0Controllers().at(index);
- }
- LVL1ControllerPtr& rtGetActivatedLVL1Controller(std::size_t index)
- {
- return rtGetActivatedLVL1Controllers().at(index);
- }
-
-
- LVL0Controller* rtGetRequestedLVL0Controller(std::size_t index) const
- {
- return rtGetRequestedLVL0Controllers().at(index);
- }
- const LVL1ControllerPtr& rtGetRequestedLVL1Controller(std::size_t index) const
- {
- return getRequestedLVL1Controllers().at(index);
- }
-
- void rtCommitActivatedControllers()
- {
- controllersActivated.commitWrite();
- }
-
- //switching (rt)
- bool rtSwitchSetup();
- void rtRunLVL1Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
- void rtDeactivateAssignedLVL1Controller(std::size_t index);
- void rtRunLVL0Controllers(const IceUtil::Time& sensorValuesTimestamp, const IceUtil::Time& timeSinceLastIteration);
-
- /**
- * @brief Hook for switching the lvl0 controller (this changes the controll mode)
- * @param index The index of the lvl0 controller
- * @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, LVL0Controller* oldLVL0, LVL0Controller* newLVL0) = 0;
-
-
- protected:
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- // data
- // //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// //
- mutable MutexType dataMutex;
- //data accessible in RT and nonRT
- std::vector<std::string> jointNames;
- std::unordered_map<std::string, std::size_t> jointNameIndices;
- /// @brief True if some error occured in switch LVL1Controllers
- std::atomic_bool switchSetupError {false};
-
- private:
- //controllers
- struct LVL0AndLVL1ControllerList
- {
- LVL0AndLVL1ControllerList(std::size_t size):
- lvl0Controllers {size, nullptr},
- lvl1Controllers {size, nullptr}
- {}
-
- LVL0AndLVL1ControllerList() = default;
- LVL0AndLVL1ControllerList(const LVL0AndLVL1ControllerList&) = default;
-
- std::vector<LVL0Controller*> lvl0Controllers;
- std::vector<LVL1ControllerPtr > lvl1Controllers;
- };
- /**
- * @brief Holds pointer to all lvl0 controllers. (index: [jointname][controlmode])
- * Is initialized by derived classes.
- * May not be accessed when the controll loop is running
- */
- 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<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)
- WriteBufferedTripleBuffer<LVL0AndLVL1ControllerList> controllersRequested;
- /// @brief Controllers the RT thread is currently running (direction RT -> nonRT) (some lvl1 controllers may be null)
- WriteBufferedTripleBuffer<LVL0AndLVL1ControllerList> controllersActivated;
-
- //other
- /// @brief Stores joints accessed via getJointTarget
- JointNameToControlModeDictionary jointsUsedByLVL1Controller;
- std::map<std::string, DynamicLibraryPtr> loadedLibs;
- };
-
- using RobotUnitPtr = IceInternal::Handle<RobotUnit>;
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorPositionTarget.h b/source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorPositionTarget.h
deleted file mode 100644
index 643a12e539d3fb3ee7e64d3784f6ab1c042aa4b0..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorPositionTarget.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- * 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::ActuatorPositionTarget
- * @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_JointPositionTarget_H
-#define _ARMARX_LIB_RobotAPI_JointPositionTarget_H
-
-#include "TargetBase.h"
-
-namespace armarx
-{
- /**
- * @defgroup Library-RTRobotUnit RTRobotUnit
- * @ingroup RobotAPI
- * A description of the library RTRobotUnit.
- *
- * @class ActuatorPositionTarget
- * @ingroup Library-RTRobotUnit
- * @brief Brief description of class ActuatorPositionTarget.
- *
- * Detailed description of class ActuatorPositionTarget.
- */
- class ActuatorPositionTarget: public TargetBase
- {
- public:
- float position = ControllerConstants::ValueNotSetNaN;
- virtual std::string getControlMode() const override
- {
- return ControlModes::PositionMode;
- }
- virtual void reset() override
- {
- position = ControllerConstants::ValueNotSetNaN;
- }
- virtual bool isValid() const override
- {
- return std::isfinite(position);
- }
- };
-
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorTorqueTarget.h b/source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorTorqueTarget.h
deleted file mode 100644
index d4e29a926c249c54a864b43be6f131f8eb70dd2d..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorTorqueTarget.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- * 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::ActuatorTorqueTarget
- * @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_JointTorqueTarget_H
-#define _ARMARX_LIB_RobotAPI_JointTorqueTarget_H
-
-#include "TargetBase.h"
-
-namespace armarx
-{
- /**
- * @defgroup Library-RTRobotUnit RTRobotUnit
- * @ingroup RobotAPI
- * A description of the library RTRobotUnit.
- *
- * @class ActuatorTorqueTarget
- * @ingroup Library-RTRobotUnit
- * @brief Brief description of class ActuatorTorqueTarget.
- *
- * Detailed description of class ActuatorTorqueTarget.
- */
- class ActuatorTorqueTarget: public TargetBase
- {
- public:
- float torque = ControllerConstants::ValueNotSetNaN;
- virtual std::string getControlMode() const override
- {
- return ControlModes::TorqueMode;
- }
- virtual void reset() override
- {
- torque = ControllerConstants::ValueNotSetNaN;
- }
- virtual bool isValid() const override
- {
- return std::isfinite(torque);
- }
- };
-
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorVelocityTarget.h b/source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorVelocityTarget.h
deleted file mode 100644
index edfc60972f11de19009564e51bd189139f9e2877..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/Targets/ActuatorVelocityTarget.h
+++ /dev/null
@@ -1,60 +0,0 @@
-/*
- * 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::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_ActuatorVelocityTarget_H
-#define _ARMARX_LIB_RobotAPI_ActuatorVelocityTarget_H
-
-#include "TargetBase.h"
-
-namespace armarx
-{
- /**
- * @defgroup Library-RTRobotUnit RTRobotUnit
- * @ingroup RobotAPI
- * A description of the library RTRobotUnit.
- *
- * @class ActuatorVelocityTarget
- * @ingroup Library-RTRobotUnit
- * @brief Brief description of class ActuatorVelocityTarget.
- *
- * Detailed description of class ActuatorVelocityTarget.
- */
- class ActuatorVelocityTarget: public TargetBase
- {
- public:
- float velocity = ControllerConstants::ValueNotSetNaN;
- virtual std::string getControlMode() const override
- {
- return ControlModes::VelocityMode;
- }
- virtual void reset() override
- {
- velocity = ControllerConstants::ValueNotSetNaN;
- }
- virtual bool isValid() const override
- {
- return std::isfinite(velocity);
- }
- };
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/Targets/PlatformWheelVelocityTarget.h b/source/RobotAPI/libraries/RTRobotUnit/Targets/PlatformWheelVelocityTarget.h
deleted file mode 100644
index c2bafb0adb4cb68407667fbf5db85c2eb9eb7ea6..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/Targets/PlatformWheelVelocityTarget.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
- * Karlsruhe Institute of Technology (KIT), all rights reserved.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author Markus Swarowsky (markus dot swarowsky at student dot kit dot edu)
- * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
- * GNU General Public License
- */
-
-
-#ifndef _ARMARX_LIB_RobotAPI_PLATFORMWHEELTARGET_H
-#define _ARMARX_LIB_RobotAPI_PLATFORMWHEELTARGET_H
-
-
-#include "TargetBase.h"
-
-
-namespace armarx
-{
-
- class PlatformWheelVelocityTarget : public TargetBase
- {
- public:
- float velocity = ControllerConstants::ValueNotSetNaN;
- virtual std::string getControlMode() const override
- {
- return ControlModes::VelocityMode;
- }
- virtual void reset() override
- {
- velocity = ControllerConstants::ValueNotSetNaN;
- }
- virtual bool isValid() const override
- {
- return std::isfinite(velocity);
- }
-
- private:
- };
-
-}
-
-#endif //_ARMARX_LIB_RobotAPI_PLATFORMWHEELTARGET_H
diff --git a/source/RobotAPI/libraries/RTRobotUnit/Targets/TargetBase.h b/source/RobotAPI/libraries/RTRobotUnit/Targets/TargetBase.h
deleted file mode 100644
index 1d515e670d3a0e8428f9c301c232d497c83338c3..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/Targets/TargetBase.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * 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::JointTargetBase
- * @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_JointTargetBase_H
-#define _ARMARX_LIB_RobotAPI_JointTargetBase_H
-
-#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include "../Constants.h"
-#include "../ControlModes.h"
-
-namespace armarx
-{
- /**
- * @defgroup Library-RTRobotUnit RTRobotUnit
- * @ingroup RobotAPI
- * A description of the library RTRobotUnit.
- *
- * @class TargetBase
- * @ingroup Library-RTRobotUnit
- * @brief Brief description of class JointTargetBase.
- *
- * Detailed description of class TargetBase.
- */
- class TargetBase
- {
- public:
- virtual std::string getControlMode() const = 0;
- virtual void reset() = 0;
- virtual bool isValid() const = 0;
- };
-}
-
-#endif
diff --git a/source/RobotAPI/libraries/RTRobotUnit/test/BasicControllerTest.cpp b/source/RobotAPI/libraries/RTRobotUnit/test/BasicControllerTest.cpp
deleted file mode 100644
index a3ff1667357a7fe758d8901aeb2e0d772ecfb25a..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/RTRobotUnit/test/BasicControllerTest.cpp
+++ /dev/null
@@ -1,596 +0,0 @@
-/*
-* 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";
-}
-